付相可，賴際舟，劉 建，何洪磊，岑益挺，姚義心

(1.南京航空航天大學(xué)，南京 211106； 2.北京空間飛行器總體設(shè)計(jì)部， 北京 100094)

0 引言

隨著科技的進(jìn)步，地面無人車得到了空前的發(fā)展。地面無人車(Unmanned Ground Vehicle，UGV)也在社會(huì)生活中起到越來越重要的作用，在巡檢、探測(cè)等領(lǐng)域扮演著重要的角色。UGV能夠完成這些任務(wù)的前提是進(jìn)行準(zhǔn)確的導(dǎo)航定位。

目前在室外環(huán)境中，無人車的定位主要采用GPS/慣導(dǎo)融合的方法。但是在室內(nèi)、橋洞等GPS不可用的環(huán)境中，需要采用新的自主方式實(shí)現(xiàn)自身的導(dǎo)航定位。在GPS拒止環(huán)境下常用的傳感器主要包括激光雷達(dá)[1-3]、視覺[4-5]、UWB[6-7]等。其中，視覺對(duì)光照的依賴性強(qiáng)，在無光條件下基本不可用；UWB則需要提前對(duì)基站進(jìn)行標(biāo)定，使用復(fù)雜，而且基站的位置精度對(duì)其定位精度有較大的影響。激光雷達(dá)是一種主動(dòng)式的感知傳感器，不依賴外界的光照條件，且測(cè)距精度高，更適合用于UGV在復(fù)雜環(huán)境下的導(dǎo)航定位。

激光雷達(dá)分為二維激光雷達(dá)和三維激光雷達(dá)。受成本等因素的限制，二維激光雷達(dá)在無人車上更為常用。通常使用同步定位與構(gòu)圖(Simultaneous Localization and Mapping，SLAM)技術(shù)進(jìn)行定位。

二維激光雷達(dá)SLAM算法目前通常采用掃描匹配的方法估算無人車的位姿，代表的算法有Hector SLAM[8-9]和MRPT SLAM[10]等。掃描匹配的過程為通過變換幀間位姿關(guān)系將兩幀點(diǎn)云數(shù)據(jù)對(duì)齊，在對(duì)齊過程中的旋轉(zhuǎn)和平移變化就是兩幀之間的位姿變化[11]。但是在實(shí)際巡檢過程中，UGV有時(shí)會(huì)處于結(jié)構(gòu)特征稀少、點(diǎn)云信息不豐富的空曠環(huán)境中，此時(shí)得到的有效點(diǎn)數(shù)量稀少，通過掃描匹配的方法估計(jì)無人車的位姿，會(huì)出現(xiàn)匹配誤差大、精度低的問題，不能滿足定位的精度需求。

針對(duì)該環(huán)境,本文提出了一種合作環(huán)境下基于慣性/里程計(jì)/激光雷達(dá)融合的UGV導(dǎo)航方法，合作環(huán)境的主要特點(diǎn)是結(jié)構(gòu)特征已知。本文主要貢獻(xiàn)可總結(jié)為以下兩點(diǎn)：

1)提出了一種基于幾何結(jié)構(gòu)特征的激光雷達(dá)定位方法，實(shí)現(xiàn)了在點(diǎn)云稀少環(huán)境下的精確定位，并且在動(dòng)態(tài)環(huán)境下的魯棒性較高；

2)通過慣性/里程計(jì)/激光雷達(dá)的融合，提高了導(dǎo)航解算的準(zhǔn)確性及可靠性。

1 合作環(huán)境下激光雷達(dá)定位方法

本文提出的激光雷達(dá)定位方法流程圖如圖1所示，該方法主要利用空間中已知結(jié)構(gòu)體的位置、尺寸和形狀信息，對(duì)UGV進(jìn)行定位。主要包括以下幾個(gè)步驟：

圖1 本文提出的激光雷達(dá)定位方法Fig.1 The proposed positioning method based on the lidar

