石 鵬，賴際舟，呂 品，鄒冰倩，付相可

(南京航空航天大學(xué)，南京 211106)

0 引言

微小型飛行器憑借其體積小、高機動性和低成本的特點，已經(jīng)被廣泛用于實際生活中，如巡檢、搜救、探測、航拍和地圖構(gòu)建等任務(wù)[1-3]。隨著對飛行任務(wù)需求的不斷增加，對飛行器的自主導(dǎo)航性能提出了越來越高的要求。無人機在室外常用全球定位系統(tǒng)(Global Positioning System，GPS)和慣性進(jìn)行組合導(dǎo)航；在室內(nèi)環(huán)境中，GPS不可用，常用導(dǎo)航手段有視覺導(dǎo)航[4-6]和激光雷達(dá)導(dǎo)航[7-8]。相較于視覺傳感器，激光雷達(dá)是一種主動式感知傳感器，不依賴于外界光照，更加適用于復(fù)雜環(huán)境中微小型飛行器的自主導(dǎo)航。

激光雷達(dá)導(dǎo)航通常通過同步定位與構(gòu)圖技術(shù)(Simultaneous Localization and Mapping，SLAM)實現(xiàn)，激光雷達(dá)SLAM方法經(jīng)過多年的發(fā)展，已經(jīng)被廣泛應(yīng)用于機器人領(lǐng)域。激光雷達(dá)分為二維激光雷達(dá)與三維激光雷達(dá)，受體積、載重、成本等因素的限制，微小型飛行器通常采用二維激光雷達(dá)進(jìn)行SLAM。

目前激光雷達(dá)SLAM中常用掃描匹配的方法估計載體位姿，所謂掃描匹配，就是將當(dāng)前環(huán)境激光掃描點與參考掃描數(shù)據(jù)進(jìn)行配準(zhǔn)[9]。根據(jù)參考掃描數(shù)據(jù)的不同，掃描匹配的方法主要分為兩類：1)相鄰兩幀掃描點匹配；2)當(dāng)前幀掃描點與地圖進(jìn)行匹配。第一種方法中，通過求解相鄰兩幀時間內(nèi)載體的相對位姿變化，然后通過累加的方式計算載體的位姿[10]，這種方法會快速累積位姿算的誤差。第二種掃描匹配方法中，將全局地圖作為參考掃描數(shù)據(jù)，利用當(dāng)前幀掃描點與世界坐標(biāo)系下的地圖進(jìn)行匹配，以此來限制位姿解算誤差的累積[11,13]，這也是目前主流的掃描匹配方法。

就地面機器人而言，通過二維激光雷達(dá)掃描匹配估計位姿的方法已經(jīng)相當(dāng)成熟，但是對于具有六自由度的無人機，這些掃描匹配方法的使用存在著較大的局限性。由于二維雷達(dá)只能掃描一個平面的環(huán)境，而飛行器存在著高度方向的運動以及橫滾角和俯仰角的姿態(tài)變化，導(dǎo)致二維激光雷達(dá)所掃描的二維環(huán)境會發(fā)生突變，且匹配中的參考掃描數(shù)據(jù)與當(dāng)前掃描點不在同一平面，致使匹配存在較大的誤差。針對該問題，目前主要有以下三種應(yīng)對方案：

1)假設(shè)無人機維持在同一高度運動，不存在較大的姿態(tài)運動，周圍障礙物大多為豎直墻面，仍然使用傳統(tǒng)的二維掃描匹配方法估計載體位姿[12]。該方法將三維環(huán)境簡化成二維環(huán)境處理，局限性大。

2)無人機搭載帶有旋轉(zhuǎn)裝置的激光雷達(dá)，通過旋轉(zhuǎn)激光雷達(dá)獲取三維掃描點數(shù)據(jù)，然后利用三維掃描匹配的方法估計載體位姿[13-14]。該方法要求無人機具備足夠的載重能力和能源，并且算法復(fù)雜，對機載計算機的運算能力和存儲提出了苛刻的要求，實際應(yīng)用時的效果不佳。

