CN108803612A - 一种海流影响下的无人巡检船直线航迹跟踪实现方法 - Google Patents

Abstract

本发明公开了一种海流影响下的无人巡检船直线航迹跟踪实现方法，具体涉及海水养殖监测技术领域。该海流影响下的无人巡检船直线航迹跟踪实现方法在考虑海流的影响下，针对海水养殖工厂化、生态化发展需求，将LOS导航算法与PID控制相结合，解决无人巡检船的直线航迹跟踪问题。LOS算法用于无人巡检船的航向控制，再通过PID控制算法完成航迹跟踪，从而使无人船驶向采集点完成水质数据采集，通过MATLAB对其进行仿真研究，仿真结果表明，该方法具有良好的控制效果。

Description

一种海流影响下的无人巡检船直线航迹跟踪实现方法

技术领域

本发明涉及海水养殖监测技术领域，具体涉及一种海流影响下的无人巡检船直线航迹跟踪实现方法。

背景技术

随着生活水平的逐步提高，海水养殖面临巨大的发展市场，行业将迎来新的发展契机。海水养殖生产模式紧随智能化技术的发展，正逐渐从传统的粗放型向现代的集约化过渡。海水养殖对水域环境要求较高，水质监测是工厂化、生态健康化养殖中的一个重要技术环节，逐渐的由人工采样、实验室分析的手段，向在线实时监测发展。近年来，我国无人船艇技术快速发展，采用无人船作为海水养殖监测搭载平台，装载多种监测传感器以及定位、导航与控制设备，以单点移动式测量代替多点分布式测量，可实现海水养殖场内的连续巡航监测。

为了科学有效监测海水养殖场环境参数，无人船要求不定时巡测养殖场内不固定分布点，其航迹的跟踪是一项重要研究内容。目前，国外对无人艇航迹跟踪的研究中，Grimblef采用最优控制技术和卡尔曼滤波器设计出了一种控制方法；Godhavn使用连续时不变状态反馈控制方法，设计出了全局指数镇定控制器；DO等基于重定义输出方法和反步法，提出了一种全局k指数稳定的船舶直线航迹的状态和输出反馈的控制律。国内对于船舶的自主航迹跟踪研究也较多，马壮教授在改进PID控制算法的过程中，设计出了模糊PID航迹控制律；周倩等人提出了一种解决船舶航迹跟踪控制的线性滑模控制方案；李铁山等利用自适应积分反演技术、耗散理论以及输入输出线性化等方法对船舶的直线航迹跟踪控制系统设计了非线性控制器。多数项目在实际仿真时，通常用一个恒值干扰来模拟海流对船舶运动的影响，造成仿真结果与实际存在较大的偏差。

发明内容

本发明的目的是针对上述不足，提出了一种将LOS导航算法与PID控制相结合，解决海流影响下的无人巡检船直线航迹跟踪问题的无人巡检船直线航迹跟踪实现方法。

本发明具体采用如下技术方案：

一种海流影响下的无人巡检船直线航迹跟踪实现方法，具体包括：

Ⅰ.建立海流环境模型：

建立二维笛卡尔坐标系(x，y)下离散栅格空间，△x、△y分别为x、y轴方向栅格的大小，栅格中任一点q定义为式(1)

q＝q(i，j)，0≦i<m,0≦j<n(1)

其中，m，n分别为x，y轴方向的最大栅格数；

将二维平面空间进行栅格化，单元栅格内的海水可视为海流状态相同，海流的速度场是一个向量场，Vc＝(u(x,y),v(x,y))，海流速度场定义为式(2)：

u(x,y)＝sin(pi/50*x)；

v(x,y)＝0.3*pi*sin(pi/20*y)sin(pi/50*x)； (2)

其中u(x,y)，v(x,y)分别为x轴方向和y轴方向的速度分量，(x,y)为无人巡检船所在位置，取做每个栅格的中心点；

Ⅱ.建立无人巡检船运动数学模型：

基于前进速度u、横移速度v和转首角速度r三项主要运动变量，建立如式(3)的平面内三自由度无人巡检船运动数学模型，

在航迹控制过程中，忽略漂角，即有u＞＞0，v≈0,前进的合速度船舶运动数学模型为式(4)：

Ⅲ.基于LOS导航算法，计算无人巡检船的直线航迹：

①求解LOS角

