CN103292804A - 一种单目自然视觉路标辅助的移动机器人定位方法 - Google Patents

Abstract

本发明公开了一种单目自然视觉路标辅助的移动机器人定位方法。首先对导航环境中的若干位置预先建立自然视觉路标特征库。机器人在利用惯性导航系统定位过程中，同时对采集到的单目图像和库中的视觉路标进行匹配。建立了基于GIST全局特征和SURF局部特征相结合的在线图像快速匹配框架，同时结合基于单目视觉的运动估计算法修正车体航向。最后利用Kalman滤波将视觉路标匹配获得的定位信息和惯性导航系统解算获得的定位信息有效地融合起来。本发明能在GPS受限的情况下具备良好的定位精度和鲁棒性；能有效修正惯性导航因噪声引起的误差；使用单目视觉大大减少了运算量。

Description

一种单目自然视觉路标辅助的移动机器人定位方法

技术领域

本发明涉及惯性导航和图像处理方法，特别是涉及一种单目自然视觉路标辅助的移动机器人定位方法。

背景技术

传统的机器人定位包括GPS、惯性导航系统等，GPS用途广泛、精度高，但受环境影响大，惯性导航系统完全自主，频率高但噪声影响大；而GPS/惯性导航系统融合定位是当今机器人导航中最常采用的组合导航定位方法之一，可以有效利用各自的优势，取长补短。但在实际应用中，很多场合下GPS信号会受到遮挡而无法使用，导致系统整体定位精度下降很快。

为了解决上述问题，目前使用的大多数方法都在寻找GPS之外的可靠定位手段。

有一类是视觉方法，它作为传统定位方法的有效补充得到越来越多的关注。如视觉里程计，它通过前后帧图像相匹配的特征点在三维空间的位置变化来估计车体运动。它一般需要双目立体相机才能获得较高的定位精度，成本较高，而单目则只能适用于路面是平坦的情况。同时定位和地图创建（SLAM）是通过机器人上携带的视觉或者激光传感器，通过概率算法实现未知环境下的定位。这类方法的优点是不需要任何环境先验信息，缺点是计算量大，实时定位有一定困难，对特征较少的环境定位精度较低。

还有一类是视觉与IMU的融合：（1）IMU与SLAM融合，IMU用来定位预测，根据图像特征点与机器人的相对位置修正定位结果，但运算量大，实时性差；另一类是IMU与视觉里程计(VO)的融合，利用VO修正IMU累积误差，但两者均为局部定位，VO的累积误差也会引入系统中。

以上这些非GPS的定位方法从理论上说都是局部的，误差还是会随着距离增加而累积。

发明内容

本发明的目的在于提供一种单目自然视觉路标辅助的移动机器人定位方法。建立了基于GIST全局特征和SURF局部特征相结合的在线图像快速匹配框架，并使用基于单目视觉的运动估计算法修正车体航向。最后利用Kalman滤波将视觉路标匹配获得的定位信息和惯性导航系统融合起来，提高了定位精度。

本发明采用的技术方案的步骤如下：

1）用单目相机采集区域内的场景图像作为自然视觉路标，提取场景图像的GIST全局特征和SURF局部特征，获取采集路标时单目相机和车体的姿态，构建视觉路标全局特征库和局部特征库；

2）移动机器人在行进过程中获取新图像，参考惯性导航系统的定位信息，将当前图像与自然视觉路标进行匹配，使用全局特征作为初步的过滤，并在此基础上使用局部特征进行精确匹配；

3）当前图像与自然视觉路标实现正确匹配定位后，因为机器人当前时刻的实际航向和路标指示的航向存在偏差，故采用基于单目图像的运动估计算法，计算当前图像与自然视觉路标间的航向与姿态偏差，获得机器人当前时刻可靠的定位信息；

4）将惯性导航系统与自然视觉路标辅助定位信息进行融合，惯性导航系统和里程计作为机器人主要的导航方式，基于自然视觉路标的绝对定位方法作为辅助导航方式，对惯性导航系统数据进行修正，惯性导航系统与自然视觉路标的融合包括位置和姿态两个方面的融合，由于相邻两帧间场景非常接近带来匹配模糊性，因此存在一个定位噪声方差；场景接近的前后两帧图像间姿态的变化很小，因此认为所求得的姿态可靠，直接作为当前姿态输出，而对位置信息则通过Kalman滤波方式进行融合。

