VBot四足机器人 Convex MPC 控制项目分析

1. 项目概述

1.1 项目目标

本项目基于 Unitree Go2 四足机器人的 Convex MPC（凸模型预测控制）开源项目，将其适配到 VBot 四足机器人 模型上。核心目标是保留原项目的 Convex MPC 控制算法，将硬绑定 Unitree Go2 的机器人模型接口替换为 VBot 的模型接口。

1.2 核心设计思想

text 复制代码

MuJoCo 负责"物理世界"，Pinocchio 负责"机器人身体计算"，MPC 负责"未来接触力规划"

MuJoCo：加载完整 VBot MJCF 模型，执行物理仿真（碰撞检测、接触动力学、积分）
Pinocchio：加载精简 VBot MJCF 模型，执行运动学、动力学、质心和雅可比计算
CentroidalMPC：基于质心动力学线性化模型的凸优化求解器，规划未来接触力

1.3 主要功能

原地 trot（小跑）步态
前进/后退运动（最高 0.3 m/s，VBot 适配版本）
侧向运动（最高 0.15 m/s）
yaw 旋转（最高 0.8 rad/s）
键盘实时控制（MuJoCo viewer 中）
仿真回放与数据可视化

2. 算法原理

2.1 控制架构总览

本项目的控制架构分为三个层级，按频率从高到低排列：

层级	名称	频率	功能
高层	MPC（模型预测控制）	~48 Hz	基于质心动力学模型，规划未来 16 步的四足接触力
中层	参考轨迹生成器	~48 Hz	根据用户速度指令生成未来质心参考轨迹和足端位置
低层	腿控制器	200 Hz	将 MPC 接触力映射为关节力矩（支撑腿），笛卡尔空间 PD 控制（摆动腿）
底层	MuJoCo 物理引擎	1000 Hz	执行物理积分、碰撞检测、接触动力学

2.2 核心控制流程

每一步控制循环的执行流程如下：

复制代码

1. MuJoCo 给出当前仿真状态 (qpos, qvel)
2. MuJoCo_VBot_Model 从 MuJoCo 读取 base 和关节状态
3. PinVBotModel 执行正运动学、雅可比计算、动力学项计算
4. ComTraj 根据当前速度指令和步态生成未来参考轨迹
5. CentroidalMPC 构建并求解 QP，得到未来 16 步的四足接触力
6. 只取 MPC 接触力序列的第一步作为当前期望接触力
7. LegController 将接触力（支撑腿）或摆腿轨迹（摆动腿）转换为关节力矩
8. 力矩经过限幅后写入 MuJoCo actuator
9. MuJoCo 积分进入下一时刻（1000 Hz 的积分步）

2.3 滚动时域控制 (Receding Horizon)

本项目采用典型的滚动时域控制策略：

text 复制代码

每次规划未来一个完整步态周期（~0.333s, 16步），但只执行当前第一步。
下一次控制 tick 到来时，重新根据最新状态规划。

2.4 步态调度 (Gait Scheduling)

采用 trot（对角小跑） 步态：

对角腿配对：(FL, RR) 和 (FR, RL)
步态频率：3.0 Hz（每个周期约 0.333s）
支撑相占空比：0.6（60% 时间在支撑相，40% 时间在摆动相）
相位偏移：FL=0.5, FR=0.0, RL=0.0, RR=0.5

2.5 摆腿轨迹生成

摆动腿使用 最小加加速度 (Minimum Jerk) 轨迹，保证位置、速度、加速度在起止点均连续。

同时使用了 Raibert 风格的足端落点规划，根据当前质心速度、期望速度、位置误差、yaw 角速度等因素综合计算落点位置。

3. 算法数学公式

3.1 质心状态向量 (Centroidal State Vector)

系统的状态向量为 12 维，包含质心的位置、姿态（ZYX 欧拉角）、线速度和角速度：

x = $p x p y p z ϕ θ ψ v x v y v z ω x ω y ω z$ T ∈ R 12 x = \begin{bmatrix} p_x & p_y & p_z & \phi & \theta & \psi & v_x & v_y & v_z & \omega_x & \omega_y & \omega_z \end{bmatrix}^T \in \mathbb{R}^{12} x= $pxpypzϕθψvxvyvzωxωyωz$ T∈R12

其中：

