CN112904867B - 用于宽幅作业的农机无人驾驶地头转弯控制方法及系统 - Google Patents

Abstract

本发明涉及一种用于宽幅作业的农机无人驾驶地头转弯控制方法及系统，设置农机的作业参数，判断农机到达地头后使农机按照转弯方向和转弯角度开始进行地头转弯，实时获取农机的第一夹角，当第一夹角大于或等于夹角阈值时，使农机开始进行地头直线行驶，确定地头直线行驶的转出点，当判断农机到达转出点时，农机再次开始进行地头转弯，实时获取农机的第二夹角，当第二夹角大于或等于夹角阈值时，农机到达地头出弯点，完成地头转弯。本发明自动判断入弯点与出弯点以及地头直线行驶段的起点和终点，使农机在每一条作业路径间的作业幅宽均相同，自动驾驶机械在区间内往复的自动化作业，减少作业覆盖的重叠和遗漏，在减少作业时间的同时提高作业效率。

Description

用于宽幅作业的农机无人驾驶地头转弯控制方法及系统

技术领域

本发明涉及农业机械自动控制领域，特别是涉及一种用于宽幅作业的农机无人驾驶地头转弯控制方法及系统。

背景技术

农业机械的自动导航是现代农业机械自动化和智能化的关键技术之一。在农业机械导航过程中，地头转弯是农机作业中不可或缺的一部分。目前市面上所使用的地头转弯控制方法，主要以预先设定好的路径为主，而农机在按照预先设定好的路径进行作业时，由于农田土地松软、潮湿，导致农机在地头转弯时发生打滑和侧滑，使农机偏离预先设定好的路径，造成作业覆盖的重叠和遗漏，增加作业时间和降低作业效率。

因此，为了提高作业效率及减少作业时间，提供一种宽幅农机作业时自主的实时生成路径方法是目前亟待解决的技术问题。

发明内容

本发明的目的是提供一种用于宽幅作业的农机无人驾驶地头转弯控制方法及系统，以实现宽幅农机在地头转弯时自主生成实时地头转弯路径，使每一条作业路径间的作业幅宽均相同，减少作业覆盖的重叠和遗漏，在减少作业时间的同时提高作业效率。

为实现上述目的，本发明提供了如下方案：

一种用于宽幅作业的农机无人驾驶地头转弯控制方法，所述方法包括：

设置农机的作业参数；所述作业参数包括：单行直线作业的起点和终点、作业幅宽、转弯方向和转弯角度；

所述农机从单行直线作业的起点开始沿着起点到终点的作业方向进行单行直线作业；

判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果；

若所述第一判断结果表示所述农机越过单行直线作业的终点到达地头，则使所述农机按照转弯方向和转弯角度开始进行地头转弯；

实时获取所述农机按照转弯方向和转弯角度进行地头转弯过程中，农机当前方向与所述作业方向之间的夹角，确定为第一夹角；

当所述第一夹角大于或等于夹角阈值时，使所述农机方向回正，并将此时的农机位置标记为回正点，使所述农机从所述回正点开始进行地头直线行驶；

根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，并根据所述回正点和所述转出点生成地头直线行驶的路径规划；

所述农机按照地头直线行驶的路径规划进行直线行驶，并判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果；

若所述第二判断结果表示所述农机到达所述地头直线行驶的转出点，则所述农机按照转弯方向和转弯角度再次开始进行地头转弯；

实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

当所述第二夹角大于或等于夹角阈值时，所述农机到达地头出弯点，完成地头转弯。

可选的，判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果，具体包括：

根据单行直线作业的起点坐标和终点坐标，确定单行直线作业的作业直线；

确定过所述起点且垂直于所述作业直线的起点直线，并确定过所述终点且垂直于所述作业直线的终点直线；

判断所述农机的当前位置坐标是否位于所述起点直线和所述终点直线之间；

若所述农机的当前位置坐标位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

若所述农机的当前位置坐标不位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

可选的，判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果，具体包括：

