CN109900289B - 基于闭环控制的路径规划方法及装置 - Google Patents

Abstract

本发明提供了一种基于闭环控制的路径规划方法，包括：获取参考路径；当以起点和第一终点间的距离为直径的圆内存在第一障碍物时，确定其第一距离；分别以起点和第一距离为半径设置第一父圆和第一终点圆；从第一父圆的子圆中确定第二父圆；当第m父圆与第一终点圆的重合度大于预设第一阈值时，生成第一和第二列表；在当前时刻，获取车辆的第一参数，并计算下一时刻的第二参数，从而确定第一轨迹集；根据第一列表和第二列表，对第一轨迹集进行处理；对处理后的第一轨迹集中的轨迹进行评价以确定第一目标轨迹，并计算其加速度和转向角度，当第n目标轨迹的终点与终点的距离之差小于预设第二阈值时，生成目标路径。由此降低了底层控制的控制难度。

Description

基于闭环控制的路径规划方法及装置

技术领域

本发明涉及数据处理技术领域，尤其涉及一种基于闭环控制的路径规划方法及装置。

背景技术

随着人工智能技术以及现代制造业的发展，自动驾驶技术已经逐渐走进人们的日常生活，潜移默化的改变着人们的出行方式。无人驾驶技术可以简要的分为感知、预测、定位、决策、规划与控制这几个方面。路径规划方法主要任务是指根据当前车辆信息、合理探索环境空间、最终规划出一条便于控制器执行、无碰撞的路径。运动规划在路径规划的基础上，对路径点的速度、转向角等信息进行设置，使得路径点信息更加完善。

对于路径规划算法来说，除了保证在复杂环境下的算法完备性、实时性之外，所生成的轨迹是否对控制模块友好，也是一个重要的评判标准。如果轨迹生成无法适应车辆模型，那么在不利于保证控制精度的同时，也对控制器的设计提出了很高的要求。

目前包含运动规划的路径规划算法一般基于随机采样的方法、基于优化的方法等。

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

基于优化的方法通过在路径生成的过程中，通过最小化代价函数来进行路径优化。代价函数一般包括障碍物距离限制、车辆加速度、方向盘转速度限制、地形限制等。同时，在优化项中增加对车辆运动信号的规划，通过不同的方法逼近车辆最佳路径。

基于随机采样的方法路径不可避免的存在一定的不确定性，且采样点的随机性导致了原始路径的波动特性，所以大部分基于随机采样的方法需要对路径进行进一步的优化，这大大增加了算法的耗时，无法满足无人车的实时路径规划需求。

基于优化的方法需要大量的计算资源且难以并行计算，同时对优化项的设计及优化方法的选用有着很高的要求，由于优化方法的复杂性，基于优化的规划算法没有确定的计算时间，且基于优化的方法缺乏全局知识，有时会陷入局部最优。

上述路径规划方法中，都没有考虑控制模块，即底层控制器的控制难度。

发明内容

本发明实施例的目的是提供一种基于闭环控制的路径规划方法及装置，以解决现有计算中存在的路径规划中不考虑底层控制器的控制难度的问题。

为解决上述问题，第一方面，本发明提供了一种基于闭环控制的路径规划方法，所述方法包括：

获取车辆的参考路径，所述参考路径包括起点和终点；

在所述参考路径上取第一终点；所述第一终点位于所述起点和终点之间；

判断以所述起点和所述第一终点间的距离为直径的圆内是否存在障碍物；

当存在第一障碍物时，确定所述起点与所述第一障碍物的第一距离；所述第一障碍物为障碍物中距离起点最近的障碍物；所述第一距离为所述起点距离所述第一障碍物的距离减去自车的安全距离之差；

以所述起点为圆心，以所述第一距离为半径，设置第一父圆；

以所述第一终点为圆心，以所述第一距离为半径，设置第一终点圆；

以所述第一父圆的圆周上的点为圆心，确定该圆周上的多个圆心中的每个与距离其最近的障碍物的距离，生成子距离集；

计算该圆周上的多个圆心中的每个与所述第一终点的几何距离，生成几何距离集；

根据所述子距离集和所述几何距离集，计算多个圆心对应的子圆的启发值集；所述子圆的半径为圆心与距离其最近的障碍物的距离减去自车的安全距离之差；

确定所述启发值集中最小的启发值对应的子圆为第二父圆；

当所述第m父圆与所述第一终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表；所述第一列表包括所述第一父圆至第m父圆的圆心位置和半径；所述第二列表包括以所述第一父圆的圆周为圆心的子圆至以所述第m父圆的圆周为圆心的子圆中圆心的位置和半径；m为大于2的整数；

在当前时刻，获取所述车辆的第一参数；

根据所述当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数；

根据所述第一参数和所述第二参数，计算所述车辆从当前时刻至下一时刻的第一轨迹集；

根据所述第一列表和所述第二列表，对所述第一轨迹集进行处理，生成处理后的第一轨迹集；

通过启发值函数，对处理后的所述第一轨迹集中的轨迹进行评价；

根据评价结果，从处理后的所述第一轨迹集中确定第一目标轨迹；

在所述第一目标轨迹上确定预瞄点的位置；根据车辆的当前速度和所述预瞄点的位置，计算预瞄距离；

