無人飛行器集群僅測距初始相對位姿確定方法研究

丁 鑫，龔柏春，管敘軍，郝明瑞

(1. 南京航空航天大學航天學院，南京 210016；2.復雜系統控制與智能協同技術重點實驗室，北京 100074)

0 引言

當前，無人飛行器通過集群協同的方式執行任務的應用越來越廣泛，例如協同偵察、森林火災探測及危險環境中執行搜救等。相較于單個高價值、多載荷的無人飛行器，集群無人飛行器具有雷達截面小、成本低、數量多、容錯率高等特點，能夠通過群體協同效應實現單個大型飛行器難以完成或者代價太大的任務。

實現集群成員間的高精度相對定位是保障集群任務的關鍵前提。傳統的集群飛行器常使用獨立于集群外的、提供公共參考坐標的外部定位系統實現成員間相對定位，如運動捕捉系統(Motion Capture System，MCS)和全球導航衛星系統(Global Navigation Sa-tellite System，GNSS)。但MCS需要預先布置于環境中，無法應用于未知環境，GNSS導航信號在復雜環境中存在衰減、多路徑效應、無信號甚至信號欺騙等問題。因此，GNSS拒止環境下無人飛行器集群的自主相對導航已經成為行業內的熱點問題。

M.Kriegleder等研究了基于無人飛行器的僅測角機載紅外傳感系統，由于紅外傳感器視野范圍有限，需要在不同方向布置機載傳感器陣列，以實現目標飛行器始終處于測量視野范圍內。也有學者研究了基于視覺相機的相對定位系統，但基于視覺的定位系統需要測量飛行器對目標飛行器進行追蹤機動，以保證目標處于觀測視野內，且當目標飛行器距離較遠時，視覺相機存在特征識別點模糊等問題，一定程度上限制了無人飛行器間的相對飛行距離。

機載無線電設備能夠實現全方位通信測距，且相鄰設備間的通信幾乎不受空間環境的影響，近年來受到了領域內研究人員的廣泛關注。通過在環境中布置多個超寬帶(Ultra-Wideband，UWB)固定錨點，可以實現對無人飛行器的測距定位。也有學者研究了基于機載藍牙傳感器，無固定測距錨點的相對定位算法，該算法需要集群成員交換地理北及速度信息。Guo K.等利用單架懸停的無人飛行器作為UWB通信測距錨點，實現集群成員間的相對定位，這種方法仍然需要地理北及速度信息，且集群成員的初始定位需要單架無人飛行器懸停才能啟動。S.Helm等研究了一種基于藍牙通信測距，并使用光流法測速輔助的相對定位算法，但光流法測速只適用于低速飛行和光線良好的任務場景，因此該方法對飛行速度及環境條件均有一定限制。F.M.Martel等研究了UWB測距和視覺即時定位與地圖構建(Simultaneous Localization and Mapping，SLAM)組合的相對定位算法，該算法通過最少6次通信測距求解2臺設備的4自由度相對位姿，視覺SLAM裝置用于測量每臺設備相對于各自位置原點的位移，但其同樣只適用于低速環境，且對機載計算資源有較高要求。T.M.Nguyen等通過在無人飛行器及追蹤目標上分別安裝2個UWB天線，同時使用光流法測速，實現了無人飛行器對移動目標的相對定位，但受限于雙天線的安裝距離，只能實現較近距離的相對定位。

因此，為了解決上述問題，實現GNSS拒止環境下無人飛行器集群成員間的自主相對導航，本文研究了一種基于機載慣性測量單元(Inertial Measurement Unit，IMU)、氣壓高度計與數據鏈測距組合的相對位姿求解算法。在建立飛行器三維運動的水平坐標系投影等效模型的基礎上，求解狀態量相對位置及航向角的線性最小二乘問題。通過引入遞推最小二乘算法(Recursive Least Squares，RLS)，實現了相對位姿估計的實時輸出，并給出了使系統狀態量不可觀測的幾種飛行形式。仿真結果表明，該算法能夠有效降低機載計算負載，并快速穩定地求解無人飛行器間的初始相對位姿。

下面首先建立二維坐標系下的相對位姿求解算法，然后引入該算法的遞推求解形式，再對所提算法的可觀測性進行理論分析，最后進行數值仿真實驗和結果分析。

1 僅測距初始相對位姿確定算法

