一種基于卡爾曼濾波的DRLMS組合導航定位算法教學內容

1、Good is good, but better carries it.精益求精，善益求善。一種基于卡爾曼濾波的DRLMS組合導航定位算法-一種基于卡爾曼濾波的DR/LMS組合導航定位算法變電站/機器人/卡爾曼濾波1引言隨著國家十二五規(guī)劃綱要關于建設智能堅強電網要求的提出,智能電網信息化、數(shù)字化、網絡化已經成為當前電網智能化發(fā)展的一個熱點研究領域。目前,承載著電網輸變電環(huán)節(jié)的變電站,多采用人工巡檢方式監(jiān)控變電設備的運行狀態(tài),這一傳統(tǒng)巡檢方式費事費力。因此,基于智能機器人的無人值班變電站巡檢技術已然成為智能化變電站發(fā)展的一個創(chuàng)新性熱點話題,而機器人實現(xiàn)自主巡檢任務的關鍵及難點在于如何構建機器人自

2、主導航定位系統(tǒng)。目前,常用的導航系統(tǒng)多種多樣,導航原理、定位精度及成本造價也存在較大差別。常用的導航定位系統(tǒng)包括黑白線識別導航、磁導航、GPS/慣性組合導航、視覺圖像導航、激光雷達/慣性組合導航等。而較為成熟的機器人黑白線識別導航技術,通過激光對地面黑白線進行反射接收識別,保證機器人始終沿預設白線行走,該方法簡單易行,但施工較大,且易受大雪天氣影響;磁導航技術則利用磁傳感與測量技術,通過在地面鋪設磁條,保證機器人始終沿預設磁航道行走,該方法雖然解決了大雪天氣遮擋黑白線的問題,但其成本較大,且變電站長期強磁干擾容易導致磁條失磁,降低其靈敏度,最終可能導致導航失效。DGPS/慣性導航系統(tǒng)1定位精度

3、可以到達亞米級,定位靈活方便,但GPS受天氣、變電站強電磁干擾等外界環(huán)境因素影響較大,考慮系統(tǒng)的容錯性,需要增加額外獨立的導航子系統(tǒng)配合使用,而該系統(tǒng)成本造價高、經濟性較差。視覺圖像導航定位系統(tǒng)借鑒人體視覺導航原理,利用圖像識別及神經網絡技術,通過機器人預先對巡檢環(huán)境自主學習,建立基于當前環(huán)境模型下的知識庫與規(guī)則庫,即利用已有學習經驗來實現(xiàn)自主巡檢任務,但該方法技術難度較大,有待進一步深化研究。本文提出的慣性/激光雷達組合導航定位技術,利用高精度激光雷達測距技術,在預設全局路徑2的前提下,通過多點全向掃描測距、信息融合與濾波處理技術,對機器人當前運行環(huán)境進行視覺建模,同時利用多點測距技術實現(xiàn)實

4、時定位,該方法成本低廉,定位精度高,但對現(xiàn)場環(huán)境的依賴性相對較大,易受外界隨機干擾,需要優(yōu)化程序算法,減小外界的隨機干擾誤差等。文獻3基于激光雷達測距信息進行機器人周邊環(huán)境建模,提出了一種時變勢場算法,并通過對比實驗引入了一種改進型算法多分辨率勢場法,該算法可以有效地實現(xiàn)機器人導航與避障,實時性非常好。文獻4討論了基于激光雷達的機器人實時位姿估計方法,通過基于Hough變換的切線角度直方圖算法和迭代切線加權最近點算法,解決了傳統(tǒng)方法中未能解決的局部最小值、類孔徑及大計算量問題,該方法精度高,實時性好,并且對隨機噪聲、遮擋和類孔徑問題具有高魯棒性特點,適用性廣。上述算法對本文中激光雷達的數(shù)據程序

