基于運(yùn)動(dòng)約束的航姿系統(tǒng)誤差自主補(bǔ)償方法

摘要： 為了提高車載捷聯(lián)航姿系統(tǒng)的長(zhǎng)航時(shí)定姿精度，提出利用載車的運(yùn)動(dòng)約束條件對(duì)航姿系統(tǒng)誤差進(jìn)行在線自主補(bǔ)償。首先，對(duì)航姿系統(tǒng)的傳感器誤差、安裝誤差等進(jìn)行分析與建模，選取航姿系統(tǒng)的各項(xiàng)誤差作為誤差補(bǔ)償濾波的系統(tǒng)狀態(tài)，建立系統(tǒng)狀態(tài)方程。然后，根據(jù)發(fā)射車等特種車輛的運(yùn)動(dòng)特點(diǎn)，利用車輛運(yùn)動(dòng)約束條件，將載車行駛過(guò)程中航姿系統(tǒng)的速度輸出在車體坐標(biāo)系橫向和垂向上的投影分量作為誤差補(bǔ)償濾波的量測(cè)，推導(dǎo)獲得誤差補(bǔ)償濾波的量測(cè)方程。最后，采用卡爾曼濾波設(shè)計(jì)航姿系統(tǒng)誤差在線估計(jì)算法。實(shí)驗(yàn)結(jié)果表明，所提方法能夠自主有效地補(bǔ)償航姿系統(tǒng)的積累誤差，經(jīng)過(guò)1 200 s的跑車實(shí)驗(yàn)，系統(tǒng)的航姿誤差始終保持有效收斂，其中航向精度優(yōu)于3.4′，水平姿態(tài)精度優(yōu)于0.4′。

關(guān)鍵詞： 車載航姿系統(tǒng); 誤差補(bǔ)償; 運(yùn)動(dòng)約束; 卡爾曼濾波

中圖分類號(hào)： V 249.3

文獻(xiàn)標(biāo)志碼： A

DOI：10.12305/j.issn.1001-506X.2024.06.28

Independent error compensation method of attitude and heading system based on motion constraints

YANG Bo^1，*， LIU Feng¹， XUE Liang¹， LIU Yunfeng²

（1.School of Missile Engineering， Rocket Force University of Engineering， Xi’an 710025， China; 2. The 4th Military Representative Office of Rocket Force Equipment Department in Chengdu Area， Chengdu 610052， China）

Abstract： In order to improve the attitude determination accuracy of vehicle strapdown attitude and heading system in long distance， the on-line independent error compensation of attitude and heading system is proposed by using the motion constraints of the vehicle. Firstly， the sensor errors and installation errors of the attitude and heading system are analyzed and modeled. Then various errors of the attitude and heading system are selected as the system state of error compensation filtering， and the corresponding system state equation is established. Secondly， according to the motion characteristics of special vehicles such as launch vehicles， using the vehicle motion constraints， the projection components of the velocity output of attitude and heading system in the transverse and vertical directions of the vehicle body coordinate system are taken as the measurement of the error compensation filtering， and the measurement equation of the error compensation filtering is derived. Finally， kalman filter is used to design the on-line estimation algorithm of attitude and heading system errors. Experiment results show that the proposed method can independently and effectively compensate the accumu-lated errors of the attitude and heading system. After 1 200 s of vehicle experiment， the heading and attitude errors of the system always keep effective convergence， in which the heading accuracy is better than 3.4′， and the horizontal attitude accuracy is better than 0.4′.

Keywords： vehicle attitude and heading system; error compensation; motion constraint; Kalman filter

0 引 言

裝甲車、發(fā)射車、測(cè)繪車等現(xiàn)代高性能特種車輛通常會(huì)搭載車載航姿系統(tǒng)，其一般以高精度慣性導(dǎo)航系統(tǒng)為核心，配以衛(wèi)星導(dǎo)航接收機(jī)、磁強(qiáng)計(jì)、里程計(jì)等輔助手段，為載車提供高精度的航向和姿態(tài)等信息^［1-6］。眾所周知，慣性導(dǎo)航系統(tǒng)雖然自主性高、抗干擾性好，但是其誤差隨時(shí)間不斷累積，無(wú)法在長(zhǎng)航時(shí)條件下確保航姿精度。因此，目前通常利用衛(wèi)星導(dǎo)航信息或磁航姿信息等輔助手段來(lái)不斷修正慣導(dǎo)系統(tǒng)^［7-11］。

