PX4 mc_pos_control 总览与控制链路

发表于 更新于 分类于 ， ， 阅读次数： 本文字数： 766 阅读时长 ≈ 3 分钟
全面梳理 PX4 多旋翼位置控制模块 mc_pos_control 的整体架构、上下游接口关系，以及模块内部从 trajectory_setpoint 到 vehicle_attitude_setpoint 的完整串级控制链路。

一、mc_pos_control 的定位

src/modules/mc_pos_control 是多旋翼位置/速度串级控制器，负责将上游的轨迹设定点（trajectory_setpoint）转换为姿态控制器可执行的姿态设定点（vehicle_attitude_setpoint）。

它不包含轨迹生成逻辑，轨迹由上游 flight_mode_manager 中的各 FlightTask 生成。

相关上游：Navigator 主循环与模式调度

二、全局控制链路

1
2
3
4
5
6
7
8
9
10
11
12
flowchart LR
    NAV["Navigator<br/>position_setpoint_triplet"] --> FMM["flight_mode_manager<br/>FlightTaskAuto<br/>triplet → trajectory_setpoint"]
    MANUAL["手动模式<br/>FlightTaskPosition/Altitude"] -->|trajectory_setpoint<br/>vehicle_constraints| MPC
    OFFBOARD["Offboard<br/>mavlink_receiver"] -->|trajectory_setpoint| MPC
    GOTO["goto_setpoint"] -->|goto_setpoint| MPC
    FMM -->|trajectory_setpoint<br/>vehicle_constraints| MPC["mc_pos_control"]
    EKF["EKF2<br/>vehicle_local_position"] --> MPC
    MODE["vehicle_control_mode"] --> MPC
    HTE["hover_thrust_estimate"] --> MPC
    MPC -->|vehicle_attitude_setpoint| ATT["mc_att_control"]
    MPC -->|vehicle_local_position_setpoint| Land["land_detector / 其他"]
    MPC -->|takeoff_status| FMM2["flight_mode_manager<br/>任务切换逻辑"]

关键链路说明：

  • 自动模式：Navigator → FlightTaskAuto → mc_pos_control
  • 手动位置模式：FlightTaskPosition → mc_pos_control（不经过 Navigator）
  • Offboard 模式：mavlink_receiver 直接发布 trajectory_setpoint，绕过 Navigator

三、模块内部层级

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
flowchart TD
    subgraph MPC["mc_pos_control"]
        Top["MulticopterPositionControl.cpp<br/>顶层调度与集成层"]
        Filter["set_vehicle_states<br/>状态预处理（陷波+低通滤波）"]
        EKFReset["adjustSetpointForEKFResets<br/>EKF重置鲁棒补偿"]
        Takeoff["TakeoffHandling<br/>起飞状态机与推力斜坡"]
        Goto["GotoControl<br/>goto目标平滑桥接层"]
        Core["PositionControl.cpp<br/>核心串级控制算法<br/>位置P + 速度PID + 推力映射"]
        Math["ControlMath.cpp<br/>几何映射与向量约束"]
        Failsafe["generateFailsafeSetpoint<br/>故障回退层"]
    end
    Top --> Filter
    Top --> EKFReset
    Top --> Takeoff
    Top --> Goto
    Top --> Core
    Core --> Math
    Top --> Failsafe
    Failsafe --> Core

四、目录结构

文件 功能
MulticopterPositionControl.cpp/hpp 主循环：状态预处理、EKF 重置补偿、起飞处理、调用核心控制器、发布输出
PositionControl/PositionControl.cpp/hpp 核心算法：位置 P 环 + 速度 PID 环 + 推力映射
PositionControl/ControlMath.cpp/hpp 几何工具：推力向量→姿态四元数、限倾角、XY 速度约束
Takeoff/Takeoff.cpp/hpp 起飞状态机：disarmed → spoolup → ready → rampup → flight
GotoControl/GotoControl.cpp/hpp goto 平滑桥接：goto_setpoint → 带约束的 trajectory_setpoint
*_params.c 系列 参数定义（增益、限速、推力、滤波、起降等）

五、输入输出接口

5.1 订阅话题

话题 关键字段 用途
vehicle_local_position x/y/z, vx/vy/vz, reset_counter EKF 位置速度反馈
trajectory_setpoint position[3], velocity[3], acceleration[3], yaw 上游轨迹指令
vehicle_constraints speed_up, speed_down, want_takeoff 任务物理约束
vehicle_control_mode flag_multicopter_position_control_enabled, flag_armed 模式使能标志
vehicle_land_detected landed, ground_contact 着地状态
hover_thrust_estimate hover_thrust, valid 自适应悬停推力
goto_setpoint position[3], heading, max_*_speed Goto 目标

5.2 发布话题

话题 下游消费者 关键字段
vehicle_attitude_setpoint mc_att_control q_d（目标姿态四元数）、thrust_body[3]
vehicle_local_position_setpoint land_detector, 状态机 完整的 pos/vel/acc/thrust 设定值
takeoff_status flight_mode_manager 起飞阶段标志

六、串级控制结构概览

1
2
3
4
5
6
7
8
9
10
position_setpoint (pos_sp)
        ↓ 位置 P 环
velocity_setpoint (vel_sp)  +  前馈速度
        ↓ 速度 PID 环（P + I + D*vel_dot）
acceleration_setpoint (acc_sp)  +  前馈加速度
        ↓ 加速度到推力/姿态映射 (_accelerationControl)
body_z 方向（归一化推力方向）→ 姿态四元数 q_d
collective_thrust → thrust_body
        ↓ 推力分配与优先级保证（垂直优先、抗积分饱和）
vehicle_attitude_setpoint

设计关键：

  • 位置环只有 P（比例），无积分（位置误差长时间积累由速度 I 项补偿）
  • 速度环为完整 PID（P 项对速度误差、I 项消除稳态误差、D 项用滤波后加速度而非速度微分）
  • 推力方向（body_z）决定姿态，推力大小（collective_thrust）决定油门

七、参数文件说明

mc_pos_control 的参数分散在多个文件中，分类清晰：

参数文件 主要参数 作用域
gain_params.c MPC_XY_P, MPC_XY_VEL_P_ACC/I/D, MPC_Z_P, MPC_Z_VEL_P/I/D 位置/速度 PID 增益
limits_params.c MPC_XY_VEL_MAX, MPC_Z_VEL_MAX_UP/DN, MPC_THR_MIN/MAX, MPC_TILTMAX_AIR/LND 物理约束边界
params.c MPC_THR_HOVER, MPC_USE_HTE, MPC_VEL_LP/NF 悬停推力、滤波器
takeoff_land_params.c MPC_TKO_RAMP_T, MPC_TKO_SPEED, MPC_LAND_SPEED 起降参数
autonomous_params.c MPC_XY_CRUISE, MPC_ACC_HOR, MPC_JERK_AUTO 自动任务轨迹约束
responsiveness_params.c MPC_RESPONSIVENESS 高层响应性参数（派生底层增益）

八、小结

mc_pos_control 的核心设计原则：

  1. 关注点分离：轨迹生成在 FlightTask，控制执行在 mc_pos_control，两层职责明确
  2. 鲁棒性优先：EKF 重置补偿、起飞斜坡保护、故障回退层保证各种异常情况下的安全
  3. 串级解耦：位置→速度→加速度→推力/姿态的串级结构，各环独立调参
  4. 自适应悬停：HTE（悬停推力估计器）实时补偿电池电压下降或载荷变化

下一篇：MulticopterPositionControl 主循环与数据清洗
系列总目录：PX4 v1.16 源码解读总目录