基于AKF的無人水翼航行器縱向姿態(tài)控制研究

孫佳宇，段富海

(大連理工大學 機械工程學院，遼寧 大連 116024)

1 引言

無人水翼航行器是將水翼與無人航行器相結合，可以大幅提高無人航行器的性能。與機翼類似，水翼工作原理是：在水中運動的水翼依據伯努利原理會產生向上的升力，將航體抬離水面航行，這樣就能有效減少水流對航體的阻力和海浪干擾[1]。保證無人水翼航行器的航行姿態(tài)，并能在復雜海況中平穩(wěn)運行是其發(fā)揮自身性能的重要前提。本文提出了一種無人水翼航行器，并建立其縱向運動數學模型。LQR控制器性能優(yōu)良且適用于復雜MIMO系統(tǒng)的控制[2]，因此選擇其作為無人水翼航行器縱向姿態(tài)控制器。

無人水翼航行器在水中航行過程中需保證姿態(tài)穩(wěn)定，但在實際航行過程中海浪干擾會降低控制器控制性能[3-5]。因此可通過KF濾波器對系統(tǒng)反饋狀態(tài)進行實時最優(yōu)估計來改善控制效果[6-8]。傳統(tǒng)KF濾波器將過程噪聲抽象為方差固定的白噪聲，所以在系統(tǒng)和噪聲特性已知的情況下，傳統(tǒng)KF濾波器可以達到最優(yōu)估計的效果。但在多數情況下系統(tǒng)的噪聲特性均為未知，將導致傳統(tǒng)KF濾波器的精度下降,甚至濾波發(fā)散。針對海浪濾波，可以利用成型濾波器將海浪有色噪聲白化[9-11]，但因為不同海況所對應的成型濾波器不同，所以此類方法并不具有普適性。為解決此類問題，本文提出了一種AKF濾波，可以根據新息卡方檢驗值自適應調節(jié)過程噪聲矩陣，以達到自適應濾波的目的。

本文基于一種無人水翼航行器，首先建立其縱向運動數學模型，并設計了縱向姿態(tài)LQR控制器；然后針對該控制系統(tǒng)設計了一種基于新息卡方檢驗值的AKF濾波器,以減小海浪干擾對無人水翼航行器縱向姿態(tài)的影響；最后通過仿真驗證了基于AKF濾波的縱向姿態(tài)控制器,可以使無人水翼航行器在波浪中平穩(wěn)運行。

2 無人水翼航行器數學模型

無人水翼航行器三維圖和結構示意圖如圖1、圖2所示，在達到一定運行速度后無人水翼航行器可進入翼航狀態(tài)，此時水翼升力可將航體抬離水面。通過伺服電機驅動調整前后水翼的轉角，即可改變水翼升力大小，從而可以調整無人水翼航行器的航行姿態(tài)，保證其在水中穩(wěn)定航行。

圖1 航行器三維圖Fig.1 Three dimensional drawing of the vehicle

圖2 航行器結構示意圖Fig.2 Schematic diagram of the vehicle

翼航狀態(tài)下無人水翼航行器縱向受力如圖3所示，Fsi為前后水翼(i=1為前翼，i=2為后翼)所產生的力；FDi為水翼所產生的阻力；f為連接桿在水中運動所產生的阻力；F為電機推力。以上參數具體計算方法可見參考文獻[12-13]。θ為縱搖角，ξ為無人水翼航行器垂直于水平面升沉量。

圖3 翼航狀態(tài)下航行器縱向受力示意圖Fig.3 Longitudinal force diagram of the vehicle under wing navigation

在Zb方向利用牛頓第二定律并以Yb為轉軸，利用剛體定軸轉動定律，可得到翼航狀態(tài)下縱向垂蕩和縱搖運動數學模型為：

(1)

航行器結構參數如表1所示。

表1 航行器結構參數Table 1 Structural parameters of the vehicle

將無人水翼航行器縱向運動數學模型在工作點:θ=2°、v=6.08 m/s、ξ=-1.04 m處線性化，可得到狀態(tài)方程為：

(2)

式(2)中：

3 AKF濾波器設計

3.1 KF濾波

KF濾波器通常采用以下離散時間模型來描述：

X(k+1)=ΦX(k)+Gu(k)+Γw(k)

Y(k)=HX(k)+v(k)

(3)

式(3)中：X(k)為系統(tǒng)在k時刻的狀態(tài)；Y(k)為k時刻對應狀態(tài)的觀測值；u(k)為系統(tǒng)在k時刻的控制量；Φ為狀態(tài)轉移矩陣；G為輸入控制矩陣；Г為噪聲驅動矩陣；H為觀測矩陣；w(k)為過程噪聲；v(k)為量測噪聲。w(k)和v(k)為不相關的均值為零、方差分別為WQ和WR的白噪聲，且系統(tǒng)的初始狀態(tài)X(0)不相關于w(k)和v(k)，即滿足：

(4)

式(4)中，δkj為克羅內克函數。

KF濾波器推導過程可參考文獻[14]，具體KF濾波器核心五步式(5)為：

(5)

式(5)中：X(k+1|k)與P(k+1|k)分別為以k時刻為基準的對系統(tǒng)在k+1時刻狀態(tài)和誤差協(xié)方差的先驗估計值；X(k+1|k+1)與P(k+1|k+1)分別為系統(tǒng)在k+1時刻狀態(tài)和誤差協(xié)方差的估計量；Kk(k+1)為系統(tǒng)在k+1時刻的濾波增益。