根据所述农机的当前位置坐标、单行直线作业的起点坐标和终点坐标，利用公式

确定农机当前位置到终点方向与作业方向的夹角，确定为第三夹角；其中，θ₃为农机当前位置到终点方向与作业方向的第三夹角，

为单行直线作业的起点A到终点B的向量，

为单行直线作业的起点A到终点B的向量的模，

为农机当前位置P到终点B的向量，

为农机当前位置P到终点B的向量的模；

判断所述第三夹角是否大于或等于夹角阈值；

若所述第三夹角小于夹角阈值，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

若所述第三夹角大于或等于夹角阈值，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

可选的，所述根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，具体包括：

根据所述转弯角度，利用公式

确定所述农机按照转弯方向和转弯角度进行地头转弯的转弯半径；其中，R为转弯半径，P为地头入弯点，P′为农机按照转弯方向和转弯角度进行地头转弯过程中的一个位置点，α为转弯角度；

根据所述转弯半径和所述作业幅宽，利用公式L＝W-2R，确定所述农机进行地头直线行驶的距离；其中，L为地头直线行驶的距离，W为作业幅宽；

根据所述地头直线行驶的距离和所述回正点，确定地头直线行驶的转出点。

可选的，判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果，具体包括：

根据所述回正点的坐标和所述转出点的坐标，确定所述农机进行地头直线行驶的地头直线；

确定过所述回正点且垂直于所述地头直线的回正点直线，并确定过所述转出点且垂直于所述地头直线的转出点直线；

判断所述农机的当前位置坐标是否位于所述回正点直线和所述转出点直线之间；

若所述农机的当前位置坐标位于所述回正点直线和所述转出点直线之间，则所述第二判断结果表示所述农机未到达地头直线行驶的转出点；

若所述农机的当前位置坐标不位于所述回正点直线和所述转出点直线之间，则所述第二判断结果表示所述农机到达地头直线行驶的转出点。

可选的，判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果，具体包括：

根据所述农机的当前位置坐标和地头直线行驶的转出点的坐标，利用公式

确定农机当前方向与农机当前位置到转出点方向的第四夹角；其中，θ₄为农机当前方向与农机当前位置到转出点方向的夹角，确定为第四夹角，

为农机当前方向的向量，

为农机当前方向的向量的模，

为农机当前位置P到转出点N的向量，

为农机当前位置P到转出点N的向量的模；

判断所述第四夹角是否大于或等于夹角阈值；

若所述第四夹角小于夹角阈值，则第二判断结果表示所述农机未到达地头直线行驶的转出点；

若所述第四夹角大于或等于夹角阈值，则第二判断结果表示所述农机到达地头直线行驶的转出点。

可选的，所述实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角，具体包括：

利用公式

实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

其中，θ₂为农机当前方向与农机当前位置到地头出弯点方向之间的第二夹角，

为农机当前方向的向量，

为农机当前方向的向量的模，

为农机当前位置P到地头出弯点B₁的向量，

为农机当前位置P到地头出弯点B₁的向量的模。

一种用于宽幅作业的农机无人驾驶地头转弯控制系统，所述系统包括：

作业参数设置模块，用于设置农机的作业参数；所述作业参数包括：单行直线作业的起点和终点、作业幅宽、转弯方向和转弯角度；

单行直线作业模块，用于所述农机从单行直线作业的起点开始沿着起点到终点的作业方向进行单行直线作业；

第一判断结果获得模块，用于判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果；

第一地头转弯模块，用于若所述第一判断结果表示所述农机越过单行直线作业的终点到达地头，则使所述农机按照转弯方向和转弯角度开始进行地头转弯；

第一夹角获取模块，用于实时获取所述农机按照转弯方向和转弯角度进行地头转弯过程中，农机当前方向与所述作业方向之间的夹角，确定为第一夹角；

地头直线行驶模块，用于当所述第一夹角大于或等于夹角阈值时，使所述农机方向回正，并将此时的农机位置标记为回正点，使所述农机从所述回正点开始进行地头直线行驶；

