四足机器人状态估计方案学习：Consistent Fusion of Leg Kinematics and IMU

1.前言

最近在学习QUAD-SDK的状态估计算法，就先把目前相关资料比较多的经典方案给看了，然后把一些收获整理出来。

目前网络上关于四足机器人状态估计方案的介绍相比运动控制来说较少，且部分可读性不高，一是不够直观，二是没有相关的代码对应。因此本文会结合理论和代码一起介绍，代码部分直接用了 @YY硕 的开源四足控制器中状态估计部分（硕哥写的简单易懂），理论部分参考了ETH的论文（这篇论文我刚入门的时候学长推给我的，非常经典，但是没点基础整不明白）、于宪元大佬的硕士毕业论文（yxyDLyyds!）和知乎用户 @浑水摸鱼 之前的一篇文章(写得好，一些看代码看不懂的tricks看这篇就明白了)。

2.正文

第0步.卡尔曼滤波介绍

Calman Filter用一句话总结就是：“将两个概率分布融合，得到一个状态空间中协方差最小的新概率分布”。相关的资料很多，本文简单介绍卡尔曼滤波的流程。建议大家去看一下b站Dr.can的视频，跟着手推一下。

1.建立系统模型 ：

状态方程： x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}

观测方程： z_{k}=Hx_{k}+v_{k}

x_{k} 是对应k时刻的状态变量， u_{k-1} 是对应k-1时刻的输入，A是状态转移矩阵，B是控制矩阵， w_{k-1} 是对应k-1时刻的过程噪声，满足一个N(0,Q)的正态分布，其中Q是正态分布的协方差矩阵。

z_{k} 是对应k时刻的测量值，H是测量矩阵， v_{k} 是对应k时刻的测量噪声，满足一个N(0,R)的正态分布，其中R是对应的协方差矩阵。

2.使用状态方程“预测”下一时刻的状态 ：

先验估计 \bar{x}_{k} ^{-} = A\bar{x}_{k-1}+Bu_{k-1}

(因为误差概率的分布均值为0，所以在式中省略了。但是我们引入新的符号P来表示新的状态估计的协方差)

先验误差协方差 P_{k}^{-}=A_{k}P_{k-1}A_{k}^{T}+Q

3.使用观测方程对预测的状态进行“滤波”:

卡尔曼增益 K_{k}=\frac{ P_{k}^{-}H^{T} }{HP_{k}^{-}H^{T}+R}

后验估计 \bar{x}_{k} = \bar{x}_{k}^{-}+K_{k}(z_{k}-H \bar{x}_{k}^{-}) ，使用测量误差 z_{k}-H \bar{x}_{k}^{-} 乘以卡尔曼增益对先验估计进行修正，得到一个协方差最小的新状态分布

更新误差协方差矩阵 P_{k}=(I-K_{k}H)P_{k}^{-}

这就是大家常听到的卡尔曼滤波“预测”和“滤波”的作用。接下来开始带入四足机器人状态估计的任务场景

第一步.建立四足机器人的状态估计模型 （这段较长且理论较多，建议先搞懂流程再抠细节）

问题描述 ：状态估计的目的是解决机器人在哪儿的问题，相关的状态变量有: 位置，速度和角度，角速度 共12个。使用的传感器有 12个电机编码器 和 一个IMU 。

解：

建立坐标系 如下：

首先 ，通过IMU可以直接获得IMU坐标系下的平动加速度，转动角度和转动角速度： ^{imu}a_{original},^{imu}\alpha_{original},^{imu}w_{original}

Body坐标系下的角速度和IMU坐标系下的角速度的关系为： ^{B}w_{OB}=^{B}R_{imu}w_{original} 。因为imu和body是固连的，所以 ^{B}R_{imu} 已知。由此得到了 ^{B}w_{OB} 。

body在world坐标系下的旋转矩阵可以表示为： ^{O}R_{B}={^{O}R_{imu0}}{^{imu0}R_{imu}}{^{imu}R_{B}} 。因为imu0坐标系和world坐标系是固连的，imu和body是固连的，所以 ^{O}R_{imu0} 和 ^{imu}R_{B} 已知。

而 ^{imu0}R_{imu} 可以通过IMU的测量值 ^{imu}\alpha_{original} 直接得到，由此可以求出 world坐标系下body的旋转矩阵 ^{O}R_{B} 和 world坐标系下body的角速度 ^{O}w_{OB} 。因为 ^{O}w_{OB\times}={^{B}R_{O}} {^{B}w_{OB\times}} {^{O}R_{B}} （公式参照yxy论文公式2.31）， \times 为反对称运算符。

