李海軍，徐海剛，裴玉鋒，郭元江，孫 偉

(北京自動(dòng)化控制設(shè)備研究所，北京 100074)

一種單陀螺單加速度計(jì)旋轉(zhuǎn)調(diào)制尋北方法

李海軍，徐海剛，裴玉鋒，郭元江，孫 偉

(北京自動(dòng)化控制設(shè)備研究所，北京 100074)

加速度計(jì)；光纖陀螺；旋轉(zhuǎn)調(diào)制；尋北；卡爾曼濾波

0 引言

尋北儀在陸軍戰(zhàn)車、火炮等軍事領(lǐng)域得到廣泛的應(yīng)用。尋北儀的基本原理是在靜態(tài)條件下，采用陀螺儀測量北向地球自轉(zhuǎn)角速率，來獲得陀螺敏感軸相對北向的偏角，即航向角。尋北原理決定了其精度容易受到陀螺零位、不水平角等因素的影響[1-3]，需要采取一定措施來消除各種因素對尋北精度的影響[4-5]。

通常采用的方法是利用2個(gè)陀螺儀，配合旋轉(zhuǎn)機(jī)構(gòu)在0～360°范圍內(nèi)等間距位置，采集陀螺測量輸出的角速度值，來獲得陀螺敏感軸相對北向的偏角，即航向角。常用的尋北方案包括兩位置方案、三位置方案以及四位置方案等[6-8]。近年來開始出現(xiàn)利用連續(xù)旋轉(zhuǎn)調(diào)制的方法，來消除陀螺零位等因素的影響，大多是采用單軸陀螺，利用旋轉(zhuǎn)機(jī)構(gòu)的連續(xù)旋轉(zhuǎn)來完成尋北計(jì)算。這種方法需要同時(shí)在轉(zhuǎn)臺(tái)平面安裝2個(gè)正交的加速度計(jì)，以完成對不水平角的補(bǔ)償[9-12]。

本文提出只需利用單軸光學(xué)陀螺和1個(gè)加速度計(jì)，利用連續(xù)旋轉(zhuǎn)方法即可達(dá)到精確尋北的目的。

通過設(shè)定中間變量，建立起陀螺儀輸出的角速度及加速度計(jì)輸出的加速度與初始緯度及航向角之間的關(guān)系。利用旋轉(zhuǎn)過程中，俯仰角和滾動(dòng)角存在相關(guān)轉(zhuǎn)換的特點(diǎn)，用單個(gè)加速度計(jì)的輸出結(jié)果補(bǔ)償初始滾動(dòng)角及俯仰角的影響；并通過建立以中間變量、陀螺漂移，以及加速度計(jì)零偏為狀態(tài)量的卡爾曼濾波誤差模型，經(jīng)濾波估計(jì)獲得中間變量值；通過數(shù)值計(jì)算獲得初始緯度及航向角，從而實(shí)現(xiàn)單軸陀螺儀及單軸加速度計(jì)連續(xù)旋轉(zhuǎn)尋北計(jì)算。

本方法不需要輸入初始緯度信息和精確調(diào)平，即可完成尋北。相對于其他尋北方法，具有簡單、實(shí)用、可靠性高、成本低等特點(diǎn)。

1 坐標(biāo)系及變量定義

為了對尋北方法進(jìn)行詳細(xì)說明，這里先定義相關(guān)的坐標(biāo)系，具體如下：

b系：設(shè)垂直于轉(zhuǎn)臺(tái)平面向上為Y向，陀螺敏感軸在轉(zhuǎn)臺(tái)平面投影為X軸，Z軸在轉(zhuǎn)臺(tái)平面內(nèi)與X、Y軸構(gòu)成右手法則；

m系：b系繞Y軸旋轉(zhuǎn)時(shí)的坐標(biāo)系；

g系：陀螺儀所在的坐標(biāo)系；

a系：加速度計(jì)所在的坐標(biāo)系；

n系：當(dāng)?shù)氐乩碜鴺?biāo)系。

各坐標(biāo)的關(guān)系如圖 1所示。

圖1 坐標(biāo)關(guān)系示意圖Fig.1 The scheme of coordinate system

為了后面公式推導(dǎo)方便，這里對各軸系之間的角度及相關(guān)變量定義如下：

1)設(shè)b系相對地理坐標(biāo)系n系的3個(gè)姿態(tài)角分別為：滾動(dòng)角γ，航向角ψ，俯仰角θ，即轉(zhuǎn)臺(tái)的初始姿態(tài)角；

2)設(shè)Xg相對Xb的安裝誤差角為θ1，通過標(biāo)定獲得；

3)設(shè)Xa相對Xb的安裝誤差角為θ2、φ，通過標(biāo)定獲得；

4)設(shè)轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)的角速率為ω；

5)從計(jì)算開始已轉(zhuǎn)過的角度為α；

6)設(shè)緯度為L，地球自轉(zhuǎn)角速率為ωie，其北向分量和天向分量分別為ΩN=ωiecosL、ΩU=ωiesinL；

