基于UWB輔助的多無人機慣導定位誤差校正方法

馬振華，張鵬飛，何 印，劉磊磊，郭偉峰

(1.中北大學 航空宇航學院，太原 030051；2.中北大學 智能武器研究院，太原 030051)

0 引言

無人機是利用無線電遙控設備和自備的程序控制裝置操縱的不載人飛行器，一經研發出來就受到各領域的廣泛關注。在民用領域使用無人機可以節省大量人力的同時還可以執行相對危險復雜的任務，不受環境限制。在軍用領域使用無人機可以憑借其飛行高度低、體積小的特點從而難以被敵方雷達發現，讓我方掌握戰場的主動權，而且還可以減少我方士兵的傷亡。隨著近幾十年我國軍工和民用領域技術的迅速發展，許多學者在多智能體控制、信息融合和無線通信等技術領域開展了大量的研究，使得無人機技術在軍事和民用中的應用也日趨成熟[1]。與此同時，需要無人機執行任務的環境越來越復雜，如戰場、搜索救援、海面漏油清理等空間廣闊的任務環境，單一無人機由于覆蓋面積小、負重量少等缺點，不能滿足任務要求。受自然界生物集群的啟發，多無人機又稱為無人機集群出現在人們視野中。無人機集群在農業、軍事、搜索救援、漏油清理、無人地帶探索等方面應用廣泛[2-4]，多無人機聯合執行任務的方式受到極大重視。相較于單架無人機，多無人機可以發揮數量優勢，通過成員之間的協作，不僅可以擴大覆蓋面積，更因為信息共享可以發揮“1+1>2”的執行任務效率，大大減少執行任務的時間，同時還可以增加信息感知范圍，任務完成的可靠性得到提高[5-7]。

在多無人機編隊執行任務過程中，作為無人機的眼睛，導航技術貫穿任務始終，高精度的位置導航信息是可靠完美執行任務的關鍵所在，但是多個無人機都搭載高精度導航設備涉及到成本問題，而且在衛星導航拒止的環境下定位變得異常艱難。慣性導航系統(inertial navigation system，INS)是目前應用普遍，不可缺少的一種隱蔽的自主導航系統[8]，其無需任何外來信息，通過陀螺儀和加速度計測量飛行器自身角速度和加速度信息從而獲得位置、速度以及姿態，具有短時精度高、更新率高、自主性強等特點，是編隊飛行的各無人機成員的最佳導航方式。但是慣導最大的缺陷就是定位誤差隨時間而積累，需要利用其他導航定位信息不斷對其校正。針對無人機集群的協同化編隊飛行，若能利用無人機之間的相對距離信息來修正慣導誤差，不僅能節省安裝其他輔助導航系統的成本，而且定位精度高，魯棒性也更強[9-10]。

目前國內外學者一般采用GPS接收機、視覺傳感器等設備來校正慣導誤差。文獻[11]分析了捷聯慣導系統的主要誤差源對系統的影響及誤差特性，通過卡爾曼濾波將GPS與捷聯慣導系統組合，用GPS的導航信息對捷聯慣導系統的誤差進行了有效校正，這是目前常用的組合導航方法，但是在衛星導航拒止的環境下，衛星對無人機的定位信息丟失，慣導誤差會迅速發散。文獻[12]提出一種雙偽衛星輔助的慣導誤差校正新方法，在偽衛星數目只有兩顆時，該方法仍可利用偽衛星的偽距測量序列將慣導的位置誤差和接收機鐘差估計出來，但是在探測不到偽衛星信號時同樣面臨方法失效的問題。文獻[13]描述了一種基于視覺的相對導航和控制策略，使用視覺慣性系統檢測未知和非合作的空間物體，用視覺導航設備獲取載體間的相對視線矢量，校正相對慣導信息，但是視覺在室外易受環境變化和光線的影響，而且利用視覺往往會延時，不利于實時定位。文獻[14]提出了利用地標的圖像信息來校正慣導系統誤差，通過光電平臺攝取在飛行區域預設地理位置已知的地標圖像信息和距離信息，對比對應的模板圖像得出飛機位置信息，再結合慣導輸出的位置信息，進而校正慣導誤差。這種方法適用于已知區域，在未知的環境并不適用。文獻[15]利用機載數據鏈的測距信息結合慣導信息建立機間相對定位模型，一定程度上減緩了慣導定位誤差的發散速度，提高了定位精度。相較于其他傳感器，本文所用的超寬帶(ultra wide band，UWB)模塊具有數據傳輸速度快、多徑分辨能力和抗干擾性能強、易攜帶、測距準確且安全性高的特點[16-17]，在多無人機慣導定位誤差校正方面，測量的相對距離信息實時性和準確性都能得到保障，使得慣導誤差校正效果更好。