p = $p x , p y , p z$ p = $p_x, p_y, p_z$ p= $px,py,pz$ ：质心在世界坐标系中的位置
r p y = $ϕ , θ , ψ$ rpy = $\\phi, \\theta, \\psi$ rpy= $ϕ,θ,ψ$ ：基座的 ZYX 欧拉角（roll, pitch, yaw）
v = $v x , v y , v z$ v = $v_x, v_y, v_z$ v= $vx,vy,vz$ ：质心在世界坐标系中的线速度
ω = $ω x , ω y , ω z$ \omega = $\\omega_x, \\omega_y, \\omega_z$ ω= $ωx,ωy,ωz$ ：基座在世界坐标系中的角速度

3.2 控制输入向量

控制输入为四足在世界坐标系中的接触力（12 维）：

u = $f F L x f F L y f F L z f F R x f F R y f F R z f R L x f R L y f R L z f R R x f R R y f R R z$ T ∈ R 12 u = \begin{bmatrix} f_{FL_x} & f_{FL_y} & f_{FL_z} & f_{FR_x} & f_{FR_y} & f_{FR_z} & f_{RL_x} & f_{RL_y} & f_{RL_z} & f_{RR_x} & f_{RR_y} & f_{RR_z} \end{bmatrix}^T \in \mathbb{R}^{12} u= $fFLxfFLyfFLzfFRxfFRyfFRzfRLxfRLyfRLzfRRxfRRyfRRz$ T∈R12

每条腿的接触力排序为：FL（左前）、FR（右前）、RL（左后）、RR（右后）。

3.3 连续时间质心动力学 (Continuous Centroidal Dynamics)

质心动力学模型基于牛顿-欧拉方程：

线运动方程：

p ˙ = v \dot{p} = v p˙=v

v ˙ = 1 m ∑ i = 1 4 f i + g \dot{v} = \frac{1}{m} \sum_{i=1}^{4} f_i + g v˙=m1i=1∑4fi+g

其中 m m m 为机器人总质量， g = $0 , 0 , - 9.81$ T g = $0, 0, -9.81$ ^T g= $0,0,-9.81$ T 为重力加速度， f i f_i fi 为第 i i i 条腿的接触力。

旋转运动方程：

ψ ˙ = R z T ω \dot{\psi} = R_z^T \omega ψ˙=RzTω

ω ˙ = I − 1 ∑ i = 1 4 ( r i × f i ) \dot{\omega} = I^{-1} \sum_{i=1}^{4} (r_i \times f_i) ω˙=I−1i=1∑4(ri×fi)

其中 R z R_z Rz 为绕 yaw 轴的旋转矩阵， I I I 为质心转动惯量在世界坐标系中的表示， r i r_i ri 为从质心到第 i i i 条腿足端的杠杆臂向量。

连续时间状态空间模型：

x ˙ = A c x + B c ( t ) u + g c \dot{x} = A_c x + B_c(t) u + g_c x˙=Acx+Bc(t)u+gc

A c = $0 3 \times 3 0 3 \times 3 I 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 R z T 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3$ A_c = \begin{bmatrix} 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & R_z^T \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \end{bmatrix} Ac= 03×303×303×303×303×303×303×303×3I3×303×303×303×303×3RzT03×303×3

B c ( t ) = $0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 1 m I 3 × 3 1 m I 3 × 3 1 m I 3 × 3 1 m I 3 × 3 I − 1 \[ r 1$ × I − 1 $r 2$ × I − 1 $r 3$ × I − 1 $r 4$ × ] B_c(t) = \begin{bmatrix} 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ \frac{1}{m}I_{3\times3} & \frac{1}{m}I_{3\times3} & \frac{1}{m}I_{3\times3} & \frac{1}{m}I_{3\times3} \\ I^{-1} $r_1$ \times & I^{-1} $r_2$ \times & I^{-1} $r_3$ \times & I^{-1} $r_4$ \times \end{bmatrix} Bc(t)= 03×303×3m1I3×3I−1 $r1$ ×03×303×3m1I3×3I−1 $r2$ ×03×303×3m1I3×3I−1 $r3$ ×03×303×3m1I3×3I−1 $r4$ ×

g c = $0 0 0 0 0 0 0 0 - 9.81 0 0 0$ T g_c = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & -9.81 & 0 & 0 & 0 \end{bmatrix}^T gc= $00000000-9.81000$ T

其中 $r i$ × $r_i$ _\times $ri$ × 为向量 r i r_i ri 的反对称矩阵（skew-symmetric matrix）。

