基于高精地图的车道级精度实时性运动规划方法

摘要

本发明公开了一种基于高精地图的车道级精度实时性运动规划方法，具体步骤为：S1根据高精地图构建车道关系邻接表；判断当前位置是否位于高精地图覆盖区域内，若在则转步骤S2，若不在则先转步骤S3后再转步骤S2；S2确定行驶的起点与终点，按照广度优先的原则搜索至少一条从起点到终点的可通行路径，获得全局路径；行驶过程中并判断是否需要变道或者避障，若不需要则从全局路径行驶至终点；若需要，则转步骤S4；S3以与当前位置距离最近的且位于高精地图所覆盖区域的车道为目标，规划至少一条前往目标车道区域的局部路径；S4以全局路径规划为导向进行局部动态路径的规划，再从规划的局部动态路径返回至全局路径中行驶至终点。

说明书

技术领域

本发明属于高精地图无人驾驶领域，具体涉及一种基于高精地图的车道级精度实时性运动规划方法。

背景技术

近些年随着高精地图、5G、人工智能、物联网等技术的快速发展，无人驾驶服务也愈来愈贴近人们的生活。但是由于目前国内的高精地图覆盖广度不够，无人车辆的运行环境可能既包括结构化的道路，也可能包括非结构化的野地。从规划的角度来看，无人驾驶车在正常的结构化，封闭化道路做决策和规划相对容易，而对于非结构化、开放性的道路场景仍然面临巨大的挑战，因此需要探索更好的规划方法帮助无人驾驶车适应不同的场景。

因此，有必要开发一种在高精地图的基础上，以全局路径规划为导向，并动态规划优化局部路径的基于高精地图的车道级精度实时性运动规划方法。

发明内容

本发明要解决的技术问题是提供一种在高精地图的基础上，以全局路径规划为导向，并动态规划优化局部路径的基于高精地图的车道级精度实时性运动规划方法。

为了解决上述技术问题，本发明采用的技术方案是：该基于高精地图的车道级精度实时性运动规划方法，具体包括以下步骤：

S1：根据高精地图构建车道关系邻接表；判断无人车辆的当前位置是否位于高精地图覆盖区域内，若在，则进行步骤S2；若不位于高精地图所覆盖区域，则转步骤S3，再进行步骤S2；

S2：确定行驶的起点与终点，按照广度优先的原则搜索至少一条从起点到终点的可通行路径，并选择最优的可通行路径，获得全局路径；行驶过程中并判断是否需要变道或者避障，若不需要则从所述全局路径行驶至终点；若需要，则转步骤S4；

S3：以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为目标，规划至少一条前往目标车道区域的局部路径；

S4：以所述全局路径规划为导向进行局部动态路径的规划，获得局部动态路径后，再从规划的局部动态路径返回至全局路径中行驶至终点。

采用上述技术方案，首先根据高精度地图进行全局路径规划，其次在无人车辆行驶的过程中根据路况，实时动态规划运动路径，使无人车辆能自主的从设定的起点行驶至设定的终点。本发明是以时间最优选择最优的可通行路径作为全局路径，至此完成全局路径的规划，为后续局部运动规划提供导向作用。

作为本发明的优选技术方案，所述步骤S1中构建车道关系邻接表的具体构建方法为：分别对每一段车道搜索其后向车道，以当前车道编码为键，以该当前车道的所有后向车道的编码的集合为值，构建搜索字典表。另外，本方法所使用的高精地图为以opendrive格式高精地图为基础，并在其内容上添加了其他的属性信息，其内包含各段车道之间的拓扑关系，邻近关系以及可通行不可通行区域等其他路况信息；高精地图中存在车道之间的拓扑关系即为每一段车道都存在其前向车道及后向车道，且每段车道都有其唯一编码，所以可依据此进行构建车道关系邻接表；在构建字典表的过程中，需要排除后向所有车道中与当前车道存在不可通行关系的车道编码。

作为本发明的优选技术方案，所述步骤S2中按照广度优先的原则搜索至少一条从起点到终点的可通行路径的具体搜索方法为：

S21：根据定位判断当前车子所在的位置，若车子处于高精地图所覆盖的区域，记录当前所在车道编码，并将当前所在车道编码加入搜索队列；

S22：查找搜索队列中的第一个车道编码，判断该第一个车道编码是否属于目标车道编码，若不是目标车道编码，则移除该第一个车道编码；然后通过步骤S1中的车道关系邻接表查找所述当前所在车道编码的对应键的值，并将当前所在车道编码的所有的邻接编码加入到搜索队列中；同时将该当前所在车道编码标记为已遍历，并以该当前所在车道编码为容器的索引值，储存该当前所在车道编码的前置车道编码信息；

