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

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

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

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

　　1引言

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

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

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

　　2航向運動控制模型

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

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

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

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

　　由于系統(tǒng)航向角速度較小，且非線性部分數(shù)量級為小數(shù)二次冪，加速了非線性部分的衰減速度，故在根據所研究AUV系統(tǒng)運動控制模型特點，將模型[4]簡化為模型(5)：20()100rzzrzzrrNuNurrININtddyyéùéùéùéùêúêúêúêú=+--êúêúëûëûêúêúëûëû(5)AUV航向湖泊試驗過程中根據需要將期望值yd設為定值，在AUV系統(tǒng)實際運行過程中，其側向速度期望值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)根據狀態(tài)方程式(6)分析動態(tài)狀體反饋控制策略的構建過程。

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

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

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

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

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

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

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

　　根據判據 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ù)求解過程實質為系統(tǒng)狀態(tài)方程的期望特征方程與加入狀態(tài)反饋控制法后含有控制參數(shù)的特征方程等價問題的求解。獲取步驟為：

　　1)預設二階航向運動模型狀態(tài)反饋控制律。設控制律為： 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) 設置期望極點。

　　根據系統(tǒng)控制品質要求，預設期望極點? ? r、 ?(其中? ? r、 ?均具有負實部)，則可獲得加入狀態(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)定性理論推導。證明：將動態(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、 ?均具有負實部，故加入動態(tài)反饋控制后 AUV 閉環(huán)系統(tǒng)穩(wěn)定。

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

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

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

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

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

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

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

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

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

　　由于分段PID控制策略在AUV系統(tǒng)航向運動過程中出現(xiàn)的運動控制品質欠佳，且需通過多次實驗驗證每一組PID控制參數(shù)組的合理性等問題，根據 AUV系統(tǒng)航向運動控制特點構建了動態(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)航向運動控制輸出曲線，航向控制任務要求：定向240°，縱向速度2節(jié)(1.08m/s)水面航行;運行150s后，變速為6節(jié)(約為3.24m/s)，后又變速為4節(jié)(2.16m/s)。從圖中放大部分曲線可以發(fā)現(xiàn)AUV在水面航向(0-200s)時，航向控制未達到預期控制效果，最大航向偏差為6°，分析其原因為運行時所設置的PID控制參數(shù)不適合水面航向;另外，AUV-I水下航行由6節(jié)變?yōu)?節(jié)過程中，航向最大抖動幅度約為0.8°，分析原因是由于不同航速下 PID控制參數(shù)不一致引起。

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

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

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

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

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

　　5 結 論

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