3.4 离散时间动力学模型 (Discrete Dynamics)

使用零阶保持（ZOH）方法将连续模型离散化：

离散状态转移矩阵 A d A_d Ad（常量）：

A d = $I 3 \times 3 0 3 \times 3 d t \cdot I 3 \times 3 0 3 \times 3 0 3 \times 3 I 3 \times 3 0 3 \times 3 d t \cdot R z T 0 3 \times 3 0 3 \times 3 I 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 0 3 \times 3 I 3 \times 3$ A_d = \begin{bmatrix} I_{3\times3} & 0_{3\times3} & dt \cdot I_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & I_{3\times3} & 0_{3\times3} & dt \cdot R_z^T \\ 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} \end{bmatrix} Ad= I3×303×303×303×303×3I3×303×303×3dt⋅I3×303×3I3×303×303×3dt⋅RzT03×3I3×3

离散控制矩阵 B d $k$ B_d $k$ Bd $k$ （时变，依赖于每个时刻的杠杆臂 r i $k$ r_i $k$ ri $k$ ）：

B d $k$ = $B p B p B p B p 1 2 d t 2 R z T W 1 1 2 d t 2 R z T W 2 1 2 d t 2 R z T W 3 1 2 d t 2 R z T W 4 B v B v B v B v d t \cdot W 1 d t \cdot W 2 d t \cdot W 3 d t \cdot W 4$ B_d $k$ = \begin{bmatrix} B_p & B_p & B_p & B_p \\ \frac{1}{2}dt^2 R_z^T W_1 & \frac{1}{2}dt^2 R_z^T W_2 & \frac{1}{2}dt^2 R_z^T W_3 & \frac{1}{2}dt^2 R_z^T W_4 \\ B_v & B_v & B_v & B_v \\ dt \cdot W_1 & dt \cdot W_2 & dt \cdot W_3 & dt \cdot W_4 \end{bmatrix} Bd $k$ = Bp21dt2RzTW1Bvdt⋅W1Bp21dt2RzTW2Bvdt⋅W2Bp21dt2RzTW3Bvdt⋅W3Bp21dt2RzTW4Bvdt⋅W4

其中：

B p = 1 2 d t 2 / m ⋅ I 3 × 3 B_p = \frac{1}{2}dt^2/m \cdot I_{3\times3} Bp=21dt2/m⋅I3×3
B v = d t / m ⋅ I 3 × 3 B_v = dt/m \cdot I_{3\times3} Bv=dt/m⋅I3×3
W i = I − 1 $r i$ × W_i = I^{-1} $r_i$ _\times Wi=I−1 $ri$ ×
d t dt dt 为 MPC 时间步长

离散重力向量 g d g_d gd：

g d = $1 2 g d t 2 0 3 \times 1 g d t 0 3 \times 1$ T g_d = \begin{bmatrix} \frac{1}{2}g dt^2 & 0_{3\times1} & g dt & 0_{3\times1} \end{bmatrix}^T gd= $21gdt203\times1gdt03\times1$ T

3.5 凸 MPC 优化问题 (Quadratic Programming Formulation)

MPC 在每个时间步求解如下凸二次规划（QP）问题：

决策变量：

w = $x 1 x 2 \dots x N u 0 u 1 \dots u N - 1$ T ∈ R N ⋅ ( 12 + 12 ) w = \begin{bmatrix} x_1 & x_2 & \cdots & x_N & u_0 & u_1 & \cdots & u_{N-1} \end{bmatrix}^T \in \mathbb{R}^{N \cdot (12 + 12)} w= $x1x2\dotsxNu0u1\dotsuN-1$ T∈RN⋅(12+12)

其中 N = 16 N = 16 N=16 为预测时域长度。

代价函数（最小化跟踪误差和控制力）：

min ⁡ w ∑ k = 0 N − 1 ( ( x k − x k r e f ) T Q ( x k − x k r e f ) + u k T R u k ) \min_w \sum_{k=0}^{N-1} \left( (x_k - x_k^{ref})^T Q (x_k - x_k^{ref}) + u_k^T R u_k \right) wmink=0∑N−1((xk−xkref)TQ(xk−xkref)+ukTRuk)

其中：