根据所述预瞄距离和车辆的动力学模型，计算预瞄点的转向角度；

根据所述预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度；

将所述第一目标轨迹的预瞄点的转向角度和加速度发送给底层控制器，以使所述底层控制器根据所述预瞄点的转向角度和加速度，在所述预瞄点对车辆进行控制；

当第n目标轨迹的终点与所述终点的距离之差小于预设第二阈值时，将所述第一目标轨迹至第n目标轨迹进行处理，生成目标路径；n为大于2的整数。

在一种可能的实现方式中，所述第一参数包括车辆在当前时刻的x坐标、y坐标、朝向、车速、方向盘转角，所述根据所述当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数，具体包括：

通过以下公式计算车辆在下一时刻的第二参数：

x_t+Δt＝x_t+vcosθcosβΔt

y_t+Δt＝y_t+vsinθcosβΔt

θ_t+Δt＝θ_t+vsinβΔt/l

v_t+Δt＝v_t+aΔt

β_t+Δt＝β_t+ωΔt

其中，x_t为车辆在当前时刻的x坐标、y_t为车辆在当前时刻的y坐标,θ_t为车辆在当前时刻的朝向，v_t为车辆在当前时刻的速度，β_t为车辆在当前时刻的方向盘转角；x_t+Δt为车辆在下一时刻的x坐标、y_t+Δt为车辆在下一时刻的y坐标,θ_t+Δt为车辆在下一时刻的朝向，v_t+Δt为车辆在下一时刻的速度，β_t+Δt为车辆在下一时刻的方向盘转角；l为车辆轴距。

在一种可能的实现方式中，所述根据所述第二列表，对所述第一轨迹集进行处理，生成处理后的第一轨迹集，具体包括：

根据所述以所述第一父圆的圆周为圆心的子圆至以所述第m父圆的圆周为圆心的子圆中圆心的位置和半径，将所述第一轨迹集中处于所述子圆之外的轨迹删除。

在一种可能的实现方式中，所述通过启发值函数，对处理后的所述第一轨迹集中的轨迹进行评价，具体包括：

通过f＝g+h计算处理后的第一轨迹集中每条轨迹的启发值；

当轨迹的启发值最小时，确定该轨迹为第一目标轨迹；

其中，f为每条轨迹的启发值，g为从起点至车辆在下一时刻所在的位置的距离；h包括圆心引导项和终点引导项。

在一种可能的实现方式中，通过公式h＝l_next+l₁+l₂...l_dist计算h；

其中，l_next为圆心引导项，表示车辆在下一时刻的位置至最近圆心的距离，l₁+l₂...l_dist为终点引导项，表示距离车辆在下一时刻的位置最近的圆心到下一圆心距离、下一圆心到下下圆心距离…最后一个圆心到终点的距离之和。

在一种可能的实现方式中，所述当所述第m父圆与所述终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表之前，所述方法还包括：

当某一父圆的所有子圆的启发值都相等时，退回至该父圆的上一级父圆，从所述第一列表中删除该父圆的圆心的位置和半径。

在一种可能的实现方式中，所述根据所述预瞄距离和车辆的动力学模型，计算预瞄点的转向角度具体包括：

利用公式

计算转向角度；

其中，δ为转向角度，L为车辆动力学模型中的车辆轴距，l为车辆后轴距，L₀为预瞄距离，η为车辆相对于预瞄点方向的夹角。

在一种可能的实现方式中，利用公式

计算预瞄距离；其中，v为车辆的当前速度。

在一种可能的实现方式中，所述根据所述预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度具体包括：

利用公式

计算车辆加速度；

其中，α为加速度，Ki和Kp为经验常数，ν为车辆的当前速度。

第二方面，本发明提供了一种基于路径规划的车辆控制装置，所述装置包括：

获取单元，所述获取单元用于获取车辆的参考路径，所述参考路径包括起点和终点；

设置单元，所述设置单元用于在所述参考路径上取第一终点；所述第一终点位于所述起点和终点之间；

判断单元，所述判断单元用于判断以所述起点和所述第一终点间的距离为直径的圆内是否存在障碍物；

确定单元，所述确定单元用于当存在第一障碍物时，确定所述起点与所述第一障碍物的第一距离；所述第一障碍物为障碍物中距离起点最近的障碍物；所述第一距离为所述起点距离所述第一障碍物的距离减去自车的安全距离之差；

所述设置单元还用于，以所述起点为圆心，以所述第一距离为半径，设置第一父圆；

所述设置单元还用于，以所述第一终点为圆心，以所述第一距离为半径，设置第一终点圆；

所述确定单元还用于，以所述第一父圆的圆周上的点为圆心，确定该圆周上的多个圆心中的每个与距离其最近的障碍物的距离，生成子距离集；

计算单元，所述计算单元用于计算该圆周上的多个圆心中的每个与所述第一终点的几何距离，生成几何距离集；

所述计算单元还用于根据所述子距离集和所述几何距离集，计算多个圆心对应的子圆的启发值集；所述子圆的半径为圆心与距离其最近的障碍物的距离减去自车的安全距离之差；

所述确定单元还用于，确定所述启发值集中最小的启发值对应的子圆为第二父圆；

