第3章 Moco-8四足机器人导航算法简介

• 3.1 Moco-8导航算法框架

如图所示Moco-8的导航系统包括了完整的姿态解算和组合导航，上述框架也适用于飞控和其他小车底盘的导航系统中。在完成对IMU数据采样后首先对其进行预处理，包括中值滤波剔除异常值，标定参数校准和低通滤波。之后，使用预处理数据完成姿态解算获取精确的姿态角和机体加速度值。为减少机器人冲击地面造成传感器数据尖峰带来的控制饱和，进一步采用状态观测器估计更平滑却又比低通滤波响应更快的角速度；另一方面，通过运动学正解时刻获取足尖位置并计算相应的雅克比矩阵为VMC反馈控制提供所需先验参数，通过对足尖位置进行微分获取足尖速度并进一步结合机器人构型得到机体中心的速度，从而构造一个简单的里程计。最终，通过融合里程计速度、机体加速度和外部GPS、光流或图像导航信息获取机器人精确的位置、速度反馈。

• 3.1 数据预处理

（1）中值滤波

中值滤波能较好地滤除传感器由于连接线松动或其他干扰引起的野值，其能减小数据结果被极大值和极小值的影响。其最典型的一种方式是采用一个固定长度的滑动窗口，计算结果时首先去除序列的最大最小值，使用余下数据的均值作为最终的滤波结果。

（2）传感器校正

IMU中需要校正的传感器主要是加速度计、陀螺仪和磁力计。其中陀螺仪主要需要消除其存在的零偏以减少在后续姿态解算时引起的积分误差，目前主要采样静置长时间采集陀螺仪原始数据并求取均值作为零偏；对于其他二者除了需要校正零偏外还需要调整各轴电位误差引起的比例不一致，简单来说就是需要通过下式对加速度传感器三轴原始数据进行缩放和平移，保证在不同倾角下其测量输出均为 [0 0 g]。

保证上述校正结果可以有效提高组合导航精度。以飞控气压计加速度计融合为例，通过标定保证各向加速度测量模值一致后IMU禁止时不同角度下机体坐标系z后加速度分量均为零，因此在融合时可以减少对气压计融合权重从而提高最终结果的数据平滑性并避免侧飞出现的突然掉高，而不校正加速度计则在飞行器倾斜后垂直加速度分量不为零造成积分误差，而滤波器对加速度偏执估计收敛较慢最终造成飞行器定高效果变差，而要加快对偏执的估计收敛速度只能提高对气压计的融合权重，这有会将其存在的噪声引入最终的滤波结果中。

对于磁力计来说，其十分容易受到外部磁场和电磁信号的影响。理想情况下磁力计输出应该分布在一个中心在原点的球体上，受干扰其不但中心会出现偏差分布形状还会变成椭圆，因此对磁力计校正的目的就是将其重新修正为正球体，而这正是网上提到的磁力计椭球拟合问题。通过最小二乘法或高斯牛顿法获取椭球体参数后，对原始磁力输出同样在各轴进行平移和比例缩放。

（3）低通滤波（LPF）

低通滤波的目的是去除传感器数据的高频噪声，如气压计融合中往往对其原始数据进行10Hz以下的低通滤波以获取角平滑的数据，目前常用的是一阶低通滤波或者性能更优异的巴特沃斯低通滤波器，以前者为例如下的LPF滤波器被大量应用于飞控的数据处理中：

LPF(float in, float* out, float fz, float dt) {
 if (fz<= 0.0f || dt <= 0.0f) 
        *out = in;
 float rc = 1/(PI*fz);
    K = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
    *out+= (in - *out) * K;
}

但低通滤波器会引起相位滞后，简单来说就是随着滤波频率的下降结果会慢慢滞后于输入，而在后续组合导航中需要对其进行补偿否则将引起误差。对于后者来说可以采用Matlab自带工具计算相应的系数，在控制台输入fdatool，完成对其采样频率Fs和低通截止频率Fc的设定即可。

