基于非線性動態逆的水下運載器控制方法研究

崔乃剛，白瑜亮，常亞武，王小剛，許江濤

(1.哈爾濱工業大學 航天學院，黑龍江 哈爾濱150001;2.哈爾濱工程大學 航天學院，黑龍江 哈爾濱150001)

0 引言

傳統的控制器設計在處理非線性系統時，通常是將過程模型線性化，然后再利用線性系統控制理論設計相應的控制器[1]。在很多公開發表的文獻中利用狀態反饋控制、LQR 方法[2－4]、切換控制方法、雙模態方法、反饋線性化方法、LQG 方法、PID 控制[5－6]及滑模變結構控制方法設計了相應的控制器。這些方法均是利用小擾動理論在水下運載器非線性模型的某一平衡位置進行泰勒展開，進而得到近似的線性模型，而后對其進行控制器設計。然而，這些方法用于非線性系統并不總能得到理想的結果。尤其在水下運載器姿態控制問題中，由于系統的高度非線性、強耦合及姿態變化較大，問題更加嚴重，其主要原因是在線性化過程中將不可忽視的非線性關系用線性關系代替或忽略了。

對于如何解決水下運載器非線性控制以及存在不確定性和外界干擾作用時的運動穩定性問題，國內外已有學者進行了研究。文獻[7]結合模糊控制原理與PID 控制應用形式設計了一種S 控制器，結合WL－2 實驗數據通過仿真驗證了S 控制器的性能。Vuilme[8]設計了一種帶有加速度反饋的MIMO Backstepping 控制器，通過數值模擬驗證了在存在洋流干擾和參數不確定性情況下，控制器具有魯棒性。文獻[9]根據滑模控制理論，設計了一種魯棒跟蹤控制器。仿真結果表明該控制器具有良好的魯棒性，對參數變化和干擾存在情況下具有良好的跟蹤性能。張浩[10]在水下運載器縱向運動模型的基礎上設計了俯仰角、偏航角以及深度的PID 控制器，并采用遺傳算法對控制器參數進行整定，同時為了解決控制對模型的依賴設計了模糊控制器，取得了較好的效果。但其PID 控制器是在簡化模型基礎上設計而來，對于非線性考慮不夠;而模糊控制器雖然解決了對模型的依賴，但其控制精度受到影響。張晶晶[11]針對五自由度水下運載器動力學模型的參數不確定性設計了一種魯棒自適應控制器，其不需要已知精確模型參數，就可以獲得較好的系統穩定性，解決了系統不確定性問題，但其忽略了水下運載器動力學模型中滾轉通道的耦合影響，具有一定的局限性。

盡管上述研究對水下運載器非線性控制以及動力學模型存在不確定性和外界干擾問題已有一定的研究成果，然而研究還不夠深入，針對性并不是很強，無法體現控制對象(水下運載器)的特殊性。因此，必須尋找新的非線性控制方法，設計新的控制器。

非線性動態逆方法理論上形式統一、物理概念上清晰直觀、使用上簡單明了[12]，能夠適應水下運載器非線性強耦合的控制要求。以時間為尺度將系統劃分成慢變的角度動力學和快變的角速度動力學，然后對各個回路分別進行設計，通過劃分快、慢子系統，可實現將全系統的控制器設計問題轉換為子系統的控制器設計問題，計算的復雜性也得以降低，但該方法只能用于系統變量在時間尺度上有明顯差別的系統[13－14]。

本文從水下運載器動力學模型建立出發，基于非線性動態逆控制理論，將系統劃分為慢回路和快回路，通過狀態反饋分別設計了慢回路姿態控制器和快回路姿態控制器，解決了非線性、強耦合的控制問題;然后通過Lyapunov 穩定性分析證明了控制系統的穩定性。最終非線性仿真驗證了控制系統的有效性。

1 水下運載器運動數學模型

考慮非線性動態逆控制的需要，建立水下運載器非線性運動數學模型。

運載器體坐標系Oxbybzb:原點位于運載器質心處，Oxb軸沿運載器縱軸指向前方;Oyb軸在縱對稱面內垂直于Oxb軸，向上為正;Ozb軸與Oxb軸、Oyb軸構成右手系。

設水下運載器的角速度ω 及質心處的速度v 在水下運載器體坐標系中的3 個分量分別為ωx、ωy、ωz，vx、vy、vz;水下運載器的質量為m，水下運載器Oxb軸、Oyb軸、Ozb軸的3 個轉動慣量分別為Jx、Jy、

