廖自威，李榮冰，雷廷萬2，杭義軍

（1.南京航空航天大學(xué)導(dǎo)航研究中心，南京210016；2.成都飛機(jī)設(shè)計(jì)研究院，成都610041）

基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法

廖自威1，李榮冰1，雷廷萬2，杭義軍1

（1.南京航空航天大學(xué)導(dǎo)航研究中心，南京210016；2.成都飛機(jī)設(shè)計(jì)研究院，成都610041）

為實(shí)現(xiàn)室內(nèi)結(jié)構(gòu)化環(huán)境中僅依靠激光雷達(dá)數(shù)據(jù)進(jìn)行實(shí)時(shí)自定位并創(chuàng)建精確的特征地圖，提出了一種基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法。幾何特征關(guān)聯(lián)與匹配方法的優(yōu)劣很大程度上影響SLAM的實(shí)時(shí)性和精度。結(jié)合室內(nèi)結(jié)構(gòu)化環(huán)境的特點(diǎn)，提出了一種完備端點(diǎn)定義與提取方法，將直線段特征與完備端點(diǎn)進(jìn)行關(guān)聯(lián)，優(yōu)化幾何特征掃描匹配過程。此外，姿態(tài)角收斂是SLAM進(jìn)行機(jī)器人位姿估計(jì)和求解一致性的關(guān)鍵。為確保姿態(tài)角準(zhǔn)確收斂，采用了基于直線擬合認(rèn)知的姿態(tài)角加權(quán)幾何平均求解方法。實(shí)驗(yàn)證明，提出的SLAM方法得到的定位精度在100mm內(nèi)，建圖精度也較高，能勝任室內(nèi)SLAM。

激光雷達(dá)；掃描匹配；特征關(guān)聯(lián)；即時(shí)定位與地圖構(gòu)建

0　引言

機(jī)器人處于未知的室內(nèi)結(jié)構(gòu)化環(huán)境中時(shí)，GPS失去導(dǎo)航作用，而慣性元件在航位推算過程中存在累計(jì)誤差，此時(shí)機(jī)器人的自定位能力就顯得尤為重要。因此通常利用外部傳感器多次觀測(cè)的強(qiáng)相關(guān)性，進(jìn)行室內(nèi)結(jié)構(gòu)化未知環(huán)境的即時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）［1］。

SLAM研究方法主要分為基于概率與基于掃描匹配（Scan-Matching）的方法，其中，基于概率的方法包括基于卡爾曼濾波（KF）的SLAM和基于粒子濾波（PF）的SLAM［2］。基于KF的SLAM算法近年來得到了廣泛的關(guān)注，但是隨著時(shí)間狀態(tài)維數(shù)增加，計(jì)算量增大，并且難以有效解決閉環(huán)回路問題?；赑F的SLAM算法能很好地應(yīng)用于非線性模型，有效解決閉環(huán)回路問題，但是存在粒子重采樣及粒子衰竭等問題?；趻呙杵ヅ涞腟LAM算法簡(jiǎn)單，其基本思想是計(jì)算相鄰兩幀掃描數(shù)據(jù)的2維姿態(tài)轉(zhuǎn)換矩陣并更新地圖［3-4］。此外，掃描匹配算法的結(jié)果也可作為基于KF或基于PF的SLAM算法的狀態(tài)擴(kuò)充。

掃描匹配實(shí)現(xiàn)相對(duì)簡(jiǎn)單，但它受到激光雷達(dá)傳感器累積噪聲的影響，往往需要在定位建圖精度及算法實(shí)時(shí)性之間權(quán)衡，因此出現(xiàn)了不同掃描匹配的方法，其中大部分都是通過迭代實(shí)現(xiàn)的，如ICP及其改進(jìn)算法等。ICP算法通常用于點(diǎn)與點(diǎn)的局部匹配，盡管許多學(xué)者對(duì)ICP算法進(jìn)行了研究改進(jìn)［5］，但由于需要多次迭代求解最優(yōu)解，實(shí)時(shí)性待優(yōu)化，仍然限制了該方法在SLAM實(shí)現(xiàn)中廣泛有效的應(yīng)用。文獻(xiàn)［6］是基于特征和特征的掃描匹配方式實(shí)現(xiàn)的，但在進(jìn)行特征點(diǎn)匹配時(shí)采用復(fù)雜的相似三角形匹配算法，算法復(fù)雜度較高，且沒有充分利用直線段特征與點(diǎn)特征的相關(guān)性特點(diǎn)。文獻(xiàn)［7］雖然采用聚類思想將一幀激光雷達(dá)掃描點(diǎn)進(jìn)行區(qū)域分割，但是在處理每一個(gè)區(qū)域的點(diǎn)簇時(shí)，對(duì)點(diǎn)簇中每個(gè)點(diǎn)都進(jìn)行了掃描點(diǎn)關(guān)聯(lián)微段求取，這種基于點(diǎn)與點(diǎn)的全局特征匹配對(duì)SLAM的實(shí)時(shí)性是不利的。

