基于UWB和IMU的電力巡檢機器人定位方法

導(dǎo)讀：為了滿足日益增長的電力需求，我國建設(shè)了大量發(fā)電站和電能轉(zhuǎn)換設(shè)施。長時間的負載運行使電力設(shè)備運行的不穩(wěn)定性增加，一般需要通過人工巡檢的方式進行檢測和維護，但傳統(tǒng)的人工巡檢方法存在著較大的安全風(fēng)險，在精度方面滿足不了需求，維護效率也比較低。隨著人工智能技術(shù)的發(fā)展，智能巡檢機器人的出現(xiàn)給這一問題帶來希望，它克服了傳統(tǒng)人工巡檢方式存在的弊端，但目前的巡檢機器人在復(fù)雜電力環(huán)境中仍存在空間定位不準(zhǔn)確等問題。為此，本文提出了一種將超寬帶（UWB）和慣性傳感器（IMU）以無跡卡爾曼濾波（UKF）進行融合的定位方法，該方法克服了UWB定位方法在有障礙物的非視距情況下定位誤差大的問題，改進了復(fù)雜電力環(huán)境下智能巡檢機器人的定位精度。

1引言

電能是維持現(xiàn)代社會運轉(zhuǎn)的重要能源，而電力系統(tǒng)是生產(chǎn)和輸送電能的主要設(shè)備，需隨著電力系統(tǒng)規(guī)模的擴大持續(xù)進行日常維護。但人力巡檢電力設(shè)施安全性差、效率低、成本高，由此發(fā)展出了自動巡檢機器人，它可以分為移動式和固定式巡檢機器人。相較于固定式機器人，移動式的自動巡檢機器人的優(yōu)點是可以自主定位，靈活地規(guī)劃巡檢路線。本文研究對比了多種定位算法，創(chuàng)新性地提出了一種融合超寬帶（UltraWideBand，UWB）技術(shù)和慣性傳感器（IMU）數(shù)據(jù)的定位方法，并且通過殘差法對模型中的觀測值進行選擇，解決復(fù)雜電力環(huán)境中常見的非視距狀態(tài)（Non-Line-Of-SightState，NLOS，即固定裝置和移動設(shè)備之間有障礙物，與之對應(yīng)的是視距狀態(tài)Line-Of-SightState，LOS，即固定裝置和移動設(shè)備之間沒有障礙物）時引起的誤差，實現(xiàn)了更好的定位精度。本文研究只用了三個基站的UWB定位系統(tǒng)在長期處于NLOS狀態(tài)的移動機器人巡檢定位問題，創(chuàng)新性地使用了UWB定位系統(tǒng)與IMU方式融合，達到較為理想的定位效果。

2UWB定位方法理論與技術(shù)

2.1常用的機器定位方法當(dāng)今，越來越多智能化機器走進千家萬戶，機器在室內(nèi)的定位問題成為了一個重要研究課題，目前室內(nèi)定位技術(shù)主要有：藍牙定位技術(shù)、Wi-Fi定位技術(shù)、ZigBee技術(shù)、UWB超寬帶技術(shù)等。其中，UWB是應(yīng)用較為廣泛的一種。表1是多種定位方法精度的比較。UWB是指信號的相對帶寬大于20%或者信號的絕對帶寬大于500MHz的無線信號。相對于其他無線電波信號，UWB信號由于其特殊的信號類型及頻譜，在復(fù)雜室內(nèi)環(huán)境下不受多路效應(yīng)影響，并具有極佳的障礙物穿透能力，定位精度可以達到厘米級別。表1各種定位系統(tǒng)定位范圍定位精度對比表2.2UWB定位方法UWB定位是通過移動設(shè)備（MS）和固定設(shè)備（BS）及上位機位置坐標(biāo)解算平臺協(xié)作完成的，需要定位的目標(biāo)和固定基站通過UWB脈沖通信，也就是測距交互過程，之后固定基站將通信距離上傳到解算平臺，即可算出移動目標(biāo)在空間中的位置。目前有如下幾種UWB測量方法：基于接收信號強度測量法、基于信號到達角度測量法、基于信號到達時間差測量法、基于信號到達時間測量法、雙向飛行時間測量法。使用這些方法可以使UWB定位在LOS狀態(tài)時取得較高的精度，但是NLOS時障礙物的反射率、穿透率等都會導(dǎo)致發(fā)射的信號在強度、方向等特征上產(chǎn)生改變從而造成定位嚴重誤差，因此在復(fù)雜的室內(nèi)環(huán)境NLOS是影響UWB定位精度的重要因素。

3基于UWB和IMU的巡檢定位方法

