CN112504288B - 基于动态规划的局部路径规划方法 - Google Patents

Abstract

本发明提供了一种基于动态规划的局部路径规划方法，包括：获取当前参考路径；当当前层不为当前参考路径的终点时，确定当前层的每个节点与上一层的每个节点间的圆弧曲线，再进行筛选，直至得到终点的上一层与终点的上一层的上一层的第一目标圆弧曲线；当当前层为当前参考路径的终点时，确定终点与终点的上一层的每个节点的圆弧曲线；对终点与终点的上一层的每个节点的圆弧曲线进行筛选，确定终点与终点的上一层间的第二目标圆弧曲线；以第二目标圆弧曲线对应的终点的上一层的节点为目标节点，直至确定起点的下一层与起点间的第二目标圆弧曲线；将相邻层的第二目标圆弧曲线进行拼接，得到局部路径。

Description

基于动态规划的局部路径规划方法

技术领域

本发明涉及数据处理领域，尤其涉及一种基于动态规划的局部路径规划方法。

背景技术

随着人工智能技术以及现代制造业的发展，无人驾驶技术已经逐渐走进人们的日常生活，并潜移默化的改变着人们的出行方式。无人驾驶技术可以简要的分为感知、预测、定位、决策、规划与控制这几个方面。规划通常指路径规划方法，主要任务是指根据当前车俩信息合理探索环境空间，最终规划出一条便于控制器执行，且无碰撞的路径。

局部路径规划是无人驾驶车辆智能性的最直观体现，如果车辆被临时出现的障碍物所遮挡，可以通过局部路径规划，快速准确的规划出一条躲避障碍物的平滑路径。

目前无人驾驶局部路径规划方案主要分为随机采样算法、启发式搜索算法以及动态规划方法三种，下面对这三种局部路径规划方案进行描述：

第一、随机采样算法是一种经典的树搜索算法，其中最著名的就是快速扩展随机树法(Randomly Exploring Randomized Trees，RRT)。RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。该方法是概率完备且不最优的。但是随机采样算法存在路径突变等问题，需要通过后续优化生成符合车辆动力学的路径。

2.启发式搜索算法以动态窗口(Dynamic Window Approach，DWA)算法、A*(A-star)算法以及其变种算法等为代表。其主要思想是根据车辆速度、角速度、转弯半径等约束，在空间内进行采样。采样后对每个得到的采样点通过设计的启发式函数进行评价与筛选，择优后重复继续探索直至探索值设定的终点。在探索至终点后，依据探索时设定的“父节点”得到最终的搜索路径。

第三、动态规划方法大多选择在车辆参考路径两侧进行逐层采样，相邻两层之间通过多项式曲线进行连接，通过长度、平滑度、碰撞性等方式对曲线进行筛选，最终将所有层间曲线相连得到搜索路径。

但是上述三种局部路径规划方案存在下列问题：

随机采样算法规划的路径不可避免的存在一定的不确定性，且采样点的随机性导致了路径的波动性大，所以基于随机采样算法需要对路径进行进一步的优化，这大大增加了算法的耗时，增加了无人驾驶车辆的实时路径规划的负担。

启发式算法由于存在大量探索采样点，每个采样点都需要通过设计的启发值函数进行评价，因此通常计算量很大。同时，启发值函数对搜索结果影响很大，不同启发值函数得到的结果往往截然不同，因此启发值函数的设计存在较多的不确定性。启发式算法的优势是搜索能力较强，能够以较高的准确度搜索到终点，但是在绝大多数实际驾驶环境中，车辆往往不需要严格的按照所谓“最优”路径行驶，只需要躲避障碍物的路径足够符合驾驶习惯，因此启发式算法无法平衡计算复杂度与结果合理性。

动态规划方法在采样层的每个采样点上，都采用与原始参考路径的路点的朝向相同的方向进行采样，而原始参考路径的路点的朝向影响下一采样层的可选节点的个数，同理采样点的朝向也对下一采样层可选节点的个数造成影响，当路点的朝向不佳时，比如，路点的朝向与下一个采样层角度越不接近直角，导致采样点的朝向也不佳，这样的处理限制了路径搜索的范围，降低了搜索能力。同时，采用多项式曲线连接相邻采样层的方式往往存在寻优操作，存在着较大计算量以及局部最优的可能。

发明内容

本发明实施例的目的是提供一种基于动态规划的局部路径规划方法，以解决现有技术中存在的上述问题。