針對(duì)室內(nèi)結(jié)構(gòu)化環(huán)境存在廣泛的直線段、角點(diǎn)等特征信息以及分布稀疏的特點(diǎn)，本文提出了基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法，利用二維激光雷達(dá)完成了工程化實(shí)現(xiàn)。該方法首先提取結(jié)構(gòu)化室內(nèi)環(huán)境中直線段特征和充分有效的角點(diǎn)特征，在此基礎(chǔ)上，提出完備端點(diǎn)定義和提取的方法，將直線段特征與完備端點(diǎn)特征進(jìn)行關(guān)聯(lián)，優(yōu)化幾何特征匹配過程。由于姿態(tài)角收斂是SLAM進(jìn)行機(jī)器人位姿估計(jì)和求解一致性的關(guān)鍵［8］，在姿態(tài)角求取時(shí)，通過加權(quán)幾何平均求解的方法確保姿態(tài)角準(zhǔn)確收斂。采用了基于 “直線段匹配、姿態(tài)角求取、完備端點(diǎn)匹配、位移量求取”策略的全局掃描匹配SLAM方法，根據(jù)幾何特征匹配規(guī)則，篩選出匹配好的幾何特征后便進(jìn)行位姿解算。本文基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法，僅利用激光雷達(dá)數(shù)據(jù)，避免了基于概率SLAM方法計(jì)算復(fù)雜度隨時(shí)間增加的問題，滿足SLAM實(shí)時(shí)性需求，且位姿解算和建圖精度均較高。

1　基于室內(nèi)結(jié)構(gòu)化環(huán)境的特征描述

1.1激光雷達(dá)數(shù)據(jù)預(yù)處理

在提取室內(nèi)環(huán)境特征之前，需要對(duì)激光雷達(dá)數(shù)據(jù)進(jìn)行預(yù)處理，主要包括激光雷達(dá)數(shù)據(jù)點(diǎn)集的平滑與區(qū)域分割。為了盡量消除激光雷達(dá)原始數(shù)據(jù)本身受激光雷達(dá)器件噪聲的影響，對(duì)激光雷達(dá)數(shù)據(jù)點(diǎn)集進(jìn)行中值濾波平滑處理。

區(qū)域分割是根據(jù)激光雷達(dá)掃描點(diǎn)的位置分布情況劃分可能屬于同一個(gè)物體的分割段。激光雷達(dá)掃描點(diǎn)依托于分割段能表達(dá)更多的信息，分割段則表征激光雷達(dá)掃描點(diǎn)整體分布情況。激光雷達(dá)數(shù)據(jù)點(diǎn)集的區(qū)域分割方法參照文獻(xiàn)［9］，為了保證載體每次不同角度和距離都能進(jìn)行準(zhǔn)確的區(qū)域分割，采用了自適應(yīng)閾值的區(qū)域分割方法。設(shè)其中用于分割判斷的增量閾值為為自適應(yīng)比例系數(shù)，初始增量閾值計(jì)算公式為:

1.2完備端點(diǎn)特征定義與提取

幾何特征關(guān)聯(lián)與匹配的優(yōu)劣很大程度上影響了SLAM方法的實(shí)時(shí)性和精度。為避免基于EKF 的SLAM方法難以保證實(shí)時(shí)性的缺陷，結(jié)合室內(nèi)結(jié)構(gòu)化環(huán)境的特點(diǎn)，提出了完備端點(diǎn)定義與提取的方法，將直線段特征與完備端點(diǎn)進(jìn)行特征關(guān)聯(lián)，優(yōu)化完備端點(diǎn)特征匹配過程。

