一种基于HybridA*算法的自动驾驶农机平滑路径规划方法

技术领域

本发明涉及农机自动驾驶技术领域，具体涉及一种基于Hybrid A

背景技术

农机自动驾驶技术广泛应用于耕作、移栽、收割、施肥、施药等农业生产过程，根据农田边界和田间障碍物分布规划农机最优作业路径，可优化作业质量、提高作业效率、减少投入成本。受制于阿克曼结构的动力学约束，现阶段的农机自动驾驶主要以直线为主。而农田环境复杂，多数为非规则边界且田间可能存在树木、配电房等障碍物，自动驾驶农机需要绕过障碍并迅速回到原有作业路径。此外，田间掉头也是路径规划的难点。

针对上述问题，结合我国农田实际情况，本发明寻求田间作业不同场景(避障、转向、掉头、换道等)的最佳路径曲线，规划农机作业的最优路径，降低非必须的行驶里程。针对农机自动驾驶主要以直线为主，本发明设计了一种基于hybrid A*的平滑路径规划方法，可以针对不同作业场景规划最优的平滑路径，有效提高农机作业效率，降低非必须的行驶里程。

发明内容

本发明针对农田边界不规则、田间存在障碍物、掉头路径规划难及现阶段的农机自动驾驶以直线为主等问题，提供了一种基于Hybrid A

本发明解决技术问题的技术方案为：

一种基于Hybrid A

Step1：建立Open列表和Close列表并进行初始化，获取起点n

Step2：判断Open列表是否为空，若Open列表为空，则算法运行结束，结果为地图中不存在一条到达终点坐标的可行路径；若Open列表非空，则取出Open 列表中综合优先值f(n)最低的节点n

所述的综合优先值f(n)的计算公式为：

f(n)＝g(n)+h(n) (I)

公式(I)中，f(n)为节点n的综合优先值，g(n)为起点到节点n的多目标代价函数，h(n)为节点n到终点的启发函数；

公式(II)中，ω

h(n)为节点n到终点之间的Reeds-Shepp曲线长度；

Step3：判断节点n

Step4：以节点n

其中，通过hybrid A

n＝(x,y,θ,g(n),f(n),*n

其中(x,y)表示节点n的位置坐标，θ表示节点n的偏航角，g(n)为起点到节点n的多目标代价函数，f(n)为节点n的综合优先值，*n

Step5：判断其中一个子节点n

Step6：判断子节点n

Step7：对Step4中得到的其他子节点依次进行Step5与Step6，直至Step4 中的子节点都进行一次后进入下一步骤；

Step8：循环执行Step2到Step7，直到满足Step2或Step3中对应的判断条件，即找到一条可行平滑路径或未找到可行路径，算法结束。

如图2所示，节点n的属性为(x

在节点n处右转所绕的圆心的坐标为：

(O

在公式(III)中，O

在节点m处左转所绕的圆心的坐标为：

(O

在公式(III)中，O′

两个圆心坐标的圆心间的欧式距离为：

在公式(V)中，|OO′|为在节点n处右转所绕的圆心与在节点m处右转所绕的圆心间的欧式距离；

切点w

α＝arccos(2R/|OO′|) (VII)

在公式(VI)与(VII)中，R为自动驾驶农机的最小转弯半径；w

切点w

在公式(VIII)，R为自动驾驶农机的最小转弯半径；w

切点w

节点n与节点m之间的Reeds-Shepp曲线长度为：

在公式(X)中，l为节点n与节点m之间的Reeds-Shepp曲线长度；l

发明内容中提供的效果仅仅是实施例的效果，而不是发明所有的全部效果，上述技术方案中的一个技术方案具有如下优点或有益效果：

(1)本发明提出基于Hybrid A

(2)结合我国农田多为不规则形状且田间存在障碍物的实际情况，本发明提出综合考虑自动驾驶农机偏航角、曲率、倒车及路径长度等附加成本的多目标代价函数，结合Reeds-Shepp曲线长度的启发函数生成综合优先值，能够保证生成的路径连续平滑，尽量保持直线行驶且避免猛打方向和倒车，并评估了转向、掉头、避障等不同场景的平滑路径曲线。

附图说明

图1为本发明的算法运行流程图；

图2为A

图3为举例节点n与节点m的Reeds-Shepp曲线示意图；

图4为依据本发明生成的地头左掉头路径图；

图5为依据本发明生成的地头右掉头路径图；

图6为依据本发明生成的田间跨越障碍物左掉头路径图；

图7为依据本发明生成的田间跨越障碍物右掉头路径图；

图8为依据本发明生成的田间跨越左障碍物作业的路径图；

图9为依据本发明生成的田间跨越右障碍物作业的路径图；

图10为依据本发明生成的田间左转向的路径图；

图11为依据本发明生成的田间右转向的路径图；

具体实施方式

为了能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本发明进行详细阐述。下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。应当注意，在附图中所图示的部件不一定按比例绘制。本发明省略了对公知组件和处理技术及工艺的描述以避免不必要地限制本发明。

一种基于Hybrid A

Step1：建立Open列表和Close列表并进行初始化，获取起点n

Step2：判断Open列表是否为空，若Open列表为空，则算法运行结束，结果为地图中不存在一条到达终点坐标的可行路径；若Open列表非空，则取出Open 列表中综合优先值f(n)最低的节点n

所述的综合优先值f(n)的计算公式为：

f(n)＝g(n)+h(n) (I)

公式(I)中，f(n)为节点n的综合优先值，g(n)为起点到节点n的多目标代价函数，h(n)为节点n到终点的启发函数；

公式(II)中，ω

h(n)为节点n到终点之间的Reeds-Shepp曲线长度；

两节点之间Reeds-Shepp曲线长度的计算方法为本领域的常规技术，以下进行举例说明，如图2所示，节点n的属性为(x

在节点n处右转所绕的圆心的坐标为：

(O

在公式(III)中，O

在节点m处左转所绕的圆心的坐标为：

(O

在公式(III)中，O′

两个圆心坐标的圆心间的欧式距离为：

在公式(V)中，|OO′|为在节点n处右转所绕的圆心与在节点m处右转所绕的圆心间的欧式距离；

切点w

α＝arccos(2R/|OO′|) (VII)

在公式(VI)与(VII)中，R为自动驾驶农机的最小转弯半径；w

切点w

在公式(VIII)，R为自动驾驶农机的最小转弯半径；w

切点w

节点n与节点m之间的Reeds-Shepp曲线长度为：

在公式(X)中，l为节点n与节点m之间的Reeds-Shepp曲线长度；l

Step3：判断节点n

Step4：以节点n

其中，通过hybrid A

n＝(x,y,θ,g(n),f(n),*n

其中(x,y)表示节点n的位置坐标，θ表示节点n的偏航角，g(n)为起点到节点n的多目标代价函数，f(n)为节点n的综合优先值，*n

A

Step5：判断其中一个子节点n

Step6：判断子节点n

Step7：对Step4中得到的其他子节点依次进行Step5与Step6，直至Step4 中的子节点都进行一次后进入下一步骤；

Step8：循环执行Step2到Step7，直到满足Step2或Step3中对应的判断条件，即找到一条可行平滑路径或未找到可行路径，算法结束。

如图4-11所示，为依据本发明生成的平滑路径示意图，由图可见通过本发明能够保证生成的路径连续平滑，尽量保持直线行驶且避免猛打方向和倒车。

上述虽然结合附图对发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。