基于擴(kuò)展卡爾曼濾波的無人機(jī)輔助定位研究

程 擎, 李怡恒, 魯合德

(中國(guó)民用航空飛行學(xué)院,四川 廣漢 618000)

0 引言

隨著低空空域的有序開放,無人機(jī)數(shù)量呈現(xiàn)爆發(fā)性增加,由于其具有體積小、機(jī)動(dòng)性好、成本低的優(yōu)勢(shì),目前被廣泛應(yīng)用于電路巡檢、物流配送、城市交通管理等行業(yè)[1]。

精準(zhǔn)定位是確保無人機(jī)能夠進(jìn)行安全工作的基礎(chǔ),目前針對(duì)無人機(jī)的主要定位技術(shù)是以全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)為主,以激光、高精度傳感器等獲取相應(yīng)的信號(hào)特征參數(shù)為輔,通過算法融合實(shí)現(xiàn)組合導(dǎo)航,計(jì)算出無人機(jī)的位置[2]。但是在城市復(fù)雜的電磁環(huán)境下,GNSS定位信號(hào)極易受到非視距和多徑效應(yīng)的影響,無人機(jī)難以接收到有效數(shù)量的GNSS信號(hào),單純依靠無人機(jī)機(jī)載傳感器獲取定位信息可能會(huì)導(dǎo)致定位不準(zhǔn)確[3]。為解決GNSS信號(hào)弱帶來的定位不準(zhǔn)的問題,文獻(xiàn)[4]提出一種基于力傳感器的系留無人機(jī)定位方法,該方法通過繩纜的狀態(tài)建立系留無人機(jī)懸鏈線模型,以此估計(jì)無人機(jī)的空間位置,將定位誤差縮減至10.7 cm,但是這種定位方法限制了無人機(jī)的活動(dòng)范圍,在復(fù)雜空間環(huán)境下難以使用;文獻(xiàn)[5]從因子圖出發(fā),結(jié)合光束法平差將視覺里程計(jì)和基準(zhǔn)影像匹配相融合,在缺少GNSS信號(hào)的情況下,提高了無人機(jī)定位精度;文獻(xiàn)[6]通過無人機(jī)搭載的相機(jī)和高度計(jì)作為傳感器,借助語義分割提取建筑物關(guān)鍵信息,再通過與衛(wèi)星圖像配準(zhǔn)的方式獲得無人機(jī)的位置信息,雖然這兩種定位方法使得無人機(jī)實(shí)現(xiàn)了自主的絕對(duì)定位,但是對(duì)無人機(jī)的飛行高度有要求,在超低空空域下不能保證定位精度。

針對(duì)以上存在的問題,本文依托擴(kuò)展卡爾曼濾波算法,提出一種無人機(jī)在缺少有效GNSS定位信號(hào)的情況下,將慣性導(dǎo)航系統(tǒng)與5G定位技術(shù)相融合,從而實(shí)現(xiàn)無人機(jī)較高的定位精度。5G技術(shù)具有高速率、低延時(shí)的特點(diǎn),大帶寬使得能夠測(cè)量到的距離變小,多輸入多輸出(MIMO)技術(shù)提供了更多的空間自由度,大規(guī)模、高密度鋪設(shè)的基站也提高了5G定位的優(yōu)勢(shì)[7]。該方法以慣性導(dǎo)航為核心,通過卡爾曼濾波算法將兩種數(shù)據(jù)相融合,融合后的數(shù)據(jù)反饋于慣性導(dǎo)航系統(tǒng),并對(duì)其做出修正[8]。仿真實(shí)驗(yàn)結(jié)果表明,在視距范圍內(nèi)該方法對(duì)長(zhǎng)時(shí)間下慣性測(cè)量單元的測(cè)量精度發(fā)散有一定的抑制效果,保障了無人機(jī)在缺乏有效GNSS信號(hào)時(shí)定位的準(zhǔn)確性。

1 定位原理