为解决上述问题，本发明提供了一种基于动态规划的局部路径规划方法，所述基于动态规划的局部路径规划方法包括：

获取当前参考路径；所述当前参考路径包括多个路点；

当所述当前参考路径上存在障碍物时，在所述多个路点的每个路点的法线方向设置线段，每个所述线段为一个层，每层包括多个等间隔的节点；每个所述路点的法线经过路点，且和路点与下一路点的连线相垂直；

当当前层不为所述当前参考路径的终点时，确定所述当前层的每个节点与上一层的每个节点间的圆弧曲线；

对所述圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线，直至得到终点的上一层与终点的上一层的上一层的第一目标圆弧曲线；

当所述当前层为所述当前参考路径的终点时，确定终点与终点的上一层的每个节点的圆弧曲线；

对所述终点与终点的上一层的每个节点的圆弧曲线进行筛选，确定终点与终点的上一层间的第二目标圆弧曲线；

以所述第二目标圆弧曲线对应的终点的上一层的节点为目标节点，继续对终点的上一层的上一层的第一目标圆弧曲线进行筛选，确定终点的上一层与终点的上一层的上一层间的第二目标圆弧曲线，直至确定起点的下一层与起点间的第二目标圆弧曲线；

将相邻层的第二目标圆弧曲线进行拼接，得到局部路径。

在一种可能的实现方式中，所述确定所述当前层的每个节点与上一层的每个节点间的圆弧曲线具体包括：

确定当前层的每个节点的位置信息；

确定当前层的上一层的每个节点的位置信息；

根据当前层的一个节点的位置信息和当前层的上一层的一个节点的位置信息，确定圆弧曲线的圆心位置；

计算所述圆心与所述当前层的一个节点的连线和所述圆心与当前层的上一层的一个节点的连线的夹角；

根据所述圆心位置、所述夹角、所述当前层的一个节点的位置和所述当前层的上一层的一个节点的位置，计算所述当前层的一个节点至当前层的上一层的一个节点的圆弧曲线。

在一种可能的实现方式中，所述对所述圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线之前，还包括：

根据当前层的一个节点与当前层的上一层的一个节点间的圆弧曲线，确定经过当前层的一个节点的圆弧曲线的切线；

确定所述当前层的一个节点与当前层的上一层的一个节点间的圆弧曲线的曲率；

根据所述曲率，确定最小转弯半径；

根据所述最小转弯半径和所述切线，确定和所述切线相切的、以所述最小转弯半径为半径的两个最小转弯半径圆；

判断是否存在和所述最小转弯半径圆相切，且和所述当前层的下一层相切的切圆；

当存在两个切圆时，根据所述两个切圆，确定所述当前层的下一层的可选节点；

当不存在切圆时，根据所述最小转弯半径圆，确定当前层的下一层的可选节点；

当同时存在切圆和最小转弯半径圆时，根据所述切圆和所述最小转弯半径圆确定当前层的下一层的可选节点。

在一种可能的实现方式中，所述确定经过当前层的一个节点的圆弧曲线的切线之后还包括：

根据所述切线，确定当前层的一个节点的朝向角；所述朝向角包括第一朝向角和第二朝向角。

在一种可能的实现方式中，所述当存在两个切圆时，根据所述两个切圆，确定所述当前层的下一层的可选节点具体包括：

当所述朝向角为第一朝向角，且存在两个切圆时，确定所述两个切圆与当前层的下一层的两个切点；

确定当前层的下一层的节点中，处于所述两个切点之间的节点为可选节点。

在一种可能的实现方式中，所述当不存在切圆时，根据所述最小转弯半径圆，确定当前层的下一层的可选节点具体包括：

当所述朝向角为第一朝向，且不存在切圆时，确定两个所述最小转弯半径圆与所述当前层的下一层的两个交点；

确定当前层的下一层的节点中，处于所述两个交点之间的节点为可选节点。

在一种可能的实现方式中，所述当同时存在切圆和最小转弯半径圆时，根据所述切圆和所述最小转弯半径圆确定当前层的下一层的可选节点具体包括：

当所述朝向角为第一朝向角，且同时存在切圆和最小转弯半径圆时，确定所述最小转弯半径圆与所述当前层的下一层的一个交点，以及确定所述切圆与所述当前层的下一层的一个切点；

根据当前层的下一层的节点中，处于所述一个交点和所述一个节点之间的节点为可选节点。

在一种可能的实现方式中，所述方法还包括：

