CN105466429A - 一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法 - Google Patents

Abstract

本发明公开了一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，所述方法包括如下步骤：第一步，建立池塘区域坐标系；第二步，确定自动投饲工作区域；第三步，判断自动投饲工作区域形状规则情况，对于形状规则池塘，采用内螺旋遍历方式池塘全覆盖自动投饲轨迹规划方法，对于形状不规则池塘，采用梳状往复遍历方式池塘全覆盖自动投饲轨迹规划方法，自动计算特征点坐标，生成池塘全覆盖自动投饲轨迹。本发明提供一种用于河蟹养殖全池自动均匀投饵喂料和其他水产养殖中需要沿池或全池自动均匀投饲的轨迹规划方法，通过池塘顶点坐标可直接解算出均匀投饲路径轨迹方法，实现精确均匀投饲，提高工作效率、降低生产成本，促进水产养殖业的健康发展。

Description

一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法

技术领域

本发明涉及一种全覆盖自动均匀投饲轨迹规划方法，特别是涉及一种用于水产养殖池塘全覆盖自动均匀投饲轨迹规划方法。

背景技术

河蟹养殖于20世纪90年代初形成产业规模，目前已成为中国渔业生产中发展最为迅速、最具潜力的支柱产业，投饵喂料是河蟹养殖中任务繁重而又关键的一项工作，因为饲料成本占到整个投资成本的50％以上，投饵喂料技术是否合理，是影响水产养殖效果和环境生态效益的一个最重要的因素。由于河蟹不能大范围运动，只能在自身附近区域觅食，因此，投饲时，饵料要均匀投放在整个池塘水面，对于饵料密度过大的水域，会造成饲料的浪费，残余饲料恶化养殖环境；而对于饵料密度过小的水域，会影响河蟹的摄食量，容易造成河蟹因抢食、争斗而受伤，继而引发疾病，导致河蟹死亡的后果。

目前，国内河蟹养殖投饵喂料一般采用3种方式：一是靠人工撑船投饵喂料，一人撑船，一人投饵，仅凭人工经验，工作效率低，劳动强度大，无法保证投饵的均匀度；二是靠投饵机投饵喂料，这种方式虽然可以通过人工操作定时定量投饵，节约劳动力，但只能固定在同一地点投饵，饵料分布在岸边很小的水域内，其他水域特别是中间水域无法覆盖，不能保证投饵的均匀度；三是通过船载投饵机喂料，通过汽油机或者柴油机驱动船，再通过安装在船上的投饵机将饵料投向池塘，由于船行进的路线全凭人工随意确定，随机性强，很难保证投饵的均匀度。

随着农业电气化、自动化技术的发展，出现了一些自动投饵设备，如申请号为02229989.0的专利“遥控自动投饵船”能根据人为遥控在池塘中投料，但完全靠人工经验确定投喂量，航行路线随机性强，工作效果差；申请号为201310155373.1的专利“水产养殖机器人”能根据实时航程、航速、料量参数实时控制投料速率，做到了出料均匀，但没有对投饵过程进行建模和定量分析，无法保证饵料落在水面上的实际分布均匀度；申请号为201410138938.X的专利“一种基于GPS导航的智能投料施药系统及装置”，能接收人机交互子系统输入的任务和指令，并控制喂料子系统执行喂料动作，实现饲料精准投喂和药物均匀泼洒，也没有给出定量分析，难以保证饵料分布均匀度；申请号为201410243137.X的专利“一种风力驱动自动投饵系统及均匀投饵方法”，利用投饵机抛料器建模、饲料颗粒斜抛运动建模、饵料在水面上的累积密度分布建模、自动投饵系统均匀投饵的最优运行参数求解等步骤自动确定自动投饵装置喂料器单位时间内落料量、抛盘转速、投饵机抛料扇角、风力驱动船行驶速度、路径规划中两个相邻投饵行程的宽度等自动投饵系统最优运行参数，利用北斗卫星导航系统，通过北斗导航装置实现路径跟踪，完成自动均匀投饵作业，但没有给出路径规划的具体原理与方法。

发明内容

本发明的目的在于针对现有技术存在的问题，提出了一种用于河蟹养殖全池自动均匀投饵喂料和其他水产养殖中需要沿池或全池自动均匀投饲的轨迹规划方法，提供均匀投饲参考路径轨迹，实现水产养殖精确均匀投饲作业。

本发明是通过以下技术方案实现的：一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，包括以下步骤：

步骤S001，建立池塘区域坐标系：设取池塘区域可以为非规则的四边形，四边形池塘的四个顶点分别为P₁、P₂、P₃和P₄，四个顶点坐标分别为和分别求取四条边P₁P₂、P₂P₃、P₃P₄以及P₄P₁的斜率、对应的四条边的倾斜角、池塘的四个顶角；

步骤S002，确定自动投饲工作区域：投饵系统在作业水域行驶路径可分解分为直线和转弯行驶两个基本运动，设置转弯时船体与岸边界的安全距离，进行全覆盖遍历方式投饵；

步骤S003，判断自动投饲工作区域形状是否规则，若是，采用内螺旋遍历方式池塘全覆盖自动投饲轨迹规划方法，跳到步骤S005；若否，采用梳状往复遍历方式池塘全覆盖自动投饲轨迹规划方法，跳到步骤S004；

步骤S004，梳状往复遍历方式池塘全覆盖自动投饲轨迹规划：确定梳状往复遍历方式自动投饲轨迹规划特征点，生成梳状往复遍历方式池塘全覆盖自动投饲轨迹；

步骤S005，内螺旋遍历方式池塘全覆盖自动投饲轨迹规划：确定内螺旋遍历方式池塘全覆盖自动投饲轨迹规划特征点，生成内螺旋遍历方式池塘全覆盖自动投饲轨迹。

进一步，所述步骤S004的具体过程为：

步骤S0041，确定梳状往复遍历方式自动投饲轨迹规划特征点：假设沿着长边P₁P₂方向进行梳状往复遍历方式投饵，只要作出与P₁P₂平行的一系列直线，求出平行直线与作业区域边界的交点，可以把P₁P₂沿着X轴平移就可以得出这些直线与作业区域边界的交点，把这些交点作为直线路径的起止点即投饵作业的路径轨迹特征点，为了确定P₁P₂向内推平行线的一系列直线沿着X轴平移的方向与距离，引入向内的向量由于 向量绕P₁顺时针旋转θ，得到向量则向量因此，当时，P₁P₂沿着X轴右移，反之左移，分别求解出该平行直线与边P′₁P′₄的交点坐标与边P′₂P′₃的交点坐标以及与边P′₃P′₄的交点坐标作为梳状往复遍历方式自动投饲轨迹规划特征点；

步骤S0042，生成梳状往复遍历方式池塘全覆盖自动投饲轨迹：梳状往复遍历方式自动投饲的轨迹采用如下格式描述：{序号、目标点坐标、航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角}，其中，航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角等参数由系统解算给出，因此，只要确定目标点的个数以及遍历顺序，就可以确定梳状往复遍历的轨迹。

进一步，所述步骤S0042中，采用几何推理法按照平行直线扫描特征点的次序来按序依次遍历，用结构体Fpoint[]来表示设定的投饲轨迹点，所述结构体包括变量：轨迹点序号intNum、目标点坐标doublepos、航行区间内的航速doublev_b、投饵机单位时间内下料量doublem_ps、抛盘转速doubleN₀、投饵扇角double

进一步，所述步骤S0042的具体实现过程包括：

步骤S101，开始；

步骤S102，对n赋初始值，n＝1；

步骤S103，n对2取余运算结果是否等于1？若是，跳到步骤S105；若否，跳到步骤S104；

步骤S104，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S106；

步骤S105，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos；

