蔣 帆, 徐 海 祥*,2, 馮 輝,2 , 卜 瑞 波
( 1.武漢理工大學(xué) 交通學(xué)院, 湖北 武漢 430063;2.武漢理工大學(xué) 高性能船舶技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室, 湖北 武漢 430063 )
隨著海洋作業(yè)的深入,傳統(tǒng)的錨泊定位技術(shù)已經(jīng)無法滿足深遠(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ì)方法的誕生.但是無論是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]將非線性無源濾波引入動(dòng)力定位系統(tǒng)中,并利用無源理論和Lyapunov方法證明了濾波器的穩(wěn)定性,但該方法的狀態(tài)估計(jì)精度受波浪頻率等參數(shù)的影響較大.為此,一些基于非線性無源理論的改進(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)鍵.
無跡卡爾曼濾波(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)的解決方案.傳感器故障診斷由來已久,但是針對(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ù)漂移帶來的影響.文獻(xiàn)[17]針對(duì)無人機(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)更新的思想,提出一種魯棒的無跡卡爾曼濾波方法,該方法通過引入一種基于指數(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).
對(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)
1995年,Julier等將無跡變換(unscented transformation,UT)的思想引入卡爾曼濾波中,提出了無跡卡爾曼濾波.與傳統(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)
當(dāng)傳感器數(shù)據(jù)出現(xiàn)異常時(shí),由于傳統(tǒng)UKF無法自適應(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)
為了分析研究和傳感器設(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)容,基于魯棒無跡卡爾曼濾波的動(dòng)力定位船舶狀態(tài)估計(jì)整體過程如圖1所示.
圖1 魯棒無跡卡爾曼濾波的狀態(tài)估計(jì)流程
為驗(yàn)證時(shí)變環(huán)境下魯棒無跡卡爾曼濾波(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ù)異常情況下魯棒無跡卡爾曼濾波的優(yōu)越性,仿真中在第300~350個(gè)周期(150~175 s)時(shí)將式(2)中的傳感器觀測(cè)值置零來模擬傳感器數(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)用.
本文針對(duì)時(shí)變環(huán)境下動(dòng)力定位船舶由于未知環(huán)境干擾、傳感器數(shù)據(jù)異常的問題而無法準(zhǔn)確及時(shí)地得到船舶位置、姿態(tài)與外界環(huán)境干擾信息的問題,提出了一種魯棒無跡卡爾曼濾波方法.針對(duì)傳統(tǒng)無跡卡爾曼濾波無法自適應(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)越性.