基于AUV航向運動的動態狀態反饋控制研究

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

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

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

　　1引言

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

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

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

　　2航向運動控制模型

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

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

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

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

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

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

　　3 動態狀態反饋控制策略設計

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

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

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

　　系統實現狀態反饋控制的首要條件是系統具有能控性。

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

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

　　根據判據 1，可推出所研究 AUV 航向誤差狀態方程(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 ，即只要存在縱向速度運動，系統是完全能控的。AUV 系統運動模型(6)具有完全能控性，故可實現極點的任意配置。

　　3.2 狀態反饋控制法控制參數確定

　　狀態反饋控制法控制參數求解過程實質為系統狀態方程的期望特征方程與加入狀態反饋控制法后含有控制參數的特征方程等價問題的求解。獲取步驟為：

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

　　將狀態反饋控制律(10)代入航向狀態誤差方程(6)，可獲取含有控制參數狀態方程為： 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 ?? ? 。

　　含有待求控制參數的狀態方程(11)的系統矩陣特征方程為： 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) 設置期望極點。

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

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

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

　　4)加入動態反饋控制后 AUV 閉環系統的穩定性理論推導。證明：將動態狀態反饋控制律(16)代入系統航向狀態誤差方程(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)故將動態狀態反饋控制律(16)加入方程(6)后，得方程(20)：

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

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

　　3.3 動態狀體反饋控制策略分析

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

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

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

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

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

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

　　4 AUV 系統航向控制湖泊試驗驗證

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

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

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

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

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

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

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

　　5 結 論

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