写在前面

上一篇原本计划先写环境搭建，但如果 ROS2 和 MuJoCo 已经熟悉，就没必要再从安装开始。

这一篇直接进入飞控代码：怎么把一个控制器接进 MuJoCo 仿真，并形成闭环。

先不要急着接 PX4 / ArduPilot。完整飞控栈当然重要，但学习阶段一上来就接完整工程，很容易变成“配置系统”，而不是理解飞控。

我更想先打通这条最小链路：

MuJoCo 四旋翼模型
        ↓  状态 / 传感器
ROS2 仿真桥接节点
        ↓
飞控控制器节点
        ↓  电机推力指令
MuJoCo actuator

这一篇的目标很明确：

先让 MuJoCo 里的四旋翼能被飞控代码控制起来。

最小飞控闭环是什么

飞控代码本质上就是一个闭环控制器。

它不断做三件事：

1. 读取飞机当前状态。
2. 和目标状态比较，算出误差。
3. 输出电机控制量，让飞机往目标状态靠近。

用最简单的高度控制举例：

目标高度 z_ref
    ↓
当前高度 z、当前垂直速度 vz
    ↓
高度控制器
    ↓
总推力 thrust
    ↓
分配到四个电机

所以第一步不需要复杂的导航、规划、地图、视觉，也不需要完整飞控栈。

只要能完成下面这件事，就说明链路已经通了：

飞机掉下去 → 控制器发现高度低了 → 增加推力 → 飞机被拉起来

工程结构

我准备先用这样的结构：

ros2_mujoco_flight/
├── models/
│   └── quad.xml
├── ros2_mujoco_bridge/
│   └── sim_node.py
├── flight_controller/
│   └── altitude_controller.py
└── launch/
    └── hover_test.launch.py

各部分职责如下：

注意，这里我没有先引入 ros2_control，也没有直接接 PX4。

原因是：第一阶段先把数据流跑清楚，后面再工程化。

ROS2 话题设计

先设计最少的话题。

仿真节点发布飞机状态：

/quad/state

控制器发布电机命令：

/quad/motor_cmd

状态消息可以先简单一点，不急着自定义复杂接口。学习阶段可以先用 nav_msgs/Odometry 表达位置、姿态、线速度、角速度。

/quad/state: nav_msgs/msg/Odometry
/quad/motor_cmd: std_msgs/msg/Float64MultiArray

其中 /quad/motor_cmd 约定为四个电机的推力：

[motor_0, motor_1, motor_2, motor_3]

后面如果要更规范，可以再改成自定义消息，例如：

QuadMotorCommand.msg
float64[4] thrust

但第一版先不要过度设计。

MuJoCo 仿真节点负责什么

MuJoCo 节点不应该写太多控制逻辑。

它主要负责四件事：

1. 加载四旋翼模型。
2. 每个仿真步推进 mj_step。
3. 从 data.qpos / data.qvel 里读取状态并发布到 ROS2。
4. 接收 /quad/motor_cmd，写入 data.ctrl。

伪代码大概是这样：

class MujocoSimNode(Node):
    def __init__(self):
        super().__init__('mujoco_sim')

        self.model = mujoco.MjModel.from_xml_path('models/quad.xml')
        self.data = mujoco.MjData(self.model)

        self.motor_cmd = np.zeros(4)

        self.state_pub = self.create_publisher(Odometry, '/quad/state', 10)
        self.cmd_sub = self.create_subscription(
            Float64MultiArray,
            '/quad/motor_cmd',
            self.on_motor_cmd,
            10,
        )

        self.timer = self.create_timer(0.002, self.step)  # 500 Hz

    def on_motor_cmd(self, msg):
        self.motor_cmd[:] = np.array(msg.data[:4])

    def step(self):
        self.data.ctrl[:4] = self.motor_cmd
        mujoco.mj_step(self.model, self.data)
        self.publish_state()

这里的关键点是：

self.data.ctrl[:4] = self.motor_cmd

这行代码就是飞控输出进入物理世界的入口。

飞控节点负责什么

飞控节点暂时只做高度控制。

它订阅当前状态：

/quad/state

然后发布四个电机推力：

/quad/motor_cmd

控制律先用最简单的 PD：

e_z = z_ref - z
e_v = 0 - vz

u = m * g + kp * e_z + kd * e_v
motor_i = u / 4

其中：

代码骨架：