無線電定位技術(shù)是通過測(cè)量目標(biāo)與接收機(jī)之間電磁波信號(hào)的特征參數(shù),利用具體的算法求解,從而達(dá)到確定待測(cè)目標(biāo)位置的一種方法。傳統(tǒng)的無線電定位技術(shù)可以通過測(cè)量信號(hào)到達(dá)時(shí)間(Time Of Arrival,TOA),到達(dá)時(shí)間差(Time Difference Of Arrival,TDOA),到達(dá)頻率差(Frequency Difference Of Arrival,FDOA)以及接收信號(hào)強(qiáng)度(Received-Signal-Strength,RSS)等參數(shù)經(jīng)過計(jì)算獲得未知目標(biāo)的位置信息[9]。目前主流定位方法包括藍(lán)牙定位、無線局域網(wǎng)絡(luò)定位、超寬帶(Ultra-Wideband,UWB)定位,RFID定位等,但是受限于技術(shù)或者成本問題難以規(guī)模化、大范圍地部署使用,不能保證在無人機(jī)缺少有效GNSS信號(hào)的情況下提供定位服務(wù)。

1.1 基于5G基站的定位模型

基于5G基站的TDOA定位如圖1所示。圖中:r為各個(gè)基站相對(duì)于無人機(jī)的距離;t為信號(hào)到達(dá)各個(gè)基站的傳播時(shí)間。

TDOA是一種基于無線電信號(hào)到達(dá)時(shí)間差的定位方法,該方法必須滿足對(duì)3個(gè)或3個(gè)以上的基站同時(shí)測(cè)量,相對(duì)于TOA,不要求無人機(jī)與基站之間存在高度的時(shí)間同步[10]。將基站與無人機(jī)之間的時(shí)間差轉(zhuǎn)化為距離差d,建立幾何關(guān)系方程組求解,即

(1)

式中,c為電磁波傳播速度。

假設(shè)無人機(jī)的位置為(x,y,z),(xn,yn,zn)代表已知各個(gè)基站的坐標(biāo)(n=1,2,3,4),則

(2)

1.2 基于Chan算法的三維定位

Chan算法是一種基于TDOA的非遞歸雙曲線方程組解法[11],在基站數(shù)量較多且測(cè)量誤差服從高斯分布時(shí)具有定位精度高、計(jì)算量小的特點(diǎn),該算法不需要初始值,在視距環(huán)境下定位精度可達(dá)到克拉美羅下界。

無人機(jī)到達(dá)各個(gè)基站的距離差為

(3)

由式(1)可知

(4)

聯(lián)合式(3)～(4),可得

dn,1+2dn,1r1=-2xn,1x-2yn,1y-2zn,1z+Ln-L1

(5)

當(dāng)基站數(shù)量為4時(shí),可假設(shè)r1為已知,則解得無人機(jī)位置為

(6)

當(dāng)定位基站數(shù)量大于4時(shí),需要經(jīng)過兩次迭代,第1次迭代用加權(quán)最小二乘法得到初始解,第2次迭代時(shí)先將非線性方程組轉(zhuǎn)變?yōu)榫€性方程組,利用第1次迭代得到的初始解進(jìn)行第2次加權(quán)最小二乘法估計(jì)得到無人機(jī)的最終位置。

由式(5)建立基于TDOA誤差矢量ε的線性方程

(7)

引入噪聲后

(8)

忽略式(8)右側(cè)二階項(xiàng),則誤差矢量ε的協(xié)方差矩陣可表示為

ψ=E(εεT)=c2BQB

(9)

式中,Q為服從高斯分布的噪聲協(xié)方差矩陣。

進(jìn)行第一次加權(quán)最小二乘法可得到一個(gè)初始解

(10)

對(duì)式(10)得到的初始解進(jìn)行重新計(jì)算可得到第1次無人機(jī)位置的估計(jì)值z(mì)a,為獲得更加準(zhǔn)確的位置估計(jì),進(jìn)行第2次加權(quán)最小二乘法,引入噪聲后

(11)

(12)

(13)

(14)

構(gòu)造新的線性方程組

(15)

根據(jù)第1次加權(quán)最小二乘法的計(jì)算過程可得

