何洪磊，賴際舟，呂 品，向林浩，李志敏

(1.南京航空航天大學(xué)，南京 211106； 2.中國船級(jí)社，北京 100007)

0 引言

近年來，隨著科技的進(jìn)步，機(jī)器人行業(yè)得以快速發(fā)展，無人設(shè)備在巡檢、運(yùn)輸?shù)阮I(lǐng)域扮演著重要角色。同時(shí)為了完成愈加復(fù)雜的工作任務(wù)，人們對(duì)其定位導(dǎo)航能力也提出了更高的行業(yè)要求。在該過程中，激光雷達(dá)憑借測距精度高、感知能力強(qiáng)等特點(diǎn)逐漸成為衛(wèi)星拒止環(huán)境下無人設(shè)備的主要導(dǎo)航傳感器之一。

針對(duì)機(jī)器人的室內(nèi)導(dǎo)航問題，當(dāng)前工程上主流的導(dǎo)航方案為：首先通過多源信息融合算法構(gòu)建高精度激光點(diǎn)云地圖[1-3]，隨后將無人設(shè)備實(shí)時(shí)激光雷達(dá)數(shù)據(jù)與構(gòu)建的先驗(yàn)地圖進(jìn)行匹配，從而獲得機(jī)體位姿。該過程首先需將雷達(dá)系與地圖系對(duì)齊，實(shí)現(xiàn)機(jī)體位姿初始化，因此需要通過點(diǎn)云粗配準(zhǔn)[4-5]的方法進(jìn)行機(jī)體的初始定位。點(diǎn)云配準(zhǔn)分為特征提取與位姿解算兩部分，Dong Z.等提出了在算法前端采用基于局部特征描述[6-9]的點(diǎn)云特征提取方法建立點(diǎn)云關(guān)聯(lián)，如三維形狀上下文、點(diǎn)特征直方圖、快速點(diǎn)特征直方圖等，該類方法通常適用于特征豐富的小場景，但在大型的結(jié)構(gòu)化場景下該類特征區(qū)分度較低，導(dǎo)致特征關(guān)聯(lián)的錯(cuò)誤率較高。Zhang X.等提出了基于概率統(tǒng)計(jì)的多元正態(tài)分布變換(Normal Distributions Transform, NDT)[10]，建立了兩幀點(diǎn)云之間的關(guān)聯(lián)，然而受限于特征配準(zhǔn)的收斂性，該方法不適用于位姿差異過大的兩幀點(diǎn)云。上述兩種方法均采用基于迭代的位姿解算方法，計(jì)算量較大，并且在大型結(jié)構(gòu)化場景的點(diǎn)云配準(zhǔn)過程中易陷入局部最優(yōu)[11]，從而產(chǎn)生錯(cuò)誤的導(dǎo)航解算。

因此在結(jié)構(gòu)化環(huán)境中，需要一種魯棒的特征提取方法，S.Ochmann和S.Oesau等提出了空間結(jié)構(gòu)的平面模型化表示方法[12-13]，可以從三維點(diǎn)云中恢復(fù)建筑物的空間環(huán)境結(jié)構(gòu)。在大型結(jié)構(gòu)化場景中，該類特征具有一定魯棒性，然而該類算法復(fù)雜度較高，同時(shí)利用該類特征進(jìn)行無人系統(tǒng)初始定位的方法還未見文獻(xiàn)報(bào)道。

基于此，本文提出了一種基于多平面空間模型的激光雷達(dá)點(diǎn)云配準(zhǔn)方法。首先對(duì)傳統(tǒng)空間結(jié)構(gòu)恢復(fù)方法進(jìn)行改進(jìn)，采用優(yōu)化特征直方圖[14]實(shí)現(xiàn)了空間平面的快速分割與擬合，以及大型結(jié)構(gòu)化空間環(huán)境的模型化表示。隨后為了避免解算過程中的局部最優(yōu)問題，不同于傳統(tǒng)算法中基于迭代的位姿求解方法，本文算法采用空間平面線性匹配方式實(shí)現(xiàn)兩幀點(diǎn)云面特征的快速配準(zhǔn)，提高了計(jì)算效率。