转出点确定模块，用于根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，并根据所述回正点和所述转出点生成地头直线行驶的路径规划；

第二判断结果获得模块，用于所述农机按照地头直线行驶的路径规划进行直线行驶，并判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果；

第二地头转弯模块，用于若所述第二判断结果表示所述农机到达所述地头直线行驶的转出点，则所述农机按照转弯方向和转弯角度再次开始进行地头转弯；

第二夹角获取模块，用于实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

地头转弯完成模块，用于当所述第二夹角大于或等于夹角阈值时，所述农机到达地头出弯点，完成地头转弯。

可选的，所述第一判断结果获得模块，具体包括：

作业直线确定子模块，用于根据单行直线作业的起点坐标和终点坐标，确定单行直线作业的作业直线；

终点直线确定子模块，用于确定过所述起点且垂直于所述作业直线的起点直线，并确定过所述终点且垂直于所述作业直线的终点直线；

农机位置判断子模块，用于判断所述农机的当前位置坐标是否位于所述起点直线和所述终点直线之间；

未越过终点表示子模块，用于若所述农机的当前位置坐标位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

到达地头表示子模块，用于若所述农机的当前位置坐标不位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

可选的，所述第一判断结果获得模块，具体包括：

第三夹角确定子模块，用于根据所述农机的当前位置坐标、单行直线作业的起点坐标和终点坐标，利用公式

确定农机当前位置到终点方向与作业方向的夹角，确定为第三夹角；其中，θ₃为农机当前位置到终点方向与作业方向的第三夹角，

为单行直线作业的起点A到终点B的向量，

为单行直线作业的起点A到终点B的向量的模，

为农机当前位置P到终点B的向量，

为农机当前位置P到终点B的向量的模；

第三夹角判断子模块，用于判断所述第三夹角是否大于或等于夹角阈值；

第一判断结果第一表示子模块，用于若所述第三夹角小于夹角阈值，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

第一判断结果第二表示子模块，用于若所述第三夹角大于或等于夹角阈值，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

根据本发明提供的具体实施例，本发明公开了以下技术效果：

本发明提供一种用于宽幅作业的农机无人驾驶地头转弯控制方法及系统，设置农机的作业参数，判断农机是否越过单行直线作业的终点到达地头，到达地头后使农机按照转弯方向和转弯角度开始进行地头转弯，地头转弯过程中，实时获取农机当前方向与作业方向之间的第一夹角，当第一夹角大于或等于夹角阈值时，使农机方向回正，并将此时的农机位置标记为回正点，使农机从所述回正点开始进行地头直线行驶，根据转弯角度、作业幅宽和回正点，确定地头直线行驶的转出点，当判断农机到达地头直线行驶的转出点时，农机按照转弯方向和转弯角度再次开始进行地头转弯，实时获取农机当前方向与农机当前位置到地头出弯点方向之间的第二夹角，当第二夹角大于或等于夹角阈值时，农机到达地头出弯点，完成地头转弯。本发明自动判断入弯点与出弯点以及地头直线行驶段的起点和终点，使农机在每一条作业路径间的作业幅宽均相同，自动驾驶机械在区间内往复的自动化作业，减少作业覆盖的重叠和遗漏，在减少作业时间的同时提高作业效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明提供的用于宽幅作业的农机无人驾驶地头转弯控制方法的流程图；

图2为本发明提供的用于宽幅作业的农机无人驾驶地头转弯控制方法的原理图；

图3为本发明提供的用于宽幅作业的农机无人驾驶地头转弯控制方法的地头检测示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的目的是提供一种用于宽幅作业的农机无人驾驶地头转弯控制方法及系统，以实现宽幅农机在地头转弯时自主生成实时地头转弯路径，使每一条作业路径间的作业幅宽均相同，减少作业覆盖的重叠和遗漏，在减少作业时间的同时提高作业效率。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明提供了一种用于宽幅作业的农机无人驾驶地头转弯控制方法，如图1-2所示，方法包括：

