我简单测试了一下，计算结果正确。

import numpy as np
import math

class NonOrthogonalGimbal:
    """
    非正交云台坐标转换类
    支持两种非正交类型：
    1. 方位轴与俯仰轴不垂直（存在安装偏差角）
    2. 双轴非正交（两轴实际轴线不垂直）
    """
    
    def __init__(self, alpha=0, beta=0, gimbal_type='azimuth_tilt'):
        """
        初始化云台非正交参数
        
        参数:
        ----------
        alpha : float
            非正交角（弧度），表示两轴实际夹角与90°的偏差
        beta : float
            第二非正交角（弧度），某些模型需要
        gimbal_type : str
            'azimuth_tilt' : 方位轴倾斜（常见于两轴云台）
            'dual_axis' : 双轴非正交（更一般模型）
        """
        self.alpha = alpha
        self.beta = beta
        self.type = gimbal_type
        
    def angles_to_direction(self, azimuth, elevation):
        """
        将云台角度（非正交坐标系）转换为笛卡尔方向向量
        
        参数:
        ----------
        azimuth : float
            方位角（弧度），绕Z轴
        elevation : float
            俯仰角（弧度），绕X或Y轴（取决于云台结构）
            
        返回:
        ----------
        numpy.ndarray
            归一化方向向量 [x, y, z] 在东北天坐标系下
        """
        if self.type == 'azimuth_tilt':
            # 模型1: 俯仰轴相对于方位轴有非正交偏差
            # 实际俯仰轴方向 = 理想俯仰轴 + 绕方位轴方向的微小旋转
            
            # 理想坐标系下的初始方向（假设俯仰轴为Y轴，方位轴为Z轴）
            # 先计算理想方向（正交情况）
            cos_el = math.cos(elevation)
            x_ideal = cos_el * math.cos(azimuth)
            y_ideal = cos_el * math.sin(azimuth)
            z_ideal = math.sin(elevation)
            
            # 应用非正交修正：俯仰轴实际绕X轴有alpha角偏差
            # 实际方向 = R_x(alpha) * 理想方向
            R_nonorth = np.array([
                [1, 0, 0],
                [0, math.cos(self.alpha), -math.sin(self.alpha)],
                [0, math.sin(self.alpha), math.cos(self.alpha)]
            ])
            
            dir_ideal = np.array([x_ideal, y_ideal, z_ideal])
            dir_actual = R_nonorth @ dir_ideal
            
        elif self.type == 'dual_axis':
            # 模型2: 双轴非正交，使用更一般的旋转矩阵
            # 方位轴绕Z轴，俯仰轴实际绕轴 u = [sin(beta), cos(beta), 0]
            # 先绕俯仰轴旋转，再绕方位轴旋转
            
            # 俯仰轴单位向量（存在非正交）
            tilt_axis = np.array([math.sin(self.beta), math.cos(self.beta), 0])
            tilt_axis = tilt_axis / np.linalg.norm(tilt_axis)
            
            # 绕任意轴旋转的罗德里格斯公式
            def rodrigues_rot(v, k, theta):
                """绕单位向量k旋转theta角"""
                cos_t = math.cos(theta)
                sin_t = math.sin(theta)
                return v * cos_t + np.cross(k, v) * sin_t + k * np.dot(k, v) * (1 - cos_t)
            
            # 初始方向（沿X轴）
            init_dir = np.array([1, 0, 0])
            
            # 先绕俯仰轴（非正交）旋转elevation角
            dir_tilted = rodrigues_rot(init_dir, tilt_axis, elevation)
            
            # 再绕Z轴旋转azimuth角
            Rz = np.array([
                [math.cos(azimuth), -math.sin(azimuth), 0],
                [math.sin(azimuth), math.cos(azimuth), 0],
                [0, 0, 1]
            ])
            
            dir_actual = Rz @ dir_tilted
            
        else:
            raise ValueError(f"Unknown gimbal type: {self.type}")
            
        # 归一化
        dir_actual = dir_actual / np.linalg.norm(dir_actual)
        return dir_actual
    
    def direction_to_angles(self, direction, initial_guess=None):
        """
        将笛卡尔方向向量转换为云台角度（非正交坐标系）
        使用数值优化方法求解逆运动学
        
        参数:
        ----------
        direction : numpy.ndarray
            目标方向向量 [x, y, z]（归一化或非归一化）
        initial_guess : tuple or None
            初始猜测角度 (azimuth, elevation)，默认None则自动计算
            
        返回:
        ----------
        tuple
            (azimuth, elevation) 云台所需角度（弧度）
        """
        # 归一化输入向量
        direction = np.array(direction)
        direction = direction / np.linalg.norm(direction)
        
        # 如果未提供初始猜测，使用理想正交解作为起点
        if initial_guess is None:
            # 理想情况下的角度
            ideal_az = math.atan2(direction[1], direction[0])
            ideal_el = math.asin(direction[2])
            initial_guess = (ideal_az, ideal_el)
        
        # 使用scipy优化（如果可用），否则使用简单梯度下降
        try:
            from scipy.optimize import minimize
            
            def error_func(angles):
                az, el = angles
                dir_pred = self.angles_to_direction(az, el)
                # 计算角度误差（点积反余弦）
                cos_theta = np.clip(np.dot(dir_pred, direction), -1, 1)
                return np.arccos(cos_theta) ** 2
            
            # 优化
            result = minimize(error_func, initial_guess, 
                            method='L-BFGS-B', 
                            bounds=[(-np.pi, np.pi), (-np.pi/2, np.pi/2)])
            
            if result.success:
                return result.x[0], result.x[1]
            else:
                # 优化失败，返回最接近的离散搜索解
                return self._grid_search_angles(direction)
                
        except ImportError:
            # 没有scipy时使用简单网格搜索
            return self._grid_search_angles(direction)
    
    def _grid_search_angles(self, direction, resolution=100):
        """网格搜索方法（备用）"""
        best_error = float('inf')
        best_angles = (0, 0)
        
        for i in range(resolution):
            az = -np.pi + 2 * np.pi * i / resolution
            for j in range(resolution//2 + 1):
                el = -np.pi/2 + np.pi * j / resolution
                dir_pred = self.angles_to_direction(az, el)
                cos_theta = np.clip(np.dot(dir_pred, direction), -1, 1)
                error = np.arccos(cos_theta)
                
                if error < best_error:
                    best_error = error
                    best_angles = (az, el)
                    
                    if error < 1e-6:  # 找到精确解
                        return best_angles
                        
        return best_angles
    
    def transform_points(self, points, azimuth, elevation):
        """
        将点云从云台坐标系转换到世界坐标系
        
        参数:
        ----------
        points : numpy.ndarray
            Nx3的点云数组
        azimuth, elevation : float
            云台当前角度
            
        返回:
        ----------
        numpy.ndarray
            转换后的点云
        """
        # 获取方向向量（从云台到目标）
        direction = self.angles_to_direction(azimuth, elevation)
        
        # 构造旋转矩阵：使云台Z轴指向direction
        # 简化版本：假设云台坐标系Z轴为视线方向
        z_axis = direction
        # 选择一个临时参考向量
        tmp = np.array([0, 0, 1]) if abs(z_axis[2]) < 0.999 else np.array([1, 0, 0])
        x_axis = np.cross(tmp, z_axis)
        x_axis = x_axis / np.linalg.norm(x_axis)
        y_axis = np.cross(z_axis, x_axis)
        
        R = np.array([x_axis, y_axis, z_axis]).T
        
        # 应用旋转
        points_transformed = points @ R.T
        
        return points_transformed

def example1():
    # 示例1: 基本使用 - 方位轴倾斜模型
    print("=== 示例1: 方位轴倾斜模型 ===")
   
    # 转换到实际指向方向
    direction = gimbal1.angles_to_direction(az, el)
    print(f"云台角度: az={np.degrees(az):.1f}°, el={np.degrees(el):.1f}°")
    print(f"实际指向方向: {direction}")
    
    # 反向转换：给定方向，求云台角度
    target_dir = np.array([0.5, 0.5, 0.7071])
    az_recovered, el_recovered = gimbal1.direction_to_angles(target_dir)
    print(f"目标方向 {target_dir} 对应的云台角度: "
          f"az={np.degrees(az_recovered):.1f}°, el={np.degrees(el_recovered):.1f}°")
    

def example2():
    print("\n=== 示例2: 双轴非正交模型 ===")
    # 双轴非正交，beta=0.1弧度
    gimbal2 = NonOrthogonalGimbal(alpha=0, beta=0.1, gimbal_type='dual_axis')
    
    direction2 = gimbal2.angles_to_direction(az, el)
    print(f"双轴非正交模型指向: {direction2}")
    
    # 对比理想正交情况
    ideal_dir = np.array([
        math.cos(el) * math.cos(az),
        math.cos(el) * math.sin(az),
        math.sin(el)
    ])
    print(f"理想正交指向: {ideal_dir}")
    print(f"角度偏差: {np.degrees(np.arccos(np.dot(direction2, ideal_dir))):.3f}°")
    

def example3():
    print("\n=== 示例3: 点云转换 ===")
    # 生成一个简单的点云（立方体8个顶点）
    points = np.array([
        [0,0,0], [1,0,0], [0,1,0], [0,0,1],
        [1,1,0], [1,0,1], [0,1,1], [1,1,1]
    ])
    
    # 云台指向 (az=30°, el=20°)
    transformed = gimbal1.transform_points(points, np.radians(30), np.radians(20))
    print(f"原始点云形状: {points.shape}")
    print(f"转换后点云前3个点:\n{transformed[:3]}")

# ========== 使用示例 ==========
if __name__ == "__main__":

    # 假设俯仰轴相对于正交位置有3度的偏差
    alpha = 3
    # 给定云台角度
    az    = 45
    el    = 30
    
    print(np.radians(2.86))
    gimbal1 = NonOrthogonalGimbal(alpha=np.radians(alpha), gimbal_type='azimuth_tilt')
    az = np.radians(az)   # 45度方位角
    el = np.radians(el)   # 30度俯仰角

    example1()
    example2()
    example3()