激光雷達(dá)原始數(shù)據(jù)點(diǎn)集采用基于平面掃描激光雷達(dá)自適應(yīng)環(huán)境特征提取方法［9］，通過自適應(yīng)閾值的區(qū)域分割和基于直線段分割的迭代折線角點(diǎn)求解方法充分有效提取出室內(nèi)結(jié)構(gòu)化環(huán)境的直線、分裂點(diǎn)與角點(diǎn)特征。完備端點(diǎn)提取在此基礎(chǔ)上進(jìn)行。

完備端點(diǎn)定義:屬于某一區(qū)域分割后的物體點(diǎn)集被相鄰另一區(qū)域分割后的物體點(diǎn)集遮擋，當(dāng)激光雷達(dá)位姿發(fā)生改變時(shí)，被遮擋物體點(diǎn)集的分裂點(diǎn)相對(duì)位姿發(fā)生變化，具有這樣特性的分裂點(diǎn)稱為非完備端點(diǎn)；而遮擋住該物體的另一物體點(diǎn)集的分裂點(diǎn)相對(duì)位姿不隨激光雷達(dá)位姿改變而改變，像這樣的分裂點(diǎn)稱為完備端點(diǎn)。

設(shè)D｛Di|i=1，2，…｝表示呈直線分布的點(diǎn)集。其中，n為點(diǎn)集D的個(gè)數(shù)。使用最小二乘法對(duì)D進(jìn)行擬合，可獲得相應(yīng)直線段特征，這些獲得的直線段端點(diǎn)通常并不是點(diǎn)集Di的端點(diǎn)。完備端點(diǎn)從Di的端點(diǎn)中提取，而Di又被成對(duì)的分裂點(diǎn)或角點(diǎn)分割開，因而完備端點(diǎn)是分裂點(diǎn)和角點(diǎn)的篩選及重新集合。若完備端點(diǎn)用點(diǎn)集PP｛PPi|i=1，2，…｝表示，則角點(diǎn)CP｛CPi|i=1，2，…｝和分裂點(diǎn)BP｛BPi|i=1，2，…｝可以按照一定的處理規(guī)則將滿足條件的點(diǎn)轉(zhuǎn)化到PP中。從完備端點(diǎn)劃分的定義可知，角點(diǎn)CP兩邊的物體不會(huì)相互遮擋，因而CP?PP。而BP因?yàn)閮蛇吙赡艽嬖谡趽鮿t僅有部分包含于PP。

圖1為激光雷達(dá)完備端點(diǎn)提取示意圖，其中包含D1、D2和D3三個(gè)呈直線狀分布的點(diǎn)集，同時(shí)也包含兩個(gè)分裂點(diǎn)和一個(gè)角點(diǎn)。完備端點(diǎn)進(jìn)行定義描述時(shí)雖然使用了不同幀的激光雷達(dá)數(shù)據(jù)進(jìn)行了說明，但是完備端點(diǎn)的提取僅僅使用一幀激光雷達(dá)數(shù)據(jù)即可完成。一幀激光雷達(dá)數(shù)據(jù)中，在兩個(gè)相鄰的分割區(qū)域的分界處，距離激光雷達(dá)較近的分裂點(diǎn)即為完備端點(diǎn)，同時(shí)，較遠(yuǎn)的分裂點(diǎn)則被確定為非完備端點(diǎn)。如圖1所示，點(diǎn)集D1的分裂點(diǎn)劃分為D1的非完備端點(diǎn)，而點(diǎn)集D2的分裂點(diǎn)劃分為D2的完備端點(diǎn)1。D2與D3之間的角點(diǎn)特征則直接劃分為完備端點(diǎn)2。

圖1　激光雷達(dá)完備端點(diǎn)提取示意圖Fig.1 Extracted diagram of perfect endpoints of laser ladar

1.3直線段特征匹配度

室內(nèi)結(jié)構(gòu)化環(huán)境存在廣泛的直線段、角點(diǎn)等特征信息，且其分布稀疏。采用最小二乘法提取直線段，其特征Fl如下:

其中，a和b分別表示擬合直線在當(dāng)前激光雷達(dá)坐標(biāo)系下的斜率和截距；Pstart、Pend、Pmiddle分別表示擬合直線的起點(diǎn)、終點(diǎn)以及重心點(diǎn)（亦即中點(diǎn)）；l表示該直線段的長(zhǎng)度；θl表示直線段在當(dāng)前激光雷達(dá)坐標(biāo)系下與x軸橫軸的夾角；PP表示直線段兩端的完備端點(diǎn)，表征直線段特征與完備端點(diǎn)特征相關(guān)聯(lián)。