LOS角为LOS矢量与X轴的夹角，方向由X轴指向LOS矢量，LOS矢量是指从无人巡检船当前位置指向期望到达的位置的矢量，假设无人巡检船当前位置为P(x,y)，期望位置为P_k+1(x_k+1,y_k+1)，并假设P_k+1和P_k可以进行测量，则“LOS角”通过式(5)计算得到：

②求解LOS的位置

以P(x,y)为圆心，以nl为半径的圆与预设路径有两个交点，其中距离(x_k,y_k)较远的交点为P_los(x_los,y_los)，

(x_los-x)²+(y_los-y)²＝(nl)² (7)

其中，l为船身长度；

当无人巡检船进入该期望航向点一定范围时，则自动放弃跟踪该期望航向点，而转入跟踪下一个期望航向点，即令k＝k+1，根据当前船舶位置(x,y)是否在以当前路径点(x_k,y_k)为圆心、半径为r的圆内为依据，即是否满足表达式(7)，来确定(x_k+1,y_k+1)能否作为下一个路径点，具体如式(8)，

(x_k-x)²+(y_k-y)²≤r² (8)

其中，r＝2l，在对LOS位置进行计算时切换到下一个期望航向点的数据进行计算，

以实现航迹连续跟踪；

③LOS直线航迹进行跟踪分析

期望航向点P_los(x_los,y_los)与无人巡检船当前位置在期望航迹上的投影点相距Δ＝nl，n＝2～5，Δ为无人巡检船的可视距离，此时，LOS控制算法表示为式(9)：

其中，α为X轴方向与期望航迹线P_kP_k+1的夹角，即设定的期望航向角，d为无人巡检船的横向跟随误差；

Ⅳ.基于LOS算法设计PID控制器：

PID控制器的控制规律如式(10)：

由于舵角δ与当前航向角ψ成比例变化而航向角偏差为式(11)：

ψ_d＝ψ-ψ_los (11)

由式(10)、(11)得出，如果某一时刻，系统存在一个较大的ψ_d，经PID控制便会产生一个舵角，该舵角会控制无人巡检船偏向期望航向点航行，此时无人巡检船的当前航向角就趋近于无人巡检船期望航向角，ψ_d减小，通过不断控制，只要ψ_d存在，便会出现一个控制舵角使ψ_d减小，直到ψ_d趋于零，即无人巡检船达到期望航迹线；

Ⅴ.对无人巡检船进行无外部干扰和有海流影响两种情况进行仿真和分析。

本发明具有如下有益效果：

将LOS法应用于无人巡检船航迹跟踪时有良好的控制效果。无人巡检船作为一种无人控制的小型船舶，可以用来进行实验研究和完成很多不适合船员完成的任务，因此，具有极大的研究前景。本文设计的基于LOS导航律的算法得到了仿真验证，仿真结果表明此算法可以在洋流干扰下实现路径跟踪，并为以后的研究提供了重要参考依据。

附图说明

图1为海流速度场模型示意图；

图2为无人巡检船运动示意图；

图3为LOS角求解示意图；

图4为航迹跟踪导航方式示意图；

图5为PID控制系统示意图；

图6a为无海流干扰时的航向角历时曲线图；

图6b为无海流干扰时横偏位移历时曲线图；

图6c为无海流干扰时航迹历时曲线图；

图7a为有海流干扰时航向角历时曲线图；

图7b为有海流干扰时横偏位移历时曲线图；

图7c为有海流干扰时航迹历时曲线图。

具体实施方式

下面结合附图和具体实施例对本发明的具体实施方式做进一步说明：

Ⅰ.建立海流环境模型：

海水养殖场内的无人巡检船，其工作环境为海水表面，受潮汐、海风以及近岸不同地理位置海水密度变化的影响，船的水平和竖直方向的水面运动会收到直接干扰。为分析海流对无人船航行的影响，建立二维笛卡尔坐标系(x，y)下离散栅格空间，△x、△y分别为x、y轴方向栅格的大小，栅格中任一点q定义为式(1)

q＝q(i，j)，0≦i<m,0≦j<n (1)

其中，m，n分别为x，y轴方向的最大栅格数；

如图1，栅格为边长2km，将二维平面空间进行栅格化，单元栅格内的海水可视为海流状态相同，海流的速度场是一个向量场，Vc＝(u(x,y),v(x,y))，海流速度场定义为式(2)：

u(x,y)＝sin(pi/50*x)；

v(x,y)＝0.3*pi*sin(pi/20*y)sin(pi/50*x)； (2)