当所述朝向角为第二朝向角时，且同时存在切圆和最小转弯半径圆时，确定所述最小转弯半径圆与所述当前层的下一层的一个交点，确定所述切圆与所述当前层的下一层的一个切点；

根据当前层的下一层中，处于所述一个交点和所述一个切点之外的节点为可选节点。

在一种可能的实现方式中，所述对所述圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线具体包括：

确定当前层的一个节点与当前层的上一层的每个节点间的圆弧曲线对应的可选节点的数量；

根据所述数量和所述圆弧曲线的长度，确定圆弧曲线的评价值；

确定评价值最高的圆弧曲线为当前层的一个节点与当前层的上一层的节点的第一目标圆弧曲线。

在一种可能的实现方式中，所述根据所述数量和所述圆弧曲线的长度，确定圆弧曲线的评价值具体包括：

根据所述数量对应的第一函数，计算第一数值；

根据所述第一数值和所述圆弧曲线长度的和值，确定评价值。

第二方面，本发明提供了一种设备，包括存储器和处理器，存储器用于存储程序，处理器用于执行第一方面任一所述的方法。

第三方面，本发明提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行如第一方面任一所述的方法。

第四方面，本发明提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如第一方面任一所述的方法。

通过应用本发明实施例提供的基于动态规划的局部路径规划方法，将当前参考路径划分为多个层，通过评价值，筛选出相邻层之间的第一目标圆弧曲线后，随后以当前参考路径的终点作为反向搜索的起始点，确定出当前参考路径的终点至当前参考路径的终点的上一层的唯一的一个地儿目标圆弧曲线，逐一向上搜索，直至找到相邻层之间唯一的第二目标圆弧曲线，最终将第二目标圆弧曲线拼接得到局部路径。进行评价值计算的过程中，充分考虑了圆弧曲线的长度、碰撞属性以及级联筛选，得到的第一目标圆弧曲线符合运动学规则，且大大减少了运算量。

本申请中，将避障问题转化为层间连接的子问题，通过圆弧曲线连接层间节点，天然具有满足车辆运动学约束的优点，能够做到快速简便的生成避障路径，级联筛选，辅助层间路径的筛选，使得路径选择时具有一定的预见性，减少不必要的计算量，通过评价值，能够有一定“预见性”的对路径进行择优，使得路径搜索更加准确、直接。

附图说明

图1为本发明实施例一提供的基于动态规划的局部路径规划方法流程示意图；

图2为本发明实施例一提供的车辆路线示意图；

图3为本发明实施例一提供的纵向切分示意图；

图4为本发明实施例一提供的层间连接示意图；

图5为本发明实施例一提供的圆弧曲线示意图；

图6为本发明实施例一提供的级联筛选朝向角判断示意图；

图7为本发明实施例一提供的第一朝向角时，存在两个切圆时确定可选节点的示意图；

图8为本发明实施例一提供的第一朝向角时，存在两个最小转弯半径圆时确定可选节点的示意图；

图9为本发明实施例一提供的第一朝向角时，存在切圆和最小转弯半径圆时确定可选节点的示意图；

图10为本发明实施例一提供的第二朝向角时，存在切圆和最小转弯半径圆时确定可选节点的示意图；

图11为本发明实施例一提供的局部路径示意图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

图1为本发明实施例一提供的基于动态规划的局部路径规划方法流程示意图。本申请的执行主体为具有计算功能的终端、服务器或者处理器。本申请以将该方法应用在无人驾驶车辆为例进行说明，当将该方法应用在无人驾驶车辆时，该方法的执行主体为自动驾驶车辆控制单元(Automated Vehicle Control Unit，AVCU)，即无人驾驶车辆的中央处理器相当于无人驾驶车辆的“大脑”。本申请还包括以下步骤：

步骤110，获取当前参考路径；当前参考路径包括多个路点。

具体的，无人驾驶车辆在行驶过程中，会进行路径规划，从而规划出全局路径，规划得到的一个全局路径，可以作为当前参考路径，车辆可以根据当前参考路径进行行驶，但是，参见图2，第一个矩形框可以表示车辆当前位置，第二个矩形框表示存在的第一障碍物，第三矩形框表示存在的第二障碍物。当当前参考路径上存在突然出现的障碍物，此时，车辆如果继续根据当前参考路径进行行驶，会存在碰撞风险，因此，可以对当前参考路径进行进一步的局部规划，以避免发生碰撞当前参考路径当前参考路径。

