室內(nèi)移動機器人目標定位性準確匹配研究

劉俊超 陳志軍 樊小朝 閆學勤

關(guān)鍵詞： 掃描匹配; 移動機器人; Kinect傳感器; 目標定位; 旋轉(zhuǎn)投影統(tǒng)計; 點云匹配

中圖分類號： 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 ?引 ?言

機器人目標定位是通過對傳感器數(shù)據(jù)進行處理來估算目標位置的技術(shù)，是實現(xiàn)機器人自主導(dǎo)航的基礎(chǔ)。而點云配準是移動機器人目標定位的核心問題之一。不少學者針對點云配準技術(shù)做了不少研究，其中Besl等最先提出的ICP是最普遍和常用的算法，但存在很多缺陷，如對先驗位姿信息敏感、收斂速度慢等[1]。因此，為克服這些缺點，許多學者在最近點選取、誤差函數(shù)、特征點提取方法的求解等方面做了改進[2?5]。但改進后的算法仍存在許多不足之處。如Bergstr?m等提出了基于魯棒性M估計的迭代最近點算法，但配準精度和穩(wěn)定性方面常用的準則函數(shù)效果不佳[6]。黃源等針對在不同視角下獲得的點云數(shù)據(jù)，提出一種基于特征提取的點云自動配準算法，但該算法復(fù)雜度高[7]。

為提高其配準效率和精度，本文提出了一種基于旋轉(zhuǎn)投影統(tǒng)計（Rotational Projection Statistics，ROPS）描述子改進的 ICP算法，以完成室內(nèi)移動機器人目標定位性的準確匹配。

1 ?室內(nèi)移動機器人目標定位模型

1.1 ?里程計的目標定位模型

機器人移動的相對位置是由里程計的測量得到，因此目標定位可以通過航跡推算來實現(xiàn)目標定位。假設(shè)k時刻機器人目標定位結(jié)果為[pk=xk，yk，θkT]且經(jīng)過[Δt]時間位移為[Δsk]，轉(zhuǎn)動角度為[Δθk]，則由航跡推算法可得k+1時定位的表達式可近似為：

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

1.2 ?基于掃描匹配的目標定位模型

因里程計有累積誤差存在，故本文采用組合導(dǎo)航的方案提高目標定位精度即引入掃描匹配。其具體方案如下：

1） 將當前掃描記為D，參考掃描為一刻記為M;

2） 將D配準到M的最優(yōu)變換（R，T），R和T分別為旋轉(zhuǎn)矩陣和平移矩陣;

3） 依據(jù)（R，T）算出移動機器人當前的目標位置，實現(xiàn)定位;

4） 將新的參考掃描M替換當前掃描D，步驟1）迭代。

本文主要研究室內(nèi)移動機器人目標定位中的準確匹配問題，對于上述掃描匹配算法的具體實現(xiàn)方式，存在多種不同的方法。

2 ?掃描匹配算法的改進

2.1 ?傳統(tǒng)ICP算法

ICP算法通過重復(fù)選擇對應(yīng)點對來計算最優(yōu)剛體變換，其實質(zhì)上是基于最小二乘的最優(yōu)匹配方法，以此達到匹配的收斂精度要求。

在配準過程中，ICP的難點在于如何查找關(guān)系點對。然而掃描待匹配的兩組很難完全重疊，會有部分點沒有對應(yīng)關(guān)系。如果匹配過程中對所有點集進行配準，則難免會把錯誤點對誤認為有效點來考慮。且由Kinect本身測量特性而知，所有點對間的關(guān)聯(lián)都是相近的，并沒有真正配準的關(guān)系點對。因而，本文選擇對應(yīng)點的方式有所改變即根據(jù)ROPS的相似性查找最近點。

2.2 ?改進的ICP算法

本文改進算法的思想為：首先，對移動機器人采集到的數(shù)據(jù)做濾波處理;其次，找出兩個匹配的特征點，估計ROPS描述子的相似度來確定對應(yīng)點對;最后，通過計算平移矩陣和旋轉(zhuǎn)矩陣進行ICP配準。配準過程如圖1所示。