1 基于多平面模型的面特征快速提取方法

三維激光雷達(dá)點(diǎn)云數(shù)據(jù)量龐大，所以需要通過特征提取的方式建立兩幀點(diǎn)云之間的關(guān)聯(lián)。傳統(tǒng)方法對(duì)特征的數(shù)量要求大，增加了算法的整體計(jì)算負(fù)擔(dān)，且在特征稀疏或特征區(qū)分度較小的環(huán)境中，難以保證算法的特征精度。針對(duì)該問題，本文利用結(jié)構(gòu)化環(huán)境特點(diǎn)，采用優(yōu)化的特征直方圖思想實(shí)現(xiàn)了空間點(diǎn)云的快速分割聚類，并根據(jù)平面一致性對(duì)點(diǎn)集進(jìn)行合并，最終實(shí)現(xiàn)了高效、魯棒空間面特征的提取。

1.1 基于特征直方圖的三維點(diǎn)云快速分割方法

首先，根據(jù)李新春等[15]提出的方法對(duì)濾波后激光雷達(dá)點(diǎn)云的法線nk進(jìn)行計(jì)算

(1)

不同于傳統(tǒng)的特征提取算法(如FPFH)，本文算法采用平面作為點(diǎn)云特征，考慮到法向量相同的點(diǎn)可以近似為同一個(gè)平面點(diǎn)，采用優(yōu)化的特征直方圖對(duì)法線特征進(jìn)行快速分析，以實(shí)現(xiàn)點(diǎn)云的粗聚類。將點(diǎn)云法線的三維特征區(qū)間asec、bsec、csec分別等分為m個(gè)特征區(qū)間，并排列組合形成m×m×m個(gè)三維特征區(qū)間，最終構(gòu)建出法線特征直方圖(見圖1)。

圖1 基于特征直方圖的特征區(qū)間劃分方法Fig.1 Feature interval partitioning method based on feature histograms

第i個(gè)特征chari表示的特征區(qū)間為

(2)

(3)

將點(diǎn)云根據(jù)其法向量特征的數(shù)值投放到不同的特征區(qū)間，形成激光雷達(dá)三維掃描特征直方圖，從而實(shí)現(xiàn)三維點(diǎn)云的快速粗分割。

1.2 基于平面一致性的點(diǎn)集合并及平面擬合方法

在式(3)中，當(dāng)閾值m選取較大時(shí)，chari的特征空間較小，同一平面點(diǎn)云易被分割成多個(gè)部分。因此，點(diǎn)云快速粗分割后，需根據(jù)平面一致性將同一平面但不同特征區(qū)間的點(diǎn)云進(jìn)行合并。

選取數(shù)據(jù)量最大的λ組特征區(qū)間內(nèi)的點(diǎn)云進(jìn)行粗?jǐn)M合，歸一化后可得平面方程

(4)

其中，t為chari內(nèi)點(diǎn)云數(shù)據(jù)量；d為方程歸一化常數(shù)。

(5)

(6)

則根據(jù)式(5)計(jì)算兩法線向量pi和pj之間的偏差eij。當(dāng)eij≤δ時(shí)，將兩特征區(qū)間內(nèi)的點(diǎn)云數(shù)據(jù)合并。根據(jù)設(shè)定的空間模型平面數(shù)量，保留點(diǎn)云數(shù)據(jù)量最大的k組點(diǎn)云。最后采用隨機(jī)抽樣一致性(Random Sample Consensus, RANSAC)算法去除各組點(diǎn)云離群點(diǎn)以提高平面魯棒性，并將濾波后的平面點(diǎn)云擬合生成k個(gè)平面，形成平面方程的形式如式(7)

(7)

2 基于多平面模型的特征快速配準(zhǔn)方法