2 公式推導(dǎo)

有了上面的變量定義就可以進(jìn)行如下公式推導(dǎo)。

地理系n系的角速率和加速度分別為：

(1)

由此可以獲得b系的角速率和加速度分別為：

(2)

因此，得m系的角速率和加速度分別為：

(3)

g系的角速率為

(4)

a系的加速度為

(5)

由姿態(tài)矩陣及轉(zhuǎn)換矩陣計(jì)算方法，并經(jīng)過小角度近似可得：

g系中x陀螺的輸出為

ωg(1)= cos(α) [ cos(θ)cos(ψ)ΩN+sin(θ)ΩU] +

sin(α) {cos(θ)sin(γ)ΩU-

[ sin(θ)sin(γ)cos(ψ) +

cos (γ) sin(ψ) ]ΩΝ}+

θ1{ [ sin(γ)sin(ψ)-

sin(θ)cos(γ)cos(ψ) ]ΩN+

cos(θ)cos(γ)ΩU+ω} +ε

(6)

a系中x加表的輸出為

fa(1)= cosα{[φcosθsinγ+ sinθ]g}+

sinα{ [ cosθsinγ-φsinθ]g}+

(7)

分別令：

(8)

則：

(9)

由前面式(8)中C、D的方程可解得：

(10)

故：

(11)

由此可將初始俯仰角及滾動(dòng)角作為已知量，對A、B中未知的緯度及方位角進(jìn)行求解。

再令:

(12)

則A、B方程可重寫為：

(13)

如果可以裝定初始緯度，則可通過A式直接計(jì)算出緯度

(14)

如果不能裝定緯度，則通過下面方法計(jì)算出航向角及緯度。

由A、B兩式經(jīng)過變化可得

(15)

其中:

(16)

則解二次方程，取合理的一個(gè)結(jié)果可得

(17)

可得：

(18)

(19)

(20)

3 濾波器模型

上面給出了詳細(xì)的公式推導(dǎo)，直接計(jì)算可以獲得尋北結(jié)果，但是由于各種噪聲的存在，如果直接計(jì)算，可能會(huì)出現(xiàn)較大偏差，為此特別設(shè)計(jì)卡爾曼濾波器，通過濾波器的濾波效果對噪聲進(jìn)行平滑，以獲得更為準(zhǔn)確的結(jié)果。

經(jīng)過推導(dǎo)可得如下公式：

(21)

其中，α=ωt，t為濾波進(jìn)行的時(shí)間。則可得量測矩陣為

(22)

為了消除角速率及加速度噪聲大的影響，對式(21)在濾波周期內(nèi)進(jìn)行積分，可得

=hk1X1+hk2X2+…+hknXn

=hk1X1+hk2Φk2X2+…+hk2Φk2X2

=[hk1+hk2Φk2+…+hknΦkn]X1

=[hk1+hk2Φk2+…+hknΦkn]Xn/Φkn

(23)

所以可得

(24)

觀測量為

(25)

通過卡爾曼濾波可得A、B、C、D的數(shù)值，再由第2節(jié)的相關(guān)公式，計(jì)算獲得最終的尋北結(jié)果及緯度估計(jì)結(jié)果等參數(shù)。

4 誤差分析

旋轉(zhuǎn)調(diào)制尋北方法的主要誤差源有陀螺漂移、加速度計(jì)零偏以及安裝誤差等。由于旋轉(zhuǎn)可以將陀螺漂移及加速度計(jì)零偏的常值分量調(diào)制掉，因此對于尋北精度的影響，主要是陀螺漂移及加速度計(jì)零偏經(jīng)過調(diào)制后的剩余誤差。該誤差與陀螺儀及加速度計(jì)的穩(wěn)定性有關(guān)，穩(wěn)定性越好，尋北精度就越高[1]。

安裝誤差主要包括，陀螺敏感軸與轉(zhuǎn)臺(tái)旋轉(zhuǎn)軸之間的不正交角，以及加速度計(jì)在旋轉(zhuǎn)臺(tái)面上的安裝桿臂等。前者造成旋轉(zhuǎn)角速度分解到陀螺敏感軸，成為等效常值陀螺漂移。后者造成加速度計(jì)感受向心加速度，成為等效常值加速度計(jì)零偏。由前面的分析可知，這兩種常值誤差不影響尋北精度。

詳細(xì)的誤差分析過程可參見文獻(xiàn)[1]。

5 試驗(yàn)驗(yàn)證

圖2給出了某條次試驗(yàn)時(shí)，方位角的估計(jì)誤差曲線，可以看出試驗(yàn)時(shí)方位角收斂較快，在3min之后基本收斂至0.5mil以內(nèi)。圖 3給出了緯度估計(jì)曲線，圖 4和圖 5分別給出了估計(jì)的初始時(shí)刻姿態(tài)角曲線，可以看出初始姿態(tài)角的估計(jì)曲線較為平穩(wěn)。