將度量?jī)蓭す饫走_(dá)原始數(shù)據(jù)的匹配度轉(zhuǎn)化為度量?jī)蓭瑪?shù)據(jù)的直線特征與完備端點(diǎn)特征的匹配度。首先，分別對(duì)兩幀激光雷達(dá)數(shù)據(jù)的任意直線段L1和L2進(jìn)行匹配度計(jì)算。在這之前，將L1與L2轉(zhuǎn)換到相同的坐標(biāo)系?？紤]到全局坐標(biāo)系的統(tǒng)一性，將L1與L2均轉(zhuǎn)化到全局坐標(biāo)系，使L1與L2坐標(biāo)系統(tǒng)一到一起。二者的匹配程度表示如下:

其中，

式中，lmin為可用于匹配的直線段的最短要求長(zhǎng)度，DLmax為可匹配的兩直線段最大重心距離差，Δθmax為可匹配的兩直線段最大夾角差。Rl1和Rl2分別表示直線段L1和L2的長(zhǎng)度比例，Rm表示直線段L1和L2之間的重心匹配度，Rθ表示直線段L1和L2之間的夾角匹配度。直線段匹配度Ml反應(yīng)了兩直線段之間的幾何空間分布的匹配程度，由其定義可知，Ml∈［0，1］且Ml越大，直線段匹配度越高。

1.4完備端點(diǎn)特征匹配度

直線段匹配成功后，便可求得穩(wěn)定收斂的當(dāng)前幀激光雷達(dá)的姿態(tài)角θk。此時(shí)，在局部完備端點(diǎn)坐標(biāo)轉(zhuǎn)換時(shí)，將上一時(shí)刻定位結(jié)果 Pk-1（θk-1，xk-1，yk-1）中的θk-1用θk替換，將當(dāng)前幀激光雷達(dá)的完備端點(diǎn)轉(zhuǎn)化到全局坐標(biāo)系中。

完備端點(diǎn)匹配依附在直線段匹配的基礎(chǔ)上，完備端點(diǎn)與直線段關(guān)聯(lián)使得完備端點(diǎn)的匹配度計(jì)算更加高效。由于激光雷達(dá)的噪聲影響，在進(jìn)行直線段特征的點(diǎn)集分割時(shí)，本該是完整的一條直線段L可能會(huì)被分裂為兩條分開的直線段La和Lb，不妨設(shè)Lb為較長(zhǎng)直線段。當(dāng)La和Lb長(zhǎng)度差異較大時(shí)，在直線段特征匹配中，存在Lb與全局地圖中相應(yīng)的直線段L成功匹配的可能性。這種情況下Lb與L雖然都分別存在兩個(gè)完備端點(diǎn)，但其中只有一對(duì)是匹配的。因此，需要對(duì)完備端點(diǎn)進(jìn)行進(jìn)一步的匹配。完備端點(diǎn)匹配程度表示如下:

其中，

R為完備端點(diǎn)PP和激光雷達(dá)掃描點(diǎn)之間的距離大小，ω表示完備端點(diǎn)可匹配的最大允許間距DPmax占R的比重。Rp表示兩完備端點(diǎn)進(jìn)一步匹配的程度。由Mp定義可知，Mp∈［0，1］，且Mp越大，完備端點(diǎn)匹配程度越高。

2　基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM算法設(shè)計(jì)

2.1室內(nèi)結(jié)構(gòu)化環(huán)境下SLAM方案設(shè)計(jì)SLAM算法的目標(biāo)是根據(jù)當(dāng)前幀激光雷達(dá)獲取的室內(nèi)環(huán)境距離參數(shù)解算得到環(huán)境特征fi=｛fi|i= 1，2，…｝以及機(jī)器人的當(dāng)前位姿狀態(tài)Pk（θk，xk，yk）。其中，θk為當(dāng)前幀激光雷達(dá)相對(duì)于全局坐標(biāo)系的旋轉(zhuǎn)量，（xk，yk）為當(dāng)前幀激光雷達(dá)相對(duì)于全局坐標(biāo)系的位移量。提出的SLAM方案結(jié)構(gòu)圖如圖2所示。

圖2　SLAM方案結(jié)構(gòu)圖Fig.2 The solution structure of SLAM

整體方案主要分為三部分，實(shí)現(xiàn)流程如下:

1）特征提取:從激光雷達(dá)原始數(shù)據(jù)中提取室內(nèi)局部直線段和角點(diǎn)特征，進(jìn)一步提取出完備端點(diǎn)，建立初始全局特征地圖。

