潘禮規(guī)，尹佳琪，徐春光

1.中山大學(xué)航空航天學(xué)院，廣東 深圳 518107

2.中國(guó)航天科工信息技術(shù)研究院，北京 100144

無(wú)人機(jī)集群編隊(duì)是無(wú)人機(jī)應(yīng)用的新模式，在執(zhí)行任務(wù)過(guò)程中，集群飛行具有諸多優(yōu)勢(shì)，已經(jīng)成為無(wú)人機(jī)領(lǐng)域的研究熱點(diǎn)（樊瓊劍等，2009；宗群等，2017）。目前，集群編隊(duì)研究主要集中在編隊(duì)飛行控制策略（Lau et al.，2015；王祥科等，2021）、緊密編隊(duì)氣動(dòng)影響（詹光等，2019）、編隊(duì)相對(duì)位置保持（彭建帥等，2021）與變換（Richert et al.，2013）以及編隊(duì)航跡規(guī)劃（Shorakaei et al.，2014；Zhang et al.，2015）等幾個(gè)方面。對(duì)軍用無(wú)人機(jī)而言，無(wú)人機(jī)集群作戰(zhàn)模式是未來(lái)發(fā)展方向之一，相比于單機(jī)作戰(zhàn)，集群作戰(zhàn)具有低可探測(cè)性、協(xié)同作戰(zhàn)、飽和攻擊以及低成本等優(yōu)勢(shì)（蔡杰等，2020）。此種作戰(zhàn)模式需具備良好的協(xié)同交互能力，協(xié)同導(dǎo)航是其中的關(guān)鍵之一。無(wú)人機(jī)協(xié)同導(dǎo)航指無(wú)人機(jī)編隊(duì)間通過(guò)機(jī)間數(shù)據(jù)鏈的信息交互，對(duì)原有導(dǎo)航信息進(jìn)行融合校準(zhǔn)，以提高集群整體的導(dǎo)航精度（許曉偉等，2017）。此外，無(wú)人機(jī)間的高精度相對(duì)定位與導(dǎo)航技術(shù)是實(shí)現(xiàn)協(xié)同導(dǎo)航的重要條件，為無(wú)人機(jī)集群編隊(duì)飛行提供了基礎(chǔ)保障。為彌補(bǔ)傳統(tǒng)導(dǎo)航系統(tǒng)的缺陷，有相關(guān)學(xué)者進(jìn)行了多方面研究。

熊駿等（2018）研究了基于UWB輔助的相對(duì)導(dǎo)航方法，結(jié)果表明僅依靠較少GPS衛(wèi)星即可達(dá)到較好的相對(duì)導(dǎo)航精度。劉曉洋等（2019b）和谷旭平等（2021）研究了分層式協(xié)同導(dǎo)航方案，該方案改善了傳統(tǒng)主從式無(wú)人機(jī)協(xié)同導(dǎo)航的性能，一定程度上校正了低精度的INS 誤差。王念曾等（2019）將UWB 測(cè)距融入到INS/GNSS的相對(duì)導(dǎo)航算法中，使小型無(wú)人機(jī)的定位精度達(dá)到了厘米量級(jí)。Xu等（2020）則研究了INS/Vis‐Nav/UWB 的分布式融合算法，同樣可獲得厘米級(jí)的相對(duì)估計(jì)精度。潘瑞鴻等（2017）利用兩無(wú)人機(jī)間的測(cè)距測(cè)角信息，構(gòu)建了以UAV 為頂點(diǎn)的三角構(gòu)型，仿真結(jié)果表明：該方法獲得的導(dǎo)航精度高于航位推算，且誤差不隨時(shí)間累積。劉曉洋等（2019a）提出了基于測(cè)距/測(cè)速的協(xié)同導(dǎo)航算法，利用擴(kuò)展卡爾曼濾波估計(jì)僚機(jī)的狀態(tài)誤差，結(jié)果表明：相比于單INS和僅測(cè)距的方案，該方法能夠獲得更高的估計(jì)精度。無(wú)人機(jī)姿態(tài)信息可通過(guò)INS 獲得，通過(guò)兩架無(wú)人機(jī)進(jìn)行差分處理可得到相對(duì)姿態(tài)信息，但其誤差隨時(shí)間推移而發(fā)散，利用相對(duì)觀測(cè)來(lái)解算無(wú)人機(jī)姿態(tài)信息的研究較少，而姿態(tài)對(duì)于無(wú)人機(jī)集群協(xié)同控制同樣至關(guān)重要。因此，在無(wú)人機(jī)集群的協(xié)同定位中，同時(shí)實(shí)現(xiàn)位置、速度和姿態(tài)信息的狀態(tài)估計(jì)顯得尤為重要。

