基于改進(jìn)卡爾曼濾波的水產(chǎn)養(yǎng)殖無人船導(dǎo)航方法*

曹莉凌，尚 進(jìn)，曹守啟

（上海海洋大學(xué)工程學(xué)院，上海 201306）

0 引 言

實(shí)現(xiàn)無人船的高效精準(zhǔn)導(dǎo)航，并對船體位置、速度實(shí)時(shí)控制，是實(shí)現(xiàn)水產(chǎn)養(yǎng)殖無人船自主巡航、智能投料、水質(zhì)監(jiān)測、水面巡檢、科學(xué)喂養(yǎng)與高效管理的重要前提與保障［1］。慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）采用加速計(jì)測量載體的加速度，利用加速度積分項(xiàng)能夠得到載體速度，但是隨著時(shí)間推移，位置誤差積累增加［2，3］。全球定位系統(tǒng)（global positioning system，GPS）［4，5］易受惡劣天氣影響造成無人作業(yè)養(yǎng)殖船GPS信號弱。GPS／INS組合導(dǎo)航，總體性能遠(yuǎn)優(yōu)于任一獨(dú)立導(dǎo)航系統(tǒng)，是目前導(dǎo)航領(lǐng)域最為理想的導(dǎo)航組合方式［6］。

記憶漸消濾波是一種基于新息的自適應(yīng)濾波方法，其核心是如何選取最佳的漸消因子值［7］。文獻(xiàn)［8］中漸消因子推算過程繁雜，不易理解，不適合水產(chǎn)養(yǎng)殖無人船導(dǎo)航系統(tǒng)。文獻(xiàn)［9］從新增益與歷史增益關(guān)系入手，增加計(jì)算的未知變量，利用增益與預(yù)測協(xié)方差關(guān)系，得到漸消因子，計(jì)算過程忽視了量測方差值的影響。文獻(xiàn)［10］自適應(yīng)無跡卡爾曼濾波（unscented Kalman filtering，UKF）算法，結(jié)合最大后驗(yàn)概率估計(jì)準(zhǔn)則和矩陣奇異值分解技術(shù)，計(jì)算過程繁瑣且不易理解，對處理器運(yùn)算能力要求高，不適合用在水產(chǎn)養(yǎng)殖無人船上。本文利用新息協(xié)方差計(jì)算值與新息協(xié)方差預(yù)測值之間關(guān)系確定漸消因子取值，顯得更加簡潔易懂，更適用于實(shí)際養(yǎng)殖無人船導(dǎo)航系統(tǒng)應(yīng)用。

1 卡爾曼濾波算法模型

1.1 標(biāo)準(zhǔn)卡爾曼濾波模型

設(shè)定系統(tǒng)狀態(tài)模型和觀測模型分別為［11］

式中 Φk，k-1為狀態(tài)轉(zhuǎn)移矩陣，為k-1 時(shí)刻到k時(shí)刻的系統(tǒng)一步轉(zhuǎn)移矩陣；Hk為觀測矩陣，Φk，k-1和Hk均為雅可比矩陣，Xk為k時(shí)刻的狀態(tài)向量，Zk為k時(shí)刻的觀測量，Γk-1為系統(tǒng)噪聲矩陣，Wk-1為系統(tǒng)噪聲，Vk為量測噪聲。

1.2 漸消卡爾曼濾波原理

當(dāng)針對系統(tǒng)所建立的數(shù)學(xué)模型不精確或物理系統(tǒng)本身運(yùn)行過程中發(fā)生改變，最新的觀測數(shù)據(jù)比過去的觀測數(shù)據(jù)更加能反映實(shí)際情況。采用濾波器使得，隨著時(shí)間的增長逐漸“忘記”過去老的觀測數(shù)據(jù)，這種濾波方法稱為漸消因子濾波［7，12］。

系統(tǒng)狀態(tài)一步預(yù)測表示為

式中為k時(shí)刻狀態(tài)一步預(yù)測值，為k-1時(shí)刻的狀態(tài)最優(yōu)估計(jì)。k時(shí)刻狀態(tài)值減去k時(shí)刻一步預(yù)測值可以得到狀態(tài)一步預(yù)測誤差。

針對系統(tǒng)狀態(tài)一步預(yù)測均方誤差，典型的嵌入漸消因子的方式有以下3種［13］

式中Pk-1為預(yù)測協(xié)方差陣，λk為漸消因子，且λk≥1，將濾波協(xié)方差膨脹λk倍，加重現(xiàn)實(shí)測量數(shù)據(jù)的狀態(tài)估計(jì)的作用，從而避免濾波器發(fā)散。本文選取第二種方式，如式（5）所示，漸消因子在等式外，結(jié)構(gòu)上具有一定優(yōu)勢［13］。

狀態(tài)估計(jì)為

如果僅使用系統(tǒng)狀態(tài)方程的狀態(tài)一步預(yù)測去估計(jì)Xk并沒有考慮到量測方程的任何信息，這導(dǎo)致估計(jì)精度不高，因此可令＝Zk-，為量測預(yù)測誤差（即新息），Kk為待定的修正系數(shù)矩陣，來調(diào)節(jié)兩種數(shù)據(jù)之間權(quán)重也就是濾波增益。

濾波增益為

狀態(tài)估計(jì)均方差為

1.3 漸消因子分析

由于新息能反映真實(shí)量測值與估計(jì)量測值之間的關(guān)系，所以常用來衡量卡爾曼濾波器的濾波性能。

新息序列為

協(xié)方差陣為

當(dāng)Kk最優(yōu)時(shí)，新息序列是白噪聲序列，而新息自協(xié)方差函數(shù)為

假如卡爾曼濾波最優(yōu)，新息序列是白噪聲序列，則Sk＝0，即新息序列不相關(guān)，處處保持正交，Kk為最優(yōu)增益。假設(shè)＝λkPk，k-1＝αkCk。其中，Ck為理論為估計(jì)，λk為漸消因子，αk為標(biāo)量因子。

文獻(xiàn)［13］中新增益陣為

計(jì)算αk，首先要確定，由文獻(xiàn)［9］得

對＝αkCk兩邊求跡運(yùn)算，考慮到比例系數(shù)αk不宜小于1，得

把式（8）代入式（12）得

假定Pk，k-1和均為滿秩，則等式兩邊先左乘，再右乘，得

如果Hk滿秩，則

Pk，k-1＝λk［Φk，k-1Pk-1+Qk-1］代入（HkPk，k-1+Rk）＝，并且對等式兩邊求跡得

以上方法需要假設(shè)條件太多，而且計(jì)算過程復(fù)雜繁瑣，不易理解。

1.4 簡易的漸消因子選取方法

建模準(zhǔn)確情況下，標(biāo)準(zhǔn)卡爾曼濾波的新息序列在理論上是白噪聲

實(shí)際的新息時(shí)間序列估計(jì)方差等于式（14），但是求出的新息協(xié)方差是對歷史信息求平均值，并不能實(shí)時(shí)得出當(dāng)前時(shí)刻的信息。根據(jù)文獻(xiàn)［13］對其改進(jìn)如下

式中Z0為k＝0時(shí)的新息向量；經(jīng)過修改后的不需要對歷史信息求均值更能精確敏銳地反映當(dāng)前時(shí)刻系統(tǒng)模型誤差狀態(tài)。

當(dāng)濾波情況最優(yōu)時(shí)，新息協(xié)方差的理論值應(yīng)與新息協(xié)方差的預(yù)測值相等，即

文獻(xiàn)［13］的改進(jìn)算法中，系統(tǒng)中存在某些不明確的建模參數(shù)，隨著濾波進(jìn)行可能會導(dǎo)致新息失配。因此，本文在文獻(xiàn)［13］改進(jìn)算法的基礎(chǔ)上，引入漸消因子實(shí)現(xiàn)系統(tǒng)的自適應(yīng)調(diào)節(jié)，即

經(jīng)移項(xiàng)，得

等式兩邊求跡，得

考慮到比例系數(shù)λk不宜小于1，可得

1.5 水產(chǎn)養(yǎng)殖無人船組合導(dǎo)航系統(tǒng)耦合方式

水產(chǎn)養(yǎng)殖無人船組合導(dǎo)航系統(tǒng)采用松耦合方式，因?yàn)樗神詈蠈?dǎo)航結(jié)構(gòu)在實(shí)際工程應(yīng)用中易于實(shí)現(xiàn)且導(dǎo)航信息具有一定余度［14］。在位置、速度方式下GPS／INS松耦合組合導(dǎo)航誤差模型中，選取慣性導(dǎo)航系統(tǒng)的位置誤差（δL，δλ，δh）、速度誤差（δVE，δVN，δVU）、姿態(tài)誤差（φE，φN，φU）、陀螺儀（εx，εy，εz）、加速度計(jì)）的一階馬爾可夫過程共15個(gè)作為組合導(dǎo)航系統(tǒng)的狀態(tài)變量，即

式中δL，δλ，δh分別為緯度、經(jīng)度和高度誤差；δVE，δVN，δVU分別為東向、北向和天向速度誤差；φE，φN，φU分別為東向、北向和天向的3個(gè)姿態(tài)誤差角；εx，εy，εz分別為三軸陀螺誤差；分別為加速度計(jì)誤差。

GPS／INS松耦合組合導(dǎo)航觀測方程有2 組量測值，一組是位置量測值，即慣性導(dǎo)航系統(tǒng)給出的緯度、經(jīng)度以及高度信息和GPS接收機(jī)給出相應(yīng)位置信息的差值，另一組是慣導(dǎo)系統(tǒng)和GPS系統(tǒng)各方向上速度的差值。即

2 系統(tǒng)仿真分析

仿真無人船運(yùn)行軌跡，再比較漸消因子卡爾曼濾波效果和標(biāo)準(zhǔn)卡爾曼濾波效果，比較漸消因子卡爾曼濾波是否比標(biāo)準(zhǔn)卡爾曼濾波在精度上有所改進(jìn)。用軌跡發(fā)生器設(shè)計(jì)無人船初始位置為緯度2. 07°、經(jīng)度0. 7°，初速度設(shè)為0.4 m／s，選取仿真時(shí)間600 s進(jìn)行無人船行駛軌跡仿真，仿真軌跡如圖1所示。

圖1 無人船行駛軌跡

利用平臺仿真實(shí)現(xiàn)了本文提出的漸消因子卡爾曼濾波及常規(guī)自適應(yīng)卡爾曼濾波的導(dǎo)航算法，對水產(chǎn)養(yǎng)殖無人船在600 s行駛時(shí)間內(nèi)經(jīng)緯度位置誤差及速度誤差進(jìn)行了對比。圖2為基于擴(kuò)展卡爾曼濾波（extended Kalman filtering，EKF）算法對無人船經(jīng)度、緯度位置與速度誤差的估計(jì)，圖3為基于漸消因子的改進(jìn)的強(qiáng)跟蹤卡爾曼濾波（strong tracking Kalman filtering，STKF）算法對無人船經(jīng)度、緯度位置和速度誤差的估計(jì)。本文對圖2和圖3 位置與速度誤差范圍進(jìn)行統(tǒng)計(jì)，其結(jié)果如表1。

表1 2 種算法誤差估計(jì)（°）

圖2 EKF算法速度、位置誤差估計(jì)

圖3 STKF算法的位置和速度誤差估計(jì)

通過表1在經(jīng)緯度上的位置、速度誤差估計(jì)數(shù)據(jù)對比，STKF算法比EKF 算法在位置誤差估算方面，精度提高了1.6～3.3倍，在速度誤差估算方面，精度提高了1.02～4倍。

3 結(jié)束語

本文提出的STKF算法，通過漸消因子在線調(diào)節(jié)預(yù)測協(xié)方差矩陣，間接調(diào)節(jié)增益矩陣，使濾波效果達(dá)到最佳效果。計(jì)算過程中增益值變化由預(yù)測協(xié)方差變化所致，直接利用理論新息協(xié)方差和預(yù)測新息協(xié)方差相等，直接計(jì)算漸消因子，通過漸消因子調(diào)節(jié)合適預(yù)測協(xié)方差值繼而得到增益。這種方法計(jì)算量小，便于理解，誤差精度上也比EKF 有一定改善。在水產(chǎn)養(yǎng)殖無人船導(dǎo)航中利用本文提出的漸消因子自適應(yīng)卡爾曼濾波來融合INS和GPS 兩種導(dǎo)航系統(tǒng)，適用性更強(qiáng)。