余勝義, 彭 惠

(北京航天發(fā)射技術(shù)研究所，北京 100076)

0 引言

相較多傳感器信息融合濾波的定位方法，航位推算定位方法的計(jì)算相對簡單，對導(dǎo)航計(jì)算機(jī)的要求較低，系統(tǒng)模型簡單，不存在長時(shí)間濾波易導(dǎo)致系統(tǒng)發(fā)散的問題[1-2]，廣泛應(yīng)用于中精度車載慣性定位系統(tǒng)中。航位推算定位的系統(tǒng)誤差主要為方位安裝偏差角誤差(以下簡稱“安偏角”)及里程計(jì)刻度系數(shù)誤差[3-5](以下簡稱“里程系數(shù)”)。對慣性定位系統(tǒng)誤差的標(biāo)定是慣性定位系統(tǒng)的前提，常見的標(biāo)定慣性定位系統(tǒng)誤差的方法有以下兩種：

一種是將安偏角和里程系數(shù)作為狀態(tài)量，通過卡爾曼濾波器估計(jì)可獲得安偏角和里程系數(shù)[6-7]，這種方法對導(dǎo)航計(jì)算機(jī)的要求較高，且受標(biāo)定路線所限，若安偏角和里程系數(shù)的可觀測性差[8]，則難以完成標(biāo)定[9-10]；

另一種是將載車運(yùn)動(dòng)投影到平面中，采用起始點(diǎn)和終止點(diǎn)的標(biāo)準(zhǔn)位置以及慣性定位系統(tǒng)的輸出進(jìn)行標(biāo)定計(jì)算，標(biāo)定計(jì)算結(jié)果與行駛路徑無關(guān)[11]，標(biāo)定計(jì)算量小，易于實(shí)現(xiàn)。為了解決常用的起點(diǎn)緯度標(biāo)定計(jì)算法在安裝偏差角90°附近時(shí)誤差較大已不適用的問題，本文對這種標(biāo)定方法的公式進(jìn)行了推導(dǎo)，并對兩種計(jì)算方法的誤差進(jìn)行了分析。

1 定位系統(tǒng)誤差可標(biāo)性證明

如圖1所示，設(shè)標(biāo)準(zhǔn)起點(diǎn)X1、Y1，標(biāo)準(zhǔn)終點(diǎn)X2、Y2，慣性定位解算終點(diǎn)X3、Y3，假設(shè)行駛?cè)讨袘T性導(dǎo)航系統(tǒng)給出方位的安偏角為ε(航向漂移很小)，里程系數(shù)為k。kΔSi是第i個(gè)解算周期中測量的行駛里程；φi+ε是第i個(gè)解算周期中測量的方位角。

圖1 航位推算原理示意圖

1.1 航位推算原理

航位推算是把每個(gè)解算周期內(nèi)測量的行駛里程分解到北、東向后累加。如圖1所示的I放大解算周期的分解三角形中，可得

ΔXi=kΔSisin(φi+ε)

=kcosεΔSisinφi+ksinεΔSicosφi

(1)

ΔYi=kΔSicos(φi+ε)

=kcosεΔSicosφi-ksinεΔSisinφi

(2)

相應(yīng)地進(jìn)行經(jīng)度γi、緯度λi更新，累加實(shí)現(xiàn)航位推算

γi=γi-1+(ΔXi)/(Ni-1cosλi-1)

(3)

λi=λi-1+(ΔYi)/RMi-1

(4)

從標(biāo)準(zhǔn)起點(diǎn)出發(fā)，慣性解算終點(diǎn)累加公式如下

(5)

(6)

若不存在系統(tǒng)誤差時(shí)，即k=1，ε=0，得到標(biāo)準(zhǔn)終點(diǎn)-起點(diǎn)之間累加公式如下

(7)

(8)

1.2 可標(biāo)性證明

把式(7)、式(8)代入式(5)、式(6)得

X3-X1=kcosε(X2-X1)+ksinε(Y2-Y1)

(9)

Y3-Y1=kcosε(Y2-Y1)-ksinε(X2-X1)

(10)

式中，tanφ=(X2-X1)/(Y2-Y1)，φ正好是圖1中航跡AC與北向夾角。

將式(9)、式(10)兩邊平方后求和，可得

(11)

(12)