步骤120，当当前参考路径上存在障碍物时，在多个路点的每个路点的法线方向设置线段，每个线段为一个层，每层包括多个等间隔的节点；每个路点的法线经过路点，且和路点与下一路点的连线相垂直。

其中，线段的长度以多个路点中的中间路点为中心，向左右两侧依次递减，或者，线段是等长的，每层的节点个数是相同的。下面以左右两侧的线段长度依次递减为例进行说明。可以等间隔的在当前参考路径上取多个路点，可以取多个路点中的中心路点，在每个路点的法线方向上，以中心路点的法线方向，设置一条线段，并且以中心路点为中心，在其他路点的法线方向上，做长度依次向两边递减的线段。每个线段上，设置等间隔的多个节点。每个线段上的节点之间的距离相等。参见图3，图3为本发明实施例一提供的纵向切分示意图，在车辆位置的中心点处，表示当前参考路径的起点，后面的虚线，依次表示第一层、第二层、第三层……直至车辆的终点。

需要说明的是，线段的长度和车辆的动力学模型相关，比如，线段的长度可以是车辆的长度的倍数或者车辆的宽度的倍数等，此处的倍数，是多次实验的经验值。

步骤130，当当前层不为当前参考路径的终点时，确定当前层的每个节点与上一层的每个节点间的圆弧曲线。

具体的，本申请可以通过圆弧曲线进行层与层之间的连接，由于采用动态规划的思想，由起点到终点的局部路径规划可以分割为每个层和相邻层的路径规划，对于相邻层的路径规划，在具体规划时分为两种情况，第一种是当前层不为当前参考路径的终点，第二种是当前层为当前参考路径的终点。

对于当前层并非当前参考路径的终点时，可以确定当前层的每个节点与上一层的节点间的圆弧曲线。参见图4，图4为本发明实施例一提供的层间连接示意图，当前层可以是节点A所在的层，节点B1、B2和B3所在的层为当前层的上一层的部分节点，当前层的一个节点，比如节点A至上一层的每一个节点都具有一条圆弧曲线，比如B1A表示节点A与节点B1的圆弧曲线，B2A表示节点A和节点B2的圆弧曲线，B3A表示节点A和节点B2的圆弧曲线。

参见图5，图5为本发明实施例一提供的圆弧曲线示意图，将当前层的节点A和当前层的上一层的节点B的位置映射在全局坐标系oxy中，节点A的坐标为(x_a,y_a),节点B为当前层的上一层的任一节点，节点B的坐标为(x_b,y_b),圆弧曲线BA，计算圆弧曲线BA的方法如下：

首先可以确定当前层的每个节点的位置信息；其次，确定当前层的上一层的每个节点的位置信息；再次，根据当前层的一个节点的位置信息和当前层的上一层的一个节点的位置信息，确定圆弧曲线的圆心位置；接着，计算圆心与当前层的一个节点的连线和圆心与当前层的上一层的一个节点的连线的夹角；最后，根据圆心位置、夹角、当前层的一个节点的位置和当前层的上一层的一个节点的位置，计算当前层的一个节点至当前层的上一层的一个节点的圆弧曲线。

具体计算公式如下,计算圆心坐标(x₀,y₀):

在得到圆心坐标后，可以通过下列公式计算得到圆弧曲线方程：

其中，(x,y)∈AB表示坐标位于点AB区间内。

步骤140，对圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线，直至得到终点的上一层与终点的上一层的上一层的第一目标圆弧曲线。

具体的，对于当前层的每个节点，具有连接到上一层的每个节点的一条圆弧曲线，比如当前层有10个节点，当前层的上一层有8个节点，则当前层的一个节点和上一层的8个节点中的每个节点间都具有一条圆弧曲线，当前层的10个节点一共具有连接到上一层节点的80个圆弧曲线。

对于80个圆弧曲线，可以进行筛选，在筛选时，可以考虑圆弧曲线长短Len、碰撞属性和级联筛选N_L三个特征。

圆弧曲线长短Len指圆弧曲线的长度，碰撞属性是指圆弧曲线是否与障碍物发生碰撞，可以先筛选掉会与障碍物发生碰撞的圆弧曲线，然后仅仅考虑不会与障碍物发生碰撞的圆弧曲线。其中，可以通过障碍物的位置信息、障碍物的预测位置信息和圆弧曲线的位置，筛选掉会与障碍物或者障碍物的预测位置信息发生碰撞的圆弧曲线，具体筛选方式为本领域技术人员所公知的，此处不再赘述。

