基于多傳感器信息融合的水下運(yùn)動(dòng)體速度測(cè)量*

徐 侃， 徐國(guó)華， 夏英凱， 趙 寅

(華中科技大學(xué) 船舶與海洋工程學(xué)院 輪機(jī)工程系，湖北 武漢 430074)

0 引 言

海洋資源開發(fā)、科學(xué)試驗(yàn)越來越多地要求進(jìn)行水下運(yùn)動(dòng)體高精度速度測(cè)量與控制。但海洋環(huán)境復(fù)雜，且具有不可見、人不接近等特點(diǎn)，其測(cè)速難度大、精度低。本文結(jié)合某科學(xué)試驗(yàn)海洋平臺(tái)，探索一種基于雙傳感器的信息融合水下高精度測(cè)速技術(shù)。

該科學(xué)試驗(yàn)平臺(tái)是工作在200 m深的水下張力腿平臺(tái)。平臺(tái)中間置有導(dǎo)軌，呈流線型的水下輪式運(yùn)動(dòng)體運(yùn)行于導(dǎo)軌上，實(shí)驗(yàn)設(shè)備放在運(yùn)動(dòng)體內(nèi)密封艙里，高精度的速度測(cè)量輔助速度控制以提供實(shí)驗(yàn)設(shè)備所需要的速度曲線。

單一的水下測(cè)速方法如慣性測(cè)量、水聲測(cè)量(DVL)等精度有限，因此,最終選用多傳感器信息融合的方式。慣性裝置(加速度計(jì))安裝于水下輪式運(yùn)動(dòng)小車內(nèi)，新增機(jī)械測(cè)速裝置在小車輪旁，通過2種測(cè)速裝置對(duì)該運(yùn)動(dòng)小車進(jìn)行綜合測(cè)速。

卡爾曼濾波是多傳感器信息融合最常用的一種經(jīng)典算法，但水下運(yùn)動(dòng)體的工作環(huán)境存在各種未知干擾，常規(guī)卡爾曼濾波算法無法對(duì)這些變化進(jìn)行檢測(cè)和調(diào)整，且常規(guī)卡爾曼濾波在計(jì)算增益時(shí)存在缺陷，易造成發(fā)散的現(xiàn)象[1]，針對(duì)這些問題，本文提出了一種模糊自適應(yīng)卡爾曼濾波算法[2]，提高了濾波器的跟隨性和濾波精度。

1 水下運(yùn)動(dòng)體測(cè)速原理

1.1 機(jī)械測(cè)速裝置

機(jī)械測(cè)速裝置由測(cè)速輪組件和接近傳感器組成。當(dāng)測(cè)速輪轉(zhuǎn)動(dòng)時(shí)，引起接近傳感器的磁場(chǎng)變化，通過信號(hào)處理輸出方波信號(hào)。測(cè)量在規(guī)定時(shí)間間隔T內(nèi)，傳感器輸出的脈沖個(gè)數(shù)，即可獲得速度值。

假設(shè)測(cè)速裝置的碼盤半徑為R，齒數(shù)為M，周期T內(nèi)，傳感器輸出的脈沖個(gè)數(shù)為N，則第k次測(cè)量的線速度可以表示為

(1)

1.2 加速度計(jì)

(2)

式中a為敏感軸方向加速度，k為標(biāo)度因數(shù)，Bf為零偏，Bv為振擺誤差系數(shù)，n為隨機(jī)噪聲。

1.3 信息融合系統(tǒng)數(shù)學(xué)模型

根據(jù)當(dāng)前統(tǒng)計(jì)模型[4]，當(dāng)載體以某一加速度運(yùn)動(dòng)時(shí)，下一瞬間加速度只能在當(dāng)前加速度值的鄰域內(nèi),則機(jī)動(dòng)加速度可由一階馬可代夫方程表示如下

(3)

(4)

設(shè)s為運(yùn)動(dòng)體位置值，v為運(yùn)動(dòng)體速度值，a為運(yùn)動(dòng)體加速度值，則其狀態(tài)方程為

(5)

設(shè)系統(tǒng)量測(cè)值為am和vm，則有