2.2.1 ?濾波處理

由于Kinect傳感器視角范圍過大，若移動機器人的移動較慢，則很有可能造成掃描點在空間上相差很小，導(dǎo)致掃描匹配次數(shù)過多且點云數(shù)量巨大影響目標定位的速度和精度。

點云濾波方法有隨機采樣法、包圍盒法等[8]。本文采用的濾波為體素柵格下采樣法[9]。

2.2.2 ?特征點提取

設(shè)相鄰掃描模型分別為[P={p1，p2，…，pM}]和[Q={q1，q2，…，qN}]，其中M和N為點云P和Q的點云數(shù)量。對參考掃描點云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值較大，則該點所在區(qū)域起伏變化明顯，也就是幾何特征比較突出，因而本文選取的特征點為sin值較大的點。取閾值[ξ]，若閾值小于sin值的點作為特征點。

最終得到特征點集[Pfeature=pf1，pf2，…，pfm]和[Qfeature=qf1，qf2，…，qfn]，其中m和n分別為P和Q的特征點個數(shù)。

2.2.3 ?基于ROPS的特征點描述

ROPS 特征描述子是擁有三維空間旋轉(zhuǎn)平移不變性的特征描述子[10]。

首先構(gòu)建局部參考系，其具體過程如下：在深度圖像表面上截取局部表面S，局部表面S由k個三角網(wǎng)格構(gòu)成。第 i個網(wǎng)格上的任一點坐標可表示為：

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

式中：[0≤s，t≤1]且[s+t≤1]，[pi1]，[pi2]，[pi3]為第i個三角網(wǎng)格上的頂點。第i個三角網(wǎng)格的散布矩陣[Ci]可以表示為：

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

局部表面網(wǎng)格S的總體散布矩陣C為：

[C=i-1twi1wi2Ci] ? ? ? ? ? ? ? （6）

式中，第i個三角網(wǎng)格的面積與局部表面網(wǎng)格S上所有三角網(wǎng)格總面積之比為[wi1]。特征點在第i個三角網(wǎng)格形心的距離以及支持半徑r相關(guān)的量為[wi2]，其定義為：

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

在網(wǎng)格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]對應(yīng)的正交特征向量。其次是特征描述子的生成，具體過程如下：以特征點為中心繞參考坐標軸旋轉(zhuǎn)一定的角度得到表面網(wǎng)格S′，并將S′上的點云分別投影到坐標平面xy，yz，xz，從而得到三幅二維點云S1，S2，S3。將二維點云分割成N×N的格子并統(tǒng)計格子里獲得的點數(shù)。根據(jù)點數(shù)值構(gòu)建N×N的分布矩陣D1，D2，D3。計算分布矩陣D的統(tǒng)計量：低階中心矩{μ11，μ12，μ21，μ22}和香農(nóng)熵e。f={μ11，μ12，μ21，μ22，e}即為網(wǎng)格S以θ角度繞坐標軸旋轉(zhuǎn)投影一次后在某個坐標平面上的子特征。最后可獲得該特征點處的特征向量fP={f1，f2，…，fn}。最終得到室內(nèi)移動機器人在移動過程中相鄰幀間的特征描述子，該特征描述子對點云密度變化、環(huán)境遮擋等干擾因素具有較強的魯棒性。

2.3 ?特征點匹配和誤匹配點剔除

設(shè)機器人移動過程中相鄰兩個掃描數(shù)據(jù)幀所得到的特征點的ROPS特征集合分別為[F=f1，f2，…，fm]和[F′=f′1，f′2，…，f′n]，在掃描的兩幅圖像中判斷特征點相似的度量標準為特征向量的歐氏距離法。設(shè)與ROPS描述子[fi]最近的兩個描述子為[f′j]和[f′j′]，計算[fi]與[f′j]以及[fi]與[f′j′]兩組特征向量的歐氏距離比值[τ]，如果[τ]與設(shè)定的閾值相比較小，則接受這對匹配點。特征（[fi]，[f′j]）所對應(yīng)的匹配點為（[pi]，[q′j]）。

