雷道豎,劉海波

(1.河南省航空物探遙感中心,河南 鄭州 450053;2.常州機(jī)電職業(yè)技術(shù)學(xué)院，江蘇 常州 213164)

0 引 言

近年來，激光雷達(dá)(Light Detection and Ranging, LIDAR)因其便利、快速、準(zhǔn)確等優(yōu)勢，在航天、林業(yè)、氣象等領(lǐng)域得以廣泛應(yīng)用[1-4]。隨著智能機(jī)器人技術(shù)的發(fā)展，激光測距雷達(dá)在室內(nèi)結(jié)構(gòu)化未知環(huán)境的即時(shí)定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)上發(fā)揮著越來越重要的作用[3-4]。

現(xiàn)有的SLAM技術(shù)主要可分為基于概率和基于掃描匹配的方法。其中，基于概率的方法包括基于卡爾曼濾波和基于粒子濾波的方法[5]?；诳柭鼮V波的方法目前在SLAM技術(shù)中應(yīng)用最為廣泛，但是隨著時(shí)間狀態(tài)維數(shù)的增加，該方法計(jì)算量顯著增大，并且難以有效解決閉環(huán)回路問題?；诹Ｗ訛V波的算法能很好地適用于非線性模型，有效解決閉環(huán)回路問題，但是存在粒子重采樣及粒子衰竭等問題?；趻呙杵ヅ涞腟LAM算法較為簡單，其基本思想是匹配相鄰兩幀掃描數(shù)據(jù)解算兩者之間的2維姿態(tài)轉(zhuǎn)換矩陣從而更新平臺(tái)位置實(shí)現(xiàn)地圖的創(chuàng)建與自主導(dǎo)航[6-7]。

目前大部分的掃描匹配都是通過迭代實(shí)現(xiàn)的，如Interactive Closest Points (ICP)及其改進(jìn)算法等[8]。ICP算法通常用于點(diǎn)與點(diǎn)的局部匹配，盡管許多學(xué)者對ICP算法進(jìn)行了研究改進(jìn)，但這些算法都需要通過最近鄰搜索等方式預(yù)先構(gòu)建有效的點(diǎn)集對應(yīng)關(guān)系。進(jìn)而在確定的對應(yīng)關(guān)系的基礎(chǔ)進(jìn)行迭代，獲取最優(yōu)解。因此，基于ICP的方法運(yùn)算量較大，實(shí)時(shí)性較差，限制了該方法在SLAM技術(shù)中的有效應(yīng)用。

室內(nèi)環(huán)境中存在廣泛的直線段特性[6]。這些特性的引入和利用可以有效提高掃描匹配的效率和性能。文獻(xiàn)[6]基于線段、角點(diǎn)等幾何特征實(shí)現(xiàn)室內(nèi)激光雷達(dá)的掃描匹配，取得了較好的效果。本文利用室內(nèi)掃描數(shù)據(jù)中的線段特性，提出了一種極坐標(biāo)系下的激光雷達(dá)掃描匹配方法。與以往在空間坐標(biāo)系下的線段匹配方法不同，本文直接在極坐標(biāo)下描述掃描數(shù)據(jù)中的線段特征以及構(gòu)建最終的匹配代價(jià)函數(shù)。這種方法有效避免了空間坐標(biāo)系下對點(diǎn)集的對應(yīng)關(guān)系的要求。針對極坐標(biāo)系下的掃描特性，采用周邊環(huán)境面積函數(shù)進(jìn)行描述。該函數(shù)描述了測量平臺(tái)在室內(nèi)不同位置時(shí)各個(gè)角度下的距離測量分布。最后，在極坐標(biāo)系下通過相鄰掃描之間的線段特性構(gòu)建匹配代價(jià)函數(shù)。線段特性的引入使得最終需要優(yōu)化的匹配代價(jià)函數(shù)具有良好的解析形式，從而大大提高了優(yōu)化求解的效率和精度。通過優(yōu)化目標(biāo)函數(shù)，獲得兩幅掃描數(shù)據(jù)之間的2維姿態(tài)變換矩陣，實(shí)現(xiàn)未知環(huán)境中地圖的更新和創(chuàng)建。實(shí)驗(yàn)中，基于公開數(shù)據(jù)集對提出方法進(jìn)行了驗(yàn)證，證明了其有效性。

