基于規(guī)劃路徑約束的機(jī)器人定位方法

胡釗政 許 聰 周 哲 鄧澤武

①(武漢理工大學(xué)信息工程學(xué)院 武漢 430070)

②(武漢理工大學(xué)智能交通系統(tǒng)研究中心 武漢 430063)

③(武漢理工大學(xué)重慶研究院 重慶 401120)

1 引言

近年來，由于攝像頭、激光雷達(dá)(Light Detection and Ranging, LiDAR)等傳感器的更新?lián)Q代和大數(shù)據(jù)、人工智能等高科技在移動機(jī)器人領(lǐng)域的廣泛應(yīng)用，機(jī)器人智能化程度越來越高，智能化移動機(jī)器人作為一種新型的技術(shù)，可以有效增強(qiáng)移動行駛的安全性和提高機(jī)器人的運(yùn)作效率。因此，越來越多的國內(nèi)外學(xué)者開始專注于智能移動機(jī)器人的相關(guān)技術(shù)研究。

移動機(jī)器人的一項(xiàng)核心技術(shù)就是自主導(dǎo)航，自主導(dǎo)航需要機(jī)器人按照預(yù)定的路徑運(yùn)行。目前，基于已知規(guī)劃路徑下的循跡機(jī)器人得到了廣泛的應(yīng)用，例如巡邏機(jī)器人、倉儲物流機(jī)器人等。而自主導(dǎo)航的關(guān)鍵在于定位。機(jī)器人定位技術(shù)是確定機(jī)器人位置和姿態(tài)的過程，在移動機(jī)器人的各個模塊中占據(jù)重要的作用。通常，現(xiàn)有的定位方法可以分為3類：第1類是基于全球?qū)Ш叫l(wèi)星系統(tǒng) (Global Navigation Satellite System, GNSS)的衛(wèi)星定位方法，例如GPS、北斗等[1]。由于原始GPS衛(wèi)星定位精度只能達(dá)到10 m左右[2]，無法滿足當(dāng)前移動機(jī)器人的高精度定位需求。通常，通過將GPS和其他傳感器集成在一起來提高定位的準(zhǔn)確性，例如基于GPS和慣性測量單元 (Inertial Measurement Unit, IMU)的集成方法可以將定位誤差提升至10 cm以下，基于GPS和載波相位差分技術(shù)的GPS/RTK(Real-Time Kinematic)集成方法定位精度達(dá)到厘米級。第2類是基于低成本的視覺傳感器的視覺定位方法，通過視覺里程計(jì)和同步定位和建圖 (Simultaneous Localization And Mapping, SLAM)技術(shù)來實(shí)現(xiàn)移動機(jī)器人的自定位。Du等人[3]提出一種基于單目相機(jī)的車道線檢測方法，以此來計(jì)算相機(jī)與車道線之間的相對位置。文獻(xiàn)[4]提出了一種基于全向SLAM的視覺定位方法，得到更加魯棒的定位結(jié)果。第3類是基于激光雷達(dá)的定位方法，利用激光雷達(dá)傳感器來感知和獲取周圍的環(huán)境信息完成建圖和導(dǎo)航的功能[5]。激光雷達(dá)傳感器對于距離的測量較為準(zhǔn)確，并且其不受光照變化的影響，因此相對于視覺定位方法，激光雷達(dá)的定位精度和可靠度較優(yōu)。在定位算法方面，基于貝葉斯濾波的方法成為當(dāng)前主流的定位方法，卡爾曼濾波器及擴(kuò)展卡爾曼濾波器、粒子濾波器是目前移動機(jī)器人導(dǎo)航領(lǐng)域中較為經(jīng)典的SLAM問題解決方法[6,7]?；趥鹘y(tǒng)傳感器的定位方法存在不同程度的問題，例如基于GPS的衛(wèi)星導(dǎo)航定位方法易受GPS信號盲區(qū)影響，特別是在城市環(huán)境中，容易受到城市峽谷中的多徑效應(yīng)影響[8]；基于傳統(tǒng)視覺的方法易受光線影響；基于激光雷達(dá)的方法存在成本高、累計(jì)誤差的問題。

