基于激光掃描和迭代貝葉斯策略的定位

許亞芳，孫作雷＋，曾連蓀，張 波

（1.上海海事大學 信息工程學院，上海201306；2.中國科學院 上海高等研究所，上海201210）

0 引 言

機器人在運動過程中由于地勢不平等原因容易出現打滑等現象，而里程計往往無法準確記錄這些信息，在進行軌跡預測時會產生較大的誤差［1－3］。因此，需借助外部傳感器獲取的觀測信息做進一步修正。多數學者采用掃描速度快、定位精度高、成本低廉的激光傳感器提取環境信息，但他們的研究多集中于室內線特征或角特征等的提取［4－6］。對室外環境而言，特征的選取和檢測更具挑戰性，選用如校園、公園等半結構化環境中的樹干、道路等物體作為路標，為機器人導航提供有效信息，已成為近年研究的趨勢［7，8］。

從統計學角度看，機器人定位是一種在線濾波問題，即給定一系列觀測值，估計機器人的當前狀態［9］。貝葉斯數據融合算法從概率上解決了機器人自治問題，許多現存方法在處理非線性系統問題時，多采用線性化方法，比較通用的為基于擴展卡爾曼濾波 （extended Kalman filter，EKF）的方法，該方法利用建立的運動模型和觀測模型，通過時間更新和觀測更新遞歸求得機器人各時刻的位姿，且其收斂性也已得到很好的證明［10，11］，在理論上提供了可靠的解決方案，但該方法存在的線性化所帶來的誤差問題，使其在應用中受到了制約。

針對機器人定位中的上述問題，本文結合激光掃描數據與里程計信息，采用迭代貝葉斯數據融合策略，從非線性最優角度出發，通過一系列線性點逐步接近最優解，以提高機器人的定位精度，抑制算法的發散現象。

1 基于激光點云的特征提取

1.1 特征描述

室外環境難以用一種或多種參數化的幾何形式表示，通常選取易于辨識且具有區分度的樹干作為檢測對象。針對樹干的形狀，將其近似為圓柱體處理。當激光掃描儀離地面的高度確定時，可將樹干等效為圓特征，如圖1所示。從傳感器發射的激光束經由樹干的表面，再次返回傳感器。根據激光的TOF （time－of－flight）原理，可以得到關于樹干表面的距離和角度的原始數據信息［12］。通過特征提取算法可估算圓心位置及對應的半徑，進而確定樹干相對于傳感器的具體位置。

圖1 等效為圓特征的樹干

1.2 特征提取方法

室外環境中，激光掃描儀的觀測距離是1 m－80 m，掃描區域180°。獲得的掃描數據為距離－角度信息，根據掃描點個數N＝361及對應的角分辨率0.5°，可計算得到每個掃描點的極坐標形式，表示為Sn＝ （dn，θn）T，n＝1，2，3，…，N。對于樹干的檢測，首先采用點簇聚類分割方法將激光點分類，再利用求平均的方法計算每個分類區域中對應樹干的位置信息及直徑。以一組激光點簇為例說明：

聚類分割將相鄰激光點的距離和對應的角度分別作差，找出相鄰距離差或相鄰角度差大于預定閾值的激光點。以符合上述條件篩選得到的激光點為分割界限，將一組數據中的361個激光點分成不同的區域Ai（i＝1，2，3，…，m），每個區域中包含一系列的激光掃描點 （（d1，θ1），…，（dn，θn），…，（dj，θj）），其中n＝1，2，…，j，（d1，θ1）是該區域的起始點， （dj，θj）是結束點。同時，計算起始激光點和結束激光點的笛卡爾坐標

具體分割方法描述如下：

步驟1 從原始數據中找出距離小于threshold＿range（設定為80m）的掃描點。若有符合條件的掃描點，計算并保存掃描點的距離和角度數據。執行步驟2；否則，將該組數據判為無效的干擾數據。

步驟2 分別將相鄰掃描點的距離和角度作差。通過設定門限，從結果差中找出距離大于threshold＿range2 （設置為1.5m）或角度大于threshold＿bearing2 （設置為5°）的掃描點，確定屬于同類區域的首尾掃描點，并依據式（1）計算保存首尾掃描點的笛卡爾坐標，執行步驟3；

步驟3 對相鄰分類區域同樣按照設定角度和距離門限的方法進行篩選。剔除幾何距離較近和被遮擋的樹干所對應的區域。

步驟4 對滿足以上步驟的分類區域內的首尾掃描點進行篩選，選擇對應直徑小于1m 的區域。如區域Ai的激光掃描點對應樹干直徑的近似計算公式為

這里記L＝ （d1＋dj）／2，表示物體到傳感器的平均距離。至此，經過以上篩選后得到符合條件的掃描點即為樹干返回的激光掃描點。

位置估算：經過聚類分割后，在機器人坐標系中，每個分割區域中的激光掃描點可依據圖2所示模型提取圓柱形路標的距離－角度信息。圖中XOY 和XvOvYv分別表示世界坐標系和機器人坐標系，小圓點表示特征圓的圓心，以該原點為圓心的大圓表示樹干特征。由圖2可知

