基于規劃路徑約束的機器人定位方法

胡釗政 許 聰 周 哲 鄧澤武

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

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

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

1 引言

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

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

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

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

2 本文算法

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

圖1 算法流程圖

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

圖2 柵格地圖的繪制

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

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

2.1.1 PI-LPM模型構建

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

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

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

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

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

2.1.2 PI-LPM模型快速更新

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 實驗結果與分析

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

3.1 仿真實驗結果與分析

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

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

圖3 半橢圓形軌跡定位結果

圖4 圓形軌跡定位結果

圖5 “S”形軌跡定位結果

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

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

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

圖6 二次規劃后的“S”形軌跡定位結果

3.2 真實實驗結果與分析

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

圖7 移動機器人及其搭載的激光雷達

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

圖8 場景1定位結果

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

圖9 場景2定位結果

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

圖10 場景1二次規劃定位結果

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

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

4 結束語

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