CN109163726A - 一种回状的全覆盖轨迹规划方法 - Google Patents

Abstract

本发明公开了一种回状的全覆盖轨迹规划方法，主要包括以下步骤，第一步是利用GPS采集作业区域的顶点，确定作业区域的平面坐标系；第二步是通过几何运算，确定作业区域的中心位置；第三步是抛幅密度曲线确定一次完整作业圈数；第四步是计算回状遍历的目标点，生成作业轨迹；第五步根据轨迹间距确定均匀投饵的最优抛幅。本发明可提高水产养殖的效益，促进水产养殖业的发展。

Description

一种回状的全覆盖轨迹规划方法

技术领域

本发明涉及全自动水产养殖船的远程监控系统设计，特别给出了回状的全覆盖轨迹规划方法。

背景技术

随着我国的经济快速发展，居民的消费水平在不断提高，消费结构在不断升级，其中水产品的消费增长率逐年上升，水产养殖业的行业盈利性和成长性保持着良好趋势。目前我国水产品的需求量在不断增长，但是大部分水产养殖还是依靠人工撑船投喂和投饵机定点投喂，人工撑船投喂方式虽然灵活，但是效率低下，并且存在安全隐患；定点投喂虽然解放了劳动力，但是缺乏灵活性，只能固定在一点投喂，无法进行全覆盖的投喂。随着自动化技术的不断发展，近年来有一些自动或半自动化的水产养殖设备投入使用，如申请号为201720211910.3的专利里的“智能投饵船”，可以使用遥控装置控制作业船进行投饵等操作，虽然一定程度上实现了控制方式的灵活性，但还是需要渔民时刻看守在作业现场，没有充分解放劳动力；申请号为201610710797.3的专利“一种自主导航河蟹养殖投饵装置及均匀投饵方法”使用自主导航作业船为载体进行均匀投饵，所提出的轨迹规划方法只是针对矩形的作业区域，当遇到五边形甚至是多边形的作业区域时则不再适用。

发明内容

针对上述专利中的不足，本发明提出了一种回状的全覆盖轨迹规划方法，以提高水产养殖的效益。

为实现上述目的，本发明采用的技术方案如下：一种回状的全覆盖轨迹规划方法，包括如下步骤：

第一步是利用GPS采集作业区域的顶点，确定池塘作业区域的平面坐标系；

第二步是通过几何运算，确定作业区域的中心位置的集合；

第三步由工作区域中心位置距离确定一次完整作业的圈数并调整轨迹间距；

第四步是计算回状遍历的目标点，生成作业轨迹；

第五步根据轨迹间距确定均匀投饵的最优抛幅。

进一步，所述第一步的具体过程包括如下内容：

步骤1.1，利用RTK-GPS测定池塘的5个顶点经纬度坐标；

步骤1.2，利用高斯投影公式将5个顶点的经纬度坐标转换为高斯坐标，通过坐标转换将高斯坐标转换为笛卡尔坐标；

步骤1.3，将5个顶点按顺时针方向记作A(x_a,y_a)、B(x_b,y_b)、C(x_c,y_c)、D(x_d,y_d)和E(x_e,y_e)。

3.根据权利要求1所述的一种回状的全覆盖轨迹规划方法，其特征在于，所述第二步的具体过程为：

步骤2.1，计算出5个顶点A、B、C、D、E依次构成的线段AB、BC、CD、DE和EA的中点坐标，分别记作P₁(x_p1,y_p1)和P₂(x_p2,y_p2)，其中，

步骤2.2，计算出AB、BC、CD、DE和EA的斜率，分别记为k_ab和k_bc，其中，

步骤2.3，计算出过点P₁垂直于直线AB的直线方程：计算出过点P₂垂直于直线BC的直线方程：

步骤2.4，确定工作区域的中心位置为M点，计为M(x_m,y_m)，其中x_m和y_m可以通联立方程组得到：

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

步骤3.1，计算出工作区域中心位置距离最外围的距离d：

