王律化,石志勇,宋金龍,王海亮

(陸軍工程大學(xué)石家莊校區(qū)，河北 石家莊 050003)

0 引言

裝有慣性器件的載體在行進(jìn)間完成初始對(duì)準(zhǔn)時(shí)，理想情況是慣性器件和相應(yīng)的輔助測(cè)量設(shè)備安裝在載體中心。但是,由于載體的設(shè)計(jì)和使用的需求，慣性測(cè)量器件和相應(yīng)的輔助設(shè)備一般不能安裝在一起。同時(shí),載體在行進(jìn)間初始對(duì)準(zhǔn)的過(guò)程中，由于載體自身的晃動(dòng)和線運(yùn)動(dòng)，不在載體運(yùn)動(dòng)中心的慣性器件和輔助測(cè)量器件由于安裝位置使得器件的測(cè)量值中含有桿臂誤差。

針對(duì)上述問(wèn)題，文獻(xiàn)[1]分析了桿臂誤差產(chǎn)生的機(jī)理，并運(yùn)用線性卡爾曼濾波對(duì)于微幅晃動(dòng)條件下的桿臂誤差進(jìn)行了補(bǔ)償。高精度捷聯(lián)慣導(dǎo)中，慣性測(cè)量器件不能等效為“點(diǎn)器件”[2-4]，因此，慣性器件內(nèi)部也會(huì)存在桿臂誤差，稱為內(nèi)桿臂誤差。為此，文獻(xiàn)[5-6]分析了慣性測(cè)量器件的內(nèi)桿臂誤差，得出內(nèi)桿臂誤差與加速度計(jì)距離3個(gè)加速度計(jì)敏感軸焦點(diǎn)的距離有關(guān)。當(dāng)載體處在晃動(dòng)條件下時(shí)，為了進(jìn)一步提高對(duì)于載體桿臂誤差的補(bǔ)償，文獻(xiàn)[7-9]提出了桿臂效應(yīng)的力學(xué)補(bǔ)償模型，但是由于安裝誤差和實(shí)際測(cè)量工具的原因[10-11]，使得在桿臂長(zhǎng)度的確定中，存在不確定桿臂[12]，桿臂數(shù)值的準(zhǔn)確測(cè)量變得十分困難。文獻(xiàn)[13]根據(jù)晃動(dòng)基座的對(duì)準(zhǔn)原理，運(yùn)用TRIAD(three-axis attitude determination)[14]雙矢量定姿算法，給出了載體在晃動(dòng)條件下確定桿臂長(zhǎng)度，并利用桿臂效應(yīng)的力學(xué)補(bǔ)償算法對(duì)于桿臂誤差進(jìn)行在線補(bǔ)償。當(dāng)載體存在線運(yùn)動(dòng)條件時(shí)，由于載體自身晃動(dòng)和線運(yùn)動(dòng)加速度的存在，使得按照上述方法解算桿臂長(zhǎng)度存在困難。

為解決載體行進(jìn)間桿臂誤差的在線補(bǔ)償問(wèn)題，在分析桿臂誤差產(chǎn)生的機(jī)理基礎(chǔ)上，推導(dǎo)了基于里程計(jì)輔助條件下慣性系統(tǒng)自對(duì)準(zhǔn)過(guò)程中解算桿臂長(zhǎng)度的方法。同時(shí)，載體行進(jìn)間對(duì)準(zhǔn)的過(guò)程中，考慮到航向失準(zhǔn)角為大角度，不能對(duì)于非線性的誤差模型線性化，因此要采用非線性濾波的方法對(duì)于桿臂誤差進(jìn)行在線估計(jì)。本文在補(bǔ)償算法上采用5階CKF(cubature Kalman filter)非線性濾波算法進(jìn)行在線補(bǔ)償。仿真和實(shí)驗(yàn)結(jié)果表明，該方法可以很好地補(bǔ)償由于桿臂所造成地行進(jìn)間對(duì)準(zhǔn)誤差，提高初始對(duì)準(zhǔn)精度。

1 桿臂誤差機(jī)理

為說(shuō)明桿臂效應(yīng)對(duì)于慣性測(cè)量器件的影響，根據(jù)文獻(xiàn)[1]桿臂效應(yīng)的機(jī)理如圖1所示。

圖1 桿臂效應(yīng)原理圖Fig.1 Principle of lever arm effect

圖中，OiXiYiZi為慣性坐標(biāo)系，ObXbYbZb為載體坐標(biāo)系，加速度計(jì)安裝于載體坐標(biāo)系的P點(diǎn)處。載體坐標(biāo)系相對(duì)于慣性坐標(biāo)系存在晃動(dòng)，晃動(dòng)角速度為ωib，同時(shí)令OiOb=rb，OiP=RP，ObP=rP。

因此，桿臂誤差可以表示為[1]

(1)

由于慣性器件固定在載體上，所以

因此，式(1)可以化簡(jiǎn)為

(2)

對(duì)于式(2)進(jìn)行變形得

(3)