ψ′=E(ε′ε′T)=4Bcov(ρa(bǔ))B′

(16)

因此

(17)

則最終目標(biāo)無人機(jī)的位置為

(18)

1.3 慣性導(dǎo)航定位及姿態(tài)更新

慣性測(cè)量單元(IMU)主要由陀螺儀和加速度計(jì)組成,部分慣性導(dǎo)航系統(tǒng)為了提高測(cè)量精度,還加裝有磁傳感器和氣壓計(jì),通過陀螺儀和加速度計(jì)獲得無人機(jī)的角速度和加速度等信息,由慣性導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航解算得到無人機(jī)的位置、姿態(tài)和速度。常用的無人機(jī)姿態(tài)解算方法有歐拉角法、方向余弦法和四元數(shù)法。四元數(shù)法相對(duì)于其他兩種方法計(jì)算量小,且易于實(shí)現(xiàn)。

載體坐標(biāo)系(b系)到導(dǎo)航坐標(biāo)系(n系)的旋轉(zhuǎn)矩陣為

(19)

得到由四元數(shù)表示的無人機(jī)姿態(tài)信息如下

(20)

式中,φ,θ,ψ分別為無人機(jī)的橫滾角,俯仰角和航向角。

相應(yīng)地,在位置更新過程中,由于位置矩陣相對(duì)姿態(tài)矩陣變化較慢,且不需要正交化處理,可采用方向余弦法來即時(shí)修正。由慣性導(dǎo)航基本方程對(duì)速度的即時(shí)修正如下

(21)

2 基于擴(kuò)展卡爾曼濾波的數(shù)據(jù)融合

2.1 數(shù)據(jù)融合模型

為提高無人機(jī)的定位精度,利用擴(kuò)展卡爾曼濾波算法對(duì)5G和慣性導(dǎo)航進(jìn)行數(shù)據(jù)融合,采用5G/IMU松組合的方式[12],以慣性測(cè)量單元所測(cè)量的數(shù)據(jù)為參考,將經(jīng)過零偏補(bǔ)償后的數(shù)據(jù)通過慣性導(dǎo)航系統(tǒng)進(jìn)行解算,得到無人機(jī)的位置、速度和姿態(tài),同時(shí)通過5G基站利用TDOA計(jì)算出無人機(jī)的位置信息,將基站定位得到的位置結(jié)果與慣性導(dǎo)航輸出的差值通過卡爾曼濾波器,實(shí)現(xiàn)對(duì)IMU的零偏誤差和慣性導(dǎo)航解算中的位置、速度、姿態(tài)誤差的估計(jì)。5G/IMU數(shù)據(jù)融合框圖如圖2所示。

圖2 5G/IMU數(shù)據(jù)融合框圖

2.2 擴(kuò)展卡爾曼濾波

考慮到實(shí)際工程應(yīng)用中大部分系統(tǒng)為非線性高斯系統(tǒng),采用擴(kuò)展卡爾曼濾波算法將非線性系統(tǒng)局部線性化,以IMU獲取無人機(jī)的速度、姿態(tài)和位置,以5G基站定位獲取無人機(jī)的實(shí)時(shí)坐標(biāo),通過擴(kuò)展卡爾曼濾波進(jìn)行數(shù)據(jù)處理,建立數(shù)學(xué)模型。

以無人機(jī)的位置坐標(biāo)和速度作為狀態(tài)變量,離散化后的狀態(tài)方程為

Xk+1=f[Xk,k]+Wk

(22)

式中:f為狀態(tài)轉(zhuǎn)移矩陣;Wk為互不相關(guān)均值為零的噪聲序列;Xk+1和Xk為某一時(shí)刻的狀態(tài)向量。

以慣性測(cè)量單元獲取的速度和姿態(tài)角數(shù)據(jù)與5G基站進(jìn)行解算得到的位置數(shù)據(jù)作為量測(cè)信息,建立觀測(cè)方程

Zk+1=h[Xk+1,k+1]+Vk+1

(23)

