CN114764249A - 实时避障路径规划方法、系统、设备及介质 - Google Patents

实时避障路径规划方法、系统、设备及介质 Download PDF

Info

Publication number
CN114764249A
CN114764249A CN202210454092.5A CN202210454092A CN114764249A CN 114764249 A CN114764249 A CN 114764249A CN 202210454092 A CN202210454092 A CN 202210454092A CN 114764249 A CN114764249 A CN 114764249A
Authority
CN
China
Prior art keywords
random tree
node
planning
new node
new
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
CN202210454092.5A
Other languages
English (en)
Other versions
CN114764249B (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.)
Xian University of Architecture and Technology
Original Assignee
Xian University of Architecture and Technology
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 Xian University of Architecture and Technology filed Critical Xian University of Architecture and Technology
Priority to CN202210454092.5A priority Critical patent/CN114764249B/zh
Publication of CN114764249A publication Critical patent/CN114764249A/zh
Application granted granted Critical
Publication of CN114764249B publication Critical patent/CN114764249B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • 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
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

本发明公开了一种实时避障路径规划方法、系统、设备及介质，方法包括：根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；以规划起点初始化随机树A，以规划终点初始化随机树B；在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果；本发明通过双树搜索策略，为自动驾驶车辆规划出从规划起点到规划终点的路径，确保了到达目标点的航向的可控性，路径规划结果执行性较优。

Description

