基于AUV航向運動的動態(tài)狀態(tài)反饋控制研究

　　摘要：針對自主水下機器人(AUV)航向運動易受縱向速度影響問題，提出了基于AUV縱向速度的動態(tài)狀體反饋控制法。所謂動態(tài)狀態(tài)反饋控制是指系統(tǒng)控制器的反饋控制參數(shù)隨著縱向速度與系統(tǒng)模型的變化而變化。該控制方法可消弱縱向速度對航向運動的影響。根據(jù)所研究AUV系統(tǒng)航向運動特點，將航向運動控制模型分解為二階控制模型，依據(jù)此模型控制特點構(gòu)建動態(tài)狀態(tài)反饋控制法。兩套AUV湖泊試驗表明：所提動態(tài)狀體反饋控制法設(shè)計簡單，具有較強的外界抗干擾能力，可消除耦合狀態(tài)量縱向速度對航向的影響，且兩套AUV系統(tǒng)均具有良好的控制品質(zhì)。

　　本文源自周煥銀; 郝以閣， 控制工程 發(fā)表時間：2021-06-03 17:36

　　關(guān)鍵詞：自主水下機器人;航向運動;動態(tài)狀體反饋控制;強耦合性

　　1引言

　　AUV(AutonomousUnderwaterVehicle)系統(tǒng)航向運動是其能否順利完成水下作業(yè)的關(guān)鍵技術(shù)之一[1-3]。然而，AUV系統(tǒng)航向控制與縱向速度有著較強的耦合關(guān)系，相互干擾，導(dǎo)致系統(tǒng)航向控制精度變差[4-6];另外，AUV系統(tǒng)所受外界波浪力易影響系統(tǒng)航向運動控制品質(zhì)，故系統(tǒng)航向運動控制策略應(yīng)具有較強的抗干擾能力[7]。

　　狀態(tài)反饋控制是通過反饋增益矩陣將系統(tǒng)特征方程的根配置為期望極點，使得被控對象狀態(tài)誤差以指數(shù)形式衰減，該控制方法具有消弱超調(diào)量、縮短調(diào)節(jié)時間等控制優(yōu)勢[8-9]。針對水下機器人運動控制問題，狀態(tài)反饋控制法通過與其他控制方法的結(jié)合，如：神經(jīng)網(wǎng)絡(luò)狀態(tài)反饋法[10]、狀態(tài)反饋滑模控制法[11]等，提高水下機器人運動控制品質(zhì)。文[12]采用全狀態(tài)反饋控制法控制某一近水面航行體，仿真驗證了該方法的漸近穩(wěn)定性，達到了期望動態(tài)性能指標(biāo)要求。ThorI.Fossen等人采用輸出狀態(tài)反饋控制法以實現(xiàn)對無人水下機器人推進器的控制，仿真結(jié)果表明該控制方法具有較強抗外界干擾流的能力[13]。ZaopengDong等采用后推狀態(tài)反饋控制法以提高欠驅(qū)動近水面無人航行體航向控制精度[14]。針對水下機器人運動控制過程中外部干擾問題，文[15]提出了狀態(tài)反饋法與H∞相結(jié)合的控制方法，仿真證明該方法的魯棒性。相對于PID控制法，狀態(tài)反饋控制法在水下機器人運動控制中的主要優(yōu)勢為無需根據(jù)系統(tǒng)硬件或外界環(huán)境調(diào)整控制參數(shù)，且具有更好的控制性能[16]。

