基于FastSLAM的繩系機器人同時定位與地圖構(gòu)建算法

王小濤，張家友，王邢波，韓亮亮

1. 南京航空航天大學 航天學院，南京 210016 2. 南京郵電大學 自動化學院，南京 210023 3. 上海市空間飛行器機構(gòu)重點實驗室，上海 201108

火星和月球表面存在著大量極端地形區(qū)域，如陡峭斜坡、松軟土壤、高聳懸崖、溝壑等[1]，對這些區(qū)域的探索可以提供大量有價值的信息[2]。人們提出多種極端地形機器人用于這些地形的探索。按照是否使用繩索，極端地形機器人分為繩系機器人和非繩系機器人。繩系機器人還可以進一步分為繩系腿式機器人(如Dante II[3-4])，繩系輪式機器人(如Cliffbot[5]、TRESSA[6]、Axel[7-8]和vScout[9-11])和繩系履帶式機器人等。在繩索的輔助下，繩系機器人能夠攀爬陡峭斜坡、高聳懸崖等，可用于完成極端地形區(qū)域的探測任務。

在復雜環(huán)境中運動時，繩系機器人的繩索會接觸并纏繞障礙物，從而在繩索與障礙物之間構(gòu)成接觸點。在返回出發(fā)點時，機器人必須沿著與出發(fā)路徑同倫的路徑移動，從而能夠按順序依次解除各接觸點，避免出現(xiàn)繩索纏繞現(xiàn)象。人們常采用下列2種繩索管理方法解決繩系機器人的繩索纏繞問題[12]，第1種方法是通過機器人運動規(guī)劃避免繩索纏繞，該方法需要環(huán)境地圖信息、機器人位置信息以及障礙物位置信息等先驗知識，嚴重限制了繩系機器人的實用性；第2種方法是定位繩索與障礙物之間的纏繞位置，在返回時機器人沿同倫路線運動以解除繩索纏繞。Kumar和Richardson[13]提出繩索纏繞檢測方法，利用繩索方位角和繩索張力檢測繩索的纏繞位置，在此基礎(chǔ)上利用“繩索跟隨”法解除繩索纏繞。然而該方法僅利用繩索方位角和張力的不一致性來 “暴力定位”接觸點，因此該方法本質(zhì)上屬于避免纏繞發(fā)生[14]的方法。Murtra和Mirats[15]融合繩索長度數(shù)據(jù)、慣性導航單元及輪式里程計數(shù)據(jù)，利用非線性優(yōu)化方法解決管道檢測機器人的定位問題。該方法僅將繩索長度作為約束條件來提高機器人定位精度，并沒有解決繩索纏繞位置的定位問題。

針對繩索纏繞問題，McGarey等[16]利用基于粒子濾波器[17-19]的FastSLAM[20]框架估計繩索纏繞位置，并實現(xiàn)繩系機器人的同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)。McGarey等[21]進一步基于機器人SLAM問題的數(shù)學描述[22]，提出繩系機器人的TSLAM(Tethered Simultaneous Localization and Mapping)框架，并利用批量處理方法解決繩系機器人的SLAM問題。該方法與視覺SLAM中的光束平差法[23]類似，將最大后驗概率估計問題轉(zhuǎn)化為求目標函數(shù)最小值的非線性優(yōu)化問題，但該方法不滿足實時性要求，無法在線運行。

針對繩系機器人與障礙物間接觸點之間的相關(guān)性及機器人運動模型和觀測模型的非線性，本文提出基于改進FastSLAM框架的繩系機器人同時定位與地圖創(chuàng)建算法。該算法將繩系機器人的SLAM問題分解為機器人位姿估計問題和接觸點位置估計問題。利用無跡濾波器估計接觸點的位置；利用粒子濾波器計算機器人位姿的后驗置信度，并利用觀測模型的無跡變換處理來簡化粒子權(quán)重的更新過程。

1 繩系機器人機械模型

針對極端地形區(qū)域的探測任務，設計了繩系輪式機器人，其機構(gòu)模型如圖1所示。繩系輪式機器人的結(jié)構(gòu)部分主要包括2個車輪、1個連接軸體、腳輪臂和繩索等部件，同時還包括車輪、腳輪臂和繩索絞盤相應的驅(qū)動傳動系統(tǒng)等部分。繩系機器人主要通過電機驅(qū)動和齒輪嚙合傳動的內(nèi)置絞盤來實現(xiàn)繩索的卷繞和伸展。