針對(duì)無(wú)人機(jī)集群的協(xié)同定位問(wèn)題，提出了一種新的相對(duì)定位技術(shù)，通過(guò)引入短距離小功率的無(wú)線電發(fā)射/接收裝置，利用載波相位觀測(cè)量建立一種機(jī)間相對(duì)觀測(cè)模型，同步解算集群內(nèi)無(wú)人機(jī)的位置、速度和姿態(tài)。該方法僅依靠機(jī)間測(cè)距就可估計(jì)出某一無(wú)人機(jī)相對(duì)于另一無(wú)人機(jī)的位置姿態(tài)及其速率狀態(tài)信息，而無(wú)需進(jìn)行測(cè)速測(cè)角，在一定程度上降低了導(dǎo)航傳感器的成本，同時(shí)也簡(jiǎn)化了傳感器的數(shù)學(xué)模型，對(duì)提高無(wú)人機(jī)集群協(xié)同控制能力具有較高的研究和應(yīng)用價(jià)值。

1 相對(duì)導(dǎo)航系統(tǒng)設(shè)計(jì)

1.1 相對(duì)導(dǎo)航方案

設(shè)計(jì)的相對(duì)導(dǎo)航方案采用主從式導(dǎo)航方式，每架無(wú)人機(jī)機(jī)身上均配備3 個(gè)天線，3 個(gè)天線構(gòu)成一定的三角幾何關(guān)系，根據(jù)此特性可構(gòu)建一個(gè)與無(wú)人機(jī)機(jī)體固連的基準(zhǔn)坐標(biāo)系，如圖1所示。

圖1 天線安裝示意圖Fig.1 Schematic diagram of antenna configuration

每架無(wú)人機(jī)相同位置的天線均能夠發(fā)射相同頻率的電磁波，且不同位置的天線可發(fā)射不同頻率的電磁波，即每架無(wú)人機(jī)都能夠發(fā)射3種點(diǎn)頻的電波。同時(shí)，每架無(wú)人機(jī)的每個(gè)天線均可實(shí)時(shí)檢測(cè)到領(lǐng)航無(wú)人機(jī)發(fā)射的3個(gè)點(diǎn)頻電波相位。

無(wú)人機(jī)集群編隊(duì)飛行時(shí)，僅由領(lǐng)航無(wú)人機(jī)發(fā)射3種點(diǎn)頻電波，而其他無(wú)人機(jī)均實(shí)時(shí)檢測(cè)主機(jī)發(fā)出的3個(gè)點(diǎn)頻信號(hào)到達(dá)本機(jī)每個(gè)天線的相位值。因此，針對(duì)主從兩架無(wú)人機(jī)而言，從無(wú)人機(jī)的每個(gè)天線位置均可檢測(cè)到領(lǐng)航無(wú)人機(jī)3個(gè)天線所發(fā)出的電磁波相位值，即每架從機(jī)均可在任意時(shí)刻獲得9個(gè)相位測(cè)量值。根據(jù)這9個(gè)相位值，可分別解算出該從飛行器相對(duì)于領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系的相對(duì)位置與姿態(tài)，從而得到相對(duì)運(yùn)動(dòng)導(dǎo)航參數(shù)。

以某一領(lǐng)航無(wú)人機(jī)的機(jī)體坐標(biāo)系作為導(dǎo)航坐標(biāo)系，將主從式結(jié)構(gòu)設(shè)計(jì)成2種方案：1MA-1FA和3MA-1FA 方案。其中，1MA-1FA 表示一架主飛行器（MA，master aircraft）對(duì)一架從飛行器（FA，follower aircraft）的觀測(cè)模型（見(jiàn)圖2（a）），3MA-1FA 表示3 架主飛行器（MA）對(duì)一架從飛行器（FA）（見(jiàn)圖2（b））。進(jìn)而，根據(jù)主飛行器疏密布局方式布置成兩種情況，以研究疏密分布情況對(duì)定位性能的影響。

圖2 主-從飛行器觀測(cè)示意圖Fig.2 Diagram of master aircraft observing follower aircraft