am=a+na,

(6)

vm=v+nv,

(7)

式中na和nv分別為加速度和速度的量測(cè)噪聲，可得系統(tǒng)量測(cè)方程

(8)

2 模糊自適應(yīng)卡爾曼濾波器設(shè)計(jì)

2.1 自適應(yīng)卡爾曼濾波算法

離散系統(tǒng)在tk時(shí)刻狀態(tài)方程與量測(cè)方程如下

Xk+1=Φk+1,kXk+ΓkWk,

(9)

Zk=HkXk+Vk,

(10)

式中Xk為估計(jì)狀態(tài)，Wk為系統(tǒng)噪聲序列，Vk為系統(tǒng)噪聲序列，Φk+1,k為tk時(shí)刻至tk+1時(shí)刻的一步轉(zhuǎn)移陣，Γk為系統(tǒng)噪聲驅(qū)動(dòng)陣，Hk為量測(cè)陣。

常規(guī)卡爾曼濾波[5]算法如下

k+1,k=Φk+1,kk,

(11)

k+1=k+1,k+Kk+1[Zk+1-Hk+1k+1,k],

(12)

(13)

(14)

Pk+1=[1-Kk+1Hk+1]Pk+1,k.

(15)

對(duì)于確定性系統(tǒng)，已知系統(tǒng)初始條件，通過求解上述方程，可得到系統(tǒng)各個(gè)時(shí)刻的準(zhǔn)確狀態(tài)，但在運(yùn)行過程中，大部分系統(tǒng)都會(huì)因干擾和噪聲的影響而產(chǎn)生誤差。為此提出一種自適應(yīng)卡爾曼濾波方法，通過對(duì)測(cè)量噪聲序列的方差陣Rk進(jìn)行實(shí)時(shí)修改，防止卡爾曼濾波器出現(xiàn)發(fā)散。

自適應(yīng)算法如下：

定義量測(cè)噪聲協(xié)方差陣Rk=[Rka0;0Rkv]，其中,Rka表示加速度量測(cè)噪聲序列的協(xié)方差，Rkv表示速度量測(cè)噪聲序列的協(xié)方差，定義Rk+1=[aRka0;0bRkv]，即第k+1次的量測(cè)噪聲協(xié)方差陣Rk+1根據(jù)系統(tǒng)當(dāng)前的運(yùn)行情況在前一次量測(cè)噪聲協(xié)方差陣的基礎(chǔ)上進(jìn)行給定，參數(shù)a和b為可調(diào)參數(shù)，其值由模糊控制器輸出。

2.2 模糊控制器設(shè)計(jì)

在等式(12)中，令r=Zk+1-Hk+1k+1,k，稱之為殘差[6]。在此，令Zk=[Zka;Zkv]，其中,Zka為加速度殘差，Zkv為速度殘差。

該模糊控制器設(shè)計(jì)為多輸入多輸出型，輸入變量為Zka和Zkv，輸出變量為量測(cè)噪聲方差調(diào)整參數(shù)a和b。輸入變量的論域?yàn)椋篫ka∈[-1,1]和Zkv∈[-2,2]；輸出變量的論域?yàn)椋篴∈[0,2]和b∈[0,2]。定義輸入變量的模糊集為：S=偏??；Z=正常；B=偏大；輸出變量的模糊集為：D=減??；Z=不變；I=增加。

在公式(13)中，殘差r=Zk+1-Hk+1k+1,k減小，表示這個(gè)時(shí)刻對(duì)下個(gè)時(shí)刻的預(yù)估值k+1,k過大，根據(jù)公式(11)可知此時(shí)刻的狀態(tài)值k過大，需要減小下一刻的狀態(tài)值k+1，根據(jù)式(12)需要減小Kk+1，再根據(jù)式(13)增大Rk+1,即可達(dá)到目的。據(jù)此規(guī)律設(shè)計(jì)模糊控制器的模糊規(guī)則如下：

ifZka∈SandZkv∈S,thena∈Iandb∈I,

ifZka∈SandZkv∈Z,thena∈Iandb∈M,