步骤S106，n＝n+1，处理下一个轨迹点；

步骤S107，判断是否n≤n_min+1，若是，跳到步骤S103；若否，跳到步骤S108；

步骤S108，判断是否边P′₁P′₄的长度小于边P′₂P′₃的长度，即是否若是，跳到步骤S112；若否，跳到步骤S109；

步骤S109，n对2取余运算结果是否等于1？若是，跳到步骤S110；若否，跳到步骤S111；

步骤S110，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S111，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S112，n对2取余运算结果是否等于1？若是，跳到步骤S114；若否，跳到步骤S113；

步骤S113，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S114，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos；

步骤S115，n＝n+1，处理下一个轨迹点；

步骤S116，判断是否n≤n_max+1，若是，跳到步骤S108；若否，跳到步骤S117；

步骤S117，结束。

进一步，所述步骤S005的具体过程为：

步骤S0051，确定内螺旋遍历方式池塘全覆盖自动投饲轨迹规划特征点，内螺旋遍历方式自动投饲轨迹规划特征点可以采用四条边分别向内推平行线，两相邻平移直线求交点的方法获取；

步骤S0052，生成内螺旋遍历方式池塘全覆盖自动投饲轨迹，内螺旋遍历方式自动投饲的轨迹采用如下格式描述：{序号目标点坐标航行至目标点的航速投饵机单位时间内下料量抛盘转速抛射幅角}，其中，航行至目标点的航速、投饵机单位时间内下料量、抛盘转速、抛射幅角等参数由系统解算给出，由于工作区域四边形的形状不确定，因此必须确定内螺旋遍历的圈数，由于等距内螺旋遍历方式轨迹特征点在各内角的角平分线上，因此，分别令为内角θ₁和θ₂角平分线交点到边P′₁P′₂的距离，为内角θ₂和θ₃角平分线交点到边P′₂P′₃的距离，为内角θ₃和θ₄角平分线交点到边P′₃P′₄的距离，为内角θ₄和θ₁角平分线交点到边P′₄P′₁的距离，则 $h_{P_1^{\′}{P}_2^{\′}}=\frac{d_{P_1^{\′}{P}_2^{\′}}}{\cot \frac{{\mathrm{\θ}}_1}{2}+\cot \frac{{\mathrm{\θ}}_2}{2}},h_{P_2^{\′}{P}_3^{\′}}=\frac{d_{P_2^{\′}{P}_3^{\′}}}{\cot \frac{{\mathrm{\θ}}_2}{2}+\cot \frac{{\mathrm{\θ}}_3}{2}},h_{P_3^{\′}{P}_4^{\′}}=\frac{d_{P_3^{\′}{P}_4^{\′}}}{\cot \frac{{\mathrm{\θ}}_3}{2}+\cot \frac{{\mathrm{\θ}}_4}{2}},h_{P_4^{\′}{P}_1^{\′}}=\frac{d_{P_4^{\′}{P}_1^{\′}}}{\cot \frac{{\mathrm{\θ}}_4}{2}+\cot \frac{{\mathrm{\θ}}_1}{2}},$ 式中，分别指的是工作区域四条边的边长，则内螺旋遍历的圈数式中R₀为投饵机抛洒半径，因此，只要按照(n＝1，2，...，n_max+1)来顺序依次遍历即可进行内螺旋遍历方式自动投饲的轨迹坐标点构建。

进一步，所述步骤S0051的具体实现过程为：引入向量分别绕P₁、P₂、P₃、P₄顺时针旋转90°，得到向内的向量则 ${\stackrel{\→}{OP}}_1^{\′\′}=\left(\right(y_{P_2}-y_{P_1})-(x_{P_2}-x_{P_1}\left)\right),{\stackrel{\→}{OP}}_2^{\′\′}=\left(\right(y_{P_3}-y_{P_2}),-(x_{P_3}-x_{P_2}\left)\right),{\stackrel{\→}{OP}}_3^{\′\′}=\left(\right(y_{P_4}-y_{P_3}),-(x_{P_4}-x_{P_3}\left)\right),$ 因此，当时，与P₁P₂平行的直线沿着X轴右移，反之左移；当时，与P₂P₃平行的直线沿着X轴右移，反之左移；当时，与P₃P₄平行的直线沿着X轴右移，反之左移；当时，与P₄P₁平行的直线沿着X轴右移，反之左移；分别求解出边P′₁P′₂向内的平行线与边P′₁P′₄向内的平行线的交点坐标边P′₂P′₃向内的平行线与边P′₁P′₂向内的平行线的交点坐标边P′₃P′₄向内的平行线与边P′₂P′₃向内的平行线的交点坐标以及边P′₄P′₁向内的平行线与边P′₃P′₄向内的平行线的交点坐标作为内螺旋遍历方式自动投饲轨迹规划特征点。

本发明采用上述技术后，具有以下技术效果：本发明通过建立池塘区域坐标系：设取池塘区域可以为非规则的四边形，获取四边形池塘的四个顶点坐标，分别求取四条边的斜率、对应的四条边的倾斜角、池塘的四个顶角；然后确定自动投饲工作区域：投饵系统在作业水域行驶路径可分解分为直线和转弯行驶两个基本运动，设置转弯时船体与岸边界的安全距离，进行全覆盖遍历方式投饵；接着根据自动投饲工作区域形状是否规则的情况，选用梳状往复遍历方式或者内螺旋遍历方式池塘全覆盖自动投饲轨迹规划方法；对于梳状往复遍历方式池塘全覆盖自动投饲轨迹规划：首先作出与长边平行的一系列直线，求出平行直线与作业区域边界的交点，把该长边沿着X轴平移就可以得出这些直线与作业区域边界的交点，把这些交点作为直线路径的起止点即投饵作业的路径轨迹特征点，为了确定该长边向内推平行线的一系列直线沿着X轴平移的方向与距离，引入向内的向量进行计算，梳状往复遍历方式自动投饲的轨迹采用如下格式描述：{序号、目标点坐标、航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角}，其中，航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角等参数由系统解算给出，因此，只要确定目标点的个数以及遍历顺序，就可以确定梳状往复遍历的轨迹；对于内螺旋遍历方式池塘全覆盖自动投饲轨迹规划：首先内螺旋遍历方式自动投饲轨迹规划特征点可以采用四条边分别向内推平行线，两相邻平移直线求交点的方法获取，为了确定与四条边分别向内推平行线的一系列直线沿着X轴平移的方向与距离，引入向内的向量进行计算，由于工作区域四边形的形状不确定，因此必须确定内螺旋遍历的圈数，然后按照顺序依次遍历即可生成内螺旋遍历方式池塘全覆盖自动投饲轨迹。本发明利用一种全覆盖池塘区域面状路径规划方法，自动生成最优投饲路径轨迹，提供一种通过池塘顶点坐标可直接解算出均匀投饲路径轨迹方法，实现精确均匀投饲，提高工作效率、降低生产成本，提高河蟹等水产品产量，保证养殖的河蟹等水产品个头大小均等，提高产值、增加效益，促进水产养殖业的健康发展。

附图说明

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

图2是本发明的池塘区域坐标系示意图；

图3是本发明的梳状往复遍历方式池塘全覆盖轨迹规划示意图；

图4是本发明的梳状往复遍历方式池塘全覆盖轨迹生成的方法流程图；

图5是本发明的内螺旋遍历方式池塘全覆盖轨迹规划示意图；

图6是本发明的内螺旋遍历方式池塘全覆盖轨迹规划具体实施例示意图；

图7是本发明的梳状往复遍历方式池塘全覆盖轨迹规划具体实施例示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施方式，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不用于限定本发明。

如图1所示，本发明提供了一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，所述方法包括如下步骤：

