四足机器人单刚体质心模型 MPC 控制项目

1. 项目介绍

本项目实现了一个基于单刚体质心模型 (Single Rigid Body Centroidal Model, SRBCM) 的模型预测控制 (MPC) 算法，用于控制仿真环境中的 Aliengo 四足机器人。控制器通过优化未来一段时间内的足端接触力，以实现期望的机器人运动，如站立、前进、侧向移动和旋转等。项目使用 MuJoCo 作为物理仿真引擎，Pinocchio 用于机器人动力学计算。

该项目的主要目的是演示如何将 SRBCM 与 MPC 相结合，应用于四足机器人的动态运动控制。通过精心设计的步态库和实时调整的 MPC 参数，机器人能够展现出稳定且灵活的运动能力。用户可以通过键盘实时控制机器人的运动目标。

2. 核心理论与模型

2.1 单刚体质心模型 (SRBCM - Single Rigid Body Centroidal Model)

单刚体质心模型是四足机器人控制中常用的一种简化模型。它将整个机器人（包括躯干和四条腿）近似为一个单一的刚体，其运动由质心的平动和绕质心的转动来描述。在这个模型中，腿的质量通常被忽略或集中到主躯干上。

该模型的状态量通常包括：

• 质心姿态：通常用欧拉角（滚转、俯仰、偏航）表示 (\Theta = [\phi, \theta, \psi]^T)。
• 质心位置：在世界坐标系中的位置 (\mathbf{p}_c = [x_c, y_c, z_c]^T)。
• 质心线速度和角速度：分别为 (\mathbf{v}_c = [\dot{x}_c, \dot{y}_c, \dot{z}_c]^T) 和 (\omega_c = [\dot{\phi}, \dot{\theta}, \dot{\psi}]^T)。在本项目中，为了与机身坐标系下的角速度对应，可能会进行坐标变换。
• （可选）重力加速度向量：有时作为一个状态量以简化动力学方程。

SRBCM 的动力学方程可以表示为： [ m\ddot{\mathbf{p}}c = \sum{i=1}^{4} \mathbf{f}_i + m\mathbf{g} ] [ \mathbf{I}_c \dot{\omega}_c + \omega_c \times (\mathbf{I}c \omega_c) = \sum{i=1}^{4} (\mathbf{r}_i \times \mathbf{f}_i) ] 其中：

• (m) 是机器人总质量。
• (\mathbf{p}_c) 是质心位置。
• (\mathbf{f}_i) 是第 (i) 条腿足端施加于地面的接触力（或地面反作用力）。
• (\mathbf{g}) 是重力加速度向量。
• (\mathbf{I}_c) 是机器人在质心处的转动惯量张量。
• (\omega_c) 是机身的角速度。
• (\mathbf{r}_i) 是从质心到第 (i) 条腿足端的向量。

在MPC控制器中，这些连续时间动力学方程会被离散化，并线性化（如果需要）以构建预测模型。本项目中，状态向量 (\mathbf{x}) 包含姿态（欧拉角）、位置、旋转后的角速度、旋转后的线速度以及重力项，共13个维度。控制输入 (\mathbf{u}) 是四条腿的足端力，每条腿3个维度，共12个维度。

2.2 Pinocchio 机器人动力学库

Pinocchio 是一个主要用 C++ 编写的机器人动力学算法库，并提供了 Python 绑定。它能够高效地计算机器人的正逆运动学、正逆动力学、雅可比矩阵、非线性效应（科氏力、离心力、重力项）等。

在本项目中，Pinocchio (通过 scripts/aliengoDynamic.py) 主要用于：

