無(wú)人飛行器集群僅測(cè)距初始相對(duì)位姿確定方法研究

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

(1. 南京航空航天大學(xué)航天學(xué)院，南京 210016；2.復(fù)雜系統(tǒng)控制與智能協(xié)同技術(shù)重點(diǎn)實(shí)驗(yàn)室，北京 100074)

0 引言

當(dāng)前，無(wú)人飛行器通過(guò)集群協(xié)同的方式執(zhí)行任務(wù)的應(yīng)用越來(lái)越廣泛，例如協(xié)同偵察、森林火災(zāi)探測(cè)及危險(xiǎn)環(huán)境中執(zhí)行搜救等。相較于單個(gè)高價(jià)值、多載荷的無(wú)人飛行器，集群無(wú)人飛行器具有雷達(dá)截面小、成本低、數(shù)量多、容錯(cuò)率高等特點(diǎn)，能夠通過(guò)群體協(xié)同效應(yīng)實(shí)現(xiàn)單個(gè)大型飛行器難以完成或者代價(jià)太大的任務(wù)。

實(shí)現(xiàn)集群成員間的高精度相對(duì)定位是保障集群任務(wù)的關(guān)鍵前提。傳統(tǒng)的集群飛行器常使用獨(dú)立于集群外的、提供公共參考坐標(biāo)的外部定位系統(tǒng)實(shí)現(xiàn)成員間相對(duì)定位，如運(yùn)動(dòng)捕捉系統(tǒng)(Motion Capture System，MCS)和全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Sa-tellite System，GNSS)。但MCS需要預(yù)先布置于環(huán)境中，無(wú)法應(yīng)用于未知環(huán)境，GNSS導(dǎo)航信號(hào)在復(fù)雜環(huán)境中存在衰減、多路徑效應(yīng)、無(wú)信號(hào)甚至信號(hào)欺騙等問(wèn)題。因此，GNSS拒止環(huán)境下無(wú)人飛行器集群的自主相對(duì)導(dǎo)航已經(jīng)成為行業(yè)內(nèi)的熱點(diǎn)問(wèn)題。

M.Kriegleder等研究了基于無(wú)人飛行器的僅測(cè)角機(jī)載紅外傳感系統(tǒng)，由于紅外傳感器視野范圍有限，需要在不同方向布置機(jī)載傳感器陣列，以實(shí)現(xiàn)目標(biāo)飛行器始終處于測(cè)量視野范圍內(nèi)。也有學(xué)者研究了基于視覺(jué)相機(jī)的相對(duì)定位系統(tǒng)，但基于視覺(jué)的定位系統(tǒng)需要測(cè)量飛行器對(duì)目標(biāo)飛行器進(jìn)行追蹤機(jī)動(dòng)，以保證目標(biāo)處于觀測(cè)視野內(nèi)，且當(dāng)目標(biāo)飛行器距離較遠(yuǎn)時(shí)，視覺(jué)相機(jī)存在特征識(shí)別點(diǎn)模糊等問(wèn)題，一定程度上限制了無(wú)人飛行器間的相對(duì)飛行距離。

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

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

下面首先建立二維坐標(biāo)系下的相對(duì)位姿求解算法，然后引入該算法的遞推求解形式，再對(duì)所提算法的可觀測(cè)性進(jìn)行理論分析，最后進(jìn)行數(shù)值仿真實(shí)驗(yàn)和結(jié)果分析。

1 僅測(cè)距初始相對(duì)位姿確定算法

1.1 運(yùn)動(dòng)解耦與水平坐標(biāo)系定義

無(wú)人飛行器在空間坐標(biāo)系下相對(duì)慣性系包含位置及姿態(tài)信息，其中姿態(tài)角由俯仰角、滾轉(zhuǎn)角及偏航角組成。當(dāng)其攜帶氣壓高度計(jì)時(shí)，高度方向的位置信息可直接獲得，此時(shí)可以將各無(wú)人飛行器的空間位置及姿態(tài)投影至與其固連的當(dāng)?shù)厮阶鴺?biāo)系{}下，該坐標(biāo)系原點(diǎn)位于質(zhì)心，軸指向天，和軸位于水平面內(nèi)，軸指向飛行器的線速度方向，軸方向由右手法則確定。軸與地理北的夾角定義為航向角，偏西向?yàn)檎?。此時(shí){}系的等效偏航角速率為

