简述
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 # 定义控制器
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
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 # 通讯接口
-
bridge
调用线程功能类型
- statusTask 监控任务状态
- spiTask SPI通讯
- microstrain IMU通讯
- Runner
- visualizationLCMTask 仿真环境数据
- sbusTask 遥控器通讯并获取控制指令
- microstrainLogger IMU数据
-
runner
The
RobotRunnerclass actually runs the control code, and connects it with either theHardwareBridgeorSimulationBridge.通过
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
定义机器人控制器，mini cheetah默认调用频率为0.5khz，cheetah3默认调用频率为1khz
状态估计
├── include
│ └── Controllers
│ ├── ContactEstimator.h // 触地估计
│ ├── OrientationEstimator.h // 姿态，角速度，加速度估计
│ ├── PositionVelocityEstimator.h // 位置，线速度估计
│ └── StateEstimatorContainer.h // 状态估计数据结构体，估计器通用接口，估计器容器
└── src
└── Controllers
├── ContactEstimator.cpp
├── OrientationEstimator.cpp
└── PositionVelocityEstimator.cpp
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
估计顺序为
ContactEstimator->OrientationEstimator->PositionVelocityEstimator
触地估计
MIT发表的《Contact Model Fusion for Event-Based Locomotion in Unstructured Terrains》实现了基于事件的触地估计，但相关代码未开源，开源控制器中使用固定周期实现。
位姿估计
并未找到状态估计实现的相关官方论文，此处参考
《基于稳定性的仿生四足机器人控制系统设计》
姿态估计
使用启动时的朝向作为 r z = 0 r_z = 0 rz=0朝向，其他姿态及角速度直接使用IMU数据。
位置估计
位置和速度估计采用kalman滤波融合imu数据和足底里程计，其中足底里程计假设支撑足不打滑，加速度直接采用加速计数据。
预测模型
状态变量为 x = [ O P c o m O V c o m O P 1 O P 2 O P 3 O P 4 ] T x = \begin{bmatrix} ^{O}P_{com} & ^{O}V_{com} & ^{O}P_1 & ^{O}P_2 & ^{O}P_3 & ^{O}P_4\end{bmatrix}^T x=[OPcomOVcomOP1OP2OP3OP4]T
- O P c o m ^{O}P_{com} OPcom 表示世界坐标系下机身质心位置，3*1矩阵
- O V c o m ^{O}V_{com} OVcom 表示世界坐标系下机身质心速度，3*1矩阵
- O P i ^{O}P_i OPi 表示世界坐标系下足端位置，3*1矩阵
输入变量为
u
=
[
a
x
a
y
a
z
]
+
g
3
×
1
=
[
a
x
a
y
a
z
]
+
[
0
0
−
9.81
]
u = \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}+g_{3 \times 1} = \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}+\begin{bmatrix}0\\0\\-9.81\end{bmatrix}
u=
axayaz
+g3×1=
axayaz
+
00−9.81
a为表示世界坐标系下机身质心加速度（通过加速度计获得），g为重力加速度
状态方程
此处位置 P P P计算忽略 a k ∗ d t 2 2 \frac{a_k*dt^2}{2} 2ak∗dt2， a k a_k ak为减去重力加速度后的加速度
P c o m , k + 1 = P c o m , k + V c o m , k ⋅ △ t V c o m , k + 1 = V c o m , k + a k ⋅ △ t P i , k + 1 = P i , k \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} Pcom,k+1=Pcom,k+Vcom,k⋅△tVcom,k+1=Vcom,k+ak⋅△tPi,k+1=Pi,k
x k + 1 = A k x k + B k u k + v k x_{k+1} = A_k x_k +B_k u_k+v_k xk+1=Akxk+Bkuk+vk
A k = [ I 3 I 3 ⋅ △ t 0 3 × 12 0 3 × 3 I 3 0 3 × 12 0 12 × 3 0 12 × 3 I 12 ] B k = [ 0 3 × 3 I 3 ⋅ △ t 0 12 × 3 ] \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} Ak= I303×3012×3I3⋅△tI3012×303×1203×12I12 Bk= 03×3I3⋅△t012×3
观测模型
观测变量
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
z=\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=[BP1BP2BP3BP4OV1OV2OV3OV4Oz1Oz2Oz3Oz4]T
-
B P i ^{B}P_i BPi 表示机器人坐标系下足端位置，3*1矩阵（触地时为负的机器人高度）
-
O V i ^{O}V_i OVi表示世界坐标系下腿的速度，3*1矩阵
-
O z i ^{O}z_i Ozi表示世界坐标系下足端位置z分量（触地时为0）
观测方程
Z
k
+
1
=
H
x
k
+
1
+
w
k
\begin{matrix} Z_{k+1} = H x_{k+1} +w_k \end{matrix}
Zk+1=Hxk+1+wk
预测更新
在进行状态估计时，当腿 i i i处于摆动相及刚触地与刚离地时，扩大腿部位置预测误差的协方差值，及腿部位置，速度及高度的观测误差协方差矩阵，扩大值通过trust控制：
程序中腿部位置对应的观测误差协方差矩阵并没有扩大
Q b l o c k ( 6 + 3 ⋅ i , 3 , 3 ) = ( 1 + ( 1 − trust ) ⋅ n b i g n u m b e r ) ∗ Q b l o c k ( 6 + i ⋅ 3 , 3 , 3 ) R b l o c k ( 3 ⋅ i , 3 , 3 ) = ( 1 + ( 1 − trust ) ⋅ n b i g n u m b e r ) ∗ R b l o c k ( 3 ⋅ i , 3 , 3 ) R b l o c k ( 12 + 3 ⋅ i , 3 , 3 ) = ( 1 + ( 1 − trust ) ⋅ n b i g n u m b e r ) ∗ R b l o c k ( 12 + 3 ⋅ i , 3 , 3 ) R b l o c k ( 24 + 3 ⋅ i , 1 , 1 ) = ( 1 + ( 1 − trust ) ⋅ n b i g n u m b e r ) ∗ R b l o c k ( 24 + 3 ⋅ i , 1 , 1 ) \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} Qblock(6+3⋅i,3,3)=(1+(1−trust)⋅nbignumber)∗Qblock(6+i⋅3,3,3)Rblock(3⋅i,3,3)=(1+(1−trust)⋅nbignumber)∗Rblock(3⋅i,3,3)Rblock(12+3⋅i,3,3)=(1+(1−trust)⋅nbignumber)∗Rblock(12+3⋅i,3,3)Rblock(24+3⋅i,1,1)=(1+(1−trust)⋅nbignumber)∗Rblock(24+3⋅i,1,1)
观测获得的速度及高度根据腿
i
i
i的状态而不同
O
V
k
=
(
1
−
trust
)
∗
O
V
^
c
o
m
,
k
−
1
+
trust
∗
O
V
c
O
z
k
=
(
1
−
trust
)
∗
(
O
P
^
c
o
m
,
z
,
k
−
1
+
B
P
z
,
c
)
\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}
OVk=(1−trust)∗OV^com,k−1+trust∗OVcOzk=(1−trust)∗(OP^com,z,k−1+BPz,c)
v
^
k
−
1
,
P
^
z
,
k
−
1
\hat{v}_{k-1},\hat{P}_{z,k-1}
v^k−1,P^z,k−1为上一周期的后验状态值，世界坐标系下质心状态。
O V k ^{O}V_k OVk在摆动时采用质心预测速度，支撑时采用控制输出的足底速度。
O z k ^{O}z_k Ozk在摆动时根据质心预测高度与足端在机器人坐标系下的位置之和计算获得，在支撑时保持为0即触地。
其中trust通过支撑周期计算获得，腿处于摆动时phase=0，支撑时phase由0变化到1。
trust
=
{
phase
0.2
phase
≤
0.2
1
0.2
<
phase
≤
0.8
phase
0.8
phase
>
0.8
\text{trust} = \left\{\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=⎩
⎨
⎧0.2phase10.8phasephase≤0.20.2<phase≤0.8phase>0.8
运动控制
├── convexMPC # MPC及步态规划
├── WBC # WBIC基类定义
└── WBC_Ctrl # WBIC控制器
通过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
-
纯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
/*!
* 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;
}
MPC
算法解析
MPC将机器人建模为一个单刚体，受四个足端力
方程解析
- 单刚体模型的平动，依据牛二，线性
- 单刚体模型的转动，角动量定理，非线性
- 单刚体模型的角速度在世界坐标系下的表示，非线性
参数解析
- P ∈ R 3 P \in R^3 P∈R3：单刚体模型的位置坐标
- f ∈ R 3 f \in R^3 f∈R3：每条腿受到的地面反作用力
- I ∈ R 3 I \in R^3 I∈R3：单刚体模型的惯性张量
- r ∈ R 3 r \in R^3 r∈R3：刚体质心指向足端力作用点的向量
- w ∈ R 3 w \in R^3 w∈R3：机器人角速度
- R ∈ R 3 × 3 R \in R^{3 \times 3} R∈R3×3：从body坐标系到world坐标系的旋转变换矩阵
- [ x ] x ∈ R 3 × 3 [x]_x \in R^{3 \times 3} [x]x∈R3×3：反对称矩阵
基本问题
- 状态量(13)：躯干角度，位置，角速度，速度，重力加速度
- 控制量(12)：四足足底反力
标准状态方程
采用前向欧拉法将状态方程离散化
x
(
k
+
1
)
=
A
k
x
(
k
)
+
B
k
u
(
k
)
x(k+1)=A_k x(k)+B_k u(k)
x(k+1)=Akx(k)+Bku(k)
约束
足底反力触地时时需要限制允许的最大反力，摆动时反力需为0
{
state
=
0
o
f
i
=
0
3
×
1
state
=
1
o
f
i
z
≤
f
m
a
x
\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=0state=1ofi=03×1ofiz≤fmax
为保证足底与地面不发生相对滑动，足底反力的水平分量不能大于其竖直分量与滑动摩擦系数
u
u
u的乘积，即满足摩擦锥条件：
∣ o f i x ∣ ≤ μ o f i z ∣ o f i y ∣ ≤ μ o f i z o f i z > 0 \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 ∣ofix∣≤μofiz∣ofiy∣≤μofizofiz>0
摩擦锥约束为非线性约束，可拆分为4个线性约束，整合足底反力约束可得完整的输入约束：
前四项约束正无穷约束即不对 o f i z {^o}f{_i^z} ofiz的最大值进行约束，在第五项才通过 f m a x f_{max} fmax约束，前四项主要约束 o f i z {^o}f{_i^z} ofiz比 o f i x {^o}f{_i^x} ofix和 o f i y {^o}f{_i^y} ofiy大
[ 0 0 0 0 0 ] ≤ [ − 1 0 μ 0 − 1 μ 1 0 μ 0 1 μ 0 0 1 ] [ o f i x o f i y o f i z ] ≤ [ + ∞ + ∞ + ∞ + ∞ f m a x ] \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} \le \begin{bmatrix} -1 & 0 & \mu \\ 0 & -1 & \mu\\ 1 & 0 & \mu\\ 0 & 1 & \mu\\ 0 &0&1 \end{bmatrix} \begin{bmatrix} {^o}f{_i^x} \\ {^o}f{_i^y} \\ {^o}f{_i^z} \end{bmatrix} \le \begin{bmatrix} +\infty\\ +\infty \\ +\infty \\ +\infty \\ f_{max} \end{bmatrix} 00000 ≤ −101000−1010μμμμ1 ofixofiyofiz ≤ +∞+∞+∞+∞fmax
工程解析
ConvexMPCLocomotion(float _dt, int _iterations_between_mpc, MIT_UserParameters* parameters);
- _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);
- 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
WBC
算法解析
整体控制任务分为四个任务，按照优先级从高到低分别为
- 支撑腿轨迹跟随任务：SingleContact（_contact_list）
- 机身转动控制任务：BodyOriTask（_task_list）
- 机身平动控制任务：BodyPosTask（_task_list）
- 摆动腿足底轨迹跟随任务：LinkPosTask（_task_list）
算法控制伪代码
机器人的腿的关节角度(16)，角速度(17)和角加速度(18)分别采用WBC优化计算
- 输入：期望关节角度（误差值），速度，加速度，质心位置，速度，加速度，旋转角度，角速度
- 输出：关节角度，速度，加速度
J 1 J_1 J1到 J 4 J_4 J4为各任务的雅克比矩阵， x x x为足端位置， q q q为关节角度。
工程解析
WBC_Ctrl UpdateModel // 更新运动学/动力学模型
LocomotionCtrl<-WBC_Ctrl ContactTaskUpdate // 更新任务
KinWBC FindConfiguration // WBC计算，计算关节角度和角速度
WBIC MakeTorque // 松弛优化前计算关节角加速度
松弛优化
MPC获得足底反力 o f M P C ^{o}f^{MPC} ofMPC，WBC获得广义加速度 q ¨ c m d \ddot{q}^{cmd} q¨cmd。
基于浮动基座机器人动力学
M
(
q
)
q
¨
+
C
(
q
,
q
˙
)
=
S
j
τ
+
J
c
T
o
f
c
M(q) \ddot{q}+C(q,\dot{q}) = S_j \tau +J^{To}_c f_c
M(q)q¨+C(q,q˙)=Sjτ+JcTofc
若只考虑6自由度的浮动基体，则只需考虑动力学方程的前六行，定义浮动基座选择矩阵
S
f
=
[
I
6
×
6
0
0
0
]
S_f = \begin{bmatrix} \text{I}_{6 \times6} &\text{0} \\ \text{0} & \text{0} \end{bmatrix}
Sf=[I6×6000]
动力学方程左右两边同时左乘
S
f
S_f
Sf可得：
S
f
M
(
q
)
q
¨
+
S
f
C
(
q
,
q
˙
)
=
S
f
J
c
T
o
f
c
S_f M(q) \ddot{q}+S_f C(q,\dot{q}) = S_f J^{To}_c f_c
SfM(q)q¨+SfC(q,q˙)=SfJcTofc
MPC和WBC所得结果应该满足该等式，但实际上并不相等，因此对两个结果添加松弛变量：
该松弛变量用于拟合两个结果之间的差异，因此应尽可能小，将两个松弛变量构造为优化方程，将动力学方程，松弛变量等式作为约束构造优化问题，然后转换为标准QP问题进行求解：
状态控制
状态 mode
ControlFSM中定义状态机状态
enum class FSM_OperatingMode {
NORMAL, // 正常运行
TRANSITIONING, // 切换中
ESTOP, // 停止
EDAMP };
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 // 空
状态切换
| 当前状态 | 可切换状态 |
|---|---|
| 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)
ConvexMPCLocomotion中定义步态
int bounding = 1; // 跳跑
int pronking = 2; // 四足跳跃
int standing = 4; // 站立
int trotRunning = 5; // 对角小跑
int pacing = 8; // 同侧溜步
int trotting = 9;
参考
于宪元-基于稳定性的仿生四足机器人控制系统设计
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
