黃俊杰，李海濱，賈翠玲

(1.內(nèi)蒙古工業(yè)大學(xué)材料科學(xué)與工程學(xué)院，內(nèi)蒙古呼和浩特 010051；2.內(nèi)蒙古工業(yè)大學(xué)工程訓(xùn)練教學(xué)部，內(nèi)蒙古呼和浩特 010051)

0 前言

隨著物聯(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 傳感器原理及誤差產(chǎn)生原因

1.1 超寬帶定位

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ì)算精度。

1.2 輪式里程計(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ì)誤差。

2 基于卡爾曼濾波的數(shù)據(jù)融合定位系統(tǒng)

輪式里程計(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ì)算流程

2.1 判斷UWB狀態(tài)

先對(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ù)融合。

2.2 基于卡爾曼濾波算法的數(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)定性。

2.3 里程計(jì)累計(jì)誤差的判斷和修正

使用融合后的數(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í)定位的精度。

3 仿真與實(shí)驗(yàn)驗(yàn)證與分析

3.1 仿真實(shí)驗(yàn)

仿真實(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ì)比

3.2 實(shí)驗(yàn)驗(yàn)證與分析

為進(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)定性和精度。

4 結(jié)論

本文作者利用里程計(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à)值。