若領(lǐng)航無(wú)人機(jī)出現(xiàn)故障無(wú)法承擔(dān)電波發(fā)射任務(wù)，則按預(yù)先指定的順序，由下一順位無(wú)人機(jī)接替領(lǐng)航任務(wù)，從而保證導(dǎo)航過(guò)程的連續(xù)性。

1.2 相對(duì)導(dǎo)航模型

1.2.1 基本位置模型以某一領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系作為導(dǎo)航坐標(biāo)系，則可得領(lǐng)航無(wú)人機(jī)機(jī)身上的3 個(gè)天線位置分別為(a，0，0)、(0，0，b)和(0，0，?b)，且其3 個(gè)天線發(fā)射的電磁波波長(zhǎng)分別記為λ1、λ2、λ3。記任意非領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系在導(dǎo)航坐標(biāo)系中的位置為(x，y，z)，且該無(wú)人機(jī)機(jī)體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的姿態(tài)角分別記為方位角ψ、俯仰角?和滾動(dòng)角γ。定義導(dǎo)航坐標(biāo)系至非領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系的旋轉(zhuǎn)矩陣由歐拉角表示，即先繞Y軸旋轉(zhuǎn)方位角ψ，再繞Z軸旋轉(zhuǎn)俯仰角?，最后繞X軸旋轉(zhuǎn)滾動(dòng)角γ計(jì)算。若已知非領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系原點(diǎn)在導(dǎo)航坐標(biāo)系中的相對(duì)位置(x，y，z)，則非領(lǐng)航無(wú)人機(jī)機(jī)背上的3 個(gè)天線在導(dǎo)航坐標(biāo)系中的位置為

式中旋轉(zhuǎn)矩陣C表示領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系到其他無(wú)人機(jī)機(jī)體坐標(biāo)系的轉(zhuǎn)換矩陣

其轉(zhuǎn)置即CT則表示其他無(wú)人機(jī)機(jī)體坐標(biāo)系至領(lǐng)航無(wú)人機(jī)機(jī)體坐標(biāo)系的轉(zhuǎn)換矩陣。

將式（2）代入式（1），得非領(lǐng)航無(wú)人機(jī)3個(gè)天線的位置為

記領(lǐng)航無(wú)人機(jī)上的3 個(gè)天線在其導(dǎo)航坐標(biāo)系中的位置為（xi0，yi0，zi0），其中i表示領(lǐng)航無(wú)人機(jī)上的第i個(gè)天線，且i= 1，2，3，其位置坐標(biāo)為

根據(jù)式（3）與（4），可得領(lǐng)航無(wú)人機(jī)上的第i個(gè)天線與其他無(wú)人機(jī)上第j個(gè)天線之間的距離rij為

式中j表示非領(lǐng)航無(wú)人機(jī)上的第j個(gè)天線，且j= 1，2，3.

記領(lǐng)航無(wú)人機(jī)上的天線i發(fā)出的電波到其他無(wú)人機(jī)上天線j的相位角為θij，則在一個(gè)測(cè)量周期內(nèi)，領(lǐng)航無(wú)人機(jī)上的3 個(gè)天線到非領(lǐng)航無(wú)人機(jī)上的3 個(gè)天線共9 個(gè)相位角可記為（θi1，θi2，θi3）.另外，相位角、波長(zhǎng)與距離之間的關(guān)系為

1.2.2 基本速度模型假設(shè)無(wú)人機(jī)的相位測(cè)量周期為T(mén)，且領(lǐng)航無(wú)人機(jī)上的第i個(gè)天線發(fā)射的電磁波到達(dá)其他無(wú)人機(jī)上的第j個(gè)天線的相位測(cè)量值為θij。若當(dāng)前時(shí)刻記為t0，且當(dāng)前時(shí)刻t0之前的第k個(gè)測(cè)量周期節(jié)點(diǎn)獲得的相位測(cè)量值為θijk，則可通過(guò)選擇合適的測(cè)量周期T來(lái)保證連續(xù)周期節(jié)點(diǎn)間的相位測(cè)量值之差δθijk=（θijk?θij(k+1)），k= 0，1，2，…，N，維持在(?π，π)的范圍內(nèi)。因此，天線間的相對(duì)速度值為

式中N表示當(dāng)前時(shí)刻之前的第N個(gè)測(cè)量周期節(jié)點(diǎn)。