假設(shè)機(jī)體為剛體，所有的運(yùn)動(dòng)變換均為剛體變換，則可利用兩幀點(diǎn)云之間的特征關(guān)聯(lián)建立位姿優(yōu)化函數(shù)，從而求取位姿變換最優(yōu)解。傳統(tǒng)算法采用基于迭代的目標(biāo)函數(shù)優(yōu)化方法，然而該方法計(jì)算量較大，并且當(dāng)特征精度較差或環(huán)境特征區(qū)分度較低時(shí)，迭代算法易陷入局部最優(yōu)。因此，本文基于線性最小二乘對(duì)面特征進(jìn)行配準(zhǔn)。首先通過空間平面排序建立兩幀點(diǎn)云的面特征關(guān)聯(lián)，而后通過線性最小二乘實(shí)現(xiàn)平面的快速配準(zhǔn)，使得算法在提高計(jì)算效率的同時(shí)避免陷入局部最優(yōu)。

2.1 基于排序的特征關(guān)聯(lián)方法

當(dāng)雷達(dá)點(diǎn)云與地圖點(diǎn)云分別擬合出k個(gè)平面后，需要建立兩幀點(diǎn)云平面間的特征關(guān)聯(lián)。然而若兩幀點(diǎn)云平面是無序的，隨機(jī)關(guān)聯(lián)平面特征，則其后續(xù)計(jì)算的時(shí)間復(fù)雜度為O((k-1)!n)；若兩幀點(diǎn)云平面是有序的，則其后續(xù)計(jì)算的時(shí)間復(fù)雜度為O((k-1)n)。因此，為了提高計(jì)算效率，對(duì)點(diǎn)云平面進(jìn)行排序。

首先求取點(diǎn)云平面與XOY平面交線

(8)

(9)

式中

(10)

如圖2所示，若空間平面模型與XOY平面相交為6條直線，根據(jù)式(9)和式(10)計(jì)算可得

gl6>gl4>gl1>gl3>gl5>gl2

(11)

將平面按照位置分?jǐn)?shù)從大到小的順序排序，即可實(shí)現(xiàn)平面的逆時(shí)針有序排列。

圖2 空間平面快速排序方法Fig.2 Spatial plane quicksort method

2.2 基于線性最小二乘的配準(zhǔn)快速解算方法

通過兩幀點(diǎn)云的面特征關(guān)聯(lián)，可形成k-1組有效平面約束。定義第i組對(duì)應(yīng)平面的平面系數(shù)為(ai,1,bi,1,ci,1)與(ai,2,bi,2,ci,2)。選取待求解變量tx、ty、θ，其平面系數(shù)的轉(zhuǎn)換關(guān)系如式(12)

[-ai,1-bi,1ci,2-ci,1]T=

(12)

其中，tx、ty、θ分別表示兩幀點(diǎn)云之間的水平位置差及航向角偏差。傳統(tǒng)基于迭代的目標(biāo)函數(shù)解算方法計(jì)算量較大且易陷入局部最優(yōu)。為了避免該問題，本文采用線性求解的思路進(jìn)行解算

ai,2ci,1tx+bi,2ci,1ty=ci,2-ci,1

(13)

若空間平面中所有平面均垂直于地面，則式(13)為恒等式。因此，構(gòu)建待優(yōu)化函數(shù)如式(14)

(14)

式中

(15)

采用最小二乘擬合算法進(jìn)行解算，可得

Aξ=B

(16)

式中

(17)

(18)

(19)

式(17)中

(20)

2.3 平面配準(zhǔn)的最優(yōu)解選定方法

兩幀點(diǎn)云的點(diǎn)云平面均逆時(shí)針排列后，一共可產(chǎn)生k-1種面特征關(guān)聯(lián)情形。首先通過預(yù)置區(qū)域Φ對(duì)位姿解算結(jié)果進(jìn)行粗篩選，并定義誤差函數(shù)ek對(duì)其配準(zhǔn)度作進(jìn)一步分析，從而提取最優(yōu)解

(21)

(22)

式中

(23)

(24)

將配準(zhǔn)結(jié)果經(jīng)過式(21)和式 (22)解算后，保留誤差最小解即為兩幀點(diǎn)云配準(zhǔn)的最優(yōu)解。

