自主水下航行器(autonomous underwater vehicle,AUV)是人类探索海洋的重要工具,AUV与水下回收平台的对接问题是迫切需要研究解决的关键技术之一。一方面,受体积与质量限制,AUV执行任务时所携带的能源十分有限,为实现水下长期工作,需要在水下进行能量补充。另一方面,AUV在水下执行复杂任务时,需要与水下平台进行信息交换,实现任务信息的上传与任务使命下载。因此,水下对接是保证AUV水下长时间续航和执行复杂作业的前提[1]。
定位是水下回收对接关键问题之一,超短基线(USBL)声学定位系统体积小定位精度高,已广泛应用于回收对接系统的设计中。Woods Hole海洋研究所的Odyssey-IB和REMUS、美国NOSC的free-swimmer和日本川崎重工的marine-bird上都选用了USBL为水下对接系统提供高精度水声定位[2]。
在回坞控制方面,蒙特利海洋研究所[3]对Dorado/Bluefin AUV的回坞对接进行了研究,将回坞对接过程分为回坞和对接2个阶段,在回坞过程中,AUV利用自主导航能力进入USBL定位区域,对接阶段在USBL校准下,采用横向跟踪控制沿对接路径进入回收站。Jantapremjit等[4]采用长基线(LBL)声学定位系统,使用基于人工势场方法的平均矢量场规划AUV回坞路径。其中在回坞阶段,采用传统人工势场法计算一条无碰航路到达对接准备点,对接阶段则通过对地标矢量切换加权,导引AUV到达对接所要求的方位和位置。Batista等基于USBL定位系统提出一种集成式导航控制律[5],获取固定在对接站上应答器位置后,将全局坐标系下的航向偏角和AUV与回坞路径的侧向距离作为对接误差,并采用Lyapunov方法设计鲁棒控制律使系统误差的收敛到零,保证了系统的稳定性,但稳定性证明复杂并且未考虑过程约束。
本文针对安装2个USBL定位应答器的回坞对接平台,不同于Batista等采取的全局坐标系下的误差定义方式,在体坐标系下直接使用应答器测量定义对接误差,并采用非线性模型预测控制(model predictive control,MPC)计算满足系统约束的速度指令。针对未知海流扰动和动态模型中的未知参数采用自适应跟踪控制使AUV能够全局渐近跟踪速度指令。仿真结果验证了对接策略的有效性。
1 对接系统与AUV模型对接系统如图 1所示,2个USBL应答器 B1和B2分别固定在对接站左右两侧,AUV通过USBL定位系统来测量应答器相对AUV的位置。定义2个坐标系来描述AUV的运动:全局坐标系Oxy与体坐标系OBxByB。其中全局坐标系的原点为B1B2的中点O,x轴沿入坞方向,y轴与x轴垂直。体坐标系固连于AUV,原点为浮心,坐标轴xB与yB 的指向分别沿轴向和右侧向。
建立如下的全驱动AUV的运动学模型与动力学模型[6]:
式中, (x,y)与ψ分别为全局坐标系下航行器的位置坐标与航向角,u、v与r分别为体坐标系下航行器的线速度与航向角速度。m11、m22、m33为包含附加质量的惯性系数,d11、d22和d33为阻尼系数,X、Y和N为控制力和力矩,Xc、Yc和Nc为海流干扰力和力矩。本文考虑AUV具有独立的速度控制,仅通过调整侧向速度和航向角速度进行对接控制。在AUV靠近对接站的过程中,AUV通过USBL定位系统获取应答器在体坐标系中的位置,表示为:
式中,(xTi,yTi)为获得的应答器位置测量,l为收发器与体坐标系原点的距离。BiD(xTiD,yTiD)(i=1,2)和BiB(xTiB,yTiB)(i=1,2)分别为应答器在全局坐标系和体坐标系下的坐标。对接过程中AUV处于全驱动航行模式,系统的对接控制目标是使得AUV沿着期望的对接路径航行直至抵达对接站。
2 模型预测对接控制设计 2.1 模型预测控制模型预测控制具有优异的约束处理能力和对复杂过程的适应性,在控制领域特别是工业控制领域得到了广泛应用。MPC基于系统模型预测一定时域内的系统状态,使用最优化方法优化系统状态轨迹并求取当前时刻的控制输入,MPC在机器人[7]、无人机[8]和水下航行器[9]控制领域得到了广泛应用。
预测控制是在每一时刻(k)求解如下优化问题:
优化问题满足: 式中, j|k和j|k分别为在时刻k+j对系统状态和输入的估计。L(·,·)为代价函数,F(Np|k)为终端代价函数。Nc和Np分别为控制时域和预测时域,通常有Np≥Nc。X、T和U分别为状态约束集、终端状态约束集和输入约束集,X、T为闭集,U为紧集,T为对终端镇定器的控制不变集,有0∈T⊆ X。值得注意的是,f(·,·)、X和U取决于控制系统,是固定的,而Nc、Np、L(·)、F(·)和终端状态约束T均为设计项,不同的取值会有不同的预测控制器性能。优化问题的优化输入向量的第1个元素施加于系统,随着时刻k 的滚动计算,得到模型预测控制律。 2.2 对接控制模型对接预测控制器所求取的优化问题是在一定预测时域里某一运动学状态的代价函数的最小化,本文考虑的预测对接控制器将运动学误差定义在航行器体坐标系,避免了通过复杂的几何方程求解全局误差。
首先,将AUV应答器位置测量方程式(3)对时间求导:
值得注意的是和基于图像的视觉伺服控制一样,在上述运动学方程中没有全局坐标系下的坐标与航向角项。考虑到回坞对接需要航行器沿着对接路径航行直到到达对接站,在AUV体坐标系中定义对接误差,即在航行过程中需满足:
为了设计运动学非线性预测控制器,动态方程(6)可用欧拉法近似成如下离散形式:
式中, 、vk=[vk rk]T和ek分别为k时刻系统的状态、输入和输出,在求解优化问题时,根据实际AUV运动特性定义如下约束:
1) 输入约束
式中,vmax=[vmax,rmax]和vmin=[vmin,rmin]T分别为AUV的最大和最小速度指令。
2) 可见性约束
为了保证在回坞过程中,AUV总是能成功获取应答器的位置,定义满足USBL定位系统的可见性约束如下:
式中, αmax 是USBL定位系统最大角度。定理1 对非线性对接系统(8)和预测控制优化问题(9)~(11)式,在预测控制算法设计中选择终端状态约束,此时终端代价函数 F(êNp|k)=0。对接预测控制器为一致渐近稳定的。
证明:参考文献[10],考虑标量优化代价函数值在k 时刻的优化值为Lyapunov函数:
在 k-1时刻优化问题的最优输入序列若T={0},不等式(13) 可化简为: 显然对预测对接控制系统,状态为任意非零值时,都有 ∇V*d(·)>0,即考虑的 Lyapunov 函数V*d(ek) 递减,对接预测控制器为一致渐近稳定。 3 非线性自适应速度跟踪控制器
采用非线性自适应控制[11]以保证AUV存在模型误差的情况下能够跟踪速度指令。为了确保跟踪误差的渐近稳定性引入误差的积分项。
定义速度跟踪误差:
式中,对(15)式求导并将(2) 式代入
式中定义正定Lyapunov函数,
对 V1 求导得到, 考虑到参数 θ未知,选择如下控制律使得1 负定: 式中,Γ1>0为常数增益矩阵,为θ 的估计值。为了设计自适应控制律,定义新的Lyapunov函数。
式中,=θ-为估计误差,>0 为常数自适应增益矩阵,求导有 取自适应律 此时有下面的定理:
定理2 考虑存在参数不确定性影响下的AUV动力学模型(2),在控制律(19)及自适应律(22)的作用下,速度跟踪误差全局渐近稳定。
证明 由(21)式定义的Lyapunov函数 V2正定,且径向无界,其导数2负半定,根据 Barbalat 引理,2→0即
采用REMUS AUV的模型参数对本文所提出的对接控制策略进行仿真验证,REMUS参数见表 1。
参数名 | 参数值 |
m11/kg | 31.43 |
m22/kg | 66 |
m33/(kg·m2) | 8.33 |
d11/(kg·s-1) | 13.5 |
d22/(kg·s-1) | 66.6 |
d33/(kg·m2·s-1) | 6.87 |
仿真中将AUV前向速度 u固定为0.8 m/s,AUV起始位置为(-120,-30),全局坐标系下起始航向角为30°,起始侧向速度v和角速度r分别为0 m/s和0 rad/s。固定在对接站的2个USBL应答器在全局坐标系下的坐标分别为B1D(0,-1)和B1D (0,1)。在非线性预测接控制器里,为保证对接方向取加权矩阵Q=R=I2×2,采样周期Tc=0.5 s,且Nc=Np=20,速度约束和可见性约束分别为:vmax=-vmin=0.3 m/s,rmax=-rmin=0.174 5 rad/s,αmax=π/3 rad。在自适应控制器中Γ1=diag(50,10),Λ=I2×2,自适应增益矩阵=0.01I7×7。
为了验证本文所设计的预测对接控制器的性能,将仿真结果与文献[12]里对速度限幅的反演对接控制算法比较,AUV航行轨迹如图 2所示,从图中可以看到,预测对接控制下AUV能较快航行到对接路径。回坞预测控制器的误差轨迹如图 3所示,预测对接算法误差的收敛速度快于反演对接控制,且误差收敛轨迹更平滑。原因在于2种控制方法的策略不同,文献[12]中的方法是简单地将2个误差解耦,由 v控制yT1B+yT2B,r控制xT1B-xT2B,不考虑速度的限幅,而预测对接是将v和r综合起来,在满足约束的条件下,先控制误差大的量yT1B+yT2B ,使AUV先指向应答器中点,然后一边侧移一边调整方向,所以优化指标收敛更快。从图 2、图 3和图 4可以看出,预测对接控制器因为对代价函数最小化的求取,在处理约束的同时也对轨迹进行了优化,能更好地利用输入和状态之间的耦合,在满足约束条件下使指标更快收敛。而反演对接控制器则只能简单地期望误差指数收敛,在输入约束的条件下无法实现指数收敛。
预测对接控制下AUV航行速度曲线、控制输入和航向角曲线分别如图 5、图 6和图 7所示,回坞预测控制器约束了AUV的航行速度,使其满足AUV的驱动能力。反演自适应速度跟踪在存在参数不确定性时具有良好的速度跟踪性能。
5 结 论本文针对搭载USBL声学定位系统的全驱动AUV,提出一种新的回坞对接控制策略。直接使用声学应答器在体坐标系下的坐标来定义对接误差,采用了非线性模型预测控制,在满足过程约束时获取优化速度指令,并设计自适应跟踪器全局渐近跟踪速度指令。仿真结果验证了回坞对接策略的有效性,比传统的反演设计具有更优的性能。
[1] | Podder T, Sibenac M, Bellingham J. AUV Docking System for Sustainable Science Missions[C]//Proceedings of IEEE International Conference on Robotics and Automation, 2004:4478-4484 |
Click to display the text | |
[2] | 燕奎臣, 吴利红. AUV水下对接关键技术研究[J]. 机器人, 2007, 29(3):267-273 Yan Kuichen, Wu Lihong. A Survey on the Key Technologies for Underwater AUV Docking[J]. Robotics, 2007, 29(3):267-273 (in Chinese) |
Cited By in Cnki (42) | |
[3] | McEwen R S, Hobson B W, McBride L, Bellingham J G. Docking Control System for a 54-cm-Diameter(21-in) AUV[J]. IEEE Journal of Oceanic Engineering, 2008, 33(4):550-562 |
Click to display the text | |
[4] | Jantapremjit P, Wilson P A. Optimal Control and Guidance for Homing and Docking Tasks Using an Autonomous Underwater Vehicle[C]//Proceedings of the 2007 IEEE International Conference on Mechatronics and Automation, 2007:243-248 |
Click to display the text | |
[5] | Batista P, Silvestre C, Oliveira P. A Two-Step Control Approach for Docking of Autonomous Underwater Vehicles[J]. International Journal of Robust Nonlinear Control, 2014, 25(10):1528-1547 |
Click to display the text | |
[6] | Fossen T I. Marine Control Systems:Guidance, Navigation and Control of Ships, Rigs and Underwater Vehicles[M]. Trondheim, Norway:Marine Cybernetics AS, 2002:5-54 |
[7] | Maurovic I, Baotic M, Petrovic I. Explicit Model Predictive Control for Trajectory Tracking with Mobile Robots[C]//Proceedings of the 2011 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, 2011:712-717 |
Click to display the text | |
[8] | Dauer J, Faulwasser T, Lorenz S, Findeisen R. Optimization-Based Feed Forward Path Following for Model Reference Adaptive Control of an Unmanned Helicopter[C]//Proceedings of AIAA Guidance, Navigation and Control Conference, 2013 |
Click to display the text | |
[9] | Molero A, Dunia R, Cappelletto J, Fernandez G. Model Predictive Control of Remotely Operated Underwater Vehicles[C]//Proceedings of the 50th IEEE Conference on Decision and Control and European Control Conference, 2011:2058-2063 |
Click to display the text | |
[10] | Mayne D Q, Rawlings J B, Rao C V, Scokaert P O M. Constrained Model Predictive Control:Stability and Optimality[J]. Automatica, 2000, 36(6):789-814 |
Click to display the text | |
[11] | Slotine J J E, Li W. Applied Nonlinear Control[M]. Prentice-Hall, 1991:350-353 |
[12] | Gao J, Liu C, Wang Y. Backstepping Adaptive Docking Control for a Full-Actuated Autonomous Underwater Vehicle with Onboard USBL System[C]//OCEANS'14 MTS/IEEE, 2014:1-6 |
Click to display the text |