class AltitudeController(Node):
    def __init__(self):
        super().__init__('altitude_controller')

        self.mass = 1.0
        self.gravity = 9.81
        self.z_ref = 1.0
        self.kp = 8.0
        self.kd = 4.0

        self.cmd_pub = self.create_publisher(
            Float64MultiArray,
            '/quad/motor_cmd',
            10,
        )

        self.state_sub = self.create_subscription(
            Odometry,
            '/quad/state',
            self.on_state,
            10,
        )

    def on_state(self, msg):
        z = msg.pose.pose.position.z
        vz = msg.twist.twist.linear.z

        ez = self.z_ref - z
        ev = 0.0 - vz

        thrust = self.mass * self.gravity + self.kp * ez + self.kd * ev
        thrust = max(0.0, thrust)

        motor_thrust = thrust / 4.0

        cmd = Float64MultiArray()
        cmd.data = [motor_thrust] * 4
        self.cmd_pub.publish(cmd)

这段代码非常粗糙，但它有一个好处：足够清楚。

它把飞控最核心的结构暴露出来了：

状态反馈 → 误差计算 → 控制律 → 执行器命令

先只做高度控制的问题

如果四旋翼模型是完整 6DoF 刚体，只做高度控制会有一个明显问题：

它可能会翻。

因为四个电机推力完全相等时，理论上只提供竖直方向总推力，不提供姿态稳定力矩。

如果模型初始姿态完全水平、扰动很小，它可能能短时间上下运动；但只要有一点姿态偏差，就会开始倾斜，甚至翻掉。

所以这一阶段有两个选择：

选择 A：先把姿态锁住

在 MuJoCo 模型里先简化自由度，只让它沿 z 轴运动。

这样可以专注验证高度控制链路：

高度状态 → 高度控制器 → 总推力 → z 方向运动

优点是简单、直观。

缺点是不是真实四旋翼完整动力学。

选择 B：直接进入姿态控制

完整保留 6DoF，然后写姿态稳定控制：

roll / pitch / yaw
角速度 p / q / r
        ↓
姿态控制器
        ↓
力矩 tau_x / tau_y / tau_z
        ↓
电机混控

优点是更接近真实飞控。

缺点是第一步复杂度明显上升。

我更倾向于先用选择 A 跑通最小链路，然后马上进入选择 B。

也就是：

01：最小高度闭环
02：完整四旋翼姿态控制
03：高度 + 姿态一起悬停

电机混控先怎么理解

等加入姿态控制后，控制器输出就不再是一个总推力，而是：

u = [T, tau_x, tau_y, tau_z]

分别表示：

然后通过混控矩阵分配到四个电机。

对于一个 X 型四旋翼，可以先抽象成：

motor_0 = T/4 - tau_y/(2l) + tau_z/(4k)
motor_1 = T/4 + tau_x/(2l) - tau_z/(4k)
motor_2 = T/4 + tau_y/(2l) + tau_z/(4k)
motor_3 = T/4 - tau_x/(2l) - tau_z/(4k)

这里暂时不纠结符号正负，因为不同坐标系、电机编号、旋向定义会改变公式。

真正写代码时，一定要做一个表：

motor id
位置 x/y
旋向 cw/ccw
正推力方向
正力矩方向

否则后面调参会非常痛苦。

第一版应该观察什么

跑最小高度闭环时，不要只看“飞没飞起来”，还要看几个量：

1. 高度 z 是否向 z_ref 收敛。
2. 垂直速度 vz 是否逐渐变小。
3. 总推力是否在合理范围。
4. 电机命令是否出现负值或过大值。
5. 仿真步长是否稳定。

可以先打印：

time, z, vz, thrust

也可以发布到 ROS2 后用 rqt_plot 看：

rqt_plot /quad/state/pose/pose/position/z

如果高度曲线是这样的，就说明方向对了：

起始高度较低
    ↓
推力增加
    ↓
高度上升
    ↓
接近目标高度
    ↓
速度被阻尼压下去

如果出现持续震荡，通常是：

• kp 太大。
• kd 太小。
• 推力单位和模型质量不匹配。
• 仿真步长太大。
• actuator 的力方向定义错了。

这一篇的结论

这一篇先不追求完整飞控，而是把飞控代码放进仿真闭环里。

最小闭环是：

MuJoCo 发布状态
        ↓
ROS2 传给控制器
        ↓
控制器计算电机推力
        ↓
ROS2 发回 MuJoCo
        ↓
MuJoCo 更新动力学

第一版控制器只做高度 PD：

u = m * g + kp * (z_ref - z) + kd * (0 - vz)

它不完美，但它是进入飞控代码的第一扇门。

下一篇我准备继续往下走：

02：MuJoCo 四旋翼模型怎么写：body、joint、actuator 和 sensor

先把模型搭清楚，再进入姿态控制和电机混控。