本文給出一種運用UWB測距信息輔助的慣導誤差校正方法，該方法突破以往的單一物體定位，利用UWB的測距信息實現多個無人機慣導定位誤差的校正，來滿足無人機編隊在衛星拒止環境下的定位需求。文中首先給出UWB測距原理和慣導相對距離算法模型，其次給出慣導誤差模型，并且設計出用UWB的測距信息校正慣導誤差的擴展卡爾曼濾波器，最后通過仿真驗證了方法的可行性。

1 無人機編隊相對距離算法

計算不同位置的無人機之間的相對距離時，一般以無人機的機體坐標系為基準坐標系，如圖1所示。無人機的機體坐標系是固定在無人機的遵循右手法則的直角坐標系，其原點位于無人機的質心，OY軸平行于機身軸線指向機頭方向，OX軸垂直于對稱面指向無人機右方，OZ軸在無人機對稱面內，垂直于XOY平面指向無人機上方，構成右手坐標系。

圖1 相對距離觀測原理圖

圖2 雙邊雙向測距原理圖

無人機的相對距離是指某一架無人機在另一架無人機機體坐標系下距離坐標系原點的之間的距離，簡單來說就是兩個機體坐標系原點的距離，只不過需要其中某一個機體坐標系作為參考基準。相對距離可以將多無人機編隊飛行時之間的距離變化直觀地表現出來，這也是為什么在機體坐標系下測距的原因。無論是利用慣導解算出的位置信息還是UWB測得的距離信息都需統一轉換到機體坐標系下進行相對距離的解算，這樣才便于二者信息的融合。

1.1 UWB無人機間測距原理

UWB是一種無線載波通信技術，它不采用正弦載波，而是利用納秒級的非正弦波窄脈沖傳輸數據，傳輸速率非常高。UWB測距原理是使用時間戳差值法計算電磁波飛行時間，從而計算距離。簡單來說，無人機搭載的UWB模塊主要是利用信號在兩個異步收發機之間的飛行時間來測量無人機之間的距離，這種簡單的單向雙邊測距如果兩個設備時鐘不同步會引入較大的測量誤差，所以本文采用雙邊雙向測距法[18-19]，原理圖如2所示。

圖中Tprop是消息傳播時間，Tround1和Tround2是從發出消息到接收到消息的時間間隔，Treply1和Treply2是從接受到消息到發出消息的延時時間，單邊雙向測距整個測距過程就是模塊A在TX時刻向模塊B發出消息，在經過Tprop時間后B模塊接收到消息，然后B模塊經過Treply的短暫延時后再向A發出消息，再經過Tprop的時間，A接收到消息。而雙邊雙向測距是在單向雙邊測距的基礎上再增加一次通訊，兩次通訊的時間可以相互彌補因為時鐘偏移而引入的誤差。從而提高測距精度。利用雙邊雙向測距法的信號傳播時間計算公式為：

(1)

計算出電磁波傳播在無人機之間的傳播時間Tprop后，已知電磁波通訊的速度為光速，則無人機1和2之間的機體坐標系下的距離計算公式為：

(2)

式中，c是光速。

1.2 慣導相對距離解算模型

慣性導航系統利用陀螺和加速度計分別測量運載體的角運動信息和線運動信息，機載計算機根據這些測量信息解算出無人機的航向、姿態、速度及位置。無人機主要搭載的是慣性導航系統中的捷聯式慣性導航系統，其原理如圖3所示。

圖3 捷聯慣導原理圖

圖4 擴展卡爾曼濾波計算流程圖

從圖3可以看出，在使用捷聯慣導時必須已知載體的初始速度、位置和姿態矩陣，結合捷聯慣導陀螺儀和加速度計測得的信息進行速度、位置和姿態矩陣的更新計算。在計算姿態矩陣時，將陀螺儀測得的角速度減去根據導航坐標系和位置速度計算得到的角速度，得到的差值用來更新姿態矩陣[8]。在解算出姿態矩陣后，就便于將加速度計測得的機體坐標系下的比例值轉換到導航坐標系中用于速度和位置的更新計算。此外，利用姿態矩陣和姿態角的關系，還可以計算得到姿態角。本節核心內容主要是給出捷聯慣導的位置詳細解算過程，從而建立基于慣導的無人機之間相對距離的解算模型。

設慣導的速度和位置的采樣更新周期T為：

T=tm-tm-1

(3)

式中，tm和tm-1分別代表tm時刻和tm-1時刻。

