基于 OCS2（Optimal Control for Switched Systems 2） 与 ros-control 的足式机器人 NMPC-WBC 控制框架
项目地址：https://github.com/qiayuanl/legged_control

---

目录

• 1. 项目概述
• 2. 项目含义与背景
• 3. 系统框架
• 4. 项目结构
• 5. 算法原理与公式
  • 5.1 非线性模型预测控制（NMPC）
  • 5.2 全身控制器（WBC）
  • 5.3 状态估计（卡尔曼滤波）
• 6. 关键模块详解
• 7. 配置参数说明
• 8. 运行步骤
• 9. 部署到自定义机器人
• 10. 参考文献

---

1. 项目概述

legged_control 是一个基于 OCS2（Optimal Control for Switched Systems 2）和 ros-control 构建的足式机器人控制栈与框架，核心采用 NMPC（非线性模型预测控制）+ WBC（全身控制） 的分层控制架构。

该框架的主要优势：

1. 据作者所知，这可能是性能最佳的开源足式机器人 MPC 控制框架；
2. 可在数小时内部署到 Unitree A1 真机；
3. 得益于 ros-control 接口，可方便地移植到自定义机器人。

支持机器人类型：Unitree A1、Aliengo、Go1（可扩展至 Go2 等）。

---

2. 项目含义与背景

2.1 问题背景

足式机器人（四足、双足）的控制是一个复杂的非线性最优控制问题，难点在于：

• 浮动基座：基座不受直接驱动，需通过接触力间接控制；
• 接触切换：足端与地面接触状态离散切换（stance/swing），形成混合动力学系统；
• 欠驱动：关节数量少于自由度，需协调多任务；
• 实时性：控制频率需达到数百 Hz 才能保证稳定。

2.2 解决思路

legged_control 采用分层控制策略：

• 上层 NMPC：在 1 秒时间窗内求解最优轨迹（位置、速度、接触力），频率约 100~200Hz；
• 下层 WBC：基于当前状态求解关节力矩，考虑完整刚体动力学，频率 500Hz~1kHz；
• 状态估计：卡尔曼滤波融合 IMU、关节编码器、接触信息估计基座位姿与速度。

2.3 学术背景

该框架发表于 IROS 2023，主要参考 ETH Zurich 的 ANYmal 控制方案，并在 OCS2 框架下实现。

---

3. 系统框架

系统整体数据流如下图所示：

┌─────────────────────────────────────────────────────────────────┐
│                     用户指令 (cmd_vel / goal)                     │
│                          ↓                                       │
│              TargetTrajectoriesPublisher                         │
│        (将速度/位置目标转换为状态轨迹参考)                          │
│                          ↓                                       │
│              ┌────────────────────────┐                          │
│              │   NMPC (SQP 求解器)     │  ← Gait Schedule (步态)  │
│              │   - 质心动力学模型       │  ← Swing Trajectory      │
│              │   - 摩擦锥/零速度约束    │                          │
│              │   - 二次跟踪代价        │                          │
│              └────────────┬───────────┘                          │
│                           ↓                                      │
│              优化状态 x* 与输入 u* (未来 1s 轨迹)                  │
│                           ↓                                      │
│              ┌────────────────────────┐                          │
│              │   WBC (QP 求解器)       │  ← 测量状态 (RBD State)  │
│              │   - 浮动基座动力学方程   │                          │
│              │   - 摆动腿跟踪          │                          │
│              │   - 基座加速度跟踪      │                          │
│              │   - 接触力跟踪          │                          │
│              │   - 力矩/摩擦锥约束     │                          │
│              └────────────┬───────────┘                          │
│                           ↓                                      │
│              关节力矩 τ + 关节位置/速度 PD 前馈                    │
│                           ↓                                      │
│              ┌────────────────────────┐                          │
│              │   硬件接口 (HybridJoint)│                          │
│              │   τ_ff + kp·(q_d-q)    │                          │
│              │       + kd·(v_d-v)     │                          │
│              └────────────┬───────────┘                          │
│                           ↓                                      │
│                    机器人电机 / Gazebo                            │
│                           ↓                                      │
│              ┌────────────────────────┐                          │
│              │  状态估计 (Kalman Filter)│ ← IMU + 关节状态 + 接触  │
│              │  估计基座位姿与速度      │                          │
│              └────────────────────────┘                          │
└─────────────────────────────────────────────────────────────────┘

控制循环时序

• 硬件控制循环：500 Hz（LeggedHWLoop），执行 read() → update() → write()；
• WBC 循环：与硬件循环同频（500 Hz），在 LeggedController::update() 中调用；
• MPC 循环：独立线程，100 Hz 求解一次完整 SQP 优化；
• MRT 接口：1000 Hz 评估当前时刻的策略（插值）。

---

4. 项目结构

