我家自动化

这个屌丝很懒，什么也没留下！

热门标签

Cheetah-Software方案分析_contact model fusion

作者：我家自动化 | 2024-04-13 05:49:21

踩

contact model fusion

简述

MIT Biomimetic Robotics Lab开源的Cheetah-Software为MIT Cheetah 3和MIT Mini Cheetah的控制系统，两个平台在控制和和通讯上都有一定区别。
请添加图片描述
MIT的Cheetah-Software方案是很经典的四足机器人控制框架，在这之后，有不少更优秀的方案，但不妨碍从Cheetah开始入手学习四足机器人。

其他一些方案参考

本体

机械结构

请添加图片描述
四足顺序

序号	位置
0	Front Right(FR)
1	Front Left(FL)
2	Hind Right(HR)
3	Hind Left(HL)

单腿顺序

序号	关节
0	Ab/Ad
1	hip
2	knee

硬件

Mini Cheetah主控采用UP board(4-core Intel Atom x5-Z8350 processor, 4 gb RAM, and roughly 5 W peak power consumption)，因UP board无CAN口，增加SPIne负责CAN通讯，UP board使用spidev通过SPI（Serial Peripheral Interface）与SPIne通讯。

SPIne上有两个STM32负责打包CAN消息，12个STM32作为电机控制器只运行FOC算法并用CAN总线与SPIne通信。

SPIne之所以用两个STM32是因为带宽不够, 每个STM32有两路CAN总线, 每一路负责三个电机（一条腿）的通讯才能达到1000Hz, 若一路负责六个电机控制频率只能降到约600Hz

请添加图片描述

软件架构

请添加图片描述

通过main_helper选择启动机器人类型和模式，运行bridge
在bridge中运行通讯相关线程，加载参数，并运行runner
在runner中初始化腿控制器，状态估计等类，并运行controller
在controller中通过状态机实现状态的切换与控制

模块分析

├── common
├── config 		# 参数
├── lcm-types 	# lcm类型
├── resources	# 仿真模型
├── robot 		# 定义runner和bridge，与实物的通讯接口（ethercat，spi，serial，imu，遥控器）
├── scripts     # 脚本
├── sim 		# 仿真，依赖QT5和OpenGL实现，通过共享内存实现通讯
├── third-party
└── user 		# 定义控制器
1
2
3
4
5
6
7
8
9

common

├── FootstepPlanner
├── include
│   ├── Collision # 仿真碰撞检测 
│   ├── Controllers # 控制相关
│   ├── ControlParameters # 控制器参数
│   ├── Dynamics # 四足机器人模型（运动学，动力学）
│   ├── Math
│   ├── SimUtilities
│   ├── SparseCMPC # 转换为稀疏矩阵存储格式CSC（Compressed Sparse Columns Format）用于OSQP求解
│   └── Utilities # 通用
└──  src
    ├── Collision
    ├── Controllers
    ├── ControlParameters
    ├── Dynamics
    ├── SimUtilities
    ├── SparseCMPC
    └── Utilities
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

Controllers

Estimator 状态估计

通过StateEstimatorContainer调用定义好的估计器，已实现估计器有
- ContactEstimator 足底接触估计，算法没有实现，只是将理想接触周期返回
- PositionVelocityEstimator 位置速度估计（包含作弊模式，用于仿真）
- OrientationEstimator 姿态估计（包含作弊模式，用于仿真）
DesiredStateCommand 控制命令
FootSwingTrajectory 摆动腿轨迹生成
LegController 单腿控制器
GaitScheduler 步态生成（分析后，并未使用）

Utilities

PeriodicTask 定义周期任务，对多线程实现的周期任务进行管理，包括启动停止，状态检测
filters 滤波器

robot

├── HardwareBridge.h	# 实物
├── SimulationBridge.h	# 仿真
├── main_helper.h 		# 启动入口
├── RobotController.h	# 控制器基类，控制器基础该类编写
├── RobotRunner.h
└── rt 					# 通讯接口
1
2
3
4
5
6

bridge

调用线程功能类型
- statusTask 监控任务状态
- spiTask SPI通讯
- microstrain IMU通讯
- Runner
- visualizationLCMTask 仿真环境数据
- sbusTask 遥控器通讯并获取控制指令
- microstrainLogger IMU数据
runner

The RobotRunner class actually runs the control code, and connects it with either the HardwareBridge or SimulationBridge.

