無(wú)人機(jī)軟連接吊艙的偽衛(wèi)星定位算法研究

榮鳳娟,張清華,張杰,徐鍵卉

(陸軍工程大學(xué),江蘇 南京 210007)

0 引 言

近年來(lái),多旋翼無(wú)人機(jī)技術(shù)與裝備發(fā)展迅猛,其靈活易操作特性使得許多科研項(xiàng)目選用它們作為載體[1-3],然而,某些科研儀器為了避免電磁干擾和物理震動(dòng),需與無(wú)人機(jī)機(jī)身保持距離,利用吊艙把科研儀器和無(wú)人機(jī)進(jìn)行軟連接是一種較好的選擇[4-5]。通常情況下無(wú)人機(jī)自帶有單頻的衛(wèi)星導(dǎo)航天線,但軟連接情況,承載儀器的吊艙和無(wú)人機(jī)的相對(duì)關(guān)系受外界環(huán)境和風(fēng)力影響,不能保持準(zhǔn)確的相對(duì)位置關(guān)系,故需要對(duì)吊艙進(jìn)行單獨(dú)定位,以得到科研儀器的動(dòng)態(tài)精確位置[6-7]。衛(wèi)星定位為常見(jiàn)的實(shí)時(shí)高精度定位手段,但是在復(fù)雜環(huán)境下,如衛(wèi)星可見(jiàn)性較差的,將無(wú)法定位,偽衛(wèi)星是一種較好的可以靈活布設(shè)發(fā)射基站的定位設(shè)備,GPS作為常見(jiàn)定位手段,但在衛(wèi)星可見(jiàn)性較差情況下無(wú)法定位,而偽衛(wèi)星可以應(yīng)對(duì)此類(lèi)情況[8-9]。本文擬利用2.4 GHz無(wú)線測(cè)距裝置作為偽衛(wèi)星定位設(shè)備,在一定條件約束下對(duì)偽星的空間最優(yōu)布設(shè)效果進(jìn)行仔細(xì)深入的分析,并采用擴(kuò)展卡爾曼濾波來(lái)應(yīng)對(duì)偽衛(wèi)星定位中的非線性問(wèn)題,同時(shí)對(duì)定位結(jié)果與GPS進(jìn)行了比較分析。

本論文的結(jié)構(gòu)安排如下:第二章介紹了偽衛(wèi)星定位設(shè)備的原理;第三章介紹了應(yīng)用中偽衛(wèi)星的定位算法,重點(diǎn)介紹了處理線性問(wèn)題的擴(kuò)展卡爾曼濾波算法,第四章為野外試驗(yàn)分析,包括現(xiàn)場(chǎng)的設(shè)置情況、空間星座分布的評(píng)估結(jié)果以及最重要的定位結(jié)果精度分析;第五章為總結(jié)和結(jié)論,從技術(shù)方法和工程試驗(yàn)效果等方面對(duì)本文的工作進(jìn)行了評(píng)價(jià),并分析了不足和下一步需要改進(jìn)的技術(shù)方法。

1 偽衛(wèi)星定位設(shè)備及定位原理

1.1 硬件組成

項(xiàng)目采用的是某公司提供的偽衛(wèi)星系統(tǒng),該系統(tǒng)的控制系統(tǒng)可以在一個(gè)標(biāo)準(zhǔn)筆記本電腦上流暢運(yùn)行,它包含多個(gè)偽衛(wèi)星發(fā)射裝置(RN),并且具有測(cè)量距離的功能,同時(shí)還具有一個(gè)負(fù)責(zé)通信的終端(GW),測(cè)定兩計(jì)算接收機(jī)(MN)的位置的距離,如圖1所示。

在試驗(yàn)中采用了兩種偽衛(wèi)星發(fā)射終端,其中終端1既可以作為偽衛(wèi)星發(fā)射基站,如圖2(a)所示,又可以作為接收機(jī)使用,終端2只可以作為接收機(jī)使用,如圖2(b)所示。

1.2 定位基本原理