1.1 運動解耦與水平坐標系定義

無人飛行器在空間坐標系下相對慣性系包含位置及姿態信息，其中姿態角由俯仰角、滾轉角及偏航角組成。當其攜帶氣壓高度計時，高度方向的位置信息可直接獲得，此時可以將各無人飛行器的空間位置及姿態投影至與其固連的當地水平坐標系{}下，該坐標系原點位于質心，軸指向天，和軸位于水平面內，軸指向飛行器的線速度方向，軸方向由右手法則確定。軸與地理北的夾角定義為航向角，偏西向為正。此時{}系的等效偏航角速率為

(1)

令無人飛行器本體系下的加速度為，{}系的加速度為，則有如下關系式成立

(2)

在水平面內的投影為′，則有

′=

(3)

此時，無人飛行器在空間中的運動已解耦為：角速度為、加速度為′的二維平面運動和高度方向的豎直運動，因而可將三維空間下無人飛行器的相對姿態求解問題簡化為二維平面下的相對航向角及相對位置求解問題。

令無人飛行器間的空間距離為，高度差為Δ，則無人飛行器在二維平面下的距離可通過式(4)求出

(4)

其中，由機載通信數據鏈通過TOA(Time of Arrival)模式測量得到；Δ由2個飛行器的機載氣壓高度計測量的高度作差得到。

圖1 坐標系定義與測距示意圖Fig.1 Coordinate system definition and UAV ranging diagram

表1 向量表示

1.2 建立相對定位算法

利用文獻[16]中的方法，2個飛行器測距時滿足如下方程

(5)

且和滿足如下表達式

=cosΔ-sinΔ+

(6)

=sinΔ+cosΔ+

(7)

將式(6)和式(7)代入式(5)后，整理可得

=-2-2+(-2-2)cosΔ+

(2-2)sinΔ+2+2+

(8)

其中

(9)

(10)

當2個飛行器進行次通信測距時，則可獲得個如式(8)所示的方程，將由這個方程構成的方程組整理成如下矩陣形式

×7=×1

(11)

式中，為包含相對位置及航向角的待求解未知量；×7為系數矩陣；×1為包含測量信息的列向量。、×7和×1的具體表示如下

(12)

(13)

(14)

由式(13)和式(14)可知，矩陣×7和×1中的元素均為已知量，將式(11)中×1移至等號左邊，并構造新的矩陣×8=[-]，因此式(11)可進一步表示為

(15)

式中

[cosΔsinΔ1]

(16)

當=5時，rank()=5，由秩-零化度定理可知，矩陣的核空間為3，因此式(15)矩陣方程的解可表示為矩陣的極大線性無關組,,的線性組合

(17)

式中，、為待求解系數。

(18)

(19)

當≥6時，利用最小二乘算法(Least Squares，LS)，可直接求得式(11)中未知量的近似解

(20)

利用上述方法最少利用5次航向角、加速度信息和測距數據,即可求解2個飛行器間的相對位置和航向角的估計值。

2 RLS

從理論上講，僅利用5次通信測距和對應時刻的加速度測量信息就可以求解出初始相對位姿，但由于數據鏈測距存在隨機誤差，IMU的陀螺儀和加速度計測量值也有常值誤差和隨機誤差，那么利用矩陣中5行7列元素間的約束關系進行位姿解算會存在較大誤差，求解得到的極大線性無關組,,波動較大，最終導致算法求解結果誤差波動較大。而利用多次通信測距數據構建LS(式(11)中≥7)，可以避免求解矩陣的極大線性無關組，同時利用多次測量數據逼近未知量的真值，降低了單次測量數據對算法求解影響的權重。同時，為了盡可能節省飛行器機載運算資源，這里采用遞推算法進行相對位姿的實時求解及輸出。

當有≥7次測量數據時，式(11)中的矩陣及列向量×1可以表示為

(21)

式中，為矩陣的第行數據，且是維數為1×7的行向量。

(22)

其中，是修正增益，是維數為7×1的列向量。具體地，僅測距初始位姿確定的RLS計算公式如下

(23)

(24)

式中，估計初值由前20次通信測距生成，即生成估計初值時=21。選取前20次測量數據生成初值的原因將在數值仿真4.2.1節中進行詳細說明，當達到總測距次數時，遞推算法結束運行。