　　在湖泊實驗過程中所研究AUV系統(tǒng)航向運動控制在前期PID控制中出現(xiàn)的主要問題為：1)PID控制參數(shù)根據(jù)AUV系統(tǒng)縱向速度的變化分段設(shè)置，期間需花費較多試驗時間驗證這些算法的有效性;2)系統(tǒng)模型參數(shù)出現(xiàn)較大變化，需通過大量外場實驗，重新設(shè)置PID控制參數(shù)，以達到預(yù)期控制效果。針對以上問題，本文提出了動態(tài)狀態(tài)反饋控制法，該控制法充分利用狀態(tài)反饋控制法的控制優(yōu)勢，提高系統(tǒng)控制品質(zhì)。所謂‘動態(tài)’是指狀態(tài)反饋控制參數(shù)根據(jù)AUV系統(tǒng)縱向速度動態(tài)和模型參數(shù)自動調(diào)整，以解決強耦合狀態(tài)縱向速度對系統(tǒng)航向運動的影響及模型參數(shù)對系統(tǒng)控制參數(shù)的影響。通過在兩套AUV系統(tǒng)的湖泊實驗對動態(tài)反饋滑模控制方法進行驗證分析。

　　2航向運動控制模型

　　本文AUV運動模型采用輪機工程學(xué)會(SNAME)術(shù)語公報的體系結(jié)構(gòu)[17]。所研究AUV系統(tǒng)航向運動控制的執(zhí)行機構(gòu)為垂直舵，該系統(tǒng)航向運動特點將選取偏航力矩方程與偏航角速度運動方程對系統(tǒng)航向運動進行控制，以及航向角從運動坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)換方程[18-19]：

　　偏航力矩方程:2rzrrrrrIrNrNurNrrNu=+++dd(1)系統(tǒng)航向角速度從載體坐標(biāo)系到地面坐標(biāo)系的轉(zhuǎn)換方程為：sincoscoscosqrjjyqq=+(2)

　　對于無特殊要求的UMV系統(tǒng)而言，其中橫搖角jpjp:/6/6-<<與縱傾角qpqp:/6/6-<<，兩角度都比較小，縱傾角速度q應(yīng)控制在比較小范圍內(nèi)，許多文獻[18]將式(2)簡化為：y=r(3)

　　方程(1)與方程(3)可構(gòu)建狀態(tài)方程為：200()01100(4)0zzrrrrrINrNurNutNrrddyyéùéùéùéù-éùêúêúêúêú=+êúëûëûëûëûëûéù+êúêúëû相關(guān)模型參數(shù)與變量解釋說明：在地面坐標(biāo)系下的位姿向量即在三個坐標(biāo)軸上的位移與以三個坐標(biāo)軸為基準(zhǔn)按右手定則獲取的角速度，y為航向角或艏向角(弧度);在載體坐標(biāo)系下的相關(guān)術(shù)語為：u為縱向速度(m/s)、v為側(cè)向速度(m/s)、r為航向角速度(rad/s);,,YNNvvr為加速度水動力參數(shù);******YYNN,,,為非線性水動力系數(shù)。

　　由于系統(tǒng)航向角速度較小，且非線性部分?jǐn)?shù)量級為小數(shù)二次冪，加速了非線性部分的衰減速度，故在根據(jù)所研究AUV系統(tǒng)運動控制模型特點，將模型[4]簡化為模型(5)：20()100rzzrzzrrNuNurrININtddyyéùéùéùéùêúêúêúêú=+--êúêúëûëûêúêúëûëû(5)AUV航向湖泊試驗過程中根據(jù)需要將期望值yd設(shè)為定值，在AUV系統(tǒng)實際運行過程中，其側(cè)向速度期望值dv與航向角速度dr的期望值為零，故各狀態(tài)誤差為：vdevv=-，rderr=-與dey=-yy;各狀態(tài)誤差微分為：vdevvv=-=，rderrr=-=與dey=-=yyy。

　　將各狀態(tài)誤差項代入狀態(tài)方程(5),所研究AUV系統(tǒng)航向狀態(tài)誤差方程為：20()100rrrzzrzzrrNuNueeININteedyydéùéùéùéùêúêúêúêú=+--êúêúëûëûêúêúëûëû(6)根據(jù)狀態(tài)方程式(6)分析動態(tài)狀體反饋控制策略的構(gòu)建過程。