1)點(diǎn)云數(shù)據(jù)預(yù)處理。完成對(duì)當(dāng)前激光雷達(dá)獲取的掃描點(diǎn)云數(shù)據(jù)聚類，并進(jìn)行角點(diǎn)的判斷，為下一步的匹配奠定基礎(chǔ)。

2)特征匹配。將分割完成的點(diǎn)云數(shù)據(jù)與已知的幾何結(jié)構(gòu)體信息進(jìn)行匹配。

3)求解中心點(diǎn)位置。求解出各個(gè)結(jié)構(gòu)體的中心點(diǎn)，計(jì)算UGV相對(duì)每個(gè)幾何結(jié)構(gòu)體中心點(diǎn)的距離。

4)估計(jì)UGV位姿。通過最小二乘法求解UGV當(dāng)前所處的位置。

1.1 點(diǎn)云數(shù)據(jù)預(yù)處理

點(diǎn)云數(shù)據(jù)預(yù)處理是激光雷達(dá)定位方法的基礎(chǔ)，其準(zhǔn)確性對(duì)后續(xù)導(dǎo)航解算起到?jīng)Q定性作用。該部分主要包括有效點(diǎn)提取、點(diǎn)云數(shù)據(jù)聚類以及角點(diǎn)判斷3個(gè)部分。

提取有效點(diǎn)的目的是為了從點(diǎn)云數(shù)據(jù)中提取處于激光雷達(dá)掃描范圍內(nèi)的數(shù)據(jù)，即提取一定范圍內(nèi)的點(diǎn)云數(shù)據(jù)。為了去除無人車自身帶來的影響，選擇的有效距離范圍為0.4～30m。

點(diǎn)云的聚類方法主要有K-means聚類[12-13]、DBSCAN聚類算法[14]以及最近鄰聚類[15-16],這三種聚類方法的優(yōu)缺點(diǎn)對(duì)比如表1所示。

表1 聚類算法優(yōu)缺點(diǎn)比較

由于本文的任務(wù)是實(shí)現(xiàn)巡檢時(shí)的UGV自主定位，實(shí)驗(yàn)環(huán)境具有以下特點(diǎn)：1)合作環(huán)境中，各個(gè)結(jié)構(gòu)體的位置是固定的；2)結(jié)構(gòu)體的尺寸和形狀是固定的。所以本文使用最近鄰聚類算法進(jìn)行點(diǎn)云分割。最近鄰聚類算法的基本原理如式(1)所示

(1)

式中，ρ(i)和ρ(i+1)是激光雷達(dá)第i個(gè)和第i+1個(gè)有效點(diǎn)；D(ρ(i),ρ(i+1))是相鄰兩點(diǎn)之間的距離；Dth是設(shè)置的閾值，其大小根據(jù)結(jié)構(gòu)體之間的距離進(jìn)行確定。

本文所使用的結(jié)構(gòu)體為長方體，在激光雷達(dá)掃描得到的點(diǎn)云數(shù)據(jù)中，每一部分最多只存在一個(gè)角點(diǎn)，可以根據(jù)點(diǎn)到直線的距離來判斷該部分是否存在角點(diǎn)。

1.2 特征匹配

特征匹配是該激光雷達(dá)導(dǎo)航定位方法的關(guān)鍵，它的準(zhǔn)確性對(duì)后續(xù)步驟起到?jīng)Q定性作用。它依據(jù)無人車的位姿以及物體彼此間的距離關(guān)系進(jìn)行判斷匹配，其具體流程如圖2所示。

圖2 特征匹配流程圖Fig.2 Flow chart of feature matching

圖2中,C_pre的值是由上一時(shí)刻識(shí)別的結(jié)構(gòu)體數(shù)量決定的,是當(dāng)前時(shí)刻選擇特征匹配模式的標(biāo)志位；C的值是由當(dāng)前時(shí)刻識(shí)別的結(jié)構(gòu)體數(shù)量決定的，是下一時(shí)刻選擇特征匹配模式的標(biāo)志位。

模式一的計(jì)算流程如下：