近年來，人們對于路徑規(guī)劃問題和定位問題的相關(guān)聯(lián)研究較少，通常作為兩個獨(dú)立的模塊進(jìn)行分析。目前，基于典型的圖搜索路徑規(guī)劃方法得到了廣泛的應(yīng)用，在已知地圖的前提下進(jìn)行路徑規(guī)劃成為主流的方法，例如由Dijkstra算法發(fā)展而來的A*算法，曹祥紅等人[9]提出了一種Dijkstra-ACO混合路徑動態(tài)規(guī)劃算法以優(yōu)化所獲取的規(guī)劃路徑；基于螞蟻覓食行為而形成的蟻群算法，Yang等人[10]通過將獨(dú)立運(yùn)行的兩個蟻群算法相結(jié)合，在復(fù)雜地圖中生成無碰撞路徑；李東方等人[11]利用人工勢場法結(jié)合流固耦合模型實(shí)現(xiàn)水下機(jī)器人避障功能。

對于已知規(guī)劃路徑下的機(jī)器人定位，我們期望移動機(jī)器人位于預(yù)設(shè)的路徑上，而移動機(jī)器人在移動之前會根據(jù)地圖信息提前規(guī)劃好自身的可行駛路徑，未來時刻，機(jī)器人的位置應(yīng)該位于或靠近規(guī)劃的行駛路徑，因此規(guī)劃的路徑可以為機(jī)器人定位產(chǎn)生重要的約束。針對機(jī)器人定位存在的問題，并使基于路徑規(guī)劃的移動機(jī)器人導(dǎo)航定位具有更高的穩(wěn)定性和準(zhǔn)確性，本文基于核密度估計(jì)方法對規(guī)劃的路徑進(jìn)行概率密度建模，提出了一種路徑約束的位置概率圖(Path-Induced Location Probability Map, PI-LPM)模型，確定移動機(jī)器人出現(xiàn)在規(guī)劃路徑附近的概率密度圖，距離規(guī)劃路徑較近的位置具有較高的概率。反之，距離越遠(yuǎn)，概率越低。最后，提出了一種規(guī)劃路徑約束的機(jī)器人定位(Robot Localization from Planned Path Constraints, RLPPC)方法，其中，以2階馬爾科夫先驗(yàn)預(yù)測模型對粒子狀態(tài)進(jìn)行預(yù)估，基于粒子濾波算法將PI-LPM模型與現(xiàn)有的傳感器定位方法(如GNSS、視覺SLAM、激光SLAM等)進(jìn)行融合。RL-PPC方法為機(jī)器人循跡導(dǎo)航提供更高的定位精度。

2 本文算法

算法整體流程圖如圖1所示，主要包括以下幾個步驟：(1)構(gòu)建柵格地圖，確定規(guī)劃的路徑。通過離線采集的激光點(diǎn)云數(shù)據(jù)，經(jīng)過相關(guān)處理構(gòu)建柵格地圖，通過標(biāo)定激光坐標(biāo)系與GPS坐標(biāo)系進(jìn)行柵格坐標(biāo)轉(zhuǎn)換，在柵格地圖上規(guī)劃路徑。(2)構(gòu)建路徑約束的位置概率圖(PI-LPM)模型，根據(jù)路徑規(guī)劃采樣點(diǎn)，基于核密度估計(jì)方法生成PI-LPM模型，針對二次規(guī)劃路徑引起的采樣點(diǎn)增減對PILPM模型快速更新。(3)根據(jù)已有的傳感器定位方法(本文實(shí)驗(yàn)使用GPS和LiDAR定位方法)，獲取定位傳感器的定位輸出結(jié)果。(4)根據(jù)RL-PPC方法，基于粒子濾波算法，通過PI-LPM模型和GPS/LiDAR定位輸出的觀測值對粒子權(quán)重進(jìn)行更新，得到融合后的定位結(jié)果。

圖1 算法流程圖