由式(3)可知，由于桿臂效應(yīng)引起的加速度計(jì)的測(cè)量誤差為

(4)

由式(4)可知，載體的桿臂誤差主要和載體姿態(tài)變化的角速度有關(guān)。為了實(shí)現(xiàn)行進(jìn)間桿臂效應(yīng)的精確補(bǔ)償，需要對(duì)于式(4)中的rp進(jìn)行精確的測(cè)量。在此基礎(chǔ)上，運(yùn)用慣性導(dǎo)航系統(tǒng)提供的載體的轉(zhuǎn)動(dòng)角速度和角度，進(jìn)行載體的力學(xué)補(bǔ)償。

2 行進(jìn)間桿臂誤差的力學(xué)補(bǔ)償

載體行進(jìn)過(guò)程中，rp的準(zhǔn)確測(cè)量成為實(shí)現(xiàn)桿臂誤差力學(xué)補(bǔ)償?shù)年P(guān)鍵。為保持載體的機(jī)動(dòng)性，隨時(shí)停車測(cè)量桿臂是不現(xiàn)實(shí)的，同時(shí)由于不確定性桿臂[12]的存在，使得桿臂長(zhǎng)度的準(zhǔn)確計(jì)算變得十分困難。

當(dāng)載體處在運(yùn)動(dòng)狀態(tài)下的時(shí)，根據(jù)文獻(xiàn)[14]，載體的運(yùn)動(dòng)機(jī)械方程可以表示為

(5)

桿臂的力學(xué)誤差δfb根據(jù)式(4)可以表示為

(6)

(7)

(8)

由于載體在初始對(duì)準(zhǔn)的過(guò)程中運(yùn)用速度為3～5 m/s，粗對(duì)準(zhǔn)時(shí)的時(shí)間一般幾十秒，相較于所計(jì)算的桿臂誤差，可以忽略由于載體位置變化所引起的重力加速度的變化。將式(6)帶入到式(7)，(8)中，并用兩式作差，得

(9)

對(duì)于式(9)進(jìn)行變形：

(10)

將式(10)計(jì)算得到的rp帶入到式(6)中計(jì)算由于桿臂效應(yīng)所引起的加速度誤差。

3 非線性濾波模型建立

載體行進(jìn)間桿臂效應(yīng)誤差的實(shí)時(shí)補(bǔ)償，可以采用濾波的方法進(jìn)行補(bǔ)償?？紤]到載體在行進(jìn)過(guò)程中，粗對(duì)準(zhǔn)結(jié)果航向失準(zhǔn)角為大角度，應(yīng)采用非線性差模型，并將桿臂所產(chǎn)生的誤差項(xiàng)進(jìn)行考慮。

(11)

式中:

捷聯(lián)慣導(dǎo)實(shí)測(cè)位置和里程計(jì)的實(shí)測(cè)位置的差值作為量測(cè)值

(12)

綜上所述，行進(jìn)間大方位失準(zhǔn)角誤差方程為

(13)

式中：f(x)的表達(dá)式具體看式(1)；噪聲向量w和v是零均值，方差為Q和R的高斯白噪聲。量測(cè)矩陣表示為

H=(03×3,03×3,I3×3,-I3×3,03×3,07×3)T.

4 非線性濾波模型解算方法

處理式(13)的濾波方法一般為貝葉斯濾波。將式(13)進(jìn)行離散化處理表示為

(14)

針對(duì)式(13)，可以采用貝葉斯估計(jì)[15]的方法進(jìn)行解算，其解算過(guò)程中多涉及到如下形式的計(jì)算：

(15)

為了解算式(15)，根據(jù)文獻(xiàn)[16]，高維積分的數(shù)值計(jì)算可以轉(zhuǎn)化為

(16)

式中:ri和wr,i為徑向積分的采樣點(diǎn)和采樣點(diǎn)對(duì)應(yīng)的權(quán)值;sj和ws,j為球面積分的采樣點(diǎn)和采樣點(diǎn)對(duì)應(yīng)的權(quán)值。I(h)的總采樣地點(diǎn)為NrNs個(gè)，當(dāng)ri中有一個(gè)采樣點(diǎn)為0時(shí)，I(h)的總采樣點(diǎn)數(shù)為(Nr-1)·(Ns+1)個(gè)。

4.1 5階球面積分方法

形如IUn(hs)的積分，根據(jù)文獻(xiàn)[17]，可以按照下列公式進(jìn)行近似計(jì)算

(17)

式(17)為2m+1階球面積分?jǐn)?shù)值解算方法，式中IUn表示被解算的函數(shù)。式中的權(quán)值函數(shù)和采樣點(diǎn)的函數(shù)值之和分別為

(18)

(19)

根據(jù)文獻(xiàn)[16],有

(20)

式中：|k|=k1+k2+…+kn；Γ(z)為伽馬函數(shù)，其定義為

。

當(dāng)所要近似的積分階數(shù)是5階的時(shí)候，2m+1=5，則m=2。將結(jié)果帶入到式(18)中得

(21)

(22)