对于不会与障碍物发生碰撞的圆弧曲线，可以从圆弧曲线长短Len和级联筛选N_L两个特征去进行评价，由于圆弧曲线长短Len是可以通过Len＝2α×R₀计算得到，其中2a是圆心角度数，R₀是半径。因此，只需要再对级联筛选N_L进行计算，即可确定最终的第一目标圆弧曲线。下面对如何进行级联筛选进行具体的说明。

在进行级联筛选时，先要考虑当前层的节点的朝向角，不同的朝向角会导致由节点出发的、能够选择的下一层的节点个数受到限制，N可以表示下一层可选节点的个数，下面对级联筛选进行具体的说明。

首先，根据当前层的一个节点与当前层的上一层的一个节点间的圆弧曲线，确定经过当前层的一个节点的圆弧曲线的切线,确定当前层的一个节点与当前层的上一层的一个节点间的圆弧曲线的曲率；其次，根据曲率，确定最小转弯半径；再次，根据最小转弯半径和切线，确定和切线相切的、以最小转弯半径为半径的两个最小转弯半径圆；再次，判断是否存在和最小转弯半径圆相切，且和当前层的下一层相切的切圆；接着，当存在两个切圆时，根据两个切圆，确定当前层的下一层的可选节点；接着，当不存在切圆时，根据最小转弯半径圆，确定当前层的下一层的可选节点；最后，当同时存在切圆和最小转弯半径圆时，根据切圆和最小转弯半径圆确定当前层的下一层的可选节点。

其中，对于圆弧曲线的曲率，可以通过现有公式进行计算，此处不再赘述，随后，可以通过曲率取倒数，得到最小转弯半径，以最小转弯半径为最小转弯半径圆的半径，得到最小转弯半径圆。最小转弯半径圆上的一个点为当前层的节点，切圆和最小转弯半径圆在当前层的节点相切，且需要和下一层相切，因此，切圆的数量可能是0个、1个或者2个中的任意一种。

上述确定下一层的可选节点的过程中，存在三种情形，下面结合当前层的节点的朝向角，对上述三种情形进行具体的描述。

可以根据上面过程中所计算的切线，确定当前层的一个节点的朝向角；朝向角包括第一朝向角和第二朝向角。比如，参见图6，图6为本发明实施例一提供的级联筛选朝向角判断示意图，当前层的节点A，与上一层的每个节点的圆弧曲线都具有一个过节点A，且和对应的圆弧曲线相切的切线，

表示节点A指向切线延长线上的某一点A′的向量，

表示节点A指向切线与下一层L交点A″的向量，两个向量

的内积的计算结果可以为正值或者负值，当为正值时，可以作为第一朝向角，当为负值时，可以作为第二朝向角，即第一朝向角指向下一层，第二朝向角背离下一层。

下面结合不同的朝向角，对上述的可选节点的选择进行更为详细的说明。通过第一朝向角和最小转弯半径圆、切圆来确定可选节点一般可以包括下面四种情况。

第一种情况、第一朝向角+两个切圆

当朝向角为第一朝向角，且存在两个切圆时，确定两个切圆与当前层的下一层的两个切点；确定当前层的下一层的节点中，处于两个切点之间的节点为可选节点。

具体的，参见图7，图7为本发明实施例一提供的第一朝向角时，存在两个切圆时确定可选节点的示意图。过节点A的每个最小转弯半径圆，处于左边的可以记为R_L，处于右边的可以记为R_R都具有一个与最小转弯半径圆和下一层相切的外切圆，此时，图7中的上面的外切圆与层L的切点t₁在层L的延长线上，另一个外切圆与层L的切点t₂在层L上。因此，可选节点N为当前层的下一层的节点中，处于两个切点之间的节点。

第二种情况、第一朝向角+最小转弯半径圆

当朝向角为第一朝向，且不存在切圆时，确定两个最小转弯半径圆与当前层的下一层的两个交点；确定当前层的下一层的节点中，处于两个交点之间的节点为可选节点。

具体的，参见图8，图8为本发明实施例一提供的第一朝向角时，存在两个最小转弯半径圆时确定可选节点的示意图。图8中，带有箭头的线为圆弧曲线BA的切线，切线上方的为可以作为左边的最小转弯半径圆，切线下方的为右边的最小转弯半径圆，两个最小转弯半径圆皆不存在与节点A相切，且和层L相切的外切圆，此时，左边的最小转弯半径圆与层L的交点为t₁，右边的最小转弯半径圆与层L的交点为t₂。因此，可选节点N为当前层的下一层L的节点中，处于两个交点之间的节点。

