隨機波浪下基于粒子濾波算法的船舶航向控制

王化明，岳彩宇，陳 林，王恒家，婁貞光

(1.浙江海洋大學船舶與海運學院，浙江舟山 316022；2.浙江海洋大學東海科學技術學院，浙江舟山 316000)

船舶航向控制是船舶運動控制的主要內容之一。在實際船舶航行中，船舶擁有良好的航向控制性能，能夠保證航行的安全、降低能源損耗。隨著控制理論的發展，將控制算法[1-4]應用在船舶控制當中，實現對船舶航向的自動控制。在低海況下，控制算法可以精確地控制船舶航向，但是船舶在高海況下航行時，船舶的控制性能會迅速下降，這是由于高海況下船舶以非線性運動為主，且隨機波浪對船舶運動狀態的影響很大以及控制算法與船舶狀態變量的交互影響所造成的。通過對船舶狀態變量進行濾波處理，得到其真實狀態值，從而可以更有效地控制船舶的航向。因此濾波算法在對船舶航向控制性能提升方面具有重要意義。

目前，利用整體型船舶運動模型研究船舶在水面運動，主要是僅考慮在靜水條件下，未考慮環境擾動對船舶運動的影響，如WANG Xuegang,et al[5]基于整體型船舶模型在靜水條件下利用支持向量機對船舶運動黑箱建模。實際生活中，環境擾動對船舶運動影響非常大，如風、水流、波浪等，其中波浪影響特別明顯，因此需要考慮波浪的影響，建立一種船舶擾動模型來準確地描述船舶的運動情況。在平穩隨機過程的波浪作用下，船舶擾動呈現的是一種非平穩非隨機過程，屬于一種非常復雜的非線性運動，船舶每一時刻運動變量都會在真實狀態附近上下波動，因此通過濾波算法估計得到船舶運動變量的真實狀態很有必要。傳統是通過卡爾曼濾波進行數據處理，如郭鵬飛等[6]利用卡爾曼濾波算法對目標的狀態進行估計。但是該算法要求系統是線性系統。隨著非線性估計理論的發展，出現了適用于非線性系統的濾波算法[7-8]，其中粒子濾波算法[9-11]非常適合解決非線性系統的狀態估計問題。

本文基于船舶非線性運動數學模型，考慮隨機波浪的影響，建立一種船舶擾動模型更準確的來描述船舶的水面運動。利用粒子濾波算法對船舶運動變量進行狀態估計，再根據比例-積分-微分(proportion integral derivative,PID)控制算法得到船舶命令舵角，對船舶的航向進行調整，使得船舶在隨機波浪下的航向更穩定。

1 船舶擾動數學模型

一般采用船舶三自度整體型運動數學模型研究船舶的水面運動。考慮波浪對船舶水面運動影響，在三自由船舶運動數學模型基礎上，引入波浪項。由于波浪對船舶的作用較為復雜，本文將有向浪簡化為隨機波浪，僅研究在隨機波浪下船舶的水面運動情況。船舶的擾動數學模型為：

船舶的姿態方程及舵機表達式為：

其中，m 表示船的重量，X，Y，N 分別表示船舶縱向運動方向、橫向運動方向、艏搖運動方向的力和力矩；u，v，r，δ，ψ 分別表示船舶縱向運動速度、橫向運動速度、艏搖角速度、舵角、航向角；Tr為時間常數，δr為命令舵角；Xw，Yw，Nw分別表示波浪對船舶縱向運動、橫向運動、艏搖運動的主干擾力和力矩。隨機波浪下船舶受到的主抗力和力矩的計算公式[12]，隨機波浪的頻率是根據能量均分法得到的。

對式(1)轉換可以得到船舶的狀態方程，即：

對上式離散化處理，經整理后可以表示為：

船舶在實際航向中，可通過姿態傳感器的測量數據得到船舶的各種狀態信息，如縱向速度橫向速度、艏搖角速度的值，由于儀器本身的因素使得測量的數據中存在中高斯噪聲。船舶的觀測方程可表示為：

其中，m1(t)，m2(t)，m3(t)為高斯白噪聲。

2 粒子濾波算法

粒子濾波(particle filter,PF)的理論來源于貝葉斯估計，通過空間中一組帶有權值的粒子來表示貝葉斯估計中的狀態量的后驗概率密度函數。它解決了貝葉斯估計中由于積分存在導致后驗概率密度函數難以計算問題，適用于任何能狀態空間模型表示的非線性系統。

對于非線性系統，通常可以由狀態方程和觀測方程表示[13-14]：

