合作環(huán)境下基于慣性/里程計/Lidar融合的UGV導(dǎo)航方法

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(1)

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

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

1.2 特征匹配

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

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

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

模式一的計算流程如下：

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

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

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

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

模式二的計算流程如下：

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

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

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

模式三的計算流程如下：

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

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

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

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

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

1.3 中心點確定

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

圖3 中心點確定流程圖Fig.3 Flow chart for determining the center point

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

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

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

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

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

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

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

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

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

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

(2)

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

(3)

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

(4)

(5)

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

(6)

(7)

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

(8)

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

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

(9)

(10)

2.2 濾波器更新

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

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

(11)

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

Φk,k-1=

(12)

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

(13)

(14)

(15)

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

(16)

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

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

(17)

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

(18)

(19)

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

(20)

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

1)激光雷達的量測量為

(21)

其中，x_lidar、y_lidar、ψL為激光雷達估計的無人車位姿。量測方程為

z_lidark=H_lidark·Xk+uk

(22)

(23)

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

(24)

2)里程計的量測量為

(25)

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

z_odomk=H_odomkXk+uk

(26)

(27)

2.3 激光雷達故障檢測

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

圖5 激光雷達故障檢測方法Fig.5 Fault detection method for lidar

3 仿真驗證及分析

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

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

圖6 仿真平臺Fig.6 Simulation platform

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

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

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

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

3.2 仿真結(jié)果分析

仿真實驗結(jié)果如圖7和圖8所示，黑色實線為本文算法結(jié)果，藍色點線為Hector-SLAM結(jié)果，紅色虛線為真值。圖7為無人車在仿真環(huán)境中的行走軌跡，圖8為x、y方向的位置誤差。本文算法和2D激光雷達SLAM的定位均方根誤差(m)分別為[σx,σy]=[0.123,0.062]和[σx,σy]=[2.864,6.594]?？梢姳疚乃惴ㄏ鄬τ趥鹘y(tǒng)2D激光雷達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é)論

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

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

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

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