從式(9)、式(10)可以看出，慣性定位的終點(diǎn)與行駛路徑無關(guān)，只與起終點(diǎn)坐標(biāo)及系統(tǒng)存在的里程系數(shù)k及安偏角ε相關(guān)。使用式(11)、式(12)即可通過起終點(diǎn)坐標(biāo)計(jì)算出系統(tǒng)誤差k、ε。從圖1可以得到k、ε的幾何意義，k為直線段長度之比AD/AC，ε為夾角∠CAD。

綜上所述，標(biāo)定求解是將載車運(yùn)動(dòng)投影到平面中，起終點(diǎn)直線連線進(jìn)行計(jì)算。而使用墨卡托投影[12-14]的平面直角坐標(biāo)系中，等角航線為直線，可以借鑒等角航線的反解計(jì)算[15-17]進(jìn)行標(biāo)定求解，比較兩等角航線的航程和航向角得到系統(tǒng)誤差，k為航程之比，ε為航向角之差。

2 慣性定位系統(tǒng)誤差標(biāo)定計(jì)算

標(biāo)定計(jì)算過程：已知起點(diǎn)A經(jīng)緯度γ1、λ1，標(biāo)準(zhǔn)終點(diǎn)C經(jīng)緯度γ2、λ2及慣性定位終點(diǎn)D經(jīng)緯度γ3、λ3，計(jì)算出AC、AD兩等角航線航程和航向角，比較即可得到安偏角及里程系數(shù)。

2.1 等角航線的航向角及航程計(jì)算

2.1.1 航向角及航程精確計(jì)算

(1)航向角

如圖2所示，等角航線是橢球面上一條與經(jīng)線保持同一角度的曲線，航線與經(jīng)線夾角即航向角，設(shè)為φ，起點(diǎn)A(經(jīng)度γ1、緯度λ1)，終點(diǎn)C(經(jīng)度γ2、緯度λ2)。

圖2 等角航線示意圖

圖2的微分三角形中

dx=dssinφ=Ncosλdγ

(13)

dy=dscosφ=RMdλ

(14)

與航位推算的經(jīng)度、緯度更新式(3)、式(4)解算周期無限小時(shí)一致。

對式(13)、式(14)進(jìn)行積分變換，可得

(15)

(2)航程

等角航線的航程S在φ≠±90°(即λ2≠λ1)時(shí)用南北向航程M(子午線弧長差)除以航向角余弦而得，在φ=±90°(即λ2=λ1)時(shí)使用緯度圈半徑計(jì)算，得

(16)

(17)

其中

可得

(18)

(19)

使用式(18)、式(19)可以對航向角和航程進(jìn)行計(jì)算。式中rλ0、RC0對應(yīng)著不同的緯度值，計(jì)算過程較復(fù)雜。文獻(xiàn)[21]提出了一種簡化計(jì)算方式，使用等量緯度差精確計(jì)算航向角，使用平均緯度計(jì)算子午線弧長差，在150n mile以內(nèi)航程有1m精度。本文更進(jìn)一步簡化，將式(18)、式(19)中rλ0、RC0近似為航程平均緯度點(diǎn)的緯度圈、子午圈曲率半徑，可在一定航程內(nèi)獲得滿足精度要求的航向角和航程。

2.1.2 使用平均緯度計(jì)算航向角及航程

設(shè)平均緯度為λ0，式(18)、式(19)中rλ0、RC0近似為rλ0≈N(λ0)cos(λ0)，RC0≈RM(λ0)，式(18)、式(19)變?yōu)槭褂闷骄暥扔?jì)算公式

(20)

(21)

式中，Δγ=γ2-γ1、Δλ=λ2-λ1，φ0、S0為近似值，設(shè)相應(yīng)的誤差為Eφ、ES。

(1)航向角計(jì)算及誤差

使用式(20)計(jì)算航向角即是對等量緯度差g進(jìn)行了簡化計(jì)算。

(22)

式中，g0為近似值，設(shè)相應(yīng)的誤差為Eg。使用泰勒公式將等量緯度q(λ2)、q(λ1)以λ0為基礎(chǔ)展開到2次項(xiàng)并求差得

ζ(Δλ/2))+q(3)(λ0-

υ(Δλ/2)))(Δλ/2)3

(23)

式中，0<ζ,υ<1，λ2-λ0=λ0-λ1=Δλ/2。

航程較短(318km以內(nèi)，Δλ≤0.05)可忽略Eg中高次項(xiàng)，計(jì)算誤差時(shí)Δλ≈S0cosφ0/a，忽略小量，可得誤差為拉格朗日余項(xiàng)差值

q(3)(λ0-υ(Δλ/2)))

(24)

忽略小量，化簡可得誤差比例為