繩系機器人裝有慣性測量單元來感知自身運動狀態(tài)。機器人還安裝張力傳感器，使繩索保持足夠張緊狀態(tài)，實現(xiàn)繩系機器人在陡峭地形上工作的能力。

圖1 繩系機器人模型Fig.1 Model of tethered robots

2 繩索機器人的運動模型和觀測模型

2.1 移動機器人的運動模型和觀測模型

當機器人在未知環(huán)境中運動時，需要同時解決機器人自身定位和環(huán)境地圖構(gòu)建問題，這2個問題相互影響，機器人自身定位影響著地圖的觀測信息，反過來環(huán)境地圖又影響著機器人的定位。移動機器人的SLAM問題涉及到機器人運動模型和觀測模型。給定t-1時刻機器人的控制輸入ut-1和機器人狀態(tài)xt-1，則機器人的運動學模型可表示為

xt=f(xt-1,ut-1)+wt-1

(1)

式中：f(xt-1,ut-1)為非線性狀態(tài)轉(zhuǎn)移函數(shù)；wt-1為均值為0、協(xié)方差矩陣為Qt-1的高斯白噪聲。

t時刻機器人對第n個路標點的觀測值為

zt,n=g(xt,yn)+vt

(2)

式中：g(xt,yn)為非線性測量函數(shù)；yn表示第n個路標點的位置；vt為均值為0、協(xié)方差矩陣為Rt的高斯白噪聲。

當機器人在二維空間運動時，機器人t時刻的狀態(tài)可用3維向量xt表示：

(3)

式中：xt、yt為t時刻機器人的位置；θt為t時刻機器人的方位角(機器人移動方向相對于x軸正方向的夾角)。t時刻機器人的運動學模型表示為

(4)

其中：T為采樣時間間隔；vt-1和ωt-1分別為t-1時刻機器人的線速度和角速度。

2.2 繩系機器人的觀測模型

在繩系機器人的運動過程中，其繩索與障礙物之間發(fā)生接觸甚至出現(xiàn)纏繞現(xiàn)象，如圖2所示，其中黑色實線表示機器人的移動軌跡，虛線表示機器人未來運動軌跡，箭頭表示機器人運動方向，點劃線表示纏繞在障礙物之間的繩索部分，點虛線表示當前障礙物與機器人之間的繩索部分。

圖2 繩系機器人系繩纏繞Fig.2 Tether winding of tethered robots

設t時刻機器人與障礙物之間的當前接觸點為第n個接觸點，其坐標yn定義為

(5)

式中：xn、yn為第n個接觸點的位置坐標。t時刻

接觸點的位置可根據(jù)機器人當前繩索總長度與方位角測量值計算得到。

t時刻機器人的測量值zt表示為

(6)

式中：dt為t時刻機器人所釋放的繩索總長度；φt為t時刻當前接觸點和機器人之間繩索與機器人前進方向之間的夾角。

t時刻機器人所釋放的繩索由2部分組成：一部分為固定長度的繩索部分dfixed,t，即點劃線所表示的接觸點之間的繩索部分；另一部分為自由長度的繩索部分dfree,t，即點線所表示的當前接觸點與移動機器人之間的繩索部分。由圖2可知機器人所釋放的固定部分繩索長度為

(7)

機器人所釋放的自由部分繩索長度為

(8)

則t時刻機器人所釋放的繩索總長度為

dt=dfixed,t+dfree,t

(9)

t時刻機器人和當前接觸點間繩索與機器人速度方向之間的夾角φt為

(10)

則機器人觀測模型可表示為

(11)

在傳統(tǒng)移動機器人SLAM問題中，機器人每時刻可以觀測到多個路標點，且各路標點之間是相互獨立的。而在繩系機器人運動過程中，其繩索與障礙物之間可能發(fā)生接觸甚至出現(xiàn)纏繞現(xiàn)象，因此繩系機器人的SLAM問題與傳統(tǒng)SLAM問題的區(qū)別在于：① 繩系機器人的觀測值與所有接觸點相關(guān)；② 由于繩索的存在，接觸點之間不相互獨立，如圖3所示，其中向量xt(t=1,2,3,4)表示繩系機器人不同時刻的狀態(tài)。