根據上式可得到

圖2 特征位置估算模型

這里ri代表樹干的半徑，L 是激光傳感器與掃描點之間的最小距離。求出的ρ和α 就是從原始激光數據中提取的樹干特征相對于傳感器的距離和角度。

圖3為對某一組原始激光點的特征提取結果。機器人位于坐標 （0，0）處。

圖3中原始激光點用圓點表示，從原始激光點提取出的特征圓用圓圈表示，而加號表示特征的圓心，為更清晰地觀察提取的結果，選取其中兩個提取點進行放大。左下方圖中的圓圈表示根據分散于圓周圍的激光點計算得出的特征的輪廓。右下圖中只有激光掃描點，說明該區域的激光掃描點為干擾點。

圖3 特征提取的結果

2 迭代貝葉斯數據融合

2.1 基于信度網絡的貝葉斯數據融合

機器人定位問題可以等效為一個無環的信度網絡模型［13］。在該模型中，包含一系列條件獨立的變量，每個變量只依賴于其前一時刻的變量。xt表示機器人在t 時刻的狀態，u1：t表示從時刻1到t的控制量，z1：t和zt分別表示從時刻1到t的觀測量和當前t時刻的觀測量。那么，當機器人在t時刻執行了動作ut后，融合t時刻的觀測信息，繼而得到關于信度網絡模型的后驗概率模型為

這里

由上式可知，計算后驗概率需要3 個概率分布，即運動模型P（xt｜xt－1，ut）、觀測模型P（zt｜xt）和機器人初始狀態的先驗概率P（x0）。

2.2 迭代策略

根據信度網絡模型及數學理論，采用最大似然估計方法可求得后驗概率的最優解

為研究機器人定位問題，所使用的機器人系統為含噪的運動模型和觀測模型。假設機器人的初始狀態滿足高斯分布，描述里程計傳感器的運動模型為高斯測量模型

式中：wt——均值為零，方差為Qt的高斯白噪聲。若觀測模型也為高斯測量模型

式中：vt——均值為零，協方差為Γt的高斯白噪聲。

將過程模型和觀測模型代入式 （8）可得到

在xi（即xt的第i個迭代值，為簡化表達，以下均省略關于時刻的下角標）處，對J 進行二階泰勒級數展開

這里Ji、梯度和海塞矩陣Jixx具體表達如下

對式 （12）求偏導并使之為零，可求得式 （12）的最小值，求解結果如下

其中

這里令Hi），表示測量方程h（·）在第i個迭代估計點＾xit的雅克比矩陣。使用矩陣逆準則可得到

將式 （16）和式 （17）代入式 （15）得到狀態的迭代公式

迭代策略的預測過程和傳統的貝葉斯濾波的預測過程是相同的，不同的是更新過程，描述如下：

For i＝0，N－1

依次執式 （18）、式 （19）

End

執行式 （19）

實際系統中運動模型和觀測模型通常是非線性的，通過線性化方法無法獲得最優線性化點，進而無法保證獲得的值為最佳收斂值。采用非線性最優方法的迭代策略，可通過每次迭代過程獲得的線性化點逐步接近最優解，最終達到降低系統定位誤差目的。

3 實 驗

利用Jose Guivant等錄制的Victoria Park數據集［14］進行實驗驗證。該數據集中，將環境中的樹干作為自然特征，通過人工駕駛車輛在指定路徑中采集車輛及樹干信息。實驗車上配備了GPS、里程計傳感器和激光掃描儀。其中GPS用于獲取車輛的實際運動軌跡，以評估定位算法的性能；里程計測算車輛的速度和航向角，用于推算車輛的航跡；激光傳感器用于感知路標在機器人坐標系中的距離－角度信息，為車輛定位提供觀測信息。由圖4 可以明顯看出，由于公園地勢、環境噪聲等原因使里程計的測量結果 （用實線表示）和GPS的定位軌跡 （圖中點線表示的不連續的路徑）嚴重不符。這表明，僅僅依靠里程計信息不足以正確地定位機器人的位置，甚至會導致推算軌跡的嚴重發散。

圖4 GPS定位軌跡和里程計的航位推算軌跡

這時結合激光掃描儀傳回的原始激光數據，利用本文所述特征提取方法，提取有效觀測信息。為簡化信息融合過程，舍棄了特征的半徑信息，僅保留特征的圓心位置信息，即將樹干等效為點特征處理。選取傳統貝葉斯濾波（EKF）和所提迭代貝葉斯策略融合里程計信息和觀測信息。將過程噪聲的標準差分別設為：σx＝0.2m，σy＝0.04m，σφ＝2°，將觀測噪聲的標準差分別為：σρ＝0.3m，σα＝0.3°，圖5中的左圖為傳統貝葉斯算法 （EKF）生成的估計軌跡 （用實線表示）和地圖 （用加號表示組成地圖的特征點），右圖為迭代貝葉斯算法 （IEKF）估計的結果（同樣使用實線和加號分別表示估計軌跡和地圖中的特征點）。從兩圖的比較可以看出，兩種算法在右側部分的路徑均與真實路徑相符合，這是由于實驗車重復在此運動并獲取觀測信息，形成很多閉合環路，經過反復地數據融合，使得定位的精度越來越精確。而在兩圖的左側部分，EKF估計的路徑與實際GPS定位的路徑相比，出現了明顯的偏差；在通過迭代數據融合方法進行改善后，軌跡的定位結果有了一定的改善，且從衛星地圖上看，估計的特征點幾乎全部落在了相應的樹木上。具體可參見本實驗的配套視頻：http：／／www.dwz.cn／iekfonVictoriaPark。