步骤3.2，投饵机取最大抛幅时，由于饵料在水面上呈扇状分布，要提高两个相邻轨迹间的投喂密度的一致性，相邻轨迹减的饵料投喂必须要存在重叠区域，将轨迹间距离初始化为有效抛幅l：其中，l_max为投饵机最大抛幅；

步骤3.3，确定一次完整作业的圈数q：其中，max[]函数是取不大于的最大整数，l为投饵机最大抛幅作业时的有效抛幅；

步骤3.4，由于距离d大概率不是l的整数倍，所以行驶q圈后会存在覆盖不到的区域，应当遍历q+1圈，进一步调整轨迹间间距为：

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

步骤4.1，记直线AB的法向量为

步骤4.2，第一次平移是相对于边界，只需要平移半个抛幅，即平移而后每次平移

步骤4.3，计算边界AB第一次平移的向量为：

其中，l_max为投饵机的最大抛幅；

步骤4.4，直线AB平移得到A’B’,方程为：

步骤4.5，同理，重复以上三步可以得到其它四条直线第一次平移后的方程，分别为：

其中，(x₂,y₂)为直线BC的法向量(x₃,y₃)为直线CD的法向量(x₄,y₄)为直线DE的法向量(x₅,y₅)为直线EA的法向量

步骤4.6，平移完成后，五条直线的5个交点就是第二圈要遍历的目标点，记为T₁，T₂，T₃，T₄和T₅；

步骤4.7，同理，重复以上五步可以确定第n(n≤q)圈所要遍历的目标点，记为T_1n，T_2n，T_3n，T_4n和T_5n；

步骤4.8，生成水产养殖船的作业目标点的集合为：{T₁,T₂...T_(5q-1),T_5q}，生成水产养殖船的作业轨迹为：T₁→T₂…→T_(5q-1)→T_5q。

进一步，所述第五步中，当间距一定时，根据抛幅与饵料的分布密度均方差的趋势示意图来调整抛幅使得饵料的分布密度均方差最小。

当间距一定时，抛幅与饵料的分布密度均方差之间是一种振荡并趋于稳定的关系，即抛幅变化量在初始化时对饵料的分布密度均方差的影响较大，而当抛幅逐渐增大时饵料的分布密度均方差的影响会逐渐减小并趋于稳定，根据抛幅与饵料的分布密度均方差的趋势示意图来调整抛幅使得饵料的分布密度均方差最小。

与现有技术相比，本发明具有以下有益效果：用户可以在手机上远程监控水产养殖船，随时随地掌握并控制水产养殖船的作业动作，极大的提高了水产养殖的效益，其次，本发明提出的轨迹规划方法适用多边形的作业区域，确定作业区域的中心位置后，将作业区域的边界轨迹向中心平移可以得到新的作业轨迹，实现了全覆盖均匀投饵，促进了水产养殖业的发展。

附图说明

图1为全自动水产养殖船前端作业设备的左视图；

图2为全自动水产养殖船前端作业设备的俯视图；

图3确定作业区域中心位置和轨迹平移方向示意图；

图4初始化轨迹间距示意图；(a)为相邻航道的轨迹重叠示意图；(b)为投饵机有效抛幅示意图；

图5计算轨迹规划目标点示意图；

图6饵料平均累积密度和饵料的分布密度均方差随抛幅变化的趋势示意图；

具体实施方式

以下结合附图及具体实施方式，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用于解释本发明，并不用于限定本发明。

如图1和图2所示，本发明所述双浮体明轮驱动平台的两侧安装有可正反转的明轮电机和装置，不仅可以控制船体动作，还可以防止水草缠绕，给池水增氧。所述控制柜11装在船体的前端，控制的顶端放置GPS的移动站设备，控制柜11内部分为两层，第一层放置控制板和GPRS通信模块5，第二层放置用以供电的12V蓄电池4，并在电池的输出端安装有电流传感器6，测得蓄电池的输出电流发送给控制板；所述投饵机安装在船体的后端，投饵机由料仓、转台和底座构成，料仓里装有开度电机，在开度电机出安装接近传感器测得开度大小并传输给控制板，在转台上安装接近传感器用以控制投饵机的抛幅，在底座安装称重传感器用以测量投饵机里的饵料剩余量；