與GPS定位的原理類(lèi)似,該定位系統(tǒng)也是基于空間后方交會(huì)。偽衛(wèi)星單點(diǎn)定位是以偽衛(wèi)星發(fā)射基站和用戶接收機(jī)天線之間的距離觀測(cè)量為基礎(chǔ),并在已知精確偽衛(wèi)星坐標(biāo)的基礎(chǔ)上來(lái)確定接收機(jī)天線所對(duì)應(yīng)的觀測(cè)點(diǎn)的坐標(biāo)(x,y,z)。

則對(duì)于第i個(gè)偽衛(wèi)星,它與接收機(jī)天線之間觀測(cè)得到的距離量為

(1)

式中:Si為偽衛(wèi)星和接收機(jī)之間的距離量,而第i個(gè)偽衛(wèi)星的坐標(biāo)(Xi,Yi,Zi)事先精確已知,則求解接收機(jī)坐標(biāo)(x,y,z)至少需要三個(gè)以上的偽衛(wèi)星。假設(shè)有三臺(tái)以上的偽衛(wèi)星同時(shí)對(duì)接收機(jī)觀測(cè)(n≥3),則有以下線性化后的方程

(2)

(3)

L=Hx+δ,

(4)

1.3 偽衛(wèi)星空間分布評(píng)價(jià)(DOP)

DOP具有測(cè)量和代數(shù)的雙重含義:在測(cè)量中代表GNSS衛(wèi)星的幾何結(jié)構(gòu)所引起的偽距定位誤差的比例,在代數(shù)中反映最小二乘平差權(quán)逆陣的跡特性。

在公式(4)中,設(shè)計(jì)矩陣為

(5)

式中:axi,ayi,azi(i=1,2,…,n)為接收機(jī)位置與偽衛(wèi)星位置之間方向矢量的余弦,假設(shè)P為單位矩陣,則式(9)有如下形式

COV(x)=(HTH)-1σ2.

(6)

則PDOP、HDOP、VDOP可以由如下公式來(lái)表示

(7)

DOP值越小,則估計(jì)值x的協(xié)方差陣就越小,即具有更高的參數(shù)估計(jì)精度。

2 偽衛(wèi)星動(dòng)態(tài)定位的EKF算法

偽衛(wèi)星定位系統(tǒng)與 GNSS 最大的不同點(diǎn)是偽衛(wèi)星發(fā)射器一般距離用戶接收機(jī)相對(duì)較近,這可能給定位過(guò)程帶來(lái)一些有利的影響,例如不用考慮電離層對(duì)偽衛(wèi)星觀測(cè)量的影響,但是同時(shí)也會(huì)帶來(lái)一些不利的影響,例如遠(yuǎn)近效應(yīng)和線性化誤差等。

在用濾波方法消除衛(wèi)星定位數(shù)據(jù)中的隨機(jī)誤差時(shí),可以使用最優(yōu)估計(jì)的方法,將真實(shí)的狀態(tài)(定位結(jié)果)從各種隨機(jī)干擾中實(shí)時(shí)最優(yōu)估計(jì)出來(lái),但應(yīng)用不同濾波器進(jìn)行最優(yōu)估計(jì)(濾波)時(shí),需要建立較準(zhǔn)確的系統(tǒng)模型和觀測(cè)模型,而且要對(duì)各種隨機(jī)誤差進(jìn)行準(zhǔn)確地建模。目前國(guó)內(nèi)外對(duì)將最優(yōu)估計(jì)方法應(yīng)用于濾波方面做了大量的研究,也取得了很多相應(yīng)的成果。

通常最小二乘方法不考慮觀測(cè)信號(hào)的統(tǒng)計(jì)特性,僅僅保證測(cè)量誤差的方差最小。同時(shí),最小二乘法的迭代僅僅考慮了當(dāng)前的信號(hào)狀況,沒(méi)有涉及前面的信號(hào)狀況。因此在偽衛(wèi)星定位的過(guò)程中,對(duì)定位結(jié)果計(jì)算的穩(wěn)定性估計(jì)不足。