3 實(shí)驗(yàn)驗(yàn)證及分析

本文分別通過Gazebo仿真與室內(nèi)結(jié)構(gòu)化場景模擬實(shí)驗(yàn)對(duì)算法進(jìn)行驗(yàn)證與分析。采樣一致性初始配準(zhǔn)(Sample Consensus Initial Alignment, SAC-IA)與迭代最近點(diǎn)(Iterative Closest Point, ICP)算法是當(dāng)前主流的點(diǎn)云配準(zhǔn)算法，因此，將本文算法與SAC-IA算法及SAC+ICP組合算法的點(diǎn)云配準(zhǔn)結(jié)果進(jìn)行對(duì)比分析。

3.1 Gazebo仿真驗(yàn)證與分析

仿真平臺(tái)采用Gazebo機(jī)器人仿真系統(tǒng)，Gazebo是一款三維動(dòng)力學(xué)仿真軟件，軟件內(nèi)置物理引擎，能夠?qū)Νh(huán)境、無人系統(tǒng)、傳感器進(jìn)行高保真的物理模擬。船艙的近觀檢驗(yàn)[16]是保障船舶安全航行的重要手段，采用無人機(jī)取代人工進(jìn)行船艙檢驗(yàn)具有重大的經(jīng)濟(jì)效益。因此，在Gazebo中根據(jù)船艙真實(shí)環(huán)境構(gòu)建如圖3所示仿真模型，以驗(yàn)證算法的有效性，同時(shí)在仿真環(huán)境中加入障礙物以驗(yàn)證算法的可靠性。

圖3 仿真環(huán)境模型Fig.3 Simulation environment

Gazebo構(gòu)建無人系統(tǒng)包括：

1) hector quad rotor微小型飛行器載體，機(jī)體實(shí)時(shí)發(fā)布位姿真值，以對(duì)本文算法進(jìn)行性能評(píng)估；

2)機(jī)載16線三維激光雷達(dá)，水平角分辨率0.3°、垂直角分辨率2°，水平視場360°、垂直視場±15°，測距誤差±2cm，測距范圍0～100m；

數(shù)據(jù)處理計(jì)算機(jī)采用Linux(Ubuntu 16.04)的機(jī)器人操作系統(tǒng)(Robot Operating System, ROS)，處理器為G2020，主頻2.9GHz。

仿真數(shù)據(jù)采集點(diǎn)如圖4所指示。其中，P0表示地圖構(gòu)建原點(diǎn)，Pi表示激光雷達(dá)數(shù)據(jù)采集點(diǎn)，各點(diǎn)箭頭表示雷達(dá)在該點(diǎn)處x軸指向,黑色方塊表示障礙物位置及其姿態(tài)。

圖4 仿真數(shù)據(jù)采集點(diǎn)示意圖Fig.4 Simulation data acquisition point

(1)地圖點(diǎn)云預(yù)處理

首先對(duì)地圖點(diǎn)云(如圖5(a)所示)進(jìn)行處理。在位姿初始化過程中，僅有鄰近地面的點(diǎn)云參與解算，所以根據(jù)高度值對(duì)地圖點(diǎn)云進(jìn)行截取(如圖5(b)所示)。仿真環(huán)境相對(duì)結(jié)構(gòu)化，噪點(diǎn)較少，故設(shè)定特征區(qū)間數(shù)量m為7，保留平面數(shù)量k為5，通過本文算法對(duì)地圖點(diǎn)云進(jìn)行平面分割(如圖5(c)所示)。由圖可見，本文算法對(duì)地圖點(diǎn)云進(jìn)行了較為精準(zhǔn)的快速分割，并自動(dòng)濾除障礙物點(diǎn)。

(a)地圖點(diǎn)云

(b)截取后地圖點(diǎn)云

(c)地圖點(diǎn)云分割圖5 地圖點(diǎn)云預(yù)處理Fig.5 Map point cloud preprocessing

