CN113311828B

CN113311828B - 一种无人车局部路径规划方法、装置、设备及存储介质

Info

Publication number: CN113311828B
Application number: CN202110500415.5A
Authority: CN
Inventors: 陆丽萍; 邱雨洁
Original assignee: Wuhan University of Technology WUT
Current assignee: Wuhan University of Technology WUT
Priority date: 2021-05-08
Filing date: 2021-05-08
Publication date: 2023-07-25
Anticipated expiration: 2041-05-08
Also published as: CN113311828A

Abstract

本发明公开一种无人车局部路径规划方法、装置、设备及存储介质，方法包括：根据无人车周围环境车的历史轨迹信息以及道路环境上下文信息获取无人车周围环境车的未来轨迹预测信息；采用人工势场法对所述无人车的行驶环境进行建模，在栅格图中建立概率地图；基于所述环境车的未来轨迹预测信息和所述概率地图，采用改进RRT算法对所述无人车进行局部路径规划，以获取无人车的规划路径；采用B样条曲线对所述无人车的规划路径进行拟合，以生成满足车辆运动约束的无人车的局部路径。本发明解决了现有技术中局部路径规划时计算强度大、耗时长、安全性不够的技术问题。

Description

一种无人车局部路径规划方法、装置、设备及存储介质

技术领域

本发明涉及路径规划技术领域，具体涉及一种无人车局部路径规划方法、装置、设备及存储介质。

背景技术

自动驾驶汽车是促进智能交通系统发展的重要工具，能够有效减少因人为错误引发的交通事故，具有重要的理论意义和实用价值。轨迹规划作为自动驾驶汽车的核心组成部分，负责解决自动驾驶汽车如何具体行驶的问题，即实时规划出安全且舒适的局部轨迹，轨迹规划的结果直接影响到自动驾驶的安全性、智能性和舒适性。

围绕轨迹规划问题已经开展了许多研究，目前常用的方法有：基于图搜索的算法、基于曲线插值的算法、基于空间采样的算法、基于数值优化的算法、基于强化学习的算法。不同类型的路径规划方法各有优劣，基于图搜索的轨迹规划方法适用于在已知环境中找到最短路径,但输入维度和计算复杂度随着搜索范围和维度的增加而大幅增长，不适用于复杂动态环境。基于曲线插值的轨迹规划方法计算过程简单，但针对于复杂动态场景的插值曲线一般阶次较高，能够准确到达终端状态的高阶曲线的关键系数难以计算。基于采样的轨迹规划方法虽然可以解决高维空间规划的时间约束问题，但生成的轨迹规划结果可能具有次优性。基于数值优化的轨迹规划方法能够在规划周期内综合考虑多种约束条件，但计算复杂度较高，轨迹生成耗时，并且依赖于全局参考轨迹。基于强化学习的路径规划方法可以得到较优路径，但需要前期长时间、多场景的训练过程，且泛化能力差，尤其应对极端场景的能力难以考证。

因此，目前的局部路径规划方法，在复杂动态环境下的规划能力较差，而且计算强度大，耗时长。

发明内容

本发明的目的在于克服上述技术不足，提供一种无人车局部路径规划方法、设备及存储介质，解决现有技术中局部路径规划时计算强度大、耗时长、安全性不够的技术问题。

为达到上述技术目的，本发明采取了以下技术方案：

第一方面，本发明提供了一种无人车局部路径规划方法，包括如下步骤：

根据无人车周围环境车的历史轨迹信息以及道路环境上下文信息获取无人车周围环境车的未来轨迹预测信息；

采用人工势场法对所述无人车的行驶环境进行建模，在栅格图中建立概率地图；

基于所述环境车的未来轨迹预测信息和所述概率地图，采用改进RRT算法对所述无人车进行局部路径规划，以获取无人车的规划路径；

采用B样条曲线对所述无人车的规划路径进行拟合，以生成满足车辆运动约束的无人车的局部路径。

优选的，所述的无人车局部路径规划方法中，所述根据无人车周围环境车的历史轨迹信息以及道路环境上下文信息获取无人车周围环境车的未来轨迹预测信息具体包括：