1 極坐標(biāo)下的掃描匹配的基本思路

如圖1所示，激光雷達(dá)可以采集平臺(tái)在室內(nèi)某一位置時(shí)不同角度的距離測量值。平臺(tái)在室內(nèi)某一位置向周圍各個(gè)角度發(fā)射激光，從而測量其到各個(gè)方向最近障礙物的距離。這些距離值的分布實(shí)際上就反映了平臺(tái)當(dāng)前在室內(nèi)所處的位置。因此，通過分析比對兩幅相鄰掃描數(shù)據(jù)在不同角度下的距離分布可以求解它們之間的相對位姿關(guān)系，從而更新平臺(tái)的位置。為了便于求解，本文首先定義周圍環(huán)境面積函數(shù)如公式(1)所示。

(1)

其中F(φ)代表角度φ下的距離測量值；S為定義的周圍環(huán)境面積。

此時(shí)，相鄰掃描數(shù)據(jù)之間的匹配誤差可以表示為：

(2)

其中F′(φ)代表變換后的掃描結(jié)果。Γ代表兩條掃描之間的姿態(tài)變換矩陣，采用仿射變換的形式，如公式(3)所示。

(3)

其中θ代表旋轉(zhuǎn)角度；tx，ty分別代表位置偏移量。

理論上，通過對公式(2)中的目標(biāo)函數(shù)進(jìn)行優(yōu)化即可求出最佳的變化矩陣。然而，直接對公式(2)進(jìn)行優(yōu)化收斂速度很慢，不利于實(shí)現(xiàn)實(shí)時(shí)的地圖更新。并且，復(fù)雜的目標(biāo)函數(shù)容易陷入局部最優(yōu)解，導(dǎo)致定位精度下降。因此，本文通過考察室掃描數(shù)據(jù)固有的線段特性提高優(yōu)化求解的效率和精度。

2 極坐標(biāo)下的線段描述

室內(nèi)激光雷達(dá)掃描數(shù)據(jù)廣泛存在線段特征。這些線段可以通過直線檢測的方法得以獲取[6,9]。通過利用這些線段特性可以有效提高掃描匹配的效率和精度。在極坐標(biāo)系下，直線可以采用如下形式表達(dá)：

(4)

(5)

對公式(4)中的線段按照公式(3)中的變換矩陣進(jìn)行仿射變換可以得到：

(6)

3 優(yōu)化求解

(7)

(8)

其中Pi(φ)定義如下：

(9)

最終，兩幅掃描數(shù)據(jù)的整體匹配誤差定義為所有線段的部分匹配誤差之后，如公式(10)所示。

(10)

其中C為調(diào)和參數(shù)，定義如下：

(11)

相比直接從公式(2)獲取姿態(tài)變換矩陣，線段特性的引入使得公式(10)中的目標(biāo)函數(shù)形式更為簡潔，便于后續(xù)的優(yōu)化求解。本文采用經(jīng)典的梯度下降法[10]最小化公式(10)中的目標(biāo)函數(shù)從而求解最佳的姿態(tài)變換矩陣。由于目標(biāo)函數(shù)具有良好的解析形式，此時(shí)梯度下降法可以較為迅速地收斂并獲得全局最優(yōu)解。

圖2顯示了在線段表示下兩幅相鄰的掃描數(shù)據(jù)的相對位置關(guān)系。圖中的灰色區(qū)域代表兩者之間的姿態(tài)偏差。通過采用提出算法進(jìn)行優(yōu)化求解，得到在最小誤差時(shí)的匹配結(jié)果如圖4所示。可以看出，此時(shí)求解的姿態(tài)變換矩陣很好地糾正了兩者之間的誤差。

