摘要
在我的想象中机器人首先应该能自由的走来走去，然后应该能流利的与主人对话。朝着这个理想，我准备设计一个能自由行走，并且可以与人语音对话的机器人。实现的关键是让机器人能通过传感器感知周围环境，并通过机器人大脑处理并输出反馈和执行动作。本章节涉及到的传感器有激光雷达、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。