基于非線性動(dòng)態(tài)逆的水下運(yùn)載器控制方法研究

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

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

0 引言

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

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

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

非線性動(dòng)態(tài)逆方法理論上形式統(tǒng)一、物理概念上清晰直觀、使用上簡(jiǎn)單明了[12]，能夠適應(yīng)水下運(yùn)載器非線性強(qiáng)耦合的控制要求。以時(shí)間為尺度將系統(tǒng)劃分成慢變的角度動(dòng)力學(xué)和快變的角速度動(dòng)力學(xué)，然后對(duì)各個(gè)回路分別進(jìn)行設(shè)計(jì)，通過劃分快、慢子系統(tǒng)，可實(shí)現(xiàn)將全系統(tǒng)的控制器設(shè)計(jì)問題轉(zhuǎn)換為子系統(tǒng)的控制器設(shè)計(jì)問題，計(jì)算的復(fù)雜性也得以降低，但該方法只能用于系統(tǒng)變量在時(shí)間尺度上有明顯差別的系統(tǒng)[13－14]。

本文從水下運(yùn)載器動(dòng)力學(xué)模型建立出發(fā)，基于非線性動(dòng)態(tài)逆控制理論，將系統(tǒng)劃分為慢回路和快回路，通過狀態(tài)反饋分別設(shè)計(jì)了慢回路姿態(tài)控制器和快回路姿態(tài)控制器，解決了非線性、強(qiáng)耦合的控制問題;然后通過Lyapunov 穩(wěn)定性分析證明了控制系統(tǒng)的穩(wěn)定性。最終非線性仿真驗(yàn)證了控制系統(tǒng)的有效性。

1 水下運(yùn)載器運(yùn)動(dòng)數(shù)學(xué)模型

考慮非線性動(dòng)態(tài)逆控制的需要，建立水下運(yùn)載器非線性運(yùn)動(dòng)數(shù)學(xué)模型。

運(yùn)載器體坐標(biāo)系Oxbybzb:原點(diǎn)位于運(yùn)載器質(zhì)心處，Oxb軸沿運(yùn)載器縱軸指向前方;Oyb軸在縱對(duì)稱面內(nèi)垂直于Oxb軸，向上為正;Ozb軸與Oxb軸、Oyb軸構(gòu)成右手系。

設(shè)水下運(yùn)載器的角速度ω 及質(zhì)心處的速度v 在水下運(yùn)載器體坐標(biāo)系中的3 個(gè)分量分別為ωx、ωy、ωz，vx、vy、vz;水下運(yùn)載器的質(zhì)量為m，水下運(yùn)載器Oxb軸、Oyb軸、Ozb軸的3 個(gè)轉(zhuǎn)動(dòng)慣量分別為Jx、Jy、

水下運(yùn)載器的動(dòng)量Q 和動(dòng)量矩K 在水下運(yùn)載器體坐標(biāo)系中的3 個(gè)分量可以表示為

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

式中:F、M 為作用在水下運(yùn)載器上的所有外力的合力與合外力矩，包括重力和浮力、流體動(dòng)力及其力矩;ω 為水下運(yùn)載器體坐標(biāo)系繞慣性坐標(biāo)系的旋轉(zhuǎn)角速度矢量。

將附加質(zhì)量的表達(dá)式引入方程的左邊，經(jīng)運(yùn)算、展開及整理，則動(dòng)力學(xué)方程可寫成矩陣的形式

式中:(Fx，F(xiàn)y，F(xiàn)z)為作用在水下運(yùn)載器上的所有合外力F 在水下運(yùn)載器體坐標(biāo)系下的3 個(gè)分量;(Mx，My，Mz)為合外力矩M 在水下運(yùn)載器體坐標(biāo)系下的3 個(gè)分量;λ11，λ22，λ33為運(yùn)載器的附加質(zhì)量;λ44，λ55，λ66，λ26，λ35為運(yùn)載器的附加轉(zhuǎn)動(dòng)慣量。

為了避免大俯仰情況下出現(xiàn)奇異，水下運(yùn)載器體坐標(biāo)系到地面坐標(biāo)系之間的轉(zhuǎn)換順序?yàn)閆(θ)→Y(ψ)→X(φ).水下運(yùn)載器旋轉(zhuǎn)角速度與3 個(gè)姿態(tài)角變化率之間的關(guān)系式采用如下方程表示:

式中:ψ、θ、φ 為水下運(yùn)載器的偏航角、俯仰角和滾轉(zhuǎn)角。

2 動(dòng)態(tài)逆控制器設(shè)計(jì)及穩(wěn)定性分析

對(duì)水下運(yùn)載器運(yùn)動(dòng)過程的控制，主要是控制姿態(tài)角和角速度，以滿足期望俯仰角θ、偏航角ψ、滾轉(zhuǎn)角φ 的要求。

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

圖1 非線性動(dòng)態(tài)逆控制結(jié)構(gòu)示意圖Fig.1 Nonlinear dynamic inversion control structure diagram

根據(jù)非線性動(dòng)態(tài)逆控制器設(shè)計(jì)的需要，此處忽略浮力造成的力矩對(duì)水下運(yùn)載器的影響，可以通過制導(dǎo)環(huán)節(jié)進(jìn)行補(bǔ)償。由前文所述水下運(yùn)載器運(yùn)動(dòng)模型可以得到慢回路和快回路運(yùn)動(dòng)方程。

記

則有

式中:Mx，My，Mz為作用在運(yùn)載器上的除去浮力造成的力矩以外的總力矩在本體坐標(biāo)系3 個(gè)軸的投影;為水動(dòng)力矩系數(shù);α，β，q，S 和L 為攻角、側(cè)滑角、動(dòng)壓、特征面積和特征長(zhǎng)度。

