基于Xtion摄像机的室内机器人视觉里程计实现方法

摘要

本发明涉及一种基于Xtion摄像机的室内机器人里程计实现方法，该方法为：首先通过安装在机器人上的Xtion摄像机对机器人前方的视场进行信息采集，获取空间点的RGB信息和三维坐标信息。然后基于SIFT特征匹配算法并结合空间点的PFH（点特征直方图）三维特征，对采集的序列图像进行粗特征点匹配，并使用RANSAC（随机采样一致性）剔除粗匹配中的误匹配点，得到精确匹配点。最后建立方程组，利用最小二乘法求解出机器人的运动参数。本方法使用Xtion摄相机进行信息采集，能够直接获取空间点的三维信息，并且利用空间点的纹理特征和三维特征进行特征提取和匹配，从而显著提高了机器人定位的效率和精度。

说明书

技术领域

本发明涉及室内移动机器人自主导航技术领域，尤其是一种应用于室内环境中轮驱动式移动机器人自主导航技术中的视觉里程实现方法。 

背景技术

在室内移动机器人的研究中，实时获得机器人高精度的运动参数是极其重要的，关系到机器人导航、避障和路径规划等任务的实现。由于机器人行进过程中碰到障碍物轮子存在长期磨损打滑现象，使得光电码盘测速电机等设备不能准确测定机器人的位移准确信息；采用GPS定位，存在分辨率低，且在室内的信号弱，不适合于室内移动机器人。 

视觉里程计通过采集分析相关图像序列，来确定机器人位置和朝向，可以弥补上述的问题，增强了机器人在任何表面以任何方式移动时的导航精度。传统的视觉里程计通过单目照相机、双目照相机或者全向照相机获取图像，通过坐标系的转换获取空间点的三维信息，并通过图像RGB信息对两帧图像进行特征提取和匹配从而获取机器人的运动参数。在获取空间点的三维信息时，由于照相机镜片误差和坐标系的转换，使得计算过程复杂、效率低并且精度不高。在对图像特 征提取、匹配时由于只利用了图像的RGB信息，丢失了三维信息，常常出现错误匹配的情况。 

发明内容

本发明的目的是提供一种基于Xtion摄像机的室内移动机器人视觉里程计实现方法，该方法利用Xtion摄相机，能够直接获取空间点的三维信息和RGB信息，并且利用提取空间点的纹理特征和三维特征进行匹配，从而显著提高了机器人定位的效率和精度。 

为实现上述目的，本发明采用以下方案实现：该方法包括以下步骤： 

步骤S01：通过安装在机器人上的Xtion摄像机对机器人前方的视 

场进行信息采集，获取空间点的RGB信息和三维坐标信息； 

步骤S02：利用SIFT特征匹配算法并结合空间点的PFH三维特征，对Xtion所采集到相邻两幅图像间的特征点进行匹配； 

步骤S03：使用RANSAC剔除匹配中的误匹配点； 

步骤S04：获得前后相邻两帧图像的匹配点其中角标p代表前一帧图像，c代表后一帧图像，i为匹配点数目；建立方程组$\left(\begin{array}{c}{x}_i^c\\ y_i^c\\ z_i^c\\ 1\end{array}\right)=\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)\left(\begin{array}{c}{x}_i^p\\ y_i^p\\ z_i^p\\ 1\end{array}\right),$其中R为旋转矩阵，T为平移矢量；利用最小二乘法即可求解出机器人的运动参数。 

进一步地，所述特征点匹配具体包括如下步骤： 

1)特征点检测：在图像二维平面空间和DoG尺度空间中同时检测局部极值以作为特征点，DoG算子定义为两个不同尺度的高斯核的差 分，DoG算子如式(1)所示： 

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)＝L(x,y,kσ)-L(x,y,σ)  (1) 

其中，G(x,y,σ)为尺度可变高斯函数，(x,y)为空间坐标，σ为尺度坐标，I(x,y)为原图，L(x,y,σ)为尺度空间。 

在检测尺度空间极值时，某个像素需要跟同一尺度的周围领域的8个像素以及相邻尺度对应位置的领域9×2个像素总共26个像素进行比较，以确保在尺度空间和2维图像空间都能检测到局部极值； 

2)特征点描述，即建立特征向量，其中包括SIFT特征向量和PFH三维特征向量； 

SIFT特征向量：每个关键点由2×2共4个种子点构成，每个种子点包含8个方向向量信息；对每个关键点采用4×4共16个种子点来描述，以产生128个数据，即最后每个关键点建立128维的SIFT特征向量； 

