基于高階卡爾曼濾波的激光捷聯(lián)慣導(dǎo)系統(tǒng)級標(biāo)定方法

劉 冰，任繼山，白煥旭，王 盛，陳鴻躍

基于高階卡爾曼濾波的激光捷聯(lián)慣導(dǎo)系統(tǒng)級標(biāo)定方法

劉 冰1，任繼山2，白煥旭1，王 盛1，陳鴻躍1

（1. 北京航天發(fā)射技術(shù)研究所，北京，100076；2. 上海航天技術(shù)研究院第八設(shè)計(jì)部，上海，201109）

隨著激光捷聯(lián)慣導(dǎo)系統(tǒng)的不斷發(fā)展，系統(tǒng)對于誤差標(biāo)定的精度要求也在不斷提高。在現(xiàn)有系統(tǒng)級標(biāo)定算法的基礎(chǔ)上，全面考慮了慣性器件零偏、安裝誤差角、標(biāo)度因數(shù)誤差、加速度計(jì)二次項(xiàng)、內(nèi)桿臂等誤差，并在計(jì)算速度和位置誤差觀測量時考慮了外桿臂誤差，提高了激光捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型的準(zhǔn)確性，并基于此設(shè)計(jì)了一種基于高階卡爾曼濾波算法的系統(tǒng)級標(biāo)定方法。通過實(shí)驗(yàn)驗(yàn)證表明，與分立式標(biāo)定方法相比，所提出的系統(tǒng)級標(biāo)定方法具有更高的標(biāo)定精度，能夠滿足高精度激光捷聯(lián)慣導(dǎo)系統(tǒng)的標(biāo)定需求。

系統(tǒng)級標(biāo)定；內(nèi)桿臂；卡爾曼濾波；捷聯(lián)慣導(dǎo)系統(tǒng)

0 引 言

慣性測量組合（Ιnertial measurement unit，ΙMU）誤差精確標(biāo)定是提高捷聯(lián)慣導(dǎo)系統(tǒng)導(dǎo)航精度的重要手段之一。慣導(dǎo)標(biāo)定技術(shù)可分為分立式標(biāo)定技術(shù)和系統(tǒng)級標(biāo)定技術(shù)，其中系統(tǒng)級標(biāo)定技術(shù)在轉(zhuǎn)臺精度有限的情況下，仍可保證較高的標(biāo)定精度，被廣泛應(yīng)用于實(shí)驗(yàn)室高精度標(biāo)定和外場不下車自標(biāo)定，系統(tǒng)級標(biāo)定方法已成為目前激光捷聯(lián)慣導(dǎo)標(biāo)定技術(shù)研究熱點(diǎn)之一。

系統(tǒng)級標(biāo)定是根據(jù)導(dǎo)航輸出誤差與誤差參數(shù)及輸入之間的關(guān)系，結(jié)合各誤差參數(shù)的可觀測性，設(shè)計(jì)合適的旋轉(zhuǎn)路徑，高精度的辨識誤差參數(shù)[1]。目前，中國系統(tǒng)級標(biāo)定技術(shù)研究越來越深入，從理論研究到具體實(shí)現(xiàn)都在逐漸成熟。楊曉霞等人首次提出了設(shè)計(jì)多位置翻滾實(shí)驗(yàn)的系統(tǒng)級標(biāo)定路徑編排原則[2]。于海龍?jiān)O(shè)計(jì)了33維卡爾曼濾波的捷聯(lián)慣導(dǎo)系統(tǒng)級標(biāo)定方法[3]，在原有誤差模型的基礎(chǔ)上增加了加速度計(jì)二次項(xiàng)誤差，并通過振動實(shí)驗(yàn)驗(yàn)證了誤差模型的有效性。張紅良在系統(tǒng)分析導(dǎo)航誤差與各誤差參數(shù)關(guān)系的基礎(chǔ)上，額外考慮了內(nèi)桿臂誤差[4]。吳賽成利用系統(tǒng)可觀測性的方法提出了一種新的編排方式[5]?？偨Y(jié)現(xiàn)有系統(tǒng)級標(biāo)定方法時發(fā)現(xiàn)，現(xiàn)有標(biāo)定方法中涉及到的誤差類型還不夠全面，如誤差模型中忽略了外桿臂誤差對系統(tǒng)觀測量的影響，誤差模型也不夠準(zhǔn)確，如內(nèi)桿臂誤差模型中忽略了安裝誤差的影響。因此，本文在原有誤差模型的基礎(chǔ)上，細(xì)化了內(nèi)桿臂誤差模型，并在計(jì)算速度位置觀測量時考慮了外桿臂誤差，提高了系統(tǒng)級標(biāo)定中誤差模型的準(zhǔn)確度，實(shí)驗(yàn)結(jié)果表明，該方法有效提高了系統(tǒng)的誤差標(biāo)定精度。

