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

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

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

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

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

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

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

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

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

1.1 UWB 系統(tǒng)

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

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

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

圖1 UWB 測(cè)距原理圖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)系之間的相對(duì)角位置關(guān)系來(lái)表示，用歐拉角來(lái)對(duì)目標(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°。

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

上述矩陣可記為

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

1.3 單錨點(diǎn)的UWB 設(shè)計(jì)

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

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

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

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

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

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

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

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

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

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

2.1 位置信息的估計(jì)

本文需要構(gòu)建CTRV 模型來(lái)描述目標(biāo)的運(yùn)動(dòng)狀態(tài)。CTRV 模型是一種廣泛應(yīng)用于目標(biāo)定位領(lǐng)域的模型，可以描述目標(biāo)在三維空間中的位置、速度和加速度等狀態(tài)。在本系統(tǒng)中，以二維平面展開(kāi)描述，且其運(yùn)動(dòng)狀態(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)在時(shí)間t 時(shí)刻的狀態(tài)，包括位置、速度和加速度，由于加速目標(biāo)勻速運(yùn)動(dòng)所以加速度a 為0，θ¨角加速度為0，即狀態(tài)方程XK+1可表示為

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

2.2 誤差分析

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

UWB 模塊通過(guò)式（17），測(cè)量物體與錨點(diǎn)之間的距離d 為

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

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

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

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

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

3.1 實(shí)驗(yàn)設(shè)計(jì)

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

圖5 實(shí)驗(yàn)小車Fig.5 Experimental car

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

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

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

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

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

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

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

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

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

4 結(jié)語(yǔ)

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