四旋翼飞行笔记：OCS2、姿态 PD 与统一控制器

这次在已有的 C++ / ROS2 / MuJoCo 控制环境中，完成了一次四旋翼闭环飞行：起飞、悬停、受到扰动后恢复、绕圈、返回，最后接地停桨。 本文记录它如何工作，以及怎样把无人机与机械臂、四足的控制代码组织在同一个 ros2_controller 包中。

最关键的一条链路是：位置控制决定需要怎样加速，姿态控制把机体转到合适的方向，推力分配决定四个旋翼分别出多少力，动力学产生新的状态，再把状态送回控制器。

1. 先看真实闭环的效果

MuJoCo 四旋翼依次起飞、悬停、受扰恢复、绕圆、返回并接地停桨，蓝线为实测轨迹，灰线为参考轨迹 — 32 秒仿真以 2 倍速回放，GIF 约 16 秒、1.8 MB。蓝线来自实际机体位置，灰线是参考；右侧显示实际旋翼推力。开启系统“减少动态效果”时显示静态关键帧。

需要看清姿态变化时，可以打开正常速度 MP4：32 秒完整飞行，或查看静态关键帧。

本次任务先升至 1.2 m。在仿真第 6 s，给机体施加一次世界坐标速度增量 (0.65, -0.35, 0.12) m/s，观察它偏离后如何恢复。随后飞行直径 1.6 m 的圆形航线，再返回中心并降落。

录像来自控制器运行时保存的完整 qpos/qvel/act。离线渲染读取这些状态并执行 mj_forward，没有重新计算一条“看起来更好”的轨迹。旋翼盘是静态可视几何，画面表达的运动是机体动力学与推力响应，不包含桨叶旋转细节。

2. 三层各自解决什么问题

四旋翼控制链路：位置 MPC、姿态 PD、推力分配与实际状态反馈

层次	输入与输出	本次实现
演示参考	时间 → 位置、速度、加速度参考	起飞、圆航线、返回和降落的解析轨迹
位置控制	实测位置/速度与参考 → 期望加速度	OCS2 MPC，另有传统 PD 对照
执行器控制	目标姿态/总推力与实测姿态/角速度 → 旋翼推力	几何姿态 PD、力矩分配、推力限幅
动力学环境	四个旋翼推力命令 → 新的机体状态	MuJoCo 刚体、重力、推力响应与接触

位置环每 20 ms 更新一次，即 50 Hz；姿态环和物理积分每 2 ms 一步，即 500 Hz。OCS2 内部的预测离散间隔是 40 ms。这三个时间尺度分别对应反馈更新、执行与预测，不能把 40 ms 当成实际物理积分步长。

位置预测采用简化的平动模型，姿态和旋翼动力学在内环及 MuJoCo 中展开。这个分工使第一版能清楚地验证每层接口；同时也意味着预测模型没有描述姿态滞后、执行器饱和等全部效应。

3. OCS2 到底优化什么

3.1 六维状态，三维加速度输入

令世界坐标位置为 $p$ ，速度为 $v$ ，本次 OCS2 使用：

$x=\begin{bmatrix}p\\v\end{bmatrix}\in\mathbb{R}^6, \qquad u=a\in\mathbb{R}^3.$

预测方程是双积分器：

$\dot p=v,\qquad \dot v=u.$

换成矩阵形式：

$\dot x=Ax+Bu,\qquad A=\begin{bmatrix}0&I\\0&0\end{bmatrix},\quad B=\begin{bmatrix}0\\I\end{bmatrix}.$

这里的输入是期望净加速度，并非旋翼推力。重力补偿和“怎样用倾斜产生水平加速度”由后面的转换与姿态控制处理。

3.2 代价函数：既要跟上，也要控制动作幅度

参考轨迹提供 $x_r=[p_r^T,v_r^T]^T$ 与 $u_r=a_r$ 。在长度为 $H$ 的预测区间上，代价可以写为：

$J=\frac12 e_x(t+H)^TQ_f e_x(t+H) +\int_t^{t+H}\ell(x,u,\tau)\,d\tau,$

$\ell=\frac12 e_x^TQe_x +\frac12 e_u^TR_u e_u+\phi(u),$

其中 $e_x=x-x_r$ ， $e_u=u-u_r$ ， $\phi$ 是加速度软约束的惩罚项。代码使用：