S23：转向步骤S22，重复判断，直到搜索队列中的第一个车道编码为目标车道所在编码，或队列为空，即没有找到起点到终点的通行路径；

S24：通过步骤S22的容器中所存储的前置车道编码信息，以递归的方式从终点所在车道编码反向搜索找到所有通行路径中所包含的车道编码，并根据全局路径代价模型选择最优路径作为全局路径。

作为本发明的优选技术方案，当所述步骤S1中若不位于高精地图所覆盖区域，则转步骤S3，所述步骤S3使用改进的RRT*算法，以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为目标，规划一条前往目标车道区域的局部路径的具体的步骤为：

S31：首先以无人车辆当前位置为根节点生长随机树；

S32：在无人车辆的可探测空间中，生成一个随机点A，同时保证在改进的RRT*算法随机树生成的过程中，以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为导向，即每隔n个随机采样点，则使点A必定落入目标车道段区域内，以加快随机树向目标车道的收敛速度；

S33：在随机树上找到距离A点最近的那个点，记为点B；

S34：从点B回溯一条到达起点的最优路径，并判断BA方向是否与该路经的最后一条边大于135°，若大于135°，则转至步骤S35，否则重复步骤S32-S34；

S35：点B朝着点A的方向按照设定的步长生长，若没有碰到障碍物就把生长后的边添加到B点后形成新生成边；

S36：随后以新生成边的端点为中心选取设定范围内的树结点，以代价最优为原则进行重新选择其父节点和布线随机树；

S37：在无人车辆行进的过程中重复步骤S31-S37，直到行驶到目标地点，获得规划路径；

S38：对得到的规划路径的随机树进行简化和平滑处理，从而获得局部路径。

作为本发明的优选技术方案，所述步骤S38中进行简化和平滑处理的方法的具体步骤包括：

S381：从规划路径的起点开始，删除位于偶数位的节点，若最后一个节点为偶数则需保留，形成一条新的通行路径；

S382：对所述步骤S381中的新的通行路径判断每一段路径是否会与障碍物发生碰撞，若发生碰撞则恢复该段路径在步骤S381中删除的节点；同时判断该段路径与相邻路径的夹角是否小于135°，若小于135°则恢复该段路径的后一段道路在步骤S381中删除的节点；

S383：重复步骤S381~382，直至无法简化为止；

S384：对简化后的该段路径进行多项式曲线拟合，当无人车辆通过简化后的该段路径并行驶至目标车道段后，然后再转向步骤S21~S24，规划出一条从起点到终点的全局路径。一般进行3次或5次多项式曲线拟合。

作为本发明的优选技术方案，所述步骤S4中当无人车辆行驶过程中若检测到前方行驶路径中存在前方有行驶车辆或障碍物时，则实时以全局路径规划为导向进行局部动态路径的规划，获得局部动态路径以避开前方行驶车辆或障碍物；其中以全局路径规划为导向进行局部动态路径的规划的方法具体步骤包括：

S41：规划方法采用高精地图格式中定义的s-t坐标系，将无人车辆的当前运动分解至s-t坐标系中的s方向及t方向，并在s方向及t方向上进行采样，获得多个采样点，然后将采样点进行两两组合，即获得一条通往目标车道的多段路径；

S42：采用步骤S38的简化和平滑处理的方法对步骤S41中获得的通往目标车道的多段路径进行简化和平滑处理，并对简化后的路径进行多次多项式曲线拟合，再进行二次优化处理，获得局部动态路径；

S43：当无人车辆从局部动态路径再回到全局路径的过程中，该无人车辆与后方车辆在s方向的间距应大于10~15m；当无人车辆再回到全局路径后，则按步骤S2进行的全局路径规划行驶至终点。

考虑到车辆在变道过程中的安全及舒适情况，采样过程中需分别对s方向及t方向上的随机落点作如下限制：

（a）一般情况下应保证变道过程中无人车辆与标线的夹角尽量小，所以需控制无人车辆在单位时间t方向与s方向上的位移变化比值应小于1；

（b）为保证车辆在行驶过程中的横向偏移靠近车道中线，应使t方向落点全部落在可行车道段区域内，且每隔15个采样点在t方向上必有一个点落在邻近车道的中心线上；

（c）对于双向车道，t方向上的采样点必须偏向当前车道中心线的一侧，即采样过程中保证t始终大于或小于0；

（d）为保持车间距，应保证s方向上采样落点位置距离前方车辆或障碍物10~15m的安全距离，安全距离可根据不同场景设置不同的距离；

