MIT Cheetah Learning (一)：State Estimate

MIT Cheetah Learning (一)：State Estimate

MIT Cheetah 3中融合了许多论文中的算法和技术。
论文链接：MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot
MIT Cheetah Learning系列将解析Cheetah的算法部分，尝试将其中每一个模块解释清楚。

Cheetah 3 中的State Estimate是参考2013年ETH的论文——
“State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU”
论文链接：State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU

因此，只需搞明白ETH的论文即可。
下面将具体介绍论文是如何用IMU与编码器实现State Estimate

Summary

以Extended Kalman Filter（EKF）算法为核心，仅使用IMU与电机的编码器，可以准确估计除了yaw和absolute position以外的机器人状态（yaw和absolute position在短距离运动中误差也仅在10%），包括roll、pitch、velocity。
在任意步态、任意地形下，都可以实现对机器人的状态估计。前提是机器人至少有一条腿与地面接触，并且假设机器人与地面接触时，仅会发生非常小的的滑动。

先验知识

A.Extended Kalman Filter

Definition:
${\theta }_k$为真实值，${\theta }_k^{\prime }$为模型预测值，$⟨{\theta }_k⟩$为估计值（系统输出），$z_k$为系统观测值，$s_k$为状态转移噪声，服从高斯分布，$v_k$为观测噪声，也服从高斯分布