以激光雷達(dá)地圖點(diǎn)云為目標(biāo)點(diǎn)云，Pi各點(diǎn)采集到的激光雷達(dá)點(diǎn)云為輸入點(diǎn)云，進(jìn)行點(diǎn)云配準(zhǔn)實(shí)驗(yàn)。

(2)配準(zhǔn)精度對(duì)比分析

輸入點(diǎn)云與目標(biāo)點(diǎn)云如表1所示，其中，綠色點(diǎn)云為目標(biāo)點(diǎn)云，紅色點(diǎn)云為輸入點(diǎn)云。

從表1中可以看出，即使有障礙物，本文算法對(duì)三維空間點(diǎn)云仍然可以進(jìn)行較為精準(zhǔn)的分割，實(shí)現(xiàn)了空間結(jié)構(gòu)的模型化處理。

點(diǎn)云配準(zhǔn)后解算位姿與各點(diǎn)位姿參考值如表2所示。

從位置1、2、3、4點(diǎn)云的配準(zhǔn)情況以及定位結(jié)果分析可以看出，SAC-IA算法的定位精度為米級(jí)，航向角平均偏差在2°以上，存在較大誤差；而本文算法定位偏差均小于0.1m，航向角偏差小于0.5°，與SAC +ICP組合算法的全局最優(yōu)解具有相當(dāng)?shù)木鹊燃?jí)。同時(shí)這也表明了本文算法中通過大數(shù)據(jù)點(diǎn)云聚類擬合的面特征具有較高的精度與可靠性。

表1 點(diǎn)云配準(zhǔn)對(duì)比Tab.1 Point cloud registration comparison

表2 位置1～4點(diǎn)云配準(zhǔn)精度對(duì)比Tab.2 Comparison of cloud registration accuracy at position 1～4

傳統(tǒng)算法在解算位置5、6、7、8處位姿的過程中，產(chǎn)生了錯(cuò)誤解算(如表3所示)。這表明傳統(tǒng)算法在特征區(qū)分度較小的大場景空間環(huán)境中容易產(chǎn)生局部最優(yōu)問題。而在該情況下，本文算法依舊保持了較高的定位精度。

表3 位置5～8點(diǎn)云配準(zhǔn)精度對(duì)比

(3)計(jì)算效率對(duì)比分析

本文算法與SAC-IA算法計(jì)算時(shí)間統(tǒng)計(jì)如表4所示。

表4 計(jì)算效率對(duì)比Tab.4 Computational efficiency comparison

由表4可得，本文算法的平均用時(shí)為3.1s，SAC-IA算法的平均用時(shí)為10.8s，SAC+ICP算法的平均用時(shí)為15.1s。所以在仿真實(shí)驗(yàn)中，本文算法的計(jì)算效率約是SAC-IA算法的3.5倍，是SAC +ICP算法的5倍。相較于傳統(tǒng)算法，本文算法減少了特征提取的數(shù)量，在位姿求解過程中，本文算法使用線性最小二乘算法取代了傳統(tǒng)算法位姿解算中采用的非線性最小二乘算法，無需迭代求取最優(yōu)解，從而提高了計(jì)算效率。

3.2 室內(nèi)結(jié)構(gòu)化場景模擬試驗(yàn)驗(yàn)證

實(shí)驗(yàn)環(huán)境及實(shí)驗(yàn)器材如圖6所示。

圖6 實(shí)驗(yàn)環(huán)境Fig.6 Experimental environment

實(shí)驗(yàn)器材包括：

1)8個(gè)高速攝像頭組成的動(dòng)態(tài)捕捉系統(tǒng)，為本文實(shí)驗(yàn)提供位姿基準(zhǔn)(cm級(jí)精度)；

2)VLP-16三維激光雷達(dá)。

計(jì)算平臺(tái)與仿真實(shí)驗(yàn)相同。實(shí)驗(yàn)設(shè)計(jì)如圖7示。

圖7 數(shù)據(jù)采集示意圖Fig.7 Data acquisition point