3.2 AKF濾波

KF濾波器的新息定義為：

e=Y(k+1)-HX(k+1|k)

(6)

在濾波過程中，當狀態(tài)變量與觀測量較為準確時，新息服從標準正態(tài)分布。根據KF濾波的正交性原理，新息的馬氏距離的平方服從卡方分布[15]，因此可構造基于新息的卡方檢驗值為：

(7)

式(7)中：φk服從自由度為m的卡方分布，即φk～χ2(m)，m為Y(k)的維數；Ce為新息e的協(xié)方差矩陣。Ce計算公式為：

Ce=HP(k+1|k)HT+WR

(8)

KF濾波器將過程噪聲抽象為方差不變的白噪聲，這就使得當過程噪聲變化時，KF濾波器的精度下降，甚至濾波發(fā)散。當過程噪聲變化時會導致系統(tǒng)的狀態(tài)發(fā)生突變，此時φk將不再服從卡方分布，因此可以根據φk的變化來調整過程噪聲矩陣WQ的值，以達到過程噪聲矩陣WQ隨過程噪聲變化而變化的目的。設定φk的上下界來調整一定程度下的過程噪聲變化。通過多次仿真實驗得知，當φk的上下界分別取置信度為90%和50%對應的卡方值時，濾波效果較好，可得WQ矩陣的判別函數為：

(9)

式(9)中，WQmin和WQmax可根據具體工況進行選取。

4 LQR控制器

LQR算法就是要在對系統(tǒng)可以進行有效控制的前提下付出最小的輸入代價[16]，對如下狀態(tài)空間方程：

(10)

LQR控制的目標就是要找到一個狀態(tài)反饋增益矩陣K，即系統(tǒng)控制輸入為u(t)=Kx(t)，使得如式(11)所示的性能指標達到最小。

(11)

狀態(tài)反饋增益矩陣K為：

K=-R-1BTP

(12)

矩陣P可通過Riccati方程求得，即：

ATP+PA-PBR-1BTP+Q=0

(13)

式(11)—(13)中：Q矩陣是狀態(tài)變量的加權矩陣，常取為半正定對角陣，各項系數分別代表設計者對其對應變量誤差的重視程度；R矩陣是控制量的加權矩陣，表示能量損失的相對重要性，常直接取單位矩陣。

將AKF與LQR控制器相結合，構成無人水翼航行器縱向姿態(tài)控制器，基于AKF的縱向姿態(tài)控制系統(tǒng)結構如圖4所示。

圖4 姿態(tài)控制系統(tǒng)結構圖Fig.4 Structure diagram of the attitude control system

5 仿真及結果分析

為無人水翼航行器設計LQR控制器，選取控制器參數Q=diag([0;100;0;1 000])，R=eye(2),可得到反饋增益矩陣為：

AKF濾波器參數設為：WQmin=diag(1;1;0.3;0.3])，WQmax=diag([10;10;3;3])，WR=0.003eye(4)。

環(huán)境干擾設為有義波高隨時間變化的PM波譜隨機海浪[17]，有義波高變化如圖5所示，遭遇角為120°。

圖5 有義波高變化曲線Fig.5 Meaningful wave height variation curve

為驗證基于AKF濾波器的縱向姿態(tài)控制器的控制效果，以隨機海浪為干擾，并與基于KF濾波器的縱向姿態(tài)控制器的控制效果作以對比。利用Matlab/Simulink搭建仿真模型，仿真模型框圖如圖6所示。以升沉量誤差Δξ和縱搖角誤差Δθ作為評定縱向姿態(tài)控制器性能的指標，仿真結果如圖7、圖8所示。

圖6 仿真模型框圖Fig.6 Simulation model block diagram

圖7 縱搖角誤差Δθ變化情況Fig.7 Pitch angle error Δθ Changes

圖8 升沉量誤差Δξ變化情況Fig.8 Heave error Δξ Changes

不同控制方式數據比較如表2所示，由表2可見，在有義波高隨時間變化的隨機海浪干擾下，基于AKF濾波器的縱向姿態(tài)控制器較基于KF濾波器的縱向姿態(tài)控制器控制效果更為優(yōu)良，無人水翼航行器縱搖角誤差Δθ明顯變小，升沉量誤差Δξ也有所改善。

表2 不同控制方式數據比較Table 2 Data comparison of different control modes

6 結論

1) 針對無人水翼航行器縱向姿態(tài)控制的問題，建立了其縱向運動數學模型并選擇LQR控制器作為其縱向運動姿態(tài)控制器。

2) 針對傳統(tǒng)KF濾波器過程噪聲矩陣固定，導致在外界噪聲未知情況下濾波精度不足的問題，設計了一種可以根據新息卡方檢驗值自適應調節(jié)過程噪聲矩陣的AKF濾波器。

3) 通過仿真對比驗證，在基于AKF濾波器的縱向姿態(tài)控制器控制下，無人水翼航行器可在隨機海浪中平穩(wěn)運行，有效降低了海浪干擾。

該研究可為無人水翼航行器的控制器設計提供理論依據，也可為其他種類航行器設計提供借鑒。