煤礦用無人駕駛輔助運輸車輛的蒙特卡羅定位方法

鮑 文 亮

(中國煤炭科工集團太原研究院有限公司，山西 太原 030006)

0 引 言

無軌膠輪車在國內(nèi)外煤礦的輔助運輸系統(tǒng)中得到了廣泛應(yīng)用[1-3]。然而由于人員密集度高和勞動強度大的特點，無軌膠輪車輔助運輸仍是煤礦生產(chǎn)中安全隱患突出的環(huán)節(jié)[4-5]。實現(xiàn)這類輔助運輸車輛無人駕駛是煤礦安全生產(chǎn)的現(xiàn)實需求，也是智慧化礦山建設(shè)的必然趨勢[6]。當(dāng)前關(guān)于煤礦用輔助運輸車輛無人駕駛技術(shù)的研究仍處于起步階段，相關(guān)文獻較少。

車輛自主定位是無人駕駛技術(shù)體系中的重要一環(huán)。定位技術(shù)用于建立車輛與環(huán)境地圖之間的相互關(guān)系，也被稱為位置估計。在移動機器人領(lǐng)域，概率技術(shù)因其魯棒性好和模型依賴程度低等優(yōu)點已成為研究熱點和發(fā)展趨勢[7-8]。移動機器人(涵蓋無人駕駛車輛)的概率定位方法基本都源于貝葉斯濾波。得到廣泛應(yīng)用的擴展卡爾曼濾波(EKF)定位和無跡卡爾曼濾波(UKF)定位將機器人位姿的置信度表示為一個參數(shù)化的高斯分布[9]。其主要局限是不能解決全局定位和綁架問題，此外需要從傳感器測量提取特征進行處理。DELLAERT等[10]提出了基于粒子濾波的定位方法，并將這一方法命名為術(shù)語蒙特卡羅定位(Monte Carlo Localization，MCL)。粒子濾波通過狀態(tài)采樣表示置信度，是貝葉斯濾波的一種非參數(shù)實現(xiàn)形式[11]。MCL易于實現(xiàn)并且適用范圍廣，受到了廣泛關(guān)注。大量的研究工作針對算法中出現(xiàn)的粒子缺乏問題[12-13]，粒子退化和建議分布機制缺陷問題[14-16]，以及粒子集規(guī)模自適應(yīng)調(diào)整優(yōu)化等方面展開[17-18]。

筆者以煤礦用輔助運輸車輛的自主精確定位問題為研究對象，在場景及需求分析的基礎(chǔ)上，討論了激光測距掃描的適用性，提出了將巷道內(nèi)車輛的精確定位維持在一個典型場景所對應(yīng)的區(qū)域地圖范圍內(nèi)進行的策略；基于車輛的速度運動模型和激光測距的似然域模型，論述了蒙特卡羅定位方法的實現(xiàn)，提出了調(diào)節(jié)似然域模型不確定性范圍，從而避免定位算法失敗的方法，仿真結(jié)果驗證了該方法有效,實現(xiàn)了車輛在煤礦井下巷道的自主定位。

1 場景分析

煤礦用車輛自主行駛時，在巷道延伸的縱向方向和垂直于巷道壁的側(cè)向方向存在不同的定位要求。縱向方向上，在一個重要地點所對應(yīng)的有限區(qū)域范圍內(nèi)，車輛需要獲取精確位置坐標；而在一段僅作為通道存在的狹長直巷內(nèi)則無需該信息，即需要時精確定位。側(cè)向的情況則不同，為確保在規(guī)定的狹窄車道內(nèi)安全行駛，車輛需要時刻了解自身相對巷道中心線基準的位置和轉(zhuǎn)向姿態(tài)角，即實時精確定位。

盡管基于特征測量的定位方法能夠?qū)崟r獲得車輛的全部三自由度位姿估計，但在井下巷道環(huán)境應(yīng)用時存在一定的局限?？紤]對重要地標進行觀測的情況，由于狹窄巷道空間走向的單一性，地標只能依線性排列設(shè)置，從而長距離測量將不可避免。此時，特征方位角的測量噪聲將對車輛側(cè)向定位精度產(chǎn)生較大制約。并且由于側(cè)向連續(xù)實時定位的要求，在車輛一次觀測的可及范圍內(nèi)至少需要設(shè)置1個地標，這在煤礦長距離巷道環(huán)境下工程實現(xiàn)代價較大。

