一种USV的航迹闭环控制方法与流程

本发明涉及USV控制技术领域，具体地说是一种USV的航迹闭环控制方法。

背景技术：

当今世界的海洋和军事应用中，无人水面艇的发展越来越受到重视。所谓无人水面艇，系指那些有携载平台拖放，靠遥控或自主方式在水面航行的小型无人化、智能化平台。

无人水面艇较之传统水面舰艇具有一些突出的优势：

1.功能齐全，多种使命兼容。

2.小型轻量，反应快速，机动灵活。

3.替代人员完成一些危险的任务。

4.活动区域广，使用成本低，无人水面艇吃水浅，对航道，港口等处水深要求低，扩张其活动范围；

5.受气候影响较小，可全天候执勤。面对难以预知、复杂的海洋环境，无人水面艇能够精确、快速的执行预定使命路径成为无人水面艇发挥效能的最基本保证，是无人水面艇研制工作的基础。

但是目前无人水面艇自身存在吃水浅、重量轻的因素制约，导致控制过程存在航向不稳，航迹不精确等问题。

技术实现要素：

针对现有技术的不足，本发明提供一种USV的航迹闭环控制方法，将PID经典控制理论与在线参数辨识并更新技术相结合，适合USV在复杂海洋环境下的非线性时变模型系统，实现航迹闭环控制。

本发明为实现上述目的所采用的技术方案是：

一种USV的航迹闭环控制方法，包括以下步骤：

步骤1：设定目标航行路径，且判断位置传感器和姿态传感器是否处于安全工作状态，如果是，则执行步骤2，否则报警；

步骤2：USV根据位置传感器和姿态传感器发送的实时位置信息和姿态信息计算偏离预定航线的距离，如果该距离小于设定距离，则延时等待后重新执行步骤2，否则执行步骤3；

步骤3：根据目标航行路径、当前偏离预定航线的距离、位置和姿态信息，计算目标航向角ψ_r(k)；

步骤4：根据目标航向角ψ_r(k)、当前航向角ψ(k)和航向角速度Δψ(k)，计算目标喷嘴角ω_r(k)；

步骤5：根据目标喷嘴角ω_r(k)、当前喷嘴角ω(k)和喷嘴角速度Δω(k)，计算输出动作量，进而控制USV动作，返回步骤2。

所述偏离预定航线的距离的计算过程为：

ε²(k)＝(x(k)-x')²+(y(k)-y')²

其中，ε(k)为偏离预定航线的距离；x(k),y(k)为USV当前位置坐标；x’,y’为USV当前位置到目标航行路径的垂线段交点坐标；(x_i,y_i),(x_i+1,y_i+1)为当前目标航行路径的两个端点坐标。

所述目标航向角ψ_r(k)的计算过程为：

期望航向ψ_r是垂足点(x’,y’)处的切线方向，则有：

ψ_r(k)＝arctan((y_i+1-y_i)/(x_i+1-x_i))

其中，(x_i,y_i),(x_i+1,y_i+1)为当前目标航行路径的两个端点坐标。

所述目标喷嘴角ω_r(k)的计算过程为：

其中，ψ_r(k)为目标喷嘴角，ψ_r(k)为目标航向角，ψ(k)为当前航向角，Δψ(k)为航向角速度，K_P1为1.05，K_I1为0.011，K_D1为-4.1。

所述输出动作量的计算过程为：

其中，ACT为输出动作量，ω_r(k)为目标喷嘴角，ω(k)为当前喷嘴角，Δω(k)为喷嘴角速度，K_P2为2.1，K_I2为0.03，K_D2为-2.5。

本发明具有以下有益效果及优点：

1.本发明对无人水面艇的航迹闭环控制效果明显，收敛速度快。

2.本发明计算过程简单，鲁棒性、适应性强。

3.本发明移植方便，可适用到其它无人水面艇。

附图说明

图1是本发明的方法流程图；

图2是本发明的闭环控制示意图；

图3是本发明的运动学示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图1所示为本发明的方法流程图。

步骤1：操作者操作人机交互计算机将航行路径下载到控制计算机中的硬盘中；

步骤2：USV等待位置传感器(校准源)、姿态传感器(惯组)进入准备状态且无故障上报后，USV开始进行航行任务。

步骤3：USV实时计算偏离预定航线的距离，若小于设定值，则延时等待后重复步骤3。若大于设定值，进入步骤4。

步骤4：控制计算机由预定路径、当前偏航距离、位置、姿态等信息，计算目标航向角ψ_r(k)。

步骤5：由目标航向角ψ_r(k)、当前航向角ψ(k)、航向角速度Δψ(k)，计算目标喷嘴角ω_r(k)。

步骤6：由目标喷嘴角ω_r(k)、当前喷嘴角ω(k)、喷嘴角速度Δω(k)，得出执行动作后返回步骤3。

如图2所示为本发明的闭环控制示意图。

水面无人艇采用惯性组合导航设备作为姿态信息输入。采用GPS校准源作为位置信息输入。

无人艇航迹偏差计算方法：

设定当前航向ψ(k)，可以通过搭载在USV的设备采集得到，期望航向ψ_r(k)、期望航迹偏差ε_r(k)由航迹控制单元给出。实际的航迹偏差ε(k)通过计算获得，其实是USV当前位置(x,y)到规划航线的最近距离，即(x,y)到规划航线的垂线段长度：

ε²(k)＝(x(k)-x')²+(y(k)-y')²

在知道规划的航线的情况下，垂足点(x’,y’)的坐标可有以下求出

当知道水面无人艇的(x(k),y(k))，则上式是关于x的高阶非线性方程，根就是求得的x’，而。期望航向ψ_r是垂足点(x’,y’)处的切线方向，有如图3所示。用若干个直线段代替规划的航线，根据生成的路径序列执行情况，可知USV当前位置属于直线航迹段i上，然后利用该航迹段的两个端点坐标(x_i,y_i),(x_i+1,y_i+1)就能够得到垂足点(x’,y’)的坐标，从而计算得到航迹偏差ε(k)和期望航向ψ_r(k)如下

ψ_r(k)＝arctan((y_i+1-y_i)/(x_i+1-x_i))

输出动作量为：

式中ACT为最后输出动作量，ω_r(k)为目标喷嘴角，ω(k)为当前喷嘴角，Δω(k)为当前喷嘴角速度，K_P2为2.1，K_I2为0.03，K_D2为-2.5。

USV的控制参数可在线修改、调整。

本发明的硬件要求是一台USV，艉部配置柴油机，柴油机连接翻斗、喷泵，油门控制柴油机的转速。在USV设计完成后，首先进行水池水动力试验，获得USV的水动力系数。然后按照本发明方法设计USV的航迹闭环控制器。最后，将使用本发明方法的USV进行航行试验，验证控制方法的正确性。