但是，衛(wèi)星導(dǎo)航信號(hào)易被干擾或遮擋，磁強(qiáng)計(jì)易受電磁環(huán)境以及地理環(huán)境影響，因此這兩種輔助慣導(dǎo)系統(tǒng)定姿的抗干擾性和自主性較差。文獻(xiàn)［2］研究了利用里程計(jì)輔助捷聯(lián)慣導(dǎo)系統(tǒng)實(shí)現(xiàn)車載自主定位定向，具有較強(qiáng)的自主性和抗干擾性，但是里程計(jì)測(cè)量效果容易受到車輛滑行或輪胎打滑的影響。這里的車輛滑行，主要指車輛沿著行駛方向或縱軸方向的滑行;而對(duì)于側(cè)滑，一般正常行駛的車輛，特別是重型特種車輛，通常難以發(fā)生側(cè)滑現(xiàn)象。文獻(xiàn)［12］研究了利用偏振光補(bǔ)償慣性累積誤差，以實(shí)現(xiàn)較高精度的航姿信息獲取，同時(shí)提高了系統(tǒng)的環(huán)境自適應(yīng)性。文獻(xiàn)［13］針對(duì)衛(wèi)星導(dǎo)航拒止環(huán)境下導(dǎo)彈發(fā)射車的定位定向問(wèn)題，提出一種基于射頻識(shí)別輔助捷聯(lián)慣導(dǎo)/里程計(jì)組合導(dǎo)航方法，考慮到射頻標(biāo)簽只能鋪設(shè)在有限的區(qū)域范圍內(nèi)，而且鋪設(shè)及維護(hù)成本均較高，因此無(wú)法實(shí)現(xiàn)普遍適用。文獻(xiàn)［14］研究采用多套旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)來(lái)提高航向和姿態(tài)精度，并提出對(duì)陀螺安裝誤差和比例因子誤差進(jìn)行在線校準(zhǔn)。文獻(xiàn)［15］為了解決磁干擾下航姿精度較低的問(wèn)題，設(shè)計(jì)了一種在磁干擾狀態(tài)下基于擴(kuò)展卡爾曼濾波器的航姿系統(tǒng)，采用擴(kuò)展卡爾曼濾波技術(shù)將來(lái)自磁力計(jì)、加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行組合。文獻(xiàn)［16］研究了基于零速的航姿誤差補(bǔ)償方法，在靜基座或振動(dòng)基座條件下，利用載體速度為零的特點(diǎn)，將航姿系統(tǒng)的速度輸出作為觀測(cè)量，設(shè)計(jì)航姿誤差補(bǔ)償濾波算法，取得了較為理想的驗(yàn)證結(jié)果。顯然，該方法僅能用于載車處于靜止或振動(dòng)環(huán)境的情形，而無(wú)法用于載車行駛狀態(tài)下航姿誤差的補(bǔ)償。

文獻(xiàn)［17］為了提高捷聯(lián)慣性航姿系統(tǒng)的定姿精度，提出一種基于誤差預(yù)測(cè)和補(bǔ)償濾波器的系統(tǒng)誤差間歇修正技術(shù)，采用比例積分控制器的思想，通過(guò)調(diào)整濾波器參數(shù)以獲得誤差最優(yōu)估計(jì)，有效提高了系統(tǒng)定姿精度，但該方法在很大程度上依賴于控制器和調(diào)節(jié)器參數(shù)的選取，準(zhǔn)確調(diào)節(jié)這些參數(shù)的難度較大。文獻(xiàn)［18-19］為了實(shí)現(xiàn)高精度定姿，提出一種用于航姿估計(jì)的自適應(yīng)互補(bǔ)濾波器，根據(jù)優(yōu)化的增益圖調(diào)整每個(gè)軸上加速度計(jì)和磁力計(jì)的權(quán)重。文獻(xiàn)［20-22］基于慣性測(cè)量單元和磁力計(jì)，提出采用非線性濾波算法來(lái)估計(jì)航向和姿態(tài)，在實(shí)時(shí)濾波器中對(duì)慣組、磁力計(jì)偏差及干擾進(jìn)行建模。文獻(xiàn)［23-24］為了提高衛(wèi)星拒止情況下的精確定位能力，提出綜合利用微慣組、衛(wèi)導(dǎo)、里程計(jì)以及磁航姿儀等多種儀器設(shè)備構(gòu)建組合航姿系統(tǒng)，具有較強(qiáng)的可靠性和抗干擾能力，但系統(tǒng)的復(fù)雜程度和成本明顯增加。

考慮到正常行駛的車輛如不發(fā)生側(cè)滑或跳躍，其行駛速度沿車體橫向、垂向分量應(yīng)該為零，即滿足車輛運(yùn)動(dòng)約束條件^［25-26］，那么可以將載車行駛過(guò)程中航姿系統(tǒng)的速度輸出作為誤差補(bǔ)償濾波的量測(cè)。為此，本文提出基于載車運(yùn)動(dòng)約束的航姿系統(tǒng)誤差補(bǔ)償方法，首先研究捷聯(lián)航姿系統(tǒng)的誤差模型;然后根據(jù)航姿系統(tǒng)的速度輸出構(gòu)造量測(cè)，建立航姿誤差補(bǔ)償?shù)臓顟B(tài)空間;采用卡爾曼濾波設(shè)計(jì)誤差補(bǔ)償濾波算法，利用濾波估計(jì)結(jié)果對(duì)車載航姿系統(tǒng)進(jìn)行誤差在線補(bǔ)償，從而自主有效地提高車載捷聯(lián)航姿系統(tǒng)的長(zhǎng)航時(shí)定姿精度。

1 車載航姿系統(tǒng)誤差自主補(bǔ)償方案

對(duì)于本文所研究的車載航姿系統(tǒng)，這里僅配備捷聯(lián)慣性系統(tǒng)，而無(wú)其他任何輔助設(shè)備，亦稱為捷聯(lián)航姿系統(tǒng);而且，其在進(jìn)行航姿解算之前，捷聯(lián)慣性系統(tǒng)已經(jīng)完成了初始對(duì)準(zhǔn)，此時(shí)經(jīng)過(guò)解算能夠?qū)崟r(shí)輸出載車的航姿、速度和位置信息。