1 慣性器件誤差模型

為了提高慣導(dǎo)系統(tǒng)的標(biāo)定精度，系統(tǒng)模型需全面綜合地考慮慣性器件的各類誤差模型，其中包括激光陀螺和加速度計(jì)的常值零偏、標(biāo)度因數(shù)誤差、安裝誤差角以及加速度計(jì)二次項(xiàng)誤差系數(shù)、內(nèi)桿臂誤差和外桿臂誤差等。

綜合各類型誤差參數(shù)，可得激光捷聯(lián)慣導(dǎo)系統(tǒng)中陀螺的誤差模型為[6]

激光捷聯(lián)慣導(dǎo)系統(tǒng)中加速度計(jì)的輸出誤差模型為

式中 δfb為加速度計(jì)的輸出誤差，為加速度計(jì)的比力輸入，為加速度計(jì)的標(biāo)度因數(shù)誤差和安裝誤差矩陣，速度計(jì)的二次項(xiàng)誤差系數(shù)，常值零偏；fr為內(nèi)桿臂帶來的加速度計(jì)輸出誤差；na為加速度計(jì)的隨機(jī)噪聲誤差。

2 內(nèi)桿臂誤差模型和外桿臂誤差模型

2.1 內(nèi)桿臂誤差模型

理想情況下，激光捷聯(lián)慣導(dǎo)系統(tǒng)中的3只加速度計(jì)是正交安裝且測量敏感中心完全重合的，但在實(shí)際情況中，受到加速度計(jì)物理尺寸和安裝精度的限制，3只加速度計(jì)的測量敏感中心并不重合，有一定的位置差異，而且這個位置差異會帶來比力測量誤差[7]，通常把這個位置差異稱為內(nèi)桿臂誤差。

根據(jù)剛體轉(zhuǎn)動理論可以推導(dǎo)出內(nèi)桿臂誤差模型為

式中 W=(ω˙b×)+(ωb×)2，(?×)表示向量(?)構(gòu)成的反對稱矩陣；ωb為ΙMU轉(zhuǎn)動角速度；為內(nèi)桿臂誤差參數(shù)；為加速度計(jì)非正交安裝誤差矩陣，

為了減少系統(tǒng)誤差模型的參數(shù)，將載體系的坐標(biāo)原點(diǎn)設(shè)置在z軸加速度計(jì)的測量敏感中心上，則rzb=[000]，內(nèi)桿臂誤差模型可簡化為

從式（3）、式（4）可以看出，內(nèi)桿臂與安裝矩陣Cn、角速度ωb和內(nèi)桿臂rb相關(guān)，其中，Cb為已知量，

aiba rb對于同一個ΙMU是不會變的，因此，內(nèi)桿臂帶來的誤差主要與角速度大小相關(guān)，所以在靜態(tài)或者小幅震動等小角度運(yùn)動下，內(nèi)桿臂帶來的影響很小，基本上可以忽略，而在高動態(tài)的大角度運(yùn)動下，內(nèi)桿臂對導(dǎo)航誤差精度有較大的影響，必須進(jìn)行精確補(bǔ)償。

2.2 外桿臂模型