获取不同场景下环境车的历史轨迹坐标以及对应的历史场景上下文信息，利用不同场景下交通车辆的历史轨迹坐标以及对应的历史场景上下文信息对长短期记忆网络进行训练；

采用奖励函数对所述长短期记忆网络进行正则化处理；

获取无人车周围环境车的实时场景上下文信息，将所述实时场景上下文信息输入所述正则化处理后的长短期记忆网络中，以得到环境车的未来轨迹预测信息。

优选的，所述的无人车局部路径规划方法中，所述采用人工势场法对所述无人车的行驶环境进行建模，在栅格图中建立概率地图具体包括：

基于无人车的实时行车环境信息建立势场函数，采用所述势场函数计算出所述无人车在实时行车环境信息下的各类势场后，基于计算出的各类势场建立概率地图。

优选的，所述的无人车局部路径规划方法中，所述势场函数具体为：

U_all＝U_lane+U_road+U_car+U_obstacle+U_goal，

U_goal＝ε-κ·(x-x_car)，

其中，U_lane表示分道线势场，U_road表示道路边界线势场，U_car表示环境车势场，U_obstace表示障碍物势场，U_goal表示目标点势场，U_all表示势场总和，A_lane表示分道线势场系数，y_lane，i表示第i条分道线在Y方向上的位置，σ_lane表示分道线势场的收敛系数，A_road表示道路边界势场系数，j的取值为1或2，y_road，j为第j条道路边界线的位置，A_car，long表示纵向势场系数，K表示距离无人车安全范围最右侧的距离，λ表示汤川势系数，σ_car表示环境车势场的收敛系数，d表示横向距离，A_obstacle表示障碍物系数，d_ods表示距离障碍物前后两侧的距离，ε是正的势场常数，κ是目标点势场的系数。

优选的，所述的无人车局部路径规划方法中，所述基于计算出的各类势场建立概率地图具体为：

其中，c_i，j表示栅格地图中的某一位置，ε是正的势场常数。

优选的，所述的无人车局部路径规划方法中，所述基于所述环境车的未来轨迹预测信息和所述概率地图，采用改进RRT算法对所述无人车进行局部路径规划，以获取无人车的局部路径具体包括：

将栅格地图中计算得到的采样概率值作为适应度度量，根据概率地图得到每个栅格点的适应度后，计算出每个栅格点的横纵坐标被选中的概率，并基于各个栅格点的横纵坐标被选中的概率计算出横纵两个维度的累积概率；

基于所述横纵两个维度的累积概率进行节点扩展，在节点之间的距离约束条件和角度约束条件下选取合适的采样新节点；

基于所述环境车的未来轨迹和所述无人车的采样新节点，对无人车进行碰撞检测；

根据所述碰撞检测结果，对无人车的路径节点进行连接或删除，并继续进行节点扩展，直至达到目标节点后，得到无人车的局部路径。

优选的，所述的无人车局部路径规划方法中，采用三次B样条曲线对所述无人车的规划路径进行拟合。

第二方面，本发明还提供一种无人车局部路径规划装置，包括：

未来轨迹预测模块，用于根据无人车周围环境车的历史轨迹信息以及道路环境上下文信息获取无人车周围环境车的未来轨迹预测信息；

概率地图建立模块，用于采用人工势场法对所述无人车的行驶环境进行建模，在栅格图中建立概率地图；

规划路径获取模块，用于基于所述环境车的未来轨迹预测信息和所述概率地图，采用改进RRT算法对所述无人车进行局部路径规划，以获取无人车的规划路径；

曲线拟合模块，用于采用B样条曲线对所述无人车的规划路径进行拟合，以生成满足车辆运动约束的无人车的局部路径。

第三方面，本发明还提供一种无人车局部路径规划设备，包括：处理器和存储器；

所述存储器上存储有可被所述处理器执行的计算机可读程序；

所述处理器执行所述计算机可读程序时实现如上所述的无人车局部路径规划方法中的步骤。

第四方面，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上所述的无人车局部路径规划方法中的步骤。