生成单元，所述生成单元用于当所述第m父圆与所述第一终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表；所述第一列表包括所述第一父圆至第m父圆的圆心位置和半径；所述第二列表包括以所述第一父圆的圆周为圆心的子圆至以所述第m父圆的圆周为圆心的子圆中圆心的位置和半径；m为大于2的整数；

所述获取单元还用于，在当前时刻，获取所述车辆的第一参数；

所述计算单元还用于，根据所述当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数；

所述计算单元还用于，根据所述第一参数和所述第二参数，计算所述车辆从当前时刻至下一时刻的第一轨迹集；

处理单元，所述处理单元用于根据所述第一列表和所述第二列表，对所述第一轨迹集进行处理，生成处理后的第一轨迹集；

评价单元，所述评价单元用于通过启发值函数，对处理后的所述第一轨迹集中的轨迹进行评价；

所述确定单元还用于，根据评价结果，从处理后的所述第一轨迹集中确定第一目标轨迹；

所述确定单元还用于在所述第一目标轨迹上确定预瞄点的位置；所述计算单元还用于，根据车辆的当前速度和所述预瞄点的位置，计算预瞄距离；

所述计算单元还用于，根据所述预瞄距离和车辆的动力学模型，计算预瞄点的转向角度；

所述计算单元还用于，根据所述预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度；

发送单元，所述发送单元用于将所述第一目标轨迹的预瞄点的转向角度和加速度发送给底层控制器，以使所述底层控制器根据所述预瞄点的转向角度和加速度，在所述预瞄点对车辆进行控制；

所述处理单元还用于当第n目标轨迹的终点与所述终点的距离之差小于预设第二阈值时，将所述第一目标轨迹至第n目标轨迹进行处理，生成目标路径；n为大于2的整数。

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

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

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

通过应用本发明提供的基于闭环控制的路径规划方法及装置，具有以下技术效果：

1、生成一系列探索圆，这些圆相当于一种快速的空间探索，其占满可通行空间。利用这些探索圆，引导启发式轨迹搜索的方向，使得启发式搜索的过程不仅考虑障碍物和终点，也考虑空间利用率，极大的增强了无人车辆规划路径的合理性。

2、生成路径的过程对车辆加速度和方向盘转速度进行采样，生成路径包含车辆的坐标、朝向、速度、方向盘转角等信息，因此生成的路径不仅在坐标、朝向上连续，在速度与方向盘转角上都是连续的，使得生成的路径更加合理，降低了无人车控制模块的控制难度、提高了实时性。

3、在路径规划中考虑到了横、纵向控制，保证了规划的实时性，降低了底层控制器的控制难度。

附图说明

图1为本发明实施一例提供的基于闭环控制的路径规划方法流程示意图；

图2为本发明实施例一提供的空间搜索示意图；

图3为本发明实施例一提供的启发式轨迹搜索示意图；

图4为发明实施例一提供的控制参数的示意图；

图5为本发明实施例二提供的基于闭环控制的路径规划装置结构示意图。

具体实施方式

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

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

图1为本发明实施例一提供的基于闭环控制的路径规划方法流程示意图。该方法应用在自动驾驶领域中，该方法的执行主体为车辆的计算处理单元，该计算处理单元可以是车辆控制单元，如图1所示，该方法包括以下步骤：

步骤101，获取车辆的参考路径，参考路径包括起点和终点。

其中，参考路径，指车辆离线或实时规划的全局路径，该路径不考虑道路上临时出现的障碍物。参考路径用来指导路径规划算法的规划目标，保证车辆在躲避障碍的同时，能够返回既定道路。

可以通过如下方法获取到参考路径，首先，接收服务器发送的行程的起点和行程的终点；然后，根据行程的起点和行程的终点，调用环境地图文件；最后，根据行程的起点、行程的终点和环境地图文件，生成参考路径。

其中，服务器可以接收用户终端发送的行程的起点或终点，环境地图文件可以是存储在服务器中，比如，车辆将包括自车当前位置的请求消息发送给服务器，服务器根据自车当前位置，将包括自车当前位置、起点和终点的环境地图文件发送给车辆。也可以是车辆中存储有环境地图文件。

车辆从服务器获取到行程的起点和终点，然后，根据环境地图文件，进行路径规划，生成参考路径。

其中，在步骤101之前，需要获取障碍物信息和车辆动力学模型。

障碍物信息主要通过从上游节点(如感知、预测等)获取障碍物位置、速度信息，将这些信息统一储存并按照距自车的相对距离大小进行排序。

车辆动力学模型主要包括车辆的长宽、车辆轴距、最小转弯半径、加速度极限值、安全距离等固有参数，其一般在配置文件中定义。

步骤102，在参考路径上取第一终点；第一终点位于起点和终点之间。

具体的，由于参考路径的长度一般较长，为了提高处理速度，可以对参考路径进行划分，比如，在参考路径上取一点，将该点称为第一终点，又比如，以参考路径的起点为一点，以D为直径做圆，该圆与参考路径的交点，可以称为第一终点。D为预设的长度。

步骤103，判断以起点和第一终点间的距离为直径的圆内是否存在障碍物。