S101，设置农机的作业参数；作业参数包括：单行直线作业的起点和终点、作业幅宽、转弯方向和转弯角度。

S102，农机从单行直线作业的起点开始沿着起点到终点的作业方向进行单行直线作业。

S103，判断农机是否越过单行直线作业的终点到达地头，获得第一判断结果。

S104，若第一判断结果表示农机越过单行直线作业的终点到达地头，则使农机按照转弯方向和转弯角度开始进行地头转弯。

S105，实时获取农机按照转弯方向和转弯角度进行地头转弯过程中，农机当前方向与作业方向之间的夹角，确定为第一夹角。

S106，当第一夹角大于或等于夹角阈值时，使农机方向回正，并将此时的农机位置标记为回正点，使农机从回正点开始进行地头直线行驶。

S107，根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，并根据回正点和转出点生成地头直线行驶的路径规划。

S108，农机按照地头直线行驶的路径规划进行直线行驶，并判断农机是否到达地头直线行驶的转出点，获得第二判断结果。

S109，若第二判断结果表示农机到达地头直线行驶的转出点，则农机按照转弯方向和转弯角度再次开始进行地头转弯。

S110，实时获取农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角。

S111，当第二夹角大于或等于夹角阈值时，农机到达地头出弯点，完成地头转弯。

具体过程如下：

本发明主要包含参数设定及开始运行、地头检测、地头直线行驶、完成地头直线行驶、开始入弯检测、完成地头转弯、开始出弯检测几个步骤。

步骤S101-S102，在进行自动作业之前需要操作人员标定单行作业长度的端点A和B、作业幅宽W、转弯方向和转弯角度α。设定完成后即可开始运行，当开始自动驾驶时农机会沿着标定的AB进行作业直到到达B点后准备掉头。

步骤S103，农机完成自动驾驶的直线作业后，接近B点，此时需进行地头检测，判断农业机械是否到达地头或超过作业区间。主要检测方式有以下两种：

第一种，判断农业机械是否超过作业区间：

根据单行直线作业的起点坐标和终点坐标，确定单行直线作业的作业直线；作业直线为Ax+By+C＝0；

确定过起点且垂直于作业直线的起点直线，并确定过终点且垂直于作业直线的终点直线；起点直线为Bx-Ay+D_A＝0和终点直线为Bx-Ay+D_B＝0；

判断农机的当前位置坐标是否位于起点直线和终点直线之间；

若农机的当前位置坐标位于起点直线和终点直线之间，则第一判断结果表示农机未越过单行直线作业的终点；

若农机的当前位置坐标不位于起点直线和终点直线之间，则第一判断结果表示农机越过单行直线作业的终点到达地头。此时确定P点为地头入弯点。

第一种，判断农业机械是否到达地头：

根据农机的当前位置坐标、单行直线作业的起点坐标和终点坐标，利用公式

确定农机当前位置到终点方向与作业方向的第三夹角；其中，θ₃为农机当前位置到终点方向与作业方向的夹角，确定为第三夹角，

为单行直线作业的起点A到终点B的向量，

为单行直线作业的起点A到终点B的向量的模，

为农机当前位置P到终点B的向量，

为农机当前位置P到终点B的向量的模；

判断第三夹角是否大于或等于夹角阈值；

若第三夹角小于夹角阈值，则第一判断结果表示农机未越过单行直线作业的终点；

若第三夹角大于或等于夹角阈值，则第一判断结果表示农机越过单行直线作业的终点到达地头。此时的P点为地头入弯点。

步骤S104，农机到达地头，根据控制器所预设的指令，农机开始从地头入弯点P按照转弯方向和转弯角度进行地头转弯。

步骤S105-S106，农机从开始进行地头转弯后，需实时进行出弯检测，判断宽幅农业机械是否到达地头直线行驶起点。检测方法为：

此时农机方向为

入弯角度(转弯角度)为α，向量

与

夹角

若θ大于或等于90°，农机方向回正，开始地头直线行驶阶段，如图3所示，此时回正点为M。