選取導(dǎo)航坐標(biāo)系x_ny_nz_n（n系）為東北天地理坐標(biāo)系，車體坐標(biāo)系x_by_bz_b（b系）為右前上坐標(biāo)系。在捷聯(lián)航姿系統(tǒng)誤差補(bǔ)償過(guò)程中，以捷聯(lián)航姿系統(tǒng)的各種誤差作為誤差補(bǔ)償濾波的系統(tǒng)狀態(tài);借助航姿系統(tǒng)輸出的航姿信息，將車輛行駛過(guò)程中航姿系統(tǒng)的速度輸出從導(dǎo)航坐標(biāo)系轉(zhuǎn)換到車體坐標(biāo)系下;以速度輸出在車體坐標(biāo)系橫向和垂向上的投影分量作為航姿誤差補(bǔ)償濾波的量測(cè);在此基礎(chǔ)上，利用卡爾曼濾波對(duì)航姿系統(tǒng)誤差進(jìn)行最優(yōu)估計(jì);最后，利用該估計(jì)結(jié)果對(duì)航姿系統(tǒng)進(jìn)行誤差校正，即最終實(shí)現(xiàn)車載航姿系統(tǒng)的誤差自主補(bǔ)償。上述誤差補(bǔ)償?shù)脑矸桨溉鐖D1所示。

2 捷聯(lián)航姿系統(tǒng)的誤差模型

由于目前車載中、高精度航姿系統(tǒng)通常是基于捷聯(lián)慣組開(kāi)發(fā)研制而來(lái)，因此可將捷聯(lián)慣組導(dǎo)航解算的系統(tǒng)誤差模型視為航姿系統(tǒng)的誤差模型。這里，考慮到長(zhǎng)航時(shí)條件下車載航姿系統(tǒng)的位置變化較大，為了有效確保航姿系統(tǒng)的長(zhǎng)航時(shí)定姿精度，除了傳感器誤差、姿態(tài)誤差、速度誤差，還需要考慮位置誤差的影響。

陀螺儀和加速度計(jì)作為捷聯(lián)航姿系統(tǒng)的傳感器，其誤差是系統(tǒng)最主要的誤差源，通常將其考慮為常值誤差與隨機(jī)白噪聲的組合，很多文獻(xiàn)對(duì)比均有較詳細(xì)的描述^［27］，這里給出其模型。

設(shè)沿航姿系統(tǒng)x、y、z軸安裝的陀螺儀誤差為ε_i（i=x，y，z），則其模型可描述如下：

ε_i=ε_bi+w_gi（1）

式中：ε_bi為陀螺隨機(jī)常值漂移，可用隨機(jī)常數(shù)來(lái)描述，即

ε·_bi=0（2）

而w_gi為陀螺快變漂移，通常可視為白噪聲。

與陀螺儀的誤差模型類似，加速度計(jì)誤差也主要考慮隨機(jī)常值誤差和白噪聲，因此沿航姿系統(tǒng)x、y、z軸安裝的加速度計(jì)的誤差模型可表示為

Δ_i=Δ_bi+w_ai（3）

式中：Δ_bi為加速度計(jì)的隨機(jī)常值誤差，滿足：

Δ·_bi=0（4）

而w_ai為加速度計(jì)的白噪聲。

此外，航姿系統(tǒng)在事先標(biāo)定補(bǔ)償后，由于車輛行駛過(guò)程中長(zhǎng)期振動(dòng)等因素、將產(chǎn)生微小的安裝誤差，導(dǎo)致其安裝軸線很難與車體坐標(biāo)系的3個(gè)軸完全保持一致，該誤差在短時(shí)間內(nèi)（如航姿系統(tǒng)本次通電過(guò)程中）不會(huì)發(fā)生明顯變化，因此在航姿誤差估計(jì)濾波中可將其視為隨機(jī)常值，即其滿足：

δA ·_i=0， i=x，y，z（5）

式中：δA_i（i=x，y，z）分別為航姿系統(tǒng)沿車體坐標(biāo)系x、y、z軸的安裝誤差。

由于受到傳感器誤差、初始條件誤差等因素的影響，捷聯(lián)航姿系統(tǒng)解算獲得的航姿、速度和位置等信息也存在一定的誤差，為此需要對(duì)航姿系統(tǒng)的航姿誤差、速度誤差和位置誤差進(jìn)行建模。

設(shè)航姿系統(tǒng)的航向角誤差、俯仰角誤差和滾動(dòng)角誤差分別為δ，δθ，δγ，顯然上述航姿誤差與航姿系統(tǒng)的數(shù)學(xué)平臺(tái)姿態(tài)誤差之間存在著關(guān)系^［28］，可描述如下：

δ=－T₁₂T₃₂T²₁₂+T²₂₂?_E－T₃₂T₂₂T²₁₂+T²₂₂?_N+?_U（6）

δθ=－T₂₂1－T²₃₂?_E+T₁₂1－T²₃₂?_N（7）

δγ=T₂₁T₃₃－T₂₃T₃₁T²₃₁+T²₃₃?_E+T₁₃T₃₁－T₁₁T₃₃T²₃₁+T²₃₃?_N（8）

式中：?_E，?_N，?_U分別為航姿系統(tǒng)的數(shù)學(xué)平臺(tái)沿東、北、天向的姿態(tài)誤差;T_ij（i，j=1，2，3）為航姿系統(tǒng)姿態(tài)矩陣Cⁿ_b的第i行、第j列元素。

根據(jù)式（6）～式（8）可以看出，航姿誤差與數(shù)學(xué)平臺(tái)姿態(tài)誤差之間是線性關(guān)系，因此可以僅對(duì)捷聯(lián)航姿系統(tǒng)的數(shù)學(xué)平臺(tái)姿態(tài)誤差進(jìn)行建模?？紤]到捷聯(lián)航姿系統(tǒng)本質(zhì)上是捷聯(lián)慣組，則可以直接采用捷聯(lián)慣導(dǎo)數(shù)學(xué)平臺(tái)姿態(tài)誤差模型，即