该系统基于GPS定位系统、传感器技术和GPRS通信模块，能够监测水产养殖船的各项工作参数，同时可以远程对水产养殖船的工作方式进行调整；

GPS定位系统测得的位置信息和传感器测得各项参数经STM32微处理器处理后，STM32处理器将处理过的数据通过GPRS通信模块发送给远程服务器，用户手机客户端的应用程序从服务器上获取船体的位置信息和处理过的各项参数，根据位置信息进行轨迹规划并将轨迹的数据打包发送给船载控制系统，根据接收到的各项参数来远程监测系统的状态，分析参数后对系统的作业状态进行调整；

上述的GPS定位系统,包括GPS基站、GPS移动站2、GPRS通信模块5，GPS基站架设在岸边空旷区域，GPS移动站2固定在控制柜11的上面，通过GPS系统测得船体的当前位置后，经STM32控制板3处理后通过GPRS通信模块5发送给服务器,其中GPRS通信模块通过串行接口连接在STM32控制板上，手机客户端从服务器上获取位置信息进行轨迹规划并将轨迹数据打包发送回STM32控制板，STM32控制板根据轨迹数据实时控制明轮10来驱动双浮体结构12的船体，其中明轮装置安装在双浮体的两侧；

上述的传感器技术，投饵机的饵料剩余量通过饵料箱1下侧的称重传感器8测得，投饵机的抛幅由抛盘上的接近传感器7测得，投饵机的流量大小由开度阀门上的接近传感器9测得，避障系统依赖于船头的三个超声波传感器13，蓄电池4剩余电量由电流传感器6测得。

上述的客户端应用程序根据接收到的参数对作业状态进行调整，所述手机客户端的应用程序当剩余电量不足时会向用户报警；当剩余饵料不足时，所述手机客户端的应用程序会改变行进轨迹，船体自主返回码头补充饵料。

上述的GPRS通信模块，所属GPRS通信模块与所述STM32控制板相连，使用AT指令配置GPRS模块的IP地址、端口和波特率，登陆服务器可以监听GPRS通信模块的输出数据，船体控制系统的位置信息和各项参数通过GPRS发送给服务器。

本发明所述的轨迹规划方法，按照如下步骤进行：

第一步建立池塘工作区域的平面坐标系；

第二步确定工作区域中心位置的集合；

第三步确定一次完整作业的圈数并调整轨迹间距；

第四步计算回状遍历的目标点，生成作业轨迹；

第五步根据轨迹间距确定均匀投饵的最优抛幅。

所述步骤第一步的具体过程包括如下内容：

1)利用RTK-GPS测定池塘的5个顶点经纬度坐标；

2)利用高斯投影公式将5个顶点的经纬度坐标转换为高斯坐标，通过坐标转换将高斯坐标转换为笛卡尔坐标；

3)将5个顶点按顺时针方向记作A(x_a,y_a)、B(x_b,y_b)、C(x_c,y_c)、D(x_d,y_d)和E(x_e,y_e)；

如图3所示，所述步骤第二步的具体过程包括如下内容：

1)计算出线段AB和BC的中点坐标，分别记作P₁(x_p1,y_p1)和P₂(x_p2,y_p2)，其中，

2)计算出AB和BC的斜率，分别记为k_ab和k_bc，其中，

3)计算出过点P₁垂直于直线AB的直线方程：

计算出过点P₂垂直于直线BC的直线方程：

4)确定工作区域的中心位置为M点，计为M(x_m,y_m)，其中x_m和y_m可以通联立方程组得到：

其中，

所述步骤第三步的具体过程包括如下内容：