步骤S107，根据转弯角度、作业幅宽和回正点，确定地头直线行驶的转出点，具体包括：

根据转弯角度，利用公式

确定农机按照转弯方向和转弯角度进行地头转弯的转弯半径；其中，R为转弯半径，P为地头入弯点，P′为农机按照转弯方向和转弯角度进行地头转弯过程中的一个位置点，α为转弯角度；

根据转弯半径和作业幅宽，利用公式L＝W-2R，确定农机进行地头直线行驶的距离；其中，L为地头直线行驶的距离，W为作业幅宽；

根据地头直线行驶的距离和回正点，确定地头直线行驶的转出点。

在确定地头直线行驶的起点和终点后并能实时生成当前作业路径。

步骤S108，农机完成地头转弯的直线作业后，接近N点，此时需进行入弯检测，判断农业机械是否到达转出点N点或超过作业区间。主要检测方式有以下两种：

第一种，判断农业机械是否超过作业区间：

根据回正点的坐标和转出点的坐标，确定农机进行地头直线行驶的地头直线；地头直线为Mx+Ny+U＝0；

确定过回正点且垂直于地头直线的回正点直线，并确定过转出点且垂直于地头直线的转出点直线；回正点直线为Mx-Ny+V_A＝0，转出点直线为Mx-Ny+V_B＝0；

判断农机的当前位置坐标是否位于回正点直线和转出点直线之间；

若农机的当前位置坐标位于回正点直线和转出点直线之间，则第二判断结果表示农机未到达地头直线行驶的转出点；

若农机的当前位置坐标不位于回正点直线和转出点直线之间，则第二判断结果表示农机到达地头直线行驶的转出点。

第二种，判断农业机械是否到达N点：

根据农机的当前位置坐标和地头直线行驶的转出点的坐标，利用公式

确定农机当前方向与农机当前位置到转出点方向的夹角，确定为第四夹角；其中，θ₄为农机当前方向与农机当前位置到转出点方向的第四夹角，

为农机当前方向的向量，

为农机当前方向的向量的模，

为农机当前位置P到转出点N的向量，

为农机当前位置P到转出点N的向量的模；

判断第四夹角是否大于或等于夹角阈值；优选地，夹角阈值为90°；

若第四夹角小于夹角阈值，则第二判断结果表示农机未到达地头直线行驶的转出点；

若第四夹角大于或等于夹角阈值，则第二判断结果表示农机到达地头直线行驶的转出点。

步骤S109，农机到达地头直线行驶终点，控制器控制农机开始按照转弯方向和转弯角度再次地头入弯。

步骤S110，实时获取农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角，具体包括：

利用公式

实时获取农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

其中，θ₂为农机当前方向与农机当前位置到地头出弯点方向之间的第二夹角，

为农机当前方向的向量，

为农机当前方向的向量的模，

为农机当前位置P到地头出弯点B₁的向量，

为农机当前位置P到地头出弯点B₁的向量的模。

步骤S111，完成地头转弯后，开始沿着B₁A₁直线行驶作业。

按照设定好的田地大小、作业幅宽以及作业行数进行自动的路径规划，生成相应的单行直线作业AB、A₁B₁、A₂B₂、…、A_nB_n作业路径和地头转弯的路径规划，在地头转弯时使用本发明提供的地头转弯控制方法，自动判断入弯点与出弯点，使每一条作业路径间的作业幅宽均为W，自动驾驶机械便可以在区间内往复的自动化作业，减少作业覆盖的重叠和遗漏，在减少作业时间的同时提高作业效率。

本发明还提供了一种用于宽幅作业的农机无人驾驶地头转弯控制系统，系统包括：

作业参数设置模块，用于设置农机的作业参数；作业参数包括：单行直线作业的起点和终点、作业幅宽、转弯方向和转弯角度；

单行直线作业模块，用于农机从单行直线作业的起点开始沿着起点到终点的作业方向进行单行直线作业；