Q  = diag(30, 30, 45, 8, 8, 12)
Ru = 0.6 I
Qf = 3 Q
预测时域 H = 1.2 s
预测离散间隔 = 0.04 s
每次求解最多 5 次迭代

位置权重鼓励机体靠近目标，速度权重鼓励运动方向与速度也匹配；输入权重限制相对参考加速度的偏离。它没有显式惩罚相邻输入之差或 jerk，不能把这套代价理解为“自动保证加速度连续”。

每个加速度分量的预测约束是 $-3\le u_i\le3\ \mathrm{m/s^2}$ ，通过 relaxed barrier 作为软约束进入代价。执行前还会再次截断加速度、限制目标倾角和旋翼推力；预测软约束与执行限幅的职责不同。

代码用 OptimalControlProblem 组合动力学、代价与约束，用 ReferenceManager 更新参考，再由 GaussNewtonDDP_MPC 选择 iLQR 求解。这些模块关系可对照 OCS2 官方最优控制模块说明。

3.3 为什么求解一整段，却只执行第一步

每次反馈更新做四件事：

读取实际位置、速度，作为新的初始状态。
采样未来 1.2 s 的参考位置、速度与加速度。
求解未来的控制序列，取当前段的第一个加速度。
执行 20 ms，再用新状态重新求解。

第 6 s 的速度扰动改变了实际状态，下一次求解就从这个偏离后的状态出发，因此控制器会给出恢复所需的加速度。

本次代码设置 useFeedbackPolicy_=false，执行首个前馈控制量；整个系统仍通过“读取实测状态 → 重新优化”形成反馈闭环。这里要区分 MPC 滚动反馈 与求解器输出的局部反馈增益，两者不是同一个开关。

4. 从加速度到机体倾斜

约定世界 Z 轴向上，机体 z 轴为旋翼合力方向。设 $R_b$ 把机体系向量转到世界系， $e_3=[0,0,1]^T$ ，则空中平动关系为：

$m\ddot p=T R_b e_3-mg e_3.$

四旋翼不能直接向世界 x 或 y 方向施加独立推力。要向前加速，需要先倾斜，让合力具有水平分量。

给定期望净加速度 $a_d$ ，期望合力为：

$F_d=m(a_d+g e_3).$

本例质量固定为 1 kg，所以源码直接写成 acceleration + [0,0,9.81]。若换质量，必须一起修改模型和这处转换，不能原样套用。

期望机体 z 轴对准合力：

$b_{3d}=\frac{F_d}{\lVert F_d\rVert}.$

再选世界 x 轴为航向参考 $b_{1c}$ ，构造：

$b_{2d}=\frac{b_{3d}\times b_{1c}}{\lVert b_{3d}\times b_{1c}\rVert}, \quad b_{1d}=b_{2d}\times b_{3d},$

$R_d=\begin{bmatrix}b_{1d}&b_{2d}&b_{3d}\end{bmatrix}.$

总推力命令采用期望力在当前机体 z 轴上的投影：

$T_d=\max\bigl(0,F_d^T R_b e_3\bigr).$

程序先限制加速度，再限制 $F_d$ 的水平分量，使期望倾角不超过 0.40 rad，约 22.9°。这是目标倾角限制；实际倾角由动力学和跟踪误差决定，另有 0.8 rad 状态越界停止条件。

5. 姿态 PD：把方向误差变成力矩

直接做欧拉角相减容易把坐标表示问题混入控制误差。本例用旋转矩阵构造姿态误差：

$e_R=\frac12\left(R_d^TR_b-R_b^TR_d\right)^\vee.$

$\vee$ 表示从反对称矩阵取出对应的三维向量。设机体系角速度为 $\omega$ 、惯量为 $I_b$ ，姿态控制输出：

$\tau_d=-K_R e_R-K_\omega\omega +\omega\times(I_b\omega).$

三项分别用于纠正方向、抑制角速度，以及补偿刚体旋转中的陀螺耦合。参数为：

KR = diag(1.8, 1.8, 0.9)
Kω = diag(0.32, 0.32, 0.22)
Ib = diag(0.02, 0.02, 0.035) kg·m²

这是本次实现的几何姿态 PD。它没有跟踪期望角速度，也没有完整的期望姿态角加速度前馈；当目标方向变化更快时，不能直接沿用当前缓慢航线的精度结论。

6. 推力分配：四个旋翼分别出多少力

