基于粒子群優(yōu)化算法的動力定位非線性觀測器設(shè)計

郭亮琨　楊宣訪　王家林　劉蕾

摘? ?要：針對船舶動力定位控制系統(tǒng)中的狀態(tài)估計問題，提出了一種非線性狀態(tài)觀測器設(shè)計方法。采用類Lyapunov方法設(shè)計無源觀測器，以估計誤差的低通濾波信號作為增廣狀態(tài)變量，通過在觀測器方程中引入增廣狀態(tài)變量以減少高頻運動分量對低頻運動參數(shù)估計的影響，采用粒子群優(yōu)化算法對觀測器增益矩陣中的9個關(guān)鍵參數(shù)進行組合尋優(yōu)進一步提高觀測器動態(tài)性能。還以一艘供給船為例進行仿真分析，驗證了所設(shè)計非線性觀測器的有效性。

關(guān)鍵詞：動力定位;觀測器;粒子群優(yōu)化

中圖分類號：TP273? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?文獻標(biāo)識碼：A

Design of Nonlinear Observer for Dynamic Positioning Based

on Particle Swarm Optimization Algorithm

GUO Liang-kun，YANG? Xuan-fang，WANG Jia-lin？覮，LIU lei

（College of Electrical Engineering，Naval University of Engineering，Wuhan，Hubei 430033，China）

Abstract：Aiming at the state estimation problem in ship dynamic positioning control system，a nonlinear state observer design scheme is proposed. The passive observer is designed by Lyapunov-like method，and the low-pass filtered signal of the estimation error is used as an augmented state variable. By introducing augmented state variables in the observer equation to reduce the impact of high-frequency motion components on low-frequency motion parameter estimation，the particle swarm optimization algorithm is used to optimize the nine key parameters in the observer gain matrix to further improve the dynamic performance of the observer. This paper takes a supply ship as an example to analyze and verify the effectiveness of the designed nonlinear observer.

Key words：dynamic positioning;observer;particle swarm optimization

動力定位（Dynamic Positioning，DP）是指船舶不借助錨泊，而是通過自身安裝的推進器來抵抗風(fēng)、浪、流等環(huán)境干擾的影響，實現(xiàn)其在海面上固定位置或預(yù)期航跡的保持[1]。船舶在海面上運動受到環(huán)境干擾的影響可分為由風(fēng)、流和二階波浪共同作用所導(dǎo)致的低頻運動和由一階波浪導(dǎo)致的高頻運動。由于高頻運動僅表現(xiàn)為周期性的小幅擺動和起伏震蕩，不會造成船體平衡位置的大范圍偏移。為避免不必要的能量浪費和推力器的機械磨損，動力定位控制系統(tǒng)只需針對干擾引起的低頻運動進行補償和控制，而盡量不響應(yīng)一階波浪和噪聲等高頻干擾的作用。但傳感器測量系統(tǒng)只能提供帶有測量噪聲的高低頻疊加的船舶位置和姿態(tài)信息，為此必須采用觀測器對船舶低頻運動狀態(tài)量進行狀態(tài)估計。在現(xiàn)代控制理論領(lǐng)域，很多學(xué)者早已對狀態(tài)觀測器進行了大量研究，包括著名的Luenberger觀測器和工程領(lǐng)域廣泛應(yīng)用的Kalman濾波器等[2-3]，但多存在需要對數(shù)學(xué)模型進行線性化處理和魯棒性不強等問題。非線性觀測器設(shè)計問題要比線性系統(tǒng)復(fù)雜得多，因而引起眾多學(xué)者的關(guān)注[4-7]。

針對水面低速運動船舶采用類Lyapunov 方法設(shè)計一種非線性狀態(tài)觀測器，在文獻[4][5]的基礎(chǔ)上，通過在觀測器方程中引入增廣狀態(tài)變量以提高觀測器濾波性能，采用粒子群優(yōu)化算法設(shè)計最佳的觀測器增益矩陣參數(shù)，進一步提高觀測器動態(tài)性能，最后通過仿真驗證觀測器設(shè)計方案的有效性。

1? ?船舶運動數(shù)學(xué)模型

1.1? ?低頻運動模型