式中:Vk+1為互不相關(guān)均值為零的噪聲序列;Zk+1為某一時(shí)刻的觀測(cè)向量。

(24)

(25)

取無人機(jī)初始位置坐標(biāo)、速度和姿態(tài)為狀態(tài)向量初始值,由標(biāo)準(zhǔn)卡爾曼濾波,得到擴(kuò)展卡爾曼濾波方程如下,整個(gè)濾波過程可分為預(yù)測(cè)階段和更新階段。

狀態(tài)一步預(yù)測(cè)為

(26)

一步預(yù)測(cè)方差為

(27)

濾波增益為

(28)

狀態(tài)估計(jì)為

(29)

估計(jì)均方差為

(30)

其中:Q為過程噪聲協(xié)方差矩陣;R為測(cè)量噪聲協(xié)方差矩陣;I為單位矩陣。

3 仿真驗(yàn)證

為驗(yàn)證該定位技術(shù)的可靠性進(jìn)行仿真分析,實(shí)驗(yàn)設(shè)定陀螺儀漂移為0.01 (°)/h,加速度計(jì)零偏為5×10-5g,加速度取9.8 m/s2,仿真環(huán)境設(shè)定為視距環(huán)境,無人機(jī)在三維空間下保持勻速,5G定位信號(hào)通過添加噪聲模擬生成,模擬時(shí)間為300 s,實(shí)驗(yàn)結(jié)果如圖3和圖4所示。

圖3 使用IMU時(shí)姿態(tài)誤差

圖4 5G/IMU融合后姿態(tài)誤差

由圖3、圖4可知,擴(kuò)展卡爾曼濾波對(duì)慣性平臺(tái)上無人機(jī)姿態(tài)角的發(fā)散起到了一定的抑制效果,前300 s內(nèi)姿態(tài)誤差均保持在1°以內(nèi)。

圖5所示為5G/IMU融合后在東向和北向上的位置誤差曲線。

從圖5中可以看出,東向和北向位置誤差在零值附近波動(dòng),最高誤差不超過20 cm,且集中分布在10 cm左右的區(qū)間內(nèi)。

圖6和圖7分別為在理想環(huán)境下5G/IMU融合后的二維位置軌跡和三維位置軌跡與實(shí)際軌跡的比較。

從圖6、圖7中可看到,使用擴(kuò)展卡爾曼濾波對(duì)兩種定位結(jié)果進(jìn)行融合后具有較高的定位精度,融合后的軌跡與真實(shí)軌跡相近。在三維位置軌跡中,無人機(jī)經(jīng)5G/IMU融合后的位置相比于單獨(dú)使用5G定位,最大位置誤差由75 cm降低為36 cm,平均位置誤差由33 cm降為16.6 cm,融合后的平均位置誤差降低了49.7%,有效提高了無人機(jī)的定位精度。

4 結(jié)束語

本文從無人機(jī)定位原理出發(fā),提出了一種在缺少有效GNSS信號(hào)的情況下,利用擴(kuò)展卡爾曼濾波算法對(duì)5G定位與慣性導(dǎo)航進(jìn)行融合的定位方法,該方法以慣性導(dǎo)航系統(tǒng)為核心,將融合后的數(shù)據(jù)對(duì)慣性導(dǎo)航系統(tǒng)的解算進(jìn)行修正,從而使無人機(jī)具有較高的定位精度。軟件仿真結(jié)果表明:5G/IMU組合定位在視距范圍內(nèi)能夠有效抑制慣性測(cè)量元件的測(cè)量精度隨著時(shí)間增加而發(fā)散的現(xiàn)象,將位置誤差和姿態(tài)角誤差控制在一定范圍內(nèi);在三維空間下組合定位的平均位置誤差相對(duì)于單獨(dú)使用5G定位降低了49.7%,達(dá)到了16.6 cm;定位可靠性較高,在不依賴GNSS信號(hào)定位的情況下使得無人機(jī)具有長(zhǎng)時(shí)間較高定位精度的能力,具有一定的工程實(shí)用價(jià)值。