(25)

帶來的角度誤差為

(26)

(2)航程計(jì)算及誤差

使用式(21)計(jì)算航程即是對等量緯度差g、子午線弧長差M均進(jìn)行了簡化計(jì)算。

L(λ2)-L(λ1)≈M0=RM(λ0)Δλ

(27)

式中，M0為近似值，設(shè)相應(yīng)的誤差為EM。

使用泰勒公式對子午線弧長L(λ2)、L(λ1)以λ0為基礎(chǔ)展開到2次項(xiàng)并求差得

L(λ2)-L(λ1)=RM(λ0)(Δλ)+

q(3)(λ0-υ(Δλ/2)))(Δλ/2)3

(28)

式中，0<ζ,υ<1，λ2-λ0=λ0-λ1=Δλ/2。

航程較短可忽略EM中高次項(xiàng)，則誤差為

L(3)(λ0-υ(Δλ/2)))

(29)

忽略小量，化簡可得誤差比例為

(30)

對二元函數(shù)S(φ,M)=M/cosφ在M0、φ0處進(jìn)行泰勒展開，航程較短時(shí)，可忽略誤差中高次項(xiàng)，求得航程誤差比例為

(31)

將式(26)、式(30)代入式(31)可得

(32)

誤差中主要值為第二項(xiàng)，再化簡

(33)

根據(jù)式(33)估算的誤差，55°緯度，277.8km(即150n mile)計(jì)算的誤差絕對值最大為28m(比例為1×10-4)，比文獻(xiàn)[21]中給出的值大，主要是因?yàn)槭褂檬?21)計(jì)算航程對等量緯度差g亦進(jìn)行了簡化計(jì)算，增加了航程計(jì)算誤差。從式(32)可以看出，對等量緯度差g進(jìn)行簡化計(jì)算帶來的誤差占主要部分。

根據(jù)式(26)、式(33)，在55°緯度，90km行駛距離，帶來的航程相對誤差最大為1×10-5，誤差最大為0.95m，角度誤差絕對值不超過4.3″。

2.2 使用平均緯度標(biāo)定計(jì)算及誤差

2.2.1 計(jì)算公式

按照式(20)、式(21)分別計(jì)算AD、AC等角航線的航向角及航程，航向角求差得標(biāo)定計(jì)算的安偏角Ap，航程相除得標(biāo)定計(jì)算的里程系數(shù)Xs。設(shè)λ0=(λ2+λ1)/2，λD=(λ3+λ1)/2，計(jì)算公式為

(34)

Xs=

(35)

2.2.2 誤差分析

使用式(20)、式(21)可以得到兩條航線AD、AC航向角及航程，設(shè)φD、SD、φ0、S0為計(jì)算值，EφD、ESD、Eφ0、ES0為計(jì)算的誤差。

標(biāo)定計(jì)算的安偏角

Ap=(φD+EφD)-(φ0-Eφ0)

=(φD-φ0)+(EφD-Eφ0)

(36)

將式(26)代入，估算誤差時(shí)，φD-φ0≈ε，SD/S0≈k，緯度取λ1，標(biāo)定計(jì)算得到的Ap與設(shè)定的ε比較，得安偏角標(biāo)定誤差

Ap-ε≈EφD-Eφ0

(k2cos2(φ0+ε)sin(2φ0+2ε)-

cos2φ0sin(2φ0))

(37)

標(biāo)定計(jì)算的里程系數(shù)Xs

(38)

將式(33)代入，估算誤差時(shí)，φD-φ0≈ε、SD/S0≈k，忽略二階小量，緯度取λ1，標(biāo)定計(jì)算得到的Xs與設(shè)定的k比較，得里程系數(shù)標(biāo)定誤差

(k2sin2(2φ0+2ε)-sin2(2φ0))

(39)

2.3 使用起點(diǎn)緯度進(jìn)行標(biāo)定計(jì)算及誤差

2.3.1 計(jì)算公式

中精度車載慣性定位設(shè)備標(biāo)定計(jì)算時(shí)常采用起點(diǎn)緯度值進(jìn)行式(34)、式(35)的計(jì)算，即λ0=λD=λ1，公式變?yōu)?/p>

(40)

Xs=

(41)

2.3.2 誤差分析

類比2.1.2節(jié)的分析，可得

Eφ≈-0.25(S0/a)tanλ1sin(2φ0)cosφ0

(42)

ES/S0≈-0.25(S0/a)tanλ1sin(2φ0)sinφ0

(43)

