AUV 的精确航迹跟踪系统的鲁棒控制

李 娟，边信黔，熊华胜，王宏健

(1.哈尔滨工程大学 自动化学院，150001 哈尔滨;2.清华大学 核能与新能源技术研究院，100084 北京)

AUV 的精确航迹跟踪能力对于AUV 来说是一项很关键的行为能力，它是完成AUV 作业任务(如海底地形调查，远程航行等)的一种重要技术手段.受海流、海浪等不确定性环境干扰因素的影响，其水下运动是一个复杂的空间六自由度运动.此外，AUV 的精确模型难以得到，即使能够获得较为精确的AUV 模型，该模型也会过于复杂，在进行控制系统设计时必须进行必要的简化，导致模型出现误差.对于AUV 这种状态交叉耦合、非线性、时变性都非常严重的复杂运动，精确航迹跟踪问题就更为复杂［1］.

对于航迹跟踪控制问题，国内外大量学者已经做了深入的研究，并在无人机、无人艇、水面船等领域提出了一些成熟的控制技术.文献［2］设计了无人机目标跟踪系统的鲁棒控制器，较好的实现了高低环与方位环的解耦，较准确的实现了目标跟踪;文献［3］基于滑模变结构控制设计了飞艇的三维航迹控制率;在船舶方面，航迹跟踪控制主要集中在水平面控制，文献［4］假定动力学模型的惯性矩阵为对角阵，基于视线方法和极坐标变换，提出了具有全局鲁棒性的状态反馈控制器，克服了舷摇角速度不能为零的局限，得到了能够跟踪直线参考路径的全局渐近稳定控制器.文献［5］则在MOOS－IvP 构架下，设计命令滤波反步法，实现了表面船对复杂路径的精确跟踪.对于水下AUV，在文献［6－11］都是利用Lypaunov 理论和Backstepping 方法，设计了欠驱动AUV 的路径跟踪控制，并取得了比较好的跟踪效果，但是仍然是集中在水平面的研究，仅有文献［9］考虑了定常海流的影响，保证了路径跟踪误差在常值海流影响下的全局渐近稳定性.

本文基于鲁棒H∞控制理论，将三维航迹跟踪控制分解为水平面和垂直面跟踪控制问题，考虑了线性近似产生的误差、不确定性和外界干扰的影响，建立了AUV 水平面和垂直面的动力学模型，设计了航向控制器和纵倾控制器，有效地实现了AUV 三维航迹跟踪控制，具有很好的稳态控制精度.

1 AUV 航迹跟踪控制系统设计

AUV 的精确航迹跟踪是指AUV 在航迹跟踪制导器和控制器的共同作用下，能够沿着指定的航线航行，并且AUV 实际航线与指令航线的偏差应该在一定的范围，即精确航迹跟踪控制器应当满足相应的控制指标.三维航迹跟踪可以认为是水平面内的航迹跟踪与垂直面内的航迹跟踪的综合.

对于AUV 的水平面精确航迹跟踪问题，可以分解为两个相对简单的子问题:航向制导器和航向控制器.航向制导器是根据AUV 当前的位置与设定的航迹之间水平面内的偏差来调整AUV 的航向，以期缩小并消除这种水平面内的航迹偏差;航向控制器是控制AUV 的航向，以期让AUV 在给定的航向指令下航行.

与AUV 的水平面精确航迹跟踪问题类同，AUV 的垂直面精确航迹跟踪问题也可以分解为两个相对简单的子问题:纵倾制导器和纵倾控制器.纵倾制导器是根据AUV 当前的位置与设定的航迹之间垂直面内的偏差来调整AUV 的纵倾，以期缩小并消除这种垂直面内的航迹偏差;纵倾控制器是控制AUV 的纵倾，以期让AUV 在给定的纵倾指令下航行.

为了简化设计，可以把三维航线跟踪分别投影到固定坐标系(地球坐标系)的水平面(Eξη)和垂直面(Eξζ)中.然后，在两个平面内分别设计制导器，他们与航向控制器、纵倾控制器以及航速控制器组成一个联合的精确航迹跟踪控制系统.其原理见图1.

图1 AUV 精确航迹跟踪原理图

2 AUV 水平面内的精确航迹跟踪控制

假设AUV 的三维空间指令航线在水平面Eξη 的投影为AB，即AB 为AUV 水平面内所需要跟踪的航迹(如图2 所示)，令航迹对应的方向为ψab.AUV 在水平面内的实际位置为(ξauv，ηauv)，该位置时相对设定航迹线的水平面内的航迹偏差为l(规定AUV 在设定航迹线的左边时l 为正值，在右边为负值).则在航迹跟踪制导器的制导算法下得到的AUV 航向指令ψcom为