通过JPosInitializer初始化机器狗姿态然后运行controller。

user

├── CMakeLists.txt
├── Controllers # 控制
│   ├── BackFlip
│   ├── BalanceController # 平衡控制，未使用
│   ├── convexMPC
│   ├── VisionMPC # 视觉引导控制
│   ├── WBC
│   └── WBC_Ctrl
├── FSM_State # 状态机　
├── main.cpp
├── MIT_Controller.cpp
├── MIT_Controller.hpp
└── MIT_UserParameters.h
1
2
3
4
5
6
7
8
9
10
11
12
13

定义机器人控制器，mini cheetah默认调用频率为0.5khz，cheetah3默认调用频率为1khz

状态估计

├── include
│   └── Controllers
│       ├── ContactEstimator.h          // 触地估计
│       ├── OrientationEstimator.h		// 姿态，角速度，加速度估计
│       ├── PositionVelocityEstimator.h // 位置，线速度估计
│       └── StateEstimatorContainer.h   // 状态估计数据结构体，估计器通用接口，估计器容器
└── src
    └── Controllers
        ├── ContactEstimator.cpp
        ├── OrientationEstimator.cpp
        └── PositionVelocityEstimator.cpp
1
2
3
4
5
6
7
8
9
10
11

StateEstimatorContainer定义了状态估计数据结构体，状态估计父类，状态估计容器。

struct StateEstimate // Result of state estimation
    
class GenericEstimator // All Estimators should inherit from this class
    
class StateEstimatorContainer // Contains all GenericEstimators, and can run them
1
2
3
4
5

估计顺序为

ContactEstimator->OrientationEstimator->PositionVelocityEstimator

触地估计

MIT发表的《Contact Model Fusion for Event-Based Locomotion in Unstructured Terrains》实现了基于事件的触地估计，但相关代码未开源，开源控制器中使用固定周期实现。

位姿估计

并未找到状态估计实现的相关官方论文，此处参考

MIT开源四足机器人状态估计算法

《基于稳定性的仿生四足机器人控制系统设计》

四足机器人状态估计1-MIT的KF融合框架

姿态估计

使用启动时的朝向作为 $r_z = 0$ 朝向，其他姿态及角速度直接使用IMU数据。

位置估计

位置和速度估计采用kalman滤波融合imu数据和足底里程计，其中足底里程计假设支撑足不打滑，加速度直接采用加速计数据。

预测模型

状态变量为

[\begin{matrix} ^{O} P_{c o m} & ^{O} V_{c o m} & ^{O} P_{1} & ^{O} P_{2} & ^{O} P_{3} & ^{O} P_{4} \end{matrix}]

$\begin{bmatrix} ^{O}P_{com} & ^{O}V_{com} & ^{O}P_1 & ^{O}P_2 & ^{O}P_3 & ^{O}P_4\end{bmatrix}$ ^T

x = [^{O} P_{co m}^{O} V_{co m}^{O} P_{1}^{O} P_{2}^{O} P_{3}^{O} P_{4}]^{T}

$^{O}P_{com}$ 表示世界坐标系下机身质心位置，3*1矩阵
$^{O}V_{com}$ 表示世界坐标系下机身质心速度，3*1矩阵
$^{O}P_i$ 表示世界坐标系下足端位置，3*1矩阵

输入变量为

[\begin{matrix} a_{x} \\ a_{y} \\ a_{z} \end{matrix}]

$\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}$ +g_{3 \times 1} =

[\begin{matrix} a_{x} \\ a_{y} \\ a_{z} \end{matrix}]

$\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}$ +

[\begin{matrix} 0 \\ 0 \\ - 9.81 \end{matrix}]

$\begin{bmatrix}0\\0\\-9.81\end{bmatrix}$

u = a_{x} a_{y} a_{z} + g_{3 \times 1} = a_{x} a_{y} a_{z} + 00 - 9.81

a为表示世界坐标系下机身质心加速度（通过加速度计获得），g为重力加速度

状态方程

此处位置 $P$ 计算忽略 $\frac{a_k*dt^2}{2}$ ， $a_k$ 为减去重力加速度后的加速度

\begin{matrix} P_{c o m, k + 1} = P_{c o m, k} + V_{c o m, k} \cdot △ t \\ V_{c o m, k + 1} = V_{c o m, k} + a_{k} \cdot △ t \\ P_{i, k + 1} = P_{i, k} \end{matrix}