選取東北天坐標系為慣性導航坐標系n，根據慣導的比力方程可得無人機的速度方程為：

(4)

解算出速度后無人機的緯度L、經度λ、高度h的微分方程為：

(5)

(6)

(7)

(8)

則有：

(9)

將式(9)在一個周期T積分可得：

(10)

即tm時刻運載體的位置為：

(11)

(12)

(13)

2 基于UWB測距輔助的慣導誤差校正模型

UWB的測距速度快、精度非常高，能達到厘米級，利用UWB模塊測量的無人機之間的距離可看作是真實距離。無人機各自搭載的慣導設備測得的位置信息相減之后轉換到機體坐標系下同樣能夠獲取無人機之間的距離。使用擴展卡爾曼濾波將UWB測得的真實距離和慣導信息融合構建慣導誤差校正模型，使用真實距離來校正慣導誤差。構建慣導誤差模型之前需要分析慣導誤差，從而確定濾波器的參數。

2.1 慣導誤差模型

慣性導航的誤差隨著時間不斷積累，在速度、位置、姿態三方面都會產生誤差，不能長時間用于導航定位。為了建立準確的慣導誤差校正模型，分析慣導在速度、位置以及姿態的誤差影響源顯得尤為關鍵。本文通過建立捷聯慣導的誤差模型[20]，明確影響誤差的各種變量，為建立慣導誤差校正模型奠定基礎。

2.1.1 姿態誤差方程

慣導姿態誤差方程為：

(14)

2.1.2 速度誤差方程

根據式(4)可得慣導速度誤差方程為：

(15)

2.1.3 位置誤差方程

選取地理坐標系為慣性導航坐標系，對式(5)的位置微分方程求偏差，考慮到式子中的RM和RN在短時間內變化很小，看作常值，可得在經度、緯度和高度誤差方程：

(16)

式中，δL表示緯度誤差，δλ表示經度誤差，δh表示高度誤差，記慣導在慣性導航坐標系下的速度分量為Vn=[VEVNVU]T，速度誤差分量為δVn=[δVEδVNδVU]T。

2.2 慣導誤差校正模型

2.2.1 狀態方程