具体的，由于在步骤101之前已经获取到障碍物信息，并且也已经对障碍物的位置进行了排序，因此，可以根据障碍物排序的结果，判断以起点和第一终点为圆上的两点的圆内，是否存在障碍物。

当不存在障碍物时，继续判断以第一终点为圆上的一点，以第二终点为圆上的另一点的圆内，是否存在障碍物。

其中，第二终点可以根据上述第一终点的确定方法来确定。

步骤104，当存在第一障碍物时，确定起点与第一障碍物的第一距离；第一障碍物为障碍物中距离起点最近的障碍物；第一距离为起点距离第一障碍物的距离减去自车的安全距离之差。

具体的，车辆动力学模型内已经包括了自车的安全距离。当在以起点和第一终点为圆上的两点的圆内，搜索到多个障碍物时，以距离起点最近的障碍物作为第一障碍物，并根据该第一障碍物的位置和起点的位置，计算起点与第一障碍物的距离。

随后，可以根据起点与第一障碍物的距离，计算第一距离。

其中，可以通过公式R＝L_obs-d_safetyL_obs计算第一距离，其中R为第一距离，L_obs为起点与第一障碍物的距离，d_safety为车辆的安全距离。

步骤105，以起点为圆心，以第一距离为半径，设置第一父圆。

步骤106，以第一终点为圆心，以第一距离为半径，设置第一终点圆。

具体的，参见图2，以起点为圆心，以第一距离为半径，生成第一父圆，参见图2最左边的深色圆，以第一终点为圆心，以第一距离为半径，生成终点圆，参见图2最右边的深色圆。

步骤107，以第一父圆的圆周上的点为圆心，确定该圆周上的多个圆心中的每个与距离其最近的障碍物的距离，生成子距离集。

具体的，在第一父圆的圆周上，以圆周上的点为圆心，搜索障碍物，比如，在第一父圆的圆周上，圆心的位置分别为R1、R2和R3处，搜索到障碍物，位置分别为A1、A2和A3，计算R1与A1、R2与A2、R3与A3的距离，生成的子距离集为{|R1-A1|、|R2-A2|、|R3-A3|}。

当然，此处的位置信息可以是经纬度信息，为了便于理解，此处仅对子距离集进行简单的示意。对于其具体的计算过程不做细化。

步骤108，计算该圆周上的多个圆心中的每个与第一终点的几何距离，生成几何距离集。

其中，几何距离包括但不限于欧拉距离、曼哈顿距离和杜宾距离。

对于具体的每种几何距离的计算，属于现有技术，本申请对此不再说明。

步骤109，根据子距离集和几何距离集，计算多个圆心对应的子圆的启发值集；子圆的半径为圆心与距离其最近的障碍物的距离减去自车的安全距离之差。

具体的，通过公式f＝d_end-R计算多个圆心对应的子圆的启发值；

其中，d_end为圆心至第一终点的几何距离，R为该子圆的半径。

步骤110，确定启发值集中最小的启发值对应的子圆为第二父圆。

具体的，通过上述计算，假设R1、R2、R3对应的子圆中，R2的启发值最小，则可以将R2作为第二父圆。

随后，可以重复步骤107至110，直到搜索到第m父圆。其中，m为大于2的整数。

步骤111，当第m父圆与第一终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表；第一列表包括第一父圆至第m父圆的圆心位置和半径；第二列表包括以第一父圆的圆周为圆心的子圆至以第m父圆的圆周为圆心的子圆中圆心的位置和半径。

具体的，此处的重合度，可以是两个圆的重合面积。当第m父圆与第一终点圆的重合面积大于预设的第一阈值时，可以确定从起点至第一终点的搜索结束。

随后，可以继续执行步骤102至步骤111，直至搜索完参考路径。该过程可以称为空间探索。

步骤112，在当前时刻，获取车辆的第一参数。

具体的，在车辆当前最大方向盘转角以及最大行驶加速度范围内，采样得到当前状态下方向盘转角以及加速度。即下文第一参数中的车速(通过加速度计算得到)和方向盘转角。。

其中，第一参数包括车辆在当前时刻的x坐标、y坐标、朝向、车速、方向盘转角。第二参数包括车辆在下一时刻的x坐标、y坐标、朝向、车速、方向盘转角。

步骤113，根据当前时刻的第一参数和车辆动力学模型，计算车辆在下一时刻的第二参数。

其中，步骤113具体包括：通过以下公式计算车辆在下一时刻的第二参数：

x_t+Δt＝x_t+vcosθcosβΔt

y_t+Δt＝y_t+vsinθcosβΔt

θ_t+Δt＝θ_t+vsinβΔt/l

v_t+Δt＝v_t+aΔt

β_t+Δt＝β_t+ωΔt

其中，x_t为车辆在当前时刻的x坐标、y_t为车辆在当前时刻的y坐标,θ_t为车辆在当前时刻的朝向，v_t为车辆在当前时刻的速度，β_t为车辆在当前时刻的方向盘转角；x_t+Δt为车辆在下一时刻的x坐标、y_t+Δt为车辆在下一时刻的y坐标,θ_t+Δt为车辆在下一时刻的朝向，v_t+Δt为车辆在下一时刻的速度，β_t+Δt为车辆在下一时刻的方向盘转角，l为车辆轴距。其中，车辆轴距属于车辆动力学模型中的参数之一，因此，在计算下一时刻的参数时，是考虑了车辆的动力学模型参数的，因此也是满足车辆动力学约束的。