所述步骤1)中，用单目相机采集区域内的场景图像作为自然视觉路标，提取场景图像的GIST全局特征和SURF局部特征，获取采集路标时单目相机和车体的姿态，构建视觉路标全局特征库和局部特征库；包括如下步骤：

1）首先采集区域内的场景图像作为自然视觉路标，自然视觉路标的选择应遵循三个原则：（1）具有丰富的特征信息；（2）采集该自然视觉路标时的单目相机位置和方向已知；（3）自然视觉路标之间的间隔为50米～100米，路标间隔低于50米时，会增大路标匹配的工作负担，降低匹配正确率；间隔超过100米时，则不能及时发挥定位修正的作用；

2）提取自然视觉路标的全局特征，GIST描述符是利用不同方向和尺度的Gabor滤波器对图像进行采样滤波，滤波后图像输出到4*4的网格中，每个网格取平均值；

3）提取自然视觉路标的SURF局部特征，SURF算子对尺度缩放、平移、旋转变换保持不变，即使在图像目标受到部分遮挡的情况下也能检索，同时对图像的亮度变化和仿射变换具有不变性，提取图像的SURF关键点能准确地刻画图像的局部内容特征；

所述步骤2)中，移动机器人在行进过程中获取新图像，参考惯性导航系统的定位信息，将当前图像与自然视觉路标进行匹配，使用全局特征作为初步的过滤，并在此基础上使用局部特征进行精确匹配；包括如下步骤：

将当前图像与自然视觉路标进行匹配，使用匹配成功的SURF关键点数占关键点总数的百分比结合尺度信息作为指标，应用投票机制来衡量当前图像与自然视觉路标的相似度，匹配的SURF数目的百分比记为N1，匹配的SURF点对中处在同一尺度的SURF点数目的百分比记为N2，则图像的相似度投票计分为

N=m1*N1+m2*N2         (1)

其中m1、m2为权重系数，m2>m1>0，当机器人在接近路标位置时，投票分数也越来越高，当过了路标位置时，投票数随即下降，赋予票数最高者相应路标的位置信息。

所述步骤3)中，当前图像与自然视觉路标实现正确匹配定位后，因为机器人当前时刻的实际航向和路标指示的航向存在偏差，故采用基于单目图像的运动估计算法，计算当前图像与自然视觉路标间的航向与姿态偏差，获得机器人当前时刻可靠的定位信息；包括如下步骤：

基于本质阵恢复运动参数，使用已经过标定的单目相机，提取采集的图像中的SURF特征，与自然视觉路标进行特征匹配，应用八点法求解本质矩阵，为减少噪声的影响，使用Ransac去除离群点，结合最小二乘准则获解本质矩阵，然后对本质矩阵进行奇异值分解：

E=UΣV^T         (2)

由本质矩阵和旋转矩阵的关系，可得两图像间旋转矩阵的求解公式如下：

$\begin{array}{c}{R}_1={\mathrm{UW}}^T{V}^T\\ R_2={\mathrm{UWV}}^T\end{array}W=\left(\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right)---\left(3\right)$

基于被测点位于相机前方的事实以及相机的位置关系可选出R的唯一正确解；由旋转矩阵R和姿态角的关系可得到两幅图像间的三个姿态角的偏差：

Δθ=arcsin(R(1,3))          (4)

Δψ=arctan(R(1,2)/R(1,1))

其中Δθ、Δψ分别表示两图像间横滚角、俯仰角、航向角之差；结合路标已知的航向和姿态，即得到当前位置准确的姿态信息。

与背景技术相比，本发明具有的有益效果是：

1）因为本发明基于视觉算法，所以能在GPS受限的情况下精确定位，算法鲁棒。

2）因为本发明使用视觉路标信息，所以能有效修正惯性导航因噪声引起的误差。