　　3 動態(tài)狀態(tài)反饋控制策略設(shè)計

　　根據(jù) AUV 系統(tǒng)航向運動狀態(tài)誤差方程(6)設(shè)置動態(tài)狀態(tài)反饋控制策略，所謂動態(tài)是指狀態(tài)反饋的控制參數(shù)可根據(jù)縱向速度與 AUV 系統(tǒng)模型參數(shù)的變化而自動調(diào)整控制參數(shù)，AUV 航向運動控制框圖如圖 1 所示。

　　狀態(tài)反饋控制法的設(shè)計步驟[20]：1)判斷系統(tǒng)能控性;2)若系統(tǒng)完全能控，預(yù)設(shè)狀態(tài)反饋控制參數(shù) (為待求量)，獲取含有控制參數(shù)的特征方程;3)預(yù)設(shè)期望極點，構(gòu)建期望特征方程;4)將含有控制參數(shù)的特征方程與期望特征方程進行對比，求取控制參數(shù)。

　　3.1 航向誤差運動模型能控性分析

　　系統(tǒng)實現(xiàn)狀態(tài)反饋控制的首要條件是系統(tǒng)具有能控性。

　　系統(tǒng)能控性分析，設(shè)系統(tǒng)狀態(tài)方程為： x Ax bu ? ? (7) 其中為 x 狀態(tài)變量， A 為系統(tǒng)矩陣， b 為輸入矩陣。判據(jù) 1 [20]系統(tǒng)完全能控的充要條件是系統(tǒng)能控判別陣 Qc ： Q b Ab c =? ? (8)滿秩。

　　判據(jù) 1 主要步驟為：首先構(gòu)建能控判別陣;然后分析能控判別陣的秩是否為滿秩;最后根據(jù)該判別陣的滿秩情況分析系統(tǒng)能控性，如果滿秩則系統(tǒng)完全能控，否則，系統(tǒng)不完全能控。

　　根據(jù)判據(jù) 1，可推出所研究 AUV 航向誤差狀態(tài)方程(6)的能控判別陣 Qc 為： 2 3 2 2 ( ) 0 r zz r zz r c zz r N u N N u I N I N Q N u I N ? ??? ?? ?? ? ? ? ?? ?? ? ? ? ? ? ? (9) 由能控判別陣(9)滿秩的充要條件是縱向速度 u ? 0 ，即只要存在縱向速度運動，系統(tǒng)是完全能控的。AUV 系統(tǒng)運動模型(6)具有完全能控性，故可實現(xiàn)極點的任意配置。

　　3.2 狀態(tài)反饋控制法控制參數(shù)確定

　　狀態(tài)反饋控制法控制參數(shù)求解過程實質(zhì)為系統(tǒng)狀態(tài)方程的期望特征方程與加入狀態(tài)反饋控制法后含有控制參數(shù)的特征方程等價問題的求解。獲取步驟為：

　　1)預(yù)設(shè)二階航向運動模型狀態(tài)反饋控制律。設(shè)控制律為： 1 1 1 1 ( ) = r r r r r e t k e k e k k e ? ? ???? ?? ? ? ? ? ?? ? ? ? (10) 其中 1 1 r k k?為待求狀態(tài)反饋控制參數(shù)。

　　將狀態(tài)反饋控制律(10)代入航向狀態(tài)誤差方程(6)，可獲取含有控制參數(shù)狀態(tài)方程為： 1 1 1 1 1 1 1 1 1 0 1 0 0 + = 1 0 r r r r r r e e e a b k k e e e a b k b k e e ?? ? ???? ? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? ? ? ?? ?? ? ? ?? ? ? ?? ? (11) 其中 2 1 1 = ; = r zz r zz r N u N u a b I N I N ?? ? 。