其中，xt-1表示系統狀態量；nt表示系統噪聲；yt表示觀察量；lt表示觀察噪聲；系統噪聲nt和觀測噪聲lt相互獨立。根據貝葉斯估計原理[15]，可求得狀態變量x0：k的后驗概率密度p(x0：t|y1：t)。根據蒙特卡羅仿真原理，任意函數g(·)的數學期望表示為：

因此，式(8)可以近似寫為：

然而由于后驗概率分布函數不能直接得到，也使得通過一組粒子去近似后驗概率密度難以實施。貝葉斯重要性采樣定理給出了這個問題的解決方法，即先從一個已知的、易于采樣的分布函數q(x0：t|y1：t)中采樣，通過對由分布函數采樣得到的粒子進行加權來近似p(x0：t|y1：t)。

對式(8)進行變換可以得到：

通過式(11)，利用帶有權值的一組離散粒子可以實現對系統狀態的估計。令u=g1(·)，v=g2(·)，r=g3(·)，利用粒子濾波對船舶運動變量的狀態進行估計。

為了實現對后驗分布的遞推估計，采用序貫重要性采樣方法，對權值更新公式進一步處理，獲得粒子權值迭代計算公式。可將分布函數改寫為：q(x0：t|y1：t)=q(xt|x0：t-1|y1：t)q(x0：t-1|y1：t)可以得到：

由于粒子濾波對系統狀態估計逼近最優估計[16]，分布概率密度函數只與xk-1和zk有關，則：

在序貫重要性采樣方法中，最常見的分布函數為：

將式(15)帶入到式(14)可得：

為了防止粒子匱乏，對粒子進行重采樣[17]，重采樣主要思想是對粒子和其相應的權值表示的概率密度函數重新進行采樣。通過提高權值較大粒子的數目和降低權值較小粒子的數目。經過均勻重采樣后，每一個粒子的權重都會變為，即：

權重的計算公式可簡化為：

在對權重進行歸一化處理后，通過式(11)對系統狀態進行估計。

3 PID 航向控制算法

增量式PID 控制算法適合對離散的數據處理，因此將其應用于船舶的航向控制。船舶指定航向與實際航向的偏差表達式為：

其中，ψd(t)為t 時刻船舶的指定航向值，ψ(t)為t 時刻船舶的航向實際值。

命令舵角表達式為：

4 算法流程

(1)整體粒子濾波及船舶航向控制流程如圖1所示。假設狀態初始概率分布結合t 時刻的值u，v，r 可以得到3 組采樣，每組含有N 個粒子，分別表示t 時刻u，v，r 的先驗分布。

圖1 粒子濾波及船舶航向控制流程Fig.1 Particle filtration affects the ship's course control process

(2)將所得到每個粒子的值代入到系統狀態方程和觀測方程后計算得到t 時刻每個粒子的觀測值，將u，v，r 代入到船舶擾動方程得到t 時刻各運動變量的值作為真實觀測值，根據式(17)計算每個粒子的權重，并對粒子的權重歸一化處理。

p(x0)為高斯分布。

(5)采用PID 航向控制算法，由ψt，δt計算得到船舶t 時刻的命令舵角。通過四階龍格庫塔法計算得到t+1 時刻的船舶的各速度值、航向角和舵角，即ut+1，vt+1，rt+1，ψt+1，δt+1，作為t+1 時刻的真實觀測值。

(6)重復步驟(2)～(5)。利用步驟(3)更新的3 組新粒子群與t+1 時刻的觀測值，計算t+1 時刻的權重及估計新的各速度值。

5 仿真實驗與分析

以“Mariner”號為對象，基于船舶擾動運動數學模型，利用MATLAB 軟件仿真船舶的航向運動。船長L為171.8 m，船寬B 為23.17 m，吃水D 為8.23 m。擾動模型中各流體動力導數的值取自PMM[18]試驗。船的質量無量綱m′為7.98×10-3，轉動慣量無量綱I′z為3.92×10-4，船舶重心的縱向坐標無量綱x′G為-2.3×10-2。波浪譜采用P-M 波譜。

利用粒子濾波算法對船舶的縱向速度、橫向速度、艏搖角速度進行優化。每組粒子數設為200，粒子初始概率密度p(x0)采用標準正態分布的概率密度。船舶縱向速度u 初值為7.717 5 m·s-1，橫向速度v 為初值0 m·s-1，艏搖角速度r 初值0 rad·s-1，舵角δ初值為0°，航向角度ψ 初值為0°，指定航向角ψd為15°。Tr取0.25。機械舵角受實際限制[19]，即|δ|≤35°。

式(17)指的是在給定的狀態量的條件下，每一種輸出值出現的可能性。從觀測方程可得對于1 個確定的輸入值，輸出值僅受到觀測噪聲的影響，而船舶各運動速度是通過傳感器采集得到，對于傳感器中的噪聲一般認為是高斯噪聲，因此每一個粒子權重的計算公式可以表示為：