Q = diag ( $1 , 1 , 50 , 10 , 20 , 1 , 2 , 2 , 1 , 1 , 1 , 1$ ) Q = \text{diag}( $1, 1, 50, 10, 20, 1, 2, 2, 1, 1, 1, 1$ ) Q=diag( $1,1,50,10,20,1,2,2,1,1,1,1$ )：状态权重矩阵
R = 10 − 5 × I 12 R = 10^{-5} \times I_{12} R=10−5×I12：控制输入权重矩阵

约束条件：

动力学等式约束： x k + 1 = A d x k + B d $k$ u k + g d , k = 0 , 1 , ... , N − 1 x_{k+1} = A_d x_k + B_d $k$ u_k + g_d, \quad k = 0, 1, \dots, N-1 xk+1=Adxk+Bd $k$ uk+gd,k=0,1,...,N−1
摩擦力锥约束（线性化）： 对于每条支撑腿，接触力必须满足摩擦锥约束。使用四棱锥近似：

− μ f z ≤ f x ≤ μ f z -\mu f_z \leq f_x \leq \mu f_z −μfz≤fx≤μfz

− μ f z ≤ f y ≤ μ f z -\mu f_z \leq f_y \leq \mu f_z −μfz≤fy≤μfz

f z ≥ 0 f_z \geq 0 fz≥0

摩擦系数 μ = 0.8 \mu = 0.8 μ=0.8。
摆动腿约束： 摆动腿的接触力严格为 0：

f x = f y = f z = 0 f_x = f_y = f_z = 0 fx=fy=fz=0
最小法向力约束： 支撑腿的最小法向力 f z ≥ 10 f_z \geq 10 fz≥10 N（防止打滑）。

3.6 QP 求解器配置

使用 CasADi 框架调用 OSQP 求解器求解 QP 问题：

热启动（warm start）：启用初始解和拉格朗日乘子的热启动
精度：绝对精度 10 − 4 10^{-4} 10−4，相对精度 10 − 4 10^{-4} 10−4
最大迭代次数：1000
自适应 ρ \rho ρ 参数

3.7 腿控制器算法

支撑腿（接触力映射为力矩）：

τ = J f o o t T ⋅ ( − f d e s ) \tau = J_{foot}^T \cdot (-f_{des}) τ=JfootT⋅(−fdes)

其中 J f o o t J_{foot} Jfoot 为足端雅可比矩阵（ 3 × 3 3 \times 3 3×3）， f d e s f_{des} fdes 为 MPC 输出的期望接触力。

摆动腿（笛卡尔空间阻抗控制）：

f c a r t = K p ( p d e s − p ) + K d ( v d e s − v ) + f f f f_{cart} = K_p (p_{des} - p) + K_d (v_{des} - v) + f_{ff} fcart=Kp(pdes−p)+Kd(vdes−v)+fff

前馈力项：

f f f = Λ ( a d e s − J ˙ q ˙ ) f_{ff} = \Lambda (a_{des} - \dot{J}\dot{q}) fff=Λ(ades−J˙q˙)

其中 Λ = ( J M − 1 J T ) − 1 \Lambda = (J M^{-1} J^T)^{-1} Λ=(JM−1JT)−1 为操作空间惯性矩阵。

最终关节力矩：

τ = J T f c a r t + ( C q ˙ + g ) \tau = J^T f_{cart} + (C\dot{q} + g) τ=JTfcart+(Cq˙+g)

其中 C q ˙ + g C\dot{q} + g Cq˙+g 为科里奥利力和重力补偿项。

摆腿轨迹：使用五次多项式（最小加加速度轨迹）：

p ( s ) = p 0 + ( p f − p 0 ) ( 10 s 3 − 15 s 4 + 6 s 5 ) p(s) = p_0 + (p_f - p_0)(10s^3 - 15s^4 + 6s^5) p(s)=p0+(pf−p0)(10s3−15s4+6s5)

其中 s = t / T s w i n g ∈ $0 , 1$ s = t/T_{swing} \in $0, 1$ s=t/Tswing∈ $0,1$ ， T s w i n g T_{swing} Tswing 为摆动时间。

足端抬升曲线：

z b u m p ( s ) = h s w ⋅ 64 s 3 ( 1 − s ) 3 z_{bump}(s) = h_{sw} \cdot 64 s^3 (1-s)^3 zbump(s)=hsw⋅64s3(1−s)3

其中 h s w = 0.1 h_{sw} = 0.1 hsw=0.1 m 为摆动腿最大抬升高度。

3.8 落点规划（Raibert 风格）

足端落点位置综合考虑以下因素：