　　含有待求控制參數(shù)的狀態(tài)方程(11)的系統(tǒng)矩陣特征方程為： 1 1 2 2 1 1 1 1 1 2 1 1 1 0 + ( , , ) 0 1 0 ( + ) - r r r a b k b k f k k a b k b k ??????? ?? ? ? ?? ? ? ? ? ? ? ? ? ?? ? (12) 2) 設(shè)置期望極點。

　　根據(jù)系統(tǒng)控制品質(zhì)要求，預(yù)設(shè)期望極點? ? r、 ?(其中? ? r、 ?均具有負(fù)實部)，則可獲得加入狀態(tài)反饋控制律后，系統(tǒng)期望特征方程為：? ??? ? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? r r E r ( ) ( ) ( )( ) 2 1 (13)

　　3) 求取狀態(tài)反饋控制參數(shù)將含有控制參數(shù)的特征方程(12)與期望特征方程(13)聯(lián)立： 2 2 1 1 2 2 2 1 1 1 ( , , )= ( ) ( + ) - = ( ) r r r r f k k E a b k b k ?? ? ?? ?? ? ? ? ? ? ? ??? ? ? ? (14) 聯(lián)立方程(14)左右兩邊函數(shù)對比，求取狀態(tài)反饋控制參數(shù)：

　　將控制參數(shù)(15)代入控制律(10)確定 AUV 系統(tǒng)航向運動狀態(tài)反饋控制律為： 1 1 1 - ( ) - r r r r a t e e b b ? ??? ? ? ???? (16)

　　4)加入動態(tài)反饋控制后 AUV 閉環(huán)系統(tǒng)的穩(wěn)定性理論推導(dǎo)。證明：將動態(tài)狀態(tài)反饋控制律(16)代入系統(tǒng)航向狀態(tài)誤差方程(6)有： 2 1 1 1 1 1 1 0 ( ) 1 0 0 - ( ) - - = - r r r zz r zz r r r r r r r r r N u N u e e I N I N t e e a t e e b b a e b b e ?? ?? ??? ???? ? ? ??? ? ? ?? ? ? ? ? ?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ?? ???? ? ? ? ? ?? ?? ? ? ? ?? ? ?? (17) 將 2 1 1 = ; = r zz r zz r N u N u a b I N I N ?? ?代入式(17)并整理得： 1 1 1 1 1 0 ( ) 1 0 0 - ( )= - r r r r r r r e e a b t e e a e t b b e ? ?? ???? ? ? ???? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ?? ? ? ? ? ?? ? ?? ? ? ? ?? ? (18)對公式(18)整理得： 1 1 1 1 1 0 1 0 - - 0 r r r r r e e a e e b a e b b e ? ?? ??? ? ? ?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ?? ? ? ? ?? ? ?? ?? ? ? ?? ?? ?(19)故將動態(tài)狀態(tài)反饋控制律(16)加入方程(6)后，得方程(20)：

　　方程(20)的特征方程為： [ ( )] 0 ? ? ? ? ? ? ? ? ? ? r r ? ? (21)故特征方程(20)的特征根為：? ? ? ? = = r; ? (22)由于? ? r、 ?均具有負(fù)實部，故加入動態(tài)反饋控制后 AUV 閉環(huán)系統(tǒng)穩(wěn)定。

　　5)相關(guān)參數(shù)與控制律說明。將 ut() 視為模型參數(shù)處理;另外，由于系數(shù) 1 b 含有 ut() 項存在于控制參數(shù)分母部分，為了避免控制參數(shù)過大，導(dǎo)致系統(tǒng)控制器控制量過大，湖泊實驗縱向速度在控制律(16)中的約束范圍為式(23)： 1 ( ) 1 ( )= ( ) ( ) 1 if u t u t u t if u t ? ??? ? (23)由于系統(tǒng)(6)采用狀態(tài)反饋控制法，其所配置極點均具有負(fù)實部，根據(jù)狀態(tài)反饋控制設(shè)計步驟，知將控制律(16)代入系統(tǒng)(6)后，系統(tǒng)相關(guān)狀態(tài)變量誤差以指數(shù)衰減為零，從而實現(xiàn)系統(tǒng)各狀態(tài)大道預(yù)期的控制目標(biāo)。