在實際過程中，由于所選點的重復(fù)性不足，測量結(jié)果可能會引入測量誤差和噪聲，環(huán)境遮擋等因素影響，會出現(xiàn)誤匹配，誤匹配點的存在大大影響目標定位的精度。在剛體變換中，任意兩個特征點的距離、主軸夾角在變換前后是不變的[11]。此距離和角度的一致特征可以用來剔除不可靠或錯誤的匹配點對，因此本文采用矩陣差分矩陣算法剔除誤匹配點，其思想如下：根據(jù)獲得的對應(yīng)點集，參考掃描點云和當前掃描點云的特征點集分別為[Pfeature=pf1，pf2，…，pfm]和[Qfeature=qf1，qf2，…，qfn]，其中（[pfi]，[qfi]）是對應(yīng)點對，如果[Pfeature]中的一點[pfk]和P中其他點之間的距離與[qfk]和[Qfeature]中其他對應(yīng)點之間的距離相比大于某個閾值，即[dPik-dQik>δ]，則認為是局外點對并將其剔除。最終得到最優(yōu)的對應(yīng)點對。

2.4 ?計算初始配準參數(shù)

根據(jù)最終三維配準點集，運用最小二乘迭代算法求解出對應(yīng)點對間最優(yōu)配準參數(shù)，最優(yōu)旋轉(zhuǎn)矩陣R3×3和平移矩陣T3×1。

3 ?目標定位

本文采用歐拉角的方式來表示旋轉(zhuǎn)矩陣。最優(yōu)變換矩陣為：

[M=RT01] ? ? ? ? ? ? ? ? ? ? ?（9）

旋轉(zhuǎn)矩陣為：

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

平移矩陣為：

[T=txtytzT] ?（11）

則目標定位結(jié)果為[ψ=x，y，z，?，θ，?]，式中

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

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

4 ?實驗結(jié)果與分析

為進一步驗證算法的可行性，本文將基于ROPS描述子改進的ICP算法和傳統(tǒng)ICP算法做實驗比較。采用NAO機器人和landmark標桿進行實驗，使用微軟Kinect傳感器從不同角度獲取并保存點云數(shù)據(jù)，分別作為參考掃描點云和當前掃描點云，實驗平臺為Windows 10 64位系統(tǒng)，VS2013 32位，并結(jié)合PCL1.7.2[12]進行配準實驗。為了對比配準前后的效果，首先將參考掃描點云和當前掃描點云以單位變換矩陣變換到統(tǒng)一坐標系下，其“配準”效果如圖2所示。圖3為傳統(tǒng)ICP算法的實驗結(jié)果，圖4為用本文改進算法的實驗結(jié)果。對比圖3和圖4，圖4的配準結(jié)果更好。實驗表明該算法都能達到理想的配準效果且比傳統(tǒng)ICP算法的配準精度更高。

表1為本文算法與其同類算法在匹配精度和時間等方面的比較，本文算法在迭代次數(shù)相同情況下相比于其他算法，其配準精度最小，在配準時間上雖比原始ICP算法和經(jīng)典3D?NDT算法耗時多，但配準精度卻比它們精確很多，綜合考慮本文算法具有更多實用價值。

5 ?結(jié) ?論

本文在ICP算法的基礎(chǔ)上，提出一種基于旋轉(zhuǎn)投影統(tǒng)計描述改進的ICP算法。對待配準點進行濾波預(yù)處理，通過點法向量夾角的正弦值提取特征點，大大提高了配準的運算速度。實驗結(jié)果表明，該算法提高了點云配準精度，降低了時間復(fù)雜度，取得了良好的配準效果，為獲得精確目標定位信息提供了良好保障。

注：本文通訊作者為陳志軍。

參考文獻

[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].計算機應(yīng)用研究，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] 袁華，龐建鏗，莫建文.基于體素化網(wǎng)格下采樣的點云簡化算法研究[J].電視技術(shù)，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.