p t d = p n o m i n a l + p d r i f t + p c o r r e c t i o n + p v e l _ c o r r e c t i o n + p r o t a t i o n p_{td} = p_{nominal} + p_{drift} + p_{correction} + p_{vel\correction} + p{rotation} ptd=pnominal+pdrift+pcorrection+pvel_correction+protation

p n o m i n a l p_{nominal} pnominal：标称落点（髋关节正下方）
p d r i f t p_{drift} pdrift：前进/侧向速度漂移项
p c o r r e c t i o n p_{correction} pcorrection：位置误差修正项
p v e l _ c o r r e c t i o n p_{vel\_correction} pvel_correction：速度误差修正项
p r o t a t i o n p_{rotation} protation：yaw 旋转修正项

4. 项目架构

4.1 目录结构

复制代码

go2-convex-mpc/
├── configs/
│   └── vbot_joint_map.yaml          # VBot 关节映射和配置
├── models/
│   ├── MJCF/
│   │   ├── go2/                     # Go2 原始模型文件
│   │   │   ├── go2.xml
│   │   │   ├── scene.xml
│   │   │   └── scene_terrain.xml
│   │   └── vbot/                    # VBot 模型文件
│   │       ├── vbot_pinocchio.xml   # Pinocchio 用精简 MJCF
│   │       └── vbot_mpc_scene.xml   # MuJoCo 用完整 MPC 场景 MJCF
│   └── URDF/go2_description/urdf/
│       └── go2_description.urdf     # Go2 URDF 模型
├── src/
│   └── convex_mpc/
│       ├── __init__.py              # 模块入口
│       ├── centroidal_mpc.py        # 核心 MPC 算法（QP 构建与求解）
│       ├── com_trajectory.py        # 质心参考轨迹生成
│       ├── gait.py                  # 步态调度与摆腿轨迹
│       ├── leg_controller.py        # 腿控制器（支撑/摆动）
│       ├── go2_robot_data.py        # Go2 机器人 Pinocchio 接口
│       ├── vbot_robot_data.py       # VBot 机器人 Pinocchio 接口
│       ├── mujoco_model.py          # Go2 MuJoCo 接口
│       ├── mujoco_vbot_model.py     # VBot MuJoCo 接口
│       └── plot_helper.py           # 绘图辅助工具
├── examples/
│   ├── ex00_demo.py                 # 综合运动演示
│   ├── ex01_trot_in_place.py        # 原地 trot
│   ├── ex02_trot_forward.py         # 前进 trot
│   ├── ex03_trot_sideway.py         # 侧向 trot
│   ├── ex04_trot_rotation.py        # 旋转 trot
│   ├── ex16_vbot_mpc_trot_in_place.py    # VBot 原地 trot 验证
│   └── ex17_vbot_mpc_keyboard_control.py # VBot 键盘控制
├── pyproject.toml                   # Python 项目配置
├── README.md                        # 原始英文说明
└── VBOT_MPC_WORKSPACE_NOTES.md      # 本文件（技术文档）

4.2 模块依赖关系

复制代码

ex16_vbot_mpc_trot_in_place.py (主脚本)
    │
    ├── PinVBotModel (vbot_robot_data.py)
    │   └── Pinocchio 正运动学、动力学、雅可比计算
    │
    ├── MuJoCo_VBot_Model (mujoco_vbot_model.py)
    │   └── MuJoCo 物理仿真、状态读取、力矩写入
    │
    ├── ComTraj (com_trajectory.py)
    │   └── 生成未来 N 步参考轨迹和离散动力学矩阵
    │
    ├── Gait (gait.py)
    │   └── 步态调度、接触时序、摆腿轨迹
    │
    ├── CentroidalMPC (centroidal_mpc.py)
    │   └── 构建并求解 QP，规划接触力
    │
    └── LegController (leg_controller.py)
        └── 接触力 → 关节力矩（支撑）/ 摆腿 PD 控制（摆动）

5. 核心模块详解

5.1 `centroidal_mpc.py` --- 核心 MPC 算法

类： CentroidalMPC

核心功能：