PFH三维特征向量：利用KD数寻找每个关键点附近K个点，并求出这些点在空间坐标系中的法线向量n；对于关键点与其附近K个点，取其所有互相两个点组合：P_i,P_j，根据判定<n_i,P_j-P_i>与<n_j,P_i-P_j>的大小来设置原点P_s和目标点P_t，其中n为法线向量，若前者大则P_s＝P_i,P_t＝P_j，反之P_s＝P_j,P_t＝P_i； 

根据上述各点建立局部坐标系：$\left(\begin{array}{c}u=n_s\\ v=u\times \frac{(P_t-P_s)}{{\left|\right|P_t-P_s\left|\right|}_2}\\ w=u\times v\end{array}\right),$在这个坐标系下计算三个参数：$\left(\begin{array}{c}\alpha =v·n_t\\ \phi =u·\frac{P_t-P_s}{{\left|\right|P_t-P_s\left|\right|}_2}\\ \theta =\arctan (\omega ·n_t,u·n_t)\end{array}\right),$将每个参数化分为5个子区间 并建立3⁵个区间的直方图中，每对点的三个参数(α,φ,θ)为所对的应直方图区间加一，统计所有点后所产生的直方图进行归一化，即为125维PFH三维特征向量； 

3)进行特征匹配以获取候选匹配点：当两帧图像的SIFT特征向量和FPH三维特征向量生成后，下一步采用关键点特征向量间的欧式距离来作为两帧图像中关键点的相似性判定度量；取两帧图像欧式距离最近的前两对关键点，在这两对关键点中，如果最近的距离除以次近的距离少于某个比例阈值，则接受这一对匹配点； 

4)消除误匹配：采用RANSAC算法去除外点，RANSAC算法是通过多次随机抽取一定的样本对参数进行估计，先得出初始参数模型，然后根据估计参数将所有数据进行分类，一部分数据在设定误差范围内，则称为内点，反之称为外点，经过多次迭代计算出最优模型参数。 

进一步地，所述最小二乘法求解出机器人的运动参数方法如下： 

求解最小二乘解使得||P_ci-(RP_pi+T)||²最小，其中P_pi和P_pi为相邻两序列图像中的前一帧图像和后一帧图像，i为对应匹配点数；首先建立协方差矩阵${\Sigma }_{\mathrm{cp}}=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}(P_{\mathrm{ci}}-{\mu }_c){(P_{\mathrm{pi}}-{\mu }_p)}^T,$其中${\mu }_n=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{P}_{\mathrm{pi}},$然后对矩阵Σ_cp进行特征值分解Σ_cp＝UDV，其中U,V为酉矩阵，D＝diag(d_i),d₁≥d₂…≥d_m≥0为奇异值矩阵，式中d_i为非零奇异值，记$S=\left(\begin{array}{cc}I,& \det \left({\Sigma }_{\mathrm{cp}}\right)\ge 0,\\ \mathrm{diag}(\mathrm{1,1},...,-1),& \det \left({\Sigma }_{\mathrm{cp}}\right)<0,\end{array}\right);$最后求解旋转矩阵R＝USV，带入原方差即可求解平移矢量T。 

相对现有技术，本发明的有益效果是： 

1、采用Xtion摄像机直接获取空间点的RGB信息和三维信息， 避免了传统视觉里程计需要多次变换坐标系，进行大量的运算才能求解出空间点的三维坐标，显著挺高了计算速度和精度。 

2、充分利用了空间点的纹理特征和三维特征对序列图像进行匹配，改善了传统视觉里程计仅仅依靠空间点的RGB信息的缺陷，使匹配结果更加精确。 

附图说明

图1是本发明总体流程图。 

具体实施方式

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

如图1所示，本实施例的方法首先通过安装在机器人上的Xtion摄像机对机器人前方的视场进行信息采集，获取空间点的RGB信息和三维坐标信息。然后基于SIFT特征匹配算法并结合空间点的PFH(点特征直方图)三维特征，对采集的序列图像进行粗特征点匹配，并使用RANSAC(随机采样一致性)剔除粗匹配中的误匹配点，得到精确匹配点。最后建立方程组，利用最小二乘法求解出机器人的运动参数。 

具体的，将Xtion摄像机安置在机器人前方，在机器人移动的同时，Xtion摄像机将记录下机器人前方的640×480个点的信息，所呈现二维图像如图二所示，三维点云信息如图三所示，所获信息为每一个空间点的三维坐标值和RGB彩色值。 

