+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
收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。
一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人
都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！