ifZka∈SandZkv∈B,thena∈Iandb∈D,

ifZka∈ZandZkv∈S,thena∈Mandb∈I,

ifZka∈ZandZkv∈Z,thena∈Mandb∈M,

ifZka∈ZandZkv∈B,thena∈Mandb∈D,

ifZka∈BandZkv∈S,thena∈Dandb∈I,

ifZka∈BandZkv∈Z,thena∈Dandb∈M,

ifZka∈BandZkv∈B,thena∈Dandb∈D.

2.3 模糊自適應(yīng)卡爾曼濾波器

該模糊自適應(yīng)卡爾曼濾波器的系統(tǒng)結(jié)構(gòu)如圖1。

圖1 模糊自適應(yīng)卡爾曼濾波器框圖

如圖1所示：該模糊自適應(yīng)卡爾曼濾波器是在常規(guī)卡爾曼濾波器基礎(chǔ)上引入量測(cè)噪聲方差權(quán)值a和b，由模糊控制器根據(jù)殘差值對(duì)權(quán)值進(jìn)行實(shí)時(shí)調(diào)整[7]，從而改善濾波效果。

3 系統(tǒng)仿真[8]

本章對(duì)2種經(jīng)典運(yùn)動(dòng)：勻速運(yùn)動(dòng)和勻加速運(yùn)動(dòng)進(jìn)行仿真，驗(yàn)證該模糊自適應(yīng)控制器設(shè)計(jì)的優(yōu)劣。

3.1 勻速運(yùn)動(dòng)

設(shè)定理想勻速運(yùn)動(dòng)的速度為1 m/s,仿真時(shí)間為1 s，系統(tǒng)噪聲序列的方差陣Q取0.1，量測(cè)噪聲序列的方差陣R的初值取為R0=[0.1 0;0 0.05]。在理想信號(hào)的基礎(chǔ)上加入幅值為0.5的隨機(jī)白噪聲信號(hào)作為原始信號(hào)，原始信號(hào)如圖2所示。

圖2 勻速運(yùn)動(dòng)濾波前信號(hào)

對(duì)原始信號(hào)進(jìn)行普通卡爾曼濾波處理，可得仿真結(jié)果如圖3。

圖3 勻速運(yùn)動(dòng)普通卡爾曼濾波仿真圖

對(duì)原始信號(hào)進(jìn)行模糊自適應(yīng)卡爾曼濾波，可得仿真結(jié)果如圖4。

圖4 勻速運(yùn)動(dòng)模糊自適應(yīng)卡爾曼濾波仿真圖

3.2 勻加速運(yùn)動(dòng)

設(shè)定理想勻加速運(yùn)動(dòng)初始速度值為0，加速度為1 m2/s,仿真時(shí)間為1 s，系統(tǒng)噪聲序列的方差陣Q取0.1，量測(cè)噪聲序列的方差陣R的初值取為R0=[0.1 0;0 0.05]。在理想信號(hào)基礎(chǔ)上加入幅值為0.5的隨機(jī)白噪聲信號(hào)作為原始信號(hào)，原始信號(hào)如圖5所示。

圖5 勻加速運(yùn)動(dòng)濾波前信號(hào)

對(duì)原始信號(hào)進(jìn)行普通卡爾曼濾波處理，可得仿真結(jié)果如圖6。

圖6 勻加速運(yùn)動(dòng)普通卡爾曼濾波仿真圖

對(duì)原始信號(hào)進(jìn)行模糊自適應(yīng)卡爾曼濾波，可得仿真結(jié)果如圖7。

圖7 勻加速運(yùn)動(dòng)模糊自適應(yīng)卡爾曼濾波仿真圖

3.3 仿真結(jié)果分析

對(duì)比普通卡爾曼濾波和模糊自適應(yīng)卡爾曼濾波的仿真結(jié)果不難看出：普通卡爾曼濾波的毛刺比較多，與理想信號(hào)相比誤差較大，且收斂性不好，而模糊自適應(yīng)卡爾曼濾波整體濾波效果好，且跟隨性優(yōu)于普通卡爾曼濾波，可以證明加入自適應(yīng)參數(shù)a和b后，在一定程度上提高濾波效果。