(1)

令無(wú)人飛行器本體系下的加速度為，{}系的加速度為，則有如下關(guān)系式成立

(2)

在水平面內(nèi)的投影為′，則有

′=

(3)

此時(shí)，無(wú)人飛行器在空間中的運(yùn)動(dòng)已解耦為：角速度為、加速度為′的二維平面運(yùn)動(dòng)和高度方向的豎直運(yùn)動(dòng)，因而可將三維空間下無(wú)人飛行器的相對(duì)姿態(tài)求解問(wèn)題簡(jiǎn)化為二維平面下的相對(duì)航向角及相對(duì)位置求解問(wèn)題。

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

(4)

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

圖1 坐標(biāo)系定義與測(cè)距示意圖Fig.1 Coordinate system definition and UAV ranging diagram

表1 向量表示

1.2 建立相對(duì)定位算法

利用文獻(xiàn)[16]中的方法，2個(gè)飛行器測(cè)距時(shí)滿足如下方程

(5)

且和滿足如下表達(dá)式

=cosΔ-sinΔ+

(6)

=sinΔ+cosΔ+

(7)

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

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

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

(8)

其中

(9)

(10)

當(dāng)2個(gè)飛行器進(jìn)行次通信測(cè)距時(shí)，則可獲得個(gè)如式(8)所示的方程，將由這個(gè)方程構(gòu)成的方程組整理成如下矩陣形式

×7=×1

(11)

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

(12)

(13)

(14)

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

(15)

式中

[cosΔsinΔ1]

(16)

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

(17)

式中，、為待求解系數(shù)。

(18)

(19)

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

(20)

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

2 RLS

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

當(dāng)有≥7次測(cè)量數(shù)據(jù)時(shí)，式(11)中的矩陣及列向量×1可以表示為

(21)

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

(22)

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

(23)

(24)

式中，估計(jì)初值由前20次通信測(cè)距生成，即生成估計(jì)初值時(shí)=21。選取前20次測(cè)量數(shù)據(jù)生成初值的原因?qū)⒃跀?shù)值仿真4.2.1節(jié)中進(jìn)行詳細(xì)說(shuō)明，當(dāng)達(dá)到總測(cè)距次數(shù)時(shí)，遞推算法結(jié)束運(yùn)行。

RLS利用時(shí)刻的測(cè)量數(shù)據(jù)對(duì)-1時(shí)刻的狀態(tài)估計(jì)值進(jìn)行修正，進(jìn)而得到時(shí)刻的狀態(tài)估計(jì)值。具體地，遞推過(guò)程中每次測(cè)量生成的數(shù)據(jù)列和及遞推算法的運(yùn)算流程分別如圖2及圖3所示。

圖2 測(cè)量數(shù)據(jù)列流程圖Fig.2 Flow chart of measurement column

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

3 狀態(tài)可觀測(cè)性分析

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

當(dāng)測(cè)距次數(shù)為5次時(shí)，將矩陣方程式(11)生成的矩陣展開(kāi)

(25)

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

圖4 算法求解結(jié)果發(fā)散形位1Fig.4 Algorithm-diverging position 1

圖5 算法求解結(jié)果發(fā)散形位2Fig.5 Algorithm-diverging position 2

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

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

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

2)飛行器B靜止，而飛行器A做任意機(jī)動(dòng)。

值得注意的是，由前面的分析可知，當(dāng)飛行器B做S形機(jī)動(dòng)時(shí)，飛行器A無(wú)論沿直線飛行或是靜止不動(dòng)，飛行器B均可估計(jì)出飛行器A的相對(duì)位置，但飛行器A靜止時(shí)，飛行器B則無(wú)法估計(jì)出相對(duì)航向角。

4 數(shù)值仿真

4.1 仿真軌跡與參數(shù)設(shè)置

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

圖6 狀態(tài)可觀測(cè)軌跡Fig.6 Observable state trajectory

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

