激光傳感器融合的ekf定位算法研究

激光傳感器融合的ekf定位算法研究

0位置傳感器的限制移動機器人的定位是機器人學研究的重點和熱點。在機器人的即時定位中,單純依靠里程計的航位推算法顯然不合適,由于里程計存在累計誤差,隨著機器人運動時間的推移,最終將導致定位失敗。一般是借助外部傳感器,如激光傳感器和視覺傳感器等,在擴展卡爾曼濾波器(EKF)的框架下對傳感器數(shù)據(jù)進行融合,從而獲得機器人的位姿信息。但是EKF也存在由非線性系統(tǒng)線性化帶來計算精度的下降,甚至造成濾波器的不穩(wěn)定。同時推導Jacobian矩陣也是比較困難的,有時難以實現(xiàn)。針對EKF濾波器的不足,文獻在UT變換的基礎上提出了Unscented卡爾曼濾波器(UKF),UKF直接利用非線性系統(tǒng)模型,無須線性化,從而提高了濾波精度。同時UKF不需要推導復雜的Jacobian矩陣,易于計算機編程。本文針對室內結構化環(huán)境下移動機器人的運動,提取室內環(huán)境中的線段特征,在UKF的大框架下對里程計數(shù)據(jù)和激光傳感器數(shù)據(jù)進行融合,從而獲得高精度的移動機器人的位姿。1段特征的提取本文針對室內結構化環(huán)境,對激光數(shù)據(jù)采用哈夫變換進行分簇處理,將每個簇得數(shù)據(jù)用最小二乘法提取環(huán)境中的線段特征。1.1局部直線檢測如圖1所示,變換(HoughTransform)是模式識別領域中對二值圖像進行直線檢測的有效方法,它檢測已知點的共線性,是一種全局性的檢測方法?？梢院芎玫匾种聘蓴_和噪聲,同時還可以將已知數(shù)據(jù)點集擬合成多條直線。在標準參數(shù)化方式下,平面直角坐標系中的直線l表達為ρ=xcosθ+ysinθ(ρ≥0,0≤θ<2π)(1)其中,ρ和θ含義如圖1所示。根據(jù)(1)式,直線l上不同的點在參數(shù)空間中被變換為一簇相交于點p點的正弦曲線。顯然,若能確定參數(shù)空間中的p點(局部最大值),就實現(xiàn)了直線檢測。首先將ρ和θ離散化,由(1)式計算出每一個數(shù)據(jù)點相應的(ρ,θ)。根據(jù)哈夫變換得投票機制,可獲得局部最大值,即數(shù)據(jù)點所在直線的參數(shù)。如圖2所示,對激光傳感器采集的一幀數(shù)據(jù)進行哈夫變換處理,可將數(shù)據(jù)點集分成AB、BC、CD3個簇,每個簇代表一條線段。1.2乘意義下的品變擬合經上述方法處理以后的數(shù)據(jù)就可以進行線段擬合了。這里將(ri,φi)轉化為直角坐標(xi,yi),其中,xi=ricosφi,yi=risinφi,并假設各數(shù)據(jù)點具有相同的不確定性。文獻對此做了詳細的研究,給出了最小二乘意義下的線段特征及其協(xié)方差矩陣,αl、ρl可以表示為αl=12arctan(ba-c)-π2(2)ρl=ˉxcosαl+ˉysinαl(3)其中,ˉx=1ΝΝ∑i=1xi;ˉy=1ΝΝ∑i=1yi;a=Ν∑i=1(xi-ˉx)2;b=2Ν∑i=1(xi-ˉx)(yi-ˉy);c=Ν∑i=1(yi-ˉy)2。線段參數(shù)的協(xié)方差矩陣為Ρl=[σ2ρρσ2ρασ2αρσ2αα]=aσ2yy-bσ2xy+cσ2xx(a-c)2+b2[e2-e-e1]+[σ2yycos2ψ+σ2xxsin2ψ-2σ2xysinψcosψΝ000](4)其中,ψ=αl+π/2;e=ˉxcosψ+ˉysinψ;σxy=0;σxx=σyy=σr;σr為激光傳感器數(shù)據(jù)的均方差。2機器人運動系統(tǒng)的建模2.1兩組k、k的距離設機器人在k時刻的全局位姿為(xk,yk,θk)T,uk=(δk,Δθk)T為k時刻的輸入控制量,其中,δk為機器人在k時刻移動的距離,Δθk為機器人在k時刻旋轉的角度。對于兩輪差分驅動,δk=(δrk+δlk)/2,Δθk=(δrk-δlk)/d,其中,δrk和δlk分別為右輪和左輪移動的距離;d為兩輪之間的距離。則里程計模型可表示為Xk+1=F(Xk,uk)+vk=(xk+δkcos(θk+Δθk)yk+δksin(θk+Δθk)θk+Δθk)+vk(5)很顯然,里程計模型是一個非線性系統(tǒng),vk為高斯白噪聲。2.2全局使用擬合設Yk=(ρl,αl)為k時刻直線在機器人坐標系下的特征參數(shù)。Gk為k時刻全局環(huán)境模型,其中包含c條線段。則由圖3的幾何關系可得觀測方程為Yk,i=Η[Xk,Gk]+nk=(ρi,g-√x2k+y2kcos(βi-αi,g)αi,g-θk)+nk(i=1,?,c)(6)其中,(ρg,αg)為直線在全局坐標系下的特征參數(shù);nk是高斯白噪聲。3基于ukf的機器人定位3.1求解jacab1針對一個非線性模型,EKF將非線性模型用泰勒展開式展開,因此EKF的估計值實際上是一階近似的。由于舍去了二階項和高階項,一階線性近似就有可能將較大的誤差引入到濾波后隨機變量的實際均值和協(xié)方差矩陣中去,而且推導Jacobian矩陣也是比較困難的,有時難以實現(xiàn)。針對EKF在非線性系統(tǒng)中存在的不足,文獻提出了Unscented卡爾曼濾波器(UKF),并成功地應用于非線性系統(tǒng)。UKF是以UT變換為基礎的,UT變換是一種計算隨機變量非線性變換的統(tǒng)計量的方法。設x是一個L維隨機變量,其均值和協(xié)方差分別為ˉx和Pxx,y是另一隨機變量,它與x成非線性關系:y=f(x)。UT變換就是選擇一些點,使得其樣本均值和協(xié)方差矩陣為ˉx和Pxx,將這些點帶入到非線性函數(shù)中得到變換后的點,并用變換后的點的均值和協(xié)方差矩陣來估計ˉy和Pyy。3.2機器人位姿預測(1)初始化。計算公式如下:?X0|0=E[X0](7)Ρ0=E[(X0-?X0|0)(X0-?X0|0)Τ](8)(2)對于給定的?Xk-1|k-1、Pk-1|k-1,用UT法求狀態(tài)一步預測?Xk|k-1,以及預測誤差的協(xié)方差陣Pk|k-1。計算σ點ε(i)k-1|k-1,i=0,…,2L,即ε(0)k-1|k-1=?Xk-1|k-1(9)ε(i)k-1|k-1=?Xk-1|k-1+(√(L+λ)Ρk-1|k-1)i(i=1,?,L)(10)ε(i)k-1|k-1=?Xk-1|k-1-(√(L+λ)Ρk-1|k-1)i(i=L+1,?,2L)(11)Wm0=λ/(L+λ)(12)W(c)0=λ/(L+λ)+(1-α2+β)(13)Wmi=W(c)i=1/[2(L+λ)](i=1,??2L)(14)其中,λ=α2(L+κ)-L,α決定ˉx周圍采樣點分布的遠近程度,κ是比例因子,一般為零,β包含x的先驗分布信息。通常令α=0.1,κ=0,β=2。對上述σ點,根據(jù)(5)式的里程計模型對k時刻系統(tǒng)的狀態(tài)進一步預測。X(i)k=F(X(i)k-1|k-1)+vk-1(15)?Xk|k-1=2L∑i=0WmiX(i)k(16)Ρk|k-1=2L∑i=0W(c)i(X(i)k-?Xk|k-1)×(X(i)k-?Xk|k-1)Τ+Ρvk(17)(3)用UT求σ點?Xk|k-1,Ρk|k-1通過量測方程的傳播。由(9)～(11)式計算σ點ε(i)k;然后進行觀測方程預測:設k時刻全局地圖共有c條線段,根據(jù)(6)式的觀測方程對全局地圖中第j條線段,在k時刻機器人位姿下的特征參數(shù)進一步預測。Y(i)k=H(ε(i)k)+nk(i=0,…,2L)(18)?Yk=2L∑i=0WmiY(i)k(i=0,??2L)(19)Ρj?Yk?Yk=2L∑i=0Wci(Y(i)k,j-?Yk,j)(Y(i)k,j-?Yk,j)Τ+Ρn(20)Ρj?Xk?Yk=2L∑i=0Wci(X(i)k,j-?Xk|k-1)(Y(i)k,j-?Yk,j)Τ(j=1,?,c)(21)(4)特征匹配。特征匹配是將系統(tǒng)觀測模型預測的線段特征,與激光傳感器得到的局部地圖的實際線段特征進行匹配。為了減少假匹配,只對長度大于30cm的線段進行匹配。同時為了獲取統(tǒng)計意義上的最佳匹配對,可以計算預測線段特征?Yk,j與實測線段特征Yik之間的Mahalanobis距離,即d(Yik,?Yjk,j)=(Yik-?Yk,j)Τ(Ρil+Ρj?Yk?Yk)-1×(Yik-?Yk,j)(22)其中,Pil為第i條實測線段特征的協(xié)方差矩陣。由于Mahalanobis距離服從χ2α(n)分布,這里n=2,在置信度α為95%的條件下,2線段特征的匹配條件為d(Yik,?Yjk,j)<5.991。(5)機器人位姿更新。設由第4步特征匹配獲得的匹配線段的數(shù)目為m條,設?X0k=?Xk|k-1,Ρ0k=Ρk|k-1,對匹配成功的m條線段(j=1,…,m)按照下列各式進行迭代,對機器人的位姿進行修正,即Κjk=Ρj?Xk?YkΡj?Yk?Yk-1(23)?Xjk=?Xj-1k+Κjk(Yjk-?Yjk)(24)Ρjk=Ρj-1k-ΚjkΡj?Yk?Yk(Κjk)Τ(25)當?shù)^程結束時,機器人在k時刻的最終位姿為?Xk=?Xmk,Ρk=Ρmk。(6)全局環(huán)境模型更新。在機器人位姿更新以后,需要對(6)式中的全局環(huán)境模型進行更新。對于步驟(4)中滿足匹配條件的2條線段,按照下式進行特征參數(shù)的合并,即Y^k=Ρm((Ρlj)-1Ykj+(ΡY^kY^kj)-1Y^kj)(j=1,?,m)(26)Ρm=((Ρlj)-1+(ΡY^kY^kj)-1)-1(j=1,?,m)(27)然后按照(28)式和(29)式將線段的特征參數(shù)轉換到全局環(huán)境模型中。ρg=ρl+(xk,yk)(cosθk-sinθksinθkcosθk)(cosαlsinαl)(28)αg=αl+θk(29)對于不滿足匹配條件的局部特征線段,則直接按照(28)式和(29)式轉換到全局地圖中,從而完成全局環(huán)境模型的更新。4前測值與定姿對比本文的算法在上海廣茂達公司的“能力風暴”越野版機器人上進行了實驗,該機器人配備了里程計和激光傳感器。激光傳感器測量數(shù)據(jù)的距離標準差σr=±0.01m,角度標準差σφ=±4.36×10-4rad。通常,環(huán)型的軌跡很難進行準確的位姿估計。所以在本實驗中,設定機器人的移動軌跡為一個半徑為3m的順時針圓形,速度v=0.5m/s,為了驗證本文算法對機器人方向角估計的準確度,在機器人上安裝了電子羅盤,比較算法對方向角的估計值和電子羅盤的讀數(shù)。圖4所示給出了移動機器人在室內環(huán)境中,利用UKF估計算法走圓形路徑的定位效果圖。從圖中可以看出,基于UKF的定位算法的定位結果與移動機器人的實際位姿是比較相符的。圖5所示為本文算法估計的機器人的位姿與實測機器人位姿的偏差。從圖中可以看出,在機器人開始移動的一段時間內,估計值與實測值誤差較大,這是因為開始階段機器人獲得的環(huán)境信息的線段特征相對較少,同一條線段匹配的次數(shù)也較少,不確定性較大。隨著時間的推移,環(huán)境信息的線段特征越來越多,同一條線段匹配次數(shù)的不斷增加,其不確定性將越來越小。所以