基于聯(lián)邦卡爾曼濾波算法的水面無人作業(yè)船導(dǎo)航精度研究

張 云

（南京交通職業(yè)技術(shù)學(xué)院 電子信息工程學(xué)院，江蘇 南京 211188）

0 引 言

隨著自動化水平日益提高，水面無人作業(yè)船舶的研究愈來愈迫切，無人作業(yè)船的航跡控制研究更是其中的熱點問題?？煽康臒o人作業(yè)船導(dǎo)航系統(tǒng)不僅是無人作業(yè)船實現(xiàn)自主作業(yè)的基本保障，還是判斷系統(tǒng)的自動化程度的主要標(biāo)準(zhǔn)。船舶的導(dǎo)航精度和可靠性制約著無人船舶的自主作業(yè)水平，這也成為實現(xiàn)水產(chǎn)養(yǎng)殖自動化的一道瓶頸。自主導(dǎo)航無人作業(yè)船不僅能夠解決勞動力不足的問題，更能有力提高水產(chǎn)養(yǎng)殖作業(yè)及生產(chǎn)效率。目前在船舶導(dǎo)航研究中，衛(wèi)星定位導(dǎo)航已經(jīng)成為近年來的研究熱點，但是其精度卻有待進(jìn)一步提升。文章主要研究水產(chǎn)養(yǎng)殖作業(yè)環(huán)境下，應(yīng)用聯(lián)邦卡爾曼濾波器來提高GPS定位精度，實現(xiàn)無人作業(yè)船的自主導(dǎo)航與航跡跟蹤。

本課題以江蘇大學(xué)電氣學(xué)院和泰州常發(fā)農(nóng)業(yè)技術(shù)有限公司共同研制的全自動無人作業(yè)船為研究平臺，搭建了以Trimble BD982 GNSS 系統(tǒng)為核心的差分定位系統(tǒng)，并利用AHRS航向姿態(tài)系統(tǒng)獲取船舶的姿態(tài)信息?；跓o人作業(yè)船舶模型以及硬件結(jié)構(gòu)設(shè)計了聯(lián)邦卡爾曼濾波器。該算法能夠同時對系統(tǒng)的多個狀態(tài)變量進(jìn)行濾波，有效地降低系統(tǒng)的狀態(tài)誤差，進(jìn)而保證了系統(tǒng)的實時性、穩(wěn)定性，提高了航跡跟蹤的精度。

1 無人作業(yè)船的定位系統(tǒng)

本文中的GPS單元是天寶公司研制的BD982型GNSS接收機模塊。TrimbleBD982模塊可以精確地提供定位信息和測量航向等。本系統(tǒng)中不需要考慮數(shù)據(jù)的兼容性，因而只需要解析GPRMC格式的數(shù)據(jù)即可，GPRMC數(shù)據(jù)的格式如下：$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*hh；以上格式中<1>代表UTC時間；<2>定位狀態(tài)，A代表有效的定位，V代表無效的定位；<3> 代表緯度；<4> 代表緯度半球，N代表北半球，S代表南半球；<5>代表經(jīng)度；<6> 代表經(jīng)度半球，E為東經(jīng)，W為西經(jīng)；<7> 代表地面速率（000.0～999.9節(jié)）；<8> 代表地面航向（000.0～359.9°）；<9> 代表UTC日期；<10> 代表磁偏角（000.0～180.0°）；<11> 代表磁偏角方向，E為東或W為西；<12> 代表模式指示（僅 NMEA0183 3.00 版本輸出，A=自主定位，D=差分，E=估算，N=數(shù)據(jù)無效）。

為了確保數(shù)據(jù)的實時性及完整性，本系統(tǒng)使用中斷的方式來接收數(shù)據(jù)。由于接收機以20 Hz的頻率不斷地向S3C2440發(fā)送數(shù)據(jù)，為了保證能夠完整地接收到GPRMC格式的信息，需要在接收之前進(jìn)行判斷?？梢钥吹?，GPCRMC格式的數(shù)據(jù)以“GPRMC”字符開頭，并以“ ”結(jié)尾，可以依此為依據(jù)判斷是否完整地接收到一串?dāng)?shù)據(jù)。