在完成滤波器生成后通过File中Export选型将滤波器结果导出至workspace中并使用[b,a]=sos2tf(Hd.sosMatrix,Hd.ScaleValues)命令获得编程语言实现滤波器时所需的矩阵参数。

• 3.2 姿态解算

四足机器人中的姿态解算我认为十分重要，特别是对于Moco-8这类直接采用舵机驱动的低成本机器人来说，没有姿态增稳其步态算法的稳定性将严重下降，同时由于机器人与地面的冲击会造成加速度计数据包含冲击力产生的加速度甚至超出量测，造成其姿态解算存在误差，而对于四足机器人来说如其横滚轴方向姿态解算存在误差会使得机体倾斜从而在重力分量作用下机器人无法在保证自身位置，如在连续踏步中机器人会发生平移并且随着机体高度的增加便宜越大（同倒立摆），这个问题可以通过增加视觉传感器或足底反馈得到解决，但精确的姿态解算仍是保证机器人稳定的基础。目前的姿态解算算法有很多如互补滤波，EKF和UKF等。这里仅介绍最简单的互补滤波，在我看来EKF、UKF算法能有效解决姿态解算的非线性问题，而随着解算周期的增大系统非线性运动后解算结果的收敛速度采用上述方法不会出现明显的下降，而互补滤波采用固定调整增益因此在解算频率下的效果较差，但目前飞控等系统中姿态解算频率往往能在500Hz以上，采用互补滤波也能较好地跟踪机体非线性运动。

互补滤波即通过综合不同系统输出来得到更优的系统状态估计，在姿态解算中采用陀螺仪、加速度计和磁力计数据融合得到高精度无漂移的姿态角，而该方法同样也被推广至气压计、超声波、加速度融合，光流、GPS和加速度计融合中。其核心是使用长期精确的传感器数据校正里程计或陀螺仪的积分误差，因此首先需要构建预测结果与真实测量间的误差。定义导航坐标系下的单位重量矢量为

  ，则如采用四元数

  来描述机器人姿态，可以构建姿态变换矩阵。

由于

  ，则可以将 

 转换到机体坐标系下从而得到预测的加速度计机体测量值：

则进一步基于向量叉积定义可以得到预测机体加速度与测量加速度间的误差向量，由于加速度计测量结果为AD采样值因此需要对其进行归一化处理：

进一步得到加速度测量误差向量：

则构建PI控制器使用该误差对陀螺仪测量结果进行校正，在此首先继续介绍磁力计融合方法，同时对磁力计测量结果归一化：

将测量结果使用四元数转换至全局坐标系下

  ，设全局坐标系的y轴指向正北则有：

将上述结果重新转换至机体坐标系得到磁力计预测输出

  ，同理可以与实际测量结果构建磁力计测量误差向量：

这里还有另一种磁力计校正方法，其首先采用磁力测量计算出航向角：

其中由四元数描述姿态角的公式如下：

则可直接由航向角误差得到磁力计测量误差向量：

采用第二种方法的好处是能十分方便的引入如GPS、SLAM或图像测量得到的航向角对姿态解算结果进行修正，但其直接采用角度对磁场倾斜偏差修正的效果较差。最终，综合加速度和磁力计得到修正陀螺仪用的总体误差向量，同时考虑机器人冲击地面带来的加速度数据突变和外部磁场干扰磁力计增量了两个自适应融合权重：

其中  ，  。

它们保证了在加速测量值或磁力计测量值超出正常情况时对陀螺仪修正幅度的减小。进一步对陀螺仪原始测量偏差进行修正：

进一步，采用一阶龙格库塔法求解四元数微分方程，得到如下的四元数离散更新公式：

• 3.3 角速度观测器

