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

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

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

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

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

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

1 船舶擾動(dòng)數(shù)學(xué)模型

一般采用船舶三自度整體型運(yùn)動(dòng)數(shù)學(xué)模型研究船舶的水面運(yùn)動(dòng)?？紤]波浪對(duì)船舶水面運(yùn)動(dòng)影響，在三自由船舶運(yùn)動(dòng)數(shù)學(xué)模型基礎(chǔ)上，引入波浪項(xiàng)。由于波浪對(duì)船舶的作用較為復(fù)雜，本文將有向浪簡(jiǎn)化為隨機(jī)波浪，僅研究在隨機(jī)波浪下船舶的水面運(yùn)動(dòng)情況。船舶的擾動(dòng)數(shù)學(xué)模型為：

船舶的姿態(tài)方程及舵機(jī)表達(dá)式為：

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

對(duì)式(1)轉(zhuǎn)換可以得到船舶的狀態(tài)方程，即：

對(duì)上式離散化處理，經(jīng)整理后可以表示為：

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

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

2 粒子濾波算法

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

對(duì)于非線性系統(tǒng)，通?？梢杂蔂顟B(tài)方程和觀測(cè)方程表示[13-14]：

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

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

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

對(duì)式(8)進(jìn)行變換可以得到：

通過式(11)，利用帶有權(quán)值的一組離散粒子可以實(shí)現(xiàn)對(duì)系統(tǒng)狀態(tài)的估計(jì)。令u=g1(·)，v=g2(·)，r=g3(·)，利用粒子濾波對(duì)船舶運(yùn)動(dòng)變量的狀態(tài)進(jìn)行估計(jì)。

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

由于粒子濾波對(duì)系統(tǒng)狀態(tài)估計(jì)逼近最優(yōu)估計(jì)[16]，分布概率密度函數(shù)只與xk-1和zk有關(guān)，則：

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

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

為了防止粒子匱乏，對(duì)粒子進(jìn)行重采樣[17]，重采樣主要思想是對(duì)粒子和其相應(yīng)的權(quán)值表示的概率密度函數(shù)重新進(jìn)行采樣。通過提高權(quán)值較大粒子的數(shù)目和降低權(quán)值較小粒子的數(shù)目。經(jīng)過均勻重采樣后，每一個(gè)粒子的權(quán)重都會(huì)變?yōu)椋矗?/p>

權(quán)重的計(jì)算公式可簡(jiǎn)化為：

在對(duì)權(quán)重進(jìn)行歸一化處理后，通過式(11)對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)。

3 PID 航向控制算法

增量式PID 控制算法適合對(duì)離散的數(shù)據(jù)處理，因此將其應(yīng)用于船舶的航向控制。船舶指定航向與實(shí)際航向的偏差表達(dá)式為：

其中，ψd(t)為t 時(shí)刻船舶的指定航向值，ψ(t)為t 時(shí)刻船舶的航向?qū)嶋H值。

命令舵角表達(dá)式為：

4 算法流程

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

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

(2)將所得到每個(gè)粒子的值代入到系統(tǒng)狀態(tài)方程和觀測(cè)方程后計(jì)算得到t 時(shí)刻每個(gè)粒子的觀測(cè)值，將u，v，r 代入到船舶擾動(dòng)方程得到t 時(shí)刻各運(yùn)動(dòng)變量的值作為真實(shí)觀測(cè)值，根據(jù)式(17)計(jì)算每個(gè)粒子的權(quán)重，并對(duì)粒子的權(quán)重歸一化處理。

p(x0)為高斯分布。

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

(6)重復(fù)步驟(2)～(5)。利用步驟(3)更新的3 組新粒子群與t+1 時(shí)刻的觀測(cè)值，計(jì)算t+1 時(shí)刻的權(quán)重及估計(jì)新的各速度值。

5 仿真實(shí)驗(yàn)與分析

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

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

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

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

5.1 直航運(yùn)動(dòng)

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

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

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

5.2 轉(zhuǎn)向運(yùn)動(dòng)

圖4 和圖5 表示的是在相同的航向控制器控制下，船舶在波浪作用下做轉(zhuǎn)向運(yùn)動(dòng)時(shí)，經(jīng)濾波處理和無濾波處理?xiàng)l件下的航向角與舵角變化曲線。船舶在1 621 s 時(shí)指定航向角由更改為。從圖4 可以看出在1 621 s之前，船舶做直航運(yùn)動(dòng)，之后在航向控制算法控制下船舶開始轉(zhuǎn)向，最后將船舶航向角平穩(wěn)調(diào)整到且保持不變，實(shí)現(xiàn)了對(duì)航向的跟蹤。圖5 在1 621 s 舵角達(dá)到最大舵角，以此快速調(diào)整船舶航向到新的指定航向角。對(duì)于船舶整個(gè)轉(zhuǎn)向運(yùn)動(dòng)，在相同的控制器控制下，經(jīng)粒子濾波處理后，船舶的航向角更接近指定航向角，且變化的范圍更小，說明了濾波提升了控制算法對(duì)船舶航向控制的精度。

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

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

5.3 Z 型運(yùn)動(dòng)

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

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

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

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

結(jié)果表明在船舶不同類型的運(yùn)動(dòng)下，經(jīng)粒子濾波處理后，航向角和舵角的均方根誤差都在一定程度得到降低，船舶的航向更加穩(wěn)定，舵角擺舵幅度得到降低，如表1 所示。

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

6 結(jié)論

本文基于整體型船舶運(yùn)動(dòng)模型，考慮波浪對(duì)船舶運(yùn)動(dòng)狀態(tài)的影響，建立三自由度船舶擾動(dòng)模型，采用PID 航向控制算法仿真船舶在波浪條件下的直航運(yùn)動(dòng)與轉(zhuǎn)向運(yùn)動(dòng)，并利用粒子濾波算法對(duì)船舶運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)?？梢缘玫揭韵陆Y(jié)論：

a.航向控制算法根據(jù)船舶運(yùn)動(dòng)狀態(tài)對(duì)船舶進(jìn)行控制，波浪對(duì)船舶運(yùn)動(dòng)狀態(tài)影響較大，直接影響到船舶的航向控制算法對(duì)船舶航向控制精度。

b.利用粒子濾波算法可以估計(jì)得到波浪環(huán)境下船舶的運(yùn)動(dòng)狀態(tài)，逼近船舶真實(shí)的運(yùn)動(dòng)狀態(tài)。

c.經(jīng)過粒子濾波估計(jì)得到船舶的運(yùn)動(dòng)狀態(tài)，代入到PID 航向控制算法中，船舶的擺舵幅值和航向角偏差都較小，船舶在波浪中航行時(shí)，其航向更加穩(wěn)定。

d.利用粒子濾波算法對(duì)船舶狀態(tài)變量進(jìn)行估計(jì)，采用相同的航向控制器控制船舶做直航運(yùn)動(dòng)、轉(zhuǎn)向運(yùn)動(dòng)及Z 型運(yùn)動(dòng)，船舶航向控制性能都得到了顯著提升。