简介
这篇论文是令我印象深刻的几篇关于MIT Cheetah的论文。除此之外还有MIT Cheetah 3提出使用凸优化的方法达到很好的动态控制效果,也算是我正式理解了四足动态控制的关键点。还有一篇就是Mini Cheetah的论文,它提出了低成本的四足机器人方案,在国内可以在1W5左右的成本做出一台四足机器人。
这篇论文第一章是Introduction,介绍了各种四足的控制方法,包括SLIP、WBC、MPC、Nonlinear TO、RL等。
理论与模型定义
第二章开始文章主体部分,从机器人理论建模开始。
机器人的质心(CoM)表示为:
p
=
[
p
x
p
y
p
z
]
\bold{p} = \left[ \begin{array}{l} p_x \\ p_y \\ p_z \end{array} \right]
p = ⎣ ⎢ ⎡ p x p y p z ⎦ ⎥ ⎤
机器人的质心速度表示为:
p
˙
=
[
p
x
˙
p
y
˙
p
z
˙
]
\dot{\bold{p}} = \left[ \begin{array}{l} \dot{p_x} \\ \dot{p_y} \\ \dot{p_z} \end{array} \right]
p ˙ = ⎣ ⎢ ⎡ p x ˙ p y ˙ p z ˙ ⎦ ⎥ ⎤
B
p
˙
=
[
B
p
x
˙
B
p
y
˙
B
p
z
˙
]
^{B}\dot{\bold{p}} = \left[ \begin{array}{l} ^{B}\dot{p_x} \\ ^{B}\dot{p_y} \\ ^{B}\dot{p_z} \end{array} \right]
B p ˙ = ⎣ ⎢ ⎡ B p x ˙ B p y ˙ B p z ˙ ⎦ ⎥ ⎤
p
˙
=
I
R
B
B
p
˙
\dot{\bold{p}} = ^{I}\bold{R}_{B} \\ ^{B}\dot{\bold{p}}
p ˙ = I R B B p ˙
对于机器人的身体旋转表示,文章中提到了三种方法,分别是欧拉角、四元数和旋转矩阵。本文中用到的主要与控制相关的方法是欧拉角。
欧拉角表示如下:
Θ
=
[
θ
ϕ
ψ
]
\bold{\Theta} = \left[ \begin{array}{l} \theta \\ \phi \\ \psi \end{array} \right]
Θ = ⎣ ⎢ ⎡ θ ϕ ψ ⎦ ⎥ ⎤
Θ
˙
=
[
θ
˙
ϕ
˙
ψ
˙
]
\dot{\bold{\Theta}} = \left[ \begin{array}{l} \dot{\theta} \\ \dot{\phi} \\ \dot{\psi} \end{array} \right]
Θ ˙ = ⎣ ⎢ ⎡ θ ˙ ϕ ˙ ψ ˙ ⎦ ⎥ ⎤
四元数表示如下:
q
˙
=
[
q
r
q
i
q
j
q
k
]
\dot{\bold{q}} = \left[ \begin{array}{l} q_r \\ q_i \\ q_j \\ q_k \end{array} \right]
q ˙ = ⎣ ⎢ ⎢ ⎢ ⎡ q r q i q j q k ⎦ ⎥ ⎥ ⎥ ⎤
旋转矩阵相关的计算如下:
I
R
˙
B
=
I
R
B
[
B
ω
]
×
^{I}\dot{\bold{R}}_{B} = ^{I}\bold{R}_{B} [^{B}\omega]_{\times}
I R ˙ B = I R B [ B ω ] ×
腿部的关节与足部末端的定义如下:
q
˙
=
[
q
i
,
A
b
/
A
d
q
i
,
h
i
p
q
i
,
k
n
e
e
]
p
=
[
p
i
,
x
p
i
,
y
p
i
,
z
]
\dot{\bold{q}} = \left[ \begin{array}{l} q_{i,Ab/Ad} \\ q_{i,hip} \\ q_{i,knee} \end{array} \right] \bold{p} = \left[ \begin{array}{l} p_{i,x} \\ p_{i,y} \\ p_{i,z} \end{array} \right]
q ˙ = ⎣ ⎢ ⎡ q i , A b / A d q i , h i p q i , k n e e ⎦ ⎥ ⎤ p = ⎣ ⎢ ⎡ p i , x p i , y p i , z ⎦ ⎥ ⎤
控制架构
控制结构如下图所示,通过手柄输入期望的控制,经过控制器解算后得到机器人的关节控制力矩。其中摆动腿与支撑腿用不同的控制方法,支撑腿用力控制,摆动腿用带补偿的PD控制。
状态估计
MIT这个机器人控制中,很重要的一个部分就是通过机器人的本体传感器,包括关节力矩传感器和IMU,不额外加视觉等传感器,机器人通过估计算法准确的得知机器人当前状态。我记得最早的四足相关的状态估计的论文是ETH出的,使用EKF进行估计,本文进行化简后,可以直接使用Kalman Filter进行状态估计。
状态转移函数如下
p
˙
=
v
\dot{\bold{{p}}} = \bold{v}
p ˙ = v
v
˙
=
I
R
˙
B
b
a
+
g
+
ω
v
\dot{\bold{v}} = ^{I}\dot{\bold{R}}_{B} \\ ^{b}a + \bold{g} + \bold{\omega}_v
v ˙ = I R ˙ B b a + g + ω v
p
˙
i
=
ω
˙
p
i
\dot{\bold{p}}_{i} = \dot{\bold{\omega}}_{p_i}
p ˙ i = ω ˙ p i
在后续文章里,我会补充关于Kalman Filer部分的公式。这里的基本思想就是,依靠支撑腿与IMU结合来估计身体的速度姿态等信息。
腿部控制
摆动腿控制
τ
F
F
,
i
=
J
i
T
Λ
i
(
B
a
i
,
r
e
f
−
J
˙
i
a
˙
i
)
+
C
i
q
˙
i
+
G
i
\bold{\tau}_{FF,i} = \bold{J}^{T}_{i} \Lambda_i (^{B}a_{i,ref} - \dot{\bold{J}}_i \dot{\bold{a}}_i) + \bold{C}_i \dot{\bold{q}}_i + \bold{G}_i
τ F F , i = J i T Λ i ( B a i , r e f − J ˙ i a ˙ i ) + C i q ˙ i + G i
τ
F
B
,
i
=
J
i
T
[
K
p
(
B
p
i
,
r
e
f
−
B
p
i
)
+
K
d
(
B
v
i
,
r
e
f
−
B
v
i
)
]
\bold{\tau}_{FB,i} = \bold{J}^{T}_{i}[ \bold{K}_p (^{B}\bold{p}_{i,ref} - ^{B}\bold{p}_i) + \bold{K}_d (^{B}\bold{v}_{i,ref} - ^{B}\bold{v}_i)]
τ F B , i = J i T [ K p ( B p i , r e f − B p i ) + K d ( B v i , r e f − B v i ) ]
τ
i
=
τ
F
F
,
i
+
τ
F
B
,
i
\bold{\tau}_i = \bold{\tau}_{FF,i} + \bold{\tau}_{FB,i}
τ i = τ F F , i + τ F B , i
支撑腿力控制
τ
i
=
J
i
T
f
i
\bold{\tau}_i = \bold{J}^{T}_{i} \bold{f}_i
τ i = J i T f i
步态规划
这篇论文使用步态相位这种方法来定义步态,通过这种方法可以直接用统一的参数定义比如Trot、Gallop、Walk等步态。论文使用下述参数来计算当前腿的相位或者是状态。这是一个很巧妙的办法,我个人觉得这种方法可以用在强化学习中,作为某种待优化的参数,来使机器人学习到更好的控制策略。
Φ
i
=
m
o
d
(
t
−
t
0
,
i
T
p
,
1
)
\Phi_i = mod \left( \tfrac{t-t_{0,i}}{T_p} , 1 \right)
Φ i = m o d ( T p t − t 0 , i , 1 )
足部触地检测
这部分可以参考MIT的论文,Contact Model Fusion for Event-based Locomotion in Unstructred Terrains。理论部分比较多,就不详细展开了。 我个人觉得可以用仿真加随机化获得数据,通过神经网络训练一个检测器出来,结合Sim-To-Real的理念,想办法直接用在实际机器人上。
Regularized Predictive Control
这一章是本文的重点,是控制算法的核心。Regularized Predictive Control在普通MPC算法的基础上,添加人为设计的运动约束,使得机器人具有更好的运动效果。论文中提到:
The philosophy behind RPC is to use our knowledge of robotic and legged systems to inform the optimization.
MIT还有一篇论文是在MPC的基础上添加Whole Body Control来优化机器人的关节输出,两个方法都有可取之处,不过在我看来,如果四足的运动控制板性能有限,可以考虑选择一个计算量需求小一点的算法,来进行整体的权衡。 (说的就是Up Board)
选择合适的Regular Heuristics Cost Function可以提高优化的结果并提升计算速度,但是若选择了不合适的函数,则可能只能求得局部最优解,甚至无法求解。为了避免手工设计的Heuristics Cost Function具有缺陷,后文作者使用一种基于学习的方法来得到更优的Heuristics Cost Function
MPC Control
这一节主要介绍MPC算法。
为了达到实时的MPC控制,我们需要尽量减少计算量,同时又要保证计算结果的有效性,因此在这里选择了简化模型的方法。
在该模型中,机器人状态定义如下:
x
=
[
p
Θ
p
˙
Θ
˙
]
\bold{x} = \left[ \begin{array}{l} \bold{p} \\ \bold{\Theta} \\ \dot{\bold{p}} \\ \dot{\bold{\Theta}} \end{array} \right]
x = ⎣ ⎢ ⎢ ⎢ ⎡ p Θ p ˙ Θ ˙ ⎦ ⎥ ⎥ ⎥ ⎤
其中,
p
\bold{p}
p 代表机器人位置,
Θ
\bold{\Theta}
Θ 代表机器人朝向的欧拉角表示。机器人的控制量为:
u
=
[
r
1
f
1
.
.
.
r
F
f
F
]
\bold{u} = \left[ \begin{array}{l} \bold{r}_1 \\ \bold{f}_1 \\ ... \\ \bold{r}_F \\ \bold{f}_F \end{array} \right]
u = ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎡ r 1 f 1 . . . r F f F ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎤
由此得到的机器人化简后离散系统的动力学:
x
k
+
1
=
A
(
Δ
t
k
)
x
k
+
B
(
Δ
t
k
)
h
(
x
k
,
u
k
,
Φ
k
)
+
d
(
Δ
t
k
)
\bold{x}_{k+1} = \bold{A}(\Delta t_k)\bold{x}_k + \bold{B}(\Delta t_k)h(\bold{x}_k, \bold{u}_k, \bold{\Phi}_k) + \bold{d}(\Delta t_k)
x k + 1 = A ( Δ t k ) x k + B ( Δ t k ) h ( x k , u k , Φ k ) + d ( Δ t k )
其中:
A
(
Δ
t
k
)
=
[
I
6
Δ
t
k
I
6
0
6
I
6
]
,
B
(
Δ
t
k
)
=
[
Δ
t
k
2
2
I
−
1
Δ
t
k
I
−
1
]
,
d
(
Δ
t
k
)
=
[
Δ
t
k
2
2
a
g
Δ
t
k
a
g
]
,
a
g
=
[
g
T
0
3
×
1
T
]
T
\bold{A}(\Delta t_k) = \left[ \begin{array}{l} \bold{I}_6 & \Delta t_k \bold{I}_6 \\ \bold{0}_6 & \bold{I}_6 \end{array} \right], \bold{B}(\Delta t_k) = \left[ \begin{array}{l} \tfrac{\Delta t_k^2}{2}\bold{I}^{-1} \\ \Delta t_k \bold{I}^{-1} \end{array} \right], \bold{d}(\Delta t_k) = \left[ \begin{array}{l} \tfrac{\Delta t_k^2}{2} \bold{a}_g \\ \Delta t_k \bold{a}_g \end{array} \right], \bold{a}_g = [\bold{g}^T \ \bold{0}_{3 \times 1}^T]^T
A ( Δ t k ) = [ I 6 0 6 Δ t k I 6 I 6 ] , B ( Δ t k ) = [ 2 Δ t k 2 I − 1 Δ t k I − 1 ] , d ( Δ t k ) = [ 2 Δ t k 2 a g Δ t k a g ] , a g = [ g T 0 3 × 1 T ] T
h
(
x
k
,
u
k
,
Φ
k
)
h(\bold{x}_k, \bold{u}_k, \bold{\Phi}_k)
h ( x k , u k , Φ k ) 函数是用于计算足部力与身体力矩的关系矩阵。
在前文提到过,机器人假设欧拉角中roll和pitch角很小,由此简化了动力学模型。MPC就是在前述的动力学方程上加上约束得到的。比如足部力需要满足摩擦角约束,比如足部力必须指向地面之类的,再通过非线性求解器得到输出控制量,进行机器人控制。
Regularized MPC
为了进一步优化MPC表现,作者提出了对常规MPC添加Cost Function的方法来改善结果。而Cost Function是通过学习方法得到的。我个人对这个部分不感兴趣,所以没有深入研究。