可得安偏角標(biāo)定計(jì)算誤差

Ap-ε≈0.25(S0/a)tanλ1·

(ksin(2φ0+2ε)cos(φ0+ε)-

sin(2φ0)cosφ0)

(44)

可得里程系數(shù)標(biāo)定計(jì)算誤差

(ksin(2φ0+2ε)sin(φ0+ε)-

sin(2φ0)sinφ0)

(45)

3 仿真分析

3.1 慣性定位航位推算流程

慣性定位的航位推算流程如圖3所示。

圖3 慣性定位航位推算流程圖

3.2 使用平均緯度標(biāo)定計(jì)算誤差仿真

設(shè)定航向角，航程按照圖3 流程分別計(jì)算得到標(biāo)準(zhǔn)點(diǎn)及含系統(tǒng)誤差的慣性定位輸出點(diǎn)經(jīng)緯度，按照前面的標(biāo)定計(jì)算公式計(jì)算出慣性定位輸出包含的系統(tǒng)誤差。

設(shè)定k=1.2，ε=92.5°，計(jì)算不同航向角下的安偏角和里程系數(shù)的標(biāo)定誤差，并與式(28)、式(29)計(jì)算比較，繪制如圖4所示，可見公式能準(zhǔn)確估算誤差：安偏角不大于0.1″，里程系數(shù)不大于5×10-7。

圖4 使用平均緯度求解標(biāo)定誤差仿真

采用計(jì)算機(jī)仿真計(jì)算，起點(diǎn)A緯度55°、經(jīng)度110°。設(shè)定不同的里程系數(shù)k及安偏角ε組合，計(jì)算不同航向角下的標(biāo)定值誤差，結(jié)果如圖5所示。安偏角標(biāo)定誤差不超過7″，里程系數(shù)標(biāo)定誤差不超過1.5×10-5。

圖5 使用平均緯度進(jìn)行標(biāo)定計(jì)算誤差

3.3 使用起點(diǎn)緯度標(biāo)定計(jì)算誤差仿真

設(shè)定k=1.2，ε=92.5°(航程變?yōu)?0km)，計(jì)算不同航向角下的安偏角和里程系數(shù)的標(biāo)定誤差，與式(44)、式(45)計(jì)算比較，繪制如圖6所示，可見公式能準(zhǔn)確估算誤差：安偏角不大于1″，里程系數(shù)不大于1×10-5。

圖6 理論公式與模擬計(jì)算標(biāo)定誤差對比

不同設(shè)置值模擬計(jì)算繪制系列如圖7所示。角度誤差最大到180″，里程系數(shù)誤差最大達(dá)9×10-4，在ε≈90°時(shí)求解誤差大。

圖7 使用起點(diǎn)緯度求解的標(biāo)定誤差

在ε≈0°附近模擬計(jì)算系列如圖8所示。安偏角求解誤差不大于50″，里程系數(shù)求解誤差不大于2.5×10-4。

圖8 安偏角小時(shí)使用起點(diǎn)緯度的標(biāo)定誤差

在進(jìn)行標(biāo)定試驗(yàn)時(shí)，若使用起點(diǎn)緯度進(jìn)行標(biāo)定計(jì)算，可根據(jù)慣性定位設(shè)備的安裝情況，估計(jì)并設(shè)定安偏角初值，使進(jìn)行航位推算時(shí)的實(shí)際安偏角在0°附近，或者進(jìn)行標(biāo)定迭代試驗(yàn)，減少該方法的標(biāo)定誤差。

4 結(jié)論

本文通過借鑒等角航線的理論得出使用平均緯度進(jìn)行標(biāo)定計(jì)算的公式，并推導(dǎo)出誤差公式，可得以下結(jié)論：

1)仿真結(jié)果表明，該方法標(biāo)定在南北緯不大于55°，90km航程以內(nèi)安偏角標(biāo)定誤差不超過6″，里程系數(shù)標(biāo)定誤差不超過1.5×10-5。若在更長的行程下可使用本文的公式進(jìn)行誤差估計(jì)。

2)使用起點(diǎn)緯度法進(jìn)行標(biāo)定求解的誤差較大，只能在更短的航程以及安偏角較小時(shí)使用，安偏角較大時(shí)則需在估計(jì)初值的前提下使用。

3)基于本文的理論，航位推算的緯度經(jīng)度更新使用起點(diǎn)緯度計(jì)算的曲率半徑，在高緯度、較大步長時(shí)會(huì)損失定位精度，后續(xù)將進(jìn)一步開展研究。