3）因为本发明使用单目视觉，所以运算代价低。

附图说明

图1基于单目自然视觉路标辅助的移动机器人定位流程图。

图2移动机器人的实验定位轨迹。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

如图1所示，基于单目自然视觉路标辅助的移动机器人定位方法包括如下步骤：

1）用单目相机采集区域内的场景图像作为自然视觉路标，提取场景图像的GIST全局特征和SURF局部特征，获取采集路标时单目相机和车体的姿态，构建视觉路标全局特征库和局部特征库；

采集区域内的场景图像作为自然视觉路标，自然视觉路标的选择应遵循三个原则：（1）具有丰富的特征信息；（2）采集该自然视觉路标时的单目相机位置和方向已知；（3）自然视觉路标之间的间隔为50米～100米，路标间隔低于50米时，会增大路标匹配的工作负担，降低匹配正确率；间隔超过100米时，则不能及时发挥定位修正的作用；

GIST描述符是利用不同方向和尺度的Gabor滤波器对图像进行采样滤波，滤波后图像输出到4*4的网格中，每个网格取平均值，本发明使用8个方向4个尺度的滤波器采样，得到（4*4）*8*4=512的多维特征向量；SURF算子对尺度缩放、平移、旋转变换保持不变，即使在图像目标受到部分遮挡的情况下也能检索，同时对图像的亮度变化和仿射变换具有不变性，提取图像的SURF关键点能准确地刻画图像的局部内容特征；

2）移动机器人在行进过程中获取新图像，参考惯性导航系统的定位信息，将当前图像与自然视觉路标进行匹配，使用全局特征作为初步的过滤，并在此基础上使用局部特征进行精确匹配；

将当前图像与自然视觉路标进行精确匹配时，记匹配成功的SURF关键点数为n_s，关键点总数为n_t，使用N1=n_s/n_t结合尺度信息作为指标，应用投票机制来衡量当前图像与自然视觉路标的相似度，匹配成功的SURF点对中处在同一尺度的SURF点数目的百分比记为N2，则图像的相似度投票计分为

N=m1*N1+m2*N2              (1)

其中m1、m2为权重系数，m2>m1>0，当机器人在接近路标位置时，投票分数也越来越高，当过了路标位置时，投票数随即下降，赋予票数最高者相应路标的位置信息；

3）当前图像与自然视觉路标实现正确匹配定位后，因为机器人当前时刻的实际航向和路标指示的航向存在偏差，故采用基于单目图像的运动估计算法，计算当前图像与自然视觉路标间的航向与姿态偏差，获得机器人当前时刻可靠的定位信息；

运动估计算法基于本质阵恢复运动参数，使用已经过标定的单目相机，提取采集的图像中的SURF特征，与自然视觉路标进行特征匹配，应用八点法求解本质矩阵，为减少噪声的影响，使用Ransac去除离群点，结合最小二乘准则获解本质矩阵，然后对本质矩阵进行奇异值分解：

E=UΣV^T         (2)

由本质矩阵和旋转矩阵的关系，可得两图像间旋转矩阵的求解公式如下：

$\begin{array}{c}{R}_1={\mathrm{UW}}^T{V}^T\\ R_2={\mathrm{UWV}}^T\end{array}W=\left(\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right)---\left(3\right)$

因为被测点位于相机前方的事实以及相机的位置关系可选出R的唯一正确解；由旋转矩阵R和姿态角的关系可得到两幅图像间的三个姿态角的偏差：

Δθ=arcsin(R(1,3))                 (4)

Δψ=arctan(R(1,2)/R(1,1))

其中

Δθ、Δψ分别表示两图像间横滚角、俯仰角、航向角之差；结合路标已知的航向和姿态，即得到当前位置准确的姿态信息。

实施例：