在步驟(1)中，本文所采用的方法參考文獻(xiàn)[12]，基于3維激光雷達(dá)獲取點(diǎn)云信息的方法繪制2維柵格地圖。本文以場景中的原始激光點(diǎn)云數(shù)據(jù)為基礎(chǔ)，對原始點(diǎn)云數(shù)據(jù)進(jìn)行點(diǎn)云預(yù)處理、剔除地面和平面投影的操作，最后根據(jù)柵格狀態(tài)的有無情況來繪制2維柵格地圖。本文使用Velodyne-16型激光雷達(dá)采集激光點(diǎn)云數(shù)據(jù)，并對采集的數(shù)據(jù)進(jìn)行處理，然后采用 (Lightweight and ground optimized-LiDAR Odometry And Mapping, Lego-LOAM)算法進(jìn)行3維地圖重建，最終投影生成2維概率柵格地圖。如圖2所示，為武漢理工大學(xué)校園內(nèi)某場景生成的2維柵格地圖，在柵格地圖的基礎(chǔ)上進(jìn)行路徑規(guī)劃，并通過規(guī)劃路徑采樣點(diǎn)生成所提出的PI-LPM模型。為了便于對不同傳感器采集到的信息進(jìn)行后續(xù)處理，將不同坐標(biāo)系下的信息統(tǒng)一到同一坐標(biāo)系當(dāng)中，采用文獻(xiàn)[13]的方法標(biāo)定激光點(diǎn)云與RTK/GPS數(shù)據(jù)的映射關(guān)系。

圖2 柵格地圖的繪制

2.1 基于路徑約束的位置概率圖(PI-LPM)模型

機(jī)器人定位過程中通過增加相關(guān)約束條件來達(dá)到提高定位精度的要求，本文在路徑規(guī)劃的基礎(chǔ)上，基于核密度估計(jì)方法創(chuàng)新性地提出了一種基于路徑約束的位置概率圖模型。通過PI-LPM模型將路徑規(guī)劃與定位建立關(guān)聯(lián)性，以規(guī)劃的路徑作為一種新的約束限制條件來提高定位精度。

2.1.1 PI-LPM模型構(gòu)建

核密度估計(jì)(Kernel Density Estimation, KDE)是在概率論中用來估計(jì)未知的密度函數(shù)，屬于非參數(shù)檢驗(yàn)方法之一[14]。由于高斯核函數(shù)具有放射性、對稱性、單模性以及多變量的特點(diǎn)，并且可以生成光滑連續(xù)可微的密度函數(shù)，本文采用高斯核密度估計(jì)來計(jì)算當(dāng)前車輛在規(guī)劃路徑中的概率密度。高斯核密度估計(jì)通過高斯核函數(shù)將N個規(guī)劃路徑采樣數(shù)據(jù)點(diǎn)的數(shù)據(jù)和帶寬當(dāng)作核函數(shù)的參數(shù)，得到N個高斯核函數(shù)，線性疊加之后形成核密度的估計(jì)函數(shù)，歸一化得到核密度概率密度函數(shù)。在移動機(jī)器人獲得其自身的規(guī)劃路徑后，機(jī)器人位置出現(xiàn)在規(guī)劃路徑點(diǎn)附近的概率滿足高斯分布。假設(shè)x為當(dāng)前機(jī)器人出現(xiàn)在規(guī)劃路徑附近的可能位置，樣本X={x1,x2,...,xN}為獨(dú)立同分布的N個規(guī)劃路徑采樣點(diǎn)位置，則機(jī)器人出現(xiàn)在路徑采樣點(diǎn)xi附近的概率可用式(1)表示

其中，K(x)為 核函數(shù)，H為帶寬矩陣，亦可稱為窗口，KH(x)為縮放核函數(shù)。本文采用維度為d的標(biāo)準(zhǔn)多元高斯核函數(shù)

移動機(jī)器人位置x由 橫向、縱向兩個維度信息構(gòu)成，取維度d=2，則式(1)可轉(zhuǎn)化為

根據(jù)核密度估計(jì)方法，將獨(dú)立同分布的N個規(guī)劃路徑采樣點(diǎn)的高斯核函數(shù)線性疊加并歸一化之后得到2維高斯核密度估計(jì)下基于規(guī)劃路徑的PILPM模型的概率密度P(x)為