第一判断结果获得模块，用于判断农机是否越过单行直线作业的终点到达地头，获得第一判断结果；

第一地头转弯模块，用于若第一判断结果表示农机越过单行直线作业的终点到达地头，则使农机按照转弯方向和转弯角度开始进行地头转弯；

第一夹角获取模块，用于实时获取农机按照转弯方向和转弯角度进行地头转弯过程中，农机当前方向与作业方向之间的夹角，确定为第一夹角；

地头直线行驶模块，用于当第一夹角大于或等于夹角阈值时，使农机方向回正，并将此时的农机位置标记为回正点，使农机从回正点开始进行地头直线行驶；

转出点确定模块，用于根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，并根据回正点和转出点生成地头直线行驶的路径规划；

第二判断结果获得模块，用于农机按照地头直线行驶的路径规划进行直线行驶，并判断农机是否到达地头直线行驶的转出点，获得第二判断结果；

第二地头转弯模块，用于若第二判断结果表示农机到达地头直线行驶的转出点，则农机按照转弯方向和转弯角度再次开始进行地头转弯；

第二夹角获取模块，用于实时获取农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

地头转弯完成模块，用于当第二夹角大于或等于夹角阈值时，农机到达地头出弯点，完成地头转弯。

第一判断结果获得模块，具体包括：

作业直线确定子模块，用于根据单行直线作业的起点坐标和终点坐标，确定单行直线作业的作业直线；

终点直线确定子模块，用于确定过起点且垂直于作业直线的起点直线，并确定过终点且垂直于作业直线的终点直线；

农机位置判断子模块，用于判断农机的当前位置坐标是否位于起点直线和终点直线之间；

未越过终点表示子模块，用于若农机的当前位置坐标位于起点直线和终点直线之间，则第一判断结果表示农机未越过单行直线作业的终点；

到达地头表示子模块，用于若农机的当前位置坐标不位于起点直线和终点直线之间，则第一判断结果表示农机越过单行直线作业的终点到达地头。

第一判断结果获得模块，具体包括：

第三夹角确定子模块，用于根据农机的当前位置坐标、单行直线作业的起点坐标和终点坐标，利用公式

确定农机当前位置到终点方向与作业方向的夹角，确定为第三夹角；其中，θ₃为农机当前位置到终点方向与作业方向的第三夹角，

为单行直线作业的起点A到终点B的向量，

为单行直线作业的起点A到终点B的向量的模，

为农机当前位置P到终点B的向量，

为农机当前位置P到终点B的向量的模；

第三夹角判断子模块，用于判断第三夹角是否大于或等于夹角阈值；

第一判断结果第一表示子模块，用于若第三夹角小于夹角阈值，则第一判断结果表示农机未越过单行直线作业的终点；

第一判断结果第二表示子模块，用于若第三夹角大于或等于夹角阈值，则第一判断结果表示农机越过单行直线作业的终点到达地头。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (9)

1.一种用于宽幅作业的农机无人驾驶地头转弯控制方法，其特征在于，所述方法包括：

设置农机的作业参数；所述作业参数包括：单行直线作业的起点和终点、作业幅宽、转弯方向和转弯角度；

所述农机从单行直线作业的起点开始沿着起点到终点的作业方向进行单行直线作业；

判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果；

若所述第一判断结果表示所述农机越过单行直线作业的终点到达地头，则使所述农机按照转弯方向和转弯角度开始进行地头转弯；

实时获取所述农机按照转弯方向和转弯角度进行地头转弯过程中，农机当前方向与所述作业方向之间的夹角，确定为第一夹角；

当所述第一夹角大于或等于夹角阈值时，使所述农机方向回正，并将此时的农机位置标记为回正点，使所述农机从所述回正点开始进行地头直线行驶；

根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，并根据所述回正点和所述转出点生成地头直线行驶的路径规划，具体包括：根据所述转弯角度，利用公式