1. 加载机器人模型：从 URDF (Unified Robot Description Format) 文件 (models/urdf/aliengo.urdf) 加载 Aliengo 机器人的运动学和动力学参数。
2. 正向运动学：根据给定的关节角度（广义坐标 q）和关节速度（广义速度 v），计算机器人各连杆（特别是足端）在世界坐标系中的位置、姿态和速度。
3. 雅可比矩阵计算：计算足端相对于关节速度的雅可比矩阵。这对于将关节空间的变化映射到操作空间（足端）的变化至关重要，并在控制中用于力/力矩的传递。代码中计算的是 LOCAL_WORLD_ALIGNED 雅可比，用于获取足端在世界坐标系下表达的，由关节运动引起的速度。
4. 参考系位姿更新：更新模型中所有定义的参考系（frames）的位姿。
5. 非线性效应计算：计算 (C(q,\dot{q})\dot{q} + g(q))，即科氏/离心力和重力项。虽然在SRBCM的MPC中不直接使用完整动力学的这些项，但Pinocchio的计算结果可以用于更复杂的控制器或模型验证。
6. 足端位置和速度获取：getLegMotionLinear 方法用于获取在机身坐标系下表达的，从机身中心到各足端的向量以及它们的相对线速度。这些信息在 run.py 中用于更新 MPC 模型中的 (B) 矩阵（特别是足端位置向量 (\mathbf{r}_i) 用于构建叉乘矩阵）以及用于摆动腿的PD控制。

Pinocchio 通过高效的算法（如 Featherstone 算法）进行动力学计算，使得在控制循环中实时获取机器人状态信息成为可能。

2.3 MuJoCo 物理仿真引擎

MuJoCo (Multi-Joint dynamics with Contact) 是一个专注于快速、准确模拟多关节动力学和接触的物理引擎。它广泛应用于机器人学、生物力学、机器学习等领域。

在本项目中，MuJoCo (run.py 中使用) 的核心作用是：

1. 仿真环境：提供一个虚拟环境来模拟 Aliengo 机器人的行为。模型从 MJCF (MuJoCo XML Format) 文件 (models/mjcf/aliengo.xml) 加载。
2. 物理仿真：根据施加到机器人关节的控制指令（本项目中是计算得到的关节力矩），模拟机器人的动态响应，包括与地面的接触和摩擦。
3. 传感器数据获取：提供模拟的传感器数据，如：
   • 足底接触传感器 (Touch_LF, Touch_LH, Touch_RF, Touch_RH)：用于判断足底是否接触地面以及接触力的大小（尽管代码中似乎是根据一个阈值判断接触状态）。
   • 机身 IMU 数据：陀螺仪 (Body_Gyro，代码中也直接从 mj_data.qvel[3:6] 获取角速度) 和加速度计 (Body_Acc)。
   • 关节状态：关节角度 (mj_data.qpos) 和关节角速度 (mj_data.qvel)。
4. 可视化：通过 mujoco.viewer 提供一个交互式的3D可视化界面，可以实时观察机器人的运动状态。

MuJoCo 允许设置详细的模型参数（质量、惯性、接触参数、关节限制等）和仿真参数（时间步长等），为开发和测试机器人控制算法提供了一个强大的平台。

2.4 四足机器人运动模型与控制

2.4.1 运动学

机器人的运动学描述了其几何结构和运动关系，不考虑力。

• 正向运动学 (FK)：由关节角度计算足端（或其他末端执行器）在特定坐标系下的位置和姿态。Pinocchio 用于此计算。
• 逆向运动学 (IK)：由期望的足端位置和姿态计算所需的关节角度。项目中 scripts/aliengoDynamic.py 包含一个基于几何的 legIK 函数，用于单腿的逆运动学求解。legsIk 方法则将其应用于四条腿。摆动腿的目标轨迹由步态规划器生成，然后通过IK解算得到目标关节角度，这些目标角度会用于VMC (虚拟模型控制) 中的PD控制器。

2.4.2 动力学

动力学研究力与运动之间的关系。

• 质心动力学 (Centroidal Dynamics)：如 2.1 节所述，是本项目MPC控制的基础。
• 完整动力学 (Whole-Body Dynamics)：考虑所有连杆的质量和惯性，以及它们之间的相互作用。Pinocchio 能够计算完整动力学，但本项目主要采用简化的SRBCM进行控制。

2.4.3 控制方式

