# MPC_webots_emulation

**Repository Path**: momaoto/mpc_webots_emulation

## Basic Information

- **Project Name**: MPC_webots_emulation
- **Description**: No description available
- **Primary Language**: C++
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 2
- **Forks**: 2
- **Created**: 2024-01-17
- **Last Updated**: 2025-12-15

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# MPC代码复现

<h4 style="text-align:right">2023.12.10 </h4>

## 原理

> 机器人控制系统，就是要解决机器人在哪儿、去哪儿、怎么去的问题，分别对应了机器人控制系统的状态估计器、轨迹规划器和控制器。

![MPC1](https://gitee.com/momaoto/xjrc_-mpc_2024_thk/raw/master/picture/%E6%8E%A7%E5%88%B6%E7%B3%BB%E7%BB%9F%E6%80%BB%E6%A1%86%E5%9B%BE.png)

原论文中第二章介绍了控制系统中的2.3步态调度、2.4状态估计、2.5质心轨迹生成、2.6摆动相规划，分别解决了在哪儿（状态估计）与去哪儿（轨迹规划）的问题。第三章使用MPC对四足机器人控制量进行优化计算，解决机器人怎么去（模型预测控制）的问题。现对以上内容尝试c++面向对象的实现。

狗腿说明：

>   狗头
>
>   2       1
>
>   4       3



## 框架设计

- `GaitScheduler`类：步态调度器，提供机器人**步态变量输出**，以供摆动相规划与支撑相控制。
- `StateEstimator`类：状态估计器，收集**IMU与编码器返回的原始状态信息**，加工并输出**世界系下质心与四足的位置速度信息**。
- `TrajectoryGenerator`类：质心轨迹生成器，接收遥控器的指令**期望速度**，生成**期望轨迹**。
- `SwingPhasePlanner`类：摆动相规划器，生成**落足点坐标**与**摆动足轨迹**。

## 具体实现

*以下为提供给用户使用的接口说明。*

---

### `params.cpp`

这里定义了四足机器人全局静态参数，使用时预先在这里设置好：

```c++
// 机器狗参数
double H_X = 0.275;  // (2.2)
double H_Y = 0.0625; // (2.2)
double L_1 = 0.088;  // (2.2)
double L_2 = 0.27;   // (2.2)
double L_3 = 0.27;   // (2.2)
double Mass = 5.0;   // 质量
// 机器狗惯性张量
double I_B_array[3][3] = {{0.31, 0, -0.05},
                          {0, 1.09, 0},
                          {-0.05, 0, 1.12}};

// 控制周期
double DELTA_T_MPC = 0.01;
double DELTA_T = 0.002;
```

---

### `GaitScheduler.cpp`

> 步态调度器，负责根据用户选择的步态模式计算输出每个控制周期四足的摆动相、支撑相相位，反映步态信息

构造参数:

```c++
GaitScheduler(float currentTime, float swingTime, float standTime, string mode);
```

- `currentTime`:当前时间
- `swingTime`:名义摆动相时间
- `standTime`:名义支撑相时间
- `mode`:步态模式

使用方法：

```c++
public:
    int n_;              // 当前迈步周期数
    double *t_fai_norm_; // 相位进度
    double *s_fai_;      // 是否处于支撑相
    double *t_st_norm_;  // 支撑相相位进度
    double *t_sw_norm_;  // 摆动相相位进度
```

> 以上数组指针均为`float[4]`数组。
>
> 无公共方法，**类实例化后直接调公共成员变量**即可。

---

### `StateEstimator.cpp`

> 状态估计器，负责估计四足机器人运动过程中难以直接测量的状态信息，对IMU返回的原始旋转矩阵、电机编码器返回的角度与转速进行转换，反映四足机器人在世界系下的质心位置、质心速度、四足位置等状态信息。其中包括一个使用卡尔曼滤波实现的足底里程计，帮助MPC算法更加精确地收集状态信息。

使用方法：

```cpp
StateEstimator se;

//传入IMU原始旋转矩阵
se.setIMUdata(Eigen::Matrix3d R_imu_imu0);
// 返回躯干旋转矩阵
se.getR_B_O();
//返回质心加速度
se.geta_com_B(Eigen::Vector3d *omega_original);
//返回世界系下质心加速度
se.geta_com_O();
//返回躯干角速度
se.getomega_OB_B(Eigen::Vector3d *omega_original);
//返回世界系下躯干角速度
se.getomega_OB_O();
//返回i足足底位置
se.getp_i_B(int i, Eigen::Vector3d *q_j_i); 
//返回i足足底速度
se.getp_dot_i_B(int i, Eigen::Vector3d *q_j_i, Eigen::Vector3d *q_dot_j_i);
//返回18*1状态变量:质心位置3*1,质心速度3*1,四足足底位置12*1
se.getx_k(Eigen::Vector3d *q_j_i, Eigen::Vector3d *q_dot_j_i);

```

---

### `TrajectoryGenerator.cpp`

> 质心轨迹生成器，负责接受手柄传来的控制命令，转化为MPC算法需要的未来h周期地机器人期望轨迹（状态），作为实际轨迹优化问题的目标。

- 接受：期望速度，期望角速度
- 返回：期望速度，期望角速度，期望位置，期望角位置（欧拉角）

构造时默认当前周期为0， 接收到手柄传来的**本体系下**的期望速度与角速度后生成**未来h周期**的、**世界系下**的期望速度，角速度，位置，欧拉角。

每次生成期望轨迹后，预测区间右移一格，重新生成未来h周期期望轨迹。

使用方法：

```cpp
TrajectoryGenerator tg;

//传入手柄控制信号，质心x,y速度与偏航角速度
tg.setExpection(double v_x, double v_y, double omega_z);
//传入旋转矩阵，计算期望轨迹
tg.calculateAll(Eigen::Matrix3d *Rot);
//返回期望轨迹到vecD
tg.getD(Eigen::Matrix<double, 13 * PREDICTION_HORIZON, 1> vecD);
```

---

### `SwingPhasePlanner.cpp`

> 摆动相规划器，负责四足足底面对不同地形时摆动相的轨迹规划与落足点规划，使机器人平稳、省力地通过不同地形。

### `MPC.cpp`

- 目的：计算出当前时刻（t时刻）应当输入的控制量$U$
- 约束：
  1. 输入$U$后系统状态与预期状态尽可能接近(误差最小)
  2. 所输入的$U$本身尽可能小(输入最小)

控制以上两个约束的权重后，求$U$为一个二次凸优化问题，以下为MPC具体实现时所需要的变量推导：

![MPC2](https://gitee.com/momaoto/xjrc_-mpc_2024_thk/raw/master/picture/MPC%E5%8F%98%E9%87%8F%E4%BE%9D%E8%B5%96.png)

根据前文实现的四个计算类编写代码，求出当前时刻最优输入控制量$U$，其中第一列为所需足底反力。

使用方法：

```c++
MPC mpc;
//设置期望轨迹、当前周期状态
mpc.setD_and_x0(Eigen::Matrix<double, 13 * PREDICTION_HORIZON, 1> *vecD,
                              Eigen::Matrix<double, 13, 1> *x_0);
//传入当前周期偏航角、名义落足点，计算MPC预测方程参数
mpc.calA_qp_and_B_qp(Eigen::Matrix<double, PREDICTION_HORIZON, 1> *psi_k_d,
                          Eigen::Matrix<double, 3 * PREDICTION_HORIZON, 3> *r_k_ix);
//计算控制量优化问题参数
mpc.qpSolveU();
```



## 其他

---

### Intel  MKL加速

在`#include`之后的预处理区添加以下代码，可利用intel硬件加速Eigen矩阵的计算：

```c++
// 使用Intel MKL 线性代数库加速
#ifndef EIGEN_USE_MKL_ALL
#define EIGEN_USE_MKL_ALL
#endif

#ifndef EIGEN_VECTORIZE_SSE4_2
#define EIGEN_VECTORIZE_SSE4_2
#endif
```

- 此外针对MIT cheetah software有相关[计算优化手段](https://zhuanlan.zhihu.com/p/185693836)

- MIT cheetah software[源码](https://github.com/mit-biomimetics/Cheetah-Software/tree/master)

---

### Eigen库函数

```c++
matrix.block<p,q>(i,j);  //从坐标(i,j)提取大小为<p,q>的块，可读可写
```

---

### qpOASES使用

- qpOASES[使用说明](https://blog.csdn.net/weixin_40709533/article/details/86064148)

### IMU相关

- [IMU模块中的一些基本概念和常见问题](https://zhuanlan.zhihu.com/p/344884686)



## 日志

- 11.19 构建第二章代码框架
- 11.20 实现`GaitScheduler`，增加性能测试函数
- 11.22 添加Eigen矩阵计算库，添加开源协议，开始实现`StateEstimator`
- 11.23 实现`StateEstimator`卡尔曼滤波前的部分
- 11.24 实现`StateEstimator`卡尔曼滤波状态方程部分，完善readme`GaitScheduler`部分
- 11.28 实现`StateEstimator`完成(含卡尔曼滤波)，通过初步测试
- 11.30 实现`TrajectoryGenerator`至地形坡度估计，暂时跳过
- 12.02 分析MPC算法具体实现逻辑
- 12.6 搭建MPC算法框架
- 12.10 实现MPC初版代码