水下運載器的動量Q 和動量矩K 在水下運載器體坐標系中的3 個分量可以表示為

將(1)式、(2)式代入普遍使用的剛體動量和動量矩方程

式中:F、M 為作用在水下運載器上的所有外力的合力與合外力矩，包括重力和浮力、流體動力及其力矩;ω 為水下運載器體坐標系繞慣性坐標系的旋轉角速度矢量。

將附加質量的表達式引入方程的左邊，經運算、展開及整理，則動力學方程可寫成矩陣的形式

式中:(Fx，Fy，Fz)為作用在水下運載器上的所有合外力F 在水下運載器體坐標系下的3 個分量;(Mx，My，Mz)為合外力矩M 在水下運載器體坐標系下的3 個分量;λ11，λ22，λ33為運載器的附加質量;λ44，λ55，λ66，λ26，λ35為運載器的附加轉動慣量。

為了避免大俯仰情況下出現奇異，水下運載器體坐標系到地面坐標系之間的轉換順序為Z(θ)→Y(ψ)→X(φ).水下運載器旋轉角速度與3 個姿態角變化率之間的關系式采用如下方程表示:

式中:ψ、θ、φ 為水下運載器的偏航角、俯仰角和滾轉角。

2 動態逆控制器設計及穩定性分析

對水下運載器運動過程的控制，主要是控制姿態角和角速度，以滿足期望俯仰角θ、偏航角ψ、滾轉角φ 的要求。

根據狀態變量動態響應過程的時間尺度差異采用層疊結構控制的思想，選取如下兩組變量:快變量ωx，ωy，ωz，慢變量θ，ψ，φ.由水下運載器航行力學可知，控制系統對于θ，ψ，φ 的作用是比較慢的，對于ωx，ωy，ωz的作用則很快，因此在求解ωx，ωy，ωz子系統的動態逆時可認為θ，ψ，φ 是常量。首先以指令姿態角與實際姿態角的偏差作為輸入，通過慢回路姿態控制器得到期望角速度;其次以期望角速度與實際角速度的偏差作為輸入，經過快回路姿態控制器得到指令舵偏;最后，指令舵偏輸入非線性模型進行彈道解算。設計快回路帶寬為慢回路帶寬的5 ～10 倍，這樣可以使得快回路快速精確地跟蹤慢回路[13]。系統框圖如圖1 所示。

圖1 非線性動態逆控制結構示意圖Fig.1 Nonlinear dynamic inversion control structure diagram

根據非線性動態逆控制器設計的需要，此處忽略浮力造成的力矩對水下運載器的影響，可以通過制導環節進行補償。由前文所述水下運載器運動模型可以得到慢回路和快回路運動方程。

記

則有

式中:Mx，My，Mz為作用在運載器上的除去浮力造成的力矩以外的總力矩在本體坐標系3 個軸的投影;為水動力矩系數;α，β，q，S 和L 為攻角、側滑角、動壓、特征面積和特征長度。

2.1 慢回路動態逆控制器設計

慢回路動態逆控制器根據由制導系統產生指令要求θc，ψc，φc以及水下運載器自身狀態變量，設計出合理的角速度指令ωxc，ωyc，ωzc，并輸出到快回路中。

首先證明慢回路動態逆控制器的穩定性。由方程可以得到水下運載器的慢回路運動模型。

式中:xf=[ωx，ωy，ωz]T;xs=[θ，ψ，φ]T;

令xsc=[θc，ψc，φc]，定義跟蹤誤差

取慢回路輸出為(θ，ψ，φ)，又因gs(xs)可逆，對(18)式進行反饋線性化，可得控制律如下

式中:Ws=diag(Wθ，Wψ，Wφ)，Wθ，Wψ，Wφ均大于0.

將(16)式代入(12)式可得系統閉環狀態方程

則有

由于Wθ，Wψ，Wφ均大于0，顯見)為負定。又由于→∞時)→∞，)為負定不變，故慢回路系統在式控制律條件下為大范圍漸進穩定。

2.2 快回路動態逆控制器設計

快回路動態逆控制器根據慢回路動態逆控制器產生的角度指令ωxc，ωyc，ωzc，以及水下運載器自身的狀態變量，獲得舵偏指令δec，δrc，δdc，最終作為控制模塊的輸入。

由方程可以得到水下運載器的快回路動力學模型

式中:

當Δ1Δ2Δ3≠0 時，則gf(x)可逆。令xfc=[ωxc，ωyc，ωzc]T，定義跟蹤誤差

快回路輸出為ωx，ωy，ωz，又因gf(x)可逆，對(22)式進行反饋線性化，可得控制律

將(23)式代入(21)式可得系統閉環狀態方程

快回路穩定性證明過程與慢回路證明過程類似。

3 計算結果與比較

在MTLAB/SIMULINK 環境下建立水下運載器非線性運動數學模型，并考慮實際舵機轉角限制。在設計控制器時，快回路帶寬是慢回路帶寬的5 ～10 倍，使得快回路精確地跟蹤慢回路。取參數Wωx=Wωy=Wωz=10 s－1，Wθ=Wψ=Wφ=2 s－1.以標準俯仰角，偏航角和滾轉角作為控制系統的指令信號，驗證所設計的控制器。初始條件:x = z =0 m，y= －40 m，α=β=φ=0°，v=10 m/s，M=1 840 kg，ψc=φc=0°.

實際舵機轉角限制為±20°.俯仰角指令信號θc如圖2 所示。

為了比較非線性動態逆控制器(簡稱DI 控制器)與PID 控制器之間的動態性能，通過跟蹤單位階躍信號，分析二者在動態性能方面的差別。

圖2 俯仰角指令信號Fig.2 Pitch angle command signal

通過圖3 可知，DI 控制器超調量為4%，穩態時間為1 s，穩態誤差為零;而PID 控制器超調量為12%，穩態時間為1.5 s，穩態誤差為0.2%.由此可知，DI 控制器實現控制需要時所需的控制力矩較小，能夠實現快速跟蹤，跟蹤精度高，體現了其精確線性化的能力。

圖3 跟蹤單位階躍信號對比圖Fig.3 Chart of tracking unit step signal

為了進一步分析DI 控制器的性能，在不考慮水動力參數不確定性和考慮水動力參數不確定兩種情況下進行非線性仿真，將DI 控制器俯仰通道仿真結果與PID 控制仿真結果進行對比分析。下面給出PID 控制器三通道控制參數:

俯仰通道比例系數Kfp= 0.347 5;積分系數Kfi=6.513 9;微分系數Kfd= －0.039 1.

偏航通道比例系數Kpp= －0.2;積分系數Kpi=9.580 1;微分系數Kpd=0.043 2.

滾轉通道比例系數Kgp=0.171;積分系數Kgi=0.006;微分系數Kgd=0.435.

在水動力參數無不確定性時，與PID 控制器俯仰通道仿真結果對比如圖4 所示，圖5 給出了DI 控制器的偏航角和滾轉角跟蹤誤差，圖6 給出了DI 實現跟蹤所需的舵偏。在水動力參數－30%不確定性時，DI 控制器與PID 控制器俯仰通道仿真結果對比如圖7 所示。圖8 給出了DI 控制器的偏航角和滾轉角跟蹤誤差，圖9 給出了實現跟蹤控制所需的舵偏。在水動力參數具有+30%不確定性時，DI 控制器與PID 控制器俯仰通道仿真結果對比如圖10 所示。

圖4 無不確定性俯仰角對比Fig.4 Tracking pitch angle with uncertainty

圖5 無不確定性DI 控制器偏航角及滾轉角跟蹤誤差Fig.5 Tracking error of yaw angle and roll angle for DI controller with uncertainty

圖6 無不確定性指令舵偏Fig.6 Rudder Instructions with uncertainty

通過仿真可以看出，在不考慮水動力參數偏差時，DI 控制器與PID 控制器都能夠很好地跟蹤指令信號，但是由跟蹤誤差可以看出DI 控制器跟蹤誤差更小。當水動力系數有30% 偏差時，PID控制跟蹤誤差明顯變大且有發散的趨勢，而DI 控制跟蹤誤差仍然很小。因此，DI 控制具有較好的跟蹤性能，能夠精確地跟蹤指令信號，同時具有一定的魯棒性。

4 結論

圖7 －30%不確定性俯仰角對比Fig.7 Tracking pitch angle with －30% uncertainty