這里，δv_E、δv_N、δv_U為航姿系統(tǒng)的東、北、天向速度誤差;δL、δλ、δh為其緯度、經(jīng)度、高度誤差。

相應(yīng)地，基于東北天地理坐標(biāo)系下比力方程，考慮各種傳感器誤差以及系統(tǒng)誤差的存在，可以通過(guò)推導(dǎo)獲得航姿系統(tǒng)的速度誤差方程：

;δVⁿ=［δv_E，δv_N，δv_U］^T;fⁿ為加速度計(jì)輸出的比力在導(dǎo)航坐標(biāo)系下的投影;Δ^b為加速度計(jì)誤差在車體坐標(biāo)系下的投影;Vⁿ為導(dǎo)航坐標(biāo)系下載車的速度。

根據(jù)東北天地理坐標(biāo)系下的位置解算微分方程，考慮系統(tǒng)速度誤差和位置誤差的存在，可以推導(dǎo)獲得航姿系統(tǒng)的位置誤差方程為

從上述捷聯(lián)航姿系統(tǒng)的誤差模型可以看出，航姿誤差與數(shù)學(xué)平臺(tái)姿態(tài)誤差、速度誤差、位置誤差相互之間都有聯(lián)系，對(duì)上述各種主要誤差進(jìn)行充分考慮并建模，有利于在長(zhǎng)航時(shí)條件下實(shí)現(xiàn)航姿誤差的有效估計(jì)與補(bǔ)償，從而確保航姿系統(tǒng)的長(zhǎng)航時(shí)定姿精度。

3 車載航姿系統(tǒng)誤差在線估計(jì)算法

為了實(shí)現(xiàn)航姿系統(tǒng)的誤差補(bǔ)償，需要實(shí)時(shí)獲得航姿誤差的估計(jì)值，由于航姿誤差與數(shù)學(xué)平臺(tái)姿態(tài)誤差之間是線性關(guān)系，因此在航姿誤差在線估計(jì)算法設(shè)計(jì)中可以將航姿系統(tǒng)的數(shù)學(xué)平臺(tái)姿態(tài)誤差作為狀態(tài)。此外，根據(jù)數(shù)學(xué)平臺(tái)姿態(tài)誤差模型及誤差傳播關(guān)系可知，還需將航姿系統(tǒng)的速度誤差、位置誤差以及各種傳感器誤差共同列入狀態(tài)，進(jìn)而建立相應(yīng)的狀態(tài)方程。同時(shí)，根據(jù)車輛運(yùn)動(dòng)約束條件，可以利用載車行駛過(guò)程中航姿系統(tǒng)的實(shí)際速度輸出作為誤差補(bǔ)償濾波的量測(cè)，并結(jié)合系統(tǒng)狀態(tài)建立量測(cè)方程。在此基礎(chǔ)上，利用卡爾曼濾波對(duì)航姿系統(tǒng)的數(shù)學(xué)平臺(tái)姿態(tài)誤差等狀態(tài)量進(jìn)行在線估計(jì)。

3.1 航姿誤差在線估計(jì)的狀態(tài)方程

根據(jù)前面的分析，結(jié)合捷聯(lián)航姿系統(tǒng)的誤差模型，選取航姿誤差在線估計(jì)的狀態(tài)，包括航姿系統(tǒng)的數(shù)學(xué)平臺(tái)姿態(tài)誤差?_E，?_N，?_U，速度誤差δv_E，δv_N，δv_U，位置誤差δL，δλ，δh，陀螺常值漂移ε_bx，ε_by，ε_bz，加速度計(jì)常值偏置Δ_bx，Δ_by，Δ_bz，安裝誤差δA_x，δA_y，δA_z，即航姿誤差在線估計(jì)的系統(tǒng)狀態(tài)X為

那么，結(jié)合系統(tǒng)狀態(tài)X，基于捷聯(lián)航姿系統(tǒng)的誤差模型式（1）～式（5）及式（9）～式（13），可以列寫出航姿誤差在線估計(jì)的狀態(tài)方程為

X·=FX+GW（15）

式中：F和G分別為狀態(tài)矩陣、噪聲驅(qū)動(dòng)陣，其可以根據(jù)捷聯(lián)航姿系統(tǒng)的誤差模型列寫獲得;W為系統(tǒng)噪聲，包括陀螺白噪聲和加速度計(jì)白噪聲，其具體形式為

W=［w_gx，w_gy，w_gz，w_ax，w_ay，w_az］^T（16）

3.2 航姿誤差在線估計(jì)的量測(cè)方程

考慮到搭載航姿系統(tǒng)的裝甲車、發(fā)射車等特種車輛通常重量較大、底盤很穩(wěn)，其在地面上正常行駛時(shí)一般難以發(fā)生側(cè)滑或跳躍，因此載車行駛速度沿車體坐標(biāo)系x_b、z_b兩個(gè)軸的投影分量應(yīng)為零。為此，可以將航姿系統(tǒng)的實(shí)際速度輸出在車體坐標(biāo)系x_b、z_b軸上的投影作為航姿誤差在線估計(jì)的量測(cè)，并以此建立相應(yīng)量測(cè)方程。