$\begin{matrix} P_{com,k+1} = P_{com,k}+V_{com,k} \cdot \triangle t\\ V_{com,k+1} = V_{com,k}+a_k \cdot \triangle t\\ P_{i,k+1} = P_{i,k} \end{matrix}$

P_{co m, k + 1} = P_{co m, k} + V_{co m, k} \cdot △ t V_{co m, k + 1} = V_{co m, k} + a_{k} \cdot △ t P_{i, k + 1} = P_{i, k}

$x_{k+1} = A_k x_k +B_k u_k+v_k$

\begin{matrix} A_{k} = [\begin{matrix} I_{3} & I_{3} \cdot △ t & 0_{3 \times 12} \\ 0_{3 \times 3} & I_{3} & 0_{3 \times 12} \\ 0_{12 \times 3} & 0_{12 \times 3} & I_{12} \end{matrix}] & B_{k} = [\begin{matrix} 0_{3 \times 3} \\ I_{3} \cdot △ t \\ 0_{12 \times 3} \end{matrix}] \end{matrix}

$\begin{matrix} A_k = \begin{bmatrix} \mathbb{I}_3 & \mathbb{I}_3 \cdot \triangle t & 0_{3 \times 12}\\ 0_{3 \times 3} & \mathbb{I}_3 & 0_{3 \times 12}\\ 0_{12 \times 3} & 0_{12 \times 3} & \mathbb{I}_{12} \end{bmatrix}& B_k = \begin{bmatrix} 0_{3 \times 3}\\\mathbb{I}_3 \cdot \triangle t\\0_{12 \times 3} \end{bmatrix} \end{matrix}$

A_{k} = I_{3} 0_{3 \times 3} 0_{12 \times 3} I_{3} \cdot △ t I_{3} 0_{12 \times 3} 0_{3 \times 12} 0_{3 \times 12} I_{12} B_{k} = 0_{3 \times 3} I_{3} \cdot △ t 0_{12 \times 3}

观测模型

观测变量

[\begin{matrix} ^{B} P_{1} & ^{B} P_{2} & ^{B} P_{3} & ^{B} P_{4} & ^{O} V_{1} & ^{O} V_{2} & ^{O} V_{3} & ^{O} V_{4} & ^{O} z_{1} & ^{O} z_{2} & ^{O} z_{3} & ^{O} z_{4} \end{matrix}]

$\begin{bmatrix} ^{B}P_1 & ^{B}P_2 & ^{B}P_3 & ^{B}P_4 & ^{O}V_1 & ^{O}V_2 & ^{O}V_3 & ^{O}V_4 & ^{O}z_1 & ^{O}z_2 & ^{O}z_3 & ^{O}z_4 \end{bmatrix}$ ^T

z = [^{B} P_{1}^{B} P_{2}^{B} P_{3}^{B} P_{4}^{O} V_{1}^{O} V_{2}^{O} V_{3}^{O} V_{4}^{O} z_{1}^{O} z_{2}^{O} z_{3}^{O} z_{4}]^{T}

$^{B}P_i$ 表示机器人坐标系下足端位置，3*1矩阵（触地时为负的机器人高度）
$^{O}V_i$ 表示世界坐标系下腿的速度，3*1矩阵
$^{O}z_i$ 表示世界坐标系下足端位置z分量（触地时为0）

观测方程

\begin{matrix} Z_{k + 1} = H x_{k + 1} + w_{k} \end{matrix}

$\begin{matrix} Z_{k+1} = H x_{k+1} +w_k \end{matrix}$

Z_{k + 1} = H x_{k + 1} + w_{k}

请添加图片描述

预测更新

在进行状态估计时，当腿 $i$ 处于摆动相及刚触地与刚离地时，扩大腿部位置预测误差的协方差值，及腿部位置，速度及高度的观测误差协方差矩阵，扩大值通过trust控制：

程序中腿部位置对应的观测误差协方差矩阵并没有扩大