• 模型预测控制 (MPC)：见 2.6 节。核心思想是基于模型预测未来的系统行为，并通过优化控制输入来最小化一个代价函数。
• 虚拟模型控制 (VMC) / 足端阻抗控制：对于摆动腿，本项目采用了一种类似VMC的方法。run.py 中，摆动腿的控制力由两部分组成：一部分是固定的前馈力 legvmc_swing_feedforward，另一部分是基于PD控制的反馈力。PD控制器试图跟踪由步态规划器 (GaitSchedule) 生成的期望足端轨迹（通过IK转换为期望关节角度，或直接在操作空间计算误差）。 [ \mathbf{\tau}{swing,i} = J_i^T ( \mathbf{K}p (\mathbf{p}{des,i} - \mathbf{p}{cur,i}) + \mathbf{K}d (\mathbf{v}{des,i} - \mathbf{v}{cur,i}) + \mathbf{f}{ff,i} ) ] (注意：代码中PD控制似乎是直接作用于关节位置误差和速度误差，然后转换为足端力，或者直接计算足端空间的PD力) 在 run.py 中，最终的关节控制力矩由雅可比转置乘以计算得到的足端力（MPC输出的支撑相力或VMC计算的摆动相力）得到： mj_data.ctrl = (Jacobians_pin.T @ leg_force_world).flatten() (虽然 run.py 的最新版本似乎直接计算 leg_force_world 然后通过其他方式映射到 mj_data.actuator_force)。 实际上，run.py 中计算 leg_force_world 时，支撑相的力直接取自MPC的输出（经过坐标变换），摆动相的力由PD控制器（跟踪 legvmc_target_posi 和 legvmc_target_vel）和前馈力 legvmc_swing_feedforward 组成。然后，这些计算出的足端空间力 leg_force_world 通过雅可比转置映射到关节力矩，并施加到MuJoCo仿真中的执行器上。

2.5 步态库生成与调度

步态是四足机器人运动的基础，它定义了腿部在支撑相和摆动相之间的切换模式和时序。

2.5.1 步态定义 (scripts/gaits.py)

此文件定义了多种预设步态，例如：

• standing_gait (站立)
• trot_gait (对角小跑)
• walk_gait (行走)
• 等其他步态。

每种步态由两部分构成：

1. _modeSequence: 一个列表，每个元素是长度为4的列表 [LF, LH, RF, RH] (代码注释提到仿真器顺序是 LF, LB, RF, RB，但从 GaitSchedule 中对 alphas 的使用来看，更像是 LF, RF, LH, RH 或者需要根据 aliengoDynamic 中 endEffectorFrameIndices 的顺序来确定)。其中 1 代表该腿处于支撑相（接触地面），0 代表摆动相。这个序列定义了一个完整步态周期内各个时间段的足接触模式。
2. _switchingTimes: 一个列表，定义了 _modeSequence 中各个模式的切换时间点。

例如，trot_modeSequence 和 trot_switchingTimes 定义了对角小跑步态，其中对角的两腿（如LF和RH）同时摆动，另外两腿（LH和RF）同时支撑，然后交替。

2.5.2 步态调度与轨迹生成 (scripts/gaitSchedule.py)

GaitSchedule 类负责根据当前时间和期望的机身运动速度（前进、侧向、旋转）来动态生成摆动腿的足端轨迹。

