室内移动机器人目标定位性准确匹配研究

刘俊超 陈志军 樊小朝 闫学勤

关键词： 扫描匹配; 移动机器人; Kinect传感器; 目标定位; 旋转投影统计; 点云匹配

中图分类号： TN953?34; P242                   文献标识码： A                    文章编号： 1004?373X（2019）02?0159?04

Research on accurate matching of target positioning for indoor mobile robot

LIU Junchao， CHEN Zhijun， FAN Xiaochao， YAN Xueqin

（School of Electrical Engineering， Xinjiang University， Urumqi 830047， China）

Abstract： For the scan matching technology used in the target positioning system of the indoor mobile robot， the traditional iterative closest point algorithm has a strict requirement for the initial position of the to?be registration point cloud， and is difficult to find the correct corresponding point pair. Therefore， a target positioning algorithm is proposed for the mobile robot on the basis of obtaining the 3D environment point cloud by means of the Kinect sensor， querying the corresponding point pair according to the similarities of rotational projection statistics feature descriptors， and performing scan matching. The point cloud map of an object is obtained by means of the Kinect sensor. The point cloud features are extracted by using the feature extraction algorithm. The rotational projection statistics feature descriptors of two to?be registration point clouds are obtained. The corresponding relationship between two descriptors is estimated by comparing the feature similarity degree of two descriptors. The distance difference matrix algorithm is adopted to remove mismatching points and calculate the initial matching parameter. The ICP registration is performed by using the least square iteration method， so as to obtain the final transformation matrix between point clouds and realize target positioning. The experimental results show that the improved algorithm can effectively improve the matching efficiency and registration accuracy of point clouds， and obtain comparatively accurate target positioning information.

Keywords： scan matching; mobile robot; Kinect sensor; target positioning; rotational projection statistics; point cloud matching

0  引  言

机器人目标定位是通过对传感器数据进行处理来估算目标位置的技术，是实现机器人自主导航的基础。而点云配准是移动机器人目标定位的核心问题之一。不少学者针对点云配准技术做了不少研究，其中Besl等最先提出的ICP是最普遍和常用的算法，但存在很多缺陷，如对先验位姿信息敏感、收敛速度慢等[1]。因此，为克服这些缺点，许多学者在最近点选取、误差函数、特征点提取方法的求解等方面做了改进[2?5]。但改进后的算法仍存在許多不足之处。如Bergstr?m等提出了基于鲁棒性M估计的迭代最近点算法，但配准精度和稳定性方面常用的准则函数效果不佳[6]。黄源等针对在不同视角下获得的点云数据，提出一种基于特征提取的点云自动配准算法，但该算法复杂度高[7]。

为提高其配准效率和精度，本文提出了一种基于旋转投影统计（Rotational Projection Statistics，ROPS）描述子改进的 ICP算法，以完成室内移动机器人目标定位性的准确匹配。

1  室内移动机器人目标定位模型

1.1  里程计的目标定位模型

机器人移动的相对位置是由里程计的测量得到，因此目标定位可以通过航迹推算来实现目标定位。假设k时刻机器人目标定位结果为[pk=xk，yk，θkT]且经过[Δt]时间位移为[Δsk]，转动角度为[Δθk]，则由航迹推算法可得k+1时定位的表达式可近似为：

[pk+1=xk+Δskcos θkyk+Δsksin θk     θk+Δθk]         （1）

1.2  基于扫描匹配的目标定位模型

因里程计有累积误差存在，故本文采用组合导航的方案提高目标定位精度即引入扫描匹配。其具体方案如下：

1） 将当前扫描记为D，参考扫描为一刻记为M;

2） 将D配准到M的最优变换（R，T），R和T分别为旋转矩阵和平移矩阵;

3） 依据（R，T）算出移动机器人当前的目标位置，实现定位;

4） 将新的参考扫描M替换当前扫描D，步骤1）迭代。