2）特征匹配與位姿解算:采用 “直線段匹配、姿態(tài)角求取、完備端點(diǎn)匹配、位移量求取”的全局掃描匹配策略，通過基于直線擬合認(rèn)知的姿態(tài)角加權(quán)幾何平均方法保證姿態(tài)角求解的穩(wěn)定收斂，利用完備端點(diǎn)與直線段相關(guān)聯(lián)的匹配方法優(yōu)化完備端點(diǎn)匹配過程，最終得到機(jī)器人穩(wěn)定收斂的位姿信息。

3）全局地圖更新:采用緩沖全局特征地圖的篩選方法優(yōu)化建立全局特征地圖的精度。

2.2基于直線擬合認(rèn)知的姿態(tài)角加權(quán)幾何平均方法

采用 “直線段匹配、姿態(tài)角求取、完備端點(diǎn)匹配、位移量求取”的全局特征掃描匹配與位姿解算策略。根據(jù)幾何特征匹配規(guī)則，篩選出匹配好的幾何特征后便立即進(jìn)行位姿解算。

二維平面內(nèi)當(dāng)前幀激光雷達(dá)定位結(jié)果用Pk（θk，xk，yk）表示。全局地圖w中得到的直線段和完備端點(diǎn)特征總體表述為fw=｛fwi|i=1，2，…，n1｝，其中，n1為全局地圖中的特征的個(gè)數(shù)。當(dāng)前幀激光雷達(dá)的局部坐標(biāo)系l下總體特征表述為fl=｛flj|j=1，2，3，…，n2｝，其中，n2為當(dāng)前幀激光雷達(dá)局部特征個(gè)數(shù)。匹配前，用上一幀定位結(jié)果Pk-1（θk-1，xk-1，yk-1）將fl轉(zhuǎn)換為全局特征f′w=｛f′wj|j=1，2，3，…，n2｝。f′w中分別包含直線段特征L′w與完備端點(diǎn)特征PP′w。匹配的過程就是從f′w中找出與fw一致的直線段特征和完備端點(diǎn)特征。匹配成功的直線段特征用于求取當(dāng)前幀激光雷達(dá)的姿態(tài)角θk，在完成姿態(tài)角求解的基礎(chǔ)上，進(jìn)行完備端點(diǎn)匹配度計(jì)算。匹配成功的完備端點(diǎn)特征則用于求取當(dāng)前幀激光雷達(dá)的位移量（xk，yk）。

設(shè)（Lli，Lwj）表示當(dāng)前幀激光雷達(dá)數(shù)據(jù)在局部坐標(biāo)系下的第i條直線段Lli與全局地圖中第j條直線段Lwj匹配成功后得到的匹配對(duì)。假設(shè)成功匹配m條當(dāng)前幀擬合直線段Lli，則相應(yīng)求得m個(gè)姿態(tài)角θki。

姿態(tài)角θk準(zhǔn)確收斂關(guān)系到整個(gè)定位結(jié)果的一致收斂，準(zhǔn)確求取θk顯得至關(guān)重要。θki中不可避免地包含有誤差，每一條匹配成功的當(dāng)前幀直線段擬合可信度不同，因而不能直接對(duì)姿態(tài)角θki求均值。令匹配成功的當(dāng)前幀擬合直線段Lli的最小二乘擬合殘差為Si，其擬合點(diǎn)集的點(diǎn)數(shù)為Ni。直線段Lli殘差Si越小，且擬合點(diǎn)數(shù)Ni越多，則直線段擬合可信度和準(zhǔn)確度越高?；诖朔椒▽?duì)姿態(tài)角θki進(jìn)行加權(quán)求解，θk滿足式:

式中，ρ1為擬合點(diǎn)數(shù)N所占權(quán)重，θNk為通過Ni計(jì)算得到的姿態(tài)角；ρ2為直線段擬合殘差S所占權(quán)重，θSk為通過Si計(jì)算得到的姿態(tài)角。θNk與θSk由式（5）求得:

式中，i=1，2，3，…，m。按照式（3）進(jìn)行完備端點(diǎn)匹配度計(jì)算，假設(shè)最終有q對(duì)好的完備端點(diǎn)匹配對(duì)（PPli，PPwi），則得到的q個(gè)位移量求解值滿足式（6）:

最終得到的當(dāng)前幀激光雷達(dá)平移量 （xk，yk）為:

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

3.1實(shí)驗(yàn)硬件設(shè)計(jì)

為了驗(yàn)證本文基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM算法的實(shí)際性能，設(shè)計(jì)了激光雷達(dá)導(dǎo)航系統(tǒng)實(shí)驗(yàn)平臺(tái)，用該平臺(tái)模擬機(jī)器人在室內(nèi)某一平面內(nèi)的運(yùn)動(dòng)過程。激光雷達(dá)測(cè)距范圍0.1 m～30m，探測(cè)角分辨率為0.25°，所構(gòu)成的激光雷達(dá)同步定位與建圖系統(tǒng)示意圖如圖3所示。

圖3　系統(tǒng)硬件設(shè)計(jì)連接框圖Fig.3 Connection diagram of the system hardware design

3.2實(shí)驗(yàn)及結(jié)果分析

圖4　室內(nèi)實(shí)驗(yàn)環(huán)境照片F(xiàn)ig.4 Photograph of indoor experimental environment

本文使用的運(yùn)動(dòng)平臺(tái)運(yùn)動(dòng)速度約為1.5m/s，轉(zhuǎn)動(dòng)時(shí)角速度約為150（°）/s。實(shí)驗(yàn)環(huán)境如圖4所示，為大小約10m×7m的長(zhǎng)方形實(shí)驗(yàn)室，實(shí)驗(yàn)環(huán)境中包含有窗戶、墻壁、墻角等常規(guī)室內(nèi)環(huán)境特征，實(shí)驗(yàn)環(huán)境中包含一扇打開的門、墻角以及六扇窗戶。實(shí)驗(yàn)時(shí)，運(yùn)動(dòng)平臺(tái)在如圖5所示的室內(nèi)結(jié)構(gòu)化未知環(huán)境中按照指示軌跡運(yùn)動(dòng)。針對(duì)室內(nèi)環(huán)境的幾何空間大小，直線匹配階段選取的匹配閾值為:最短匹配長(zhǎng)度lmin取30mm，兩直線段最大重心距離差DLmax取為50mm，可匹配的兩直線段最大夾角差Δθmax為10°。在直線匹配成功的基礎(chǔ)上完成姿態(tài)角的求解。圖6是使用本文提出的基于直線段擬合認(rèn)知的姿態(tài)角加權(quán)幾何平均求解方法與姿態(tài)角求取的均值法的結(jié)果對(duì)比。從圖6中可以看出，本文提出的基于認(rèn)知的姿態(tài)角加權(quán)幾何平均方法比均值求解法更加收斂，驗(yàn)證了姿態(tài)角幾何加權(quán)平均求解算法的有效性。

圖5　平臺(tái)運(yùn)動(dòng)軌跡示意圖Fig.5 Diagram of platform motion trajectory

圖6　姿態(tài)角幾何加權(quán)求解法與均值求解法對(duì)比Fig.6 The attitude angle geometric weighted method compared with the average method

為了比較本文完備端點(diǎn)與直線段進(jìn)行特征關(guān)聯(lián)方法的有效性，將與直線匹配關(guān)聯(lián)后的完備端點(diǎn)匹配與單獨(dú)角點(diǎn)匹配結(jié)果進(jìn)行對(duì)比。其中，完備端點(diǎn)匹配與單獨(dú)角點(diǎn)匹配的匹配閾值均采用自適應(yīng)閾值取值，其取值滿足式（8）:

式中，l表示角點(diǎn)或完備端點(diǎn)距離坐標(biāo)原點(diǎn)的距離，單位為mm。實(shí)驗(yàn)時(shí)共采集激光雷達(dá)數(shù)據(jù)幀數(shù)為1887幀。完備端點(diǎn)匹配與單獨(dú)角點(diǎn)匹配結(jié)果如表1和表2所示。

表1　完備端點(diǎn)匹配法Table 1 Perfect points matching

表2　角點(diǎn)匹配法Table 2 Corners matching