\begin{matrix} Q_{b l o c k} (6 + 3 \cdot i, 3, 3) = (1 + (1 - trust) \cdot n_{b i g n u m b e r}) * Q_{b l o c k} (6 + i \cdot 3, 3, 3) \\ R_{b l o c k} (3 \cdot i, 3, 3) = (1 + (1 - trust) \cdot n_{b i g n u m b e r}) * R_{b l o c k} (3 \cdot i, 3, 3) \\ R_{b l o c k} (12 + 3 \cdot i, 3, 3) = (1 + (1 - trust) \cdot n_{b i g n u m b e r}) * R_{b l o c k} (12 + 3 \cdot i, 3, 3) \\ R_{b l o c k} (24 + 3 \cdot i, 1, 1) = (1 + (1 - trust) \cdot n_{b i g n u m b e r}) * R_{b l o c k} (24 + 3 \cdot i, 1, 1) \end{matrix}

$\begin{matrix} Q_{block}(6+3\cdot i,3,3) = (1+(1-\text{trust}) \cdot n_{bignumber}) * Q_{block}(6+i\cdot3,3,3)\\ R_{block}(3 \cdot i,3,3) = (1+(1-\text{trust}) \cdot n_{bignumber}) * R_{block}(3 \cdot i,3,3)\\ R_{block}(12+3 \cdot i,3,3) = (1+(1-\text{trust}) \cdot n_{bignumber}) * R_{block}(12+3 \cdot i,3,3)\\ R_{block}(24+3 \cdot i,1,1) = (1+(1-\text{trust}) \cdot n_{bignumber}) * R_{block}(24+3 \cdot i,1,1)\\ \end{matrix}$

Q_{b l oc k} (6 + 3 \cdot i, 3, 3) = (1 + (1 - trust) \cdot n_{bi g n u mb er}) * Q_{b l oc k} (6 + i \cdot 3, 3, 3) R_{b l oc k} (3 \cdot i, 3, 3) = (1 + (1 - trust) \cdot n_{bi g n u mb er}) * R_{b l oc k} (3 \cdot i, 3, 3) R_{b l oc k} (12 + 3 \cdot i, 3, 3) = (1 + (1 - trust) \cdot n_{bi g n u mb er}) * R_{b l oc k} (12 + 3 \cdot i, 3, 3) R_{b l oc k} (24 + 3 \cdot i, 1, 1) = (1 + (1 - trust) \cdot n_{bi g n u mb er}) * R_{b l oc k} (24 + 3 \cdot i, 1, 1)

观测获得的速度及高度根据腿 $i$ 的状态而不同

\begin{matrix} ^{O} V_{k} = (1 - trust) *^{O} {\hat{V}}_{c o m, k - 1} + trust *^{O} V_{c} \\ ^{O} z_{k} = (1 - trust) * (^{O} {\hat{P}}_{c o m, z, k - 1} +^{B} P_{z, c}) \end{matrix}

$\begin{matrix} ^{O}V_k = (1- \text{trust}) * ^{O}\hat{V}_{com,k-1} + \text{trust}*^{O}V_c\\ ^{O}z_k = (1- \text{trust}) * (^{O}\hat{P}_{com,z,k-1}+^{B}P_{z,c}) \end{matrix}$

^{O} V_{k} = (1 - trust) *^{O} \hat{V}_{co m, k - 1} + trust *^{O} V_{c}^{O} z_{k} = (1 - trust) * (^{O} \hat{P}_{co m, z, k - 1} +^{B} P_{z, c})

\hat{v}_{k-1},\hat{P}_{z,k-1}

为上一周期的后验状态值，世界坐标系下质心状态。

$^{O}V_k$ 在摆动时采用质心预测速度，支撑时采用控制输出的足底速度。

$^{O}z_k$ 在摆动时根据质心预测高度与足端在机器人坐标系下的位置之和计算获得，在支撑时保持为0即触地。