對于水面動力定位船舶，一般用縱蕩、橫蕩和艏搖三個自由度的運動進行描述[4]。建立兩個坐標(biāo)系統(tǒng)：隨船坐標(biāo)系xoy和地球坐標(biāo)系XOY，如圖1所示：

可選取η = [x，y，ψ]T，v = [u，v，r]T，船舶三自由度低頻運動模型為[4]：

■ = R（ψ）v? ? ? （1）

M■ = -Dv + RT（ψ）b + τ? ? ? （2）

式中，M表示系統(tǒng)的慣性矩陣（其包含附加質(zhì)量），并滿足正定要求M = M T > 0;D∈R3×3為線性阻尼系數(shù)矩陣，且D > 0;τ∈R3和b∈R3分別表示推進控制力與力矩和環(huán)境干擾力與力矩;R（ψ）為坐標(biāo)轉(zhuǎn)換矩陣：

R（ψ） = cosψ? ?-sinψ? ?0sinψ? ? ?cosψ? ?0? ?0? ? ? ? ? 0? ? ? 1? ? ? ?（3）

1.2? ?環(huán)境力模型

環(huán)境干擾力通常指風(fēng)、二階波浪、流對船舶的作用力。真實海洋環(huán)境干擾力在船舶的三自由度上來說十分緩慢，因此環(huán)境力模型可由一階馬爾科夫過程描述：

■ = -T -1b + Eb ωb? ? ? ? （4）

式中，b∈R3為環(huán)境干擾力和力矩;T為三維對角矩陣;Eb∈R3×3和wb∈R3為噪聲幅值矩陣和零均值高斯白噪聲向量。

1.3? ?高頻運動模型

在實際中，船舶的高頻運動是對一階波浪的響應(yīng)，通常看作是在三個自由度上附加了阻尼項的二階諧波振蕩器：

hi（s） = ■? ? （5）

式中，ζi（i = 1，…，3）一般取值為0.05～0.2，為相對阻尼系數(shù);σi和波浪強度相關(guān);ω0i （i = 1，…，3）為波浪PM譜的主導(dǎo)頻率。

將其轉(zhuǎn)換為狀態(tài)空間形式，得到船舶高頻運動模型：

■h = Ah ξh + Eh ωh

ηh = Ch ξh? ? ? ? ? （6）

式中， ξh = [ ξx，ξy，ξψ，xh，yh，ψh]T，ηh = [xh，yh，ψh]T表示三自由度方向上的高頻位置和艏向角度;Eh∈R6×3為噪聲幅值矩陣;ωh∈R3為零均值高斯白噪聲向量;

Ah =? 03×3? ? ? ? ?I3×3-Ω2? ? ?-2ΛΩ，Ch = 03×3? ? I3×3

其中，Ω = diag{ω01，ω02，ω03}，Λ = diag{ζ1，ζ2，ζ3}。

1.4? ?系統(tǒng)模型

系統(tǒng)的測量模型為

y = η + ηh + ωy? ? ? （7）

式中，ωy∈R3為零均值的測量白噪聲;y∈R3為測量輸出。

假設(shè)1? ?在觀測器設(shè)計以及穩(wěn)定性分析時，忽略其高斯白噪聲項[4]，即ωb = ωh = ωy = 0。

假設(shè)2? ?由于高頻艏搖角度非常小，在轉(zhuǎn)換矩陣的計算中可忽略不計，即R（ψ + ψh） = R（ψ）。

結(jié)合以上假設(shè)，得到DP系統(tǒng)模型：

■h = Ah ξh

ηh = Ch ξh

■ = R（ψ）v

■ = -T -1bb

M■ = -Dv + τ + RT（ψ）b

y = η + ηh? ? ?（8）

2? ?增廣無源觀測器

2.1? ?增廣無源觀測器設(shè)計

根據(jù)文獻[1]，結(jié)合上述假設(shè)和DP系統(tǒng)模型（8），無源觀測器方程通常設(shè)計如下：

■h = Ah ■h + K1■

■ = R（ψ）■ + K2■

■ = -T -1b■ + K3■

M■ = -D■ + RT（ψ）■ + τ + RT（ψ）K4■