4 試 驗(yàn)

真實(shí)條件下工作環(huán)境更加復(fù)雜，得到的量測(cè)信號(hào)比理想信號(hào)有更多噪聲，本章將通過實(shí)際試驗(yàn)對(duì)該系統(tǒng)進(jìn)行驗(yàn)證。

試驗(yàn)中所用主要設(shè)備包括：小車測(cè)速控制柜、機(jī)械測(cè)速裝置及加速度計(jì)。

試驗(yàn)環(huán)境為室內(nèi)船池，試驗(yàn)過程中，機(jī)械測(cè)速裝置和加速度計(jì)固定在航車上，小車測(cè)速控制柜負(fù)責(zé)對(duì)航車的速度和加速度進(jìn)行實(shí)時(shí)采集與處理，得出融合速度曲線。

現(xiàn)場(chǎng)試驗(yàn)環(huán)境如圖8所示。

圖8 船池試驗(yàn)與試驗(yàn)設(shè)備圖

試驗(yàn)中，分別用加速度計(jì)單獨(dú)測(cè)速、機(jī)械測(cè)速裝置單獨(dú)測(cè)速以及融合測(cè)速3種方法對(duì)航車的速度值進(jìn)行估計(jì)，可得實(shí)際速度曲線如圖9。

由上述試驗(yàn)結(jié)果不難看出，加速度計(jì)測(cè)速和測(cè)速裝置測(cè)速所得的速度曲線均有一定的毛刺，且兩者所測(cè)速度值有一定的偏差，采用融合測(cè)速方法可以有效去除信號(hào)中的噪聲，使速度曲線更加平滑，能夠達(dá)到對(duì)2種單獨(dú)測(cè)速方法進(jìn)行融合的效果。

圖9 船池現(xiàn)場(chǎng)試驗(yàn)速度曲線圖

5 結(jié) 論

多傳感器信息融合方法，克服了單傳感器易受環(huán)境影響、精度低等問題。依據(jù)模糊推理規(guī)則，實(shí)時(shí)調(diào)整濾波參數(shù)，有效地解決了常規(guī)卡爾曼濾波跟隨性差、濾波精度低、易發(fā)散的缺點(diǎn)。經(jīng)過仿真試驗(yàn)和實(shí)際試驗(yàn)，證實(shí)了該信息融合系統(tǒng)設(shè)計(jì)合理，可以解決目前水下領(lǐng)域的測(cè)速問題，具有較高的實(shí)際應(yīng)用價(jià)值。

參考文獻(xiàn):

[1] 黃漫國(guó),樊尚春,鄭德智,等.多傳感器數(shù)據(jù)融合技術(shù)研究進(jìn)展[J].傳感器與微系統(tǒng),2010,29(3)：5-8.

[2] 趙 寅,徐國(guó)華,楊 超,等.基于模糊卡爾曼濾波算法的速度估算方法[J].儀表技術(shù)與傳感器,2012(12):80-83.

[3] 何海洋.擺式力平衡加速度計(jì)測(cè)試方法[J].計(jì)測(cè)技術(shù),2008(6):33-37.

[4] 房建成,申功勛,萬德鈞.GPS/DR組合導(dǎo)航系統(tǒng)自適應(yīng)擴(kuò)展卡爾曼濾波模型的建立[J].控制理論與應(yīng)用,1998,15(3):385-390.

[5] Harold W Sorenson.Kalman filtering:Theory and application[M].New York:IEEE Press,1985.

[6] 馬瑞平,魏 東,張明廉.一種改進(jìn)的自適應(yīng)卡爾曼濾波及在組合導(dǎo)航中的應(yīng)用[J].中國(guó)慣性技術(shù)學(xué)報(bào),2006,14(6):37-40.

[7] 柏 菁,劉建業(yè),袁 信.模糊自適應(yīng)卡爾曼濾波技術(shù)研究[J].航天控制,2002(1)：18-23.

[8] 劉金琨.先進(jìn)PID控制Matlab仿真[M].北京:電子工業(yè)出版社,2011.