RLS利用時刻的測量數據對-1時刻的狀態估計值進行修正，進而得到時刻的狀態估計值。具體地，遞推過程中每次測量生成的數據列和及遞推算法的運算流程分別如圖2及圖3所示。

圖2 測量數據列流程圖Fig.2 Flow chart of measurement column

圖3 遞推算法流程圖Fig.3 Flow chart of RLS algorithm

3 狀態可觀測性分析

理論上講，如果通過單次或多次的測量值作為輸入，能夠確定出唯一的系統狀態量(物理可行解)，那么這個系統就是可觀測的。與完備測量系統不同，本文研究的欠測量系統的狀態可觀測性是變化的，會受到相對運動的約束，因此有必要對系統進行狀態可觀測性分析，給出使系統狀態不可觀測的相對運動形式，用于指導飛行實踐。下面就根據上文中提出的擬合求解方法對系統狀態可觀測性進行分析。

當測距次數為5次時，將矩陣方程式(11)生成的矩陣展開

(25)

若rank()=5，則矩陣方程式(15)可以求得唯一解。因此，只要使的秩小于5，算法就無法求解出未知量的真值。使rank()<5直觀的方法有：2個飛行器中的其中1個絕對靜止(如圖4所示)；2個飛行器均沿直線飛行(如圖5所示)。

圖4 算法求解結果發散形位1Fig.4 Algorithm-diverging position 1

圖5 算法求解結果發散形位2Fig.5 Algorithm-diverging position 2

當測距次數大于5次時，使用RLS辨識式(11)中未知量的值。由于矩陣的每一列數據分別代表待辨識參數對應分量的觀測值，因此當矩陣的某一列觀測值為0時，該分量的觀測值始終不存在，也就無法求解得到該分量的最小二乘辨識，或者求解值趨于無窮大。觀察式(13)中矩陣的元素組成可知，當觀測飛行器B沿直線飛行時，=0(其在本體系下的軸分量為0)；當觀測飛行器B靜止時，和均為0(和軸分量均為0)。同理，當被觀測飛行器A沿直線飛行時，=0；當被觀測飛行器A靜止時，和均為0。

為了得到飛行器A相對于飛行器B的位置及航向角，只需辨識待求解未知量的[cosΔsinΔ]分量，而以上分量分別對應于矩陣的前四列，只要這幾列的觀測值不為0，就可通過LS辨識出其對應的真值。由前面的分析可知，當2個飛行器以如下描述的軌跡飛行時，飛行器B無法辨識出飛行器A的相對位置及航向角：

1)飛行器B沿直線飛行，且飛行器A沿直線飛行或靜止；

2)飛行器B靜止，而飛行器A做任意機動。

值得注意的是，由前面的分析可知，當飛行器B做S形機動時，飛行器A無論沿直線飛行或是靜止不動，飛行器B均可估計出飛行器A的相對位置，但飛行器A靜止時，飛行器B則無法估計出相對航向角。

4 數值仿真

4.1 仿真軌跡與參數設置

假設飛行器A在二維水平慣性系下的初始位置為[0，100]，飛行器B初始位置為[0，0]，2個飛行器以小于5m/s的相對速度飛行，總飛行時長為60s。為了驗證可觀測性分析的結論，這里生成了兩類飛行軌跡：圖6所示為使系統狀態可觀測的飛行軌跡，2個飛行器分別以不同角速度做勻速S形機動；圖7(a)和圖7(b)所示為第3節狀態可觀測分析中指出的1)和2)兩種使系統狀態不可觀測情況對應的典型飛行軌跡。圖7(a)展示了2個飛行器均沿直線勻速飛行時的軌跡，圖7(b)展示了飛行器A做勻速S形機動，飛行器B靜止不動時的飛行軌跡。

圖6 狀態可觀測軌跡Fig.6 Observable state trajectory

(a) 2個飛行器均沿直線飛行的軌跡

(b)飛行器B靜止，而飛行器A做S形機動的軌跡圖7 狀態不可觀測軌跡Fig.7 Unobservable state trajectory

采用的機載IMU由陀螺儀和加速度計組成，假設二者的測量誤差主要包括常值誤差和隨機誤差；數據鏈測距誤差只包含隨機誤差，將該誤差建模為零均值高斯白噪聲。這些傳感器的誤差參數如表2所示。

表2 機載測量儀器誤差

4.2 算法求解結果

