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

权利要求书

1.一种基于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曲线长度；

Step3：判断节点n^*是否为终点，如果是则算法运行结束，Close列表中的所有节点根据位置坐标和偏航角信息连接起来即为可行平滑路径；否则继续执行下一步；

Step4：以节点n^*为中心点通过hybrid A^*算法进行子节点扩展，可得到以n^*为父节点的若干子节点；

Step5：判断其中一个子节点n_c是否在Close列表中，并对子节点n_c进行障碍物碰撞检测，若子节点n_c在Close列表或者碰触障碍物则跳过该子节点，不进行处理；否则进行下一步；

Step6：判断子节点n_c是否在Open列表中，若不在，则说明子节点n_c还未遍历，将其加入Open列表；若在，则说明已经遍历过子节点n_c，此时需要判断子节点n_c当前的多目标代价函数g(n_c)是否小于之前计算的多目标代价函数g(n_c)，如果小于之前计算的多目标代价函数g(n_c)，将存储在Open列表中的节点n_c的综合优先值f(n)进行更新，否则不进行处理；对其他子节点进行同样的操作；

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

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

2.如权利要求1所述的一种基于Hybrid A*算法的自动驾驶农机平滑路径规划方法，其特征在于，若位置坐标与偏航角相同则为相同节点。

3.如权利要求1所述的一种基于Hybrid A*算法的自动驾驶农机平滑路径规划方法，其特征在于，在步骤step3中，判断节点n^*是否为终点的方式为判断节点n^*与终点的偏航角和距离是否在给定阈值范围内。

4.如权利要求1所述的一种基于Hybrid A*算法的自动驾驶农机平滑路径规划方法，其特征在于，在步骤Step4中，通过hybrid A^*算法进行子节点扩展中的节点n的属性形式如下：

n＝(x，y，θ，g(n)，f(n)，*n_p)

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