設(shè)動(dòng)態(tài)系統(tǒng)的狀態(tài)方程和測(cè)量方程分別為:

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

(8)

Zk=HkXk+Vk.

(9)

為了減少線性化誤差,學(xué)者們使用擴(kuò)展卡爾曼濾波(EKF)。在考慮了泰勒展開(kāi)的二階項(xiàng)的條件下,二階擴(kuò)展卡爾曼濾波能使濾波性能得到改善。但是由于要同時(shí)計(jì)算雅克比矩陣和漢森矩陣,使得濾波計(jì)算量和不穩(wěn)定性都大大增加。

EKF基于模型和觀測(cè)模型統(tǒng)計(jì)信息,提供系統(tǒng)狀態(tài)最小方差估計(jì)。狀態(tài)方程和測(cè)量方程在其前一估計(jì)處進(jìn)行線性化處理:

(10)

從而將非線性系統(tǒng)轉(zhuǎn)換成線性離散系統(tǒng)形式,再利用線性卡爾曼濾波器進(jìn)行估計(jì)。其算法具體實(shí)現(xiàn)如下[12]:

時(shí)間更新:

(11)

測(cè)量更新:

(12)

其中,Kk被稱(chēng)作卡爾曼濾波增益

(13)

雖然時(shí)間更新和測(cè)量更新中狀態(tài)分別用非線性方程表示,但協(xié)方差陣使用線性化的方程進(jìn)行傳播。

3 野外試驗(yàn)分析

3.1 現(xiàn)場(chǎng)設(shè)置

為了對(duì)本試驗(yàn)中的六旋翼無(wú)人機(jī)軟連接吊艙進(jìn)行動(dòng)態(tài)定位分析,本文的研究人員選取了位于南京湯山地區(qū)的一塊平坦區(qū)域進(jìn)行場(chǎng)地試驗(yàn)(如圖3所示)。首先在場(chǎng)地的周?chē)茉O(shè)6個(gè)高度約15 m的鐵塔,用于安裝偽衛(wèi)星基站的天線,而基站的初始位置由全站儀進(jìn)行確定,其平面位置如圖4所示,具體的基站坐標(biāo)如表1所示。

編號(hào)X/mY/mZ/m 1420129.816203.62580.795 1542119.455235.32676.42 1611158.838227.54878.962 1614148.813237.0678.073 1615132.364243.76479.768 1616114.442204.80472.123

為了對(duì)偽衛(wèi)星定位的效果進(jìn)行分析,試驗(yàn)采用GPS RTK 技術(shù)與偽衛(wèi)星定位結(jié)果進(jìn)行比對(duì),以驗(yàn)證偽衛(wèi)星定位的絕對(duì)精度。試驗(yàn)中將偽衛(wèi)星觀測(cè)站(OS)與GPS RTK天線采用捆綁的方式固定在一起,并利用軟連接與無(wú)人機(jī)進(jìn)行連接(如圖5所示)。而圖6為正在進(jìn)行的動(dòng)態(tài)定位試驗(yàn),無(wú)人機(jī)飛行的范圍在由鐵塔圍成的多邊形區(qū)域內(nèi)部。

3.2 空間星座構(gòu)型

根據(jù)基站布設(shè)的位置,利用2.3節(jié)中的DOP值理論對(duì)試驗(yàn)場(chǎng)內(nèi)的基站空間分布情況進(jìn)行評(píng)價(jià)。其中圖7(a)的淺色線條為無(wú)人機(jī)的飛行軌跡。圖7(b)、7(c)和7(d)分別為PDOP、HDOP和VDOP的分布情況,可以發(fā)現(xiàn),無(wú)人機(jī)的飛行區(qū)域基本位于DOP值較為理想的區(qū)域,非常有利于試驗(yàn)的開(kāi)展。

3.3 定位結(jié)果分析