本文控制方法在動態特性方面較PID 控制方法具有較大提升，體現了非線性動態逆方法對非線性系統具有精確線性化的能力;在水動力參數存在不確定性情況下，本文控制器仍具有較好的跟蹤性能，體現了該控制方法具有一定的魯棒性;最重要的是本文控制器設計過程中不需要對動力學模型進行小擾動線性化，因此非常適用于非線性、強耦合、大攻角航行以及姿態變化劇烈的水下運載器控制系統。但是，由于逆誤差的存在，會對控制器的性能產生影響，當逆誤差很大時控制器甚至出現不穩定。后續考慮引入魯棒控制方法對本文控制器進行改進，以保證在逆誤差存在的情況下，控制器仍能使系統滿足魯棒性要求，進一步提高水下運載器的控制品質。

圖8 －30%不確定性DI 控制器偏航角及滾轉角跟蹤誤差Fig.8 Tracking error of yaw angle and roll angle for DI controller with －30% uncertainty

圖9 －30%不確定性指令舵偏Fig.9 Rudder Instructions with －30% uncertainty

圖10 30%不確定性俯仰角對比Fig.10 Tracking pitch angle with 30% uncertainty

圖11 30%不確定性DI 控制器偏航角及滾轉角跟蹤誤差Fig.11 Tracking error of yaw angle and roll angle for DI controller with 30% uncertainty

圖12 30%不確定性指令舵偏Fig.12 Rudder Instructions with 30% uncertainty

References)

[1]李怡勇，沈懷榮.無人機非線性動態逆控制器的設計研究[J].航天控制，2007，25(5):54 －57.LI Yi－yong，SHEN Huai－rong.Gain－scheduling design of UAV flight controller[J].Aerospace Control，2007，25(5):54 －57.(in Chinese)

[2]Leonard N E，Graver J G.MODEL－based feedback control of autonomous underwater gliders[J].IEEE Journal of Oceanic Engineering，2001，26(10):633 －645.

[3]Lei K，Zhang Y，Fan H，et al.Matlab－based simulation of buoyancy－driven underwater glider motion[J].Journal of Ocean University of China，2008，7(2):113 －118.

[4]Wang Y，Zhang H，Wang S.Trajectory control strategies for the underwater glider[C]∥International Conference on Measuring Technology and Mechatronics Automation，2009.ICMTMA'09.IEEE，2009，1:918 －921.

[5]Bachmayer R，Graver J G，Leonard N E.Glider control:a close look into the current glider controller structure and future developments[C]∥Proceedings of OCEANS 2003.San Diego，CA:IEEE，2003:951 －954.

[6]Seo D C，Jo G，Choi H S.Pitching control simulations of an underwater glider using CFD analysis[C]∥Proceedings of OCEANS 2008.Kobe:IEEE，2008:1 －5.

[7]Wang B，Su Y，Wan L，et al.Modeling and motion control system research of a mini underwater vehicle[C]∥International Conference on Mechatronics and Automation，2009.ICMA 2009.IEEE，2009:4463 －4467.

[8]Vuilmet C.A MIMO backstepping control with acceleration feedback for torpedo[C]∥Proceedings of the 38th Southeastern Symposium on System Theory.Cookeville:IEEE，2006:157 －162.

[9]Yang H，Ma J.Sliding mode tracking control of an autonomous underwater glider[C]∥2010 International Conference on Computer Application and System Modeling.Taiyuan:IEEE，2010:555 －558.

[10]張浩.水下運載器參數辨識與控制方法研究[D].西安:西北工業大學，2006.ZHANG Hao.Parameter identification and control methods of underwater vehicles[D].Xi'an:Northwestern Polytechnical University，2006.(in Chinese)

[11]張晶晶.基于遠程遙操作的水下運載器的控制器設計[D].上海:上海交通大學，2011.ZHANG Jing－jing.Control design of remotely operated vehicles based on teleoperation[D].Shanghai:Shanghai Jiaotong University，2011.(in Chinese)

[12]李春文，馮元琨.多變量非線性控制的逆系統方法[M].北京:清華大學出版社，1991.LI Chun－wen，FENG Yuan－kun.Multivariable nonlinear inverse system control method[M].Beijing:Tsinghua University Press，1991.(in Chinese)

[13]雷延花，陳士櫓.非線性動態逆在大攻角導彈控制系統設計中的應用[J].彈箭與制導學報，2002，23(1):109 －112.LEI Yan－hua，CHEN Shi－lu.Nonlinear dynamic inversion applied to design of control system for high angle of attack missile[J].Journal of Projectiles，Rockets，Missiles and Guidance.2002，23(1):109 －112.(in Chinese)

[14]Wise K.Nonlinear aircraft flight control using dynamic inversion[C]∥Proceedings of the 11th American Control Conference.Chicago:IEEE，1992:1322 －1326.