圖3 繩系機器人的SLAM問題Fig.3 SLAM problem of tethered robots

已知t時刻繩系機器人的位姿，當前接觸點的坐標可由式(9)和式(11)計算得到：

(12)

由式(12)可知t時刻當前接觸點的位置與其他所有接觸點相關(guān)，如圖3所示，其中點虛線表示機器人與當前接觸點之間的繩索部分，加粗虛線表示機器人對應的其他接觸點。

2.3 繩系機器人SLAM問題描述

繩系機器人的SLAM問題可描述為：根據(jù)機器人傳感器的測量值和控制信息，計算機器人運動狀態(tài)和接觸點位置的后驗概率密度為

p(x,y|z,u)

(13)

機器人運動狀態(tài)和接觸點位置的后驗概率分布式(13)可通過2種方法得到：濾波算法和最大似然估計方法。后者需要利用非線性優(yōu)化方法來解決最大值估計問題，所需計算量非常大，難以在嵌入式設備上實時實現(xiàn)。本文采用濾波算法解決上述繩系機器人的SLAM問題。

因為當前時刻的觀測值可能是由當前接觸點產(chǎn)生的，也可能是由新接觸點產(chǎn)生的，甚至可能是由噪聲產(chǎn)生的，因此觀測值為多模態(tài)分布。擴展卡爾曼濾波算法是基于單峰高斯分布的，不適用于繩系機器人的SLAM問題。粒子濾波可用于非線性、非高斯及多模態(tài)概率模型，因此可用于解決繩系機器人的SLAM問題。本文將繩系機器人的SLAM算法分解為用于機器人狀態(tài)估計的粒子濾波算法和用于接觸點位置估計的無跡濾波算法2部分。與McGarey等所提出的基于FastSLAM框架的繩系機器人SLAM算法[16]的不同之處在于：① 利用無跡濾波算法代替擴展卡爾曼濾波算法來估計接觸點的位置；② 在機器人狀態(tài)估計的粒子濾波算法中，用非線性觀測模型的無跡變換方法簡化粒子權(quán)重更新過程。

3 基于粒子濾波的FastSLAM算法

針對繩系機器人觀測模型的非線性和多模態(tài)特性，本文基于粒子濾波算法和無跡變換提出改進FastSLAM框架來解決繩系機器人的SLAM問題。

3.1 SLAM問題分解

Doucet等[18]提出的Rao-Blackwellized粒子濾波器對聯(lián)合后驗分布的估計問題進行因式分解，可用于高效解決機器人定位和地圖構(gòu)建問題。Montemerlo等[20]將該方法擴展到機器人的SLAM問題，將機器人位姿和路標點位置的聯(lián)合后驗概率分布p(x1:t,y1:k|z1:t,u1:t)的估計問題分解為用于機器人位姿估計的粒子濾波器和基于機器人軌跡的地圖創(chuàng)建部分?；谏鲜鏊枷?，繩系機器人的后驗概率密度函數(shù)也可分解為

p(x|z,u)·p(y|x,z,u)=

p(x|z,u)·p(y|x,z)

(14)

式中：最后一個等式是根據(jù)下列假設得到的：接觸點位置y只與機器人位姿x和觀測值z相關(guān)，而與控制輸入無關(guān)。

一般FastSLAM框架將各個路標點看作是相互獨立的，因此p(y|x,z)可分解為各個路標點的后驗概率密度函數(shù)的乘積[21]。而繩系機器人的觀測值與當前時刻所有接觸點相關(guān)，為了能夠利用FastSLAM算法，本文對觀測模型進行近似處理：

p(zt|xt,y)≈p(zt|xt,yn)

(15)

式中：yn為當前接觸點。式(15)表示觀測值只與當前接觸點相關(guān)，與其他接觸點無關(guān)，因此式(14)分解為

(16)

其中：N為當前接觸點的總數(shù)量。

粒子濾波器使用有限的粒子集來近似后驗概率密度，其中每個粒子表示一種可能的系統(tǒng)狀態(tài)。根據(jù)式(16)，粒子可表示為

(17)

3.2 新接觸點檢測

在運動過程中繩系機器人可能與新的障礙物發(fā)生接觸，從而產(chǎn)生新接觸點，因此在每一時刻必須判斷是否有新接觸點產(chǎn)生。新接觸點是根據(jù)繩索長度測量值和估計值之間的不一致性來確定的，檢測方法如圖4所示。t時刻繩系機器人與當前接觸點之間的距離估計值為

