Mujoco六自由度火箭LQR控制仿真：从建模到闭环实战

简介本资源面向航天动力学仿真与控制方向的学习者与研究者提供一套基于Mujoco物理引擎的模型火箭六自由度仿真与控制验证方案重点解决二维矢量喷管推力方向控制与姿态稳定问题。压缩包共28个文件约7.77MB包含15个stl三维模型文件、2个xml场景与模型配置、2个py控制脚本、6张png效果图以及说明文档与附赠资料覆盖从几何建模到仿真验证的完整链路。项目以欧拉角描述刚体姿态构建六自由度运动模型并设计LQR线性二次调节器实现状态反馈与轨迹跟踪读者可据此理解矢量喷管建模、动力学方程推导与最优控制参数整定方法。目前已有134人学习适合具备一定控制理论基础、希望借助Mujoco快速搭建火箭仿真环境并验证控制算法的读者参考。1. 拆开这个火箭仿真包Mujoco 里跑通六自由度 LQR 控制到底要几步模型火箭的六自由度仿真很多人第一反应是搭个 Simulink 框图或者用现成的气动工具链但真要把矢量喷管、欧拉角姿态解算和 LQR 反馈塞进一个能实时跑的物理引擎里坑比想象中多。这个资源包给的就是一条已经铺好的路基于 Mujoco 物理引擎搭一个带二维矢量喷管的模型火箭做六自由度刚体动力学仿真再用 LQR 控制器验证姿态稳定和跟踪效果。压缩包里Rocket-LQR-Control-main是主目录main.py是入口lqr_control.py是控制器实现vtol目录下放着scene.xml和vtol.xml两个模型描述文件assets里是网格资源images里是几张仿真截图和 Onshape 建模图。适合谁正在做火箭姿态控制课程设计、想拿 Mujoco 练手刚体动力学、或者需要一套能改参数就能跑的 LQR 验证框架的从业者。它不教你从零推导欧拉方程但给你一个能直接python main.py跑起来的闭环。2. 环境搭起来Mujoco 安装与模型文件结构拆解2.1 为什么选 Mujoco 而不是自己写积分器模型火箭的六自由度运动本质是刚体在三维空间里的平动加转动牛顿-欧拉方程写出来不难难的是接触、约束和数值稳定性。自己用 RK4 写积分器步长稍大就发散喷管推力一突变姿态角就飞了。Mujoco 用的是隐式积分加约束求解对刚体堆叠和关节约束的处理成熟得多而且mj_step内部做了大量数值优化同样的仿真时长Mujoco 能比手写积分器稳一个量级。这个资源包选 Mujoco核心原因就是它把“物理正确”这件事兜住了你只需要关心控制逻辑和模型参数。另一个现实理由是 Mujoco 的 XML 模型描述足够直观。vtol.xml里定义刚体、关节、几何体和执行器改推力大小、改转动惯量、改喷管偏转角都是改几个数字的事不用重新编译。对于需要反复调 LQR 权重的场景这种“改完就跑”的迭代速度很关键。2.2 安装 Mujoco 与依赖的实操步骤资源包没有锁死 Python 版本但 Mujoco 官方轮子对 3.8 到 3.11 支持最好。我一般用 conda 建个干净环境避免和系统里的 numpy 版本打架。# 创建独立环境Python 3.10 是 Mujoco 兼容性最稳的版本之一 conda create -n rocket_sim python3.10 -y conda activate rocket_sim # 安装 Mujoco 本体2.x 和 3.x 的 API 有差异资源包按 3.x 写 pip install mujoco # 控制相关的依赖numpy 做矩阵运算scipy 用来解 Riccati 方程 pip install numpy scipy matplotlib装完之后验证一下import mujoco print(mujoco.__version__) # 输出应该是 3.x.x如果是 2.x 后面加载模型可能报 API 不匹配这里有个细节Mujoco 3.x 把渲染后端拆开了如果main.py里带可视化窗口还需要装mujoco的 viewer 依赖Linux 下通常要pip install mujoco-python-viewer或者直接用mujoco.viewer。资源包的main.py如果默认开渲染第一次跑之前先确认显示环境可用否则会卡在窗口初始化。2.3 模型文件结构scene.xml 和 vtol.xml 各管什么Mujoco 的模型组织是“场景引用模型”的模式。scene.xml是顶层场景负责光照、相机、地面和全局设置vtol.xml是火箭本体定义刚体树、关节、几何和 actuator。资源包里这两个文件分开好处是改火箭参数不用动场景改场景也不影响火箭定义。打开vtol.xml你会看到几个关键块XML 块作用调参时关注什么worldbody定义火箭的刚体层级质心位置、几何尺寸joint六自由度关节通常是 free joint关节阻尼、armatureactuator矢量喷管推力执行器推力范围、偏转角度限制option积分器步长和重力timestep、gravityfree joint是六自由度的核心它让火箭在空间里不受约束地平动和转动。LQR 控制的就是这个 free joint 对应的广义坐标和速度。如果你把 free joint 换成 hinge 或 slide自由度就降了六自由度仿真也就名不副实。提示改timestep之前先看main.py里的控制频率。Mujoco 的物理步长通常设 0.001 到 0.005 秒控制步长可以按 10 倍或 20 倍降频两者不匹配会导致 LQR 输出抖动。3. 六自由度建模与欧拉角动力学从 free joint 到状态空间3.1 六自由度状态量到底怎么取模型火箭的六自由度指的是质心在三维空间里的三个平动自由度加三个转动自由度。在 Mujoco 里一个 free joint 自动给你 7 个广义坐标3 个位置 4 个四元数和 6 个广义速度3 个线速度 3 个角速度。但 LQR 需要的是线性状态空间四元数不能直接线性化所以资源包的做法是从 Mujoco 拿四元数转成欧拉角再用欧拉角加角速度构造状态向量。常见状态向量取法有两种一种是[x, y, z, roll, pitch, yaw, vx, vy, vz, wx, wy, wz]共 12 维另一种是只取俯仰和偏航相关的通道做降维。资源包的lqr_control.py里状态维度大概率是 12 维全状态因为六自由度验证需要完整姿态信息。import numpy as np from scipy.spatial.transform import Rotation as R def quat_to_euler(quat): # Mujoco 的四元数顺序是 w, x, y, zscipy 要的是 x, y, z, w quat_scipy np.array([quat[1], quat[2], quat[3], quat[0]]) r R.from_quat(quat_scipy) # 返回 roll, pitch, yaw单位弧度 return r.as_euler(xyz, degreesFalse) def build_state(data): # 位置和线速度直接从 qpos/qvel 取 pos data.qpos[0:3] vel data.qvel[0:3] # 姿态从四元数转欧拉角角速度从 qvel 后三位取 euler quat_to_euler(data.qpos[3:7]) omega data.qvel[3:6] return np.concatenate([pos, euler, vel, omega])这段代码的逻辑是Mujoco 的qpos前三位是位置接着四位是四元数qvel前三位是线速度后三位是角速度。四元数转欧拉角这一步是必须的因为 LQR 的线性化是在欧拉角空间做的。参数上要注意as_euler(xyz)的顺序如果你的火箭定义里姿态旋转顺序不同这里要跟着改否则 roll 和 yaw 会串。3.2 欧拉角动力学方程的线性化处理LQR 的前提是系统能写成x_dot Ax Bu的形式。火箭的完整动力学是非线性的但资源包的做法是在悬停或小角度姿态附近做线性化。具体来说把欧拉角动力学在小角度下近似为roll_dot ≈ wx、pitch_dot ≈ wy、yaw_dot ≈ wz这样姿态运动学就线性化了。推力项的处理更关键。二维矢量喷管意味着推力方向可以在两个平面内偏转通常对应俯仰和偏航两个通道。推力大小设为常数或慢变偏转角作为控制输入。线性化后的 B 矩阵就是把推力对力矩的偏导数填进去。def linearize_rocket(mass, inertia, thrust, arm_length): # 状态 12 维控制 2 维俯仰偏转、偏航偏转 A np.zeros((12, 12)) B np.zeros((12, 2)) # 位置对速度的导数单位矩阵 A[0:3, 6:9] np.eye(3) # 欧拉角对角速度的导数小角度下近似单位阵 A[3:6, 9:12] np.eye(3) # 角加速度对姿态角的导数重力梯度项这里简化处理 # 实际资源包里会根据惯量矩阵和推力臂长填具体数值 Ixx, Iyy, Izz inertia A[9, 3] mass * 9.81 * arm_length / Ixx A[10, 4] mass * 9.81 * arm_length / Iyy # 控制输入对俯仰和偏航角加速度的影响 B[10, 0] thrust * arm_length / Iyy B[11, 1] thrust * arm_length / Izz return A, B这段是线性化的骨架实际资源包里A和B的数值会根据vtol.xml里的质量、惯量和推力参数算出来。参数说明mass是火箭总质量inertia是三个主轴惯量thrust是喷管额定推力arm_length是推力作用点到质心的力臂。这几个参数在vtol.xml里都能找到对应字段改模型的时候要同步改这里否则 LQR 增益会失配。注意线性化只在平衡点附近有效。如果你的仿真里火箭要做大角度机动比如翻滚 90 度这个线性模型会失效LQR 输出会饱和甚至发散。资源包的验证场景大概率是小角度稳定别拿它直接跑特技动作。3.3 用 Mujoco 的 mj_jac 验证力矩映射手算 B 矩阵容易出错Mujoco 提供了mj_jac可以自动算雅可比。一个更稳的做法是给喷管一个微小偏转用mj_jac算出质心处的力和力矩再数值差分得到 B 矩阵。资源包如果没做这一步你可以自己补上能省掉很多调参的玄学时间。def numerical_B(model, data, epsilon1e-4): # 保存原始控制量 ctrl_backup data.ctrl.copy() B_num np.zeros((12, model.nu)) for i in range(model.nu): # 正向扰动 data.ctrl[:] ctrl_backup data.ctrl[i] epsilon mujoco.mj_forward(model, data) state_plus build_state(data) # 反向扰动 data.ctrl[:] ctrl_backup data.ctrl[i] - epsilon mujoco.mj_forward(model, data) state_minus build_state(data) # 中心差分 B_num[:, i] (state_plus - state_minus) / (2 * epsilon) data.ctrl[:] ctrl_backup return B_num这个数值差分法不依赖解析推导适合模型复杂或手算容易漏项的情况。代价是每次调用要多跑两次mj_forward但离线算一次 B 矩阵的开销可以忽略。4. LQR 控制器设计与 Mujoco 闭环接入4.1 解 Riccati 方程拿增益矩阵LQR 的核心是解代数 Riccati 方程得到最优反馈增益K R^{-1} B^T P。资源包的lqr_control.py里应该用了scipy.linalg.solve_continuous_are来解。Q 和 R 的选取直接决定控制效果Q 大姿态收敛快但控制量大R 大控制平滑但响应慢。from scipy.linalg import solve_continuous_are def lqr_gain(A, B, Q, R): # 解连续时间代数 Riccati 方程 P solve_continuous_are(A, B, Q, R) # 计算反馈增益 K np.linalg.inv(R) B.T P return K # Q 和 R 的典型初值 Q np.diag([10, 10, 10, # 位置权重 50, 50, 50, # 姿态权重姿态比位置更重要 1, 1, 1, # 线速度权重 5, 5, 5]) # 角速度权重 R np.diag([1, 1]) # 两个控制通道的代价参数说明Q 矩阵的对角线元素代表对各个状态偏差的惩罚。姿态权重给到 50是因为火箭姿态一旦偏了推力方向就错位置会跟着飞。R 给 1 是基准如果控制量抖动厉害把 R 加到 10 或 100增益会降下来控制变柔。这个调参过程没有公式就是试但方向是明确的。4.2 把 LQR 输出接到 Mujoco 的 actuatorMujoco 的data.ctrl是控制输入数组长度等于model.nu。资源包里vtol.xml定义了几个 actuatordata.ctrl就有几个通道。LQR 算出来的u -K x要按 actuator 的顺序填进去。def control_step(model, data, K, target_state): # 当前状态 x build_state(data) # 误差target_state 是期望的悬停状态 error x - target_state # LQR 反馈 u -K error # 限幅防止执行器饱和 u np.clip(u, -1.0, 1.0) # 写入 Mujoco 控制量 data.ctrl[:] u return u这里的关键是target_state。如果做姿态稳定目标就是全零状态位置归零、姿态归零、速度归零。如果做跟踪目标状态随时间变化LQR 就变成了跟踪控制器。资源包的验证场景大概率是稳定控制target_state设成平衡点即可。限幅那一步别省。Mujoco 的 actuator 有ctrlrange超了会被截断但截断发生在物理步里你在控制层先限幅能避免积分饱和导致的振荡。4.3 仿真主循环的时间同步main.py的主循环结构通常是读状态 → 算控制 → 写 ctrl →mj_step→ 循环。控制频率和物理频率的比值要固定常见做法是物理步长 0.002 秒控制步长 0.02 秒每 10 个物理步算一次控制。# 物理步长从 model.opt.timestep 取 physics_dt model.opt.timestep control_dt 0.02 steps_per_control int(control_dt / physics_dt) for i in range(total_steps): if i % steps_per_control 0: control_step(model, data, K, target_state) mujoco.mj_step(model, data)这个结构里control_step只在控制周期触发中间物理步保持 ctrl 不变。如果你每个物理步都算 LQR计算量翻 10 倍不说控制量高频抖动还会激发模型的高频模态仿真反而容易崩。提示steps_per_control必须是整数如果control_dt不是physics_dt的整数倍取整之后控制周期会漂移。要么把物理步长设成控制步长的整数分之一要么在代码里做累加器补偿。5. 避坑与排查跑不通的时候先看这几处5.1 现象仿真一启动火箭就飞出去原因LQR 增益符号反了或者target_state和初始状态差太远控制量直接饱和。Mujoco 的 free joint 初始位置如果在原点但target_state设了非零高度第一步控制量就是满推力。解决先把target_state设成和初始状态一致确认火箭能悬停不动再逐步加目标偏差。检查K矩阵的符号u -K error里的负号不能丢。5.2 现象姿态角在 0 附近高频抖动原因控制频率太高或者 Q 矩阵里角速度权重给太大LQR 增益过高系统带宽超过了物理步长能稳定解析的范围。解决把控制周期从 0.01 秒放宽到 0.02 或 0.05 秒或者把 Q 里角速度权重从 5 降到 1。Mujoco 的timestep也可以适当减小但优先调控制频率。5.3 现象四元数转欧拉角出现跳变原因欧拉角在 ±90 度附近有万向锁as_euler返回的角度会从 π 跳到 -πLQR 看到的状态突变控制量跟着跳。解决仿真场景限制在小角度范围内或者改用四元数误差做反馈。资源包如果只做小角度稳定这个问题不会暴露但一旦做大幅机动就会翻车。5.4 现象mj_step报错或仿真变慢原因模型 XML 里有非法几何体或关节配置Mujoco 在编译模型时报错。另一种可能是assets里的网格文件路径不对vtol.xml引用了不存在的 STL 或 OBJ。解决先用mujoco.MjModel.from_xml_path单独加载scene.xml看报错信息指向哪个文件。路径问题在 XML 里通常是file...字段相对路径要相对于 XML 所在目录。5.5 现象LQR 增益算出来是 NaN原因solve_continuous_are要求 (A, B) 可稳定如果 B 矩阵全零或者 A 矩阵有正实部不可控模态Riccati 方程无解。解决检查linearize_rocket里的 B 矩阵确认推力对力矩的映射不为零。如果arm_length设成 0B 就是零矩阵增益必然 NaN。6. 进阶验证用能量法检查 LQR 是否真的在干活跑通稳定控制之后怎么判断 LQR 是真的在优化还是只是碰巧没发散我一般会加一个能量监控。LQR 的最优性体现在它最小化∫ (x^T Q x u^T R u) dt所以仿真结束后可以把这个积分算出来和手动调的其他增益对比。如果 LQR 的积分值明显更低说明 Riccati 解是对的。def cost_integral(states, controls, Q, R, dt): total 0.0 for x, u in zip(states, controls): total (x Q x u R u) * dt return total把main.py里的状态和控制量存下来跑两组增益一组是 LQR 算的一组是随便给的比例增益。同样的初始偏差看谁的 cost 小。这个验证比看曲线收敛更硬因为曲线收敛可能只是增益大但代价未必最优。另一个进阶玩法是改vtol.xml里的推力臂长重新算 LQR观察增益怎么变。臂长增加同样的力矩需要更小的偏转角B 矩阵变大K 应该变小。如果 K 没变说明你的线性化没把臂长代进去B 矩阵是错的。还有一个我踩过的坑Mujoco 的qvel里角速度是体坐标系下的而欧拉角导数是世界坐标系下的两者在小角度下近似相等但角度一大就差很多。如果你要做大姿态机动状态方程里得加坐标变换矩阵否则 LQR 会以为角速度就是欧拉角变化率控制精度直接崩。从那以后我每次做六自由度控制都先把体坐标系和世界坐标系的转换关系在纸上推一遍再写代码。希望帮到你。本文还有配套的精品资源点击获取