CN116817953A - 车辆路径规划方法、装置、电子设备及存储介质 - Google Patents

车辆路径规划方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN116817953A
CN116817953A CN202310796892.XA CN202310796892A CN116817953A CN 116817953 A CN116817953 A CN 116817953A CN 202310796892 A CN202310796892 A CN 202310796892A CN 116817953 A CN116817953 A CN 116817953A
Authority
CN
China
Prior art keywords
path
vehicle
point
coordinate system
heuristic
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.)
Pending
Application number
CN202310796892.XA
Other languages
English (en)
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.)
Beijing Yihang Yuanzhi Technology Co Ltd
Original Assignee
Beijing Yihang Yuanzhi 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 Beijing Yihang Yuanzhi Technology Co Ltd filed Critical Beijing Yihang Yuanzhi Technology Co Ltd
Priority to CN202310796892.XA priority Critical patent/CN116817953A/zh
Publication of CN116817953A publication Critical patent/CN116817953A/zh
Pending legal-status Critical Current

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/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/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

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

本公开提供了一种车辆路径规划方法、装置、电子设备及存储介质。本公开的车辆路径规划方法包括：获取车辆的当前状态信息和参考路径信息；根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息；根据启发路径长度阈值、车辆当前状态信息和N个参考点的信息生成车辆的规划路径，规划路径上各轨迹点的位置通过所述笛卡尔坐标系的坐标表示。本公开实施例能够直接在笛卡尔坐标系下完成路径规划，无需坐标转换。

Description