根據(jù)式（3）給出的非領(lǐng)航無(wú)人機(jī)上3 個(gè)天線的位置與自身機(jī)體坐標(biāo)系原點(diǎn)以及姿態(tài)角的關(guān)系表達(dá)式，其兩邊對(duì)時(shí)間求一階導(dǎo)，可得相對(duì)速度與機(jī)體坐標(biāo)系的原點(diǎn)速度以及姿態(tài)角變化率的關(guān)系為

式中C1、C2表示式（3）中相應(yīng)向量對(duì)姿態(tài)角速率的Jacobian矩陣，且

根據(jù)式（5）給出的領(lǐng)航無(wú)人機(jī)上的第i個(gè)天線與非領(lǐng)航無(wú)人機(jī)上的第j個(gè)天線間的距離表達(dá)式，其兩邊對(duì)時(shí)間求導(dǎo)可得相對(duì)速度為

聯(lián)合基本位置方程與基本速度方程，可求解任意非領(lǐng)航無(wú)人機(jī)機(jī)身上3個(gè)天線的位置與速度參數(shù)，進(jìn)而得到無(wú)人機(jī)機(jī)體坐標(biāo)系的位置及其姿態(tài)參數(shù)。

取基本位置速度模型的非線性向量方程為

式中Z表示觀測(cè)向量；h(X)表示非線性向量函數(shù)。

若觀測(cè)量取Z=[ri1ri2ri3r?i1r?i2r?i3]T，則基本位置速度關(guān)系方程為

式中v為觀測(cè)模型噪聲，滿(mǎn)足均值為零的高斯白噪聲序列。

2 擴(kuò)展卡爾曼濾波算法

擴(kuò)展卡爾曼濾波算法根據(jù)狀態(tài)向量最優(yōu)估計(jì)值對(duì)狀態(tài)方程和觀測(cè)方程做泰勒近似并保留一階項(xiàng)，將狀態(tài)方程和觀測(cè)方程線性化處理（秦永元等，2015）。

2.1 狀態(tài)方程

非領(lǐng)航無(wú)人機(jī)在導(dǎo)航坐標(biāo)系中的離散時(shí)間狀態(tài)方程為

式中Xk表示系統(tǒng)狀態(tài)向量，利用非領(lǐng)航無(wú)人機(jī)在k時(shí)刻3 個(gè)天線的位置、速度進(jìn)行描述，即Xk=[x1x2x3x?1x?2x?3]T，其中xj=［xj yj zj］T，x?j=［x?j y?j z?j］T，j= 1，2，3；f[?]表示非線性狀態(tài)向量函數(shù)；wk表示系統(tǒng)模型噪聲，滿(mǎn)足均值為0的高斯白噪聲序列，其統(tǒng)計(jì)特性為

式中Qk表示過(guò)程噪聲方差矩陣；δk，j表示克羅內(nèi)克δ函數(shù)。

假設(shè)已知系統(tǒng)在tk?1時(shí)刻的系統(tǒng)狀態(tài)參數(shù)最優(yōu)估計(jì)值X?k?1和估計(jì)值的協(xié)方差矩陣Pk?1。將式（9）的離散時(shí)間狀態(tài)方程在系統(tǒng)狀態(tài)X?k?1值處進(jìn)行泰勒近似，忽略二階以上的高階項(xiàng)，得

式中X?k?1表示tk?1時(shí)刻非領(lǐng)航無(wú)人機(jī)狀態(tài)參數(shù)的最優(yōu)估計(jì)值。

令

則有

2.2 觀測(cè)方程

與狀態(tài)方程類(lèi)似，擴(kuò)展卡爾曼濾波的非線性觀測(cè)方程是在狀態(tài)方程得到的一步預(yù)測(cè)值X?k|k?1處進(jìn)行泰勒近似，并僅保留線性項(xiàng)，從而將非線性問(wèn)題進(jìn)行線性化處理。

非領(lǐng)航無(wú)人機(jī)在導(dǎo)航坐標(biāo)系中的離散時(shí)間觀測(cè)方程為

式中Zk為系統(tǒng)觀測(cè)向量，利用k時(shí)刻主飛行器對(duì)從飛行器的相位量測(cè)值表示，即Zk=[ri1ri2ri3r?i1r?i2r?i3]T；h[?]表示非線性向量函數(shù)；vk表示系統(tǒng)觀測(cè)噪聲，滿(mǎn)足均值為零的高斯白噪聲序列。