至此，我们光通过IMU数据就已经解决了状态估计问题中的一半了，接下来需要使用卡尔曼滤波计算 world坐标系下body的位置和速度 。

然后 ，我们需要引入一个 足底里程计 ，因为如果光靠imu的数据进行积分运算会造成巨大的累计误差，通过引入编码器的绝对位置信息可以很大程度上较少积分运算带来的误差。为了方便处理，我们假设支撑足在地面上不打滑（即支撑相时foot在world坐标系下的速度为0）。

接下来 ， 建立系统的状态方程 。

定义 状态变量：

x= [^{o}p_{COM},^{o}v_{COM},^{o}p_{1},^{o}p_{2},^{o}p_{3},^{o}p_{4}]

分别表示body在world坐标系下的位置，body在world坐标系下的速度，4个foot在world坐标系下的位置。

由简单的物理定律以及上文的假设，我们可以得到：

定义向量 ^{o}p_{foot}=[{_{1}^{o}p},{_{2}^{o}p},{_{3}^{o}p},{_{4}^{o}p}] ，建立卡尔曼滤波的离散状态方程：

紧凑表达就是： x_{k+1}=Ax_{k}+Bu_{k}

预测过程的误差协方差矩阵为：

然后 ， 建立系统的观测方程

定义 观测变量 ：

z=[ {^{o}p-^{o}p_{1}}, {^{o}p-^{o}p_{2}}, {^{o}p-^{o}p_{3}}, {^{o}p-^{o}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}} ]

分别为在world坐标系下body和foot的位置差，world坐标系下foot的速度，world坐标系下foot位置的z分量（特意加入z是因为foot的高度方向误差会直接影响机器人的质心高度和抬腿高度，在每次触地时清零以消除累计误差）

接着需要用编码器角度 _{i}q^{j} 表示 ^{o}p_{i} 和 ^{o}v_{com} (建立观测方程的时候需要用到)

使用编码器获得电机角度，可以计算body坐标系下的foot位置：

对上式求偏微分，得到body坐标系下的足底速度与关节速度之间的关系：

foot的位置在world坐标系下表示为： _{i}^{o}p=^{o}p_{com}+{^{o}R_{B}} {^{B}_{i}p}

为了求world标系下foot的速度，对上式微分： \frac{d{_{i}^{o}p}}{dt}=^{o}v_{com}+^{o}R_{B}({^{B}w_{OB\times}}\cdot{_{i}^{B}p}+{\frac{_{i}^{B}p}{t}})

由foot在支撑相下不打滑的假设， \frac{d{_{i}^{o}p}}{dt}=^{o}v_{com}+^{o}R_{B}({^{B}w_{OB\times}}\cdot{_{i}^{B}p}+{\frac{_{i}^{B}p}{t}})=0

得到 ^{o}v_{com}=-^{o}R_{B}({^{B}w_{OB\times}}\cdot{_{i}^{B}p}+{\frac{_{i}^{B}p}{t}})

至此，建立系统的观测方程：

紧凑表示： z_{k+1}=C_{k+1}x_{k+1}

测量过程的误差协方差矩阵为：

最后 ，由于足底里程计支撑相时foot在world坐标系下速度为0的假设，所以 当foot在摆动相时需要额外做补充 ：“腿处于摆动象时，扩大腿部位置预测误差的协方差值，以测量值为准”。具体解释：当某个腿处于摆动相时，提高腿的位置预测误差协方差（因为假设不在成立），提高腿的速度和高度z的观测误差（预测过程和测量过程加权）

处理方法：引入每条腿的置信度trust和腿支撑状态相位（还有一种用足端力判断的方法，在程序中介绍），摆腿时phase=0,支撑时phase在0到1之间变化：

world坐标系下的foot速度观测：

v_{i}=(1-trust)\cdot{v_{COM}}+trust\cdot{-^{o}v} , ^{o}v=^{o}R_{B}({v_{rel}}+^{o}w_{B}\times{r_{rel}})

其中 ^{o}v 是测量过程得到的质心速度， v_{rel} 是foot在body坐标系下的速度， r_{rel} 是foot在body坐标系下的位置

world坐标系下foot在z方向上的高度观测：
z_{i}=(1-trust)\cdot{(p_{f}(3)+p_{0}(3))}

其中 p_{f} 表示foot在world坐标系下的位置， p_{0} 表示body在world坐标系下的位置。且上式之在摆动相时不为0,在支撑相时为了消除累计误差恒为0

上面就是状态估计器的完整原理了，看不懂的地方建议参考yxy论文和另一篇知乎文章。接下来结合代码做介绍，代码基本都有注释，所以不过多讲解了。状态估计器的c++代码总共就不到200行，也强烈建议大家原理看模糊的时候结合代码对照学习。

