干擾環(huán)境下船舶的定位算法研究

馬社祥， 李慧寧

(天津理工大學(xué)電氣電子工程學(xué)院，天津 300384)

1 引言

全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)接收導(dǎo)航衛(wèi)星發(fā)送的導(dǎo)航定位信號，并以導(dǎo)航衛(wèi)星為動態(tài)已知點，實時測定運(yùn)動載體在航位置和速度，進(jìn)而完成導(dǎo)航。隨著GNSS的發(fā)展，可供測量的衛(wèi)星越來越多。在未來的幾年里，來自四個GNSS系統(tǒng)(GPS、伽利略、北斗、GLONASS)的大約120顆衛(wèi)星將被使用，這可以減少確定正確位置的時間。正常的情況下，GNSS需要同時接收到四顆及以上衛(wèi)星進(jìn)行定位，但在干擾環(huán)境下，某些時刻只能接收到單顆衛(wèi)星的信號，短時間內(nèi)船舶定位精確度下降，威脅航行的安全。因此，干擾環(huán)境下船舶的定位算法研究具有重大價值。

常用的測量方法有到達(dá)時間定位(TOA)、到達(dá)時間差定位(TDOA)、到達(dá)角交會定位(AOA)等，其中偽距定位的優(yōu)點是數(shù)據(jù)處理簡單，對定位條件的要求低，不存在整周模糊度的問題，可以非常容易地實現(xiàn)實時定位。傳統(tǒng)的偽距定位算法采用的是最小二乘(least，LS)算法、高斯牛頓迭代最小二乘(Gauss-Newton iterative least square，GNILS)算法、Kalman濾波算法等。本文提出了將GNILS算法、Kalman算法、列文伯格-馬夸爾特(Levenberg-Marquardt，L-M)算法相結(jié)合的方法，解算出在干擾環(huán)境下船舶的位置坐標(biāo)，保證船舶的正常航行。GNILS算法計算速度快，用該算法的結(jié)果作為初始值代入到Kalman算法，使用列文伯格-馬夸爾特算法優(yōu)化卡爾曼濾波算法的預(yù)測協(xié)方差矩陣。通過仿真驗證，該算法可以在接收的單顆衛(wèi)星的數(shù)據(jù)時進(jìn)行定位，雖然增加了計算量，但可以加快收斂的速度，提高偽距定位精度。

2 系統(tǒng)數(shù)學(xué)模型

偽距是指衛(wèi)星定位過程中，地面接收機(jī)到衛(wèi)星之間的大概距離，衛(wèi)星時鐘和接收機(jī)時鐘不可能完全同步，存在時鐘差，在實際的傳播過程中，還有電離層、對流層、多徑效應(yīng)等產(chǎn)生的影響。在偽距定位中主要的誤差來源為衛(wèi)星和接收機(jī)的鐘差，其它因素可在衛(wèi)星星歷中計算得出，用后綴為n文件讀取衛(wèi)星的導(dǎo)航信息，用后綴為o文件讀取觀測的數(shù)據(jù)，即可得到偽距信息。

如圖1所示，假設(shè)某一時刻只能接收到一顆衛(wèi)星的信號，船舶在持續(xù)接收一段時間后，即接收到四個及以上時刻的衛(wèi)星位置信息，應(yīng)用偽距定位模型，便可以解算出船舶的位置。忽略電離層等產(chǎn)生的影響，衛(wèi)星對接收的第i時刻信號的偽距可表示為

(1)

(2)

其中，

ρ

為第

i

時刻衛(wèi)星天線到船舶天線的偽距值，可由衛(wèi)星星歷得出；(

x

，，

y

，，

z

，)為衛(wèi)星在第

i

時刻的位置矢量，可以通過衛(wèi)星星歷計算得出；(

x

，

y

，

z

)表示船舶在地心地固坐標(biāo)下的位置矢量，為待解量；

c

為光速；

δ

表示接收機(jī)的鐘差，為未知量。

圖1 偽距定位示意圖

3 基于偽距定位的解算模型