确定所述农机按照转弯方向和转弯角度进行地头转弯的转弯半径；根据所述转弯半径和所述作业幅宽，利用公式L＝W-2R，确定所述农机进行地头直线行驶的距离；根据所述地头直线行驶的距离和所述回正点，确定地头直线行驶的转出点；其中，R为转弯半径，P为地头入弯点，P′为农机按照转弯方向和转弯角度进行地头转弯过程中的一个位置点，α为转弯角度；L为地头直线行驶的距离，W为作业幅宽；

所述农机按照地头直线行驶的路径规划进行直线行驶，并判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果；

若所述第二判断结果表示所述农机到达所述地头直线行驶的转出点，则所述农机按照转弯方向和转弯角度再次开始进行地头转弯；

实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

当所述第二夹角大于或等于夹角阈值时，所述农机到达地头出弯点，完成地头转弯。

2.根据权利要求1所述的用于宽幅作业的农机无人驾驶地头转弯控制方法，其特征在于，判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果，具体包括：

根据单行直线作业的起点坐标和终点坐标，确定单行直线作业的作业直线；

确定过所述起点且垂直于所述作业直线的起点直线，并确定过所述终点且垂直于所述作业直线的终点直线；

判断所述农机的当前位置坐标是否位于所述起点直线和所述终点直线之间；

若所述农机的当前位置坐标位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

若所述农机的当前位置坐标不位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

3.根据权利要求1所述的用于宽幅作业的农机无人驾驶地头转弯控制方法，其特征在于，判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果，具体包括：

根据所述农机的当前位置坐标、单行直线作业的起点坐标和终点坐标，利用公式

确定农机当前位置到终点方向与作业方向的夹角，确定为第三夹角；其中，θ₃为农机当前位置到终点方向与作业方向的第三夹角，

为单行直线作业的起点A到终点B的向量，

为单行直线作业的起点A到终点B的向量的模，

为农机当前位置P到终点B的向量，

为农机当前位置P到终点B的向量的模；

判断所述第三夹角是否大于或等于夹角阈值；

若所述第三夹角小于夹角阈值，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

若所述第三夹角大于或等于夹角阈值，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

4.根据权利要求1所述的用于宽幅作业的农机无人驾驶地头转弯控制方法，其特征在于，判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果，具体包括：

根据所述回正点的坐标和所述转出点的坐标，确定所述农机进行地头直线行驶的地头直线；

确定过所述回正点且垂直于所述地头直线的回正点直线，并确定过所述转出点且垂直于所述地头直线的转出点直线；

判断所述农机的当前位置坐标是否位于所述回正点直线和所述转出点直线之间；

若所述农机的当前位置坐标位于所述回正点直线和所述转出点直线之间，则所述第二判断结果表示所述农机未到达地头直线行驶的转出点；

若所述农机的当前位置坐标不位于所述回正点直线和所述转出点直线之间，则所述第二判断结果表示所述农机到达地头直线行驶的转出点。

5.根据权利要求1所述的用于宽幅作业的农机无人驾驶地头转弯控制方法，其特征在于，判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果，具体包括：

根据所述农机的当前位置坐标和地头直线行驶的转出点的坐标，利用公式

确定农机当前方向与农机当前位置到转出点方向的第四夹角；其中，θ₄为农机当前方向与农机当前位置到转出点方向的夹角，确定为第四夹角，

为农机当前方向的向量，

为农机当前方向的向量的模，

为农机当前位置P到转出点N的向量，

为农机当前位置P到转出点N的向量的模；

判断所述第四夹角是否大于或等于夹角阈值；

若所述第四夹角小于夹角阈值，则第二判断结果表示所述农机未到达地头直线行驶的转出点；

若所述第四夹角大于或等于夹角阈值，则第二判断结果表示所述农机到达地头直线行驶的转出点。

6.根据权利要求1所述的用于宽幅作业的农机无人驾驶地头转弯控制方法，其特征在于，所述实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角，具体包括：

利用公式

实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

其中，θ₂为农机当前方向与农机当前位置到地头出弯点方向之间的第二夹角，

为农机当前方向的向量，