綜合上述定位需求，側(cè)向的激光測距掃描為車輛自主定位提供了一個可供選擇的合理解決方案。測距掃描提供原始測量數(shù)據(jù)，而不是特征測量。此時側(cè)向位置和姿態(tài)角能夠獲得實時的測量更新；對于縱向位置，當(dāng)車輛未到達重要區(qū)域而不需要精確定位時，可以維持在一個多假設(shè)狀態(tài)。

煤礦井下車輛行駛的巷道環(huán)境的一個典型場景如圖1所示，圖中同時繪出了車輛側(cè)向測距掃描的情形，其中，A、B、C、D為圖中4個固定點，x′和y′為車體坐標系的2個坐標軸，θ為車輛局部坐標系相對于全局坐標系的轉(zhuǎn)角，逆時針旋轉(zhuǎn)為正。在一段狹長直巷的中間某個位置發(fā)生巷道的交匯連接，該位置處出現(xiàn)的可能是硐室、聯(lián)絡(luò)巷或交叉巷。通常對于車輛來說這里對應(yīng)一個重要地點或場所，車輛行駛的全域環(huán)境基本可認為是一定數(shù)量的該典型場景的集合。

圖1 礦用車輛行駛的巷道環(huán)境的一個典型場景Fig.1 Typical scene of laneways in which vehicle for coal mine operates

基于分層定位技術(shù)，提出將車輛精確定位問題維持在一個典型場景所對應(yīng)的區(qū)域地圖范圍內(nèi)進行的策略，并進一步詳細討論蒙特卡羅定位方法的實現(xiàn)。需要強調(diào)說明的是，任何一個典型場景內(nèi)只包含1處巷道交匯。這樣劃分的意義在于，在某一場景內(nèi)的全局定位問題下，當(dāng)傳感器未觀測到巷道交匯處時，位姿置信度可能呈現(xiàn)1個多峰分布，在縱向方向維持多個位置假設(shè)；而當(dāng)傳感器一旦觀測到巷道交匯地點時，置信度迅速收斂到1個單峰分布，車輛將確信自己獲得了正確的位置估計。如果1個典型場景中包含多處巷道交匯，通常在觀測到2個以上巷道交匯地點前置信度將保持多峰分布，即維持多假設(shè)的情況，車輛有可能因此錯過在某個重要地點獲得正確位置估計的時機，即未能實現(xiàn)“需要時精確定位”，從而影響正常行駛。

2 蒙特卡羅定位算法

定義全局坐標系如圖1所示，其中x軸沿巷道中心線定義，y軸在巷道橫截面內(nèi)方向垂直于巷道壁。車輛在全局坐標系下的位姿狀態(tài)變量為

(1)

式中，(x,y)為車輛局部坐標系原點在全局坐標系下的坐標值。

在概率體系下，k時刻車輛的位姿由置信度bel(ξk)描述，其定義為后驗概率分布p(ξk|ο1:k,u1:k)。

bel(ξk)?p(ξk|ο1:k,u1:k)

(2)

式中，ο1:k和u1:k分別為k時刻以前所有的測量值和控制量。

當(dāng)車輛首次進入某個典型場景時，首先面對的是在這個區(qū)域地圖范圍內(nèi)的全局定位問題，同時考慮到車輛側(cè)向的激光測距掃描提供的是原始測量值而非特征測量，從而EKF或UKF定位方法不適用于這一定位問題的解決。MCL通過一個狀態(tài)采樣獲得的粒子集描述置信度bel(ξk)，能夠解決位置跟蹤、全局定位以及綁架問題，并且可以處理傳感器的原始測量值而無需提取特征，算法按照如下步驟實現(xiàn)：

1)給定k-1時刻的粒子集χk-1，當(dāng)前k時刻的uk、οk，以及環(huán)境地圖m。

