合作環境下基于慣性/里程計/Lidar融合的UGV導航方法

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

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

0 引言

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

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

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

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

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

1)提出了一種基于幾何結構特征的激光雷達定位方法，實現了在點云稀少環境下的精確定位，并且在動態環境下的魯棒性較高；

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

1 合作環境下激光雷達定位方法

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

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

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

2)特征匹配。將分割完成的點云數據與已知的幾何結構體信息進行匹配。

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

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

1.1 點云數據預處理

點云數據預處理是激光雷達定位方法的基礎，其準確性對后續導航解算起到決定性作用。該部分主要包括有效點提取、點云數據聚類以及角點判斷3個部分。

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

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

表1 聚類算法優缺點比較

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

(1)

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

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

1.2 特征匹配

特征匹配是該激光雷達導航定位方法的關鍵，它的準確性對后續步驟起到決定性作用。它依據無人車的位姿以及物體彼此間的距離關系進行判斷匹配，其具體流程如圖2所示。

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

圖2中,C_pre的值是由上一時刻識別的結構體數量決定的,是當前時刻選擇特征匹配模式的標志位；C的值是由當前時刻識別的結構體數量決定的，是下一時刻選擇特征匹配模式的標志位。

模式一的計算流程如下：

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

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

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

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

模式二的計算流程如下：

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

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

3)根據確定結構體的位置信息，推算其他結構體的位置信息，確認在點云數據中是否存在其他結構體。

模式三的計算流程如下：

1)根據無人車在導航系下的位置篩選出可能是結構體的點云數據geari_m(j)；

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

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

4)根據確定結構體的位置信息，推算其他結構體的位置信息，確認在點云數據中是否存在其他結構體。

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

1.3 中心點確定

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

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

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

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

1)當可匹配的結構體有2個或者3個時，根據不同結構體上的直線方程，確定平行的直線，再根據平行直線間的距離判斷這條直線屬于結構體的哪一側。

2)根據結構體不同方向的長度進行判斷。

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

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

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

圖4 慣性/里程計/激光雷達融合算法結構圖Fig.4 Structure of inertial/odometer/lidar fusion algorithm

2.1 狀態預測

本文以UGV初始時刻所在位置為原點，以UGV機體系為三軸方向構建導航坐標系，EKF所構造的16維狀態量為

(2)

姿態四元數的預測采用如下公式

(3)

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

(4)

(5)

速度預測采用如下公式

(6)

(7)

位置預測采用如下公式

(8)

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

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

(9)

(10)

2.2 濾波器更新

本文中通過EKF融合IMU預測的狀態、激光雷達解算的位姿和里程計的速度信息，從而獲取更精準的導航信息。

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

(11)

式中，Φ是狀態轉移矩陣。

Φk,k-1=

(12)

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

(13)

(14)

(15)

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

(16)

系統的噪聲矩陣為

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

(17)

系統的濾波增益方程、狀態估值方程和估計均方誤差方程如下

(18)

(19)

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

(20)

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

1)激光雷達的量測量為

(21)

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

z_lidark=H_lidark·Xk+uk

(22)

(23)

式中，uk為量測噪聲，偏航角ψL與四元數的關系如下

(24)

2)里程計的量測量為

(25)

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

z_odomk=H_odomkXk+uk

(26)

(27)

2.3 激光雷達故障檢測

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

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

3 仿真驗證及分析

3.1 仿真環境設計

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

圖6 仿真平臺Fig.6 Simulation platform

仿真環境中各傳感器參數設置如下：

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

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

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

3.2 仿真結果分析

仿真實驗結果如圖7和圖8所示，黑色實線為本文算法結果，藍色點線為Hector-SLAM結果，紅色虛線為真值。圖7為無人車在仿真環境中的行走軌跡，圖8為x、y方向的位置誤差。本文算法和2D激光雷達SLAM的定位均方根誤差(m)分別為[σx,σy]=[0.123,0.062]和[σx,σy]=[2.864,6.594]。可見本文算法相對于傳統2D激光雷達SLAM方法能夠實現較為精準的導航。

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

(a)X軸方向位置誤差

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

綜上所述，在特征稀少的合作環境中，使用本文算法能夠實現無人車在巡檢過程中的精準可靠導航。

4 結論

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

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

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

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