EKF的状态转移方程和观测方程如下：
$$\{\begin{array}{ll}{\theta }_k=f({\theta }_{k-1})+s_k& (1)\\ z_k=h({\theta }_k)+v_k& (2)\end{array}$$
对(1)(2)式进行泰勒展开（雅克比矩阵性质：$f(x)=f(x_0)+J\cdot (x-x_0)$），得到
$$\{\begin{array}{ll}{\theta }_k=f({\theta }_{k-1})+s_k=f(⟨{\theta }_{k-1}⟩)+F_{k-1}({\theta }_{k-1}-⟨{\theta }_{k-1}⟩)+s_k& (3)\\ z_k=h({\theta }_k)+v_k=h({\theta }_k^{\prime })+H(\theta -{\theta }_k^{\prime })+v_k& (4)\end{array}$$
引入反馈
$$⟨{\theta }_k⟩={\theta }_k^{\prime }+K_k(z_k-h({\theta }_k^{\prime }))$$
由以上式子可以得到EKF的Predict和Update部分
Predict:
$$\{\begin{array}{l}{\theta }_k^{\prime }=f(⟨{\theta }_{k-1}⟩)\\ {\Sigma }_k^{\prime }=F_{k-1}{\Sigma }_{k-1}^{\prime }{F}_{k-1}^T+Q\end{array}$$
Update:
$$\{\begin{array}{l}{S}_k=(H_k{\Sigma }_k^{\prime }{H}_k^T+R{)}^{-1}\\ K_k={\Sigma }_k^{\prime }{H}_k^T{S}_k\\ ⟨{\theta }_k⟩={\theta }_k^{\prime }+K_k(z_k-h({\theta }_k^{\prime }))\\ {\Sigma }_k=(I-K_k{H}_k){\Sigma }_k^{\prime }\end{array}$$

其中，雅克比矩阵$F_k=\frac{\partial f}{\partial \theta }{|}_{⟨{\theta }_{k-1}⟩}$

雅克比矩阵$H_k=\frac{\partial h}{\partial \theta }{|}_{{\theta }_k^{\prime }}$

协方差矩阵${\Sigma }_k=⟨({\theta }_k-⟨{\theta }_k⟩)({\theta }_k-⟨{\theta }_k⟩{)}^T⟩$，表示估计值与真实值之间的误差

协方差矩阵${\Sigma }_k^{\prime }=⟨({\theta }_k-{\theta }_k^{\prime })({\theta }_k-{\theta }_k^{\prime }{)}^T⟩$，表示预测值与真实值之间的误差

状态转移噪声协方差矩阵$Q=⟨s_k{s}_k^T⟩$

观测噪声协方差矩阵$R=⟨v_k{v}_k^T⟩$

$K_k$为卡尔曼增益

具体推导可参考扩展卡尔曼滤波

B.四元数、旋转矩阵、李群李代数等

参考书籍：《视觉SLAM十四讲(第二版)》—— 高翔著

以下定义会在后续会使用到：

• $(\cdot {)}^{\times }$表示将向量转化成反对称矩阵
• $\Omega (\cdot )$将任意的角速度转化为4$\times 4$矩阵，表示相应的四元速率
  $$\Omega :\omega \mapsto \Omega (\omega )=\left[\begin{array}{cccc}0& {\omega }_z& -{\omega }_y& {\omega }_x\\ -{\omega }_z& 0& {\omega }_x& {\omega }_y\\ {\omega }_y& -{\omega }_x& 0& {\omega }_z\\ -{\omega }_x& -{\omega }_y& -{\omega }_z& 0\end{array}\right]$$
• $\zeta (\cdot )$ 将旋转向量的误差转化为四元数误差
  $$\zeta :v\mapsto \zeta (v)=\left[\begin{array}{c}sin(\frac{1}{2}||v||)\frac{v}{||v||}\\ cos(\frac{1}{2}||v||)\end{array}\right]$$

Part 1——Sensor Device and Measurement Models

A. Encoders

增量式编码器提供了所有电机的转动真实角度$\alpha$，该反馈信息受到$n_{\alpha }$高斯噪声的影响
$$\tilde{\alpha }=\alpha +n_{\alpha }$$
因此由正运动学可知所有足端在$BodyFrame$($B$)下的坐标$s_i$
$$s_i=lki{n}_i(\alpha )+n_{s,i}$$
$lki{n}_i(\cdot )$表示腿部正运动学模型，$n_{s,i}$表示离散高斯噪声（校准误差与运动学模型误差）
定义——$R_{\alpha }$为$n_{\alpha }$的协方差矩阵，$R_s$为$n_{i,s}$的协方差矩阵

B. IMU

由Part 1中IMU模型可知

加速度计得到在$B$下的加速度$f$，陀螺仪得到在$B$下的角速度$\omega$

旋转矩阵$C$表示从世界坐标系$I$到机器人坐标系$B$变换

$a$表示$I$下的加速度

因此可得
$$f=C(a-g)$$ $$\tilde{f}=f+b_f+w_f$$ $${\dot{b}}_f={\omega }_{bf}$$ $$\tilde{\omega }=\omega +b_{\omega }+w_{\omega }$$ $${\dot{b}}_{\omega }={\omega }_{b\omega }$$
其中$\tilde{f},\tilde{\omega }$是受高斯噪声项${\omega }_f,w_{\omega }$和偏置项$b_f,b_{\omega }$影响得到的测量值，且偏置项的导数可以由高斯噪声${\omega }_{bf},{\omega }_{b\omega }$表示

定义以上四个高斯噪声的协方差矩阵为$Q_f,Q_{bf},Q_{\omega },Q_{b\omega }$

Part 2——State Estimate

A.Filter State Definition

Definition:
$r$为在世界坐标系$I$下，机器人身体中心的位置

$v$为机器人在世界坐标系$I$下的速度

$q$为世界坐标系$I$到机器人坐标系$B$的旋转四元数

$p_1,p_2,...,p_N$为机器人足端在世界坐标系$I$下的位置

$b_f,b_{\omega }$为在机器人坐标系$B$下，IMU的偏置项（可由旋转矩阵转化到世界坐标系$I$下）

因此我们定义
$$x:=(r,v,q,p_1,...,p_N,b_f,b_{\omega })$$ $$P:=cov(\Delta x)$$ $$\Delta x:=(\Delta r,\Delta v,\Delta q,\Delta p_1,...,\Delta p_N,\Delta b_f,\Delta b_{\omega })$$

B.Prediction Model(IMU)

由Part 1中IMU模型可知
$$\begin{array}{rl}& \dot{r}=v\\ & \dot{v}=a=C^T(\tilde{f}-b_f-{\omega }_f)+g\\ & \dot{q}=\frac{1}{2}\Omega (\omega )q=\frac{1}{2}\Omega (\tilde{\omega }-b_{\omega }-{\omega }_{\omega })q\\ & \dot{p_i}=C^T{\omega }_{p,i},\forall i\in 1,...,N\\ & \dot{b_f}={\omega }_{bf}\\ & \dot{b_{\omega }}={\omega }_{b\omega }\end{array}$$
白噪声${\omega }_{p,i}$表示足端与地面接触可能产生的微小滑动

$Q_{p,i}$为${\omega }_{p,i}$的协方差矩阵，定义如下:
$$Q_{p,i}:=\left[\begin{array}{ccc}{\omega }_{p,i,x}& 0& 0\\ 0& {\omega }_{p,i,y}& 0\\ 0& 0& {\omega }_{p,i,z}\end{array}\right]$$

C.Measurement Model(Encoders)

由Part 1中Encoders模型可知
$$\begin{gathered} \widetilde{s_i}:=lki{n}_i(\tilde{\alpha }) \\ \approx lki{n}_i(\alpha )+J_{lkin,i}{n}_{\alpha } \\ \approx s_i\underset{n_i}{\underbrace{-n_{s,i}+J_{lkin,i}{n}_{\alpha }}} \end{gathered}$$
其中，雅克比矩阵$J_{lkin,i}:=\frac{\partial lki{n}_i(\alpha )}{\partial {\alpha }_i}$
$n_i$可视为经过线性离散化的噪声，包含编码器噪声和正运动学计算噪声，即观测误差
$R_i$是每一条腿的观测误差协方差矩阵，同时由Part 1.A可知
$$R_i=R_s+J_{lkin,i}{R}_{\alpha }{J}_{lkin,i}^T$$

由定义可知，$\widetilde{s_i}$可以表示$\widetilde{s_i}=C(p_i-r)+n_i$，即足端坐标与机器人中心坐标（世界坐标系$I$）的差值乘上旋转矩阵

D.Extended Kalman Filter

将编码器视为观测值，$\Delta x$视为真实值，即可建立EKF状态转移方程与观测方程，如下
$$\{\begin{array}{l}\Delta x_k=f(\Delta x_{k-1})+q_k\\ s_k=h(\Delta x_k)+v_k\end{array}$$
Predict:
经过模型线性化处理之后（参考文章Computing integrals involving the matrix exponential），得到关于$\Delta x$的协方差矩阵$F_k$，以及离散过程噪声的协方差矩阵$Q_k$（$Q_k$包含了$Q_f,Q_{bf},Q_{\omega },Q_{b\omega }$）
两个协方差矩阵$F_k,Q_k$的具体表达式在论文中由展示，在这里便不展示了（会涉及反对称阵，四元数，e指数，罗德里格斯公式等）
因此可以得到
$$\{\begin{array}{l}{\Delta }^{\prime }x=f(⟨\Delta x_{k-1}⟩)\\ P_{k+1}^{-}=F_k{P}_k^{+}{F}_k^T+Q_k\end{array}$$

Update:
引入$y_k$，定义如下：
$$y_k:=\left(\begin{array}{l}({\tilde{s}}_{1,k}-{\hat{C}}_k^{-}({\hat{p}}_{1,k}^{-}-{\hat{r}}_k^{-})\\ ⋮\\ ({\tilde{s}}_{N,k}-{\hat{C}}_k^{-}({\hat{p}}_{N,k}^{-}-{\hat{r}}_k^{-})\end{array}\right)$$
关于式子${\tilde{s}}_{i,k}-{\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-})$，使用泰勒展开，忽略所有的高阶项，得到
$${\tilde{s}}_{i,k}-{\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-})\approx -{\hat{C}}_k^{-}\Delta r_k^{-}+{\hat{C}}_k^{-}\Delta p_{i,k}^{-}+({\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-}){)}^{\times }\Delta {\varphi }_k^{-}$$
证明如下：

定义$$\frac{\partial g(x_k)}{\partial x_k}:={\tilde{s}}_{i,k}-{\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-})$$
$$\because \frac{\partial g(x_k)}{\partial x_k}=-{\hat{C}}_k^{-}\Delta r_k^{-}+{\hat{C}}_k^{-}\Delta p_{i,k}^{-}+({\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-}){)}^{\times }\Delta {\varphi }_k^{-}+G(\Delta v_k^{-})$$
$$=-{\hat{C}}_k^{-}\Delta r_k^{-}+{\hat{C}}_k^{-}\Delta p_{i,k}^{-}+({\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-}){)}^{\times }\Delta {\varphi }_k^{-}$$
$G(\Delta v_k^{-})$可视为$-{\hat{C}}_k^{-}\Delta r_k^{-}$的高阶项，因此可以忽略
$$\therefore {\tilde{s}}_{i,k}-{\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-})\approx -{\hat{C}}_k^{-}\Delta r_k^{-}+{\hat{C}}_k^{-}\Delta p_{i,k}^{-}+({\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-}){)}^{\times }\Delta {\varphi }_k^{-}$$
因此我们可以得到雅克比矩阵$H_k=\frac{\partial y_k}{\partial x_k}$
$$H_k=\left[\begin{array}{cccccccc}-{\hat{C}}_k^{-}& 0& ({\hat{C}}_k^{-}({\hat{p}}_{1,k}^{-}-{\hat{r}}_k^{-}){)}^{\times }& {\hat{C}}_k^{-}& \cdots & 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& \ddots & ⋮& ⋮& ⋮\\ -{\hat{C}}_k^{-}& 0& ({\hat{C}}_k^{-}({\hat{p}}_{N,k}^{-}-{\hat{r}}_k^{-}){)}^{\times }& {\hat{C}}_k^{-}& \cdots & 0& 0& 0\end{array}\right]$$
由Part 2.C可知，$R_k$是观测误差的协方差矩阵
$$R_k=\left[\begin{array}{ccc}{R}_{1,k}& & \\ & \ddots & \\ & & R_{N,k}\end{array}\right]$$
因此我们可以得到EFK的Update等式，如下：
$$\{\begin{array}{ll}{S}_k=(H_k{P}_k^{-}{H}_k^T+R_k)& \\ K_k=P_k^{-}{H}_k^T{S}_k^{-1}& \\ ⟨\Delta x_k⟩=K_k{y}_k& (\ast )\\ P_k^{+}=(I-K_k{H}_k)P_k^{-}& \end{array}$$
$(\ast )$式证明如下：
$$⟨\Delta x_{i,k}⟩=\Delta x_{i,k}^{\prime }+K_k({\tilde{s}}_{i,k}-{\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-}))$$
由于$\Delta x_{i,k}^{\prime }$与${\hat{C}}_k^{-}({\hat{p}}_{i,k}^{-}-{\hat{r}}_k^{-})$式等价的
$$\therefore ⟨\Delta x_{i,k}⟩=K_k{y}_k$$