步骤114，根据第一参数和第二参数，计算车辆从当前时刻至下一时刻的第一轨迹集。

具体的，可以通过模型，计算车辆从当前时刻至下一时刻的轨迹，得到第一轨迹集，参见图3，图3中的从左起第一个圆为第一父圆、第二个圆为第一父圆的子圆(其中当第一父圆的子圆的启发值最小时，其称为第二父圆)第三个圆为第二父圆的子圆(其中当第二父圆的子圆的启发值最小时，其称为第三父圆)，在当前时刻至下一时刻的第一轨迹集包括虚线11、虚线12、虚线13、实线14、虚线15。

当车辆行驶至下一时刻时，在下一时刻至下下一时刻之间的第二轨迹集包括虚线21、虚线22、实线23、虚线24、虚线25。其中，下一时刻至下下一时刻的第二轨迹集的确定方法，与计算当前时刻至下一时刻的第一轨迹集的方法相同，此处不再赘述。

步骤115，根据第一列表和第二列表，对第一轨迹集进行处理，生成处理后的第一轨迹集。

具体的，根据第一父圆至第m父圆的位置和半径，以及以第一父圆的圆周为圆心的子圆至以第m父圆的圆周为圆心的子圆中圆心的位置和半径，将第一轨迹集中处于子圆之外的轨迹删除。

继续参见图3，图3中，从左起，第一个圆、第二个圆之间的虚线和实线中，虚线11和15超出了第一个圆与第二个圆的范围，因此，删除第一轨迹集中的11和15，处理后的第一轨迹集中包括虚线12、虚线13、实线14。

第二轨迹集中，虚线24和虚线25超出了第二个圆与第三个圆的范围，因此，删除第二轨迹集中的24和25，处理后的第二轨迹集包括21、22和23。

步骤116，通过启发值函数，对处理后的第一轨迹集中的轨迹进行评价。

步骤117，根据评价结果，从处理后的第一轨迹集中确定第一目标轨迹。

具体的，通过f＝g+h计算处理后的第一轨迹集中每条轨迹的启发值；

当轨迹的启发值最小时，确定该轨迹为第一目标轨迹。

其中，f为每条轨迹的启发值，g为从起点至车辆在下一时刻所在的位置的距离；h包括圆心引导项和终点引导项；

通过公式h＝l_next+l₁+l₂...l_dist计算h；

其中，l_next为圆心引导项，表示车辆在下一时刻的位置至最近圆心的距离，l₁+l₂...l_dist为终点引导项，表示距离车辆在下一时刻的位置最近的圆心到下一圆心距离、下一圆心到下下圆心距离…最后一个圆心到终点的距离之和。

其中，当计算出第一目标轨迹后，继续以下一时刻作为开始时间，以下下一时刻作为结束时间，重复步骤112至步骤117，确定出第二目标轨迹。该过程可以称为启发式轨迹搜索。

比如，在图3中，计算出第一轨迹集中，14的启发值最小，第二轨迹集中，23的启发值最小，因此，可以确定14为第一目标轨迹，23为第二目标轨迹。

步骤118，在第一目标轨迹上确定预瞄点的位置。

具体的，为了保证轨迹符合控制要求并降低下层控制难度，可以通过计算出预瞄点的转向角度和加速度，来实现对车辆的控制。

其中，预瞄点就是控制器当前认为需要跟踪到达的位置，预瞄距离就是车辆当前距离预瞄点的距离。预瞄距离主要是在控制中保证对轨迹跟踪的鲁棒性，防止跟踪效果由于局部路径而抖动。

步骤119，根据车辆的当前速度和预瞄点的位置，计算预瞄距离。

其中，可以根据

计算预瞄距离；其中，v为车辆的当前速度。

具体的，在目标路径中，包括多个路点，每一个路点包括该路点的期望速度。

图4为发明实施例一提供的控制参数的示意图，参见图4，可以看到预瞄距离L₀。

步骤120，根据预瞄距离和车辆的动力学模型，计算预瞄点的转向角度。

具体的，利用公式

计算转向角度；

其中，δ为转向角度，L为车辆动力学模型中的车辆轴距，l为车辆后轴距，即车辆转弯中心点距后轴中心点的距离，L₀为预瞄距离，η为车辆相对于预瞄点方向的夹角。其示意图参见图4。通过该转向角度，能够保证规划路径的横向控制精度。

步骤121，根据预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度。

具体的，第一目标轨迹上的预瞄点的期望速度，在规划出第一目标轨迹时，已经包含在其上。

可以利用公式

计算车辆在预瞄点的加速度；

其中，α为加速度，Ki和Kp为比例、积分控制量，为经验常数，是根据调试效果结合经验值设定的；ν为车辆的当前速度。通过该加速度，能保证规划路径的纵向速度控制精度。

可以理解的是，此处的Ki和Kp也可以是其它控制方法，示例而非限定，可以是比例控制、比例微分等控制方式。

步骤122，将所述第一目标轨迹的预瞄点的转向角度和加速度发送给底层控制器，以使所述底层控制器根据所述预瞄点的转向角度和加速度，在所述预瞄点对车辆进行控制。