系統(tǒng)級標(biāo)定方法一般采用速度和位置作為觀測量，理想情況下認(rèn)為標(biāo)定時速度為零且位置不變，但在實(shí)際操作過程中，由于轉(zhuǎn)臺機(jī)械結(jié)構(gòu)和安裝精度的限制，ΙMU的敏感中心和轉(zhuǎn)臺的旋轉(zhuǎn)中心很難重合在一點(diǎn)，即觀測點(diǎn)和ΙMU敏感中心之間存在桿臂，一般稱之為外桿臂[4]。假設(shè)ΙMU測量敏感點(diǎn)到轉(zhuǎn)軸中心點(diǎn)的桿臂矢量在載體系中表示為lb，則補(bǔ)償后的速度和位置觀測量分別為

由于外桿臂的存在，實(shí)際標(biāo)定時慣組的速度位置有微小變化，尤其在角速度激勵較大時，如不考慮外桿臂會對系統(tǒng)級標(biāo)定方法的標(biāo)定精度造成較大影響。

3 卡爾曼濾波器設(shè)計(jì)

將慣性器件的誤差模型代入到系統(tǒng)誤差傳播方程中得到高階的激光捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型，在此基礎(chǔ)上，結(jié)合外桿臂誤差模型，建立用于系統(tǒng)級標(biāo)定的高階卡爾曼濾波器。

3.1 卡爾曼狀態(tài)變量的選取

濾波器選取了包括激光陀螺和加速度計(jì)的零偏、標(biāo)度因數(shù)誤差、安裝誤差角、加速度計(jì)二次項(xiàng)誤差系數(shù)、內(nèi)桿臂、外桿臂等9個誤差類型，再加上系統(tǒng)的姿態(tài)誤差、速度誤差和位置誤差，形成濾波器的狀態(tài)變量，共42維狀態(tài)變量，選取如下：

式中 ?i,δvi分別為姿態(tài)誤差和速度誤差，其中，i表示E，N，U方向；δL，δλ，δh分別為緯度、精度和高程誤差；εi(i=x, y, z )為陀螺零偏；?i(i=x, y, z)為加速度計(jì)零偏；(i=x, y, z)為外桿臂參數(shù)；δkg,δka分別為陀螺和加速度計(jì)的刻度系數(shù)和安裝誤差角；i=x, y, z)分別為3個加速度計(jì)的二次項(xiàng)誤差系數(shù)；i=x, y, z)為內(nèi)桿臂參數(shù)。

3.2 狀態(tài)方程建立

系統(tǒng)級標(biāo)定濾波模型的狀態(tài)方程為式中 W(t)為高斯白噪聲，是系統(tǒng)激勵噪聲向量；A(t)為過程系統(tǒng)矩陣，在以往誤差模型基礎(chǔ)上增加內(nèi)桿臂誤差模型和外桿臂誤差模型，形成一個42維高階矩陣。

3.3 量測方程的建立

選擇速度誤差和位置誤差作為系統(tǒng)觀測量，激光捷聯(lián)慣導(dǎo)系統(tǒng)卡爾曼濾波的量測方程為

式中 Z(t)為觀測矩陣；H為量測矩陣；X( t)為狀態(tài)變量；V(t)為量測噪聲向量。

需要注意的是建立觀測矩陣和量測矩陣時，需要考慮外桿臂帶來的速度位置影響。

式中 上標(biāo)n為導(dǎo)航坐標(biāo)系；RM為地球子午圈曲率半徑。

量測矩陣：

4 系統(tǒng)實(shí)驗(yàn)與結(jié)果驗(yàn)證

4.1 系統(tǒng)標(biāo)定實(shí)驗(yàn)

實(shí)驗(yàn)中采用某激光捷聯(lián)慣導(dǎo)系統(tǒng)，旋轉(zhuǎn)編排參考Camberlein設(shè)計(jì)的編排方式，因?yàn)樵谠心Ｐ偷幕A(chǔ)上增加了內(nèi)、外桿臂模型，為了更好激勵和分離內(nèi)外桿臂誤差參數(shù)，在原有編排基礎(chǔ)上增加了一組高動態(tài)編排，即分別繞x，y，z軸正、反快速（60（°）/s）旋轉(zhuǎn)3圈[8]。按照設(shè)定好的編排路徑進(jìn)行標(biāo)定實(shí)驗(yàn)，采集實(shí)驗(yàn)數(shù)據(jù)，進(jìn)行濾波計(jì)算。圖1、圖2給出了部分標(biāo)定參數(shù)的濾波過程。

