一種基于卡爾曼濾波的DRLMS組合導(dǎo)航定位算法教學(xué)內(nèi)容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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