1.步骤S001，建立池塘区域坐标系，如图2所示，由于水产养殖池塘一般要求长方形，而一般自然形成或人工开挖的池塘均非规则的矩形，而可能是普通的四边形，为了不失一般性，假设四边形池塘的四个顶点分别为P₁、P₂、P₃和P₄，四个顶点坐标分别为和则四条边P₁P₂、P₂P₃、P₃P₄以及P₄P₁的斜率分别为

$k_{P_1{P}_2}=\frac{y_{P_2}-y_{P_1}}{x_{P_2}-x_{P_1}},k_{P_2{P}_3}=\frac{y_{P_3}-y_{P_2}}{x_{P_3}-x_{P_2}},k_{P_3{P}_4}=\frac{y_{P_4}-y_{P_3}}{x_{P_4}-x_{P_3}},k_{P_4{P}_1}=\frac{y_{P_1}-y_{P_4}}{x_{P_1}-x_{P_4}},$

对应的四条边的倾斜角分别为

${\mathrm{\θ}}_{P_1{P}_2}=arctan\left(k_{P_1{P}_2}\right)+\mathrm{\π}\·sign\left(k_{P_1{P}_2}\right),{\mathrm{\θ}}_{P_2{P}_3}=arctan\left(k_{P_2{P}_3}\right)+\mathrm{\π}\·sign\left(k_{P_2{P}_3}\right),$

${\mathrm{\θ}}_{P_3{P}_4}=arctan\left(k_{P_3{P}_4}\right)+\mathrm{\π}\·sign\left(k_{P_3{P}_4}\right),{\mathrm{\θ}}_{P_4{P}_1}=arctan\left(k_{P_4{P}_1}\right)+\mathrm{\π}\·sign\left(k_{P_4{P}_1}\right),$