■ = ■ + Ch ■h? ? ? ? ?（9）

式中，■ = y - ■為估計誤差，■，■∈R3為低頻運動（艏向、角速度）的估計值，■h∈R6為高頻運動估計值，K1∈R6×3，K2，K3，K4∈R3×3為觀測器增益矩陣。

為提高非線性觀測器的精度，利用低通濾波器將估計誤差■中的高低頻信號分離，在原有觀測器方程中，再引入新狀態(tài)變量，是估計誤差■的低通濾波信號：

■g = -T -1gxg + ■ = -T -1gxg + ■ + Ch ■h? ? ?（10）

式中，xg∈R3，Tg = diag{Tg1，Tg2，Tg3} 為濾波參數(shù)。

從xg中可以推導(dǎo)出高通濾波信號：

■g = -T -1gxg + ■ = -T -1gxg + ■ + Ch ■h? ? ?（11）

公式（10）和（11）經(jīng)過拉氏變換得到傳遞函數(shù)的形式：

x（i）g（s） = ■■（i）（s）■（i）g（s） = ■■（i）（s），（i = 1，2，3）? ? ?（12）

要求，ωcgi = ■ < ωoi 即濾波器的截止頻率必須要小于波浪譜的主導(dǎo)頻率，才能起到濾波作用。加入新狀態(tài)變量后的增廣無源觀測器方程如下：

■h = Ah ■h + K1g ■g

■ = R（ψ）■ + K2■ + K2 g xg

■ = -T -1b■ + K3 g xg

M■ = -D■ + RT（ψ）■ + τ + RT（ψ）（K4■ + K4 g xg）

■ = ■ + Ch ■h? ? ? ? ?（13）

式中：K1g∈R6 × 3，K2 g ，K3 g ，K4 g ∈R3 × 3為觀測器新的增益矩陣。

2.2? ?穩(wěn)定性分析

設(shè)■ = v - ■為低頻速度、角速度估計誤差;■ = η - ■為低頻位置艏向估計誤差;■h = ξh - ■h為高頻估計誤差;■ = b - ■為環(huán)境干擾估計誤差;由DP系統(tǒng)模型（8）和觀測器方程（13）可以得到觀測器估計誤差方程：

■h = Ah ■h - K1g ■g

■ = R（ψ）■ - K2■ - K2 g xg

■ = -T -1b■ + K3 g xg

M■ = -D■ + RT（ψ）■ - RT（ψ）（K4■ + K4 g xg）

■g = -T -1gxg + ■

■ = ■ + Ch ■h? ? ? ? ?（14）

定義■ = [ ■Th，■T，■T，xTg]T ■ = K4 ■ + K4 g xg - ■，可得到：

■ = A■ + BR（ψ）■? ? ? ? （15）

■ = C■? ? ? ?（16）

■ = M-1[-D■ - RT（ψ）■]? ? ? （17）

式中，

A = Ah-K1 g Ch? ? -K1 g? ? ?0? ? ?K1 gT -1g? ? -K2Ch? ? ? -K2? ? ? 0? ? ? -K2 g? ? ? ? 0? ? ? ? ? ? 0? ? ?-T -1b? ?-K3 g? ? ? ?Ch? ? ? ? ? ? ? ? ? ? ? I? ? ? ? 0? ? ? ?-T -1g

B = 0I00，C = [K4Ch? ? K4? ?-I? ? K4 g]

引理1（正實引理）? ?假定G（s） = C（sI - A）-1B為n × n維的傳遞函數(shù)矩陣，其中系統(tǒng)是可觀可控的。當(dāng)且僅當(dāng)存在正定矩陣P = PT和Q = QT滿足以下公式時，傳遞函數(shù)矩陣G（s）是嚴(yán)格正實的。

PA + AT P = -Q

BT P = C? ? ? ? ?（18）

通過選擇合適的觀測器增益矩陣使由公式

（15-16）構(gòu)成的系統(tǒng)滿足正實引理。現(xiàn)選擇觀測器增益矩陣結(jié)構(gòu)如下：

K1 g = diag{k11 g，k12 g，k13 g}diag{k21 g，k22 g，k23 g}，K2 = diag{k31，k32，k33}