ψ' 是为了补偿航迹偏差而得到的航向修正值.它的具体表达式为

式中:ψmax，lmax2，lmax1，Kp1，Kp，Kd，Ki均为常数，它是一组可调参数.ψ' 的修整考虑到了AUV 航迹跟踪的快速性和航迹跟踪的性能问题.

图2 水平面航向制导

2.1 AUV 航向控制器设计模型

在得出AUV 方向舵模式下的航向控制器设计模型时，假设AUV 的航行速度稳定于u0，重心在运动坐标系的原点上，并忽略垂直面运动和横摇运动参数的影响(即认为Z 方向的速度和X、Y向的角速度w，p，q 为小量)，并联立AUV 的姿态方程［9，12］，可以得到

式中:m 和L 为AUV 的长度和质量，dψ，dv，dr包括了设计模型中线性近似产生的误差、不确定性和外界干扰，对于系统的有界输入，它们是有界的.

令AUV 的艏向指令为常数ψr，满足˙ψr=0，引入变量ψe=ψr-ψ 可得

因此可以得到AUV 方向舵模式下的航向控制器设计模型

考虑到AUV 运动过程中可能受到的外界定常干扰(如海流)会造成AUV 的艏向控制有静态误差，可以引入一个积分单元来消除这种定常干扰，即

2.2 AUV 航向控制器设计

航迹跟踪时，航向控制器为鲁棒H∞控制器.根据鲁棒H∞控制理论［14］，对AUV 航向动态系统(3)，可以得到几个AUV 运行工况的鲁棒H∞状态反馈控制器.

AUV 鲁棒H∞状态反馈控制器形式为

式中:K 为状态反馈控制器的增益矩阵，x=［ψIψev r］T.

AUV 航速为2 kn 时:

AUV 航速为6kn 时:

AUV 航速为10 kn 时:

基于Riccati 方程解插值的鲁棒H∞控制原理，通过对设计好的第i 和第i+1 工况点的H∞控制器Ki和Ki+1进行插值，可计算出任意第j 个工况点的控制器Kj［13］.当AUV 航速为4 kn 时，插值结果:

对应状态反馈控制器作用下的闭环系统特征根为

3 AUV 垂直面内的精确航迹跟踪控制

假设AUV 的三维空间指令航线在垂直面Eξζ 的投影为CD，即CD 为AUV 垂直面内所需要跟踪的航迹(如图3 所示)，令航迹CD 与水平面夹角对应的倾角为θcd(规定斜向上为正，向下为负).AUV 在垂直面内的实际位置为(ξauv，ζauv)，该位置时相对设定航迹线的垂直面内航迹偏差为h(规定AUV 在设定航迹线的上面时h 为正值，在下面为负值).则在航迹跟踪制导器的制导算法下得到的AUV 纵倾指令θcom为

θ' 是为了补偿航迹偏差而得到的纵倾修正值.它的具体表达式为

式中，θmax，hmax2，hmax1，Kp1，Kp，Kd，Ki均 为常数，它是一组可调参数，θ' 的修正既考虑到了AUV 航迹跟踪的快速性又考虑了航迹跟踪的性能(如消除定常干扰)问题.

图3 垂直面纵倾制导

3.1 AUV 纵倾控制器设计模型

假设AUV 的标称航行速度为u0，重心在运动坐标系的原点上，并忽略水平面运动和横摇运动参数的影响(即认为v，p，r 为小量)，忽略运动参数的高阶项，联立AUV 的姿态方程［13，15］，可以得到

其中，dθ，dq，dw包括了设计模型中线性近似产生的误差、不确定性因素和外界干扰，对于系统的有界输入，它们是有界的.

对上述方程进行分析可以知道，AUV 纵倾控制时，水平舵的控制作用可以认为是由两部分组成:一部分是用于克服AUV 纵倾稳态下扶正力矩影响的δs0，一部分是用于调节AUV 纵倾动态过程的控制作用δs1.即

由分析可知

令AUV 的纵倾角指令为常数θr，满足˙θr=0，θ 为AUV 实际纵倾角，引入变量θe=θr-θ 可得

同样，可以得到引入积分单元之后的纵倾动态调节子系统:

3.2 AUV 鲁棒H∞纵倾控制器设计

航迹跟踪时，纵倾控制器为鲁棒H∞控制器.经设计得到AUV 纵倾的鲁棒H∞状态反馈控制器形式为:

式中:K 为状态反馈控制器的增益矩阵，