在確定核函數(shù)的前提下，對核密度估計(jì)效果影響起決定作用的是帶寬矩陣H，帶寬的選擇反映了PI-LPM模型的曲線整體平緩程度，較大的帶寬得到的概率分布曲線較為平緩，較小的帶寬得到的概率分布曲線較為陡峭，本文通過經(jīng)驗(yàn)值來設(shè)置帶寬的大小。PI-LPM模型提供一種路徑約束的限制條件，在粒子濾波定位過程中為后驗(yàn)分布的獲取提供一種靜態(tài)的先驗(yàn)概率分布，為RL-PPC方法中粒子權(quán)重更新提供一個重要的分配因素。

2.1.2 PI-LPM模型快速更新

PI-LPM模型是在全局路徑規(guī)劃軌跡下建立，然而通常在移動機(jī)器人行駛過程中往往出現(xiàn)臨時障礙物。為了滿足機(jī)器人的避障需求，需要對移動機(jī)器人進(jìn)行二次規(guī)劃。二次規(guī)劃會引起全局規(guī)劃路徑采樣點(diǎn)的增減，此時需要對PI-LPM模型進(jìn)行快速更新。更新之后的PI-LPM模型概率密度可表示為

其中，右邊分子第1項(xiàng)為全局規(guī)劃路徑的N個采樣點(diǎn)的核密度估計(jì)概率密度函數(shù)，第2、第3項(xiàng)分別為二次規(guī)劃路徑所減少的λ1個 采樣點(diǎn)和增加的λ2個采樣點(diǎn)的核密度估計(jì)概率密度函數(shù)，Pi(x)為機(jī)器人出現(xiàn)在路徑采樣點(diǎn)xi附近的概率。通過提前存儲全局規(guī)劃采樣點(diǎn)生成的概率密度函數(shù)圖，在二次規(guī)劃路徑時對部分變化的路徑采樣點(diǎn)更新概率密度函數(shù)圖來減少計(jì)算時間，做到對PI-LPM模型在二次規(guī)劃問題中的實(shí)時更新。

2.2 基于規(guī)劃路徑約束的機(jī)器人定位(RL-PPC)方法

傳感器定位實(shí)質(zhì)上是通過各種傳感器采集到的觀測數(shù)據(jù)，對機(jī)器人的位姿關(guān)系加以約束，提高機(jī)器人的定位精度。本文提出的基于路徑規(guī)劃的位置概率圖模型可以作為一種新的約束條件，結(jié)合其他傳感器的觀測數(shù)據(jù)，提出一種規(guī)劃路徑約束的機(jī)器人定位(RL-PPC)方法。該方法在粒子濾波的框架下，將移動機(jī)器人位置在PI-LPM模型下的概率密度與其他傳感器獲取的觀測信息進(jìn)行評估，用于分配粒子權(quán)重。RL-PPC方法可以將機(jī)器人的路徑規(guī)劃問題和定位問題建立聯(lián)系，并有效提高機(jī)器人的定位精度。

在移動機(jī)器人自定位場景中，粒子濾波是應(yīng)用較為廣泛的算法之一，通過大量的粒子來模擬移動機(jī)器人的自身位姿，并結(jié)合傳感器的觀測數(shù)據(jù)和地圖數(shù)據(jù)來計(jì)算粒子的權(quán)重[15]。粒子濾波是通過有限個參數(shù)(粒子)來近似表示系統(tǒng)的后驗(yàn)概率分布，通過這一近似表示來對非線性系統(tǒng)進(jìn)行估計(jì)。系統(tǒng)狀態(tài)表示為[16]

其中，xk為 系統(tǒng)狀態(tài)，yk為 觀測數(shù)據(jù)，vk?1為系統(tǒng)噪聲，uk為 觀測噪聲，fk為 狀態(tài)轉(zhuǎn)移函數(shù)，hk為測量函數(shù)。在貝葉斯框架下，系統(tǒng)狀態(tài)估計(jì)問題就是利用蒙特卡洛方法根據(jù)已有的觀測數(shù)據(jù)遞推出當(dāng)前狀態(tài)的后驗(yàn)分布p(xk|y1:k)

其中，p(yk|xk)為似然函數(shù)，由觀測方程式(7)決定。