具体的，路径规划和控制是完全分开的，路径规划是由车辆控制单元进行的，控制是由底层控制器进行的。路径规划只输出包含期望速度的轨迹，如何跟踪轨迹完全由控制模块决定。本申请，给出了底层输出控制量的“推荐值”即加速度a和转向角度，供底层控制器参考，因此可以认为提升了底层控制器的控制精度。

继续执行步骤112至步骤122，从而可以计算出每一个目标轨迹上的预瞄点的转向角度和加速度，然后将每一个目标轨迹上的预瞄点的转向角度和加速度发送给底层控制器，以提高底层控制器的控制精度。

步骤123，当第n目标轨迹的终点与终点的距离之差小于预设第二阈值时，将第一目标轨迹至第n目标轨迹进行处理，生成目标路径。

继续以图3为例，在图3中，经过计算，确定的第一目标轨迹为14，第二目标轨迹为23，将14和23拼接，如果后续存在其它第n目标轨迹，继续拼接，得到目标轨迹。其中，n为大于2的整数。

具体的，步骤118包括：将第一目标轨迹至第n目标轨迹进行拼接，生成原始目标路径；

当原始目标路径不满足车辆运动学约束时，进行平滑处理，生成目标路径。

其中，车辆动力学模型中，包括最小转弯半径，在将多个目标轨迹进行拼接时，计算拼接处的曲率，将曲率和车辆的最小转弯半径的倒数进行比较，当曲率不大于最小转弯半径的倒数时，该曲率符合要求，当曲率大于最小转弯半径的倒数时，进行平滑处理。示例而非限定，可以根据均值滤波的方式，进行平滑处理。

进一步的，在步骤111之前还包括：

当某一父圆的所有子圆的启发值都相等时，退回至该父圆的上一级父圆，从第一列表中删除该父圆的圆心的位置和半径。

由此，通过应用本发明提供的基于闭环控制的路径规划方法，具有以下几个优势：

1、生成一系列探索圆，这些圆相当于一种快速的空间探索，其占满可通行空间。利用这些探索圆，引导启发式轨迹搜索的方向，使得启发式搜索的过程不仅考虑障碍物和终点，也考虑空间利用率，极大的增强了无人车辆规划路径的合理性。

2、生成路径的过程对车辆加速度和方向盘转速度进行采样，生成路径包含车辆的坐标、朝向、速度、方向盘转角等信息，因此生成的路径不仅在坐标、朝向上连续，在速度与方向盘转角上都是连续的，使得生成的路径更加合理，降低了无人车控制模块的控制难度、提高了实时性。

3、在路径规划中考虑到了横、纵向控制，降低了底层控制器的控制难度。

本发明实施例二提供了一种基于闭环控制的路径规划装置。图5为本发明实施例二提供的基于闭环控制的路径规划装置结构示意图。如图5所示，该路径规划装置包括：获取单元501、设置单元502、判断单元503、确定单元504、计算单元505、生成单元506、处理单元507、评价单元508和发送单元509。

获取单元501用于获取车辆的参考路径，参考路径包括起点和终点；

设置单元502用于在参考路径上取第一终点；第一终点位于起点和终点之间；

判断单元503用于判断以起点和第一终点间的距离为直径的圆内是否存在障碍物；

确定单元504用于当存在第一障碍物时，确定起点与第一障碍物的第一距离；第一障碍物为障碍物中距离起点最近的障碍物；第一距离为起点距离第一障碍物的距离减去自车的安全距离之差；

设置单元502还用于，以起点为圆心，以第一距离为半径，设置第一父圆；

设置单元502还用于，以第一终点为圆心，以第一距离为半径，设置第一终点圆；

确定单元504还用于，以第一父圆的圆周上的点为圆心，确定该圆周上的多个圆心中的每个与距离其最近的障碍物的距离，生成子距离集；

计算单元505用于计算该圆周上的多个圆心中的每个与第一终点的几何距离，生成几何距离集；

计算单元505还用于根据子距离集和几何距离集，计算多个圆心对应的子圆的启发值集；子圆的半径为圆心与距离其最近的障碍物的距离减去自车的安全距离之差；

确定单元504还用于，确定启发值集中最小的启发值对应的子圆为第二父圆；

生成单元506用于当第m父圆与第一终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表；第一列表包括第一父圆至第m父圆的圆心位置和半径；第二列表包括以第一父圆的圆周为圆心的子圆至以第m父圆的圆周为圆心的子圆中圆心的位置和半径；m为大于2的整数；

获取单元501还用于，在当前时刻，获取车辆在起点处的第一参数；

计算单元505还用于，根据当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数；

计算单元505还用于，根据第一参数和第二参数，计算车辆从当前时刻至下一时刻的第一轨迹集；

处理单元507用于根据第一列表和第二列表，对第一轨迹集进行处理，生成处理后的第一轨迹集；

评价单元508用于通过启发值函数，对处理后的第一轨迹集中的轨迹进行评价；

确定单元504还用于，根据评价结果，从处理后的第一轨迹集中确定第一目标轨迹；

确定单元还用于在第一目标轨迹上确定预瞄点的位置；