预计算稀疏矩阵结构 ：在初始化时根据 horizon N 预计算 Hessian 矩阵 H H H、摩擦约束矩阵 A i n e q A_{ineq} Aineq 的稀疏结构。
QP 构建 ：每次 MPC 更新时，根据当前足端位置计算时变的 B d B_d Bd 矩阵，组装完整的约束矩阵 A A A，计算代价函数的线性项 g g g。
QP 求解：使用 CasADi 调用 OSQP 求解器，支持热启动。
边界约束 ：支撑腿法向力下限 f z ≥ 10 f_z \geq 10 fz≥10 N，摆动腿三轴力均为 0。

关键参数：

参数	值	说明
`Q`	`diag([1,1,50,10,20,1,2,2,1,1,1,1])`	状态代价权重
`R`	`1e-5 * I_12`	输入代价权重
`MU`	0.8	摩擦系数
`NX`	12	状态维数
`NU`	12	输入维数
`fz_min`	10 N	最小法向接触力

5.2 `com_trajectory.py` --- 参考轨迹生成

类： ComTraj

核心功能：

根据速度指令生成未来 N 步的参考状态轨迹（位置、姿态、速度、角速度）
生成接触时序表（contact_table）
计算连续时间动力学矩阵 A c , B c ( t ) A_c, B_c(t) Ac,Bc(t)
计算离散时间动力学矩阵 A d , B d $k$ A_d, B_d $k$ Ad,Bd $k$
使用 update_model_simplified 在简化模型上预测未来足端位置

关键方法：

generate_traj()：每次 MPC 更新时调用，生成完整参考轨迹和动力学矩阵
compute_x_ref_vec()：返回 12×N 的参考状态矩阵
_continuousDynamics()：计算 A c , B c ( t ) , g c A_c, B_c(t), g_c Ac,Bc(t),gc
_discreteDynamics()：将连续模型离散化为 A d , B d $k$ , g d A_d, B_d $k$ , g_d Ad,Bd $k$ ,gd

5.3 `gait.py` --- 步态调度

类： Gait

核心功能：

根据当前时间计算步态相位和接触时序
计算足端着地点位置（Raibert 落点规划）
生成摆动腿的最小加加速度轨迹

关键参数：

参数	值	说明
`PHASE_OFFSET`	`[0.5, 0.0, 0.0, 0.5]`	四腿相位偏移（trot）
`HEIGHT_SWING`	0.1 m	摆腿最大高度

5.4 `leg_controller.py` --- 腿控制器

类： LegController

核心功能：

支撑腿：将 MPC 接触力通过雅可比转置映射为关节力矩
摆动腿：笛卡尔空间 PD + 前馈控制，跟踪最小加加速度轨迹
自动检测腿从支撑相切换到摆动相的时刻，并初始化摆腿轨迹

关键参数：

参数	值	说明
`KP_SWING`	`diag([400, 400, 400])`	摆动腿位置增益
`KD_SWING`	`diag([75, 75, 75])`	摆动腿速度增益

5.5 `vbot_robot_data.py` --- VBot Pinocchio 接口

类： PinVBotModel

类： VBotConfigurationState

核心功能：

加载 vbot_pinocchio.xml 精简 MJCF 模型
维护 VBot 当前广义坐标（ q ∈ R 19 q \in \mathbb{R}^{19} q∈R19）和广义速度（ v ∈ R 18 v \in \mathbb{R}^{18} v∈R18）
计算质心位置、速度、欧拉角
计算四足足端位置、速度、杠杆臂
计算足端雅可比矩阵（ 3 × 3 3 \times 3 3×3 和 3 × 18 3 \times 18 3×18）
计算 J ˙ q ˙ \dot{J}\dot{q} J˙q˙ 项
计算动力学项（重力 g g g、科里奥利力 C C C、惯性矩阵 M M M）

关节顺序说明：

MPC/控制器内部 12 维力矩顺序：

text 复制代码

[FL_hip, FL_thigh, FL_calf, FR_hip, FR_thigh, FR_calf,
 RL_hip, RL_thigh, RL_calf, RR_hip, RR_thigh, RR_calf]

Pinocchio 内部关节顺序（与 MuJoCo 不同）：

text 复制代码

FR → FL → RR → RL

5.6 `mujoco_vbot_model.py` --- VBot MuJoCo 接口

类： MuJoCo_VBot_Model

核心功能：

加载 vbot_mpc_scene.xml 完整 MPC 场景模型
通过关节名称（而非位置索引）读取 qpos 和 qvel
将 MuJoCo 状态同步到 PinVBotModel
将 12 维关节力矩写入 MuJoCo actuator
仿真回放功能（replay_simulation）

