IMU在AGV小车SLAM建图中的应用浅析

        导引与控制是AGV小车的关键部分。AGV小车依靠导引可以沿一定路线自动行驶。根据导航方式不同，可以分为直接坐标导引、电磁导引、磁带导引、光学导引、激光导引、惯性导航、图象识别导引、GPS（全球定位系统）导航等，通过导航方式AGV确定自身位置，按照预先规定的路径行走。

       其中惯性导航是在AGV小车上安装陀螺仪，在行驶区域的地面上安装定位块，AGV可通过对陀螺仪偏差信号的计算及地面定位块信号的采集来确定自身的位置和方向，从而实现导引。此项技术在军方运用较早，其主要优点是技术先进，定位准确性高，灵活性强，便于组合和兼容，适用领域广，已被国外的许多AGV生产厂家采用。其缺点是制造成本较高，导引的精度和可靠性与陀螺仪的制造精度及使用寿命密切相关。

而目前比较研究比较多且较先进的激光导引是采用SLAM机器人定位技术（实时定位与地图生成）实现AGV的路径地图。简单来讲当AGV机器人在一个陌生环境走一圈即自动扫描并生成该场景电子地图，该电子地图是AGV的激光器每秒几万个坐标点拼接生成，然后在已知地图任意创建工作站点即属性，AGV在行驶运动过程中实时与已知的基准地图坐标进行匹配，从而实现AGV高精度地位。部分厂家的激光AGV通过多种传感器融合优化算法其重复运行精度达到5毫米，并且环境动态移动的人和物对其没有任何影响。

激光导引AGV

1.0 IMU指的是惯性测量单元。

IMU大多用在需要进行运动控制的设备，如汽车和机器人上。也被用在需要用姿态进行精密位移推算的场合，如潜艇、飞机、导弹和航天器的惯性导航设备等。

     惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

1.1 IMU的优点

   IMU 有输出频率高、能输出6DoF自由度测量信息等优点 。视觉 SLAM 存在输出频率低、旋转运动时、或运动速率加快时定位易失败等问题，

1.2 IMU的不足

      IMU的数据必须经过标定，至少要将零偏等数据计算出来；

      IMU的零偏包括固定零偏（受到应力和其他因素的影响）和启动零偏（每次启动不一样，启动之后就固定不变）；

      标定一般标定的是固定零偏和启动零偏，班宁传感出品的IMU在算法上进行了优化，目前已经做到了很高的精度，完全满足了AGV的应用场景。同时班宁的IMU在-40℃-85℃范围内进行校准，满足全温区的使用。

1.3 用IMU定位难点分析

IMU定位原理

     对于acc+gyr方式：acc+gyr得到比较准确姿态，去除acc重力分量影响；二次积分可以得到位置；

     对于acc+gyr+mag方式：需要acc+gyr+mag方式得到比较准确姿态，其他处理方式一样；

难点分析

IMU定位不准，主要由于3点造成：

a. 姿态不准导致重力影响去除不了，误差不断累积；

b. 器件本身漂移不能去除，如acc从静止到运动再到静止，acc积分得到的速度应该为0， 而实际不为0；

c. 通过笔者的测试，班宁的BNS系列IMU，在本身漂移方面有着优异的表现；

现对上述3点进行说明：

重力影响：可以采用一定的方法(如MahonyAHRS、MadgwickAHRS、Kalman，具体可参见xio)：对于acc+gyr方式，acc只能在静止时得到姿态，运动时不能，因而在运动情况下姿态不准确，重力影响去除不了；对于acc+gyr+mag，可以用mag得到姿态，提高姿态准确性，当有磁场干扰时，该方式效果也存在问题；

本身漂移：对于此问题，一般的处理方法为用高通滤波去除，但高通滤波后得到的只是相对运动关系，不能得到准确的位置信息；因而在实际应用中可以根据实际情况设计相应的处理方法，如步态定位中，可以采用运动静止判断，如下图所示为步态数据运动静止判断矫正后的效果：

所以在应用中，IMU定位存在不可避免的漂移，需针对具体应用设计相应的处理算法。

由于大多数AGV在使用IMU的过程中，都是希望直接得到相对航向角Yaw,俯仰角Pitch,横滚角Roll，还有加速度的信息，班宁的IMU可以高精度的输出这些信息。

1.4 IMU实际数据

       以班宁的BNS系列IMU为例，提供6DoF自由度的姿态信息。

       IMU采用符合CAN输出特性，CAN总线数据速率250KHz，信号更新频率200Hz。同时支持485和SPI输出。消息格式报告数据，以16进制输出结果表示。

      以ID为F02D的加速度三轴方向的数据值及算法如下：

         IMU在静止状态下的部分连续数据记录如下：

       可以明显的看到低位值BL的数据存在变动，这就是即使在静止状态下也会有的零偏。这就需要进行标定固定零偏以及通过算法估计启动零偏的残差值和温漂。

       未标定的SLAM建图效果对比如下：

无IMU地图        有IMU地图（未标定）

       SLAM地图的好坏有很多因素的综合叠加影响，譬如建图的顺序，先闭环后细节， 激光的距离，移动物体的影响等，所以笔者认为结合激光和IMU各自的优点，取长补短融合传感建图，并在AGV行进中定位，利用IMU的特点帮助在高速运动和旋转中与地图匹配定位，并且能够进行多自由度的分析，及姿态的调整和极限位置保护。

1.5 视觉传感器和IMU传感器融合

       同SLAM发展过程类似，视觉融合IMU问题也可以分成基于滤波和基于优化两大类。

 同时按照是否把图像特征信息加入状态向量来进行分类，可以分为松耦合和紧耦合两大类。

       松耦合将视觉传感器和 IMU 作为两个单独的模块，两个模块均可以计算得到位姿信息，然后一般通过 EKF 进行融合。

     紧耦合则是指将视觉和 IMU 得到的中间数据通过一个优化滤波器进行处理，紧耦合需要把图像特征加入到特征向量中，最终得到位姿信息的过程。由于这个原因，系统状态向量最终的维度也会非常高，同时计算量也很大。

       总体来看，松耦合算法相对简单，且扩展性很强，可以加入多种传感器信息进行融合，但融合得到的位姿估计精度相对较差；紧耦合算法实现起来相对复杂，可扩展性差，但融合得到的位姿估计精度较高。

       总之，IMU和激光传感器的融合算法是核心，初期建图的策略及行进中的算法误差补偿也是至关重要的。

       附一下笔者实测班宁的IMU部分数据：

部分引用文章及出处：

————————————————

    1. 原文链接：https://blog.csdn.net/j05073094/java/article/details/54380188

    2. 原文链接：https://www.i-so.cn/agv-news-agvxcdyxt850/

    3. 资料参考：http://www.cnblogs.com/hitcm/p/6327442.html