与现有技术相比，本发明提供的无人车局部路径规划方法、装置、设备及存储介质，针对驾驶场景中其他交通车辆不确定性的影响，在轨迹规划的过程中综合考虑其他交通车辆未来的运动轨迹，将车辆的交互作用融入轨迹规划的结果中，及时处理未来时刻存在的碰撞可能性对RRT算法搜索过程的影响，从而为无人车规划出更加安全的轨迹，增强轨迹规划结果的安全性、智能性和合理性。

附图说明

图1是本发明提供的无人车局部路径规划方法的一较佳实施例的流程图；

图2是本发明例动态场景下考虑其他交通车辆未来轨迹的无人车局部路径规划方法的一较佳实施例的技术路线图；

图3是本发明基于LSTM编码器-解码器架构对交通车辆未来轨迹进行预测的算法框架图；

图4是本发明基于LSTM编码器-解码器架构对交通车辆未来轨迹进行预测的网络结构图；

图5是本发明基于RRT算法进行碰撞检测时的一较佳实施例的时空栅格图；

图6是本发明车辆包围盒与栅格地图的一较佳实施例的模型图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

请参阅图1，本发明实施例提供的无人车局部路径规划方法，包括如下步骤：

S100、根据无人车周围环境车的历史轨迹信息以及道路环境上下文信息获取无人车周围环境车的未来轨迹预测信息。

具体的，所述步骤S100具体包括：

采用奖励函数对所述长短期记忆网络进行正则化处理；

获取无人车周围环境车的实时场景上下文信息，将所述实时场景上下文信息输入所述正则化处理后的长短期记忆网络中，以得到环境车辆的未来轨迹预测信息。

具体来说，请参阅图2和图3，神经网络包括编码器，解码器和分析器，本发明实施例从历史轨迹和对应的场景上下文信息中预测车辆未来的轨迹。如图4所示，第i辆环境车过去t-1时刻的位移和对应的的场景上下文向量/>首先经过全连接层进行嵌入操作，后一个全连接层将位移信息与场景上下文信息进行混合产生最终的状态向量/>每个全连接层都要进行ReLU激活操作。场景上下文向量是分析器从场景上下文信息中提取的，比如静态场景图片，临近车辆的隐藏状态等。编码器RNN从过去隐藏状态/>记忆细胞状态向量/>中产生当前隐藏状态/>和记忆细胞/>为了正则化的目的，让编码器RNN从当前隐藏状态向量中预测当前位移，如下式：

其中，φ(a；W)＝b是一个全连接层，利用参数W将向量a映射到向量b，W_fp代表第P层全连接层的参数。

解码器RNN的隐藏状态和记忆细胞/>分别被初始化为给出上一时刻的历史位移/>车辆未来轨迹位移则以下式形式预测：

其中，1≤k≤M。ReLU(·)和W_D分别表示ReLU激活操作和解码器参数。

利用奖励函数进行正则化处理，具体来说，占据栅格图中的车辆位置以及临近车辆的隐藏状态/>经过池化后得到场景上下文向量/>如下式产生奖励函数：

其中，Sig(·)代表Sigmoid函数。分析器提取的环境特征越有意义，该奖励函数值越高，从而反过来使分析器能提取更有意义的环境特征。

S200、采用人工势场法对所述无人车的行驶环境进行建模，在栅格图中建立概率地图。

具体的，所述步骤S200具体包括：

具体来说，车辆在行驶过程中，环境影响因素主要包括车道线(包括分道线和道路边界线)、环境车、静态障碍物以及目标点。因此，可将行车环境对被控车的总势场U_all定义为分道线势场U_lane，道路边界线势场U_road，环境车势场U_car，障碍物势场U_obstac，目标点势场U_goal等各类势场的总和，即：

U_all＝U_lane+UU_road+U_car+U_obstatacle+U_goal。

其中，分道线势场的作用是确保车辆在当前车道内行驶，避免不必要的换道行为，所以在分道线位置势场取最大值，往两边逐渐减小。同时，分道线的势场应该足够小，以确保被控车在需要换道避障时能跨越。具体实施时，采用类高斯函数的形式来进行建模，具体如下：

