載體行進間對準桿臂誤差補償算法*

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

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

0 引言

裝有慣性器件的載體在行進間完成初始對準時，理想情況是慣性器件和相應的輔助測量設備安裝在載體中心。但是,由于載體的設計和使用的需求，慣性測量器件和相應的輔助設備一般不能安裝在一起。同時,載體在行進間初始對準的過程中，由于載體自身的晃動和線運動，不在載體運動中心的慣性器件和輔助測量器件由于安裝位置使得器件的測量值中含有桿臂誤差。

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

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

1 桿臂誤差機理

為說明桿臂效應對于慣性測量器件的影響，根據文獻[1]桿臂效應的機理如圖1所示。

圖1 桿臂效應原理圖Fig.1 Principle of lever arm effect

圖中，OiXiYiZi為慣性坐標系，ObXbYbZb為載體坐標系，加速度計安裝于載體坐標系的P點處。載體坐標系相對于慣性坐標系存在晃動，晃動角速度為ωib，同時令OiOb=rb，OiP=RP，ObP=rP。

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

(1)

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

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

(2)

對于式(2)進行變形得

(3)

由式(3)可知，由于桿臂效應引起的加速度計的測量誤差為

(4)

由式(4)可知，載體的桿臂誤差主要和載體姿態變化的角速度有關。為了實現行進間桿臂效應的精確補償，需要對于式(4)中的rp進行精確的測量。在此基礎上，運用慣性導航系統提供的載體的轉動角速度和角度，進行載體的力學補償。

2 行進間桿臂誤差的力學補償

載體行進過程中，rp的準確測量成為實現桿臂誤差力學補償的關鍵。為保持載體的機動性，隨時停車測量桿臂是不現實的，同時由于不確定性桿臂[12]的存在，使得桿臂長度的準確計算變得十分困難。

當載體處在運動狀態下的時，根據文獻[14]，載體的運動機械方程可以表示為

(5)

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

(6)

(7)

(8)

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

(9)

對于式(9)進行變形：

(10)

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

3 非線性濾波模型建立

載體行進間桿臂效應誤差的實時補償，可以采用濾波的方法進行補償?？紤]到載體在行進過程中，粗對準結果航向失準角為大角度，應采用非線性差模型，并將桿臂所產生的誤差項進行考慮。

(11)

式中:

捷聯慣導實測位置和里程計的實測位置的差值作為量測值

(12)

綜上所述，行進間大方位失準角誤差方程為

(13)

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

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

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

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

(14)

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

(15)

為了解算式(15)，根據文獻[16]，高維積分的數值計算可以轉化為

(16)

式中:ri和wr,i為徑向積分的采樣點和采樣點對應的權值;sj和ws,j為球面積分的采樣點和采樣點對應的權值。I(h)的總采樣地點為NrNs個，當ri中有一個采樣點為0時，I(h)的總采樣點數為(Nr-1)·(Ns+1)個。

4.1 5階球面積分方法

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

(17)

式(17)為2m+1階球面積分數值解算方法，式中IUn表示被解算的函數。式中的權值函數和采樣點的函數值之和分別為

(18)

(19)

根據文獻[16],有

(20)

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

。

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

(21)

(22)

ws1所對應的采樣點為

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

(23)

4.2 徑向積分法則

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

(24)

因此，當運用時矩匹配法解算5階徑向高維積分時，得到如下方程：

(25)

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

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

(26)

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

(27)

5 仿真和實驗驗證

為了充分驗證采用力學方程和非線性誤差方程結合對于行進間對準過程中對于桿臂長度的估計精度和在補償桿臂誤差后對準過程中位置的影響，通過仿真和實際實驗的方法進行驗證。

5.1 仿真條件

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

5.2 實驗驗證

通過圖3桿臂在載體坐標系下仿真時間內的估計結果可以看出，在100 s以后，對于載體桿臂長度的估計趨于穩定。為表明載體桿臂對于對準精度的影響，通過無人車實際實驗的方法驗證對準過程中對于桿臂誤差的補償與否對于位置精度的影響。實驗過程中，采用高精度差分GPS所測量的實時路線作為路徑真值，GPS的輸出為1 Hz，定位精度為1 m。通過2次實驗對比補償桿臂誤差與否對于估計精度的影響實驗結果如圖4，5所示。

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

圖3 桿臂長度在不同方向上的估計值Fig.3 Estimate of the arm length in different directions

圖4 實驗用無人車Fig.4 Experiment UGV

圖5 實驗路徑圖Fig.5 Experimental path

從以上圖中不難看出，桿臂效應對于載體對準時位置估計精度的影響主要是在轉彎運動時，當載體直線運動狀態，即載體的姿態變化不大的情況下，桿臂效應對于載體定位誤差的影響不大。

6 結束語

通過對于載體行進間對準過程中桿臂長度和誤差補償的仿真和實驗表明：行進間對準過程中，對于載體桿臂誤差主要受到載體姿態變化角速度的影響，采用5階CKF濾波算法，將桿臂誤差考慮到濾波的狀態方程中，可以有效地提高載體在對準過程的定位精度，并且通過此算法，可以實現桿臂誤差的實時補償。