实验使用pioneer3机器人作为在线采集与算法测试的平台，平台装备了PointGrey Bumblebee立体相机一台，实验中只用到了其中的一个相机。车上还装有两台Novatel GPS和一台NV-IMU200的IMU，其中GPS的最高频率是20hz，相机最高每秒采集10帧图像,IMU的频率为100hz。GPS应用RTK技术定位精确度最高可达2cm，实验应用双GPS测量采集路标时相机的方向和车体初始航向，基线距离为50cm。实验环境为室外草地，使用Sokia SRX1全站仪系统对车体位置进行精确定位作为测量真值，全站仪（TS）通过跟踪安装在车体上的全向棱镜进行车体定位，精确度可达±2mm。

实验设计路线为环形，总长为43.79m。实验用bumblebee2立体相机采集320*240大小的图像，相机距地面0.78m高，俯仰角为10.7度，本次实验预先采集区域内5个标志场景的图像作为视觉路标，并记录测量位置车体的航向角。车体在规划的区域内行进时每帧不超过30cm，旋转角度不超过5度，共采集图像194帧。表1分别记录了在路标处基于惯性导航系统/里程计定位、视觉路标单点定位位置误差。表中清楚可见单点的视觉路标匹配定位精度较高，噪声方差小，融合后将大大修正了惯性导航系统/里程计定位的累积误差。

该实验中全局向量间距离阈值设为1.5，排除了与路标相差较大的场景图像，有效提升了匹配速度；本实验中，视觉路标匹配定位的误差均为一帧，即在投票分数达到最高点后的下一帧中立即赋予位置信息，延时即为一帧。

将单点视觉定位的位移信息和修正后的航向(本实验重在对姿态的航向修正)与惯性导航系统解算的位移和航向输出进行融合分别得到惯性导航系统/里程计组合、融合视觉路标以及航向修正后的轨迹估计，全站仪作为测量真值作为进行对比如图2。图中IMU/里程计组合导航的计算的轨迹，与全站仪数据相比，误差明显比修正后的大。这是由于车轮在草地上易打滑，车体在做定向旋转和前进时，实际无法达到要求的里程和角度，尤其在转弯时，航向转角的不足使得惯性导航系统数据向外漂移，不断累积的误差使得漂移现象愈加显著。融入视觉路标修正后，与路标相匹配的5个图像所在位置的单点误差得到明显的改善，从而造成曲线五处的间断，相当于起到了复位的作用；而航向角的修正使整个轨迹趋势明显靠近全站仪数据，整个定位误差大大减小。

同时，本文以五个路标位置为间隔，将轨迹分成六段逐段分析（如表3），由表中可知惯性导航系统/里程计的误差主要在分段4及之后的车体转向较多的路段，说明误差主要来源与车体实际航向与惯性导航系统解算的偏差，修正后直行路段（如路段3）累积误差非常有限，而转向较多路段（如路段4）累积误差明显大于其它路段但均已明显大大改善，充分表明视觉路标位置引入使单点定位的误差改善，而车体航向的校正使整个轨迹趋向正常，两者结合大大改善系统位置误差。

表1路标视觉单点定位解算误差

表2图像快速匹配框架各部分平均时间

	GIST提取时间	SURF提取时间	SURF匹配时间	SFM算法
					每帧时间/ms	50	175	80	40

表3分段定位误差比较

Claims (4)

1.一种单目自然视觉路标辅助的移动机器人定位方法，其特征在于，包括如下步骤： 

1）用单目相机采集区域内的场景图像作为自然视觉路标，提取场景图像的GIST全局特征和SURF局部特征，获取采集路标时单目相机和车体的姿态，构建视觉路标全局特征库和局部特征库； 

2）移动机器人在行进过程中获取新图像，参考惯性导航系统的定位信息，将当前图像与自然视觉路标进行匹配，使用全局特征作为初步的过滤，并在此基础上使用局部特征进行精确匹配； 

3）当前图像与自然视觉路标实现正确匹配定位后，因为机器人当前时刻的实际航向和路标指示的航向存在偏差，故采用基于单目图像的运动估计算法，计算当前图像与自然视觉路标间的航向与姿态偏差，获得机器人当前时刻可靠的定位信息； 