车辆路径规划方法、装置、电子设备及存储介质
技术领域
本公开涉及一种车辆路径规划方法、装置、电子设备及存储介质。
背景技术
随着自动驾驶技术的发展，路径规划技术得到了广泛应用。路径规划一般分为局部路径规划和全局路径规划，全局路径规划是根据起点和终点的位置规划一条车道级的路径，局部路径规划是规划车辆未来一段时间的行驶路径。
相关技术中，车辆的局部路径规划主要基于两种坐标系来实现，分别是笛卡尔坐标系和Frenet坐标系。车辆的局部路径规划主要有混合A星(Hybrid Astar)算法和基于横纵向位置采样的Lattice算法。其中，混合A星算法是笛卡尔坐标系下的代表算法，该算法能够在复杂场景下规划出满足车辆运动学规律的行驶路径，但是混合A星算法规划出的路径曲率是不连续的，对车辆的控制要求高且舒适性差，同时这种算法比较耗时，对算力要求较高，实现该算法所需的硬件成本较高。基于横纵向位置采样的Lattice算法是Frenet坐标系下的代表算法，这种算法规划的路径具有高阶连续性，但是这种算法规划的路径需要在笛卡尔坐标和Frenet坐标下相互转换，存在坐标转换畸变的问题，且这种算法对参考线的光滑性要求较高，同时算法也比较耗时，对算力要求较高，实现该算法所需的硬件成本也比较高。
发明内容
为了解决上述技术问题中的至少一个，本公开提供了一种车辆路径规划方法、装置、电子设备及存储介质。
根据本公开的第一方面，提供了一种车辆路径规划方法，包括：
获取车辆的当前状态信息和参考路径信息，车辆的当前状态信息包括车辆当前位置的笛卡尔坐标系坐标、当前的航向角和曲率；
根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息，每个参考点的信息包括参考点的笛卡尔坐标系坐标、航向角和道路曲率，N为大于或等于3的整数；
根据启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息生成车辆的规划路径，所述规划路径上各轨迹点的位置通过所述笛卡尔坐标系的坐标表示。
在本公开第一方面的一些可能的实施方式中，所述N个参考点包括第一参考点，所述第一参考点为所述参考路径上距离车辆当前位置最近的轨迹点。
在本公开第一方面的一些可能的实施方式中，所述N个参考点包括第三参考点，所述第三参考点包括Q个第一采样点，每个第一采样点与参考路径上满足如下条件的轨迹点之间的道路横向距离等于该采样点对应的第一预定偏移量：
se＝ss+T
其中，se表示轨迹点相对于参考路径起点的路径长度，T表示启发路径长度阈值，ss表示第一参考点相对于参考路径起点的路径长度，Q为大于或等于1的整数。
在本公开第一方面的一些可能的实施方式中，所述N个参考点包括第二参考点，所述第二参考点包括与所述第一采样点一一对应的Q个第二采样点，每个第二采样点与参考路径上满足如下条件的轨迹点之间的道路横向距离等于其对应的第二预定偏移量：
其中，sm表示轨迹点相对于参考路径起点的路径长度，T表示启发路径长度阈值，ss表示第一参考点相对于参考路径起点的路径长度，所述第二预定偏移量为相应第一采样点的第一预定偏移量的二分之一。
在本公开第一方面的一些可能的实施方式中，所述规划路径包括与Q个第三参考点一一对应的Q条路径；或者，所述规划路径根据与Q个第三参考点一一对应的Q条路径确定。
在本公开第一方面的一些可能的实施方式中，还包括：针对笛卡尔坐标系各个坐标轴分别构建六次多项式，所述六次多项式以启发路径长度为自变量、以相应坐标轴坐标为函数值；所述根据启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息生成车辆的规划路径，包括：基于启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息，确定各个六次多项式的系数并进行多项式拟合以确定规划路径上各个轨迹点的笛卡尔坐标系坐标。
在本公开第一方面的一些可能的实施方式中，所述六次多项式的系数根据路径起点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度导数、路径终点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数、路径锚点的笛卡尔坐标系对应坐标轴坐标、以及启发路径长度阈值确定；所述路径起点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度导数根据车辆当前位置的笛卡尔坐标系对应坐标轴坐标、车辆当前的航向角和曲率确定；所述路径终点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度导数根据所述第三参考点的笛卡尔坐标系对应坐标轴坐标、航向角和道路曲率确定；路径锚点的笛卡尔坐标系对应坐标轴坐标根据所述第二参考点的笛卡尔坐标系对应坐标轴坐标、第二预定偏移量以及车辆当前位置到第一参考点的距离确定。
根据本公开的第二方面，提供了一种车辆路径规划装置，包括：
信息获取单元，用于获取车辆的当前状态信息和参考路径信息，车辆的当前状态信息包括车辆当前位置的笛卡尔坐标系坐标、车辆当前的航向角和曲率；
参考点确定单元，用于根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息，每个参考点的信息包括参考点的笛卡尔坐标系坐标、航向角和道路曲率，N为大于或等于3的整数；
路径生成单元，用于根据启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息生成车辆的规划路径，所述规划路径上各轨迹点的位置通过所述笛卡尔坐标系的坐标表示。
根据本公开的第三方面，提供了一种电子设备，包括：
存储器，所述存储器存储执行指令；以及，
处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行上述的车辆路径规划方法。
根据本公开的第四方面，提供了一种可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述的车辆路径规划方法。
本公开实施例无需坐标转换，可以直接在笛卡尔坐标系下完成，算力消耗少，能够降低硬件成本和提高路径规划效率。
附图说明
附图示出了本公开的示例性实施方式，并与其说明一起用于解释本公开的原理，其中包括了这些附图以提供对本公开的进一步理解，并且附图包括在本说明书中并构成本说明书的一部分。
图1是根据本公开的一些实施方式的车辆路径规划方法的流程示意图。
图2是根据本公开的一些实施方式的参考点选取示意图。
图3是根据本公开的一些实施方式的启发路径长度选取示意图。
图4是根据本公开的一些实施方式的规划路径可视化示意图。
图5是根据本公开的一些实施方式的规划路径中启发路径长度增量示意图。
图6是根据本公开的一些实施方式的规划路径曲率关系示意图。
图7是本公开的一个实施方式的采用处理系统的硬件实现方式的车辆路径规划装置的结构示意框图。
具体实施方式
下面结合附图和实施方式对本公开作进一步的详细说明。可以理解的是，此处所描述的具体实施方式仅用于解释相关内容，而非对本公开的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本公开相关的部分。
需要说明的是，在不冲突的情况下，本公开中的实施方式及实施方式中的特征可以相互组合。下面将参考附图并结合实施方式来详细说明本公开的技术方案。
除非另有说明，否则示出的示例性实施方式/实施例将被理解为提供可以在实践中实施本公开的技术构思的一些方式的各种细节的示例性特征。因此，除非另有说明，否则在不脱离本公开的技术构思的情况下，各种实施方式/实施例的特征可以另外地组合、分离、互换和/或重新布置。
在附图中使用交叉影线和/或阴影通常用于使相邻部件之间的边界变得清晰。如此，除非说明，否则交叉影线或阴影的存在与否均不传达或表示对部件的具体材料、材料性质、尺寸、比例、示出的部件之间的共性和/或部件的任何其它特性、属性、性质等的任何偏好或者要求。此外，在附图中，为了清楚和/或描述性的目的，可以夸大部件的尺寸和相对尺寸。当可以不同地实施示例性实施例时，可以以不同于所描述的顺序来执行具体的工艺顺序。例如，可以基本同时执行或者以与所描述的顺序相反的顺序执行两个连续描述的工艺。此外，同样的附图标记表示同样的部件。
当一个部件被称作“在”另一部件“上”或“之上”、“连接到”或“结合到”另一部件时，该部件可以直接在所述另一部件上、直接连接到或直接结合到所述另一部件，或者可以存在中间部件。然而，当部件被称作“直接在”另一部件“上”、“直接连接到”或“直接结合到”另一部件时，不存在中间部件。为此，术语“连接”可以指物理连接、电气连接等，并且具有或不具有中间部件。
本文使用的术语是为了描述具体实施例的目的，而不意图是限制性的。如这里所使用的，除非上下文另外清楚地指出，否则单数形式“一个(种、者)”和“所述(该)”也意图包括复数形式。此外，当在本说明书中使用术语“包含”和/或“包括”以及它们的变型时，说明存在所陈述的特征、整体、步骤、操作、部件、组件和/或它们的组，但不排除存在或附加一个或更多个其它特征、整体、步骤、操作、部件、组件和/或它们的组。还要注意的是，如这里使用的，术语“基本上”、“大约”和其它类似的术语被用作近似术语而不用作程度术语，如此，它们被用来解释本领域普通技术人员将认识到的测量值、计算值和/或提供的值的固有偏差。
本公开实施例的路径规划主要是指局部路径规划。
图1示出了本公开的一些实施方式的车辆路径规划方法的流程示意图。如图1所示，本公开实施例的车辆路径规划方法可以包括如下步骤：
步骤S102，获取车辆的当前状态信息和参考路径信息，车辆的当前状态信息包括车辆当前位置的笛卡尔坐标系坐标、车辆当前的航向角和曲率；
参考路径信息可以包括但不限于参考路径上各个轨迹点相对于参考路径起点的路径长度、笛卡尔坐标系坐标、航向角和道路曲率等。参考路径可以是但不限于车辆当前行驶的车道中心线或通过车辆的全局规划路径，参考路径信息可以来自高精地图，也可以通过车辆的视觉传感器感知到的车道线数据来提供。
车辆的当前状态信息可以通过车辆的感知模块或者定位模块来获取，感知模块是指通过车辆的传感器系统采集的数据感知车辆当前状态的模块，定位模块是指基于车辆的传感器系统提供的数据进行车辆定位从而获取车辆当前状态的模块。
本公开实施例的笛卡尔坐标系可以根据需要选取。一些实施方式中，笛卡尔坐标系可以是平行于地面的二维直角坐标系，笛卡尔坐标系可以包括x轴和y轴，笛卡尔坐标系的原点可以是车辆后轴中心或者车辆此次行驶过程中的起点，笛卡尔坐标系的原点可以根据需要灵活选取。其他实施方式中，笛卡尔坐标系还可以是三维直角坐标系，x轴与y轴所形成的平面平行于地面，Z轴垂直于地面指向天空。对于笛卡尔坐标系的定义，本公开实施例不作限制。
本公开实施例中，车辆位置以车辆后轴中心位置为基准。也即，车辆的当前位置是指车辆后轴中心的当前位置。
步骤S104，根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息，每个参考点的信息包括参考点的笛卡尔坐标系坐标、航向角和道路曲率，N为大于或等于3的整数；
启发路径长度阈值可以取标定值。具体应用中，启发路径长度阈值可以根据车辆的行驶速度、道路曲率等灵活调整。例如，启发路径长度阈值可以设定为10、11、12、15或其他标定值。
N个参考点中可以包括第一参考点，该第一参考点为参考路径上距离车辆当前位置最近的轨迹点。第一参考点的信息可以直接从车辆的参考路径信息中提取。具体地，可以根据车辆当前位置，在参考路径上找到与车辆当前位置距离最近的轨迹点Ps，该轨迹点Ps即为第一参考点。以包括x轴和y轴且平行于地面的的笛卡尔坐标系为例，第一参考点的信息可以表示为(ss,xs,ys,θs,κs)，ss表示第一参考点相对于参考路径起点的路径长度，xs表示第一参考点的笛卡尔坐标系x轴坐标，ys表示第一参考点的笛卡尔坐标系y轴坐标，θs表示第一参考点的航向角，κs表示第一参考点的道路曲率。
N个参考点中还可以包括第三参考点，第三参考点包括Q个第一采样点，每个第一采样点与参考路径上满足如下式(1)所示条件的轨迹点Pe之间的道路横向距离等于其对应的第一预定偏移量：
se＝ss+T (1)
其中，se表示轨迹点Pe相对于参考路径起点的路径长度，T表示启发路径长度阈值，ss表示第一参考点Ps相对于参考路径起点的路径长度，Q为大于或等于1的整数。
第三参考点的信息可以根据轨迹点Pe的信息和第一预定偏移量L确定。第一预定偏移量L可以根据需要灵活设定，可以设定为固定值。假设参考路径左侧偏移为正、右侧偏移为负，第一预定偏移量L的值可以根据道路宽度实时调整，也可以采用标定值。
假设采样5个点，即，Q＝5，第一预定偏移量L可以设定为集合{-2，-1，0，1，2}，轨迹点Pe的道路横向方向上对称采样后得到的5个第一采样点分别为：位于轨迹点Pe左侧且道路横向距离等于2的点、位于轨迹点Pe左侧且道路横向距离等于1的点、轨迹点Pe、位于轨迹点Pe右侧且道路横向距离等于2的点、位于轨迹点Pe右侧且道路横向距离等于1的点。这些第一采样点的笛卡尔坐标系坐标、航向角和道路曲率均可以根据轨迹点Pe的笛卡尔坐标系坐标、航向角和道路曲率确定，轨迹点Pe的笛卡尔坐标系坐标、航向角和道路曲率可以从参考路径信息中提取。
以包括x轴和y轴且平行于地面的笛卡尔坐标系为例，第三参考点的信息可以表示为(se,xe,ye,θe,κe)，se表示第三参考点相对于参考路径起点的路径长度，xe表示第三参考点的笛卡尔坐标系x轴坐标，ye表示第三参考点的笛卡尔坐标系y轴坐标，θe表示第三参考点的航向角，κe表示第三参考点的道路曲率。
N个参考点中还可以包括第二参考点，第二参考点可以包括与前述第一采样点一一对应的Q个第二采样点，每个第二采样点与参考路径上满足如下式(2)所示条件的轨迹点Pm之间的道路横向距离等于其对应的第二预定偏移量：
其中，sm表示轨迹点Pm相对于参考路径起点的路径长度，T表示启发路径长度阈值，ss表示第一参考点相对于参考路径起点的路径长度。
式(2)中选用是因为选用第一参考点与第三参考点的中间点作为第二参考点。需要说明的是，第二参考点也可以是第一参考点与第三参考点之间的四分之一点、三分之一点或其他点作为第二参考点，相应的式(2)中的/>应替换为/>等。对于第二参考点的选择及其第二预定偏移量的设定，均可根据实际需求灵活调整，对此，本公开实施例不作限制。第二预定偏移量可以根据第一预定偏移量确定。例如，某个第二采样点的第二预定偏移量可以是相应第一采样点的第一预定偏移量的二分之一、四分之一等等。
第二参考点的信息可以根据轨迹点Pm的信息和第一预定偏移量L确定。假设采样5个点，即，Q＝5，第一预定偏移量L设定为集合{-2，-1，0，1，2}，第二预定偏移量取第一预定偏移量的二分之一，轨迹点Pm的道路横向方向上对称采样后得到的5个第二采样点分别为：位于轨迹点Pm左侧且道路横向距离等于1的点、位于轨迹点Pm左侧且道路横向距离等于0.5的点、轨迹点Pm、位于轨迹点Pm右侧且道路横向距离等于1的点、位于轨迹点Pm右侧且道路横向距离等于0.5的点。这些第二采样点的笛卡尔坐标系坐标、航向角和道路曲率均可以根据轨迹点Pm的笛卡尔坐标系坐标、航向角和道路曲率确定，轨迹点Pm的笛卡尔坐标系坐标、航向角和道路曲率可以从参考路径信息中提取。
以包括x轴和y轴且平行于地面的笛卡尔坐标系为例，第二参考点的信息可以表示为(sm,xm,ym,θm,κm)，sm表示第二参考点相对于参考路径起点的路径长度，xm表示第二参考点的笛卡尔坐标系x轴坐标，ym表示第二参考点的笛卡尔坐标系y轴坐标，θm表示第二参考点的航向角，κm表示第二参考点的道路曲率。
图2示出了参考点的选取示意图。通过选取三个参考点Ps、Pm和Pe，尤其是可用于确定路径锚点的第二参考点Pm，可以降低路径畸变概率，提升规划路径的安全性和舒适性。
尽管上述实施例中选定的参考点包括三种，即第一参考点、第二参考点和第三参考点，但可以理解的是，参考点也可以选定为四种、五种或者更多。也即，N可以是3，也可以取4、5或其他值。对此，本公开实施例不作限制。
步骤S106，根据启发路径长度阈值、车辆当前状态信息和N个参考点的信息生成车辆的规划路径，规划路径上各轨迹点的位置通过笛卡尔坐标系的坐标表示。
一些实施方式中，步骤S106或者步骤S102之前，还可以包括：针对笛卡尔坐标系各个坐标轴分别构建六次多项式，六次多项式以启发路径长度为自变量、以相应坐标轴坐标为函数值。步骤S106中，基于启发路径长度阈值、车辆当前状态信息和N个参考点的信息，确定各个六次多项式的系数并进行多项式拟合以确定规划路径上各个轨迹点的笛卡尔坐标系坐标。
以前述包括x轴与y轴的笛卡尔坐标系为例，针对笛卡尔坐标系各个坐标轴分别构建六次多项式，可以包括：构建用于计算规划路径中各轨迹点的笛卡尔坐标系x轴坐标的第一多项式和用于计算规划路径中各轨迹点的笛卡尔坐标系y轴坐标的第二多项式，第一多项式与第二多项式均为六次多项式且以启发路径长度为自变量。
考虑到高次多项式容易畸变，这些畸变可能降低规划路径的安全性和舒适性，因此，本公开实施例优选采用了六次多项式。但可以理解的是，具体应用中，多项式可以结合场景、精度需求等各种因素自由选定。例如，还可采用七次多项式或更高次的多项式，对此，本公开实施例不作限制。
以前述包括x轴与y轴的笛卡尔坐标系为例，第一多项式x(t)与第二多项式y(t)采用六次多项式的表达式如下式(3)所示：
其中，x(t)表示规划路径上启发路径长度为t的轨迹点的笛卡尔坐标系x轴坐标，y(t)表示规划路径上启发路径长度为t的轨迹点的笛卡尔坐标系y轴坐标，t等于0的轨迹点为路径起点，t等于T的轨迹点为路径终点，T表示预先设定的启发路径长度阈值，a0、a1、a2、a3、a4、a5、a6表示第一多项式的7个系数，b0、b1、b2、b3、b4、b5、b6表示第二多项式的7个系数。
图3示出了规划路径上启发路径长度t的选取示例图。图3中，规划起点是指规划路径的路径起点，规划终点是指规划路径的路径终点。如图3所示，启发路径长度t可以取区间[0，T]中的离散值，这些离散值对应规划路径上离散的多个轨迹点，规划路径上相邻轨迹点在参考路径上的间隔长度相等。通常，t的选取粒度越细，路径规划的精度越高。例如，T设定为12米时，规划路径上相邻轨迹点在参考路径上的间隔长度设定为0.05米，此时t可以取区间[0，12]中的如下值：0、0.05、0.1、……、12。又例如，T设定为12米时，规划路径上相邻轨迹点在参考路径上的间隔长度设定为0.1米，此时t可以取区间[0，12]中的如下值：0、0.1、0.2、……、12。
步骤S106中，六次多项式的系数根据路径起点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数、路径终点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数、路径锚点的笛卡尔坐标系对应坐标轴坐标、以及启发路径长度阈值确定。
其中，路径起点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数根据车辆当前位置的笛卡尔坐标系对应坐标轴坐标、车辆当前的航向角和曲率确定。
其中，路径终点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数根据第三参考点的笛卡尔坐标系对应坐标轴坐标、航向角和道路曲率确定。
其中，路径锚点的笛卡尔坐标系对应坐标轴坐标根据第二参考点的笛卡尔坐标系对应坐标轴坐标、第二预定偏移量以及车辆当前位置到第一参考点的距离确定。
以前述包括x轴与y轴的笛卡尔坐标系为例，假设采用式(3)所示的六次多项式，第一多项式x(t)的7个系数可以通过下式(4)确定，第二多项式y(t)的7个系数可以通过下式(5)确定。
其中，T表示预先设定的启发路径长度阈值，x0表示规划路径起点的笛卡尔坐标系x轴坐标，x′ 0表示x0关于启发路径长度t的一阶导数，x′ 0 ′表示x0关于启发路径长度t的二阶导数，x′ 0″表示x0关于启发路径长度t的三阶导数，x1表示规划路径终点位置的笛卡尔坐标系x轴坐标，x′ 1表示x1关于启发路径长度t的一阶导数，x′ 1 ′表示x1关于启发路径长度t的二阶导数，x′ 1″表示x1关于启发路径长度t的三阶导数，y0表示规划路径终点位置的笛卡尔坐标系Y轴坐标，y′ 0表示y0关于启发路径长度t的一阶导数，y′ 0 ′表示y0关于启发路径长度t的二阶导数，y′ 0″表示y0关于启发路径长度t的三阶导数，y1表示规划路径终点位置的笛卡尔坐标系Y轴坐标，y′ 1表示y1关于启发路径长度t的一阶导数，y′ 1 ′表示y1关于启发路径长度t的二阶导数，y′ 1″表示y1关于启发路径长度t的三阶导数。xa表示路径锚点的笛卡尔坐标系X轴，ya表示路径锚点的笛卡尔坐标系y轴坐标。
其中，x0及其关于启发路径长度t的导数可以通过下式(6)确定，y0及其关于启发路径长度t的导数可以通过下式(7)确定，x1及其关于启发路径长度t的导数可以通过下式(8)确定，y1及其关于启发路径长度t的导数可以通过下式(9)确定，路径锚点的笛卡尔坐标系坐标可以通过下式(10)～(12)确定。
其中，xv表示车辆当前位置(即，车辆后轴中心位置)的笛卡尔坐标系x轴坐标，yv表示车辆当位置的笛卡尔坐标系y轴坐标，θv表示车辆当前的航向角，kv表示车辆当前的曲率，xe表示第三参考点的笛卡尔坐标系x轴坐标，ye表示第三参考点的笛卡尔坐标系y轴坐标，θe表示第三参考点的航向角，ke表示第三参考点的道路曲率。xm表示第二参考点的笛卡尔坐标系x轴坐标，ym表示第二参考点的笛卡尔坐标系y轴坐标，θm表示第二参考点的航向角，km表示第二参考点的道路曲率，L表示第三参考点采用的第一预定偏移量，ds表示笛卡尔坐标系下车辆当前位置到第一参考点Ps的距离，xs表示第一参考点的笛卡尔坐标系x轴坐标，ys表示第一参考点的笛卡尔坐标系y轴坐标。
步骤S106中，规划路径可以包括与Q个第三参考点一一对应的Q条路径。或者，步骤S106中得到的规划路径可以根据与Q个第三参考点一一对应的Q条路径确定。例如，步骤S106中的规划路径可以是与Q个第三参考点一一对应的Q条路径中代价最小的一条路径。以前文Q＝5，第一预定偏移量设定为集合{-2，-1，0，1，2}为例，步骤S106得到的规划路径的可视化示意图如图4，可见，规划路径有5条，用户可以根据需要选定其中一条作为最终的规划路径。
下面对式(6)～(11)的原理及其推倒过程做详细说明。
本公开实施例的规划路径是以启发路径长度为参数的多项式拟合得到的路径，规划路径与规划起点的位置连续、航向角连续、曲率连续。
首先，位置连续，也即笛卡尔坐标系的x轴、y轴的坐标连续。要保证规划路径位置连续，需满足下式(13)～(14)所示的条件：
x0＝xv (13)
y0＝yv (14)
其次，航向角连续，也即规划路径的起始切线方向与规划起点的朝向相等。如图5所示，规划路径起点的朝向与x轴的夹角等于θv，假设启发路径发生Δt的变化率，引起x方向的变化等于Δx，引起y方向的变化等于Δy。需要约束路径起点的方向与规划起点的朝向相等，也即，以下式(15)～(16)成立。
Δx＝Δt·cos(θv) (15)
Δy＝Δt·sin(θv) (16)
记x′0为规划路径起点关于启发路径长度t的一阶导数，记y′0为规划路径起点关于启发路径长度t的一阶导数，则存在如下式(17)～(18)所示的关系：
最后，曲率连续，也即规划路径起点的曲率与规划起点的曲率相等。记x″0为规划路径起点关于启发路径长度t的二阶导数，记y″0为规划路径起点关于启发路径长度t的二阶导数，则存在如下式(19)～(20)所示的关系：
参见图6的曲率关系，启发路径发生Δt的变化率和曲率变化率Δθ满足：R·Δθ＝Δt，也即，弧长变化量等于半径乘以角度变化量。由此曲率关系可以得到下式(21)：
通过式(13)～(21)得到关于规划路径起点的式(6)～(7)。同样地，规划路径终点与规划路径起点一样，需要满足位置连续、航向角连续和曲率连续的关系，与式(13)～(21)同理，可以得到规划路径终点的关系式(8)～(9)。锚点位置只需要约束位置相等即可，也即，锚点位置必须在规划路径上，根据该约束可以直接得到式(10)～(11)。
本公开实施例的车辆路径规划方法在笛卡尔坐标系下进行，将笛卡尔坐标系中各个坐标轴作为不同维度来考虑(例如，将x轴和y轴分为两个维度考虑)，每个维度是关于路径长度的六次多项式，通过多项式拟合确定每个维度下各个轨迹点的坐标，即可生成车辆的规划路径。本公开实施例无需坐标转换，可以直接在笛卡尔坐标系下完成，算力消耗少，不仅能够降低硬件成本，还可提高路径规划效率，从而满足车辆局部路径规划的实时性需求。
本公开实施例的车辆路径规划方法以车辆当前位置为起点，经过指定锚点且终点也以参考路径上的轨迹点为基准，平滑连续，具有曲率连续的优点。
本公开实施例仅需从参考路径中选取3个轨迹点即可实现路径规划，对参考路径的要求较低，无需参考路径连续、高阶可导，算法简单、可靠、易于实现。
此外，本公开实施例还可通过指定锚点的方式来进行路径规划，可以有效降低路径畸变概率，提升规划路径的安全性和舒适性。
图7是本公开的一个实施方式的采用处理系统的硬件实现方式的车辆路径规划装置的结构示意框图。
该装置可以包括执行上述流程图中各个或几个步骤的相应模块。因此，可以由相应模块执行上述流程图中的每个步骤或几个步骤，并且该装置可以包括这些模块中的一个或多个模块。模块可以是专门被配置为执行相应步骤的一个或多个硬件模块、或者由被配置为执行相应步骤的处理器来实现、或者存储在计算机可读介质内用于由处理器来实现、或者通过某种组合来实现。
该硬件结构可以利用总线架构来实现。总线架构可以包括任何数量的互连总线和桥接器，这取决于硬件的特定应用和总体设计约束。总线800将包括一个或多个处理器900、存储器1000和/或硬件模块的各种电路连接到一起。总线800还可以将诸如外围设备、电压调节器、功率管理电路、外部天线等的各种其他电路1100连接。
总线800可以是工业标准体系结构(ISA，Industry Standard Architecture)总线、外部设备互连(PCI，Peripheral Component)总线或扩展工业标准体系结构(EISA，Extended Industry Standard Component)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，该图中仅用一条连接线表示，但并不表示仅有一根总线或一种类型的总线。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本公开的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本公开的实施方式所属技术领域的技术人员所理解。处理器执行上文所描述的各个方法和处理。例如，本公开中的方法实施方式可以被实现为软件程序，其被有形地包含于机器可读介质，例如存储器。在一些实施方式中，软件程序的部分或者全部可以经由存储器和/或通信接口而被载入和/或安装。当软件程序加载到存储器并由处理器执行时，可以执行上文描述的方法中的一个或多个步骤。备选地，在其他实施方式中，处理器可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行上述方法之一。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，可以具体实现在任何可读存储介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。
就本说明书而言，“可读存储介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。可读存储介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式只读存储器(CDROM)。另外，可读存储介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在存储器中。
应当理解，本公开的各部分可以用硬件、软件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施方式方法的全部或部分步骤是可以通过程序来指令相关的硬件完成，程序可以存储于一种可读存储介质中，该程序在执行时，包括方法实施方式的步骤之一或其组合。
此外，在本公开各个实施方式中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个可读存储介质中。存储介质可以是只读存储器，磁盘或光盘等。
图7是根据本公开的一个实施方式的车辆路径规划装置700的一种结构示意图。如图7所示，根据本公开的车辆路径规划装置700可以包括：
信息获取单元702，用于获取车辆的当前状态信息和参考路径信息，车辆的当前状态信息包括车辆当前位置的笛卡尔坐标系坐标、车辆当前的航向角和曲率；
参考点确定单元704，用于根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息，每个参考点的信息包括参考点的笛卡尔坐标系坐标、航向角和道路曲率，N为大于或等于3的整数；
路径生成单元706，用于根据启发路径长度阈值、车辆当前状态信息和N个参考点的信息生成车辆的规划路径，规划路径上各轨迹点的位置通过笛卡尔坐标系的坐标表示。
一些实施方式中，车辆路径规划装置700还可以包括：构建单元708，用于针对笛卡尔坐标系各个坐标轴分别构建六次多项式，六次多项式以启发路径长度为自变量、以相应坐标轴坐标为函数值。
本公开实施例的车辆路径规划装置700的其他技术细节可参见前文方法部分，不再赘述。
本公开还提供了一种电子设备，包括：存储器，存储器存储执行指令；以及处理器或其他硬件模块，处理器或其他硬件模块执行存储器存储的执行指令，使得处理器或其他硬件模块执行上述的车辆路径规划方法。
本公开还提供了一种可读存储介质，可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现上述的车辆路径规划方法。
在本说明书的描述中，参考术语“一个实施方式/方式”、“一些实施方式/方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施方式/方式或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施方式/方式或示例中。在本说明书中，对上述术语的示意性表述不必须的是相同的实施方式/方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施方式/方式或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施方式/方式或示例以及不同实施方式/方式或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
本领域的技术人员应当理解，上述实施方式仅仅是为了清楚地说明本公开，而并非是对本公开的范围进行限定。对于所属领域的技术人员而言，在上述公开的基础上还可以做出其它变化或变型，并且这些变化或变型仍处于本公开的范围内。

