[发明专利]一种基于HybridA*算法的自动驾驶农机平滑路径规划方法

说明书

本发明涉及农机自动驾驶技术领域，公开了一种基于HybridA^*算法的自动驾驶农机平滑路径规划方法，通过Step4中加入的偏航角信息，可以确保农机转向角度满足最小转弯半径的约束，达到了规划出带有非完整性约束的平滑路径的效果。能够规划出带有非完整性约束的平滑路径，通过Step2中加入启发函数h(n)，可以保证始终朝着终点方向进行路径规划，减少非必须的行驶里程，通过Step2中的多目标代价函数g(n)，可以保证农机尽量直线行驶，有效降低减速再加速的转向及掉头过程，达到有效提高自动驾驶农机作业效率的效果。

技术领域

本发明涉及农机自动驾驶技术领域，具体涉及一种基于Hybrid A^*算法的自动驾驶农机平滑路径规划方法。

背景技术

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

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

发明内容

本发明针对农田边界不规则、田间存在障碍物、掉头路径规划难及现阶段的农机自动驾驶以直线为主等问题，提供了一种基于Hybrid A^*算法的自动驾驶农机平滑路径规划方法，能够规划出带有半径约束的平滑路径，减少非必须的行驶里程，有效提高农机的作业效率和质量。

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

一种基于Hybrid A^*算法的自动驾驶农机平滑路径规划方法，包括以下步骤：

Step1：建立Open列表和Close列表并进行初始化，获取起点n_s的位置坐标和偏航角和终点n_t的位置坐标和偏航角，并把起点n_s放入Open列表中；

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

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

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

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

公式(II)中，ω_yaw为路径搜索转向角的代价权重，ω_cur为路径搜索曲率的代价权重，ω_bck为路径搜索倒车的代价权重，ω_dst为路径搜索距离长度的代价权重，steer为转向角花费，cur为曲率花费，back为反向路劲长度花费，dist 为正向路径长度花费；

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