5.7 `plot_helper.py` --- 数据可视化

提供以下绘图函数：

plot_contact_forces()：绘制四足接触力
plot_traj_tracking()：3D 轨迹跟踪图
plot_mpc_result()：完整 MPC 结果（力、力矩、状态）
plot_swing_foot_traj()：摆动腿轨迹
plot_solve_time()：MPC 求解时间统计
plot_full_traj()：参考轨迹 vs 实际轨迹对比

6. 模型文件说明

6.1 `vbot_pinocchio.xml` --- Pinocchio 精简模型

本文件仅包含机器人本体，无 target_marker、heading_arrow 等无关 body，用于 Pinocchio 动力学计算。

模型结构：

复制代码

worldbody
  └── base (自由浮动关节)
      ├── FR_hip → FR_thigh → FR_calf → FR_foot
      ├── FL_hip → FL_thigh → FL_calf → FL_foot
      ├── RR_hip → RR_thigh → RR_calf → RR_foot
      └── RL_hip → RL_thigh → RL_calf → RL_foot

广义坐标维度：nq=19（base 位置 3 + 四元数 4 + 12 关节角）
广义速度维度：nv=18（base 速度 6 + 12 关节速度）
总质量：约 9.016 kg

6.2 `vbot_mpc_scene.xml` --- MuJoCo MPC 场景模型

在完整 VBot 模型基础上添加：

地面（ground plane，摩擦系数 0.8）
光源和天空盒
target_marker（用于可视化目标方向）
robot_heading_arrow（机器人朝向指示器）
desired_heading_arrow（期望朝向指示器）
12 个 motor actuator（对应 12 个关节）
全部传感器（关节位置、速度、力矩，IMU，加速度计等）

6.3 力矩限制（来自 XML）

关节	最大力矩	安全使用 (0.9×)
hip	17 Nm	15.3 Nm
thigh	17 Nm	15.3 Nm
calf	34 Nm	30.6 Nm

7. 配置文件说明

`configs/vbot_joint_map.yaml`

yaml 复制代码

base_body: base                                 # 基座 body 名称
floating_joint: joint_fixed_world              # 浮动关节名称

mpc_leg_order: [FL, FR, RL, RR]               # MPC 腿顺序
pin_leg_order: [FR, FL, RR, RL]               # Pinocchio 腿顺序

joints:
  FL: [FL_hip_joint, FL_thigh_joint, FL_calf_joint]
  FR: [FR_hip_joint, FR_thigh_joint, FR_calf_joint]
  RL: [RL_hip_joint, RL_thigh_joint, RL_calf_joint]
  RR: [RR_hip_joint, RR_thigh_joint, RR_calf_joint]

default_joint_angles:
  FL: [0.0, 0.9, -1.8]     # 默认站姿关节角 (rad)
  FR: [-0.0, 0.9, -1.8]
  RL: [0.0, 0.9, -1.8]
  RR: [-0.0, 0.9, -1.8]

torque_limits:
  hip: 17.0                 # 髋关节最大力矩 (Nm)
  thigh: 17.0               # 大腿关节最大力矩 (Nm)
  calf: 34.0                # 小腿关节最大力矩 (Nm)

8. 运行步骤

8.1 环境配置

bash 复制代码

# 1. 进入工作目录
cd /home/fatu08/go2-convex-mpc

# 2. 激活 conda 环境
conda activate go2-convex-mpc

# 3. 验证依赖
python -c "import mujoco, pinocchio, casadi, convex_mpc; print('All OK')"

8.2 运行原地 trot 验证

bash 复制代码

# 基本运行（5 秒仿真，无 viewer，无绘图）
python -m examples.ex16_vbot_mpc_trot_in_place

# 打开 MuJoCo 回放
VBOT_MPC_REPLAY=1 python -m examples.ex16_vbot_mpc_trot_in_place

# 打开绘图
VBOT_MPC_PLOTS=1 python -m examples.ex16_vbot_mpc_trot_in_place

# 同时打开回放和绘图
VBOT_MPC_REPLAY=1 VBOT_MPC_PLOTS=1 python -m examples.ex16_vbot_mpc_trot_in_place

8.3 运行键盘控制

bash 复制代码

cd /home/fatu08/go2-convex-mpc
conda activate go2-convex-mpc
python -m examples.ex17_vbot_mpc_keyboard_control

键盘控制说明：

