基于视觉与陀螺仪组合的机器人粒子滤波定位*

叶泳骏，陈新度，吴 磊,刁世普,2

(1. 广东工业大学 a.广东省计算机集成制造重点实验室，广州 510006; b.省部共建精密电子制造技术与装备国家重点实验室，广州 510006; 2. 广东技术师范大学机电学院，广州 510665)

0 引言

基于SLAM导航[1-2]的自主移动机器人导航系统相较于传统的磁导航[3-4]，二维码导航[5]，反射板导航[6]等方式，拥有着无需改造导航环境，可自主规划路径等优势，已经广泛应用于多种室内移动机器人中。当前学者们已经从地图构建，定位，感知，路径规划与运动控制五大基础模块开展了大量的工作，来赋能移动机器人自主化，智能化。

众所周知，定位是机器人导航的最核心模块，其他模块需基于定位准确的条件来保证机器人稳定工作，而SLAM导航是利用外部传感器信息与先验地图数据关联和内部传感器信息自身估计，来实现机器人精确定位。因此为了提高自主移动机器人SLAM导航定位性能，开展地图与定位相关联的工作是十分有必要的。文献[7]提出了一种基于ORB[8]特征点的SLAM系统，利用相机采集图像来构建视觉特征点地图，从而实现重定位与连续位姿估计，但是该算法生成的地图不保存障碍信息，不能应用于机器人路径规划与避障当中。文献[9]提出了基于图优化的激光SLAM系统，构建出便于机器人路径规划的激光栅格地图，利用激光等传感器的数据关联实现定位，但是由于单线激光信息并不那么丰富，从而导致重定位效果不佳。文献[10]提出了一种基于ROS的包含建图、定位、路径规划的机器人导航框架，是少有综合多模块的开源工作，为机器人落地提供了许多实用的设计。

基于前人的研究基础上，本文提出了一种结合视觉与陀螺仪的室内移动机器人改进粒子滤波定位方法。该定位方法是在ROS导航框架[11]AMCL定位模块[12]的基础上实现，并改进其初始化定位效果不稳定与过程定位效果的不足。本文基于视觉与陀螺仪组合，通过利用数据关联与多传感器融合技术，提升定位效果，为机器人配送、运输等场景提供更稳定更准确的技术方案。

1 问题描述

AMCL定位模块实际应用中发现存在初始化与过程定位方面的问题。一方面是在室内和需要满足自主的条件下，靠模块内置的GPS或人为指定初始位置的方法是不理想的，而让粒子布满全图再慢慢收敛的内置方法又会因单帧激光信息不丰富导致很大可能会导致错误定位，如图1a情况。一方面是由于轮速计存在较大累积误差，原模块仅靠轮速计进行粒子群预测过程定位质量欠佳。在机器人导航过程中，特别是当机器人旋转时，会出现当前激光数据与地图存在重合度较差，定位质量较低的情况，如图1b情况。

(a)初始化定位问题 (b) 过程定位问题

2 改进的定位方法

ROS导航框架的定位模块AMCL，是一种自适应的粒子滤波方法，其一般使用轮速计与激光雷达两个传感器数据以及先验地图信息进行机器人的位姿估计。本文针对模块中的不足，在原方法基础上进行改进，如图2所示，其中黑色实线框部分为原方法概况，浅色虚线框部分为改进部分。对于模块中初始定位与过程定位方面的问题，本文提出对应的改进措施。一方面是通过获取视觉地图坐标，并地图坐标转换，从而提高初始化定位稳定性，另一方面是通过对视觉里程计、陀螺仪、轮速计分别得到的位姿估计进行卡尔曼滤波融合，从而提高过程定位质量。

图2 AMCL定位模块概况与改进部分

2.1 基于数据关联的初始化定位

2.1.1 获取视觉地图坐标

获取视觉地图坐标主要由词袋模型[13]与PNP(Pespective-N-Point)两个部分组成。定位系统通过相机最新获取的当前图像帧，根据词袋模型与地图中的关键帧进行图像匹配，得到当前帧特征点与三维地图点的对应关系。然后再通过PNP恢复出当前相机相对于视觉地图的变换关系，从而获取得到视觉地图坐标。

图像匹配中利用词袋模型分为两个步骤。第一是产生一个词袋，通过对大量的图片提取特征点，并对这些特征点进行k叉树聚类，得到的叶子节点则为每一个词。第二是使用词袋模型，图片可以通过词袋模型来进行编码，得到词向量，作为进行图片相似度与快速匹配的依据。词向量是关于词索引与其权重的向量，可以通过式(1)来判断两图像词向量v1与v2的相似度，得分越大则越相似。

(1)

当获取当前帧特征点与地图点的匹配关系，则可以根据2D-3D点的对应关系使用PNP算法来得到当前帧相机的位姿。先假设当前相机位姿李代数ξ与最相似图片的位姿一样，又已知相机内参K，地图点尺度si，地图点三维坐标Pi，对应的二维点像素坐标ui，通过把地图点投影到当前帧上，利用彼此重投影误差的最小二乘形式来构建代价函数,如式(2)，通过优化得到最优相机位姿。

(2)

2.1.2 地图坐标转换

如图2所示，改进方法采用了从ORBSLAM2[7]与Cartographer[9]得到的两个地图，即视觉的特征点地图[14]与激光的灰度栅格地图[15]，且两个地图为独立构建，后续需要采样拟合求取彼此变换关系，通过把获取得到的视觉地图坐标变换至机器人常用的灰度栅格地图中，方可使用。变换关系见式(3)：

GP=GTFFP

(3)

