Cheetah-Software方案分析_contact model fusion

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上物联网嵌入式知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频，并且后续会持续更新

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

如果你需要这些资料，可以戳这里获取

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=[OPcom​​OVcom​​OP1​​OP2​​OP3​​OP4​​]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=

​ax​ay​az​​

​+g3×1​=

​ax​ay​az​​

​+

​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​=Ak​xk​+Bk​uk​+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​=

​I3​03×3​012×3​​I3​⋅△tI3​012×3​​03×12​03×12​I12​​

​​Bk​=

​03×3​I3​⋅△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=[BP1​​BP2​​BP3​​BP4​​OV1​​OV2​​OV3​​OV4​​Oz1​​Oz2​​Oz3​​Oz4​​]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∗OVc​Ozk​=(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​,Pz,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.2phase​10.8phase​​phase≤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)=Ak​x(k)+Bk​u(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{_iz} \le f_{max} \end{cases}

{state=0state=1​ofi​=03×1​ofiz​≤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{_ix} \right | \le \mu {^o}f{_iz} \ \left |{^o}f{_iy} \right |\le \mu {^o}f{_iz} \ {^o}f{_iz} > 0

∣ofix​∣≤μofiz​∣ofiy​∣≤μofiz​ofiz​>0

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

前四项约束正无穷约束即不对

o

f

i

z

{^o}f{_iz}

ofiz​的最大值进行约束，在第五项才通过

f

m

a

x

f_{max}

fmax​约束，前四项主要约束

o

f

i

z

{^o}f{_iz}

ofiz​比

o

f

i

x

{^o}f{_ix}

ofix​和

o

f

i

y

{^o}f{_iy}

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

收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。

如果你需要这些资料，可以戳这里获取

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人

都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

Bk​u(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{_iz} \le f_{max} \end{cases}

{state=0state=1​ofi​=03×1​ofiz​≤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{_ix} \right | \le \mu {^o}f{_iz} \ \left |{^o}f{_iy} \right |\le \mu {^o}f{_iz} \ {^o}f{_iz} > 0

∣ofix​∣≤μofiz​∣ofiy​∣≤μofiz​ofiz​>0

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

前四项约束正无穷约束即不对

o

f

i

z

{^o}f{_iz}

ofiz​的最大值进行约束，在第五项才通过

f

m

a

x

f_{max}

fmax​约束，前四项主要约束

o

f

i

z

{^o}f{_iz}

ofiz​比

o

f

i

x

{^o}f{_ix}

ofix​和

o

f

i

y

{^o}f{_iy}

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

收集整理了一份《2024年最新物联网嵌入式全套学习资料》，初衷也很简单，就是希望能够帮助到想自学提升的朋友。
[外链图片转存中…(img-qvg4hqi4-1715765929801)]
[外链图片转存中…(img-wJMO6bhS-1715765929802)]

如果你需要这些资料，可以戳这里获取

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人

都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！