海流环境自主水下机器人动力定位控制器设计

窦 刚，姜文刚，张楠楠

(江苏科技大学 电子信息学院， 江苏 镇江 212003)

水下机器人作为人类探索海洋的重要手段，在海洋资源的开发和利用中起至关重要的作用。随着微电子、计算机、智能化、导航等技术的快速发展，自主水下航行器(AUV)的研究逐渐成为各国海洋装备技术的前沿[1-4]。AUV是一种自身携带能源、推进器、导航、传感等设备，无需人为操控的水下机器人，依靠嵌入式控制计算机和自主控制软件来执行预定的任务。目前，AUV已经多次成功应用于海底石油、天然气以及矿产资源的勘探。发展具备水下作业能力的AUV是目前水下机器人领域的一个重要方向，其目标是依靠自身携带的机械手等装置，自主完成水下精密作业任务。

动力定位早先应用于船舶控制系统，逐渐取代了传统的锚泊定位，成为深海作业船舶依赖的关键技术，广泛应用于海上定点作业和系泊。其由于强大的适用性和机动性，在其他领域也发挥着至关重要的作用，如海洋平台、ROV、AUV等。水下机器人动力定位系统是一种闭环的控制系统，其功能是通过不断检测实际位置和期望位置的偏差，再根据外界工作环境的影响计算出使本体恢复或者保持指定位置的推力大小，并通过主推进器和多个辅助推进器的操控，达到克服海流等环境干扰，保持自身位置和姿态稳定的目的。这是保证正确有效地执行水下作业的关键[5]。从20世纪末开始，人们便采用PID控制、自适应控制、滑模控制、模糊控制、神经网络控制及其改进算法等，开展了水下机器人运动控制的理论和实践研究，并取得了一系列成果[6-9]。其中，对于动力定位控制问题也进行了大量的研究。文献[10]针对水流速度未知干扰，提出了一种基于反演技术的自适应变结构控制策略，但控制模型较为简单，仅考虑了水平面的情况；文献[11] 提出了一种基于Kalman滤波的海流、不平衡力估计方法，对以后控制方法的研究有重要意义；文献[12]提出了一种新的环境最优动力定位控制策略，保持艏向角顶着海流，降低了辅助推进器的能源消耗，虽然没有考虑姿态角的控制要求，但是能耗的降低具有实际意义。文献[13]提出了一种基于Lyapunov的模型预测控制(LMPC)方法，通过推力的优化策略，实现了自主水下航行器的动态定位控制。近年来，一些学者分别采用被动机械臂(Passive Arm)[14]和视觉系统(Visual System)[15]获得相对位置信息，但由于一些限制，无法获得远距离的位置。文献[16]提出了一种基于逆USBL系统的AUV水下动力定位控制方案，将多个USBL应答器固定在工作区域，同时将收发器安装在AUV上，避免了水声信号的远距离传播，可以获得精确的全局位置信息，从而实现动力定位控制。

目前存在的动力定位智能控制算法主要应用在船舶和大型海洋平台[17]，对于小型水下机器人的研究动力定位极为有限。自主水下机器人结构小、建模复杂、模型参数易发生漂移，对控制器的设计提出了更高的要求。文中重点研究了在动力定位过程中如何克服未知定常海流、外部无法估计测量干扰以及实际作业时存在的参数漂移问题，设计控制器并且证明了在未知干扰下良好的定位能力。

1 海流环境中AUV数学模型

海流是海水大范围大规模相对稳定的非周期性流动，目前还没有精确描述其运动规律的模型，海流的变化一般发生在大尺度的时间和空间变化之下。对于水下作业的机器人来说，时间和空间都处于特定的情况，一般可以认为海流流速和流向都是稳定的，故海流的干扰可以看成是未知定常干扰。

本文研究对象是装有侧向辅助推进器和垂向辅助推进器的六自由度全驱动AUV，海洋环境中六自由度运动坐标系如图1所示。

图1 运动坐标系

水下机器人的运动坐标系由地面坐标系和载体坐标系组成，AUV在水下任意位置和姿态都可以用原点O为中心的地面坐标系表示，其中原点O表示定位目标点。根据国际水池会议及造船和轮机工程学会推荐的参数符号[18]来描述水下机器人的运动，具体变量含义如表1所示。

表1 6DOF运动参数

可用如下方程描述AUV运动动力学特性：

(1)

CAM(vr)vr+D(vr)vr+g(η)=τ+τenv

(2)

