石豪東，付 銘，夏雨嘯，劉銀華

（青島大學(xué) 自動化學(xué)院，青島 266071）

隨著經(jīng)濟(jì)發(fā)展不斷地提高，民生科技的需求增加，大量服務(wù)于人們生活的機(jī)器人隨之出現(xiàn)，例如掃地機(jī)器人、預(yù)警機(jī)器人等。

這些產(chǎn)品的出現(xiàn)意味著人們對定位技術(shù)精度和可靠性的要求不斷提高，各種新型定位技術(shù)和算法也應(yīng)運(yùn)而生。市場上的室內(nèi)定位技術(shù)層出不窮，WiFi、紅外定位、藍(lán)牙等技術(shù)已經(jīng)大量應(yīng)用于室內(nèi)定位，但對于復(fù)雜環(huán)境下尤其是非視距環(huán)境（none line of sight，NLOS）下，WiFi 和紅外定位技術(shù)會因信號的衰減或者是障礙物的遮擋而導(dǎo)致定位產(chǎn)生很大的誤差。在環(huán)境復(fù)雜的室內(nèi)場所GPS、北斗等衛(wèi)星定位技術(shù)由于衛(wèi)星信號較弱，無法穿透屋頂、隧道等障礙，這導(dǎo)致此類定位技術(shù)無法用于室內(nèi)環(huán)境[1]。而基于UWB 的定位跟蹤技術(shù)具有定位精度高、魯棒性好、適應(yīng)性強(qiáng)等優(yōu)勢，已經(jīng)成為當(dāng)前定位技術(shù)中的研究熱點。

UWB 定位系統(tǒng)是基于超寬帶通信的，超寬帶通信因為其高頻特性，能夠較容易地獲取時間信息，基于時間特征的定位技術(shù)能夠獲取到更高精度的定位結(jié)果[2]。在文獻(xiàn)[3]中描述了一個精度可以達(dá)到4 cm 的超寬帶定位系統(tǒng)與之比較的是GPS 定位系統(tǒng)，實驗結(jié)果表明超寬帶技術(shù)的效果非常優(yōu)越。UWB雖然可以實現(xiàn)高精度的定位，但是需要多錨點的距離信息共同計算目標(biāo)位置，如果減少UWB 的錨點數(shù)量，就會導(dǎo)致位置信息的丟失，無法準(zhǔn)確定位。

慣性傳感單元（inertial measurement units，IMU）集成了加速度計、角速度計和磁力計3 種傳感器，得益于其成本較低、體積較小以及精度較高，其在姿態(tài)識別、導(dǎo)航定位等研究領(lǐng)域中被廣泛使用。但由于其具有零偏以及隨意游走的誤差特性，所以單獨(dú)使用IMU 時誤差較大。

UWB 無線信號雖然可以提供高精度的距離測量數(shù)據(jù)，但由于受到障礙物和信號衰減的影響，它在室內(nèi)或者復(fù)雜環(huán)境下的定位精度受到限制。而IMU 慣性傳感器可以提供相對運(yùn)動狀態(tài)的信息[4]，但隨著時間的推移，它的誤差會不斷累積，從而導(dǎo)致定位精度下降。為了克服UWB 和IMU 各自的限制，研究者們提出了許多融合算法和濾波算法[5]，卡爾曼濾波算法是一種常用的濾波算法，它可以通過處理UWB 和IMU 數(shù)據(jù)，得到更加準(zhǔn)確和可靠的目標(biāo)定位跟蹤結(jié)果，但在非線性系統(tǒng)中卡爾曼濾波技術(shù)的穩(wěn)定性和魯棒性有待提高，考慮通過擴(kuò)展卡爾曼濾波（EKF）將UWB 和IMU 所提供的位置信息進(jìn)行融合，通過融合這兩種數(shù)據(jù)來提高定位精度和魯棒性。

因此，本文旨在探討基于UWB 和IMU 的定位技術(shù)，并利用EKF 算法進(jìn)行數(shù)據(jù)處理，以提高定位精度和魯棒性。在系統(tǒng)設(shè)計和實驗過程中，將對UWB和IMU 數(shù)據(jù)進(jìn)行預(yù)處理，構(gòu)建數(shù)學(xué)模型來描述目標(biāo)運(yùn)動狀態(tài)，利用EKF 算法進(jìn)行數(shù)據(jù)融合和濾波處理，以實現(xiàn)高精度和魯棒性的目標(biāo)定位跟蹤。

1 定位系統(tǒng)架構(gòu)

1.1 UWB 系統(tǒng)