3)采用2.5維環(huán)境假設(shè)，即假設(shè)環(huán)境中障礙物表面為連續(xù)的豎直面；根據(jù)飛行器的姿態(tài)將二維掃描點數(shù)據(jù)投影于水平面，然后使用二維掃描匹配的方法估計位姿[16-18]。該方法對環(huán)境的構(gòu)成做出了2.5D的假設(shè)，仍然不能解決高程方向激光雷達(dá)感知環(huán)境發(fā)生突變對定位造成的影響。

針對于微小型飛行器，上述方法都不能較好地解決其精準(zhǔn)可靠導(dǎo)航的問題。本文提出了一種利用慣性輔助的激光雷達(dá)Robust-SLAM方法，考慮到微小型飛行器的載重、能耗等限制因素，該方法仍然使用二維激光雷達(dá)掃描匹配方法估計載體位姿，其主要貢獻(xiàn)可總結(jié)為以下兩點：

1)改進(jìn)了激光雷達(dá)SLAM方法的框架，在傳統(tǒng)框架的基礎(chǔ)上，引入了感知環(huán)境突變檢測的環(huán)節(jié)。

2)相對于傳統(tǒng)的慣性與激光雷達(dá)SLAM組合的方法，該方法充分利用慣性信息輔助整個SLAM過程，加強了組合程度。

該方法僅假設(shè)環(huán)境中障礙物表面主要為豎直面，適用于存在多個高低不同豎直平面的環(huán)境，相對于2.5D環(huán)境假設(shè)適用范圍更廣，在高程方向存在感知環(huán)境突變的環(huán)境中，使用本文提出的方法能夠穩(wěn)定可靠地對微小型飛行器進(jìn)行實時精準(zhǔn)的導(dǎo)航。

1 慣性輔助的激光雷達(dá)Robust-SLAM

圖1所示為傳統(tǒng)激光雷達(dá)SLAM方法框架，主要包含3個部分：掃描匹配、參考掃描數(shù)據(jù)更新和構(gòu)圖。本文提出的SLAM方法框圖如圖2所示，相對于傳統(tǒng)方法，該方法的主要改進(jìn)歸納為以下幾點：

1)在利用IMU信息預(yù)測載體位姿的基礎(chǔ)上進(jìn)行掃描匹配。

2)引入突變檢測環(huán)節(jié)，根據(jù)檢測結(jié)果決定濾波器和參考掃描數(shù)據(jù)的更新策略。

3)提出了利用IMU狀態(tài)預(yù)測信息檢測激光雷達(dá)感知環(huán)境是否發(fā)生突變的方法。

4)利用掃描匹配的位姿估計結(jié)果與預(yù)測的狀態(tài)融合之后的結(jié)果更新參考掃描數(shù)據(jù)和構(gòu)圖。

圖1 傳統(tǒng)激光雷達(dá)SLAM方法Fig.1 Typical LIDAR SLAM method

圖2 本文提出的SLAM方法Fig.2 The proposed SLAM method

1.1 狀態(tài)預(yù)測

(1)

(2)

1.2 掃描匹配方法

目前,二維掃描匹配方法發(fā)展成熟，其中迭代最近點(Iterative Closest Point，ICP)算法不依賴于環(huán)境結(jié)構(gòu)化特征，適用范圍廣。本文基于ICP進(jìn)行掃描匹配，ICP具體原理參照文獻(xiàn)[19]，為抑制誤差累積，掃描匹配過程中以二維全局地圖作為參考掃描數(shù)據(jù)。

考慮到飛行器的姿態(tài)運動和高機動性，文中在ICP的基礎(chǔ)上，對掃描匹配進(jìn)行兩點改進(jìn)：1)點云數(shù)據(jù)預(yù)處理中補償飛行器姿態(tài)；2)利用IMU預(yù)測的位姿作為ICP迭代初值。

1.2.1 點云數(shù)據(jù)姿態(tài)補償

記h系為激光雷達(dá)測量理想坐標(biāo)系，其XOY面為水平面，相對于世界系的偏航角與機體系相同。坐標(biāo)系關(guān)系如圖3所示。