第二步.相关变量定义

维度和协方差大小的定义：

A1BasicEKF.h
#define STATE_SIZE 18   // 状态变量维度
#define MEAS_SIZE 28    // 观测变量维度
#define PROCESS_NOISE_PIMU 0.01     // 位置预测协方差
#define PROCESS_NOISE_VIMU 0.01     // 速度预测协方差
#define PROCESS_NOISE_PFOOT 0.01    // foot位置预测协方差
#define SENSOR_NOISE_PIMU_REL_FOOT 0.001    // 足端位置测量协方差
#define SENSOR_NOISE_VIMU_REL_FOOT 0.1      // 足端速度测量协方差
#define SENSOR_NOISE_ZFOOT 0.001    // 足端高度测量协方差

预测过程需要用到的变量定义：

A1BasicEKF.h
 // state
 // 0 1 2 pos 3 4 5 vel 6 7 8 foot pos FL 9 10 11 foot pos FR 12 13 14 foot pos RL 15 16 17 foot pos RR 
 Eigen::Matrix<double, STATE_SIZE, 1> x; // 先验估计 estimation state
 Eigen::Matrix<double, STATE_SIZE, 1> xbar; // 后验估计 estimation state after process update
 Eigen::Matrix<double, STATE_SIZE, STATE_SIZE> P; // 先验估计协方差 estimation state covariance
 Eigen::Matrix<double, STATE_SIZE, STATE_SIZE> Pbar; // 后验估计协方差 estimation state covariance after process update
 Eigen::Matrix<double, STATE_SIZE, STATE_SIZE> A; // 状态转移矩阵 estimation state transition
 Eigen::Matrix<double, STATE_SIZE, 3> B; // 输入矩阵
 Eigen::Matrix<double, STATE_SIZE, STATE_SIZE> Q; // 预测协防差 estimation state transition noise

简单提一下，这边的Eigen是一个C++的库，专门用于矩阵运算，没接触的可以先去学习一下，上手很快

测量过程需要用到的变量定义

A1BasicEKF.h 
 // observation
 // 0 1 2   FL pos residual  世界坐标系下，质心位置和足端位置之差
 // 3 4 5   FR pos residual
 // 6 7 8   RL pos residual
 // 9 10 11 RR pos residual
 // 12 13 14 vel residual from FL    不应该叫vel residual吧，就是全局坐标系下的速度
 // 15 16 17 vel residual from FR
 // 18 19 20 vel residual from RL
 // 21 22 23 vel residual from RR
 // 24 25 26 27 foot height  足端的高度 
 Eigen::Matrix<double, MEAS_SIZE, 1> y; // 实际测量 observation
 Eigen::Matrix<double, MEAS_SIZE, 1> yhat; // 测量矩阵×状态先验估计 estimated observation
 Eigen::Matrix<double, MEAS_SIZE, 1> error_y; // y-yhat estimated observation
 Eigen::Matrix<double, MEAS_SIZE, 1> Serror_y; //? S^-1*error_y
 Eigen::Matrix<double, MEAS_SIZE, STATE_SIZE> C; // 测量矩阵 estimation state observation
 Eigen::Matrix<double, MEAS_SIZE, STATE_SIZE> SC; //? S^-1*C
 Eigen::Matrix<double, MEAS_SIZE, MEAS_SIZE> R; // 测量协方差矩阵 estimation state observation noise

这边的S矩阵是等会用来计算卡尔曼增益的一个trick，并不影响理解

其他相关变量

 // helper matrices 
 Eigen::Matrix<double, 3, 3> eye3; // 3x3 identity
 Eigen::Matrix<double, MEAS_SIZE, MEAS_SIZE> S; //? Innovation (or pre-fit residual) covariance
 Eigen::Matrix<double, STATE_SIZE, MEAS_SIZE> K; // 卡尔曼增益 kalman gain
 bool assume_flat_ground = false;    //! 假设地面是平的
 // variables to process foot force
 double smooth_foot_force[4];
 double estimated_contacts[4];

第三步.状态估计器初始化