4.2.1 狀態可觀測軌跡求解結果

采用4.1節圖6生成的系統可觀測軌跡進行飛行器間初始相對位置及航向角的遞推求解。飛行器測距時間間隔為1s，采用飛行0～20s(20次測距)的數據進行初值生成，而不是5次測距。選取該測距次數的主要原因在于：在采樣間隔較小時，飛行器在各自初始坐標系下的相對運動不明顯，采樣點連線近似直線，即飛行器的飛行形式接近圖5給出的使求解算法失真的飛行形位，初始估計偏差較大。

圖8和圖9分別展示了使用RLS進行1000次打靶仿真求得的B飛行器對A飛行器的初始位置估計誤差及其分布。初始航向角估計誤差的分布如圖10所示。

圖8 X和Y軸位置估計誤差Fig.8 X- and Y-axis position error

圖9 距離估計誤差Fig.9 Distance estimation error

圖10 航向角誤差估計Fig.10 Heading angle estimation error

由圖8及圖9可知，運用RLS生成的位置估計初值誤差及標準差較大，表明初值估計波動大，可信度較低。迭代運算的前20s(測距20次)，初始位置估計及標準差迅速收斂。迭代至40s時，位置估計誤差趨于穩定，且誤差均值約為9m，達到初始相對距離的10%以內。由圖10可知，航向角初始估計誤差均值較小，但標準差較大，表明其估計誤差波動較大，估計值可信度較低。迭代運算的前20s，標準差下降明顯，估計誤差迅速收斂。當迭代運算至40s時，估計誤差趨于穩定，且誤差均值在1°以內。

4.2.2 狀態不可觀測軌跡求解結果

由第3章狀態可觀測性理論分析可知，在圖7(a)和圖7(b)兩種飛行軌跡下，飛行器B無法估計出飛行器A的相對位置及航向角。圖11和圖12分別展示了在這兩種飛行軌跡下飛行器B對飛行器A的相對位置和航向角估計誤差及其分布。

(a) X和Y軸位置估計誤差

(b) 航向角誤差估計圖11 2個飛行器均沿直線飛行時，飛行器B對飛行器A的相對位置及航向角估計誤差Fig.11 The relative position and heading angle estimation error of UAV B relative to UAV A when both UAVs are flying in straight lines

(a) X和Y軸位置估計誤差

(b) 航向角誤差估計圖12 飛行器B靜止，而飛行器A做S形機動時，飛行器B對飛行器A的相對位置及航向角估計誤差Fig.12 The relative position and heading angle estimation error of UAV B relative to UAV A when UAV B stands still and UAV A flies a S trajectory

由圖11和圖12的仿真結果可知，迭代至40s時，兩種飛行軌跡下的相對位置單軸估計誤差均值均大于1000m，且相對航向角估計誤差均方差均大于100°。因此，飛行器B無法估計出飛行器A的相對位置和航向角，即仿真結果驗證了理論分析中狀態不可觀測的飛行軌跡。

為了進一步說明無人飛行器的飛行軌跡對遞推算法可觀測性的影響，下面將通過表格的形式對2個飛行器在不同軌跡組合下，飛行器B對飛行器A的相對位置及航向角的估計結果進行仿真總結，仿真結果如表3所示。

表3 飛行軌跡和仿真結果

表3中，“√”表示飛行器B在該飛行軌跡下可以估計出飛行器A的相對位置軸分量、軸分量或航向角Δ的值，“×”表示無法估計出對應的值。

(26)

因此,可以認為這種情況下飛行器B仍然對飛行器A的軸位置分量具有觀測性。

由表3可以看出，當飛行器B做S形機動時，其對飛行器A的相對位姿狀態量有較高的可觀測性；當飛行器B沿直線飛行時，其對飛行器A相對位姿狀態量的可觀測性開始降低；當飛行器B完全靜止時，其完全失去了對飛行器A狀態量的可觀測性。因此，表3的仿真結果驗證了第3節的可觀測性分析結論，并為算法的飛行實踐提供了有意義的指導。

4.3 算法求解速率分析