其中，A_lane表示分道线势场系数，y_ane，i表示第i条分道线在Y方向上的位置，σ_lane表示分道线势场的收敛系数，由车道宽度决定。

道路的边界是车辆不能逾越的区域，因而在车辆靠近道路边界时，势场值应迅速增大，并在道路边界达到最大值。道路边界线势场值需要在离道路边界越近的时候上升越快，防止车辆与之碰撞。根据这个特征，道路边界的具体势场函数如下：

其中，A_road表示道路边界势场系数，j的取值为1或2，y_road，j为第j条道路边界线的位置。

进一步的，在计算环境车势场时，先建立被控车与环境车之间的纵向势场函数，使得车辆势场在远距离的时候，随着距离的减小上升比较平缓，近距离的时候则上升迅速，这种特性可以帮助后车在较远距离时维护安全距离，在比较近的距离引导车辆换道，如下式：

其中，A_car，long表示纵向势场系数，K表示距离无人车安全范围最右侧的距离，λ表示汤川势系数，决定势场影响的范围。

当纵向势场建立完成后，环境车的整体势场可在其基础上往横向延伸，采用一个类高斯函数的形式进行计算，环境车的总势场如下式：

其中，σ_car表示环境车势场的收敛系数，d表示横向距离。

在进行障碍物的势场建立时，由于车辆在行驶过程中碰到的障碍物主要包括前方道路施工、滑落的大型石块等，这类障碍物与车辆的交互关系是迫使车辆绕开行驶。因此，根据这个交互关系，设计势场函数，在障碍物所在区域势场取最大值，在障碍物的前后两侧，根据距离来计算势场，距离越近势场越大，而且上升的趋势很快，具体如下式：

其中，A_obstacle表示障碍物系数，d_obs表示距离障碍物前后两侧的距离。

在进行目标点势场势场的建立时，由于车辆需要沿着车道往前行驶，因此可以把车辆前方作为目标点。目标点势场的作用是保证被控车朝车道前方行驶，即车辆前方的势场值要低于后方的势场值，势场点离被控车越远，势场值就越小，势场值与该距离成反比，所以采用简单的一次函数作为目标点势场函数，还要保证势场值是非负的，具体如下式：

U_goal＝ε-κ·(x-x_car)，

其中，ε是正的势场常数，κ是目标点势场的系数，目标点势场可使被控车保持向前行驶的趋势。

当计算出了各个势场后，即可根据各个势场建立概率地图。具体来说，传统RRT算法中，随机采样过程是基于整个栅格地图的均匀采样，即栅格地图内的位置被选中作为下一个扩展节点的概率是一致的，根据人工势场对环境建模的思想，目标位置的引力场引导被控对象向着目标方向运动，距离目标点越近的位置，其吸引势场越小，被扩展的概率应该越大；同理，离障碍物越近的位置，排斥势场越强，被扩展为下一个随机节点的概率应该越小，基于人工势场建立的概率分布模型，可以促使随机树在靠近目标点和远离障碍物的安全区域有更高的采样概率。

基于此，在计算得到当前栅格地图下的各类环境影响因素后，将势场和作为启发式采样的引导信息，构架启发式采样概率分布模型。在势场引导下的采样概率地图可表示为：

其中，c_i，j表示栅格地图中的某一位置，ε是正的势场常数，用于避免产生奇异值。

S300、基于所述环境车的未来轨迹预测信息和所述概率地图，采用改进RRT算法对所述无人车进行局部路径规划，以获取无人车的规划路径。

具体的，所述步骤S300具体包括：

具体来说，改进RRT算法进行路径规划的步骤具体分为四步，包括基于轮盘赌的自适应采样、最邻近点搜索、碰撞检测以及剪枝策略。

具体的，在基于轮盘赌的自适应采样步骤中，轮盘赌算法的核心思想是个体被选中的概率与其适应度大小成正比，针对启发式采样问题，将栅格地图中计算得到的采样概率值作为适应度度量，根据概率地图得到每个栅格点的适应度f(c_i，j)后，计算得到每个栅格点横纵坐标被选中的概率为：

然后计算横纵两个维度的累积概率为：