其中，GP为灰度栅格地图中的齐次坐标，FP为特征点地图上的齐次坐标，GTF为从特征点地图到灰度栅格地图的变换矩阵。

通过让机器人运动至均匀散布在活动范围的多个位置，根据第i位置从两SLAM系统得到的GPi与FPi是指代同一物理位置，两者之间差GTF变换的原因，可利用两组三维点对应关系计算式(4)～式(8)，从而得到GTF。

(4)

(5)

(6)

(7)

(8)

2.2 基于多传感器融合的过程定位

2.2.1 轮速计与陀螺仪运动估计

视觉里程计、陀螺仪、轮速计，皆可估计机器人的运动状态，得到机器人位姿变换。视觉里程计得到位姿的方法原理与前述一样。而轮速计与机器人运动机构相关，本文采用的是差速轮，即仅利用左右轮独立驱动实现前行与旋转的运动机构。在采样时间间隔Δt中，从两轮编码器中经比率换算得到的左右轮走过的距离dl与dr，已知L为左右轮距，(xk,yk,θk)为k时刻下机器人的运动状态，轮速计模型根据式(9)～式(11)求出(xk+1,yk+1,θk+1)的机器人状态。

(9)

(10)

(11)

(12)

(13)

2.2.2 卡尔曼滤波

对于上述各种的机器人的运动状态预测，都有各自的位姿估计，不过根据各个传感器的特性与数据获取方式，皆有对应的优劣，针对这些优劣，采用卡尔曼滤波数据融合的方法根据各数据的协方差对应加权，得到优势互补的最优位姿估计。

(14)

(15)

(16)

(17)

(18)

本文根据上述公式，通过协方差来加权得到较好的估计位姿。在位置x,y估计方面，视觉里程计与轮速计估计协方差设置一样，即结果各取一半。在姿态θ方面，把陀螺仪估计协方差设置很小，轮速计与视觉里程计估计协方差设置较大，即结果主要取自陀螺仪。

3 实验结果与分析

3.1 实验准备

本文实验使用的平台如图3a所示，包括一个差速轮自制底盘，一个SCIK的tim-561型号单线激光雷达，一个小觅智能的S1040型号双目相机，一个ALUBI的IMLPMS-ME1型号IMU和一台CPU为i7-9750H的戴尔笔记本。实验场地为一个回字形的室内办公场地，如图3b所示，在此场地进行关于初始化定位，过程定位，到点定位的3个实验。初始化实验则在该场地中机器人运动范围，即红色箭头线所示位置附近范围，对比测试两种方法的初始化定位平均成功率与平均定位时间。过程定位实验则是让机器人运动如图3b中红色箭头线所示，进行两种方法的定位质量比较。到点定位实验则是从系统最终到点定位精度来整体衡量改进效果。

(a) 实验平台 (b) 实验场地

3.2 初始化定位

该实验让机器人在活动范围均匀分布的位置进行初始化定位，对于改进方法则是让其当前帧与视觉特征点地图进行数据关联获取全局位置，对于原方法则是让AMCL中的粒子全局分布，经过旋转与激光采样对粒子收敛，获取全局位置。经过各40组实验测试，结果见表1，新系统的成功得到准确的初始化定位次数为38次，失败2次，成功率为95%，平均定位时间为2.67 s；旧系统成功得到准确的初始化定位次数为17次，失败23次，成功率为42.5%，平均定位时间为16.68 s。改进方法失败主要由于早晚靠窗处的光照强度差异过大，导致视觉特征点地图中的特征与当前帧的特征关联不上从而导致失败。

表1 初始化定位

3.3 过程定位

按照既定轨迹改进方法与原方法的定位质量对比如图4所示。

图4 过程定位质量图

该定位质量是单帧激光通过定位估计通过变换，看其激光点对于地图的重合度百分比来定义的。由数据可见，添加了视觉与陀螺仪对系统的定位效果有了明显的提高，而在40～70 s的区间内新系统的定位质量出现下降趋势，是因为该路段存在较多椅子与部分可移动的障碍物，在地图构建到测试的时间间隔中被挪动，从而出现定位质量效果下降，但整体仍然比原方法的效果要好。

3.4 到点定位

为了观察机器人系统整体的定位效果，以机器人重复到点定位精度作为指标对比改进前后的精度提升情况。实验过程先让机器人记录下某一指定地点，根据机器人底盘的铝合金板后侧与右侧作为标准，把贴纸贴在地面，以此作为标记，记作位置0，后续让机器人规划离开，再重新到该点，以同样方法作标记，两个系统各重复3组，一组10次，测量其x,y,θ上的误差，并统计其平均误差。

实验数据与改进方法实验效果见表2与图5。表2可见到点定位精度有一定幅度的提升，位置平均误差提高了10 mm～30 mm不等，角度平均误差提高1°左右。图5中改进方法试验效果中可见机器人到点定位的分布比较稳定，效果也不错。重复到点定位精度的提升反映出在初始化定位与过程定位的提升对系统整体的定位效果有一定帮助。

表2 到点定位精度对比

图5 第二组改进方法到点定位效果

4 结束语

本文针对现有AMCL模块的初始化定位稳定性以及过程定位精度的不足，提出了基于视觉与陀螺仪组合的移动机器人改进定位方法。该方法基于视觉数据关联，以提高初始化定位稳定性；基于卡尔曼滤波的传感器数据融合方法，为粒子群提供了更好的预测，以提高过程定位精度。实验结果表明，所提出的改进定位方法相较于已有方法，在定位稳定性和定位精度方面都有一定提升。因此，本文提出的移动机器人改进定位方法，在高精度鲁棒定位方面有良好的借鉴价值。