(b)飛行器B靜止，而飛行器A做S形機(jī)動(dòng)的軌跡圖7 狀態(tài)不可觀測(cè)軌跡Fig.7 Unobservable state trajectory

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

表2 機(jī)載測(cè)量?jī)x器誤差

4.2 算法求解結(jié)果

4.2.1 狀態(tài)可觀測(cè)軌跡求解結(jié)果

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

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

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

圖9 距離估計(jì)誤差Fig.9 Distance estimation error

圖10 航向角誤差估計(jì)Fig.10 Heading angle estimation error

由圖8及圖9可知，運(yùn)用RLS生成的位置估計(jì)初值誤差及標(biāo)準(zhǔn)差較大，表明初值估計(jì)波動(dòng)大，可信度較低。迭代運(yùn)算的前20s(測(cè)距20次)，初始位置估計(jì)及標(biāo)準(zhǔn)差迅速收斂。迭代至40s時(shí)，位置估計(jì)誤差趨于穩(wěn)定，且誤差均值約為9m，達(dá)到初始相對(duì)距離的10%以內(nèi)。由圖10可知，航向角初始估計(jì)誤差均值較小，但標(biāo)準(zhǔn)差較大，表明其估計(jì)誤差波動(dòng)較大，估計(jì)值可信度較低。迭代運(yùn)算的前20s，標(biāo)準(zhǔn)差下降明顯，估計(jì)誤差迅速收斂。當(dāng)?shù)\(yùn)算至40s時(shí)，估計(jì)誤差趨于穩(wěn)定，且誤差均值在1°以內(nèi)。

4.2.2 狀態(tài)不可觀測(cè)軌跡求解結(jié)果

由第3章?tīng)顟B(tài)可觀測(cè)性理論分析可知，在圖7(a)和圖7(b)兩種飛行軌跡下，飛行器B無(wú)法估計(jì)出飛行器A的相對(duì)位置及航向角。圖11和圖12分別展示了在這兩種飛行軌跡下飛行器B對(duì)飛行器A的相對(duì)位置和航向角估計(jì)誤差及其分布。

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

(b) 航向角誤差估計(jì)圖11 2個(gè)飛行器均沿直線飛行時(shí)，飛行器B對(duì)飛行器A的相對(duì)位置及航向角估計(jì)誤差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軸位置估計(jì)誤差

(b) 航向角誤差估計(jì)圖12 飛行器B靜止，而飛行器A做S形機(jī)動(dòng)時(shí)，飛行器B對(duì)飛行器A的相對(duì)位置及航向角估計(jì)誤差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的仿真結(jié)果可知，迭代至40s時(shí)，兩種飛行軌跡下的相對(duì)位置單軸估計(jì)誤差均值均大于1000m，且相對(duì)航向角估計(jì)誤差均方差均大于100°。因此，飛行器B無(wú)法估計(jì)出飛行器A的相對(duì)位置和航向角，即仿真結(jié)果驗(yàn)證了理論分析中狀態(tài)不可觀測(cè)的飛行軌跡。

為了進(jìn)一步說(shuō)明無(wú)人飛行器的飛行軌跡對(duì)遞推算法可觀測(cè)性的影響，下面將通過(guò)表格的形式對(duì)2個(gè)飛行器在不同軌跡組合下，飛行器B對(duì)飛行器A的相對(duì)位置及航向角的估計(jì)結(jié)果進(jìn)行仿真總結(jié)，仿真結(jié)果如表3所示。

表3 飛行軌跡和仿真結(jié)果

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

(26)

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

由表3可以看出，當(dāng)飛行器B做S形機(jī)動(dòng)時(shí)，其對(duì)飛行器A的相對(duì)位姿狀態(tài)量有較高的可觀測(cè)性；當(dāng)飛行器B沿直線飛行時(shí)，其對(duì)飛行器A相對(duì)位姿狀態(tài)量的可觀測(cè)性開(kāi)始降低；當(dāng)飛行器B完全靜止時(shí)，其完全失去了對(duì)飛行器A狀態(tài)量的可觀測(cè)性。因此，表3的仿真結(jié)果驗(yàn)證了第3節(jié)的可觀測(cè)性分析結(jié)論，并為算法的飛行實(shí)踐提供了有意義的指導(dǎo)。