计算单元505还用于，根据车辆的当前速度和预瞄点的位置，计算预瞄距离；

计算单元505还用于，根据预瞄距离和车辆的动力学模型，计算预瞄点的转向角度；

计算单元505还用于，根据第一目标轨迹上的路点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度；

发送单元509用于将将所述第一目标轨迹的预瞄点的转向角度和加速度发送给底层控制器，以使所述底层控制器根据所述预瞄点的转向角度和加速度，在所述预瞄点对车辆进行控制。

处理单元507还用于，当第n目标轨迹的终点与终点的距离之差小于预设第二阈值时，将第一目标轨迹至第n目标轨迹进行处理，生成目标路径；n为大于2的整数。

图5中每个模块的具体功能与上述实施例一的描述相同，此处不再赘述。可以理解的是，该实施例二的技术效果也和上述实施例一相同，此处不再赘述。

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

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

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

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

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

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

Claims (10)

1.一种基于闭环控制的路径规划方法，其特征在于，所述方法包括：

获取车辆的参考路径，所述参考路径包括起点和终点；

在所述参考路径上取第一终点；所述第一终点位于所述起点和终点之间；

判断以所述起点和所述第一终点间的距离为直径的圆内是否存在障碍物；

当存在第一障碍物时，确定所述起点与所述第一障碍物的第一距离；所述第一障碍物为障碍物中距离起点最近的障碍物；所述第一距离为所述起点距离所述第一障碍物的距离减去自车的安全距离之差；

以所述起点为圆心，以所述第一距离为半径，设置第一父圆；

以所述第一终点为圆心，以所述第一距离为半径，设置第一终点圆；

以所述第一父圆的圆周上的点为圆心，确定该圆周上的多个圆心中的每个与距离其最近的障碍物的距离，生成子距离集；

计算该圆周上的多个圆心中的每个与所述第一终点的几何距离，生成几何距离集；

根据所述子距离集和所述几何距离集，计算多个圆心对应的子圆的启发值集；所述子圆的半径为圆心与距离其最近的障碍物的距离减去自车的安全距离之差；

确定所述启发值集中最小的启发值对应的子圆为第二父圆；

当第m父圆与所述第一终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表；所述第一列表包括所述第一父圆至第m父圆的圆心位置和半径；所述第二列表包括以所述第一父圆的圆周为圆心的子圆至以所述第m父圆的圆周为圆心的子圆中圆心的位置和半径；m为大于2的整数；

在当前时刻，获取所述车辆的第一参数；

根据所述当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数；

根据所述第一参数和所述第二参数，计算所述车辆从当前时刻至下一时刻的第一轨迹集；

根据所述第一列表和所述第二列表，对所述第一轨迹集进行处理，生成处理后的第一轨迹集；

通过启发值函数，对处理后的所述第一轨迹集中的轨迹进行评价；

根据评价结果，从处理后的所述第一轨迹集中确定第一目标轨迹；

在所述第一目标轨迹上确定预瞄点的位置；

根据车辆的当前速度和所述预瞄点的位置，计算预瞄距离；

根据所述预瞄距离和车辆的动力学模型，计算预瞄点的转向角度；

根据所述预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度；

将所述第一目标轨迹的预瞄点的转向角度和加速度发送给底层控制器，以使所述底层控制器根据所述预瞄点的转向角度和加速度，在所述预瞄点对车辆进行控制；

当第n目标轨迹的终点与所述终点的距离之差小于预设第二阈值时，将所述第一目标轨迹至第n目标轨迹进行处理，生成目标路径；n为大于2的整数。

2.根据权利要求1所述的方法，其特征在于，所述第一参数包括车辆在当前时刻的x坐标、y坐标、朝向、车速、方向盘转角，所述根据所述当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数，具体包括：

通过以下公式计算车辆在下一时刻的第二参数：

x_t+Δt＝x_t+v_t·cosθ_t·cosβ_t·Δt

y_t+Δt＝y_t+v_t·sinθ_t·cosβ_t·Δt

θ_t+Δt＝θ_t+v_t·sinβ_t·Δt/l

v_t+Δt＝v_t+a·Δt

β_t+Δt＝β_t+ω·Δt

其中，x_t为车辆在当前时刻的x坐标、y_t为车辆在当前时刻的y坐标,θ_t为车辆在当前时刻的朝向，v_t为车辆在当前时刻的速度，β_t为车辆在当前时刻的方向盘转角；x_t+Δt为车辆在下一时刻的x坐标、y_t+Δt为车辆在下一时刻的y坐标,θ_t+Δt为车辆在下一时刻的朝向，v_t+Δt为车辆在下一时刻的速度，β_t+Δt为车辆在下一时刻的方向盘转角；l为车辆轴距。

3.根据权利要求1所述的方法，其特征在于，所述根据所述第二列表，对所述第一轨迹集进行处理，生成处理后的第一轨迹集，具体包括：

根据所述以所述第一父圆的圆周为圆心的子圆至以所述第m父圆的圆周为圆心的子圆中圆心的位置和半径，将所述第一轨迹集中处于所述子圆之外的轨迹删除。

4.根据权利要求1所述的方法，其特征在于，所述通过启发值函数，对处理后的所述第一轨迹集中的轨迹进行评价，具体包括：