傳統(tǒng)的解算算法為最小二乘算法，該算法可以簡便地求得未知數(shù)據(jù)，并使求得數(shù)據(jù)與實際數(shù)據(jù)之間誤差的平方和最小。高斯牛頓迭代算法在最小二乘的基礎(chǔ)上進(jìn)行迭代，設(shè)置門限值，完成對船舶的定位解算。Kalman算法不僅和當(dāng)前時刻相關(guān)，還和前一時刻有關(guān)，不斷更新濾波增益的值，從而得到最優(yōu)值。

3.1 改進(jìn)高斯牛頓迭代最小二乘法解算

由于高斯牛頓迭代最小二乘算法是線性系統(tǒng)下的狀態(tài)估計，而偽距方程是非線性的。首先要設(shè)置一個初始值

X

=(

x

，

y

，

z

，

δ

)，在該初始值進(jìn)行泰勒級數(shù)展開，如式(2)所示

其中

(3)

(4)

(5)

(6)

(7)

(8)

(9)

定義變量，如上式(8)、(9)、(10)所示，化簡可得

Δ

ρ

=

H

·Δ

x

(10)

在當(dāng)前初始值下，

LS

算法解算后的結(jié)果為Δ

x

=(

H

·

H

)·

H

·Δ

ρ

(11)

根據(jù)Δ

x

修正初始值，得到

LS

的最終結(jié)果為

x

=

x

+Δ

x

(12)

(13)

x

=

x

-1+Δ

x

-1

(14)

高斯牛頓迭代算法的缺點是，當(dāng)初始和真實值相差太大時，無法忽略二次項的影響，收斂速度慢，甚至?xí)霈F(xiàn)無法收斂的問題。針對此問題，提出了用

L

-

M

算法進(jìn)行改進(jìn)，如下式所示

(15)

在高斯牛頓算法的基礎(chǔ)上，引入阻尼因子，L-M算法結(jié)合了高斯牛頓法和梯度下降法的特點。在整個計算過程中，要保證μ>0，確保迭代的下降方向。當(dāng)μ較大時，L-M算法即為梯度下降法；當(dāng)μ較小時，L-M算法即為高斯牛頓法。該算法結(jié)合兩者的優(yōu)點，在遠(yuǎn)離實際值時用梯度下降法，下降速度快，在靠近實際值時用高斯牛頓法，避免了初始值誤差過大。

3.2 改進(jìn)卡爾曼濾波算法

初始值的選取會影響Kalman算法收斂時長以及精確度，初始值越接近真實值，Kalman濾波的效果越好。傳統(tǒng)上用網(wǎng)格搜索的方式確定出最優(yōu)位置網(wǎng)格點作為初始值，其迭代次數(shù)較多，本文使用GNILS算法解算出來的最優(yōu)值作為初始值，GNILS算法較傳統(tǒng)的方法有精度高，收斂次數(shù)少等優(yōu)點。

Kalman算法求解的原則是使?fàn)顟B(tài)誤差的方差達(dá)到最小，由于該算法也是線性系統(tǒng)下的狀態(tài)估計，需要將偽距方程進(jìn)行線性化處理，即進(jìn)行泰勒級數(shù)展開，如上式(2)所示。首先給定初始值，狀態(tài)變量

X

=(

x

，

y

，

z

，

δt

)以及協(xié)方差初始值

P

。系統(tǒng)的狀態(tài)方程為

X

=

Φ

，-1·

X

-1+

Γ

-1·

W

-1

(16)

其中，

X

，

X

-1分別為第

k

時刻和第

k

-1時刻的系統(tǒng)狀態(tài)向量，

Φ

，-1為

t

-1時刻至

t

時刻的一步轉(zhuǎn)移矩陣，根據(jù)船舶的運(yùn)行軌跡確定矩陣。

Γ

-1為系統(tǒng)噪聲驅(qū)動陣，

W

-1為系統(tǒng)的噪聲。

量測方程定義為

Z

=

H

·

X

+

V

(17)

其中，

Z

為觀測向量，即偽距；

H

為觀測矩陣，

V

為系統(tǒng)噪聲。

W

，

V

為相互獨立均值為零的高斯白噪聲。