式中，η表示地面坐标系下AUV位置和姿态向量；v表示机器人运动的速度加速度矢量；R表示载体坐标系到地面坐标系的转换矩阵；MRB为刚体惯性矩阵；CRB是刚体科里奥利向心力矩阵,MAM为流体作用产生的附加质量惯性矩阵；CAM是流体作用产生的科里奥利向心力矩阵；D是由水动力产生的阻尼力和力矩矩阵；vr表示AUV在载体坐标系下相对流体的速度；vc表示载体坐标系下水流的速度矢量；g(η)为回复力矢量；τ为推进器各方向合力与力矩矢量；τenv为外部环境扰动力矢量。

(3)

g(η)=τ+τenv

(4)

上式中各向量和矩阵的具体表达如下：

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(13)

D(vr)=-diag{Xu+Xu|u||ur|,Yv+Yv|v||vr|,

Zw+Xw|w||wr|,Kp+Kpp|p|,Mq+Mq|q||q|,

Nr+Nr|r||r|}

(14)

(15)

(16)

其中：W为水下机器人重力；B为水下机器人浮力；(xB,yB,zB)为水下机器人载体浮心坐标；c、s和t分别是cos、sin和tan的简写。

本文的控制目标是选择控制力和力矩τ使AUV从任意初始状态η0平稳精确到达期望的位姿ηd并保持，从而保证作业任务过程中的稳定性。

2 控制器设计

复杂环境下的AUV控制同时存在模型参数不确定性、传感器测量误差以及外界环境干扰，传统单一的控制方法难以取得良好的效果。完整的动力定位系统由控制器单元和推力分配单元组成，其工作原理如下：首先，位置传感器、姿态传感器、海流传感器等开始实时测量数据，将这些数据通过非线性观测器实时估计位置信息和水流信息；然后，将这些信息输入计算设备中，与预定的位置姿态进行比照，由自适应滑模控制器计算得到所需的推力和力矩；最后，生成推力指令，由推力分配单元调整各个方向推进器的推力进行推力的一次修正。如此不断地调整推力指令，进行差别修正，完成最终的动力定位。一般来说，只需几分钟的时间就可以稳定停在预定位置，并且面对突然的外界干扰可以实时计算出恢复力和力矩，保持系统稳定。整个系统组成如图2所示。

图2 动力定位控制系统组成

2.1 滑模控制器设计

水下机器人具有强耦合和非线性的特点，而且考虑到时变运动，环境复杂不确定，难以建立精确的运动模型。在机器人的各种控制算法中，滑模变结构控制的滑动模态对于参数变化以及系统未建模的部分具有很强的鲁棒性，这使得滑模控制方法成为控制水下机器人的有效方法之一。

滑模变结构控制是一种特殊的非线性控制方法，表现为控制的不连续性，其特殊之处表现为结构的不固定，可以根据系统当前状态不断变化，使系统到达预定滑动模态。滑动模态的存在使这种方法具有快速响应、对参数变化和扰动不敏感、设计方法简单等特点。AUV系统是一个典型的非线性2阶系统，下面将利用反步法设计滑模控制器。

第1步：

定义地球坐标系下位置姿态跟踪误差z1，其中η为当前的位姿信息，ηd为期望位置姿态矢量，有：

z1=η-ηd

(17)

选取第一个Lyapunov函数：

(18)

对V1求导并代入运动学方程公式(3)可得：

(19)

设载体坐标系下AUV相对速度vr为虚拟控制量，K1为正定对角矩阵，取稳定项α：

α=-R-1(K1z1+vc-ηd)

(20)

第2步：

定义载体坐标系下相对速度跟踪误差：

z2=vr-α

(21)

选取第2个Lyapunov函数：

(22)

然后对V2求导并将第一个Lyapunov函数代入有：

(23)

由于惯性矩阵M为正定对角矩阵，为方便后面计算，取K2=M，代入动力学方程(4)并结合式(21)可推导得：

(24)

D(vr)α-τenv

(25)

选取正实数对角参数矩阵Λ，设计滑模切换控制率：

τst=-Λsign(s)

(26)

考虑到滑模控制方法存在的抖振问题，利用边界层处理方法，设置边界厚度为0.02，sat(s)是经过处理后的切换控制，则最后设计的控制率为：

τ=τeq+τst=-K3z2-RTz1+

(27)

以上的控制率设计均假设AUV模型中的参数已知且不变，但在实际系统中，参数往往会随着时间发生一定的偏移，虽然滑模变结构控制方法具有一定的鲁棒性，但还是会对动力定位的结果产生一定影响，如何消除参数摄动的影响需要对控制器进一步设计。

2.2 自适应参数估计器设计

基于物理机理建立的动力学模型参数很难精确获取[19]，为实现高精度系统建模，需要通过在线估计系统未知参数以降低模型不确定性对建模的影响。以下将利用自适应参数估计方法研究参数在不确定情况下的在线参数估计问题。首先，对AUV动力学模型做如下假设：

假设1：AUV模型不确定项一致有界；

