，

(廣東交通職業(yè)技術學院，廣州 510800 )

針對船載GPS/DR組合導航系統(tǒng)的非線性航跡數(shù)據(jù)關聯(lián)問題[1-2]，提出一種附有航向約束條件的自適應UKF(unscented kalman filter)[3-4]算法CAUKF(constrained adaptive unscented kalman filter)，即采用UKF實現(xiàn)非線性系統(tǒng)中狀態(tài)估計的遞推，在此基礎上采用自適應方法對自主船位推算系統(tǒng)的狀態(tài)進行估計。導航系統(tǒng)原理見圖1。

圖1 船載導航系統(tǒng)原

相比于傳統(tǒng)的擴展卡爾曼濾波(extended kalman filter，EKF)，CAUKF不需系統(tǒng)模型的具體解析形式，而且有效減少了EKF引入的線性化誤差對數(shù)據(jù)關聯(lián)概率及目標狀態(tài)估計的影響，因此在減少運算量的同時具有更高的跟蹤精度和穩(wěn)定性。

1 船載導航系統(tǒng)模型

設采樣時間為T，對系統(tǒng)狀態(tài)變化過程采用機動載體的“當前”統(tǒng)計模型[5-6]，結合速度自適應算法，得到系統(tǒng)離散的狀態(tài)方程為

X(k+1)=ΦX(k)+Gw(k)

(1)

w(k)——零均值高斯白噪聲，其方差為

Φ、G——狀態(tài)矩陣和轉(zhuǎn)移矩陣,

Φ(k/k-1)=diag{Φx,Φy,e-αεwT,1}

(2)

其中：αεw——一階馬爾科夫過程的相關時間常數(shù)的倒數(shù)。

觀測值為距離s和方位角θ，其標準方差分別表示為σs和σθ，將其轉(zhuǎn)化到笛卡爾坐標系下，x=s·cosθ，y=s·sinθ，此時測量方程為

(3)

式中：vi(k)——笛卡爾坐標系下的量測噪聲。vi(k)與系統(tǒng)過程噪聲w(k)是相互獨立的，假定其為零均值高斯白噪聲且其方差矩陣為

(4)

由式(3)可見觀測方程是復雜的非線性函數(shù)。

2 非線性自適應UKF算法描述

Unscented變換的實質(zhì)是基于加權統(tǒng)計線性回歸技術來解決隨機變量的不確定性，即利用先驗分布構造一組確定性的采樣點(Sigma點)來獲取系統(tǒng)的相關統(tǒng)計參量，并利用線性回歸變換后的Sigma點表示狀態(tài)的后驗分布。這種統(tǒng)計線性化技術充分考慮了高斯隨機變量的統(tǒng)計特性[7]，和截斷Taylor級數(shù)的EKF相比，可以降低線性化誤差，而且其不需要模型的具體解析形式，避免了復雜的Jacobian矩陣的計算，更易于實現(xiàn)。

根據(jù)式(1)和式(3)描述的導航系統(tǒng)機動目標跟蹤問題，采用順序方式依次處理每個信息源的量測，將之轉(zhuǎn)化為處理多個非線性單信息源的目標跟蹤問題，以第i個信息源為例，最終的狀態(tài)估計與協(xié)方差估計由第Ni個信息源得到：

(5)

第i個信息源所對應的量測中間狀態(tài)估計與協(xié)方差估計為

(6)

(7)

假定狀態(tài)初始值X0滿足X0～N(0,Px,0)，Px,0為X0的方差陣，則非線性UKF實現(xiàn)如下。

(8)

式中:n——狀態(tài)向量的維數(shù)，

i=0,1,…,n,n+1,…,2n；

λ=α2(nx+κ)-n——一個尺度調(diào)節(jié)因子。

其中：κ——次級尺度調(diào)節(jié)因子，通常設置為0。

(9)

(10)

式中：m——隨機量測向量維數(shù);

β——用來描述x的分布信息,對于高斯分布，β的最優(yōu)值為2;

ωm、ωc——求一階統(tǒng)計特性時的權系數(shù)。

(11)

4) 計算輸出地一步提前預測，即

(12)

5) 在獲得新的測量zk后，進行濾波更新。

(13)

式中:Kk——濾波增益陣。

3 約束參考模型及其優(yōu)化估計

(14)

故增加約束條件的測量方程式(3)可改寫為

HX=F

(15)

(16)

由式(16)構建Lagrange方程為

(17)

因而可得最優(yōu)解為

(18)

4 仿真算例