K2 g = {k31 g，k32 g，k33 g}，K3 g = {k41 g，k42 g，k43 g}

K4 = diag{k51，k52，k53}，K4 g = {k51 g，k52 g，k53 g}

參考文獻[5]，增益矩陣參數(shù)的調(diào)整規(guī)則：

k1ig = 2（ωoi ωci+ωcgi ωoi-2ζi ωcgi ωci）（ζi - ζni）■

k2ig = 2（ω2oi-ωci ωcgi）（ ζni - ζi）■

k3i = （ωoi ωci-2ωci ζi ωcgi +2ωci ζni ωcgi）■

k3ig = 2ω2cgiωci（ ζi - ζni）■

其中，ωci > ωoi表示陷波濾波器的截止頻率;ζni > ζi決定陷波作用，表示為設(shè)計阻尼系數(shù);為滿足系統(tǒng)嚴(yán)格正實的要求，增益矩陣k3ig，k4的參數(shù)只需滿足如下不等式：

■≤■< ωcgi < ωoi < ωci（i=1，2，3）? ?（19）

上述公式可以確定增益矩陣k3ig，k4參數(shù)的取值范圍，但實際取值未知，因此本文選用粒子群優(yōu)化算法對觀測器增益矩陣參數(shù)值進行組合尋優(yōu)，以提高觀測器動態(tài)性能。

考慮如下的Lyapunov函數(shù)：

V = ■■T M■ + ■■T P ■? ? ? （20）

對V求導(dǎo)得：

■ = -■■T（PA+ATP）■+■T RT（ψ）BTP ■-

■TR（ψ）■-■■T（D+DT）■? ? ? ? （21）

由于系統(tǒng)傳遞函數(shù)是嚴(yán)格正實的，帶入公式（18）可得：

■ = -■■TQ■ - ■■T（D+DT）■? ? ? ? （22）

根據(jù)Lyapunov穩(wěn)定性判據(jù)，觀測器是全局指數(shù)穩(wěn)定的。

2.3? ?粒子群優(yōu)化算法

1995年，美國學(xué)者J.Kennedy和R.C.Eberhart被鳥群覓食行為所啟發(fā)，提出粒子群優(yōu)化算法（Particle Swarm Optimization，PSO）。很多科學(xué)研究靈感都來自于自然界的動植物的行為，PSO與很多進化類的算法相似，其本身不依賴于優(yōu)化問題的嚴(yán)格數(shù)學(xué)性質(zhì)，具有本質(zhì)并行性，也具有進化和群體智能等優(yōu)點，是一種基于多個智能體的仿生優(yōu)化算法[8]。

PSO算法主要源于模擬鳥群的捕食行為。模擬一群鳥飛行覓食行為，鳥之間通過集體的協(xié)作達到最優(yōu)目的。設(shè)想在一個場景中，一群鳥在隨機搜索食物，但在這塊區(qū)域內(nèi)只有一塊食物，所有鳥也都不清楚具體的位置，但它們可以感受到離食物的距離，找到食物的最優(yōu)策略就是根據(jù)自身經(jīng)驗判斷食物位置和搜索目前距離食物最近的鳥的周圍區(qū)域。粒子群優(yōu)化算法也正是從其中得到了啟發(fā)。

在PSO算法中，將鳥的尋食行為對比到優(yōu)化搜索，每個尋優(yōu)問題解都想象成一只鳥，即"無質(zhì)量的粒子"。而且所有的粒子都可以被判斷目前的位置好壞，每一個粒子也賦予記憶功能，能記住所搜索的最佳位置，每個粒子運動中，由速度決定它的運動方向和距離，粒子通過追隨自身的個體最好位置與群體的全局最好位置來不斷地調(diào)整自己的位置信息[9]。

考慮最小化問題，假設(shè)N維的搜索空間，M為粒子群規(guī)模，則在第t次迭代步為：

第i個粒子的當(dāng)前位置：

Xi（t） = [Xi，1（t），Xi，2（t），…，Xi，N（t）]，將Xi代入適應(yīng)函數(shù)求適應(yīng)值;

第i個粒子當(dāng)前速度：