Part 3——Observability Analysis

A.Nonlinear Model

参考文章Nonlinear controllability and observability

该部分从数学的角度证明了对于该机器人系统来说，有以下几个结论：

• absolute position和yaw是无法被观测的，但是其他的所有状态都可以被准确估计。
• 对于任意的非线性地形，只要有至少三条腿与地面接触，就可以准确估计机器人状态。
• 当机器人与地面没有任何接触时，状态无法估计，收敛效果极差

具体证明过程可参考论文本身，数学部分较为复杂，便不一一阐述

B.Extended Kalman Filter

参考文章Analysis and improvement of the consistency of extended Kalman filter based SLAM

参考文章A counter example to the theory of simultaneous localization and map building

该部分从数学的角度证明了对于该机器人系统的EKF模型来说，有以下结论：

• 对于无法观测的非线性子空间系统，将其线性离散化之后，依然是无法观测的

Part 4——Conclusion

A.实验结论

• 对于任意地形和任意步态来说，absolute position和yaw是无法观测的，但是在移动短距离的实验中，误差还是能够控制在10%左右，并非无法衡量。
• roll、pitch和velocity可以准确观测，可以很好的作为机器人的反馈信息。

（这个实验结果太牛了，必须附上）

B.后续

对于Part 3的证明，还有一些问题没有搞清楚，特别是非线性机器人模型的可观测性这个部分。

后面在搭建好四足机器人平台之后，尝试复现State Estimate。