水下自主航行器回坞控制和仿真的研究

水下自主航行器回坞控制和仿真的研究
方凤玲;卢正华;史剑光;金波;杨灿军
【摘要】Autonomous Underwater Vehicle (AUV)docking mission involves the navigation and positioning control of the vehicle,which aims to control vehicle attitude and complete the docking process. The work takes the effect of ocean currents on the docking process into consideration.The docking process is divided into three phases:remote tracking navigation,close centerline navigation,and terminal attitude adjustment.Corresponding to different docking phases,three different guidance methods are adopted to achieve path planning, which are line-of-sight, cross-track-error, and designing-reference-heading methods.PID controller is used for the back-to-dock motion control.The dynamic model of AUV is set up mathematically and the docking process is simulated with Matlab/Simulink.The results validate the performance of the navigation and positioning control method discussed in this paper.%水下自主航行器的回坞定位导航控制的任务是使水下自主航行器在洋流的影响下以一定的航艏向和姿态驶向接驳系统，并完成入坞这一过程。

回坞过程分为3个阶段：远距离追踪式回航阶段、近距离中心线回航阶段、末端姿态调整阶段。

针对不同阶段分别采用不同的引导方式：视线法、横向误差法、设计参考航向法实现回坞过程的路径规划，用 Proportional Integral Derivative (PID)控制法对回坞过程进行运动控制，并且利用数学模型在 Matlab/simulink 中对回坞定位导航控制进行仿真。

仿真结果显示了文章所采用定位导航控制的有效性。