改進(jìn)粒子濾波的RL-PPC方法利用機(jī)器人在規(guī)劃路徑附近的概率與激光雷達(dá)傳感器(或其他定位傳感器)的觀測值來對定位系統(tǒng)的后驗(yàn)概率分布進(jìn)行修正，提高定位精度。后驗(yàn)概率分布由2階馬爾科夫先驗(yàn)分布xk?2:k?1、基于PI-LPM模型的特殊觀測和傳感器觀測數(shù)據(jù)3部分決定。粒子濾波定位分為預(yù)測階段，更新階段和重采樣階段，RL-PPC方法主要在預(yù)測階段與更新階段對粒子濾波算法進(jìn)行改進(jìn)。在預(yù)測階段，通過2階馬爾科夫先驗(yàn)預(yù)測模型來預(yù)測當(dāng)前時刻的粒子分布；在更新階段，將基于路徑約束的位置概率圖(PI-LPM)模型看作一種特殊的觀測數(shù)據(jù)，和定位傳感器的觀測數(shù)據(jù)一起作為輸入，對粒子集更新權(quán)重；在重采樣階段，根據(jù)粒子的權(quán)重值對粒子進(jìn)行篩選，舍棄權(quán)重低的粒子，保留權(quán)重高的粒子。多次重復(fù)上述3個階段，最終粒子會聚集在真實(shí)位置附近，得到精確的定位結(jié)果。

(1)粒子初始化。規(guī)定粒子的數(shù)量M，并將粒子平均的分布在規(guī)劃的區(qū)域當(dāng)中。一般來說，較多的粒子數(shù)目可以真實(shí)的呈現(xiàn)貝葉斯后驗(yàn)分布，提高狀態(tài)的可信度，但是會提高算法的運(yùn)行時間。而較少的粒子數(shù)目則容易丟失準(zhǔn)確位置，降低定位精度。本文通過GPS得到粗略的初始位置坐標(biāo)，然后在初始坐標(biāo)附近以GPS的定位精度誤差5 m為高斯方差得到高斯分布取樣，取粒子數(shù)目M=100，每個粒子權(quán)重均勻設(shè)置為1 /M初始化粒子。

(2)2階馬爾科夫先驗(yàn)預(yù)測模型。當(dāng)利用歷史定位信息對當(dāng)前狀態(tài)進(jìn)行預(yù)測時，在短時間內(nèi)，可以將機(jī)器人的運(yùn)動狀態(tài)近似地看作為勻速運(yùn)動狀態(tài)，因此基于勻速運(yùn)動模型的式(9)可以根據(jù)前兩個時刻的機(jī)器人位置信息來預(yù)測當(dāng)前時刻的機(jī)器人位置，該過程中粒子的狀態(tài)轉(zhuǎn)移只依賴于前兩個時刻的狀態(tài)，這個過程可以稱之為2階馬爾科夫模型[17]

直接利用后驗(yàn)分布得到粒子的抽樣分布較為困難，通過引入重要密度函數(shù) π(xk|xk?2:k?1,y1:k)來采集帶有粒子權(quán)重的樣本集，根據(jù)2階馬爾科夫模型，選取重要密度函數(shù)為

通過重要密度函數(shù)采樣的粒子集表示為

(3)基于PI-LPM模型的粒子權(quán)重更新。通過重要密度函數(shù)采樣得到粒子集后，需要對粒子權(quán)重進(jìn)行評價。根據(jù)重要密度函數(shù)式(10)，粒子的權(quán)重更新迭代式為

粒子權(quán)重分配由兩部分因素決定：一部分是根據(jù)PI-LPM模型得到的機(jī)器人在位姿xk時處于規(guī)劃路徑附近的概率分布p()，此概率分布通過式(14)獲取

另一部分是由傳感器觀測數(shù)據(jù)決定的概率密度分布p()。在機(jī)器人定位過程中，它表示機(jī)器人處于位姿xk時 傳感器觀測數(shù)據(jù)yk出現(xiàn)的概率，當(dāng)觀測的噪聲符合高斯分布時，通過式(15)計(jì)算得出