Claims (10)

1.一种车辆路径规划方法，其特征在于，包括：
获取车辆的当前状态信息和参考路径信息，车辆的当前状态信息包括车辆当前位置的笛卡尔坐标系坐标、当前的航向角和曲率；
根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息，每个参考点的信息包括参考点的笛卡尔坐标系坐标、航向角和道路曲率，N为大于或等于3的整数；
根据启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息生成车辆的规划路径，所述规划路径上各轨迹点的位置通过所述笛卡尔坐标系的坐标表示。
2.根据权利要求1所述的车辆路径规划方法，其特征在于，所述N个参考点包括第一参考点，所述第一参考点为所述参考路径上距离车辆当前位置最近的轨迹点。
3.根据权利要求2所述的车辆路径规划方法，其特征在于，所述N个参考点包括第三参考点，所述第三参考点包括Q个第一采样点，每个第一采样点与参考路径上满足如下条件的轨迹点之间的道路横向距离等于该采样点对应的第一预定偏移量：
se＝ss+T
其中，se表示轨迹点相对于参考路径起点的路径长度，T表示启发路径长度阈值，ss表示第一参考点相对于参考路径起点的路径长度，Q为大于或等于1的整数。
4.根据权利要求3所述的车辆路径规划方法，其特征在于，所述N个参考点包括第二参考点，所述第二参考点包括与所述第一采样点一一对应的Q个第二采样点，每个第二采样点与参考路径上满足如下条件的轨迹点之间的道路横向距离等于其对应的第二预定偏移量：
其中，sm表示轨迹点相对于参考路径起点的路径长度，T表示启发路径长度阈值，ss表示第一参考点相对于参考路径起点的路径长度，所述第二预定偏移量为相应第一采样点的第一预定偏移量的二分之一。
5.根据权利要求4所述的车辆路径规划方法，其特征在于，所述规划路径包括与Q个第三参考点一一对应的Q条路径；或者，所述规划路径根据与Q个第三参考点一一对应的Q条路径确定。
6.根据权利要求4所述的车辆路径规划方法，其特征在于，
还包括：针对笛卡尔坐标系各个坐标轴分别构建六次多项式，所述六次多项式以启发路径长度为自变量、以相应坐标轴坐标为函数值；
所述根据启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息生成车辆的规划路径，包括：基于启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息，确定各个六次多项式的系数并进行多项式拟合以确定规划路径上各个轨迹点的笛卡尔坐标系坐标。
7.根据权利要求6所述的车辆路径规划方法，其特征在于，
所述六次多项式的系数根据路径起点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数、路径终点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度的导数、路径锚点的笛卡尔坐标系对应坐标轴坐标、以及启发路径长度阈值确定；
所述路径起点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度导数根据车辆当前位置的笛卡尔坐标系对应坐标轴坐标、车辆当前的航向角和曲率确定；
所述路径终点的笛卡尔坐标系对应坐标轴坐标及其关于启发路径长度导数根据所述第三参考点的笛卡尔坐标系对应坐标轴坐标、航向角和道路曲率确定；
路径锚点的笛卡尔坐标系对应坐标轴坐标根据所述第二参考点的笛卡尔坐标系对应坐标轴坐标、第二预定偏移量以及车辆当前位置到第一参考点的距离确定。
8.一种车辆路径规划装置，其特征在于，包括：
信息获取单元，用于获取车辆的当前状态信息和参考路径信息，车辆的当前状态信息包括车辆当前位置的笛卡尔坐标系坐标、当前的航向角和曲率；
参考点确定单元，用于根据车辆的当前状态信息、参考路径信息和预先设定的启发路径长度阈值获取N个参考点的信息，每个参考点的信息包括参考点的笛卡尔坐标系坐标、航向角和道路曲率，N为大于或等于3的整数；
路径生成单元，用于根据启发路径长度阈值、车辆当前状态信息和所述N个参考点的信息生成车辆的规划路径，所述规划路径上各轨迹点的位置通过所述笛卡尔坐标系的坐标表示。
9.一种电子设备，其特征在于，包括：
存储器，所述存储器存储执行指令；以及
处理器，所述处理器执行所述存储器存储的执行指令，使得所述处理器执行权利要求1至7中任一项所述的车辆路径规划方法。
10.一种可读存储介质，其特征在于，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现权利要求1至7中任一项所述的车辆路径规划方法。
CN202310796892.XA 2023-06-30 2023-06-30 车辆路径规划方法、装置、电子设备及存储介质 Pending CN116817953A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310796892.XA CN116817953A (zh) 2023-06-30 2023-06-30 车辆路径规划方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310796892.XA CN116817953A (zh) 2023-06-30 2023-06-30 车辆路径规划方法、装置、电子设备及存储介质