本節對5次測距算法、LS以及RLS的求解效率進行對比分析，算法程序的運算環境為6核Inter Core i5-10400處理器。以4.1節使系統狀態可觀測的無人飛行器軌跡(圖6)為例，測距時間間隔為1s，分別對以上三種相對位姿求解算法進行了1000次打靶仿真，獲得各算法對應的平均求解時間及相對位置求解精度,如表4所示。表4中，相對位置求解誤差指和軸求解誤差的平方和開根，即距離誤差。算法運行時間越短，位置誤差均值及方差越小，則認為求解效率越高。對于5次測距算法，當總測距數據大于5次，如30次測距時，按照總測距次數等間隔取樣5次進行位姿解算。

表4 算法求解效率

由表4可以得出以下結論：

1)從算法求解時間來看，5次測距算法耗時最長，算法運算量最大，這是因為在測量數據較少的情況下，對測量數據的進一步解算(如生成矩陣的極大線性無關組、線性無關組的系數解算)需要較多運算步驟。直接對線性方程組求取最小二乘逆的算法(LS)求解時間最短，與其在求解結果方面等價的RLS次之，且遞推算法隨著測距次數的增多，單次遞推求解時間不變，總運算時間線性增長。

2)從算法求解誤差來看，當測距次數為30次時，5次測距算法求解的位置誤差均值及方差最小，LS及RLS算法位置求解誤差及方差相當。當測距次數大于30且不斷增大時，LS及RLS算法的位置誤差及方差均快速下降，且均小于5次測距算法對應的求解誤差。

綜上可知，在測距次數較少時，5次測距算法能夠利用測距數據間的關系解算得到比LS及RLS精度更高的結果，但需要付出比其他兩種算法更多的時間代價，且該算法只利用了5次測量信息，因此誤差波動極大。RLS能夠在每次遞推過程中輸出當前的相對位姿估計，且每次計算只消耗極小的機載算力，相較于在進行一段飛行時間后消耗較多機載算力才能給出相同估計精度的LS，以及誤差波動較大的5次測距算法，RLS在計算的實時性及穩定性方面具有顯著優勢。

4.4 算法敏感性分析

運用RLS求解2個無人飛行器初始相對位置的誤差源主要包括：飛行器間測距誤差，由機載IMU常值漂移引起的本體系下的位置誤差。由于該遞推算法在解算過程中同時存在位置誤差及測距誤差，無法反映單個誤差因素對算法精度帶來的影響。圖13展示了當飛行器位置信息準確時，測距誤差對姿態解算精度的影響。圖14展示了飛行器測距無誤差，但其中1個飛行器本體系下的位置測量存在誤差時，對姿態解算精度的影響。

圖13 航向角及位置估計誤差關于測距噪聲均方差的曲線Fig.13 Heading angle and position mean absolute errors(MAE) as functions of distance measurement noise standard deviation

圖14 航向角及位置估計誤差關于位置測量噪聲的曲線Fig.14 Heading angle and position mean absolute errors(MAE) as functions of position measurement noise standard deviation

由圖13可知，航向角及位置估計誤差與測距噪聲近似成正比關系。觀察式(8)可以發現，只有項包含距離測量量，因此當測距誤差逐漸增大，而其他系數項均不變時，各項對應的未知量求解誤差均勻增大。

從圖14可以看出，航向角估計誤差及位置估計誤差隨位置測量噪聲的增大均呈現增大趨勢，且估計誤差的增大趨勢逐漸放緩。比較圖13及圖14可以發現，估計算法對測距敏感度遠大于位置測量敏感度。這可以由式(8)說明，此時當其中1個飛行器位置測量存在誤差時，式(8)左側誤差增大，右側前三項未知量系數誤差同樣增大，一定程度上減小了誤差對未知量的影響。

5 結論

本文針對GNSS拒止環境下無人飛行器間的相對定位問題，建立了一種基于機載IMU與數據鏈測距組合的相對定位算法，并運用數值仿真進行驗證，得到以下結論：

1)進行相對位姿估計的飛行器B沿直線飛行或靜止時，會導致被估計飛行器A的相對狀態量部分或全部不可觀測。當飛行器B做S形機動時，無論飛行器A如何機動，飛行器B都能獲得較好的相對狀態量的可觀測性。

2)將RLS應用于多次測距能夠實時快速迭代求解初始相對位姿，最終求解的相對位置誤差均值穩定在初始距離的10%以內。

3)僅測距定姿算法的求解誤差與測距噪聲及位置噪聲近似呈正比，且算法對測距噪聲的敏感度遠高于位置噪聲，這是由僅測距算法的結構造成的。