粒子滤波与ORB特征检测结合的移动机器人定位算法*

2019-06-25肖宇峰

传感器与微系统 2019年7期

黄鹤，肖宇峰，刘冉，张华

(西南科技大学信息工程学院特殊环境机器人技术四川省重点实验室，四川绵阳 621010)

0 引言

自主移动机器人技术主要包括定位、地图构建与路径规划。定位是机器人自主移动的首要问题，也是实现地图构建和路径规划的关键。贝叶斯滤波器是概率定位方法的理论基础，其核心思想是利用机器人收集的历史数据来估计状态空间的后验概率密度[1]。基于概率定位算法主要有卡尔曼滤波、马尔可夫定位、多假设跟踪和粒子滤波。其中粒子滤波是使用最广泛的定位算法，具有实现简单、运算量低、定位精度高等优点。粒子滤波定位算法核心是通过若干个加权随机粒子来表达后验概率密度，适用于非线性非高斯系统[1]。随着计算机视觉的发展，基于图像局部特征点检测定位算法逐渐达到实时。ORB是在FAST(features from accelerated segment test)[2]特征基础上，由Rublee E等人在2011年提出的一种快速特征点提取和描述的算法，具有旋转不变性、尺度不变性和较高实时性等特点[3]。文献[4]提出基于SURF(speeded up robust features)的视觉定位算法，通过单应矩阵和投影约束信息来降低时间复杂度。机器人绑架问题是指机器人经过一个未知运动移动到其它位置后的重定位问题[5]。文献[6]中基于机器人在两个连续时间中所看到的环境的相似性来检测机器人是否被绑架。文献[7]中提出基于传感器读出两个连续时间帧机器人的位移是否正常来检测机器人是否被绑架。

传统粒子滤波算法不能有效解决绑架问题。基于粒子滤波的自适应蒙特—卡罗定位算法在全局随机撒粒子，通过激光雷达扫描周围环境来匹配先验地图使粒子收敛来实现重定位。但当场景较大时面临粒子收敛速度缓慢，重定位效率不高的问题。另一种方法是通过增加其它传感器来对机器人进行定位。文献[8]提出基于反向传播(back propagation,BP)神经网络的WiFi指纹定位与粒子滤波复合的全局定位方法。

本文在粒子滤波定位算法基础上，通过加入环境中的相机观测结果来校正绑架后由运动预测产生误差较大的粒子。相机基于ORB特征匹配检测到机器人后，根据相机与地图的关联关系来确定机器人所在的子地图。在子地图区域中撒粒子通过粒子滤波器收敛后实现重定位。在Gazebo仿真平台上的实验表明，本文算法能够较好地解决机器人绑架问题，且比AMCL粒子收敛速度更快、重定位误差更低。

1 平台构架

本文方法基于安装视频监控系统的公共服务环境，比如车站、大型超市等场所。方法运行平台除了机器人外还包括监控相机、图形工作站、无线网络,如图1所示。

图1 粒子滤波与ORB特征检测结合的移动机器人定位方法平台构架

相机的栅格地图模型如图2所示。

图2 相机—栅格地图模型

全局栅格地图表示为M,第i个相机视域对应的地图表示为Mi，则有如下公式

M=M1∪M2∪M3∪…ML

(1)

所有相机的栅格地图加起来构成全局栅格地图。对于机器人来说，传感器越多则感知周围环境的能力越强。但是传感器所带来的花销与机器人占有的有限资源相矛盾。每增加一个传感器对于电池的续航能力以及数据处理的实时性来说都是巨大的考验。本文所提出的构架很好地解决了这一矛盾，引入相机传感器但是把计算开销放置在机器人外围，把图像处理后的结果通过无线网络发送至机器人端。通过相机栅格地图作为桥梁将图像处理的结果与粒子滤波算法结合起来。因为图像检测与粒子滤波是并行进行的，所以能提升了定位的效率。

2 基于ORB特征匹配的机器人检测

通过环境中的相机观测到机器人是重定位的第一步。相机图像与机器人基于ORB特征进行匹配，当匹配点对超过设定的阈值认为检测到了机器人。考虑到相机不会移动，当相邻两帧图像一致时认为机器人没有被突然移动到此区域。通过帧间差法对比前后两帧图像是否有变化，当有变化时才进行ORB特征提取与匹配。这种方式可以降低运算量。

ORB特征提取是由FAST算法发展来的，特征点描述是根据BRIEF(binary robust independent elementary features)[10]特征描述算法改进的。ORB特征是将FAST特征点的检测方法与BRIEF特征描述子结合起来，并在原来的基础上做了改进与优化。

BRIEF算法提取的描述子是一个二进制串。对于特征点的邻域空间p其二进制测试τ定义如下

(2)

式中p(x)为x点的灰度值。BRIEF描述子可表示为如下的n维的二进制串

(3)

式中x和y服从以特征点为中心的高斯分布，n选取256，在文献[10]中被证明是最好的。但BRIEF描述子不具备旋转不变性，ORB对其进行了改进，提出了Steered BRIEF。对特征点31×31邻域内选取对点集，定义一个2×n的矩阵S

(4)

利用角度θ和相应的旋转矩阵Rθ，对矩阵S构造一个具有旋转性的Sθ

(5)

Sθ=RθS

(6)

Steered BRIEF描述子可表示为

gn(p,θ)=fn(p)|(xi,yi)∈Sθ

(7)

