傳感器數(shù)據(jù)異常下動(dòng)力定位魯棒狀態(tài)估計(jì)方法

蔣 帆， 徐 海 祥*,2， 馮 輝,2 ， 卜 瑞 波

( 1.武漢理工大學(xué) 交通學(xué)院， 湖北 武漢 430063；2.武漢理工大學(xué) 高性能船舶技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室， 湖北 武漢 430063 )

0 引 言

隨著海洋作業(yè)的深入，傳統(tǒng)的錨泊定位技術(shù)已經(jīng)無(wú)法滿足深遠(yuǎn)水域定位作業(yè)要求，動(dòng)力定位(dynamic positioning，DP)技術(shù)以其不受水深限制、定位精度高且成本低、機(jī)動(dòng)性強(qiáng)等獨(dú)特優(yōu)勢(shì)應(yīng)運(yùn)而生，現(xiàn)已廣泛應(yīng)用于鋪管船、科考船、海洋平臺(tái)等海洋結(jié)構(gòu)物的定位作業(yè)中[1-2].

動(dòng)力定位船舶為了實(shí)現(xiàn)定位、循跡等作業(yè)，其控制系統(tǒng)需要實(shí)時(shí)獲取準(zhǔn)確可靠的船舶運(yùn)動(dòng)狀態(tài)與外界環(huán)境信息.作為動(dòng)力定位船舶的“眼睛”，狀態(tài)估計(jì)的作用正是從攜帶噪聲信號(hào)的觀測(cè)數(shù)據(jù)中估計(jì)船舶運(yùn)動(dòng)狀態(tài).20世紀(jì)70年代，Balchen等[3]將卡爾曼濾波(Kalman filter，KF)引入動(dòng)力定位中，KF的引入是動(dòng)力定位系統(tǒng)發(fā)展的一個(gè)里程碑事件，它的出現(xiàn)宣告了在動(dòng)力定位領(lǐng)域真正意義上基于模型的狀態(tài)估計(jì)方法的誕生.但是無(wú)論是KF還是隨后改進(jìn)提出的擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)都對(duì)于船舶的數(shù)學(xué)建模、外界環(huán)境干擾和傳感器系統(tǒng)噪聲的建模過于理想化，狀態(tài)估計(jì)會(huì)產(chǎn)生一定誤差.1999年，F(xiàn)ossen 等[4]將非線性無(wú)源濾波引入動(dòng)力定位系統(tǒng)中，并利用無(wú)源理論和Lyapunov方法證明了濾波器的穩(wěn)定性，但該方法的狀態(tài)估計(jì)精度受波浪頻率等參數(shù)的影響較大.為此，一些基于非線性無(wú)源理論的改進(jìn)方法相繼提出[5-9]，這些方法通過調(diào)整增益矩陣以匹配復(fù)雜時(shí)變環(huán)境，但應(yīng)用這些方法的前提都是傳感器測(cè)量信號(hào)正常.然而在實(shí)際工程作業(yè)中，復(fù)雜時(shí)變的海洋環(huán)境、自然與人為的電磁干擾等都會(huì)導(dǎo)致傳感器數(shù)據(jù)出現(xiàn)異常，進(jìn)而影響狀態(tài)估計(jì)的準(zhǔn)確性和整個(gè)動(dòng)力定位系統(tǒng)的穩(wěn)定性，更有甚者會(huì)導(dǎo)致動(dòng)力定位船舶發(fā)生碰撞、失控等事故.因此對(duì)于動(dòng)力定位船舶而言，設(shè)計(jì)魯棒的狀態(tài)估計(jì)算法以處理傳感器數(shù)據(jù)異常情況顯得尤為關(guān)鍵.

無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)是針對(duì)非線性系統(tǒng)狀態(tài)估計(jì)提出的濾波方法，其在動(dòng)力定位系統(tǒng)狀態(tài)估計(jì)的有效性已被國(guó)內(nèi)外學(xué)者相繼驗(yàn)證[10-13].然而UKF在處理傳感器數(shù)據(jù)異常情況下的狀態(tài)估計(jì)問題時(shí)，會(huì)導(dǎo)致濾波結(jié)果發(fā)散，難以達(dá)到滿意的估計(jì)效果.針對(duì)上述問題，諸多學(xué)者采用故障診斷的方法對(duì)傳感器故障類型、原因等進(jìn)行診斷，最終結(jié)合容錯(cuò)控制給出相應(yīng)的解決方案.傳感器故障診斷由來(lái)已久，但是針對(duì)動(dòng)力定位的方法卻很少.文獻(xiàn)[14]提出了一種基于神經(jīng)網(wǎng)絡(luò)預(yù)測(cè)器的傳感器故障診斷方法，該方法首先對(duì)傳感器數(shù)據(jù)進(jìn)行卡爾曼濾波，隨后利用神經(jīng)網(wǎng)絡(luò)進(jìn)行故障診斷，由于類似卡爾曼濾波這些基于模型的濾波器會(huì)使含有異常傳感器數(shù)據(jù)的濾波結(jié)果發(fā)散，因此這種“先濾波后診斷”的處理方式違背了自洽性原則，難以用于工程實(shí)際.文獻(xiàn)[15]通過設(shè)計(jì)有限脈沖響應(yīng)濾波器，并利用殘差平方和判據(jù)與信噪比判據(jù)對(duì)船用羅經(jīng)進(jìn)行故障診斷，仿真結(jié)果驗(yàn)證該方法的有效性，然而采用信噪比判據(jù)會(huì)存在一定的滯后效應(yīng).事實(shí)上，動(dòng)力定位系統(tǒng)對(duì)數(shù)據(jù)的實(shí)時(shí)性要求非常高，滯后效應(yīng)對(duì)動(dòng)力定位控制會(huì)產(chǎn)生不利影響.

鑒于此，一些學(xué)者另辟蹊徑，將狀態(tài)估計(jì)與傳感器故障診斷相結(jié)合，保證在傳感器數(shù)據(jù)異常情況下狀態(tài)估計(jì)準(zhǔn)確性和實(shí)時(shí)性.文獻(xiàn)[16]就超微型衛(wèi)星系統(tǒng)發(fā)生故障的情況提出了魯棒自適應(yīng)UKF，通過假設(shè)檢驗(yàn)的方法檢測(cè)故障的發(fā)生，再利用協(xié)方差匹配原理更新噪聲協(xié)方差以匹配模型參數(shù)攝動(dòng)以及傳感器數(shù)據(jù)漂移帶來(lái)的影響.文獻(xiàn)[17]針對(duì)無(wú)人機(jī)出現(xiàn)傳感器/執(zhí)行器故障問題，提出了基于卡爾曼濾波和故障診斷的狀態(tài)估計(jì)方法.動(dòng)力定位系統(tǒng)是一個(gè)閉環(huán)反饋控制系統(tǒng)，傳感器數(shù)據(jù)異常對(duì)動(dòng)力定位系統(tǒng)的影響之大有別于其他開環(huán)系統(tǒng)(比如組合導(dǎo)航等)，具體而言，傳感器數(shù)據(jù)異常不僅影響動(dòng)力定位狀態(tài)估計(jì)的精度，還會(huì)導(dǎo)致控制系統(tǒng)產(chǎn)生錯(cuò)誤的控制指令，而該控制指令又是下一周期狀態(tài)估計(jì)輸入的一部分，如此反復(fù)最終導(dǎo)致難以實(shí)現(xiàn)定位等作業(yè)要求.文獻(xiàn)[18]提出了一種改進(jìn)的UKF，利用觀測(cè)噪聲協(xié)方差矩陣R自適應(yīng)更新處理動(dòng)力定位船舶傳感器數(shù)據(jù)出現(xiàn)離群值的問題，并通過仿真驗(yàn)證了方法的可行性，然而其研究只單獨(dú)限于狀態(tài)估計(jì)部分，缺乏整體性.

本文在上述研究的基礎(chǔ)上，汲取文獻(xiàn)[18]對(duì)觀測(cè)噪聲協(xié)方差矩陣R自適應(yīng)更新的思想，提出一種魯棒的無(wú)跡卡爾曼濾波方法，該方法通過引入一種基于指數(shù)加權(quán)的觀測(cè)噪聲協(xié)方差矩陣R自適應(yīng)更新模塊和一種基于卡方分布假設(shè)檢驗(yàn)方法的過程不確定性識(shí)別模塊處理傳感器數(shù)據(jù)異常情況并估計(jì)未知環(huán)境力.最后以某平臺(tái)供應(yīng)船的船模為仿真對(duì)象，進(jìn)行仿真對(duì)比實(shí)驗(yàn).

1 數(shù)學(xué)模型

對(duì)于水面船舶而言，其運(yùn)動(dòng)包括6個(gè)自由度，即縱蕩、橫蕩、垂蕩、縱搖、橫搖、艏搖.而動(dòng)力定位船舶往往忽略垂蕩、橫搖、縱搖對(duì)水平面運(yùn)動(dòng)的影響，即只考慮3個(gè)自由度(縱蕩、橫蕩、艏搖)運(yùn)動(dòng).因此，針對(duì)動(dòng)力定位船舶的非線性運(yùn)動(dòng)數(shù)學(xué)模型[19]可以表示為

(1)

式中：ξh為船舶高頻運(yùn)動(dòng)位置與速度向量；Ah為赫爾維茨矩陣；Eh、Eb、Eυ都表示噪聲的幅值矩陣；ηh為船舶高頻運(yùn)動(dòng)位置向量；η為船舶低頻運(yùn)動(dòng)位置向量；J(ψ)為坐標(biāo)轉(zhuǎn)換矩陣；υ為速度矩陣；b為環(huán)境力矩陣；Tb為時(shí)間常數(shù)矩陣；M表示船舶慣性矩陣；D表示船舶阻尼系數(shù)矩陣；τ為推進(jìn)器產(chǎn)生的推力和力矩；ωh、ωb、ωυ、vy都表示零均值高斯白噪聲；y為傳感器觀測(cè)位置；下標(biāo)h、b、υ、y分別表示船舶高頻運(yùn)動(dòng)、環(huán)境力、船舶低頻運(yùn)動(dòng)以及傳感器量測(cè)方程.相關(guān)矩陣的具體形式為

A22=-diag{2ζ1ω01，2ζ2ω02，2ζ3ω03}；

Σ=diag{Kω1，Kω2，Kω3}

式中：ω0i(i=1，2，3)表示對(duì)應(yīng)自由度的波浪譜譜峰頻率；ζi(i=1，2，3)表示對(duì)應(yīng)自由度的阻尼系數(shù)；Kω i(i=1，2，3)表示對(duì)應(yīng)自由度的增益系數(shù)，Kω i=2ζω0iσi(σi為描述波浪強(qiáng)度的常量).

將式(1)寫成狀態(tài)空間模型：

(2)

2 無(wú)跡卡爾曼濾波

1995年，Julier等將無(wú)跡變換(unscented transformation，UT)的思想引入卡爾曼濾波中，提出了無(wú)跡卡爾曼濾波.與傳統(tǒng)擴(kuò)展卡爾曼濾波處理非線性系統(tǒng)狀態(tài)估計(jì)問題的方式不同，UKF摒棄了對(duì)非線性函數(shù)進(jìn)行線性化的傳統(tǒng)做法，它不需要求導(dǎo)計(jì)算Jacobian矩陣而是用固定數(shù)量的參數(shù)去逼近狀態(tài)的后驗(yàn)概率密度，由于沒有將高階項(xiàng)忽略，其估計(jì)精度較高且穩(wěn)定性較強(qiáng).因此，UKF被廣泛應(yīng)用于目標(biāo)跟蹤[20]、信號(hào)處理[21]和機(jī)器學(xué)習(xí)[22]等眾多領(lǐng)域.

(1)sigma點(diǎn)采樣與對(duì)應(yīng)權(quán)值計(jì)算

(3)

(4)

(2)預(yù)測(cè)(時(shí)間更新)

(5)

(3)更新(測(cè)量更新)

(6)

3 觀測(cè)噪聲協(xié)方差矩陣自適應(yīng)更新

當(dāng)傳感器數(shù)據(jù)出現(xiàn)異常時(shí)，由于傳統(tǒng)UKF無(wú)法自適應(yīng)更新觀測(cè)噪聲協(xié)方差會(huì)導(dǎo)致估計(jì)精度下降，為此本文設(shè)計(jì)了一種基于指數(shù)加權(quán)的觀測(cè)噪聲協(xié)方差矩陣R估計(jì)器.

首先，定義新息序列為

(7)

將式(2)中的觀測(cè)方程代入式(7)并結(jié)合動(dòng)力定位狀態(tài)估計(jì)數(shù)學(xué)模型可得：

(8)

式中：xk為實(shí)際狀態(tài)向量；vk為觀測(cè)噪聲向量.

定義誤差向量Δxk為

(9)

易得：

(10)

式中：E(·)表示期望.由于新息序列具有弱自相關(guān)性(正交性)，新息序列協(xié)方差矩陣可通過下式計(jì)算得到：

(11)

故觀測(cè)噪聲協(xié)方差矩陣可以表示為

(12)

對(duì)于新息序列協(xié)方差的估計(jì)，文獻(xiàn)[11]采用算數(shù)平均法，其表達(dá)式為

(13)

式中：εi為第i時(shí)刻的新息序列.由于算數(shù)平均法只針對(duì)k時(shí)刻之前的新息序列協(xié)方差平方和進(jìn)行算數(shù)平均作為k時(shí)刻新息序列協(xié)方差的估計(jì)值，對(duì)歷史數(shù)據(jù)的利用程度都相同，新息序列協(xié)方差的估計(jì)精度會(huì)隨周期數(shù)增加而下降[17].為此，本節(jié)設(shè)計(jì)了一種基于指數(shù)加權(quán)的新息序列協(xié)方差估計(jì)方法.

對(duì)于k時(shí)刻之前的新息序列協(xié)方差，其權(quán)重系數(shù)

(14)

式中：δ(i)為第i時(shí)刻新息序列協(xié)方差的權(quán)重；b為漸衰因子，且滿足0

式(13)可以轉(zhuǎn)化為

δ(k)bk-1+δ(k)bk-2+…+δ(k)b+δ(k)=1

(15)

在漸衰因子b確定后，δ(k)就可以用含b的代數(shù)表達(dá)式表達(dá)，即

(16)

則第i時(shí)刻新息序列協(xié)方差的權(quán)重δ(i)可表達(dá)為

δ(i)=δ(k)·bk-i

(17)

(18)

由式(16)可得：

(19)

(20)

4 過程不確定識(shí)別

為了分析研究和傳感器設(shè)備檢修，動(dòng)力定位系統(tǒng)往往需要明確傳感器數(shù)據(jù)何時(shí)發(fā)生異常.為此，本文設(shè)計(jì)基于卡方分布的假設(shè)檢驗(yàn)方法，用以對(duì)過程不確定性進(jìn)行識(shí)別，且規(guī)定觀測(cè)噪聲協(xié)方差矩陣R只在傳感器數(shù)據(jù)發(fā)生異常時(shí)進(jìn)行自適應(yīng)更新.具體過程如下：

首先，提出兩種假設(shè)：

假設(shè)1零假設(shè)γ0：傳感器數(shù)據(jù)正常；

假設(shè)2替代假設(shè)γ1：傳感器數(shù)據(jù)異常.

其次，通過形如下式的統(tǒng)計(jì)量檢測(cè)傳感器數(shù)據(jù)異常：

(21)

結(jié)合上述內(nèi)容，基于魯棒無(wú)跡卡爾曼濾波的動(dòng)力定位船舶狀態(tài)估計(jì)整體過程如圖1所示.

圖1 魯棒無(wú)跡卡爾曼濾波的狀態(tài)估計(jì)流程

5 仿真實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證時(shí)變環(huán)境下魯棒無(wú)跡卡爾曼濾波(robust unscented Kalman filter，RUKF)在估計(jì)環(huán)境擾動(dòng)方面的有效性和處理傳感器數(shù)據(jù)異常情況的優(yōu)越性，設(shè)計(jì)了仿真對(duì)比實(shí)驗(yàn)，仿真實(shí)驗(yàn)在相同硬件設(shè)備(Windows 7旗艦版)和相同程序編譯平臺(tái)(MATLAB 2014b)條件下進(jìn)行.

仿真實(shí)驗(yàn)以一艘縮尺比為1∶20的平臺(tái)供應(yīng)船的模型為對(duì)象，與模型相關(guān)的船模參數(shù)、水動(dòng)力導(dǎo)數(shù)如表1所示.

船舶數(shù)學(xué)運(yùn)動(dòng)模型中相關(guān)參數(shù)的選取如下：ω0i=0.8 rad/s (i=1，2，3)，ζi=0.3 (i=1，2，3)，σi=1.04 (i=1，2，3).

為模擬時(shí)變環(huán)境力，時(shí)間常數(shù)矩陣Tb取為

3個(gè)自由度(縱蕩、橫蕩和艏搖)上的觀測(cè)噪聲協(xié)方差矩陣R為

表1 平臺(tái)供應(yīng)船模型相關(guān)參數(shù)

過程噪聲協(xié)方差矩陣Q=diag{0，0，0，0.1，0.1，0.000 1，0，0，0，0，0，0，0.000 1，0.000 1，0.000 1}.

為模擬閉環(huán)控制系統(tǒng)兼顧動(dòng)力定位系統(tǒng)各個(gè)模塊的整體性并使船舶保持在定位點(diǎn)，仿真采用PID控制器模擬產(chǎn)生每一周期的控制力，控制器的設(shè)計(jì)如下：

(22)

船舶初始位置設(shè)置為[0，0，0]，定位點(diǎn)位置設(shè)置為[10 m，10 m，15°]，采樣周期為0.5 s，仿真時(shí)長(zhǎng)為1 000個(gè)周期.

為驗(yàn)證傳感器數(shù)據(jù)異常情況下魯棒無(wú)跡卡爾曼濾波的優(yōu)越性，仿真中在第300～350個(gè)周期(150～175 s)時(shí)將式(2)中的傳感器觀測(cè)值置零來(lái)模擬傳感器數(shù)據(jù)異常情況.

仿真結(jié)果如圖2～4所示.

(a) 北向位置變化

(b) 東向位置變化

(c) 艏向角度變化

圖2為采用UKF時(shí)，船舶北向、東向和艏向的傳感器測(cè)量位置、實(shí)際位置以及估計(jì)位置變化圖.圖3為采用RUKF時(shí)，船舶北向、東向和艏向的傳感器測(cè)量位置、實(shí)際位置以及估計(jì)位置變化圖.從圖中可以看出，UKF和RUKF最終都能使船舶到達(dá)目標(biāo)點(diǎn)實(shí)現(xiàn)定位功能，但在150～175 s期間傳感器數(shù)據(jù)出現(xiàn)異常的情況下，UKF對(duì)船舶位置與姿態(tài)的估計(jì)產(chǎn)生較大的偏差且其估計(jì)精度受影響的時(shí)間要比發(fā)生異常的時(shí)間更長(zhǎng)，與UKF相比，RUKF雖然在數(shù)據(jù)異常期間估計(jì)精度也有所下降，但其下降幅度遠(yuǎn)低于UKF，且數(shù)據(jù)異常后能更快恢復(fù)其估計(jì)精度，從而更為平穩(wěn)、更為快速地到達(dá)目標(biāo)位置.

(a) 北向位置變化

(b) 東向位置變化

(c) 艏向角度變化

圖4為UKF和RUKF的船舶北向、東向和艏向估計(jì)誤差變化對(duì)比圖.從圖中可以看出，在傳感器數(shù)據(jù)出現(xiàn)異常時(shí)，RUKF的估計(jì)誤差遠(yuǎn)低于UKF，且其能在更短的時(shí)間恢復(fù)到未出現(xiàn)數(shù)據(jù)異常時(shí)的估計(jì)精度.

圖5為過程不確定性識(shí)別模塊卡方分布假設(shè)檢驗(yàn)統(tǒng)計(jì)量β變化圖.從圖中可以看到，當(dāng)傳感器數(shù)據(jù)出現(xiàn)異常時(shí)，統(tǒng)計(jì)量β迅速增大并超過卡方臨界值，此時(shí)替代假設(shè)γ1成立，這充分驗(yàn)證了提出的基于卡方檢測(cè)的過程不確定性識(shí)別方法的有效性.

(a) 北向位置估計(jì)誤差

(b) 東向位置估計(jì)誤差

(c) 艏向角度估計(jì)誤差

圖5 卡方分布假設(shè)檢驗(yàn)統(tǒng)計(jì)量β變化

圖6為UKF和RUKF對(duì)X、Y、Z3個(gè)方向未知外界環(huán)境力(矩)的估計(jì)對(duì)比圖.由圖可知，UKF在傳感器數(shù)據(jù)出現(xiàn)異常時(shí)且在之后較長(zhǎng)一段時(shí)間內(nèi)對(duì)環(huán)境力(矩)的估計(jì)精度明顯下降，而RUKF仍能保持較高的估計(jì)精度.

(a) X方向外界環(huán)境力估計(jì)對(duì)比

(b) Y方向外界環(huán)境力估計(jì)對(duì)比

(c) Z方向外界環(huán)境力矩估計(jì)對(duì)比

綜合上述分析可知，對(duì)UKF而言，傳感器數(shù)據(jù)出現(xiàn)異常會(huì)導(dǎo)致狀態(tài)估計(jì)(包括位置姿態(tài)以及外界環(huán)境力)精度下降從而使控制系統(tǒng)產(chǎn)生有偏差的控制指令，該控制指令又作為下一周期狀態(tài)估計(jì)的輸入，造成誤差累加，而最終能達(dá)到定位點(diǎn)完成定位工作得益于PID控制器“利用偏差以消除偏差”的工作原理.對(duì)RUKF而言，由于過程不確定性識(shí)別模塊的加入，能夠準(zhǔn)確及時(shí)地識(shí)別傳感器數(shù)據(jù)何時(shí)發(fā)生異常，又通過觀測(cè)噪聲協(xié)方差矩陣R的自適應(yīng)更新，提高了狀態(tài)估計(jì)精度，加快了收斂速度，表現(xiàn)出很好的魯棒性.因此，RUKF更適用于動(dòng)力定位實(shí)際工程應(yīng)用.

6 結(jié) 語(yǔ)

本文針對(duì)時(shí)變環(huán)境下動(dòng)力定位船舶由于未知環(huán)境干擾、傳感器數(shù)據(jù)異常的問題而無(wú)法準(zhǔn)確及時(shí)地得到船舶位置、姿態(tài)與外界環(huán)境干擾信息的問題，提出了一種魯棒無(wú)跡卡爾曼濾波方法.針對(duì)傳統(tǒng)無(wú)跡卡爾曼濾波無(wú)法自適應(yīng)更新觀測(cè)噪聲協(xié)方差的缺陷，設(shè)計(jì)了基于指數(shù)加權(quán)的觀測(cè)噪聲協(xié)方差矩陣R估計(jì)器.在此基礎(chǔ)上，引入了基于卡方分布的假設(shè)檢驗(yàn)方法用以確定傳感器數(shù)據(jù)發(fā)生異常的時(shí)刻.最后，設(shè)計(jì)了仿真對(duì)比實(shí)驗(yàn)，驗(yàn)證了所提出的狀態(tài)估計(jì)方法的有效性和優(yōu)越性.