• 初始化：接收一个步态定义（如 trot_modeSequence 和 trot_switchingTimes）、摆动腿抬起高度 height 等参数。
• getGait(t, baseVx, baseVy, baseOmegaZ) 方法:
  1. 根据当前时间 t 和步态定义，确定当前处于哪个步态相位 (mode)，以及在该相位内的时间进度 phaseTime 和相位总时长 modePeriod。
  2. 对于摆动腿：
     • 计算目标足端轨迹。该轨迹由几部分组成：
       • 基础摆动：使用摆线函数 (cycloidValueX 和 cycloidValueZ) 生成平滑的抬腿和前伸/后摆运动。抬腿高度由 self.height 控制。
       • 机身平移补偿：摆动腿的目标落足点会根据机身的期望平移速度 baseVx, baseVy 进行调整，以使机器人能够稳定前进或侧移。计算为 -modePeriod * baseV_xy * 0.5 作为起点偏置，并通过 cycloidValueX 的总位移参数 modePeriod * baseV_xy 实现。
       • 机身旋转补偿：摆动腿的目标落足点还会根据机身的期望旋转速度 baseOmegaZ 进行调整。通过计算旋转引起的足端期望位移 rotDist 来实现，确保落足点能配合身体的旋转。
     • 输出该摆动腿在当前时刻的目标足端位置 target_q (3D) 和目标足端速度 target_v (3D) （相对于机身）。
  3. 对于支撑腿，其目标足端位置和速度在此模块中通常设为零或不明确指定，因为它们的行为主要由MPC控制器通过优化地面反作用力来决定。

run.py 中，gaitSche = GaitSchedule(trot_modeSequence, trot_switchingTimes, height=0.04) 初始化了一个使用Trot步态的调度器。在主循环中，调用 gaitSche.getGait(...) 来获取当前的目标步态模式 gait_mode、摆动腿目标位置 gait_tq 和速度 gait_tv。这些信息随后用于设置MPC的约束（哪些腿可以产生力）和VMC的目标。

2.6 MPC 控制算法 (scripts/mpc.py 和 doc/MPC.md)

模型预测控制 (Model Predictive Control, MPC) 是一种先进的控制策略，它通过以下步骤工作：

1. 预测：使用系统的动态模型，在当前时刻预测系统未来一段时间（称为预测时域 (N)）的行为。
2. 优化：在一个有限的预测时域内，通过求解一个优化问题来计算最优的控制输入序列。优化目标通常是最小化一个预定义的代价函数，该代价函数衡量了系统状态与期望状态的偏差以及控制输入的能耗。
3. 执行：将计算得到的最优控制序列的第一个元素施加到实际系统上。
4. 滚动时域：在下一个采样时刻，获取新的系统状态测量值，并重复以上步骤。

2.6.1 MPC 公式推导 (doc/MPC.md)

该文档详细推导了线性MPC如何转化为一个二次规划 (QP) 问题。

• 系统模型：离散时间线性系统 (\mathbf{x}(k+1) = \mathbf{A}\mathbf{x}(k) + \mathbf{B}\mathbf{u}(k))。
• 预测方程：未来 (N) 步的状态序列 (\mathbb{X}_k) 可以表示为当前状态 (\mathbf{x}_k) 和未来控制序列 (\mathbb{U}_k) 的线性组合：(\mathbb{X}_k = \mathbf{M} \mathbf{x}_k + \mathbf{C} \mathbb{U}_k)。
  • (\mathbf{M}) 将当前状态映射到未来状态。
  • (\mathbf{C}) 将控制输入映射到未来状态。
• 代价函数：通常是二次形式，惩罚状态误差和控制量： [ J = \sum_{i=0}^{N-1} (\mathbf{x}(k+i|k) - \mathbf{x}{ref}(k+i|k))^T \mathbf{Q} (\mathbf{x}(k+i|k) - \mathbf{x}{ref}(k+i|k)) + \mathbf{u}(k+i|k)^T \mathbf{R} \mathbf{u}(k+i|k) ] [ + (\mathbf{x}(k+N|k) - \mathbf{x}_{ref}(k+N|k))^T \mathbf{Q}N (\mathbf{x}(k+N|k) - \mathbf{x}{ref}(k+N|k)) ] 其中 (\mathbf{Q}), (\mathbf{R}) 是权重矩阵，(\mathbf{Q}_N) 是终端权重矩阵。
• QP 形式：将代价函数代入预测方程，可以将其转化为关于控制序列 (\mathbb{U}k) 的标准 QP 问题： [ \min{\mathbb{U}_k} \frac{1}{2} \mathbb{U}_k^T \mathbf{H} \mathbb{U}_k + \mathbf{g}^T \mathbb{U}_k ] 受限于： [ \mathbf{L} \mathbb{U}_k \le \mathbf{k} ] (这里的 (\mathbf{H}) 和 (\mathbf{g}) 矩阵由 (\mathbf{M}, \mathbf{C}, \mathbf{Q}, \mathbf{R}, \mathbf{Q}_N) 和当前状态 (\mathbf{x}k), 参考轨迹 (\mathbf{x}{ref}) 导出。)

