Mujoco 实战解析：基于LQR的人形机器人单腿平衡控制与仿真实现

1. 从零开始Mujoco环境搭建与人形模型加载嘿朋友们今天咱们来点硬核的手把手带你用Mujoco实现一个超酷的项目让一个单腿站立的人形机器人保持平衡。听起来是不是有点科幻别担心跟着我的步骤走你也能在自己的电脑上跑起来。我当年第一次接触这个的时候也感觉一头雾水但实际做下来你会发现核心思路其实很清晰关键是动手去试。首先咱们得把“舞台”搭好。Mujoco是一个物理仿真引擎咱们的机器人就在这里面“活”起来。第一步你得去DeepMind的官方GitHub仓库把Mujoco的源码克隆下来。别被“源码”吓到其实就是把模型文件、示例代码这些资源拿到手。打开你的终端找个顺眼的目录执行下面这行命令git clone https://github.com/google-deepmind/mujoco克隆完成后你会看到一个名为mujoco的文件夹。这里面宝贝可多了有各种机器人模型、教程和Python接口。我们今天要用到的“单腿平衡人形机器人”模型就藏在mujoco/model/humanoid/这个路径下文件名叫humanoid.xml。这个模型已经预设好了关节、执行器就是电机和接触属性咱们直接拿来用就行。接下来在Python里咱们需要导入必要的库并把模型加载进来。我习惯先导入这些import mujoco import numpy as np import mediapy as media import matplotlib.pyplot as plt from scipy import linalg然后读取模型文件并创建仿真环境# 读取模型XML文件 with open(mujoco/model/humanoid/humanoid.xml, r) as f: xml f.read() # 创建模型和数据对象 model mujoco.MjModel.from_xml_string(xml) data mujoco.MjData(model) # 创建一个渲染器方便我们看效果 renderer mujoco.Renderer(model)这几行代码一执行机器人模型就算加载到内存里了。model对象描述了机器人的所有固定属性比如质量、惯性、关节限制而data对象则存储了仿真过程中动态变化的状态比如位置、速度、受力。你可以把model想象成机器人的“蓝图”data就是根据蓝图造出来的、正在运行的“实体”。模型加载好后咱们先让它动一下看看。Mujoco仿真的核心是mj_step函数它根据物理定律计算下一刻的状态。但在这之前通常需要先调用mj_forward来更新一下动力学相关的量。咱们先让机器人摆出它内置的一个关键帧姿势看看# 重置到第一个关键帧通常是站立姿势 mujoco.mj_resetDataKeyframe(model, data, 1) # 前向动力学计算 mujoco.mj_forward(model, data) # 更新渲染场景并显示 renderer.update_scene(data) media.show_image(renderer.render())如果一切顺利你应该能看到一个灰色的人形模型站在那里。但这时候它还没“活”因为没有给执行器发送任何控制信号。这个模型内置了一些执行器我们可以通过设置data.ctrl数组来驱动关节。先来个简单的测试给所有执行器随机信号看看它会怎么“抽搐”import time DURATION 2 # 仿真2秒 FRAMERATE 60 frames [] mujoco.mj_resetDataKeyframe(model, data, 1) while data.time DURATION: # 生成随机控制信号模拟噪声输入 data.ctrl np.random.randn(model.nu) # 执行一步仿真 mujoco.mj_step(model, data) # 每隔固定时间捕获一帧图像 if len(frames) data.time * FRAMERATE: renderer.update_scene(data) pixels renderer.render() frames.append(pixels) # 生成视频 media.show_video(frames, fpsFRAMERATE)运行这段代码你会看到一个手舞足蹈、最终瘫倒在地的机器人。这很正常因为随机信号就像胡乱扯动木偶的线不可能让它站稳。我们的目标就是设计一个聪明的大脑控制器来取代这些随机信号让机器人即使在受到干扰时也能稳住。1.1 理解模型状态与关键概念在深入设计控制器之前咱们必须搞清楚Mujoco里几个核心的“状态变量”这就像开车得先知道方向盘、油门和刹车在哪一样。最重要的两个是qpos和qvel。qpos是广义位置向量。对于这个人形机器人它的长度 (model.nq) 是23。这23个数字都代表啥前7个很特殊描述了机器人“根身体”通常是骨盆在三维空间中的全局位姿。它用一个三维坐标[x, y, z]和一个四元数[qw, qx, qy, qz]来表示。四元数可能有点抽象你可以简单理解为一种能避免“万向节锁”的、描述物体旋转的方式。剩下的16个就是各个关节的角度了比如髋关节的俯仰、翻滚膝关节的弯曲等。qvel是广义速度向量长度 (model.nv) 是22。注意这里长度和qpos不一样了因为四元数有4个数但描述旋转的速度角速度只需要3个数。所以qvel的前6个是根身体的线速度和角速度后16个是各个关节的角速度。那data.ctrl又是什么它是控制输入向量长度是model.nu这里是21。每个元素对应一个执行器电机的输出力矩。我们的控制器任务就是根据当前机器人的qpos和qvel也就是状态计算出这21个力矩值发送给data.ctrl从而产生稳定站立的动作。最后还有一个重要的函数mj_inverse它做的是“逆动力学”计算。给定一个姿态 (qpos) 和期望的加速度 (qacc)它能算出需要施加多少力 (qfrc_inverse) 才能产生这个加速度。这个函数在寻找平衡点时会非常有用咱们马上就会用到。理解这些基础概念就像拿到了工具箱里的扳手和螺丝刀后面组装“平衡控制器”这台精密机器时你才知道每个零件该往哪拧。2. 寻找黄金平衡点逆动力学的妙用要让机器人单腿站稳第一步不是急着设计复杂的控制算法而是先找到一个理想的“平衡点”。这个点就是机器人一个完美的站立姿势在这个姿势下如果没有任何干扰它只需要施加很小的关节力矩就能对抗重力保持静止。你可以把它想象成走钢丝时那个最省力、最稳定的身体中心位置。怎么找这个点呢官方教程里用了一个非常巧妙的方法核心就是利用刚才提到的mj_inverse函数。思路是这样的我们想让机器人静止也就是所有关节的加速度qacc为零。那么在某个姿势下调用逆动力学计算让qacc0得到的qfrc_inverse就是维持这个静止姿势所需要的内力。如果这个内力完全由执行器电机提供且在执行器的出力范围内那这个姿势就是一个可行的平衡点。但这里有个坑人形机器人是浮动的它的根身体骨盆在空间中是自由的。如果我们简单地把所有qacc设为零逆动力学计算会试图在根关节一个虚拟的、连接世界和根身体的关节上产生一个巨大的力来“固定”它这显然不现实因为根关节没有真实的执行器。所以我们需要微调机器人的整体高度让维持静止所需的内力尽可能小特别是根关节上的力要趋近于零。这样所有的力就都能由真实的腿部关节电机来提供了。我们来写代码实现这个过程。首先我们让机器人摆出一个大概的站立姿势使用关键帧1mujoco.mj_resetDataKeyframe(model, data, 1) mujoco.mj_forward(model, data)然后我们尝试微调机器人在垂直方向Z轴的高度。我们让高度在一个小范围内变化比如上下1毫米对于每个高度计算维持静止所需的内力特别是Z方向的分量height_offsets np.linspace(-0.001, 0.001, 2001) # -1mm 到 1mm取2001个点 vertical_forces [] # 用来记录每个高度对应的Z方向根关节力 for offset in height_offsets: mujoco.mj_resetDataKeyframe(model, data, 1) mujoco.mj_forward(model, data) data.qacc 0 # 设定加速度为零 data.qpos[2] offset # 调整高度qpos[2]是根身体的Z坐标 mujoco.mj_inverse(model, data) # 计算逆动力学 # qfrc_inverse[2] 是根身体在Z方向所需的力 vertical_forces.append(data.qfrc_inverse[2])计算完后我们找出使vertical_forces的绝对值最小的那个高度偏移量idx np.argmin(np.abs(vertical_forces)) best_offset height_offsets[idx] print(f找到的最佳高度偏移是{best_offset*1000:.4f} 毫米)在我的测试中这个值大约是 -0.45 毫米。这意味着把机器人模型从初始关键帧姿势再往下“按”0.45毫米它就能达到一个非常省力的平衡状态。我们可以画个图直观感受一下plt.figure(figsize(10, 6)) plt.plot(height_offsets * 1000, vertical_forces, linewidth3, label所需Z方向力) plt.axvline(xbest_offset*1000, colorred, linestyle--, labelf最佳偏移 {best_offset*1000:.2f}mm) # 计算机器人的总重量作为参考 total_mass model.body_subtreemass[1] # 根身体的子树质量即全身质量 weight total_mass * np.linalg.norm(model.opt.gravity) # 重力 质量 * 重力加速度 plt.axhline(yweight, colorgreen, linestyle--, labelf全身重量 ({weight:.1f} N)) plt.xlabel(高度偏移 (毫米)) plt.ylabel(所需垂直力 (N)) plt.grid(True, whichmajor, color#DDDDDD, linewidth0.8) plt.legend() plt.title(寻找最优平衡高度) plt.show()这张图会告诉你一个非常有趣的现象当机器人脚部接触地面时高度偏移为负需要很大的力把它“推上去”当脚离开地面时高度偏移为正又需要很大的力把它“拉下来”。而在中间某个点我们找到的红色虚线处所需的垂直力几乎为零并且等于机器人的自重绿色虚线。这说明在这个高度地面恰好提供了支撑力来平衡重力关节电机只需要处理维持姿势的内部力矩而不需要对抗重力。这就是我们要的平衡点qpos0。2.1 计算平衡点对应的稳态控制输入找到平衡位置qpos0后我们还需要知道在这个位置上各个执行器应该输出多大的稳态力矩ctrl0才能刚好产生维持姿势所需的内力。这相当于给控制器设定一个“基准线”。我们已经有了在qpos0处、加速度为零时的所需内力qfrc0就是上面计算出的qfrc_inverse。现在需要找到一个控制向量ctrl0使得执行器产生的力qfrc_actuator等于qfrc0。这涉及到一个从控制空间到力空间的映射矩阵在Mujoco里叫做actuator_moment矩阵。我们可以通过伪逆来求解这个最小二乘问题# 保存平衡点的状态 qpos0 data.qpos.copy() qfrc0 data.qfrc_inverse.copy() # 获取执行器力矩映射矩阵稀疏格式转密集格式 actuator_moment np.zeros((model.nu, model.nv)) mujoco.mju_sparse2dense( actuator_moment, data.actuator_moment.reshape(-1), data.moment_rownnz, data.moment_rowadr, data.moment_colind.reshape(-1), ) # 计算稳态控制输入ctrl0 qfrc0 * pinv(actuator_moment) ctrl0 np.atleast_2d(qfrc0) np.linalg.pinv(actuator_moment) ctrl0 ctrl0.flatten() print(稳态控制输入 ctrl0:, ctrl0)算出来之后我们可以验证一下把ctrl0赋给data.ctrl再做一次前向动力学计算看看执行器产生的力是否和预期的qfrc0匹配。data.ctrl ctrl0 mujoco.mj_forward(model, data) print(执行器实际产生的力:, data.qfrc_actuator) print(与预期力的差值:, data.qfrc_actuator - qfrc0)理论上对于完全驱动的关节比如手臂、腿这个差值应该非常小。对于根关节浮动基座由于没有执行器差值可能不为零但这部分力会由地面的接触力来提供。至此我们就得到了LQR控制器设计的两个核心基础平衡状态(qpos0, qvel0)和对应的平衡控制输入ctrl0。控制器的工作就是当机器人偏离这个状态时计算出额外的控制量-K * dx把它拉回来。3. LQR控制器设计如何量化“好”与“坏”好了现在我们知道机器人理想的站立姿势和对应的电机出力了。但现实是它总会因为各种原因偏离这个姿势比如风吹、地面不平或者我们故意推它一下。LQR线性二次型调节器就是一种非常经典的控制方法它能告诉我们当机器人偏离平衡点时每个电机应该额外增加或减少多少力矩才能用最“经济”的方式把它拉回来。LQR的核心思想是“权衡”。它通过两个矩阵来定义这种权衡状态代价矩阵 Q和控制代价矩阵 R。Q矩阵惩罚状态偏离平衡点的程度R矩阵惩罚我们使用控制力电机力矩的“代价”。控制器会寻找一个最优的增益矩阵K使得总的代价状态偏差 控制代价最小。你可以把它想象成开车Q大意味着你非常讨厌偏离车道中心宁愿多打方向盘R大意味着你非常珍惜汽油方向盘转猛了耗油所以你宁愿车子稍微偏一点也要保持平稳驾驶。首先设定R矩阵比较简单。因为控制输入的各个维度不同电机通常相互独立我们常将其设为单位矩阵表示对所有电机力矩的惩罚权重相同。nu model.nu # 控制输入维度21 R np.eye(nu) # 21x21的单位矩阵关键在于设计Q矩阵这直接决定了控制器关心什么。我们的目标是什么是让机器人不要摔倒。而摔倒最直接的体现就是质心CoM偏离支撑脚。所以我们首先要构造一个惩罚质心位置偏差的项。Mujoco提供了计算雅可比矩阵的函数能告诉我们身体某一点的速度与关节速度之间的关系。我们可以计算躯干质心相对于左脚质心的雅可比矩阵之差这个矩阵的转置乘以自身就得到了一个正定矩阵它能够度量质心相对于脚的位置偏差的“代价”。nv model.nv # 速度维度22 # 重置到平衡点 mujoco.mj_resetData(model, data) data.qpos qpos0 mujoco.mj_forward(model, data) # 计算躯干质心的雅可比矩阵 (3 x nv) jac_com np.zeros((3, nv)) mujoco.mj_jacSubtreeCom(model, data, jac_com, model.body(torso).id) # 计算左脚质心的雅可比矩阵 jac_foot np.zeros((3, nv)) mujoco.mj_jacBodyCom(model, data, jac_foot, None, model.body(foot_left).id) # 差异雅可比描述躯干质心相对于左脚质心的运动 jac_diff jac_com - jac_foot # 构造平衡代价矩阵Q_balance jac_diff^T * jac_diff Q_balance jac_diff.T jac_diff这个Q_balance矩阵非常大22x22它主要惩罚那些导致质心偏移的运动。但光有这个还不够我们可能还希望机器人的关节不要偏离初始姿势太多尤其是负责平衡的腿部和躯干关节。同时对于手臂这类对平衡影响较小的关节可以允许它们有更大的活动自由。我们需要根据关节的功能进行分组并赋予不同的权重。首先把关节自由度索引找出来# 根关节自由度前6个浮动基座 root_dofs list(range(6)) # 身体关节自由度剩下的 body_dofs list(range(6, nv)) # 通过关节名识别不同组别 joint_names [model.joint(i).name for i in range(model.njnt)] # 躯干关节abdomen排除Z轴旋转 abdomen_dofs [ model.joint(name).dofadr[0] for name in joint_names if abdomen in name and not z in name ] # 左腿关节hip, knee, ankle排除Z轴旋转 left_leg_dofs [ model.joint(name).dofadr[0] for name in joint_names if left in name and (hip in name or knee in name or ankle in name) and not z in name ] # 平衡相关关节 躯干 左腿 balance_dofs abdomen_dofs left_leg_dofs # 其他关节手臂、右腿等 other_dofs [dof for dof in body_dofs if dof not in balance_dofs]3.1 构建完整的Q矩阵并求解LQR增益现在我们可以构建一个针对关节位置偏差的惩罚矩阵Q_joint并对不同组别的关节赋予不同的权重系数。平衡相关关节的权重应该大一些其他关节小一些根关节浮动基座的权重设为0因为它的位置已经由质心代价项间接控制了。# 定义权重系数 BALANCE_COST 1000.0 # 质心平衡项的全局权重很大 BALANCE_JOINT_COST 3.0 # 平衡相关关节的权重 OTHER_JOINT_COST 0.3 # 其他关节的权重 # 初始化关节位置代价矩阵 Q_joint np.eye(nv) # nv x nv 单位矩阵 # 根关节代价设为0 Q_joint[root_dofs, root_dofs] * 0 # 平衡关节赋予较高代价 for i in balance_dofs: Q_joint[i, i] * BALANCE_JOINT_COST # 其他关节赋予较低代价 for i in other_dofs: Q_joint[i, i] * OTHER_JOINT_COST # 完整的位置状态代价矩阵Q_pos w_balance * Q_balance Q_joint Q_pos BALANCE_COST * Q_balance Q_joint注意我们的状态向量x实际上是[dq, qvel]其中dq是位置偏差经过特殊处理后面会讲qvel是速度。我们希望同时惩罚位置偏差和速度。因此完整的Q矩阵是一个2*nv乘2*nv的块对角矩阵左上角是Q_pos右下角是速度惩罚矩阵。为了简单我们常将速度惩罚也设为单位矩阵乘以一个系数这里我们先设为1。# 速度代价矩阵也设为对角阵系数暂取1 Q_vel np.eye(nv) # 构建完整的Q矩阵 Q np.block([ [Q_pos, np.zeros((nv, nv))], [np.zeros((nv, nv)), Q_vel] ])有了Q和R我们还需要系统在平衡点附近的线性化模型即状态矩阵A和控制矩阵B。它们描述了状态和控制输入如何影响下一个时刻的状态。Mujoco非常贴心地提供了mjd_transitionFD函数用有限差分的方法自动计算这两个矩阵。# 确保在平衡点和稳态控制输入处进行线性化 mujoco.mj_resetData(model, data) data.qpos qpos0 data.ctrl ctrl0 A np.zeros((2*nv, 2*nv)) B np.zeros((2*nv, nu)) epsilon 1e-6 # 有限差分的扰动步长 flg_centered True # 使用中心差分精度更高 # 调用函数计算A和B mujoco.mjd_transitionFD(model, data, epsilon, flg_centered, A, B, None, None)现在万事俱备我们可以求解离散时间代数Riccati方程得到那个梦寐以求的最优反馈增益矩阵K了。我们使用scipy.linalg.solve_discrete_are函数。from scipy import linalg # 求解Riccati方程得到P矩阵 P linalg.solve_discrete_are(A, B, Q, R) # 计算最优反馈增益矩阵 K K np.linalg.inv(R B.T P B) B.T P A print(fLQR增益矩阵K的形状{K.shape}) # 应该是 (21, 44)这个K矩阵就是控制器的“大脑”。它的每一行对应一个执行器电机每一列对应一个状态变量前22个是位置偏差后22个是速度。当机器人状态偏离平衡点dx时控制器就会计算u -K dx然后将ctrl0 u发送给电机。K矩阵里的每个数字都精确地告诉了我们“某个关节的角度偏了一点点某个电机应该增加多少力矩来纠正它”。4. 闭环仿真与调优实战让机器人真正站稳理论准备就绪是时候让我们的LQR控制器上阵了。我们将进入闭环仿真阶段也就是让控制器根据每一时刻机器人的状态实时计算控制命令并观察它能否在干扰下保持平衡。首先我们实现一个最简单的闭环仿真。这里有一个关键细节状态偏差dx中的位置部分不能直接用data.qpos - qpos0。因为qpos的前7个元素包含四元数直接相减没有几何意义。我们需要用mj_differentiatePos函数来计算两个位姿之间的“差异速度”这个差异在切空间里是一个3维向量对于旋转函数会帮我们处理这个复杂的数学问题输出一个nv维的向量dq这才是真正可用的位置偏差。DURATION 5 # 仿真5秒 FRAMERATE 60 # 初始化状态 mujoco.mj_resetData(model, data) data.qpos qpos0 # 设置到平衡点 dq np.zeros(model.nv) # 用于存储位置偏差的向量 frames [] while data.time DURATION: # 计算当前位姿与平衡点位姿的差异 (dq) mujoco.mj_differentiatePos(model, dq, 1, qpos0, data.qpos) # 构建完整状态偏差向量 dx [dq, qvel] dx np.hstack((dq, data.qvel)).T # 转置成列向量 # LQR控制律u ctrl0 - K * dx data.ctrl ctrl0 - K dx # 执行一步仿真 mujoco.mj_step(model, data) # 录制视频帧 if len(frames) data.time * FRAMERATE: renderer.update_scene(data) pixels renderer.render() frames.append(pixels) # 播放视频 media.show_video(frames, fpsFRAMERATE)运行这段代码你应该能看到一个奇迹机器人稳稳地单腿站立几乎不动这证明了我们的LQR控制器在理想环境下是有效的。但现实世界充满干扰为了测试控制器的鲁棒性我们需要给它加点“料”。4.1 注入噪声与可视化增强一个在平衡点附近轻微晃动的机器人比一个完全僵直的机器人更真实也更能体现控制器的能力。我们可以在控制输入上叠加一个平滑的随机噪声。同时为了更酷炫地展示效果我们还可以让摄像头绕着机器人旋转并开启接触力可视化。DURATION 12 TOTAL_ROTATION 15 # 摄像头总旋转角度 CTRL_NOISE_RATE 0.8 # 控制噪声的带宽系数 BALANCE_STD 0.01 # 平衡关节的噪声标准差小 OTHER_STD 0.08 # 其他关节的噪声标准差大一些 # 设置摄像机 camera mujoco.MjvCamera() mujoco.mjv_defaultFreeCamera(model, camera) camera.distance 2.3 # 摄像机距离 # 开启接触力可视化 scene_option mujoco.MjvOption() scene_option.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] True model.vis.map.force 0.01 # 接触力箭头缩放系数 # 定义两个辅助函数 def unit_smooth(t): 0到1的平滑过渡函数 return 1 - np.cos(t * 2 * np.pi) def azimuth(time): 根据时间计算摄像机方位角 return 100 unit_smooth(time / DURATION) * TOTAL_ROTATION # 生成平滑的随机噪声序列 np.random.seed(42) # 固定随机种子确保结果可复现 nsteps int(np.ceil(DURATION / model.opt.timestep)) perturb np.random.randn(nsteps, nu) # 生成原始高斯噪声 # 为不同关节分配不同的噪声强度 CTRL_STD np.empty(nu) for i in range(nu): joint_id model.actuator(i).trnid[0] # 执行器驱动的关节ID dof_id model.joint(joint_id).dofadr[0] # 关节对应的自由度ID # 如果是平衡相关关节噪声小否则噪声大 CTRL_STD[i] BALANCE_STD if dof_id in balance_dofs else OTHER_STD # 对噪声进行卷积平滑避免突变 width int(nsteps * CTRL_NOISE_RATE / DURATION) kernel np.exp(-0.5 * np.linspace(-3, 3, width)**2) kernel / np.linalg.norm(kernel) # 归一化 for i in range(nu): perturb[:, i] np.convolve(perturb[:, i], kernel, modesame) # 高分辨率渲染器 renderer mujoco.Renderer(model, width1280, height720) frames [] # 重新开始仿真 mujoco.mj_resetData(model, data) data.qpos qpos0 step 0 while data.time DURATION: # 计算状态偏差 mujoco.mj_differentiatePos(model, dq, 1, qpos0, data.qpos) dx np.hstack((dq, data.qvel)).T # LQR控制律 噪声 data.ctrl ctrl0 - K dx CTRL_STD * perturb[step] step 1 # 仿真步进 mujoco.mj_step(model, data) # 更新摄像机角度并渲染 if len(frames) data.time * FRAMERATE: camera.azimuth azimuth(data.time) renderer.update_scene(data, camera, scene_option) pixels renderer.render() frames.append(pixels) media.show_video(frames, fpsFRAMERATE)运行这段增强版的代码你会看到一个更生动的场景机器人在轻微地左右摇摆、调整手臂以保持平衡脚底与地面之间有彩色的力箭头显示红色代表压力大。摄像机缓缓环绕让你能从各个角度欣赏它的“舞姿”。这证明我们的LQR控制器不仅能维持静态平衡还能在一定程度上抵抗持续的随机扰动。4.2 参数调优经验谈Q和R的博弈看到这里你可能已经成功了。但如果你发现机器人站得不够稳或者关节抖动得很厉害那就需要调整Q和R矩阵中的权重参数了。这是我踩过不少坑总结出来的经验增大BALANCE_COST如果机器人容易向某个方向摔倒说明质心偏差惩罚不够。大胆提高BALANCE_COST比如从1000调到5000甚至10000。这会让控制器不惜一切代价把质心拉回脚掌上方。调整BALANCE_JOINT_COST和OTHER_JOINT_COST如果负责平衡的主要关节如支撑腿的踝、膝、髋晃动太大就增加BALANCE_JOINT_COST。如果手臂摆动过于剧烈看起来不自然可以适当增加OTHER_JOINT_COST来抑制它们。调整速度惩罚我们在构造Q矩阵时速度惩罚Q_vel用的是单位矩阵。如果机器人收敛到平衡点的过程太慢或者有持续的低频振荡可以尝试增大速度惩罚的对角线值比如乘以5或10。这相当于给系统增加了“阻尼”能让它更快地安静下来。调整R矩阵如果你发现某些电机输出力矩非常大甚至饱和了可以单独增大R矩阵中对应那个电机的对角线元素。这告诉控制器“用这个电机要更省着点”它会寻找更温和的控制策略。调参是个耐心活没有银弹。我的建议是每次只改一个参数观察仿真效果并记录下机器人的质心轨迹、关节角度和控制器输出。多用matplotlib画图分析比如绘制质心在水平面(x, y)的轨迹如果它始终在一个小范围内波动说明控制器是有效的。通过这样的迭代你会对“代价”如何影响“行为”有更深刻的直觉理解。最终你会得到一个在扰动下依然优雅屹立的单腿平衡机器人这份成就感就是对我们这些开发者最好的奖励。