2 航向姿態(tài)參考系統(tǒng)

本課題中所使用的姿態(tài)模塊是北京博創(chuàng)的UPAHRS微型姿態(tài)方位參考系統(tǒng)。該系統(tǒng)用于測量無人作業(yè)船的前進(jìn)速度、橫漂速度、航向角等信息。它在無人作業(yè)船自主導(dǎo)航中起到非常關(guān)鍵的作用。

航向姿態(tài)參考系統(tǒng)（AHRS）屬于小型捷聯(lián)慣導(dǎo)設(shè)備。它是由高性能三軸陀螺儀、三軸電子羅盤、三軸加速度計與微處理器組成，采用多傳感器融合算法得到高精度姿態(tài)信息，在其模塊內(nèi)部建有感測器補償、全姿態(tài)解算與融合算法，可以精確輸出基于選定參考坐標(biāo)系的姿態(tài)角與航向角。

UP-AHRS姿態(tài)模塊它具有非常小的體積，功耗也相當(dāng)?shù)停ㄐ∮?.5 W的功率），重量輕巧，在外殼的防護(hù)下，抗震及耐壓的性能很不錯，所以該姿態(tài)模塊非常可靠。

3 聯(lián)邦卡爾曼濾波在航跡控制中的應(yīng)用設(shè)計

3.1 聯(lián)邦卡爾曼濾波理論

為了提高系統(tǒng)的導(dǎo)航精度，引入聯(lián)邦卡爾曼濾波，該濾波器的結(jié)構(gòu)特點為其典型的二級式濾波設(shè)計。該濾波器的第一級濾波的主要功能為對這一級上的子濾波器進(jìn)行局部預(yù)測估計。第二級的主要功能則是將子濾波器傳遞過來的局部估計結(jié)果通過1個主濾波器進(jìn)行全局預(yù)測。在濾波過程中，通常會利用方差上界原理，來將各個濾波過程實行適度的改善，從而可以致使各個子濾波器的局部預(yù)測估計值在實際上是不相關(guān)的。聯(lián)邦卡爾曼濾波器的工作原理如下：第一，首先將經(jīng)過第一級的各個子濾波器預(yù)估后的局部估計值以及估計誤差的方差陣Pg傳送到第二級濾波結(jié)構(gòu)，即主濾波器中。第二，第一級子濾波器的估計量再與第二級主濾波器的估計量進(jìn)行數(shù)據(jù)融合，從而獲得系統(tǒng)的全局最優(yōu)估計值。第三，經(jīng)過第一級濾波器和第二級濾波器融合得到的系統(tǒng)的全局估計值以及與之對應(yīng)的估計誤差的方差陣在放大至(βi-1Pg≤1)以后，通過反饋這一方式傳遞給第一級的子濾波器，從而子濾波器的預(yù)測估計值得到更新。

與此同步，第二級中主濾波器的估計誤差的方差陣也會更新成全局估計誤差的方差陣的βm-1倍，此時該誤差的方差陣為。因為βi(i=1,2,…,N,m)是依據(jù)信息的分配原則決定，所以βi也被稱為是系統(tǒng)的“信息分配系數(shù)”。βi的值不一樣，則聯(lián)邦濾波器的總體結(jié)構(gòu)與自身特性也不一樣，所以能夠通過分配的不一樣的βi值得到不一樣的聯(lián)邦濾波器性能，像容錯能力、濾波精度與計算量大小等等性能。

3.2 濾波器設(shè)計

水面無人作業(yè)船上安裝有GPS、姿態(tài)參考系統(tǒng)等各類用于測量系統(tǒng)狀態(tài)的傳感器，它們可以用來實時檢測無人作業(yè)船的經(jīng)緯度、航向角、航行速度等船體的航行參數(shù)。若要得到更好的導(dǎo)航精準(zhǔn)度，一種方法為提升各類測量傳感器的量測精度，除此之外還可以運用Kalman濾波的方法對各類導(dǎo)航參數(shù)實行信息融合，從而得到關(guān)于系統(tǒng)狀態(tài)的最優(yōu)估計。