legged_control-master/
├── legged_common/              # 公共接口定义
│   └── include/legged_common/hardware_interface/
│       ├── ContactSensorInterface.h    # 接触传感器接口
│       └── HybridJointInterface.h      # 混合关节接口(力矩+PD)
│
├── legged_hw/                  # 硬件接口基类
│   ├── include/legged_hw/
│   │   ├── LeggedHW.h                  # 硬件接口抽象基类
│   │   └── LeggedHWLoop.h              # 控制循环(500Hz)
│   └── src/
│       ├── LeggedHW.cpp                # URDF加载/接口注册
│       └── LeggedHWLoop.cpp            # 定时循环实现
│
├── legged_interface/           # NMPC 最优控制问题建模 ★
│   ├── include/legged_interface/
│   │   ├── LeggedInterface.h           # OCP问题构建主类
│   │   ├── SwitchedModelReferenceManager.h  # 步态/参考管理
│   │   ├── LeggedRobotPreComputation.h # 预计算
│   │   ├── constraint/                 # 约束
│   │   │   ├── FrictionConeConstraint.h       # 摩擦锥
│   │   │   ├── ZeroVelocityConstraintCppAd.h  # 零速度(支撑相)
│   │   │   ├── ZeroForceConstraint.h          # 零力(摆动相)
│   │   │   ├── NormalVelocityConstraintCppAd.h# 法向速度
│   │   │   ├── SwingTrajectoryPlanner.h       # 摆动轨迹规划
│   │   │   └── LeggedSelfCollisionConstraint.h# 自碰撞
│   │   ├── cost/
│   │   │   └── LeggedRobotQuadraticTrackingCost.h  # 二次跟踪代价
│   │   └── initialization/
│   │       └── LeggedRobotInitializer.h       # 初始化器
│   └── src/                            # 对应实现
│
├── legged_wbc/                 # 全身控制器 ★
│   ├── include/legged_wbc/
│   │   ├── WbcBase.h                   # WBC基类(任务构造)
│   │   ├── WeightedWbc.h               # 加权WBC(单QP)
│   │   ├── HierarchicalWbc.h           # 分层WBC(HoQp)
│   │   ├── HoQp.h                      # 分层QP求解
│   │   └── Task.h                      # 任务(等式/不等式)
│   └── src/
│       ├── WbcBase.cpp                 # 7个任务构造
│       ├── WeightedWbc.cpp             # qpOASES 求解
│       ├── HierarchicalWbc.cpp         # 零空间分层求解
│       └── HoQp.cpp                    # 零空间投影
│
├── legged_estimation/          # 状态估计 ★
│   ├── include/legged_estimation/
│   │   ├── StateEstimateBase.h         # 估计基类
│   │   ├── LinearKalmanFilter.h        # 卡尔曼滤波
│   │   └── FromTopiceEstimate.h        # 从话题读取(cheater)
│   └── src/
│       ├── StateEstimateBase.cpp
│       ├── LinearKalmanFilter.cpp      # 18维状态KF
│       └── FromTopicEstimate.cpp
│
├── legged_controllers/         # 控制器主入口 ★
│   ├── include/legged_controllers/
│   │   ├── LeggedController.h          # 主控制器
│   │   ├── SafetyChecker.h             # 安全检查(姿态)
│   │   └── TargetTrajectoriesPublisher.h
│   ├── src/
│   │   ├── LeggedController.cpp        # init/update/启动MPC线程
│   │   └── TargetTrajectoriesPublisher.cpp  # cmd_vel→轨迹
│   ├── config/
│   │   ├── controllers.yaml            # 控制器类型注册
│   │   ├── joy.yaml                    # 手柄映射
│   │   └── {a1,aliengo,go1}/           # 各机器人配置
│   │       ├── task.info               # 任务/代价/约束参数
│   │       ├── gait.info               # 步态定义
│   │       └── reference.info          # 参考轨迹参数
│   └── launch/
│       └── load_controller.launch      # 加载控制器
│
├── legged_gazebo/              # Gazebo 仿真
│   ├── include/legged_gazebo/LeggedHWSim.h  # 仿真硬件接口
│   ├── config/default.yaml             # 仿真参数
│   └── worlds/empty_world.world
│
├── legged_examples/legged_unitree/      # Unitree 机器人实现
│   ├── legged_unitree_description/      # URDF 描述
│   │   ├── urdf/
│   │   │   ├── robot.xacro              # 机器人主文件
│   │   │   ├── common/leg.xacro         # 单腿建模
│   │   │   ├── common/transmission.xacro# 传动
│   │   │   └── {a1,aliengo,go1}/const.xacro  # 机器人常量
│   │   ├── meshes/                      # 3D 模型
│   │   └── launch/empty_world.launch    # Gazebo 启动
│   └── legged_unitree_hw/               # 真机硬件接口
│       ├── include/legged_unitree_hw/UnitreeHW.h
│       ├── src/UnitreeHW.cpp            # UDP 通信
│       ├── config/a1.yaml               # 500Hz/功率限制
│       └── lib/                         # Unitree SDK
│
├── legged_control/             # 元包
└── qpoases_catkin/             # qpOASES 依赖

---

5. 算法原理与公式

5.1 非线性模型预测控制（NMPC）