在(0，1)区间内产生均匀分布的随机数，并选择使得q_i、q_j大于该随机数的最小[i，j]，[s_x，s_y]＝[i，j]即完成一次启发式采样过程。

在最邻近点搜索步骤，在针对无人驾驶车辆进行路径规划时，应考虑节点之间的夹角关系，一方面过大的夹角对于无人车而言可能无法到达，另一方面最邻近节点的选择应驱使轨迹变得平滑。因此，在最邻近节点搜索过程中，在计算节点之间的欧几里德距离基础上，添加最大夹角约束，并在最邻近度量中加入角度差，由于距离和角度的量纲不同，使用线性归一化方法对两个变量进行处理，点距离度量公式如下式：

其中，x_rand为采样点横坐标，x_vertice为路径树节点横坐标，y_rand为采样点纵坐标，y_vertce为路径树节点纵坐标，θ_rand为采样点航向角，θ_vertice为路径树节点航向角，N₁(·)，N₂(·)分别为距离和角度的线性归一化方法。

在碰撞检测步骤，采用时空占据栅格图，在占据栅格图的基础上增加时间维度，结合环境车辆的轨迹预测结果计算不同时刻下每个栅格的占用状态，从而对自动驾驶汽车与环境车辆之间的交互作用关系进行碰撞检测。将环境车辆的预测轨迹以及无人车的可行路径点分别投影到时空栅格图中，判断自动驾驶汽车与环境车辆之间在未来时刻是否会发生碰撞，具体实施时，如图6所示，碰撞检测就是判断在图6所示的长方体内，环境车辆的行驶轨迹与无人车可行路径之间是否会存在交点。

由于无人车非质点，在进行碰撞检测时，需要根据路径点位置、航向角以及车长、车宽信息对碰撞检测区域进行计算，因此，本发明实施例依据车辆尺寸和航向角，配置一个简单的包围盒，对包围盒与栅格地图重叠的部分进行简答测试，采用对包围盒与栅格地图重叠的部分进行简答测试作为最简便的方式，即AABB包围盒方式，如图5所示。

具体实施时，采用简化的两轮车模型并选择后轴中点为路径点，L_f，L_r分别为后轴中点与车辆前端距离和后轴中点与车辆后端距离，W为车宽，θ为方位角，由后轴中点路径点可得车辆方位角以及各顶点计算公式如下述公式所示：

θ＝arctan(dy/dx)

在剪枝策略步骤中，首先从初始节点开始依次连接后续的路径节点，以初始节点、后续节点以及后续节点的子节点作为控制节点进行曲线拟合，若曲线上的节点经过离散采样与空间内的障碍物栅格无交集，那么初始节点与该后续节点之间的路径点删除，将两节点直接相连，以此类推，否则，保留初始节点和发生碰撞节点的父节点之间的线路，并将发生碰撞的路径节点设置为下一个起始节点，继续执行上述过程，直到达到目标节点。

S400、采用B样条曲线对所述无人车的规划路径进行拟合，以生成满足车辆运动约束的无人车的局部轨迹。

具体的，采用三次B样条曲线对所述无人车的规划路径进行拟合。所述步骤S400包括两个步骤，即曲率求解和路径规划。

在曲率求解阶段，由参数曲线求解曲率，如下所示：

在路径规划阶段，由曲率公式可知曲率要求x，y二阶可导，因此选择三次B样条曲线进行路径规划，样条基函数如下式：

其中，N_i，k(t)表示第i个k阶B样条基函数。

由上式可得到3此B样条表达式、一阶导数函数、二阶导数函数如下：

其中，p_1，i，x～p_4，ix为第i段B样条曲线4个控制点横向坐标，p_1，iy～p_4，iy为第i段B样条曲线4个控制点纵向坐标。

本发明主要针对在复杂动态环境下，比如城区，轨迹规划所面临的问题是动态环境下的实时规划问题，而现有的轨迹规划方法多集中于对车辆运动学、动力学约束等问题的考虑，较少提及驾驶场景中其他交通车辆不确定性的影响，因此，本发明从这一角度出发，在轨迹规划的过程中综合考虑其他交通车辆未来的运动轨迹，将车辆的交互作用融入轨迹规划的结果中，及时处理未来时刻存在的碰撞可能性对RRT算法搜索过程的影响，从而为无人车规划出更加安全的轨迹，增强轨迹规划结果的安全性、智能性和合理性。