为农机当前方向的向量的模，

为农机当前位置P到地头出弯点B₁的向量，

为农机当前位置P到地头出弯点B₁的向量的模。

7.一种用于宽幅作业的农机无人驾驶地头转弯控制系统，其特征在于，所述系统包括：

作业参数设置模块，用于设置农机的作业参数；所述作业参数包括：单行直线作业的起点和终点、作业幅宽、转弯方向和转弯角度；

单行直线作业模块，用于所述农机从单行直线作业的起点开始沿着起点到终点的作业方向进行单行直线作业；

第一判断结果获得模块，用于判断所述农机是否越过单行直线作业的终点到达地头，获得第一判断结果；

第一地头转弯模块，用于若所述第一判断结果表示所述农机越过单行直线作业的终点到达地头，则使所述农机按照转弯方向和转弯角度开始进行地头转弯；

第一夹角获取模块，用于实时获取所述农机按照转弯方向和转弯角度进行地头转弯过程中，农机当前方向与所述作业方向之间的夹角，确定为第一夹角；

地头直线行驶模块，用于当所述第一夹角大于或等于夹角阈值时，使所述农机方向回正，并将此时的农机位置标记为回正点，使所述农机从所述回正点开始进行地头直线行驶；

转出点确定模块，用于根据转弯角度、作业幅宽和所述回正点，确定地头直线行驶的转出点，并根据所述回正点和所述转出点生成地头直线行驶的路径规划；

第二判断结果获得模块，用于所述农机按照地头直线行驶的路径规划进行直线行驶，并判断所述农机是否到达地头直线行驶的转出点，获得第二判断结果；

第二地头转弯模块，用于若所述第二判断结果表示所述农机到达所述地头直线行驶的转出点，则所述农机按照转弯方向和转弯角度再次开始进行地头转弯；

第二夹角获取模块，用于实时获取所述农机按照转弯方向和转弯角度再次进行地头转弯过程中，农机当前方向与农机当前位置到地头出弯点方向之间的夹角，确定为第二夹角；

地头转弯完成模块，用于当所述第二夹角大于或等于夹角阈值时，所述农机到达地头出弯点，完成地头转弯。

8.根据权利要求7所述的用于宽幅作业的农机无人驾驶地头转弯控制系统，其特征在于，所述第一判断结果获得模块，具体包括：

作业直线确定子模块，用于根据单行直线作业的起点坐标和终点坐标，确定单行直线作业的作业直线；

终点直线确定子模块，用于确定过所述起点且垂直于所述作业直线的起点直线，并确定过所述终点且垂直于所述作业直线的终点直线；

农机位置判断子模块，用于判断所述农机的当前位置坐标是否位于所述起点直线和所述终点直线之间；

未越过终点表示子模块，用于若所述农机的当前位置坐标位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

到达地头表示子模块，用于若所述农机的当前位置坐标不位于所述起点直线和所述终点直线之间，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

9.根据权利要求7所述的用于宽幅作业的农机无人驾驶地头转弯控制系统，其特征在于，所述第一判断结果获得模块，具体包括：

第三夹角确定子模块，用于根据所述农机的当前位置坐标、单行直线作业的起点坐标和终点坐标，利用公式

确定农机当前位置到终点方向与作业方向的夹角，确定为第三夹角；其中，θ₃为农机当前位置到终点方向与作业方向的第三夹角，

为单行直线作业的起点A到终点B的向量，

为单行直线作业的起点A到终点B的向量的模，

为农机当前位置P到终点B的向量，

为农机当前位置P到终点B的向量的模；

第三夹角判断子模块，用于判断所述第三夹角是否大于或等于夹角阈值；

第一判断结果第一表示子模块，用于若所述第三夹角小于夹角阈值，则所述第一判断结果表示所述农机未越过单行直线作业的终点；

第一判断结果第二表示子模块，用于若所述第三夹角大于或等于夹角阈值，则所述第一判断结果表示所述农机越过单行直线作业的终点到达地头。

Images