其中，P0表示地圖構(gòu)建原點(diǎn)，Pi表示激光雷達(dá)數(shù)據(jù)采集點(diǎn)，各點(diǎn)箭頭表示雷達(dá)在該點(diǎn)處x軸指向,黑色十字星表示動(dòng)態(tài)捕捉系統(tǒng)的高速攝像頭位置。

(1) 地圖點(diǎn)云預(yù)處理

構(gòu)建的實(shí)驗(yàn)環(huán)境地圖如圖8所示。

圖8 地圖點(diǎn)云Fig.8 Map point cloud

由于室內(nèi)環(huán)境噪點(diǎn)較多且點(diǎn)云中地面點(diǎn)較少，適當(dāng)調(diào)大特征區(qū)間數(shù)量m，將其設(shè)定為10，保留平面數(shù)量k為4，通過本文算法對(duì)地圖點(diǎn)云進(jìn)行平面分割(如圖9所示)。由圖9可見，在實(shí)際環(huán)境中，本文算法對(duì)點(diǎn)云進(jìn)行了較為精準(zhǔn)的分割。

(a)分割后地圖點(diǎn)云

(b)分割點(diǎn)云俯視圖圖9 地圖點(diǎn)云預(yù)處理Fig.9 Map point cloud preprocessing

(2)配準(zhǔn)精度對(duì)比分析

傳統(tǒng)算法的點(diǎn)云配準(zhǔn)結(jié)果與本文算法的點(diǎn)云分割及配準(zhǔn)結(jié)果如表5所示，其中，綠色點(diǎn)云為地圖點(diǎn)云，紅色點(diǎn)云為各點(diǎn)位的數(shù)據(jù)采集點(diǎn)云。

表5 點(diǎn)云配準(zhǔn)對(duì)比

由表5可見，經(jīng)本文分割算法處理后得到的平面點(diǎn)質(zhì)量較高，其具體的點(diǎn)云配準(zhǔn)精度對(duì)比分析如表6所示。

表6 點(diǎn)云配準(zhǔn)精度對(duì)比

由表6可得，在實(shí)際環(huán)境中，本文算法與SAC+ICP組合算法精度相當(dāng)。然而，傳統(tǒng)算法在位置1處陷入了局部最優(yōu)解，產(chǎn)生了錯(cuò)誤的導(dǎo)航解算方向，在該情況下本文算法仍保持了較高的解算精度。

(3)計(jì)算效率對(duì)比分析

本文算法與傳統(tǒng)算法計(jì)算時(shí)間統(tǒng)計(jì)如表7所示。

表7 計(jì)算效率對(duì)比Tab.7 Computational efficiency comparison

由表7可得，本文算法的平均用時(shí)為1.8s，SAC-IA算法的平均用時(shí)為7.0s，SAC +ICP算法的平均用時(shí)為8.9s。因此，在實(shí)地實(shí)驗(yàn)中，本文算法的計(jì)算效率約是SAC-IA算法的3.9倍，是SAC +ICP算法的5倍，與仿真環(huán)境中計(jì)算效率結(jié)果一致。

4 結(jié)論

針對(duì)傳統(tǒng)點(diǎn)云位姿初始化算法計(jì)算量大、易陷入局部最優(yōu)的問題，本文提出了一種面向大型結(jié)構(gòu)化場景的點(diǎn)云位姿初始化方法。結(jié)合封閉環(huán)境空間模型的特點(diǎn)，對(duì)面特征提取、特征關(guān)聯(lián)、位姿解算策略進(jìn)行了研究。通過Gazebo仿真與室內(nèi)結(jié)構(gòu)化場景進(jìn)行驗(yàn)證，可以得到如下結(jié)論：

1)本文利用結(jié)構(gòu)化環(huán)境特點(diǎn)，采用空間模型化思想進(jìn)行點(diǎn)云特征配準(zhǔn)，避免了無人系統(tǒng)位姿初始化過程中的局部最優(yōu)問題，并且具有較高的位姿解算精度；

2)不同于傳統(tǒng)基于迭代的優(yōu)化方法，本文采用線性最小二乘進(jìn)行位姿求解，提高了算法的計(jì)算效率。