PX4 控制器架构（一）：从位置目标点到力矩输出

当你用摇杆推了一把无人机，或者发送了一个 goto 指令，PX4 内部发生了什么？这个指令不是"直接飞到那里"------它要经历多层串级变换、十余个 uORB topic 的传递、三条不同频率的管道，最终才变成四个电机的 PWM 信号。

本系列文章专注 controller 部分------从位置目标点生成到力矩/推力输出。控制分配（Control Allocation）和电机混控将在后续系列中单独展开。本文是系列开篇，目标是建立一张清晰的架构地图：让你知道"一个位置指令是如何逐层降级为力矩输出的"，以及每层控制器在什么频率下运行、依赖什么数据、输出什么结果。

本文的核心重点是 PX4 基于 SO(3) 的姿态控制范式 ------与传统欧拉角控制不同，PX4 的位置-速度控制器输出的不是 roll/pitch/yaw 角度，而是期望加速度 ，再从期望加速度的方向中提取目标姿态。推力方向（tilt）与偏航（yaw）在这一过程中被严格解耦，这正是 SO(3) 几何控制在 PX4 中的工程落地。

1. 控制链路全景

PX4 多旋翼控制器采用经典的串级结构（cascade control），但与传统航模"位置环输出 roll/pitch 角度"的直觉不同，PX4 从位置到姿态的转换有一个关键的中间步骤------期望加速度。这是理解整个 SO(3) 控制范式的钥匙。

1.1 从外到内的完整链路

复制代码

用户指令/航线
    │
    ▼
┌─────────────────────────────────────────────────────────────────────────┐
│  FlightTask (flight_mode_manager)                                       │
│  输入：用户摇杆/航线点/外部指令                                           │
│  输出：trajectory_setpoint (位置/速度/加速度/偏航)                        │
└─────────────────────────────────────────────────────────────────────────┘
    │
    ▼
┌─────────────────────────────────────────────────────────────────────────┐
│  mc_pos_control (PositionControl)                                       │
│  ├─ 位置环：位置误差 → 速度设定点 (P控制)                                 │
│  ├─ 速度环：速度误差 → 期望加速度 (PID控制)                               │
│  │                                                                  │
│  │  【SO(3) 控制核心环节】                                            │
│  │  期望加速度 a_sp = [a_x, a_y, a_z]^T (NED，z朝下为正)              │
│  │       │                                                           │
│  │       ▼                                                           │
│  │  重力补偿：a_cmd = a_sp - g (g = [0,0,g]^T)                        │
│  │       │                                                           │
│  │       ▼                                                           │
│  │  推力方向（朝上）n_T = a_cmd / ||a_cmd||                            │
│  │       │                                                           │
│  │       ▼                                                           │
│  │  机体 z 轴（朝下）b3 = -n_T = (g-a_sp)/||g-a_sp||                  │
│  │       │                                                           │
│  │       ▼                                                           │
│  │  结合 yaw 设定点 → 构造完整旋转矩阵 R_d ∈ SO(3)                     │
│  │       │                                                           │
│  │       ▼                                                           │
│  │  四元数 q_d (目标姿态)                                              │
│  │       +                                                            │
│  │  推力大小 T = m * ||a_cmd|| / cos(theta)                           │
│  │  （theta 为倾角，受 _lim_tilt 限制；若 a_cmd 水平分量过大，           │
│  │   body_z 会被截断到 _lim_tilt，推力需除以 cos(theta) 补偿）           │
│  │                                                                  │
│  输出：vehicle_attitude_setpoint (q_d + thrust_body)                    │
└─────────────────────────────────────────────────────────────────────────┘
    │
    ▼
┌─────────────────────────────────────────────────────────────────────────┐
│  mc_att_control (AttitudeControl)                                       │
│  输入：当前姿态 q + 目标姿态 q_d                                         │
│  计算：误差四元数 q_e = q^{-1} ⊗ q_d                                     │
│  输出：vehicle_rates_setpoint (角速度设定点 omega_sp)                    │
└─────────────────────────────────────────────────────────────────────────┘
    │
    ▼
┌─────────────────────────────────────────────────────────────────────────┐
│  mc_rate_control (RateControl)                                          │
│  输入：当前角速度 omega + 目标角速度 omega_sp                             │
│  控制：PID (P + I + D + FF)                                             │
│  输出：vehicle_thrust_setpoint + vehicle_torque_setpoint                 │
└─────────────────────────────────────────────────────────────────────────┘
    │
    ▼