第三种情况、第一朝向角+切圆+最小转弯半径圆

当朝向角为第一朝向角，且同时存在切圆和最小转弯半径圆时，确定最小转弯半径圆与当前层的下一层的一个交点，以及确定切圆与当前层的下一层的一个切点；根据当前层的下一层的节点中，处于一个交点和一个节点之间的节点为可选节点。

具体的，参见图9，图9为本发明实施例一提供的第一朝向角时，存在切圆和最小转弯半径圆时确定可选节点的示意图。图9中，带有箭头的线为圆弧曲线BA的切线，切线上方的可以作为左边的最小转弯半径圆，与下一层L的交点为t₁，切线下方的可以作为右边的最小转弯半径圆，右边的最小转弯半径圆具有切圆，切圆与下一层L的切点为t₂。因此，可选节点N为L层中，处于交点与切点之间的节点。

第四种情况、第二朝向角+切圆+最小转弯半径圆

当朝向角为第二朝向角时，且同时存在切圆和最小转弯半径圆时，确定最小转弯半径圆与当前层的下一层的一个交点，确定切圆与当前层的下一层的一个切点；根据当前层的下一层中，处于一个交点和一个切点之外的节点为可选节点。

具体的，参见图10，图10为本发明实施例一提供的第二朝向角时，存在切圆和最小转弯半径圆时确定可选节点的示意图。图10中，带有箭头的线为圆弧曲线BA的切线，切线左边的可以作为左边的最小转弯半径圆，其具有外切圆，外切圆与下一层L的切点为t₂，切线右边的可以作为右边的最小转弯半径圆，其与下一层L具有两个交点，此时可选节点为t₁和t₂之外的节点，图10中t₂之外没有节点，则可选节点为t₁之外的节点。

在图7-图10中，分别以R_L表示左边的最小转弯半径圆，以R_R表示右侧的最小转弯半径圆，以

表示R_L的切圆，以

表示R_R的切圆。

在确定当前层的一个节点与上一层的每个节点对应的可选节点后，可以根据可选节点的数量，对当前层的一个节点与上一层的每个节点的圆弧曲线进行评价，从而得到当前层的一个节点与上一层的节点的一条圆弧曲线，并将其作为第一目标圆弧曲线，相应的，当前层有多少个节点，则当前层的第一目标圆弧曲线的个数就有多少个。下面对如何确定第一目标圆弧曲线进行具体的说明。

首先，确定当前层的一个节点与当前层的上一层的每个节点间的圆弧曲线对应的可选节点的数量；其次，根据数量和圆弧曲线的长度，确定圆弧曲线的评价值；最后，确定评价值最高的圆弧曲线为当前层的一个节点与当前层的上一层的节点的第一目标圆弧曲线。

其中，根据可选节点的数量和圆弧曲线的长度，确定圆弧曲线的评价值具体包括：根据可选节点的数量对应的第一函数，计算第一数值；根据第一数值和圆弧曲线长度的和值，确定评价值。

第一函数可以参见公式(3)：

N_L＝K·N 公式(3)

其中，N_L为级联筛选的第一数值，N为可选节点个数，K为常数，可以根据经验值设定。

而评价值可以通过公式(4)进行计算：

f＝Len+N_L 公式(4)

其中，f为评价值，Len为圆弧曲线长度。

比如，当前层的节点A与上一层节点B1的可选节点为5，与节点B2的可选节点为2，与节点B3的可选节点为4，再结合节点A分别与B1、B2和B3的圆弧曲线的长度，假设长度依次记为3、4、5，k为1.5，则可以根据公式(3)和(4)，计算得到当前层的节点A与上一层节点B1、B2和B3的评价值依次为10.5，7，11，则可以确定AB3之间的圆弧曲线为第一目标圆弧曲线，从而据此可以确定当前层的每个节点至上一层的节点的第一目标圆弧曲线。

可以理解的是，步骤140中，需要确定出除当前路径的终点所在的层外的其它层之间的第一目标圆弧曲线，当前层的第一目标圆弧曲线的个数与当前层的节点个数相同，即当前层的每个节点都具有一个与当前层的上一层相连的第一目标圆弧曲线。

步骤150，当当前层为当前参考路径的终点时，确定终点与终点的上一层的每个节点的圆弧曲线。

此处，确定终点与终点上一层的每个节点的圆弧曲线的方法参见公式(1)和(2)中的计算方法，此处不再赘述。