基于上述无人车局部路径规划方法，本发明还相应的提供一种无人车局部路径规划装置，包括：

由于上文已对无人车局部路径规划方法进行详细描述，在此不再赘述。

基于上述无人车局部路径规划方法，本发明还相应的提供一种无人车局部路径规划设备，包括：处理器和存储器；

所述处理器执行所述计算机可读程序时实现如上述各实施例所述的无人车局部路径规划方法中的步骤。

基于上述无人车局部路径规划方法，本发明还相应的提供一种计算机可读存储介质，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上述各实施例所述的无人车局部路径规划方法中的步骤。

综上所述，本发明提供的无人车局部路径规划方法、装置、设备及存储介质，针对驾驶场景中其他交通车辆不确定性的影响，在轨迹规划的过程中综合考虑其他交通车辆未来的运动轨迹，将车辆的交互作用融入轨迹规划的结果中，及时处理未来时刻存在的碰撞可能性对RRT算法搜索过程的影响，从而为无人车规划出更加安全的轨迹，增强轨迹规划结果的安全性、智能性和合理性。

以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

Claims

1.一种无人车局部路径规划方法，其特征在于，包括如下步骤：

采用B样条曲线对所述无人车的规划路径进行拟合，以生成满足车辆运动约束的无人车的局部路径;

建立概率地图具体为：

，

其中，表示势场总和，/>表示栅格地图中的某一位置，/>是正的势场常数;

所述基于所述环境车的未来轨迹预测信息和所述概率地图，采用改进RRT算法对所述无人车进行局部路径规划，以获取无人车的局部路径具体包括：

基于所述环境车的未来轨迹预测信息和所述无人车的采样新节点，对无人车进行碰撞检测；

2.根据权利要求1所述的无人车局部路径规划方法，其特征在于，所述根据无人车周围环境车的历史轨迹信息以及道路环境上下文信息获取无人车周围环境车的未来轨迹预测信息具体包括：

获取不同场景下环境车的历史轨迹坐标以及对应的历史场景上下文信息，利用不同场景下环境车的历史轨迹坐标以及对应的历史场景上下文信息对长短期记忆网络进行训练；

采用奖励函数对所述长短期记忆网络进行正则化处理；

3.根据权利要求1所述的无人车局部路径规划方法，其特征在于，所述采用人工势场法对所述无人车的行驶环境进行建模，在栅格图中建立概率地图具体包括：

4.根据权利要求3所述的无人车局部路径规划方法，其特征在于，所述势场函数具体为：

，

其中，表示分道线势场，/>表示道路边界线势场，/>表示环境车势场，/>表示障碍物势场，/>表示目标点势场， />表示分道线势场系数，/>表示第i条分道线在Y方向上的位置，/>表示分道线势场的收敛系数，/>表示道路边界势场系数，j的取值为 1 或 2，/>为第j条道路边界线的位置，/>表示纵向势场系数，K表示距离无人车安全范围最右侧的距离，/>表示汤川势系数，/>表示环境车势场的收敛系数，d表示横向距离，/>表示障碍物系数，/>表示距离障碍物前后两侧的距离，/>是正的势场常数，/>是目标点势场的系数。

5.根据权利要求1所述的无人车局部路径规划方法，其特征在于，采用三次B样条曲线对所述无人车的规划路径进行拟合。

6.一种无人车局部路径规划装置，其特征在于，包括：

曲线拟合模块，用于采用B样条曲线对所述无人车的规划路径进行拟合，以生成满足车辆运动约束的无人车的局部路径；

建立概率地图具体为：

，

7.一种无人车局部路径规划设备，其特征在于，包括：处理器和存储器；

所述处理器执行所述计算机可读程序时实现如权利要求1-5任意一项所述的无人车局部路径规划方法中的步骤。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如权利要求1-5任意一项所述的无人车局部路径规划方法中的步骤。