这里定义 $sign\left(x\right)=\left\{\begin{array}{cc}0,& x\&GreaterEqual;0\\ 1,& x<0\end{array}\right.,$

池塘的四个顶角分别为

${\mathrm{\&theta;}}_1=arctan\left(\frac{k_{P_1{P}_2}-k_{P_4{P}_1}}{1+k_{P_1{P}_2}\&CenterDot;k_{P_4{P}_1}}\right)+\mathrm{\&pi;}\&CenterDot;sign\left(\frac{k_{P_1{P}_2}-k_{P_4{P}_1}}{1+k_{P_1{P}_2}\&CenterDot;k_{P_4{P}_1}}\right),$

${\mathrm{\&theta;}}_2=arctan\left(\frac{k_{P_2{P}_3}-k_{P_1{P}_2}}{1+k_{P_2{P}_3}\&CenterDot;k_{P_1{P}_2}}\right)+\mathrm{\&pi;}\&CenterDot;sign\left(\frac{k_{P_2{P}_3}-k_{P_1{P}_2}}{1+k_{P_2{P}_3}\&CenterDot;k_{P_1{P}_2}}\right),$

${\mathrm{\&theta;}}_3=arctan\left(\frac{k_{P_3{P}_4}-k_{P_2{P}_3}}{1+k_{P_3{P}_4}\&CenterDot;k_{P_2{P}_3}}\right)+\mathrm{\&pi;}\&CenterDot;sign\left(\frac{k_{P_3{P}_4}-k_{P_2{P}_3}}{1+k_{P_3{P}_4}\&CenterDot;k_{P_2{P}_3}}\right),$

2.步骤S002，确定自动投饲工作区域，如图2所示，投饵系统在作业水域行驶路径可分解分为直线行驶和转弯两个基本运动，对于已知的作业水域，自动投饵系统从起始点沿着不同的方向行驶，会在不同的位置进行转弯，则全区域覆盖投饵任务完成时的总转弯次数就会不同，因此自动投饵系统行驶的距离也将不同。为了保证投饵系统能顺利转弯，必须设置转弯时船体与岸边界的安全距离，为了避免饵料抛洒到堤岸上造成浪费，投饵系统转弯时停止投饵，待转过弯后直线行驶时继续投饵，同时，最佳的作业方向应平行于四边形的一条长边。假设沿着长边P₁P₂方向进行全覆盖遍历方式投饵，与岸边界P₂P₃、P₃P₄以及P₄P₁的安全距离为d_s＝k₀L_b，其中L_b为船长，k₀为安全距离系数。因此，要确定工作区域的四个边界点，分别为P′₁、P′₂、P′₃和P′₄，设四个边界点坐标分别为 和由于边界点的确定存在方向性，在此，引入向量进行相应的计算。

向量 $\stackrel{\&RightArrow;}{{\mathrm{OP}}_1^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_1}+\stackrel{\&RightArrow;}{P_1{P}_1^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_1}+\frac{\stackrel{\&RightArrow;}{P_1{P}_2}}{\left|\stackrel{\&RightArrow;}{P_1{P}_2}\right|}\&CenterDot;\frac{d_s}{{\mathrm{sin\&theta;}}_1},$

则边界点P′₁的坐标为 $(x_{P_1}^{\&prime;},y_{P_1}^{\&prime;})=(x_{P_1},y_{P_1})+\frac{(x_{P_2}-x_{P_1},y_{P_2}-y_{P_1})}{\sqrt{{(x_{P_2}-x_{P_1})}^2+{(y_{P_2}-y_{P_1})}^2}}\&CenterDot;\frac{d_s}{{\mathrm{sin\&theta;}}_1};$

向量 $\stackrel{\&RightArrow;}{{\mathrm{OP}}_2^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_2}+\stackrel{\&RightArrow;}{P_2{P}_2^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_2}+\frac{\stackrel{\&RightArrow;}{P_2{P}_1}}{\left|\stackrel{\&RightArrow;}{P_2{P}_1}\right|}\&CenterDot;\frac{d_s}{{\mathrm{sin\&theta;}}_2},$

则边界点P′₂的坐标为 $(x_{P_2}^{\&prime;},y_{P_2}^{\&prime;})=(x_{P_2},y_{P_2})+\frac{(x_{P_1}-x_{P_2},y_{P_1}-y_{P_2})}{\sqrt{{(x_{P_1}-x_{P_2})}^2+{(y_{P_1}-y_{P_2})}^2}}\&CenterDot;\frac{d_s}{{\mathrm{sin\&theta;}}_2};$

向量 $\stackrel{\&RightArrow;}{{\mathrm{OP}}_3^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_3}+\stackrel{\&RightArrow;}{P_3{P}_3^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_3}+\frac{(\frac{\stackrel{\&RightArrow;}{P_3{P}_2}}{\left|\stackrel{\&RightArrow;}{P_3{P}_2}\right|}+\frac{\stackrel{\&RightArrow;}{P_3{P}_4}}{\left|\stackrel{\&RightArrow;}{P_3{P}_4}\right|})}{|\frac{\stackrel{\&RightArrow;}{P_3{P}_2}}{\left|\stackrel{\&RightArrow;}{P_3{P}_2}\right|}+\frac{\stackrel{\&RightArrow;}{P_3{P}_4}}{\left|\stackrel{\&RightArrow;}{P_3{P}_4}\right|}|}\&CenterDot;\frac{d_s}{sin\frac{{\mathrm{\&theta;}}_3}{2}},$

则边界点P′₃的坐标为

$(x_{P_3}^{\&prime;},y_{P_3}^{\&prime;})=(x_{P_3},y_{P_3})+\frac{(\frac{(x_{P_2}-x_{P_3},y_{P_2}-y_{P_3})}{\sqrt{{(x_{P_2}-x_{P_3})}^2+{(y_{P_2}-y_{P_3})}^2}}+\frac{(x_{P_4}-x_{P_3},y_{P_4}-y_{P_3})}{\sqrt{{(x_{P_4}-x_{P_3})}^2+{(y_{P_4}-y_{P_3})}^2}})}{\sqrt{(2+2\frac{(x_{P_2}-x_{P_3})\&CenterDot;(x_{P_4}-x_{P_3})+(y_{P_2}-y_{P_3})\&CenterDot;(y_{P_4}-y_{P_3})}{\sqrt{{(x_{P_2}-x_{P_3})}^2+{(y_{P_2}-y_{P_3})}^2}\&CenterDot;\sqrt{{(x_{P_4}-x_{P_3})}^2+{(y_{P_4}-y_{P_3})}^2}}}}\&CenterDot;\frac{d_s}{sin\frac{{\mathrm{\&theta;}}_3}{2}}$ 向量 $\stackrel{\&RightArrow;}{{\mathrm{OP}}_4^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_4}+\stackrel{\&RightArrow;}{P_4{P}_4^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_4}+\frac{(\frac{\stackrel{\&RightArrow;}{P_4{P}_3}}{\left|\stackrel{\&RightArrow;}{P_4{P}_3}\right|}+\frac{\stackrel{\&RightArrow;}{P_4{P}_1}}{\left|\stackrel{\&RightArrow;}{P_4{P}_1}\right|})}{|\frac{\stackrel{\&RightArrow;}{P_4{P}_3}}{\left|\stackrel{\&RightArrow;}{P_4{P}_3}\right|}+\frac{\stackrel{\&RightArrow;}{P_4{P}_1}}{\left|\stackrel{\&RightArrow;}{P_4{P}_1}\right|}|}\&CenterDot;\frac{d_s}{sin\frac{{\mathrm{\&theta;}}_4}{2}},$

则边界点P′₄的坐标为

$(x_{P_4}^{\&prime;},y_{P_4}^{\&prime;})=(x_{P_4},y_{P_4})+\frac{(\frac{(x_{P_3}-x_{P_4},y_{P_3}-y_{P_4})}{\sqrt{{(x_{P_3}-x_{P_4})}^2+{(y_{P_3}-y_{P_4})}^2}}+\frac{(x_{P_1}-x_{P_4},y_{P_1}-y_{P_4})}{\sqrt{{(x_{P_1}-x_{P_4})}^2+{(y_{P_1}-y_{P_4})}^2}})}{\sqrt{(2+2\frac{(x_{P_3}-x_{P_4})\&CenterDot;(x_{P_1}-x_{P_4})+(y_{P_3}-y_{P_4})\&CenterDot;(y_{P_1}-y_{P_4})}{\sqrt{{(x_{P_3}-x_{P_4})}^2+{(y_{P_3}-y_{P_4})}^2}\&CenterDot;\sqrt{{(x_{P_1}-x_{P_4})}^2+{(y_{P_1}-y_{P_4})}^2}}}}\&CenterDot;\frac{d_s}{sin\frac{{\mathrm{\&theta;}}_4}{2}}$

3.步骤S003，判断自动投饲工作区域形状是否规则，若是，采用内螺旋遍历方式池塘全覆盖自动投饲轨迹规划方法，跳到步骤S005；若否，采用梳状往复遍历方式池塘全覆盖自动投饲轨迹规划方法，跳到步骤S004；

4.步骤S004，梳状往复遍历方式池塘全覆盖自动投饲轨迹规划，如图3所示；

(4-A)首先，确定梳状往复遍历方式自动投饲轨迹规划特征点，假设沿着长边P₁P₂方向进行梳状往复遍历方式投饵，只要作出与P₁P₂平行的一系列直线，求出平行直线与作业区域边界的交点，因此，可以把P₁P₂沿着X轴平移就可以得出这些直线与作业区域边界的交点，把这些交点作为直线路径的起止点即投饵作业的路径轨迹特征点。为了确定与P₁P₂平行的一系列直线沿着X轴平移的方向与距离，不失一般性，引入向量由于 $\stackrel{\&RightArrow;}{P_1{P}_1^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_1^{\&prime;}}=\stackrel{\&RightArrow;}{{\mathrm{OP}}_1}=(x_{P_1^{\&prime;}}-x_{P_1},y_{P_1^{\&prime;}}-y_{P_1}),$ 向量绕P₁顺时针旋转θ，得到向量则

$\begin{array}{c}\stackrel{\&RightArrow;}{P_1{P}_1^{\&prime;\&prime;}}=\stackrel{\&RightArrow;}{P_1{P}_1^{\&prime;}}\&CenterDot;\left(\begin{array}{cc}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right)=(x_{P_1^{\&prime;}}-x_{P_1},y_{P_1^{\&prime;}}-y_{P_1})\&CenterDot;\left(\begin{array}{cc}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right)\\ =\left(\right((x_{P_1^{\&prime;}}-x_{P_1})\&CenterDot;\cos \mathrm{\&theta;}+(y_{P_1^{\&prime;}}-y_{P_1})\&CenterDot;\sin \mathrm{\&theta;}),(-(x_{P_1^{\&prime;}}-x_{P_1})\&CenterDot;\sin \mathrm{\&theta;}+(y_{P_1^{\&prime;}}-y_{P_1})\&CenterDot;\cos \mathrm{\&theta;})\end{array}$

当θ＝90°时， ${\stackrel{\&RightArrow;}{OP}}_1^{\&prime;\&prime;}=\left(\right(y_{P_1^{\&prime;}}-y_{P_1}),-(x_{P_1^{\&prime;}}-x_{P_1}\left)\right),$ 因此，当 $y_{P_1^{\&prime;}}-y_{P_1}>0$ 时，P₁P₂沿着X轴右移，反之左移。设梳状往复遍历方式第n条平行直线与边P₁P₂的间距为d(n)，则该平行直线与边P′₁P′₄的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_1^{\&prime;}}=k_{P_1{P}_2}(x-x_{P_1^{\&prime;}}+\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|})\\ y-y_{P_1^{\&prime;}}=k_{P_4{P}_1}(x-x_{P_1^{\&prime;}}),(x-x_{P_1^{\&prime;}})\&CenterDot;(x-x_{P_4^{\&prime;}})\&le;0\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_1{P}_4}\left(n\right)=x_{P_1^{\&prime;}}-\frac{k_{P_1{P}_2}}{k_{P_1{P}_2}-k_{P_4{P}_1}}\&CenterDot;\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|}\\ x_{P_1{P}_4}\left(n\right)=x_{P_1^{\&prime;}}-\frac{k_{P_1{P}_2}\&CenterDot;k_{P_4{P}_1}}{k_{P_1{P}_2}-k_{P_4{P}_1}}\&CenterDot;\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|}\end{array}\right.,\left(x_{P_1{P}_4}\right(n)-x_{P_1^{\&prime;}})\&CenterDot;\left(x_{P_1{P}_4}\right(n)-x_{P_4^{\&prime;}})\&le;0---(4-1)$

平行直线与边P′₂P′₃的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_2^{\&prime;}}=k_{P_1{P}_2}(x-x_{P_2^{\&prime;}}+\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|})\\ y-y_{P_2^{\&prime;}}=k_{P_2{P}_3}(x-x_{P_2^{\&prime;}}),(x-x_{P_2^{\&prime;}})\&CenterDot;(x-x_{P_3^{\&prime;}})\&le;0\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_2{P}_3}\left(n\right)=x_{P_2^{\&prime;}}-\frac{k_{P_1{P}_2}}{k_{P_1{P}_2}-k_{P_2{P}_3}}\&CenterDot;\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|}\\ x_{P_2{P}_3}\left(n\right)=x_{P_2^{\&prime;}}-\frac{k_{P_1{P}_2}\&CenterDot;k_{P_2{P}_3}}{k_{P_1{P}_2}-k_{P_2{P}_3}}\&CenterDot;\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|}\end{array}\right.,\left(x_{P_2{P}_3}\right(n)-x_{P_2^{\&prime;}})\&CenterDot;\left(x_{P_2{P}_3}\right(n)-x_{P_3^{\&prime;}})\&le;0---(4-2)$