濾波計算回路

(18)

(19)

增益計算回路

(20)

(21)

P

=(

I

-

K

·

H

)·

P

-1

(22)

上式為離散型卡爾曼濾波的基本方程，卡爾曼濾波具有兩個明顯的信息更新過程：時間更新過程和量測更新過程。隨著歷元數(shù)目的增加，濾波增益值會逐漸減小，新接收到的觀測數(shù)據(jù)對定位結(jié)果的影響逐漸降低，導(dǎo)致結(jié)果的發(fā)散。針對此問題，本問題出了用L-M算法改進(jìn)Kalman濾波算法，增加阻尼因子μ，確保迭代的下降方向，解決了協(xié)方差的估計偏低的問題。用L-M算法調(diào)整Kalman算法的預(yù)測協(xié)方差矩陣為

P

-1=(

I

-

P

-1·(

P

-1+

μ

·

I

))·

P

-1

(23)

4 仿真分析

本文采用MATLAB進(jìn)行仿真，實驗數(shù)據(jù)采用的是2019年7月19日的GPS的衛(wèi)星星歷。同一時刻，會接收四顆以上的衛(wèi)星的信號，選取第一時刻高度角最大的衛(wèi)星，來進(jìn)行本次仿真。整個仿真的步驟如圖2所示。

圖2 歷元解算示意圖

其計算船舶坐標(biāo)，步驟總結(jié)如下：

1) 解析衛(wèi)星星歷，可得到衛(wèi)星坐標(biāo)，偽距等信息，列出偽距方程。

2) 設(shè)置初始值為0，用改進(jìn)GNILS算法解算出的位置，待其誤差穩(wěn)定后，將其結(jié)果作為初始值代入到Kalman算法。

3) 采用改進(jìn)型Kalman算法對船舶的位置進(jìn)行狀態(tài)估計，用L-M算法對協(xié)方差矩陣改進(jìn)，得出最終船舶的位置。

圖3 GNILS定位精度

從圖中可以看出，在單星的情況下，GNILS算法解算出船舶的位置。在歷元數(shù)目較少時，衛(wèi)星的的位置空間幾何分布差，導(dǎo)致定位誤差大，但隨著歷元數(shù)目的增大，定位的結(jié)果趨近于穩(wěn)定。

圖4 改進(jìn)GNILS算法定位精度

仿真共采用400歷元，每個歷元之間間隔30秒，由于初始值設(shè)置為0，導(dǎo)致前30個歷元的誤差較大。圖3和圖4比較可以看出，改進(jìn)后的算法，定位的精度優(yōu)于原始算法，在400歷元的解算中，三維坐標(biāo)的精度分別提高了45.3%、62.6%、77.3%。當(dāng)截掉前30歷元，三維坐標(biāo)的精度分別提高17.6%、51.2、21.5%。

從圖5可以看出，將改進(jìn)后的GNILS值作為Kalman算法的初始值，有效地改善了前30 歷元的定位精度。

圖5 Kalman算法定位精度

圖6 改進(jìn)Kalman算法定位精度

比較圖5和圖6，標(biāo)的殘差分別提高了60.4%、71.7%、79.1%，而且平滑性更好。該算法在干擾環(huán)境中可以大致定位出船舶的位置，保證船舶的安全行駛。

表1中的dx、dy、dz分別為解算出來船舶的位置與準(zhǔn)確位置之間的誤差。可以看出，四種算法都可以在干擾環(huán)境中，都能解算出船舶的位置?？梢钥闯?，改進(jìn)型Kalman算法的性能最好。

表1 單歷元解算統(tǒng)計結(jié)果/m

5 結(jié)論

本文針對干擾環(huán)境中船舶只能接收到單顆衛(wèi)星信號的問題，改進(jìn)解算算法，保證船舶的正常定位。GNILS算法計算時間短，但是誤差大，用該算法收斂后的值來改善Kalman算法不收斂的問題，再結(jié)合L-M算法，更改Kalman算法的協(xié)方差矩陣。從定位的結(jié)果可以看出，改進(jìn)算法要比傳統(tǒng)算法在精度上有很大的提升。