1. 姿态解算为何对四足机器人至关重要

在开发无人机飞控的时候我们都知道首先要完成对无人机的姿态计算，因为它决定了无人机在空中的悬停精度，高实时、高精度的姿态解算加上优秀姿态闭环，室内环境下无人机在无定位的情况下也可以做基本不动。同时由于无人机在融合GPS信号的时候需要全局加速度，姿态解算的误差将累计到加速度分量上，在融合时会导致某自由度的误差，如侧飞时的掉高问题！

无人机精确悬停对组合导航要求很高

对于无人机来说组合导航精度80%由姿态解算决定。早期开源的Pixhawk飞控其只采用了简单的PID，但却实现了非常稳定的悬停，目前被大量用于工业无人机中，其开源代码中最核心的就是EKF2组合导航部分，类似的还有我个人非常喜欢的德国开源飞控AutoQuad，其则采用了UKF具有更加优异的效果。

PX4开源飞控最核心的实际是EKF2组合导航模块

因此Mini Cheetha开源在我看来非常类似PX4项目，其即提供了完整的软硬件解决方案，又解决了当时机器人最大的痛点，PX4解决了无人机低成本组合导航的问题，MIT解决了四足机器人稳定控制难的问题，而之前介绍最新的Quad-SDK则解决了未来四足机器人感知运动规划的问题。但其中最核心的还是具有精确的姿态解算，这样与仿真中的效果才能更加一致，同时相比无人机我觉得四足机器人对姿态解算实时性和精度的要求更高，原因如下：

（1）四足机器人本体依赖运动学实现速度测量

我们知道无人机组合导航估计自身速度和经纬度时，采用IMU加速度与GPS多普勒测量的速度（注意：这个速度并不是用定位结果的微分，而是基于多普勒效应测量的结果，我刚开始做无人机时候就是采用微分速度，其测量根本没法保证无人机悬停）以及多星的定位实现三层融合，而由于GPS定位误差很大因此主要是依赖其速度测量，该速度测量一般10Hz并且存在滞后，因此需要做滞后补偿并依赖IMU数据提高其连续性和实时性。下图为典型无人机组合导航的框架：

无人机组合导航框架

可以看到除了姿态解算外，滤波器还需要完成对加速度计测量偏差的估计，从而让姿态逐渐水平，完成对GPS定位精度评估动态调节融合权重，基于GPS定位结果完成对IMU磁场抗干扰的处理，完成航向对齐。而目前的四足机器人组合导航往往是松耦合的，以MIT的方案为例其直接采用了IMU计算姿态角度完成坐标系旋转，而由于足端仅测量了机体层的速度，因此IMU如果存在测量误差则状态估计也会存在误差，而且该误差无法像用GPS那样修正Mems的测量偏差；

（2）四足机器人足地冲击造成测量失效

无人机IMU主要的噪声来源一个是本身电气测量的白噪声，另外一个就是由于电机震动和机架震动带来的噪声，因此我们在使用中往往会增加相应的阻尼和减震材料通过物理的方式滤除该部分的机械噪声，如PX4 Cube的IMU设计，另外高度悬停是采用的气压计比较害怕气流变化带来的压差，因此很多IMU内部还会设计气压道来缓冲外气流造成的气压变化，进一步通过融合GPS就可以估计出风阻力，从而提高在气流扰动下无人机的悬停精度：

IMU减震设计

而四足机器人由于依赖足地接触提供支撑力，其周期性的冲击往往会造成加速度计超出量程，而如果不对齐进行处理，将造成姿态测量慢慢飘逸，最终导致机体姿态侧倾，而姿态又决定了落足点，最终落足点位置不对导致侧向速度控制性能变差，然后就摔倒了；这样的情况在高速机动中更加明显，由于目前四足机器人高速机动主要采用提高步频的方式，使得加速度冲击非常频繁，而高速步态控制又不是很稳定，非常容易出现跨步后机体较大的角加速度，而此时姿态融合的实时性和准确性就是否重要了，否则在高速奔跑中一步失误的扰动就可以导致机器人前倾栽倒！

四足机器人侧踹下姿态扰动
高速奔跑足地冲击

2. 常用四足机器人IMU模组

如上文所述，目前四足机器人采用的IMU往往是独立的模组，直接输出解算姿态给机器人运动控制模型，如Mini Cheetha和Spotmini采用的都是LORD的IMU，而MIT开源代码中是直接读取了其姿态数据：

