px4多旋翼姿态控制程序分析

### PX4 多旋翼无人机姿态控制程序解析 #### 姿态控制器架构概述 PX4 的多旋翼姿态控制系统是一个分层设计的闭环反馈系统，主要包括内环角度速率控制和外环角度控制两部分。该系统通过接收来自外部传感器的数据并结合设定点信息来调整电机输出，实现稳定飞行。 #### 内环角速度控制 为了精确地维持期望的姿态，在内部存在一个快速响应的角度速率回路。此环节负责将实际测量得到的身体坐标系下的角速度转换为目标指令所对应的参考值之间的差异，并利用PID算法计算出所需的转矩命令： ```cpp // 计算角速度误差 Vector3f rate_error = _rate_setpoint - gyro; // 应用 PID 控制律获得力矩需求 _vector_thrust_body += _ctrl_rate.getControl(rate_error, dt); ``` 上述代码片段展示了如何基于当前陀螺仪读数`gyro`与预期角速度设定点 `_rate_setpoint` 来求得偏差 `rate_error` ，进而调用 PID 调节器对象 `_ctrl_rate` 获取最终施加于机体上的额外推力矢量[^1]。 #### 外环姿态控制 在外围则构建了一个较慢但更注重精度的态度保持机制。它接受由导航模块产生的航向、俯仰及横滚角作为输入信号，并将其转化为相应的角速度设定点传递给内环处理: ```cpp // 更新姿态估计 _quat.setFromEuler(roll_sp, pitch_sp, yaw_sp); // 将四元数形式的姿态设为新的状态机目标 _attitude_setpoint.q_d = _quat; // 使用四元数差乘法获取旋转矩阵中的前两个轴方向 _matrix_R_to_earth = Matrix3f(_attitude_setpoint.q_f).transpose(); _matrix_R_to_body = Matrix3f(_quat).transpose(); // 得到相对于水平面投影后的期望朝向变化率 Vector2f body_z_axis_proj = rpy_from_rotation(_matrix_R_to_body)[2].xy().normalize(); Vector2f earth_z_axis_proj = rpy_from_rotation(_matrix_R_to_earth)[2].xy().normalize(); // 设置横向偏移补偿项 float lateral_offset_compensation = atan2(-body_z_axis_proj.y(), body_z_axis_proj.x()) - atan2(-earth_z_axis_proj.y(), earth_z_axis_proj.x()); // 构建完整的三维空间内的角速度请求 Vector3f att_rate_setpoint(lateral_offset_compensation * Kp_roll, (pitch_angle_desired - attitude_estimate.pitch) * Kp_pitch, heading_rate_command); ``` 这里实现了从欧拉角表示的姿态目标至角速度域的具体映射过程；其中涉及到四元数运算用于表征刚体转动关系，同时也考虑到了侧风等因素可能引起的附加扰动修正[^3]。 #### 推力分配策略 当完成内外双闭环调节之后，下一步就是依据所得总合力矩大小合理安排各执行机构的工作强度。对于常规布局类型的多旋翼平台而言，通常会采用如下方式来进行动力学解耦和平滑过渡： ```cpp for (int i = 0; i < NUM_ACTUATORS; ++i){ float thrust_i = base_thrust + mix_factor[i][ROLL]*moment_x + mix_factor[i][PITCH]*moment_y+ mix_factor[i[YAW]]*moment_z; // 防止出现异常情况导致失控现象的发生 if (!isfinite(thrust_i)){ thrust_i = 0.f; } } ``` 这段伪码描述了按照预定义好的混合系数矩阵 `mix_factor[][]` 对整体作用效果按需拆分成单个螺旋桨应承担的任务份额，并且加入了必要的数值检验措施以保障安全性[^4]。