A1BasicEKF.cpp
// 构造函数，对常数矩阵进行初始化
A1BasicEKF::A1BasicEKF () 
    // constructor
    eye3.setIdentity();
    // C is fixed
    C.setZero();    //! 测量矩阵C 固定
    for (int i=0; i<NUM_LEG; ++i) {
        C.block<3,3>(i*3,0) = -eye3;  //-pos
        C.block<3,3>(i*3,6+i*3) = eye3;  //foot pos
        C.block<3,3>(NUM_LEG*3+i*3,3) = eye3;  // vel
        C(NUM_LEG*6+i,6+i*3+2) = 1;  // height z of foot
    // Q R are fixed
    //! 预测协方差矩阵Q 固定
    Q.setIdentity();
    Q.block<3,3>(0,0) = PROCESS_NOISE_PIMU*eye3;               // position transition
    Q.block<3,3>(3,3) = PROCESS_NOISE_VIMU*eye3;               // velocity transition
    for (int i=0; i<NUM_LEG; ++i) {
        Q.block<3,3>(6+i*3,6+i*3) = PROCESS_NOISE_PFOOT*eye3;  // foot position transition
    //! 测量协防差矩阵R 固定
    R.setIdentity();
    for (int i=0; i<NUM_LEG; ++i) {
        R.block<3,3>(i*3,i*3) = SENSOR_NOISE_PIMU_REL_FOOT*eye3;                        // fk estimation
        R.block<3,3>(NUM_LEG*3+i*3,NUM_LEG*3+i*3) = SENSOR_NOISE_VIMU_REL_FOOT*eye3;      // vel estimation
        R(NUM_LEG*6+i,NUM_LEG*6+i) = SENSOR_NOISE_ZFOOT;                               // height z estimation
    // set A to identity
    A.setIdentity();
    // set B to zero
    B.setZero();
    assume_flat_ground = true;  //! 假设平地
//! 不是平地的时候，测量协方差矩阵中对应足端高度z的项直接拉满.先假设地是平的，不管这个
A1BasicEKF::A1BasicEKF (bool assume_flat_ground_):A1BasicEKF() 
    // constructor
    assume_flat_ground = assume_flat_ground_;
    // change R according to this flag, if we do not assume the robot moves on flat ground,
    // then we cannot infer height z using this way
    if (assume_flat_ground == false) {
        for (int i=0; i<NUM_LEG; ++i) {
            R(NUM_LEG*6+i,NUM_LEG*6+i) = 1e5;   // height z estimation not reliable
// 初始化状态空间
void A1BasicEKF::init_state(A1CtrlStates& state) {
    filter_initialized = true;
    //! 先验估计协方差初始化
    P.setIdentity();
    P = P * 3;
    //! 状态先验估计 set initial value of x
    x.setZero();
    x.segment<3>(0) = Eigen::Vector3d(0, 0, 0.09);  //! 质心位置初始化，有个高度初值
    for (int i = 0; i < NUM_LEG; ++i) {
        Eigen::Vector3d fk_pos = state.foot_pos_rel.block<3, 1>(0, i);
        x.segment<3>(6 + i * 3) = state.root_rot_mat * fk_pos + x.segment<3>(0);    //! 阻断在世界坐标系下
}

以上两个函数（中间那个忽略）的作用分别是常数矩阵参数初始化，状态变量x的初始化。第一个函数的实现流程参照状态方程和观测方程的展开式，第二个函数的实现流程参照状态方程的展开式，其中足端位置初始化的方法就是 ^{o}p_{foot}=^{o}p_{B}+^{o}R_{B}\cdot{^{B}p_{foot}}

第四步.进行状态估计

A1BasicEKF.cpp
void A1BasicEKF::update_estimation(A1CtrlStates& state, double dt) 
    //! 更新状态转移矩阵和输入矩阵 update A B using latest dt
    A.block<3, 3>(0, 3) = dt * eye3;
    B.block<3, 3>(3, 0) = dt * eye3;
    //! 输入是加速度，为（世界坐标系下）机体加速度和重力加速度之和 control input u is Ra + ag
    Eigen::Vector3d u = state.root_rot_mat * state.imu_acc + Eigen::Vector3d(0, 0, -9.81);
    // contact estimation, do something very simple first
    if (state.movement_mode == 0) 
    {  //! stand状态下，四个足端都是触地的
        for (int i = 0; i < NUM_LEG; ++i) 
            estimated_contacts[i] = 1.0;
    {  //! walk状态下，根据足端受力大小判断
        for (int i = 0; i < NUM_LEG; ++i) 
            estimated_contacts[i] = std::min(std::max( (state.foot_force(i)) / (100.0 - 0.0), 0.0), 1.0);
//        estimated_contacts[i] = 1.0/(1.0+std::exp(-(state.foot_force(i)-100)));
    //! 更新预测协方差Q update Q
    Q.block<3, 3>(0, 0) = PROCESS_NOISE_PIMU * dt / 20.0 * eye3;
    Q.block<3, 3>(3, 3) = PROCESS_NOISE_VIMU * dt * 9.8 / 20.0 * eye3;
    // update Q R for legs not in contact
    for (int i = 0; i < NUM_LEG; ++i) 
        //! 对于没有触地的足端，预测协方差矩阵中对应足端位置的项的协方差拉满
        Q.block<3, 3>(6 + i * 3, 6 + i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * dt * PROCESS_NOISE_PFOOT * eye3;  // foot position transition
        // for estimated_contacts[i] == 1, Q = 0.002
        // for estimated_contacts[i] == 0, Q = 1001*Q
        //! 对于没有触地的足端，测量协方差矩阵中对应residual(质心位置和足端位置之差)的项的协方差拉满。 
        R.block<3, 3>(i * 3, i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_PIMU_REL_FOOT * eye3;                       // fk estimation
        //! 对于没有触地的足端，测量协方差矩阵中对应足端速度的项的协方差拉满。
        R.block<3, 3>(NUM_LEG * 3 + i * 3, NUM_LEG * 3 + i * 3) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_VIMU_REL_FOOT * eye3;      // vel estimation
        if (assume_flat_ground) 
            //! 在平地上走的前提下。如果足端没有触地，测量协方差矩阵中对应足端高度的项的协方差拉满。
            R(NUM_LEG * 6 + i, NUM_LEG * 6 + i) = (1 + (1 - estimated_contacts[i]) * 1e3) * SENSOR_NOISE_ZFOOT;       // height z estimation
    //! 预测过程。得到状态先验估计和先验估计协方差矩阵 process update
    xbar = A*x + B*u ;
    Pbar = A * P * A.transpose() + Q;
    // measurement construction
    yhat = C*xbar;
//    leg_v = (-J_rf*av-skew(omega)*p_rf);
//    r((i-1)*3+1:(i-1)*3+3) = body_v - R_er*leg_v;
    //! 实际测量过程 actual measurement
    for (int i=0; i<NUM_LEG; ++i) 
        Eigen::Vector3d fk_pos = state.foot_pos_rel.block<3,1>(0,i);
        y.block<3,1>(i*3,0) = state.root_rot_mat*fk_pos;   // 世界坐标系下足端的位置fk estimation
        //! 足端在全局坐标系下的速度 = 在body坐标系下的速度 + 质心在全局坐标系下的速度
        Eigen::Vector3d leg_v = -state.foot_vel_rel.block<3,1>(0,i) - Utils::skew(state.imu_ang_vel)*fk_pos;    //? 世界坐标系下的足端速度 吗？
        //! 支撑相的时候就用算出来的足端速度（因为足端的速度是根据足端的位置算出来的，而此时足端的位置是可靠的），摆动相的时候（足端位置和速度不可靠）用质心的速度
        y.block<3,1>(NUM_LEG*3+i*3,0) = (1.0-estimated_contacts[i])*x.segment<3>(3) +  estimated_contacts[i]*state.root_rot_mat*leg_v;      // vel estimation
        //! 支撑相的时候默认足端高度是0（这个假设yxy的论文中提到过）,摆动相的时候通过质心高度和编码器得到的足端位置计算（加入足端z，是因为他会对质心的运动状态有影响）
        y(NUM_LEG*6+i) = (1.0-estimated_contacts[i])*(x(2)+fk_pos(2)) + estimated_contacts[i]*0;                               // height z estimation
    S = C * Pbar *C.transpose() + R;
    S = 0.5*(S+S.transpose());
    error_y = y - yhat; //! 测量误差 = 实际测量值 - 测量矩阵×先验状态
    Serror_y = S.fullPivHouseholderQr().solve(error_y);     //! 这一项是:（z-Hx）*（H×P×HT + R）卡尔曼增益的分母
    x = xbar + Pbar * C.transpose() * Serror_y; //! 后验状态估计
    //! 这个S矩阵奇奇怪怪的。反正就是算出来了后验估计协方差
    SC = S.fullPivHouseholderQr().solve(C);
    P = Pbar - Pbar * C.transpose() * SC * Pbar;
    P = 0.5 * (P + P.transpose());  
    //! tricks：减少位置漂移 reduce position drift
    if (P.block<2, 2>(0, 0).determinant() > 1e-6) {
        P.block<2, 16>(0, 2).setZero();
        P.block<16, 2>(2, 0).setZero();
        P.block<2, 2>(0, 0) /= 10.0;
    // final step
    // put estimated values back to A1CtrlStates& state
    //! 是大于50N才是接触
    for (int i = 0; i < NUM_LEG; ++i) 
        if (estimated_contacts[i] < 0.5) 
            state.estimated_contacts[i] = false;
            state.estimated_contacts[i] = true;
//    std::cout << x.transpose() <<std::endl;