圖3 坐標(biāo)系示意圖Fig.3 The general view of the coordinate system

(3)

1.2.2 初值預(yù)測

(4)

利用IMU進(jìn)行預(yù)測迭代初值，具備以下兩點優(yōu)勢：1)加速了ICP匹配迭代過程；2)在一定程度上避免了迭代過程中得到局部最優(yōu)解。

1.3 突變檢測方法

在實際應(yīng)用中，由于飛行器飛行過程中的高度變化，搭載于飛行器上的二維激光雷達(dá)感知的二維環(huán)境結(jié)構(gòu)會發(fā)生突變現(xiàn)象。該突變會造成掃描匹配估計的位姿產(chǎn)生較大誤差，本文提出了一種利用IMU信息檢測該突變現(xiàn)象的方法，構(gòu)建突變檢測函數(shù)fd，若fd≥1則認(rèn)為發(fā)生突變，fd的表達(dá)式為：

(5)

式中，εxk、εyk和εψk根據(jù)式(6)計算：

(6)

(7)

(8)

式(5)中，Pξk為ICP中當(dāng)前幀激光點數(shù)據(jù)在參考掃描數(shù)據(jù)中成功找到對應(yīng)的匹配點的比例值[19]，Eratio為設(shè)置的Pξk對應(yīng)的閾值。Eεx、Eεy和Eεψ分別為設(shè)置的位姿變化誤差εxk、εyk和εψk的閾值，根據(jù)使用IMU預(yù)測狀態(tài)在掃描匹配迭代周期時間內(nèi)的誤差累積情況設(shè)定。

1.4 濾波器更新

本文中通過擴展卡爾曼濾波器(Extended Kalman Filter,EKF)校正利用IMU信息預(yù)測得到的狀態(tài)量，根據(jù)突變檢測環(huán)節(jié)的結(jié)果,確定量測模型選取策略：

Zk=HkXk+μk

(9)

(10)

式中，μk為量測噪聲，偏航角ψ和四元數(shù)之間的關(guān)系如下：

(11)

(12)

(13)

1.5 參考掃描數(shù)據(jù)更新

掃描匹配估計位姿的原理為：利用當(dāng)前掃描數(shù)據(jù)和參考掃描數(shù)據(jù)中的重疊部分進(jìn)行配準(zhǔn)。因此，若激光雷達(dá)感知環(huán)境發(fā)生突變，當(dāng)前掃描到的環(huán)境結(jié)構(gòu)與參考掃描數(shù)據(jù)重疊部分很少，甚至沒有重疊部分，從而導(dǎo)致位姿估計產(chǎn)生較大誤差。

在本文的掃描匹配算法中，選擇二維全局地圖作為參考掃描數(shù)據(jù)，根據(jù)上述的位姿誤差產(chǎn)生原因，參考掃描數(shù)據(jù)更新過程，由突變檢測結(jié)果選擇如下不同的策略：

1)若激光雷達(dá)感知環(huán)境沒有發(fā)生突變，則更新策略為擴增二維全局地圖，插入之前地圖中不存在的激光點數(shù)據(jù)。

2)若檢測到感知環(huán)境發(fā)生突變，則重新初始化二維全局地圖。首先清空二維地圖中的點數(shù)據(jù)，再根據(jù)濾波器更新的狀態(tài)量和當(dāng)前時刻的掃描點數(shù)據(jù)重新構(gòu)建二維全局地圖。

1.6 構(gòu)圖

本文提出的SLAM方法中構(gòu)建的地圖，包括：飛行器所在高度的二維全局地圖和三維全局地圖。其中二維全局地圖作為掃描匹配的參考數(shù)據(jù)，更新過程如1.5節(jié)所述。本文中構(gòu)建的三維地圖為三維柵格地圖，每個立方體元素的大小為5cm×5cm×5cm，使用三維構(gòu)圖工具octomap完成地圖構(gòu)建，octomap中以八叉樹結(jié)構(gòu)存儲地圖，高效利用存儲空間，建立的地圖直觀且分辨率調(diào)節(jié)方便。新的掃描點在世界坐標(biāo)系下的位置坐標(biāo)通過式(14)計算：