為了驗證本文算法的有效性，應用自主開發(fā)的內(nèi)河船舶監(jiān)控系統(tǒng)(內(nèi)含自主繪制的電子地圖)和船載遠程終端[8]，在廣東鶴山港至香港中港碼頭航線間采集的實際航跡數(shù)據(jù)對CAUKF算法進行驗證。利用高斯-呂格投影將原有GPS數(shù)據(jù)轉(zhuǎn)換為笛卡爾直角坐標，選取GPS接收機輸出的東向(對應x向)、北向(對應y向)位置及陀螺儀輸出的相對轉(zhuǎn)角和里程儀輸出的采樣周期T內(nèi)行進的距離作為系統(tǒng)觀測量，系統(tǒng)的離散化最優(yōu)觀測方程參見式(3)和式(15)，其量測向量可表示為

(19)

式中:εk(w)——k時刻陀螺儀的隨機漂移誤差;

φ(k)——k時刻里程儀標定系數(shù)。

設初始狀態(tài)參數(shù)為[-0.5,0.014 5,0,-0.099 8]T，觀測噪聲各向量方差均為0.12，系統(tǒng)過程噪聲協(xié)方差陣Qk=diag[1,0.1,1,0.1]，εk(w)=0.01 rad/s，φ(k)=0.015 m/s，T=1 s，目標的動態(tài)矩陣及輸入轉(zhuǎn)移矩陣分別為

(20)

根據(jù)上述條件和參數(shù)，分別采用式(8)～(18)的CAUKF算法和EKF算法對GPS/DR組合導航系統(tǒng)進行10次Monte-Carlo仿真濾波，仿真時間為4 000 s。航跡跟蹤的仿真結果與真實航跡的比較見圖2。

圖2 算法濾波跟蹤航跡與真實航跡比較(局部放大)

可以看出，在運算量減少的情況下，CAUKF濾波后位置數(shù)據(jù)形成的航跡與EKF濾波后位置數(shù)據(jù)形成的航跡相比更加平滑，與實際航跡更加接近。

為了比較CAUKF和EKF算法的濾波精度，分別給出了x向和y向的CAUKF和EKF平均估計誤差變化曲線，見圖3、4。

圖3 x向的CAUKF和EKF算法平均估計誤差

圖4 y向的CAUKF和EKF算法平均估計誤差

可以看出，兩種算法平均估計誤差的變化趨勢基本相同，但由于CAUKF算法采用了確定性采樣的方法來處理觀測方程的非線性問題，避免了EKF引入的線性化誤差，因而其濾波精度和魯棒性明顯高于EKF。同時，隨著時間的推移，CAUKF算法能夠更快適應量測噪聲方差陣的動態(tài)變化，估計誤差收斂迅速，比EKF具有更好的跟蹤效果。

5 結束語

由于CAUKF采用Unscented變換的思想來處理系統(tǒng)觀測方程的非線性問題，所以用作目標跟蹤預測等典型的非線性濾波器時具有良好的性能表現(xiàn)。同時由于該算法不需計算非線性函數(shù)的Jacobian矩陣，有效避免了EKF引入的線性化誤差，具有更高的濾波精度和魯棒性，在實現(xiàn)時也更為方便。仿真結果表明，本濾波算法在非線性系統(tǒng)的狀態(tài)預測和目標跟蹤方面具有良好的實用性。

[1] BAR-Shalom Y.Tracking a maneuvering target using input estimation versus interacting multiple model algorithm[J].IEEE Transactions on Aerospace and Electronic Systems,1989,25(2)：296-300.

[2] CELINE B,TARUNRAJ S.Adaptive track fusion in a multi-sensor environment[C]//International Fusion 2000 Conference,Paris:The International Society of Information Fusion,2000：24-31.

[3] JULIER S J,ULMANN J K.Anew method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactions on Automatic Control,2000,45: 477-482.

[4] JULIER S J,UHLMANN J K.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE,2004,92(3)：401-422.

[5] JOTTO L,Venturini G.Development and Experimental Validation of an adaptive extended Kalman filter forlocalization of mobile robots[J].IEEE Trans on Robotics and Automation,1999,15(2)：219-229.

[6] 房建成，申功勛，萬德均.自適應卡爾曼濾波器在陸地車輛導航中的應用[J].北京航空航天大學學報，1999,25(2)：235-239.

[7] KAZUFUMI ITO,XIONG Kaiqi.Gauss Filters for Nonlinear Filtering Problems[J].IEEE Transactions on Automatic Control,2000,45(5)：910-927.

[8] 王貴恩，孫永林，葉 鳴.基于船位推算和GPS的嵌入式內(nèi)河船舶遠程終端設計[J].交通與計算機，2007,25(3)：91-93.