其中，Σ為傳感器觀測數(shù)據(jù)的協(xié)方差矩陣，hk(xk(i))為 機(jī)器人位于位姿xk時 第i個粒子在測量函數(shù)下的映射，為傳感器觀測數(shù)據(jù)。本文只驗(yàn)證所提出的方法在2維平面中的位置誤差，未對機(jī)器人姿態(tài)角度進(jìn)行深入比較，在定位系統(tǒng)中可以選擇粒子權(quán)重最大的粒子的姿態(tài)作為當(dāng)前機(jī)器人的定位姿態(tài)角。

根據(jù)以上兩個部分的概率結(jié)合式(13)對每一個粒子的權(quán)重進(jìn)行更新，粒子權(quán)重歸一化后的權(quán)重用表示，通過迭代更新后的粒子集來表示后驗(yàn)分布

其中，δ(.)為狄拉克函數(shù)。在所提出的RL-PPC方法中，粒子的權(quán)重大小既體現(xiàn)了機(jī)器人位置在規(guī)劃路徑周圍的存在概率，又體現(xiàn)了機(jī)器人在激光雷達(dá)定位結(jié)果下的可信度。通過粒子濾波的框架，將路徑規(guī)劃問題與定位問題以概率密度的形式結(jié)合，為機(jī)器人循跡導(dǎo)航提供精確的定位結(jié)果。

(4)重采樣階段。對于每一個粒子，其粒子權(quán)重的大小決定了粒子的可信度。對于不同權(quán)重大小的粒子，通過蒙特卡羅采樣方法選擇保留權(quán)重大的粒子，舍棄權(quán)重小的粒子。但是通過幾次的迭代以后，會出現(xiàn)粒子退化的現(xiàn)象，為了保持粒子數(shù)目不變，根據(jù)權(quán)重大小的比例復(fù)制權(quán)重大的粒子來重新采樣粒子。將重采樣后的粒子代入2階馬爾科夫先驗(yàn)預(yù)測式(9)得到新的預(yù)測粒子群，經(jīng)過反復(fù)迭代，得到接近真實(shí)位置的粒子來實(shí)現(xiàn)對機(jī)器人的準(zhǔn)確定位。

3 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本文所提出的PI-LPM模型和RLPPC方法，可以結(jié)合已有的不同定位方法，并提高移動機(jī)器人導(dǎo)航的定位精度，實(shí)驗(yàn)設(shè)計(jì)了基于GPS定位系統(tǒng)的仿真數(shù)據(jù)實(shí)驗(yàn)和基于激光雷達(dá)(LiDAR)定位系統(tǒng)的真實(shí)場景數(shù)據(jù)實(shí)驗(yàn)。通過真實(shí)實(shí)驗(yàn)和仿真實(shí)驗(yàn)兩種場景對本文算法進(jìn)行測試和驗(yàn)證。兩組實(shí)驗(yàn)中，仿真實(shí)驗(yàn)的真值(GroundTruth)通過仿真輸出端口獲取，真實(shí)實(shí)驗(yàn)的真值(GroundTruth)通過高精度的RTK設(shè)備獲取。

3.1 仿真實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證本文算法的性能，在MATLAB平臺下進(jìn)行如下仿真實(shí)驗(yàn)。仿真實(shí)驗(yàn)環(huán)境：Intel酷睿i5處理器、2.30 GHz主頻、8 GB內(nèi)存、編程語言環(huán)境為MATLAB R2020b。其中，小車循跡跟蹤仿真實(shí)驗(yàn)環(huán)境在MATLAB平臺下的Simulink模塊實(shí)現(xiàn)，小車模型選用差速模型，實(shí)驗(yàn)設(shè)置預(yù)瞄距離0.2 m，小車速度為0.5 m/s進(jìn)行仿真。

本文分別設(shè)計(jì)了半橢圓、圓和S形3種形狀的仿真路徑進(jìn)行試驗(yàn)，分別得到401, 749, 751個采樣軌跡點(diǎn)，并在真值基礎(chǔ)上添加均值為0、標(biāo)準(zhǔn)差為1 m的高斯噪聲作為GPS傳感器的輸入，得到3組不同軌跡下的仿真實(shí)驗(yàn)結(jié)果如表1和圖3—圖5所示，圖3(b)、圖4(b)、圖5(b)分別為3組軌跡下的PI-LPM模型圖，其意義表示越靠近規(guī)劃路徑，具有越高的概率，在PI-LPM模型圖中體現(xiàn)為具有更高的灰度值。從3組軌跡的比較圖來看，所提出的RL-PPC方法可以得到更加光滑的定位軌跡結(jié)果。通過表1可以看出，使用RL-PPC方法后的3種仿真軌跡的定位結(jié)果相比較于GPS傳感器，最大誤差分別減少1.6442 m, 2.0618 m, 1.5413 m，平均誤差分別減少0.6141 m, 0.6279 m, 0.5635 m，誤差在1 m 范圍內(nèi)概率分別增加4 0.9 7%, 4 3.7 9%,37.94%，能顯著提升定位精度。