Publications (1)

Publication Number Publication Date
CN116817953A true CN116817953A (zh) 2023-09-29

Family

ID=88114242

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310796892.XA Pending CN116817953A (zh) 2023-06-30 2023-06-30 车辆路径规划方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN116817953A (zh)

Similar Documents

Publication Publication Date Title
US11460318B2 (en) Method and apparatus for reconstructing motion track, storage medium, and electronic device
Chen et al. High-precision lane-level road map building for vehicle navigation
CN108519094B (zh) 局部路径规划方法及云处理端
Jo et al. Generation of a precise roadway map for autonomous cars
CN108180921B (zh) 利用gps数据的ar-hud导航系统及其导航方法
CN113448335A (zh) 路径规划方法和装置、车辆和可读存储介质
CN105264586A (zh) 用于车辆的占用地图
KR20230148259A (ko) 차량 궤적 편차 보정 방법, 장치 및 전자기기
CN112269965A (zh) 一种非完整约束条件下的连续曲率路径优化方法
CN111814286B (zh) 一种面向自动驾驶的车道级地图几何模型建立方法
CN111829536B (zh) 导航地图路网生成方法、装置、存储介质及电子设备
CN110441760B (zh) 一种基于先验地形图的大范围海底地形图拓展构图方法
CN110262506A (zh) 预瞄点的确认方法、车辆行驶控制方法、装置及设备
CN113177256B (zh) 一种道路曲线的曲线要素通用获取方法
CN112748421A (zh) 一种基于直行路段自动驾驶的激光雷达校准方法
CN103592665A (zh) 基于b样条曲线的车载终端定位轨迹拟合方法
CN113191342A (zh) 车道定位方法及电子设备
CN113050660B (zh) 误差补偿方法、装置、计算机设备及存储介质
CN101145240A (zh) 摄像机图像对道路场景多点高精度标定方法
CN116817953A (zh) 车辆路径规划方法、装置、电子设备及存储介质
CN102052925A (zh) 基于空间关系约束的适配区景象匹配方法
CN116817955B (zh) 车辆路径规划方法、装置、电子设备及存储介质
CN116383321A (zh) 决策环境构建方法、装置、车辆及存储介质
CN116295478A (zh) 车辆的导航方法及装置
Stannartz et al. Comparison of curve representations for memory-efficient and high-precision map generation

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