4）将惯性导航系统与自然视觉路标辅助定位信息进行融合，惯性导航系统和里程计作为机器人主要的导航方式，基于自然视觉路标的绝对定位方法作为辅助导航方式，对惯性导航系统数据进行修正，惯性导航系统与自然视觉路标的融合包括位置和姿态两个方面的融合，由于相邻两帧间场景非常接近带来匹配模糊性，因此存在一个定位噪声方差；场景接近的前后两帧图像间姿态的变化很小，因此认为所求得的姿态可靠，直接作为当前姿态输出，而对位置信息则通过Kalman滤波方式进行融合。 

2.根据权利要求1所述的一种单目自然视觉路标辅助的移动机器人定位方法，其特征在于，所述步骤1)中，用单目相机采集区域内的场景图像作为自然视觉路标，提取场景图像的GIST全局特征和SURF局部特征，获取采集路标时单目相机和车体的姿态，构建视觉路标全局特征库和局部特征库；包括如下步骤： 

1）首先采集区域内的场景图像作为自然视觉路标，自然视觉路标的选择应遵循三个原则：（1）具有丰富的特征信息；（2）采集该自然视觉路标时的单目相机位置和方向已知；（3）自然视觉路标之间的间隔为50米～100米，路标间隔低于50米时，会增大路标匹配的工作负担，降低匹配正确率；间隔超过100米时，则不能及时发挥定位修正的作用； 

2）提取自然视觉路标的全局特征，GIST描述符是利用不同方向和尺度的Gabor滤波器对图像进行采样滤波，滤波后图像输出到4*4的网格中，每个网格 取平均值； 

3）提取自然视觉路标的SURF局部特征，SURF算子对尺度缩放、平移、旋转变换保持不变，即使在图像目标受到部分遮挡的情况下也能检索，同时对图像的亮度变化和仿射变换具有不变性，提取图像的SURF关键点能准确地刻画图像的局部内容特征。 

3.根据权利要求1所述的一种单目自然视觉路标辅助的移动机器人定位方法，其特征在于，所述步骤2)中，移动机器人在行进过程中获取新图像，参考惯性导航系统的定位信息，将当前图像与自然视觉路标进行匹配，使用全局特征作为初步的过滤，并在此基础上使用局部特征进行精确匹配；包括如下步骤： 

将当前图像与自然视觉路标进行匹配，使用匹配成功的SURF关键点数占关键点总数的百分比结合尺度信息作为指标，应用投票机制来衡量当前图像与自然视觉路标的相似度，匹配的SURF数目的百分比记为N1，匹配的SURF点对中处在同一尺度的SURF点数目的百分比记为N2，则图像的相似度投票计分为 

N=m1*N1+m2*N2              (1) 

其中m1、m2为权重系数，m2>m1>0，当机器人在接近路标位置时，投票分数也越来越高，当过了路标位置时，投票数随即下降，赋予票数最高者相应路标的位置信息。 

4.根据权利要求1所述的一种单目自然视觉路标辅助的移动机器人定位方法，其特征在于：所述步骤3)中，当前图像与自然视觉路标实现正确匹配定位后，因为机器人当前时刻的实际航向和路标指示的航向存在偏差，故采用基于单目图像的运动估计算法，计算当前图像与自然视觉路标间的航向与姿态偏差，获得机器人当前时刻可靠的定位信息；包括如下步骤： 

基于本质阵恢复运动参数，使用已经过标定的单目相机，提取采集的图像中的SURF特征，与自然视觉路标进行特征匹配，应用八点法求解本质矩阵，为减少噪声的影响，使用Ransac去除离群点，结合最小二乘准则获解本质矩阵，然后对本质矩阵进行奇异值分解： 

E=UΣV^T            (2) 

由本质矩阵和旋转矩阵的关系，可得两图像间旋转矩阵的求解公式如下： 

基于被测点位于相机前方的事实以及相机的位置关系可选出R的唯一正确 解；由旋转矩阵R和姿态角的关系可得到两幅图像间的三个姿态角的偏差： 

Δθ=arcsin(R(1,3))           (4) 

Δψ=arctan(R(1,2)/R(1,1)) 

其中

Δθ、Δψ分别表示两图像间横滚角、俯仰角、航向角之差；结合路标已知的航向和姿态，即得到当前位置准确的姿态信息。 

Images