假設(shè)已獲得系統(tǒng)在tk時(shí)刻的狀態(tài)一步預(yù)測(cè)值X?k|k?1和預(yù)測(cè)值的協(xié)方差矩陣Pk|k?1。將式（12）的離散時(shí)間觀測(cè)方程在模型預(yù)測(cè)值X?k|k?1處進(jìn)行泰勒展開(kāi)，僅保留一階線性項(xiàng)，得

式中X?k|k?1為系統(tǒng)狀態(tài)量的一步預(yù)測(cè)值。

令

則有

上述簡(jiǎn)要推導(dǎo)了非線性狀態(tài)方程與非線性觀測(cè)方程進(jìn)行線性化處理的過(guò)程，因?yàn)樯弦粴v元的狀態(tài)最優(yōu)估計(jì)值X?k?1為已知量，所以Uk?1與Yk為非隨機(jī)的確定序列，其對(duì)狀態(tài)預(yù)測(cè)協(xié)方差矩陣和觀測(cè)噪聲不產(chǎn)生影響，則將式（10）代入（11）可得下一歷元狀態(tài)一步預(yù)測(cè)值為

相應(yīng)地，狀態(tài)方程一步預(yù)測(cè)值的協(xié)方差矩陣傳遞表達(dá)式為

根據(jù)卡爾曼濾波的量測(cè)更新階段，k時(shí)刻經(jīng)過(guò)量測(cè)修正的狀態(tài)最優(yōu)估計(jì)值為

式中Kk為卡爾曼增益，表示時(shí)間更新與量測(cè)修正的權(quán)重，其表達(dá)為

相應(yīng)地，量測(cè)更新階段的協(xié)方差矩陣為

綜上所述，擴(kuò)展卡爾曼濾波（EKF）算法的遞推方程為

3 模型求解

3.1 狀態(tài)更新

任意非領(lǐng)航無(wú)人機(jī)3個(gè)天線在導(dǎo)航坐標(biāo)系中的狀態(tài)方程為

式中Φk|k?1表示狀態(tài)轉(zhuǎn)移矩陣，非領(lǐng)航無(wú)人機(jī)的飛行狀態(tài)參數(shù)為

式中I表示單位陣；τ=tk?tk?1表示觀測(cè)周期間隔。

3.2 觀測(cè)更新

3.2.1 1MA-1FA方案1MA-1FA編號(hào)表示一架主飛行器觀測(cè)一架從飛行器的模型。式（8）給出了單架領(lǐng)航無(wú)人機(jī)對(duì)任意非領(lǐng)航無(wú)人機(jī)的觀測(cè)方程，其相應(yīng)的觀測(cè)矩陣為

式中矩陣H內(nèi)的各個(gè)塊矩陣為

式中下標(biāo)i表示主飛行器第i個(gè)天線，下標(biāo)j表示從飛行器第j個(gè)天線。

3.2.2 3MA-1FA 方案3MA-1FA 編號(hào)表示3 架主飛行器觀測(cè)一架從飛行器的模型。由式（8）給出的單架主機(jī)觀測(cè)方程，可進(jìn)一步得到3架主飛行器對(duì)單架從飛行器的觀測(cè)方程為

相應(yīng)地，其觀測(cè)矩陣為

式中Hk表示第k（k= 1，2，3）架主飛行器對(duì)從飛行器的觀測(cè)矩陣，且

式中Hk內(nèi)部的塊矩陣如式（13）所示。

3.3 導(dǎo)航參數(shù)更新

通過(guò)上述方法可估計(jì)出飛行器3 個(gè)天線的位置與速度信息，再根據(jù)天線在機(jī)身上固有的安裝位置關(guān)系，可得到從飛行器機(jī)體坐標(biāo)系的位置與速度信息為

根據(jù)式（3）和式（6），可進(jìn)一步獲得該飛行器在導(dǎo)航坐標(biāo)系中的相對(duì)姿態(tài)角及其速率估值，即

式中滾動(dòng)角γ表達(dá)式中的cij表示式（2）中旋轉(zhuǎn)矩陣C內(nèi)的元素，方位角速率ψ?表達(dá)式中的cij表示式（7）中Jacobian 矩陣C2內(nèi)的元素，此處i、j分別表示矩陣的行列；為避免?解的雙重性，一般取0°≤?<180°；且當(dāng)y>0時(shí)，?符號(hào)取“+”；當(dāng)y=0時(shí)，?取“0°”；當(dāng)y<0時(shí)，?符號(hào)取“?”。