4 AUV 精确航迹跟踪控制仿真

AUV 的精确航迹跟踪试验主要是检验航迹跟踪算法、AUV 制导器、AUV 控制器综合效果以及它的性能.由于AUV 航迹跟踪算法并不是与AUV 航速有直接的关系，航速对它的影响是由中间控制器(如航向控制器)体现.因此，这里仅选择AUV 在4kn 航速下进行一系列航迹跟踪试验，并以此来验证AUV 精确航迹跟踪控制的有效性，海流干扰模型可以参见文献［15］.

4.1 同一平面的口字形航迹线跟踪仿真试验

在水平面内完成了不同海流下口字形航迹跟踪仿真试验，跟踪曲线如图4 所示，误差曲线如图5 所示，从仿真结果可以看出，在所设计的航向控制器的作用下，AUV 能够克服海流的影响快速跟踪到设定航迹.

图4 AUV 精确航迹跟踪轨迹图

4.2 三维航迹跟踪仿真试验

从三维航迹跟踪仿真试验可以看出，在所设计的航向控制器和纵倾控制器的共同作用下，AUV 也能够克服外界干扰海流的影响运动到设定航迹上，跟踪误差收敛于0，且在海流的影响下控制效果良好，充分说明该控制器能够有效克服外界干扰的影响.

图5 AUV 航迹跟踪误差曲线

图6 AUV 精确航迹跟踪轨迹图

图7 AUV 航迹跟踪误差曲线

5 结论

本文主要研究了欠驱动AUV 动力学运动模型存在不确定性以及外界干扰下的AUV 精确航迹跟踪问题，将三维跟踪问题分为水平面控制和垂直面控制两部分，分别完成了水平面内的精确航迹跟踪控制和垂直面的精确航迹跟踪控制.仿真试验表明，本文提出的AUV 的精确航迹跟踪控制器具有很好的控制性能，能够较快的精确跟踪设定航迹，能够抑制海流对AUV 航迹跟踪的定常干扰，有很好的稳态控制精度.

［1］EVEN B，PAVLOV A，PETTERSEN K Y.Integral LOS control for path following of underactuated marine surface vessels in the presence of constant ocean currents［C］//Proceeding of the 47th IEEE Conference on Decision and Control.United States Mexico:Institute of Electrical and Electronics Engineers Inc，2008:49840－4991.

［2］陈澜，吴梅，安静文.无人侦察机目标跟踪系统鲁棒控制［J］.测控技术，2005，24(5):58－61.

［3］王晓亮，刘丹，单雪雄.飞艇三维轨迹控制研究［J］.上海交通大学学报:自然科学版，2006，40(12):2164－2168.

［4］DO K D，JIANG Z P，PAN J.Robust and adaptive path following for underactuated ship［J］.Automatic，2004，40:929－944.

［5］VLADIMIR D，DULA N.Command filtered backstepping design in MOOS-IvP helm［C］//Proceedings of the 2010 American Control Conference，ACC2010.Piscataway United states:IEEE Computer Society.2010:5997－6003.

［6］LAPIERRE L，SOETANTO D.Nonlinear path－following control for an AUV［J］.Ocean Engineering，2007，34:1734－1744.

［7］LIONEL L，BRUNO J.Robust nonlinear path-following control of an AUV［J］.Ocean Engineering，2008，33:1756－1784.

［8］SILVESTRE C，PASCOAL A.Control of the INFANTE AUV using gain scheduled static output feedback［J］.Control Engineering Practice，2004，12:1501－1509.

［9］施淑伟，严卫生，高剑等.常值海流作用下的AUV 水平面路径跟踪控制［J］.兵工学报，2010，31(3):375－380.

［10］PEDRO AGUIAR A，HESPANHA J P.Trajectory-following and path-following of underactuated autonomous vehicles with parametric modeling uncertainty ［J］.IEEE Transactions on Automatic Control，2007，52(8):1435－1441.

［11］BIDYADHAR S.DEBABRATA A.Design of a path following controller for an underactuated AUV［J］.Archives of Control Sciences，2009，19(3):245－259.

［12］马岭，崔维成.NTSM 控制的AUV 路径跟踪控制研究［J］.中国造船，2006，47(4):76－83.

［13］熊华胜.AUV 鲁棒控制技术研究［D］.哈尔滨:哈尔滨工程大学，2006:30－44.

［14］吴敏，何勇，佘锦华.鲁棒控制理论［M］.高等教育出版社，2010:89－123.

［15］李殿璞.船舶运动与建模［M］.哈尔滨:哈尔滨工程大学出版社，1999:123－146.