平行直线与边P′₃P′₄的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_1^{\&prime;}}=k_{P_1{P}_2}(x-x_{P_1^{\&prime;}}+\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|})\\ y-y_{P_4^{\&prime;}}=k_{P_3{P}_4}(x-x_{P_4^{\&prime;}}),(x-x_{P_3^{\&prime;}})\&CenterDot;(x-x_{P_4^{\&prime;}})\&le;0\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_3{P}_4}\left(n\right)=\frac{1}{k_{P_1{P}_2}-k_{P_3{P}_4}}(k_{P_1{P}_2}\&CenterDot;x_{P_1^{\&prime;}}-k_{P_3{P}_4}\&CenterDot;x_{P_4^{\&prime;}}+y_{P_4^{\&prime;}}-y_{P_1^{\&prime;}}-k_{P_1{P}_2}\&CenterDot;\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|})\\ x_{P_3{P}_4}\left(n\right)=y_{P_4^{\&prime;}}+\frac{k_{P_3{P}_4}}{k_{P_1{P}_2}-k_{P_3{P}_4}}(k_{P_1{P}_2}\&CenterDot;x_{P_1^{\&prime;}}-k_{P_1{P}_2}\&CenterDot;x_{P_4^{\&prime;}}+y_{P_4^{\&prime;}}-y_{P_1^{\&prime;}}-k_{P_1{P}_2}\&CenterDot;\frac{d\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_1^{\&prime;}}}{|y_{P_1}-y_{P_1^{\&prime;}}|})\\ \left(x_{P_3{P}_4}\right(n)-x_{P_3^{\&prime;}})\&CenterDot;\left(x_{P_3{P}_4}\right(n)-x_{P_4^{\&prime;}})\&le;0\end{array}\right.---(4-3)$

由于起始投饵行程与边界P₁P₂的间距应为投饵机抛洒半径R₀，因此，第1条平行直线与边P₁P₂的间距d(1)＝R₀，假设两个相邻投饵行程的间距为d₀，则第n条平行直线与边P₁P₂的间距d(n)＝R₀+(n-1)·d₀，n∈N，代入式4-1即可得到边P′₁P′₄上的特征点坐标代入式4-2即可得到边P′₂P′₃上的特征点坐标代入式4-3即可得到边P′₃P′₄上的特征点坐标

(4-B)其次，生成梳状往复遍历方式池塘全覆盖自动投饲轨迹，梳状往复遍历方式自动投饲的轨迹采用如下格式描述：{序号、目标点坐标、航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角}，其中，航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角等参数由系统解算给出，因此，只要确定目标点的个数以及遍历顺序，就可以确定梳状往复遍历的轨迹，这里，采用几何推理法按照平行直线扫描特征点的次序来按序依次遍历，用结构体Fpoint[]来表示设定的投饲轨迹点，所述结构体包括变量：轨迹点序号intNum、目标点坐标doublepos、航行区间内的航速doublev_b、投饵机单位时间内下料量doublem_ps、抛盘转速doubleN₀、投饵扇角double结构体Fpoint[]详细如下所示，

由于边P′₁P′₄和P′₂P′₃长度大小不确定，因此取

$\begin{array}{c}{n}_{\min }=\min (\&lsqb;\frac{d_{P_4^{\&prime;}{P}_1^{\&prime;}}-R_0/{\mathrm{sin\&theta;}}_1}{d_0/{\mathrm{sin\&theta;}}_1}\&rsqb;,\&lsqb;\frac{d_{P_2^{\&prime;}{P}_3^{\&prime;}}-R_0/{\mathrm{sin\&theta;}}_2}{d_0/{\mathrm{sin\&theta;}}_2}\&rsqb;)\\ =\min (\&lsqb;\frac{\sqrt{{(x_{P_4^{\&prime;}}-x_{P_1^{\&prime;}})}^2+{(y_{P_4^{\&prime;}}-y_{P_1^{\&prime;}})}^2}-R_0/{\mathrm{sin\&theta;}}_1}{d_0/{\mathrm{sin\&theta;}}_1}\&rsqb;,\&lsqb;\frac{\sqrt{{(x_{P_2^{\&prime;}}-x_{P_3^{\&prime;}})}^2+{(y_{P_2^{\&prime;}}-y_{P_3^{\&prime;}})}^2}-R_0/{\mathrm{sin\&theta;}}_2}{d_0/{\mathrm{sin\&theta;}}_2}\&rsqb;)\end{array}$

取 $n_{\max }=\max (\&lsqb;\frac{d_{P_4^{\&prime;}{P}_1^{\&prime;}}-R_0/{\mathrm{sin\&theta;}}_1}{d_0/{\mathrm{sin\&theta;}}_1}\&rsqb;,\&lsqb;\frac{d_{P_2^{\&prime;}{P}_3^{\&prime;}}-R_0/{\mathrm{sin\&theta;}}_2}{d_0/{\mathrm{sin\&theta;}}_2}\&rsqb;)$ $=\max (\&lsqb;\frac{\sqrt{{(x_{P_4^{\&prime;}}-x_{P_1^{\&prime;}})}^2+{(y_{P_4^{\&prime;}}-y_{P_1^{\&prime;}})}^2}-R_0/{\mathrm{sin\&theta;}}_1}{d_0/{\mathrm{sin\&theta;}}_1}\&rsqb;,\&lsqb;\frac{\sqrt{{(x_{P_2^{\&prime;}}-x_{P_3^{\&prime;}})}^2+{(y_{P_2^{\&prime;}}-y_{P_3^{\&prime;}})}^2}-R_0/{\mathrm{sin\&theta;}}_2}{d_0/{\mathrm{sin\&theta;}}_2}\&rsqb;)$

其中，所述梳状往复遍历方式池塘全覆盖轨迹生成方法，包括如下步骤，如图4所示：

步骤S101，开始；

步骤S102，对n赋初始值，n＝1；

步骤S103，n对2取余运算结果是否等于1？若是，跳到步骤S105；若否，跳到步骤S104；

步骤S104，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S106；

步骤S105，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos；

步骤S106，n＝n+1，处理下一个轨迹点；

步骤S107，判断是否n≤n_min+1，若是，跳到步骤S103；若否，跳到步骤S108；

步骤S108，判断是否边P′₁P′₄的长度小于边P′₂P′₃的长度，即是否若是，跳到步骤S112；若否，跳到步骤S109；

步骤S109，n对2取余运算结果是否等于1？若是，跳到步骤S110；若否，跳到步骤S111；

步骤S110，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S111，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S112，n对2取余运算结果是否等于1？若是，跳到步骤S114；若否，跳到步骤S113；

步骤S113，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S114，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos；

步骤S115，n＝n+1，处理下一个轨迹点；

步骤S116，判断是否n≤n_max+1，若是，跳到步骤S108；若否，跳到步骤S117；

步骤S117，结束；

5.步骤S005，内螺旋遍历方式池塘全覆盖自动投饲轨迹规划，如图5所示；