圖1 6個內(nèi)桿臂參數(shù)估計(jì)曲線

圖2 3個外桿臂參數(shù)估計(jì)曲線

各誤差參數(shù)在震蕩后都趨于平穩(wěn)，說明各誤差參數(shù)得到了較好的估計(jì)。為了進(jìn)一步驗(yàn)證算法的可靠性，在不同時間采集了多組實(shí)驗(yàn)數(shù)據(jù)，實(shí)驗(yàn)結(jié)果顯示，各誤差參數(shù)估計(jì)結(jié)果都有較好的重復(fù)性。表1給出了部分誤差參數(shù)的標(biāo)定結(jié)果。

表1 部分系統(tǒng)級標(biāo)定參數(shù)估計(jì)結(jié)果

4.2 標(biāo)定結(jié)果精度驗(yàn)證

標(biāo)定結(jié)果的精度一般通過對比不同標(biāo)定結(jié)果補(bǔ)償后的導(dǎo)航精度來評價。這種方法雖能較好驗(yàn)證標(biāo)定結(jié)果的總體精度，但具體各類誤差參數(shù)的標(biāo)定精度如何，仍需要在特定的工況下進(jìn)行驗(yàn)證，比如內(nèi)桿臂誤差只有在載體處于高動態(tài)環(huán)境下補(bǔ)償效果才會比較明顯。因此本文中除了對系統(tǒng)級標(biāo)定進(jìn)行導(dǎo)航精度驗(yàn)證外，還增加了快速旋轉(zhuǎn)實(shí)驗(yàn)和晃動實(shí)驗(yàn)來驗(yàn)證內(nèi)桿臂的標(biāo)定精度。將慣導(dǎo)比力誤差方程與內(nèi)桿臂模型相結(jié)合，可以推導(dǎo)出內(nèi)桿臂誤差與導(dǎo)航速度誤差δv在短時間內(nèi)的變化規(guī)律：

在快速旋轉(zhuǎn)實(shí)驗(yàn)中，當(dāng)對系統(tǒng)進(jìn)行內(nèi)桿臂誤差補(bǔ)償后，速度誤差的跳動減小98%，說明系統(tǒng)級標(biāo)定得到的內(nèi)桿臂參數(shù)具有較高的精度。

在長時間劇烈晃動實(shí)驗(yàn)中，將捷聯(lián)慣導(dǎo)系統(tǒng)固定在三軸轉(zhuǎn)臺上，進(jìn)行長時間搖擺，模擬載體的動態(tài)環(huán)境，對比結(jié)果如圖4所示，可以看到內(nèi)桿臂誤差補(bǔ)償后，速度誤差減小明顯。

圖3 補(bǔ)償內(nèi)桿臂前后快速旋轉(zhuǎn)實(shí)驗(yàn)的東、北向速度誤差對比

圖4 晃動實(shí)驗(yàn)中補(bǔ)償前后的北向速度誤差對比

將慣導(dǎo)系統(tǒng)安裝在轉(zhuǎn)臺上，實(shí)驗(yàn)時每隔5 min以30（°）/s轉(zhuǎn)速將方位角轉(zhuǎn)動180°，慣組位置不變。分別應(yīng)用分立式的標(biāo)定結(jié)果和系統(tǒng)級方法的標(biāo)定結(jié)果進(jìn)行導(dǎo)航解算。采集25 h的ΙMU數(shù)據(jù)，第1 h進(jìn)行對準(zhǔn)，然后純慣性導(dǎo)航24 h，兩種標(biāo)定結(jié)果的導(dǎo)航位置誤差如圖5所示，其中采用分立式標(biāo)定結(jié)果的東向位置誤差為2 375 m，北向位置誤差為2 287 m，而采用系統(tǒng)級結(jié)果的東向誤差為781.5 m，北向位置誤差為961 m，從定位誤差結(jié)果可以看出，系統(tǒng)級標(biāo)定方法有效提高了系統(tǒng)的導(dǎo)航精度。