利用SIFT特征匹配算法并结合空间点的三维特征，，获得两图像中的粗特征点匹配，并使用RANSAC(随机采样一致性)剔除粗匹配 中的误匹配点，得到精确匹配点。具体步骤如下： 

1)特征点检测：在图像二维平面空间和DoG(Difference-of-Gaussian)尺度空间中同时检测局部极值以作为特征点，DoG算子定义为两个不同尺度的高斯核的差分，DoG算子如式(1)所示： 

D(x,y,σ)＝(G(x,y,kσ)-G(x,y,σ))*I(x,y)＝L(x,y,kσ)-L(x,y,σ)  (1) 

其中，G(x,y,σ)为尺度可变高斯函数，(x,y)为空间坐标，σ为尺度坐标，I(x,y)为原图，L(x,y,σ)为尺度空间。 

在检测尺度空间极值时，某个像素需要跟同一尺度的周围领域的8个像素以及相邻尺度对应位置的领域9×2个像素总共26个像素进行比较，以确保在尺度空间和2维图像空间都能检测到局部极值。 

2)特征点描述，即建立特征向量，其中包括SIFT特征向量和PFH三维特征向量。 

SIFT特征向量：每个关键点由2×2共4个种子点构成，每个种子点包含8个方向向量信息。为了增强匹配稳定性，对每个关键点采用4×4共16个种子点来描述，这样对于每个关键点就可以产生128个数据，即最后每个关键点建立128维的SIFT特征向量。 

PFH三维特征向量：利用KD数寻找每个关键点附近K个点，并求出这些点在空间坐标系中的法线向量n。对于关键点与其附近K个点，取其所有互相两个点组合：P_i,P_j，根据判定<n_i,P_j-P_i>与<n_j,P_i-P_j>的大小来设置原点P_s和目标点P_t，其中n为法线向量，若前者大则P_s＝P_i,P_t＝P_j，反之P_s＝P_j,P_t＝P_i。 

根据上述各点建立局部坐标系：$\left(\begin{array}{c}u=n_s\\ v=u\times \frac{(P_t-P_s)}{{\left|\right|P_t-P_s\left|\right|}_2}\\ w=u\times v\end{array}\right),$在这个坐标系下计算三个参数：$\left(\begin{array}{c}\alpha =v·n_t\\ \phi =u·\frac{P_t-P_s}{{\left|\right|P_t-P_s\left|\right|}_2}\\ \theta =\arctan (\omega ·n_t,u·n_t)\end{array}\right),$将每个参数化分为5个子区间并建立3⁵个区间的直方图中，每对点的三个参数(α,φ,θ)为所对的应直方图区间加一，统计所有点后所产生的直方图进行归一化，即为125维PFH三维特征向量。 

3)进行特征匹配以获取候选匹配点：当两帧图像的SIFT特征向量和FPH三维特征向量生成后，下一步采用关键点特征向量间的欧式距离来作为两帧图像中关键点的相似性判定度量。取两帧图像欧式距离最近的前两对关键点，在这两对关键点中，如果最近的距离除以次近的距离少于某个比例阈值，则接受这一对匹配点。 

4)消除误匹配：本文采用RANSAC算法去除外点，RANSAC算法是通过多次随机抽取一定的样本对参数进行估计，先得出初始参数模型，然后根据估计参数将所有数据进行分类，一部分数据在设定误差范围内，则称为内点，反之称为外点，经过多次迭代计算出最优模型参数。 

获得相邻两帧图像的匹配点其中i为匹配点数目。建立方程组$\left(\begin{array}{c}{x}_i^c\\ y_i^c\\ z_i^c\\ 1\end{array}\right)=\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)\left(\begin{array}{c}{x}_i^p\\ y_i^p\\ z_i^p\\ 1\end{array}\right),$其中R为旋转矩阵，T为平移矢量。利用最小二乘法即可求解出机器人的运动参数。图四和图五为相邻前后两帧图像旋转平移变换示意图，求解出的变换结果为 (单位：m)：$\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}0.992999& 0.0260354& 0.115213& -0.0287673\\ -0.0232128& 0.999398& -0.0257746& 0.0418021\\ -0.115815& 0.0229197& 0.993006& 0.284341\\ 0& 0& 0& 1\end{array}\right).$

实例中根据里程计记录，摄像机实际旋转5°，移动x＝0.03m,y＝0.04m,z＝0.30m。根据测量结果，最大绝对误差为0.016m，相对误差5.7％。