CN105629976A - 一种水产养殖作业船自动导航路径跟踪方法 - Google Patents

Abstract

本发明公开了一种水产养殖作业船自动导航路径跟踪方法。该方法包括以下步骤：第一步：采集反映作业区轮廓的角点的位置信息，经轨迹规划，得到作业船的目标航线和航速；第二步：采集作业船实时的位置、航向和航速等数据，设定作业船的航行速度并判断是否需要切换目标路径；第三步：采用实时插点的方法计算出作业船的跟踪目标点；第四步：将作业船的位姿和运动状态数据与跟踪目标点及目标路径进行对比分析，得到航向与航速偏差，通过PD、PI算法得出航向与航速控制量，最终得到左右明轮控制量；第五步：根据最终的控制量对作业船进行实时导航控制。本发明能显著提高自动导航路径跟踪的效果，为水草清理和均匀投饵作业的高效完成提供了保障。

Description

一种水产养殖作业船自动导航路径跟踪方法

技术领域

本发明涉及自动导航路径跟踪领域，特别是河蟹养殖用水产养殖作业船的自动导航路径跟踪。

背景技术

我国是河蟹养殖大国，而河蟹养殖过程中主要存在着水草清理和均匀投饵两大难题。水草是河蟹喜食的饵料，同时也是河蟹的栖身场所，水草可以通过光合作用增氧，还可以吸收水中的营养盐，改善水体质量，防止水体富营养化。然而，若不及时对水草进行清理，露出水面的水草会阻碍空气中的氧气进入水体，腐烂的水草会过多地消耗水中的溶氧，导致池塘底质、水质的恶化；同时，由于河蟹的活动范围有限，只能在自身附近区域觅食，饵料的投放不均会导致局部饵料过多或过少，饵料过多时会导致浪费、增加养殖的成本，饵料过少时又容易引起河蟹因抢食而相互残杀。因此，用于水草清理或均匀投饵作业的水产养殖自动作业船具有重要的实用价值。水产养殖作业船导航控制系统中，船体的路径跟踪控制方法是其中的关键环节，能否控制船体严格按照设定路径航行会直接地影响到水草清理的效率和饵料投放的均匀度，一种简单、高效的路径跟踪方法可以增强导航控制系统的实时性，提高水产养殖作业船的工作效率。

目前已有一些自动导航路径跟踪方法，但都存在着一些不足，如申请号为200910082462.1的专利“农业机械的导航方法、导航装置和农业机械”根据农业机械的两轮运动学模型以及最优控制理论中的最小时间问题计算获得农业机械的预期路径信息，其计算量大、实时性不高；申请号为200710029018.4的专利“一种农业机械的导航控制方法”需要通过测量多个等间距的目标点来确定行进路线并通过查表的方法进行导航跟踪，实际操作过程复杂，灵活性差，很难达到较高的精度。

发明内容

本发明的目的是为了解决现有的农业机械导航控制方法复杂、路径跟踪效果不理想的问题，使作业船能精确按照设定路径航行，提高水草清理的效率以及投饵的均匀度。本发明所涉及的水产养殖适用于各种鱼、虾、蟹等水产品。

本发明为解决上述问题，采用的技术方案包含以下步骤：

第一步：使用基于RTK模式的高精度GPS系统采集能反映目标作业区轮廓的各个角点的经纬度坐标，经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径；

第二步：通过基于RTK模式的高精度GPS系统以及惯导模块采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，设定作业船的航行速度并判断是否需要进行目标路径切换；

第三步：采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度等因素确定；

第四步：将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船的左右明轮控制量；

第五步：根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。

进一步，所述第一步中经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径，具体步骤如下：

步骤1.1，计算作业船与能反映目标作业区轮廓的各个角点{J₀，J₁，...，J_n}的距离{L₀，L₁，...，L_n}，并将距离最近的点作为目标航线的起点T₀；

步骤1.2，以各个角点的经纬度坐标为基准，按照路径最短原则推算出作业船的目标航线：

T₀→T₁→...→T_k-1→T_K→T_K+1→...→T_N

对应的目标点集合为：