通过f＝g+h计算处理后的第一轨迹集中每条轨迹的启发值；

当轨迹的启发值最小时，确定该轨迹为第一目标轨迹；

其中，f为每条轨迹的启发值，g为从起点至车辆在下一时刻所在的位置的距离；h包括圆心引导项和终点引导项。

5.根据权利要求4所述的方法，其特征在于，通过公式h＝l_next+l₁+l₂...l_dist计算h；

其中，l_next为圆心引导项，表示车辆在下一时刻的位置至最近圆心的距离，l₁+l₂...l_dist为终点引导项，表示距离车辆在下一时刻的位置最近的圆心到下一圆心距离、下一圆心到下下圆心距离…最后一个圆心到终点的距离之和。

6.根据权利要求1所述的方法，其特征在于，所述当第m父圆与所述终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表之前，所述方法还包括：

当某一父圆的所有子圆的启发值都相等时，退回至该父圆的上一级父圆，从所述第一列表中删除该父圆的圆心的位置和半径。

7.根据权利要求1所述的方法，其特征在于，所述根据所述预瞄距离和车辆的动力学模型，计算预瞄点的转向角度具体包括：

利用公式

计算转向角度；

其中，δ为转向角度，L为车辆动力学模型中的车辆轴距，l为车辆轴距，L₀为预瞄距离，η为车辆相对于预瞄点方向的夹角。

8.根据权利要求7所述的方法，其特征在于，利用公式

计算预瞄距离；其中，v为车辆的当前速度。

9.根据权利要求1所述的方法，其特征在于，所述根据所述预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度具体包括：

利用公式

计算车辆加速度；

其中，α为加速度，K_i和K_p为经验常数，ν为车辆的当前速度。

10.一种基于闭环控制的路径规划装置，其特征在于，所述装置包括：

获取单元，所述获取单元用于获取车辆的参考路径，所述参考路径包括起点和终点；

设置单元，所述设置单元用于在所述参考路径上取第一终点；所述第一终点位于所述起点和终点之间；

判断单元，所述判断单元用于判断以所述起点和所述第一终点间的距离为直径的圆内是否存在障碍物；

确定单元，所述确定单元用于当存在第一障碍物时，确定所述起点与所述第一障碍物的第一距离；所述第一障碍物为障碍物中距离起点最近的障碍物；所述第一距离为所述起点距离所述第一障碍物的距离减去自车的安全距离之差；

所述设置单元还用于，以所述起点为圆心，以所述第一距离为半径，设置第一父圆；

所述设置单元还用于，以所述第一终点为圆心，以所述第一距离为半径，设置第一终点圆；

所述确定单元还用于，以所述第一父圆的圆周上的点为圆心，确定该圆周上的多个圆心中的每个与距离其最近的障碍物的距离，生成子距离集；

计算单元，所述计算单元用于计算该圆周上的多个圆心中的每个与所述第一终点的几何距离，生成几何距离集；

所述计算单元还用于根据所述子距离集和所述几何距离集，计算多个圆心对应的子圆的启发值集；所述子圆的半径为圆心与距离其最近的障碍物的距离减去自车的安全距离之差；

所述确定单元还用于，确定所述启发值集中最小的启发值对应的子圆为第二父圆；

生成单元，所述生成单元用于当第m父圆与所述第一终点圆的重合度大于预设第一阈值时，生成第一列表和第二列表；所述第一列表包括所述第一父圆至第m父圆的圆心位置和半径；所述第二列表包括以所述第一父圆的圆周为圆心的子圆至以所述第m父圆的圆周为圆心的子圆中圆心的位置和半径；m为大于2的整数；

所述获取单元还用于，在当前时刻，获取所述车辆的第一参数；

所述计算单元还用于，根据所述当前时刻的第一参数和车辆的动力学模型，计算车辆在下一时刻的第二参数；

所述计算单元还用于，根据所述第一参数和所述第二参数，计算所述车辆从当前时刻至下一时刻的第一轨迹集；

处理单元，所述处理单元用于根据所述第一列表和所述第二列表，对所述第一轨迹集进行处理，生成处理后的第一轨迹集；

评价单元，所述评价单元用于通过启发值函数，对处理后的所述第一轨迹集中的轨迹进行评价；

所述确定单元还用于，根据评价结果，从处理后的所述第一轨迹集中确定第一目标轨迹；

所述确定单元还用于在所述第一目标轨迹上确定预瞄点的位置；所述计算单元还用于，根据车辆的当前速度和所述预瞄点的位置，计算预瞄距离；

所述计算单元还用于，根据所述预瞄距离和车辆的动力学模型，计算预瞄点的转向角度；

所述计算单元还用于，根据所述预瞄点的期望速度和车辆的当前速度，计算车辆在预瞄点的加速度；

发送单元，所述发送单元用于将所述第一目标轨迹的预瞄点的转向角度和加速度发送给底层控制器，以使所述底层控制器根据所述预瞄点的转向角度和加速度，在所述预瞄点对车辆进行控制；

所述处理单元还用于当第n目标轨迹的终点与所述终点的距离之差小于预设第二阈值时，将所述第一目标轨迹至第n目标轨迹进行处理，生成目标路径；n为大于2的整数。

Images