一种路径规划方法技术

本发明专利技术提供一种路径规划方法，该方法通过对车辆建模得到车辆的约束，同时在估价函数中加入方向代价并带入A*算法，使得所规划路径更加符合车辆的运动学模型,转折次数少，路线更平滑。将本发明专利技术方法应用于学校校园路径规划，结果表明：在规划相同路径上面，改进的A*算法规划的路径相比传统A*所规划的路径，长度降低约2.22％，累计路径转折点降低约65.28％，累计转折角度降低约49.03％，所规划出的路径能够更好满足车辆运动学模型。

A Path Planning Method

The invention provides a path planning method, which obtains vehicle constraints through vehicle modeling, adds direction cost into the evaluation function and brings in A* algorithm to make the planned path more in line with the vehicle kinematics model, with fewer turning times and smoother route. The method is applied to campus path planning. The results show that the length of the path planned by the improved A* algorithm decreases by 2.22%, the turning point of the cumulative path decreases by 65.28%, and the turning angle decreases by 49.03% compared with the path planned by the traditional A* algorithm. The planned path can better satisfy the vehicle kinematics model.

【技术实现步骤摘要】
一种路径规划方法
本专利技术涉及控制工程
，尤其涉及一种路径规划方法。
技术介绍
随着无人驾驶的快速发展，无人车运用在日常生活中的可能性也越来越大。而无人车的安全性、可靠性和舒适性尤为重要，这就对无人车运动路线的优化提出了更高的要求。因此，路径规划一直是无人车研究中的一个热点，对于其规划的好坏常常用规划的路径长度、代价和时间等因素来评价。目前，学者们对路径规划方法进行了大量研究，主要包括：基于采样的Voronoi图方法、快速搜索随机树等方法；基于节点的Dijkstra、A*算法、D*算法等；基于模型的人工势场法、动态窗口法等；基于生物启发式的神经网络、遗传算法、蚁群算法等。其中，A*算法便于理解和实现，是目前使用最广泛的规划法之一。由于A*算法能够有效的求解出最优路径，同时利用启发函数能够简单的评估地图上任意点到终点的距离，从而能够有效的减少搜索空间，提高搜索效率。所以被广泛的运用在移动机器人领域。但是，在应用中，经典的A*算法仍存在一些问题，如：所规划的路径距离障碍物太近，路线不够平滑，转折个数比较多等，导致规划出来的路线可能紧贴障碍物。王维，裴东等人通过对当前结点及父节点的代价进行指数衰减，使得A*算法在离目标较远时能够较快的靠近目标。王红卫,马勇等通过将延长线路的中间节点删除,提出平滑A*算法，使得A*算法的结果更加平滑。Saska和吴宪祥等提出基于三次样条曲线的参数优化问题，使得路径利于机器人的运动控制。单伟和孟正大提出使用极多项式和使用分段多项式函数是A*规划的路径来满足移动机器人的动力学特性。但上述改进算法考虑的是两轮车的运动学特性，不适用于无人驾驶汽车的路径规划。由于在车辆的实际运动中需要路线是平滑的，具有连续的曲率的和不能有较大的转折的。而传统A*算法所规划的路径则存在这些问题。
技术实现思路
本专利技术的目的在于解决上述现有技术存在的缺陷，提供一种路径规划方法，该方法对车辆进行建模分析，得到最小的转弯半径和最大的曲率。同时，提出方向代价解决车辆在起始时刻方向盘打死问题。然后，对提取出来的转折点进行带有阿克曼转向约束的路径优化，使A*算法所规划出的路径更加符合车辆的运动学特性。一种路径规划方法，包括以下步骤：步骤1：初始化起点、终点的方向；步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角是否大于如果则用y＝k*x+b估计移动代价；如果则用y＝a*x2+b*x+c估计移动路径代价；步骤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)1＝a*x2+b*x+c，同时，判断提取出来的转折点中，是否包含终点，如果含有终点，则进入步骤13,否则执行下一步；步骤12：对y(x)1＝a*x2+b*x+c求一阶导和二阶导，带入公式(10)中，判断是否满足公式(11)，如果满足，则进入步骤13；否则移动转折点，判断移动次数是否小于3，如果小于3，更新最小的曲率并重新执行步骤12，如果大于3，则保留曲率最小的转折点，然后进入步骤13；所述更新最小的曲率包括：移动转折点pose[i-1]，首先求出y(x)1的对称轴如公式(12)所示，判断所提取的出来的转折点是否大于同时，通过判断a的正负号确定y(x)1的开口朝向，如a＞0且则将pose[i-1]向左上角移动一个栅格，如果a＞0且则将pose[i-1]向右上角移动一个栅格，如果a＜0且则将pose[i-1]向左下角移动一个栅格，如果a＜0且则将pose[i-1]向右下角移动一个栅格；步骤13：拟合转折点，进行插值；步骤14：结束规划，输出路径Path；K＜Kmax(11)indexnew是移动后的节点的索引,index是此刻节点的索引，nx表示地图的宽度。有益效果：本专利技术通过建立车辆模型的约束，提出改进A*算法克服了传统的A*算法转折点多，路线不平滑和曲率不完整等问题。同时，考虑车辆的初始航向提出方向代价，能够较好的解决初始航向过大，方向盘打死的问题。通过在实验证明，与传统A*算法相比，改进的A*算法具有规划路径光滑、转折次数大幅减少，规划路径较短等优点，且避免了方向打死的问题，更适合于无人车的运动学特性。附图说明图1为本专利技术野马E70电动汽车的简化的车辆模型；图2为采用传统A*算法的曼哈顿距离所规划的局部路径；图3为采用本专利技术改进启发函数所规划的局部路径；图4为当时，改进h(n)所规划的路径；图5为未加方向代价的路径点；图6为加入方向代价的路径点；图7为未加入到路沿惩罚代价的路径点图8为加入到路沿惩罚代价的路径点图9为改进A*算法流程图；图10为转折点移动算法流程图；图11为某学校的校园地图；图12(a)为利用改进A*算法在图11中区域一测试的结果；图12(b)为利用传统A*算法在图11中区域一测试的结果；图13(a)为利用改进A*算法在图11中区域二测试的结果；图13(b)为利用传统A*算法在图11中区域二测试的结果。具体实施方式为使本专利技术的目的、技术方案和优点更加清楚，下面本专利技术中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。车辆的运动学模型由于汽车是一种典型的含有非完整性约束系统，本专利技术以平台野马E70电动汽车的简化车辆模型为例，如图1所示，在惯性坐标系XY下，为车体的横摆角(航向角)，L是轴距，R是转弯半径，θ为车辆纵轴与x轴之间的夹角。由于车辆只能用前轮转向，所以需要满足阿克曼转向条件只考虑车辆在二维平面上运动，不考虑俯仰和侧倾带来的影响，则该车辆的动力学模型为dx*sin(θ)-dy*cos(θ)＝0(1)其中，Kmax车辆最大转弯曲率本文档来自技高网...

【技术保护点】
1.一种路径规划方法，其特征在于，包括以下步骤：步骤1：初始化起点、终点的方向；步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角

【技术特征摘要】
1.一种路径规划方法，其特征在于，包括以下步骤：步骤1：初始化起点、终点的方向；步骤2：判断起点和终点之间是否有障碍物，如果存在障碍物，则进入步骤3；如果没有障碍物，再进一步判断车身朝向与当前点和目标点构成的直线之间的夹角是否大于如果则用y＝k*x+b估计移动代价；如果则用y＝a*x2+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；如果该扩展节点为终点，则将关闭节点...

【专利技术属性】
技术研发人员：付克昌，杨瑶，蒋涛，袁建英，郭德全，刘甲甲，周楠，张国良，
申请(专利权)人：成都信息工程大学，
类型：发明
国别省市：四川,51