UWB 通過窄脈沖實現(xiàn)高帶寬信號傳輸，傳輸速率高，不容易被其他信號影響，從而能實現(xiàn)高精度的目標(biāo)定位。其測距原理為檢測多個錨點之間的信號傳輸時間（TOF）和信號到達(dá)時間（TOA）計算兩個節(jié)點之間的信號的接收與發(fā)送的時間，記錄響應(yīng)時間差通過式（1）即可獲得信號的平均飛行時間ttof：

根據(jù)式（2），即可完成測距，其中c 為光速：

如圖1 所示，計算出2 個節(jié)點之間的距離后，根據(jù)文獻(xiàn)[6]中的描述，超寬帶定位系統(tǒng)在預(yù)設(shè)位置上部署固定節(jié)點的情況下，可以通過最小二乘法的三邊定位法，如圖2 所示，計算錨點與固定節(jié)點的距離從而獲得目標(biāo)的位置信息。

圖1 UWB 測距原理圖Fig.1 UWB ranging schematic diagram

圖2 三邊定位原理圖Fig.2 Three-sided positioning schematic diagram

1.2 慣性導(dǎo)航系統(tǒng)

IMU 定位系統(tǒng)需要設(shè)置載體坐標(biāo)系與導(dǎo)航坐標(biāo)系，所述的載體坐標(biāo)系（b 系）Y 軸指向前進(jìn)方向，X 軸水平向右，Z 軸垂直向下；導(dǎo)航坐標(biāo)系（n 系）采用北東地坐標(biāo)系；姿態(tài)角由載體坐標(biāo)系到導(dǎo)航坐標(biāo)系之間的相對角位置關(guān)系來表示，用歐拉角來對目標(biāo)姿態(tài)進(jìn)行描述，歐拉角包括俯仰角、橫滾角和偏航角，俯仰角是坐標(biāo)系沿Y 軸旋轉(zhuǎn)的角度，旋轉(zhuǎn)范圍為-90°～+90°；橫滾角是坐標(biāo)系繞X 軸旋轉(zhuǎn)的角度，旋轉(zhuǎn)范圍為-180°～+180°，偏航角是坐標(biāo)系繞Z軸旋轉(zhuǎn)的角度，旋轉(zhuǎn)范圍為0°～360°。

兩個坐標(biāo)系之間的相互轉(zhuǎn)換可以用姿態(tài)矩陣來表示：

上述矩陣可記為

則通過上述2 個矩陣的關(guān)系可以獲得3 個姿態(tài)角（橫滾角、俯仰角、偏航角）的計算公式分別為

1.3 單錨點的UWB 設(shè)計

配置多個錨點的UWB 定位技術(shù)難點在于定位目標(biāo)時，目標(biāo)機(jī)器人為節(jié)點，所構(gòu)架的多個錨點如果與節(jié)點時鐘不同步就無法使用上述的三邊定位法計算目標(biāo)位置。而且需要在預(yù)設(shè)的場地提前安裝校準(zhǔn)錨點，需要大量的前期人工工作。如圖3 所示，現(xiàn)實生活中多是隨機(jī)化的場景，所以提前構(gòu)建場地安裝錨點是很難實現(xiàn)的。

圖3 UWB 錨點設(shè)置Fig.3 UWB anchor setting diagram

單錨點技術(shù)實現(xiàn)目標(biāo)定位，不再需要繁雜的設(shè)備安裝，而且有效地降低了技術(shù)成本。本方案雖然簡化了系統(tǒng)構(gòu)建的復(fù)雜度但還存在一個問題，如果只保留一個UWB 錨點，即只通過單一信號源獲得的距離信息完成精確的目標(biāo)定位是遠(yuǎn)遠(yuǎn)不夠的。所以在只有一個UWB 錨點的情況下，需要加入其他傳感器來獲得目標(biāo)的位置信息，采用從低成本的IMU傳感器中觀測目標(biāo)在3 個軸向上的加速度和角速度，從而獲得目標(biāo)的姿態(tài)信息，結(jié)合UWB 提供的位置信息經(jīng)過擴(kuò)展卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合從而獲得精確的位置估計。

2 系統(tǒng)數(shù)學(xué)模型

在本節(jié)中闡述了如何實現(xiàn)兩種傳感器數(shù)據(jù)的信息融合，以及模型的構(gòu)建?；跀?shù)學(xué)運(yùn)動模型進(jìn)行多傳感器融合，根據(jù)現(xiàn)實生活中機(jī)器人的運(yùn)動狀態(tài)通常是在勻速狀態(tài)下進(jìn)行的，所以采用二階的CTRV模型作為系統(tǒng)的狀態(tài)模型。該模型的運(yùn)動狀態(tài)為位置、速度、加速度和方向。CTRV 模型假設(shè)對象以恒定的角速度進(jìn)行勻速運(yùn)動，加速度為系統(tǒng)的過程噪聲，通過加速度噪聲對系統(tǒng)進(jìn)行誤差補(bǔ)償，減少震蕩，提高系統(tǒng)的精度與穩(wěn)定性，增強(qiáng)系統(tǒng)的魯棒性，其中估計器一般使用卡爾曼估計器計算出目標(biāo)位置，如式（8）和式（9）所示：

