跳转至

状态估计、控制器与执行器链路

状态估计器回答“飞行器现在怎样运动”，控制器计算“需要什么力和力矩”，控制分配再回答“每个电机或舵机应输出多少”。这三层的数据不能跳过单位、坐标、时间和有效性检查直接相连。

状态估计到执行器输出链路

状态估计器的输出不是传感器原始值

估计器可能融合：

  • 陀螺仪；
  • 加速度计；
  • 磁力计；
  • 气压计；
  • 全球导航卫星系统；
  • 光流；
  • 测距；
  • 空速；
  • 外部视觉或里程计。

它输出的姿态、角速度、速度、位置、偏差和不确定度，通常已经经过坐标变换、模型预测和多源校正。

上层模块应同时检查：

  • 状态是否有效；
  • 数据时间戳和年龄；
  • 坐标系；
  • 单位；
  • 原点和高度基准；
  • 估计器实例；
  • 创新、一致性或健康标志；
  • 发生重置时的跳变量。

目标值来自哪里

目标值可能来自：

  • 遥控器；
  • 姿态或速度模式；
  • 位置控制器；
  • 任务规划器；
  • 伴随计算机；
  • 安全或失效策略。

模式切换不仅是更换一个目标来源，还可能需要：

  • 重置积分状态；
  • 对齐当前目标；
  • 限制目标变化率；
  • 处理坐标系转换；
  • 确认新模式所需状态有效；
  • 避免输出阶跃。

串级控制

多旋翼常使用多层控制：

位置目标
→ 速度目标
→ 姿态与推力目标
→ 角速度目标
→ 力矩需求
→ 执行器命令

并非所有模式都会使用全部层级。人工角速度模式可能直接产生角速度目标；位置模式则会经过更多外环。

内环通常需要更高更新率和更低延迟。外环频率较低，不代表它可以使用过期或坐标错误的数据。

控制器接口必须完整

控制器输入至少需要：

  • 目标值；
  • 测量或估计值；
  • 时间步长；
  • 有效状态；
  • 允许输出范围；
  • 模式和复位条件。

输出至少需要：

  • 控制量；
  • 饱和状态；
  • 限幅原因；
  • 控制器健康状态；
  • 可用于日志的中间量。

只传递一个浮点数组会隐藏单位、坐标和有效性，增加误接风险。

时间步长不能默认恒定

积分和微分计算都依赖时间步长dt。若调度发生抖动，固定使用名义周期可能造成误差。

常见处理方式包括：

  • 从单调时间戳计算实际dt；
  • 对异常过小或过大的dt设置边界；
  • 在长时间暂停或模式切换后重置状态；
  • 记录周期和截止期限违约；
  • 不允许负值、零值或非数值进入控制器。

具体策略必须与算法设计一致，不能简单地把任何异常dt截成一个固定值后继续飞行。

饱和与抗积分饱和

执行器输出存在上下限。当期望力矩超过能力时：

  • 某些电机达到最大或最小；
  • 控制分配无法同时满足全部轴；
  • 积分项可能继续累积；
  • 恢复后可能产生明显过冲。

抗积分饱和（Anti-Windup）用于在输出受限时约束或回退积分状态。它必须知道实际饱和方向和控制分配结果，不能只对积分值设置一个孤立上限。

控制分配不是简单加减

控制分配需要考虑：

  • 电机或舵机几何位置；
  • 推力方向；
  • 旋转方向和反扭矩；
  • 执行器最小、最大和失效值；
  • 不同控制轴优先级；
  • 非线性推力模型；
  • 执行器失效或禁用；
  • 倾转、舵面或其他可变构型。

四旋翼的固定混控矩阵只是最简单情况。更换电机编号、构型、旋向或输出协议后必须重新验证。

从归一化命令到物理输出

执行器驱动可能接收归一化命令、转速目标、推力目标或脉宽值。随后还可能经过：

  • 最小和最大限制；
  • 解锁状态门控；
  • 失效值；
  • 电机停止或怠速逻辑；
  • 电池电压补偿；
  • 非线性映射；
  • 协议编码；
  • 通道重排；
  • 输出更新和遥测。

必须区分“控制器输出”“分配后命令”和“最终写入硬件的值”。日志只记录其中一层时，不能据此推断其他层完全一致。

安全状态优先于普通目标

以下状态可能覆盖正常控制命令：

  • 未解锁；
  • 解锁前检查失败；
  • 遥控或数据链失联；
  • 状态估计无效；
  • 电池或电源故障；
  • 执行器故障；
  • 看门狗和系统错误；
  • 上锁/解除动力；
  • 停桨或电机停止；
  • 飞行终止。

这些状态的优先级、锁存和恢复条件必须明确。不能新增一个控制模式后绕过现有安全状态机。

PX4 v1.17.0的多旋翼链路示例

以下对象可以把抽象框图落到真实源码。名称和关系只对固定的v1.17.0基线负责：

阶段 代表模块或主题 追踪重点
IMU整理 vehicle_imu主题 积分后的角增量、速度增量和采样时间
状态估计 src/modules/ekf2 订阅vehicle_imu，发布vehicle_attitude等状态
姿态控制 src/modules/mc_att_control 姿态目标与实际姿态生成vehicle_rates_setpoint
角速度控制 src/modules/mc_rate_control 角速度与目标生成vehicle_torque_setpoint和vehicle_thrust_setpoint
控制分配 src/modules/control_allocator 把力和力矩目标分配到执行器，发布actuator_motors
输出驱动 目标板启用的输出模块与驱动 把actuator_motors映射到真实通道和协议

在源码根目录执行：

rg "vehicle_imu" src/modules/ekf2
rg "vehicle_rates_setpoint" src/modules/mc_att_control src/modules/mc_rate_control
rg "vehicle_torque_setpoint|vehicle_thrust_setpoint" \
  src/modules/mc_rate_control src/modules/control_allocator
rg "actuator_motors" src/modules/control_allocator src/drivers

每次搜索都要继续回答三个问题：

  1. 谁发布、谁订阅；
  2. timestamp和timestamp_sample分别表示什么；
  3. 哪个板级配置和运行状态真正启用了该模块。

不要把这张表理解为函数直接逐层调用。PX4主要通过uORB主题连接模块，调度时刻和数据年龄必须单独检查。

修改控制链前的最小证据

  • 输入和输出单位明确；
  • 坐标系和旋转方向明确；
  • 时间戳来源和最大样本年龄明确；
  • 模式进入、退出和状态重置明确；
  • 输出限幅和饱和反馈明确；
  • 非数值、无穷大和越界输入被处理；
  • 执行器断开、失联和估计失效行为明确；
  • 仿真覆盖静止、阶跃、扰动和饱和；
  • 基线与修改版使用相同测试场景比较；
  • 首次硬件验证拆除螺旋桨。

检查理解

  1. 状态估计输出为什么不能只看数值而忽略时间和有效标志？
  2. 模式切换时为什么可能需要重置控制器状态？
  3. 固定使用名义dt有什么风险？
  4. 抗积分饱和为什么需要知道实际输出限制？
  5. 为什么控制器输出不一定等于最终电机命令？

主要参考