雖然UWB在定位精度上具有較好的性能，但是當(dāng)UWB基站數(shù)較少，并且一個或幾個基站處在NLOS狀態(tài)下時，UWB定位方式會有很大的誤差。本文應(yīng)用多傳感器融合的方法，利用傳感器間的優(yōu)勢互補，采用UWB和IMU設(shè)備數(shù)據(jù)進行融合。在UWB的基站處于NLOS狀態(tài)時利用IMU設(shè)備數(shù)據(jù)與UWB設(shè)備數(shù)據(jù)結(jié)合濾波算法進行最終的位置估計，并采用無跡卡爾曼濾波（UnscentedKalmanFilter，UKF）方式，因為UKF比擴展卡爾曼濾波算法（ExtendedKalmanFilter，EKF）在非線性系統(tǒng)中有更好的性能。3.1UKF濾波當(dāng)使用UWB定位系統(tǒng)時，至少需要三個定位基站才可以進行定位，但是在復(fù)雜的環(huán)境中會遇到障礙物，此時處于LOS下的定位基站就小于三個，大大影響定位精度，解決辦法有兩種，第一種增加基站數(shù)量，但是這種方法不能從根本解決問題，依然可能遇到處在LOS狀態(tài)下的基站數(shù)小于三個的情況，而且大量鋪設(shè)基站會大大增加成本。第二種方法是加入目標(biāo)跟蹤算法得到NLOS下的測距值，本實驗選用了UKF，它是通過提前選擇與狀態(tài)變量有關(guān)的采樣點，對其進行加權(quán)來表示狀態(tài)向量的分布，然后通過非線性變換以及加權(quán)得到狀態(tài)向量的非線性變換。在非線性問題上UKF因其特殊的濾波效果，相比于EKF可以進行進一步的線性化，UKF的核心是利用UT（UnscentedTransform）變換來對均值和協(xié)方差進行處理，從而避免線性化帶來的誤差。3.2UWB與IMU融合方法3.2.1UWB和IMU融合定位系統(tǒng)模型在實際應(yīng)用中，基于UWB和IMU的數(shù)據(jù)進行融合，利用不同傳感器的定位優(yōu)勢互補實現(xiàn)比單獨UWB或者單獨IMU定位方式更好的定位效果。在融合過程中，各子系統(tǒng)在組合上分為松耦合、緊耦合和深耦合，不同在于測出的數(shù)據(jù)是否直接互相校正。本文采用深耦合的方式，它相較于松耦合性能更好，但是實現(xiàn)難度也比松耦合高。深耦合是指在傳感器觀測的數(shù)據(jù)上進行最優(yōu)估計，不同系統(tǒng)的傳感器可以相互輔助修正提高精度。UWB在LOS狀態(tài)下可以獲得厘米級的定位精度，但是在NLOS狀態(tài)時誤差很大，而IMU定位方式可以忽略NLOS的影響，缺點是隨著測量過程累計誤差不斷增加。為此，本文采用UWB和IMU數(shù)據(jù)融合的定位方式，在采集數(shù)據(jù)階段使用直接濾波的方式獲取傳感器的數(shù)據(jù)，通過緊耦合的方式利用不同性能和優(yōu)勢的傳感器獲得的數(shù)據(jù)互相校正。同時，提出UWB和IMU融合定位系統(tǒng)模型。先將IMU傳感器中加速度計獲得的數(shù)據(jù)XbYbZb作為系統(tǒng)的輸入（abx,k、aby,k、abz,k分別表示k時刻在XbYbZb坐標(biāo)系中x、y、z方向上的加速度），UWB測得距離Zk=[d1.k

d2.k

d3.k]T作為觀測向量（di,k表示k時刻第i個基站測得的距離），將目標(biāo)的位置和速度作為狀態(tài)向量XK=[Px,k

Py,k

Vx,k

Vy,k]T（Px,k、Py,k分別表示k時刻x、y方向上的位置坐標(biāo)，vx,k、vy,k分別表示k時刻x、y方向上的速度）建立系統(tǒng)模型如式（1）所示：其中，F(xiàn)：狀態(tài)轉(zhuǎn)移矩陣；G：噪聲驅(qū)動矩陣；B：輸入控制矩陣；H：系統(tǒng)觀測矩陣；表示系統(tǒng)過程噪聲矩陣（wpx,k和wpy,k表示k時刻x、y方向上的位置噪聲，和表示k時刻x、y方向上的速度噪聲）；表示系統(tǒng)觀測噪聲矩陣（ndi,k表示k時刻第i個基站測得的距離噪聲）。3.2.2基于UKF的UWB和IMU融合定位當(dāng)室內(nèi)環(huán)境復(fù)雜，對于大多數(shù)時刻處于NLOS的UWB定位誤差很大，針對這種情況，本文提出了一種改進UKF濾波算法用于UWB和IMU定位數(shù)據(jù)的融合。對于上一小節(jié)已建立的定位系統(tǒng)模型，系統(tǒng)的狀態(tài)方程如式（2）所示：其中，Px,k和Py,k：k時刻位置坐標(biāo)；vx,k和vy,k：k時刻在x、y方向速度分量；vx,k-1和vy,k-1：k時刻在x、y方向速度分量；ts：采樣間隔。[anx

any]T為[abx

aby]T經(jīng)過旋轉(zhuǎn)矩陣轉(zhuǎn)換得到的UWB定位坐標(biāo)系下的加速度（anx、any表示在XnYnZn坐標(biāo)系下x、y方向上的加速度，abx、aby表示在XbYbZb坐標(biāo)系下x、y方向上的加速度），坐標(biāo)變換示意圖如圖1所示。圖1坐標(biāo)轉(zhuǎn)換示意圖將方程組用矩陣表示如式（3）所示：其中Vk為系統(tǒng)過程噪聲，其對應(yīng)協(xié)方差為Qn，系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和控制輸入矩陣如式（4）所示：在所有基站的坐標(biāo)已知情況下，通過測距方程可以得到目標(biāo)的位置方程如式（5）所示：di為第i個固定基站距離移動基站的距離。計算得出系統(tǒng)觀測矩陣如式（6）所示：系統(tǒng)觀測矩陣如式（7）所示：由此可以通過UKF對上述模型進行融合。