四个旋翼在机体系的位置按顺序为：

旋翼	x / m	y / m	反扭矩符号
r0	+0.18	+0.18	+
r1	−0.18	+0.18	−
r2	−0.18	−0.18	+
r3	+0.18	−0.18	−

令四个向上的推力为 $f_0,\ldots,f_3$ 。由力臂叉乘可知： $\tau_x=yf$ ， $\tau_y=-xf$ ；反扭矩系数取 $c=0.015\ \mathrm m$ 。所以分配关系为：

$\begin{bmatrix}T\\\tau_x\\\tau_y\\\tau_z\end{bmatrix} =M\begin{bmatrix}f_0\\f_1\\f_2\\f_3\end{bmatrix},$

$M=\begin{bmatrix} 1&1&1&1\\ 0.18&0.18&-0.18&-0.18\\ -0.18&0.18&0.18&-0.18\\ 0.015&-0.015&0.015&-0.015 \end{bmatrix}.$

控制器先用 $M^{-1}$ 求旋翼推力，再逐项截断到 0～6 N。水平悬停时四个旋翼合力抵消重力，每个约为：

$f_i=\frac{mg}{4}=2.4525\ \mathrm N.$

如果发生截断，实际可实现的总力/力矩通常会偏离请求值。本例没有带优先级的约束分配 QP；本次完整飞行记录中，旋翼限幅触发次数为 0。

MuJoCo 中各旋翼通过 body 上的 site 施力，力臂自然产生滚转与俯仰力矩，gear 的旋转分量表达偏航反扭矩。推力还经过 25 ms 一阶响应，因而图里的“命令推力”和“实际推力”可以短暂不同。执行器传动、激活动力学与力生成的分工可对照 MuJoCo 官方执行器模型。

7. 轨迹平滑和降落接触也会影响效果

起飞、平移、返回和下降采用五次时间缩放。令归一化时间 $s\in[0,1]$ ：

$\alpha(s)=10s^3-15s^4+6s^5, \quad p_r=p_0+\alpha(s)(p_1-p_0).$

它让每段的起止速度、加速度为零，参考速度和加速度由解析求导获得。圆航线也用这一时间律推进角度，从而平滑开始和结束一圈；本例圆周速度不是恒定的。

降落时遇到过一个值得记住的问题：把参考高度设为落架刚好触地的几何高度 0.09 m，机体会稳定在它上方不到 1 mm，仍然没有接触。只看画面或高度，很容易误判“降落完成”。

最终将末段参考降到 0.075 m，比支撑高度低 15 mm，让控制器继续给出小幅向下的作用。MuJoCo 地面接触支撑机体，并非直接将机体位置改到地下。到 31 s，程序同时检查有接触、实际高度小于 0.12 m、速度小于 0.2 m/s，然后才停桨。停桨后实际高度约 0.0899 m。

8. 一个 controller 包怎样支持多种机器人

复用的核心是状态输入、时间语义、命令输出与停止/复位机制。公共接口只有两项算法操作：

template<class State, class Command>
class IController {
 public:
  virtual ~IController() = default;
  virtual void reset(const State& state, ControlTime time) = 0;
  virtual const Command& update(const State& state, ControlTime time) = 0;
};

具体物理量仍由类型表达：

控制类型	状态	输出
`IJointController`	关节位置/速度及算法所需基座状态	`q_ref/dq_ref/tau_ff/kp/kd`
`IRotorController`	机体姿态、机体系角速度	四个旋翼推力，单位 N

传统关节控制、Go2 ONNX 与 YuMi OCS2 继续使用关节接口。新增 RotorController 使用旋翼接口，两者实现同一个模板协议。旋翼推力 N 和转动关节力矩 Nm 保持独立类型，避免把相同长度的数组当成可互换命令。

目录按职责划分：

ros2_controller/
  include/ros2_controller/
    control_interface.hpp   # 通用接口、控制时间、停止锁存
    controller.hpp          # 关节控制接口与传统/ONNX 实现
    ocs2_controller.hpp     # YuMi 双臂控制
    rotor_controller.hpp    # 旋翼控制接口
  src/rotor_controller.cpp  # 姿态 PD、推力分配、限幅
  examples/flight/          # 位置控制、航线、仿真宿主、录制
  models/quadrotor.xml
  test/flight_test.cpp