2)對所有j=1，2，…，N(N為粒子數(shù)量)進行采樣。

(3)

(4)

算法的第3步根據(jù)每一個預(yù)測狀態(tài)的權(quán)值大小進行重要性采樣，實現(xiàn)了貝葉斯濾波的測量更新過程。得到的粒子集χk能夠描述任意形狀(如多峰分布)的置信度bel(ξk)分布。如果為簡單地獲取定位估計，可采用高斯近似進行密度提取。分別以樣本均值和樣本方差作為近似高斯分布的一階矩和二階矩參數(shù)，得到定位估計。

3 運動與測量模型

3.1 運動模型

礦用輔助運輸車輛一般采用阿克曼轉(zhuǎn)向或鉸接轉(zhuǎn)向的驅(qū)動結(jié)構(gòu)，其中前者用于運人車等輕型膠輪車，后者用于支架搬運車等重型車。這2類結(jié)構(gòu)都具有非完整運動學(xué)約束[19]，具體來說，它們都具有1個瞬時旋轉(zhuǎn)中心，控制自由度都等于2，由1個差分自由度和1個可操控自由度組成。對于阿克曼轉(zhuǎn)向結(jié)構(gòu)，可選取車輛縱向速度v和旋轉(zhuǎn)角速度ω作為名義控制輸入。而對于鉸接轉(zhuǎn)向結(jié)構(gòu)車輛，其前后車體滿足一個與鉸接角相關(guān)的運動學(xué)約束關(guān)系，可選取前車體的縱向速度v及其旋轉(zhuǎn)角速度ω作為名義控制輸入。由此，采用統(tǒng)一的速度運動概率模型的采樣算法完成式(3)表示的采樣過程[20]。首先，向名義控制中加入控制噪聲采樣。

(5)

(6)

式中，Δt為采樣間隔。

3.2 測量模型

(7)

(8)

(9)

這里將激光測距返回的最大距離讀數(shù)視為測量失敗，將其簡單丟棄。則式(7)—式(9)給出了激光測距掃描的似然域測量模型，由此可根據(jù)式(4)進行定位算法中權(quán)值的計算。

為適應(yīng)MCL，筆者提出不確定性范圍可調(diào)的似然域模型修正方法，即將式(7)修正為

(10)

其中，σLHF為可調(diào)節(jié)參數(shù)。增大似然域不確定性σLHF使得在正確狀態(tài)附近范圍內(nèi)的預(yù)測狀態(tài)值能夠獲得更高的測量概率(權(quán)值)，這時重要性采樣能夠基于測量信息采樣出更接近真實值的狀態(tài)。

4 仿真驗證及分析

表1 仿真用運動參數(shù)Table 1 Motion model parameters used in simulation

MCL方法首先要解決全局定位問題。選取粒子數(shù)N=30 000，在地圖范圍內(nèi)，按照均勻分布在車輛位姿狀態(tài)空間進行采樣，獲得初始時刻粒子集，粒子在x-y平面的分布情況如圖2所示。

圖2 初始時刻的粒子在x-y平面的分布情況Fig.2 The distribution of particles in the x-y plane at the initial monment

MCL仿真過程中，調(diào)節(jié)模型參數(shù)σLHF=1 m，獲得了良好的估計效果。進行參數(shù)調(diào)節(jié)時，當(dāng)σLHF取值較小(向σsens靠近)，則粒子基本不能收斂到真實位姿附近，與分析情況相符。仿真得到MCL過程中不同時刻的粒子在x-y平面的分布情況如圖3所示。設(shè)車輛的各位姿狀態(tài)是條件獨立的，通過高斯近似對粒子集進行密度提取，并以高斯分布的均值作為車輛位姿估計，則車輛位置估計的情況如圖4所示，車輛姿態(tài)角估計如圖5所示。

圖3 不同時刻的粒子在x-y平面的分布情況Fig.3 The distribution of particles in the x-y plane at different monments

圖4 MCL的車輛位置估計Fig.4 Position estimation of vehicle by MCL