式中：xk為狀態(tài)向量，描述了機(jī)器人在坐標(biāo)系中的位置和速度信息；uk為輸入向量機(jī)器人的加速度和角速度信息；zk為觀測向量，包含了機(jī)器人的位置和方向信息；fk為狀態(tài)轉(zhuǎn)移矩陣；Bk為控制輸入矩陣；hx為觀察矩陣；其中wk（0～Q）和vk分別表示過程噪聲和傳感器噪聲描述了狀態(tài)轉(zhuǎn)移過程中的不確定性，它們都是符合高斯分布的白噪聲。

由于機(jī)器人的運(yùn)動過程并不是一個連續(xù)的線性的過程，無法用傳統(tǒng)的卡爾曼濾波器進(jìn)行位置信息估計。

對于非線性系統(tǒng)可以將機(jī)器人的運(yùn)動和對其測量過程作為一個擴(kuò)展卡爾曼濾波問題來進(jìn)行建模，對系統(tǒng)函數(shù)進(jìn)行泰勒級數(shù)展開從而把系統(tǒng)線性化，然后再使用卡爾曼估計器獲取系統(tǒng)位置信息。基于傳統(tǒng)的卡爾曼濾波系統(tǒng)線性化獲得的擴(kuò)展卡爾曼濾波，如式（10）和式（11）所示：

擴(kuò)展卡爾曼濾波分為預(yù)測和更新2 個階段，如圖4 所示。預(yù)測階段的控制輸入由IMU 中的陀螺儀、加速度計和磁力計提供，在更新階段使用的觀測信息是UWB 模塊測量的距離信息和IMU 測量的偏航角。

圖4 系統(tǒng)估計框架Fig.4 System estimation framework

2.1 位置信息的估計

本文需要構(gòu)建CTRV 模型來描述目標(biāo)的運(yùn)動狀態(tài)。CTRV 模型是一種廣泛應(yīng)用于目標(biāo)定位領(lǐng)域的模型，可以描述目標(biāo)在三維空間中的位置、速度和加速度等狀態(tài)。在本系統(tǒng)中，以二維平面展開描述，且其運(yùn)動狀態(tài)服從恒定加速度模型。

機(jī)器人的狀態(tài)向量X=[PxPyvθ]T，其中x 和y 分別表示機(jī)器人的橫縱坐標(biāo)；v 為線速度；θ 為偏航角為角速度。

式中：Px，Py分別為機(jī)器人在x 和y 方向的位置信息，系統(tǒng)狀態(tài)方程描述了目標(biāo)在時間t 時刻的狀態(tài)，包括位置、速度和加速度，由于加速目標(biāo)勻速運(yùn)動所以加速度a 為0，θ¨角加速度為0，即狀態(tài)方程XK+1可表示為

式中：Δt 為采樣時間；θt為t 時刻的角度。在本系統(tǒng)中，將UWB 和IMU 數(shù)據(jù)進(jìn)行融合，以提高定位精度和魯棒性。因此，系統(tǒng)的狀態(tài)方程是由IMU 傳感器數(shù)據(jù)和系統(tǒng)狀態(tài)向量組成的，即在EKF 的預(yù)測階段通過狀態(tài)轉(zhuǎn)移矩陣fx來描述目標(biāo)的運(yùn)動狀態(tài)演化過程。使用IMU 模塊通過陀螺儀、加速度計、磁力計計算物體的加速度和角速度，然后將其輸入到預(yù)測模型。由于狀態(tài)轉(zhuǎn)移函數(shù)是非線性的，因此需要對其進(jìn)行線性化。具體而言，在每個采樣時間將狀態(tài)向量作為一個局部線性近似點，并使用其一階導(dǎo)數(shù)構(gòu)建一個線性模型，這個模型通常被稱為擴(kuò)展卡爾曼濾波中的雅可比矩陣。狀態(tài)轉(zhuǎn)移函數(shù)的雅可比矩陣式可以通過求解矩陣的偏導(dǎo)數(shù)計算得到：

2.2 誤差分析

本文把UWB 測量的距離和IMU 測量的角度θ作為觀測向量Hx，系統(tǒng)的觀測方程為hx，即：

UWB 模塊通過式（17），測量物體與錨點之間的距離d 為