5、處理方法有一定的借鑒作用。本文基于DR/LMS的組合導航定位技術,特別地,提出了一種基于卡爾曼濾波算法的定位精度優(yōu)化估計方法,通過DR子系統(tǒng)與LMS子系統(tǒng)的相互補充與修正,對原有系統(tǒng)定位及導航精度(定位精度可以達到cm級)加以改進與優(yōu)化,進一步提高了導航定位的穩(wěn)定性與精確性。文獻5通過對外界隨機干擾噪聲的統(tǒng)計與建模,對Kalman濾波理論做了詳細的推導與誤差分析,同時提出了改進型的非線性卡爾曼濾波模型與算法,并給出了實例分析與驗證。2DR導航定位系統(tǒng)原理我們這里提出的DR(Dead-Reckoning)航位推算是一種基于機器人車體運動模型的、自主式導航定位系統(tǒng),可以實現(xiàn)二維平面內的航位推算。本

6、系統(tǒng)采用里程計與激光雷達的組合式傳感器定位系統(tǒng)。里程計主要用于采樣機器人左右輪的行走路程,而機器人航位角的采樣計算則是高精度激光雷達的應用之一(激光雷達測距系統(tǒng)同時實現(xiàn)外部測距定位與航向角的采樣計算)。基于激光雷達的機器人偏轉航向角測量系統(tǒng)的優(yōu)點是有效解決了強電磁干擾條件下電子羅盤失效以及光纖陀螺累積誤差帶來的定位精度大幅降低等一系列難題,但不足的是其需要依賴于外部參考測距對象,獨立性較差。2.1DR導航定位系統(tǒng)原理圖簡介DR導航定位系統(tǒng)航位推算原理如圖1所示。已知初始點A的定位坐標、導航測量角及光電位移傳感器測量里程可以計算出系統(tǒng)下一時刻在B點的坐標估計值。其中,為車輪旋轉一周的傳動里程,k

7、為旋轉光電位移傳感器的采樣脈沖計數(shù)(車輪每旋轉一周進行一次脈沖采樣),則為程序采樣周期內,機器人的行駛距離。表示機器人與道路方向(Y軸方向)的夾角大小,其值由激光測距系統(tǒng)采樣間接計算得到。因此,在已知機器人初始位置的前提下,由式(1)可以求解得到機器人在X、Y軸方向的行駛位移。(1)圖1DR系統(tǒng)導航定位原理圖通過上式,可以實時推算機器人當前所在坐標。但鑒于機器人車體本身存在機械不對稱性,里程計存在一定的測量誤差,并考慮左右輪電機出力的不同步性,其定位精度經現(xiàn)場測試,可以達到80cm100cm左右,雖然可以基本滿足現(xiàn)場應用,但由于慣性DR導航精度具有隨時間發(fā)散的特性,即長期穩(wěn)定性差,必須輔助外在

8、定位系統(tǒng)進行及時的校正。2.2DR導航定位系統(tǒng)程序流程圖DR導航定位系統(tǒng)程序流程圖見圖2所示。圖2DR導航定位系統(tǒng)程序流程圖3激光雷達測距導航定位原理本系統(tǒng)采用了SICK公司生產的LMS激光雷達全向測距傳感器,該測距系統(tǒng)采用雙脈沖快速測距技術,以50hz的掃描頻率實現(xiàn)360度、20m范圍內障礙物的快速掃描、高精度(mm級)測距需求。測距系統(tǒng)與上位機LABVIEW軟件系統(tǒng)通過以太網接口實現(xiàn)相互通信,數(shù)據傳輸及處理速度能夠滿足實際工業(yè)需求。機器人通過發(fā)送采樣相應請求指令,對周圍360度、20m范圍內障礙物進行距離采樣,采樣角度間隔為0.5度或0.25度可選。那么,對于同一障礙物或標記物,實際中會得

