CN114370874B - 一种车辆、车辆路径规划方法及装置 - Google Patents
一种车辆、车辆路径规划方法及装置 Download PDFInfo
- Publication number
- CN114370874B CN114370874B CN202011103841.7A CN202011103841A CN114370874B CN 114370874 B CN114370874 B CN 114370874B CN 202011103841 A CN202011103841 A CN 202011103841A CN 114370874 B CN114370874 B CN 114370874B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- path
- target
- point
- starting point
- 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.)
- Active
Links
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/20—Instruments for performing navigational calculations
-
- 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/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提供了一种车辆、车辆路径规划方法及装置，属于车辆路径规划领域。该方法包括：若车辆偏离参考路径，当全局路径中仅包含一条路径时，将参考路径作为目标路径；当全局路径中包含多条路径时，若车辆在所有路径外侧，将距离车辆最近的路径作为目标路径；若车辆在两条路径中间，根据这两条路径到车辆的距离与设定的最小距离阈值之间的关系以及车头朝向确定目标路径；根据目标路径上与车辆距离最近的点的坐标和航向角、起始点的坐标和航向角以及车头朝向确定目标点的坐标和航向角；规划出从起始点到目标点的最短路径，使车辆沿该最短路径行驶到目标路径。本发明进行车辆偏离参考路径时的二次规划时能精确确定目标点的坐标和航向角。
Description
技术领域
本发明涉及一种车辆、车辆路径规划方法及装置，属于车辆路径规划技术领域。
背景技术
自动驾驶车辆是一种集环境感知、轨迹预测、地图定位、行为决策、路径规划、反馈控制等多个模块于一体的智能车辆系统，它集中运用了信息感知、信号处理、人工智能、车辆工程、自动控制等多个相关技术，在科研、工业、军事等领域具有广阔的发展空间和应用前景。
路径规划是实现自动驾驶车辆自主导航与控制的关键环节，根据行为决策给出的驾驶任务和环境感知给出的实时环境信息，按照一定的搜索算法为自动驾驶车辆提供最优可行驶路径，从而实现车辆的自主导航。基于环境信息获取的完整程度，目前的路径规划算法的研究主要分为两大类，一种是基于完整环境信息的全局路径规划，包括可视图法、栅格法、模拟退火算法等，一种是基于传感器实时获取环境信息的局部路径规划，包括遗传算法、人工势场法、蚁群算法、粒子群算法等，在实际运用中根据不同的需求可以选用不同的路径规划算法。
对于全局路径规划来讲，当全局路径规划完成后，一般将规划好的全局路径作为参考路径并控制车辆沿参考路径行驶，但是在控制过程中由于一些因素的影响车辆并不会完全沿着参考路径行驶，即车辆会出现偏离参考路径的情况，这时候就需要进行二次路径规划使车辆重新回到参考路径行驶，而二次路径规划如何实现是需要解决的问题。
发明内容
本发明的目的在于提供一种车辆、车辆路径规划方法及装置，用以解决车辆偏离参考路径时的二次规划问题。
为实现上述目的，本发明提供了一种车辆路径规划方法，该方法包括以下步骤：
(1)获取已规划好的全局路径，所述全局路径中包含至少一条路径；当所述全局路径中仅包含一条路径时，将该路径作为车辆行驶的参考路径；当所述全局路径中包含多条路径时，从中选择一条作为车辆行驶的参考路径，其余路径作为车辆行驶的备用路径；
(2)车辆沿所述参考路径行驶的过程中实时获取车辆的当前坐标和当前航向角，并根据车辆的当前坐标判断车辆是否偏离所述参考路径；
(3)若车辆偏离所述参考路径，则通过以下步骤确定车辆行驶的目标路径；
当所述全局路径中仅包含一条路径时，将所述参考路径作为目标路径；
当所述全局路径中包含多条路径时，若车辆在所有路径外侧，则将距离车辆最近的路径作为目标路径；若车辆在两条路径中间，则根据这两条路径到车辆的距离与设定的最小距离阈值之间的关系以及车头朝向确定目标路径；
(4)确定所述目标路径上与车辆距离最近的点，并获取该点的坐标和航向角，根据该点的坐标和航向角、起始点的坐标和航向角以及车头朝向，确定目标点的坐标和航向角；其中，将车辆的当前坐标和当前航向角作为起始点的坐标和航向角；
(5)规划出从起始点到目标点的最短路径，使车辆沿该最短路径行驶到所述目标路径。
本发明还提供了一种车辆路径规划装置，该装置包括处理器和存储器，所述处理器执行由所述存储器存储的计算机程序，以实现上述的车辆路径规划方法。
本发明还提供了一种车辆，包括车辆本体和车辆路径规划装置，该车辆路径规划装置包括处理器和存储器，所述处理器执行由所述存储器存储的计算机程序，以实现上述的车辆路径规划方法。
本发明的有益效果是：当车辆偏离参考路径时，结合已规划好的全局路径所包含的路径条数分情况确定目标路径，然后通过确定目标路径上与车辆距离最近的点，根据该点的坐标和航向角、起始点的坐标和航向角(即车辆的当前坐标和当前航向角)以及车头朝向，确定目标点的坐标和航向角；最后，规划出从起始点到目标点的最短路径，使车辆沿该最短路径行驶到目标路径，完成车辆偏离参考路径时的二次路径规划。本发明能够精确确定目标点的坐标和航向角，保证最终确定的目标点能够使车辆行驶状态更符合实际运行要求。
为了确定车辆在两条路径中间时的目标路径，进一步地，在上述车辆、车辆路径规划方法及装置中，当车辆在两条路径中间时目标路径的确定过程包括：分别计算这两条路径到车辆的距离，并将这两条路径到车辆的距离分别与设定的最小距离阈值进行比较，若这两条路径中有一个路径到车辆的距离小于设定的最小距离阈值，则将该路径作为目标路径；若这两条路径到车辆的距离均大于设定的最小距离阈值，则将车头朝向的路径作为目标路径。
为了精确确定目标点的坐标和航向角，进一步地，在上述车辆、车辆路径规划方法及装置中，所述目标点的坐标和航向角的确定过程包括：
根据目标路径上与车辆距离最近的点的坐标和起始点的坐标，计算这两点之间的距离，将计算得到的距离作为目标点距起始点的横向距离；
根据目标路径上与车辆距离最近的点的航向角和起始点的航向角以及车头朝向，确定目标点与起始点的相对角度；其中，当所述全局路径中仅包含一条路径时，若车头朝向目标路径，则将|α-α＇|作为目标点与起始点的相对角度，若车头背离目标路径，则将-|α-α＇|作为目标点与起始点的相对角度；所述α为起始点的航向角，所述α＇为目标路径上与车辆距离最近的点的航向角；当所述全局路径中包含多条路径时，若车辆在所有路径外侧，则当车头朝向目标路径时，将|α-α＇|作为目标点与起始点的相对角度，当车头背离目标路径时，将-|α-α＇|作为目标点与起始点的相对角度；若车辆在两条路径中间，则将|α-α＇|作为目标点与起始点的相对角度；
结合所述目标点距起始点的横向距离、所述目标点与起始点的相对角度、以及预先确定的目标点距起始点的横向距离、目标点与起始点的相对角度以及目标点距起始点的纵向距离之间的对应关系，确定目标点距起始点的纵向距离；
根据起始点的坐标以及所述目标点距起始点的横向距离、纵向距离，得到目标点的坐标；
根据起始点的航向角以及所述目标点与起始点的相对角度，得到目标点的航向角。
进一步地，在上述车辆、车辆路径规划方法及装置中，所述对应关系通过试验标定，标定过程如下：
给定一个起始点的坐标和航向角，改变目标点距起始点的横向距离和目标点与起始点的相对角度，基于车辆最小转弯半径采用Dubins算法，确定多组目标点距起始点的横向距离和目标点与起始点的相对角度对应的目标点距起始点的最佳纵向距离，进而得到所述对应关系；其中，针对每组目标点距起始点的横向距离和目标点与起始点的相对角度，通过给定不同的目标点距起始点的纵向距离得到多条Dubins曲线，从中选择曲线长度最短、最平滑且保持车辆前进方向不变的Dubins曲线作为该组目标点距起始点的横向距离和目标点与起始点的相对角度对应的最佳曲线，并将该最佳曲线对应的目标点距起始点的纵向距离作为该组目标点距起始点的横向距离和目标点与起始点的相对角度对应的目标点距起始点的最佳纵向距离。
进一步地，在上述车辆、车辆路径规划方法及装置中，采用Dubins算法规划出从起始点到目标点的最短路径。
进一步地，在上述车辆、车辆路径规划方法及装置中，所述Dubins算法中的曲率半径大于或等于车辆最小转弯半径。
这样做的有益效果是：依据车辆最小转弯半径来确定Dubins算法的曲率半径，既保证了车辆动力学对半径和曲率的要求，又能保证规划出的路径最短；
进一步地，在上述车辆、车辆路径规划方法及装置中，所述Dubins曲线仅包含LSL、RSR、RSL、LSR四种类型。
这样做的有益效果是：考虑到车辆实际行驶中不宜出现多次大角度转弯的情况，只考虑中间路径是直线的四种Dubins曲线，即只考虑LSL、RSR、RSL、LSR四种类型的Dubins曲线，更符合车辆的实际运行工况。
进一步地，在上述车辆、车辆路径规划方法及装置中，所述步骤(2)中，当车辆到参考路径的距离大于设定的最大距离阈值时，判断车辆偏离参考路径；通过确定参考路径上与车辆距离最近的点的坐标，根据该点的坐标和车辆的当前坐标计算得到车辆到参考路径的距离。
附图说明
图1是本发明方法实施例1中的车辆路径规划方法流程图；
图2-1是本发明方法实施例1中利用Dubins算法确定目标点最佳纵向距离的示意图；
图2-2是本发明方法实施例1中Δx＝1、θ＝0°时，Δy＝2m对应的最佳曲线示意图；
图2-3是本发明方法实施例1中Δx＝1、θ＝0°时，Δy＝2.5m对应的最佳曲线示意图；
图2-4是本发明方法实施例1中Δx＝1、θ＝0°时，Δy＝3m对应的最佳曲线示意图；
图2-5是本发明方法实施例1中Δx＝1、θ＝60°时，Δy＝1m对应的最佳曲线示意图；
图2-6是本发明方法实施例1中Δx＝1、θ＝60°时，Δy＝1.5m对应的最佳曲线示意图；
图2-7是本发明方法实施例1中Δx＝1、θ＝60°时，Δy＝2m对应的最佳曲线示意图；
图3-1是本发明方法实施例1中单车道场景下第1种工况对应的路径规划示意图；
图3-2是本发明方法实施例1中单车道场景下第2种工况对应的路径规划示意图；
图3-3是本发明方法实施例1中单车道场景下第3种工况对应的路径规划示意图；
图3-4是本发明方法实施例1中单车道场景下第4种工况对应的路径规划示意图；
图4是本发明方法实施例2中的车辆路径规划方法流程图；
图5-1是本发明方法实施例2中双车道场景下第1种工况对应的路径规划示意图；
图5-2是本发明方法实施例2中双车道场景下第2种工况对应的路径规划示意图；
图5-3是本发明方法实施例2中双车道场景下第3种工况对应的路径规划示意图；
图5-4是本发明方法实施例2中双车道场景下第4种工况对应的路径规划示意图；
图5-5是本发明方法实施例2中双车道场景下第5种工况对应的路径规划示意图；
图5-6是本发明方法实施例2中双车道场景下第6种工况对应的路径规划示意图；
图6是本发明装置实施例中的车辆路径规划装置结构示意图。
具体实施方式
本发明提供了一种基于Dubins算法的局部路径规划方法，用于解决车辆偏离参考路径时的二次规划问题。
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。
方法实施例1：
本实施例针对已规划好的全局路径中仅包含一条路径的情况。
如图1所示，本实施例的车辆路径规划方法包括以下步骤：
(1)获取已规划好的全局路径，该全局路径中仅包含一条路径，将该路径作为车辆行驶的参考路径；
其中，已规划好的全局路径可以是直接利用导航定位系统获取的，或者是通过制作环境高精地图，采用A*(A-Star算法)、Dijkstra(狄克斯特拉算法)等算法计算得到的，全局路径上各路径点的坐标、航向角等信息以离散点的形式储存。
(2)车辆沿参考路径行驶的过程中实时获取车辆的当前坐标和当前航向角，并根据车辆的当前坐标判断车辆是否偏离参考路径；
其中，当车辆到参考路径的距离大于设定的最大距离阈值(该阈值根据实际需要设置)时，判断车辆偏离参考路径。其中，通过找到参考路径上与车辆距离最近的点，利用该点的坐标和车辆的当前坐标计算得到车辆到参考路径的距离；车辆的当前坐标和当前航向角可以利用由各种传感器构成的环境感知与定位系统(例如包括视觉传感器、激光雷达传感器、超声波传感器、陀螺仪等)获取。
(3)若车辆偏离参考路径，则将参考路径作为目标路径，并将车辆的当前坐标和当前航向角作为起始点的坐标pi(xi,yi)和起始点的航向角α；
(4)找到参考路径上与车辆距离最近的点A，获取点A的坐标A(xa,ya)和航向角α＇，利用起始点的坐标和点A的坐标计算起始点与点A之间的距离disA，并将disA作为目标点距起始点的横向距离Δx；当车头朝向目标路径时，将|α-α＇|作为目标点与起始点的相对角度θ，当车头背离目标路径时，将-|α-α＇|作为目标点与起始点的相对角度θ；然后，结合确定的Δx和θ，以及预先确定的目标点距起始点的横向距离、目标点与起始点的相对角度以及目标点距起始点的纵向距离之间的对应关系，确定目标点距起始点的纵向距离Δy，得到目标点的坐标pj(xj,yj)＝(xi+Δx,yi+Δy)和目标点的航向角β＝α+θ；
(5)采用Dubins算法规划出从起始点到目标点的最短路径，使车辆沿该最短路径行驶到到目标路径。
本实施例中，预先确定的目标点距起始点的横向距离(以下用Δx表示)、目标点与起始点的相对角度(以下用θ表示)以及目标点距起始点的纵向距离(以下用Δy表示)之间的对应关系，以下简称Δx、θ和Δy之间的对应关系，可以用表格、数组、链表等方式呈现。
下面详细介绍Δx、θ和Δy之间的对应关系的确定方法：
本实施例中，Δx、θ和Δy之间的对应关系基于车辆最小转弯半径采用Dubins算法确定，利用Dubins算法能够规划出从起始点到目标点的6种Dubins曲线，每种Dubins曲线均同时满足曲率约束(即曲率半径约束，曲率半径根据车辆最小转弯半径确定)、车辆前进方向和目标点与起始点的相对角度。其中，Dubins曲线由三段路径组成，包括LSL、RSR、RSL、LSR、RLR、LRL六种形式，L表示沿逆时针方向圆弧运动，S表示直线运动，R表示沿顺时针方向圆弧运动，由于车辆实际行驶中不宜出现多次大角度转弯的情况，本实施例只考虑中间路径是直线的四种Dubins曲线，即只考虑LSL、RSR、RSL、LSR四种类型的Dubins曲线，这四种Dubins曲线的曲线长度计算公式如下：
式中，Llsl、Lrsr、Lrsl、Llsr分别为LSL、RSR、RSL、LSR四种类型的Dubins曲线的曲线长度，(xi,yi,α)为起始点的坐标及航向角、(xj,yj,β)为目标点的坐标及航向角，d为单位化后的距离，r为根据车辆最小转弯半径确定的曲率半径，可取为等于或略大于车辆最小转弯半径的值。
由上述四种类型的Dubins曲线的曲线长度计算公式可知，Dubins曲线的曲线长度由目标点距起始点的横向距离Δx、目标点与起始点的相对角度θ＝±|α-β|以及目标点距起始点的纵向距离Δy确定，本实施例中通过给定一个起始点的坐标和航向角，改变目标点距起始点的横向距离和目标点与起始点的相对角度，基于车辆最小转弯半径采用Dubins算法，确定多组目标点距起始点的横向距离和目标点与起始点的相对角度对应的目标点距起始点的最佳纵向距离，进而得到Δx、θ和Δy之间的对应关系；具体地，当给定一组Δx和θ时，通过给出一个Δy值能够得到四种Dubins曲线，从中选择曲线长度最短的作为该Δy值对应的最佳曲线，通过改变Δy的值能够获得不同的Δy值对应的最佳曲线，通过比较这多个Δy值对应的最佳曲线，从中选择曲线长度最短、最平滑且保持车辆前进方向不变的曲线作为该组Δx和θ对应的最佳曲线，并将这条曲线对应的Δy值作为该组Δx和θ对应的目标点最佳纵向距离(即目标点距起始点的最佳纵向距离)，从而通过给定多组Δx和θ，得到每组Δx和θ对应的目标点最佳纵向距离，最终得到Δx、θ和Δy之间的对应关系。其中，车辆沿某曲线行驶时转弯幅度越小，说明曲线越平滑。
如图2-1所示，起始点pi的坐标和航向角固定，目标点pj距起始点pi的横向距离固定，且目标点pj的航向角固定，相当于Δx和θ固定，通过给出不同的Δy值得到三个不同的目标点pj1、pj2、pj3，其中，目标点pj1对应的最佳曲线为曲线1，目标点pj2对应的最佳曲线为曲线2，目标点pj3对应的最佳曲线为曲线3，这3条曲线中曲线长度最短、最平滑且保持车辆前进方向不变的曲线为曲线1，因此选择曲线1作为该组Δx和θ对应的最佳曲线，曲线1对应的Δy值作为该组Δx和θ对应的目标点最佳纵向距离。
也就是说，需要通过大量试验数据标定Δx、θ和Δy之间的对应关系，下面给出一种标定过程中各参数的取值方式：(1)取Dubins算法中的曲率半径为车辆最小转弯半径；(2)按照10°间隔取θ，θ的取值范围为[-90～90]；(3)按照0.5m间隔取Δx，Δx的取值范围为[0,5]；通过大量试验获得多组Δx和θ对应的目标点最佳纵向距离Δy，最终得到Δx、θ和Δy之间的对应关系，从而能将其作为后续步骤查找目标点最佳纵向距离的依据。作为其他实施方式，标定过程中各参数(即Dubins算法中的曲率半径、θ的取值范围和取值间隔、Δx的取值范围和取值间隔)的取值方式均可以根据实际需要调整。
下面分别以Δx＝1、θ＝0°和Δx＝1、θ＝60°为例说明目标点最佳纵向距离Δy的选取方式：
图2-2、图2-3、图2-4分别为Δx＝1、θ＝0°时，Δy＝2m、2.5m、3m对应的最佳曲线，可以看出这3条曲线中曲线长度最短、最平滑且能够保持车辆前进方向的曲线为Δy＝3m对应的最佳曲线，则将Δy＝3m对应的最佳曲线作为Δx＝1、θ＝0°对应的最佳曲线，因此，Δx＝1、θ＝0°时对应的目标点最佳纵向距离Δy＝3m；
图2-5、图2-6、图2-7分别为Δx＝1、θ＝60°时，Δy＝1m、1.5m、2m时对应的最佳曲线，可以看出这3条曲线中曲线长度最短、最平滑且能够保持车辆前进方向的曲线为Δy＝2m对应的最佳曲线，则将Δy＝2m对应的最佳曲线作为Δx＝1、θ＝60°对应的最佳曲线，因此，Δx＝1、θ＝60°时对应的目标点最佳纵向距离Δy＝2m。
综上所述，本实施例基于车辆最小转弯半径采用Dubins算法确定了Δx、θ和Δy之间的对应关系，利用该对应关系能够获得不同车辆位姿下的目标点最佳纵向距离。
下面详细介绍目标点坐标和目标点航向角的确定方法：
本实施例中，已规划好的全局路径中仅包含一条路径，且该路径为车辆行驶的参考路径；在实际应用中，一般车辆行驶在单车道场景下时仅规划一条全局路径，因此下面以车辆行驶在单车道场景下为例，详细介绍已规划好的全局路径中仅包含一条路径时，目标点坐标和目标点航向角的确定方法。
图3-1至图3-4对应单车道场景下目标点确定的4种工况，图中pi代表起始点，起始点pi的坐标为(xi,yi)，起始点的航向角为α，pj代表目标点，目标点pj的坐标为(xj,yj)，目标点pj的航向角为β，pj所在的黑直线代表目标路径，pi和pj之间的黑色实曲线代表利用Dubins算法规划出的从起始点到目标点的最短路径，图中点A表示目标路径上与车辆距离最近的点，点A的坐标为A(xa,ya)，点A的航向角为α＇，disA表示起始点与点A之间的距离，起始点pi、目标点pj、点A的坐标均为车辆坐标系下的坐标，其中，起始点pi的坐标和航向角为车辆的当前坐标和航向角。
图3-1为车辆在参考路径左侧，且车头朝向参考路径的工况，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝|α-α＇|；
图3-2为车辆在参考路径右侧，且车头朝向参考路径的工况，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝|α-α＇|；
图3-3为车辆在参考路径左侧，且车头背离参考路径的工况，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝-|α-α＇|；
图3-4为车辆在参考路径右侧，且车头背离参考路径的工况，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝-|α-α＇|；
其中，在车辆坐标系下，利用点A的横坐标xa的正负确定车头朝向，当xa>0时，车头朝向参考路径；当xa<0时，车头背离参考路径。
确定好Δx和θ后，结合预先确定的Δx、θ和Δy之间的对应关系，确定目标点距起始点的纵向距离Δy，得到目标点坐标pj(xj,yj)＝(xi+Δx,yi+Δy)和目标点航向角β＝α+θ。
本实施例中用直线表示目标路径，当然目标路径也可以用曲线表示，但是计算目标点坐标和目标点航向角所需要的只是目标路径上距离车辆最近的点的坐标和航向角，因此目标路径用直线或曲线表示并不会对本实施例方法产生影响。
本实施例的车辆路径规划方法的关键在于目标点的确定方法，即根据目标路径上距车辆最近的点的坐标和航向角、起始点的坐标和航向角以及车头朝向，确定目标点的坐标和航向角；作为其他实施方式，起始点到目标点的最短路径还可以利用现有的其他路径规划方法进行规划，不局限于Dubins算法。
方法实施例2：
本实施例针对已规划好的全局路径中包含多条路径的情况。
如图4所示，本实施例的车辆路径规划方法包括以下步骤：
(1)获取已规划好的全局路径，该全局路径中包含多条路径，从中选择一条作为车辆行驶的参考路径，其余路径作为车辆行驶的备用路径；
其中，已规划好的全局路径可以是直接利用导航定位系统获取的，或者是通过制作环境高精地图，采用A*(A-Star算法)、Dijkstra(狄克斯特拉算法)等算法计算得到的，全局路径上各路径点的坐标、航向角等信息以离散点的形式储存。
(2)车辆沿参考路径行驶的过程中实时获取车辆的当前坐标和当前航向角，并根据车辆的当前坐标判断车辆是否偏离参考路径；车辆是否偏离参考路径的判断方法与方法实施例1相同，此处不再赘述；
(3)若车辆偏离参考路径，则将车辆的当前坐标和当前航向角作为起始点的坐标pi(xi,yi)和起始点的航向角α；
(4)当车辆在全局路径所包含的所有路径外侧时，将距离车辆最近的路径作为目标路径；确定目标路径上与车辆距离最近的点P，获取点P的坐标P(xp,yp)和航向角α＇，将起始点与点P的距离disP作为目标点距起始点的横向距离Δx；当车头朝向目标路径时，将|α-α＇|作为目标点与起始点的相对角度θ，当车头背离目标路径时，将-|α-α＇|作为目标点与起始点的相对角度θ；
若车辆在两条路径中间，则根据这两条路径到车辆的距离与设定的最小距离阈值之间的关系以及车头朝向确定目标路径；具体地，车辆在两条路径中间时，将这两条路径中与车辆的距离小于设定的最小距离阈值disMin(该阈值根据实际需要设置)的路径作为目标路径，若这两条路径与车辆的距离均大于设定的最小距离阈值disMin，则将车头朝向的路径作为目标路径；确定目标路径上与车辆距离最近的点P，并获取点P的坐标P(xp,yp)和航向角α＇，将起始点与点P的距离disP作为目标点距起始点的横向距离Δx；将|α-α＇|作为目标点与起始点的相对角度θ；
(5)结合确定的Δx和θ，以及预先确定的目标点距起始点的横向距离、目标点与起始点的相对角度以及目标点距起始点的纵向距离之间的对应关系，确定目标点距起始点的纵向距离Δy；得到目标点坐标pj(xj,yj)＝(xi+Δx,yi+Δy)和目标点航向角β＝α+θ；
(6)采用Dubins算法规划出从起始点到目标点的最短路径，使车辆沿该最短路径行驶到目标路径。
其中，预先确定的目标点距起始点的横向距离、目标点与起始点的相对角度以及目标点距起始点的纵向距离之间的对应关系(即Δx、θ和Δy之间的对应关系)的确定方法与方法实施例1相同，此处不再赘述。
下面详细介绍目标点坐标和目标点航向角的确定方法：
本实施例中，已规划好的全局路径中包含多条路径，从中选择一条作为车辆行驶的参考路径，其余路径作为车辆行驶的备用路径；在实际应用中，一般车辆行驶在多车道场景下时需要规划多条全局路径，全局路径的个数与车道数相同，因此下面以车辆行驶在双车道场景下为例，详细介绍已规划好的全局路径中包含2条路径时，目标点坐标和目标点航向角的确定方法，全局路径中包含3条以上路径时目标点坐标和目标点航向角的确定方法与此类似，不再赘述。
图5-1至图5-6对应双车道场景下目标点确定的6种工况，图中的两条黑直线代表全局路径中包含的2条路径，图中pi代表起始点，起始点pi的坐标为(xi,yi)，起始点的航向角为α，pj代表目标点，目标点pj的坐标为(xj,yj)，目标点pj的航向角为β，pj所在的黑直线代表目标路径，pi和pj之间的黑色实曲线代表利用Dubins算法规划出的从起始点到目标点的最短路径，图中点A表示其所在路径上距车辆距离最近的点，点A的坐标为A(xa,ya)，点A的航向角为αA＇，disA表示起始点与点A之间的距离，图中点B表示其所在路径上距车辆距离最近的点，点B的坐标为B(xb,yb)，点B的航向角为αB＇，disB表示起始点与点B之间的距离，起始点pi、目标点pj、点A、点B的坐标均为车辆坐标系下的坐标，其中，起始点pi的坐标和航向角为车辆的当前坐标和航向角。
图5-1为车辆在两条路径外侧(左侧)，且两条路径中距车辆最近的路径是点A所在的路径，即将点A所在的路径作为目标路径，且车头朝向目标路径的工况，此时，目标路径上与车辆距离最近的点为点A，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝|α-αA＇|；
图5-2为车辆在两条路径外侧(右侧)，且两条路径中距车辆最近的路径是点B所在的路径，即将点B所在的路径作为目标路径，且车头朝向目标路径的工况，此时，目标路径上与车辆距离最近的点为点B，目标点距起始点的横向距离Δx＝disB，目标点与起始点的相对角度θ＝|α-αB＇|；
图5-3为车辆在两条路径外侧(左侧)，且两条路径中距车辆最近的路径是点A所在的路径，即将点A所在的路径作为目标路径，且车头背离目标路径的工况，此时，目标路径上与车辆距离最近的点为点A，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝-|α-αA＇|；
图5-4为车辆在两条路径外侧(右侧)，且两条路径中距车辆最近的路径是点B所在的路径，即将点B所在的路径作为目标路径，且车头背离目标路径的工况，此时，目标路径上与车辆距离最近的点为点B，目标点距起始点的横向距离Δx＝disB，目标点与起始点的相对角度θ＝-|α-αB＇|；
图5-5为车辆在两条路径中间，且这两条路径与车辆的距离均大于设定的最小距离阈值disMin(即disA>＝disMin,disB>＝disMin)，但车头朝向的路径为点A所在的路径的工况，即将点A所在的路径作为目标路径，此时，目标路径上与车辆距离最近的点为点A；此时，目标点距起始点的横向距离Δx＝disA，目标点与起始点的相对角度θ＝|α-αA＇|；
图5-6为车辆在两条路径中间，且这两条路径与车辆的距离均大于设定的最小距离阈值disMin(即disA>＝disMin,disB>＝disMin)，但车头朝向的路径为点B所在的路径的工况，即将点B所在的路径作为目标路径，此时，目标路径上与车辆距离最近的点为点B；此时，目标点距起始点的横向距离Δx＝disB，目标点与起始点的相对角度θ＝|α-αB＇|；
其中，在车辆坐标系下，利用点A的横坐标xa或者点B的横坐标xb的正负确定车头朝向，当xa>0时，车头朝向点A所在的路径；当xa<0时，车头背离点A所在的路径；当xb>0时，车头朝向点B所在的路径；当xb<0时，车头背离点B所在的路径。
确定好Δx和θ后，结合预先确定的Δx、θ和Δy之间的对应关系，确定目标点距起始点的纵向距离Δy，得到目标点坐标pj(xj,yj)＝(xi+Δx,yi+Δy)和目标点航向角β＝α+θ。
本实施例中用直线表示目标路径，当然目标路径也可以用曲线表示，但是计算目标点坐标和目标点航向角所需要的只是目标路径上距车辆距离最近的点的坐标和航向角，因此目标路径用直线或曲线表示并不会对本实施例方法产生影响。
本实施例的车辆路径规划方法的关键在于目标路径和目标点的确定方法，即根据车辆与全局路径中所包含的所有路径的位置关系(即车辆在所有路径外侧还是在两条路径中间)和车头朝向确定目标路径，根据目标路径上距车辆最近的点的坐标和航向角、起始点的坐标和航向角以及车头朝向，确定目标点的坐标和航向角；作为其他实施方式，起始点到目标点的最短路径还可以利用现有的其他路径规划方法进行规划，不局限于Dubins算法。
装置实施例：
本实施例的车辆路径规划装置，如图6所示，该装置包括处理器、存储器，存储器中存储有可在处理器上运行的计算机程序，所述处理器在执行所述计算机程序时实现上述方法实施例中的方法。
也就是说，以上方法实施例中的方法应理解为可由计算机程序指令实现车辆路径规划方法的流程。可提供这些计算机程序指令到处理器，使得通过处理器执行这些指令产生用于实现上述方法流程所指定的功能。
本实施例所指的处理器是指微处理器MCU或可编程逻辑器件FPGA等的处理装置。
本实施例所指的存储器包括用于存储信息的物理装置，通常是将信息数字化后再以利用电、磁或者光学等方式的媒体加以存储。例如：利用电能方式存储信息的各式存储器，RAM、ROM等；利用磁能方式存储信息的的各式存储器，硬盘、软盘、磁带、磁芯存储器、磁泡存储器、U盘；利用光学方式存储信息的各式存储器，CD或DVD。当然，还有其他方式的存储器，例如量子存储器、石墨烯存储器等等。
通过上述存储器、处理器以及计算机程序构成的装置，在计算机中由处理器执行相应的程序指令来实现，处理器可以搭载各种操作系统，如windows操作系统、linux系统、android、iOS系统等。
车辆实施例：
本实施例的车辆包括车辆本体和车辆路径规划装置，该车辆路径规划装置已在装置实施例中进行了详细介绍，此处不再赘述。
Claims (9)
1.一种车辆路径规划方法，其特征在于，该方法包括以下步骤：
(1)获取已规划好的全局路径，所述全局路径中包含至少一条路径；当所述全局路径中仅包含一条路径时，将该路径作为车辆行驶的参考路径；当所述全局路径中包含多条路径时，从中选择一条作为车辆行驶的参考路径，其余路径作为车辆行驶的备用路径；
(2)车辆沿所述参考路径行驶的过程中实时获取车辆的当前坐标和当前航向角，并根据车辆的当前坐标判断车辆是否偏离所述参考路径；
(3)若车辆偏离所述参考路径，则通过以下步骤确定车辆行驶的目标路径；
当所述全局路径中仅包含一条路径时，将所述参考路径作为目标路径；
当所述全局路径中包含多条路径时，若车辆在所有路径外侧，则将距离车辆最近的路径作为目标路径；若车辆在两条路径中间，则根据这两条路径到车辆的距离与设定的最小距离阈值之间的关系以及车头朝向确定目标路径；
(4)确定所述目标路径上与车辆距离最近的点，并获取该点的坐标和航向角，根据该点的坐标和航向角、起始点的坐标和航向角以及车头朝向，确定目标点的坐标和航向角；其中，将车辆的当前坐标和当前航向角作为起始点的坐标和航向角；
(5)规划出从起始点到目标点的最短路径，使车辆沿该最短路径行驶到所述目标路径；所述目标点的坐标和航向角的确定过程包括：
根据目标路径上与车辆距离最近的点的坐标和起始点的坐标，计算这两点之间的距离，将计算得到的距离作为目标点距起始点的横向距离；
根据目标路径上与车辆距离最近的点的航向角和起始点的航向角以及车头朝向，确定目标点与起始点的相对角度；其中，当所述全局路径中仅包含一条路径时，若车头朝向目标路径，则将|α-α＇|作为目标点与起始点的相对角度，若车头背离目标路径，则将-|α-α＇|作为目标点与起始点的相对角度；所述α为起始点的航向角，所述α＇为目标路径上与车辆距离最近的点的航向角；当所述全局路径中包含多条路径时，若车辆在所有路径外侧，则当车头朝向目标路径时，将|α-α＇|作为目标点与起始点的相对角度，当车头背离目标路径时，将-|α-α＇|作为目标点与起始点的相对角度；若车辆在两条路径中间，则将|α-α＇|作为目标点与起始点的相对角度；
结合所述目标点距起始点的横向距离、所述目标点与起始点的相对角度、以及预先确定的目标点距起始点的横向距离、目标点与起始点的相对角度以及目标点距起始点的纵向距离之间的对应关系，确定目标点距起始点的纵向距离；
根据起始点的坐标以及所述目标点距起始点的横向距离、纵向距离，得到目标点的坐标；
根据起始点的航向角以及所述目标点与起始点的相对角度，得到目标点的航向角。
2.根据权利要求1所述的车辆路径规划方法，其特征在于，当车辆在两条路径中间时目标路径的确定过程包括：分别计算这两条路径到车辆的距离，并将这两条路径到车辆的距离分别与设定的最小距离阈值进行比较，若这两条路径中有一个路径到车辆的距离小于设定的最小距离阈值，则将该路径作为目标路径；若这两条路径到车辆的距离均大于设定的最小距离阈值，则将车头朝向的路径作为目标路径。
3.根据权利要求1所述的车辆路径规划方法，其特征在于，所述对应关系通过试验标定，标定过程如下：
给定一个起始点的坐标和航向角，改变目标点距起始点的横向距离和目标点与起始点的相对角度，基于车辆最小转弯半径采用Dubins算法，确定多组目标点距起始点的横向距离和目标点与起始点的相对角度对应的目标点距起始点的最佳纵向距离，进而得到所述对应关系；其中，针对每组目标点距起始点的横向距离和目标点与起始点的相对角度，通过给定不同的目标点距起始点的纵向距离得到多条Dubins曲线，从中选择曲线长度最短、最平滑且保持车辆前进方向不变的Dubins曲线作为该组目标点距起始点的横向距离和目标点与起始点的相对角度对应的最佳曲线，并将该最佳曲线对应的目标点距起始点的纵向距离作为该组目标点距起始点的横向距离和目标点与起始点的相对角度对应的目标点距起始点的最佳纵向距离。
4.根据权利要求3所述的车辆路径规划方法，其特征在于，采用Dubins算法规划出从起始点到目标点的最短路径。
5.根据权利要求3或4所述的车辆路径规划方法，其特征在于，所述Dubins算法中的曲率半径大于或等于车辆最小转弯半径。
6.根据权利要求3或4所述的车辆路径规划方法，其特征在于，所述Dubins曲线仅包含LSL、RSR、RSL、LSR四种类型。
7.根据权利要求1所述的车辆路径规划方法，其特征在于，所述步骤(2)中，当车辆到参考路径的距离大于设定的最大距离阈值时，判断车辆偏离参考路径；通过确定参考路径上与车辆距离最近的点的坐标，根据该点的坐标和车辆的当前坐标计算得到车辆到参考路径的距离。
8.一种车辆路径规划装置，其特征在于，该装置包括处理器和存储器，所述处理器执行由所述存储器存储的计算机程序，以实现如权利要求1-7任一项所述的车辆路径规划方法。
9.一种车辆，包括车辆本体和车辆路径规划装置，其特征在于，该车辆路径规划装置包括处理器和存储器，所述处理器执行由所述存储器存储的计算机程序，以实现如权利要求1-7任一项所述的车辆路径规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011103841.7A CN114370874B (zh) | 2020-10-15 | 2020-10-15 | 一种车辆、车辆路径规划方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011103841.7A CN114370874B (zh) | 2020-10-15 | 2020-10-15 | 一种车辆、车辆路径规划方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114370874A CN114370874A (zh) | 2022-04-19 |
| CN114370874B true CN114370874B (zh) | 2023-08-25 |
Family
ID=81139033
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011103841.7A Active CN114370874B (zh) | 2020-10-15 | 2020-10-15 | 一种车辆、车辆路径规划方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114370874B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116740984A (zh) * | 2023-05-23 | 2023-09-12 | 广州汽车集团股份有限公司 | 车辆加塞的处理方法、装置、电子设备及存储介质 |
Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107664503A (zh) * | 2016-07-29 | 2018-02-06 | 上海汽车集团股份有限公司 | 车辆路径规划方法及装置 |
| CN107702716A (zh) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、系统和装置 |
| CN108088456A (zh) * | 2017-12-21 | 2018-05-29 | 北京工业大学 | 一种具有时间一致性的无人驾驶车辆局部路径规划方法 |
| CN108898866A (zh) * | 2018-07-04 | 2018-11-27 | 深圳万智联合科技有限公司 | 一种有效的智能车辆控制系统 |
| CN109017793A (zh) * | 2018-07-26 | 2018-12-18 | 中南大学 | 基于前后轴融合参考的自主招车导航及控制方法 |
| CN109976329A (zh) * | 2017-12-28 | 2019-07-05 | 郑州宇通客车股份有限公司 | 一种车辆避障换道路径的规划方法 |
| CN110083158A (zh) * | 2019-04-28 | 2019-08-02 | 深兰科技（上海）有限公司 | 一种确定局部规划路径的方法和设备 |
| CN110361013A (zh) * | 2019-07-22 | 2019-10-22 | 上海应用技术大学 | 一种用于车辆模型的路径规划系统及方法 |
| CN110609557A (zh) * | 2019-10-09 | 2019-12-24 | 中国人民解放军陆军装甲兵学院 | 无人车混合路径规划算法 |
| CN110737261A (zh) * | 2018-07-03 | 2020-01-31 | 郑州宇通客车股份有限公司 | 一种车辆的自动停靠站控制方法及系统 |
| CN111289008A (zh) * | 2020-04-28 | 2020-06-16 | 南京维思科汽车科技有限公司 | 一种用于无人驾驶车辆的局部路径规划算法 |
| CN111457931A (zh) * | 2019-01-21 | 2020-07-28 | 广州汽车集团股份有限公司 | 自动驾驶车辆的局部路径重规划控制方法、装置、系统及存储介质 |
| CN111653113A (zh) * | 2020-04-20 | 2020-09-11 | 浙江吉利汽车研究院有限公司 | 车辆的局部路径确定方法、装置、终端和存储介质 |
| CN111736581A (zh) * | 2019-03-19 | 2020-10-02 | 北京奇虎科技有限公司 | 一种智能移动设备的全局路径规划方法和装置 |
-
2020
- 2020-10-15 CN CN202011103841.7A patent/CN114370874B/zh active Active
Patent Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107664503A (zh) * | 2016-07-29 | 2018-02-06 | 上海汽车集团股份有限公司 | 车辆路径规划方法及装置 |
| CN107702716A (zh) * | 2017-08-31 | 2018-02-16 | 广州小鹏汽车科技有限公司 | 一种无人驾驶路径规划方法、系统和装置 |
| CN108088456A (zh) * | 2017-12-21 | 2018-05-29 | 北京工业大学 | 一种具有时间一致性的无人驾驶车辆局部路径规划方法 |
| CN109976329A (zh) * | 2017-12-28 | 2019-07-05 | 郑州宇通客车股份有限公司 | 一种车辆避障换道路径的规划方法 |
| CN110737261A (zh) * | 2018-07-03 | 2020-01-31 | 郑州宇通客车股份有限公司 | 一种车辆的自动停靠站控制方法及系统 |
| CN108898866A (zh) * | 2018-07-04 | 2018-11-27 | 深圳万智联合科技有限公司 | 一种有效的智能车辆控制系统 |
| CN109017793A (zh) * | 2018-07-26 | 2018-12-18 | 中南大学 | 基于前后轴融合参考的自主招车导航及控制方法 |
| CN111457931A (zh) * | 2019-01-21 | 2020-07-28 | 广州汽车集团股份有限公司 | 自动驾驶车辆的局部路径重规划控制方法、装置、系统及存储介质 |
| CN111736581A (zh) * | 2019-03-19 | 2020-10-02 | 北京奇虎科技有限公司 | 一种智能移动设备的全局路径规划方法和装置 |
| CN110083158A (zh) * | 2019-04-28 | 2019-08-02 | 深兰科技（上海）有限公司 | 一种确定局部规划路径的方法和设备 |
| CN110361013A (zh) * | 2019-07-22 | 2019-10-22 | 上海应用技术大学 | 一种用于车辆模型的路径规划系统及方法 |
| CN110609557A (zh) * | 2019-10-09 | 2019-12-24 | 中国人民解放军陆军装甲兵学院 | 无人车混合路径规划算法 |
| CN111653113A (zh) * | 2020-04-20 | 2020-09-11 | 浙江吉利汽车研究院有限公司 | 车辆的局部路径确定方法、装置、终端和存储介质 |
| CN111289008A (zh) * | 2020-04-28 | 2020-06-16 | 南京维思科汽车科技有限公司 | 一种用于无人驾驶车辆的局部路径规划算法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于GIS与约束条件下的最优路径规划研究;王美玲;潘允辉;;北京理工大学学报(08);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114370874A (zh) | 2022-04-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | Real-time trajectory planning for autonomous urban driving: Framework, algorithms, and verifications | |
| Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
| CN113204236B (zh) | 一种智能体路径跟踪控制方法 | |
| Wang et al. | A COLREGs-based obstacle avoidance approach for unmanned surface vehicles | |
| Ma et al. | A two-level path planning method for on-road autonomous driving | |
| CN112141091B (zh) | 解决车位偏移和定位偏移的二次泊车方法、系统及车辆 | |
| CN113759939A (zh) | 一种受限水域智能航行方法及装置 | |
| CN112327888B (zh) | 路径规划方法、装置、电子设备和存储介质 | |
| CN114370874B (zh) | 一种车辆、车辆路径规划方法及装置 | |
| CN113515111B (zh) | 一种车辆避障路径规划方法及装置 | |
| CN113625702A (zh) | 基于二次规划的无人车同时路径跟踪与避障方法 | |
| Kamran et al. | Minimizing safety interference for safe and comfortable automated driving with distributional reinforcement learning | |
| CN116266380A (zh) | 一种环境数据重建方法、装置、系统及存储介质 | |
| CN115494849A (zh) | 一种自动驾驶车辆导航控制方法及系统 | |
| CN114185337A (zh) | 一种车辆、车辆预碰撞检测方法及装置 | |
| CN116069037A (zh) | 无人车编队轨迹规划控制方法、装置、设备及存储介质 | |
| WO2023097874A1 (zh) | 用于行驶轨迹规划的方法和装置 | |
| CN114088094A (zh) | 一种无人艇的智能航路规划方法及系统 | |
| CN109297502A (zh) | 基于图像处理与gps导航技术的激光投影指向方法及装置 | |
| CN116653963B (zh) | 车辆变道控制方法、系统和智能驾驶域控制器 | |
| Huang et al. | Preliminary steps in understanding a target & control based driver steering model | |
| De Lima et al. | Sensor-based control with digital maps association for global navigation: a real application for autonomous vehicles | |
| CN211427151U (zh) | 一种应用于封闭场地无人驾驶货运车辆上的自动引导系统 | |
| Seo et al. | Hybrid tracker based optimal path tracking system of autonomous driving for complex road environments | |
| CN114537381B (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 | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: No. 6, Yutong Road, Guancheng Hui District, Zhengzhou, Henan 450061 Applicant after: Yutong Bus Co.,Ltd. Address before: No.1, Shibali Heyu Road, Guancheng Hui District, Zhengzhou City, Henan Province Applicant before: ZHENGZHOU YUTONG BUS Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |