曹莉凌,尚 進(jìn),曹守啟
(上海海洋大學(xué)工程學(xué)院,上海 201306)
實(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)用。
設(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為量測噪聲。
當(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ì)均方差為
由于新息能反映真實(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ù)雜繁瑣,不易理解。
建模準(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,可得
水產(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)各方向上速度的差值。即
仿真無人船運(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倍。
本文提出的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)。