1)根據(jù)無人車在導(dǎo)航系下的位置篩選出可能是已知幾何結(jié)構(gòu)的點(diǎn)云數(shù)據(jù)geari_m(j)(geari_m為第i個(gè)物體，其中j=1,2,3,)；

2)計(jì)算geari_m(j)之間的距離，并與已知的信息進(jìn)行比較，分別從geari_m(j)中選出符合條件次數(shù)最多的點(diǎn)geari_m；

3)計(jì)算geari_m之間的距離，確定是否符合要求；

4)根據(jù)確定已知物體的位置信息，推算其他物體的位置信息，確認(rèn)在點(diǎn)云數(shù)據(jù)中是否存在其他物體。

模式二的計(jì)算流程如下：

1)計(jì)算各類的平均值與上一時(shí)刻結(jié)構(gòu)體在機(jī)體系下的歐式距離，如果小于閾值，則認(rèn)為是同一結(jié)構(gòu)體geari_m；

2)計(jì)算geari_m之間的距離，確定是否符合要求；

3)根據(jù)確定結(jié)構(gòu)體的位置信息，推算其他結(jié)構(gòu)體的位置信息，確認(rèn)在點(diǎn)云數(shù)據(jù)中是否存在其他結(jié)構(gòu)體。

模式三的計(jì)算流程如下：

1)根據(jù)無人車在導(dǎo)航系下的位置篩選出可能是結(jié)構(gòu)體的點(diǎn)云數(shù)據(jù)geari_m(j)；

2)計(jì)算各類的平均值與上一時(shí)刻結(jié)構(gòu)體在機(jī)體系下坐標(biāo)的距離，如果小于閾值，則認(rèn)為是同一結(jié)構(gòu)體gear_known；

3)將geari_m(j)與geari_known進(jìn)行比較,確定其他結(jié)構(gòu)體的位置信息；

4)根據(jù)確定結(jié)構(gòu)體的位置信息，推算其他結(jié)構(gòu)體的位置信息，確認(rèn)在點(diǎn)云數(shù)據(jù)中是否存在其他結(jié)構(gòu)體。

同時(shí)，由于本文所考慮的巡檢環(huán)境是對(duì)稱結(jié)構(gòu)，使用距離進(jìn)行判斷時(shí)，有時(shí)會(huì)將后方的2個(gè)結(jié)構(gòu)體位置判斷錯(cuò)誤，因此需要叉乘法對(duì)其進(jìn)行輔助判斷。

1.3 中心點(diǎn)確定

結(jié)構(gòu)體中心點(diǎn)的位置估計(jì)精度對(duì)導(dǎo)航定位的精度有著直接影響。該部分主要包括直線擬合、直線方向的判定和中心點(diǎn)的坐標(biāo)解算3個(gè)步驟，其流程圖如圖3所示。

圖3 中心點(diǎn)確定流程圖Fig.3 Flow chart for determining the center point

本文是通過最小二乘法進(jìn)行直線擬合的，其目標(biāo)函數(shù)為ax+by-1=0。因?yàn)樵趯?duì)結(jié)構(gòu)體中心點(diǎn)進(jìn)行確定時(shí)需要進(jìn)行直線平移，而結(jié)構(gòu)體的不同面平移量并不相同，所以要進(jìn)行直線方向的判定。

本文使用的直線方向判定方法有兩種，分別如下：

1)當(dāng)可匹配的結(jié)構(gòu)體有2個(gè)或者3個(gè)時(shí)，根據(jù)不同結(jié)構(gòu)體上的直線方程，確定平行的直線，再根據(jù)平行直線間的距離判斷這條直線屬于結(jié)構(gòu)體的哪一側(cè)。

2)根據(jù)結(jié)構(gòu)體不同方向的長度進(jìn)行判斷。

之所以采用兩種方法進(jìn)行直線方向的判定，是由于無人車在行駛過程中會(huì)存在以下兩種情況：1)激光雷達(dá)只能掃描到一個(gè)結(jié)構(gòu)體；2)根據(jù)點(diǎn)云擬合的線段只是結(jié)構(gòu)體一側(cè)的部分信息。