在EKF 的更新階段，通過計算卡爾曼增益來融合IMU 和UWB 的觀測值，得到一個最終的估計值?？柭鲆媸且粋€重要的參數(shù)，用于確定如何將當(dāng)前的觀測信息與先前的估計值進(jìn)行融合。所以卡爾曼增益的計算為系統(tǒng)估計的關(guān)鍵。

式中：Kk為系統(tǒng)的卡爾曼增益；Jh為觀測矩陣的雅可比矩陣；Pk為更新誤差的協(xié)方差矩陣；Pkˉ為先驗誤差的協(xié)方差矩陣；Q 為誤差的協(xié)方差矩陣，可由式（21）計算獲得：

將獲得的測量值作為觀測向量輸入到擴(kuò)展卡爾曼濾波器中，以獲得更加準(zhǔn)確的位置和速度估計值。

本系統(tǒng)的誤差主要來自于加速度，雖然假設(shè)系統(tǒng)加速度a 和角加速度θ¨為常數(shù)，但在實際的運(yùn)動中，速度的改變是無法忽略的，所以過程噪聲會影響系統(tǒng)的準(zhǔn)確性，需要在更新階段對系統(tǒng)進(jìn)行誤差補(bǔ)償可以更加符合真實軌跡，wk是一個零均值的高斯白噪聲向量，即：

3 實驗與分析

3.1 實驗設(shè)計

為了驗證該算法的可行性與準(zhǔn)確性，在自主研發(fā)的室內(nèi)機(jī)器人，如圖5 所示，搭載了Decawave 公司生產(chǎn)的DWM1000 芯片和自主研發(fā)的9 軸IMU，傳輸頻率為50 Hz/s。DWM1000 芯片負(fù)責(zé)接收機(jī)器人與錨點間的距離信息；9 軸IMU 負(fù)責(zé)提供加速度信息用于系統(tǒng)預(yù)測與更新，通過WiFi 將兩個傳感器測得的位置信息傳輸?shù)絇C 端。給機(jī)器人設(shè)置一段固定路徑，其原點為UWB 錨點所在位置，獲取位置信息驗證跟蹤路徑的準(zhǔn)確性。

圖5 實驗小車Fig.5 Experimental car

3.2 實驗分析

分別設(shè)置2 種軌跡，分析該算法的可行性，記錄實驗數(shù)據(jù)如表1 所示。

表1 跟蹤誤差數(shù)據(jù)分析Tab.1 Tracking error data analysis

首先對直線運(yùn)動進(jìn)行誤差分析，如圖6 和圖7所示，在卡爾曼濾波的情況下，系統(tǒng)的平均誤差大約在0.21 m，其中波動最大處超過了0.30 m，而經(jīng)過擴(kuò)展卡爾曼濾波處理下的跟蹤軌跡的平均誤差只有0.13 m，而且沒有大幅度的波動。

圖6 卡爾曼濾波估計直線路徑Fig.6 KF estimates linear paths

圖7 擴(kuò)展卡爾曼濾波估計直線路徑Fig.7 EKF estimates linear paths

如圖8 和圖9 所示，在曲線軌跡下運(yùn)動的小車明顯出現(xiàn)了更大的誤差，尤其是通過卡爾曼濾波器估計出的位置有很大的偏差，平均誤差甚至超過了0.5 m，而通過擴(kuò)展卡爾曼濾波器所估計出的位置平均誤差只有0.18 m。雖然在速度有大幅度波動時系統(tǒng)產(chǎn)生了較大的誤差，但是系統(tǒng)可以快速的恢復(fù)穩(wěn)定，具有良好的魯棒性。

圖8 卡爾曼濾波估計曲線路徑Fig.8 KF estimates curve path

圖9 擴(kuò)展卡爾曼濾波估計曲線路徑Fig.9 EKF estimates curve path

4 結(jié)語

本文設(shè)計了一種單錨點的UWB 目標(biāo)定位算法，通過擴(kuò)展卡爾曼濾波實現(xiàn)UWB 和IMU 的數(shù)據(jù)融合，提高定位系統(tǒng)的精度。經(jīng)過實驗，單錨點的UWB 在IMU 的配合下可以大幅提高定位的精確性，驗證了本方法的可行性。實驗也表明了在速度發(fā)生突變時，系統(tǒng)誤差增大，跟蹤軌跡發(fā)生了漂移，影響了系統(tǒng)的準(zhǔn)確性，這也是此方法存在的不足，后續(xù)的研究重點考慮解決速度發(fā)生突變時系統(tǒng)產(chǎn)生的漂移所帶來的誤差影響，提高系統(tǒng)的魯棒性。