Vi（t） = [Vi，1（t），Vi，2（t），…，Vi，N（t）]

第i個粒子經(jīng)歷過的最好的位置（pbest）表示為：

Pi（t） = [Pi，1（t），Pi，2（t），…，Pi，N（t）]

然后將第i個粒子當(dāng)前位置的適應(yīng)值與個體最好位置的適應(yīng)值相比較，如果優(yōu)于后者，則令當(dāng)Pi（t） = Xi（t），否則令Pi（t） = Pi（t-1）。

全局最好位置則是粒子群搜索到的當(dāng)前具有最好目標(biāo)函數(shù)值的位置，即為：

G（t） = Pg（t） = [Pg，1（t），Pg，2（t），…，Pg，N（t）]

其中，1≤g≤M，也可以表示為gbest（t），其中g(shù) = arg ■{ f [Pi（t）]}。

PSO算法的速度和位置的更新公式如下：

Vi， j（t+1）=w·Vi， j（t）+c1·r1，i， j（t）·（Pi， j（t）-

Xi， j（t））+c2·r2，i， j（t）·（Gj（t）-Xi， j（t））? ? ?（23）

Xi， j（t + 1） = Vi， j（t） + Xi， j（t）? ? ? （24）

其中，1≤i≤M，1≤j≤N，t表示第t次迭代，c1和c2為加速度常數(shù)，又稱學(xué)習(xí)因子，其中c1指的是粒子指向自身最好位置方向，c2指的是粒子指向全局最好位置;r1，i，j（t）和r2，i，j（t）主要為了增加搜索性，是均勻分布的相互獨立的隨機數(shù)序列，取值范圍為（0，1）;w表示慣性權(quán)重，調(diào)節(jié)對解空間的搜索范圍。通常采用線性遞減公式：

w = wstart - ■ × t? ? ? ?（25）

式中，t為當(dāng)前迭代次數(shù)，tmax為最大迭代次數(shù)，wstart表示初始慣性權(quán)重，wend表示終止慣性權(quán)重。為保證觀測器估計偏差最小，目標(biāo)函數(shù)選取為：

min f（x） = ‖■‖2F? ? ?（26）

式中，‖■‖F(xiàn)為估計誤差矩陣的F-范數(shù)。

PSO算法流程如圖2所示：

3? ?仿真驗證

為了對所設(shè)計的增廣無源觀測器的有效性進行驗證，以某供給船為仿真對象進行仿真實驗。該船長L=76.2 m，質(zhì)量m=4.156×106 kg，主要參數(shù)為：

M = 5.3122×106? ? ? ? ? ?0? ? ? ? ? ? ? ? ? ? 0? ? ? ? 0? ? ? ? ? ?8.2831×106? ? ? ? ? ? 0? ? ? ? ? ? 0? ? ? ? ? ? ? ? ? ?0? ? ? ? ? ?3.7454×106

D = 5.0242×104? ? ? ? ? ?0? ? ? ? ? ? ? ? ? ? 0? ? ? ? 0? ? ? ? ? ?2.7229×105? ?-4.3933×106? ? ? ? 0? ? ? ? ?-4.3933×105? ? 4.1894×108

環(huán)境力模型時間常數(shù)矩陣?。?/p>

T = 1000? ? ? ? 0? ? ? ? ? 0? ?0? ? ? ? 1000? ? ? ?0? ?0? ? ? ? ? ? 0? ? ? 1000

選取船舶高頻模型中的相對阻尼系數(shù)ζi = 0.1，波浪譜的波峰頻率ωoi = 0.89 rad/s，低通濾波器截止頻率ωcgi = 0.5 rad/s，設(shè)計阻尼系數(shù)ζni = 1.0，陷波濾波器截止頻率ωci = 1.1 rad/s。

在增廣無源觀測器中，一階波浪的頻率特性可以直接設(shè)計出其中k1 g k2，k2 g中的12個參數(shù)，矩陣 k3 g k4，k4 g中的9個參數(shù)只能通過經(jīng)驗法反復(fù)試湊，觀測器增益參數(shù)的設(shè)置較為繁瑣，且難以達到組合最優(yōu)。為此，采用粒子群優(yōu)化算法進行尋優(yōu)，以提高觀測器的估計精度。