5.1.1 最优控制问题建模

NMPC 在每个控制周期求解如下连续时间最优控制问题（OCP）：

$$\{\begin{array}{l}\underset{\mathbf{u}(\cdot )}{\min }\varphi (\mathbf{x}(t_I))+{\int }_{t_0}^{t_I}l(\mathbf{x}(t),\mathbf{u}(t),t)dt\\ \text{s.t.}\mathbf{x}(t_0)={\mathbf{x}}_0\text{(初始状态)}\\ \dot{\mathbf{x}}(t)=\mathbf{f}(\mathbf{x}(t),\mathbf{u}(t),t)\text{(系统动力学)}\\ {\mathbf{g}}_1(\mathbf{x}(t),\mathbf{u}(t),t)=0\text{(状态-输入等式约束)}\\ {\mathbf{g}}_2(\mathbf{x}(t),t)=0\text{(纯状态等式约束)}\\ \mathbf{h}(\mathbf{x}(t),\mathbf{u}(t),t)\ge 0\text{(不等式约束)}\end{array}$$

5.1.2 状态与输入定义

系统状态 $\mathbf{x}\in {\mathbb{R}}^{24}$ 与输入 $\mathbf{u}\in {\mathbb{R}}^{24}$ 定义为：

$$\mathbf{x}=\left[\begin{array}{c}{\mathbf{h}}_{com}\\ {\mathbf{q}}_b\\ {\mathbf{q}}_j\end{array}\right],\mathbf{u}=\left[\begin{array}{c}{\mathbf{f}}_c\\ {\mathbf{v}}_j\end{array}\right]$$

其中：

• ${\mathbf{h}}_{com}\in {\mathbb{R}}^6$：归一化质心动量 $[v_{com,x},v_{com,y},v_{com,z},L_x/m,L_y/m,L_z/m{]}^T$；
• ${\mathbf{q}}_b\in {\mathbb{R}}^6$：基座位姿 $[p_x,p_y,p_z,{\theta }_z,{\theta }_y,{\theta }_x{]}^T$（ZYX 欧拉角）；
• ${\mathbf{q}}_j\in {\mathbb{R}}^{12}$：12 个关节角（4 腿 × 3 关节 HAA/HFE/KFE）；
• ${\mathbf{f}}_c\in {\mathbb{R}}^{12}$：4 个足端接触力（每个 3 维）；
• ${\mathbf{v}}_j\in {\mathbb{R}}^{12}$：12 个关节速度。

5.1.3 质心动力学模型

系统动力学采用 归一化质心动力学：

$${\dot{\mathbf{h}}}_{com}=\left[\begin{array}{c}{\dot{\mathbf{v}}}_{com}\\ \dot{\mathbf{L}}/m\end{array}\right]=\left[\begin{array}{c}\frac{1}{m}{\sum }_{i=1}^4{\mathbf{f}}_{c,i}+\mathbf{g}\\ {\sum }_{i=1}^4({\mathbf{r}}_{c,i}-{\mathbf{r}}_{com})\times {\mathbf{f}}_{c,i}\end{array}\right]$$

基座运动学：
$${\dot{\mathbf{q}}}_b={\mathbf{J}}_b^{-1}(\mathbf{q})\cdot {\mathbf{v}}_{base}$$

关节运动学：
$${\dot{\mathbf{q}}}_j={\mathbf{v}}_j$$

动力学模型通过 CppAd 自动微分 生成（LeggedRobotDynamicsAD），避免手写雅可比。

5.1.4 代价函数

采用二次跟踪代价：

$$l(\mathbf{x},\mathbf{u},t)=(\mathbf{x}-{\mathbf{x}}_{ref}(t){)}^T\mathbf{Q}(\mathbf{x}-{\mathbf{x}}_{ref}(t))+(\mathbf{u}-{\mathbf{u}}_{ref}(t){)}^T\mathbf{R}(\mathbf{u}-{\mathbf{u}}_{ref}(t))$$

权重矩阵 $\mathbf{Q}$（24×24）与 $\mathbf{R}$（24×24）在 task.info 中配置。输入代价中关节速度项通过雅可比转换到任务空间：

$${\mathbf{R}}_{joint}={\mathbf{J}}_{base\to feet}^T{\mathbf{R}}_{task}{\mathbf{J}}_{base\to feet}$$

5.1.5 约束

(1) 摩擦锥约束（支撑腿，软约束形式）：

$$\mu \cdot f_z-\sqrt{f_x^2+f_y^2+ϵ}\ge 0$$

其中 $\mu$ 为摩擦系数（A1 默认 0.3），$ϵ$ 为正则化项避免梯度奇异。采用 Relaxed Barrier 惩罚函数转化为软约束：

$$\text{penalty}(h)=\{\begin{array}{ll}-\mu \ln (\delta -h)& h\le \delta -ϵ\\ \text{二次平滑}& \text{otherwise}\end{array}$$

(2) 零速度约束（支撑腿足端）：

$${\mathbf{v}}_{foot,i}={\mathbf{J}}_i(\mathbf{q})\dot{\mathbf{q}}=0,i\in \text{stance}$$

(3) 零力约束（摆动腿）：

$${\mathbf{f}}_{c,i}=0,i\in \text{swing}$$

(4) 摆动轨迹约束（摆动腿 z 轴位置）：

足端高度跟踪三次样条曲线：

$$z_{foot}(t)=\text{CubicSpline}(t;z_{liftoff},z_{mid},z_{touchdown})$$

中点高度 $z_{mid}=\max (z_{liftoff},z_{touchdown})+h_{swing}$，摆动高度 $h_{swing}=0.08$ m。

(5) 自碰撞约束（软约束）：指定 link 对（如 LF_calf, RF_calf）间最小距离 $d_{min}=0.05$ m。

5.1.6 求解方法

采用 多重打靶（Multiple Shooting） 将连续 OCP 离散化为 NLP，再用 SQP（序列二次规划） 求解：

1. 时间窗 $[t_0,t_I]$ 离散为 $N$ 段，每段 $\Delta t=0.015$ s；
2. 每个时间点设状态 ${\mathbf{x}}_k$ 与输入 ${\mathbf{u}}_k$ 为决策变量；
3. 动力学约束通过 RK2 积分器耦合相邻节点；
4. SQP 每次迭代求解一个 QP 子问题，QOASES/HPIPM 求解；
5. 默认 sqpIteration=1（每次 MPC 只迭代一次 SQP，依赖热启动）。

配置参数（task.info 中 sqp 段）：

• dt = 0.015：离散时间步；
• sqpIteration = 1：SQP 迭代次数；
• nThreads = 3：并行线程数；
• integratorType = RK2：二阶龙格库塔积分。

---

5.2 全身控制器（WBC）

5.2.1 决策变量

WBC 仅考虑当前时刻，决策变量为：

$${\mathbf{x}}_{wbc}=\left[\begin{array}{c}\ddot{\mathbf{q}}\\ {\mathbf{f}}_c\\ \mathbf{\tau }\end{array}\right]\in {\mathbb{R}}^{18+12+12=42}$$

其中：

• $\ddot{\mathbf{q}}\in {\mathbb{R}}^{18}$：广义坐标加速度（6 基座 + 12 关节）；
• ${\mathbf{f}}_c\in {\mathbb{R}}^{12}$：4 足端接触力；
• $\mathbf{\tau }\in {\mathbb{R}}^{12}$：12 关节力矩。

5.2.2 任务定义

WBC 定义了 7 个任务，分为等式约束 $\mathbf{Ax}=\mathbf{b}$ 与不等式约束 $\mathbf{Dx}\le \mathbf{f}$：

任务	类型	公式	优先级
浮动基座动力学方程	等式(约束)	$\mathbf{M}\ddot{\mathbf{q}}+\mathbf{h}(\mathbf{q},\dot{\mathbf{q}})={\mathbf{J}}^T{\mathbf{f}}_c+{\mathbf{S}}^T\mathbf{\tau }$	约束
力矩限制	不等式(约束)	$	\tau_i
摩擦锥	不等式(约束)	$\mu f_z\ge \sqrt{f_x^2+f_y^2}$	约束
支撑腿零运动	等式(约束)	${\mathbf{J}}_i\ddot{\mathbf{q}}+{\dot{\mathbf{J}}}_i\dot{\mathbf{q}}=0$	约束
摆动腿跟踪	等式(任务)	${\mathbf{J}}_i\ddot{\mathbf{q}}={\ddot{\mathbf{p}}}_{des}-{\dot{\mathbf{J}}}_i\dot{\mathbf{q}}$	高
基座加速度跟踪	等式(任务)	${\ddot{\mathbf{q}}}_{base}={\ddot{\mathbf{q}}}_{base,des}$	中
接触力跟踪	等式(任务)	${\mathbf{f}}_c={\mathbf{f}}_{c,des}$	低

其中摆动腿期望加速度采用 PD 反馈：

$${\ddot{\mathbf{p}}}_{des}=k_p({\mathbf{p}}_{des}-{\mathbf{p}}_{meas})+k_d({\dot{\mathbf{p}}}_{des}-{\dot{\mathbf{p}}}_{meas})$$

$k_p=350,k_d=37$（A1 配置）。

5.2.3 浮动基座动力学方程

基于 Pinocchio 的 CRBA 计算质量矩阵 $\mathbf{M}$，RNEA 计算非线性项 $\mathbf{h}=\mathbf{C}\dot{\mathbf{q}}+\mathbf{g}$：

$$\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{ccc}\mathbf{M}& -{\mathbf{J}}^T& -{\mathbf{S}}^T\end{array}\right]}}\left[\begin{array}{c}\ddot{\mathbf{q}}\\ {\mathbf{f}}_c\\ \mathbf{\tau }\end{array}\right]=\underset{\mathbf{b}}{\underbrace{-\mathbf{h}}}$$