本文主要研究室内移动机器人目标定位中的准确匹配问题，对于上述扫描匹配算法的具体实现方式，存在多种不同的方法。

2  扫描匹配算法的改进

2.1  传统ICP算法

ICP算法通过重复选择对应点对来计算最优刚体变换，其实质上是基于最小二乘的最优匹配方法，以此达到匹配的收敛精度要求。

在配准过程中，ICP的难点在于如何查找关系点对。然而扫描待匹配的两组很难完全重叠，会有部分点没有对应关系。如果匹配过程中对所有点集进行配准，则难免会把错误点对误认为有效点来考虑。且由Kinect本身测量特性而知，所有点对间的关联都是相近的，并没有真正配准的关系点对。因而，本文选择对应点的方式有所改变即根据ROPS的相似性查找最近点。

2.2  改进的ICP算法

本文改进算法的思想为：首先，对移动机器人采集到的数据做滤波处理;其次，找出两个匹配的特征点，估计ROPS描述子的相似度来确定对应点对;最后，通过计算平移矩阵和旋转矩阵进行ICP配准。配准过程如图1所示。

2.2.1  滤波处理

由于Kinect传感器视角范围过大，若移动机器人的移动较慢，则很有可能造成扫描点在空间上相差很小，导致扫描匹配次数过多且点云数量巨大影响目标定位的速度和精度。

点云滤波方法有随机采样法、包围盒法等[8]。本文采用的滤波为体素栅格下采样法[9]。

2.2.2  特征点提取

设相邻扫描模型分别为[P={p1，p2，…，pM}]和[Q={q1，q2，…，qN}]，其中M和N为点云P和Q的点云数量。对参考扫描点云P，计算出P中每个点[pi]在不同邻域下[r1]，[r2]下的法向量[n（pi，r1）]，然后求出两个法向量夹角的正弦绝对值：

[Psin=sin θ1=1-n（pi，r1）?n（pi，r2）n（pi，r1）?n（pi，r2）2]   （2）

同理，参考扫描点Q中的点[qi]的正弦值为

[Qsin=sin θ2=1-n（qi，r1）?n（qi，r2）n（qi，r1）?n（qi，r2）2]  （3）

若sin值较大，则该点所在区域起伏变化明显，也就是几何特征比较突出，因而本文选取的特征点为sin值较大的点。取阈值[ξ]，若阈值小于sin值的点作为特征点。

最终得到特征点集[Pfeature=pf1，pf2，…，pfm]和[Qfeature=qf1，qf2，…，qfn]，其中m和n分别为P和Q的特征點个数。

2.2.3  基于ROPS的特征点描述

ROPS 特征描述子是拥有三维空间旋转平移不变性的特征描述子[10]。

首先构建局部参考系，其具体过程如下：在深度图像表面上截取局部表面S，局部表面S由k个三角网格构成。第 i个网格上的任一点坐标可表示为：

[pi（s，t）=pi1+s（pi2-pi1+t（pi3-pi1））]   （4）

式中：[0≤s，t≤1]且[s+t≤1]，[pi1]，[pi2]，[pi3]为第i个三角网格上的顶点。第i个三角网格的散布矩阵[Ci]可以表示为：

[Ci=0101-s（pi（s，t）-p）（pi（s，t）-p）Tdtds0101-sdtds]    （5）

局部表面网格S的总体散布矩阵C为：

[C=i-1twi1wi2Ci]               （6）

式中，第i个三角网格的面积与局部表面网格S上所有三角网格总面积之比为[wi1]。特征点在第i个三角网格形心的距离以及支持半径r相关的量为[wi2]，其定义为：

[wi2=r-p-pi1+pi2+pi332]      （7）

在网格S中，特征值分解总体散布矩阵C：

[CV=EV]               （8）