圖3 半橢圓形軌跡定位結(jié)果

圖4 圓形軌跡定位結(jié)果

圖5 “S”形軌跡定位結(jié)果

表1 不同軌跡下RL-PPC方法定位誤差對比

針對二次規(guī)劃問題引起的路徑規(guī)劃軌跡采樣點(diǎn)變化，導(dǎo)致PI-LPM模型發(fā)生改變，對2.1.2節(jié)中所提出的PI-LPM快速更新方法進(jìn)行驗(yàn)證。在“S”形軌跡基礎(chǔ)上仿真設(shè)計(jì)二次規(guī)劃后的軌跡，一次規(guī)劃的“S”形軌跡采樣點(diǎn)共751個，二次規(guī)劃后的軌跡采樣點(diǎn)在原軌跡基礎(chǔ)上減少67個采樣點(diǎn)，新增54個采樣點(diǎn)，共738個采樣點(diǎn)，根據(jù)式(5)獲取更新后的PI-LPM模型，并得到RL-PPC方法的定位結(jié)果，如圖6所示。從誤差概率分布圖6(b)可以看出，使用RL-PPC方法后的定位誤差在1 m內(nèi)的概率占比遠(yuǎn)高于GPS的定位誤差在1 m內(nèi)的占比，RL-PPC定位方法同樣適用于二次規(guī)劃后PI-LPM模型。得到二次規(guī)劃前后的“S”形軌跡定位結(jié)果對比，如表2所示。根據(jù)表2數(shù)據(jù)對比看出所提出的PI-LPM快速更新方法不會影響RL-PPC定位結(jié)果。

表2 “S”形軌跡二次規(guī)劃前后RL-PPC定位誤差對比

圖6 二次規(guī)劃后的“S”形軌跡定位結(jié)果

3.2 真實(shí)實(shí)驗(yàn)結(jié)果與分析

本文真實(shí)實(shí)驗(yàn)采用的機(jī)器人為課題組開發(fā)的履帶式差速模型機(jī)器人(如圖7所示)，為了驗(yàn)證該算法對激光定位算法的精度提升同樣具有適用性，在該移動機(jī)器人平臺上搭載一臺16線的3維激光雷達(dá)進(jìn)行數(shù)據(jù)采集。實(shí)驗(yàn)的激光雷達(dá)選用Velodyne公司生產(chǎn)的VLP-16型激光雷達(dá)，其中激光雷達(dá)的垂直角分辨率為2°，水平角分辨率為0.2°，采集頻率為10 Hz。

圖7 移動機(jī)器人及其搭載的激光雷達(dá)

實(shí)驗(yàn)采集路段1位于武漢理工大學(xué)內(nèi)的一處環(huán)狀道路該位置GPS信號較為良好，場景環(huán)境開闊，環(huán)狀道路總長度約186 m。試驗(yàn)路段路況和在谷歌地球下的投影軌跡如圖8(a)所示。通過在履帶機(jī)器人上搭載的VLP-16激光雷達(dá)傳感器采集的激光點(diǎn)云數(shù)據(jù)，通過規(guī)劃路徑形成的PI-LPM模型的位置概率如圖8(b)所示，根據(jù)激光雷達(dá)采集到的點(diǎn)云信息，并使用文獻(xiàn)[18]中的LOAM算法的定位結(jié)果作為一種傳感器的觀測，利用所提出的RL-PPC方法提高LOAM算法的定位精度。定位結(jié)果見圖8(c)所示，根據(jù)圖8(d)中的誤差概率分布圖可以看出，RL-PPC方法將LOAM算法中誤差在1 m內(nèi)的概率占比由45.58%提升到98.04%，在0.5 m內(nèi)的概率占比由28.72%提升到93.08%，顯著提升了已有的定位方法。