在VMC反馈控制中我们设计了PD控制器来模拟弹簧的不同特性，其中姿态控制部分需要使用机体角速度作为微分控制器的反馈，由于无法保证机器人时刻处于地面支撑状态同时Moco-8采用的舵机无法实现良好的减震阻尼，因此在移动中陀螺仪原始测量数据存在着十分严重的噪声，如果使用其作为微分反馈则无法给定较大的控制参数同时还会引起机器人剧烈的震荡，因此需要设计一个角速度观测器或者说滤波器在保证能跟踪机器人姿态变化的前提下平滑噪声，如下图所示红线为陀螺仪测量值蓝线为观测结果。下面以机体z轴角速度为例介绍结合机器人模型采用互补滤波的估计角速度的方法。

首先由Moco-8机器人构型可以采用差速运动模型计算出由机器人自身驱动产生的预测角速度：

其中W为机器人宽度，则可构建一个简单的互补滤波器综合预测角度与实际的陀螺仪测量数据：

则该方法可以在保证对陀螺仪测量值较低的截止滤波频率下（<5Hz）减小相位滞后提高数据实时性，同理其也可以应用于俯仰和横滚轴的反馈数据处理。在Moco-8的官方VMC库中则采用了ESO扩展状态观测器来完成该功能，其在观测角速度的同时估计外力扰动并用于抗扰控制，提高机器人姿态控制稳定性。

• 3.4 运动学里程计

对于地面机器人来说里程计十分重要，其可以帮助机器人推算自身的全局位姿从而实现初步的导航，或者使用里程计数据与GPS、SLAM和视觉数据融合得到更可靠的导航定位结果。足式机器人来说可以通过足尖速度结合机器人模型构建一个简单的运动学里程计。首先再次给出Moco-8单腿运动学正解表达式：

为简化后续计算可将工作空间坐标系固定在机体轴上则可忽略

  ，则进一步对虚拟腿长进行分解得到足尖的位置如下：

则要求解各腿足尖速度有两种方式：

(1)足尖位置微分：可直接对各足尖速度进行微分计算并通过低通滤波处理，该方法十分简单，但是微分计算会受到采样周期波动影响并放大数据的噪声。

(2)雅克比矩阵求解：在VMC算法中我们求取了雅克比矩阵J，其将关节角速度与足尖线速度映射，因此也可以直接使用其完成快速对足尖速度的解算。由于该方法可以直接使用编码器或控制输出作为数据源，因此其估计结果噪声小、数据连续。

最终，在获取各腿足尖线速度后仅使用支撑相的结果结合机器人构型估计其机体线速度，由于Moco-8采用差速驱动模型因此其机体系x和z轴速度如下：

其中

  为左侧所有支撑腿足尖速度的均值，其中

  为右侧所有支撑腿足尖速度的均值。

• 3.5 卡尔曼组合导航

使用运动学里程计进行航迹推测能得到机器人在全局坐标系下的位姿并用于导航控制，但由于机器人存在打滑导致里程计推算中误差会不断累计，因此需要通过引入外部位置、速度传感器来提高导航精度纠正累计误差。目前，对于上述组合导航问题主要采用卡尔曼滤波器实现，其已经被广泛应用于飞控导航系统中。

（1）差速模型建模下的导航算法设计

相比于飞控导航算法Moco-8采用的组合导航算法十分类似，只是在系统运动模型上不同，但考虑下后续系统兼容性和单片机计算能力有限，我们之间采用了与飞控类似的导航算法，下面还是先介绍一下差速模型下的滤波器设计，这里不再具体介绍KF、EKF的公式和理论推导，首先考虑如下差速逆运动学模型：

其中

  为左右两侧驱动器的线速度，Moco-8则直接采用上一节运动学里程计结果。

• 状态预测

对于足式机器人来说其十分容易出现足尖打滑的现象，并造成里程计错误的测量结果，如不考虑该问题则可能导致严重的误差，因此在系统状态建模中我们考虑里程计不精确标定、机器人受阻或足尖打滑引起的错误测量。定义系统状态为

  其包括了机器人在{G}系下的二维位置和朝向角、机器人机体速度和角速度以及各码盘存在的测量偏差，系统的状态转移方程如下:

公式中

  为系统过程噪声其对应协方差矩阵为

  ，过程噪声描述了系统状态中未建模的部分一般采用高斯白噪声进行建模，假设各轴间噪声相互独立则有:

进一步，结合系统状态定义和你运动学模型推导出 t时刻状态的预测值为:

其中

  为系统采样周期，则依据EKF算法得到预测状态对应协方差矩阵

  为:

公式中 

其中  。

• 状态更新

考虑机器人在室内使用SLAM算法或室外使用GPS获取到自身全局位置，则状态更新使用全局定位结果和里程计测量线速度对状态预测值进行校正，则k时刻其对应的量测预测值如下：

最终，采用EKF算法计算卡尔曼增益、修正状态预测值并计算其对应的后验协方差矩阵。

其中  。

（2）匀加速模型下的导航算法设计（Moco-8使用）

受限于单片机运算量无法同时实现VMC步态算法和差速模型EKF，因此实际的代码中我们仅使用了匀加速模型和KF算法完成组合导航系统的设计，由于x-y-z轴均使用相同系统模型，因此下面仅以x轴为例介绍滤波器的设计。

定义系统状态为

  其包括了全局位置、速度和加速度偏差，则k时刻状态的预测值为：

其中

  为使用姿态矩阵将机体加速度测量值转换到全局坐标系下的结果，对应量测更新来说主要包括全局位置测量和全局速度测量，其中后者同样使用姿态矩阵将前文的运动学里程计机体速度转换到全局坐标系，即 

 其中：

则量测矩阵  ，进一步基于KF算法完成滤波处理这里不再推导。

（3）使用Matlab快速生成C语言代码

对于在单片机中实现所设计好的导航算法有很多中方法，如采用ARM自带的矩阵库，采用C语言编写的矩阵运算库或者在Matlab中编辑算法并自动生成，这里介绍最后一种方法，其能快速地将算法从仿真移植到单片机中，但是导出的代码运算效率较低，同时如果在项目中存在多个不同仿真文件导出的C程序可能会在实际运行中出现错误。下面以气压计和加速度计融合时的卡尔曼滤波器设计为例进行Matlab 代码导出介绍，其系统状态为

  ，系统预测状态为：

量测值为气压计测量高度和机体加速度，预测系统的量测方程为 

 ，则其对应的Matlab程序如下：

function [xa_apo,Pa_apo]=KalmanFilter_core(xa_apo,Pa_apo,z,z_flag,q,r,dt)
A=[1 dt 0.5*dt^2 0
   0 1 dt 0
   0 0 1 0
   0 0 0 1];
Q=diag([ q(1),q(2),q(3),q(4)]);
R=diag([r(1),r(2)]);
%% 保存系统状态
pos=xa_apo(1); 
vel=xa_apo(2);
acc=xa_apo(3);
bias=xa_apo(4);
%% 状态预测
pos=pos+ vel*dt+ 0.5*acc*dt^2;
vel=vel+ acc*dt;
x_apr=[pos,vel,acc,bias]';  
P_apr=A*Pa_apo*A'+Q;
%% 量测更新
H=[z_flag 0 0 0;
   0 0 1 1];
s_k=H*P_apr*H'+R;
K_k=(P_apr*H'/(s_k));
y_k=z-H*x_apr;
xa_apo=x_apr+K_k*y_k;
Pa_apo=(eye(4)-K_k*H)*P_apr;
end

在Matlab中输入coder打开自动代码生成软件并载入对应.m文件，在Define一栏中定义与算法设计一致的矩阵维度：

在代码设置Setting中配置代码输出方式为所有代码都在单一文件内：

最终，电机GENERATE生成代码，在没有错误后将代码生成文件中的.c和.h文件添加到STM32单片机程序项目中编译调用即可。

在下一节中将介绍如何基于Moco-8开源项目自己搭建机器人，打印机架、加工PCB和组装机器人，以及如何第一次测试机器人行走！