假设2：模型参数具有线性化参数形式,即满足如下关系式：

(28)

为了得到参数估计的自适应率取第3个Lyapunov函数：

(29)

(30)

为了消除估计误差，设计如下参数自适应估计率使上式负定或者负半定，γ表示参数估计率：

(31)

结合2.1中设计的滑模控制器，得到如下模型参数自适应滑模控制率：

τ=-K3z2-RTz1-Λsat(s)+

(32)

2.3 水流速度观测器设计

一般而言，AUV工作环境的水流并不是静止的，而自身携带的传感设备很难测到AUV的绝对速度，此时必须考虑模型中未知的水流速度。下面将单独设计水流速度观测器用于在线估计水流速度，以便在控制通道中消除影响。

根据参考文献[20]设计如下状态观测器：

(33)

(34)

结合上面2个公式可得到观测器的误差状态方程：

(35)

2.4 稳定性分析

定理1 对于由式(3)、式(4)描述的六自由度AUV，给定合适的控制参数矩阵K1、K2、K3、Λ，参数学习率γ以及增益矩阵A和B，在控制率式(32)、参数自适应率式(31)和海流观测器式(33)的共同作用下，从任意初始状态η0出发，能够在有限时间内到达预设状态ηd，且误差全局渐进稳定。

证明首先构造一个适合的李雅普诺夫函数：

V(t) =V1(t) +V2(t) +V3(t) +V4(t)=

(36)

(37)

在2.1节的设计中已经保证式(37)前3项是全局渐进稳定的。下面证明加入海流观测器后系统的全局稳定性，取矩阵C，有

(38)

设Q为任意正定矩阵，取矩阵P使如下关系式成立：

CTP+PC=-Q

(39)

对第4项李雅普诺夫函数求导可得：

(40)

代入式(37)最终得：

(41)

3 算例仿真

为了阐述本文提出的方法的有效性和实用性，采用某六自由度全驱动AUV为控制对象进行仿真，各自由度方向标称参数如下：

m=117,Ixx=10.7,Iyy=11.8,Izz=13.4

Xu=-120,Yv=-90,Zw=-150

Kp=-15,Mq=-15,Nr=-18

Xu|u|=-90,Yv|v|=-90,Zw|w|=-120

Kp|p|=-10,Mq|q|=-12,Nr|r|=-15

动力定位初始状态为：

动力定位期望位姿状态为：

地面坐标系下海流速度为：

海流观测器初始值为：

控制器中矩阵参数选择如下：

K1=0.2I,K2=MI,K3=I

γ=0.1,Λ=20I,A=B=I

利用Matlab编写S函数在Similink中进行仿真，仿真时间设为100 s，在第40 s时对AUV的纵向和侧向施加大小分别为50、100 N，持续时间3.5 s的干扰力。定位过程中水下机器人位置和姿态变化曲线见图3；自适应滑模控制器的六自由度推力和力矩输出见图4；海流观测器的水流速度估计曲线见图5，稳定状态下的定位误差见表2。

图3 位置姿态变化曲线

图4 推力和力矩输出曲线

图5 海流速度分量估计曲线

xyzφθΨ0.020.020.0110-45×10-50.003

以上仿真显示，海流观测器在10 s之内准确估计出海流速度；在推力指令的控制之下水下机器人30 s左右到达了预定状态，整个过程平缓无超调；40 s时在突变干扰下迅速调整位姿保持稳定；稳定之后仍然继续产生推力指令，此时已到达目标位置，推力主要用于抵消海流的影响，保证AUV作业时的稳定性；在动力过程中由于回复力的存在和各自由度之间的耦合作用，各个姿态角都会发生一定的变化，但在控制器的作用下逐渐稳定。可以看出：文中提出的自适应滑模算法具有很快的响应速度；动力定位过程中位置定位误差小于2 cm，姿态角定位误差小于0.003 rad，精度高，满足作业需求；并且可以从突变干扰中快速恢复稳定，整个动力定位系统具有良好的稳态性能和较好的鲁棒性。

4 结束语

本文主要研究在海流干扰下作业型自主水下机器人的动力定位问题。根据AUV在水下作业的精度和鲁棒性的要求，综合考虑了水流的干扰和作业设备引起的模型参数变化，提出了基于海流速度观测器的自适应滑模控制方法，利用海流速度观测器在线估计并在控制通道中进行补偿，通过自适应方法和滑模控制理论设计参数自适应率和推力控制率，克服AUV作业过程中的参数变化对系统稳定性的影响，理论证明系统的全局稳定性。最后的仿真结果显示：此动力定位控制系统具有良好的稳定性和极小的定位误差，说明控制器的设计是合理的，可以考虑将此方法应用于作业型自主水下机器人。