4. 俯仰方位运动学建模

俯仰方位转台的运动学建模，是把“电机转了多少、平台指向哪里”这件事用数学关系固定下来。对重型光学平台而言，转台通常由方位轴（绕竖直轴旋转）和俯仰轴（绕水平轴旋转）串联构成，负载质量大、惯量高，任何角度解算错误都会直接反映为指向偏差或结构干涉。因此本节从坐标系定义出发，逐步建立欧拉角、旋转矩阵、正逆运动学，并分析奇异位形对工程使用的影响。

一、坐标系定义：把物理轴映射为数学轴

建模第一步是明确坐标系。工程中常用三套坐标系：基座坐标系 {B} 固定在转台底座上，Z_B 竖直向上；方位坐标系 {A} 随方位轴转动，原点位于方位轴与俯仰轴交点；俯仰坐标系 {P} 随俯仰框架转动，其 X_P 沿俯仰轴，Z_P 指向光学载荷视轴。三套坐标系原点重合于转台回转中心，可避免平移分量进入姿态解算。

定义方位角 θ_az 为 {A} 相对 {B} 绕 Z_B 的转角，俯仰角 θ_el 为 {P} 相对 {A} 绕 X_A 的转角。零位约定为：θ_az=0 时视轴指向正北，θ_el=0 时视轴水平。这一约定必须与编码器零位、机械限位和软件零点三者一致，否则后续矩阵再正确也无法得到真实指向。

二、欧拉角与旋转矩阵

方位—俯仰转台属于两自由度串联结构，其姿态可用 Z-X 顺序的欧拉角描述。从 {B} 到 {P} 的旋转矩阵为两次基本旋转的乘积：

R = Rz(θ_az) · Rx(θ_el)

Rz(θ_az) = [ cosθ_az  -sinθ_az  0 ]
           [ sinθ_az   cosθ_az  0 ]
           [    0         0     1 ]

Rx(θ_el) = [ 1      0          0     ]
           [ 0   cosθ_el  -sinθ_el ]
           [ 0   sinθ_el   cosθ_el ]

展开后得到完整旋转矩阵 R，其第三行第一列元素 r₃₁ = -sinθ_el，第一行第一列 r₁₁ = cosθ_azcosθ_el，第二行第一列 r₂₁ = sinθ_azcosθ_el。这三个元素是逆运动学解算的关键。需要特别注意的是，旋转矩阵的乘法顺序不可交换：先方位后俯仰与先俯仰后方位得到的姿态完全不同，工程实现中必须与机械装配顺序严格对应。

三、正运动学：由关节角求视轴指向

正运动学解决“已知 θ_az 和 θ_el，求视轴在基座坐标系中的方向”。设视轴在 {P} 中为单位向量 v_P = [0, 0, 1]^T，则其在 {B} 中的方向为：

v_B = R · v_P = [ cosθ_az·sinθ_el, sinθ_az·sinθ_el, cosθ_el ]^T

以示例参数 θ_az=30°、θ_el=45° 代入，可得 v_B ≈ [0.612, 0.354, 0.707]。该向量表示视轴在基座坐标系中的方向余弦，其方位角为 atan2(0.354, 0.612) ≈ 30°，俯仰角为 acos(0.707) = 45°，与输入一致，说明正运动学解算正确。实际工程中，还需将方向余弦转换为经纬仪或星敏感器可读的指向角，并叠加编码器零位偏差和结构变形修正量。

四、逆运动学：由目标指向反解关节角

逆运动学解决“已知目标方向，求 θ_az 和 θ_el”。设目标单位向量在 {B} 中为 [x, y, z]^T，由旋转矩阵元素可得：

θ_el = asin(-r31) = asin(z)   （当视轴为 Z_P 时）
θ_az = atan2(y, x)

更一般地，若目标向量为 [x, y, z]^T，则 θ_el = asin(z)，θ_az = atan2(y, x)。例如目标指向 [0.5, 0.5, 0.707]^T，则 θ_el = asin(0.707) = 45°，θ_az = atan2(0.5, 0.5) = 45°。逆解存在两个分支：θ_el 在 [-90°, 90°] 内唯一，但 θ_az 存在 θ_az 与 θ_az+180° 的歧义，需根据机械限位和最短路径原则选择。重型转台还应考虑电缆缠绕方向，避免方位轴连续单向旋转导致线缆扭断。

五、奇异位形分析

奇异位形是指雅可比矩阵降秩、逆解不唯一或关节速度趋于无穷的位形。对方位—俯仰转台，当 θ_el = ±90° 时，视轴与方位轴重合，此时方位角失去对指向的调节能力，雅可比矩阵的方位分量变为零，出现奇异。工程上表现为：接近天顶时，微小的指向变化需要极大的方位角速度，甚至无法解算。

判断奇异的方法是对雅可比矩阵 J 求行列式。对于本结构，det(J) = cosθ_el，当 θ_el = ±90° 时 det(J)=0。实际使用中应设置俯仰角软限位，例如限制在 ±85° 以内，并在控制软件中加入奇异回避策略：当 θ_el 接近限值时，降低方位轴增益或切换为笛卡尔空间插补。此外，重型转台在奇异点附近惯量耦合加剧，需在轨迹规划阶段进行速度前馈补偿，避免方位电机过流报警。

综上，俯仰方位转台的运动学建模核心在于：坐标系定义要统一、旋转矩阵顺序要正确、正逆解要闭环验证、奇异位形要提前规避。这四步完成后，才能为后续的轨迹规划、伺服控制和误差补偿提供可靠的数学基础。