4.3 算法求解速率分析

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

表4 算法求解效率

由表4可以得出以下結(jié)論：

1)從算法求解時(shí)間來(lái)看，5次測(cè)距算法耗時(shí)最長(zhǎng)，算法運(yùn)算量最大，這是因?yàn)樵跍y(cè)量數(shù)據(jù)較少的情況下，對(duì)測(cè)量數(shù)據(jù)的進(jìn)一步解算(如生成矩陣的極大線性無(wú)關(guān)組、線性無(wú)關(guān)組的系數(shù)解算)需要較多運(yùn)算步驟。直接對(duì)線性方程組求取最小二乘逆的算法(LS)求解時(shí)間最短，與其在求解結(jié)果方面等價(jià)的RLS次之，且遞推算法隨著測(cè)距次數(shù)的增多，單次遞推求解時(shí)間不變，總運(yùn)算時(shí)間線性增長(zhǎng)。

2)從算法求解誤差來(lái)看，當(dāng)測(cè)距次數(shù)為30次時(shí)，5次測(cè)距算法求解的位置誤差均值及方差最小，LS及RLS算法位置求解誤差及方差相當(dāng)。當(dāng)測(cè)距次數(shù)大于30且不斷增大時(shí)，LS及RLS算法的位置誤差及方差均快速下降，且均小于5次測(cè)距算法對(duì)應(yīng)的求解誤差。

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

4.4 算法敏感性分析

運(yùn)用RLS求解2個(gè)無(wú)人飛行器初始相對(duì)位置的誤差源主要包括：飛行器間測(cè)距誤差，由機(jī)載IMU常值漂移引起的本體系下的位置誤差。由于該遞推算法在解算過(guò)程中同時(shí)存在位置誤差及測(cè)距誤差，無(wú)法反映單個(gè)誤差因素對(duì)算法精度帶來(lái)的影響。圖13展示了當(dāng)飛行器位置信息準(zhǔn)確時(shí)，測(cè)距誤差對(duì)姿態(tài)解算精度的影響。圖14展示了飛行器測(cè)距無(wú)誤差，但其中1個(gè)飛行器本體系下的位置測(cè)量存在誤差時(shí)，對(duì)姿態(tài)解算精度的影響。

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

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

由圖13可知，航向角及位置估計(jì)誤差與測(cè)距噪聲近似成正比關(guān)系。觀察式(8)可以發(fā)現(xiàn)，只有項(xiàng)包含距離測(cè)量量，因此當(dāng)測(cè)距誤差逐漸增大，而其他系數(shù)項(xiàng)均不變時(shí)，各項(xiàng)對(duì)應(yīng)的未知量求解誤差均勻增大。

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

5 結(jié)論

本文針對(duì)GNSS拒止環(huán)境下無(wú)人飛行器間的相對(duì)定位問(wèn)題，建立了一種基于機(jī)載IMU與數(shù)據(jù)鏈測(cè)距組合的相對(duì)定位算法，并運(yùn)用數(shù)值仿真進(jìn)行驗(yàn)證，得到以下結(jié)論：

1)進(jìn)行相對(duì)位姿估計(jì)的飛行器B沿直線飛行或靜止時(shí)，會(huì)導(dǎo)致被估計(jì)飛行器A的相對(duì)狀態(tài)量部分或全部不可觀測(cè)。當(dāng)飛行器B做S形機(jī)動(dòng)時(shí)，無(wú)論飛行器A如何機(jī)動(dòng)，飛行器B都能獲得較好的相對(duì)狀態(tài)量的可觀測(cè)性。

2)將RLS應(yīng)用于多次測(cè)距能夠?qū)崟r(shí)快速迭代求解初始相對(duì)位姿，最終求解的相對(duì)位置誤差均值穩(wěn)定在初始距離的10%以內(nèi)。

3)僅測(cè)距定姿算法的求解誤差與測(cè)距噪聲及位置噪聲近似呈正比，且算法對(duì)測(cè)距噪聲的敏感度遠(yuǎn)高于位置噪聲，這是由僅測(cè)距算法的結(jié)構(gòu)造成的。