PX4无人机控制开发踩坑记录：控制频率、坐标系转换与欧拉角多解问题

前言这里记录自己设计无人机控制代码过程中遇到的常见问题和自己踩过的坑。1. 控制频率的影响本质上无人机控制系统通常以离散形式运行。对于相同的控制参数设置不同的控制频率会显著影响系统的闭环性能。随着控制频率的提高系统的有效控制带宽增大使得控制器能够采用更为激进的控制策略从而获得更快的动态响应。因此在高速飞行或快速机动等场景中通常需要较高的控制频率以保证控制性能和稳定性。我们假设一个简单的系统模型x˙v \dot{x} vx˙vv˙areal \dot{v} a_{real}v˙areal​a˙realacmd−arealτact \dot{a}_{real} \frac{a_{cmd} - a_{real}}{\tau_{act}}a˙real​τact​acmd​−areal​​其中xxx位置vvv速度acmda_{cmd}acmd​控制器输出的期望加速度areala_{real}areal​执行器实际产生的加速度τact\tau_{act}τact​执行器时间常数控制器采用简单的Position-P Velocity-P形式acmdkp(xref−xm)kv(vref−vm) a_{cmd} k_p (x_{ref}-x_m) k_v (v_{ref}-v_m)acmd​kp​(xref​−xm​)kv​(vref​−vm​)其中xm,vmx_m, v_mxm​,vm​为带有测量延迟的状态。下图展示了相同参数不同控制频率下的影响我使用了较为激进的控制参数kp15.0,kv10.0k_p 15.0,\quad k_v 10.0kp​15.0,kv​10.0从图中可以看出频率越高响应速度越快收敛效果越好。除此之外轨迹规划的效果也依赖于较高的控制频率。例如当无人机以10 m/s 的速度沿半径为 6 m 的圆轨迹飞行时如果控制频率较低轨迹执行效果将更接近一个多边形而不是理想的圆形。2. 坐标转换本人在设计控制器时曾在这一问题上踩过坑。以常见的 PX4 飞控为例在发布控制话题时需要明确所使用的坐标系。例如通过 MAVROS 获取的 GPS 数据通常采用ENU 坐标系而一些里程计如 VIO则通常以初始机头方向为 x 轴、天空方向为 z 轴并满足右手坐标系进行定义。此外还需要注意位置与速度是否在同一坐标系中表达。例如在 PX4 中/mavros/local_position/odom的消息类型为nav_msgs/Odometry其中pose表达在header.frame_id坐标系中twist表达在child_frame_id坐标系中因此位置通常是在世界坐标系如 odom 或 map中表示而速度则是在机体坐标系中表示。而我之前使用的一种 VIO 里程计中位置与速度是统一在同一坐标系中表达的。在更换为 RTK 定位后由于坐标系定义发生变化再加上同时存在欧拉角处理和控制频率等问题导致调试了较长时间才发现这一差异。因此在根据里程计数据设计控制算法并计算期望位姿时需要特别注意将结果转换到控制话题所使用的坐标系中。例如在使用姿态控制话题/mavros/setpoint_raw/attitude时需要保证发布的期望姿态与飞控内部使用的坐标系一致。在实现中我使用如下公式进行姿态转换quqimu⊗qodom−1⊗qd q_u q_{\text{imu}} \otimes q_{\text{odom}}^{-1} \otimes q_dqu​qimu​⊗qodom−1​⊗qd​其中qimuq_{\text{imu}}qimu​IMU 坐标系下的姿态四元数表示qodomq_{\text{odom}}qodom​里程计坐标系下的姿态四元数表示qdq_dqd​控制器计算得到的期望姿态quq_uqu​最终计算得到的期望姿态用于发布到/mavros/setpoint_raw/attitude该公式的核心是先计算IMU 坐标系与里程计坐标系之间的旋转关系qIOqimu⊗qodom−1 q_{IO} q_{\text{imu}} \otimes q_{\text{odom}}^{-1}qIO​qimu​⊗qodom−1​该四元数表示从 odom 坐标系到 IMU 坐标系的旋转变换。随后将控制器在 odom 坐标系中计算得到的期望姿态qdq_dqd​通过该变换映射到 IMU 坐标系中quqIO⊗qd q_u q_{IO} \otimes q_dqu​qIO​⊗qd​最终得到的quq_uqu​即为在 IMU 坐标系下表达的期望姿态可以直接作为姿态控制指令发布给/mavros/setpoint_raw/attitude。简单来说这一步的作用是将控制器在 odom 坐标系中计算得到的期望姿态转换到 IMU 坐标系下再发送给飞控。如果忽略这一转换在 VIO、RTK 和 IMU 坐标系定义不一致的情况下很容易导致姿态控制出现偏差甚至无法稳定。3. 欧拉角表示的不唯一性在实际实现过程中当调用 Eigen 提供的 eulerAngles(0,1,2) 函数时观察到姿态角会出现突变现象。这一问题的根本原因在于 欧拉角表示本身并不是唯一的。对于同一个旋转矩阵或四元数往往存在多组欧拉角可以表示相同的空间姿态。因此在进行 旋转矩阵或四元数到欧拉角的转换时数学上会出现多解的情况。为了保证数值计算的稳定性并尽量避免奇异情况如接近万向节锁 gimbal lock许多数值库在实现欧拉角分解时会采用 分支选择branch switching策略。Eigen 的 eulerAngles() 函数实现参考了 Graphics Gems IV 中的相关算法在特定条件下会自动切换到另一组等价的欧拉角解以保持分解结果的数值稳定性。然而这种分支切换在数值上可能表现为 欧拉角的突然跳变例如 yaw 增加或减少 π。当这些欧拉角进一步用于三角函数运算如计算 sin、cos时就可能导致控制量出现符号突变从而影响控制算法的稳定性。在实际无人机飞行场景中姿态变化通常具有一定范围约束例如 roll 和 pitch 一般不会超过 ±90°。在这种情况下可以通过 固定欧拉角分解的分支例如直接使用四元数解析公式计算 roll、pitch、yaw避免自动分支切换带来的角度突变问题从而保证姿态角在数值上的连续性。这种处理方式在无人机控制系统中更加稳定和可靠。