船舶運動總時間為3 200 s，采樣間隔為1 s，采用臨界比例法多次測試得到1 組參數值，即Kp=3，KI=0.000 1，KD=0.5。在2 級海況(有義波高為0.5 m)下仿真船舶未濾波處理以及在經過粒子濾波處理后的船舶直航運動(航向保持)與轉向運動(航向跟蹤)。

5.1 直航運動

圖2 和圖3 表示的是在相同的航向控制器控制下，船舶在波浪作用下做直航運動時，經濾波處理和無濾波處理條件下的航向角與舵角變化曲線。從圖2 可以看出經粒子濾波后船舶的航向變化很小，在±2°以內，大幅度提升了船舶航向控制的精度。由圖3 可看出經粒子濾波處理后，舵角的幅度控制在±5°，降低了操舵的幅度，減少了舵機的磨損，降低了能源的消耗。對于無濾波時，舵角的幅值較大，一方面是船體受波浪擾動影響；另一方面跟PID 控制參數取值有關，雖然采用臨界比例法確定了一組控制參數，但是本質上還是通過經驗確定的，存在著誤差，且在波浪的影響下，又進一步擴大了誤差，進而影響到操舵的幅值。

圖2 波浪條件下航向角變化Fig.2 Course angle variation in sea condition

圖3 波浪條件下舵角變化Fig.3 Change of rudder angle in sea condition

5.2 轉向運動

圖4 和圖5 表示的是在相同的航向控制器控制下，船舶在波浪作用下做轉向運動時，經濾波處理和無濾波處理條件下的航向角與舵角變化曲線。船舶在1 621 s 時指定航向角由更改為。從圖4 可以看出在1 621 s之前，船舶做直航運動，之后在航向控制算法控制下船舶開始轉向，最后將船舶航向角平穩調整到且保持不變，實現了對航向的跟蹤。圖5 在1 621 s 舵角達到最大舵角，以此快速調整船舶航向到新的指定航向角。對于船舶整個轉向運動，在相同的控制器控制下，經粒子濾波處理后，船舶的航向角更接近指定航向角，且變化的范圍更小，說明了濾波提升了控制算法對船舶航向控制的精度。

圖4 波浪條件下航向角變化Fig.4 Course Angle variation in sea condition

圖5 波浪條件下舵角變化Fig.5 Change of rudder Angle in sea condition

5.3 Z 型運動

圖6 和圖7 表示船舶做Z 型運動時，經濾波處理和無濾波處理條件下的航向角與舵角變化曲線。船舶在運動剛開始時保持直航運動，然后在1 621 s 將指令航向角調整到+10°，在2 929 s 時刻將指令航向角調整到-10°，以此讓船舶做Z 型運動。從圖中可以看出經粒子濾波處理后船舶航向角雖然在波浪影響下出現波動，但是波動范圍很小，波動范圍在±2°以內，舵角變化范圍在±5°以內，與未濾波時的舵角相比較，降低了操舵幅度，減少能源損耗。

圖6 波浪條件下航向角變化Fig.6 Course Angle variation in sea condition

圖7 波浪條件下舵角變化Fig.7 Change of rudder Angle in sea condition

研究船舶航向控制性主要考慮航向角的變化及舵角的擺動情況。采用均方根統計方法對舵角和航向角的誤差進行統計[5]，即：

結果表明在船舶不同類型的運動下，經粒子濾波處理后，航向角和舵角的均方根誤差都在一定程度得到降低，船舶的航向更加穩定，舵角擺舵幅度得到降低，如表1 所示。

表1 航向角和舵角的均方根誤差Tab.1 Mean square root error of heading angle and rudder angle

6 結論

本文基于整體型船舶運動模型，考慮波浪對船舶運動狀態的影響，建立三自由度船舶擾動模型，采用PID 航向控制算法仿真船舶在波浪條件下的直航運動與轉向運動，并利用粒子濾波算法對船舶運動狀態進行估計。可以得到以下結論：

a.航向控制算法根據船舶運動狀態對船舶進行控制，波浪對船舶運動狀態影響較大，直接影響到船舶的航向控制算法對船舶航向控制精度。

b.利用粒子濾波算法可以估計得到波浪環境下船舶的運動狀態，逼近船舶真實的運動狀態。

c.經過粒子濾波估計得到船舶的運動狀態，代入到PID 航向控制算法中，船舶的擺舵幅值和航向角偏差都較小，船舶在波浪中航行時，其航向更加穩定。

d.利用粒子濾波算法對船舶狀態變量進行估計，采用相同的航向控制器控制船舶做直航運動、轉向運動及Z 型運動，船舶航向控制性能都得到了顯著提升。