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

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

1引言

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

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

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

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

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

d2.k

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

Py,k

Vx,k

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

any]T為[abx

aby]T經(jīng)過(guò)旋轉(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)過(guò)程噪聲，其對(duì)應(yīng)協(xié)方差為Qn，系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣和控制輸入矩陣如式（4）所示：在所有基站的坐標(biāo)已知情況下，通過(guò)測(cè)距方程可以得到目標(biāo)的位置方程如式（5）所示：di為第i個(gè)固定基站距離移動(dòng)基站的距離。計(jì)算得出系統(tǒng)觀測(cè)矩陣如式（6）所示：系統(tǒng)觀測(cè)矩陣如式（7）所示：由此可以通過(guò)UKF對(duì)上述模型進(jìn)行融合。

4實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

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