1)计算出工作区域中心位置距离最外围的距离d：

2)投饵机取最大抛幅时，由于饵料在水面上呈扇状分布，要提高两个相邻轨迹间的投喂密度的一致性，相邻轨迹减的饵料投喂必须要存在重叠区域，由图4(a)和图4(b)可以将轨迹间距离初始化为有效抛幅l：

其中，l_max为投饵机最大抛幅；

3)确定一次完整作业的圈数q：

其中，max[]函数是取不大于的最大整数，l为投饵机最大抛幅作业时的有效抛幅；

4)由于距离d大概率不是l的整数倍，所以行驶q圈后会存在覆盖不到的区域，应当遍历q+1圈，进一步调整轨迹间间距为

如图5所示，所述步骤第四步的具体过程包括如下内容：

1)记直线AB的法向量为

2)第一次平移是相对于边界，只需要平移半个抛幅，即平移而后每次平移

3)计算边界AB第一次平移的向量为：

其中，l_max为投饵机的最大抛幅；

4)直线AB平移得到A’B’,方程为：

5)同理，重复以上三步可以得到其它四条直线第一次平移后的方程，分别为：

其中，(x₂,y₂)为直线BC的法向量(x₃,y₃)为直线CD的法向量(x₄,y₄)为直线DE的法向量(x₅,y₅)为直线EA的法向量

6)平移完成后，五条直线的5个交点就是第二圈要遍历的目标点，记为T₁，T₂，T₃，T₄和T₅；

7)同理，重复以上五步可以确定第n(n≤q+1)圈所要遍历的目标点，记为T_5n-4，T_5n-3，T_5n-2，T_5n-1和T_5n；

8)生成水产养殖船的作业目标点的集合为：{T₁,T₂...T_(5q-1),T_5q}，生成水产养殖船的作业轨迹为：T₁→T₂…→T_(5q-1)→T_5q；

如图6所示，所述步骤第五步的具体过程包括如下内容：

当间距一定时，饵料的分布密度均方差随抛幅变化的趋势如图6所示，根据图6调整抛幅使得饵料的分布密度均方差最小以到达均匀投饵的目的。当间距一定时，抛幅与饵料的分布密度均方差之间是一种振荡并趋于稳定的关系，即抛幅变化量在初始化时对饵料的分布密度均方差的影响较大，而当抛幅逐渐增大时饵料的分布密度均方差的影响会逐渐减小并趋于稳定，根据抛幅与饵料的分布密度均方差的趋势示意图来调整抛幅使得饵料的分布密度均方差最小。

综上，本发明的一种全自动水产养殖船远程监控系统及轨迹规划方法，所述的远程监控系统，包括前端作业设备、服务器端、客户端以及通信模块，所述的前端作业设备主要包括明轮驱动的双浮体结构的船体、GPS定位设备、信息采集模块、投饵装置以及基于STM32F4的控制板；所述服务器端主要包括在计算机上用JAVASwing编写的服务器程序，用于监听和发送客户端输出的信息；所述客户端主要包括STM32开发板中中断服务程序、船体与通信模块的通信程序以及用户手机中的应用程序；所述通信模块采用GPRS模块对数据进行TCP/IP协议转换；所述的轨迹规划方法主要包括以下步骤，第一步是利用GPS采集作业区域的顶点，确定作业区域的平面坐标系；第二步是通过几何运算，确定作业区域的中心位置；第三步是抛幅密度曲线确定一次完整作业圈数；第四步是计算回状遍历的目标点，生成作业轨迹；第五步根据轨迹间距确定均匀投饵的最优抛幅。本发明可提高水产养殖的效益，促进水产养殖业的发展。

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

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

Claims (6)

1.一种回状的全覆盖轨迹规划方法，其特征在于，包括如下步骤：

第一步是利用GPS采集作业区域的顶点，确定池塘作业区域的平面坐标系；

第二步是通过几何运算，确定作业区域的中心位置的集合；

