[发明专利]一种基于作业行的自主导航作业路径规划方法

权利要求书

1.一种基于作业行的自主导航作业路径规划方法，包括：

步骤A-数据采集：获取路径规划相关作业地块的坐标数据、作业车辆的位置偏差L、车身宽度W，设定作业行间隔距离D；

所述作业地块为四边形地块，所述坐标数据采集顺序为：点₁、点₂、点₃、点₄分别为作业地块的四个顶点，按照点₁→点₂→点₃→点₄的顺序方向完成作业地块四个顶点的坐标数据采集，边₁₂为作业地块最长的边，边₁₂平行于边₃₄；

所述坐标数据为平面坐标，点₁的坐标数据为(x₁,y₁)，点₂的坐标数据为(x₂,y₂)，点3的坐标数据为(x₃,y₃)，点4的坐标数据为(x₄,y₄)；

所述作业车辆位置偏差L为车载卫星定位点距作业机具末端的长度；

所述作业车辆宽度W为作业机具的作业宽度；

所述作业行间隔距离D≥车辆宽度W；

步骤B-生成覆盖作业地块的作业行，包括：

步骤B1：边₁₄上要求的目标点为P₁(x_p1,y_p1)，边₁₄上所有目标点集为作业行端点集合G₁；边₂₃上要求的目标点为P₂(x_p2,y_p2)，边₂₃上所有目标点集为作业行端点集合G₂；

初始化集合G₁、集合G₂为空集，设定h为点₁到作业行的垂直距离，初始状态h＝D/2；

利用公式(1)计算边₁₄与边₁₂的夹角θ₁，利用公式(2)计算边₂₃与边₁₂的夹角θ₂；

利用公式(3)计算点₁、点₄间的距离d₁₄，利用公式(4)计算点₂、点₃间的距离d₂₃；

比较h与d₁₄×sinθ₁的大小，若h大于d₁₄×sinθ₁则退出运算，否则进入步骤B2；

步骤B2：利用公式(5)、公式(6)计算P₁坐标x_p1，y_p1，利用公式(7)、公式(8)计算P₂坐标x_p2，y_p2；

将计算所得P₁坐标值追加至集合G₁中，即G₁＝{(x_p1,y_p1)}，将计算所得P₂坐标值追加至集合G₂中，即G₂＝{(x_p2,y_p2)}，集合G₁、集合G₂中点考虑追加点的次序性，即先增加的点在前，后增加的点在后；

步骤B3：令h值增加D，判断P₁是否超出边₃₄，即比较h与d₁₄×sinθ₁的大小，若h大于d₁₄×sinθ₁则退出运算，集合G₁、集合G₂即为最终求得的作业行端点，否则进入B2继续运算；

集合G₁数据格式为{(G1X₁,G1Y₁),(G1X₂,G1Y₂),……}，集合G₂数据格式为{(G2X₁,G2Y₁),(G2X₂,G2Y₂),……}；

步骤B4：集合G₁、G₂中对应次序的点组成的线段为作业行，即G₁中第1个点与G₂中第1个点组成的线段为作业地块的第1个作业行，后续作业行均依据此对应关系；

步骤C-计算入行点、出行点，包括：

步骤C1：对集合G₁、G₂数据按作业装备沿作业行的工作方向，增加属性值：入行点为1，出行点为0，划分入行点、出行点，具体划分方法为：

步骤C1-1：遍历集合G1，并统计集合G1中作业行端点的个数N，取m＝1，m值对应集合G₁、集合G₂中第m个作业行端点；

步骤C1-2：计算m除以2的余数，即m％2值：

若m％2等于0，则集合G₁第m个作业行端点增加属性为0，集合G₂第m个作业行端点增加属性为1；

若m％2等于1，则集合G₁第m个作业行端点增加属性为1，集合G₂第m个作业行端点增加属性为0；

步骤C1-3：m值增加1，判断m与N的大小，若m大于N，则计算完成，否则，则跳转至步骤C1-2继续执行；计算完成后，集合G₁和集合G₂中每个数组的最后一位均为属性值，即集合G₁变为：{(G1X₁,G1Y₁,1),(G1X₂,G1Y₂,0),(G1X₃,G1Y₃,1),……}，集合G₂变为：{(G2X₁,G2Y₁,0),(G2X₂,G2Y₂,1),(G2X₃,G2Y₃,0),……}；

步骤C2：计算入行点、出行点位置坐标，设置车载北斗卫星定位设备输出位置点与机具工作点间距L，出行点为作业行延长线距作业行端点L处，入行点为作业内距作业行端点L处，具体计算方法为：

步骤C2-1：初始化入行点集合R、出行点集合C，集合R与集合C均为有序集合，按坐标数值追加先后顺序排列，取n值为1，n用于遍历集合G₁；

步骤C2-2：利用公式(9)计算作业行的长度d，即两个作业端点间的距离；

步骤C2-3：判断集合G₁中第n个数据中属性值若为1：则利用公式(10)及公式(11)分别求解入行点坐标q₁(x_q1,y_q1)，并将计算所得坐标内添加属性值1，变为(x_q1,y_q1,1)，追加至集合R中，再利用公式(12)及公式(13)求解出行点坐标q₂(x_q2，y_q2)，并将计算所得坐标内添加属性值0，变为(x_q2，y_q2，0)，追加至集合C中；

x_q1＝(L×(G1X_n-G2X_n))/d+G1X_n……………………公式(10)

y_q1＝(L×(G1Y_n-G2Y_n))/d+G1Y_n……………………公式(11)

x_q2＝(L×(G1X_n-G2X_n))/d+G2X_n……………………公式(12)

y_q2＝(L×(G1Y_n-G2Y_n))/d+G2Y_n……………………公式(13)

步骤C2-4：n值增加1，比较n与N大小，若n值大于N则计算结束，集合R为所有入行点集，暂记作R：{(x_r1，y_r1，1)，(x_r2，y_r2，1)……}，集合C为所有出行点集，暂记作C：{(x_c1，y_c1，0)，(x_c2，y_c2，0)……}，若n值小于N则跳转至步骤C2-2继续运算；

步骤D-生成作业路径，包括：

步骤D1：取k＝1，用于遍历集合R，初始化路径集合Road，Road为有序集合，按坐标数值追加先后顺序排列；

步骤D2：从集合G₁中取第k个数据追加至集合Road中，从集合R中取第k个数据追加至集合Road中，从集合G₂中取第k个数据追加至集合Road中，从集合C中取第k个数据追加至集合Road中；

步骤D3：k值增加1，比较k值与N值大小，若k值大于N，则作业路径Road计算完成，其数据为Road：{(G1X₁，G1Y₁，1)，(x_r1，y_r1，1)，(G2X₁，G2Y₁，0)，(x_c1，y_c1，0)，……，(G1X_N，G1Y_N，1)，(x_rN，y_rN，1)，(G2X_N，G2Y_N，0)，(x_cN，y_cN，0)}否则，则跳转至步骤D2继续运算。