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

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

Info

Publication number
CN112985445A
CN112985445A CN202110421757.8A CN202110421757A CN112985445A CN 112985445 A CN112985445 A CN 112985445A CN 202110421757 A CN202110421757 A CN 202110421757A CN 112985445 A CN112985445 A CN 112985445A
Authority
CN
China
Prior art keywords
path
lane
point
precision map
planning
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110421757.8A
Other languages
English (en)
Other versions
CN112985445B (zh
Inventor
徐忠建
陈磊
钱志奇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Speed China Technology Co Ltd
Original Assignee
Speed Space Time Information Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Speed Space Time Information Technology Co Ltd filed Critical Speed Space Time Information Technology Co Ltd
Priority to CN202110421757.8A priority Critical patent/CN112985445B/zh
Publication of CN112985445A publication Critical patent/CN112985445A/zh
Application granted granted Critical
Publication of CN112985445B publication Critical patent/CN112985445B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
    • G01C21/3484—Personalized, e.g. from learned user behaviour or user-defined profiles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Social Psychology (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Traffic Control Systems (AREA)

Abstract

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

Description

基于高精地图的车道级精度实时性运动规划方法
技术领域
本发明属于高精地图无人驾驶领域，具体涉及一种基于高精地图的车道级精度实时性运动规划方法。
背景技术
近些年随着高精地图、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个等分点确定备选局部动态路径，半径计算公式为：
Figure DEST_PATH_IMAGE001
；
式中，
Figure 198818DEST_PATH_IMAGE002
表示曲率半径，可根据不同的场景以及行驶速度选取不同的取值范围；
S422：分别对局部动态路径以及备选局部动态路径计算代价值，并保留代价较小的路径，代价函数为：
Figure DEST_PATH_IMAGE003
式中，c表示代价值；k1，k2分别为权重系数，权重系数之和为1，各个权重系数可根据实际情况设定；n表示每条路径的结点数量；di表示每条路径中各段路径与障碍物的垂直距离；tj表示每条路径各点在t方向上的位移。
与现有技术相比，本发明提出的基于高精地图的车道级精度实时性运动规划方法，充分考虑到了人的驾驶行为习惯、车的动力学约束以及安全性等指标，并结合全局路径规划的统筹性与局部路径规划的细节性优势，能够在大多数结构以及非结构化的环境中实现无人车辆较为舒适安全的自主导航运动。
附图说明
图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为优化后路径，半径计算公式为：
Figure 443855DEST_PATH_IMAGE004
；
式中，
Figure DEST_PATH_IMAGE005
表示曲率半径，可根据不同的场景以及行驶速度选取不同的取值范围；
S422：分别对局部动态路径以及备选局部动态路径计算代价值，并保留代价较小的路径，代价函数为：
Figure 285909DEST_PATH_IMAGE006
式中，c表示代价值；k1，k2分别为权重系数，权重系数之和为1，各个权重系数可根据实际情况设定；n表示每条路径的结点数量；di表示每条路径中各段路径与障碍物的垂直距离；tj表示每条路径各点在t方向上的位移。
S43：当无人车辆从局部动态路径再回到全局路径的过程中，该无人车辆与后方车辆在s方向的间距应大于10~15m；当无人车辆再回到全局路径后，则按步骤S2进行的全局路径规划行驶至终点。
以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种基于高精地图的车道级精度实时性运动规划方法，其特征在于，具体包括以下步骤：
S1：根据高精地图构建车道关系邻接表；判断无人车辆的当前位置是否位于高精地图覆盖区域内，若在，则进行步骤S2；若不位于高精地图所覆盖区域，则转步骤S3，再进行步骤S2；
S2：确定行驶的起点与终点，按照广度优先的原则搜索至少一条从起点到终点的可通行路径，并选择最优的可通行路径，获得全局路径；行驶过程中并判断是否需要变道或者避障，若不需要则从所述全局路径行驶至终点；若需要，则转步骤S4；
S3：以与当前位置距离最近的且位于所述高精地图所覆盖区域的车道为目标，规划至少一条前往目标车道区域的局部路径；
S4：以所述全局路径规划为导向进行局部动态路径的规划，获得局部动态路径后，再从规划的局部动态路径返回至全局路径中行驶至终点。
2.根据权利要求1所述的基于高精地图的车道级精度实时性运动规划方法，其特征在于，所述步骤S1中构建车道关系邻接表的具体构建方法为：分别对每一段车道搜索其后向车道，以当前车道编码为键，以该当前车道的所有后向车道的编码的集合为值，构建搜索字典表。
3.根据权利要求2所述的基于高精地图的车道级精度实时性运动规划方法，其特征在于，所述步骤S2中按照广度优先的原则搜索至少一条从起点到终点的可通行路径的具体搜索方法为：
S21：根据定位判断当前无人车辆所在的位置，若无人车辆处于高精地图所覆盖的区域，记录当前所在车道编码，并将当前所在车道编码加入搜索队列；
S22：查找搜索队列中的第一个车道编码，判断该第一个车道编码是否属于目标车道编码，若不是目标车道编码，则移除该第一个车道编码；然后通过步骤S1中的车道关系邻接表查找所述当前所在车道编码的对应键的值，并将当前所在车道编码的所有的邻接编码加入到搜索队列中；同时将该当前所在车道编码标记为已遍历，并以该当前所在车道编码为容器的索引值，储存该当前所在车道编码的前置车道编码信息；
S23：转向步骤S22，重复判断，直到搜索队列中的第一个车道编码为目标车道所在编码，或队列为空，即没有找到起点到终点的通行路径；
S24：通过步骤S22的容器中所存储的前置车道编码信息，以递归的方式从终点所在车道编码反向搜索找到所有通行路径中所包含的车道编码，并根据全局路径代价模型选择最优路径作为全局路径。
4.根据权利要求1所述的基于高精地图的车道级精度实时性运动规划方法，其特征在于，当所述步骤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：对得到的规划路径的随机树进行简化和平滑处理，从而获得局部路径。
5.根据权利要求4所述的基于高精地图的车道级精度实时性运动规划方法，其特征在于，所述步骤S38中进行简化和平滑处理的方法的具体步骤包括：
S381：从规划路径的起点开始，删除位于偶数位的节点，若最后一个节点为偶数则需保留，形成一条新的通行路径；
S382：对所述步骤S381中的新的通行路径判断每一段路径是否会与障碍物发生碰撞，若发生碰撞则恢复该段路径在步骤S381中删除的节点；同时判断该段路径与相邻路径的夹角是否小于135°，若小于135°则恢复该段路径的后一段道路在步骤S381中删除的节点；
S383：重复步骤S381~382，直至无法简化为止；
S384：对简化后的该段路径进行多项式曲线拟合，当无人车辆通过简化后的该段路径并行驶至目标车道段后，然后再转向步骤S21~S24，规划出一条从起点到终点的全局路径。
6.根据权利要求5所述的基于高精地图的车道级精度实时性运动规划方法，其特征在于，所述步骤S4中当无人车辆行驶过程中若检测到前方行驶路径中存在前方有行驶车辆或障碍物时，则实时以全局路径规划为导向进行局部动态路径的规划，获得局部动态路径以避开前方行驶车辆或障碍物；其中以全局路径规划为导向进行局部动态路径的规划的方法具体步骤包括：
S41：规划方法采用高精地图格式中定义的s-t坐标系，将无人车辆的当前运动分解至s-t坐标系中的s方向及t方向，并在s方向及t方向上进行采样，获得多个采样点，然后将采样点进行两两组合，即获得一条通往目标车道的多段路径；
S42：采用步骤S38的简化和平滑处理的方法对步骤S41中获得的通往目标车道的多段路径进行简化和平滑处理，并对简化后的路径进行多次多项式曲线拟合，再进行二次优化处理，获得局部动态路径；
S43：当无人车辆从局部动态路径再回到全局路径的过程中，该无人车辆与后方车辆在s方向的间距应大于10~15m；当无人车辆再回到全局路径后，则按步骤S2进行的全局路径规划行驶至终点。
7.根据权利要求6所述的基于高精地图的实时性车道级精度运动的规划方法，其特征在于，所述步骤S42中所述二次优化处理的方法的步骤包括：
S421：若简化和平滑处理后的多段路径中存在偏离车道中心线过大的点，假设P为偏离较大的点，则以BC路径为弦，以R半径画圆，并在劣弧BC中***n个等分点确定备选局部动态路径，半径计算公式为：
Figure DEST_PATH_IMAGE002
；
式中，
Figure DEST_PATH_IMAGE004
表示曲率半径，可根据不同的场景以及行驶速度选取不同的取值范围；
S422：分别对局部动态路径以及备选局部动态路径计算代价值，并保留代价较小的路径，代价函数为：
Figure DEST_PATH_IMAGE006
式中，c表示代价值；k1，k2分别为权重系数，权重系数之和为1，各个权重系数可根据实际情况设定；n表示每条路径的结点数量；di表示每条路径中各段路径与障碍物的垂直距离；tj表示每条路径各点在t方向上的位移。
CN202110421757.8A 2021-04-20 2021-04-20 基于高精地图的车道级精度实时性运动规划方法 Active CN112985445B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110421757.8A CN112985445B (zh) 2021-04-20 2021-04-20 基于高精地图的车道级精度实时性运动规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110421757.8A CN112985445B (zh) 2021-04-20 2021-04-20 基于高精地图的车道级精度实时性运动规划方法

Publications (2)

Publication Number Publication Date
CN112985445A true CN112985445A (zh) 2021-06-18
CN112985445B CN112985445B (zh) 2021-08-13

Family

ID=76341238

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110421757.8A Active CN112985445B (zh) 2021-04-20 2021-04-20 基于高精地图的车道级精度实时性运动规划方法

Country Status (1)

Country Link
CN (1) CN112985445B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113393055A (zh) * 2021-07-05 2021-09-14 苏州清研捷运信息科技有限公司 一种货车导航沿途限行数据的预处理及使用方法
CN113483775A (zh) * 2021-06-30 2021-10-08 上海商汤临港智能科技有限公司 路径预测方法及装置、电子设备及计算机可读存储介质
CN113721608A (zh) * 2021-08-16 2021-11-30 河南牧原智能科技有限公司 机器人局部路径规划方法、***和可读存储介质
CN113778102A (zh) * 2021-09-22 2021-12-10 重庆长安汽车股份有限公司 Avp全局路径规划***、方法、车辆及存储介质
CN113865600A (zh) * 2021-09-28 2021-12-31 北京三快在线科技有限公司 一种高精地图的构建方法及装置
CN114003026A (zh) * 2021-06-22 2022-02-01 的卢技术有限公司 一种基于Apollo框架改进的变道机制
CN114326737A (zh) * 2021-12-30 2022-04-12 深兰人工智能(深圳)有限公司 路径规划方法、装置、电子设备及计算机可读存储介质
CN114413926A (zh) * 2021-12-13 2022-04-29 武汉中海庭数据技术有限公司 基于mapbox引擎osm数据与高精度数据的地图显示方法
CN114724377A (zh) * 2022-06-01 2022-07-08 华砺智行(武汉)科技有限公司 基于车路协同技术的无人驾驶车辆引导方法及***
CN115493609A (zh) * 2022-09-27 2022-12-20 禾多科技（北京）有限公司 车道级路径信息生成方法、装置、设备、介质和程序产品
CN116539058A (zh) * 2023-05-08 2023-08-04 山东省交通规划设计院集团有限公司 一种具有线路规划功能的导航***

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6169956B1 (en) * 1997-02-28 2001-01-02 Aisin Aw Co., Ltd. Vehicle navigation system providing for determination of a point on the border of a map stored in memory on the basis of a destination remote from the area covered by the map
CN107664503A (zh) * 2016-07-29 2018-02-06 上海汽车集团股份有限公司 车辆路径规划方法及装置
CN107992050A (zh) * 2017-12-20 2018-05-04 广州汽车集团股份有限公司 无人驾驶汽车局部路径运动规划方法和装置
CN108007471A (zh) * 2016-10-28 2018-05-08 北京四维图新科技股份有限公司 车辆引导区块获取方法和装置以及自动驾驶方法和***
CN108227695A (zh) * 2016-12-14 2018-06-29 现代自动车株式会社 自动驾驶控制装置、包括该装置的***及其方法
CN108519773A (zh) * 2018-03-07 2018-09-11 西安交通大学 一种结构化环境下无人驾驶车辆的路径规划方法
US20200149906A1 (en) * 2017-08-31 2020-05-14 Guangzhou Xiaopeng Motors Technology Co., Ltd. Path planning method, system and device for autonomous driving
CN111289006A (zh) * 2020-03-20 2020-06-16 上海商汤临港智能科技有限公司 车道导航路径生成方法及装置、驾驶控制方法及装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6169956B1 (en) * 1997-02-28 2001-01-02 Aisin Aw Co., Ltd. Vehicle navigation system providing for determination of a point on the border of a map stored in memory on the basis of a destination remote from the area covered by the map
CN107664503A (zh) * 2016-07-29 2018-02-06 上海汽车集团股份有限公司 车辆路径规划方法及装置
CN108007471A (zh) * 2016-10-28 2018-05-08 北京四维图新科技股份有限公司 车辆引导区块获取方法和装置以及自动驾驶方法和***
CN108227695A (zh) * 2016-12-14 2018-06-29 现代自动车株式会社 自动驾驶控制装置、包括该装置的***及其方法
US20200149906A1 (en) * 2017-08-31 2020-05-14 Guangzhou Xiaopeng Motors Technology Co., Ltd. Path planning method, system and device for autonomous driving
CN107992050A (zh) * 2017-12-20 2018-05-04 广州汽车集团股份有限公司 无人驾驶汽车局部路径运动规划方法和装置
CN108519773A (zh) * 2018-03-07 2018-09-11 西安交通大学 一种结构化环境下无人驾驶车辆的路径规划方法
CN111289006A (zh) * 2020-03-20 2020-06-16 上海商汤临港智能科技有限公司 车道导航路径生成方法及装置、驾驶控制方法及装置

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114003026A (zh) * 2021-06-22 2022-02-01 的卢技术有限公司 一种基于Apollo框架改进的变道机制
CN113483775A (zh) * 2021-06-30 2021-10-08 上海商汤临港智能科技有限公司 路径预测方法及装置、电子设备及计算机可读存储介质
CN113393055A (zh) * 2021-07-05 2021-09-14 苏州清研捷运信息科技有限公司 一种货车导航沿途限行数据的预处理及使用方法
CN113721608A (zh) * 2021-08-16 2021-11-30 河南牧原智能科技有限公司 机器人局部路径规划方法、***和可读存储介质
CN113778102B (zh) * 2021-09-22 2023-05-12 重庆长安汽车股份有限公司 Avp全局路径规划***、方法、车辆及存储介质
CN113778102A (zh) * 2021-09-22 2021-12-10 重庆长安汽车股份有限公司 Avp全局路径规划***、方法、车辆及存储介质
CN113865600A (zh) * 2021-09-28 2021-12-31 北京三快在线科技有限公司 一种高精地图的构建方法及装置
CN114413926A (zh) * 2021-12-13 2022-04-29 武汉中海庭数据技术有限公司 基于mapbox引擎osm数据与高精度数据的地图显示方法
CN114413926B (zh) * 2021-12-13 2023-10-13 武汉中海庭数据技术有限公司 基于mapbox引擎osm数据与高精度数据的地图显示方法
CN114326737A (zh) * 2021-12-30 2022-04-12 深兰人工智能(深圳)有限公司 路径规划方法、装置、电子设备及计算机可读存储介质
CN114724377A (zh) * 2022-06-01 2022-07-08 华砺智行(武汉)科技有限公司 基于车路协同技术的无人驾驶车辆引导方法及***
CN115493609A (zh) * 2022-09-27 2022-12-20 禾多科技（北京）有限公司 车道级路径信息生成方法、装置、设备、介质和程序产品
CN116539058A (zh) * 2023-05-08 2023-08-04 山东省交通规划设计院集团有限公司 一种具有线路规划功能的导航***

Also Published As

Publication number Publication date
CN112985445B (zh) 2021-08-13

Similar Documents

Publication Publication Date Title
CN112985445B (zh) 基于高精地图的车道级精度实时性运动规划方法
US11460311B2 (en) Path planning method, system and device for autonomous driving
US8121749B1 (en) System for integrating dynamically observed and static information for route planning in a graph based planner
US11747826B2 (en) Method for route optimization based on dynamic window and redundant node filtering
CN111694356B (zh) 一种行驶控制方法、装置、电子设备及存储介质
CN110333659B (zh) 一种基于改进a星搜索的无人驾驶汽车局部路径规划方法
CN111375205B (zh) 游戏中寻路路径的处理方法、装置、电子设备及存储介质
CN115713856A (zh) 一种基于交通流预测与实际路况的车辆路径规划方法
CN110320919B (zh) 一种未知地理环境中的巡回机器人路径优化方法
KR20210121595A (ko) 최적 경로 탐색 장치 및 이의 동작 방법
CN112033426B (zh) 行驶路径规划方法、装置及电子设备
CN107289938A (zh) 一种地面无人平台局部路径规划方法
KR102425741B1 (ko) 차선 인식 실패 대응 자율 주행 방법 및 자율주행 차량 주행 가이드 데이터 구축 방법
CN114115298A (zh) 一种无人车路径平滑方法及***
CN115079702A (zh) 一种混合道路场景下的智能车辆规划方法和***
CN108919805A (zh) 一种车辆无人驾驶辅助***
CN113515111A (zh) 一种车辆避障路径规划方法及装置
CN110849385B (zh) 基于双层启发搜索共轭梯度下降的轨迹规划方法及***
CN116560360A (zh) 面向复杂动态场景的医用护理机器人实时动态路径规划方法及***
CN116610109A (zh) 基于梯度的前向蚁群算法无人车路径规划方法
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
CN115326070A (zh) 城市低空环境下基于三维切线图法的无人机航迹规划方法
Moreira Deep Reinforcement Learning for Automated Parking
CN114200939B (zh) 一种机器人防碰撞路径规划方法
CN114740898B (zh) 一种基于自由空间与a*算法的三维航迹规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP01 Change in the name or title of a patent holder

Address after: 210042 8 Blocks 699-22 Xuanwu Avenue, Xuanwu District, Nanjing City, Jiangsu Province

Patentee after: Speed Technology Co.,Ltd.

Address before: 210042 8 Blocks 699-22 Xuanwu Avenue, Xuanwu District, Nanjing City, Jiangsu Province

Patentee before: SPEED TIME AND SPACE INFORMATION TECHNOLOGY Co.,Ltd.

CP01 Change in the name or title of a patent holder