(5-A)首先，确定内螺旋遍历方式池塘全覆盖自动投饲轨迹规划特征点，内螺旋遍历方式自动投饲轨迹规划特征点可以采用四条边分别向内推平行线，两相邻平移直线求交点的方法获取，为了确定与四条边分别向内推平行线的一系列直线沿着X轴平移的方向与距离，不失一般性，同理引入向量分别绕P₁、P₂、P₃、P₄顺时针旋转90°，得到向内的向量则 ${\stackrel{\&RightArrow;}{OP}}_1^{\&prime;\&prime;}=\left(\right(y_{P_2}-y_{P_1}),-(x_{P_2}-x_{P_1}\left)\right),$ ${\stackrel{\&RightArrow;}{OP}}_2^{\&prime;\&prime;}=\left(\right(y_{P_3}-y_{P_2}),-(x_{P_3}-x_{P_2}\left)\right),{\stackrel{\&RightArrow;}{OP}}_3^{\&prime;\&prime;}=\left(\right(y_{P_4}-y_{P_3}),-(x_{P_4}-x_{P_3}\left)\right),{\stackrel{\&RightArrow;}{OP}}_4^{\&prime;\&prime;}=\left(\right(y_{P_1}-y_{P_4}),-(x_{P_1}-x_{P_4}\left)\right),$ 因此，当时，与P₁P₂平行的直线沿着X轴右移，反之左移；当时，与P₂P₃平行的直线沿着X轴右移，反之左移；当时，与P₃P₄平行的直线沿着X轴右移，反之左移；当时，与P₄P₁平行的直线沿着X轴右移，反之左移。

设内螺旋遍历方式第n条平行直线与工作区域对应边界的间距为d₂(n)，则边P′₁P′₂向内的平行线与边P′₁P′₄向内的平行线的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_1^{\&prime;}}=k_{P_4{P}_1}(x-x_{P_1^{\&prime;}}+\frac{d_2(n-1)}{{\mathrm{sin\&theta;}}_{P_4{P}_1}}\&CenterDot;\frac{y_{P_4}-y_{P_1}}{|y_{P_4}-y_{P_1}|})\\ y-y_{P_1^{\&prime;}}=k_{P_1{P}_2}(x-x_{P_1^{\&prime;}}+\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_2}}{|y_{P_1}-y_{P_2}|})\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_1^{\&prime;}{P}_2^{\&prime;}}\left(n\right)=x_{P_1^{\&prime;}}-\frac{k_{P_4{P}_1}}{k_{P_4{P}_1}-k_{P_1{P}_2}}\&CenterDot;\frac{d_2(n-1)}{{\mathrm{sin\&theta;}}_{P_4{P}_1}}\&CenterDot;\frac{y_{P_4}-y_{P_1}}{|y_{P_4}-y_{P_1}|}+\frac{k_{P_1{P}_2}}{k_{P_4{P}_1}-k_{P_1{P}_2}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_2}}{|y_{P_1}-y_{P_2}|}\\ y_{P_1^{\&prime;}{P}_2^{\&prime;}}\left(n\right)=y_{P_1^{\&prime;}}-\frac{k_{P_4{P}_1}\&CenterDot;k_{P_1{P}_2}}{k_{P_4{P}_1}-k_{P_1{P}_2}}\&CenterDot;(\frac{d_2(n-1)}{{\mathrm{sin\&theta;}}_{P_4{P}_1}}\&CenterDot;\frac{y_{P_4}-y_{P_1}}{|y_{P_4}-y_{P_1}|}-\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_2}}{|y_{P_1}-y_{P_2}|})\end{array}\right.$

边P′₂P′₃向内的平行线与边P′₁P′₂向内的平行线的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_2^{\&prime;}}=k_{P_1{P}_2}(x-x_{P_2^{\&prime;}}+\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_2}}{|y_{P_1}-y_{P_2}|})\\ y-y_{P_2^{\&prime;}}=k_{P_2{P}_3}(x-x_{P_2^{\&prime;}}+\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_2{P}_3}}\&CenterDot;\frac{y_{P_2}-y_{P_3}}{|y_{P_2}-y_{P_3}|})\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_2^{\&prime;}{P}_3^{\&prime;}}\left(n\right)=x_{P_2^{\&prime;}}-\frac{k_{P_1{P}_2}}{k_{P_1{P}_2}-k_{P_2{P}_3}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_2}}{|y_{P_1}-y_{P_2}|}+\frac{k_{P_2{P}_3}}{k_{P_1{P}_2}-k_{P_2{P}_3}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_2{P}_3}}\&CenterDot;\frac{y_{P_2}-y_{P_3}}{|y_{P_2}-y_{P_3}|}\\ y_{P_2^{\&prime;}{P}_3^{\&prime;}}\left(n\right)=y_{P_2^{\&prime;}}-\frac{k_{P_1{P}_2}\&CenterDot;k_{P_2{P}_3}}{k_{P_1{P}_2}-k_{P_2{P}_3}}\&CenterDot;(\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_1{P}_2}}\&CenterDot;\frac{y_{P_1}-y_{P_2}}{|y_{P_1}-y_{P_2}|}-\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_2{P}_3}}\&CenterDot;\frac{y_{P_2}-y_{P_3}}{|y_{P_2}-y_{P_3}|})\end{array}\right.$

边P′₃P′₄向内的平行线与边P′₂P′₃向内的平行线的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_3^{\&prime;}}=k_{P_2{P}_3}(x-x_{P_3^{\&prime;}}+\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_2{P}_3}}\&CenterDot;\frac{y_{P_2}-y_{P_3}}{|y_{P_2}-y_{P_3}|})\\ y-y_{P_3^{\&prime;}}=k_{P_3{P}_4}(x-x_{P_3^{\&prime;}}+\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_3{P}_4}}\&CenterDot;\frac{y_{P_3}-y_{P_4}}{|y_{P_3}-y_{P_4}|})\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_3^{\&prime;}{P}_4^{\&prime;}}\left(n\right)=x_{P_3^{\&prime;}}-\frac{k_{P_2{P}_3}}{k_{P_2{P}_3}-k_{P_3{P}_4}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_2{P}_3}}\&CenterDot;\frac{y_{P_2}-y_{P_3}}{|y_{P_2}-y_{P_3}|}+\frac{k_{P_3{P}_4}}{k_{P_2{P}_3}-k_{P_3{P}_4}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_3{P}_4}}\&CenterDot;\frac{y_{P_3}-y_{P_4}}{|y_{P_3}-y_{P_4}|}\\ y_{P_3^{\&prime;}{P}_4^{\&prime;}}\left(n\right)=y_{P_3^{\&prime;}}-\frac{k_{P_2{P}_3}\&CenterDot;k_{P_3{P}_4}}{k_{P_2{P}_3}-k_{P_3{P}_4}}\&CenterDot;(\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_2{P}_3}}\&CenterDot;\frac{y_{P_2}-y_{P_3}}{|y_{P_2}-y_{P_3}|}-\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_3{P}_4}}\&CenterDot;\frac{y_{P_3}-y_{P_4}}{|y_{P_3}-y_{P_4}|})\end{array}\right.$

边P′₄P′₁向内的平行线与边P′₃P′₄向内的平行线的交点坐标可通过如下方程组求解，

$\left\{\begin{array}{c}y-y_{P_4^{\&prime;}}=k_{P_3{P}_4}(x-x_{P_4^{\&prime;}}+\frac{d_2(n-1)}{{\mathrm{sin\&theta;}}_{P_3{P}_4}}\&CenterDot;\frac{y_{P_3}-y_{P_4}}{|y_{P_3}-y_{P_4}|})\\ y-y_{P_4^{\&prime;}}=k_{P_4{P}_1}(x-x_{P_4^{\&prime;}}+\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_4{P}_1}}\&CenterDot;\frac{y_{P_4}-y_{P_1}}{|y_{P_4}-y_{P_1}|})\end{array}\right.$

解之得：