通過(guò)本節(jié)介紹的方法，即可估計(jì)出飛行器位置、速度、姿態(tài)角以及姿態(tài)角速率等相對(duì)導(dǎo)航參數(shù)。

4 仿真驗(yàn)證

4.1 基本數(shù)據(jù)

采用數(shù)值仿真方法對(duì)上述模型進(jìn)行仿真實(shí)驗(yàn)研究。為便于分析本文建立的機(jī)間相對(duì)觀測(cè)模型的可行性，假設(shè)領(lǐng)航無(wú)人機(jī)已經(jīng)處于穩(wěn)定飛行狀態(tài)，非領(lǐng)航無(wú)人機(jī)相對(duì)于領(lǐng)航無(wú)人機(jī)的位置、姿態(tài)變化關(guān)系滿(mǎn)足

式中f= 1/200表示飛行器姿態(tài)的周期變化頻率。

已知每架無(wú)人機(jī)天線參數(shù)為a=1.0 m，b=1.0 m，主飛行器3 個(gè)天線發(fā)射的電磁波波長(zhǎng)分別為λ1=1 m、λ2=2 m、λ3=3 m，頻率取20 Hz。本文定義的導(dǎo)航坐標(biāo)系中，1MA-1FA 方案對(duì)應(yīng)的主飛行器點(diǎn)位為（0，0，0）m；3MA-1FA方案則按主飛行器疏密分布方式布置成兩種情況：

1）取L=10 m，則主飛行器點(diǎn)位依次為（0，0，0）、（0，0，10）和（10，0，5）m；

2）取L=100 m，則主飛行器點(diǎn)位依次取（0，0，0）、（0，0，100）和（100，0，50）m。

4.2 初始條件

取初始時(shí)刻非領(lǐng)航無(wú)人機(jī)的位置狀態(tài)誤差為（-1.335 00，-1.621 60，-1.682 19）m，其速度狀態(tài)誤差為（0.818 60，0.686 10，0.741 50）m/s。相應(yīng)地，位置初始協(xié)方差陣為diag（1.873 06，3.147 71，3.703 52），速度初始協(xié)方差陣為diag（0.699 28，0.460 08，0.505 87）。結(jié)合初始時(shí)刻飛行器姿態(tài)角與天線安裝參數(shù)，則可得3個(gè)天線組成的初始時(shí)刻狀態(tài)向量與協(xié)方差陣。考慮相位量測(cè)值服從高斯分布，其誤差轉(zhuǎn)換至距離觀測(cè)量噪聲為0.15（3σ）m，轉(zhuǎn)換至速度觀測(cè)量噪聲取0.3（3σ）m/s。利用上述初始條件進(jìn)行擴(kuò)展卡爾曼濾波估計(jì)，其結(jié)果如4.3節(jié)所示。

4.3 結(jié)果與分析

根據(jù)上述方法，可得到1MA-1FA 方案和3MA-1FA（L=10 m，L10）方案的位置、速度濾波結(jié)果如圖3和圖4所示，姿態(tài)角及姿態(tài)角速度如圖5和圖6所示。

圖3 位置誤差曲線Fig.3 Position error curves

圖4 速度誤差曲線Fig.4 Velocity error curves

圖5 姿態(tài)角誤差曲線Fig.5 Attitude angle error curves

圖6 姿態(tài)角速率誤差曲線Fig.6 Attitude angular rate error curves

圖3給出了位置估計(jì)誤差變化曲線，由圖可看出，1MA-1FA方案的結(jié)果震蕩較為顯著，誤差在±0.2 m范圍內(nèi)波動(dòng)，而3MA-1FA 方案的結(jié)果波動(dòng)明顯減小，誤差小于0.1 m，且波動(dòng)范圍在±0.05 m 內(nèi)，效果提升了75%，表明相比于單架主機(jī)，利用3架主機(jī)可顯著提高對(duì)從飛行器的定位精度。圖4給出了速度估計(jì)誤差變化曲線。從圖4可看出，兩種方案濾波前期均存在較大誤差，隨著濾波時(shí)間推移，速度估計(jì)誤差逐漸趨于收斂，相比于1MA-1FA 方案，3MA-1FA 方案具有更高的速度估計(jì)精度，而且3 個(gè)方向上的估計(jì)誤差收斂至相同精度等級(jí)的速度更快。