利用EKF算法對(duì)無(wú)人機(jī)軟連接下的偽衛(wèi)星觀測(cè)站進(jìn)行了動(dòng)態(tài)定位,同時(shí)利用GPS RTK得到共位坐標(biāo)。圖8(a)～圖8(d)分別表示了偽衛(wèi)星和GPS定位三維和X、Y及Z方向的定位結(jié)果比較,通過(guò)比較可以發(fā)現(xiàn),在絕大多數(shù)的時(shí)間內(nèi),二者定位的結(jié)果都比較吻合(如圖8(a)所示),但在圖8(b)的30～40 s的時(shí)間段內(nèi)(灰色區(qū)域),偽衛(wèi)星和GPSR TK的定位結(jié)果出現(xiàn)了較大的差異,此差異可能是由于無(wú)人機(jī)在X方向機(jī)動(dòng)較快,而造成了偽衛(wèi)星定位精度的降低。

為了進(jìn)一步分析二者的差異,對(duì)偽衛(wèi)星和GPS的定位結(jié)果進(jìn)行差分比較,通過(guò)分析圖9可以發(fā)現(xiàn),偽衛(wèi)星在X方向的定位結(jié)果比Y和Z方向略差,X方向是無(wú)人機(jī)行進(jìn)過(guò)程的主要方向,相當(dāng)于徑向,其機(jī)動(dòng)性最強(qiáng)。而且在X方向還存在數(shù)據(jù)跳變(Data jitter)的情況,這可能是偽衛(wèi)星基站的不穩(wěn)定造成的。對(duì)于誤差分布,在X方向一定時(shí)間段內(nèi)還存在著明顯的誤差漂移(Error drift),這可能是基站在X方向測(cè)距存在著累積誤差。對(duì)于精度更好的Y和Z方向,其在無(wú)人機(jī)的起飛和降落階段,同樣存在著較大的誤差(Larger errors),這可能是無(wú)人機(jī)初始運(yùn)行的時(shí)候,機(jī)動(dòng)性能較強(qiáng)且存在無(wú)固定規(guī)律的抖動(dòng)造成的。

以GPS RTK測(cè)量結(jié)果為真值,對(duì)偽衛(wèi)星定位誤差絕對(duì)值的統(tǒng)計(jì)結(jié)果見(jiàn)表2.其中,X方向的最大誤差為0.5 m左右,Y和Z方向的最大誤差為0.2 m左右,三維方向的最大誤差為0.5 m左右。三維平均誤差優(yōu)于0.2 m,標(biāo)準(zhǔn)差為0.15 m左右。偽衛(wèi)星的定位精度基本可以滿足未爆彈探測(cè)定位無(wú)人機(jī)對(duì)吊艙定位的需求。

表2 定位誤差絕對(duì)值統(tǒng)計(jì)

4 結(jié)束語(yǔ)

針對(duì)六旋翼無(wú)人機(jī)軟連接吊艙的動(dòng)態(tài)定位問(wèn)題,論文在野外試驗(yàn)場(chǎng)地搭建了偽衛(wèi)星定位系統(tǒng),并利用EKF算法對(duì)吊艙的實(shí)時(shí)位置進(jìn)行了估計(jì)。同時(shí),試驗(yàn)中采用了GPS RTK定位的結(jié)果對(duì)偽衛(wèi)星定位結(jié)果進(jìn)行了評(píng)估,結(jié)果表明,采用此偽衛(wèi)星系統(tǒng)及EKF算法可以在三維方向達(dá)到5 cm左右的精度,完全可以滿足復(fù)雜條件下的無(wú)人機(jī)吊艙定位,可以很好地運(yùn)用到實(shí)際工程中,為無(wú)人機(jī)定位提供了新的手段和方法,具有重要的意義。

但是,在無(wú)人機(jī)起降及機(jī)動(dòng)性強(qiáng)的時(shí)間段,偽衛(wèi)星的定位都存在較大偏差,需要在以后的研究中通過(guò)物理模型和數(shù)學(xué)加以改進(jìn)。此外,偽衛(wèi)星基站的持續(xù)穩(wěn)定供電,也是下一步需要著重研究的工程化問(wèn)題。