实时避障路径规划方法、系统、设备及介质
技术领域
本发明属于车辆无人驾驶技术领域，特别涉及一种实时避障路径规划方法、系统、设备及介质。
背景技术
在无人驾驶矿用卡车自主运行时，由于矿区道路环境复杂，常常有山体落石发生；因此，对无人驾驶矿用卡车的实时避障路径规划已成为亟待解决的问题之一；目前，大多采用传统的快速搜索随机树算法(Rapidly-exploring random tree，RRT)进行路径规划，其存在耗时较长，到达目标点的航向不可控，路径规划结果可执行性较差的问题。
发明内容
针对现有技术中存在的技术问题，本发明提供了一种实时避障路径规划方法、系统、设备及介质，以解决现有的路径规划过程，耗时较长，到达目标点的航向不可控，路径规划结果可执行性较差的技术问题。
为达到上述目的，本发明采用的技术方案为：
本发明提供了一种实时避障路径规划方法，包括：
根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；
以规划起点初始化随机树A，以规划终点初始化随机树B；
在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；
将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
进一步的，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域的过程，具体如下：
获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域；
将所述矩形区域以预设固定长度划分为若干个栅格；
以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；
将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域。
进一步的，以规划起点初始化随机树A，以规划终点初始化随机树B的过程，具体如下：
将规划起点作为随机树A的根节点，将规划终点作为随机树B的根节点。
进一步的，在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇的过程，具体如下：
(1)对所述随机树A进行搜索：
在所述自动驾驶车辆的可行驶区域内，随机生成采样点xrand；
根据预设代价函数在随机树A上搜索得到节点xnearest；其中，所述节点xnearest为使所述预设代价函数的值最小，且所述节点xnearest到所述采样点xrand的转弯角度小于最大允许转弯角度θmax；
沿节点xnearest与采样点xrand的连线方向上，拓展最大生长步长Lmax，得到节点xnew；
对节点xnew与节点xnearest之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；
若节点xnew与节点xnearest之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点xnew到随机树A中，即节点xnew为随机树A的新节点xA；
若节点xnew与节点xnearest之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点xnew与节点xnearest之间没有障碍物，之后将新的节点xnew将加入到随机树A中，即新的节点xnew为随机树A的新节点xA；
(2)按照上述对所述随机树A进行搜索操作，对所述随机树B进行搜索，至随机树B生长至新节点xB；
(3)判断所述随机树A的新节点xA与随机树B的新节点xB是否满足预设连接要求，若满足，则将随机树A的新节点xA与随机树B的新节点xB连接，算法终止，即随机树A与随机树B相遇；否则，返回步骤(1)和步骤(2)，分别对随机树A和随机树B继续进行拓展，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求。
进一步的，所述预设代价函数为：
Figure BDA0003619972080000031
其中，Df为采样点xrand到随机树A或随机树B上的节点的欧氏距离；
Figure BDA0003619972080000032
为父节点到采样点xrand的航向变化量；Dg为采样点xrand通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数；a2是参数；β为生成路径总路程的权值。
进一步的，对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测的过程，具体如下：
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(xleft,yleft)；
根据自动驾驶车辆的宽度w以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(xright,yright)为：
Figure BDA0003619972080000041
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
进一步的，所述预设的连接要求具体如下：
随机树A的新节点xA到随机树B的新节点xB的距离小于最大生长步长Lmax；
随机树A的新节点xA的新节点xA的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树B的新节点xB到新节点xB的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树A的新节点xA到随机树B的新节点xB的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
本发明还提供了一种实时避障路径规划系统，包括：
可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；
随机树模块，用于以规划起点初始化随机树A，以规划终点初始化随机树B；
搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；
路径模块，用于将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
本发明还提供了一种实时避障路径规划的车辆，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现所述的实时避障路径规划方法的步骤。
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的实时避障路径规划方法的步骤。
与现有技术相比，本发明的有益效果为：
本发明提供了一种实时避障路径规划方法及系统，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域，并构建随机树A和随机树B，通过双树搜索策略，为自动驾驶车辆规划出从规划起点到规划终点的路径，确保了到达目标点的航向的可控性，路径规划结果执行性较优；通过在双树搜索策略中引入代价函数策略、变步长搜索策略及主干停止生长策略，降低了双树路径规划的计算难度，在满足轨迹平滑性需求的基础上，提高规划速度。
附图说明
图1为本发明所述的实时避障路径规划方法的流程图；
图2为实施例中的自动驾驶车辆的可行驶区域示意图；
图3为实施例中的双树快速搜索的结果示意图。
具体实施方式
为了使本发明所解决的技术问题，技术方案及有益效果更加清楚明白，以下具体实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明提供了一种实时避障路径规划方法，包括以下步骤：
步骤1、根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；其中，确定自动驾驶车辆的可行驶区域的过程，具体如下：
步骤11、获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域。
步骤12、将所述矩形区域以预设固定长度划分为若干个栅格。
步骤13、以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；其中，所述道路为所述规划起点与所述规划终点之间的道路。
步骤14、将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域。
步骤2、以规划起点初始化随机树A，以规划终点初始化随机树B；其中，将规划起点作为随机树A的根节点，将规划终点作为随机树B的根节点。
步骤3、在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A和所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇。
其中，利用预设的双向RRT算法，对所述随机树A和所述随机树B进行搜索的过程，具体如下：
(1)对所述随机树A进行搜索：
在所述自动驾驶车辆的可行驶区域内，随机生成采样点xrand。
根据预设代价函数在随机树A上搜索得到节点xnearest；其中，所述节点xnearest为使所述预设代价函数的值最小，且所述节点xnearest到所述采样点xrand的转弯角度小于最大允许转弯角度θmax；其中，所述预设代价函数为：
Figure BDA0003619972080000061
其中，Df为采样点xrand到随机树A或随机树B上的节点的欧氏距离；
Figure BDA0003619972080000062
为父节点到采样点xrand的航向变化量；Dg为采样点xrand通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数，以使得在遍历寻找父节点的过程中排除掉该节点；a2是参数，a2的选择决定了生成路径的平滑程度和生成路径的速度，a2越大，生成的路径越平滑，但生成路径的速度越慢；β为生成路径总路程的权值。
沿节点xnearest与采样点xrand的连线方向上，拓展最大生长步长Lmax，得到节点xnew。
对节点xnew与节点xnearest之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；其中，所述碰撞检测过程，具体如下：
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(xleft,yleft)；
根据自动驾驶车辆的宽度w以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(xright,yright)为：
Figure BDA0003619972080000071
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
若节点xnew与节点xnearest之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点xnew到随机树A中，即节点xnew为随机树A的新节点xA；
若节点xnew与节点xnearest之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点xnew与节点xnearest之间没有障碍物，之后将新的节点xnew将加入到随机树A中，即新的节点xnew为随机树A的新节点xA；
(2)按照上述对所述随机树A进行搜索操作，对所述随机树B进行搜索，至随机树B生长至新节点xB；
(3)判断所述随机树A的新节点xA与随机树B的新节点xB是否满足预设连接要求，若满足，则将随机树A的新节点xA与随机树B的新节点xB连接，算法终止，即随机树A与随机树B相遇；否则，返回步骤(1)和步骤(2)，分别对随机树A和随机树B继续进行拓展，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求。
其中，所述预设连接要求，具体如下：
随机树A的新节点xA到随机树B的新节点xB的距离小于最大生长步长Lmax；
随机树A的新节点xA的新节点xA的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树B的新节点xB到新节点xB的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树A的新节点xA到随机树B的新节点xB的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
步骤4、将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
本发明所述的实时避障路径规划方法，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域，并构建随机树A和随机树B，通过双树搜索策略，为自动驾驶车辆规划出从规划起点到规划终点的路径，确保了到达目标点的航向的可控性，路径规划结果执行性较优；通过在双树搜索策略中引入代价函数策略、变步长搜索策略及主干停止生长策略，降低了双树路径规划的计算难度，在满足轨迹平滑性需求的基础上，提高规划速度。
本发明还提供了一种实时避障路径规划系统，包括可行域模块、随机树模块、搜索模块及路径模块；可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；随机树模块，用于以规划起点初始化随机树A，以规划终点初始化随机树B；搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；路径模块，用于将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
本发明还提供了一种实时避障路径规划设备，包括：存储器，用于存储计算机程序；处理器，用于执行所述计算机程序时实现实时避障路径规划方法的步骤。
所述处理器执行所述计算机程序时实现上述实时避障路径规划方法的步骤，例如：根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；以规划起点初始化随机树A，以规划终点初始化随机树B；在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
或者，所述处理器执行所述计算机程序时实现上述系统中各模块的功能，例如：可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；随机树模块，用于以规划起点初始化随机树A，以规划终点初始化随机树B；搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；路径模块，用于将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成预设功能的一系列计算机程序指令段，所述指令段用于描述所述计算机程序在所述实时避障路径规划设备中的执行过程。例如，所述计算机程序可以被分割成可行域模块、随机树模块、搜索模块及路径模块，各模块具体功能如下：可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；随机树模块，用于以规划起点初始化随机树A，以规划终点初始化随机树B；搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；路径模块，用于将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
所述实时避障路径规划设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述实时避障路径规划设备可包括，但不仅限于处理器、存储器。本领域技术人员可以理解，上述仅仅是实时避障路径规划设备的示例，并不构成对实时避障路径规划设备的限定，可以包括比上述更多的部件，或者组合某些部件，或者不同的部件，例如所述实时避障路径规划设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器可以是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器是所述实时避障路径规划设备的控制中心，利用各种接口和线路连接整个实时避障路径规划设备的各个部分。
所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述实时避障路径规划设备的各种功能。
所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(SmartMediaCard,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(FlashCard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种实时避障路径规划方法的步骤。
所述实时避障路径规划系统集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。
基于这样的理解，本发明实现上述实时避障路径规划方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，所述计算机程序在被处理器执行时，可实现上述实时避障路径规划方法的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或预设中间形式等。
所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-OnlyMemory，ROM)、随机存取存储器(RandomAccessMemory，RAM)、电载波信号、电信信号以及软件分发介质等。
需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。
实施例
以自动驾驶车辆在某矿区道路上的避障路径规划过程为例。
如附图1所示，本实施例1提供了一种实时避障路径规划方法，具体包括以下步骤：
步骤1、根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；所述自动驾驶车辆的可行驶区域的确定过程，具体如下：
步骤11、获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域；其中，所述道路两侧道沿坐标的获取过程，具体为：
获得车辆到左侧道沿的距离，从而确定左侧道沿坐标分别为(X0i,Y0i),(i＝0,1,…m)，右侧道沿的坐标分别为(X1j,Y1j),(j＝0,1,…n)。
所述矩形区域的构成过程为：道路两侧道沿的最小坐标(xmin,ymin)作为起始点，最大坐标(xmax,ymax)作为终止点，将最小坐标(xmin,ymin)作为矩形其中一个角的坐标，最大坐标(xmax,ymax)作为最小坐标对角的坐标，从而得到矩形区域。
步骤12、将所述矩形区域内以预设固定长度划分为若干栅格；其中，所述栅格为正方形，栅格的边长为S，且所述栅格的边长S大于等于测量长度A；其中，所述测量长度A为获取规划起点和规划终点之间道路的两侧道沿坐标时，对所述每隔测量长度A进行一次测量。
步骤13、以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；其中，所述道路为所述规划起点与所述规划终点之间的道路；其中，筛选出处于道路内的所有定位点的过程，具体如下：
设定位点为Ps(xs,ys)，找到左侧道沿的最近点P0near(x0near,y0near)和它在运动方向的下一个点的坐标P0near+1(x0near+1,y0near+1)，找到右侧道沿的最近点P1near(x1near,y1near)和它在运动方向的下一个点的坐标P1near+1(x1near+1,y1near+1)；
根据向量计算方法η＝(xnear-xs)×(ynear+1-ys)-(ynear-ys)×(xnear+1-xs)；
当η为正时，坐标Ps位于向量Pnear→Pnear+1的左边；当η为负时，坐标Ps位于向量Pnear→Pnear+1的右边。
分别把找到的左右两侧道沿的最近点和下一点的坐标带入η中，得到ηleft和ηright。当ηleft为负且ηright为正时，表示该点位于两侧道沿之间，为道路内；
其中xnear,ynear分别为：一侧道沿最近点的横坐标和纵坐标，xnear+1,ynear+1分别为：一侧道沿最近点运动方向的下一个点的坐标和纵坐标。
步骤14、将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域；具体的，在筛选出处于道路内的所有定位点后，依次判断车辆宽度和安全距离与每个定位点到道沿最近距离的关系，Dmin≥WT/2+Dsafe，其中，Dmin为当前定位点到道沿的最近距离，WT为自动驾驶车辆的宽度，Dsafe为安全距离，剔除不满足上述关系的定位点；本实施例中，所述自动驾驶车辆的可行驶区域，如附图2所示。
步骤2、以规划起点初始化随机树A，以规划终点初始化随机树B；其中，将规划起点作为随机树A的根节点，将规划终点作为随机树B的根节点。
步骤3、在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A和所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇。
其中，利用预设的双向RRT算法，对所述随机树A和所述随机树B进行搜索的过程，具体如下：
(1)对所述随机树A进行搜索：
在所述自动驾驶车辆的可行驶区域内，随机生成采样点xrand。
根据预设代价函数在随机树A上搜索得到节点xnearest；其中，所述节点xnearest为使所述预设代价函数的值最小，且所述节点xnearest到所述采样点xrand的转弯角度小于最大允许转弯角度θmax；其中，所述预设代价函数为：
Figure BDA0003619972080000141
其中，Df为采样点xrand到随机树A或随机树B上的节点的欧氏距离；
Figure BDA0003619972080000142
为父节点到采样点xrand的航向变化量；Dg为采样点xrand通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数，以使得在遍历寻找父节点的过程中排除掉该节点；a2是参数，a2的选择决定了生成路径的平滑程度和生成路径的速度，a2越大，生成的路径越平滑，但生成路径的速度越慢；β为生成路径总路程的权值。
本实施例中，所述随机树搜索代价函数最小节点的过程中，在树中寻找代价函数最小节点的过程中，并不是一定要从根节点开始的。在一开始的搜索过程中，我们规定当节点数小于M时，寻找代价函数最小的节点的过程是从根节点开始的，保障树的高速扩张性；当节点数大于M时，如果发生某一父节点J的子节点数达到预设最大值P，则取出该父节点J所对应其所在树中第Nj个节点的属性，下一次寻找代价函数从Nj开始寻找。
沿节点xnearest与采样点xrand的连线方向上，拓展最大生长步长Lmax，得到节点xnew。
对节点xnew与节点xnearest之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；其中，所述碰撞检测过程，具体如下：
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(xleft,yleft)；
根据自动驾驶车辆的宽度W以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(xright,yright)为：
Figure BDA0003619972080000143
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
若节点xnew与节点xnearest之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点xnew到随机树A中，即节点xnew为随机树A的新节点xA；
若节点xnew与节点xnearest之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点xnew与节点xnearest之间没有障碍物，之后将新的节点xnew将加入到随机树A中，即新的节点xnew为随机树A的新节点xA；
(2)按照上述对所述随机树A进行搜索操作，对所述随机树B进行搜索，至随机树B生长至新节点xB；
(3)判断所述随机树A的新节点xA与随机树B的新节点xB是否满足预设连接要求，若满足，则将随机树A的新节点xA与随机树B的新节点xB连接，算法终止，即随机树A与随机树B相遇；否则，返回步骤(1)和步骤(2)，分别对随机树A和随机树B继续进行拓展，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求。
其中，所述预设连接要求，具体如下：
随机树A的新节点xA到随机树B的新节点xB的距离小于最大生长步长Lmax；
随机树A的新节点xA的新节点xA的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树B的新节点xB到新节点xB的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树A的新节点xA到随机树B的新节点xB的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
本实施例中，所述的随机树A和随机树B上的节点，具有如下属性：该节点的代价函数值val；左侧前车轮坐标(xleft,yleft)；右侧前车轮坐标(xright,yright)；到起始点的距离Dg；该节点是父节点的第几个节点；从父节点到该节点的角度θj，逆时针为正；该节点是树生成的第Nj个节点；该节点指向父节点的指针。
步骤4、将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果，如附图3所示；由记录下停止生长时随机树A的终止节点xA和随机树B的终止节点xB，通过该节点指向父节点的指针获取父节点的信息，一直循环指到根节点为止，从而获取一棵树的路径。把从起始节点生长树得到的路径反向存储，再与从终止节点生长树得到的路径相合并，即可得到一条完整的路径。
本发明所述的实时避障路径规划方法、系统、设备及介质，使用超快速双树RRT算法，通过双树的策略保证到达目标点的航向问题，并引入代价函数代替传统的距离搜索条件，在满足轨迹平滑性需求的基础上，提高规划速度，从而满足无人矿卡在避障路径规划中的实时需求。
上述实施例仅仅是能够实现本发明技术方案的实施方式之一，本发明所要求保护的范围并不仅仅受本实施例的限制，还包括在本发明所公开的技术范围内，任何熟悉本技术领域的技术人员所容易想到的变化、替换及其他实施方式。

Claims (10)

1.一种实时避障路径规划方法，其特征在于，包括：
根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；
以规划起点初始化随机树A，以规划终点初始化随机树B；
在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；
将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
2.根据权利要求1所述的一种实时避障路径规划方法，其特征在于，根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域的过程，具体如下：
获取规划起点和规划终点之间道路的两侧道沿坐标，并将所述规划起点的道沿坐标和所述规划终点的道沿坐标作为对角连线，构成矩形区域；
将所述矩形区域以预设固定长度划分为若干个栅格；
以每个栅格的四个角作为定位点，筛选出处于道路内的所有定位点；
将处于道路内的所有定位点形成的区域作为自动驾驶车辆的可行驶区域。
3.根据权利要求1所述的一种实时避障路径规划方法，其特征在于，以规划起点初始化随机树A，以规划终点初始化随机树B的过程，具体如下：
将规划起点作为随机树A的根节点，将规划终点作为随机树B的根节点。
4.根据权利要求1所述的一种实时避障路径规划方法，其特征在于，在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇的过程，具体如下：
(1)对所述随机树A进行搜索：
在所述自动驾驶车辆的可行驶区域内，随机生成采样点xrand；
根据预设代价函数在随机树A上搜索得到节点xnearest；其中，所述节点xnearest为使所述预设代价函数的值最小，且所述节点xnearest到所述采样点xrand的转弯角度小于最大允许转弯角度θmax；
沿节点xnearest与采样点xrand的连线方向上，拓展最大生长步长Lmax，得到节点xnew；
对节点xnew与节点xnearest之间进行障碍物检测，并对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测；
若节点xnew与节点xnearest之间无障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则将加入节点xnew到随机树A中，即节点xnew为随机树A的新节点xA；
若节点xnew与节点xnearest之间有障碍物，且自动驾驶车辆的两个前轮未与所述自动驾驶车辆的可行驶区域的边界发生碰撞，则以预设的所述自动驾驶车辆的可行驶区域的栅格长度为递减量，逐步递减直至新的节点xnew与节点xnearest之间没有障碍物，之后将新的节点xnew将加入到随机树A中，即新的节点xnew为随机树A的新节点xA；
(2)按照上述对所述随机树A进行搜索操作，对所述随机树B进行搜索，至随机树B生长至新节点xB；
(3)判断所述随机树A的新节点xA与随机树B的新节点xB是否满足预设连接要求，若满足，则将随机树A的新节点xA与随机树B的新节点xB连接，算法终止，即随机树A与随机树B相遇；否则，返回步骤(1)和步骤(2)，分别对随机树A和随机树B继续进行拓展，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求。
5.根据权利要求4所述的一种实时避障路径规划方法，其特征在于，所述预设代价函数为：
Figure FDA0003619972070000031
其中，Df为采样点xrand到随机树A或随机树B上的节点的欧氏距离；
Figure FDA0003619972070000032
为父节点到采样点xrand的航向变化量；Dg为采样点xrand通过父节点后顺着树枝返回根节点的欧氏距离的总路程之和；α1是参数；a2是参数；β为生成路径总路程的权值。
6.根据权利要求4所述的一种实时避障路径规划方法，其特征在于，对自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界进行碰撞检测的过程，具体如下：
根据随机数生成策略，生成自动驾驶车辆的左前轮坐标(xleft,yleft)；
根据自动驾驶车辆的宽度w以及车辆的行进方向θj，计算自动驾驶车辆的右前轮坐标；其中，所述自动驾驶车辆的右前轮坐标(xright,yright)为：
Figure FDA0003619972070000033
获取与自动驾驶车辆的右前轮平行位置的道沿坐标；
根据所述与自动驾驶车辆的右前轮平行位置的道沿坐标及所述自动驾驶车辆的右前轮坐标，判断自动驾驶车辆的右前轮与道沿是否发生碰撞，即得到自动驾驶车辆的两个前轮与所述自动驾驶车辆的可行驶区域的边界的碰撞检测结果。
7.根据权利要求4所述的一种实时避障路径规划方法，其特征在于，所述预设的连接要求具体如下：
随机树A的新节点xA到随机树B的新节点xB的距离小于最大生长步长Lmax；
随机树A的新节点xA的新节点xA的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树B的新节点xB到新节点xB的父节点的角度与随机树A的新节点xA到随机树B的新节点xB的角度差的绝对值小于最大允许转弯角θmax；
随机树A的新节点xA到随机树B的新节点xB的路径上，自动驾驶车辆的左前轮和右前轮与所述自动驾驶车辆的可行驶区域的边界均无发生碰撞行为。
8.一种实时避障路径规划系统，其特征在于，包括：
可行域模块，用于根据规划起点和规划终点，确定自动驾驶车辆的可行驶区域；
随机树模块，用于以规划起点初始化随机树A，以规划终点初始化随机树B；
搜索模块，用于在所述自动驾驶车辆的可行驶区域内，利用预设的双向RRT算法，分别对所述随机树A及所述随机树B进行搜索，直至随机树A的新节点xA与随机树B的新节点xB满足预设连接要求，即随机树A与随机树B相遇；
路径模块，用于将随机树A的新节点xA与随机树B的新节点xB连接，生成一条从规划起点到规划终点的路径，即得到所述实时避障路径规划结果。
9.一种实时避障路径规划的车辆，其特征在于，包括：
存储器，用于存储计算机程序；
处理器，用于执行所述计算机程序时实现如权利要求1-7任一项所述的实时避障路径规划方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7任一项所述的实时避障路径规划方法的步骤。
CN202210454092.5A 2022-04-27 2022-04-27 实时避障路径规划方法、系统、设备及介质 Active CN114764249B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210454092.5A CN114764249B (zh) 2022-04-27 2022-04-27 实时避障路径规划方法、系统、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210454092.5A CN114764249B (zh) 2022-04-27 2022-04-27 实时避障路径规划方法、系统、设备及介质

Publications (2)

Publication Number Publication Date
CN114764249A true CN114764249A (zh) 2022-07-19
CN114764249B CN114764249B (zh) 2024-09-10

Family

ID=82364721

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210454092.5A Active CN114764249B (zh) 2022-04-27 2022-04-27 实时避障路径规划方法、系统、设备及介质

Country Status (1)

Country Link
CN (1) CN114764249B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115540892A (zh) * 2022-11-28 2022-12-30 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 一种固定线路车辆绕障终点选择方法及系统
CN115855095A (zh) * 2022-12-01 2023-03-28 酷黑科技（北京）有限公司 一种自主导航方法、装置及电子设备
CN117387633A (zh) * 2023-12-13 2024-01-12 山东常林智能装备科技有限公司 一种无人驾驶挖掘机的路径规划方法、电子设备及介质
CN118603104A (zh) * 2024-08-08 2024-09-06 中国空气动力研究与发展中心计算空气动力研究所 一种山谷地形低空高效飞行的航路优化方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110035051A1 (en) * 2009-08-10 2011-02-10 Samsung Electronics Co., Ltd Path planning apparatus and method for robot
CN112923944A (zh) * 2021-01-29 2021-06-08 的卢技术有限公司 一种自动驾驶路径规划方法、系统及计算机可读存储介质
CN113204238A (zh) * 2021-04-22 2021-08-03 武汉理工大学 一种移动机器人的路径规划方法、设备及存储介质

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110035051A1 (en) * 2009-08-10 2011-02-10 Samsung Electronics Co., Ltd Path planning apparatus and method for robot
CN112923944A (zh) * 2021-01-29 2021-06-08 的卢技术有限公司 一种自动驾驶路径规划方法、系统及计算机可读存储介质
CN113204238A (zh) * 2021-04-22 2021-08-03 武汉理工大学 一种移动机器人的路径规划方法、设备及存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
荆泽成 等: "基于改进RRT的核退役机器人避障方法", 《现代制造工程》, 28 February 2022 (2022-02-28), pages 46 - 51 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115540892A (zh) * 2022-11-28 2022-12-30 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 一种固定线路车辆绕障终点选择方法及系统
CN115540892B (zh) * 2022-11-28 2023-03-24 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 一种固定线路车辆绕障终点选择方法及系统
CN115855095A (zh) * 2022-12-01 2023-03-28 酷黑科技（北京）有限公司 一种自主导航方法、装置及电子设备
CN117387633A (zh) * 2023-12-13 2024-01-12 山东常林智能装备科技有限公司 一种无人驾驶挖掘机的路径规划方法、电子设备及介质
CN117387633B (zh) * 2023-12-13 2024-05-14 山东常林智能装备科技有限公司 一种无人驾驶挖掘机的路径规划方法、电子设备及介质
CN118603104A (zh) * 2024-08-08 2024-09-06 中国空气动力研究与发展中心计算空气动力研究所 一种山谷地形低空高效飞行的航路优化方法
CN118603104B (zh) * 2024-08-08 2024-10-01 中国空气动力研究与发展中心计算空气动力研究所 一种山谷地形低空高效飞行的航路优化方法

Also Published As

Publication number Publication date
CN114764249B (zh) 2024-09-10

Similar Documents

Publication Publication Date Title
CN114764249B (zh) 实时避障路径规划方法、系统、设备及介质
CN112595337B (zh) 避障路径规划方法、装置、电子装置、车辆及存储介质
CN113353103A (zh) 一种弯道车速控制方法、装置、设备及介质
CN113085850B (zh) 车辆避障方法、装置、电子设备及存储介质
CN110196592A (zh) 一种轨迹线的更新方法、系统、终端和存储介质
CN112731334B (zh) 一种激光定位车辆的方法及装置
CN112379697B (zh) 轨迹规划方法、装置、轨迹规划器、无人机及存储介质
CN111267857B (zh) 一种自动驾驶车辆的变道轨迹生成方法及装置
CN113252027B (zh) 井下无人驾驶车辆局部路径规划方法、装置、设备及存储介质
CN114593743A (zh) 一种基于改进双向rrt算法的路径规划方法及装置
CN113515111B (zh) 一种车辆避障路径规划方法及装置
CN115326051A (zh) 一种基于动态场景的定位方法、装置、机器人及介质
CN113050660B (zh) 误差补偿方法、装置、计算机设备及存储介质
CN112197783A (zh) 一种考虑车头指向的两阶段多抽样的rrt路径规划方法
CN116125980A (zh) 无人货车驾驶方法、装置、电子设备及存储介质
CN115097826A (zh) 一种车辆掉头轨迹规划方法及装置
CN113432618A (zh) 轨迹生成方法、装置、计算设备及计算机可读存储介质
CN114489060A (zh) 无人船编队控制方法、终端设备及计算机可读存储介质
CN114511590A (zh) 基于单目视觉3d车辆检测与跟踪的路口多引导线构建方法
CN115837921B (zh) 车辆轨迹碰撞检测方法、装置、设备及存储介质
CN116382308B (zh) 智能化移动机械自主寻径与避障方法、装置、设备及介质
CN116817907A (zh) 路径生成方法、装置、服务器及存储介质
CN114296452B (zh) 自动泊车路径规划方法、设备及计算机可读存储介质
CN114475603B (zh) 自动倒车方法、系统、设备及计算机可读存储介质
CN118331272A (zh) 一种基于引导采样的轨迹规划方法、系统、终端及介质

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