2.1 慢回路動(dòng)態(tài)逆控制器設(shè)計(jì)

慢回路動(dòng)態(tài)逆控制器根據(jù)由制導(dǎo)系統(tǒng)產(chǎn)生指令要求θc，ψc，φc以及水下運(yùn)載器自身狀態(tài)變量，設(shè)計(jì)出合理的角速度指令ωxc，ωyc，ωzc，并輸出到快回路中。

首先證明慢回路動(dòng)態(tài)逆控制器的穩(wěn)定性。由方程可以得到水下運(yùn)載器的慢回路運(yùn)動(dòng)模型。

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

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

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

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

將(16)式代入(12)式可得系統(tǒng)閉環(huán)狀態(tài)方程

則有

由于Wθ，Wψ，Wφ均大于0，顯見)為負(fù)定。又由于→∞時(shí))→∞，)為負(fù)定不變，故慢回路系統(tǒng)在式控制律條件下為大范圍漸進(jìn)穩(wěn)定。

2.2 快回路動(dòng)態(tài)逆控制器設(shè)計(jì)

快回路動(dòng)態(tài)逆控制器根據(jù)慢回路動(dòng)態(tài)逆控制器產(chǎn)生的角度指令ωxc，ωyc，ωzc，以及水下運(yùn)載器自身的狀態(tài)變量，獲得舵偏指令δec，δrc，δdc，最終作為控制模塊的輸入。

由方程可以得到水下運(yùn)載器的快回路動(dòng)力學(xué)模型

式中:

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

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

將(23)式代入(21)式可得系統(tǒng)閉環(huán)狀態(tài)方程

快回路穩(wěn)定性證明過程與慢回路證明過程類似。

3 計(jì)算結(jié)果與比較

在MTLAB/SIMULINK 環(huán)境下建立水下運(yùn)載器非線性運(yùn)動(dòng)數(shù)學(xué)模型，并考慮實(shí)際舵機(jī)轉(zhuǎn)角限制。在設(shè)計(jì)控制器時(shí)，快回路帶寬是慢回路帶寬的5 ～10 倍，使得快回路精確地跟蹤慢回路。取參數(shù)Wωx=Wωy=Wωz=10 s－1，Wθ=Wψ=Wφ=2 s－1.以標(biāo)準(zhǔn)俯仰角，偏航角和滾轉(zhuǎn)角作為控制系統(tǒng)的指令信號(hào)，驗(yàn)證所設(shè)計(jì)的控制器。初始條件:x = z =0 m，y= －40 m，α=β=φ=0°，v=10 m/s，M=1 840 kg，ψc=φc=0°.

實(shí)際舵機(jī)轉(zhuǎn)角限制為±20°.俯仰角指令信號(hào)θc如圖2 所示。

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

圖2 俯仰角指令信號(hào)Fig.2 Pitch angle command signal

通過圖3 可知，DI 控制器超調(diào)量為4%，穩(wěn)態(tài)時(shí)間為1 s，穩(wěn)態(tài)誤差為零;而PID 控制器超調(diào)量為12%，穩(wěn)態(tài)時(shí)間為1.5 s，穩(wěn)態(tài)誤差為0.2%.由此可知，DI 控制器實(shí)現(xiàn)控制需要時(shí)所需的控制力矩較小，能夠?qū)崿F(xiàn)快速跟蹤，跟蹤精度高，體現(xiàn)了其精確線性化的能力。

圖3 跟蹤單位階躍信號(hào)對(duì)比圖Fig.3 Chart of tracking unit step signal

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

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

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

滾轉(zhuǎn)通道比例系數(shù)Kgp=0.171;積分系數(shù)Kgi=0.006;微分系數(shù)Kgd=0.435.

在水動(dòng)力參數(shù)無不確定性時(shí)，與PID 控制器俯仰通道仿真結(jié)果對(duì)比如圖4 所示，圖5 給出了DI 控制器的偏航角和滾轉(zhuǎn)角跟蹤誤差，圖6 給出了DI 實(shí)現(xiàn)跟蹤所需的舵偏。在水動(dòng)力參數(shù)－30%不確定性時(shí)，DI 控制器與PID 控制器俯仰通道仿真結(jié)果對(duì)比如圖7 所示。圖8 給出了DI 控制器的偏航角和滾轉(zhuǎn)角跟蹤誤差，圖9 給出了實(shí)現(xiàn)跟蹤控制所需的舵偏。在水動(dòng)力參數(shù)具有+30%不確定性時(shí)，DI 控制器與PID 控制器俯仰通道仿真結(jié)果對(duì)比如圖10 所示。

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

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

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

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

4 結(jié)論

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

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

圖8 －30%不確定性DI 控制器偏航角及滾轉(zhuǎn)角跟蹤誤差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%不確定性俯仰角對(duì)比Fig.10 Tracking pitch angle with 30% uncertainty

圖11 30%不確定性DI 控制器偏航角及滾轉(zhuǎn)角跟蹤誤差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ī)非線性動(dòng)態(tài)逆控制器的設(shè)計(jì)研究[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，F(xiàn)an 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]張浩.水下運(yùn)載器參數(shù)辨識(shí)與控制方法研究[D].西安:西北工業(yè)大學(xué)，2006.ZHANG Hao.Parameter identification and control methods of underwater vehicles[D].Xi'an:Northwestern Polytechnical University，2006.(in Chinese)

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

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

[13]雷延花，陳士櫓.非線性動(dòng)態(tài)逆在大攻角導(dǎo)彈控制系統(tǒng)設(shè)計(jì)中的應(yīng)用[J].彈箭與制導(dǎo)學(xué)報(bào)，2002，23(1):109 －112.LEI Yan－h(huán)ua，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.