劉俊超 陳志軍 樊小朝 閆學(xué)勤

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

中圖分類號(hào)： TN953?34; P242 ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼： A ? ? ? ? ? ? ? ? ? ?文章編號(hào)： 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 ?引 ?言

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

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

1 ?室內(nèi)移動(dòng)機(jī)器人目標(biāo)定位模型

1.1 ?里程計(jì)的目標(biāo)定位模型

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

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

1.2 ?基于掃描匹配的目標(biāo)定位模型

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

1） 將當(dāng)前掃描記為D，參考掃描為一刻記為M;

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

3） 依據(jù)（R，T）算出移動(dòng)機(jī)器人當(dāng)前的目標(biāo)位置，實(shí)現(xiàn)定位;

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

本文主要研究室內(nèi)移動(dòng)機(jī)器人目標(biāo)定位中的準(zhǔn)確匹配問(wèn)題，對(duì)于上述掃描匹配算法的具體實(shí)現(xiàn)方式，存在多種不同的方法。

2 ?掃描匹配算法的改進(jìn)

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

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

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

2.2 ?改進(jìn)的ICP算法

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

2.2.1 ?濾波處理

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

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

2.2.2 ?特征點(diǎn)提取

設(shè)相鄰掃描模型分別為[P={p1，p2，…，pM}]和[Q={q1，q2，…，qN}]，其中M和N為點(diǎn)云P和Q的點(diǎn)云數(shù)量。對(duì)參考掃描點(diǎn)云P，計(jì)算出P中每個(gè)點(diǎn)[pi]在不同鄰域下[r1]，[r2]下的法向量[n（pi，r1）]，然后求出兩個(gè)法向量夾角的正弦絕對(duì)值：

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

同理，參考掃描點(diǎn)Q中的點(diǎn)[qi]的正弦值為

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

若sin值較大，則該點(diǎn)所在區(qū)域起伏變化明顯，也就是幾何特征比較突出，因而本文選取的特征點(diǎn)為sin值較大的點(diǎn)。取閾值[ξ]，若閾值小于sin值的點(diǎn)作為特征點(diǎn)。

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

2.2.3 ?基于ROPS的特征點(diǎn)描述

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

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

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

式中：[0≤s，t≤1]且[s+t≤1]，[pi1]，[pi2]，[pi3]為第i個(gè)三角網(wǎng)格上的頂點(diǎn)。第i個(gè)三角網(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個(gè)三角網(wǎng)格的面積與局部表面網(wǎng)格S上所有三角網(wǎng)格總面積之比為[wi1]。特征點(diǎn)在第i個(gè)三角網(wǎng)格形心的距離以及支持半徑r相關(guān)的量為[wi2]，其定義為：

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

在網(wǎng)格S中，特征值分解總體散布矩陣C：

[CV=EV] ? ? ? ? ? ? ? （8）

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

2.3 ?特征點(diǎn)匹配和誤匹配點(diǎn)剔除

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

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

2.4 ?計(jì)算初始配準(zhǔn)參數(shù)

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

3 ?目標(biāo)定位

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

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

旋轉(zhuǎn)矩陣為：

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

平移矩陣為：

[T=txtytzT] ?（11）

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

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

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

4 ?實(shí)驗(yàn)結(jié)果與分析

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

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

5 ?結(jié) ?論

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

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

參考文獻(xiàn)

[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] 黃源，達(dá)飛鵬，陶海躋.一種基于特征提取的點(diǎn)云自動(dòng)配準(zhǔn)算法[J].中國(guó)激光，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] 陳達(dá)梟，蔡勇，張建生.散亂點(diǎn)云精簡(jiǎn)的一種改進(jìn)算法[J].計(jì)算機(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)格下采樣的點(diǎn)云簡(jiǎn)化算法研究[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.