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

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

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