2.6.2 项目中的 MPC 实现 (scripts/mpc.py)

该文件定义了 Mpc 基类和 SRBCM_Mpc 子类。

• Mpc 类：
  • 实现了通用的MPC框架，根据 (\mathbf{A}, \mathbf{B}, N), 状态/控制约束, 和 (\mathbf{Q}, \mathbf{R}, \mathbf{Q}_N) 权重，预计算QP问题中的矩阵 (\mathbf{H}) (代码中 mat_HH) 和与当前状态相关的线性项构建块 (\mathbf{E}) (代码中 mat_EE)。
  • setTarget(x_tar): 设置目标状态 (\mathbf{x}_{ref})，并计算QP代价函数中的线性项 mat_qqTC (对应 (\mathbf{g}) 中的一部分)。
  • solve(x): 输入当前状态 x，构建并使用 qpsolvers.solve_qp (指定 quadprog 求解器) 求解QP问题，返回整个预测时域的控制序列 (\mathbb{U}_k)。
• SRBCM_Mpc 类：
  • 继承自 Mpc，专门为单刚体质心模型设计。
  • 增加了摩擦锥约束：这是一个关键部分，确保足端力在物理上可行。摩擦锥约束通常表示为 (\sqrt{f_x^2 + f_y^2} \le \mu f_z)，其中 (\mu) 是摩擦系数，(f_x, f_y, f_z) 是足端接触力的三个分量。为了适应线性QP求解器，这个非线性约束通常被线性化为一个多边形逼近（金字塔形状），如代码中 frictionPyramic 矩阵所示： [ \begin{bmatrix} 0 & 0 & -1 \\ 1 & 0 & -\mu \\ -1 & 0 & -\mu \\ 0 & 1 & -\mu \\ 0 & -1 & -\mu \\ \end{bmatrix} \begin{bmatrix} f_x \\ f_y \\ f_z \end{bmatrix} \le \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} ] 这个约束确保 (f_z > 0) (压力) 且水平力在摩擦极限内。
  • 这些线性化的摩擦锥约束被添加到 QP 问题的约束矩阵 qp_mat_G 中。
  • whole_time_friction 参数控制摩擦约束是应用于整个预测时域还是仅应用于预测时域的初始部分（如第一个控制步）。在 run.py 中，当 whole_time_friction=False 时，似乎是只对第一个控制步的力施加摩擦约束，这可能是为了简化或加速计算。

2.6.3 run.py 中的 MPC 应用

• 模型参数：
  • 状态 (\mathbf{x}): [roll, pitch, yaw, px, py, pz, omega_x_body, omega_y_body, omega_z_body, vx_body, vy_body, vz_body, g_const] (13维)。姿态和位置是世界系，角速度和线速度是经过机身姿态旋转到特定参考系（可能是水平对齐的机身系）下的。
  • 控制 (\mathbf{u}): [f_LFx, f_LFy, f_LFz, f_LHx, ..., f_RHz] (12维)，即四条腿的足端力。
  • (A, B) 矩阵：根据 SRBCM 的离散化和线性化动力学构建。(B) 矩阵的构建涉及到机器人当前的姿态（旋转矩阵 (R_{MatBase}) 和 (R_{MatZ})）、足端相对机身的位置向量 ((TransBase2ends)) 和转动惯量。这些参数是时变的，因此 (A) 和 (B) 矩阵的部分元素会在每个控制周期更新。