其中 $\mathbf{S}=[0_{12\times 6},{\mathbf{I}}_{12}]$ 为关节选择矩阵，$\mathbf{J}$ 为足端雅可比堆叠。

5.2.4 求解方法

项目提供两种 WBC 实现：

(1) WeightedWbc（加权法，默认）：将所有任务合并为单个 QP：

$$\underset{\mathbf{x}}{\min }\sum _i{w}_i\Vert {\mathbf{A}}_i\mathbf{x}-{\mathbf{b}}_i{\Vert }^2\text{s.t.}{\mathbf{A}}_{con}\mathbf{x}={\mathbf{b}}_{con},{\mathbf{D}}_{con}\mathbf{x}\le {\mathbf{f}}_{con}$$

权重（A1 配置）：$w_{swing}=100,w_{base}=1,w_{force}=0.01$。使用 qpOASES 求解。

(2) HierarchicalWbc（分层法）：严格优先级，通过零空间投影（HoQp）：

$$\begin{array}{rl}& \text{Task 0 (约束): 浮动基座方程 + 力矩限制 + 摩擦锥 + 零运动}\\ & \text{Task 1: 基座加速度 + 摆动腿}\\ & \text{Task 2: 接触力跟踪}\end{array}$$

高优先级任务的解投影到低优先级任务的零空间：

