基于AKF的無人水翼航行器縱向姿態控制研究

孫佳宇，段富海

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

1 引言

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

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

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

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

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

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

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

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

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

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

(1)

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

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

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

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

(4)

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

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

(5)

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

3.2 AKF濾波

KF濾波器的新息定義為：

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

(6)

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

(7)

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

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

(8)

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

(9)

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

4 LQR控制器

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

(10)

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

(11)

狀態反饋增益矩陣K為：

K=-R-1BTP

(12)

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

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

(13)

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

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

圖4 姿態控制系統結構圖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)。

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

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

為驗證基于AKF濾波器的縱向姿態控制器的控制效果，以隨機海浪為干擾，并與基于KF濾波器的縱向姿態控制器的控制效果作以對比。利用Matlab/Simulink搭建仿真模型，仿真模型框圖如圖6所示。以升沉量誤差Δξ和縱搖角誤差Δθ作為評定縱向姿態控制器性能的指標，仿真結果如圖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濾波器的縱向姿態控制器較基于KF濾波器的縱向姿態控制器控制效果更為優良，無人水翼航行器縱搖角誤差Δθ明顯變小，升沉量誤差Δξ也有所改善。

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

6 結論

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

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

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

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