　　3.3 動態(tài)狀體反饋控制策略分析

　　本文所構(gòu)建的動態(tài)狀體反饋控制法設(shè)計簡單，且能保證 AUV 系統(tǒng)良好的控制品質(zhì)。動態(tài)狀體反饋控制法主要優(yōu)勢是：1)具有根據(jù)系統(tǒng)縱向速度動態(tài)調(diào)整控制參數(shù)的控制品質(zhì);2)如果系統(tǒng)控制執(zhí)行機構(gòu)相似，且已知系統(tǒng)模型參數(shù)，該方法可自動調(diào)整控制參數(shù)適應(yīng)于被控對象;3)該方法利用了狀態(tài)反饋控制的控制優(yōu)勢，保證系統(tǒng)狀態(tài)誤差指數(shù)衰減，可降低系統(tǒng)穩(wěn)態(tài)誤差和提高系統(tǒng)動態(tài)性能能。該控制策略設(shè)計步驟為：

　　1)航向狀態(tài)方程確定。根據(jù) AUV 系統(tǒng)航向運動控制特點，構(gòu)建系統(tǒng)航向運動控制狀態(tài)方程;

　　2)AUV 系統(tǒng)航向運動能控性分析。分析系統(tǒng)航向運動控制模型的能控性，AUV 系統(tǒng)航向控制狀態(tài)方程多為能控系統(tǒng);

　　3)期望特征方程構(gòu)建。根據(jù) AUV 系統(tǒng)航向遇到弄期望動態(tài)特性要求，配置狀態(tài)反饋閉環(huán)極點，根據(jù)式(13)以得到加入狀態(tài)反饋后的期望特征方程;

　　4)狀態(tài)反饋控制參數(shù)求取。期望特征方程(13) 與加入控制律(12)后的特征方程進行對比，根據(jù)式(14)獲得狀態(tài)反饋控制參數(shù);

　　5)模型參數(shù)代入。將系統(tǒng)狀態(tài)方程(5)所含模型參數(shù)代入控制律(16)，從而確定系統(tǒng)控制律，此控制律將在縱向速度約束條件(17)下動態(tài)調(diào)整。

　　4 AUV 系統(tǒng)航向控制湖泊試驗驗證

　　由于分段PID控制策略在AUV系統(tǒng)航向運動過程中出現(xiàn)的運動控制品質(zhì)欠佳，且需通過多次實驗驗證每一組PID控制參數(shù)組的合理性等問題，根據(jù) AUV系統(tǒng)航向運動控制特點構(gòu)建了動態(tài)狀體反饋控制法。并在兩套AUV系統(tǒng)(型號為：AUV-I 與 AUV-II)上對動態(tài)狀體反饋控制方法控制優(yōu)勢進行驗證分析，同時在水面與水下湖泊試驗對該控制策略的魯棒性與穩(wěn)定性進行了驗證分析。

　　圖2為PID控制下AUV系統(tǒng)(將所研究AUV系統(tǒng)命名為AUV-I)航向運動控制輸出曲線，航向控制任務(wù)要求：定向240°，縱向速度2節(jié)(1.08m/s)水面航行;運行150s后，變速為6節(jié)(約為3.24m/s)，后又變速為4節(jié)(2.16m/s)。從圖中放大部分曲線可以發(fā)現(xiàn)AUV在水面航向(0-200s)時，航向控制未達到預(yù)期控制效果，最大航向偏差為6°，分析其原因為運行時所設(shè)置的PID控制參數(shù)不適合水面航向;另外，AUV-I水下航行由6節(jié)變?yōu)?節(jié)過程中，航向最大抖動幅度約為0.8°，分析原因是由于不同航速下 PID控制參數(shù)不一致引起。