{T₀，T₁，...，T_k-1，T_K，T_K+1，...，T_N}

其经纬度坐标为：

{(x_T0，y_T0)，(x_T1，y_T1)，...，(x_Tk-1，y_Tk-1)，(x_Tk，y_Tk，(x_Tk+1，y_Tk+1)，...，(x_Tn，y_Tn)}

其中x坐标表示经度，y坐标表示纬度，T_N为作业船目标航线的终点；

步骤1.3，结合目标航线设定作业船的目标巡航速度V_L和目标转弯速度V_S。

步骤1.4，确定路径T₀→T₁为作业船起始的目标路径。

进一步，所述第二步中设定作业船的航行速度并判断是否需要进行目标路径切换，具体步骤如下：

步骤2.1，根据作业船当前位置Q及目标路径终点T_k+1的经纬度坐标计算出作业船与目标路径终点T_k+1的距离D₁；

步骤2.2，若步骤2.1中计算得到的距离D₁大于减速半径R₁，则认为作业船还未逼近目标路径终点，设定作业船的航行速度V₁为较大的巡航速度V_L；若D₁小于或等于减速半径R₁，则认为作业船已逼近目标路径终点，设定作业船的航行速度V₁为较小的转弯速度V_S；

步骤2.3，若步骤2.1中计算得到的距离D₁大于目标半径R₂，则认为作业船还未到达目标路径终点，不需要进行目标路径的切换；若D₁小于或等于目标半径R₂，则认为作业船已到达目标路径终点，需要将当前的目标路径由T_k→T_k+1切换成T_k+1→T_k+2；

其中，R₁、R₂的值由作业船的船体尺寸等因素确定。

进一步，所述第三步的具体过程如下：

步骤3.1，计算作业船的当前位置Q(x₁，y₁)在当前目标路径T_K(x_TK，y_TK)→T_K+1(x_TK+1，y_TK+1)上的投影点M(x₂，y₂)，计算的公式为：

$x_2=\frac{(y_1-y_{TK})\×(x_1\×(y_{TK+1}-y_{TK})+x_{TK}\×(y_1-y_{TK+1}\left)\right)+y_{TK+1}\×{(x_1-x_{TK})}^2}{{(x_1-x_{TK})}^2+{(y_1-y_{TK})}^2}$

$y_2=\frac{(x_1-x_{TK})\×(y_{TK}\×(x_1-x_{TK+1})-y_1\×(x_{TK}-x_{TK+1}\left)\right)+y_{TK+1}\×{(y_1-y_{TK})}^2}{{(x_1-x_{TK})}^2+{(y_1-y_{TK})}^2}$

其中，横坐标x表示经度，纵坐标y表示纬度。

步骤3.2，由两点经纬度坐标计算两点距离的公式得到投影点M点到目标路径终点T_k+1点的距离d，计算的公式为：

$d=2\×6378137\×\arcsin \left(\sqrt{{\sin }^2\left(\frac{y_{TK+1}-y_2}{2}\right)+cos\left(y_{TK+1}\right)\×cos\left(y_2\right)\×{\sin }^2\left(\frac{x_{TK+1}-x_2}{2}\right)}\right)$

其中，6378137为地球半径，单位为m。

步骤3.3，以投影点M点作为起始点沿着T_k→T_k+1方向前伸距离D₂至P(x₃，y₃)点，P(x₃，y₃)点坐标的计算公式为：

$x_3=x_2\±\frac{L_1\×(x_2-x_{TK})}{d}$

$y_3=y_2\±\frac{D_2\×(y_2-y_{TK})}{d}$

式中D₂的值由作业船的船体尺寸、航行速度等因素确定。

进一步，所述第四步中，将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，计算得到作业船的航向偏差与速度偏差，具体步骤如下：

步骤4.1，航向偏差ΔΨ的确定：作业船的航向偏差ΔΨ为作业船的目标航向Ψ₁与当前航向Ψ₂的差值，计算的公式为：

ΔΨ＝Ψ₁-Ψ₂

其中，当前航向Ψ₂由惯导模块采集得到，目标航向Ψ₁根据作业船当前的经纬度坐标Q(x，y)与跟踪目标点的经纬度坐标P(x，y)计算得到；

步骤4.2，航速偏差ΔV的确定：作业船的航速偏差ΔV为作业船目标速度V₁与当前速度V₂的差值，计算的公式为：

ΔV(k)＝V₁-V₂

目标速度V₁由第二步得到，当前速度V₂由惯导模块采集得到。

进一步，所述第四步中，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船最终的控制量即左右明轮速度，具体步骤如下：

步骤4.a，通过PD控制算法推算出航向控制量Ψ^*：第i时刻的航向控制量Ψ^*(i)由以下公式推算得出：

Ψ^*(i)＝K_p×ΔΨ(i)+K_d×(ΔΨ(i)-ΔΨ(i-1))

其中，ΔΨ(i)与ΔΨ(i-1)分别为第i、i-1时刻的航向偏差，K_p为比例参数，K_d为微分参数，K_p和K_d参数均根据实验效果现场整定；

步骤4.b，通过PI控制算法推算出航速控制量V^*：第i时刻的航速控制量V^*(i)由以下公式推算得出：

$V^{*}\left(i\right)=K_p\×\mathrm{\Δ}V\left(i\right)+K_i\×\underset{j=1}{\overset{i}{\Σ}}\mathrm{\Δ}V\left(j\right)$

其中，ΔV(i)为第i的航速偏差，K_p为比例参数，K_i为积分参数，K_p和K_i参数均根据实验效果现场整定；

步骤4.c，融合航向控制量Ψ^*与航速控制量V^*得到作业船左右明轮控制量：本发明以速度控制量V^*为基准，左右明轮控制量分别加上和减去来进行解耦，得到左右明轮的控制量分别为：

左明轮为：

$U_1=V^{*}+\frac{1}{2}{\mathrm{\Ψ}}^{*}$

右明轮为：

$U_2=V^{*}-\frac{1}{2}{\mathrm{\Ψ}}^{*}.$

本发明采用上述技术后，具有以下技术效果：本发明首先通过基于RTK模式的高精度GPS系统采集能反映作业区轮廓的各个角点的经纬度坐标，经轨迹规划得到作业船的目标航线和航速，将不规则的作业区域规划为多个首尾相连的直线路径，降低了作业船目标航线获取的难度；然后通过基于RTK模式的高精度GPS系统以及惯导模块实时地采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，根据作业船与当前目标路径终点的距离设置作业船的航行速度并判断是否需要进行目标路径切换，可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率；接着采用实时插点的方式计算得到作业船的跟踪目标点，可提高作业船对于航向偏差的敏感度，使作业船的跟踪轨迹更加地逼近目标路径；接着将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差，分别通过PD、PI控制算法推算出作业船的航向控制量及航速控制量，并将两者融合得到作业船的左右明轮控制量，其中，由于航速控制的输出量直接作用于明轮转速而不是船速，航速控制采用PI控制，通过积分环节以消除航速的稳态误差，航向控制采用PD控制，通过微分环节以增大阻尼系数、保持船舶的稳定；最后，根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。与现有方法相比，本发明可以实时地反映出航向的误差，实现方法简单、有效，通过实时插点的方式在线计算出作业船的跟踪目标点，将有限个角点细化成无数个跟踪目标点，能显著提高自动导航路径跟踪的效果，为水草清理和均匀投饵作业的高效完成提供保障。

附图说明

图1为水产养殖作业船结构图；

图2为作业船自动导航控制系统的原理结构框图；

图3为作业区轮廓示意图；

图4为使用多个角点描述作业区轮廓示意图；

图5为计算作业船与各个角点距离示意图；

图6为设定作业船目标航线示意图；

图7为作业船设定航行速度及目标路径切换示意图；

图8为基于实时插点方法计算作业船跟踪目标点示意图；

图9为作业船自动导航路径跟踪轨迹图；

图10为作业船自动导航路径跟踪误差曲线图。

具体实施方式

下面将结合实施例对本发明做进一步的详细描述，但本发明的实施方式不限于此。

本实施例采用的水产养殖作业船的结构如图1所示，船上包括船体1，船体1前端的水草切割装置2和水草收集装置3，船体1中部的集草仓4、水草铺平装置5，船体1中部两侧的明轮6，以及船体1后端的操作台7和投饵机8。船体1上装载了Trimble公司的高精度GPS系统、SBGIG-500AMEMS高精度惯导模块、ARM9的主控制板和明轮电机控制器等设备，构成作业船自动导航控制系统，其中，GPS系统与惯导模块采用串口连接，GPS系统、惯导模块均通过串口与ARM9主控制器连接。其原理结构框图如图2所示。

高精度GPS系统由基站和移动站两个部分组成RTK测量模式，基站和移动站通过无线电台进行数据传输，其定位精度可达厘米级；ARM9主控制板完成数据采集、数据处理、路径跟踪算法及控制指令输出等功能，其程序是采用ADS1.2开发平台在Windows7环境下编写完成的。

具体实施步骤如下：

第1步.设定作业船的目标航线和航速，并确定起始目标路径

本发明使用基于RTK模式的高精度GPS系统采集能反映目标作业区轮廓的各个角点的经纬度坐标，经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径。由于水产养殖作业船的目标作业区域多为不规则的形状，本发明首先使用多个能反映目标作业区域轮廓的角点{J₀，J₁，...，J_n}来描述目标作业区域；然后根据已知两点的经纬度计算两点间距离的公式：

$L=2\×6378137\×\arcsin \left(\sqrt{{\sin }^2\left(\frac{{\mathrm{Lat}}_1-{\mathrm{Lat}}_2}{2}\right)+cos\left({\mathrm{Lat}}_1\right)\×cos\left({\mathrm{Lat}}_2\right)\×{\sin }^2\left(\frac{{\mathrm{Lng}}_1-{\mathrm{Lng}}_2}{2}\right)}\right)$

分别计算出作业船的当前位置Q(x，y)同各个角点{J₀，J₁，...，J_n}的距离{L₀，L₁，...，L_n}，并将距离最近的点作为行进路线的起点T₀。其中，Lat表示经度，Lng表示纬度，L的单位为m。

接着，以各个角点的经纬度坐标为基准，按照路径最短原则推算出作业船的行进路线：

T₀→T₁→...→T_k-1→T_K→T_K+1→...→T_N

对应的目标点集合为：

{T₀，T₁，...，T_k-1，T_K，T_K+1，...，T_N}

其经纬度坐标为：

{(x_T0，y_T0)，(x_T1，y_T1)，...，(x_Tk-1，y_Tk-1)，(x_Tk，y_Tk)，(x_Tk+1，y_Tk+1)，...，(x_Tn，y_Tn)}

其中x坐标表示经度，y坐标表示纬度，T_N为作业船目标航线的终点。

接着，结合目标航线设定作业船的目标巡航速度V_L和目标转弯速度V_S，其中，V_L值过大时会导致水草清理不彻底以及投饵量不足等问题，其值过小时又会降低作业船的作业效率。V_S值过大时会增加作业船转弯的难度，过小时又会增加转弯时间，降低作业船的工作效率，设置合适的巡航速度V_L和转弯速度V_S可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率。

最终，确定路径T₀→T₁为作业船起始的目标路径。

这种方式将不规则的作业区域规划为多个首尾相连的直线路径，与现有的技术相比，降低了作业船目标航线获取的难度。

第2步.设定作业船航行速度并判断是否需要进行目标路径切换

本发明通过基于RTK模式的高精度GPS系统以及惯导模块采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，设定作业船的航行速度并判断是否需要进行目标路径切换。本发明中作业船的目标航线由多条首尾相连的直线目标路径组成，目标路径的终点即为路线的转弯点。为了保证作业船自动导航的精度，需要实时地设定作业船的航行速度并更新作业船当前的目标路径，其示意图如图7所示。

Q(x，y)点为作业船的当前位置，T_k→T_k+1为当前的目标路径，T_k+1→T_k+2为下一条目标路径。由于作业船在水中航行时具有很大的惯性，为防止作业船冲出设定的目标航线，必须降低船速并提前进行目标路径的切换。主控制器在作业船逼近目标路径终点的过程中不断地计算作业船当前位置与目标路径终点的距离D₁，当D₁大于减速半径R₁时，则认为作业船还未逼近目标路径终点，设定作业船的航行速度V₁为较大的巡航速度V_L；若D₁小于或等于减速半径R₁，则认为作业船已逼近目标路径终点，设定作业船的航行速度V₁为较小的转弯速度V_S；若D₁大于目标半径R₂，则认为作业船还未到达目标路径终点，不需要进行目标路径的切换；若D₁小于或等于目标半径R₂，则认为作业船已到达目标路径终点，需要将当前的目标路径由T_k→T_k+1切换成T_k+1→T_k+2。

其中，R₁、R₂的值由作业船的船体尺寸等因素确定。R₁值过大时会导致作业船过早减速，降低作业船的工作效率；R₁值过小时又会导致作业船减速太晚，使得作业船冲出目标路径。R₂值过大或过小时会导致作业船过早或过晚地切换目标路径，增大作业船的转弯半径。设置合适的R₁、R₂可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率。

第3步.采用实时插点的方式得到作业船的跟踪目标点

本发明采用实时插点的方法实时地计算出作业船的跟踪目标点，其计算示意图如图8所示。

Q(x₁，y₁)为船体的当前位置坐标，T_K(x_TK，y_TK)、T_K+1(x_TK+1，y_TK+1)为当前目标路径的起止点。其中，横坐标x表示经度，纵坐标y表示纬度，目标路径方向为T_k→T_k+1。若此时作业船直接将T_k+1点作为跟踪目标点，其行走路线Q→T_k+1将会较大地偏离目标路径T_k→T_k+1。作Q点到路径T_k→T_k+1的垂线相交于M(x₂，y₂)点，根据点到直线的垂直交点计算公式可得M点的坐标为：

$x_2=\frac{(y_1-y_{TK})\×(x_1\×(y_{TK+1}-y_{TK})+x_{TK}\×(y_1-y_{TK+1}\left)\right)+y_{TK+1}\×{(x_1-x_{TK})}^2}{{(x_1-x_{TK})}^2+{(y_1-y_{TK})}^2}$

$y_2=\frac{(x_1-x_{TK})\×(y_{TK}\×(x_1-x_{TK+1})-y_1\×(x_{TK}-x_{TK+1}\left)\right)+y_{TK+1}\×{(y_1-y_{TK})}^2}{{(x_1-x_{TK})}^2+{(y_1-y_{TK})}^2}$

其中QM为作业船偏离目标路径的距离，由经纬度坐标计算两点距离的公式可得M点到点T_k+1的距离d为：

$d=2\×6378137\×\arcsin \left(\sqrt{{\sin }^2\left(\frac{y_{TK+1}-y_2}{2}\right)+cos\left(y_{TK+1}\right)\×cos\left(y_2\right)\×{\sin }^2\left(\frac{x_{TK+1}-x_2}{2}\right)}\right)$

式中，6378137m为地球半径，d的单位为m。

以M点作为起始点沿着T_k→T_k+1方向前伸距离D₂至P点，可求得P(x₃，y₃)点的坐标为：

$x_3=x_2\±\frac{L_1\×(x_2-x_{TK})}{d}$

$y_3=y_2\±\frac{D_2\×(y_2-y_{TK})}{d}$

其中，D₂的值由作业船的船体尺寸、航行速度等因素确定，D₂的值过大时会导致计算得出的跟踪目标点距离作业船较远，当作业船偏离目标路径时很难回调；D₂的值过小时又会导致作业船对于航向的变化过于敏感，造成系统的震荡。

接着，主控制器将会以点P作为跟踪目标点对作业船进行自动导航控制。当船体由Q运动至Q′时，跟踪目标点也同时向前运动至P′，直至到达T_K+1点为止。

这种方法能够实时地反映出航向的误差，将有限个角点细化成无数个跟踪目标点，可以使作业船的航行轨迹更加地逼近设定路径，能显著提高作业船自动导航路径跟踪的效果。

第4步.航向、航速双闭环控制

本发明将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差；分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船的左右明轮控制量。

为了减小水产养殖作业船的转弯半径，本发明采用两侧均可正反转的明轮作为驱动装置，并设计了相应的航向闭环、航速闭环解耦方法，保证两者能够互不干扰，实现解耦控制。当前航向Ψ₂由惯导模块采集得到，目标航向Ψ₁根据作业船当前的经纬度坐标Q(x，y)与跟踪目标点的经纬度坐标P(x，y)计算得到；目标速度V₁由第2步得到，当前速度V₂由惯导模块采集得到。

首先，根据公式：

Ψ^*(k)＝K_p×ΔΨ(k)+K_d×(ΔΨ(k)-ΔΨ(k-1))

得到航向闭环的调整量Ψ^*。式中，K_p、K_d为PD控制器的比例、微分系数，ΔΨ为目标航向Ψ₁与当前航向Ψ₂的差值，即

ΔΨ(k)＝Ψ₁(k)-Ψ₂(k)

接着，根据公式：

$V^{*}\left(k\right)=K_p\×\mathrm{\Δ}V\left(k\right)+K_i\×\underset{j=1}{\overset{k}{\Σ}}\mathrm{\Δ}V\left(j\right)$

得到航速闭环的调整量V^*。式中，K_p、K_i为航速PI控制器的比例、积分参数，ΔV为目标航速V₁和当前航速V₂的差值，即

ΔV(k)＝V₁(k)-V₂(k)

本实施例将GPS设备的主天线放置在左右明轮连线的中点处以实现航向闭环和航速闭环的解耦控制。当左右明轮速度分别为和时，作业船将会以主天线为中心点原地转向，此时由于主天线没有发生位移，GPS设备输出的速度值为0，即航向调整不会影响到航速；当左右明轮的速度同时为V^*时，作业船将保持当前航向前进或者后退，此时作业船的航向变化量为0，即航速调整不会影响到航向。本实施例以速度调整量V^*为基准，左右明轮速度分别加上和减去来进行解耦，得到左右明轮的速度分别为：

左明轮：

$U_1=V^{*}+\frac{1}{2}{\mathrm{\Ψ}}^{*}$

右明轮：

$U_2=V^{*}-\frac{1}{2}{\mathrm{\Ψ}}^{*}$

ARM9主控制器根据U₁及U₂的值设定左右明轮的转速，灵活控制船体进行加速、减速、左转或右转等动作，控制船体按照设定航线航行。

第5步.根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。

具体实施例

上述第1步规划作业船的目标航线和航速中，设定水产养殖作业船的目标作业区域为图3所示的不规则的形状，根据目标作业区域的形状特征设置4个能反映目标作业区域轮廓的角点{J₀，J₁，J₂，J₃}，将原始的曲线路径分解为图4所示的多个首尾相连的直线路径，使用高精度GPS系统采集这些角点的经纬度坐标分别为(119.51700896，32.19759716)、(119.51744361，32.19757820)、(119.51740220，32.19784642)、(119.51698746，32.19785174)，并将其以二维数组的形式储存在主控制器之中。接着，由已知两点的经纬度计算两点间距离的公式：

$L=2\×6378137\×\arcsin \left(\sqrt{{\sin }^2\left(\frac{{\mathrm{Lat}}_1-{\mathrm{Lat}}_2}{2}\right)+cos\left({\mathrm{Lat}}_1\right)\×cos\left({\mathrm{Lat}}_2\right)\×{\sin }^2\left(\frac{{\mathrm{Lng}}_1-{\mathrm{Lng}}_2}{2}\right)}\right)$

分别计算出作业船的当前位置Q(x，y)同各个角点的距离{L₀，L₁，L₂，L₃}，其中，Lat表示经度，Lng表示纬度，L的单位为m。接着将距离作业船最近的角点作为行进路线的起点T₀，其示意图如图5所示。按照路径最短的原则对作业船的行进路线进行规划，得到作业船的行进路线为：

T₀→T₁→T₂→T₃

作业船相应的行进路线数据为：

{(x_T0，y_T0)，(x_T1，y_T1)，(x_T2，y_T2)，(x_T3，y_T3)}

最后，结合目标路径设定目标巡航速度为：V_L＝0.8m.s^-1，目标转弯速度为：V_S＝0.35m.s^-1。

其示意图如图6所示。

这种方式将不规则的作业区域规划为多个首尾相连的直线路径，与现有的技术相比，降低了作业船行进路线数据获取的难度。

上述第2步中设定作业船航行速度并判断是否需要路径切换中，R₁、R₂的值应由作业船的船体尺寸等因素确定。在本实施例中，考虑到水产养殖作业船的船体尺寸(长×宽×高)分别为4.0m×1.6m×1.2m，设置R₁＝3m、R₂＝1.5m，可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率。

上述第3步采用实时插点的方式得到作业船的跟踪目标点中，设定前伸距离D₂的值为2.5m，在作业船偏离目标路径时可以很快地回调且不会造成系统的震荡。

本实施例中作业船的自动导航路径跟踪轨迹图如图9所示，运行过程中的误差曲线如图10所示。由实验结果可以看到：作业船在转弯点处不易偏离目标路径，并且当船体出现偏差时会很快地进行回调，其在转弯与直行时偏离目标路径的最大误差分别为0.36m和0.09m，能很好地满足蟹塘内水草清理及均匀投饵的要求。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (6)

1.一种水产养殖作业船自动导航路径跟踪方法，其特征在于，包括以下步骤：

第一步：使用基于RTK模式的高精度GPS系统采集能反映目标作业区轮廓的各个角点的经纬度坐标，经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径；

第二步：通过基于RTK模式的高精度GPS系统以及惯导模块采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，设定作业船的航行速度并判断是否需要进行目标路径切换；

第三步：采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度等因素确定；

第四步：将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船的左右明轮控制量；

第五步：根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。

2.如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第一步中经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径，具体步骤如下：

步骤1.1，计算作业船与能反映目标作业区轮廓的各个角点{J₀，J₁，...，J_n}的距离{L₀，L₁，...，L_n}，并将距离最近的点作为目标航线的起点T₀；

步骤1.2，以各个角点的经纬度坐标为基准，按照路径最短原则推算出作业船的目标航线：

T₀→T₁→...→T_k-1→T_K→T_K+1→...→T_N

对应的目标点集合为：

{T₀，T₁，...，T_k-1，T_K，T_K+1，...，T_N}

其经纬度坐标为：

{(x_T0，y_T0)，(x_T1，y_T1)，...，(x_Tk-1，y_Tk-1)，(x_Tk，y_Tk)，(x_Tk+1，y_Tk+1)，...，(x_Tn，y_Tn)}

其中x坐标表示经度，y坐标表示纬度，T_N为作业船目标航线的终点；

步骤1.3，结合目标航线设定作业船的目标巡航速度V_L和目标转弯速度V_S。

步骤1.4，确定路径T₀→T₁为作业船起始的目标路径。

3.如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第二步中设定作业船的航行速度并判断是否需要进行目标路径切换，具体步骤如下：

步骤2.1，根据作业船当前位置Q及目标路径终点T_k+1的经纬度坐标计算出作业船与目标路径终点T_k+1的距离D₁；

步骤2.2，若步骤2.1中计算得到的距离D₁大于减速半径R₁，则认为作业船还未逼近目标路径终点，设定作业船的航行速度V₁为较大的巡航速度V_L；若D₁小于或等于减速半径R₁，则认为作业船已逼近目标路径终点，设定作业船的航行速度V₁为较小的转弯速度V_S；

步骤2.3，若步骤2.1中计算得到的距离D₁大于目标半径R₂，则认为作业船还未到达目标路径终点，不需要进行目标路径的切换；若D₁小于或等于目标半径R₂，则认为作业船已到达目标路径终点，需要将当前的目标路径由T_k→T_k+1切换成T_k+1→T_k+2；

其中，R₁、R₂的值由作业船的船体尺寸等因素确定。

4.如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第三步的具体过程如下：

步骤3.1，计算作业船的当前位置Q(x₁，y₁)在当前目标路径T_K(x_TK，y_TK)→T_K+1(x_TK+1，y_TK+1)上的投影点M(x₂，y₂)，计算的公式为：

$x_2=\frac{(y_1-y_{TK})\×(x_1\×(y_{TK+1}-y_{TK})+x_{TK}\×(y_1-y_{TK+1}\left)\right)+y_{TK+1}\×{(x_1-x_{TK})}^2}{{(x_1-x_{TK})}^2+{(y_1-y_{TK})}^2}$

$y_2=\frac{(x_1-x_{TK})\×(y_{TK}\×(x_1-x_{TK+1})-y_1\×(x_{TK}-x_{TK+1}\left)\right)+y_{TK+1}\×{(y_1-y_{TK})}^2}{{(x_1-x_{TK})}^2+{(y_1-y_{TK})}^2}$

其中，横坐标x表示经度，纵坐标y表示纬度。

步骤3.2，由两点经纬度坐标计算两点距离的公式得到投影点M点到目标路径终点T_K+1点的距离d，计算的公式为：

$d=2\×6378137\×\arcsin \left(\sqrt{{\sin }^2\left(\frac{y_{TK+1}-y_2}{2}\right)+cos\left(y_{TK+1}\right)\×cos\left(y_2\right)\×{\sin }^2\left(\frac{x_{TK+1}-x_2}{2}\right)}\right)$

其中，6378137为地球半径，单位为m。

步骤3.3，以投影点M点作为起始点沿着T_k→T_k+1方向前伸距离D₂至P(x₃，y₃)点，P(x₃，y₃)点坐标的计算公式为：

$x_3=x_2\±\frac{L_1\×(x_2-x_{TK})}{d}$

$y_3=y_2\±\frac{D_2\×(y_2-y_{TK})}{d}$

式中D₂的值由作业船的船体尺寸、航行速度等因素确定。

5.如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第四步中，将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，计算得到作业船的航向偏差与速度偏差，具体步骤如下：

步骤4.1，航向偏差ΔΨ的确定：作业船的航向偏差ΔΨ为作业船的目标航向Ψ₁与当前航向Ψ₂的差值，计算的公式为：

ΔΨ＝Ψ₁-Ψ₂

其中，当前航向Ψ₂由惯导模块采集得到，目标航向Ψ₁根据作业船当前的经纬度坐标Q(x，y)与跟踪目标点的经纬度坐标P(x，y)计算得到；

步骤4.2，航速偏差ΔV的确定：作业船的航速偏差ΔV为作业船目标速度V₁与当前速度V₂的差值，计算的公式为：

ΔV(k)＝V₁-V₂

目标速度V₁由第二步得到，当前速度V₂由惯导模块采集得到。

6.如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第四步中，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船最终的控制量即左右明轮速度，具体步骤如下：

步骤4.a，通过PD控制算法推算出航向控制量Ψ^*：第i时刻的航向控制量Ψ^*(i)由以下公式推算得出：

Ψ^*(i)＝K_p×ΔΨ(i)+K_d×(ΔΨ(i)-ΔΨ(i-1))

其中，ΔΨ(i)与ΔΨ(i-1)分别为第i、i-1时刻的航向偏差，K_p为比例参数，K_d为微分参数，K_p和K_d参数均根据实验效果现场整定；

步骤4.b，通过PI控制算法推算出航速控制量V^*：第i时刻的航速控制量V^*(i)由以下公式推算得出：

$V^{*}\left(i\right)=K_p\×\mathrm{\Δ}V\left(i\right)+K_i\×\underset{j=1}{\overset{i}{\Σ}}\mathrm{\Δ}V\left(j\right)$

其中，ΔV(i)为第i的航速偏差，K_p为比例参数，K_i为积分参数，K_p和K_i参数均根据实验效果现场整定；

步骤4.c，融合航向控制量Ψ^*与航速控制量V^*得到作业船左右明轮控制量：本发明以速度控制量V^*为基准，左右明轮控制量分别加上和减去来进行解耦，得到左右明轮的控制量分别为：

左明轮为：

$U_1=V^{*}+\frac{1}{2}{\mathrm{\Ψ}}^{*}$

右明轮为：

$U_2=V^{*}-\frac{1}{2}{\mathrm{\Ψ}}^{*}.$