• 权重和约束：
  • mpc_Q_stance, mpc_QN_stance (站立时) 和 mpc_Q_move_flat, mpc_QN_move_flat (运动时) 定义了不同状态下的状态和终端状态权重。
  • mpc_R 定义了控制输入的权重。
  • mpc_xmin, mpc_xmax 定义了状态量的约束。
  • mpc_umin, mpc_umax 定义了控制输入的约束（足端力的大小限制）。
• 目标状态 xtarget：根据键盘输入（前进速度 target_base_trot_vforward，旋转速度 target_base_trot_omegaZ 等）和步态周期动态更新。例如，期望的偏航角会随着时间累积，期望的水平位置也会根据速度目标进行累积。
• 动态调整 B 矩阵：根据当前步态模式 (gait_mode)，仅将处于支撑相的腿对应的 (B) 矩阵列向量包含在优化问题中 (mpc_B_impl)。摆动腿不产生地面反作用力。
• 求解与执行：调用 mpc_node.solve(mpc_x.flatten()) 求解QP，得到未来 (N) 步的最优足端力序列。仅取第一个时间步的力 sol[:mpc_node.dim_n] 作为当前时刻的期望支撑力。

3. 相关依赖和安装方式

建议在python3.8环境中使用，其他版本python可能会遇到Pinocchio的未知错误，本项目主要依赖以下库：

• MuJoCo: 物理仿真引擎。
  • 安装说明请参考其官方网站：MuJoCo Installation
  • 通常需要下载对应的二进制文件并可能需要许可证（自2021年10月起，MuJoCo被DeepMind收购后已可免费使用）。
• mujoco-python: MuJoCo 的 Python 绑定。
  • pip install mujoco
• Pinocchio: 机器人动力学库。
  • 安装说明请参考其官方文档：Pinocchio Installation
  • 通常可以通过包管理器（如 apt on Ubuntu）或从源码编译安装。例如，在Ubuntu上：

    sudo apt install robotpkg-py3X-pinocchio # X为python版本，如py38

    或者使用 conda:

    conda install -c conda-forge pinocchio

• NumPy: 科学计算库。
  • pip install numpy
• SciPy: 科学计算库，本项目中用于旋转变换。
  • pip install scipy
• QP Solvers (qpsolvers): 用于求解 MPC 中的二次规划问题。项目中指定使用 quadprog 作为求解器。
  • pip install qpsolvers[quadprog] (安装 qpsolvers 并包含 quadprog 依赖) 或者 pip install qpsolvers quadprog
• Keyboard: 用于监听键盘输入以控制机器人。
  • pip install keyboard (在Linux上可能需要root权限或特定的udev规则配置才能全局监听键盘)

所有Python依赖项及其建议版本已在 requirements.txt 中列出：

mujoco
numpy
scipy
qpsolvers
quadprog
keyboard
pinocchio

可以通过以下命令安装：

pip install -r requirements.txt

注意: Pinocchio 和 MuJoCo 的安装可能需要额外的系统依赖或配置，请务必参考它们各自的官方文档。对于Pinocchio，确保Python绑定与其C++核心库版本兼容。

4. 使用说明：run.py

run.py 是项目的主执行文件。它集成了机器人模型加载、传感器数据处理、状态估计（主要是姿态和速度的获取与转换）、步态调度、MPC控制器构建与求解、以及将控制指令发送到MuJoCo仿真器的整个流程。

4.1 运行脚本

直接在终端中运行：

python run.py

4.2 控制方式

脚本启动后，会打开一个 MuJoCo 的可视化窗口。机器人可以通过键盘进行控制：

• ↑: 增加前进速度。
• ↓: 减少前进速度（或后退）。
• ←: 增加向左的角速度（逆时针旋转）。
• →: 增加向右的角速度（顺时针旋转）。
• Space (空格键): 切换是否进入/退出 Trot (小跑) 模式。当不处于 Trot 模式时，机器人尝试保持站立。

控制指令会更新MPC的目标状态（如期望的前进速度 target_base_trot_vforward 和期望的角速度 target_base_trot_omegaZ）。

