基于聯邦卡爾曼濾波算法的水面無人作業船導航精度研究

張 云

（南京交通職業技術學院 電子信息工程學院，江蘇 南京 211188）

0 引 言

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

本課題以江蘇大學電氣學院和泰州常發農業技術有限公司共同研制的全自動無人作業船為研究平臺，搭建了以Trimble BD982 GNSS 系統為核心的差分定位系統，并利用AHRS航向姿態系統獲取船舶的姿態信息。基于無人作業船舶模型以及硬件結構設計了聯邦卡爾曼濾波器。該算法能夠同時對系統的多個狀態變量進行濾波，有效地降低系統的狀態誤差，進而保證了系統的實時性、穩定性，提高了航跡跟蹤的精度。

1 無人作業船的定位系統

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

為了確保數據的實時性及完整性，本系統使用中斷的方式來接收數據。由于接收機以20 Hz的頻率不斷地向S3C2440發送數據，為了保證能夠完整地接收到GPRMC格式的信息，需要在接收之前進行判斷。可以看到，GPCRMC格式的數據以“GPRMC”字符開頭，并以“ ”結尾，可以依此為依據判斷是否完整地接收到一串數據。

2 航向姿態參考系統

本課題中所使用的姿態模塊是北京博創的UPAHRS微型姿態方位參考系統。該系統用于測量無人作業船的前進速度、橫漂速度、航向角等信息。它在無人作業船自主導航中起到非常關鍵的作用。

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

UP-AHRS姿態模塊它具有非常小的體積，功耗也相當低（小于0.5 W的功率），重量輕巧，在外殼的防護下，抗震及耐壓的性能很不錯，所以該姿態模塊非常可靠。

3 聯邦卡爾曼濾波在航跡控制中的應用設計

3.1 聯邦卡爾曼濾波理論

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

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

3.2 濾波器設計

水面無人作業船上安裝有GPS、姿態參考系統等各類用于測量系統狀態的傳感器，它們可以用來實時檢測無人作業船的經緯度、航向角、航行速度等船體的航行參數。若要得到更好的導航精準度，一種方法為提升各類測量傳感器的量測精度，除此之外還可以運用Kalman濾波的方法對各類導航參數實行信息融合，從而得到關于系統狀態的最優估計。

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

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

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

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

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

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

4 仿真試驗

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

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

5 結 論

在無人作業船的導航中，設計了聯邦濾波器。該聯邦濾波器結構中包含有兩個子濾波器，由AHRS與GPS構成。。該算法能夠同時對系統的多個狀態變量進行濾波，有效地降低系統的狀態誤差，進而保證了系統的實時性、穩定性，提高了濾波精度。應用聯邦卡爾曼濾波器可以有效提高GPS定位以及AHRS測姿的精度。