在無人作業(yè)船的導(dǎo)航中，普遍采用航向姿態(tài)參考系統(tǒng)（AHRS）與GPS相組合的導(dǎo)航模式。本文所設(shè)計的聯(lián)邦濾波器就是基于這種組合方式的濾波模式。該聯(lián)邦濾波器結(jié)構(gòu)中包含有兩個子濾波器，由AHRS與GPS構(gòu)成。當(dāng)βm=0時，這樣就沒有信息分配到主濾波器的狀態(tài)方程中去。在此情況下，主濾波器里不做濾波計算，而只做信息融合，同時將融合后的信息反饋至各子濾波器。

基于系統(tǒng)的量測信息，取系統(tǒng)的狀態(tài)變量為X=[xkykUψ]T，其中xk、yk分別表示經(jīng)度與緯度，U為船舶相對水的合速度，ψ為船舶的航向角。系統(tǒng)的全局狀態(tài)方程為：

表示該系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，而系統(tǒng)的噪聲陣為Γ=I4×4，W(k)為獨立的高斯白噪聲。同時兩個子濾波器的狀態(tài)方程和全局狀態(tài)方程一致。

由于各子濾波器的狀態(tài)方程與全局狀態(tài)方程相同，在主濾波器中可以先對全局狀態(tài)估計值進(jìn)行時間更新，并獲得預(yù)測狀態(tài)的估計量，其算法如下：

對主濾波器實行濾波誤差方差矩陣的預(yù)測更新過程如下：

然后按信息分配法則將預(yù)測誤差方差陣Pi(k+1/k)=βi-1Pg(k+1/k)分配給各子濾波器，省去在各子濾波器中單獨進(jìn)行預(yù)測計算的時間，可以有效減少計算量。

4 仿真試驗

對文中所設(shè)計的聯(lián)邦濾波器進(jìn)行仿真實驗，試驗中設(shè)置融合周期和局部濾波周期長1 s，仿真時間設(shè)置成900 s，設(shè)置信息分配系數(shù)為β1=β2=0.5。計劃航速為2 kn，計劃航向為60°。其他有關(guān)參數(shù)如下：σ1=15 m；σ2=15 m，σ3=0.1 m/s；σ4=0.0087 rad。 圖 1與圖2為GPS的經(jīng)緯度數(shù)據(jù)在濾波前后的對比圖：圖中虛線曲線是未經(jīng)濾波的GPS數(shù)據(jù)，虛線曲線上的多處地方存在毛刺和突變，經(jīng)緯度數(shù)據(jù)在這些時刻波動性比較大，這些地方都將使導(dǎo)航出現(xiàn)誤差；實線曲線是經(jīng)過濾波后的數(shù)據(jù)，可以很明顯的看出其中的毛刺與突變明顯減少，數(shù)據(jù)更加平滑，這將有效降低GPS數(shù)據(jù)誤差較大給航跡跟蹤帶來的影響。

同理AHRS的航向數(shù)據(jù)濾波前后的對比如圖3所示，虛線波形是聯(lián)邦濾波前的數(shù)據(jù)，這些數(shù)據(jù)中有許多突變的點，誤差有些大，最小值為59.3°，最大值為60.9°，航向角數(shù)據(jù)的波動可能對航跡跟蹤效果的影響不是非常大，但是經(jīng)過濾波后的數(shù)據(jù)更精確，對于提升航跡跟蹤的精度起到一定的作用。

5 結(jié) 論

在無人作業(yè)船的導(dǎo)航中，設(shè)計了聯(lián)邦濾波器。該聯(lián)邦濾波器結(jié)構(gòu)中包含有兩個子濾波器，由AHRS與GPS構(gòu)成。。該算法能夠同時對系統(tǒng)的多個狀態(tài)變量進(jìn)行濾波，有效地降低系統(tǒng)的狀態(tài)誤差，進(jìn)而保證了系統(tǒng)的實時性、穩(wěn)定性，提高了濾波精度。應(yīng)用聯(lián)邦卡爾曼濾波器可以有效提高GPS定位以及AHRS測姿的精度。