(14)

2 飛行實驗與分析

2.1 實驗設(shè)計

圖4所示為實驗的飛行平臺實物圖，以DJI M100飛行器開發(fā)平臺為基礎(chǔ)建立實驗平臺，其飛行控制系統(tǒng)模塊為DJI N1飛控，M100飛行平臺上具備IMU、氣壓計和磁羅盤模塊；機載計算機為DJI Manifold；使用的激光雷達(dá)為單線HOKUYO UTM-30LX，僅具備2D掃描能力，最大探測距離為30m，掃描范圍為270°，更新頻率為40Hz。圖4中,紅色標(biāo)記1傳感器為UWB移動端標(biāo)簽，其二維定位精度為分米級，用于評估本文提出的導(dǎo)航算法的水平定位精度。

圖4 飛行器平臺Fig.4 Aerial vehicle platform

實驗環(huán)境為室內(nèi)車庫，其場景簡化示意圖如圖5所示，其中黃色標(biāo)記1、2對應(yīng)的藍(lán)色模塊為紙箱，標(biāo)記3棕色模塊為車庫中的通風(fēng)管道，灰色模塊為車庫中墻壁。

圖5 實驗環(huán)境示意圖Fig.5 The general view of test environment

為驗證該算法在激光雷達(dá)感知環(huán)境發(fā)生突變時的導(dǎo)航精度，飛行實驗中飛行器將從圖5中標(biāo)記1處的紙箱中央的地面起飛，飛行場景如圖6所示，三維飛行軌跡如圖12中黑色曲線所示。

圖6 飛行場景Fig.6 The flight scene in the parking garage

飛行過程中由于激光束掃描到的障礙物發(fā)生突變，存在以下3處感知環(huán)境發(fā)生突變的區(qū)域：飛行器飛過實驗場景中標(biāo)記1、2 對應(yīng)的紙箱上端，以及3對應(yīng)的通風(fēng)管道上下端的高度區(qū)域。

實驗環(huán)境中搭建了分米級別定位精度的UWB定位系統(tǒng)，用于評估本文提出的導(dǎo)航算法的性能，該系統(tǒng)中移動端發(fā)送其與各節(jié)點的距離信息至PC端，在PC端解算移動端的位置信息，其示意圖如圖7所示。

圖7 UWB定位系統(tǒng)Fig.7 The UWB localization system

2.2 實驗結(jié)果與分析

本文算法主要針對三維環(huán)境中傳統(tǒng)二維激光雷達(dá)SLAM算法存在的定位問題進(jìn)行改進(jìn)，因此主要分析實驗結(jié)果中水平二維的導(dǎo)航結(jié)果。為了更好地分析導(dǎo)航性能，將實驗結(jié)果中本文算法與無載波通信技術(shù)(Ultra Wideband，UWB)和一種傳統(tǒng)的SLAM方法(mrpt-SLAM)進(jìn)行了對比。

圖8～圖10中黑色實線為本文算法結(jié)果，藍(lán)色虛線為mrpt-SLAM結(jié)果，紅色點畫線為UWB定位結(jié)果。圖8所示為飛行實驗的二維飛行軌跡，圖9所示為偏航角結(jié)果；以分米級別精度的UWB定位信息作為室內(nèi)基準(zhǔn)軌跡，傳統(tǒng)方法與本文提出方法的定位誤差如圖10所示，本文算法和mrpt-SLAM的定位均方根誤差(m)分別為[σx,σy]=[0.2,0.2]和[σx,σy]=[4.8,5.0]?？梢姳疚乃惴軌?qū)崿F(xiàn)較為精準(zhǔn)的導(dǎo)航；而mrpt-SLAM導(dǎo)航的誤差較大。

圖8 飛行軌跡Fig.8 The flight trajectory

圖9 偏航角Fig.9 The yaw angle