┌─────────────────────────────────────────────────────────────────────────┐
│  control_allocator (下一系列专题)                                        │
│  输入：推力向量 + 力矩向量                                               │
│  输出：actuator_motors (各电机归一化转速)                                │
└─────────────────────────────────────────────────────────────────────────┘

1.2 链路表格

层级	模块	输入	中间量	输出	物理意义
设定点生成	FlightTask	用户指令/航线	---	trajectory_setpoint	"我想去哪里、以什么速度"
位置环	mc_pos_control	当前位置 + 目标位置	速度设定点	---	"当前位置差多少，该以什么速度去追"
速度环	mc_pos_control	当前速度 + 速度设定点	期望加速度 a_sp	---	"当前速度差多少，该以什么加速度去追"
SO(3)姿态生成	mc_pos_control	期望加速度 a_sp + 偏航设定	推力方向 n_T + 目标旋转矩阵 R_d	vehicle_attitude_setpoint (q_d + T)	"为了产生这个加速度，机体该指向哪里、输出多大推力"
姿态跟踪	mc_att_control	当前姿态 q + 目标姿态 q_d	误差四元数 q_e	vehicle_rates_setpoint	"为了转到目标姿态，该以什么角速度旋转"
角速度跟踪	mc_rate_control	当前角速度 + 目标角速度	角速度误差	thrust + torque	"为了产生目标角速度，需要多大推力和力矩"

1.3 为什么强调"期望加速度 → 目标四元数"？

这是 PX4 与传统多旋翼控制器的本质区别：

传统欧拉角控制器：

位置环输出 -> roll 角度 + pitch 角度 -> 姿态环跟踪这些角度

问题：

roll 和 pitch 是耦合的------同时改变两者时，推力方向的变化不是直观的；
万向锁导致大机动时控制失效；
偏航与 tilt 的耦合难以处理。

PX4 的 SO(3) 控制器：

速度环输出 -> 期望加速度向量 -> 推力方向（tilt）-> 结合偏航构造完整四元数

优势：

方向是原生物理量：加速度的方向就是推力的反方向，这是三维空间中一个无歧义的单位向量；
tilt/yaw 严格解耦：先确定推力方向（决定 roll/pitch），再叠加偏航旋转，两者在 SO(3) 上是独立操作；
全局无奇异性：四元数表示在整个 SO(3) 流形上光滑，没有万向锁。

源码对应：src/modules/mc_pos_control/PositionControl.cpp 中的 update() 函数最终调用 ControlMath::thrustToAttitude() 完成这一转换。

2. uORB Topic 数据流

PX4 模块间通过 uORB（micro Object Request Broker）消息总线通信。控制链路涉及的关键 topic 如下：

2.1 trajectory_setpoint：外层设定点

trajectory_setpoint 是整个控制链路的起点。它由 flight_mode_manager 中的各种 FlightTask 生成：

ManualPosition：摇杆映射为速度/位置设定点
Auto：航线追踪，生成平滑轨迹
Orbit：绕圈飞行
Offboard：外部系统（ROS/MAVROS）直接注入

关键字段：

cpp 复制代码

struct trajectory_setpoint_s {
    float position[3];      // NED 坐标系目标位置 [m]
    float velocity[3];      // NED 坐标系目标速度 [m/s]
    float acceleration[3];  // NED 坐标系目标加速度 [m/s^2]
    float yaw;              // 目标偏航角 [rad]
    float yawspeed;         // 目标偏航速率 [rad/s]
};

注意：这些字段不必全部有效。mc_pos_control 会根据当前模式和控制需求，选择使用哪些字段。例如手动模式主要用 velocity，自动模式主要用 position。

2.2 vehicle_attitude_setpoint：从加速度到姿态的桥梁

这是位置控制层的输出、姿态控制层的输入。它的诞生过程就是 SO(3) 控制的核心：

cpp 复制代码

struct vehicle_attitude_setpoint_s {
    float q_d[4];           // 目标姿态四元数 [w, x, y, z]
    float thrust_body[3];   // 机体系推力向量 [N]（通常只有 z 分量非零）
    float yaw_sp_move_rate; // 偏航设定点变化率 [rad/s]
    float roll_body;        // 目标 roll（欧拉角，供日志/显示用）
    float pitch_body;       // 目标 pitch（欧拉角，供日志/显示用）
    float yaw_body;         // 目标 yaw（欧拉角，供日志/显示用）
};