圖8 場景1定位結(jié)果

實(shí)驗(yàn)采集路段2位于武漢理工大學(xué)內(nèi)一處直角道路，道路總長度97 m。試驗(yàn)路段路況和在谷歌地球下的投影路徑如圖9(a)所示，其路徑的PI-LPM模型的位置概率如圖9(b)所示，按照場景1的方法得出的RL-PPC方法定位結(jié)果如圖9(c)所示，根據(jù)圖9(d)中的誤差概率分布圖看出，場景2中利用RLPPC方法將LOAM算法中誤差在1 m內(nèi)的概率占比由52.19%提升到100%，在0.5 m內(nèi)的占比由17.61%提升到61.32%，場景2利用RL-PPC方法定位結(jié)果同樣提升較大。

圖9 場景2定位結(jié)果

針對二次規(guī)劃問題的真實(shí)實(shí)驗(yàn)驗(yàn)證，我們在實(shí)驗(yàn)場景1的基礎(chǔ)上增加臨時障礙物，在障礙物附近進(jìn)行局部二次規(guī)劃，相較于實(shí)驗(yàn)場景1的一次規(guī)劃路徑如圖10(a)所示。二次規(guī)劃的路徑增加了131個采樣點(diǎn)，減少了405個采樣點(diǎn)，根據(jù)式(5)獲取更新后的PI-LPM概率模型如圖10(b)所示，并得到RL-PPC方法的定位結(jié)果如圖10(c)所示。從誤差概率分布圖10(d)可以看出，二次規(guī)劃后使用RL-PPC方法后的定位誤差在0.5 m內(nèi)的概率占比為60.78%遠(yuǎn)高于LOAM的定位誤差在0.5 m內(nèi)的占比10.08%。RL-PPC定位方法可以適用于在真實(shí)場景下的二次規(guī)劃問題。

圖10 場景1二次規(guī)劃定位結(jié)果

為了體現(xiàn)本文所提RL-PPC方法對于已有定位方法的提升效果，選取具有代表性的激光SLAM定位方法：文獻(xiàn)[18]的LOAM算法和文獻(xiàn)[12]的LeGO-LOAM算法進(jìn)行對比，在實(shí)驗(yàn)場景1下本文算法融合這兩種方法的對比結(jié)果如表3所示。從定位耗時對比和誤差對比可以看出，通過融合本文的RL-PPC方法后的定位結(jié)果在耗時上基本沒有顯著的時間增加，并且能起到提升原有定位方法精度的作用，在確定規(guī)劃路徑的前提下，本文所提的方法可以作為對已有定位方法的補(bǔ)充，進(jìn)一步提升移動機(jī)器人定位精度。

表3 定位誤差與定位耗時對比

4 結(jié)束語

本文提出了一種基于路徑約束的位置概率圖(PI-LPM)模型。該模型從機(jī)器人規(guī)劃的路徑著手，將規(guī)劃的路徑作為一種新的約束條件，以概率的形式來表征機(jī)器人在規(guī)劃的路徑附近位置的可能性，通過核密度估計(jì)方法將路徑采樣點(diǎn)信息轉(zhuǎn)變?yōu)楦怕蕡D模型?；诖烁怕蕡D模型并結(jié)合現(xiàn)有的定位方法，在粒子濾波框架下提出基于規(guī)劃路徑約束的機(jī)器人定位(RL-PPC)方法。該方法以2階馬爾科夫先驗(yàn)預(yù)測模型對粒子狀態(tài)進(jìn)行預(yù)估，結(jié)合傳感器定位觀測結(jié)果與PI-LPM模型對粒子權(quán)重進(jìn)行評估，迭代更新后得到融合后的定位結(jié)果。通過仿真實(shí)驗(yàn)與真實(shí)實(shí)驗(yàn)結(jié)果表明，RL-PPC方法可以結(jié)合當(dāng)前已有的定位算法，并能在定位算法基礎(chǔ)上繼續(xù)提升定位精度，增強(qiáng)機(jī)器人的循跡導(dǎo)航能力。