第三步由工作区域中心位置距离确定一次完整作业的圈数并调整轨迹间距；

第四步是计算回状遍历的目标点，生成作业轨迹；

第五步根据轨迹间距确定均匀投饵的最优抛幅。

2.根据权利要求1所述的一种回状的全覆盖轨迹规划方法，其特征在于，所述第一步的具体过程包括如下内容：

步骤1.1，利用RTK-GPS测定池塘的5个顶点经纬度坐标；

步骤1.2，利用高斯投影公式将5个顶点的经纬度坐标转换为高斯坐标，通过坐标转换将高斯坐标转换为笛卡尔坐标；

步骤1.3，将5个顶点按顺时针方向记作A(x_a,y_a)、B(x_b,y_b)、C(x_c,y_c)、D(x_d,y_d)和E(x_e,y_e)。

3.根据权利要求1所述的一种回状的全覆盖轨迹规划方法，其特征在于，所述第二步的具体过程为：

步骤2.1，计算出5个顶点A、B、C、D、E依次构成的线段AB、BC、CD、DE和EA的中点坐标，分别记作P₁(x_p1,y_p1)和P₂(x_p2,y_p2)，其中，

步骤2.2，计算出AB、BC、CD、DE和EA的斜率，分别记为k_ab和k_bc，其中，

步骤2.3，计算出过点P₁垂直于直线AB的直线方程：计算出过点P₂垂直于直线BC的直线方程：

步骤2.4，确定工作区域的中心位置为M点，计为M(x_m,y_m)，其中x_m和y_m可以通联立方程组得到：

4.根据权利要求1所述的一种回状的全覆盖轨迹规划方法，其特征在于，所述第三步的具体过程如下：

步骤3.1，计算出工作区域中心位置距离最外围的距离d：

步骤3.2，投饵机取最大抛幅时，由于饵料在水面上呈扇状分布，要提高两个相邻轨迹间的投喂密度的一致性，相邻轨迹减的饵料投喂必须要存在重叠区域，将轨迹间距离初始化为有效抛幅l：其中，l_max为投饵机最大抛幅；

步骤3.3，确定一次完整作业的圈数q：其中，max[]函数是取不大于的最大整数，l为投饵机最大抛幅作业时的有效抛幅；

步骤3.4，由于距离d大概率不是l的整数倍，所以行驶q圈后会存在覆盖不到的区域，应当遍历q+1圈，进一步调整轨迹间间距为：

5.根据权利要求1所述的一种回状的全覆盖轨迹规划方法，其特征在于，所述第四步的具体过程如下：

步骤4.1，记直线AB的法向量为

步骤4.2，第一次平移是相对于边界，只需要平移半个抛幅，即平移而后每次平移

步骤4.3，计算边界AB第一次平移的向量为：

其中，l_max为投饵机的最大抛幅；

步骤4.4，直线AB平移得到A’B’,方程为：

步骤4.5，同理，重复以上三步可以得到其它四条直线第一次平移后的方程，分别为：

其中，(x₂,y₂)为直线BC的法向量(x₃,y₃)为直线CD的法向量(x₄,y₄)为直线DE的法向量(x₅,y₅)为直线EA的法向量

步骤4.6，平移完成后，五条直线的5个交点就是第二圈要遍历的目标点，记为T₁，T₂，T₃，T₄和T₅；

步骤4.7，同理，重复以上五步可以确定第n(n≤q)圈所要遍历的目标点，记为T_1n，T_2n，T_3n，T_4n和T_5n；

步骤4.8，生成水产养殖船的作业目标点的集合为：{T₁,T₂...T_(5q-1),T_5q}，生成水产养殖船的作业轨迹为：T₁→T₂…→T_(5q-1)→T_5q。

6.根据权利要求1所述的一种回状的全覆盖轨迹规划方法，其特征在于，所述第五步中，当间距一定时，根据抛幅与饵料的分布密度均方差的趋势示意图来调整抛幅使得饵料的分布密度均方差最小。