ws1所對(duì)應(yīng)的采樣點(diǎn)為

將式(21)和(22)以及采樣點(diǎn)代入到式(17)中

(23)

4.2 徑向積分法則

對(duì)于5階及5階以下的高維徑向積分，可以使用高斯-拉格朗日積分方法，但是，這個(gè)方法不能運(yùn)用于5維以上的數(shù)值解算。為提高徑向高維積分解算方法的適用性，采用時(shí)矩匹配法解算徑向積分。時(shí)矩匹配法具體表示為

(24)

因此，當(dāng)運(yùn)用時(shí)矩匹配法解算5階徑向高維積分時(shí)，得到如下方程：

(25)

式中:Γ(z+1)=zΓ(z)。

由于式(25)中有4個(gè)未知數(shù)，只有3個(gè)方程，同時(shí)為了保證采用時(shí)矩匹配法數(shù)值解算徑向積分的采樣點(diǎn)數(shù)最少，可以令r1=0，于是求得式(25)的解為

(26)

將式(26)，(23)，(16)代入到式(15)中，運(yùn)用5階容積規(guī)則(Nr=2,Ns=2n2)解算得

(27)

5 仿真和實(shí)驗(yàn)驗(yàn)證

為了充分驗(yàn)證采用力學(xué)方程和非線性誤差方程結(jié)合對(duì)于行進(jìn)間對(duì)準(zhǔn)過(guò)程中對(duì)于桿臂長(zhǎng)度的估計(jì)精度和在補(bǔ)償桿臂誤差后對(duì)準(zhǔn)過(guò)程中位置的影響，通過(guò)仿真和實(shí)際實(shí)驗(yàn)的方法進(jìn)行驗(yàn)證。

5.1 仿真條件

載體總的精對(duì)準(zhǔn)仿真時(shí)間為400 s，假設(shè)載體的初始位置為34.245 1°N/108.908 4°E，高度0 m，陀螺為激光陀螺，其常值漂移為0.015 (°)/h，隨機(jī)漂移為0.001 (°)/h，加速度計(jì)常值零偏為450 μg，隨機(jī)漂移為10 μg，里程計(jì)的刻度系數(shù)誤差為2‰，并且假設(shè)桿臂長(zhǎng)度沿載體坐標(biāo)3個(gè)方向的分別長(zhǎng)度為：(0.45,0.28,0.57) m，載體在運(yùn)動(dòng)過(guò)程中的路徑仿真如圖2,3所示。

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

通過(guò)圖3桿臂在載體坐標(biāo)系下仿真時(shí)間內(nèi)的估計(jì)結(jié)果可以看出，在100 s以后，對(duì)于載體桿臂長(zhǎng)度的估計(jì)趨于穩(wěn)定。為表明載體桿臂對(duì)于對(duì)準(zhǔn)精度的影響，通過(guò)無(wú)人車實(shí)際實(shí)驗(yàn)的方法驗(yàn)證對(duì)準(zhǔn)過(guò)程中對(duì)于桿臂誤差的補(bǔ)償與否對(duì)于位置精度的影響。實(shí)驗(yàn)過(guò)程中，采用高精度差分GPS所測(cè)量的實(shí)時(shí)路線作為路徑真值，GPS的輸出為1 Hz，定位精度為1 m。通過(guò)2次實(shí)驗(yàn)對(duì)比補(bǔ)償桿臂誤差與否對(duì)于估計(jì)精度的影響實(shí)驗(yàn)結(jié)果如圖4，5所示。

圖2 路徑仿真圖Fig.2 Path simulation

圖3 桿臂長(zhǎng)度在不同方向上的估計(jì)值Fig.3 Estimate of the arm length in different directions

圖4 實(shí)驗(yàn)用無(wú)人車Fig.4 Experiment UGV

圖5 實(shí)驗(yàn)路徑圖Fig.5 Experimental path

從以上圖中不難看出，桿臂效應(yīng)對(duì)于載體對(duì)準(zhǔn)時(shí)位置估計(jì)精度的影響主要是在轉(zhuǎn)彎運(yùn)動(dòng)時(shí)，當(dāng)載體直線運(yùn)動(dòng)狀態(tài)，即載體的姿態(tài)變化不大的情況下，桿臂效應(yīng)對(duì)于載體定位誤差的影響不大。

6 結(jié)束語(yǔ)

通過(guò)對(duì)于載體行進(jìn)間對(duì)準(zhǔn)過(guò)程中桿臂長(zhǎng)度和誤差補(bǔ)償?shù)姆抡婧蛯?shí)驗(yàn)表明：行進(jìn)間對(duì)準(zhǔn)過(guò)程中，對(duì)于載體桿臂誤差主要受到載體姿態(tài)變化角速度的影響，采用5階CKF濾波算法，將桿臂誤差考慮到濾波的狀態(tài)方程中，可以有效地提高載體在對(duì)準(zhǔn)過(guò)程的定位精度，并且通過(guò)此算法，可以實(shí)現(xiàn)桿臂誤差的實(shí)時(shí)補(bǔ)償。