表1 尋北驗(yàn)證試驗(yàn)估計(jì)結(jié)果及重復(fù)性誤差

圖2 某條次試驗(yàn)尋北誤差Fig.2 The north-seeking errors in experiment

圖3 某條次試驗(yàn)緯度估計(jì)誤差Fig.3 The latitude estimation errors in experiment

圖4 某條次試驗(yàn)滾動(dòng)角估計(jì)值Fig.4 The roll attitude estimation in experiment

圖5 某條次試驗(yàn)俯仰角估計(jì)值Fig.5 The pitch attitude estimation in experiment

由試驗(yàn)結(jié)果可以看出，本文提出的方法可以較快地完成方位角及緯度的估計(jì)，驗(yàn)證了本方法的正確性和有效性。

6 結(jié)論

本文針對旋轉(zhuǎn)調(diào)制尋北問題，提出了一種利用單陀螺配合單加速度計(jì)進(jìn)行旋轉(zhuǎn)調(diào)制尋北的方法，該方法不需要輸入初始緯度，不需要對平臺(tái)進(jìn)行精確調(diào)平，具有簡單、實(shí)用、成本低等特點(diǎn)。文中對尋北方法的實(shí)現(xiàn)流程進(jìn)行了詳細(xì)的介紹，對計(jì)算公式進(jìn)行了詳細(xì)的推導(dǎo)，并利用原理樣機(jī)進(jìn)行了試驗(yàn)驗(yàn)證，結(jié)果表明，尋北重復(fù)性誤差可以優(yōu)于0.5mil(1σ)，緯度估計(jì)誤差為0.0538°(1σ)。

[1] 殷楠,劉建鋒,葛磊，等. 陀螺隨機(jī)游走對定向設(shè)備雙位置尋北精度影響分析[J]. 電子設(shè)計(jì)工程，2015，23(24):110-112.

[2] 李險(xiǎn)鋒,楊海忠. 基于四位置法旋轉(zhuǎn)調(diào)制尋北儀的軸向擺動(dòng)分析[J]. 科學(xué)技術(shù)與工程,2012,12(29):7800-7802.

[3] 蔣慶仙,王成賓,馬小輝,等. 利用AR模型進(jìn)行動(dòng)態(tài)尋北中有色噪聲的控制[J]. 傳感技術(shù)學(xué)報(bào),2014,27(3):347-350.

[4] Yang J H, Zheng Q L, Yang H R, et al. The improvement of FOG north-seeker two-position north determinining scheme[C]. IEEE International Conference on Information and Automation, 2010:2248-2251.

[5] Zhang Y, Cao J L, Wu W Q, et al. The analysis of turntable error and arithmetic design for north-finder based on rate biased RLG[J]. Advanced Materials Research, 2011： 383-390.

[6] 薛海建,郭曉松,周召發(fā),等. 激光陀螺任意二位置尋北儀及誤差分析[J]. 紅外與激光工程，2015，44(6):1784-1789.

[7] 王海明,李四海. 一種緯度未知的全姿態(tài)光纖陀螺尋北方法[J]. 中國慣性技術(shù)學(xué)報(bào)，2014，22(1):63-66.

[8] 段苛苛,李鄧化. 光纖陀螺尋北儀連續(xù)旋轉(zhuǎn)尋北方案及算法研究[J]. 儀器儀表學(xué)報(bào)，2014，35(4):801-806.

[9] 徐海剛,郭宗本. 一種實(shí)用旋轉(zhuǎn)調(diào)制式陀螺尋北儀的設(shè)計(jì)[J]. 兵工學(xué)報(bào)，2010，31(5):616-619.

[10] 白云超,李學(xué)琴,馬小輝,等. 采用旋轉(zhuǎn)調(diào)制技術(shù)的高精度陀螺尋北方案[J]. 慣性技術(shù)學(xué)報(bào)，2010,18(4):421-424.

[11] Celikel O. Application of the vector modulation method to the north finder capability gyroscope as a directional sensor[J]. Measurement Science and Technology, 2011, 22(3):1-12.

[12] Eduardo S R, Vlandimir A N. Comparison between two basic schemes in function of the true earth’s north detection[C]//Saint Petersburg International Conference on Integrated Navigation Systems， 2006:315-318.

A Method for Revolution-Modulation North Seeker Based on Single Gyroscope and Accelerometer

LI Hai-jun, XU Hai-gang, PEI Yu-feng, GUO Yuan-jiang, SUN Wei

(Beijing Institute of Automatic Control Equipment，Beijing 100074，China)

Accelerometer；FOG；Revolution-modulation；North seeker；Kalman filter

10.19306/j.cnki.2095-8110.2016.05.008

2014-10-11；

2015-04-10。

李海軍(1981-)，男，碩士，高級工程師，主要從事導(dǎo)航、制導(dǎo)與控制方面的研究。E-mail：vantime@126.com

U666.1

A

2095-8110(2016)05-0041-05