圖5與圖6分別顯示了姿態(tài)角誤差和姿態(tài)角速率誤差變化曲線。由圖5可見(jiàn)，方案1MA-1FA中姿態(tài)角具有較大的誤差，該結(jié)果與圖3中位置誤差相對(duì)應(yīng)，此姿態(tài)角根據(jù)飛行器自身3個(gè)天線構(gòu)成的幾何關(guān)系進(jìn)行求解，因此位置狀態(tài)估計(jì)精度很大程度上決定了姿態(tài)角解算精度，從而導(dǎo)致1MA-1FA 方案中求解姿態(tài)角存在較大誤差。但3MA-1FA 方案中姿態(tài)角誤差顯著減小，表明增加主飛行器數(shù)量，可明顯提高從飛行器位姿估計(jì)精度。由圖6 也可看出，3MA-1FA 方案較1MA-1FA 方案可得到更高的姿態(tài)角速率估計(jì)精度，且收斂速度更快。該姿態(tài)角速率根據(jù)飛行器自身天線存在的三角幾何構(gòu)型，利用位置速度狀態(tài)估計(jì)值進(jìn)行解算，因此與位置速度狀態(tài)估計(jì)精度相關(guān)聯(lián)，結(jié)合3MA-1FA 方案在位置和速度估計(jì)上具有較高的精度，從而求解出的姿態(tài)角速率能收斂到較好的精度范圍。

3MA-1FA方案中，針對(duì)3架主飛行器布置較為分散（L=100 m，L100）的情況，其位置、速度濾波結(jié)果如圖7所示，姿態(tài)角及其速率解算估值如圖8所示。

圖7 位置和速度誤差曲線（3MA-1FA，L=100 m）Fig.7 Position and velocity error curves

圖8 姿態(tài)角和姿態(tài)角速率誤差曲線（3MA-1FA，L=100 m）Fig.8 Attitude angle and attitude angular rate error curves

圖7（a）顯示了位置誤差變化曲線。從圖7（a）可看出，L100 布置方式可獲得更高的濾波估計(jì)精度，位置誤差在±0.01 m 范圍內(nèi)波動(dòng)，其估計(jì)精度較圖3 中L10 方式提高了80%。圖7（b）顯示了速度誤差變化曲線，可明顯看出L100 布置方式的速度收斂得更快，較圖4 中L10 方式提升了約33.3%。圖8（a）和圖8（b）分別給出了姿態(tài)角及其速率估值誤差曲線，從中可看出，L100 布置方式的姿態(tài)角誤差顯著減小，速率估值與L10 相當(dāng)，且姿態(tài)角波動(dòng)范圍較圖5 中L10 更穩(wěn)定，相比于1MA-1FA 方案，效果提升愈加明顯。從L10 與L100 的結(jié)果對(duì)比中，表明了相比于密集分布，分散布置可顯著提高無(wú)人機(jī)集群的導(dǎo)航定位性能。

5 結(jié) 論

針對(duì)無(wú)人機(jī)集群的相對(duì)導(dǎo)航算法通常只進(jìn)行相對(duì)距離或速度測(cè)量，無(wú)法獲得飛行器相對(duì)姿態(tài)信息等問(wèn)題，本文基于載波相位測(cè)量，設(shè)計(jì)了一種具有三角幾何特性的天線構(gòu)型，從中可解算出飛行器姿態(tài)及其速率信息，仿真結(jié)果表明：

1）利用載波相位觀測(cè)，提出主從式結(jié)構(gòu)的機(jī)間觀測(cè)模型，通過(guò)擴(kuò)展卡爾曼濾波可有效估計(jì)出飛行器的位置、速度參數(shù)，表明該方法可行。

2）隨著主飛行器數(shù)量的增加，改善了測(cè)量幾何，位置、速度狀態(tài)估計(jì)精度顯著提升，從而解算出較為準(zhǔn)確的姿態(tài)信息，提供了一種確定飛行器姿態(tài)的方法。

3）主從式結(jié)構(gòu)中，主飛行器分布方式很大程度上影響了狀態(tài)估計(jì)精度，相比于密集分布，較為分散的布置方式可獲得更高的估計(jì)精度，并據(jù)此獲得更高精度的姿態(tài)信息。