設(shè)置PSO 算法參數(shù)如下：

搜索空間的維數(shù)N = 9;規(guī)模M = 30，加速常數(shù)c1 = c2 = 2，最大迭代次數(shù)tmax = 100，初始慣性權(quán)重wstart = 0.9，終止慣性權(quán)重wstart = 0.4。

獲得DP船舶仿真結(jié)果如圖3～圖5所示，圖中虛線為估計值，實線為期望值。由于篇幅的限制，僅給出縱蕩方向的仿真曲線。

從圖3～圖5可以看出，所設(shè)計的增廣無源觀測器可以準(zhǔn)確估計出船舶低頻運動分量及高頻運動分量，從而可以實現(xiàn)只針對低頻運動補償?shù)臓顟B(tài)反饋控制，使高頻運動分量不進入反饋回路，避免不必要的能源浪費和機械磨損。同傳統(tǒng)的線性狀態(tài)觀測器相比，本設(shè)計方案不需要對船舶運動的數(shù)學(xué)模型進行近似的線性化處理。

仿真結(jié)果顯示，同文獻[4]相比，由于引入了增廣狀態(tài)變量和采用粒子群優(yōu)化算法設(shè)計組合最優(yōu)的觀測器增益矩陣參數(shù)，觀測器動態(tài)性能和估計精度得到明顯改善。

4? ?結(jié)? ?論

針對船舶動力定位控制系統(tǒng)中的非線性狀態(tài)觀測器設(shè)計問題，提出了一種基于粒子群優(yōu)化算法的增廣無源狀態(tài)觀測器。以估計誤差的低通濾波信號作為增廣狀態(tài)變量，通過在觀測器方程中引入增廣狀態(tài)變量，以減少高頻運動分量對低頻運動參數(shù)估計的影響，在此基礎(chǔ)上采用粒子群優(yōu)化算法對觀測器增益矩陣中的9個參數(shù)進行組合尋優(yōu)，進一步提高觀測器的動態(tài)性能。以一艘供給船為案例進行仿真分析，仿真結(jié)果表明所設(shè)計的增廣無源觀測器可以很好地濾除高頻分量和噪聲的干擾，從測量信息中準(zhǔn)確估計出船舶低頻運動的位移和速度分量。

參考文獻

[1]? ? 邊信黔，付明玉，王元慧. 船舶動力定位[M].北京科學(xué)出版社，2011.

[2]? ? FUNG P，GRIMBLE M J.Dynamic ship positioning using a self-tuning Kalman filter[J].? IEEE Transactions on Automatic Control，1983，28（3）：339-350.

[3]? ? FOSSEN T I，GROVLEN A. Nonlinear output feedback control of dynamically positioned ships using vectorial observer backstepping[J].? IEEE Transactions on Control Systems Technology，1998，6（1）：121-128.

[4]? ? FOSSEN T I，STRAND J P. Passive nonlinear observer design for ships using Lyapunov methods：full-scale experiments with a supply vessel[J].? Automatica，1999，35（3）：3-16.

[5]? ? 何黎明，田作華，施頌椒. 船舶動力定位系統(tǒng)的一增廣非線性觀測器設(shè)計[J]. 船舶工程，2003，25（4）：47-49.

[6]? ? 張相宜，楊宣訪，王家林. 一種魯棒的動力定位系統(tǒng)狀態(tài)估計算法[J]. 計算技術(shù)與自動化，2017，36（3）：6-11.

[7]? ? 賈寶柱，謝燈峰，董秋軍. 船舶動力定位系統(tǒng)非線性無源觀測器的仿真[J]. 船舶工程，2018，40（1）：198-201.

[8]? ? 謝業(yè)海，徐慧璇. 基于PSO優(yōu)化的船舶動力定位觀測器[J]. 控制工程，2015，22（1）：118-122.

[9]? ? 薛彩霞，袁偉，俞孟蕻，等. 遺傳粒子群優(yōu)化算法在船舶動力定位控制中的應(yīng)用[J]. 中國艦船研究，2016，11（4）：111-115.