$$\mathbf{x}={\mathbf{x}}_{prev}+{\mathbf{Z}}_{prev}{\mathbf{x}}_{curr}$$

其中 ${\mathbf{Z}}_{prev}$ 为前序任务雅可比的零空间基。

5.2.5 输出与电机命令

WBC 解出 $\mathbf{\tau }$ 后，与 NMPC 给出的期望关节位置 ${\mathbf{q}}_{des}$、速度 ${\dot{\mathbf{q}}}_{des}$ 组合为 混合命令：

$${\tau }_{motor}={\tau }_{wbc}+k_p(q_{des}-q)+k_d({\dot{q}}_{des}-\dot{q})$$

其中 $k_p=3,k_d=0$（低增益，仅作接触缓冲），${\tau }_{wbc}$ 为前馈力矩。该接口由 HybridJointInterface 实现。

---

5.3 状态估计（卡尔曼滤波）

5.3.1 状态与观测

采用线性卡尔曼滤波估计基座位置与速度（参考 Flayols et al. 2017）。

状态向量 $\hat{\mathbf{x}}\in {\mathbb{R}}^{18}$：

$$\hat{\mathbf{x}}=\left[\begin{array}{c}{\mathbf{p}}_{base}\\ {\mathbf{v}}_{base}\\ {\mathbf{p}}_{foot,1}\\ {\mathbf{p}}_{foot,2}\\ {\mathbf{p}}_{foot,3}\\ {\mathbf{p}}_{foot,4}\end{array}\right]\in {\mathbb{R}}^{18}$$

观测向量 $\mathbf{y}\in {\mathbb{R}}^{28}$：

$$\mathbf{y}=\left[\begin{array}{c}{\mathbf{p}}_{foot,1}-{\mathbf{p}}_{base}\\ ⋮\\ {\mathbf{p}}_{foot,4}-{\mathbf{p}}_{base}\\ {\mathbf{v}}_{foot,1}-{\mathbf{v}}_{base}\\ ⋮\\ {\mathbf{v}}_{foot,4}-{\mathbf{v}}_{base}\\ z_{foot,1}\\ ⋮\\ z_{foot,4}\end{array}\right]$$

足端位置/速度由正运动学从关节编码器计算，足端高度假设接触时为 0。

5.3.2 状态转移方程

$${\hat{\mathbf{x}}}_{k|k-1}=\mathbf{A}{\hat{\mathbf{x}}}_{k-1}+\mathbf{B}{\mathbf{a}}_{imu}$$

其中：

$$\mathbf{A}=\left[\begin{array}{ccc}{\mathbf{I}}_3& \Delta t{\mathbf{I}}_3& 0\\ 0& {\mathbf{I}}_3& 0\\ 0& 0& {\mathbf{I}}_{12}\end{array}\right],\mathbf{B}=\left[\begin{array}{c}\frac{1}{2}\Delta t^2{\mathbf{I}}_3\\ \Delta t{\mathbf{I}}_3\\ 0\end{array}\right]$$

${\mathbf{a}}_{imu}$ 为 IMU 线加速度（转换到世界系，扣除重力）。

5.3.3 观测方程

$$\mathbf{y}=\mathbf{C}\hat{\mathbf{x}}$$

$\mathbf{C}$ 将状态映射到足端相对基座的位置/速度。

5.3.4 卡尔曼更新

预测：

$${\mathbf{P}}_{k|k-1}=\mathbf{A}{\mathbf{P}}_{k-1}{\mathbf{A}}^T+\mathbf{Q}$$

更新：

$${\mathbf{K}}_k={\mathbf{P}}_{k|k-1}{\mathbf{C}}^T(\mathbf{C}{\mathbf{P}}_{k|k-1}{\mathbf{C}}^T+\mathbf{R}{)}^{-1}$$

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{y}}_k-\mathbf{C}{\hat{\mathbf{x}}}_{k|k-1})$$

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k\mathbf{C}){\mathbf{P}}_{k|k-1}$$