其中trust通过支撑周期计算获得，腿处于摆动时phase=0，支撑时phase由0变化到1。
$\text{trust} = \left\{$

\begin{matrix} \frac{phase}{0.2} & phase \leq 0.2 \\ 1 & 0.2 < phase \leq 0.8 \\ \frac{phase}{0.8} & phase > 0.8 \end{matrix}

$\begin{matrix} \frac{\text{phase}}{0.2} & \text{phase} \le 0.2\\ 1 & 0.2 < \text{phase} \le 0.8 \\ \frac{\text{phase}}{0.8} & \text{phase} >0.8 \end{matrix}$ \right.

trust = ⎩ ⎨ ⎧ \frac{phase}{0.2} 1 \frac{phase}{0.8} phase \leq 0.2 0.2 < phase \leq 0.8 phase > 0.8

运动控制

请添加图片描述

├── convexMPC # MPC及步态规划
├── WBC		  # WBIC基类定义
└── WBC_Ctrl  # WBIC控制器
1
2
3

通过MPC跟踪质心位姿，生成躯干位置，速度，加速度，旋转和角速度，和四足触地力，通过步态规划生成足端位置，速度和加速度，WBIC基于MPC结果和WBC通过松弛优化获得控制量，关节前馈力矩，角度，角速度。MPC和WBIC可独立使用。

在BalanceStand模式下，机器人四足站立，四足触地力为重力除四，直接使用当前状态作为WBIC输入，实现控制。

在Locomotion模式下，可以直接使用MPC输出控制，也可使用MPC+WBIC，经测试MPC+WBIC更加稳定。

当前控制方案存在三个假设：

The roll and pitch angles are small.
States are close to the commanded trajectory: A time-varying linearization is performed.
Pitch and roll velocities and off-diagonal terms of the inertia tensor are small

convexMPC

├── convexMPC
    ├── convexMPC_interface
    ├── ConvexMPCLocomotion	# 构造mpc问题
    ├── convexMPC_util	
    ├── Gait 				# 生成步态
    ├── RobotState			# 机器人状态信息
    └── SolverMPC			# 求解MPC
1
2
3
4
5
6
7

纯convexMPC控制

摆动腿直接输出足端规划的p和v，支撑腿输出足端规划的p和v，mpc计算的足端反力。
convexMPC+WBIC

convexMPC输出足端轨迹规划的p,v,a和mpc计算足端反力，躯干期望位置，速度和加速度，角度和角速度，WBIC计算控制输出关节力矩，位置，速度。

足端规划

基于贝塞尔曲线(Bézier curve)生成足端轨迹。

├── include
│   ├── Controllers
│   │   ├── FootSwingTrajectory.h
└── src
    ├── Controllers
        ├── FootSwingTrajectory.cpp
1
2
3
4
5
6

/*!
 * Compute foot swing trajectory with a bezier curve
 * @param phase : How far along we are in the swing (0 to 1)
 * @param swingTime : How long the swing should take (seconds)
 */
template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier(T phase, T swingTime) {
  // xy插补
  _p = Interpolate::cubicBezier<Vec3<T>>(_p0, _pf, phase);
  _v = Interpolate::cubicBezierFirstDerivative<Vec3<T>>(_p0, _pf, phase) / swingTime;
  _a = Interpolate::cubicBezierSecondDerivative<Vec3<T>>(_p0, _pf, phase) / (swingTime * swingTime);

  T zp, zv, za;
  
  // 高度插补
  if(phase < T(0.5)) {
    zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 4 / (swingTime * swingTime);
  } else {
    zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 4 / (swingTime * swingTime);
  }

  _p[2] = zp;
  _v[2] = zv;
  _a[2] = za;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

MPC

算法解析

请添加图片描述

单刚体模型受力分析

MPC将机器人建模为一个单刚体，受四个足端力
请添加图片描述

简化的刚体动力学模型

方程解析

单刚体模型的平动，依据牛二，线性
单刚体模型的转动，角动量定理，非线性
单刚体模型的角速度在世界坐标系下的表示，非线性

参数解析

$\in R^3$ ：单刚体模型的位置坐标
$\in R^3$ ：每条腿受到的地面反作用力
$\in R^3$ ：单刚体模型的惯性张量
$\in R^3$ ：刚体质心指向足端力作用点的向量
$\in R^3$ ：机器人角速度
$\in R^{3 \times 3}$ ：从body坐标系到world坐标系的旋转变换矩阵
$[x]_x \in R^{3 \times 3}$ ：反对称矩阵

基本问题

状态量(13)：躯干角度，位置，角速度，速度，重力加速度
控制量(12)：四足足底反力

标准状态方程

请添加图片描述

采用前向欧拉法将状态方程离散化
$x(k+1)=A_k x(k)+B_k u(k)$
请添加图片描述

约束

足底反力触地时时需要限制允许的最大反力，摆动时反力需为0

{\begin{cases} state = 0 & ^{o} f_{i} = 0_{3 \times 1} \\ state = 1 & ^{o} f_{i}^{z} \leq f_{m a x} \end{cases}

$\begin{cases} \text{state} = 0 & {^o}f_i = 0_{3 \times 1} \\ \text{state} = 1 & {^o}f{_i^z} \le f_{max} \end{cases}$

{state = 0 state = 1^{o} f_{i} = 0_{3 \times 1}^{o} f_{i}^{z} \leq f_{ma x}

为保证足底与地面不发生相对滑动，足底反力的水平分量不能大于其竖直分量与滑动摩擦系数

u

的乘积，即满足摩擦锥条件：

请添加图片描述

$\left |{^o}f{_i^x} \right | \le \mu {^o}f{_i^z} \\ \left |{^o}f{_i^y} \right |\le \mu {^o}f{_i^z} \\ {^o}f{_i^z} > 0$

摩擦锥约束为非线性约束，可拆分为4个线性约束，整合足底反力约束可得完整的输入约束：

前四项约束正无穷约束即不对 ${^o}f{_i^z}$ 的最大值进行约束，在第五项才通过 $f_{max}$ 约束，前四项主要约束 ${^o}f{_i^z}$ 比 ${^o}f{_i^x}$ 和 ${^o}f{_i^y}$ 大

[\begin{matrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{matrix}]

$\begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix}$ \le

[\begin{matrix} - 1 & 0 & μ \\ 0 & - 1 & μ \\ 1 & 0 & μ \\ 0 & 1 & μ \\ 0 & 0 & 1 \end{matrix}]

$\begin{bmatrix} -1 & 0 & \mu \\ 0 & -1 & \mu\\ 1 & 0 & \mu\\ 0 & 1 & \mu\\ 0 &0&1 \end{bmatrix}$

[\begin{matrix} ^{o} f_{i}^{x} \\ ^{o} f_{i}^{y} \\ ^{o} f_{i}^{z} \end{matrix}]

$\begin{bmatrix} {^o}f{_i^x} \\ {^o}f{_i^y} \\ {^o}f{_i^z} \end{bmatrix}$ \le

[\begin{matrix} + \infty \\ + \infty \\ + \infty \\ + \infty \\ f_{m a x} \end{matrix}]

$\begin{bmatrix} +\infty\\ +\infty \\ +\infty \\ +\infty \\ f_{max} \end{bmatrix}$

00000 \leq - 1 0100 0 - 1 010 μ μ μ μ 1^{o} f_{i}^{x}^{o} f_{i}^{y}^{o} f_{i}^{z} \leq + \infty + \infty + \infty + \infty f_{ma x}

工程解析

ConvexMPCLocomotion(float _dt, int _iterations_between_mpc, MIT_UserParameters* parameters);
1

_dt 程序循环时间
_iterations_between_mpc 单次MPC运行中WBIC运行次数

mpc计算周期为dtMPC = dt * iterationsBetweenMPC

WBIC计算周期为dt

OffsetDurationGait(int nSegment, Vec4<int> offset, Vec4<int> durations, const std::string& name);
1

nSegment 步态段数
offset 相位差，不同腿的触地顺序
durations 负载因子，触地时长

步态时长为dtMPC*nSegment

WBIC

WBIC, WHOLE-BODY IMPULSE CONTROL(WBC+MPC)

├── WBC
│   ├── ContactSpec.hpp
│   ├── Task.hpp	# 任务基类
│   ├── WBC.hpp		# 基类
│   └── WBIC　
│       ├── KinWBC.cpp	# WBC计算
│       ├── KinWBC.hpp
│       ├── WBIC.cpp	# 继承WBC进行松弛优化
│       └── WBIC.hp
└── WBC_Ctrl
    ├── ContactSet
    ├── LocomotionCtrl 	# 继承WBC控制器，定义运动下的任务更新
    ├── TaskSet 		# 预定义任务
    ├── WBC_Ctrl.cpp 	# WBC控制器，调用KinWBC和WBIC
    └── WBC_Ctrl.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

WBC

算法解析

整体控制任务分为四个任务，按照优先级从高到低分别为

支撑腿轨迹跟随任务：SingleContact（_contact_list）
机身转动控制任务：BodyOriTask（_task_list）
机身平动控制任务：BodyPosTask（_task_list）
摆动腿足底轨迹跟随任务：LinkPosTask（_task_list）

算法控制伪代码

机器人的腿的关节角度(16)，角速度(17)和角加速度(18)分别采用WBC优化计算

请添加图片描述

输入：期望关节角度（误差值），速度，加速度，质心位置，速度，加速度，旋转角度，角速度
输出：关节角度，速度，加速度

请添加图片描述

$J_1$ 到 $J_4$ 为各任务的雅克比矩阵， $x$ 为足端位置， $q$ 为关节角度。

工程解析

WBC_Ctrl 			     UpdateModel 	   // 更新运动学/动力学模型
LocomotionCtrl<-WBC_Ctrl ContactTaskUpdate // 更新任务
KinWBC 					 FindConfiguration // WBC计算，计算关节角度和角速度
WBIC                     MakeTorque        // 松弛优化前计算关节角加速度
1
2
3
4

松弛优化

MPC获得足底反力 $^{o}f^{MPC}$ ，WBC获得广义加速度 $\ddot{q}^{cmd}$ 。

基于浮动基座机器人动力学
$\ddot{q}+C(q,\dot{q}) = S_j \tau +J^{To}_c f_c$

若只考虑6自由度的浮动基体，则只需考虑动力学方程的前六行，定义浮动基座选择矩阵
$S_f =$

[\begin{matrix} I_{6 \times 6} & 0 \\ 0 & 0 \end{matrix}]

$\begin{bmatrix} \text{I}_{6 \times6} &\text{0} \\ \text{0} & \text{0} \end{bmatrix}$

S_{f} = [I_{6 \times 6} 0 00]

动力学方程左右两边同时左乘

S_f

可得：

S_f M(q) \ddot{q}+S_f C(q,\dot{q}) = S_f J^{To}_c f_c

MPC和WBC所得结果应该满足该等式，但实际上并不相等，因此对两个结果添加松弛变量：

请添加图片描述

该松弛变量用于拟合两个结果之间的差异，因此应尽可能小，将两个松弛变量构造为优化方程，将动力学方程，松弛变量等式作为约束构造优化问题，然后转换为标准QP问题进行求解：
请添加图片描述

状态控制

状态 mode

ControlFSM中定义状态机状态

enum class FSM_OperatingMode { 
    NORMAL, 		// 正常运行
    TRANSITIONING,	// 切换中
    ESTOP,			// 停止
    EDAMP };
1
2
3
4
5

FSM_States中定义状态

// Normal robot states
#define K_PASSIVE 0　	　　// 无操作，输出力矩为0
#define K_STAND_UP 1		// 站起，工作空间下PD控制
#define K_BALANCE_STAND 3 	// 平衡站立，固定激励的WBIC
#define K_LOCOMOTION 4	    // 运动，MPC+WBIC
#define K_RECOVERY_STAND 6  // 恢复站立
#define K_VISION 8　　　　　　// 融合视觉规划控制
#define K_BACKFLIP 9        // 后空翻
#define K_FRONTJUMP 11      // 前跳

// Specific control states
#define K_JOINT_PD 51           // 关节空间PD控制
#define K_IMPEDANCE_CONTROL 52  // 工作空间PD控制

#define K_INVALID 100           // 空
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

状态切换

当前状态	可切换状态
PASSIVE	JOINT_PD, STAND_UP, RECOVERY_STAND
StandUp	BALANCE_STAND, LOCOMOTION, PASSIVE
BALANCE_STAND	LOCOMOTION, PASSIVE, RECOVERY_STAND, BACKFLIP
LOCOMOTION	BALANCE_STAND, PASSIVE, STAND_UP, RECOVERY_STAND

balanceStand

机器人处于站立状态，四足触底，可控状态为躯干高度，躯干绕x,y,z旋转
locomotion

机器人前后左右移动，绕z旋转

步态 gait

步态在locomotion下才有效，当前步态通过convexMPC/Gait定义的类OffsetDurationGait构造（common目录下的GaitScheduler构建的步态实测无效）

OffsetDurationGait::OffsetDurationGait(int nSegment, Vec4<int> offsets, Vec4<int> durations, const std::string &name)
1

ConvexMPCLocomotion中定义步态

int bounding = 1;    // 跳跑
int pronking = 2;    // 四足跳跃
int standing = 4;    // 站立
int trotRunning = 5; // 对角小跑
int pacing = 8;      // 同侧溜步
int trotting = 9;
1
2
3
4
5
6

参考

MIT Cheetah 完整开源代码与论文简介

腿足机器人控制方案阅读笔记

于宪元-基于稳定性的仿生四足机器人控制系统设计

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家自动化/article/detail/414738?site