SLAM+语音机器人DIY系列：（三）感知与大脑——2.带自校准九轴数据融合IMU惯性传感器
温馨提示
摘要
在我的想象中机器人首先应该能自由的走来走去，然后应该能流利的与主人对话。朝着这个理想，我准备设计一个能自由行走，并且可以与人语音对话的机器人。实现的关键是让机器人能通过传感器感知周围环境，并通过机器人大脑处理并输出反馈和执行动作。本章节涉及到的传感器有激光雷达、IMU、轮式里程计、麦克风、音响、摄像头，和用于处理信息的嵌入式主板。关于传感器的ROS驱动程序开发和在机器人上的使用在后面的章节会展开，本章节重点对机器人传感器和嵌入式主板进行讲解，主要内容：
2.带自校准九轴数据融合IMU惯性传感器
IMU是惯性测量单元的简称，用于测量物体的三轴姿态角（roll、pitch、yaw）、三轴加速度（acc_x、acc_y、acc_z）、三轴角速度（w_x、w_y、w_z）等。IMU惯性测量单元在制造过程中，由于物理因素，导致IMU惯性测量单元实际的坐标轴与理想的坐标轴之间会有一定的偏差，同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。这里提到的自校准就是要通过给的补偿值来减小或消除坐标轴的偏差及原始值的固定偏差，也就是所谓的IMU内部标定。如果将IMU安装到机器人或摄像头上后，需要知道IMU与机器人或摄像头的相对位置，这个时候进行的标定就是所谓的IMU外部标定。特此说明，这里提到的自校准指IMU内部标定。这里提到的九轴数据融合，是指通过三轴加速度、三轴角速度数据融合得到更加精准的三轴加速度、三轴角速度，同时通过三轴加速度、三轴角速度、三轴磁力计数据融合得到测量物体的三轴姿态角。选用一款带自校准九轴数据融合的IMU，能很好的提升机器人的SLAM建图与导航性能，同时降低机器人上软件的开发难度。出于这一点，我们选用了一款基于MPU9250的.带自校准九轴数据融合IMU，如图6。
（图6）带自校准九轴数据融合IMU惯性传感器
2.1.自校准
IMU误差模型：
误差主要来自三部分：噪声（bias and noise）、尺度误差（scale error）、轴偏差（axis misalignment）。通过对这些误差的补偿来实现IMU测量数据的校准，校准的数学表达如图7。
（图7）校准的数学表达
噪声部分考虑零偏Bias和高斯白噪声noise。零偏Bias也叫随机游走，一般是由传感器内部构造、温度变换多方面综合影响的结果；高斯白噪声noise，一般是由于AD转换引起的一种外部噪声。
尺度误差部分，来自于AD转换中量化过程，比如采样电压值1V对应acc(x)轴的1.4g，同样采样电压值1V对应acc(y)轴的1.6g，也就是说不通的轴上AD转换量化是不同的。
轴偏差部分，三轴加速度、三轴角速度、三轴磁力计的坐标轴严格上都不是正交坐标系。但是最后我们期望的使用值默认是在正交坐标系下测量的，所以就需要将在非正交坐标系测量的原始值变换到正交坐标系中。
加速度校准：
校准过程中需要判断传感器是否处于静止状态，其实很简单，在时间t内（t一般取50s），分别计算acc(x)、acc(y)、acc(z)三轴数据的方差var[acc(x)]、var[acc(y)]、var[acc(z)]，如果var[acc(x)]+var[acc(y)]+var[acc(z)]小于阈值H（H为经验值，需要通过实验法确定），就判断传感器静止。
在静止状态下，加速度计测量值的二范数等于当地重力加速度g。在这一约束条件下，利用最小二乘法进行优化问题求解，由于加速度采样值取得是一个小窗口采样区间的平均值，所以可以忽略白噪声Na的影响，便可以求解出待标定参数Ta、Sa、Ba。在静止状态取一串加速度采样值共M个，构建代价函数如图8，对代价函数进行最优化求解即可，可以选用ceres或g2o等优化工具来完成具体的优化计算过程。
（图8）代价函数
角速度校准：
角速度校准分为两部分：用Allan方差校准零偏Bias、用最优化方法求解尺度误差Sw和轴偏差Tw。加速度校准中需要用到加速度校准信息，所以加速度校准好坏影响整个IMU校准。
同加速度校准一样，也需要在静止状态下采集角速度，简便的做法是在采集加速度的同时也采集角速度。利用角速度采样值计算Allan方差，Allan方差计算比较繁琐就不展开，请直接参考wiki百科：https://en.wikipedia.org/wiki/Allan_variance。
Allan方差中共有5个噪声参数：量化噪声Q、角度随机游走N、零偏B、角速度随机游走K、角速度斜坡R。通过图9的步骤便可以求出角速度的零偏Bw，由于角速度采样时是一个小窗口采样区间的平均值，所以可以忽略白噪声Nw的影响。
（图9）用Allan方差校准零偏Bias
角速度的零偏Bw校准完后，将零偏值Bw带入，接着校准尺度误差Sw和轴偏差Tw。挑选加速度校准过程中两静止状态夹杂的动态片段，这样经过校准后的静态值的平均值可作为acc_calib向量的起始值acc_calib(k-1)和旋转完成后的结束值acc_calib(k)。
通过图10的步骤便可以求出角速度的尺度误差Sw和轴偏差Tw。
（图10）求角速度的尺度误差Sw和轴偏差Tw
磁力计校准：
与加速度校准比较类似，磁力计测量值的二范数等于当地磁场强度m，不同的是磁力的测量不需要在静止状态。在这一约束条件下，利用最小二乘法进行优化问题求解，由于磁力采样值取得是一个小窗口采样区间的平均值，所以可以忽略白噪声Nm的影响，便可以求解出待标定参数Tm、Sm、Bm。构建代价函数如图11，对代价函数进行最优化求解即可。
（图11）代价函数
由于需要用加速度/磁力计进行融合，需要将磁力计测量值变换到加速度坐标系。变换矩阵Tm2a求解比较简单，通过多组关联的加速度acc_calib和磁力计mag_calib，通过最小二次法优化求解，可以求出变换矩阵Tm2a。这里基于一个假设，磁力向量与加速度向量成固定夹角，也就是两向量的点乘为一个常数C，利用这个约束可以构建代价函数如图12，对代价函数进行最优化求解即可。
（图12）代价函数
IMU校准软件流程图：
（图13）代价函数
IMU手动校准与自校准对比：
通常的校准过程是，将IMU接入PC端，手动将IMU置不同的状态，PC端通过采集这些数据，完成校准。另一种校准过程是，将校准算法内置在IMU模块上的MCU中，在IMU工作的过程中实时采集数据，并自动完成校准，不需要人为的干预。本文介绍的MPU9250模块就属于自校准这一方式。手动校准与自校准对比如图14。
（图14）手动校准与自校准对比
2.2.九轴数据融合
IMU的校准完成后，就可以利用校准后的九轴数据进行数据融合，得到IMU的空间姿态了。
MPU9250模块包括三轴加速度计、三轴陀螺仪和三轴磁力计。因为加速度/磁力计具有高频噪声，也就是说它们的瞬时值不够精确，解算出来的姿态会震荡，但长期来看姿态方向是对的。而陀螺仪具有低频噪声，即每个时刻的得到的角速度是比较精确的，使用积分就能得到旋转角度（姿态），但是积分会累积误差，因此积分到后面姿态就不对了，也就是漂移现象。 加速度/磁力计和陀螺仪在频域上的特性互补，可以融合这三种传感器的数据，提高精度和系统的动态特性。
由于欧拉角在求解姿态时存在万向节锁，不能用于全姿态解算；故采用四元数Quaternion进行姿态解算。常用的九轴数据融合算法包括：高低通互补滤波、扩展卡尔曼滤波EKF、Mahony滤波。在实际情况中根据不同的用途进行选择，这里对这三种滤波方法进行原理性的介绍。
高低通互补滤波：
（图15）高低通互补滤波的过程
高低通互补滤波的过程如图15,通过加权系数融合陀螺仪估计姿态和加速度/磁力计融合姿态，达到对陀螺仪低通滤波、对加速度/磁力计高通滤波的效果。
扩展卡尔曼滤波EKF：
和高低通互补滤波过程类似，也是分为估计、校正。
（图16）EKF滤波的过程
EKF滤波的过程如图16，想要了解EKF滤波的详细推导请直接参考wiki百科：https://en.wikipedia.org/wiki/Extended_Kalman_filter。估计过程中，利用陀螺仪积分推算出估计姿态；在校正过程中，计算估计姿态下的加速度/磁力计，再用在实际姿态下测量的加速度/磁力计按照EKF校正方程进行校正。
Mahony滤波：
同样，Mahony滤波也分为估计、校正。主要使用了PI控制思想。
（图17）Mahony滤波的过程
Mahony滤波的过程如图17。在估计过程中，同样利用陀螺仪积分推算出估计姿态；在校正过程中，计算估计姿态下的加速度/磁力计，测量在实际姿态下的加速度/磁力计，这两个向量叉乘便可以得到姿态误差e，姿态误差e通过PI控制过程可以实现对陀螺仪测量角速度的补偿，利用补偿后的角速度重新估计出来的姿态，就是滤波后的姿态了。
（图18）MPU9250模块九轴数据融合后的效果
MPU9250模块九轴数据融合后的效果如图18，图中左侧显示的分别是加速度、角速度、磁力计的实时数据，图中右侧显示的是融合后的姿态实时数据，可以看出融合后的姿态还是非常平稳的。
2.3.在机器人中使用IMU
（图19）在机器人中使用MPU9250-IMU模块
MPU9250-IMU模块通过串口与机器人相连接，机器人中通过运行IMU的ROS驱动，来实现读取串口的IMU数据和将IMU数据发布到/imu这个主题，这样机器人上的其它节点就可以通过订阅/imu主题来获取IMU数据了。关于IMU的ROS驱动在机器人上的具体使用，将在后面的章节中具体讲解。
后记
------SLAM+语音机器人DIY系列【目录】快速导览------
第1章：Linux基础
第2章：ROS入门
第3章：感知与大脑
第4章：差分底盘设计
第5章：树莓派3开发环境搭建
第6章：SLAM建图与自主避障导航
2.google-cartographer机器人SLAM建图
第7章：语音交互与自然语言处理
第8章：高阶拓展
2.centos7下部署Django(nginx+uwsgi+django+python3)
参考文献
[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.