由圖3可得，在MCL開始后的一段時間里，初始時刻均勻分布的粒子聚集在2個區(qū)域之內(nèi)。此時狀態(tài)置信度對應(yīng)1個多峰分布，即存在多個可能的位姿假設(shè)，這一階段反映了車輛的全局定位過程。隨著MCL的進行，粒子聚集到了一個確定的區(qū)域附近，該階段反映出車輛已進入位置跟蹤過程。圖4的位置估計曲線也清晰地表現(xiàn)出這一過程變化。當(dāng)置信度具有多峰分布時，高斯近似密度不能有效擬合真實置信度分布，從而使得位置估計具有較大偏差。當(dāng)置信度變?yōu)閱畏宸植紩r，高斯近似精度大幅提升，位置估計迅速逼近實際運動軌跡。事實上，結(jié)合圖4和圖5可以看出，在整個MCL過程中，側(cè)向位置估計和姿態(tài)角估計都很好地反映了實際狀態(tài)值，真正導(dǎo)致置信度多峰分布的因素是縱向位置的后驗分布特性。在t≈8 s時，置信度由多峰分布收斂為單峰分布，該過程反映了車輛側(cè)向的激光測距掃描觀測到地圖中的巷道交匯這一事實。綜上可知，車輛的定位特性確實滿足了側(cè)向“實時精確定位”和縱向“需要時精確定位”的要求。

為對比定位性能和效果，基于相同的實際運行軌跡，對EKF定位的實現(xiàn)情況進行了仿真。首先，EKF定位不能解決全局定位問題，因此仿真使用了車輛真實初始位姿附近的一個窄高斯分布作為初始值進行位置跟蹤。其次，EKF不能直接處理前述的車輛側(cè)向激光測距掃描的測量信息，仿真采用基于地標特征測量的方法實現(xiàn)EKF定位。考慮到煤礦巷道實際環(huán)境特點，假設(shè)可同時觀測的地標數(shù)量為1個，地標在地圖中的設(shè)置坐標為(100,0)，單位m。最后，分別設(shè)置特征方位角的測量誤差σ為0.05和0.10 rad，仿真得到EKF定位的車輛位置估計情況如圖6所示?；贛CL以及2種條件下的EKF定位仿真結(jié)果，繪制各自的側(cè)向位置估計誤差曲線，如圖7所示。

圖6 EKF的車輛位置估計Fig.6 Position estimation of vehicle by EKF

圖7 側(cè)向位置估計誤差對比Fig.7 Comparison of errors of lateral position estimation

計算MCL和2種條件下EKF定位的側(cè)向估計誤差統(tǒng)計值，見表2。

表2 側(cè)向位置估計誤差對比Table 2 Comparison of errors of lateral position estimation

可以看出，在基于特征測量的EKF位置跟蹤定位情況下，側(cè)向的定位精度受特征方位角測量誤差的影響較為顯著。對于煤礦巷道環(huán)境長距離單特征測量，適宜的特征測量方法和高精度傳感器成為現(xiàn)實制約。

5 結(jié) 論

1)通過定義煤礦巷道環(huán)境的典型場景，礦井全域地圖可分解為若干典型場景區(qū)域地圖，礦用車輛的定位問題在區(qū)域地圖范圍內(nèi)得到解決，簡化了車輛全局定位的復(fù)雜度和運算量。

2)在巷道環(huán)境中，縱向和側(cè)向相關(guān)位姿狀態(tài)估計的及時性要求不同，采用車輛速度運動模型及激光測距傳感器似然域模型的蒙特卡羅定位算法能夠很好地適應(yīng)這一要求，解決了全局定位和位置跟蹤問題。

3)為適應(yīng)蒙特卡羅定位算法的特點，將似然域模型中的不確定性參數(shù)作為可調(diào)節(jié)量，當(dāng)傳感器噪聲水平較低時，適當(dāng)增大該量值能夠保證算法成功運行。

4)相比巷道環(huán)境下的擴展卡爾曼濾波定位，除能夠解決全局定位問題外，蒙特卡羅定位方法使側(cè)向位置估計精度得到有效保證。