基于IMU在线校准的SLAM精度提升方法研究.pdf

2024 年第 3 期仪 表 技 术 与 传 感 器Instrument Technique and Sensor基金项目:中国博士后科学基金(2019M650095);国家自然科学基金项目(51905258);江苏省研究生科研与实践创新计划项目(SJCX23_1189)收稿日期:2023-08-29基于 IMU 在线校准的 SLAM 精度提升方法研究周霆威,乔贵方,刘 娣,付冬梅,张 颖南京工程学院自动化学院 摘要:基于多传感器融合的 SLAM 算法是当前移动机器人实现精准定位和导航的关键技术之一,在分析了惯性测量单元(IMU)的系统误差会对 SLAM 算法的建图和导航精度影响较大,并且 IMU 的系统误差会随工作时间发生变化的问题后,提出了一种易于实现的 IMU 在线标定方法。该标定方法首先建立了 IMU 中加速度计和陀螺仪的误差模型。其次通过二维云台实现 IMU 的姿态调整。然后通过读取加速度传感器的静态数值实现加速度计的误差参数辨识。最后以标定后的加速度计输出数据和陀螺仪计算的旋转矩阵,实现陀螺仪的误差参数辨识。为验证以上标定算法的效果,文中基于标定前后的 IMU 分别进行 SLAM 建图实验,实验结果表明,基于标定后 IMU 的建图精度更好。关键词:惯性测量单元;误差标定;即时定位与地图构建;移动机器人;在线校准中图分类号:TP242 文献标识码:AResearch on Improving Accuracy of SLAM Based on IMU Online CalibrationZHOU Tingwei,QIAO Guifang,LIU Di,FU Dongmei,ZHANG YingSchool of Automation,Nanjing Institute of TechnologyAbstract:SLAM algorithm based on multi-sensor fusion is one of the key technologies for mobile robots to realize accurate positioning and navigation.However,the systematic errors of the inertial measurement unit have a large impact on the mapping and navigation accuracy of SLAM algorithm.Besides,the systematic errors of IMU change with the working time.To solve this problem,this paper proposed an easy-to-implement online calibration method for IMU.The calibration method firstly established the error model of accelerometer and gyroscope in IMU.Secondly,the IMU attitude adjustment was realized by 2D gimbal.Third-ly,the error parameter identification of the accelerometer was realized by reading the static value of accelerometer sensor.Finally,the error parameter identification of the gyroscope was realized with the calibrated accelerometer output data and the rotation ma-trix calculated by the gyroscope.In order to verify the effect of the above calibration algorithm,this paper carried out SLAM map-ping experiments based on the IMU with and without calibration,respectively.The experimental results show that the accuracy of the mapping is better based on calibrated IMU.Keywords:IMU;error calibration;SLAM;mobile robot;online calibration0 引言随着机器人技术的发展,移动机器人可代替或协助人类完成各种困难工作1。SLAM 技术2是移动机器人实现精确建图,并进行路径规划的核心技术之一3。目前 SLAM 算法主要分为基于激光雷达的SLAM 算法与基于视觉相机的 SLAM 算法。但基于单一传感器的 SLAM 算法在建图和导航精度方面具有一定的局限性。随着传感器技术的发展,通过多传感器的数据融合能够实现高精度的 SLAM 算法。基于多传感器融合的 SLAM 算法主要有视觉与惯性测量单元(inertial measurement units,IMU)的融合 SLAM4、激光雷达与 IMU 融合 SLAM5以及激光雷达、视觉和IMU 融合 SLAM6等。以上算法中 IMU 提供精确的位姿估计,但 IMU 存在的系统误差与随机误差决定了其位姿估计精度,进而影响 SLAM 技术的建图精度。因此,在使用前需要对 IMU 进行精确标定,标定过程中会采用精密转台等相关设备进行 IMU 的误差测量。文献7中采用了多位置法对加速度计的零偏和尺度参数进行校准,但该校准过程需要在单轴转台上进行。文献8中利用三轴惯导测试转台进行了 IMU 六位置静态分立式标定,有效地提高了 IMU 的测量精度。但是上述标定方法均是利用外部精密设备对IMU 进行标定,同时标定过程较为复杂。文献9通过手动调整 IMU 的位姿来对其系统误差进行离线标1 仪 表 技 术 与 传 感 器第 3 期定,无需采用外部测量设备,但不适用于移动机器人平台的自动化运行。而 IMU 的系统误差和随机误差会随时间推移而发生变化,离线标定无法满足后期的精度要求。因此,本文提出了一种易于实现的在线校准方法,用于解决 IMU 长期使用过程中的误差问题。首先建立了 IMU 中加速度计与陀螺仪的误差模型;其次设计了 IMU 的在线两步标定方法,实现误差模型的参数辨识;最后通过实验验证本文提出的在线两步标定方法,并基于搭建的移动机器人平台验证 IMU 在线标定对 SLAM 建图精度的影响。1 误差模型的分析与构建理想情况下,IMU 中加速度计与陀螺仪为同一参考正交坐标系,但在制造过程中会引入不同的误差源,如非正交误差、比例因数误差和零位误差。本文将加速度计与陀螺仪的误差模型表示为:aI=TaKa(aS+ba+na)(1)I=TgKg(S+bg+ng)(2)式中:aI、aS分别为加速度计的实际值和测量值;I、S分别为陀螺仪的实际值和测量值;Ta、Tg分别为加速度计与陀螺仪的非正交误差的坐标轴偏移矩阵;Ka、Kg分别为加速度计与陀螺仪的比例因数误差的缩放矩阵;ba、bg和 na、ng分别为加速度计与陀螺仪的零位误差和噪声。非正交误差如图1 所示,因此,非正交坐标系与正交坐标系之间的转换矩阵为T=1-yzzyxz1-zx-xyyx1(3)式中 ij表示加速度计或陀螺仪在实际坐标系 i 轴绕着理想坐标系 j 轴的旋转。图 1 IMU 内部传感器理想坐标系(xI,yI,zI)与实际坐标系(xS,yS,zS)本文分别定义了 Fa为非正交加速度计坐标系、FOa为正交加速度计坐标系、Fg为非正交陀螺仪坐标系、FOg为正交陀螺仪坐标系、FI为理想 IMU 坐标系。在此基础上,为运算方便,本文定义将 Fa的 x 轴与 FOa的 x 轴重合,并将 Fa的 y 轴位于 FOa的 x-y 平面内,此时假设 FOa与 FI坐标系重合,则 Fa与 FI之间的转换矩阵为Ta=1-yzzy01-zx001(4)式中利用 ij来代替一般情况下的 ij,表示加速度计旋转的角度量。陀螺仪的测量与加速度计参考相同坐标系,则 Fg与 FI之间的转换矩阵为Tg=1-yzzyxz1-zx-xyyx1(5)式中利用 ij来代替一般情况下的 ij,表示陀螺仪的旋转角度量。误差模型中比例因数误差矩阵 Ka与 Kg的定义分别为:Ka=sax000say000saz(6)Kg=sgx000sgy000sgz(7)式中:sai、sgi分别为加速度计与陀螺仪关于 i 轴上测量的数字量与实际物理量的比例因子。加速度计和陀螺仪的零位误差 ba与 bg定义分别为:ba=bax bay bazT(8)bg=bgx bgy bgzT(9)式中 bai、bgi分别为加速度计或陀螺仪关于 i 轴上的零位误差。2 IMU 标定方法IMU 标定主要分为 2 步:首先辨识加速度计的误差参数,然后基于加速度计的标定结果实施陀螺仪传感器的误差参数辨识。2.1 加速度计标定方法基于上述的 IMU 误差模型,加速度计待辨识的误2 第 3 期周霆威等:基于 IMU 在线校准的 SLAM 精度提升方法研究 差参数向量 acc如下:acc=yz,zy,zx,sax,say,saz,bax,bay,bazT(10)在标定过程中,本文利用滑动窗口对窗口内的数据进行均值计算,可忽略噪声值的影响,将误差模型修改为aI=h(aS,acc)=TaKa(aS+ba)(11)加速度计标定数据测量过程如下:1)将 IMU 静置 Tinit=50 s,并计算 Tinit时间的测量方差 init;2)调整 IMU 姿态,并保持静止 twait=3 s;3)设定 Twindow=1 s 的滑动窗口,并计算该窗口内的均值 aSt和方差 t;4)如果 tinit,则将 aSt加到测量数据中,否则放弃该段数据;5)重复步骤 2)步骤 4),采集 N=40 段数据;6)利用 L-M 算法求解 acc。当 IMU 处于静止时,加速度计向量的模长与重力加速度值相等。因此,通过多次改变 IMU 的姿态,可根据重力加速度的测量值与实际值构建代价函数:L(acc)=Nk=1(g2-h(aSk,acc)2)2(12)式中g为该地重力矢量的实际大小。2.2 陀螺仪标定方法本文利用 Allan 方差对陀螺仪的零位误差进行标定10,其中零位误差一般取 Allan 方差曲线底部平坦区域的最小值,噪声误差同样进行忽略。其余误差参数采用非线性优化方法进行辨识,构造陀螺仪误差参数向量 gyro如下:gyro=yz,zy,xz,zx,xy,yx,sgx,sgy,sgzT(13)陀螺仪标定数据的测量过程如下:1)将 2 段静态数据中的加速度计方向向量均值作为初始值和终值;2)旋转过程中,对陀螺仪输出数据进行积分,计算得到两端静态数据之间的旋转矩阵;3)利用加速度计方向向量初始值和旋转矩阵计算加速度计方向向量终值的预测值;4)重复步骤 1)步骤 3),采集 N=40 段数据;5)使用 L-M 算法辨识 gyro。陀螺仪的数据测量是以标定后的加速度计输出值为参考真值。通过多次改变 IMU 的姿态,可根据加速度计输出值和陀螺仪旋转矩阵,定义如下代价函数:L(gyro)=Nk=1gak-Si,gyro,gak-12(14)式中:gak为第 k 段数据中加速度计方向向量的均值;为利用静态数据之间陀螺仪数据的积分计算旋转矩阵与上一时刻的重力加速度向量的乘积。3 实验验证及结果分析3.1 移动机器人平台搭建本文采用的移动机器人为两轮差分结构,移动机器人上安装主控单元 Raspberry Pi 4B、二维激光雷达RPLIDAR A2、二轴舵机云台和 IMU 模块 BWT901CL。二维激光雷达 RPLIDAR A2 的测量半径为0.212 m,角度分辨率为 0.45,扫描频率为 10 Hz。IMU 模块BWT901CL 的采样频率为 0.2200 Hz。二轴舵机云台的控制分辨率为 0.088。以上器件的安装如图 2所示。图 2 移动机器人平台搭建IMU 模块 BWT901CL 安装在轴舵机云台上,X 轴正向为移动机器人的前行方向,X-Y 平面与移动机器人上平面平行。3.2 IMU 的误差参数辨识根据前文所述的标定方法,将 IMU 频率设置为100 Hz。为实现移动机器人 IMU 的在线校准,本文通过二轴舵机云台调节 IMU 模块的姿态,并根据上述步骤构建出待标定的原始数据集,即可实现无需拆装的IMU 在线校准功能。依据静置 Tinit时段内数据的方差值完成静态检测,利用静态检测器分析并筛选出数据集中相应数据,具体实现为:若某段数据的方差小于该值,则检测器显示为高电平,定义为静止段;反之则显示为低电平,定义为动态段,该检测结果如图 3 所示。数据集中 X、Y、Z 各轴输出数据(部分)如图 4 所示。通过静态检测器能够较好地避免二轴舵机云台调节 IMU 模块姿态所产生的误差。从而获得用于加速度计误差参数标定的观测均值 aSi,部分数据如表 1所示,并利用 L-M 优化算法计算得到的加速度计参数误差如表 2 所示。3 仪 表 技 术 与 传 感 器第 3 期表 1 标定校准过程数据处理(部分)静态数据段均值 aSi静态数据段均值 aSi+1旋转矩阵 R(观测值)-0.534 710 0.691 854 9.762 830-0.572 171-9.676 360-1.471 6300.972 523-0.211 437 0.097 4310.036 574-0.274 552-0.960 8760.229 915 0.938 038-0.259 275-0.572 171-9.676 360-1.471 630-9.666 210 0.563 702-1.528 740 0.082 0300.994 664-0.062 559-0.996 4870.082 918 0.011 728 0.016 8520.061 378 0.997 972-9.666 210 0.563 702-1.528 740 0.312 098 9.762 940-0.813 662 0.140 3870.989 996-0.014 099-0.989 5290.139 810-0.035 851-0.033 5220.018 9850.999 258 0.312 098 9.762 940-0.813 662 9.678 610-0.355 348-1.489 890 0.089 693 0.995 7710.019 861-0.995 530 0.089 0430.031 478 0.029 577-0.022 5950.999 307表 2 IMU 标定后的参数误差非正交误差比例因数误差零偏误差加速度计Ta=1-0.000 609 9690.002 655 920010.000 762 083001Ka=1.002 440001.002 530001.001 35ba=-0.136 491 0.041 374-0.023 922陀螺仪Tg=10.064 533 50.037 881 2-0.061 774 810.026 452 6 0.067 361 70.089 289 41Kg=1.006 320001.035 770001.013 91bg=-0.028 206-0.051 448-0.010 394图 3 静态检测器检测结果 在完成加速度计标定后,将数据 aSi进行误差校正后再用于陀螺仪的误差模型辨识,除此以外,根据静态段中陀螺仪的积分数据计算得到的旋转矩阵 R,如表 1 所示,将以上数据代入 L-M 优化算法计算得到陀螺仪误差模型的参数误差,如表 2 所示。3.3 SLAM 地图构建精度对比 为验证 IMU 标定结果对于 SLAM 建图精度的影响,本文基于以上移动机器人构建的实验场景如图 5所示,该实验为 8 m8 m 正方形场地。为了更好地对比建图精度,控制移动机器人从 A点依次按照图5 中的逆时针和顺时针在实验场景中移动,并记录相关传感器的数据,构建移动机器人 SLAM(a)加速度计 X 轴误差标定采集数据(部分)(b)加速度计 Y 轴误差标定采集数据(部分)(c)加速度计 Z 轴误差标定采集数据(部分)图 4 数据集中 X、Y、Z 轴部分输出数据数据包。本文采用 Cartographer 算 法 进 行 地 图 构建11,并在算法中将 IMU 数据、二维激光雷达的点云4 第 3 期周霆威等:基于 IMU 在线校准的 SLAM 精度提升方法研究 图 5 SLAM 建图实验环境搭建数据与里程计数据进行融合,算法流程如图6 所示,在该算法中 IMU 的输出数据需要通过在线校准后再与其他传感器进行数据融合。图 6 基于 Cartographer 的融合建图结构框图在对比实验中,分别采用原始的 IMU 输出数据和校正后 IMU 输出数据进行 SLAM 建图,通过 Cartogra-pher 算法获得的最终地图如图 7 所示。从图 7(a)中可以看出,用未标定的 IMU 构建的地图在四边都存在一定的错位畸变现象,同时变现也较为模糊。而从图7(b)中可以看出,用标定后的 IMU 构建的地图边界较为平整、清晰,建图效果相对图 7(a)更好,因此,可以得到经过标定的 IMU 能够有效地提高移动机器人的建图精度。(a)标定前(b)标定后图 7 IMU 标定前后建图效果对比 4 结论本文针对移动机器人的 SLAM 建图精度展开研究,提出了一种易于实现的 IMU 在线标定方法。该方法不依赖外部测量设备,仅需移动机器人通过二维舵机云台进行多个姿态调节,即可实现 IMU 中加速度计和陀螺仪误差模型参数的精确辨识,较大程度地提高了 IMU 的标定效率。最后为了验证本文提出的 IMU在线标定方法对于 SLAM 建图精度的提升效果,基于搭建的移动机器人硬件平台和 Cartographer 算法分别对标定前和标定后的二维地图精度进行对比,结果表明,经过标定后的 IMU 能够有效地提升建图精度,从而也说明了本文提出的 IMU 在线标定算法具有较好的标定精度。参考文献:1 曹风魁,庄严,闫飞,等.移动机器人长期自主环境适应研究进展和展望J.自动化学报,2020,46(2):205-221.2 TAHERI H,ZHAO C.SLAM;definition and evolutionJ.Engineering Applications of Artificial Intelligence,2021,97:104032.3 HICHRI B,GALLALA A,GIOVANNINI F,et al.Mobile ro-bots path planning and mobile multirobots control:A reviewJ.Robotica,2022,40(12):4257-4270.4 CAMPOS C,ELVIRA R,RODRIGUEZ J J G,et al.Orb-slam3:An accurate open-source library for visual,visual-in-ertial,and multimap slamJ.IEEE Transactions on Robot-ics,2021,37(6):1874-1890.5 DEBEUNNE C,VIVET D.A review of visual-LiDAR fusion based simultaneous localization and mappingJ.Sensors,2020,20(7):2068.6 WISTH D,CAMURRI M,DAS S,et al.Unified multi-modal landmark tracking for tightly coupled lidar-visual-inertial odometryJ.IEEE Robotics and Automation Letters,2021,6(2):1004-1011.7 SYED Z,AGGARWAL P,GOODALL C,et al.A new multi-position calibration method for MEMS inertial navigation sys-tems J.Measurement Science and Technology,2007,18(7):1897-1907.8 赵桂玲,高伟,李仔冰,等.高精度光纤陀螺组件标定方法J.沈阳工业大学学报,2012,34(2):209-214;230.9 TEDALDI D,PRETTO A,MENEGATTI E.A robust and easy to implement method for IMU calibration without external equip-ments C/2014 IEEE International Conference on Robotics and Automation(ICRA).IEEE,2014:3042-3049.10 马群,王庆,阳媛,等.基于 Allan 方差的 MEMS 陀螺仪随机误差辨识与抑制J.传感器与微系统,2019,38(6):62-65.11 DWIJOTOMO A,ABDUL RAHMAN M A,MOHAMMED ARIFF M H,et al.Cartographer slam method for optimiza-tion with an adaptive multi-distance scan schedulerJ.Applied Sciences,2020,10(1):347.作者简介:周霆威(1999),硕士研究生,主要研究方向为多传感器融合的 SLAM 技术。E-mail:通信作者:乔贵方(1987),副教授,硕士生导师,博士,主要研究方向为工业机器人测试与标定、机器人仿生控制技术。E-mail:qiaoguifang 5