Spotmini采用的LORD IMU计算芯片也是STM32

国内其他厂商如宇树采用的是板载Mems的方案因此大大降低了成本，绝影早期采用了MTi系列的陀螺仪，但没有明显资料显示有更深度集成的方案，这两年关于足式机器人里程计与IMU姿态解算的紧耦合组合导航也有了很多相关论文。

3. 飞控中的低成本Mems姿态解算方法

下面介绍一下之前在我飞控中采用的EKF姿态解算算法：

https://github.com/golaced/Oldx_fly_controller​github.com/golaced/Oldx_fly_controller

其采用四元数作为系统状态来解决欧拉角存在的万向锁问题，同时以扩展卡尔曼滤波器设计相关融合算法。算法中使用了IMU输出的机体加速度矢量：

机体角速度矢量：

和机体磁场强度矢量：

来估计系统姿态角：

配套MATLAB下的代码如下，可以采用Coder自动生成进行测试：
function [Xs,Pk,Xsy_m,P_mag,att_f,gyro_f]=EKF_UPDATE(Xs,Pk,Xsy_m,P_mag,wn,wbn,an,wyn,wybn,mn,measure,z_flag,att_f,gyro_f,T)
    
%     wn_var  = 1e-6 * ones(1,4);               
%     wbn_var = 1e-8 * ones(1,3);             
%     an_var  = 1e-1 * ones(1,3);              
%     Q = diag([wn_var, wbn_var]); 
%     R = diag(an_var);         
%     Q_mag = diag([1e-6,1e-8]);
%     R_mag= 1e-3;
%1e-6,1e-8,1e-1,1e-6,1e-8,1e-3 
    pitchEKF=0;
    rollEKF=0;
    yawEKF=0;
    z_2=0;
    acc_m=measure(1:3);
    gyro_m=measure(4:6);
    mag_m=measure(7:9);
    yaw_out=measure(10);
    
    wn_var  = wn * ones(1,4);               
    wbn_var = wbn * ones(1,3);             
    an_var  = an * ones(1,3);  

    Q = diag([wn_var, wbn_var]); 
    R = diag(an_var);  
    
    Q_mag = diag([wyn,wybn]);
    R_mag = mn;

    acc = acc_m/norm(acc_m,2);
    z = acc; 
    
    x=Xs;%获取外部状态
    yaw_mag=Xsy_m(1);
    z_bias=Xsy_m(2);
    
    gyro_x_bias = gyro_m(1)-x(5);
    gyro_y_bias = gyro_m(2)-x(6);
    gyro_z_bias = gyro_m(3);
    
    A_11=[1,-(T/2)*gyro_x_bias,-(T/2)*gyro_y_bias,-(T/2)*gyro_z_bias;
            (T/2)*gyro_x_bias,1,(T/2)*gyro_z_bias,-(T/2)*gyro_y_bias;
            (T/2)*gyro_y_bias,-(T/2)*gyro_z_bias,1,(T/2)*gyro_x_bias;
           (T/2)*gyro_z_bias,(T/2)*gyro_y_bias,-(T/2)*gyro_x_bias,1];
    
    A_12=[0,0,0;
          0,0,0;
          0,0,0;
          0,0,0]; 
    A_21=[0,0,0,0;
          0,0,0,0;
          0,0,0,0];
    A_22=[1,0,0;
          0,1,0;
          0,0,1];   
    A=[A_11,A_12;A_21,A_22];
    
     Ak = eye(7)+T/2*...
         [0    -(gyro_x_bias)  -(gyro_y_bias) -(gyro_z_bias)   x(2) x(3)  x(4);
          (gyro_x_bias) 0    (gyro_z_bias)  -(gyro_y_bias)   -x(1) x(4)  -x(3);
          (gyro_y_bias) -(gyro_z_bias)  0  (gyro_x_bias)      -x(4) -x(1) x(2);
          (gyro_z_bias) (gyro_y_bias)   -(gyro_x_bias)  0     x(3) -x(2) -x(1);
          0 0 0 0 0 0 0;
          0 0 0 0 0 0 0;
          0 0 0 0 0 0 0];%状态预测
      
    x_ = (A*x')';  %四元数限幅度
    x_(1:4) = x_(1:4)/norm(x_(1:4),2); 
   
    Pk_ = Ak * Pk(:,:) * Ak' + Q; %计算协方差
    hk = [2*(x_(2)*x_(4)-x_(1)*x_(3)) 2*(x_(1)*x_(2)+x_(3)*x_(4)) x_(1)^2+x_(4)^2-x_(2)^2-x_(3)^2]; 
    Hk = 2*[  -x_(3)  x_(4) -x_(1)  x_(2) 0 0 0;
               x_(2)  x_(1)  x_(4)  x_(3) 0 0 0;
               x_(1) -x_(2) -x_(3)  x_(4) 0 0 0];%观测方程

    Kk = Pk_ * Hk' * ((Hk * Pk_ * Hk' + R)^(-1));  %KF增益
    if(z_flag(2))%加速度有测量值
        x = (x_' + Kk * (z - hk)')';      %修正状态
        x(1:4) = x(1:4)/norm(x(1:4),2); %四元数限幅度
        Pk(:,:) = (eye(7) - Kk*Hk) * Pk_;      %计算后验协方差 
    else
        x = x_;
        Pk(:,:)= Pk_;
    end
    q=[x(1),x(2),x(3),x(4)];%获取系统状态四元数
    Xs=x;
    
   [pitchEKF,rollEKF,yawEKF] = quattoeuler(q);%获取纯加速度与陀螺仪的姿态角
   att_f(1)=pitchEKF;
   att_f(2)=rollEKF;
   
   %加入磁力计的融合算法 
   if (norm(mag_m,2)>0 || yaw_out~=0 || z_flag(3)|| z_flag(4))
       mag_data = mag_m;
       %yawEKF(1) = Get_Yaw(pitchEKF(1),rollEKF(1),mag_data);%直接使用P R重投影磁力计测量航向角度  
       A_2 = [1,-T;0,1];
       H_2 = [1,0];

       yaw_mag(1) = CalEndAngle_Zcoord(yaw_mag(1),(gyro_m(3)-z_bias(1))*T );%预测
       z_bias(1) = z_bias(1); 
       x_mag_ = [yaw_mag(1),z_bias(1)];

       P_22 = A_2 * P_mag(:,:) * A_2' + Q_mag; %更新协方差

       if yaw_out==0
         z_2 = Get_Yaw(pitchEKF,rollEKF,mag_data); %获取测量值 角度
       else
         z_2 = yaw_out;
       end

       K_2 = P_22 * H_2' * ((H_2 * P_22 * H_2' + R_mag)^(-1)); %计算卡尔曼增益 
       if(z_flag(3) || z_flag(4))
           x_mag = x_mag_+ (K_2 * (z_2 - x_mag_(1)))';  %修正状态   
           P_mag(:,:) = (eye(2) - K_2*H_2) * P_22;   %计算后验方程 
       else
           x_mag = x_mag_;
           P_mag(:,:)=P_22;
       end
       yaw_mag(1) = x_mag(1);%更新外部状态
       z_bias(1) =  x_mag(2);

       if(abs(z_bias(1))>0.1)% 限制状态
         z_bias(1)=0.1 ;
       end

       Xsy_m(1)=yaw_mag;
       Xsy_m(2)=z_bias;

       att_f(3)=yaw_mag(1);
   else
       att_f(3)=yawEKF;
   end
   gyro_f(1)=gyro_m(1)-Xs(5);
   gyro_f(2)=gyro_m(2)-Xs(6);
   gyro_f(3)=gyro_m(3)-Xsy_m(2);
   
end

下面给出了姿态解算部分所参考网络的原始例子：

链接：https://pan.baidu.com/s/1jttYzKercFvfTnae-3wvfg 
提取码：k6ig 
--来自百度网盘超级会员V3的分享

4. 总结

上述例子，实际就是飞控中常用的姿态解算部分，其可以用于搭建自己四足机器人低成本姿态解算的方案，当然核心的问题还是没有考虑之前提到的加速度冲击、测量偏差等问题，但相比传统直接输出姿态的模块，在该代码上进行扩展增加相应的卡方检测、自适应鲁棒滤波等技术可以大幅度提升姿态解算的可靠性，同时也可以进一步加入视觉里程计的定位结果实现对测量偏差的修正，毕竟四足机器人更多是在室内机动！