9、到多個采樣距離值,因此需要對這些采樣距離值進行均值濾波等一系列數(shù)據處理與合并。而如何保證濾波后的距離值與實際障礙物或標記物的一一映射關系,即實現(xiàn)采樣數(shù)據的識別與驗證則是整個定位系統(tǒng)的關鍵。本定位子系統(tǒng)誤差來源:(1)道路兩側給定參考標記物的地理坐標測量誤差;(2)實測目標距離值濾波合并后的處理誤差。實現(xiàn)的功能:(1)機器人通過測量其距離道路兩側標記物(物理坐標給定)的各距離值,通過圓或三角形方程求解,粗略計算其所在的地理坐標(x,y)。(2)機器人利用掃描道路兩側標記物得到的掃描角度間隔,并結合道路方向與機器人車頭方向(即掃描中線角度值),精確計算(誤差相互抵消)得到機器人的實際行走方向,即航

10、向角。而機器人正常行走的二維控制參數(shù)就是上述計算得到的x坐標(橫向偏移)與導航角。3.1LMS導航定位系統(tǒng)原理圖簡介圖3LMS系統(tǒng)導航定位原理圖LMS導航定位系統(tǒng)原理圖見圖3所示,其定位與導航計算式分別見式(2)(3):(2)其中,分別為參考點1,2的測量坐標值;,分別為機器人距離參考點1,2的激光測距半徑值;則為需要求解的機器人實際坐標值。該定位原理簡單,但重點、難點在于如何對多點掃描距離值進行濾波處理,包括多點合并、濾波等效以及干擾值的驗證與排錯等諸多環(huán)節(jié),其預處理流程圖見圖4。只有保證測距目標與實際距離的一一對應,以及多點濾波合并后的高精度性,才能進一步提高定位精度。其定位精度經現(xiàn)場測試

11、,可以達到2040cm,可以滿足現(xiàn)場工業(yè)應用需求。航向角計算式如下:(3)其中,、表示采樣預處理后,機器人距離當前兩參考目標1,2的有效半徑值;表示半徑、之間的掃描間隔角度;表示經激光測距得到的參考點1,2之間的等效距離值(這里,考慮到1,2參考點間距的人工量測誤差,我們以高精度激光測距系統(tǒng)的理論計算值為計算標準);表示距離機器人最近的參考目標1或2分別與機器人定位點、次近參考目標2或1連線的夾角計算值;、分別表示參考目標坐標點1,2連線的斜率、道路方向的斜率(考慮到道路方向,即Y軸方向斜率為無窮大,這里我們將X,Y坐標軸進行調換求兩直線的夾角);r表示道路方向與參考目標點1,2連線的夾角;表

12、示、中的較小者對應的掃描角度值;表示機器人實際車頭方向與之間的間隔角度(其中是車頭方向的掃描角度,固定不變);表示機器人理想前進方向(即Y軸道路方向)與之間的間隔角度(三角形求解);表示機器人實際車頭方向與道路方向的偏角計算值,即我們要求的機器人航向角。本算法實現(xiàn)相對簡單,只需要通過激光測距系統(tǒng)對當前環(huán)境參數(shù)進行識別、采樣即可快速實現(xiàn)在線計算與控制。其中、作差抵消了多點掃描帶來的絕對誤差,有效提高了航向角精度。經現(xiàn)場測試,可以達到0.1度甚至更高,可以滿足實際工業(yè)應用需求。此外,本算法的特別之處在于,當選取的參考點1,2關于道路左右對稱,即時,由式(3)中(b)(e)(g)可以看出,航向角僅與