其中u(x,y)，v(x,y)分别为x轴方向和y轴方向的速度分量，(x,y)为无人巡检船所在位置，取做每个栅格的中心点，生成的海流如图1所示。

Ⅱ.建立无人巡检船运动数学模型：

由于在水平面运动的无人巡检船配有一个螺旋桨和一个舵机，通过纵向推进力和转首力矩实现三个自由度的运动，因此，主要研究基于前进速度u、横移速度v和转首角速度r三项主要运动变量，建立如式(3)的平面内三自由度无人巡检船运动数学模型，

在航迹控制过程中，由于控制系统闭环反馈的作用，使无人巡检船运动变化在平衡状态附近，此时无人巡检船漂角较小，作用于船上的流体力为线性的，忽略漂角，即有u＞＞0，v≈0前进的合速度船舶运动数学模型为式(4)：

分析过程中假定了航迹线与X轴重合，如图2所示。

Ⅲ.基于LOS导航算法，计算无人巡检船的直线航迹：

LOS((Line of Sight))导航算法是一种简单可靠的导航方法，在机器人、无人机、无人船等领域得到广泛应用。无人巡检船控制系统属于欠驱动系统，而LOS算法能将传统的控制量从三个自由度的船舶位置(x,y)和航向角减少到两个自由度的航向角和航行速度，把欠驱动系统转变为全驱动系统进行分析。

LOS算法体现的是对船舶行为和舵手动作之间的直观理解，即:如果让一艘船的航向保持对准“LOS角”，那么就能使该船到达所期望的位置^[6^]。在航向控制系统中加入位置反馈进行设计，把航迹控制问题看成一系列航向保持与航向改变问题，进而达到航迹跟踪的效果。

①求解LOS角

LOS角为LOS矢量与X轴的夹角，方向由X轴指向LOS矢量，LOS矢量是指从无人巡检船当前位置指向期望到达的位置的矢量，如图3所示，假设无人巡检船当前位置为P(x,y)，期望位置为P_k+1(x_k+1,y_k+1)，并假设P_k+1和P_k可以进行测量，则“LOS角”通过式(5)计算得到：

②求解LOS的位置

以P(x,y)为圆心，以n(L_pp)为半径的圆与预设路径有两个交点，其中距离(x_k,y_k)较远的交点为P_los(x_los,y_los)，

(x_los-x)²+(y_los-y)²＝(nL_pp)² (7)

其中，L_pp为船身长度；

当无人巡检船进入该期望航向点一定范围时，则自动放弃跟踪该期望航向点，而转入跟踪下一个期望航向点，即令k＝k+1，根据当前船舶位置(x,y)是否在以当前路径点(x_k,y_k)为圆心、半径为R₀的圆内为依据，即是否满足表达式(7)，来确定(x_k+1,y_k+1)能否作为下一个路径点，具体如式(8)，

(x_k-x)²+(y_k-y)²≤R₀ ² (8)

其中，R₀＝2L_pp，在对LOS位置进行计算时切换到下一个期望航向点的数据进行计算，以实现航迹连续跟踪；

③对LOS直线航迹进行跟踪分析

如图4所示，期望航向点P_los(x_los,y_los)与无人巡检船当前位置在期望航迹上的投影点相距Δ＝n(L_pp)，n＝2～5，Δ为无人巡检船的可视距离，此时，LOS控制算法表示为式(9)：

其中，α为X轴方向与期望航迹线P_kP_k+1的夹角，即设定的期望航向角，d为无人巡检船的横向跟随误差；

Ⅳ.基于LOS算法设计PID控制器，如图5所示：

PID控制器的控制规律如式(10)：

由于舵角δ与当前航向角ψ成比例变化而航向角偏差为式(11)：

ψ_d＝ψ-ψ_los (11)

由式(10)、(11)得出，如果某一时刻，系统存在一个较大的ψ_d，经PID控制便会产生一个舵角，该舵角会控制无人巡检船偏向期望航向点航行，此时无人巡检船的当前航向角就趋近于无人巡检船期望航向角，ψ_d减小，通过不断控制，只要ψ_d存在，便会出现一个控制舵角使ψ_d减小，直到ψ_d趋于零，即无人巡检船达到期望航迹线。

Ⅴ.对无人巡检船进行无外部干扰和有海流影响两种情况进行仿真和分析。

如图6a-7c所示，采用MATLAB对该实现方法进行仿真进行，MATLAB仿真实验中，无人巡检船进行无外部干扰和有海流影响两种情况的对比实验，以船长为3m，船宽为1.2m的无人巡检船为例进行直线航迹控制实验设计。初始航向偏角130°，初始横偏位移800m，分无海流干扰、有海流干扰两种环境下进行仿真，仿真结果如图6所示