完備端點(diǎn)匹配法在直線匹配基礎(chǔ)上進(jìn)行，在所選匹配閾值的約束條件下，完備端點(diǎn)匹配法得到的誤匹配率均為0%。而單獨(dú)的角點(diǎn)匹配方法由于缺少與直線段的關(guān)聯(lián)，當(dāng)激光雷達(dá)運(yùn)動(dòng)轉(zhuǎn)角較大（如第500幀）以及進(jìn)入新的環(huán)境中（如第1500幀）時(shí)，其角點(diǎn)誤匹配率也較大，因而對(duì)定位精度的影響較大。需要說明的是，雖然從完備端點(diǎn)定義上表明角點(diǎn)是完備端點(diǎn)的子集，但是表1中的完備端點(diǎn)并不確定是當(dāng)前幀所有的完備端點(diǎn)，而是在直線匹配的基礎(chǔ)上保留下來的完備端點(diǎn)數(shù)目。因此表1中可匹配的完備端點(diǎn)數(shù)與表2中的所有角點(diǎn)數(shù)并不滿足包含關(guān)系。表1和表2的對(duì)比表明完備端點(diǎn)匹配方法能夠精確篩選出可靠的匹配點(diǎn)對(duì)，完成點(diǎn)集匹配。

圖7是將角點(diǎn)與直線段等幾何特征各自單獨(dú)匹配的掃描匹配SLAM方法的結(jié)果。其中，里圈點(diǎn)集表示運(yùn)動(dòng)軌跡，外圈線段表示環(huán)境特征地圖。可以看出，該SLAM算法定位結(jié)果的偏差大，后半段定位已經(jīng)不能反映平臺(tái)真實(shí)的運(yùn)動(dòng)狀態(tài)，且建圖結(jié)果也出現(xiàn)了明顯錯(cuò)誤。

圖7 幾何特征獨(dú)立匹配SLAM方法結(jié)果Fig.7 The SLAM result of geometric features mapping separately

圖8為使用本文提出的幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法的建圖與定位結(jié)果。雖然無法與理想運(yùn)動(dòng)軌跡進(jìn)行精確對(duì)比，但是從圖8中運(yùn)動(dòng)路線重疊的部分仍然可以看出，本文一步全局匹配SLAM方法的定位結(jié)果精度較高，其誤差在100mm內(nèi)。

圖8　幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法結(jié)果Fig.8 The indoor scan-mapping SLAM result of geometric features association

4　結(jié)論

為實(shí)現(xiàn)室內(nèi)結(jié)構(gòu)化環(huán)境中僅依靠激光雷達(dá)進(jìn)行實(shí)時(shí)自定位并創(chuàng)建精確的特征地圖，提出了一種基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法。幾何特征關(guān)聯(lián)與匹配方法的優(yōu)劣很大程度上影響SLAM的實(shí)時(shí)性和精度。針對(duì)室內(nèi)結(jié)構(gòu)化環(huán)境的特點(diǎn)，提出了完備端點(diǎn)定義與提取方法，將直線段特征與完備端點(diǎn)進(jìn)行關(guān)聯(lián)，優(yōu)化幾何特征掃描匹配過程。此外，姿態(tài)角收斂是SLAM進(jìn)行機(jī)器人位姿估計(jì)和求解一致性的關(guān)鍵。為確保姿態(tài)角準(zhǔn)確收斂，采用了基于直線擬合認(rèn)知的姿態(tài)角加權(quán)幾何平均求解方法。與傳統(tǒng)基于ICP及其改進(jìn)掃描匹配算法不同，采用 “直線段匹配、姿態(tài)角求取、完備端點(diǎn)匹配、位移量求取”策略進(jìn)行SLAM位姿解算，該算法平均每?jī)蓭ㄎ慌c建圖解算時(shí)間在100ms左右，滿足SLAM實(shí)時(shí)性需求。本文基于幾何特征關(guān)聯(lián)的室內(nèi)掃描匹配SLAM方法不需要里程計(jì)進(jìn)行航位推算的位姿估計(jì)，也無需進(jìn)行多傳感器的數(shù)據(jù)融合，僅使用激光雷達(dá)數(shù)據(jù)實(shí)現(xiàn)室內(nèi)SLAM，定位誤差在100mm內(nèi)，且建圖精度也較高，可用于靜態(tài)或動(dòng)態(tài)環(huán)境，勝任室內(nèi)SLAM任務(wù)。在未來的工作上進(jìn)一步與慣性器件進(jìn)行組合濾波，優(yōu)化室內(nèi)組合導(dǎo)航。

［1］ Bailey T，Durrant Whyte H.Simultaneous localization and mapping（SLAM）：Part II［J］.IEEE Robotics and Automation Magazine，2006，13（3）：108-117.

