简介：这份资源面向航天动力学仿真、控制算法学习与验证的开发者及学生，围绕基于Mujoco物理引擎的二维矢量喷管模型火箭，提供六自由度运动建模、欧拉角姿态描述、刚体动力学与LQR控制器设计的完整工程实现，可用于火箭飞行特性研究、控制策略对比与算法迭代验证。压缩包共28个文件、约7.77MB，包含15个stl三维模型文件、6张png效果图、2个xml场景与模型配置、2个py控制与仿真脚本，以及说明文档和项目说明，覆盖从模型搭建到控制验证的主要环节。已有134人学习关注。读者可据此获得可运行的仿真工程结构、矢量喷管推力方向控制思路、LQR状态反馈增益设计方法，以及Mujoco环境下的参数调试与动态响应分析参考，适合作为课程设计、毕业设计或相关课题的起步模板。
1. 二维矢量喷管模型火箭仿真：为什么六自由度建模总在俯仰通道翻车
很多人第一次做模型火箭动力学仿真，都会掉进同一个坑：把二维矢量喷管当成一个简单的力偏转，直接在三自由度平面上跑，结果俯仰角一过 30 度，轨迹就飞得亲妈都不认识。问题不在控制器，而在你根本没把刚体六自由度运动建模当回事。模型火箭的推力矢量偏转会产生力矩耦合，滚转、偏航、俯仰三个通道互相拉扯，二维喷管只摆一个平面，但箭体在三维空间里翻滚，欧拉角动力学一旦出现万向节死锁，仿真直接变玄学。
这个方向解决的核心问题是：在 Mujoco 物理引擎里，用二维矢量喷管作为唯一执行机构，验证 LQR 控制器能不能在六自由度刚体运动下把火箭稳定到目标姿态。适合谁？做飞行控制算法验证的工程师、搞小型火箭姿态控制的同学、以及想用 Mujoco 做刚体动力学仿真但不想从零写积分器的人。你不需要真实火箭，只需要一个能跑通的仿真闭环，把建模、控制器设计、参数整定、失败排查串起来。下面按我实际搭过的路径，从环境到代码到踩坑，一步步拆。
2. 用 Mujoco 搭二维矢量喷管火箭：从 XML 到六自由度刚体
2.1 为什么选 Mujoco 而不是自己写欧拉方程
自己写六自由度欧拉角动力学不是不行，但你要处理四元数归一化、角速度耦合、接触力、积分步长，随便一个符号错误就能让火箭在 0.1 秒内发散。Mujoco 的优势在于它内置了刚体动力学求解器，你只需要定义质量、惯量、关节类型、执行器，剩下的交给它。常见做法是：用 free joint 表示火箭的六自由度运动，用两个 hinge joint 模拟二维矢量喷管的偏转，喷管推力通过 actuator 施加在箭体上。
我一般会先确认 Mujoco 版本，Python 绑定用mujoco包，至少 3.0 以上，因为老版本对 free joint 的 actuator 支持有差异。安装命令很简单：
pip install mujoco numpy scipy matplotlib逻辑说明：mujoco 是物理引擎核心，numpy 做矩阵运算，scipy 用来解 Riccati 方程设计 LQR，matplotlib 画轨迹和姿态曲线。参数说明：不需要指定版本号，但如果你用 conda，注意别和 pybullet 混装，两者对 OpenGL 的依赖会打架。
2.2 火箭模型的 XML 关键字段与惯量设置
Mujoco 的模型文件是 XML，核心结构是<worldbody>里放一个<body>作为火箭，给它加<freejoint>，然后挂两个<body>作为喷管，用<joint type="hinge">限制在二维平面。下面是我常用的最小可用模型骨架：
<mujoco model="rocket_2d_tvc"> <option timestep="0.001" gravity="0 0 -9.81"/> <worldbody> <body name="rocket" pos="0 0 1"> <freejoint name="rocket_free"/> <geom type="capsule" size="0.05 0.3" mass="1.0" rgba="0.8 0.2 0.2 1"/> <inertial pos="0 0 0" mass="1.0" diaginertia="0.01 0.01 0.002"/> <body name="nozzle_x" pos="0 0 -0.3"> <joint name="nozzle_x_joint" type="hinge" axis="1 0 0" range="-0.3 0.3"/> <geom type="box" size="0.02 0.02 0.05" mass="0.05" rgba="0.2 0.2 0.8 1"/> </body> <body name="nozzle_y" pos="0 0 -0.3"> <joint name="nozzle_y_joint" type="hinge" axis="0 1 0" range="-0.3 0.3"/> <geom type="box" size="0.02 0.02 0.05" mass="0.05" rgba="0.2 0.8 0.2 1"/> </body> </body> </worldbody> <actuator> <motor name="thrust" joint="rocket_free" gear="0 0 1 0 0 0" ctrlrange="0 20"/> <motor name="tvc_x" joint="nozzle_x_joint" gear="1" ctrlrange="-1 1"/> <motor name="tvc_y" joint="nozzle_y_joint" gear="1" ctrlrange="-1 1"/> </actuator> </mujoco>逻辑说明：freejoint让火箭有 6 个自由度（3 平移 + 3 转动），diaginertia是绕三个轴的转动惯量，这里给的是细长体近似值。两个喷管分别绕 X 轴和 Y 轴偏转，模拟二维矢量喷管。motor执行器里，thrust的gear是0 0 1 0 0 0，表示推力沿 Z 轴正向，不产生力矩；tvc_x和tvc_y直接控制喷管关节角度。
参数说明：timestep设 0.001 秒，太小会慢，太大会丢稳定性；diaginertia必须和你的质量分布匹配，随便填会导致姿态振荡频率完全不对。我见过有人把惯量写成 1.0，结果 LQR 增益算出来巨大，一上仿真就抖成筛子。
2.3 推力矢量如何映射到六自由度力矩
二维矢量喷管的推力方向由两个喷管偏转角决定。假设喷管偏转后推力方向在箭体坐标系下为：
F_body = T * [sin(theta_y), -sin(theta_x), cos(theta_x)*cos(theta_y)]其中 T 是推力大小，theta_x 和 theta_y 是两个喷管的偏转角。这个力作用在喷管位置，相对质心产生力矩：
tau = r_nozzle × F_bodyr_nozzle 是喷管相对质心的位置向量，这里取[0, 0, -0.3]。在 Mujoco 里，你不需要手动算这个叉乘，因为motor执行器施加在喷管关节上，物理引擎会自动计算反作用力矩。但你要理解这个映射关系，否则调 LQR 的时候根本不知道增益在补偿什么。
常见做法是：先用小角度近似，把力矩线性化为tau_x ≈ -T * 0.3 * theta_x，tau_y ≈ T * 0.3 * theta_y，然后基于这个线性模型设计 LQR。非线性部分留给 Mujoco 去验证。
3. LQR 控制器设计：从线性化到 Mujoco 闭环
3.1 六自由度状态空间线性化与 Q/R 矩阵选取
LQR 需要线性模型。在悬停或小角度假设下，火箭的俯仰和偏航通道可以解耦成两个二阶系统。状态向量取[theta, theta_dot]，输入是喷管偏转角。以俯仰通道为例：
theta_ddot = (T * L / I_yy) * theta_x其中 L 是喷管到质心的距离，I_yy 是绕 Y 轴的转动惯量。写成状态空间：
A = [[0, 1], [0, 0]] B = [[0], [T*L/I_yy]]LQR 的代价函数是J = ∫ (x^T Q x + u^T R u) dt。Q 和 R 的选取直接决定响应速度和超调。我一般先设 Q = diag([100, 10])，R = 1，然后根据仿真曲线调。角度误差权重大，角速度权重小，这样控制器会优先把角度拉回来。
import numpy as np from scipy.linalg import solve_continuous_are def lqr(A, B, Q, R): P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ B.T @ P return K # 俯仰通道参数 T = 10.0 # 推力 N L = 0.3 # 力臂 m Iyy = 0.01 # 转动惯量 kg*m^2 A = np.array([[0, 1], [0, 0]]) B = np.array([[0], [T*L/Iyy]]) Q = np.diag([100, 10]) R = np.array([[1]]) K = lqr(A, B, Q, R) print("LQR gain:", K)逻辑说明：solve_continuous_are解连续时间代数 Riccati 方程，得到 P 矩阵，再算增益 K。参数说明：T 和 L 必须和 XML 里一致，Iyy 要和diaginertia对应。如果 K 算出来是[100, 20]这种量级，说明 Q/R 比例合理；如果 K 是[10000, 2000]，说明 R 太小，控制器会非常激进，仿真里喷管会疯狂摆动。
3.2 在 Mujoco 里挂载 LQR 并跑通第一个闭环
Mujoco 的 Python 接口用mujoco.MjModel和mujoco.MjData。控制循环里，每一步读状态，算 LQR 输出，写data.ctrl。下面是最小闭环：
import mujoco import numpy as np model = mujoco.MjModel.from_xml_path("rocket_2d_tvc.xml") data = mujoco.MjData(model) # 初始状态：稍微偏离平衡 data.qpos[2] = 1.0 # 高度 data.qpos[3:6] = [0.1, 0, 0] # 欧拉角，绕X轴偏0.1 rad mujoco.mj_forward(model, data) K_pitch = np.array([[50, 10]]) # 俯仰通道增益 K_yaw = np.array([[50, 10]]) # 偏航通道增益 for step in range(5000): # 读欧拉角和角速度 quat = data.qpos[3:7] euler = np.zeros(3) mujoco.mju_quat2Mat(euler, quat) # 简化示意，实际用 mju_quat2Mat 转矩阵再取欧拉角 theta_x = euler[0] theta_y = euler[1] omega = data.qvel[3:6] # LQR 控制律 u_x = -K_pitch @ np.array([theta_x, omega[0]]) u_y = -K_yaw @ np.array([theta_y, omega[1]]) # 推力保持恒定，喷管偏转限幅 data.ctrl[0] = 10.0 data.ctrl[1] = np.clip(u_x, -1, 1) data.ctrl[2] = np.clip(u_y, -1, 1) mujoco.mj_step(model, data)逻辑说明：mju_quat2Mat把四元数转成旋转矩阵，再提取欧拉角。实际代码里我会用mujoco.mju_quat2Mat和mju_mat2Euler组合，避免自己写转换。data.ctrl的顺序和 XML 里 actuator 定义顺序一致。参数说明：K_pitch和K_yaw来自上一步的 LQR 计算，这里为了简洁直接写数值。np.clip限幅到 ±1，对应喷管最大偏转 0.3 rad 的近似线性区。
3.3 仿真步长、积分器与实时性权衡
Mujoco 默认用欧拉积分，timestep设 0.001 秒。如果你把步长放到 0.01，火箭在俯仰振荡时会明显发散，因为积分误差累积。我一般会跑 5000 步，对应 5 秒仿真，足够看到姿态收敛。实时性方面，Python 循环每步大概 0.1 毫秒，5000 步不到 1 秒，完全能接受。
如果你要跑更长时间，建议用mujoco.mj_step的批量模式，或者把控制频率降到 100 Hz，物理步长保持 0.001，中间用零阶保持。常见做法是：控制周期 0.01 秒，物理步长 0.001 秒，每 10 个物理步更新一次控制量。
4. 避坑与排查：二维矢量喷管仿真里最容易翻车的 5 个点
4.1 现象：火箭一离地就绕 Z 轴狂转，滚转通道完全失控
原因：二维矢量喷管只有两个偏转轴，无法产生滚转力矩。如果你在 XML 里给喷管加了绕 Z 轴的关节，或者推力作用线不经过质心，就会产生意外的滚转力矩。更隐蔽的是，freejoint的转动自由度是全耦合的，俯仰和偏航的交叉项会激励滚转。
解决：检查喷管位置是否严格在质心正下方，pos的 X 和 Y 分量必须为 0。如果滚转仍然发散，在 LQR 里加一个滚转阻尼项，用data.qvel[5]反馈到喷管偏转上，虽然不能直接产生滚转力矩，但可以通过耦合间接抑制。
4.2 现象：LQR 增益算出来正常，但仿真里喷管疯狂抖动
原因：Q/R 选取让控制器带宽远高于物理步长能响应的频率。比如 K 的角速度增益到了 1000，而timestep是 0.001，控制量在相邻步之间剧烈变化，Mujoco 的求解器会把它当成高频激励。
解决：降低 Q 的角速度权重，或者增大 R。我一般会把 R 从 1 调到 10，K 的角速度项降到 20 左右。另一个办法是加低通滤波，但最简单的是重新整定 Q/R。
4.3 现象：欧拉角在 90 度附近跳变，姿态曲线出现尖峰
原因：欧拉角表示在俯仰角接近 ±90 度时出现万向节死锁，mju_mat2Euler返回的角度会突变。如果你的火箭要做大角度机动，欧拉角根本不够用。
解决：改用四元数做状态反馈。LQR 的线性模型仍然用欧拉角小角度近似，但反馈时用四元数误差。具体做法是：目标四元数和当前四元数做差，转成轴角，再乘增益。这样在大角度下也不会跳变。
4.4 现象：推力恒定但高度一直掉，LQR 只顾姿态不管高度
原因：LQR 只设计了姿态通道，没有把高度和垂直速度纳入状态向量。推力设 10 N，但重力是 9.81 N，净推力只有 0.19 N，火箭缓慢下坠。如果你把推力设成 20 N，火箭会加速上升，但姿态控制器可能来不及响应。
解决：要么把推力设成略大于重力，让火箭悬停；要么在状态向量里加高度和垂直速度，设计一个外环 PID 控制推力。我一般会先让火箭在重力平衡点附近悬停，单独验证姿态，再叠加高度控制。
4.5 现象：仿真跑着跑着报错“mjWARN: too many contacts”或直接崩溃
原因：火箭的几何体和其他物体发生了穿透，或者diaginertia设得太小导致数值不稳定。Mujoco 对惯量矩阵的正定性有要求，如果某个轴的惯量接近零，求解器会发散。
解决：检查diaginertia三个分量是否都大于 1e-6。如果火箭是细长体，绕长轴的惯量可以小，但不能为零。另外，把geom的contype和conaffinity设成 0，禁用接触检测，除非你真的需要碰撞。
5. 进阶技巧：用四元数误差反馈把大角度机动跑稳
欧拉角反馈在小角度下没问题，但一旦俯仰超过 60 度，万向节死锁就会让控制器发疯。我后来改成四元数误差反馈，效果立竿见影。核心思路是：目标姿态用四元数表示，当前姿态也从data.qpos里读四元数，两者做差得到误差四元数，再转成轴角向量，乘 LQR 增益。
def quat_error(q_target, q_current): # q_target 和 q_current 都是 [w, x, y, z] q_err = np.zeros(4) mujoco.mju_negQuat(q_err, q_current) mujoco.mju_mulQuat(q_err, q_target, q_err) # 转成轴角 axis = np.zeros(3) angle = np.zeros(1) mujoco.mju_quat2AxisAngle(axis, angle, q_err) return axis * angle[0] # 在控制循环里 q_target = np.array([1, 0, 0, 0]) # 目标：水平 q_current = data.qpos[3:7] err = quat_error(q_target, q_current) omega = data.qvel[3:6] u = -K @ np.concatenate([err, omega])逻辑说明：mju_negQuat求共轭，mju_mulQuat做四元数乘法，mju_quat2AxisAngle把误差四元数转成轴角。轴角向量直接作为姿态误差，和角速度拼成状态向量，乘增益得到控制量。参数说明：K 还是用 LQR 算出来的，但状态向量从[theta, theta_dot]变成[axis*angle, omega]，维度从 2 变成 6。你需要重新算 LQR，把 A 和 B 扩展成 6 维。
验证方法：给一个 80 度的初始俯仰角，看火箭能不能在 2 秒内回正。如果超调超过 20%，把 Q 里的角度权重调大。我试过 Q = diag([200, 200, 200, 20, 20, 20])，R = 1，响应很干脆。
另一个技巧是：在 Mujoco 里开mujoco.mj_rne算逆动力学，验证你的力矩映射对不对。如果逆动力学算出来的力矩和你的 LQR 输出对不上，说明 XML 里的执行器配置有问题。这个习惯帮我省了很多后悔药。
最后说个血泪经验：别在第一次跑通前就调 Q/R。先把推力设成重力平衡，喷管限幅设小，跑一个 10 度的初始偏差，看姿态能不能收敛。收敛了再加大角度，再调增益。顺序反了，你会在抖振和发散之间反复横跳。希望帮到你。
本文还有配套的精品资源，点击获取