5.3.5 接触自适应噪声

当足端接触时，足端位置可信度高（噪声小）；未接触时不可信（噪声放大 100 倍）：

$${\mathbf{Q}}_{foot,i}=\{\begin{array}{ll}{\mathbf{Q}}_{foot}& \text{if contact}\\ 100\cdot {\mathbf{Q}}_{foot}& \text{if swing}\end{array}$$

5.3.6 外部里程计融合

支持订阅 /tracking_camera/odom/sample（如 T265 追踪相机）修正漂移，通过 updateFromTopic() 将外部位姿注入状态估计。

---

6. 关键模块详解

6.1 LeggedController（控制器主入口）

文件：legged_controllers/src/LeggedController.cpp

作为 ros-control 的 MultiInterfaceController 插件，管理整个控制流程：

• init()：加载 task/urdf/reference 文件，构建 LeggedInterface、MPC、WBC、StateEstimate；
• starting()：等待 MPC 首次策略就绪；
• update()：每个控制周期（500Hz）执行：
  1. updateStateEstimation()：读取 IMU/关节/接触，更新卡尔曼滤波，转换到质心状态；
  2. mpcMrtInterface_->setCurrentObservation()：将当前状态发送给 MPC；
  3. mpcMrtInterface_->updatePolicy() / evaluatePolicy()：获取优化后的状态/输入；
  4. wbc_->update()：求解 WBC 得到力矩；
  5. safetyChecker_->check()：姿态安全检查（翻车保护）；
  6. hybridJointHandles_[j].setCommand()：下发混合命令；
• setupMrt()：启动独立 MPC 线程，按 mpcDesiredFrequency=100Hz 调用 advanceMpc()。

6.2 LeggedInterface（OCP 建模）

文件：legged_interface/src/LeggedInterface.cpp

构建 OCS2 的 OptimalControlProblem：

• setupModel()：从 URDF 创建 PinocchioInterface 与 CentroidalModelInfo；
• setupOptimalControlProblem()：组装动力学、代价、约束：
  • 动力学：LeggedRobotDynamicsAD（CppAd 自动微分质心动力学）；
  • 代价：LeggedRobotStateInputQuadraticCost（Q/R 加权跟踪）；
  • 约束：摩擦锥（软）、零力、零速度、法向速度、自碰撞（软）；
• setupReferenceManager()：创建 SwitchedModelReferenceManager，管理步态调度与摆动轨迹规划；
• loadGaitSchedule()：从 reference.info 加载步态模式序列。

6.3 WBC 实现

WbcBase（任务构造）

文件：legged_wbc/src/WbcBase.cpp

提供 7 个 formulateXxxTask() 方法，每个返回 Task{A, b, D, f} 结构。updateMeasured() 调用 Pinocchio 计算 $\mathbf{M},\mathbf{h},\mathbf{J},\dot{\mathbf{J}}$。

WeightedWbc（默认）

文件：legged_wbc/src/WeightedWbc.cpp

将约束任务作为硬约束，跟踪任务加权求和构造代价：

$$\mathbf{H}=\sum _i{w}_i{\mathbf{A}}_i^T{\mathbf{A}}_i,\mathbf{g}=-\sum _i{w}_i{\mathbf{A}}_i^T{\mathbf{b}}_i$$

调用 qpOASES 求解 QP：min 0.5 x^T H x + g^T x s.t. lbA ≤ A x ≤ ubA。

HierarchicalWbc（分层）

文件：legged_wbc/src/HierarchicalWbc.cpp

通过 HoQp 类实现零空间投影，严格保证优先级。HoQp 递归构造：HoQp(task2, HoQp(task1, HoQp(task0)))。

6.4 状态估计

文件：legged_estimation/src/LinearKalmanFilter.cpp

KalmanFilterEstimate 实现 5.3 节所述算法。关键点：

• update() 中根据 contactFlag_ 动态调整 $\mathbf{Q}/\mathbf{R}$；
• 通过 eeKinematics_ 计算足端位置/速度（正运动学）；
• 发布 nav_msgs/Odometry 到 /odom；
• 支持外部里程计融合（T265 等）。

FromTopicStateEstimate（cheater 模式）直接从 Gazebo ground truth 话题读取，仅用于仿真调试。

6.5 硬件接口

LeggedHW（基类）

文件：legged_hw/include/legged_hw/LeggedHW.h

注册四种接口：

• JointStateInterface：关节位置/速度/力矩；
• ImuSensorInterface：IMU 姿态/角速度/线加速度；
• HybridJointInterface：混合命令（pos_des, vel_des, kp, kd, ff）；
• ContactSensorInterface：足端接触布尔值。

UnitreeHW（Unitree 实现）

文件：legged_examples/legged_unitree/legged_unitree_hw/include/legged_unitree_hw/UnitreeHW.h

通过 Unitree SDK（3.3.1 / 3.8.0）UDP 通信：

• read()：接收 LowState（关节状态、IMU）；
• write()：发送 LowCmd（混合命令转换为 q_des, v_des, kp, kd, tau_ff）；
• 接触检测：足端力阈值判断（contact_threshold=40）。

LeggedHWLoop（控制循环）

文件：legged_hw/include/legged_hw/LeggedHWLoop.h

独立线程按 loop_frequency=500Hz 执行 read() → controller_manager_->update() → write()，线程优先级 95（实时）。

6.6 TargetTrajectoriesPublisher（目标生成）

文件：legged_controllers/src/TargetTrajectoriesPublisher.cpp

将用户指令转换为 MPC 参考轨迹：

• goalToTargetTrajectories()：/move_base_simple/goal → 目标位姿轨迹（用于导航）；
• cmdVelToTargetTrajectories()：/cmd_vel → 速度参考轨迹（用于遥控）。

到达时间估计：t = max(|dyaw|/ω_rot, |disp|/v_disp)，默认 $v_{disp}=0.5$ m/s, ${\omega }_{rot}=1.57$ rad/s。

---

7. 配置参数说明

7.1 task.info（核心配置）

文件：legged_controllers/config/a1/task.info

段落	参数	含义	A1 默认值
centroidalModelType	0/1	0=完整质心动力学, 1=单刚体	0
model_settings	positionErrorGain	零速度约束位置误差增益	0.0
	phaseTransitionStanceTime	相位切换站立时间	0.1 s
swing_trajectory_config	swingHeight	摆动高度	0.08 m
	liftOffVelocity	起摆速度	0.05 m/s
	touchDownVelocity	落地速度	-0.1 m/s
ddp/sqp/ipm	dt	离散时间步	0.015 s
	maxNumIterations/sqpIteration	迭代次数	1
	nThreads	并行线程	3
mpc	timeHorizon	预测时域	1.0 s
	mpcDesiredFrequency	MPC 求解频率	100 Hz
Q	对角元素	状态跟踪权重	见文件
R	对角元素	输入跟踪权重	见文件
frictionConeSoftConstraint	frictionCoefficient	摩擦系数	0.3
torqueLimitsTask	HAA/HFE/KFE	关节力矩限制	33.5 Nm
swingLegTask	kp/kd	摆动腿 PD	350/37
weight	swingLeg/baseAccel/contactForce	WBC 任务权重	100/1/0.01
kalmanFilter	各噪声参数	KF 过程/观测噪声	见文件

7.2 gait.info（步态定义）

文件：legged_controllers/config/a1/gait.info

支持 12 种步态，通过 modeSequence 与 switchingTimes 定义。模式命名约定：LF_RH 表示左前+右后支撑。

常用步态：

步态	模式序列	周期	说明
stance	STANCE	0.5s	原地站立
trot	LF_RH ↔ RF_LH	0.6s	对角小跑
flying_trot	LF_RH → FLY → RF_LH → FLY	0.4s	飞奔（有腾空相）
pace	LF_LH ↔ RF_RH	0.6s	侧步（同侧）
dynamic_walk	三足→二足循环	1.0s	动态步行
static_walk	三足轮换	1.2s	静态步行

7.3 reference.info（参考轨迹）

文件：legged_controllers/config/a1/reference.info

• comHeight：质心高度（A1 为 0.3 m）；
• defaultJointState：默认关节角（站立姿态）；
• targetDisplacementVelocity / targetRotationVelocity：目标到达速度；
• initialModeSchedule / defaultModeSequenceTemplate：初始与默认步态调度。

---

8. 运行步骤

8.1 环境准备

依赖：

• ROS Noetic (Ubuntu 20.04)
• OCS2 + Pinocchio + hpp-fcl（按 OCS2 文档编译 ocs2_legged_robot_ros）
• qpOASES（已包含 qpoases_catkin）

8.2 编译

# 编译核心控制器与机器人描述
catkin build legged_controllers legged_unitree_description

# 编译 Gazebo 仿真（不要在机载电脑上编译）
catkin build legged_gazebo

# 编译真机硬件接口（仅真机需要）
catkin build legged_unitree_hw

8.3 仿真运行

步骤 1：设置机器人类型

export ROBOT_TYPE=a1   # 可选: a1, aliengo, go1

步骤 2：启动 Gazebo 仿真

roslaunch legged_unitree_description empty_world.launch

步骤 3：加载控制器

roslaunch legged_controllers load_controller.launch cheater:=false

cheater:=true 会额外加载 legged_cheater_controller（使用 Gazebo ground truth，禁止用于真机）。

步骤 4：启动控制器

rosservice call /controller_manager/switch_controller "start_controllers: ['controllers/legged_controller']
stop_controllers: ['']
strictness: 0
start_asap: false
timeout: 0.0"

或使用 GUI：

sudo apt install ros-noetic-rqt-controller-manager
rosrun rqt_controller_manager rqt_controller_manager

步骤 5：发送指令

• 步态切换：在 load_controller.launch 终端输入步态名（trot/pace/walk 等）；
• 速度控制：发布 /cmd_vel；
• 目标导航：在 RViz 中使用 2D Nav Goal 发布 /move_base_simple/goal。

8.4 真机运行

# 启动硬件接口（替代 Gazebo）
roslaunch legged_unitree_hw legged_unitree_hw.launch

# 后续步骤与仿真相同
roslaunch legged_controllers load_controller.launch cheater:=false
# 启动 legged_controller（切勿启动 cheater_controller）

机载电脑建议：使用 NUC 等外部计算设备，作者使用 11 代 NUC，MPC 频率可达 200 Hz。

8.5 注意事项

• 步态与目标相互独立：机器人四足着地时无需切换到 stance 步态（它已经是 stance）；
• target_trajectories_publisher 仅为示例，可整合手柄/键盘输入实现步态与高度切换、启停控制；
• 真机部署时务必先在仿真中验证参数。

---

9. 部署到自定义机器人

部署流程简单，仅需两步：

9.1 实现硬件接口

参考 legged_examples/legged_unitree/legged_unitree_hw 中的 UnitreeHW 类：

1. 继承 LeggedHW；
2. 实现 read()：从机器人读取关节状态、IMU、接触信息；
3. 实现 write()：将 HybridJointHandle 的 posDes/velDes/kp/kd/ff 下发到电机；
4. 实现 setupJoints() / setupImu() / setupContactSensor() 配置硬件。

关节命名约定（必须一致）：

LF_HAA, LF_HFE, LF_KFE   (左前: 髋外展/髋屈伸/膝屈伸)
LH_HAA, LH_HFE, LH_KFE   (左后)
RF_HAA, RF_HFE, RF_KFE   (右前)
RH_HAA, RH_HFE, RH_KFE   (右后)

9.2 编写 URDF

参考 legged_examples/legged_unitree/legged_unitree_description：

1. 编写 xacro 文件，joint/link 命名与上述约定一致；
2. 在 const.xacro 中定义质量、惯量、尺寸等常量；
3. 使用 legged_common/scripts/generate_urdf.sh 生成 URDF 到 /tmp/legged_control/<robot>.urdf；
4. 在 legged_controllers/config/<robot>/ 下创建 task.info、gait.info、reference.info（可复制 A1 配置修改）。

9.3 调参建议

1. 质量/惯量：URDF 中务必准确（影响动力学模型）；
2. Q/R 权重：基座位置权重应远大于关节权重（A1: 1000 vs 5）；
3. 摩擦系数：根据地面材质调整（A1 默认 0.3 偏保守）；
4. 力矩限制：根据电机规格设置（A1: 33.5 Nm）；
5. KF 噪声：IMU 噪声大则增大 imuProcessNoise*，足端接触可靠则减小 footSensorNoise*。

---

10. 参考文献

1. T. Flayols, A. Del Prete, P. Wensing, A. Mifsud, M. Benallegue, and O. Stasse, “Experimental evaluation of simple estimators for humanoid robots,” IEEE-RAS Int. Conf. Humanoid Robot., pp. 889–895, 2017.

2. J. P. Sleiman, F. Farshidian, M. V. Minniti, and M. Hutter, “A Unified MPC Framework for Whole-Body Dynamic Locomotion and Manipulation,” IEEE Robot. Autom. Lett., vol. 6, no. 3, pp. 4688–4695, 2021.

3. R. Grandia, F. Jenelten, S. Yang, F. Farshidian, and M. Hutter, “Perceptive Locomotion through Nonlinear Model Predictive Control,” IEEE Trans. Robot., 2022.

4. C. Dario Bellicoso, C. Gehring, J. Hwangbo, P. Fankhauser, and M. Hutter, “Perception-less terrain adaptation through whole body control and hierarchical optimization,” IEEE-RAS Int. Conf. Humanoid Robots, pp. 558–564, 2016.

5. Q. Liao, Z. Li, A. Thirugnanam, J. Zeng, K. Sreenath, “Walking in narrow spaces: Safety-critical locomotion control for quadrupedal robots with duality-based optimization,” IEEE/RSJ IROS, pp. 2723–2730, 2023.

---

附录：关键数据维度速查

量	维度	说明
NMPC 状态 $\mathbf{x}$	24	6 质心动量 + 6 基座位姿 + 12 关节角
NMPC 输入 $\mathbf{u}$	24	12 接触力 + 12 关节速度
WBC 决策变量 ${\mathbf{x}}_{wbc}$	42	18 广义加速度 + 12 接触力 + 12 力矩
卡尔曼状态 $\hat{\mathbf{x}}$	18	3 基座位置 + 3 基座速度 + 12 足端位置
卡尔曼观测 $\mathbf{y}$	28	12 足端相对位置 + 12 相对速度 + 4 足端高度
MPC 时域	1.0 s	离散为约 67 个节点（dt=0.015）
控制频率	500 Hz	硬件循环 + WBC
MPC 频率	100 Hz	SQP 求解