13、當前采樣參數(shù)有關,而與目標參考點的坐標選取無關,進一步提高了系統(tǒng)的獨立性。3.2LMS導航定位系統(tǒng)的程序流程圖LMS導航定位系統(tǒng)程序流程圖如圖4所示。圖4LMS系統(tǒng)導航定位程序流程圖4卡爾曼濾波在組合導航定位系統(tǒng)中的應用卡爾曼濾波本質上是一種建立在時域空間內的線性最小方差估計算法,具有時域狀態(tài)遞推性,適用于對多維隨機過程(平穩(wěn)、非平穩(wěn))進行狀態(tài)估計,包括了連續(xù)與離散兩類算法。其算法簡潔高效,便于在計算機上實現(xiàn),而卡爾曼濾波在實際組合導航系統(tǒng)中是其較為成功的一個典范。本文以隨機線性離散卡爾曼濾波為理論基礎,實現(xiàn)了變電站智能機器人的實時精確導航與定位。4.1卡爾曼濾波在DR/LMS組合導航定位系統(tǒng)

14、中的現(xiàn)場應用基于Kalman濾波的DR/LMS組合導航定位系統(tǒng)整體結構圖如圖5所示:上述兩導航定位子系統(tǒng)各有其優(yōu)缺點:DR系統(tǒng)自主導航定位能力較強,但位移傳感器測量精度較低,使得系統(tǒng)定位精度隨時間發(fā)散,即長期穩(wěn)定性差;LMS系統(tǒng)雖然其精度高,但受外在環(huán)境影響較大,主要表現(xiàn)在參考點的識別與交替更新在某些不可預測條件下的誤判現(xiàn)象,從而導致定位錯亂等。為了保證系統(tǒng)的定位精度、提高系統(tǒng)的穩(wěn)定性,這里我們引入一種基于卡爾曼濾波的組合導航技術,即DR/LMS組合導航技術,通過最優(yōu)線性估計算法,使兩個定位子系統(tǒng)相互校正,取長補短,既可以及時校正DR航位推算隨時間帶來的航位漂移,又可以有效地修正LMS系統(tǒng)對參

15、考點的錯誤識別,極大地改善了整個系統(tǒng)的動態(tài)穩(wěn)定性與準確性。基于卡爾曼濾波模型,并考慮到導航角本身的計算精度可以滿足需求,因此,這里我們僅對DR/LMS組合導航系統(tǒng)的X、Y定位坐標作Kalman濾波,方程如下:狀態(tài)方程:(4)觀測方程:(5)考慮到前后兩次采樣時間極小,故認為,因此,這里我們取k-1表示k-1狀態(tài)到k狀態(tài)過渡過程的等效航向角。上式中,分別表示機器人在X軸、Y軸方向的狀態(tài)位置坐標;k-1表示狀態(tài)控制量,即計算航向角;表示各采樣階段的轉移控制系數(shù);、表示DR推算過程噪聲序列;、表示DR推算過程噪聲輸入系數(shù);、分別表示激光測距計算得到的機器人定位點的x、y坐標測量等效值;、表示LMS測

16、量系統(tǒng)等效噪聲序列。根據變電站現(xiàn)場環(huán)境,系統(tǒng)過程噪聲序列與觀測噪聲序列可以看做是均值恒定的高斯白噪聲隨機序列,并設系統(tǒng)過程噪聲(即估計噪聲)方差為,系統(tǒng)觀測噪聲(即LMS測量噪聲)方差為。且在整個濾波過程中,系統(tǒng)過程噪聲序列與觀測噪聲序列不相關,系統(tǒng)初始狀態(tài)與過程噪聲序列、觀測噪聲序列也不相關。系統(tǒng)定位坐標初始狀態(tài)、由激光測距系統(tǒng)給定,濾波誤差初始值P0已知。具體的噪聲建模及參數(shù)確定,由于篇幅限制不作詳述,可以參考文獻5。依據Kalman濾波求解步驟,DR/LMS系統(tǒng)的濾波估計求解過程如下:I.狀態(tài)一步預測:(6)II.狀態(tài)估計:(7)III.濾波增益:(8)IV.一步預測誤差:(9)V.濾波