4實驗驗證及結(jié)果分析

4.1實驗設(shè)備及環(huán)境當(dāng)電力環(huán)境下的遮擋物較多、UWB處在NLOS狀態(tài)時定位精度會受到很大影響，為了解決這個問題本文提出以UKF方式對UWB和IMU數(shù)據(jù)進行融合的巡檢機器人定位方法，改進了單獨UWB在NLOS狀態(tài)下精度不足的問題。為了驗證本文提出方法的精確性和有效性，設(shè)計了如下實驗：使用三個固定基站BS和一個裝在移動機器人（模擬電力巡檢機器人）上的移動基站MS作為實驗裝置，每個UWB基站都使用DW1000芯片；實驗在一個擺放有多個障礙物的、模擬復(fù)雜電力場境的室內(nèi)進行。4.2實驗結(jié)果本實驗?zāi)M空曠場地作為對照組，設(shè)置未擺放障礙物的空曠場景和擺放多個障礙物的復(fù)雜室內(nèi)場景進行算法的實驗。圖2是無障礙物遮擋的小車UWB定位軌跡圖。圖2無遮擋時UWB定位軌跡圖由圖2可知，無障礙物遮擋時單一UWB算法可以很好地跟蹤真實軌跡，定位精準(zhǔn)。當(dāng)有較少遮擋物存在時，對移動端采用不同算法進行定位軌跡對比。圖3是較少遮擋情況移動端采用UWB和UKF融合的UWB/IMU算法的軌跡圖。圖3有較少遮擋時殘差法融合誤差分布圖從圖3中可以看出在有遮擋環(huán)境下本文提出的UWB和IMU以UKF融合的方法，相比于單獨使用UWB算法軌跡的偏移更小。當(dāng)遮擋物較多時，需將UWB定位算法、UKF僅運用于UWB、UKF融合UWB/IMU的算法進行實驗分析，誤差分布如圖4所示。圖4有較多遮擋時殘差法融合誤差分布圖較多遮擋時采用不同的算法在x軸、y軸方向誤差的對比如圖5、圖6所示。圖5較多遮擋時不同算法x軸坐標(biāo)誤差對比圖圖6較多遮擋時不同算法y軸坐標(biāo)誤差對比圖從圖5、圖6可知，當(dāng)存在較多遮擋時，使用UKF融合UWB和IMU在NLOS環(huán)境下對定位誤差緩解效果要好于單獨UWB定位和僅將UKF應(yīng)用于UWB的算法，更好跟蹤真實的參考軌跡。因此，在移動端存在軌跡突變，如轉(zhuǎn)彎上的非線性嚴重的情形時，驗證了UKF融合的UWB和IMU的算法具有更好的誤差緩解性。再從累計誤差分布的角度看，在較少遮擋時使用本文提出的融合方法比單純的UWB方法的算法誤差小，如圖7所示。圖7有遮擋時累計誤差分布圖在較多遮擋情形時，通過UWB、UKF/UWB、UKF融合UWB/IMU的累積分布圖如圖8所示。圖8有較多遮擋時累計誤差分布圖從累積分布圖中可以看到使用UKF融合UWB和IMU的算法在誤差0.15~0.2m處就達到了0.9左右，可以明顯看出本文提出的算法在定位誤差緩解上明顯的優(yōu)越性。除了從誤差累計分布來評判系統(tǒng)性能外，本實驗還分別統(tǒng)計了各算法在定位精度上評價指標(biāo)，分別從最大定位誤差（MPE）、均方誤差（MSE）和均方根誤差（RMSE）的評價指標(biāo)上進行了比較分析，如表2、表3所示。表2有遮擋時各方法的評價指標(biāo)表3有較多遮擋時各方法的評價指標(biāo)綜上，使用本文提出的UWB和IMU的融合方法可以有效改進電力巡檢機