按键	功能
`W`	增加前进速度 (+0.05 m/s)
`S`	减小前进速度 / 后退 (-0.05 m/s)
`A`	增加右侧向速度 (+0.03 m/s)
`D`	增加左侧向速度 (-0.03 m/s)
`Q`	增加左转 yaw 角速度 (+0.15 rad/s)
`E`	增加右转 yaw 角速度 (-0.15 rad/s)
`Space`	停止（所有速度归零）
`R`	重置机器人姿态
`Esc`	退出

初始速度限幅（保守值）：

方向	范围
前进/后退 x	$-0.30, 0.30$ m/s
侧向 y	$-0.15, 0.15$ m/s
yaw 角速度	$-0.80, 0.80$ rad/s

8.4 Go2 原始示例

bash 复制代码

# 综合运动演示
python -m examples.ex00_demo

# 原地 trot
python -m examples.ex01_trot_in_place

# 前进 trot
python -m examples.ex02_trot_forward

# 侧向 trot
python -m examples.ex03_trot_sideway

# 旋转 trot
python -m examples.ex04_trot_rotation

8.5 语法检查

bash 复制代码

python -m py_compile examples/ex16_vbot_mpc_trot_in_place.py
python -m py_compile examples/ex17_vbot_mpc_keyboard_control.py

9. 参数调优指南

9.1 频率设置

参数	当前值	说明
`SIM_HZ`	1000 Hz	MuJoCo 物理积分频率
`CTRL_HZ`	200 Hz	腿控制器频率
`MPC_HZ`	~48 Hz	MPC 更新频率
`GAIT_HZ`	3.0 Hz	步态频率（trot）
`GAIT_DUTY`	0.6	支撑相占空比

9.2 推荐调参顺序

如果 VBot 在仿真中出现不稳定，建议按以下顺序调整参数：

期望机身高度 Z_POS_DES_BODY：确保与模型默认站姿一致（0.462 m）
步态频率 GAIT_HZ：降低至 2.0~2.5 Hz 可获得更稳定但稍慢的运动
占空比 GAIT_DUTY：增大至 0.65 可增加支撑时间比例
摆腿高度 HEIGHT_SWING：减小至 0.06~0.08 m 可减少摆动冲击
摆动腿 PD 增益 KP_SWING / KD_SWING：降低位置增益可减小摆腿冲击
MPC 代价权重 Q：增大位置和姿态权重，减小速度权重
力矩限幅 TAU_LIM：如果力矩饱和，适当增大限幅
键盘速度限幅：如果运动不稳，优先降低速度限幅而不是调大 MPC 权重

9.3 关键调试参数位置

参数	文件	行号
`COST_MATRIX_Q`	`centroidal_mpc.py`	L12
`COST_MATRIX_R`	`centroidal_mpc.py`	L13
`MU` (摩擦系数)	`centroidal_mpc.py`	L15
`KP_SWING`, `KD_SWING`	`leg_controller.py`	L10-L11
`PHASE_OFFSET`	`gait.py`	L8
`HEIGHT_SWING`	`gait.py`	L9
`GAIT_HZ`, `GAIT_DUTY`	示例脚本 (ex16, ex17)	配置区
`TAU_LIM`	示例脚本 (ex16, ex17)	配置区
速度限幅	`ex17_vbot_mpc_keyboard_control.py`	L84-L86

10. 参考论文

本项目控制算法主要参考以下论文：

"Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control"

作者: Y. Ding, A. Pandala, H.-W. Park

链接: https://dspace.mit.edu/bitstream/handle/1721.1/138000/convex_mpc_2fix.pdf

核心贡献：

使用质心动力学线性化模型
将接触力优化问题转化为凸二次规划
滚动时域控制实现动态行走

附录：常用命令速查

bash 复制代码

# 激活环境
conda activate go2-convex-mpc

# VBot 原地 trot（无界面）
python -m examples.ex16_vbot_mpc_trot_in_place

# VBot 原地 trot + 回放
VBOT_MPC_REPLAY=1 python -m examples.ex16_vbot_mpc_trot_in_place

# VBot 原地 trot + 绘图
VBOT_MPC_PLOTS=1 python -m examples.ex16_vbot_mpc_trot_in_place

# VBot 键盘控制（交互式 viewer）
python -m examples.ex17_vbot_mpc_keyboard_control

# 项目根目录
cd /home/fatu08/go2-convex-mpc

VBot四足机器人 Convex MPC 控制项目分析