黃俊杰,李海濱,賈翠玲
(1.內(nèi)蒙古工業(yè)大學(xué)材料科學(xué)與工程學(xué)院,內(nèi)蒙古呼和浩特 010051;2.內(nèi)蒙古工業(yè)大學(xué)工程訓(xùn)練教學(xué)部,內(nèi)蒙古呼和浩特 010051)
隨著物聯(lián)網(wǎng)技術(shù)的快速發(fā)展,工廠、商店、醫(yī)院等都在走向智能化,人們對(duì)定位的穩(wěn)定性和精度的要求變得越來(lái)越高。在室外環(huán)境中,依靠全球衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System,GNSS)如GPS、北斗等與慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)的組合導(dǎo)航可以達(dá)到很高的定位精度,但是由于GNSS信號(hào)容易受復(fù)雜環(huán)境影響,在室內(nèi)無(wú)法提供有效的定位數(shù)據(jù),因此開(kāi)發(fā)可靠的室內(nèi)定位系統(tǒng)(Indoor Positioning System,IPS)具有很高的實(shí)用價(jià)值。
超寬帶(Ultra-Wideband,UWB)是利用極窄脈沖傳輸?shù)凸β薀o(wú)線信號(hào)的新興通信技術(shù),時(shí)間分辨率高,定位精度可以達(dá)到厘米級(jí)別,在穿透性、抗多徑能力和抗干擾性方面都有著很好的表現(xiàn),加上高的傳輸功率和低功耗的特點(diǎn),所以UWB廣泛應(yīng)用于室內(nèi)高精度定位。但是同其他無(wú)線定位系統(tǒng)一樣,數(shù)據(jù)抖動(dòng)和非視距環(huán)境下的數(shù)據(jù)失真也是UWB系統(tǒng)存在的主要問(wèn)題。從動(dòng)輪式里程計(jì)固定在機(jī)器人上,結(jié)合陀螺儀的角度信息可以對(duì)機(jī)器人的位置信息進(jìn)行實(shí)時(shí)計(jì)算,具有結(jié)構(gòu)簡(jiǎn)單、成本低、短時(shí)精度高等優(yōu)點(diǎn)。從動(dòng)輪式里程計(jì)在短時(shí)內(nèi)有很好的定位效果,但是隨著時(shí)間增長(zhǎng)會(huì)產(chǎn)生較大的累計(jì)誤差,所以單一的里程計(jì)模式不適合長(zhǎng)時(shí)間定位。文獻(xiàn)[9]提出了一種結(jié)合馬爾可夫鏈和指紋匹配算法的UWB技術(shù),是一種解決NLOS(Non-Line of Sight)誤差問(wèn)題的有效方法,但是需要先對(duì)UWB的數(shù)據(jù)在指紋數(shù)據(jù)庫(kù)中進(jìn)行匹配,計(jì)算量過(guò)大。文獻(xiàn)[10]提出了一種基于UWB、里程計(jì)和RGB-D融合的室內(nèi)定位方法,使用了加權(quán)平均的思想,成功將誤差抑制在10 cm以內(nèi),但并沒(méi)有對(duì)UWB處于嚴(yán)重非視距狀態(tài)下的情況進(jìn)行處理。文獻(xiàn)[11]提出了一種基于改進(jìn)自適應(yīng)加權(quán)數(shù)據(jù)融合的粒子濾波定位算法,與擴(kuò)展卡爾曼融合算法相比其定位精度提高了15%以上。文獻(xiàn)[12]通過(guò)求解LiDAR(Light Detection and Ranging)定位信息與粒子之間的幾何距離來(lái)更新粒子的權(quán)重,從而彌補(bǔ)UWB的非視距誤差。但是這2種方法都沒(méi)有對(duì)嚴(yán)重非視距狀態(tài)進(jìn)行判斷,而且沒(méi)有對(duì)IMU(Inertial Measurement Unit)和LiDAR的累計(jì)誤差進(jìn)行判斷和消除,導(dǎo)致系統(tǒng)長(zhǎng)時(shí)間運(yùn)行效果不好。
針對(duì)上述問(wèn)題,本文作者提出一種基于卡爾曼濾波的UWB和里程計(jì)數(shù)據(jù)融合的定位方法。該方法將里程計(jì)短時(shí)精度高和UWB長(zhǎng)時(shí)誤差有界性的優(yōu)點(diǎn)進(jìn)行融合,有望在單片機(jī)上實(shí)現(xiàn)長(zhǎng)時(shí)高精度定位。作者先以里程計(jì)的數(shù)據(jù)作為參考量對(duì)UWB數(shù)據(jù)進(jìn)行非視距情況的判斷;然后使用卡爾曼濾波算法對(duì)里程計(jì)和UWB數(shù)據(jù)進(jìn)行不同信賴度下的融合;再使用融合后的數(shù)據(jù)對(duì)里程計(jì)數(shù)據(jù)進(jìn)行累計(jì)誤差的判斷和修正,來(lái)減小里程計(jì)的累計(jì)誤差,提高系統(tǒng)長(zhǎng)時(shí)間運(yùn)行的精度和穩(wěn)定性,以實(shí)現(xiàn)長(zhǎng)時(shí)高精度定位。
1.1.1 定位算法原理
UWB基站和標(biāo)簽使用雙向飛行時(shí)間法(Two Way-Time of Flight, TW-TOF)可以得到基站和標(biāo)簽之間的距離,如圖1所示。
圖1 TW-TOF測(cè)距方法 圖2 三邊定位算法原理
標(biāo)簽在時(shí)刻發(fā)射請(qǐng)求性質(zhì)的脈沖信號(hào),基站在時(shí)刻接收到信號(hào),在時(shí)刻發(fā)射一個(gè)響應(yīng)性質(zhì)的脈沖信號(hào),標(biāo)簽在時(shí)刻接收到,由此可以計(jì)算出信號(hào)總飛行時(shí)間和處理時(shí)間。無(wú)線電傳播速度為光速,則可以得到兩模塊的距離。
(1)
在超寬帶定位過(guò)程中,采用三基站一標(biāo)簽的定位方法,使用三邊定位算法得到標(biāo)簽的坐標(biāo)信息,如圖2所示。
假設(shè)標(biāo)簽的坐標(biāo)是(,),第個(gè)基站的坐標(biāo)是(,),基站到標(biāo)簽的距離是,此時(shí)二維模型下標(biāo)簽坐標(biāo)滿足式(2):
(2)
將式(2)各公式進(jìn)行平方后得到一個(gè)非線性方程組,用方程組中的前-1個(gè)方程減去第個(gè)方程,整理后得到線性方程組=
其中:
(3)
(4)
使用最小二乘法求解方程可以得到標(biāo)簽坐標(biāo):
=()
(5)
1.1.2 非視距誤差產(chǎn)生原因
當(dāng)UWB信號(hào)傳播遭到障礙物的阻擋而無(wú)法直線傳輸時(shí),就會(huì)出現(xiàn)UWB定位的非視距問(wèn)題。在非視距傳播情況下,信號(hào)只能穿透障礙物或者借助反射、折射和衍射的方式進(jìn)行傳輸,使得到的信息含有大量失真信號(hào),嚴(yán)重影響三邊定位算法的計(jì)算精度。
輪式里程計(jì)定位系統(tǒng)由圖3所示的正交從動(dòng)輪式里程計(jì)和一個(gè)JY61六軸陀螺儀組成,輪式里程計(jì)由2個(gè)正交的全向輪和2個(gè)光電編碼器組成,由陀螺儀來(lái)提供姿態(tài)角。
圖3 正交從動(dòng)輪式里程計(jì)
將里程計(jì)固定到移動(dòng)機(jī)器人底盤(pán)上,里程計(jì)的2個(gè)全向輪相對(duì)于移動(dòng)機(jī)器人車(chē)體中心的垂直距離分別為和。計(jì)算單個(gè)采樣周期內(nèi)機(jī)器人的位移坐標(biāo)增量,然后積分,從而得到機(jī)器人相對(duì)于參考坐標(biāo)系的位姿狀態(tài)。如圖4所示,在一個(gè)采樣周期內(nèi),2個(gè)光電編碼器測(cè)得2個(gè)全向輪旋轉(zhuǎn)軌跡的增量為d和d。機(jī)器人相對(duì)于機(jī)器人車(chē)體坐標(biāo)系的位移增量分別記為d和d,可以由d和d以及機(jī)器人旋轉(zhuǎn)角與、等參數(shù)表示。機(jī)器人相對(duì)于參考坐標(biāo)系的位移增量分別記為d和d,可由d和d及機(jī)器人旋轉(zhuǎn)角表示。機(jī)器人的旋轉(zhuǎn)角度為可由JY61六軸陀螺儀給出。
圖4 機(jī)器人移動(dòng)姿態(tài)
最后將d和d進(jìn)行積分,得到機(jī)器人相對(duì)于參考坐標(biāo)系的位姿信息(,,)。
(6)
輪式里程計(jì)的數(shù)據(jù)具有信號(hào)頻率高、短時(shí)精度高的特點(diǎn),但是如果不經(jīng)過(guò)校核,容易產(chǎn)生較大的累計(jì)誤差。
輪式里程計(jì)配合JY61六軸陀螺儀通過(guò)公式(6)可以得到機(jī)器人位姿信息,短時(shí)精度高,但是長(zhǎng)時(shí)會(huì)有較大的累計(jì)誤差,數(shù)據(jù)可信度降低。UWB可以得到一個(gè)沒(méi)有累計(jì)誤差的坐標(biāo)信息,視距情況下可維持10 cm左右的定位精度,但在非視距環(huán)境下會(huì)出現(xiàn)數(shù)據(jù)失真導(dǎo)致數(shù)據(jù)可信度降低?;诶锍逃?jì)短時(shí)精度高和UWB長(zhǎng)時(shí)誤差的有界性,將2種信息進(jìn)行有機(jī)融合以達(dá)到長(zhǎng)時(shí)高精度定位的目的。首先用輪式里程計(jì)所得數(shù)據(jù)作為參考量,對(duì)UWB進(jìn)行狀態(tài)判斷,判斷UWB是否處于視距環(huán)境中;再使用卡爾曼濾波算法對(duì)里程計(jì)和UWB進(jìn)行不同信賴度下的融合;然后使用融合得到的數(shù)據(jù)對(duì)里程計(jì)數(shù)據(jù)進(jìn)行累計(jì)誤差的判斷與修正,從而降低里程計(jì)的累計(jì)誤差,提高系統(tǒng)定位精度。數(shù)據(jù)融合的計(jì)算流程如圖5所示。
圖5 數(shù)據(jù)融合的計(jì)算流程
先對(duì)UWB數(shù)據(jù)進(jìn)行一次一階低通濾波,以初步減小數(shù)據(jù)的抖動(dòng),然后分兩層對(duì)UWB數(shù)據(jù)狀態(tài)進(jìn)行判斷。
第一層先對(duì)UWB的單點(diǎn)數(shù)據(jù)進(jìn)行判斷,需要獲知UWB基站是否得到標(biāo)簽的距離信息,因?yàn)樵诙ㄎ恢锌赡艹霈F(xiàn)障礙物引起基站無(wú)法得到標(biāo)簽信息的情況,使三邊定位算法不能正常使用,導(dǎo)致UWB數(shù)據(jù)失效。通過(guò)第一層的判斷可以排除無(wú)法得到單點(diǎn)數(shù)據(jù)的情況,如能得到單點(diǎn)數(shù)據(jù)則進(jìn)入第二層判斷。
在第二層中對(duì)UWB數(shù)據(jù)做是否進(jìn)入非視距狀態(tài)與非視距程度的判斷。用當(dāng)前的UWB數(shù)據(jù)對(duì)里程計(jì)的數(shù)據(jù)求距離偏差,在里程計(jì)的數(shù)據(jù)上設(shè)定多層環(huán)形區(qū)域,通過(guò)判斷UWB所屬區(qū)域?qū)Ψ且暰喑潭冗M(jìn)行分類量化,為后續(xù)數(shù)據(jù)融合提供依據(jù)。將UWB所處狀態(tài)分為3類,則有式(7)所示分類量化結(jié)果。
(7)
式中:為第類視距狀態(tài),其中=(1,2,3);表示狀態(tài)閾值;為距離偏差值。表示UWB處于視距狀態(tài),因?yàn)槲闹袑?shí)驗(yàn)使用的UWB系統(tǒng)的定位精度為10 cm,所以取值可在15 cm左右,但是不能小于UWB本身的定位精度。表示UWB處于輕度非視距狀態(tài),這個(gè)非視距狀態(tài)的數(shù)據(jù)尚可以使用,具有一定的可信度,表示允許使用的非視距狀態(tài)誤差距離的最大值。表示UWB已經(jīng)處于嚴(yán)重的非視距狀態(tài),數(shù)據(jù)已經(jīng)完全不可信。經(jīng)過(guò)上述兩層判斷,實(shí)現(xiàn)了以里程計(jì)為參考量的UWB非視距程度的分類量化。
在上述工作基礎(chǔ)上,將對(duì)里程計(jì)數(shù)據(jù)和分類量化后的UWB數(shù)據(jù)進(jìn)行數(shù)據(jù)融合。
卡爾曼濾波的數(shù)學(xué)模型包括預(yù)測(cè)模型和觀測(cè)模型2個(gè)部分,離散化的形式如式(8):
(8)
(9)
(10)
(11)
(12)
(13)
在上述卡爾曼濾波算法公式中已省略對(duì)單位矩陣的書(shū)寫(xiě)。
用式(7)分類量化的結(jié)果設(shè)置參數(shù),表示UWB數(shù)據(jù)處于視距狀態(tài),則和可以設(shè)置為相同的值,即設(shè)里程計(jì)和UWB值具有相同的可信度,這對(duì)參數(shù)使用、表示。表示UWB處于輕度非視距狀態(tài),則通過(guò)增大、減少來(lái)提高系統(tǒng)對(duì)里程計(jì)的信賴度,這對(duì)參數(shù)用、來(lái)表示。表示UWB處于嚴(yán)重非視距狀態(tài),UWB數(shù)據(jù)失去作用,此時(shí)系統(tǒng)以里程計(jì)數(shù)據(jù)為準(zhǔn),設(shè)置為單位矩陣,設(shè)置為零矩陣,此時(shí)=、=。綜上所述,將UWB的視距狀態(tài)與和的參數(shù)取值歸納為式(14)。
(14)
在UWB處于不同狀態(tài)下使用式(14)改變預(yù)測(cè)值噪聲的協(xié)方差和觀測(cè)值噪聲的協(xié)方差,從而改變?nèi)诤舷到y(tǒng)對(duì)觀測(cè)和預(yù)測(cè)的信賴程度,進(jìn)而達(dá)到自適應(yīng)的效果,提高了系統(tǒng)的穩(wěn)定性。
使用融合后的數(shù)據(jù)來(lái)判斷里程計(jì)的累計(jì)誤差。以得到的融合后的數(shù)據(jù)為圓心設(shè)定一個(gè)圓形許用區(qū)域,通過(guò)判斷里程計(jì)數(shù)據(jù)所在區(qū)域?qū)锍逃?jì)累計(jì)誤差進(jìn)行判斷。如果里程計(jì)的數(shù)據(jù)連續(xù)多次超出圓形許用區(qū)域,則表明里程計(jì)已經(jīng)產(chǎn)生了較大的累計(jì)誤差。設(shè)定的圓形許用區(qū)域半徑越大則對(duì)里程計(jì)的累計(jì)誤差的容忍度越大,圓形許用區(qū)域半徑越小則對(duì)里程計(jì)的修正會(huì)越靈敏。但實(shí)驗(yàn)使用的UWB的定位精度為10 cm,視距情況下的數(shù)據(jù)融合對(duì)里程計(jì)和UWB信賴度相同,融合后的數(shù)據(jù)在兩者之間,所以圓形許用區(qū)域的最小半徑不能小于5 cm。
(Corrected)=(orignal)+
(15)
再用修正的里程計(jì)的數(shù)據(jù)參與數(shù)據(jù)融合,從而提高系統(tǒng)長(zhǎng)時(shí)定位的精度。
仿真實(shí)驗(yàn)使用開(kāi)源機(jī)器人仿真軟件Webots,仿真機(jī)器人由全向運(yùn)動(dòng)底盤(pán)、輪式里程計(jì)和UWB定位系統(tǒng)組成。其中全向運(yùn)動(dòng)底盤(pán)和輪式里程計(jì)通過(guò)在Webots中直接建模生成,UWB定位系統(tǒng)使用軟件中的GPS傳感器通過(guò)設(shè)定傳感器的噪聲來(lái)模擬產(chǎn)生不同狀態(tài)的UWB信號(hào)。仿真機(jī)器人模擬完成如圖6所示的蛇形運(yùn)動(dòng),先向軸正方向運(yùn)動(dòng)3 m,然后向軸正方向運(yùn)動(dòng)1 m,向軸負(fù)向運(yùn)動(dòng)3 m,軸正向運(yùn)動(dòng)1 m,如此反復(fù),得到的數(shù)據(jù)如圖6—圖7所示。
圖6 理想軌跡與UWB數(shù)據(jù)形成的軌跡 圖7 理想軌跡與里程計(jì)數(shù)據(jù)形成的軌跡
圖6為UWB數(shù)據(jù)的軌跡圖,可以看出:機(jī)器人從(0,2)到(2,2)的運(yùn)動(dòng)期間設(shè)定UWB為非視距狀態(tài),具體表現(xiàn)為UWB數(shù)據(jù)抖動(dòng)幅度變大。圖7為里程計(jì)數(shù)據(jù)的軌跡,里程計(jì)在運(yùn)行過(guò)程中產(chǎn)生了累計(jì)誤差,隨著時(shí)間的增加誤差也在增加。
圖8為最終融合得到的數(shù)據(jù),系統(tǒng)成功判斷出UWB的狀態(tài),并進(jìn)行了不同信賴度的融合,得到穩(wěn)定的數(shù)據(jù)。并且在(3.057,2.625)m點(diǎn)處融合后的數(shù)據(jù)判斷出里程計(jì)的累計(jì)誤差,并對(duì)里程計(jì)數(shù)據(jù)進(jìn)行了修正。然后使用修正過(guò)的里程計(jì)數(shù)據(jù)繼續(xù)與UWB進(jìn)行融合,減小了融合后數(shù)據(jù)的累計(jì)誤差,提高了系統(tǒng)長(zhǎng)時(shí)間運(yùn)行的穩(wěn)定性。
圖8 理想軌跡與最終融合后數(shù)據(jù)的軌跡
圖9為每個(gè)UWB原始數(shù)據(jù)與理想位置坐標(biāo)的誤差和每個(gè)融合后數(shù)據(jù)與理想位置坐標(biāo)的誤差對(duì)比,可以看出:融合后的數(shù)據(jù)相對(duì)于UWB原始數(shù)據(jù)的抖動(dòng)和誤差都明顯減小,而且在非視距情況融合后的數(shù)據(jù)依然有很好的穩(wěn)定性和準(zhǔn)確性。融合后的數(shù)據(jù)誤差會(huì)隨著時(shí)間的增加逐漸變大,這是由于里程計(jì)的累計(jì)誤差逐漸增大所致,但當(dāng)里程計(jì)的累計(jì)誤差達(dá)到一定程度時(shí),又會(huì)通過(guò)融合后的數(shù)據(jù)對(duì)里程計(jì)數(shù)據(jù)進(jìn)行修正,從而將里程計(jì)數(shù)據(jù)的累計(jì)誤差控制在一定范圍之內(nèi)。UWB原始數(shù)據(jù)的平均誤差為0.101 5 m,融合后的平均誤差為0.048 3 m,融合后的數(shù)據(jù)誤差減少了52.41%,達(dá)到了良好的預(yù)期效果。
圖9 UWB原始數(shù)據(jù)的誤差和融合后數(shù)據(jù)的誤差對(duì)比
為進(jìn)一步驗(yàn)證文中方法在單片機(jī)系統(tǒng)中的應(yīng)用效果,擬開(kāi)展實(shí)物機(jī)器人定位實(shí)驗(yàn)研究。實(shí)驗(yàn)采用基于Decawave公司的DW1000芯片的UWB產(chǎn)品,該產(chǎn)品定位范圍為50 m,組成的系統(tǒng)定位誤差為10 cm左右。采用的輪式里程計(jì)為自制的正交從動(dòng)式輪式里程計(jì)如圖3所示,平均運(yùn)行10 m會(huì)產(chǎn)生5 cm的累計(jì)誤差。JY61六軸陀螺儀采用TDK公司的MPU6050姿態(tài)傳感器,通過(guò)JY61的處理器讀取MPU6050的測(cè)量數(shù)據(jù)然后通過(guò)串口輸出,靜態(tài)測(cè)量精度穩(wěn)定在0.05°,動(dòng)態(tài)精度為0.1°。實(shí)驗(yàn)場(chǎng)景為9 m×21 m的實(shí)驗(yàn)室,由3個(gè)UWB基站組成一個(gè)等腰三角形,所有基站在距地面高為2.40 m的同一平面上,UWB標(biāo)簽置于機(jī)器人頂端。機(jī)器人實(shí)物系統(tǒng)如圖10所示。
圖10 實(shí)驗(yàn)機(jī)器人的圖片
實(shí)驗(yàn)分為5組,每組做5次實(shí)驗(yàn),每組實(shí)驗(yàn)分別為單獨(dú)使用輪式里程計(jì)、在視距情況下單獨(dú)使用UWB、在視距情況下進(jìn)行文中方法的融合、在存在8 m非視距情況下單獨(dú)使用UWB、在存在8 m非視距情況下進(jìn)行文中方法的融合,以所得實(shí)驗(yàn)數(shù)據(jù)分別作為定位坐標(biāo)進(jìn)行運(yùn)動(dòng)控制。利用UWB標(biāo)簽加蓋來(lái)產(chǎn)生非視距情況。
理想實(shí)驗(yàn)軌跡與仿真時(shí)的相同,一次實(shí)驗(yàn)共運(yùn)行20 m,平均運(yùn)行時(shí)間為59.82 s,其中2~10 m區(qū)間段為產(chǎn)生非視距的區(qū)域。機(jī)器人以stm32f103單片機(jī)為控制器,進(jìn)行數(shù)據(jù)處理和機(jī)器人控制。機(jī)器人每10 ms進(jìn)行一次閉環(huán)糾偏,每次的糾偏距離是機(jī)器人當(dāng)前坐標(biāo)與終點(diǎn)和起點(diǎn)直線的垂直距離,即當(dāng)前機(jī)器人偏離目標(biāo)位置的距離。機(jī)器人將用這個(gè)偏離的距離作為輸入量,使用PID算法對(duì)機(jī)器人進(jìn)行軌跡糾偏,以保證機(jī)器人回到目標(biāo)路徑上??偧m偏距離可以反映機(jī)器人一段時(shí)間內(nèi)運(yùn)動(dòng)的平穩(wěn)性,總糾偏距離越小表明機(jī)器人運(yùn)動(dòng)越平穩(wěn),糾偏距離為零表示機(jī)器人保持在理想路徑之上。將所得實(shí)驗(yàn)數(shù)據(jù)列于表1。
表1 得到的5組數(shù)據(jù)
由表1可以看出:單獨(dú)使用里程計(jì)的數(shù)據(jù)運(yùn)動(dòng)20 m平均產(chǎn)生了100 mm的終點(diǎn)偏差,這個(gè)誤差是長(zhǎng)距離運(yùn)動(dòng)導(dǎo)致的累計(jì)誤差,且這個(gè)誤差會(huì)隨著運(yùn)動(dòng)距離的增加而增加,但里程計(jì)數(shù)據(jù)穩(wěn)定,總糾偏距離只有5.696 m,機(jī)器人運(yùn)行平穩(wěn)。
使用純UWB數(shù)據(jù)進(jìn)行定位的機(jī)器人運(yùn)動(dòng)過(guò)程中抖動(dòng)極大,尤其是在非視距狀態(tài)下UWB數(shù)據(jù)極不穩(wěn)定,終點(diǎn)偏差分別為112、123 mm,在視距狀態(tài)下運(yùn)動(dòng)20 m平均產(chǎn)生了152.844 m的總糾偏距離,在存在8 m的非視距狀態(tài)下運(yùn)動(dòng)更是平均產(chǎn)生了292.265 m的總糾偏距離,這表明單獨(dú)使用UWB數(shù)據(jù)不適合機(jī)器人定位。
使用文中算法在一直處于視距狀態(tài)下運(yùn)動(dòng)20 m終點(diǎn)偏差為37 mm,平均總糾偏距離為28.224 m,機(jī)器人運(yùn)行較平穩(wěn)。在存在8 m的非視距環(huán)境中運(yùn)動(dòng)20 m,終點(diǎn)偏差為51 mm,平均總糾偏距離為27.764 m,系統(tǒng)成功判斷出UWB是否處于非視距狀態(tài)并自動(dòng)進(jìn)行了不同信賴度的數(shù)據(jù)融合,大幅減小了UWB數(shù)據(jù)的抖動(dòng),并且將里程計(jì)數(shù)據(jù)的累計(jì)誤差控制在一定范圍內(nèi),得到的數(shù)據(jù)穩(wěn)定且精度較高。相比單獨(dú)使用UWB的數(shù)據(jù),在視距環(huán)境中穩(wěn)定性提高了81.5%,精度提高了67.1%;在存在8 m非視距的環(huán)境中穩(wěn)定性提高了90.5%,精度提高了59.2%。由此可見(jiàn),使用文中融合方法可以大幅提高定位系統(tǒng)的穩(wěn)定性和精度。
本文作者利用里程計(jì)的短時(shí)精度高和UWB的長(zhǎng)時(shí)誤差有界性,提出了一種基于卡爾曼濾波的數(shù)據(jù)融合方法,成功地將2種信息有機(jī)融合達(dá)到了長(zhǎng)時(shí)高精度定位的目的。通過(guò)軟件仿真和實(shí)驗(yàn)驗(yàn)證表明:在UWB一直處于視距環(huán)境中融合后的數(shù)據(jù)相比單獨(dú)使用UWB的數(shù)據(jù)穩(wěn)定性提高了81.5%,精度提高了67.1%;在存在8 m非視距環(huán)境中,相對(duì)于單獨(dú)使用UWB的數(shù)據(jù)穩(wěn)定性提高了90.5%,精度提高了59.2%。系統(tǒng)運(yùn)行穩(wěn)定,實(shí)現(xiàn)了長(zhǎng)時(shí)間連續(xù)可靠的定位,具有較大的工程應(yīng)用價(jià)值。