$\left\{\begin{array}{c}{x}_{P_4^{\&prime;}{P}_1^{\&prime;}}\left(n\right)=x_{P_4^{\&prime;}}-\frac{k_{P_3{P}_4}}{k_{P_3{P}_4}-k_{P_4{P}_1}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_3{P}_4}}\&CenterDot;\frac{y_{P_3}-y_{P_4}}{|y_{P_3}-y_{P_4}|}+\frac{k_{P_4{P}_1}}{k_{P_3{P}_4}-k_{P_4{P}_1}}\&CenterDot;\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_4{P}_1}}\&CenterDot;\frac{y_{P_4}-y_{P_1}}{|y_{P_4}-y_{P_1}|}\\ y_{P_4^{\&prime;}{P}_1^{\&prime;}}\left(n\right)=y_{P_4^{\&prime;}}-\frac{k_{P_3{P}_4}\&CenterDot;k_{P_4{P}_1}}{k_{P_3{P}_4}-k_{P_4{P}_1}}\&CenterDot;(\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_3{P}_4}}\&CenterDot;\frac{y_{P_3}-y_{P_4}}{|y_{P_3}-y_{P_4}|}-\frac{d_2\left(n\right)}{{\mathrm{sin\&theta;}}_{P_4{P}_1}}\&CenterDot;\frac{y_{P_4}-y_{P_1}}{|y_{P_4}-y_{P_1}|})\end{array}\right.$

由于起始投饵行程与工作区域对应边界的间距应为投饵机抛洒半径R₀，因此，第1条平行直线与工作区域对应边界的间距d₂(1)＝R₀，令d₂(0)＝0，假设两个相邻投饵行程的间距为d₀，则第n条平行直线与工作区域对应边界的间距d₂(n)＝R₀+(n-1)·d₀，n∈N，分别代入上述4式即可得到轨迹特征点坐标

(5-B)其次，生成内螺旋遍历方式池塘全覆盖自动投饲轨迹，内螺旋遍历方式自动投饲的轨迹采用如下格式描述：{序号目标点坐标航行至目标点的航速投饵机单位时间内下料量抛盘转速抛射幅角}，其中，航行至目标点的航速、投饵机单位时间内下料量、抛盘转速、抛射幅角等参数由系统解算给出，因此，只要确定目标点的个数以及遍历顺序，就可以确定内螺旋遍历方式的轨迹，这里，按照 来顺序依次遍历，由于工作区域四边形的形状不确定，因此必须确定内螺旋遍历的圈数，由于等距内螺旋遍历方式轨迹特征点在各内角的角平分线上，因此，分别令为内角θ₁和θ₂角平分线交点到边P′₁P′₂的距离，为内角θ₂和θ₃角平分线交点到边的距离，为内角θ₃和θ₄角平分线交点到边P′₃P′₄的距离，为内角θ₄和θ₁角平分线交点到边P′₄P′₁的距离，则

$h_{P_1^{\&prime;}{P}_2^{\&prime;}}=\frac{d_{P_1^{\&prime;}{P}_2^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_1}{2}+\cot \frac{{\mathrm{\&theta;}}_2}{2}},h_{P_2^{\&prime;}{P}_3^{\&prime;}}=\frac{d_{P_2^{\&prime;}{P}_3^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_2}{2}+\cot \frac{{\mathrm{\&theta;}}_3}{2}},h_{P_3^{\&prime;}{P}_4^{\&prime;}}=\frac{d_{P_3^{\&prime;}{P}_4^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_3}{2}+\cot \frac{{\mathrm{\&theta;}}_4}{2}},h_{P_4^{\&prime;}{P}_1^{\&prime;}}=\frac{d_{P_4^{\&prime;}{P}_1^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_4}{2}+\cot \frac{{\mathrm{\&theta;}}_1}{2}},$ 式中，分别指的是工作区域四条边的边长，则 $n_{\max }^{\&prime;}=\&lsqb;\frac{min(h_{P_1^{\&prime;}{P}_2^{\&prime;}},h_{P_2^{\&prime;}{P}_3^{\&prime;}},h_{P_3^{\&prime;}{P}_4^{\&prime;}},h_{P_4^{\&prime;}{P}_1^{\&prime;}})-R_0}{d_0}\&rsqb;.$ 因此，只要按照 (n＝1，2，...，n_max+1)来顺序依次遍历即可进行内螺旋遍历方式自动投饲的轨迹坐标点构建。

以下结合附图，对本发明一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图6和图7所示，为本发明的具体实施例。

测试池塘：(1)池塘I的四个顶点P₁、P₂、P₃和P₄的经纬度坐标分别为(119.51065753，31.54744914)，(119.51392136，31.54601313)，(119.51284982，31.54479897)，(119.51015859，31.54629197)，对基于GPS导航的自动均匀投饲系统，系统接收到的定位数据是WGS-84坐标系下的大地经纬度坐标，这是一种椭球面坐标，无法直接用于平面路径规划分析与计算，因此，必须对接收到的GPS大地经纬度坐标利用高斯-克吕格投影到高斯平面上，并以笛卡尔直角坐标系来表示。采用6°带高斯投影正算公式计算出四个顶点的高斯-克吕格平面坐标分别为(3494406.59，20738432.55)m，(3494413.70，20738742.60)m，(3494117.39，20738647.55)m，(3494277.14，20738388.09)m，四个顶点的安全距离d_s＝4m，投饵机抛洒半径R₀＝8m，两个相邻投饵行程的间距为d₀＝12m，该池塘形状较为规则，采用内螺旋遍历方式池塘全覆盖自动投饲轨迹规划方法，如图6所示，求解出内螺旋遍历方式自动投饲轨迹规划特征点28个，通过高斯-克吕格投影反算公式，将特征点的高斯平面坐标转换为经纬度坐标，生成内螺旋遍历方式池塘全覆盖自动投饲轨迹结构体Fpoint[]，此时，路径总长度为5239.74m，转弯数28个，覆盖率为90.10％；

(2)池塘II的四个顶点P₁、P₂、P₃和P₄的经纬度坐标分别为(119.51497756，31.54474264)，(119.51657131，31.54661328)，(119.51785839，31.54525094)，(119.51573338，31.5443866)，采用6°带高斯投影正算公式计算出四个顶点的高斯-克吕格平面坐标分别为(3494115.78，20738849.83)m，(3494326.77，20738996.46)m，(3494178.46，20739122.21)m，(3494077.94，20738922.54)m，四个顶点的安全距离d_s＝4m，投饵机抛洒半径R₀＝8m，两个相邻投饵行程的间距为d₀＝12m，该池塘形状不规则，采用梳状往复遍历方式池塘全覆盖自动投饲轨迹规划方法，如图7所示，求解出梳状往复遍历方式自动投饲轨迹规划特征点35个，通过高斯-克吕格投影反算公式，将特征点的高斯平面坐标转换为经纬度坐标，生成梳状往复遍历方式池塘全覆盖自动投饲轨迹结构体Fpoint[]，此时，路径总长度为3669.45m，转弯数35个，覆盖率为92.11％。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，其特征在于，包括以下步骤：

步骤S001，建立池塘区域坐标系：设取池塘区域可以为非规则的四边形，四边形池塘的四个顶点分别为P₁、P₂、P₃和P₄，四个顶点坐标分别为和分别求取四条边P₁P₂、P₂P₃、P₃P₄以及P₄P₁的斜率、对应的四条边的倾斜角、池塘的四个顶角；

步骤S002，确定自动投饲工作区域：投饵系统在作业水域行驶路径可分解分为直线和转弯行驶两个基本运动，设置转弯时船体与岸边界的安全距离，进行全覆盖遍历方式投饵；

步骤S003，判断自动投饲工作区域形状是否规则，若是，采用内螺旋遍历方式池塘全覆盖自动投饲轨迹规划方法，跳到步骤S005；若否，采用梳状往复遍历方式池塘全覆盖自动投饲轨迹规划方法，跳到步骤S004；

步骤S004，梳状往复遍历方式池塘全覆盖自动投饲轨迹规划：确定梳状往复遍历方式自动投饲轨迹规划特征点，生成梳状往复遍历方式池塘全覆盖自动投饲轨迹；