4.3 代码主要流程 (run.py 中的 if __name__ == "__main__":)

1. 初始化:

   • 加载 Aliengo 的 URDF (models/urdf/aliengo.urdf) 用于 Pinocchio 动力学计算 (AliengoDynamic)。
   • 加载 Aliengo 的 MJCF (models/mjcf/aliengo.xml) 用于 MuJoCo 仿真。
   • 设置机器人初始姿态和关节位置。
   • 初始化步态调度器 (GaitSchedule)，默认使用 Trot 步态。
   • 初始化低通滤波器 (LowpassFilter) 用于平滑速度估计。
   • 设置MPC参数：预测时域 mpc_N，SRBCM模型参数 ((A, B) 矩阵的骨架)，权重矩阵 ((Q, R, Q_N))，状态和控制的约束。
   • 初始化键盘监听器 (KeyboardListener)。

2. 主控制循环 (while viewer.is_running():):

   • 键盘输入处理: 从 KeyboardListener 获取用户输入的控制指令（是否小跑、线速度x、角速度z）。更新目标速度 target_base_trot_vforward, target_base_trot_omegaZ。
   • 状态获取与估计:
     • 从 mj_data 获取传感器信息：足底接触状态、陀螺仪、加速度计、关节角度和速度。
     • 计算当前机身的姿态（RPY欧拉角）和位置。
     • 对速度进行低通滤波。
     • 将状态量（姿态、位置、速度）转换到 Pinocchio 和 MPC 所需的格式。特别地，线速度和角速度会通过当前机身姿态的旋转矩阵转换到特定参考系下（通常是与世界坐标系Z轴对齐，但XY平面随机器人偏航角旋转的坐标系）。
   • 动力学更新 (Pinocchio):
     • 调用 quadDyn.update(pin_q, pin_v) 更新 Pinocchio 模型的状态，计算雅可比矩阵、足端相对机身的位置和速度 (TransBase2ends, VLinearBase2ends)、机身旋转矩阵 RMatBase 等。
   • MPC 参数更新:
     • 根据当前的机器人状态（如旋转矩阵 RMatBase, RMatZ，足端位置 TransBase2ends）更新MPC模型中的 (A) 矩阵和 (B) 矩阵的部分元素。这是因为 SRBCM 的线性化模型依赖于当前姿态和足端位置。
   • 步态逻辑与MPC目标设定:
     • 如果处于 Trot 模式 (start_trot 为 True)：
       • 调用 gaitSche.getGait(...) 获取当前步态相位 gait_mode (哪些腿支撑/摆动)，以及摆动腿的目标足端轨迹 (gait_tq, gait_tv)。
       • 更新 MPC 的目标状态 xtarget：期望的偏航角、位置会根据目标速度和预测时长进行累积。
       • 选择运动时的MPC权重 mpc_Q_move_flat, mpc_QN_move_flat。
       • 构建实际用于QP的控制输入矩阵 mpc_B_impl，只包含当前支撑腿对应的列。
       • 更新步态时间 trot_time。
     • 如果处于站立模式：
       • 所有腿都视为支撑相 (gait_mode = [1,1,1,1])。
       • 选择站立时的MPC权重 mpc_Q_stance, mpc_QN_stance。
       • mpc_B_impl 使用完整的 (B) 矩阵。
   • MPC 求解:
     • 实例化 SRBCM_Mpc 控制器 mpc_node (如果参数有变，或者在循环外实例化一次)。
     • 设置MPC的当前状态 mpc_x (由姿态、位置、转换后的速度等构成)。
     • 设置MPC的目标状态 xtarget。
     • 调用 mpc_node.solve(mpc_x) 求解QP，得到优化后的足端力序列 sol。
     • 提取第一步的控制输入 mpc_u = sol[:mpc_node.dim_n]，这些是期望的支撑腿作用于地面的反作用力（在特定坐标系下）。
   • 计算关节控制指令 (VMC for swing legs, MPC output for stance legs):
     • 遍历四条腿：
       • 支撑腿 (legmode 为 1): 期望足端力直接来自MPC的输出 mpc_u (注意符号和坐标系可能需要调整)。
       • 摆动腿 (legmode 为 0):
         • 期望足端力由PD控制器计算，跟踪由 GaitSchedule 生成的 legvmc_target_posi (减去初始足端偏移并加上当前足端位置) 和 legvmc_target_vel。
         • 还会加上一个前馈力 legvmc_swing_feedforward。
     • 将计算得到的四条腿的期望足端力 leg_force_world (在世界坐标系或某个统一坐标系下) 存储起来。
   • 力矩计算与仿真步进:
     • 使用Pinocchio计算的雅可比矩阵的转置 Jacobians_pin.T (应该是每个腿的雅可比)，将足端力 leg_force_world 映射为关节力矩 tau。 tau_i = J_i^T f_i
     • 将计算得到的关节力矩（或者直接是计算出的足端力，如果MuJoCo模型中执行器是直接施加足端力的话，但通常是关节力矩）设置到 mj_data.ctrl 或 mj_data.actuator_force。 根据 run.py 中的 setCtrlParam 和 mj_model.actuator_gainprm 的设置，mj_data.ctrl 更有可能是目标关节位置或速度，而实际的力/力矩由底层的PD控制器或执行器模型产生。然而，代码的后续部分又明确计算 leg_force_world 并似乎打算将其转换为关节力矩。这一块的具体映射需要仔细查看 setCtrlParam 的作用以及MuJoCo模型中执行器的定义。如果 actuator_gainprm 被设为0，则 mj_data.ctrl 可能直接作为力/力矩输入。
     • 在 run.py 中，mj_model = setCtrlParam(mj_model, 0., 0.) 表明执行器的 gain 都被设为0，这意味着 mj_data.ctrl[i] 理论上应该直接作为关节i的力/力矩。但后面又出现了 mj_data.actuator_force[:12] = (Jacobians_pin.T @ leg_force_world).flatten() 这样的代码，这表明是直接设置执行器力。
     • 执行 mujoco.mj_step(mj_model, mj_data) 进行一步物理仿真。
   • 同步与渲染:
     • 控制仿真速度以匹配期望的渲染帧率 (render_gap)。
     • 调用 viewer.sync() 更新可视化窗口。