(18)

(19)

則表示有新接觸點yn+1產(chǎn)生；否則表示沒有新接觸點產(chǎn)生。

當有新接觸點產(chǎn)生時，其位置初始化為

(20)

式中：機器人與新接觸點之間的距離d2可由三角形余弦定理計算得到：

(21)

其中：

(22)

圖4 檢測新的接觸點Fig.4 Detection of new contact point

3.3 FastSLAM算法

FastSLAM算法根據(jù)上一時刻的機器人狀態(tài)后驗置信度、控制輸入和觀測值計算機器人當前時刻的狀態(tài)后驗置信度，并迭代更新接觸點的均值和協(xié)方差陣，具體步驟如下。

根據(jù)t-1時刻的測量值z1:t-1和控制輸入u1:t-1預測t時刻的機器人狀態(tài)xt的置信度：

u1:t-1)dxt-1=

(23)

式中：bel(xt-1)為t-1時刻的機器人狀態(tài)后驗置信度；p(xt|xt-1,ut-1)為機器人運動方程的概率模型。

粒子濾波算法利用有限的粒子集近似后驗概率密度，其中每個粒子表示一種可能的系統(tǒng)狀態(tài)。從先驗置信度bel(xt-1)抽取隨機樣本集為

(24)

2) 更新接觸點均值和協(xié)方差矩陣

當沒有新接觸點產(chǎn)生時，根據(jù)貝葉斯準則，接觸點的后驗置信度更新如下：

p(yn|x1:t,z1:t)=

ηp(zt|yn,xt)p(yn|x1:t-1,z1:t-1)

(25)

式中：η表示歸一化常數(shù);p(zt|yn,xt)為觀測值概率模型；p(yn|x1:t-1,z1:t-1)表示接觸點的先驗置信度。當沒有新接觸點產(chǎn)生時，接觸點的先驗置信度可用t-1時刻的接觸點后驗置信度代替。

由于觀測模型為非線性函數(shù)，這里使用無跡濾波方法估計接觸點位置，該方法能夠極大提高接觸點定位精度。根據(jù)t-1時刻接觸點位置的均值和協(xié)方差，無跡變換的2n+1個σ點選取如下：

(26)

利用觀測方程得到2n+1個σ點的觀測預測值為

(27)

(28)

其中：權(quán)重wg和wc的計算公式為

(29)

其中：β=2。

基于當前時刻觀測值zt，利用卡爾曼濾波增益更新地圖中接觸點的均值和協(xié)方差矩陣：

(30)

其中卡爾曼濾波增益矩陣計算式為

(31)

當機器人繩索與障礙物之間產(chǎn)生新接觸點時，接觸點位置均值和協(xié)方差的初始化可通過無跡變換方法得到：

(32)

式中：zt為當前觀測值；Rt為觀測值噪聲協(xié)方差陣；g-1(·)為測量方程的逆。則新接觸點的均值和協(xié)方差矩陣為

(33)

3) 計算粒子權(quán)重

利用當前時刻的所有控制輸入和測量值，計算當前時刻機器人狀態(tài)xt的后驗置信度：

bel(xt)=p(xt|z1:t,u1:t)=

(34)

(35)

利用無跡變換方法對觀測模型進行線性化處理，可以得到式(35)的閉式解。此時粒子權(quán)重為

(36)

(37)

由于部分粒子的權(quán)重非常小，它們所表示的狀態(tài)的可能性非常低，保留這些粒子并不會對系統(tǒng)有顯著的性能改善，因此需要對粒子集進行重采樣。重采樣后的粒子集為

(38)

此時每個粒子具有相同的權(quán)重，新粒子集不包含權(quán)重較低的粒子。一般來說，不需要在每次迭代過程中都執(zhí)行重采樣，典型方法是根據(jù)粒子的有效性Neff來確定是否進行重采樣[24]，Neff的計算式為

(39)

當Neff低于設定閾值時，進行粒子集重采樣。

綜上所述，整個算法流程如圖5所示。

圖5 所提算法流程圖Fig.5 Flow chart of the proposed algorithm

4 仿真實驗

