在利用測量信息優(yōu)化分布式EKF估計過程的SLAM方法中，分布式結(jié)構(gòu)將時變的觀測信息分布化處理，根據(jù)路標(biāo)點信息對應(yīng)建立若干平行的子系統(tǒng)，并且子系統(tǒng)維數(shù)不隨時間變化。各子系統(tǒng)單獨進(jìn)行濾波估計，將各自的估計結(jié)果送到主濾波器中，主濾波器在保證系統(tǒng)在精度較高或一致性較高的情況下合理地做出融合判定，最后輸出最優(yōu)的機器人位姿估算結(jié)果。屬于機器人自主導(dǎo)航領(lǐng)域。
背景技術(shù)：
：SLAM(SimultaneousLocalizationandMapping)即同步定位與地圖構(gòu)建，其基本思想是：讓機器人在未知環(huán)境中從未知位置開始移動,通過自身所帶傳感器掃描到的路標(biāo)點的信息進(jìn)行自身位置估計，同時構(gòu)建增量式地圖。在SLAM過程中，根據(jù)觀測信息進(jìn)行實時的地圖估計與更新，其中濾波器設(shè)計，數(shù)據(jù)融合算法都是關(guān)鍵問題。SLAM問題可以建立在不同的算法結(jié)構(gòu)中，對于傳統(tǒng)的集中式SLAM，將機器人位姿和路標(biāo)點信息均加入到狀態(tài)向量中，在整個SLAM濾波過程中，無論采用哪種濾波方式，狀態(tài)向量由于一直附帶時變的路標(biāo)點信息進(jìn)行后續(xù)濾波運算，隨著路標(biāo)點數(shù)量的增多狀態(tài)向量維數(shù)也不斷增多，計算量相應(yīng)增大，并且根據(jù)路標(biāo)點的觀測情況動態(tài)時變。本發(fā)明中SLAM算法采用分布式結(jié)構(gòu)，在分布式結(jié)構(gòu)中將機器人位姿及路標(biāo)點信息進(jìn)行分布化處理，每個有效的路標(biāo)點對應(yīng)一個子濾波器。因此，狀態(tài)向量的維數(shù)相對固定，不會隨著時變的路標(biāo)點信息而變化，每一個子濾波器都有各自的狀態(tài)方程和觀測方程，各子濾波器的估計結(jié)果都被傳送到主濾波器進(jìn)行融合運算。在主濾波器中，根據(jù)各個子濾波器的估計結(jié)果及相應(yīng)的誤差情況計算機器人位姿的最終結(jié)果。由于擴展卡爾曼濾波方法的簡單性與有效性，在各子濾波器中通常選用EKF算法。但是，EKF線性化過程忽略高階項產(chǎn)生的線性誤差導(dǎo)致了子濾波器估計精度存在局限性。主濾波器的核心是融合算法，選擇單一的引入誤差協(xié)方差矩陣或新息矩陣信息參與數(shù)據(jù)融合過于單薄，難以保證系統(tǒng)精度。技術(shù)實現(xiàn)要素：本發(fā)明針對分布式SLAM系統(tǒng)中EKF子濾波器估計精度存在的局限性問題，采用由傳感器直接測得的觀測量計算濾波器雅克比矩陣，提高了子濾波器估計精度。并且考慮到一致性對系統(tǒng)的影響，改進(jìn)分布式系統(tǒng)融合的方案，提出利用誤差協(xié)方差陣和匹配信息共同參與數(shù)據(jù)融合過程的方式對算法進(jìn)行優(yōu)化。下文將具體描述優(yōu)化分布式結(jié)構(gòu)中的子濾波器以及分布式系統(tǒng)的融合方案的過程。最后通過真實實驗證明了本發(fā)明算法的可行性與有效性。機器人的運動模型和觀測模型如圖1所示。在分布式SLAM問題中，假設(shè)機器人的姿態(tài)表示為其中(xr,yr)表示機器人在地圖中的坐標(biāo)，表示航向角。路標(biāo)點在地圖中的位置表示為：mi＝(xi,yi)T,i＝1:n，其中mi表示第i個路標(biāo)點對應(yīng)的位置信息，(xi,yi)表示路標(biāo)點在地圖中的坐標(biāo)。t時刻機器人的狀態(tài)矢量表示為Xt＝(xt,rT,mt,iT)T。機器人的運動學(xué)模型可用公式(1)描述st＝f(st-1,ut)+wt(1)其中，移動機器人運動模型描述了t時刻機器人的狀態(tài)在運動的控制信號ut和噪聲干擾wt作用下隨時間的變化過程。wt表示過程噪聲，用來表示機器人運動過程中存在的不確定性誤差。機器人的觀測模型可以用公式(2)表示Ζ(t)＝h(xr(t),mi(t))+v(t)(2)其中Z為t時刻機器人通過激光測距儀所獲得的觀測信息(r,β)T，r表示機器人與路標(biāo)點間的距離，β表示第i個路標(biāo)點相對機器人的方向角，v為測量誤差。假設(shè)t時刻觀測到n個有效的特征點，分布式系統(tǒng)可表示為：X·r=f(Xr)+wz1=h(Xr(t),m1)+v1X·r=f(Xr)+wz2=h(Xr(t),m2)+v2...X·r=f(Xr)+wzn=h(Xr(t),mn)+vn---(3)]]>可見，在分布式SLAM中，根據(jù)觀測到的路標(biāo)點信息建立相互平行的子濾波器，每一個子濾波器都有各自的狀態(tài)方程和觀測方程。最終每一個子濾波器的估計結(jié)果都被傳送到主濾波器，而在主濾波器中，根據(jù)各個子濾波器的估計結(jié)果及相應(yīng)的誤差情況計算出機器人位姿的最優(yōu)結(jié)果。分布式濾波器結(jié)構(gòu)圖如圖2。在分布式SLAM算法子濾波器估計單元中，EKF算法的第一步是根據(jù)運動模型和t-1時刻的狀態(tài)對t時刻狀態(tài)進(jìn)行預(yù)測x‾t,r*x‾t,i*=f(x‾t-1,r,ut-1)x‾t-1,i---(4)]]>其中，和是機器人在t-1時刻所估計的機器人位置結(jié)果和路標(biāo)點位置結(jié)果。第i個子系統(tǒng)t時刻的觀測方程具體形式如下：解決SLAM問題時，EKF是在噪聲符合零均值的高斯白噪聲的假設(shè)下，利用將f(xt-1,rut-1)和h(xr(t),mi(t))展成泰勒級數(shù)并略去二階以上高階項從而得到非線性系統(tǒng)的線性化模型。因此，EKF關(guān)鍵的步驟是準(zhǔn)確地求解狀態(tài)方程和觀測方程的兩個雅克比矩陣。計算狀態(tài)轉(zhuǎn)移雅克比矩陣▿ft-1=∂f∂Xr|x‾t-1,r---(6)]]>計算觀測信息雅克比矩陣▿ht=∂h∂X|x‾t*---(7)]]>預(yù)測結(jié)果的協(xié)方差陣如下：Pt*=Prr,t*Pri,t*Pir,t*Pii,t*=▿ft-1Prr,t-1▿ft-1T+Qt-1▿ft-1Pri,t-1Pir,t-1▿ft-1TPii,t-1---(8)]]>Pir,t-1=Pri,t-1T]]>其中Prr,t-1和Pri,t-1分別是t-1時刻機器人狀態(tài)向量協(xié)方差和機器人與路標(biāo)點狀態(tài)向量的協(xié)方差。Pii,t-1和Qt-1分別是t-1時刻路標(biāo)點狀態(tài)向量的協(xié)方差和機器人運動的過程噪聲的協(xié)方差。t時刻第i個子系統(tǒng)根據(jù)運動模型預(yù)測的和Pt*按照以下方式進(jìn)行更新過程：Ki(t)=Pi*(t)▿hiT[▿hiPi*(t)▿hiT+Ri]-1,i=1,2...,n---(9)]]>X‾i(t)=X‾i*(t)+Ki(t)[Zi(t)-Hi(t)X‾i*(t)],i=1,2...,n---(10)]]>Pi(t)=[I-Ki(t)▿hi]Pi*(t),i=1,2...,n---(11)]]>其中，Ki(t)和分別是子濾波器t時刻的卡爾曼增益和觀測方程的雅克比矩陣，Rt代表觀測噪聲矩陣，和Pi(t)分別代表子濾波器t時刻對應(yīng)的狀態(tài)向量估計值和相應(yīng)的協(xié)方差。可見，t時刻子系統(tǒng)的位姿估計是根據(jù)泰勒一階展開通過線性化的h(Xt)進(jìn)而計算得到的，即Z=h(Xt)≈h(X0)+▿h(Xt-X0)---(12)]]>其中X0表示觀測方程線性化時進(jìn)行泰勒展開時所在的點，即根據(jù)運動模型所預(yù)測的點最后，將Z＝Zt代入時，得到EKFSLAM的最優(yōu)估計結(jié)果。當(dāng)觀測方程在進(jìn)行泰勒展開時的斜率是如下形式時，擁有最高的估計精度，kreal=Zt-h(X0)Xt-X0---(13)]]>當(dāng)將kreal和Zt分別代入Ht和Z中，則Xt能被最精確地估計。如圖3所示。在圖3中，X0處的三條曲線A,B,C分別代表采用不同計算方法求出的X0處切線，即中所對應(yīng)的各項值。其中，曲線A對應(yīng)的是式(12)中的算法，即理論上最接近要求的計算值kreal，曲線C對應(yīng)的是傳統(tǒng)EKF中的計算值，而曲線B則表示通過利用各種優(yōu)化的方法，試圖將中對應(yīng)的k在kEKF的基礎(chǔ)上靠近真實觀測方程雅克比矩陣中各項的kreal?；诖朔N思想，可以結(jié)合系統(tǒng)的觀測方式和預(yù)測方式，設(shè)計適合實際系統(tǒng)的求取方法，從而得到精度更優(yōu)的濾波器性能。針對以上非線性系統(tǒng)的線性化誤差分析，改進(jìn)的關(guān)鍵點是采用何處的X值進(jìn)行線性化。結(jié)合分布式獨有的結(jié)構(gòu)，即對于各子濾波器觀測到的路標(biāo)點情況，分別計算相應(yīng)的線性化結(jié)果，最終融合得到最優(yōu)的估計結(jié)果。每一個子濾波器對應(yīng)不同的線性化結(jié)果hi，由于各子濾波器的雅克比矩陣hi存在差異性，根據(jù)線性化盡可能最優(yōu)的原則，采用均值逼近的方法，對進(jìn)行改進(jìn)。根據(jù)分布式EKF中子濾波器t時刻迭代更新的計算式(14)Kt=Pt-▿htT(▿htPt-▿htT+Rt)-1---(14)]]>其中觀測方程的雅克比矩陣可以表示為式(15)的形式▿ht=-xi-xr(xi-xr)2+(yi-yr)2-yi-yr(xi-xr)2+(yi-yr)20yi-yr(xi-xr)2+(yi-yr)2yi-yr(xi-xr)2+(yi-yr)2-1001---(15)]]>如式(15)，在整個擴展卡爾曼濾波過程中，傳統(tǒng)算法中將里程計數(shù)據(jù)、激光傳感器數(shù)據(jù)代入濾波器過程得出狀態(tài)估計結(jié)果，再代入式(15)中。由于得到的機器人位姿、路標(biāo)點位置并不準(zhǔn)確，該處理方式易造成的不準(zhǔn)確，故提出如下的算法：式(15)中的表達(dá)式可以借助觀測量，通過如下式(16)的表示方法進(jìn)行求取：xt-xt,i=-rt*sign(cos(βt+φt-π2))1+tan2(βt+φt-π2)yt-yt,i=-rt*sign(cos(βt+φt-π2))*|tan(βt+φt-π2)|1+tan2(βt+φt-π2)---(16)]]>雖然t時刻理想狀況的xreal無法直接得到，但可以由觀測量間接推導(dǎo)得到，分別將由激光傳感器得到的rt、βt和估計的航向角φt代入式(17)中進(jìn)行計算，進(jìn)而得到在點xreal下求得的相應(yīng)觀測方程雅克比矩陣則式(14)中的可以表示為如下形式：▿hnew=(▿hx*+▿hxreal)/2---(17)]]>改進(jìn)后的t時刻第i個子系統(tǒng)的估計過程如下：Ki(t)=Pi*(t)▿hi,newT[▿hi,newPi*(t)▿hi,newT+Ri]-1,i=1,2...,n---(18)]]>X‾i(t)=X‾i*(t)+Ki(t)[Zi(t)-Hi(t)X‾i*(t)],i=1,2...,n---(19)]]>Pi(t)=[I-Ki(t)▿hi,new]Pi*(t),i=1,2...,n---(20)]]>考慮到一致性對系統(tǒng)的影響，本發(fā)明改進(jìn)了分布式系統(tǒng)融合的方案，通過協(xié)方差矩陣子模塊和匹配信息子模塊，充分發(fā)揮分布式結(jié)構(gòu)下觀測信息的作用，保證系統(tǒng)在精度較高或一致性較高的情況下均能合理地做出融合判定。首先，利用誤差協(xié)方差陣定義融合權(quán)重：設(shè)第i個子濾波器協(xié)方差陣為Pi，定義相應(yīng)子濾波器在融合過程中的權(quán)重為δp,i。δp,i=1/PiΣi=1N(1/Pi)---(21)]]>依據(jù)以上定義，協(xié)方差陣Pi表征子濾波器的精度情況，子濾波器對應(yīng)的協(xié)方差越大，說明該子濾波器的估計精度相對較低。相反，協(xié)方差陣越小，表征子濾波器的估計精度較高，應(yīng)當(dāng)令主濾波器更多繼承該子濾波器的估計值，即該子濾波器權(quán)重值較大。協(xié)方差矩陣Pi描述子濾波器估計的精度情況，其主要描述當(dāng)前時刻t的子濾波器權(quán)重情況，但從時間的維度上，無法反映從時刻t-1到t的子濾波器準(zhǔn)確情況，為了衡量在運行過程中t-1到t時刻子濾波器的精度情況，并且使其反映到子濾波器的權(quán)重上,考慮在數(shù)據(jù)融合中引入匹配誤差，從而豐富信息融合過程。同時需要考慮的是，由于子濾波器在運行過程中，其對某一路標(biāo)點的“置信度”會隨著系統(tǒng)運行的時間逐漸增高，這一規(guī)律是影響分布式系統(tǒng)一致性的重要原因，當(dāng)觀測噪聲R無窮大時，看作近似對系統(tǒng)一致性沒有影響，而如此則會降低子濾波器精度。所以，當(dāng)子系統(tǒng)運行時，既需要精度高時實現(xiàn)合理性的融合，又需要在噪聲較大而一致性較好時實現(xiàn)合理性的融合。從讓系統(tǒng)最大限度地挖掘觀測信息中所表征的特性，以及考慮一致性影響的角度出發(fā)，加入能夠反映子濾波器測量噪聲情況的匹配誤差信息參與融合算法。該方法中的融合系數(shù)是以相鄰時刻的臨域匹配值大小來確定的，在機器人運行過程中，每一時刻的觀測量都要進(jìn)行匹配過程，在該過程中，是以匹配成功的路標(biāo)點t-1和t時刻在地圖上的映射值來衡量子濾波器性質(zhì)。設(shè)t時刻的觀測到的第i個路標(biāo)點的坐標(biāo)為(xi,t,yi,t)，地圖中與之匹配的t-1時刻的路標(biāo)點的坐標(biāo)為(mix,t-1,miy,t-1)。定義匹配誤差為同一路標(biāo)點相鄰時刻的距離誤差，表示為ri。則有ri=(xi,t-mix,t-1)2+(yi,t-miy,t-1)2---(22)]]>此時，利用匹配誤差定義融合權(quán)重：設(shè)第i個子濾波器匹配誤差為ri，定義相應(yīng)子濾波器在融合過程中的權(quán)重為δr,i。δr,i=1/riΣi=1N1/ri---(23)]]>根據(jù)以上定義，匹配誤差用來衡量各子濾波器的可信賴程度。子濾波器的匹配誤差越大，該子濾波器的觀測噪聲越大，子濾波器的觀測精度越低，融合時子濾波器的信任度低，該自濾波器的權(quán)重值就越??；相反，子濾波器的匹配誤差越小，該子濾波器的觀測噪聲越小，子濾波器的觀測精度越高，融合時給與該子濾波器較高的信任度，即該子濾波器的權(quán)重值就越高。在本文的融合算法中，每個子系統(tǒng)存在以上兩個融合子模塊，即誤差協(xié)方差模塊和匹配融合模塊。兩個子模塊分別采用協(xié)方差陣各項系數(shù)以及匹配最小鄰域的誤差進(jìn)行權(quán)重的求解，最后，按照式(24)可以通過子模塊的權(quán)重，計算最終的權(quán)重分配情況：δp,i·α+δr,i·γ＝ηi(24)在公式(24)中δp,i是第i個子系統(tǒng)由協(xié)方差子模塊計算出的權(quán)重值，δM,i是第i個子系統(tǒng)由匹配融合模塊計算出的權(quán)重值。ηi是改進(jìn)的信息融合方法綜合計算出的子系統(tǒng)權(quán)重。而α和γ分別是δp,i和δM,i在改進(jìn)的信息融合方法中所占的比例，其中α和γ服從公式(25)。α+γ＝1(25)因此，主濾波器輸出的t時刻機器人位姿的最優(yōu)估計結(jié)果滿足公式(26)。X‾r(t)=η1X‾r,1(t)+η2X‾r,2(t)+...ηnX‾r,n(t)---(26)]]>附圖說明圖1：機器人的運動模型和觀測模型圖2：分布式濾波器結(jié)構(gòu)圖圖3：EKF線性化誤差示意圖圖4：優(yōu)化的分布式SLAM原理框圖圖5：移動平臺示意圖圖6：橢圓形實驗環(huán)境示意圖圖7：IDH-EKF算法和DH-EKF算法路徑估計結(jié)果圖8：IDH-EKF算法和DH-EKF算法與GPS數(shù)據(jù)的誤差比較(X軸方向)圖9：IDH-EKF算法和DH-EKF算法與GPS數(shù)據(jù)的誤差比較(Y軸方向)圖10：矩形實驗環(huán)境示意圖圖11：IDH-EKF算法和DH-EKF算法路徑估計結(jié)果具體實施方式步驟1：建立模型機器人模型及坐標(biāo)系如圖1所示，以機器人初始時刻的位置為坐標(biāo)原點，以正東和正北方向為x軸和y軸的正方向。將整個狀態(tài)向量分為機器人位姿估計和路標(biāo)估計共五維狀態(tài)，t時刻機器人狀態(tài)矢量表示為Xt＝(xt,rT,mt,iT)T。設(shè)機器人的姿態(tài)表示為其中(xr,yr)表示機器人在地圖中的坐標(biāo)，表示航向角。路標(biāo)點在地圖中的位置表示為mi＝(xi,yi)T,i＝1:n，其中mi表示第i個路標(biāo)點對應(yīng)的位置信息，(xi,yi)表示路標(biāo)點在地圖中的坐標(biāo)值。步驟2：初始化地圖初始化，利用激光傳感器掃描數(shù)據(jù)計算得到的路標(biāo)點位置信息建立環(huán)境地圖。步驟3：地圖匹配并建立分布式子濾波器。獲取機器人t時刻測得的路標(biāo)點信息，并與已在全局地圖中存儲的路標(biāo)點信息相匹配。對于t時刻匹配失敗的路標(biāo)點信息，直接將其添加至全局地圖；對于t時刻成功匹配的路標(biāo)點，利用公式(3)分別對應(yīng)建立子濾波器。步驟4：子濾波器線性化利用公式(6)、(7)計算得到狀態(tài)方程和觀測方程的雅克比矩陣。根據(jù)公式(15)、(16)、(17)給出的新的線性化方法，可以得到子濾波器雅克比矩陣新的計算方法。改進(jìn)后子濾波器的估計更新過程如公式(18)、(19)、(20)所示。步驟5：子濾波器濾波過程通過擴展卡爾曼濾波算法對上述步驟中建立的各個子濾波器的狀態(tài)向量和協(xié)方差分別進(jìn)行預(yù)測和更新。預(yù)測階段：利用t-1時刻機器人位姿估計和該子濾波器對應(yīng)的協(xié)方差矩陣，預(yù)測t時刻狀態(tài)和協(xié)方差的先驗估計值，如公式(4)、(8)所示。更新階段：有了t時刻狀態(tài)的預(yù)測結(jié)果，然后再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，可以通過式子(9)，(10)和(11)得到現(xiàn)在狀態(tài)的最優(yōu)化估算值和協(xié)方差的更新值。步驟6：計算融合權(quán)值根據(jù)步驟5中各個子濾波器得到的機器人狀態(tài)的最優(yōu)估算值以及誤差協(xié)方差矩陣，利用公式(21)計算每個子濾波器協(xié)方差矩陣的權(quán)重；根據(jù)公式(22)得到子濾波器的匹配誤差值，利用公式(23)計算每個子濾波器匹配模塊的權(quán)重。步驟7：建立主濾波器，并對各子濾波器的結(jié)果進(jìn)行數(shù)據(jù)融合由公式(24)、(25)得到主濾波器中各子濾波器的融合權(quán)重。把各子濾波器中機器人位姿的狀態(tài)向量及其權(quán)值傳送給主濾波器，利用公式(26)主濾波器對各子濾波器中的數(shù)據(jù)進(jìn)行融合計算，最終得到機器人位姿的最優(yōu)估計，輸出結(jié)果。本文利用測量信息優(yōu)化分布式EKF估計過程的SLAM方法的原理框圖如圖4所示。下面通過真實實驗對本發(fā)明提出的利用測量信息優(yōu)化分布式EKF估計的SLAM方法進(jìn)行驗證。實驗所用平臺是自己搭建的兩輪小車，傳感器使用URG-04LX的激光傳感器，移動平臺的實物圖如圖5。實驗所用的環(huán)境是橢圓形和長方形跑道，以長筒作為路標(biāo)點在跑道兩旁隨機擺放，橢圓形實驗環(huán)境如圖6所示，矩形實驗環(huán)境如圖10所示，圓點表示小車起始位置，箭頭指示方向為小車行駛方向，具體路徑如圖所示。通過使用本發(fā)明算法和傳統(tǒng)的分布式SLAM算法對比，證明本算法的可行性和有效性。橢圓形跑道實驗結(jié)果對比如圖7，虛線(DH-EKF)表示的是單純利用航向輔助分布式EKF估計的運動軌跡，圖中實線(IDH-EKF)是基于本文利用測量信息優(yōu)化分布式EKF估計過程的SLAM方法得到的，兩者均能在運行一周后停留在起始點附近，圖中標(biāo)注的三個橢圓形區(qū)域為GPS的測算區(qū)域。利用場地環(huán)境的GPS測定結(jié)果中搜星數(shù)目較為理想的三段GPS數(shù)據(jù)為基準(zhǔn)，三段GPS時間起始點分別是：93.53s，138.42s，318.43s，每個時間段所對應(yīng)的兩種算法在x軸方向和y軸方向上與GPS數(shù)據(jù)的誤差情況如圖8和圖9所示。矩形跑道實驗結(jié)果對比如圖11，虛線(DH-EKF)表示的是利用基于航向輔助分布式SLAM的運動軌跡，可以觀察出其運行軌跡大致符合矩形軌道，但最終并沒有能夠回到初始起點；圖中實線(IDH-EKF)是基于本文利用測量信息優(yōu)化分布式EKF估計過程的SLAM方法得到的，其估計軌跡符合矩形軌道規(guī)律，同時，軌跡終點接近原點，可見其收斂性更好。通過上述兩組實驗證實，本發(fā)明中的優(yōu)化方法能有效地提高分布式SLAM算法在特殊環(huán)境變化時的穩(wěn)定性和精度。當(dāng)前第1頁1 2 3