圖5 估計的路徑及特征

繼續調整系統的噪聲，可看出兩種算法對噪聲的敏感程度。當過程噪聲調整為最初的3倍時，得到如圖6所示的定位結果。圖6 （a）使用傳統貝葉斯濾波進行定位，得到左側軌跡已明顯向左偏移，相比之下，文中所提算法估計的結果仍然與真實路徑相符。當調節至最初噪聲的5倍時，如圖7所示，傳統算法的估計結果已出現了嚴重的發散現象，迭代貝葉斯濾波算法雖然出現了局部的偏差，但相比傳統算法，已較優地控制了定位誤差。

圖6 估計的路徑 （過程噪聲為圖5的3倍）

圖7 估計的路徑 （過程噪聲為圖5的5倍）

4 結束語

本文從原始激光掃描數據中提取有效的觀測信息，通過迭代貝葉斯數據融合方法，將其與里程計信息相結合，為驗證所提算法的有效性，在著名的Victoria Park數據集上，與傳統算法進行比較。從實驗結果可看出，本文算法融入迭代思想后，抑制了發散現象的出現，對機器人的定位有了明顯地提高，且相比傳統算法，其噪聲的容忍能力更強。

［1］Siegwart R，Nourbakhsh I R，Scaramuzza D.Introduction to autonomous mobile robots［M］.MIT Press，2011.

［2］ZHANG Qi，MENG Heqing，ZHOU Rong，et al.A modified self－localization algorithm on mobile robot localization ［J］.Journal of Wuhan University of Technology （Information ＆Management Engineering），2014，36 （1）：9－13 （in Chinese）.［張琪，蒙禾青，周嶸，等.一種改進的移動機器人自定位算法 ［J］.武漢理工大學學報：信息與管理工程版，2014，36 （1）：9－13.］

［3］ZHAO Yilu，CHEN Xiong，HAN Jianda.Scan matching based SLAM in outdoor environment ［J］.Robot，2010，32（5）：655－60 （in Chinese）.［趙一路，陳雄，韓建達.基于掃描匹配的室外環境SLAM 方法 ［J］.機器人，2010，32 （5）：655－60.］

［4］ZHU Jianguo，GAO Junyao，LI Kejie，et al.Research on feature map building of mobile robot in indoor unknown environment［J］.Computer Measurement ＆ Contorl，2012，19（12）：3044－3046 （in Chinese）. ［朱建國，高峻峣，李科杰，等.室內未知環境下移動機器人特征地圖創建研究 ［J］.計算機測量與控制，2012，19 （12）：3044－3046.］

［5］Lin S－Y，Chen Y－C.SLAM and navigation in indoor environments ［M］.Advances in Image and Video Technology.Springer，2012：48－60.

［6］He X，Cai Z.Feature extraction from 2Dlaser range data for indoor navigation of aerial robot［C］／／Proceedings of the Chinese Automation Congress.IEEE，2013：306－309.

［7］Boyko A，Funkhouser T.Extracting roads from dense point clouds in large scale urban environment［J］.ISPRS Journal of Photogrammetry and Remote Sensing，2011，66 （6）：S2－S12.

［8］Wurm K M，Kretzschmar H，Kümmerle R，et al.Identifying vegetation from laser data in structured outdoor environments［J］.Robot Auton Syst，2014，62 （5）：675－684.

［9］Dudek G，Jenkin M.Computational principles of mobile robotics［M］.Cambridge University Press，2010.

［10］Li H－P，Xu D－M，Zhang F－B，et al.Consistency analysis of EKF－based SLAM by measurement noise and observation times［J］.Acta Automatica Sinica，2009，35 （9）：1177－1184.

［11］Zhang HQ，Dou LH，Chen J，et al.Consistency analysis of EKF－SLAM for a basic scenario ［J］.Transactions of Beijing Institute of Technology，2011，31 （10）：1194－1197.

［12］Fu G，Corradi P，Menciassi A，et al.An integrated triangulation laser scanner for obstacle detection of miniature mobile robots in indoor environment［J］.IEEE／ASME Transactions on Mechatronics，2011，16 （4）：778－783.

［13］Kaess M，Ila V，Roberts R，et al.The Bayes tree：An algorithmic foundation for probabilistic robot mapping ［M］.Algorithmic Foundations of Robotics IX.Springer，2011：157－173.

［14］Nebot E.Victoria park dataset［EB／OL］.Available：http：／／www.personal.acfr.usyd.edu.au／nebot／dataset.htm，2012.