式中：对角矩阵[E=λ1，λ2，λ3]，[λ1]，[λ2]，[λ3]为特征值且[λ1≥λ2≥λ3];矩阵[V=v1，v2，v3]，[v1]，[v2]，[v3]为[λ1]，[λ2]，[λ3]对应的正交特征向量。其次是特征描述子的生成，具体过程如下：以特征点为中心绕参考坐标轴旋转一定的角度得到表面网格S′，并将S′上的点云分别投影到坐标平面xy，yz，xz，从而得到三幅二维点云S1，S2，S3。将二维点云分割成N×N的格子并统计格子里获得的点数。根据点数值构建N×N的分布矩阵D1，D2，D3。计算分布矩阵D的统计量：低阶中心矩{μ11，μ12，μ21，μ22}和香农熵e。f={μ11，μ12，μ21，μ22，e}即为网格S以θ角度绕坐标轴旋转投影一次后在某个坐标平面上的子特征。最后可获得该特征点处的特征向量fP={f1，f2，…，fn}。最终得到室内移动机器人在移动过程中相邻帧间的特征描述子，该特征描述子对点云密度变化、环境遮挡等干扰因素具有较强的鲁棒性。

2.3  特征点匹配和误匹配点剔除

设机器人移动过程中相邻两个扫描数据帧所得到的特征点的ROPS特征集合分别为[F=f1，f2，…，fm]和[F′=f′1，f′2，…，f′n]，在扫描的两幅图像中判断特征点相似的度量标准为特征向量的欧氏距离法。设与ROPS描述子[fi]最近的两个描述子为[f′j]和[f′j′]，计算[fi]与[f′j]以及[fi]与[f′j′]两组特征向量的欧氏距离比值[τ]，如果[τ]与设定的阈值相比较小，则接受这对匹配点。特征（[fi]，[f′j]）所对应的匹配点为（[pi]，[q′j]）。

在实际过程中，由于所选点的重复性不足，测量结果可能会引入测量误差和噪声，环境遮挡等因素影响，会出现误匹配，误匹配点的存在大大影响目标定位的精度。在刚体变换中，任意两个特征点的距离、主轴夹角在变换前后是不变的[11]。此距离和角度的一致特征可以用来剔除不可靠或错误的匹配点对，因此本文采用矩阵差分矩阵算法剔除误匹配点，其思想如下：根据获得的对应点集，参考扫描点云和当前扫描点云的特征點集分别为[Pfeature=pf1，pf2，…，pfm]和[Qfeature=qf1，qf2，…，qfn]，其中（[pfi]，[qfi]）是对应点对，如果[Pfeature]中的一点[pfk]和P中其他点之间的距离与[qfk]和[Qfeature]中其他对应点之间的距离相比大于某个阈值，即[dPik-dQik>δ]，则认为是局外点对并将其剔除。最终得到最优的对应点对。

2.4  计算初始配准参数

根据最终三维配准点集，运用最小二乘迭代算法求解出对应点对间最优配准参数，最优旋转矩阵R3×3和平移矩阵T3×1。

3  目标定位

本文采用欧拉角的方式来表示旋转矩阵。最优变换矩阵为：

[M=RT01]                      （9）

旋转矩阵为：

[R=rxx  rxy  rxzryx  ryy  ryzrzx  rzy  rzz]                   （10）

平移矩阵为：

[T=txtytzT]  （11）

则目标定位结果为[ψ=x，y，z，?，θ，?]，式中

[x，y，z=tx，ty，tz]    （12）

[（?，θ，?）=（arctan（rzyrzz），arcsin（-rxz），arctan（rxyrxx））]   （13）

4  实验结果与分析

为进一步验证算法的可行性，本文将基于ROPS描述子改进的ICP算法和传统ICP算法做实验比较。采用NAO机器人和landmark标杆进行实验，使用微软Kinect传感器从不同角度获取并保存点云数据，分别作为参考扫描点云和当前扫描点云，实验平台为Windows 10 64位系统，VS2013 32位，并结合PCL1.7.2[12]进行配准实验。为了对比配准前后的效果，首先将参考扫描点云和当前扫描点云以单位变换矩阵变换到统一坐标系下，其“配准”效果如图2所示。图3为传统ICP算法的实验结果，图4为用本文改进算法的实验结果。对比图3和图4，图4的配准结果更好。实验表明该算法都能达到理想的配准效果且比传统ICP算法的配准精度更高。