作为本发明的优选技术方案，所述步骤S42中所述二次优化处理的方法的步骤包括：

若简化和平滑处理后的多段路径中存在偏离车道中心线过大的点，假设P为偏离较大的点，则以BC路径为弦，以R半径画圆，并在劣弧BC中插入n个等分点确定备选局部动态路径，半径计算公式为：

式中，

S422：分别对局部动态路径以及备选局部动态路径计算代价值，并保留代价较小的路径，代价函数为：

式中，c表示代价值；k

与现有技术相比，本发明提出的基于高精地图的车道级精度实时性运动规划方法，充分考虑到了人的驾驶行为习惯、车的动力学约束以及安全性等指标，并结合全局路径规划的统筹性与局部路径规划的细节性优势，能够在大多数结构以及非结构化的环境中实现无人车辆较为舒适安全的自主导航运动。

附图说明

图1是本发明基于高精地图的车道级精度实时性运动规划方法的流程示意图；

图2是本发明基于高精地图的车道级精度实时性运动规划方法中的RRT*算法的改进的示意图；

图3是本发明基于高精地图的车道级精度实时性运动规划方法中的路径简化方法的示意图；

图4是本发明基于高精地图的车道级精度实时性运动规划方法中s-t坐标系示意图；

图5是本发明基于高精地图的车道级精度实时性运动规划方法中路径优化方法的示意图；

图 6是本发明基于高精地图的车道级精度实时性运动规划方法中局部动态路径优化后效果图。

具体实施方式

下面将结合本发明的实施例图中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。

实施例：如图1所示，该基于高精地图的实时性车道级精度运动的规划方法，具体包括以下步骤：

S1：根据高精地图构建车道关系邻接表；判断无人车辆的当前位置是否位于高精地图覆盖区域内，若在，则进行步骤S2；若不位于高精地图所覆盖区域，则转步骤S3，再进行步骤S2；

所述步骤S1中构建车道关系邻接表的具体构建方法为：分别对每一段车道搜索其后向车道，以当前车道编码为键，以该当前车道的所有后向车道的编码的集合为值，构建搜索字典表；

S2：确定行驶的起点与终点，按照广度优先的原则搜索至少一条从起点到终点的可通行路径，并选择最优的可通行路径，获得全局路径；行驶过程中并判断是否需要变道或者避障，若不需要则从所述全局路径行驶至终点；若需要，则转步骤S4；

所述步骤S2中按照广度优先的原则搜索至少一条从起点到终点的可通行路径的具体搜索方法为：

S21：根据定位判断当前车子所在的位置，若车子处于高精地图所覆盖的区域，记录当前所在车道编码，并将当前所在车道编码加入搜索队列；

S22：查找搜索队列中的第一个车道编码，判断该第一个车道编码是否属于目标车道编码，若不是目标车道编码，则移除该第一个车道编码；然后通过步骤S1中的车道关系邻接表查找所述当前所在车道编码的对应键的值，并将当前所在车道编码的所有的邻接编码加入到搜索队列中；同时将该当前所在车道编码标记为已遍历，并以该当前所在车道编码为容器的索引值，储存该当前所在车道编码的前置车道编码信息；

S23：转向步骤S22，重复判断，直到搜索队列中的第一个车道编码为目标车道所在编码，或队列为空，即没有找到起点到终点的通行路径；

S24：通过步骤S22的容器中所存储的前置车道编码信息，以递归的方式从终点所在车道编码反向搜索找到所有通行路径中所包含的车道编码，并根据全局路径代价模型选择最优路径作为全局路径；

S3：以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为目标，规划至少一条前往目标车道区域的局部路径；

当所述步骤S1中若不位于高精地图所覆盖区域，则转步骤S3，所述步骤S3使用改进的RRT*算法，以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为目标，规划一条前往目标车道区域的局部路径的具体的步骤为：

S31：首先以无人车辆当前位置为根节点生长随机树；

S32：在无人车辆的可探测空间中，生成一个随机点A，同时保证在改进的RRT*算法随机树生成的过程中，以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为导向，即每隔n个随机采样点，则使点A必定落入目标车道段区域内，以加快随机树向目标车道的收敛速度；

S33：在随机树上找到距离A点最近的那个点，记为点B；

S34：从点B回溯一条到达起点的最优路径，并判断BA方向是否与该路经的最后一条边大于135°，若大于135°，则转至步骤S35，否则重复步骤S32-S34；

S35：点B朝着点A的方向按照设定的步长生长，若没有碰到障碍物就把生长后的边添加到B点后形成新生成边；