步骤160，对终点与终点的上一层的每个节点的圆弧曲线进行筛选，确定终点与终点的上一层间的第二目标圆弧曲线。

具体的，此处在计算终点与终点的上一层间的第二目标圆弧曲线时，进行的是反向计算，即是与之前的顺序相反，倒序进行，即以终点的上一层为当前层，以终点为当前层的上一层，进行计算，从而确定出终点、终点的上一层之间的圆弧曲线分别对应的可选节点，然后根据公式(3)和(4)，进行筛选，将评价值最高的确定为第二目标圆弧曲线，此时第二目标圆弧曲线的个数是唯一一个。

步骤170，以第二目标圆弧曲线对应的终点的上一层的节点为目标节点，继续对终点的上一层的上一层的第一目标圆弧曲线进行筛选，确定终点的上一层与终点的上一层的上一层间的第二目标圆弧曲线，直至确定起点的下一层与起点间的第二目标圆弧曲线。

具体的，在确定终点与终点的上一层的唯一的第二目标圆弧曲线后，即可确定与终点连接的终点的上一层的节点，继续搜索该节点的对应的第一目标圆弧曲线，将该节点对应的第一目标圆弧曲线作为第二目标圆弧曲线，继续进行搜索，直到确定参考路径起点与参考路径起点的下一层的唯一的第二目标圆弧曲线为止。

步骤180，将相邻层的第二目标圆弧曲线进行拼接，得到局部路径。

具体的，相邻层的第二目标圆弧曲线进行拼接，即可得到局部路径，得到的局部路径参见图11，从而可以得到局部路径，避免发生碰撞。

通过应用本发明实施例提供的基于动态规划的局部路径规划方法，将当前参考路径划分为多个层，通过评价值，筛选出相邻层之间的第一目标圆弧曲线后，随后以当前参考路径的终点作为反向搜索的起始点，确定出当前参考路径的终点至当前参考路径的终点的上一层的唯一的一个地儿目标圆弧曲线，逐一向上搜索，直至找到相邻层之间唯一的第二目标圆弧曲线，最终将第二目标圆弧曲线拼接得到局部路径。进行评价值计算的过程中，充分考虑了圆弧曲线的长度、碰撞属性以及级联筛选，得到的第一目标圆弧曲线符合运动学规则，且大大减少了运算量。

本申请中，将避障问题转化为层间连接的子问题，通过圆弧曲线连接层间节点，天然具有满足车辆运动学约束的优点，能够做到快速简便的生成避障路径，级联筛选，辅助层间路径的筛选，使得路径选择时具有一定的预见性，减少不必要的计算量，通过评价值，能够有一定“预见性”的对路径进行择优，使得路径搜索更加准确、直接。

本发明实施例二提供了一种设备，包括存储器和处理器，存储器用于存储程序，存储器可通过总线与处理器连接。存储器可以是非易失存储器，例如硬盘驱动器和闪存，存储器中存储有软件程序和设备驱动程序。软件程序能够执行本发明实施例提供的上述方法的各种功能；设备驱动程序可以是网络和接口驱动程序。处理器用于执行软件程序，该软件程序被执行时，能够实现本发明实施例一提供的方法。

本发明实施例三提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行本发明实施例一提供的方法。

本发明实施例四提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现本发明实施例一提供的方法。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于动态规划的局部路径规划方法，其特征在于，所述基于动态规划的局部路径规划方法包括：

获取当前参考路径；所述当前参考路径包括多个路点；

当所述当前参考路径上存在障碍物时，在所述多个路点的每个路点的法线方向设置线段，每个所述线段为一个层，每层包括多个等间隔的节点；每个所述路点的法线经过路点，且和路点与下一路点的连线相垂直；

当当前层不为所述当前参考路径的终点时，确定所述当前层的每个节点与上一层的每个节点间的圆弧曲线；

对所述圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线，直至得到终点的上一层与终点的上一层的上一层的第一目标圆弧曲线；

当所述当前层为所述当前参考路径的终点时，确定终点与终点的上一层的每个节点的圆弧曲线；

对所述终点与终点的上一层的每个节点的圆弧曲线进行筛选，确定终点与终点的上一层间的第二目标圆弧曲线；

以所述第二目标圆弧曲线对应的终点的上一层的节点为目标节点，继续对终点的上一层的上一层的第一目标圆弧曲线进行筛选，确定终点的上一层与终点的上一层的上一层间的第二目标圆弧曲线，直至确定起点的下一层与起点间的第二目标圆弧曲线；