表1为本文算法与其同类算法在匹配精度和时间等方面的比较，本文算法在迭代次数相同情况下相比于其他算法，其配准精度最小，在配准时间上虽比原始ICP算法和经典3D?NDT算法耗时多，但配准精度却比它们精确很多，综合考虑本文算法具有更多实用价值。

5  结  论

本文在ICP算法的基础上，提出一种基于旋转投影统计描述改进的ICP算法。对待配准点进行滤波预处理，通过点法向量夹角的正弦值提取特征点，大大提高了配准的运算速度。实验结果表明，该算法提高了点云配准精度，降低了时间复杂度，取得了良好的配准效果，为获得精确目标定位信息提供了良好保障。

注：本文通讯作者为陈志军。

参考文献

[1] BESL P J， MCKAY N D. A method for registration of 3?D shapes [J]. IEEE transactions on pattern analysis and machine intelligence， 1992， 14（2）： 239?256.

[2] ALMEIDA J M S， SANTOS V M F. Real?time egomotion of a nonholonomic vehicle using LIDAR measurements [J]. Journal of field robotics， 2013， 30（1）： 129?141.

[3] HUANG Xiaoshui， ZHANG Jian， FAN Lixin， et al. A systematic approach for cross?source point cloud registration by preserving macro and micro structures [J]. IEEE transactions on image processing， 2017， 26（7）： 3261?3276.

[4] AGAMENNONI G， FONTANA S， SIEGWART R Y， et al. Point  clouds registration with probabilistic data association [C]// Proceedings of International Conference on Intelligent Robots and Systems. Daejeon： IEEE， 2016： 4092?4098.

[5] PANKAJ D S， NIDAMANURI R R. Robust multiview registration of point clouds [C]// Proceedings of International Conference on Communication Systems and Networks. Thiruvananthapuram： IEEE， 2016： 50?55.

[6] BERGSTR?M P， EDLUND O. Robust registration of point sets using iteratively reweighted least squares [J]. Computational optimization and applications， 2014， 58（3）： 543?561.

[7] 黄源，达飞鹏，陶海跻.一种基于特征提取的点云自动配准算法[J].中国激光，2015，42（3）：250?256.

HUANG Yuan， DA Feipeng， TAO Haiji. An automatic registration algorithm for point cloud based on feature extraction [J]. Chinese journal of lasers， 2015， 42（3）： 250?256.

[8] 陈达枭，蔡勇，张建生.散乱点云精简的一种改进算法[J].计算机应用研究，2016，33（9）：2841?2843.

CHEN Daxiao， CAI Yong， ZHANG Jiansheng. Improved algorithm for simplfying scattered point cloud data [J]. Application research of computers， 2016， 33（9）： 2841?2843.

[9] 袁华，庞建铿，莫建文.基于体素化网格下采样的点云简化算法研究[J].电视技术，2015，39（17）：43?47.

YUAN Hua， PANG Jiankeng， MO Jianwen. Research on simplification algorithm of point cloud based on voxel grid [J]. Video engineering， 2015， 39（17）： 43?47.

[10] GUO Y， SOHEL F， BENNAMOUN M， et al. Rotational projection statistics for 3D local surface description and object recognition [J]. International journal of computer vision， 2013， 105（1）： 63?86.

[11] HUANG Q X， FL?RY S， GELFAND N， et al. Reassembling  fractured objects by geometric matching [J]. ACM transactions on graphics， 2006， 25（3）： 569?578.

[12] RUSU R B， COUSINS S. 3D is here： point cloud library （PCL） [C]// Proceedings of International Conference on Robotics and Automation. Shanghai： IEEE， 2011： 1?4.