S36：随后以新生成边的端点为中心选取设定范围内的树结点，以代价最优为原则进行重新选择其父节点和布线随机树；

S37：在无人车辆行进的过程中重复步骤S31-S37，直到行驶到目标地点，获得规划路径；

S38：对得到的规划路径的随机树进行简化和平滑处理，从而获得局部路径。

所述步骤S38中进行简化和平滑处理的方法的具体步骤包括：

S381：从规划路径的起点开始，删除位于偶数位的节点，若最后一个节点为偶数则需保留，形成一条新的通行路径；

S382：对所述步骤S381中的新的通行路径判断每一段路径是否会与障碍物发生碰撞，若发生碰撞则恢复该段路径在步骤S381中删除的节点；同时判断该段路径与相邻路径的夹角是否小于135°，若小于135°则恢复该段路径的后一段道路在步骤S381中删除的节点；如图3所示：假设1至8号点之间存在障碍物三角形，初始的路径规划为图中黑色实线所示，经过简化后为路线为1-5-8；

S383：重复步骤S381~382，直至无法简化为止；

S384：对简化后的该段路径进行多项式曲线拟合，当无人车辆通过简化后的该段路径并行驶至目标车道段后，然后再转向步骤S21~S24，规划出一条从起点到终点的全局路径；

S4：以所述全局路径规划为导向进行局部动态路径的规划，获得局部动态路径后，再从规划的局部动态路径返回至全局路径中行驶至终点；

所述步骤S2中若无人车辆需要避障或变道，则转步骤S4，如图5所示，所述步骤S4中当无人车辆行驶过程中若检测到前方行驶路径中存在前方有行驶车辆或障碍物时，则实时以全局路径规划为导向进行局部动态路径的规划，获得局部动态路径以避开前方行驶车辆或障碍物；其中以全局路径规划为导向进行局部动态路径的规划的方法具体步骤包括：

S41：采用高精地图格式中定义的s-t坐标系，将无人车辆的当前运动分解至s-t坐标系中的s方向及t方向，并在s方向及t方向上进行采样，获得多个采样点，然后将采样点进行两两组合，即获得一条通往目标车道的多段路径；因道路大都存在弯曲的情况，为简化计算，满足实时性要求，本发明使用opendrive高精地图格式中定义的s-t坐标系，如图4所示，s坐标是沿着道路参考线方向，t坐标位于参考线的侧向位置，左为正，右为负；首先根据无人车辆的位置确定当前所在的车道段，其次根据高精地图中的车道邻近关系信息以及车辆的感知系统确定其可通行的车道区域，最后以全局路径规划为导向确定最终要行驶到的车道段区域；

考虑到车辆在变道过程中的安全及舒适情况，采样过程中需分别对s方向及t方向上的随机落点作如下限制：

（a）一般情况下应保证变道过程中无人车辆与标线的夹角尽量小，所以需控制无人车辆在单位时间t方向与s方向上的位移变化比值应小于1；

（b）为保证车辆在行驶过程中的横向偏移靠近车道中线，应使t方向落点全部落在可行车道段区域内，且每隔15个采样点在t方向上必有一个点落在邻近车道的中心线上；

（c）对于双向车道，t方向上的采样点必须偏向当前车道中心线的一侧，即采样过程中保证t始终大于或小于0；

（d）为保持车间距，应保证s方向上采样落点位置距离前方车辆或障碍物10~15m的安全距离，安全距离可根据不同场景设置不同的距离；

S42：采用步骤S38的简化和平滑处理的方法对步骤S41中获得的通往目标车道的多段路径进行简化和平滑处理，并对简化后的路径进行多次多项式曲线拟合，再进行二次优化处理，获得局部动态路径；

所述步骤S42中所述二次优化处理的方法的步骤包括：

S421：若简化和平滑处理后的多段路径中存在偏离车道中心线过大的点，假设P为偏离较大的点，则以BC路径为弦，以R半径画圆，并在劣弧BC中插入n个等分点确定备选局部动态路径，如图6所示，A-B-P-C-D初始规划路径，A-B-P1-P2-P3-C-D为优化后路径，半径计算公式为：

式中，

S422：分别对局部动态路径以及备选局部动态路径计算代价值，并保留代价较小的路径，代价函数为：

式中，c表示代价值；k

S43：当无人车辆从局部动态路径再回到全局路径的过程中，该无人车辆与后方车辆在s方向的间距应大于10~15m；当无人车辆再回到全局路径后，则按步骤S2进行的全局路径规划行驶至终点。

以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。