2 慣性/里程計(jì)/激光雷達(dá)融合算法設(shè)計(jì)

單獨(dú)使用激光雷達(dá)估計(jì)無人車的位姿容易受到周圍環(huán)境的影響，導(dǎo)致解算出現(xiàn)誤差甚至無法解算，所以本文引入慣性與里程計(jì)進(jìn)行輔助解算。單獨(dú)慣性或里程計(jì)，雖然其短時(shí)間內(nèi)精度高，但是其精度會(huì)隨著距離而發(fā)散。因此，本文采用擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)算法將里程計(jì)、激光雷達(dá)與慣性傳感器進(jìn)行容錯(cuò)組合，融合算法結(jié)構(gòu)圖如圖4所示。

圖4 慣性/里程計(jì)/激光雷達(dá)融合算法結(jié)構(gòu)圖Fig.4 Structure of inertial/odometer/lidar fusion algorithm

2.1 狀態(tài)預(yù)測(cè)

本文以UGV初始時(shí)刻所在位置為原點(diǎn)，以UGV機(jī)體系為三軸方向構(gòu)建導(dǎo)航坐標(biāo)系，EKF所構(gòu)造的16維狀態(tài)量為

(2)

姿態(tài)四元數(shù)的預(yù)測(cè)采用如下公式

(3)

其中，Δt為離散采樣周期；Q(k)和Q(k-1)分別為k時(shí)刻和k-1時(shí)刻的姿態(tài)四元數(shù)；Ω(k)通過式(4)進(jìn)行計(jì)算

(4)

(5)

速度預(yù)測(cè)采用如下公式

(6)

(7)

位置預(yù)測(cè)采用如下公式

(8)

其中，Pn(k)和Pn(k-1)是k和k-1時(shí)刻UGV在導(dǎo)航系的坐標(biāo)。

陀螺儀零偏和加速度計(jì)零偏預(yù)測(cè)采用如下公式

(9)

(10)

2.2 濾波器更新

本文中通過EKF融合IMU預(yù)測(cè)的狀態(tài)、激光雷達(dá)解算的位姿和里程計(jì)的速度信息，從而獲取更精準(zhǔn)的導(dǎo)航信息。

一步預(yù)測(cè)均方誤差Pk|k-1的計(jì)算公式如下

(11)

式中，Φ是狀態(tài)轉(zhuǎn)移矩陣。

Φk,k-1=

(12)

式中，M4×4、U4×3、N3×4的計(jì)算方法如下所示

(13)

(14)

(15)

濾波器的噪聲矩陣Γ的計(jì)算方法如下所示

(16)

系統(tǒng)的噪聲矩陣為

W=[εwxεwyεwzεaxεayεaz

(17)

系統(tǒng)的濾波增益方程、狀態(tài)估值方程和估計(jì)均方誤差方程如下

(18)

(19)

Pk|k=(I-KkHk)Pk|k-1

(20)

其中，H是量測(cè)矩陣，根據(jù)量測(cè)信息的不同選擇不同的量測(cè)模型。

1)激光雷達(dá)的量測(cè)量為

(21)

其中，x_lidar、y_lidar、ψL為激光雷達(dá)估計(jì)的無人車位姿。量測(cè)方程為

z_lidark=H_lidark·Xk+uk

(22)

(23)

式中，uk為量測(cè)噪聲，偏航角ψL與四元數(shù)的關(guān)系如下

(24)

2)里程計(jì)的量測(cè)量為

(25)

其中，v_odomx和v_odomy為里程計(jì)得到的機(jī)體系下速度通過濾波后的航向角轉(zhuǎn)換到導(dǎo)航系下x、y方向的速度。量測(cè)方程為

z_odomk=H_odomkXk+uk

(26)

(27)

2.3 激光雷達(dá)故障檢測(cè)

