基于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系统的湖泊实验对动态反馈滑模控制方法进行验证分析。

　　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 系统湖泊试验过程中均取得了良好的控制品质。