[发明专利]一种路径规划方法

权利要求书

1.一种路径规划方法，其特征在于，包括以下步骤：

步骤1：初始化起点、终点的方向；

步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角是否大于如果则用y＝k*x+b估计移动代价；如果则用y＝a*x²+b*x+c估计移动路径代价，然后进入步骤3；

步骤3：将起始位置放到开启列表，将关闭列表初始化为空；执行A^*算法的扩展过程,即搜寻规划节点，所述开启列表表示待搜索节点列表；

步骤4：判断开启列表是否为空，如果为空，则寻找路径失败，退出程序；如果不为空，则进入步骤5；

步骤5：计算节点与起始点的方向代价o(n)＝a(n)+d(n)和路沿惩罚代价c(n)；其中，a(n)为起始点的朝向到当前位置的角度的代价，d(n)是起始点到当前结点n的直线代价；

步骤6：利用公式(4)计算开启列表中估价函数f(n)；

f(n)＝g(n)+h(n)+o(n)+c(n) (4)

其中，g(n)是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的估计代价；

步骤7：将开启列表中具有最小f(n)所对应的节点，作为扩展节点，并将该节点加入关闭列表中,同时，取扩展节点的邻近节点，判断其是否在关闭列表中，如果不是，则将该邻近节点加入开启列表中，并更新开启列表中其余节点的代价函数f(n)；

步骤8：判断步骤7中的扩展节点是否为终点，如果不是，则返回步骤5；如果该扩展节点为终点，则将关闭节点中的节点保存为规划路径，即Path1；

在步骤8之后，还包括以下步骤：

步骤9：获取所述路径path1；

步骤10：利用直线拟合算法删除冗余点得到转折点；

步骤11：依次从path1中取pose[i-2],pose[i-1],pose[i]3个转折点进行拟合y(x)¹＝a*x²+b*x+c，同时，判断提取出来的转折点中，是否包含终点，如果含有终点，则进入步骤13,否则执行下一步；

步骤12：对y(x)¹＝a*x²+b*x+c求一阶导和二阶导，带入公式(10)中，判断是否满足公式(11)，如果满足，则进入步骤13；否则移动转折点，判断移动次数是否小于3，如果小于3，更新最小的曲率并重新执行步骤12，如果大于3，则保留曲率最小的转折点，然后进入步骤13；

所述更新最小的曲率包括：移动转折点pose[i-1]，首先求出y(x)¹的对称轴如公式(12)所示，判断所提取的出来的转折点是否大于同时，通过判断a的正负号确定y(x)¹的开口朝向，如a＞0且则将pose[i-1]向左上角移动一个栅格，如果a＞0且则将pose[i-1]向右上角移动一个栅格，如果a＜0且则将pose[i-1]向左下角移动一个栅格，如果a＜0且则将pose[i-1]向右下角移动一个栅格；

步骤13：拟合转折点，进行插值；

步骤14：结束规划，输出路径Path；

K＜K_max (11)

index_new是移动后的节点的索引,index是此刻节点的索引，nx表示地图的宽度。