q_d 的构造过程（SO(3) 控制的核心）：

速度环输出期望加速度 ：asp∈R3\mathbf{a}_{sp} \in \mathbb{R}^3asp∈R3（NED 坐标系，z 朝下为正）
计算重力补偿后的净加速度 ：acmd=asp−g\mathbf{a}{cmd} = \mathbf{a}{sp} - \mathbf{g}acmd=asp−g（g= $0,0,g$ T\mathbf{g} = $0, 0, g$ ^Tg= $0,0,g$ T）
推力方向（朝上） ：nT=acmd/∣∣acmd∣∣\mathbf{n}T = \mathbf{a}{cmd} / ||\mathbf{a}_{cmd}||nT=acmd/∣∣acmd∣∣
机体 z 轴方向（朝下，与推力反向） ：b3=−nT=(g−asp)/∣∣g−asp∣∣\mathbf{b}3 = -\mathbf{n}T = (\mathbf{g} - \mathbf{a}{sp}) / ||\mathbf{g} - \mathbf{a}{sp}||b3=−nT=(g−asp)/∣∣g−asp∣∣
- 这是机体 z 轴在 NED 坐标系中的期望方向
- 负号是因为推力向上（NED z 轴向下），而加速度指令向下
倾角限制 ：若 asp\mathbf{a}_{sp}asp 水平分量过大导致 tilt 角超过 _lim_tilt，则将 b3\mathbf{b}3b3 截断到 _lim_tilt 边界；此时为维持 NED z 方向的加速度，推力需按 T=Tnominal/cos⁡θT = T{nominal} / \cos\thetaT=Tnominal/cosθ 补偿（θ\thetaθ 为实际倾角）
构造正交基 ：给定 b3\mathbf{b}_3b3 和偏航角 ψ\psiψ，构造完整的旋转矩阵 RdR_dRd
- RdR_dRd 的第一列 b1\mathbf{b}_1b1：由偏航决定的前向方向投影到垂直于 b3\mathbf{b}_3b3 的平面
- RdR_dRd 的第二列 b2=b3×b1\mathbf{b}_2 = \mathbf{b}_3 \times \mathbf{b}_1b2=b3×b1
旋转矩阵转四元数 ：qd=RotMatToQuat(Rd)q_d = \text{RotMatToQuat}(R_d)qd=RotMatToQuat(Rd)

为什么 roll_body / pitch_body 只是"显示用"？

控制律本身完全基于四元数运算；
欧拉角字段仅用于日志记录、地面站显示、以及极少数兼容代码；
真正的控制误差是 qe=q−1⊗qdq_e = q^{-1} \otimes q_dqe=q−1⊗qd，不是 roll/pitch/yaw 的差值。

2.3 vehicle_rates_setpoint：从姿态到角速度

姿态控制层输出角速度设定点，供速率环跟踪：

cpp 复制代码

struct vehicle_rates_setpoint_s {
    float roll;     // 绕机体 x 轴角速度 [rad/s]
    float pitch;    // 绕机体 y 轴角速度 [rad/s]
    float yaw;      // 绕机体 z 轴角速度 [rad/s]
    float thrust_body[3];  // 推力向量透传
};

2.4 vehicle_thrust_setpoint + vehicle_torque_setpoint：力矩输出

速率控制层的最终输出是两个 3D 向量：

cpp 复制代码

struct vehicle_thrust_setpoint_s {
    float xyz[3];   // NED 坐标系推力向量 [N]
};

struct vehicle_torque_setpoint_s {
    float xyz[3];   // 机体系力矩向量 [N*m]
};

这是抽象控制量（virtual control），与具体机型无关。四旋翼、六旋翼、倾转旋翼都输出同样的 thrust + torque 向量，由下游的 control_allocator 解算为各电机转速。

3. 频率链路：谁跑多快？

控制系统的性能很大程度上取决于内外环的频率配比。PX4 采用事件驱动的工作队列（Work Queue），各模块由数据更新触发，而非固定周期轮询。

3.1 各层典型频率

模块	触发源	设计频率	工作队列	优先级
flight_mode_manager	混合（定时 + 事件）	10--50 Hz	hp_default	中
mc_pos_control（位置+速度环）	vehicle_local_position	50 Hz	nav_and_controllers	高
mc_att_control（姿态环）	vehicle_attitude	250 Hz	nav_and_controllers	高
mc_rate_control（角速度环）	vehicle_angular_velocity	1 kHz	rate_ctrl	最高