Steered BRIEF方法得到的特征描述子具有旋转不变性，同时也降低了特征点邻域内匹配点对的相关性。为了解决这一问题，提出了rBRIEF[9]。通过贪婪搜索算法找到相关性最小的256种点对，得到rBRIEF描述子。

ORB特征描述子是以二进制形式存储，因而通过计算汉明距离(Hamming distance)来进行匹配[10]。汉明距离越小则表明相似程度越高。这种匹配方式存在误匹配，通过PROSAC (progressive sample consensus)算法可以剔除误匹配点[11]。PROSAC算法首先生成一个具有较高匹配概率的子集，然后从子集中抽样计算，这样最优参数采样会较早出现[12]。

3 粒子滤波与ORB特征检测结合的定位方法

在基于概率的机器人定位中的一个关键概念是置信度(belief)，它反映了机器人有关环境状态的内部信息。粒子滤波定位通过随机化粒子(particle)来表征置信度bel(xt)。粒子滤波的核心思想是用一系列从后验概率得到的随机状态采样表示bel(xt)。

(8)

式中z1∶t为1～t时刻的观测量，u1∶t为1～t时刻的控制量，m为环境地图。基于粒子滤波算法的定位是根据后验分布的N个有权值的样本集来表示后验置信度bel(xt)。样本密度越大表示样本代表着机器人位置的概率越大

(9)

由于机器人被绑架的位姿是不确定的，即被绑架后的当前位姿与上一时刻的位置不具相关性，因此,根据运动模型预测的粒子其权重会变得非常低。经过滤波后得到的位姿与真实位姿相差较大。针对绑架问题本文改进了粒子滤波算法的运动预测阶段，通过引入相机观测的结果来修正基于运动模型所预测的粒子。机器人绑架后重定位流程如图3所示。

图3 机器人绑架后重定位流程

1)相机观测：通过帧差法来确定存在运动目标的相机群Ci(i=1,2,…,N)，基于ORB特征匹配确定相机ci检测到机器人。相机视野覆盖整个环境，若Ci中没有匹配到机器人，则在余下的相机中进行匹配。

2)初始化粒子：相机检测到机器人后通过无线网络将信息发送给机器人端。每一个相机ci关联一个栅格地图mi。在mi中初始化高斯分布的粒子集Xt。

3)粒子滤波：基于激光雷达扫描数据匹配地图来计算Xt中粒子得分。选择得分最高的粒子作为机器人位姿。通过重采样舍弃评分低的粒子，对评分高的粒子复制，保持粒子群数量基本不变。

粒子滤波定位算法中基于运动模型的粒子无法准确预测机器人绑架后的位置。本文通过引入视觉检测方法来解决非正常运动情况下的预测问题。相机基于ORB特征匹配检测到机器人后，将其粗定位在一个区域。根据相机与地图的关联关系，在对应的子地图中撒粒子并进行粒子滤波定位，最终实现机器人重定位。

4 仿真

实验在机器人操作系统(robot operating system,ROS)和Gazebo平台上进行仿真。ROS版本为Indigo，Gazebo版本为7.0，电脑系统是Ubuntu14.04。ROS是一个开源的机器人操作系统，遵循BSD开源许可协议。Gazebo是一个三维的机器人仿真软件，能够模拟真实实验环境。移动机器人模型如图4所示，由Kobuki底盘和HOKUYO激光雷达组成。在Gazebo中环境模型如图5(a)所示，长17.6 m,宽16.6 m，14个相机的视域覆盖整个环境。图5(b)是基于ROS系统下的slam_gmapping建立的栅格地图。相机对应的栅格子地图如图6所示。

图4 移动机器人模型

图5 Gazebo仿真环境建模

图6 相机关联的栅格子地图

图7(a)是基于汉明距离的匹配结果，图中左边为给定的机器人图像，右边是相机中机器人的图像，连线表示描述子点对的匹配。图中一些交叉的地方是误匹配的结果。图7(b)是PROSAC算法剔除误匹配后的结果。当机器人一部分出现在相机视域中，由图7(c)可以看出仍然具有较好的匹配结果。图7可以看出,ORB特征具有旋转不变性和尺度不变性。

图7 机器人的ORB特征匹配

机器人绑架实验结果如图8所示。将其由初始位置绑架至左下方。

图8 机器人绑架仿真实验对比

为了使粒子能够覆盖机器人真实位置AMCL需要在全局撒粒子如图8(d)所示。而本文算法通过相机观测到机器人后，在相机关联地图内生成粒子如图8(a)所示。撒粒子范围由全局缩小至局部。每一个粒子存储一个位姿信息，因而粒子数目越多需要的存储空间越大。激光雷达扫描周围环境并与地图匹配，并对每一个粒子计算评分。粒子评分越高说明越接近真实位姿。每一次迭代保留评分高的粒子并丢掉评分低的粒子。粒子数量逐渐减少并且粒子逐渐收敛如图8(b)和(e)所示。粒子越多则计算开销就越大，收敛速度越慢。

重定位时间对比如图9(a)所示。相比AMCL本文方法有效地缩短了收敛时间。在相似度高的环境下，可能会出现激光数据与地图的误匹配，这会导致靠近机器人真实位置的粒子得分反而较低。本文通过引入相机观测的结果较大地缩减撒粒子区域，减小了由于激光误匹配所带来的影响，具有较低的定位误差。图9(b)是重定位后定位误差对比，本文方法的定位误差低于AMCL。