圖2 相鄰兩幅掃描的相對位置關(guān)系(灰色區(qū)域表示它們之間的誤差)

圖3 采用本文方法優(yōu)化得到的匹配結(jié)果

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

4.1 實(shí)驗(yàn)數(shù)據(jù)集

為了驗(yàn)證提出算法的有效性，本文采用公開的因特爾實(shí)驗(yàn)室(Intel Research Lab)數(shù)據(jù)集[11]進(jìn)行了驗(yàn)證實(shí)驗(yàn)。同時(shí)，將提出算法與經(jīng)典的ICP算法[8]以及文獻(xiàn)[6]提出的基于幾何特征關(guān)聯(lián)的算法(記為幾何關(guān)聯(lián))進(jìn)行了性能對比。

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

采用三類方法分別對英特爾實(shí)驗(yàn)室數(shù)據(jù)集中的前1700幅掃描數(shù)據(jù)進(jìn)行處理，得到的結(jié)果分別如圖4中的(a)，(b)，(c)所示。根據(jù)圖中的結(jié)果，選用的1700幅掃描數(shù)據(jù)可以顯現(xiàn)出地圖構(gòu)建和定位過程中出現(xiàn)的累積誤差。從圖中可以直觀得看出，本文算法的定位結(jié)果誤差相對較小，并沒有出現(xiàn)明顯的錯(cuò)誤。另外兩類對比方法的累積誤差相對較大。例如，對于圖4(b)中ICP算法的結(jié)果，它在每一條掃描處均存在較大的誤差，這也導(dǎo)致了地圖繪制的開始和結(jié)束均存在一定程度的失配。

圖4 不同算法上的定位結(jié)果

為了進(jìn)一步體現(xiàn)本文算法的優(yōu)勢，圖5顯示了各類算法累積定位誤差(與已知的真值對比求得)隨掃描數(shù)目的變化曲線。對比而言，本文算法的累計(jì)誤差始終顯著低于其它兩類方法，且隨著迭代次數(shù)的增長率最低。這一結(jié)果表明提出方法在每一次的匹配過程中誤差均較低，即每一次的匹配精度均達(dá)到了較高的水平。表1進(jìn)一步對比了不同算法的平均時(shí)間消耗以及定位誤差?？梢钥闯?，本文方法具有最小的時(shí)間消耗以及定位誤差且顯著低于另外兩類方法。這是因?yàn)榫€段特性的引入使得后續(xù)的優(yōu)化目標(biāo)函數(shù)具有明確的解析表達(dá)形式，因此求解收斂加快從而提高了運(yùn)算效率。同時(shí)，求解過程中，算法陷入局部最優(yōu)解的可能性降低，因此定位精度也相對較高。

圖5 不同算法的累計(jì)定位誤差隨掃描數(shù)目的變化曲線

表1 各類算法的平均時(shí)間消耗和定位誤差

5 結(jié) 語

本文提出一種基于極坐標(biāo)系下激光雷達(dá)掃描匹配的SLAM算法。首先在極坐標(biāo)系下設(shè)計(jì)了環(huán)境面積函數(shù)描述了平臺(tái)在室內(nèi)不同位置是的距離測量值分布，進(jìn)而得到了求解姿態(tài)變換矩陣的目標(biāo)函數(shù)。為了提高求解效率和精度，本文充分利用室內(nèi)掃描數(shù)據(jù)中的線段特性。通過這一特性的引入，使得最終的優(yōu)化函數(shù)具有明確的解析形式，大大方便了后續(xù)的優(yōu)化求解?；诠_的英特爾實(shí)驗(yàn)室數(shù)據(jù)集進(jìn)行了實(shí)驗(yàn)并與其它算法進(jìn)行了性能對比，驗(yàn)證了提出方法的有效性。