将相邻层的第二目标圆弧曲线进行拼接，得到局部路径。

2.根据权利要求1所述的方法，其特征在于，所述确定所述当前层的每个节点与上一层的每个节点间的圆弧曲线具体包括：

确定当前层的每个节点的位置信息；

确定当前层的上一层的每个节点的位置信息；

根据当前层的一个节点的位置信息和当前层的上一层的一个节点的位置信息，确定圆弧曲线的圆心位置；

计算所述圆心与所述当前层的一个节点的连线和所述圆心与当前层的上一层的一个节点的连线的夹角；

根据所述圆心位置、所述夹角、所述当前层的一个节点的位置和所述当前层的上一层的一个节点的位置，计算所述当前层的一个节点至当前层的上一层的一个节点的圆弧曲线。

3.根据权利要求1所述的方法，其特征在于，所述对所述圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线之前，还包括：

根据当前层的一个节点与当前层的上一层的一个节点间的圆弧曲线，确定经过当前层的一个节点的圆弧曲线的切线；

确定所述当前层的一个节点与当前层的上一层的一个节点间的圆弧曲线的曲率；

根据所述曲率，确定最小转弯半径；

根据所述最小转弯半径和所述切线，确定和所述切线相切的、以所述最小转弯半径为半径的两个最小转弯半径圆；

判断是否存在和所述最小转弯半径圆相切，且和所述当前层的下一层相切的切圆；

当存在两个切圆时，根据所述两个切圆，确定所述当前层的下一层的可选节点；

当不存在切圆时，根据所述最小转弯半径圆，确定当前层的下一层的可选节点；

当同时存在切圆和最小转弯半径圆时，根据所述切圆和所述最小转弯半径圆确定当前层的下一层的可选节点。

4.根据权利要求3所述的方法，其特征在于，所述确定经过当前层的一个节点的圆弧曲线的切线之后还包括：

根据所述切线，确定当前层的一个节点的朝向角；所述朝向角包括第一朝向角和第二朝向角。

5.根据权利要求4所述的方法，其特征在于，所述当存在两个切圆时，根据所述两个切圆，确定所述当前层的下一层的可选节点具体包括：

当所述朝向角为第一朝向角，且存在两个切圆时，确定所述两个切圆与当前层的下一层的两个切点；

确定当前层的下一层的节点中，处于所述两个切点之间的节点为可选节点。

6.根据权利要求4所述的方法，其特征在于，所述当不存在切圆时，根据所述最小转弯半径圆，确定当前层的下一层的可选节点具体包括：

当所述朝向角为第一朝向，且不存在切圆时，确定两个所述最小转弯半径圆与所述当前层的下一层的两个交点；

确定当前层的下一层的节点中，处于所述两个交点之间的节点为可选节点。

7.根据权利要求4所述的方法，其特征在于，所述当同时存在切圆和最小转弯半径圆时，根据所述切圆和所述最小转弯半径圆确定当前层的下一层的可选节点具体包括：

当所述朝向角为第一朝向角，且同时存在切圆和最小转弯半径圆时，确定所述最小转弯半径圆与所述当前层的下一层的一个交点，以及确定所述切圆与所述当前层的下一层的一个切点；

根据当前层的下一层的节点中，处于所述一个交点和所述一个切点之间的节点为可选节点。

8.根据权利要求4所述的方法，其特征在于，所述方法还包括：

当所述朝向角为第二朝向角时，且同时存在切圆和最小转弯半径圆时，确定所述最小转弯半径圆与所述当前层的下一层的一个交点，确定所述切圆与所述当前层的下一层的一个切点；

根据当前层的下一层中，处于所述一个交点和所述一个切点之外的节点为可选节点。

9.根据权利要求3所述的方法，其特征在于，所述对所述圆弧曲线进行筛选，得到与当前层的节点的数量相同的第一目标圆弧曲线具体包括：

确定当前层的一个节点与当前层的上一层的每个节点间的圆弧曲线对应的可选节点的数量；

根据所述数量和所述圆弧曲线的长度，确定圆弧曲线的评价值；

确定评价值最高的圆弧曲线为当前层的一个节点与当前层的上一层的节点的第一目标圆弧曲线。

10.根据权利要求9所述的方法，其特征在于，所述根据所述数量和所述圆弧曲线的长度，确定圆弧曲线的评价值具体包括：

根据所述数量对应的第一函数，计算第一数值；

根据所述第一数值和所述圆弧曲线长度的和值，确定评价值。

Images