17、估計誤差:(10)4.2DR/LMS組合導航系統(tǒng)現(xiàn)場運行結果分析(1)基于Matlab的主要導航參數(shù)波形分析機器人視覺區(qū)域范圍內對參考標記點的自主識別與更新曲線如圖6所示,圖(1)綠色曲線表示機器人視覺范圍內距離參考標記點的距離值變化曲線,即隨著機器人的前進路徑方向,其距離參考標記點的距離值逐漸減小,直至進入下一段參考域時,參考標記將會發(fā)生更新替換,此時參考距離值會發(fā)生瞬時突變,即上圖中綠色曲線由小及大的突變,也就是說機器人將根據測量距離(綠色)這一視覺參量來及時更新參考標記點,以便進入下一參考域的定位導航階段。圖(2)藍色曲線表示與測量距離相對應的測量角度變化趨勢,其主要用于視覺識別中干擾物

18、的查找、驗證及糾錯等。中圖綠色曲線表示突變脈沖,即參考距離值發(fā)生指定裕度內的突變時,輸出記為布爾值1,反之,記為0。相應地,對于中圖每次的突變脈沖,圖(3)則對應顯示了該時刻機器人參考標記點的依次替換、更新過程。在本系統(tǒng)中,機器人實現(xiàn)理想直線行走的關鍵在于兩個控制量的計算:X坐標偏移量Vx和導航角。由于取Y軸方向為機器人理想行駛方向,故X坐標能實時記錄機器人與中心路徑(X=)的偏移量Vx。圖7顯示了機器人沿Y軸方向行駛過程中縱坐標Y的變化曲線。其中藍色曲線(一部分與紅色曲線相互重合)表示未經限幅濾波的軌跡變化,參雜了外界噪聲的干擾,而紅色曲線則添加了限幅濾波,過濾掉了實際過程中不可能出現(xiàn)的坐標

19、變化量,該試驗曲線線性度非常好,完全符合現(xiàn)場運行軌跡。經過卡爾曼濾波后的Y坐標變化曲線線性度與此紅色曲線類似,區(qū)別僅僅在于相對挺高了定位精度。圖8顯示了組合導航系統(tǒng)X坐標定位值的精度變化,圖中道路中心線方向的直線方程為X=3.3附近。其中,黃線代表機器人實際行走過程中X定位值的正常變化趨勢,即X坐標的理想變化曲線;藍線代表DR/LMS組合導航定位系統(tǒng)原始定位坐標X的變化曲線;紅線代表組合導航系統(tǒng)經限幅濾波、均值濾波等一系列常用經典濾波后的濾波定位坐標X的變化曲線;綠線則代表組合導航系統(tǒng)引入Kalman濾波后的X值變化曲線。因此,我們可以明顯地看出,與X值理想定位變化趨勢相比,經過Kalman濾

20、波后的定位波形具有更高的精度與可信度。經現(xiàn)場測試,其定位精度完全符合實際中的運行軌跡,精度可以達到10cm左右,引入模糊控制后的系統(tǒng)參數(shù)可調裕度大,運行效果非常理想。(2)基于Labview的導航系統(tǒng)現(xiàn)場軌跡試驗波形分析圖9機器人現(xiàn)場運行軌跡模擬圖圖9對變電站現(xiàn)場地理環(huán)境進行了直觀的描述,坐標系的選取如圖中所示。這里,取道路方向為Y軸方向,道路寬度介于X=1.74.7之間。其中,方塊表示參考電氣附件(桿塔),各個電氣附件的Y坐標如圖中所示,而將道路左側電氣附件的X坐標統(tǒng)一定義為0,道路右側電氣附件的X坐標根據現(xiàn)場測量結果統(tǒng)一定義為6.8,這樣也就建立了導航系統(tǒng)的參考坐標知識庫。根據機器人現(xiàn)場運行軌跡,這里我們用圖中藍色曲線近似模擬機器人的實際行走路線。在Y=40.1的標記桿塔之前,機器人有偏離道路中心線向右行走的趨勢,偏離幅度在10cm20cm之間,而經過該