本節(jié)利用Monte Carlo仿真實驗來驗證本文所提出的基于FastSLAM框架的繩系機器人SLAM算法的性能。設仿真步長為0.01，仿真步數(shù)為500。繩系機器人的初始坐標為(0,1)，繩索固定于(0,0)點。機器人攜帶傳感器測量其所釋放繩索長度和繩索與機器人前進方向之間的夾角，假設每個傳感器的測量噪聲為均值為0.003、協(xié)方差為0.002 5的高斯白噪聲。繩系機器人在二維平面內(nèi)沿規(guī)劃路徑運動，在每一時刻利用傳感器檢測繩索長度和方位角信息，并利用不同算法估計接觸點位置和機器人運動軌跡。

利用里程計和本文提出的繩系機器人SLAM算法得到的機器人運動軌跡分別如圖6所示。其中三角形表示所給定的機器人與障礙物之間的接觸點位置，虛線表示機器人的規(guī)劃軌跡(真實軌跡)，點劃線表示基于里程計得到的機器人估計軌跡，實線表示基于本文算法得到的機器人估計軌跡。由圖可知，基于本文算法得到的機器人估計軌跡比利用里程計得到的機器人估計軌跡更接近機器人真實軌跡，所以本文算法可有效估計機器人的運動軌跡。

圖6 繩系機器人的運動軌跡及其估計Fig.6 True and estimated trajectories of tethered robots

當繩系機器人沿規(guī)劃路徑運動時，其繩索與障礙物發(fā)生接觸并構(gòu)成接觸點。繩系機器人的繩索在接觸點之間的分布狀況如圖7所示，其中三角形表示接觸點位置，虛線表示機器人的規(guī)劃軌跡，實線表示機器人從起點運動到終點后繩索狀況。圓點表示利用本文算法得到的接觸點估計位置。由圖7可知，接觸點估計位置逐漸收斂到接觸點真值附近，表明本文算法可有效地定位接觸點位置。

圖7 機器人系繩的軌跡Fig.7 Tether trajectory of robots

本文對里程計方法、McGarey算法[16]和本文算法的估計性能進行了比較。利用這3種算法得到的繩系機器人位姿估計均方根誤差RMSE (Root Mean Square Error)如圖8所示，其中PF+UKF表示本文所提出的繩系機器人SLAM算法，PF+EKF表示McGarey所提出的繩系機器人SLAM算法。仿真結(jié)果表明:本文算法比其他2種算法的估計性能更好。主要是因為本文的繩系機器人SLAM算法采用無跡濾波算法代替擴展卡爾曼濾波來估計接觸點位置。該方法提高了接觸點的位置估計精度,進而改善了機器人位姿估計精度。

圖8 不同算法得到的繩系機器人位姿的估計誤差Fig.8 Estimated errors of different algorithms for tethered robots pose

本文還對McGarey的繩系機器人SLAM算法和本文的繩系機器人SLAM算法的運行時間進行比較。本文算法單步執(zhí)行時間大約為0.071 8 s，McGarey算法單步執(zhí)行時間大約為0.073 1 s。本文算法的計算復雜度較低，主要因為McGarey算法在粒子濾波重采樣過程中將每個粒子分裂為3個副本。而繩系機器人在每一時刻只有一個當前接觸點，McGarey算法產(chǎn)生了大量冗余重復粒子，這種冗余隨著時間推移逐漸加重，降低了粒子的多樣性，增加了計算復雜度。

5 結(jié) 論

基于無跡變換和粒子濾波算法，提出利用FastSLAM框架解決繩系機器人的同時定位和地圖創(chuàng)建問題。該算法利用無跡濾波估計接觸點位置；利用粒子濾波估計機器人位姿，并利用無跡變換簡化粒子權(quán)重更新過程。仿真結(jié)果表明本文算法可有效估計繩索與障礙物之間接觸點的位置、提高機器人位姿估計的精度，而且降低了計算復雜度。但本文提出的繩索機器人SLAM算法具有以下局限性：

1) 在運動過程中繩索需要保持拉緊狀態(tài)，因此該算法僅適用于2D場景。

2) 假設機器人與障礙物之間的接觸為點接觸，不影響繩索長度。

在后續(xù)研究中，將進一步考慮沒有上述局限的機器人SLAM算法；利用多傳感信息融合方法改善機器人和接觸點定位精度；將繩系機器人應用于實際場合。