　　圖3為AUV-I水面航行航向運動控制數(shù)據(jù)分析圖。系統(tǒng)水面航行任務(wù)要求為：縱向速度為2節(jié)(1.1m/s)，水面定向240°航行結(jié)果如圖2所示。分析圖3可知AUV系統(tǒng)水面航行過程中航向最大穩(wěn)態(tài)誤差未超過1.5°，無穩(wěn)態(tài)誤差，航向運動具有良好的控制品質(zhì)。

　　圖 4 為 AUV-I 系統(tǒng)下潛后水下航向控制試驗圖。AUV-I 系統(tǒng)水下航行任務(wù)要求為：縱向速度變化順序為 7 節(jié)(2.6m/s)、2 節(jié)(1.08m/s)、3 節(jié)(1.6m/s)，航向變化順序為：240°、60°，試驗結(jié)果表明系統(tǒng)航向控制過程具有轉(zhuǎn)向平穩(wěn)、無超調(diào)、調(diào)節(jié)速度快，穩(wěn)態(tài)誤差小等良好控制品質(zhì)，且縱向速度的變化不影響系統(tǒng)航向運動控制。

　　AUV-I 湖泊試驗數(shù)據(jù)分析表明動態(tài)狀體反饋控制律(16)在約束條件(17)下，系統(tǒng)具有良好的控制品質(zhì)，且不易受耦合項縱向速度影響。

　　將控制策略(16)及約束條件(17)應(yīng)用于另一套 AUV 系統(tǒng)中，稱之為 AUV-II，該系統(tǒng)航向控制執(zhí)行機構(gòu)與 AUV-I 相似，即均為尾部兩垂直舵控制。水下任務(wù)要求如圖 5 所標(biāo)注。定向航行過程中，系統(tǒng)縱向速度由 8 節(jié)(3.82m/s)轉(zhuǎn)速到 5 節(jié)(2.5m/s) 再到 4 節(jié)(2.18m/s)，航向任務(wù)為定向 240°轉(zhuǎn)向到 331°再轉(zhuǎn)向到 61°再轉(zhuǎn)向到 151°，試驗數(shù)據(jù)分析表明該控制策略在 AUV-II 航向運動控制中具有良好的控制品質(zhì)，靜態(tài)誤差小，縱向速度變化不影響系統(tǒng)航向運動。

　　通過 AUV 系統(tǒng)水面、水下航向湖泊試驗數(shù)據(jù)分析表明動態(tài)狀體反饋控制發(fā)具有良好的運動控制品質(zhì)，相對分段 PID 控制策略削弱了耦合狀態(tài)項縱向速度對航向運動的影響。通過在兩套 AUV 系統(tǒng)試驗表明該控制策略通用性強，若已知 AUV 系統(tǒng)相關(guān)控制模型參數(shù)，該控制方法可適用于具有相同控制執(zhí)行機構(gòu)的 AUV 系統(tǒng)。采用該控制方法避免了根據(jù) AUV 系統(tǒng)載體不斷調(diào)試控制參數(shù)的過程，保證了系統(tǒng)航向運行的穩(wěn)定性。

　　5 結(jié) 論

　　針對 AUV 系統(tǒng)航向運動控制模型特點，提出了動態(tài)狀態(tài)反饋控制法控制 AUV 系統(tǒng)航向。通過 AUV 系統(tǒng)水面試驗表明所提動態(tài)狀態(tài)反饋控制法對外界干擾具有較強的抗干擾能力，水下試驗表明該控制算法具有無超調(diào)、響應(yīng)快且靜態(tài)誤差小等特點，保證了系統(tǒng)航向運動的控制品質(zhì); 且該控制方法可削弱耦合項縱向速度對航向運動的干擾問題，另外，該控制方法具有較好的通用性，在兩套 AUV 系統(tǒng)湖泊試驗過程中均取得了良好的控制品質(zhì)。