关键设计------串级带宽分离：

位置环（50 Hz）→ 姿态环（250 Hz）：5 倍频率比（1/5），姿态环带宽是位置环的 5 倍；
姿态环（250 Hz）→ 速率环（1 kHz）：4 倍频率比（1/4），速率环带宽是姿态环的 4 倍；
这种 4~5 倍的级联带宽分离是串级控制的工程铁律------内环必须足够快，才能在外环更新前完成跟踪，否则外环看到的将是"响应迟钝的内环"，导致振荡或失稳；
rate_ctrl 工作队列的优先级高于 nav_and_controllers，确保角速度环不会被位置/姿态环饿死。

3.2 IMU 的两条管道

IMU 数据从驱动层出来后，分两条独立路径进入不同消费者：

估计路径（左侧）：

sensor_gyro -> vehicle_imu（积分降采样，默认 200Hz）-> EKF2（100Hz）-> vehicle_attitude / vehicle_local_position
特点：低通滤波 + 延迟补偿，用于状态估计

控制路径（中间）：

sensor_gyro -> vehicle_angular_velocity（Butterworth 2 阶低通，截止频率 IMU_GYRO_CUTOFF，默认 40Hz）-> mc_rate_control（设计 1 kHz，实际默认 400 Hz，可通过 IMU_GYRO_RATEMAX 配置）
特点：低延迟、带陷波滤波（notch filter），用于实时控制

关键区别：

控制路径的陀螺仪数据不过 EKF2，直接从传感器驱动层经滤波后进入 rate control；
这保证了 rate control 的延迟最小（约 2-3ms），而 EKF2 的 attitude 输出延迟约 10ms（100Hz 周期 + 融合延迟）。

4. 模块拓扑详解

4.1 mc_pos_control：位置-速度-加速度三环 + SO(3) 姿态生成

mc_pos_control 不是单一的控制器，而是多个子模块的集合：

复制代码

mc_pos_control
├── PositionControl          // 核心 P-PID 串级 + SO(3) 姿态生成
│   ├── 位置环：位置误差 -> 速度设定点
│   ├── 速度环：速度误差 -> 期望加速度
│   └── thrustToAttitude()：期望加速度 -> q_d + T   【SO(3) 核心】
├── GotoControl              // 轨迹平滑与约束
└── Takeoff                  // 起飞状态机

PositionControl 是算法核心：

cpp 复制代码

// 伪代码示意（对应 PositionControl::_accelerationControl() + thrustToAttitude()）
void PositionControl::update() {
    // 1. 位置环（P 控制）
    vel_sp = K_p_pos * (pos_sp - pos);
    
    // 2. 速度环（PID 控制）
    acc_sp = K_p_vel * (vel_sp - vel) + K_i_vel * integral + K_d_vel * derivative;
    
    // 3. 【SO(3) 控制核心】期望加速度 -> 目标姿态 + 推力
    // _accelerationControl() + thrustToAttitude() 的实现：
    Vector3f a_cmd = acc_sp - g;              // 重力补偿后的净加速度
    Vector3f b3 = -(a_cmd).normalized();      // 机体 z 轴方向（朝下，与推力反向）
    
    // 倾角限制：若水平机动要求过大 tilt，截断到 _lim_tilt
    limitTilt(b3, Vector3f(0, 0, 1), _lim_tilt);
    
    // 推力大小：T = m * ||a_cmd|| / cos(theta)
    // 其中 theta 为 b3 与 NED z 轴的夹角；源码中通过 _hover_thrust 标定将加速度映射为推力
    float cos_tilt = b3(2);                   // dot(b3, NED_z) = cos(theta)
    float thrust_magnitude = (a_cmd.length() * mass) / cos_tilt;
    
    // 结合偏航角构造完整旋转矩阵
    Vector3f b1 = computeHeading(yaw_sp, b3); // 偏航方向投影到垂直于 b3 的平面
    Vector3f b2 = b3.cross(b1);                // 右手系正交补
    Matrix3f R_d;                              // 目标旋转矩阵 R_d = [b1, b2, b3]
    R_d.col(0) = b1;
    R_d.col(1) = b2;
    R_d.col(2) = b3;
    
    q_d = R_d.toQuaternion();                  // 旋转矩阵转四元数
    
    // NED 坐标系推力向量：方向沿 -b3（朝上），大小为 thrust_magnitude
    // 源码中 _thr_sp = b3 * collective_thrust，其中 collective_thrust 为负数
    thrust_vector = -b3 * thrust_magnitude;
}