在UGV巡檢過程中，激光雷達(dá)會(huì)出現(xiàn)得不到足夠的有效信息的情況，此時(shí)解算誤差大甚至不能進(jìn)行位置解算，也不能夠?qū)⒓す饫走_(dá)的信息作為量測(cè)帶入濾波框架。針對(duì)此問題，本文提出了一種激光雷達(dá)的故障檢測(cè)方法，根據(jù)融合導(dǎo)航算法的輸出結(jié)果以及可識(shí)別的結(jié)構(gòu)體的數(shù)量對(duì)雷達(dá)的解算結(jié)果進(jìn)行判別，故障檢測(cè)具體框架如圖5所示。thr是判定雷達(dá)解算結(jié)果是否正確的一個(gè)閾值，其大小由無人車的行駛速度和慣性器件的輸出頻率決定。

圖5 激光雷達(dá)故障檢測(cè)方法Fig.5 Fault detection method for lidar

3 仿真驗(yàn)證及分析

3.1 仿真環(huán)境設(shè)計(jì)

為了驗(yàn)證本文算法在UGV巡檢中的定位精度，在Gazebo仿真環(huán)境中搭建無人車模型和仿真環(huán)境，同時(shí)將本文所提算法與激光雷達(dá)SLAM進(jìn)行對(duì)比。仿真平臺(tái)如圖6所示。

圖6 仿真平臺(tái)Fig.6 Simulation platform

仿真環(huán)境中各傳感器參數(shù)設(shè)置如下：

1)激光雷達(dá)的探測(cè)距離是25m，掃描角度是-180°～180°，角度分辨率是0.25°；

2)加速度計(jì)的精度為 0.002g，陀螺儀的精度為0.02(°)/s；

3)里程計(jì)的精度為0.6%。

3.2 仿真結(jié)果分析

仿真實(shí)驗(yàn)結(jié)果如圖7和圖8所示，黑色實(shí)線為本文算法結(jié)果，藍(lán)色點(diǎn)線為Hector-SLAM結(jié)果，紅色虛線為真值。圖7為無人車在仿真環(huán)境中的行走軌跡，圖8為x、y方向的位置誤差。本文算法和2D激光雷達(dá)SLAM的定位均方根誤差(m)分別為[σx,σy]=[0.123,0.062]和[σx,σy]=[2.864,6.594]。可見本文算法相對(duì)于傳統(tǒng)2D激光雷達(dá)SLAM方法能夠?qū)崿F(xiàn)較為精準(zhǔn)的導(dǎo)航。

圖7 UGV的行駛軌跡Fig.7 Trajectory of UGV

(a)X軸方向位置誤差

(b)Y軸方向位置誤差圖8 位置誤差Fig.8 Position estimation errors

綜上所述，在特征稀少的合作環(huán)境中，使用本文算法能夠?qū)崿F(xiàn)無人車在巡檢過程中的精準(zhǔn)可靠導(dǎo)航。

4 結(jié)論

本文針對(duì)合作環(huán)境下，無人車巡檢中的自主導(dǎo)航問題展開了研究，提出了一種基于慣性/里程計(jì)/激光雷達(dá)的UGV導(dǎo)航方法，解決了因有效點(diǎn)數(shù)量稀少而導(dǎo)致定位誤差大的問題。主要內(nèi)容為以下幾點(diǎn)：

1)提出了一種基于已知結(jié)構(gòu)特征的激光雷達(dá)定位方法；

2)進(jìn)行了慣性/里程計(jì)/激光雷達(dá)的融合算法研究，相較于2D激光雷達(dá)SLAM，其定位精度及魯棒性均得到提高。

仿真結(jié)果表明，本文提出的方法能夠有效實(shí)現(xiàn)無人車在巡檢過程中的精準(zhǔn)可靠導(dǎo)航。在后續(xù)工作中會(huì)針對(duì)激光雷達(dá)對(duì)運(yùn)動(dòng)物體的檢測(cè)展開研究，進(jìn)一步提高算法的魯棒性和精度。