4.4 注意事项

• 坐标系：代码中涉及多个坐标系（世界系、机身系、Pinocchio使用的坐标系、MPC状态表示中的坐标系）。理解这些坐标系之间的转换关系对于理解算法至关重要。例如，RMatBase 是机身姿态的旋转矩阵，RMatZ 用于将角速度从世界系转换到某个投影平面。MPC状态中的速度和角速度都经过了 RMatBase 的转换。
• 参数调整：MPC的性能高度依赖于权重矩阵 ((Q, R, Q_N))、预测时域 (N)，以及SRBCM模型的准确性。这些参数可能需要根据具体任务和机器人特性进行调整。
• EKF/滤波器：aliengoEKF.py 中定义了 LowpassFilter，用于平滑速度测量值。更复杂的系统可能会使用EKF（如该文件中也定义的 AliengoEKF 类，但未在 run.py 中启用）来估计机器人的完整状态，特别是当传感器噪声较大或存在模型不确定性时。

5. 其他可能的改进与扩展

• 更精确的状态估计：使用 AliengoEKF 或其他滤波器融合IMU和运动学信息，以获得更准确的机身姿态、位置和速度估计。
• 全身控制 (WBC)：结合MPC的质心轨迹规划和全身控制器，以更精确地分配关节力矩并处理更复杂的约束。
• 自适应步态：根据地形或任务动态调整步态参数（如步长、步高、步态周期）。
• 在线足端轨迹优化：不仅仅使用预定义的摆线，而是根据环境和任务优化摆动腿的轨迹。
• 鲁棒性增强：考虑模型不确定性和外部扰动，设计更鲁棒的控制器。

希望这份文档能帮助您理解本项目的结构和核心算法！