RotorController 只接收上层给出的目标姿态和总推力；起飞、绕圈、返回以及位置 MPC 留在示例层。这样以后换任务或轨迹，可以继续使用同一个执行器控制算法。换机型则要核实惯量、旋翼位置、反扭矩系数、力限值和增益。

执行宿主共用 ControlLatch：停止后保持锁存，显式复位才能恢复。ROS 示例消费外部 /clock 时，暂停不推进物理，时间回跳会停止并要求复位。公共接口约定如何调用算法，宿主负责实际调度和状态读写；当前这套仍是独立节点，标准 controller_manager 插件接入属于另一个步骤。相关分工可结合ROS 2 Control 使用指南理解。

9. 实测结果与它的适用范围

两种位置控制模式使用相同的 MuJoCo 名义模型、参考任务和速度扰动；底层姿态控制与旋翼分配一致。

指标	OCS2 位置 MPC	传统位置 PD
圆航线位置 RMS 误差	1.13 cm	2.14 cm
圆航线最大位置误差	1.85 cm	3.55 cm
扰动前悬停 RMS	0.85 mm	1.44 mm
扰动后 2 s 位置误差	4.13 mm	1.68 mm
最大实际倾角	19.0°	13.9°
接地检查后停桨	通过	通过

OCS2 在本次圆航线上的误差更小，传统 PD 在所选扰动恢复时刻的误差更小。结果反映的是这一个模型、这组权重与增益、这一条航线，不能据此认为某种算法在所有场景都更好。

当前模型质量 1 kg，采用名义惯量、一阶旋翼推力和理想状态反馈。没有加入传感器噪声、状态估计、风场或复杂气动；位置预测也没有包括完整姿态动力学。本机 Linux arm64 / Docker 环境中，OCS2 完成 1525 次求解，记录的最大单次耗时约 1.59 ms；这是一轮仿真的记录，不是最坏情况时延或硬实时保证。

验收还覆盖了推力/力矩符号与重力平衡、非法目标、停止/复位、ROS 目标响应、仿真时钟暂停/回跳，并复跑原有关节控制测试。保留的实测指标与媒体校验记录可用于核对本文数字。

10. 如何复现和继续读代码

以下命令对应本次已验收的本地开发工作区，依赖已有的 OCS2/MuJoCo 控制器基础镜像；本篇媒体提交本身不等于完整后端源码发布。

# 构建同一个 ros2_controller，验收两种飞行模式并生成 GIF/MP4
./bin/site.sh ros2-quadrotor-demo

# 也可指定尚不存在的结果目录，已有目录会被拒绝
./bin/site.sh ros2-quadrotor-demo build/quadrotor/my-flight

阅读时建议依次看 examples/flight/flight.cpp 的 Mpc::update()、合力到目标姿态的转换，再看 src/rotor_controller.cpp 的 update() 和 allocate()。最后回到 flight_demo.cpp，检查状态记录与降落判据，便能把公式、控制输出和 GIF 中的运动一一对应。

这次依赖的 OCS2 固定版本为 930e3f4438b9e4cb5c191e39280e7307984dbda7，使用其核心求解包。可继续查阅该版本 MPC 实现以及站内OCS2 的 MPC 运行结构笔记。

11. 扩展演示：给同一段飞行接入雷达与 IMU 建图

在上面的飞行运动记录上增加虚拟雷达与 IMU 后，可以把标准传感器数据送入独立的 ros2_mapping 组件。它与 Go2 四足共用同一个建图入口，仅切换话题、坐标系和安装外参。

左侧为已有飞行状态回放，右侧为实际 Cartographer 3D 注册点云与估计轨迹。27.70 秒 MP4，1.5 倍速，点击播放。打开 / 下载视频。

本轮无人机建图位置 RMSE 约 4.4 cm；这是传感器回放中的定位误差，与第 9 节的控制跟踪误差含义不同。新增场景与传感器没有重新参与动力学求解，SLAM 位姿也尚未反馈给飞控，因此前文的理想状态反馈控制结论保持原来的适用范围。

四足的对照视频、两套外参配置、建图数据流和复现命令，见雷达与 IMU 建图笔记：四足和无人机共用一个组件。

关于重复运动的误差补偿，可继续阅读迭代学习控制：怎样接入通用关节控制器。该篇基于本仓库接口讨论位置参考与力矩前馈两种接入设计；当前飞行控制链尚未加入 ILC。