為此，首先對(duì)航姿系統(tǒng)的實(shí)際速度輸出在車體坐標(biāo)系下的投影進(jìn)行分析。設(shè)航姿系統(tǒng)實(shí)際輸出的在導(dǎo)航坐標(biāo)系下的載車速度為v^ⁿ，其輸出的載車姿態(tài)矩陣為?ⁿ_b，則該速度在車體坐標(biāo)系x_by_bz_b下的投影v^^b為

式中：v^ⁿ=［v^_E，v^_N，v^_U］^T;v^^b=［v^_x，v^_y，v^_z］^T。

將航姿系統(tǒng)的數(shù)學(xué)平臺(tái)姿態(tài)誤差記為矢量Φ，其安裝誤差記為δA，這里Φ=［?_E，?_N，?_U］^T，δA=［δA_x，δA_y，δA_z］^T。那么，當(dāng)考慮航姿系統(tǒng)數(shù)學(xué)平臺(tái)姿態(tài)誤差、安裝誤差的影響時(shí)，式（17）可進(jìn)一步寫為

v^^b=（I－［δA］）C^b_n（Ι+［Φ］）v^ⁿ（18）

式中：C^b_n為載車真實(shí)姿態(tài)矩陣Cⁿ_b的轉(zhuǎn)置;［δA］、［Φ］分別為矢量Φ和δA的反對(duì)稱陣，即

記導(dǎo)航坐標(biāo)系下載車速度的真實(shí)值為vⁿ，航姿系統(tǒng)的速度誤差為δvⁿ，即v^ⁿ=vⁿ+δvⁿ，將該式代入式（18），并展開(kāi)式（18）的右邊，可得

忽略式（19）中關(guān)于誤差的二階及二階以上小量，并進(jìn)行整理變形可得

設(shè)航姿系統(tǒng)的速度誤差在車體坐標(biāo)系下的投影為δv^b，即δv^b=v^^b－v^b，結(jié)合式（20）可得

設(shè)δv^b=［δv_x，δv_y，δv_z］^T，δvⁿ=［δv_E，δv_N，δv_U］^T，將式（21）中各矩陣的表達(dá)式分別代入式（21）中，經(jīng)展開(kāi)、整理可得：

根據(jù)前文分析，將載車行駛過(guò)程中航姿系統(tǒng)的實(shí)際速度輸出在車體坐標(biāo)系x_b、z_b軸上的投影v^_x、v^_z作為航姿誤差在線估計(jì)的量測(cè)Z，即

式中：v^_x、v^_z可根據(jù)式（17）計(jì)算獲得。

由于載車行駛速度在車體坐標(biāo)系x_b、z_b軸上的投影應(yīng)該為零，因此式（25）可表示為

此時(shí)，將式（22）、式（24）代入到式（26）中，并結(jié)合狀態(tài)X的表達(dá)式式（14），可將式（26）寫為

Z=HX+V（27）

式中：H為量測(cè)矩陣，H=［H₁O_2×9H₂］;V為速度白噪聲V=［V_δvxV_δvz］，其中

式（27）即為航姿誤差在線估計(jì)的量測(cè)方程。從而，基于上述狀態(tài)方程與量測(cè)方程，就可以對(duì)系統(tǒng)狀態(tài)X即車載航姿系統(tǒng)的各種誤差進(jìn)行在線濾波估計(jì)，這里濾波算法采用如下離散型卡爾曼濾波基本方程，即

進(jìn)而通過(guò)對(duì)航姿誤差的在線濾波估計(jì)，可以實(shí)時(shí)計(jì)算獲得每個(gè)時(shí)刻航姿系統(tǒng)各種誤差的最優(yōu)估計(jì)值X^_k，此時(shí)利用該估計(jì)值對(duì)航姿系統(tǒng)進(jìn)行誤差在線修正補(bǔ)償。

最后，利用修正后的載車姿態(tài)矩陣Cⁿ_b實(shí)時(shí)計(jì)算出載車的航向、姿態(tài)角，即可實(shí)現(xiàn)車載航姿系統(tǒng)誤差的在線自主補(bǔ)償。

4 實(shí)驗(yàn)驗(yàn)證

4.1 仿真驗(yàn)證及結(jié)果分析

設(shè)航姿系統(tǒng)中陀螺常值漂移為0.03°/h，白噪聲隨機(jī)游走為0.003°/h;加速度計(jì)常值偏置為10^-4g，白噪聲隨機(jī)游走為10^-5g·s;航姿系統(tǒng)初始對(duì)準(zhǔn)水平精度為3′，方位精度為10′，初始速度誤差為0 m/s，初始位置誤差為10 m，其沿車體坐標(biāo)系x、y、z軸的安裝誤差分別為5′、10′、8′;航姿解算過(guò)程中載車處于運(yùn)動(dòng)狀態(tài)。

設(shè)計(jì)不同的行駛軌跡，對(duì)航姿誤差補(bǔ)償方法分別進(jìn)行50次仿真，并對(duì)50次仿真結(jié)果的絕對(duì)值求均值，以此作為航姿誤差補(bǔ)償效果的評(píng)估依據(jù)。首先，設(shè)計(jì)僅做勻速直線運(yùn)動(dòng)的簡(jiǎn)單軌跡，將其記為運(yùn)動(dòng)軌跡1，假設(shè)載車不做任何機(jī)動(dòng)，即始終做勻速直線運(yùn)動(dòng)，仿真時(shí)間為900 s，載車運(yùn)動(dòng)軌跡如圖2所示。