設共有k架無人機，以慣導誤差作為狀態量，則第i(0

(17)

(18)

wi(t)是零均值的白噪聲序列，Gi(t)為系統的誤差系數矩陣，Fi(t)為狀態矩陣：

(19)

(20)

2.2.2 量測方程

以UWB的測距信息作為觀測量，由于本文假定UWB在安裝過程中經過標定，已經和無人機機體坐標系重合，因此無人機i獲取的與無人機j的相對距離可表達為：

(21)

(22)

式中，Hi(t)是第i架無人機的量測矩陣，Vi是第i架無人機量測噪聲序列，這里看作是零均值的白噪聲序列。

2.2.3 擴展卡爾曼濾波模型

構建完狀態方程(17)和量測方程(22)后利用擴展卡爾曼濾波進行更新計算，其中將狀態方程和量測方程離散化得：

Xk=Φk，k-1Xk-1+Γk-1Wk-1

(23)

Zk=HKXk+Vk

(24)

進一步可得擴展卡爾曼濾波公式為：

(25)

(26)

(28)

(29)

(30)

式中，Φk，k-1為離散化的狀態轉移矩陣，R和Q為量測噪聲和系統噪聲的協方差矩陣，P為估計協方差矩陣。具體算法流程圖如4所示。

3 仿真分析

3.1 仿真初始條件

設定兩架無人機編隊飛行，分別為無人機1和2，都搭載相同的捷聯慣導系統、通信設備和UWB模塊，無人機2伴隨無人機1飛行，兩架無人機除了起始位置不同，飛行軌跡完全相同，通過UWB測量無人機間的相對距離來相互校正慣導定位誤差。仿真過程中考慮UWB測距噪聲為高斯噪聲。仿真參數如表1所示。

表1 仿真參數設置

3.2 仿真結果及分析

通過對兩架無人機編隊飛行只依賴慣導定位和搭載UWB輔助慣導定位的兩種不同情況進行仿真。無人機1和無人機2的100 s內純慣導定位和UWB輔助慣導定位速度誤差曲線走勢圖如圖5和圖6所示，并結合速度誤差仿真數據給出了校正前后的速度均方根誤差值如表2和表3所示。

表2 無人機1速度誤差校正前后對比

表3 無人機2速度誤差校正前后對比

圖5 無人機1速度誤差校正前后對比圖

圖6 無人機2速度誤差校正前后對比圖

從圖5可以看出在衛星拒止環境下，無人機1只依賴慣導定位時，初始速度誤差比較小，但隨著定位時間的增加，速度誤差也在不斷地增加，在100 s時東向和北向速度誤差達到0.5 m/s，在天向速度誤差雖然在波動，但是誤差同樣有所增加，總的來說速度誤差呈現發散的情況。而無人機1利用UWB測距輔助慣導定位時三個方向的速度誤差都圍繞0 m/s上下微小的波動，初始速度誤差相較于純慣導會大一些，這主要和濾波器參數設置有關，但這微小的誤差在可接受范圍之內，且隨著定位時間的增加誤差迅速收斂，而且結合表2可以直觀地看出利用UWB輔助校正慣導速度誤差的收斂情況與可行性。

從圖6可以看出在拒止環境下，無人機2只依賴慣導定位面臨著和無人機1同樣的問題，東向和北向速度誤差隨著定位時間的增加越來越大，天向速度誤差在0 s到70 s內先增加后減小，在70 s到100 s時速度誤差又在不斷增加，相較于UWB輔助慣導定位而言可認為其在發散。同時結合表三速度均方根誤差值可證明利用UWB的測距信息可以有效抑制無人機慣導速度誤差發散，提升定位中的速度計算精度。

無人機1和無人機2的100 s以內的純慣導定位和UWB輔助慣導定位的三個方向的位置誤差曲線走勢如圖7和圖8所示，并結合仿真數據給出校正前后的波動峰值如表4和表5所示。

表4 無人機1位置誤差校正前后波動峰值

表5 無人機2位置誤差校正前后波動峰值

圖7 無人機1位置誤差校正前后對比圖

圖8 無人機2位置誤差校正前后對比圖

從圖7可以看出無人機1只依賴慣導定位時在東向和北向初始10 s的誤差比較小，但是在10 s之后定位誤差開始發散，隨著定位時間的增加誤差也在不斷增大。天向定位誤差在15 s前比較小，但在15 s之后同樣開始發散，只依賴慣導無法勝任無人機1的導航定位任務。反觀UWB測距輔助慣導定位三個方向在100 s以內的定位誤差都在0 m上下微小波動，結合表4來看，只依賴慣導定位時，東向和北向的波動峰值都超過了25 m，在天向達到了0.21 m，UWB輔助慣導定位的各方向位置誤差則不超過1 m，校正結果顯著，慣導的定位誤差得到有效抑制。

從圖8可以看出無人機2只依賴慣導定位時的各方向定位誤差曲線走勢與無人機1基本一致，這是因為在仿真時設置的二者搭載相同的慣導系統且軌跡相同，同樣位置誤差隨著定位時間的增加在不斷增加。通過UWB測量的無人機間的相對距離同樣能夠校正無人機2的慣導定位誤差，誤差曲線走勢平穩，定位精度高。根據表5的峰值數據可以直觀的看到校正前的東向和北向的定位誤差在100 s的時間內超過了25 m，在天向波動峰值為0.21 m，校正后的各方向定位誤差不超過1 m。

上述內容對仿真結果中速度和位置誤差的校正前后曲線圖以及表中數據進行了詳細分析，可以說明本文利用UWB輔助慣導定位的方法對于校正慣導定位誤差、提升慣導定位精度的可行性與有效性。

本文利用擴展卡爾曼濾波算法將UWB測量的無人機之間的相對距離信息和慣導信息融合，進而校正慣導的速度和位置誤差。該方法主要存在以下幾點優勢：

1)UWB利用納秒級的非正弦波窄脈沖傳輸數據，具有多徑分辨能力強和抗干擾性能強的特點，采用雙邊雙向測距消除了設備時鐘不同步引入的測距誤差，進一步提升了測距精度，經過標定和無人機機體坐標系重合后可以精確測量無人機機體坐標系下的相對距離，保證了相對距離數據來源的可靠性。

2)UWB僅用來測距，不參與定位，只需考慮測距過程的白噪聲誤差，使得擴展卡爾曼濾波融合算法實現相對簡單，能夠快速收斂，提升了算法的運行速度，保證了定位的實時性。

4 結束語

本文以在衛星導航拒止的環境下，多個無人機只依賴慣導無法長時間的編隊飛行為背景，提出了一種利用UWB輔助的慣導定位誤差校正方法，該方法融合了UWB測距信息和慣導信息，可利用無人機間相對距離校正參與測距的無人機慣導的速度和位置誤差，抑制慣導誤差的發散，結合仿真結果驗證了方法的可行性與有效性，為多無人機編隊在衛星拒止環境下的定位提供技術參考。但文章所用的擴展卡爾曼濾波算法會引入線性化誤差，還可以針對這一點進行改進，進一步提升定位精度。