［2］ 祝繼華，鄭南寧，袁澤劍，何永健.基于ICP算法和粒子濾波的未知環(huán)境地圖創(chuàng)建［J］.自動(dòng)化學(xué)報(bào)，2009，35（8）：1107-1113. ZHU Ji-hua，ZHENG Nan-ning，YUAN Ze-jian，HE Yong-jian.A SLAM approach by combining ICP algorithm and particle filter［J］.Acta Automatica Sinica，2009，35 （8）：1107-1113.

［3］ Tsardoulias E，Petrou L.Critical rays scan match SLAM ［J］.Journal of Intelligent&Robotic Systems，2013，72 （3/4）：441-462.

［4］ Nieto J，Bailey T，Nebot E.Scan-slam：combining ekfslam and scan correlation［C］.Field and Service Robotics，Springer Berlin Heidelberg，2006：167-178.

［5］ Holz D，Behnke S.Sancta simplicitas-on the efficiency and achievable results of SLAM using ICP-based incremental registration［C］.Robotics and Automation（ICRA），2010 IEEE International Conference on IEEE，2010：1380-1387.

［6］ 武二永，項(xiàng)志宇，沈敏一，等.大規(guī)模環(huán)境下基于激光雷達(dá)的機(jī)器人 SLAM算法［J］.浙江大學(xué)學(xué)報(bào)（工學(xué)版），2008，41（12）：1982-1986. WU Er-yong，XIANG Zhi-yu，SHEN Min-yi，et al.Robot SLAM algorithm based on laser range finder for large scale environment［J］.Journal of Zhejiang University（Engineering Science），2008，41（12）：1982-1986.

［7］ 郭瑞，孫鳳池，苑晶，等.一種基于幾何統(tǒng)計(jì)特征的全局掃描匹配方法［J］.模式識(shí)別與人工智能，2011，24 （3）：314-320. GUO Rui，SUN Feng-chi，YUAN Jing，et al.A global scan matching method based on geometric statistic features ［J］.Pattern Recognition and Artificial Intelligence，2011，24（3）：314-320.

［8］ 李久勝，李永強(qiáng)，周荻.基于EKF的SLAM算法的一致性分析［J］.計(jì)算機(jī)仿真，2008，25（6）：155-160. LI Jiu-sheng，LI Yong-qiang，ZHOU Di.Analysis of the consistency of EKF-based SLAM ［J］.Computer Simulation，2008，25（6）：155-160.

［9］ 杭義軍，劉建業(yè)，李榮冰，等.基于混合特征匹配的微慣性/激光雷達(dá)組合導(dǎo)航方法［J］.航空學(xué)報(bào)，2014，35 （9）：2583-2592. HANG Yi-jun，LIU Jian-ye，LI Rong-bing，et al.MEMS IMU/LADAR intergrated navigation method based on mixed features matching［J］.Acta Aeronautica et Astronautica Sinica，2014，35（9）：2583-2592.

Indoor Scan-matching SLAM Method Based on Geometric Features Association

LIAO Zi-wei1，LI Rong-bing1，LEI Ting-wan2，HANG Yi-jun1
（1.Navigation Research Center，Nanjing University of Aeronautics and Astronautics，Nanjing 210016；2.Chengdu Aircraft Design and Research Institute，Chengdu 610041）

To realize the real-time positioning and precise mapping which only using ladar data in structured indoor environment，an indoor scan-matching SLAM method based on geometric feature association is proposed.The performance of geometric feature association and matching method greatly influence the timeliness and precision of SLAM.Combined with the characteristics of the structured indoor environment，the perfect endpoints definition and extraction method proposed associate points and lines together which optimize the process of geometric features matching.Besides，the convergence of heading is the key to the consistency of pose estimation and calculation of the robot in SLAM method.To ensure the convergence of the heading，an heading weighted geometric average method based on the cognitive of line fitting was proposed.Experiments shows that with the application of the SLAM solution proposed，the accuracy of the positioning is limited in100mm，and the accuracy of mapping is high enough and compete for indoor SLAM.

laser ladar；scan matching；feature association；simultaneous localization and mapping（SLAM）

TP24

A

1674-5558（2016）01-01132

10.3969/j.issn.1674-5558.2016.03.005

2015-06-03

江蘇省2014年度普通高校研究生科研實(shí)踐計(jì)劃項(xiàng)目（編號(hào):SJLX_0134）。

廖自威，男，碩士，研究方向?yàn)榧す饫走_(dá)數(shù)據(jù)融合與室內(nèi)導(dǎo)航。