自动驾驶（五十六）---------Yawrate和Imu标定

      前面介绍过GPS和IMU的惯导，也介绍过车辆轨迹预测，这中间很重要是为了车身航迹推算，但是在这之前，需要对传感器进行标定，本文主要介绍IMU的原理，从传感器限制角度介绍IMU的标定。

1. 传感器介绍

       IMU是测量物体三轴姿态角及加速度的装置。一般IMU包括三轴陀螺仪及三轴加速度计，某些9轴IMU还包括三轴磁力计。首先介绍这三种传感器的原理。

        1. 陀螺仪                 

        x方向是驱动方向，使m沿x轴有一速度，该速度已知。当垂直于xy平面有角速度时根据科里奥利力在y方向上会有科里奥利力产生，距离改变，导致电容改变，可以算出旋转轴垂直于xy平面的角速度

                                         

         2. 加速度计           

         我们假设在失重环境下有一个球放在一个一样大的方盒子里面，如图1，这时如果有一个加速度向左，如图2，则盒子右侧便能检测到一个力。这时我们根据重力加速度在各个方向上的分量便能求解出物体的姿态了，但是水平方向偏航角与重力加速度垂直无法求得。

         3. 磁力计

         简单来说磁力计就是一个指南针，正好弥补了加速度计无法测量的水平方向的偏航角的问题，假设当前有磁力计测量得到的磁场强度向量m=[mx,my,mz] 然后计算得到其在大地坐标系下的投影mn=[mnx,mny,mnz] 设飞机的偏航角为anglez ，则：

                                   

2. 误差介绍

        IMU的误差来主要来自于三部分，包括噪声、尺度因子和轴偏差。这里介绍这几种误差不需要深入理解，只需要知道IMU在使用过程中存在不可抗拒的误差。

        1. 噪声包含高斯白噪声、随机游走(这里指零偏Bias)，高斯白噪声好理解，传感器不可能能精确测量物理值，一定存在噪声；随机游走是一个离散模型，可以把它看做是一种布朗运动，或者将其称之为维纳过程。该模型可以看做是高斯白噪声的积分。该噪声参数一般是由传感器的内部构造、温度等变化量综合影响下的结果。

        2. 其实在这个地方上说IMU的尺度因子不太恰当，应该将其称之为__尺度误差。__这部分的误差来自于传感器的数字信号向物理量转换的误差，比如说AD向加速度/角速度转换的时候的误差。尺度误差的表达比较简单，是传感器出厂时自带的误差。

        3. 一般情况下，加速度计的坐标系AF和陀螺仪的坐标系GF都不是正交的坐标系，但我们正常使用的时候都是默认测量量是在正交坐标系下的，所以就需要一个变换矩阵将测量量从非正交坐标系 AF\GF下转到正交坐标系BF下，将其称之为机体坐标系。所以在轴偏角分解时存在误差。

3. 误差标定

       IMU在校准过程中，加速度计和陀螺仪是分开校准的，一般是先校准加速度计，然后利用准确的加速度计信息再来校准陀螺仪。

      1. 加速度计标定很简单，把IMU静止放置在桌面，统计各个方向的加速度值，计算出稳定的偏差，一般保持4s。

      2. 旋转IMU使其保持不同的姿态，重复第一步，统计各个角度的偏差平均值，旋转次数一般为30-50次。

      3. 计算出总的各个方向的偏差期望，最后输出为测量值减去偏差的期望。

      陀螺仪的校准有两个部分：Allan方差校准Bias和优化方式求解尺度因子及轴偏差。在校准陀螺仪的时候，要使用到加速度的校准信息，所以加速度校准的好坏关系到整个IMU的校准效果。

     (1)将陀螺仪静止放置时间TT ,单个采样周期为 τ0τ0，共有NN 组采样值。
  (2)计算单次采样输出角度 θθ 和 平均因子 mm，mm 要尽量取得均匀。

                                                           
  (3)计算Allan方差，当 m 取不同的值的时候会有不同的Allan方差值。

                                       其中，τ=mτ0。
  (4)一般在绘制Allan方差曲线的时候使用的是Allan方差的平方根，拟合Allan方差曲线。

4. Yawrate 标定

      Yaw 是横摆角，而横摆角对时间微分，我们就得到了横摆角速度 Yaw-Rate，横摆角是对车身姿态变化的重要参数，是IMU标定参数中的一部分，本来是不需要再单独讲解的，但是一般我们是很难获取IMU的原始数据，又因为Yawrate的重要性，这里在单独介绍一种简单的标定方法。

       1. 在车身速度为0时，统计所有输出的Yawrate，计算出速度为0时Yawrate的期望。

       2. 统计车身在不同速度下，并且方向盘转角小于2度时所有输出的Yawrate，计算出不同速度下YawRate的期望。

       3. 在车身不同速度下，计算出一个期望 = E0*n0 + Ei*ni；E0为速度为零时的期望，Ei是速度为i时的期望。

       4. 在满足以上条件下的Yawrate的值安装一定时间和一定比例更新对应条件下的期望。

       5. 在对应车速下，把原始Yawrate减去对应条件下的期望作为输出。