【期刊名称】《广东第二师范学院学报》
【年(卷),期】2015(000)003
【总页数】8页(P75-82)
【关键词】水下自主航行器;回坞;PID;视线法;横向误差法;设计参考航向法
【作者】方凤玲;卢正华;史剑光;金波;杨灿军
【作者单位】福建信息职业技术学院机电工程系，福建福州 350003;浙江大学流体动力与机电系统国家重点实验室，浙江杭州 310027;浙江大学流体动力与机电系统国家重点实验室，浙江杭州 310027;浙江大学流体动力与机电系统国家重点实验室，浙江杭州 310027;浙江大学流体动力与机电系统国家重点实验室，浙江杭州 310027
【正文语种】中文
【中图分类】TP242
0 引言
图1 海底观测网络示意图
海洋立体观测对于海洋资源勘探、海洋环境监测具有非常重要的意义．水下自主航行器（AUV）作为一种新型的水下探测工具，技术日趋成熟．由于受到电磁容量的限制，AUV作业运行时间短．AUV接驳技术的出现，不仅使AUV实现了在水下持续工作，而且还可以让AUV停靠在海底回收平台（dock）上，等到发现异常情况才通过海底观测网络控制其实现水下巡航．毫无疑问，AUV与海洋立体观测一体化的技术将在未来的海洋监测中发挥越来越重要的作用．海底观测网络［1］
如图 1 所示．本文研究AUV在洋流影响下以一定的航艏向和姿态驶向海底回收平台（dock），并完成入坞的整个回坞定位导航控制问题．
图2 REMUS水下回收平台原理图
本文研究的控制仿真针对美国伍兹霍尔海洋研究所（WHOI）研制的 REMUS－STD型 AUV 进行［2］，而dock则采用圆锥导筒式结构［3］，如图2所示．在回坞过程中，以超短基线（USBL）水声定位设备［4］作为主要的导航设备，用视线法、横向误差法和设计参考航向法相结合进行回坞过程的路径规划，同时设计了Proportional Integral Derivative（PID）控制器对AUV的航向和深度进行控制［5］．为了验证回坞导航控制的效果，建立了六自由度AUV的数学模型，并在Matlab／Simulink中进行仿真．
1 AUV的数学模型［6］
利用Fossen［7］思想建立适用于在Matlab／Simulink环境下仿真的六自由度运动方程．
图3 水下自主航行器坐标系与运动参数定义
1．1 坐标系与AUV运动参数的选择
六自由度AUV的常用坐标系［8］为惯性坐标系、载体坐标系，如图3所示．惯性坐标系OEX EY EZ E的原点选在dock上USBL的安装位置，X E在水平面内沿锥形罩中心线指向套筒方向为正，ZE垂直向下方向为正，Y E垂直于X E和Z E，其坐标系满足右手坐标系定义．而体坐标系则与AUV相连，其原点OB定义在AUV浮心，X B沿AUV纵轴，并以指向AUV前进方向为正方向，ZB垂直于X B 指向AUV下方，Y B垂直于X B和Z B，使坐标系满足右手坐标系定义．
AUV运动参数定义如表1所示．
表1 水下自主航行器运动参数定义表自由度名称体坐标系重力／重力矩体坐标系线速度／角速度惯性坐标系位置／姿态角1 进退（Surge）X u x 2 横移
（Sway） Y v y 3 升降（Heave） Z w z 4 横滚（Roll）K p φ 5 俯仰（Pitch）M q θ 6 转艏（Yaw）N r ψ
1．2 基本假设
简化AUV整体数学模型以方便研究，作出假设：AUV为刚体，其外形关于纵平
面OXZ对称；将AUV的浮力中心定义为体坐标系原点，从而假设惯性积近似为零；将AUV在水下的浮力和重力设置为相等，从而忽略水静力对动力学方程的影响．并假设重心坐标分量x G＝y G＝0，而z G≠0．
1．3 AUV的运动学方程
AUV数学模型的运动学方程主要描述AUV在惯性坐标系下的空间位置（x e，y e，z e）和姿态（φ、θ、ψ）与体坐标系下的速度（u、v、w）和角速度（p、q、r）间的关系．将AUV体坐标系中的运动参数转换到惯性坐标系中描述就可以得到AUV的运动学方程：
1．4 水下自主航行器的动力学方程
根据牛顿动量定理和动量矩定理，AUV的动力学方程组可表示为：
式中Q、K分别为AUV的动量、动量矩；［X Y Z］T为作用在AUV上的全部外力，而［K M N ］T为作用在AUV上的全部外力矩．把AUV刚体动力学的动量、动量矩及其所受到的水静力、水静力矩、水动力、推进力、控制力及控制力矩代入上式，并整理、简化得到如下的六自由度AUV动力学方程：
1．5 考虑洋流干扰的数学模型
AUV回坞过程难免受到环境干扰，而水面波浪引起的干扰一般可以忽略，可以只
考虑洋流的作用．采用相对运动的概念可以在方程（1）、（2）中将洋流的作用
加以考虑．即：
式中ubc，vbc，w bc 表示体坐标系中的洋流速度．用u－ubc，v－vbc，w－w bc 分别代替方程（1）、（2）中的u、v、w，就得到了考虑洋流影响的六自由度AUV的数学模型．
2 AUV回坞控制系统设计
从式（3）可知AUV的动力学方程具有严重非线性、强耦合性等特点，因此AUV 的运动控制器不仅要求控制准确性高而且还要有很强的鲁棒性．本文因此采用PID 控制．
2．1 AUV的总体控制结构
AUV的总体控制结构如图4所示．
2．2 航向控制器设计
航向控制用来控制AUV按照规划航向或朝着预定目标点前进．文中的航向控制器采用PID控制，其结构如图5所示．它接收水下回坞制导控制器的航向指令ψd，通过输出垂直舵偏转角度指令δr，d来控制航行器的实际航向ψ．
因为没有对动力学方程（3）进行线性化解耦处理，所以先将控制俯仰自由度的水平舵偏转角度设置为零，然后利用临界比例度法进行参数整定，并将得到的参数作为初值在Simulink的NCD优化工具箱中进行优化处理，得到最终的控制参数为K p＝4．28；K i＝0．008；K d＝4．55．
图4 AUV总体控制结构图
图5 航向控制器结构示意图
图6 深度控制器结构示意图
2．3 深度控制器设计
深度控制器用来实现AUV的定深航行．本文的深度控制器仍然采用PID控制，它
实质是由两个控制器组成内外环串联的结构，如图6所示．其中内环为俯仰角控
制器；而外环为深度控制器．整个控制器通过接收水下回坞制导控制器的深度指令，输出水平舵偏转角度指令δs，d来控制航行器的实际深度．因此其实质是通过对
俯仰角度的直接控制来实现对深度的间接控制．此外考虑REMUS的运动性能及
其水下运行的实际状况，通过在外环深度控制器后添加一个饱和环节来限制深度控制器输出的俯仰角指令，即将数学模型中俯仰角的定义：
深度控制器同样采用临界比例度法进行参数整定，将得到的参数作为初值在Simulink的NCD优化工具箱中进行优化处理，得到最终的控制参数为K p＝1．4；K i＝0．001；K d＝2．4．
3 AUV回坞导引定位系统
水下自主航行器的回坞导引定位系统［9］的结构示意图如图7所示．其中的回坞引导控制器通过USBL和电子罗盘分别确定AUV相对于dock的位置和姿态，采
用合适的制导方式，合理的规划AUV的航行轨迹和姿态，从而控制AUV成功回坞．
根据AUV与dock的距离将回坞引导过程分为远距离追踪式回航（200～1 000 m）、近距离中心线回航（10～200 m）和回坞末端引导（10 m以内）三个阶段，并在三个阶段采用不同的引导方式．完整的回坞流程如图8所示．
图7 航行器回坞引导与运动控制结构示意图
图8 航行器回坞引导流程图
图9 基于视线法的纯跟踪制导示意图
3．1 远距离纯跟踪引导
远距离采用基于视线法的纯跟踪引导［1 0］．将dock的锥形导向罩开口区域内
的某一点设定为预备目标点，即“追逐目标点”，通过控制AUV的航艏向角度与
视线角度成比例，使AUV逐渐靠近该点，具体的引导过程如图9所示．
在图9中，ψd为视线角度，ψ为AUV的航艏向角度，视线法引导的过程就是将视线角度作为航向控制器的输入指令，并控制航向角度误差e＝ψd－ψ．当ψ＝ψd，且AUV以一定的前向速度航行时，就实现了AUV头部朝向预备目标点．其中视线角度可以通过下式计算获得：
式中（x b，yb）是AUV浮心在惯性坐标系的坐标，而（200，0）则是预备目标点的坐标．
3．2 近距离横向误差控制引导
AUV要成功进入dock的锥形导向罩，就必须切换到近距离模式进行引导，即对AUV的前进方向和横向误差［11］进行控制，使AUV的航行轨迹尽量沿着锥形导向罩的中心线，并使其前进方向指向锥形导向罩．如图10所示，AUV相对于dock的锥形罩中心线的横向误差也就是AUV的绝对位置坐标在OEX E坐标轴上的分量y E．
在无洋流干扰的理想情况下，AUV到达预备目标点（－200，0），并完成横向误差控制之后，只要将AUV的航艏向稳定控制在ψ＝0°，便可以保证航行器以最理想的姿态进入回收平台的锥形导向罩区域内．但是当洋流尤其是侧翼洋流存在时，侧翼洋流就会使上述的定航艏向前进偏离dock的锥形罩的中心线，甚至离开可回收区域．此时就必须控制AUV航行轨迹的横向误差值．即通过AUV与接驳站的相对位置产生一定的偏航角度ψcrab来控制侧翼洋流的影响．引导过程如图11所示．该控制器以横向误差作为反馈量，通过输出偏航角度指令，实现对横向误差的控制．此外，为了保证AUV始终指向dock锥形导向罩区域，需在控制器的输出后加一个饱和环节，将输出偏航角度限制在
图10 横向误差定义示意图
图11 近距离横向误差控制引导示意图
3．3 末端姿态调整
在末端姿态调整阶段，通过对AUV航向的调整来实现对整体末端姿态调整，即通过设计一条参考航向曲线，控制AUV航向沿着该参考航向逐渐减小偏航角度，最终期望航行器以航艏向与锥形罩中心线平行的姿态进入锥形导向罩区域．
本文选取一条余弦曲线作为参考航向曲线，该曲线如下式所示：
式中ψref是参考航向，ψcrab是AUV偏航角度，d f是此阶段AUV相对于锥形罩的起始距离值，d是AUV与锥形罩之间的距离值，两距离值均为正．可按照下式对d f取值：
式中｜d f，min｜是d f可取的最短距离值，而r max是AUV最大航向角速率．此外k是取正值的比例系数，也是该参考航向曲线的控制系数，当d f的取值确定后，参考航向曲线也就按照式（6）确定下来．
4 AUV回坞导航控制的仿真
为了验证AUV回坞过程导航控制系统的效果，采用Matlab软件对回坞导航进行仿真．
4．1 在Matlab／simulink中建立仿真模型
将式（3）中每个方程的微分项移到方程左边，而非微分项则移到方程右边，就可以得到动力学的微分方程．由此方程及文献［8］的REMUS－STD的模型参数，在simulink中用积分器建立REMUS－STD的仿真模型，如图12所示．仿真模型左下角部分是考虑洋流影响时需要加入的模块．
4．2 航行器回坞引导运动控制的仿真
在完成REMUS－STD仿真模型的各运动控制器的设计的基础上，通过一系列运动
仿真，如水平面极限转弯运动仿真、纵倾面内俯仰运动仿真、最大俯仰角度下潜运动仿真等，得到了REMUS－STD仿真模型的部分运动参数特性．
图12 Simulink中六自由度水下航行器仿真模型
在上述基础上，利用仿真模型分别进行一定初始条件下的远距离制导运动仿真、不同海况下的较大横向误差的制导与运动控制仿真和不同初始条件下的末端姿态调整仿真，以此来验证基于视线法的纯跟踪制导下的回收方向控制、基于PID控制器的横向误差控制及末端姿态调整的控制的有效性．仿真结果分别如图13～图15所示．其中图13中AUV回收起点位置为（－600，200），起始航向为ψ＝0°，预备目标点为（－200，0）；图14中起点位置为（－200，6），目标点为（0，0），起始航向姿态ψ＝0°；图15中AUV速度ud ＝1．54 m／s，起点位置为（－200，2），r＝0．2°，系数k＝1．1，横向误差余量y＝0．35 m洋流流速Vc，x ＝0 m／s，Vc，y ＝0．25 m／s，AUV起始姿态ψ0＝0°．
图13 纯跟踪引导仿真之航行轨迹图
图14 横向误差控制引导仿真轨迹图
图15 末端姿态调整仿真结果
5 结束语
本文主要研究引导与控制六自由度AUV在一定距离范围内以一定姿态准确进入圆锥导筒式dock，自主完成整个回坞过程．同时以REMUS－STD为仿真模型进行水下自主回坞引导运动控制仿真，从而验证引导与运动控制器的有效性．由于本文所涉及的AUV还在研制中，无法进行实物实验，只能简单通过仿真形式验证这种水下自主回坞组合引导方式即运动控制的有效性．
参考文献：
［1］陈鹰，杨灿军，陶春辉．海底观测系统［M］．北京：海洋出版社，2006：112．
［2］ALLEN B，STOKEY R，AUSTIN T，et al．REMUS：a small，low cost AUV system description，field trials and performance results［J］．Oceans，1997，2：994－1000．
［3］卢正华．基于超短基线的水下自主航行器回收导航策略的研究与仿真［D］．杭州：浙江大学，2013：6．
［4］STOKEY R，ALLEN B，AUSTIN T，et al．Enabling technologies for REMUS docking ：an integral component of an autonomous ocean sampling network［J］．IEEE Journal of Oceanic Engineering，2001，26（4）：487－497．
［5］高剑，严卫生，徐德民，等．自主水下航行器的回坞导引和入坞控制算法［J］．Computrer Engineering and Application，2012，48（3）：7－9．［6］ROBERTS G N ，SUTTON R．无人水下航行器进展［M］．北京：电子工
业出版社，2009：16－30．
［7］FOSSEN T I，FJELLSTAD O E．Position and attitude tracking of AUV：a quaternion feedback approach［J］．IEEE Journal of Oceanic Engineering，1994，19（4）：512－518．
［8］PRESTERO T．Verification of a Six－Degree of freedom simulation model for the REMUS autonomous underwater vehicle［D］．Bosten：Massachusetts Institute of Technology，2001：20－23．
［9］NAEEN W I，SUTTON R，AHMAD S M，et al．A review of guidance laws applicable to unmanned underwater vehicles［J］．The Journal of Navigation，2003，56（1）：15－29．
［10］DO K D．Practical control of under－actuated ships［J］．Ocean Engineering，2010，37（13）：1111－1119．
［11］WOOLSEY C A，TECHY L．Cross－track control of a slender，under －actuated AUV using potential shaping［J］．Ocean Engineering，2009，36（1）：82－91．。