圖10 位置誤差Fig.10 The position estimation errors

從圖中可以看出，飛行器飛過紙箱上端時，激光雷達(dá)激光束從標(biāo)記1處紙箱突然掃描到標(biāo)記2處紙箱和墻面，感知環(huán)境發(fā)生突變，此時mrpt-SLAM定位誤差突然變大，其偏航角結(jié)果也發(fā)生了突變；與此同時，在之后的飛行過程中，由于mrpt-SLAM并未對其參考掃描數(shù)據(jù)(二維全局地圖)[19]進(jìn)行修正，其導(dǎo)航誤差逐漸累積。本文算法經(jīng)過突變檢測環(huán)節(jié)及相應(yīng)的參考掃描數(shù)據(jù)更新，解決了環(huán)境突變造成的影響，從而實現(xiàn)了精準(zhǔn)魯棒的位姿估計。

飛行過程中，本文算法突變檢測函數(shù)結(jié)果如圖11所示，一共檢測到5個時間段發(fā)生突變(數(shù)字標(biāo)記處)，分別對應(yīng)圖12中的5個數(shù)字標(biāo)記處的飛行區(qū)域；對應(yīng)飛行環(huán)境如圖5所示，按時間順序5個發(fā)生突變的飛行過程：1)上升經(jīng)過紙箱上端；2)上升經(jīng)過標(biāo)記2紙箱上端；3)下降經(jīng)過標(biāo)記2紙箱上端的對應(yīng)高度；4)上升經(jīng)過標(biāo)記2紙箱上端；5)上升和下降經(jīng)過標(biāo)記3通風(fēng)管道下端高度。結(jié)果表明，利用本文提出的突變檢測算法能夠有效檢測出激光雷達(dá)感知發(fā)生突變的情況。

圖11 突變檢測函數(shù)值Fig.11 The value of the abrupt change detection function

圖12所示為本文算法的三維構(gòu)圖，立方體元素的顏色代表不同高度，紅色對應(yīng)最大高度，圖中黑線為飛行器的三維飛行軌跡，數(shù)字標(biāo)記處為激光雷達(dá)感知環(huán)境發(fā)生突變的區(qū)域。對應(yīng)圖5和圖6的飛行環(huán)境圖可見，本算法構(gòu)建的地圖與實際情況一致，能夠較好地反映出飛行環(huán)境的三維結(jié)構(gòu)。

綜上所述，通過使用本文算法能夠解決激光雷達(dá)感知環(huán)境突變對導(dǎo)航造成的影響，實現(xiàn)微小型飛行器在三維環(huán)境中的精準(zhǔn)可靠導(dǎo)航。

圖12 三維構(gòu)圖結(jié)果Fig.12 The 3D map

3 結(jié)論

本文針對使用激光雷達(dá)SLAM的微小型飛行器進(jìn)行自主導(dǎo)航的問題展開了研究，提出了一種基于慣性輔助的激光雷達(dá)Robust-SLAM方法，解決了在高程方向激光雷達(dá)感知環(huán)境突變時位姿估計誤差大的問題。主要內(nèi)容為以下幾點：

1)在SLAM中引入激光雷達(dá)感知環(huán)境突變檢測，并結(jié)合IMU信息和匹配中點配對率，構(gòu)建突變檢測函數(shù)。

2)根據(jù)檢測結(jié)果實施不同的濾波器更新策略和SLAM中參考掃描數(shù)據(jù)更新策略。

3)利用濾波后的位姿信息進(jìn)行三維構(gòu)圖。

在室內(nèi)車庫進(jìn)行了飛行實驗，將本文算法的導(dǎo)航結(jié)果與分米級別精度的UWB定位系統(tǒng)結(jié)果進(jìn)行對比，表明本文提出的方法能夠有效實現(xiàn)微小型飛行器在三維空間中的精準(zhǔn)可靠導(dǎo)航。在后續(xù)的工作中，將會針對激光雷達(dá)SLAM中回環(huán)檢測部分展開研究，進(jìn)一步提高SLAM導(dǎo)航的魯棒性和精度。