船舶航向是可控制可操纵的，从图6可以看出，航迹偏差几乎为指数衰减，操舵角震荡较小。总体上看，在本实验过程中，实际航向角可收敛于期望航向角，仿真结果证明了基于LOS和PID控制技术进行船舶直线航迹控制的有效性。

当然，上述说明并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换，也应属于本发明的保护范围。

Claims (1)

1.一种海流影响下的无人巡检船直线航迹跟踪实现方法，其特征在于，具体包括：

Ⅰ.建立海流环境模型：

建立二维笛卡尔坐标系(x，y)下离散栅格空间，△x、△y分别为x、y轴方向栅格的大小，栅格中任一点q定义为式(1)

q＝q(i，j)，0≦i<m,0≦j<n (1)

其中，m，n分别为x，y轴方向的最大栅格数；

将二维平面空间进行栅格化，单元栅格内的海水可视为海流状态相同，海流的速度场是一个向量场，Vc＝(u(x,y),v(x,y))，海流速度场定义为式(2)：

u(x,y)＝sin(pi/50*x)；

v(x,y)＝0.3*pi*sin(pi/20*y)sin(pi/50*x)； (2)

其中u(x,y)，v(x,y)分别为x轴方向和y轴方向的速度分量，(x,y)为无人巡检船所在位置，取做每个栅格的中心点；

Ⅱ.建立无人巡检船运动数学模型：

基于前进速度u、横移速度v和转首角速度r三项主要运动变量，建立如式(3)的平面内三自由度无人巡检船运动数学模型，

在航迹控制过程中，忽略漂角，即有u＞＞0，v≈0,前进的合速度船舶运动数学模型为式(4)：

Ⅲ.基于LOS导航算法，计算无人巡检船的直线航迹：

①求解LOS角

LOS角为LOS矢量与X轴的夹角，方向由X轴指向LOS矢量，LOS矢量是指从无人巡检船当前位置指向期望到达的位置的矢量，假设无人巡检船当前位置为P(x,y)，期望位置为P_k+1(x_k+1,y_k+1)，并假设P_k+1和P_k可以进行测量，则“LOS角”通过式(5)计算得到：

②求解LOS的位置

以P(x,y)为圆心，以nl为半径的圆与预设路径有两个交点，其中距离(x_k,y_k)较远的交点为P_los(x_los,y_los)，

(x_los-x)²+(y_los-y)²＝(nl)² (7)

其中，l为船身长度；

当无人巡检船根据采集路线的规划，进入期望航向点一定范围时，则放弃当前跟踪该期望航向点，而转入跟踪下一个期望航向点，即令k＝k+1，根据当前船舶位置(x,y)是否在以当前路径点(x_k,y_k)为圆心、半径为r的圆内为依据，即是否满足表达式(7)，来确定(x_k+1,y_k+1)能否作为下一个路径点，具体如式(8)，

(x_k-x)²+(y_k-y)²≤r² (8)

其中，r＝2l，在对LOS位置进行计算时切换到下一个期望航向点的数据进行计算，以实现航迹连续跟踪；

③对LOS直线航迹进行跟踪分析

期望航向点P_los(x_los,y_los)与无人巡检船当前位置在期望航迹上的投影点相距Δ＝nl，n＝2～5，Δ为无人巡检船的可视距离，此时，LOS控制算法表示为式(9)：

其中，α为X轴方向与期望航迹线P_kP_k+1的夹角，即设定的期望航向角，d为无人巡检船的横向跟随误差；

Ⅳ.基于LOS算法设计PID控制器：

PID控制器的控制规律如式(10)：

由于舵角δ与当前航向角ψ成比例变化而航向角偏差为式(11)：

ψ_d＝ψ-ψ_los (11)

由式(10)、(11)得出，如果某一时刻，系统存在一个较大的ψ_d，经PID控制便会产生一个舵角，该舵角会控制无人巡检船偏向期望航向点航行，此时无人巡检船的当前航向角就趋近于无人巡检船期望航向角，ψ_d减小，通过不断控制，只要ψ_d存在，便会出现一个控制舵角使ψ_d减小，直到ψ_d趋于零，即无人巡检船达到期望航迹线；

Ⅴ.对无人巡检船进行无外部干扰和有海流影响两种情况进行仿真和分析。