1.一种农业机械自动导航路径规划方法,其特征在于,包括:
由每行作业的起点、每行作业的终点和作业幅宽确定多条作业路径;
获取农业机械的当前作业方向和当前位置;
计算农业机械的当前位置与当前作业路径的偏移量;
由所述偏移量判断所述农业机械在当前位置是否偏离当前作业路径,得到路径偏移判断结果,并根据所述路径偏移判断结果调整农业机械的行进方向;
由当前作业路径、当前距离和当前夹角对行进方向调整后的农业机械进行地头检测,得到地头检测结果;所述当前距离为当前位置与当前作业路径的终点之间的距离;所述当前夹角为当前农机方向与当前作业方向之间的夹角;所述当前农机方向为当前位置与当前作业路径的终点之间的连线的方向;
由所述地头检测结果确定下一时刻的作业路径,并将下一时刻的作业路径对应的作业方向作为当前作业方向,将下一时刻的作业路径对应的位置作为当前位置,返回获取农业机械的当前作业方向和当前位置的步骤,直至作业完成。
2.根据权利要求1所述的一种农业机械自动导航路径规划方法,其特征在于,若农业机械运行过程中作业中断,则执行重连过程;所述重连过程包括:计算第一距离;所述第一距离为作业中断后所处的位置与初始设定路径之间的距离;
将所述第一距离与所述作业幅宽的商值确定为目标作业行;
由所述目标作业行、所述第一距离和所述作业幅宽计算第二距离;所述第二距离为作业中断后所处的位置与目标作业行之间的距离;
若所述第二距离小于或等于所述作业幅宽的一半,则将所述目标作业行确定为当前作业路径,并在农业机械行驶至当前作业路径时,根据当前夹角确定当前作业方向,再返回获取农业机械的当前作业方向和当前位置的步骤,直至作业完成;
若所述第二距离大于所述作业幅宽的一半,则将所述目标作业行的下一行确定为当前作业路径,并在农业机械行驶至当前作业路径时,根据当前夹角确定当前作业方向,再返回获取农业机械的当前作业方向和当前位置的步骤,直至作业完成。
3.根据权利要求1所述的一种农业机械自动导航路径规划方法,其特征在于,所述计算农业机械的当前位置与当前作业路径的偏移量,具体包括:确定当前作业路径的直线方程Ax+By+C=0;其中,A和B为直线方程的系数,C为直线方程的常数项,(x,y)为当前作业路径上的点的坐标;
由当前位置(m,n)和所述当前作业路径的直线方程计算偏移量其中,m为当前位置的横坐标,n为当前位置的纵坐标。
4.根据权利要求1所述的一种农业机械自动导航路径规划方法,其特征在于,所述由所述偏移量判断所述农业机械在当前位置是否偏离当前作业路径,得到路径偏移判断结果,并根据所述路径偏移判断结果调整农业机械的行进方向,具体包括:判断所述偏移量是否大于最大偏移量;
若所述偏移量大于最大偏移量,则调整农业机械的行进方向;
若所述偏移量小于或等于最大偏移量,则保持农业机械的行进方向。
5.根据权利要求1所述的一种农业机械自动导航路径规划方法,其特征在于,所述由当前作业路径、当前距离和当前夹角对行进方向调整后的农业机械进行地头检测,得到地头检测结果,具体包括:确定当前作业路径的直线方程Ax+By+C=0;其中,A和B为直线方程的系数,C为直线方程的常数,(x,y)为当前作业路径上的点的坐标;
确定第一垂直直线的直线方程Bx-Ay+DA=0和第二垂直直线的直线方程Bx-Ay+DB=0;
所述第一垂直直线为过当前作业路径的起点且与所述当前作业路径垂直的直线;所述第二垂直直线为过当前作业路径的终点且与所述当前作业路径垂直的直线;其中,DA为第一垂直直线方程的常数项,DB为第二垂直直线方程的常数项;
判断当前位置的坐标是否位于所述第一垂直直线与所述第二垂直直线之间,得到第一判断结果;
若所述第一判断结果为是,则确定农业机械未越界;若所述第一判断结果为否,则确定农业机械越界;
判断当前距离是否小于地头警戒值,得到第二判断结果;
若所述第二判断结果为是,则确定农业机械到达边界;若所述第二判断结果为否,则确定农业机械未到达边界;
判断当前夹角是否大于90度,到第三判断结果;
若所述第三判断结果为是,则确定农业机械已经超过终点;若所述第三判断结果为否,则确定农业机械未超过终点。
6.根据权利要求5所述的一种农业机械自动导航路径规划方法,其特征在于,由所述地头检测结果确定下一时刻的作业路径,具体包括:若所述地头检测结果为当前位置的坐标未位于所述第一垂直直线与所述第二垂直直线之间、当前距离小于地头警戒值或者当前夹角大于90度,则控制农业机械掉头,生成下一时刻的作业路径其中,n表示作业的总行数,W为作业幅宽。
7.根据权利要求2所述的一种农业机械自动导航路径规划方法,其特征在于,所述由所述目标作业行、所述第一距离和所述作业幅宽计算第二距离,具体为:f=Cp-N*W;
其中,f为第二距离,Cp为第一距离,N为目标作业行,W为作业幅宽。
8.一种农业机械自动导航路径规划系统,其特征在于,包括:
作业路径确定模块,用于由每行作业的起点、每行作业的终点和作业幅宽确定多条作业路径;
数据获取模块,用于获取农业机械的当前作业方向和当前位置;
偏移量计算模块,用于计算农业机械的当前位置与当前作业路径的偏移量;
偏移判断模块,用于由所述偏移量判断所述农业机械在当前位置是否偏离当前作业路径,得到路径偏移判断结果,并根据所述路径偏移判断结果调整农业机械的行进方向;
地头检测模块,用于由当前作业路径、当前距离和当前夹角对行进方向调整后的农业机械进行地头检测,得到地头检测结果;所述当前距离为当前位置与当前作业路径的终点之间的距离;所述当前夹角为当前农机方向与当前作业方向之间的夹角;所述当前农机方向为当前位置与当前作业路径的终点之间的连线的方向;
路径更新模块,用于由所述地头检测结果确定下一时刻的作业路径,并将下一时刻的作业路径对应的作业方向作为当前作业方向,将下一时刻的作业路径对应的位置作为当前位置,返回获取农业机械的当前作业方向和当前位置的步骤,直至作业完成。
9.根据权利要求8所述的一种农业机械自动导航路径规划系统,其特征在于,若农业机械运行过程中作业中断,则执行重连模块;所述重连模块包括:第一计算单元,用于计算第一距离;所述第一距离为作业中断后所处的位置与初始设定路径之间的距离;
目标行确定单元,用于将所述第一距离与所述作业幅宽的商值确定为目标作业行;
第二计算单元,用于由所述目标作业行、所述第一距离和所述作业幅宽计算第二距离;
所述第二距离为作业中断后所处的位置与目标作业行之间的距离;
路径确定单元,用于若所述第二距离小于或等于所述作业幅宽的一半,则将所述目标作业行确定为当前作业路径,并在农业机械行驶至当前作业路径时,根据当前夹角确定当前作业方向,再返回获取农业机械的当前作业方向和当前位置的步骤,直至作业完成;若所述第二距离大于所述作业幅宽的一半,则将所述目标作业行的下一行确定为当前作业路径,并在农业机械行驶至当前作业路径时,根据当前夹角确定当前作业方向,再返回获取农业机械的当前作业方向和当前位置的步骤,直至作业完成。
10.根据权利要求8所述的一种农业机械自动导航路径规划系统,其特征在于,所述偏移量计算模块,具体包括:直线方程确定单元,用于确定当前作业路径的直线方程Ax+By+C=0;其中,A和B为直线方程的系数,C为直线方程的常数项,(x,y)为当前作业路径上的点的坐标;
偏移量计算单元,用于由当前位置(m,n)和所述当前作业路径的直线方程计算偏移量其中,m为当前位置的横坐标,n为当前位置的纵坐标。