关键点：

位置环和速度环输出的是物理量（速度、加速度），不是角度；
只有在最后一步，才通过几何关系将加速度方向映射为姿态------这是 SO(3) 控制的标志性特征；
roll 和 pitch 从未作为独立控制量出现，它们是推力方向的"副产品"。

GotoControl 负责设定点平滑：

限制 jerk、加速度、速度
保证轨迹连续性（position/velocity/acceleration 三阶连续）
处理航向平滑

Takeoff 是起飞状态机：

idle -> rampup（推力斜坡）-> flight（正常飞行）
避免起飞瞬间的推力跳变

4.2 mc_att_control：四元数姿态控制 + 手动模式映射

mc_att_control 包含两部分：

AttitudeControl：核心四元数 P 控制（Grob 论文实现）
- 输入：当前姿态四元数 q + 目标姿态四元数 q_d
- 计算误差四元数 q_e = q^{-1} * q_d
- 输出：omega_sp = 2 * K_p * imag(q_e_canonical)
- 核心创新：tilt/yaw 解耦------优先保证推力方向（roll/pitch）准确，再尽量跟踪偏航
手动模式下的特殊处理：
- 摇杆输入不直接指定目标姿态，而是经过 generate_attitude_setpoint() 映射；
- roll/pitch 摇杆控制的是倾斜角大小和方向（而非独立角度）；
- 油门曲线非线性映射（throttle_curve()）。

4.3 mc_rate_control：PID 角速度控制

mc_rate_control 的核心是 RateControl 类：

P 项：比例增益
I 项：积分（带抗饱和）
D 项：微分（带低通滤波）
FF 项：前馈增益

关键设计：

PID 参数以平行形式（parallel form）存储：P + I/s + sD；
积分器带限幅（setIntegratorLimit），防止饱和 windup；
D 项经过低通滤波，抑制高频噪声；
输出为归一化力矩（ $-1, 1$ ），下游 control_allocator 负责映射到实际电机。

5. SO(3) 控制的本质：为什么 PX4 不直接输出 roll/pitch？

理解了"期望加速度 -> 目标四元数"的转换后，我们可以回答一个根本问题：

5.1 传统欧拉角控制的缺陷

传统控制器（如很多开源飞控的早期版本）的链路是：

复制代码

位置误差 -> [P] -> roll_sp, pitch_sp -> 姿态环 -> 电机

问题：

roll 和 pitch 不是独立控制自由度：多旋翼的物理控制量是推力方向（一个单位向量），不是两个独立角度。强行将推力方向拆成 roll/pitch 引入了人为的耦合；
万向锁：当 pitch 接近 90 度时，roll 和 yaw 的区分失效，控制器可能输出奇异指令；
偏航与 tilt 的耦合：改变 yaw 时，如果姿态用欧拉角表示，roll/pitch 的物理含义会随偏航旋转而改变，控制增益需要重新解释。

5.2 PX4 的 SO(3) 控制范式

PX4 的链路是：

复制代码

速度误差 -> [PID] -> 期望加速度 a_sp
    -> 重力补偿 a_cmd = a_sp - g
    -> 机体 z 轴 b3 = (g - a_sp) / ||g - a_sp||  (朝下)
    -> 结合偏航 -> 目标旋转矩阵 R_d
    -> 四元数 q_d

优势：

方向是物理原生量 ：b3\mathbf{b}_3b3 是三维空间中一个无歧义的单位向量，直接对应"推力该指向哪里"；
全局无奇异性：四元数在整个 SO(3) 流形上光滑表示，没有万向锁；
tilt/yaw 严格解耦：先构造推力方向（决定 b3），再绕 b3 旋转偏航角，两个操作在 SO(3) 上可交换且独立；
与 Lee 理论一致：Lee 2010 的 SE(3) 控制器中，姿态指令正是通过"期望推力方向"构造的，PX4 继承了这一核心思想。

5.3 与 Lee 2010 论文的对应

Lee 论文中的姿态指令构造（简化版）：