為了充分說(shuō)明航姿誤差補(bǔ)償效果，基于圖2中的載車運(yùn)動(dòng)軌跡，對(duì)誤差補(bǔ)償前后車載航姿系統(tǒng)分別進(jìn)行仿真驗(yàn)證，仿真結(jié)果如圖3所示。

圖3中“實(shí)線”為補(bǔ)償后的航姿誤差曲線，“虛線”為未補(bǔ)償?shù)暮阶苏`差曲線。由圖3可知，在勻速直線運(yùn)動(dòng)條件下，基于運(yùn)動(dòng)約束的航姿誤差補(bǔ)償方法依然取得了較為理想的補(bǔ)償效果：航向、俯仰、橫滾角誤差收斂效果顯著，3個(gè)角的誤差分別穩(wěn)定在1.5′、0.7′和0.4′以內(nèi)，而且在仿真第100 s左右時(shí)航姿誤差就達(dá)到穩(wěn)定狀態(tài);而未經(jīng)誤差補(bǔ)償?shù)暮阶讼到y(tǒng)的誤差依舊發(fā)散，航姿誤差最高達(dá)11.5′。這說(shuō)明該航姿誤差補(bǔ)償方法能夠適用于載車僅做勻速直線運(yùn)動(dòng)的情況，而且補(bǔ)償效果良好。

隨后，設(shè)計(jì)包含加速、勻速、轉(zhuǎn)彎、爬坡、下坡等多種運(yùn)動(dòng)形式的復(fù)雜軌跡，記為運(yùn)動(dòng)軌跡2，仿真時(shí)間為3 600 s，載體運(yùn)動(dòng)軌跡2如圖4所示。

基于圖4中的載車運(yùn)動(dòng)軌跡，對(duì)誤差補(bǔ)償前后的車載航姿系統(tǒng)分別進(jìn)行仿真驗(yàn)證，結(jié)果如圖5所示。

根據(jù)圖5可以看出，未經(jīng)誤差補(bǔ)償?shù)暮阶讼到y(tǒng)根本無(wú)法完成高精度定姿，仿真3 600 s后誤差最高達(dá)16.6′;而經(jīng)過(guò)誤差補(bǔ)償以后，航姿系統(tǒng)的航向、俯仰、橫滾角誤差均取得了顯著的收斂效果：3個(gè)角的誤差分別穩(wěn)定在5.2′、0.5′和0.3′以內(nèi)。同時(shí)，根據(jù)圖5中誤差補(bǔ)償后的結(jié)果可以看出，相比于兩個(gè)水平姿態(tài)角，航向角的精度明顯略低，這是由于在航姿誤差在線估計(jì)的狀態(tài)空間模型中，數(shù)學(xué)平臺(tái)天向誤差角的可觀測(cè)性較差，從而導(dǎo)致航向角誤差的補(bǔ)償效果也相對(duì)較差。

4.2 跑車實(shí)驗(yàn)驗(yàn)證及結(jié)果分析

利用實(shí)驗(yàn)室現(xiàn)有的激光捷聯(lián)慣導(dǎo)系統(tǒng)開(kāi)展航姿誤差自主補(bǔ)償實(shí)驗(yàn)驗(yàn)證。為了獲得高精度的航姿參考信息，將激光捷聯(lián)慣導(dǎo)系統(tǒng)與衛(wèi)星導(dǎo)航系統(tǒng)進(jìn)行組合導(dǎo)航，以組合導(dǎo)航輸出的航向、姿態(tài)信息作為航姿參考基準(zhǔn);以純捷聯(lián)慣導(dǎo)輸出的航向、姿態(tài)信息作為航姿系統(tǒng)輸出，在此基礎(chǔ)上進(jìn)行航姿誤差自主補(bǔ)償，主要實(shí)驗(yàn)設(shè)備及實(shí)驗(yàn)車如圖6和圖7所示。其中，激光捷聯(lián)慣導(dǎo)系統(tǒng)中陀螺儀常值漂移低于0.01°/h，加速度計(jì)常值偏置低于10^-4g，系統(tǒng)水平對(duì)準(zhǔn)精度優(yōu)于1′，方位對(duì)準(zhǔn)精度優(yōu)于3′;衛(wèi)星導(dǎo)航定位精度優(yōu)于10 m，測(cè)速精度優(yōu)于0.1 m/s。

依托上述實(shí)驗(yàn)設(shè)備在校園內(nèi)開(kāi)展跑車實(shí)驗(yàn)，在跑車實(shí)驗(yàn)過(guò)程中，載車在道路上正常行駛，期間包含有加速、減速、轉(zhuǎn)彎、爬坡、下坡以及勻速等運(yùn)動(dòng)形式，但不做刻意機(jī)動(dòng)，車速最高為25 km/h，跑車行駛軌跡如圖8所示。

航姿解算時(shí)間為1 200 s，基于相同的行駛路線對(duì)航姿誤差補(bǔ)償方法先后進(jìn)行10次跑車實(shí)驗(yàn)。將航姿誤差補(bǔ)償前、后的結(jié)果與航姿參考基準(zhǔn)作比較，從而得出每次航姿誤差實(shí)驗(yàn)結(jié)果，并對(duì)10次實(shí)驗(yàn)結(jié)果的絕對(duì)值求均值，實(shí)驗(yàn)結(jié)果如圖9所示。

從上述跑車實(shí)驗(yàn)結(jié)果可以看出，利用本文所研究的方法對(duì)車載航姿系統(tǒng)的誤差進(jìn)行補(bǔ)償取得了明顯的效果：在1 200 s的實(shí)驗(yàn)時(shí)間內(nèi)，系統(tǒng)的航向角誤差能夠最終穩(wěn)定在3.3′以內(nèi)，俯仰角、橫滾角誤差則均穩(wěn)定在0.4′以內(nèi)，而且航姿誤差能夠保持有效收斂。相比之下，補(bǔ)償前系統(tǒng)的航向誤差則明顯發(fā)散，最大達(dá)到5.1′;俯仰角和橫滾角誤差也呈發(fā)散趨勢(shì)，最大達(dá)到0.8′;同時(shí)，可以看出，隨著定姿時(shí)間的不斷推移，未經(jīng)誤差補(bǔ)償?shù)暮阶讼到y(tǒng)的誤差將會(huì)越來(lái)越大。因此，跑車實(shí)驗(yàn)結(jié)果也證明了本文所研究方法的有效性和必要性。

5 結(jié) 論

為了有效提高車載捷聯(lián)航姿系統(tǒng)的長(zhǎng)航時(shí)定姿精度，本文提出利用載車的運(yùn)動(dòng)約束條件對(duì)航姿系統(tǒng)的誤差進(jìn)行在線自主補(bǔ)償。通過(guò)將載車行駛過(guò)程中航姿系統(tǒng)的速度輸出在車體坐標(biāo)系的投影分量作為誤差補(bǔ)償濾波的量測(cè)，采用卡爾曼濾波對(duì)航姿誤差進(jìn)行實(shí)時(shí)估計(jì)，并利用估計(jì)結(jié)果對(duì)航姿系統(tǒng)進(jìn)行誤差補(bǔ)償。所提方法自主性強(qiáng)、精度高、抗干擾性好、工程易于實(shí)現(xiàn)，特別適用于戰(zhàn)場(chǎng)復(fù)雜電磁環(huán)境下特種車輛的高精度定姿定向，故具有較為廣泛的應(yīng)用前景。

參考文獻(xiàn)

［1］黎蕾蕾， 孫紅星， 李德仁， 等. 車載移動(dòng)測(cè)量中定位定姿系統(tǒng)誤差校正與補(bǔ)償研究［J］. 武漢大學(xué)學(xué)報(bào)： 信息科學(xué)版， 2016， 41（9）： 1245-1252.

LI L L， SUN H X， LI D R， et al. Calibration and compensation study on position and orientation system in mobile mapping application［J］. Geomatics and Information Science of Wuhan University， 2016， 41（9）： 1245-1252.

［2］KEIGHOBADI J， FARAJI J， JANABI-SHARIFI F， et al. Design and experimental evaluation of block-pulse functions and Legendre polynomials observer for attitude-heading reference system［J］. ISA Transactions， 2021， 116（10）： 232-244.

［3］LI D H， GAO Y， XIE J X， et al. Aircraft attitude data fusion based on gradient descent algorithm［C］∥Proc.of the 5th International Conference on Robotics， Control and Automation， 2021.

［4］王麗芬， 楊功流， 單友東， 等. 車載定位定向系統(tǒng)誤差在線補(bǔ)償方法［J］. 中國(guó)慣性技術(shù)學(xué)報(bào)， 2015， 23（2）： 145-149.

WANG L F， YANG G L， SHAN Y D， et al. On-line error compensation of vehicular position and orientation determination system［J］. Journal of Chinese Inertial Technology， 2015， 23（2）： 145-149.

［5］NIKOLAY V， MIKHAIL V， DMITRY T. Heading and attitude determination system with low-cost IMU embedded inside one of multiple antennas［C］∥Proc.of the IEEE/ION Position， Location and Navigation Symposium， 2018.

［6］LUO L， HUANG Y L， ZHANG Z， et al. A position loci-based in-motion initial alignment method for low-cost attitude and heading reference system［J］. IEEE Trans.on Instrumentation and Measurement， 2021， 70： 7500618.

［7］鄧瑀， 嚴(yán)恭敏， 楊小康. 基于SINS/GNSS/ADS融合的MEMS航姿參考系統(tǒng)研究［C］∥慣性技術(shù)發(fā)展動(dòng)態(tài)發(fā)展方向研討會(huì)——慣性傳感器技術(shù)與應(yīng)用， 2020.

DENG Y， YAN G M， YANG X K. Research on MEMS attitude and heading reference system based on SINS/GNSS/ADS［C］∥Proc.of the Conference on the Dynamic Development Direction of Inertial Technology—Inertial Technology and Application， 2020.

［8］SUN W， SUN P L， WU J J. An adaptive fusion attitude and heading measurement method of MEMS/GNSS based on cova-riance matching［J］. Micromachines， 2022， 13（10）： 1787.

［9］TAO C H， SONG Z P， WENG Z P. MCTLS-assisted completed SINS/GPS integrated and applied to low-cost attitude and heading reference system［J］. Mathematical Problems in Engineering， 2021， 2021： 4260162.

［10］胡永江， 武斌， 陳鵬， 等. 小型尾坐式飛行器航姿測(cè)量系統(tǒng)設(shè)計(jì)與驗(yàn)證［J］. 系統(tǒng)工程與電子技術(shù)， 2017， 39（4）： 866-875.

HU Y J， WU B， CHEN P， et al. Design and validation of attitude and heading measurement system of small tail-sitter aircraft［J］. Systems Engineering and Electronics， 2017， 39（4）： 866-875.

［11］李翔， 石琦. 融合光流與慣性傳感器的擴(kuò)展卡爾曼姿態(tài)濾波［J］. 電子測(cè)量技術(shù)， 2021， 44（17）： 88-92.

LI X， SHI Q. Extended Kalman attitude filter incorporating optical flow and inertial sensors［J］. Electronic Measurement Technology， 2021， 44（17）： 88-92.

［12］LIU X， YANG J， LI W S， et al. Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system［J］. IEEE Trans.on Industrial Informatics， 2023， 19（1）： 62-73.

［13］陳思， 仲啟媛， 譚立龍， 等. 基于RFID的車載定位定向技術(shù)研究與應(yīng)用［J］. 火炮發(fā)射與控制學(xué)報(bào)， 2019， 40（1）： 13-18.

CHEN S， ZHONG Q Y， TAN L L， et al. Research and application of vehicle position and azimuth determining technology based on RFID［J］. Journal of Gun Launch amp; Control， 2019， 40（1）： 13-18.

［14］WANG M， WANG L， HAN H. Research on improving heading and attitudes accuracy by online calibration of errors based on multi-RINSs joint rotation modulation［J］. IEEE Sensors Journal， 2022， 22（5）： 4503-4513.

［15］LI Y Y， ZHU T， LI R Y. Analysis of attitude and heading computer system in magnetic disturbance state［C］∥Proc.of the 4th International Conference on Artificial Intelligence and Advanced Manufacturing， 2022： 202-207.

［16］王平， 張廣鵬， 楊波， 等. 基于零速的航姿誤差補(bǔ)償方法研究［J］. 儀器儀表學(xué)報(bào)， 2018， 39（11）： 125-132.

WANG P， ZHANG G P， YANG B， et al. Research on error compensation method of attitude and heading based on zero velocity［J］. Chinese Journal of Scientific Instrument， 2018， 39（11）： 125-132.

［17］FARHANGIAN F， LANDRY R. Accuracy improvement of attitude determination systems using EKF-based error prediction filter and PI controller［J］. Sensors， 2020， 20（14）： 4055.

［18］VERTZBERGER E， KLEIN I. Attitude adaptive estimation with smartphone classification for pedestrian navigation［J］. IEEE Sensors Journal， 2021， 21（7）： 9341-9348.

［19］VERTZBERGER E， KLEIN I. Attitude and heading adaptive estimation using a data driven approach［C］∥Proc.of the International Conference on Indoor Positioning and Indoor Navigation， 2021.

［20］FARAHAN S B， MACHADO J J M， GOMES F A D， et al. 9-DOF IMU-based attitude and heading estimation using an extended Kalman filter with bias consideration［J］. Sensors， 2022， 22（9）： 3416.

［21］ASSEFINEW W， JINSEOK J， SEUNGKI K， et al. Improved attitude and heading accuracy with double quaternion parameters estimation and magnetic disturbance rejection［J］. Sensors， 2021， 21（16）： 5475.

［22］YU J Y， ZHANG X， KHAN A S M. Attitude heading refe-rence algorithm based on transformed cubature Kalman filter［J］. Measurement and Control， 2020， 53（7/8）： 1446-1453.

［23］王琮， 陳安升， 陳帥， 等. MIMU/GNSS/ODO/高度計(jì)/航姿儀組合導(dǎo)航微系統(tǒng)硬件設(shè)計(jì)［J］. 航天控制， 2020， 38（5）： 73-79.

WANG C， CHEN A S， CHEN S， et al. Hardware design of MIMU/GNSS/ODO/altimeter/attitude instrument integrated navigation system［J］. Aerospace Control， 2020， 38（5）： 73-79.

［24］FAN J C， WANG Z C， LIN M X， et al. Research on vehicle attitude and heading reference system based on multi-sensor information fusion［J］. Proceedings of the Institution of Mechanical Engineers， Part D Journal of Automobile Engineering， 2020， 234（13）： 3056-3067.

［25］YANG B， XI J X， YANG J， et al. An alignment method for strapdown inertial navigation systems assisted by Doppler radar on a vehicle-borne moving base［J］. Sensors， 2019， 19（20）： 4577.

［26］LI X， LI H X， HUANG G W， et al. Non-holonomic constraint （NHC）-assisted GNSS/SINS positioning using a vehicle motion state classification （VMSC）-based convolution neural network［J］. GPS Solutions， 2023， 27（3）： 144-153.

［27］秦永元， 張洪鉞， 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理［M］. 4版. 西安： 西北工業(yè)大學(xué)出版社， 2021.

QIN Y Y， ZHANG H Y， WANG S H. Kalman filter and integrated navigation principle［M］. 4th ed. Xi’an： Northwestern Polytechnic University Press， 2021.

［28］YANG B， WANG Y G， XUE L， et al. Accurate integrated position and orientation method for vehicles based on strapdown inertial navigation system/Doppler radar［J］. Measurement and Control， 2018， 51（9/10）： 431-442.

作者簡(jiǎn)介

楊 波（1980—），男，副教授，博士研究生導(dǎo)師，博士，主要研究方向?yàn)閼T性導(dǎo)航與組合導(dǎo)航、自主定位定向技術(shù)。

劉 楓（1998—），男，碩士研究生，主要研究方向?yàn)閼T性導(dǎo)航與組合導(dǎo)航。

薛 亮（1982—），男，副教授，博士，主要研究方向?yàn)镸EMS慣性傳感器、多傳感器數(shù)據(jù)融合。

劉云峰（1974—），男，高級(jí)工程師，博士，主要研究方向?yàn)樾畔⒀b備應(yīng)用和質(zhì)量管控。