步骤S005，内螺旋遍历方式池塘全覆盖自动投饲轨迹规划：确定内螺旋遍历方式池塘全覆盖自动投饲轨迹规划特征点，生成内螺旋遍历方式池塘全覆盖自动投饲轨迹。

2.根据权利要求1所述的一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，其特征在于，所述步骤S004的具体过程为：

步骤S0041，确定梳状往复遍历方式自动投饲轨迹规划特征点：假设沿着长边P₁P₂方向进行梳状往复遍历方式投饵，只要作出与P₁P₂平行的一系列直线，求出平行直线与作业区域边界的交点，可以把P₁P₂沿着X轴平移就可以得出这些直线与作业区域边界的交点，把这些交点作为直线路径的起止点即投饵作业的路径轨迹特征点，为了确定P₁P₂向内推平行线的一系列直线沿着X轴平移的方向与距离，引入向内的向量由于 $(x_{P_1^{\&prime;}}-x_{P_1},y_{P_1^{\&prime;}}-y_{P_1}),$ 向量绕P₁顺时针旋转θ，得到向量则向量因此，当时，P₁P₂沿着X轴右移，反之左移，分别求解出该平行直线与边P′₁P′₄的交点坐标与边P′₂P′₃的交点坐标以及与边P′₃P′₄的交点坐标作为梳状往复遍历方式自动投饲轨迹规划特征点；

步骤S0042，生成梳状往复遍历方式池塘全覆盖自动投饲轨迹：梳状往复遍历方式自动投饲的轨迹采用如下格式描述：{序号、目标点坐标、航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角}，其中，航行区间内的航速、喂料器单位时间内落料量、抛盘转速、投饵扇角等参数由系统解算给出，因此，只要确定目标点的个数以及遍历顺序，就可以确定梳状往复遍历的轨迹。

3.根据权利要求2所述的一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，其特征在于，所述步骤S0042中，采用几何推理法按照平行直线扫描特征点的次序来按序依次遍历，用结构体Fpoint[]来表示设定的投饲轨迹点，所述结构体包括变量：轨迹点序号intNum、目标点坐标doublepos、航行区间内的航速doublev_b、投饵机单位时间内下料量doublem_ps、抛盘转速doubleN₀、投饵扇角

4.根据权利要求3所述的一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，其特征在于，所述步骤S0042的具体实现过程包括：

步骤S101，开始；

步骤S102，对n赋初始值，n＝1；

步骤S103，n对2取余运算结果是否等于1？若是，跳到步骤S105；若否，跳到步骤S104；

步骤S104，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S106；

步骤S105，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos；

步骤S106，n＝n+1，处理下一个轨迹点；

步骤S107，判断是否n≤n_min+1，若是，跳到步骤S103；若否，跳到步骤S108；

步骤S108，判断是否边P′₁P′₄的长度小于边P′₂P′₃的长度，即是否若是，跳到步骤S112；若否，跳到步骤S109；

步骤S109，n对2取余运算结果是否等于1？若是，跳到步骤S110；若否，跳到步骤S111；

步骤S110，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S111，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S112，n对2取余运算结果是否等于1？若是，跳到步骤S114；若否，跳到步骤S113；

步骤S113，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos，跳到步骤S115；

步骤S114，遍历顺序是从点到点将坐标赋给Fpoint[2n-1].pos，将坐标赋给Fpoint[2n].pos；

步骤S115，n＝n+1，处理下一个轨迹点；

步骤S116，判断是否n≤n_max+1，若是，跳到步骤S108；若否，跳到步骤S117；

步骤S117，结束。

5.根据权利要求1所述的一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，其特征在于，所述步骤S005的具体过程为：

步骤S0051，确定内螺旋遍历方式池塘全覆盖自动投饲轨迹规划特征点，内螺旋遍历方式自动投饲轨迹规划特征点可以采用四条边分别向内推平行线，两相邻平移直线求交点的方法获取；

步骤S0052，生成内螺旋遍历方式池塘全覆盖自动投饲轨迹，内螺旋遍历方式自动投饲的轨迹采用如下格式描述：{序号目标点坐标航行至目标点的航速投饵机单位时间内下料量抛盘转速抛射幅角}，其中，航行至目标点的航速、投饵机单位时间内下料量、抛盘转速、抛射幅角等参数由系统解算给出，由于工作区域四边形的形状不确定，因此必须确定内螺旋遍历的圈数，由于等距内螺旋遍历方式轨迹特征点在各内角的角平分线上，因此，分别令为内角θ₁和θ₂角平分线交点到边P′₁P′₂的距离，为内角θ₂和θ₃角平分线交点到边P′₂P′₃的距离，为内角θ₃和θ₄角平分线交点到边P′₃P′₄的距离，为内角θ₄和θ₁角平分线交点到边P′₄P′₁的距离，则

$h_{P_1^{\&prime;}{P}_2^{\&prime;}}=\frac{d_{P_1^{\&prime;}{P}_2^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_1}{2}+\cot \frac{{\mathrm{\&theta;}}_2}{2}},h_{P_2^{\&prime;}{P}_3^{\&prime;}}=\frac{d_{P_2^{\&prime;}{P}_3^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_2}{2}+\cot \frac{{\mathrm{\&theta;}}_3}{2}},h_{P_3^{\&prime;}{P}_4^{\&prime;}}=\frac{d_{P_3^{\&prime;}{P}_4^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_3}{2}+\cot \frac{{\mathrm{\&theta;}}_4}{2}},h_{P_4^{\&prime;}{P}_1^{\&prime;}}=\frac{d_{P_4^{\&prime;}{P}_1^{\&prime;}}}{\cot \frac{{\mathrm{\&theta;}}_4}{2}+\cot \frac{{\mathrm{\&theta;}}_1}{2}},$ 式中，分别指的是工作区域四条边的边长，则内螺旋遍历的圈数式中R₀为投饵机抛洒半径，因此，只要按照 $\left(x_{P_1^{\&prime;}{P}_2^{\&prime;}}\right(n),y_{P_1^{\&prime;}{P}_2^{\&prime;}}(n\left)\right),\left(x_{P_2^{\&prime;}{P}_3^{\&prime;}}\right(n),y_{P_2^{\&prime;}{P}_3^{\&prime;}}(n\left)\right),\left(x_{P_3^{\&prime;}{P}_4^{\&prime;}}\right(n),y_{P_3^{\&prime;}{P}_4^{\&prime;}}(n\left)\right),\left(x_{P_4^{\&prime;}{P}_1^{\&prime;}}\right(n),y_{P_4^{\&prime;}{P}_1^{\&prime;}}(n\left)\right),(n=1,2,...,n_{\max }+1)$ 来顺序依次遍历即可进行内螺旋遍历方式自动投饲的轨迹坐标点构建。

6.根据权利要求5所述的一种水产养殖池塘全覆盖自动均匀投饲轨迹规划方法，其特征在于，所述步骤S0051的具体实现过程为：引入向量分别绕P₁、P₂、P₃、P₄顺时针旋转90°，得到向内的向量则 因此，当时，与P₁P₂平行的直线沿着X轴右移，反之左移；当时，与P₂P₃平行的直线沿着X轴右移，反之左移；当时，与P₃P₄平行的直线沿着X轴右移，反之左移；当时，与P₄P₁平行的直线沿着X轴右移，反之左移；分别求解出边P′₁P′₂向内的平行线与边P′₁P′₄向内的平行线的交点坐标边P′₂P′₃向内的平行线与边P′₁P′₂向内的平行线的交点坐标边P′₃P′₄向内的平行线与边P′₂P′₃向内的平行线的交点坐标以及边P′₄P′₁向内的平行线与边P′₃P′₄向内的平行线的交点坐标作为内螺旋遍历方式自动投饲轨迹规划特征点。