给定期望位置轨迹 pd(t)p_d(t)pd(t)，位置控制律输出期望加速度 ad\mathbf{a}dad。考虑重力补偿后的净加速度为 acmd=ad−g\mathbf{a}{cmd} = \mathbf{a}d - \mathbf{g}acmd=ad−g。定义期望机体 z 轴（朝下）为 b3d=−acmd/∣∣acmd∣∣=(g−ad)/∣∣g−ad∣∣\mathbf{b}{3d} = -\mathbf{a}{cmd} / ||\mathbf{a}{cmd}|| = (\mathbf{g} - \mathbf{a}_d) / ||\mathbf{g} - \mathbf{a}_d||b3d=−acmd/∣∣acmd∣∣=(g−ad)/∣∣g−ad∣∣。结合期望偏航 ψd\psi_dψd，构造完整的期望旋转矩阵 Rd= $b1d,b2d,b3d$ R_d = $\\mathbf{b}_{1d}, \\mathbf{b}_{2d}, \\mathbf{b}_{3d}$ Rd= $b1d,b2d,b3d$ 。

PX4 的 ControlMath::thrustToAttitude() 几乎就是这一过程的工程实现，只是用四元数代替旋转矩阵存储，避免存储冗余。

6. 状态估计在控制链路中的位置

EKF2（或 attitude_estimator_q）不是控制器的一部分，但它是所有控制层的输入源。

vehicle_attitude (四元数 + 角速度) -> mc_att_control
vehicle_local_position (位置 + 速度) -> mc_pos_control
vehicle_angular_velocity (滤波后角速度) -> mc_rate_control (主用)
vehicle_acceleration (机体加速度) -> mc_pos_control

重要事实：

mc_rate_control 主要使用 vehicle_angular_velocity（来自 sensors 模块的滤波陀螺仪），而非 EKF2 输出的角速度；
mc_att_control 使用 vehicle_attitude（EKF2 100Hz 输出），但姿态环的带宽远低于速率环，100Hz 足够；
mc_pos_control 使用 vehicle_local_position（EKF2 100Hz），位置环带宽更低，50-100Hz 足够。

为什么 rate control 不用 EKF2 的角速度？

EKF2 的角速度输出有额外延迟（预测-校正周期 + 协方差传播）；
Rate control 需要最低延迟，直接用最原始的滤波陀螺仪数据；
EKF2 的角速度在 EKF2 故障时可能不可用，而 vehicle_angular_velocity 始终可用。

7. 为什么不是"纯 SE(3) 控制"？

Lee 2010 的论文提出了一套在 SE(3) 上的完整几何跟踪控制律：需要动力学模型（质量、惯性矩阵）、直接控制力矩、Lyapunov 稳定性证明。PX4 的实现是这套理论的工程简化版。

特性	Lee 2010 理论框架	PX4 实际实现
动力学模型	需要完整的刚体动力学	不需要，假设角速度环为一阶惯性
控制输出	直接输出力矩 tau 和推力 T	姿态环输出角速度设定点，速率环输出归一化力矩
姿态表示	SO(3) 旋转矩阵或四元数	四元数
稳定性证明	Lyapunov 全局渐近稳定	无理论保证，依赖工程调参和串级带宽分离
计算复杂度	涉及矩阵指数、李括号	仅四元数乘法和向量运算，适合嵌入式

简化的核心假设：

角速度环足够快：假设 omega 可以瞬时跟踪 omega_sp，从而姿态环只需输出 omega_sp 而非直接力矩；
推力与姿态解耦：假设总推力沿机体 z 轴，通过调整姿态来改变推力方向；
小角度线性化：PID 控制律在本质上是对误差的小角度线性化。

这些假设在常规飞行（小倾斜角、非饱和状态）下成立，但在大机动、电机饱和、强风扰下会失效------这也是后续文章要讨论的边界条件。

8. 总结与系列预览

本文建立了 PX4 控制器架构的全景地图。核心要点：

四层串级：位置 -> 速度 -> SO(3)姿态生成 -> 姿态跟踪 -> 角速度 -> 力矩，每层都在做坐标变换；
SO(3) 控制的核心环节：速度环输出期望加速度，再通过 thrustToAttitude() 将加速度方向映射为目标四元数------这是 PX4 与传统欧拉角控制的本质区别；
uORB topic 数据流：trajectory_setpoint -> vehicle_attitude_setpoint -> vehicle_rates_setpoint -> vehicle_thrust/torque_setpoint；
频率由内向外递减：rate (1 kHz) → attitude (250 Hz) → position (50 Hz)，级联带宽比 4~5 倍；
IMU 双管道：控制路径低延迟直通 rate control，估计路径经 EKF2 后供外环使用；
理论根基 ：Lee 2010 的 SE(3) 几何控制（推力方向构造姿态）+ Grob 2016 的嵌入式四元数简化。
关于我们：
灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。
实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。