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

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

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

0 引 言

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

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

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

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

1.1 特征描述

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

圖1 等效為圓特征的樹干

1.2 特征提取方法

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

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

具體分割方法描述如下：

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

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

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

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

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

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

根據(jù)上式可得到

圖2 特征位置估算模型

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

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

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

圖3 特征提取的結(jié)果

2 迭代貝葉斯數(shù)據(jù)融合

2.1 基于信度網(wǎng)絡的貝葉斯數(shù)據(jù)融合

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

這里

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

2.2 迭代策略

根據(jù)信度網(wǎng)絡模型及數(shù)學理論，采用最大似然估計方法可求得后驗概率的最優(yōu)解

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

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

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

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

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

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

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

其中

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

將式 （16）和式 （17）代入式 （15）得到狀態(tài)的迭代公式

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

For i＝0，N－1

依次執(zhí)式 （18）、式 （19）

End

執(zhí)行式 （19）

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

3 實 驗

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

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

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

圖5 估計的路徑及特征

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

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

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

4 結(jié)束語

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

［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）.［趙一路，陳雄，韓建達.基于掃描匹配的室外環(huán)境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）. ［朱建國，高峻峣，李科杰，等.室內(nèi)未知環(huán)境下移動機器人特征地圖創(chuàng)建研究 ［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，F(xiàn)unkhouser 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.