圖5 東向、北向位置誤差對比曲線

5 結(jié) 論

在現(xiàn)有激光捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型的基礎(chǔ)上，細(xì)化了內(nèi)桿臂誤差模型，并在計(jì)算速度和位置誤差觀測量時考慮了外桿臂誤差，提高了系統(tǒng)誤差模型的精度，設(shè)計(jì)了一種基于高階卡爾曼濾波算法的系統(tǒng)級標(biāo)定方法。實(shí)驗(yàn)結(jié)果表明，該系統(tǒng)級標(biāo)定方法能夠高精度的標(biāo)定出激光捷聯(lián)慣導(dǎo)系統(tǒng)中的各誤差參數(shù)，有效地提高了激光捷聯(lián)慣導(dǎo)系統(tǒng)的導(dǎo)航精度。

[1] Goshen M D, Bar Ι Y. A unified approach to inertial navigation system error modeling[J]. Journal of Guidance Control and Dynamics, 1992, 15(3): 472-480.

[2] 楊曉霞, 黃一. 激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)的一種系統(tǒng)級標(biāo)定方法[J]. 中國慣性技術(shù)學(xué)報, 2008, 16(1): 1-7.

[3] 于海龍. 提高強(qiáng)振動環(huán)境下激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)精度的方法研究[D].長沙: 國防科學(xué)技術(shù)大學(xué), 2012.

[4] 張紅良. 陸用高精度激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)誤差參數(shù)估計(jì)方法研究[D].長沙: 國防科學(xué)技術(shù)大學(xué), 2010.

[5] 吳賽成, 秦石喬, 王省書, 胡春生. 激光陀螺慣性測量單元系統(tǒng)級標(biāo)定方法[J]. 中國慣性技術(shù)學(xué)報, 2011, 19(2): 185-189.

[6] 袁保倫. 四頻激光陀螺旋轉(zhuǎn)式慣導(dǎo)系統(tǒng)研究[D]. 長沙: 國防科學(xué)技術(shù)大學(xué), 2007.

[7] 嚴(yán)恭敏, 周琪, 翁浚, 秦永元. 捷聯(lián)慣導(dǎo)系統(tǒng)內(nèi)桿臂補(bǔ)償方法及試驗(yàn)驗(yàn)證[J]. 宇航學(xué)報, 2012, 33(1): 62-67.

[8] 江奇淵, 湯建勛, 韓松來, 袁保倫. 36維Kalman濾波的激光陀螺捷聯(lián)慣導(dǎo)系統(tǒng)級標(biāo)定技術(shù)[J]. 紅外與激光工程, 2015, 44(5): 1579-1586.

Systematic Calibration Method Based on High-order Kalman Filter for Laser Gyro SINS

Liu Bing1, Ren Ji-shan2, Bai Huan-xu1, Wang Sheng1, Chen Hong-yue1
(1. Beijing Ιnstitute of Space Launch Technology, Beijing, 100076; 2. The 8th Ιnstitute of Shanghai Academy of Spaceflight Technology, Shanghai, 201109）

Along with navigation precision of laser gyro strap-down inertial navigation system (SΙNS) increases continuously, the requirement of error calibration accuracy is advancing to ever-higher levels. The research status of systematic calibration was analyzed, the error items that was considered includes biases scale factor errors installation error angle quadratic term error coefficient inner level arm, and outer arm was considered. writer emphasized the influences of inner and outer lever arm parameters to the systematic calibration, therefore, a 42-order system error model was established, proposed a systematic calibration method based on high-order Kalman filter. Experiments results indicate that, compare with traditional method, the error calibration accuracy of systematic calibration proposed in this paper is high and satisfies the demands of high precision inertial navigation system calibration.

Systemcalibration; Ιnner level arm; Kalman filter; Strap-down inertial navigation system

U666.1

A

1004-7182(2017)04-0090-05

DOΙ：10.7654/j.issn.1004-7182.20170421

2016-10-14；

2017-05-17

劉 冰（1991-），男，助理工程師，主要研究方向?yàn)檐囕d定位定向技術(shù)