CN110836671B - 轨迹规划方法、轨迹规划装置、存储介质与电子设备 - Google Patents
轨迹规划方法、轨迹规划装置、存储介质与电子设备 Download PDFInfo
- Publication number
- CN110836671B CN110836671B CN201911114431.XA CN201911114431A CN110836671B CN 110836671 B CN110836671 B CN 110836671B CN 201911114431 A CN201911114431 A CN 201911114431A CN 110836671 B CN110836671 B CN 110836671B
- Authority
- CN
- China
- Prior art keywords
- node
- child
- open list
- value
- nodes
- 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
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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
Abstract
本公开提供一种轨迹规划方法、装置、存储介质与电子设备,涉及智能导航技术领域。该方法包括:在地图中获取起点和终点,确定起点的状态参数,状态参数包括时刻;建立开放列表,以记录其中每个节点的状态参数,将起点加入开放列表;依次以开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定该父节点的子节点的状态参数,并将子节点加入开放列表,将该父节点从开放列表移除;当将终点加入开放列表时,从终点开始,依次连接子节点及其父节点,直到连接到起点,得到目标路径;根据目标路径中每个节点的状态参数,生成从起点到终点的目标轨迹。本公开可以实现在动态场景中的轨迹规划,并且符合车辆等物体的运动学特性。
Description
技术领域
本公开涉及智能导航技术领域,尤其涉及一种轨迹规划方法、轨迹规划装置、计算机可读存储介质与电子设备。
背景技术
运动规划(Motion Planning)是在给定的起点与终点之间确定一条符合约束条件的路径,广泛应用于车辆导航、自动驾驶、无人机、机器人控制等领域。
目前,运动规划大多停留在路径规划的阶段,例如业界常用的A星(A*)算法或混合A星(Hybrid A*)算法,针对于复杂非结构化静态场景,在起点与终点之间规划出一条几何曲线的路径。然而,路径规划大多针对于静态场景,当应用于动态场景时,存在以下问题:无法有效地实现动态碰撞躲避,若场景中存在移动障碍物(如其他车辆),可能发生碰撞,或者为了避开移动障碍物的路线,规划出较差的路径;路径规划可能违背车辆实际的运动学特性,例如可能规划出车辆正常行驶中难以完成的急转弯路径等。
因此,有必要提出一种轨迹规划方法,以解决现有的路径规划方法难以应用于动态场景的问题。
需要说明的是,在上述背景技术部分公开的信息仅用于加强对本公开的背景的理解,因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。
发明内容
本公开提供了一种轨迹规划方法、轨迹规划装置、计算机可读存储介质与电子设备,进而至少在一定程度上克服现有的路径规划方法难以应用于动态场景的问题。
本公开的其他特性和优点将通过下面的详细描述变得显然,或部分地通过本公开的实践而习得。
根据本公开的第一方面,提供一种轨迹规划方法,包括:在地图中获取起点和终点,确定所述起点的状态参数;建立开放列表,以记录其中每个节点的状态参数,将所述起点加入所述开放列表,每个节点的状态参数包括该节点对应的时刻;依次以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数,并将所述子节点加入所述开放列表,将所述父节点从所述开放列表移除;当将所述终点加入所述开放列表时,从所述终点开始,依次连接子节点及其父节点,直到连接到所述起点,得到目标路径;根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹。
可选的,所述状态参数还包括姿态和速度。
可选的,所述根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数,包括:获取每种状态变量组合;基于所述父节点的状态参数,分别计算通过所述每种状态变量组合运动所述预设间隔时间后,所到达的子节点,并计算所述子节点的状态参数。
可选的,所述状态变量包括速度变量和转向变量。
可选的,在将所述子节点加入所述开放列表时,通过以下方式确定所述子节点的代价值:根据所述子节点的父节点的第一子代价值,以及所述子节点及其父节点之间的状态参数之差,确定所述子节点的第一子代价值;将所述子节点到所述终点的代价值确定为所述子节点的第二子代价值;根据所述子节点的第一子代价值和第二子代价值确定所述子节点的代价值;其中,所述起点的第一子代价值为0。
可选的,任一节点到所述终点的代价值通过以下方式确定:计算所述任一节点到所述终点的Reeds-Shepp曲线的长度,作为第一长度;通过A星算法确定所述任一节点到所述终点的路径的长度,作为第二长度;将所述第一长度和所述第二长度中较大的长度值作为所述任一节点到所述终点的代价值。
可选的,在将所述子节点加入所述开放列表时,所述方法还包括:当所述子节点已经位于所述开放列表中时,获取所述子节点的原父节点;基于所述原父节点计算所述子节点的原代价值,以及基于当前父节点,计算所述子节点的当前代价值;当所述原代价值小于所述当前代价值时,保留所述子节点和所述原父节点之间的关系;当所述原代价值大于所述当前代价值时,将所述子节点的父节点更新为所述当前父节点。
可选的,在将所述子节点加入所述开放列表后,所述方法还包括:当满足预设条件时,生成所述子节点到所述终点的Reeds-Shepp曲线;基于所述子节点的状态参数,计算采用匀速移动时,所述Reeds-Shepp曲线中各节点的状态参数;根据所述Reeds-Shepp曲线中各节点的状态参数判断所述Reeds-Shepp曲线是否与所述地图中的障碍物发生碰撞;当确定所述Reeds-Shepp曲线与所述地图中的障碍物发生碰撞时,继续执行以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数的步骤;当确定所述Reeds-Shepp曲线不与所述地图中的障碍物发生碰撞时,从所述子节点开始,依次连接子节点及其父节点,直到连接到所述起点,并结合所述Reeds-Shepp曲线,得到目标路径;根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹。
可选的,在将所述子节点加入所述开放列表时,移除其中位于封闭列表中的子节点,将剩余的子节点加入所述开放列表。
可选的,在将所述父节点从所述开放列表移除后,所述方法还包括:将所述父节点加入所述封闭列表。
可选的,在根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数后,所述方法还包括:当根据所述子节点的状态参数确定所述子节点与所述地图中的障碍物发生碰撞时,将所述子节点加入所述封闭列表。
可选的,在将所述父节点从所述开放列表移除后,如果所述开放列表为空,则判断轨迹规划失败。
可选的,在将所述起点加入所述开放列表前,所述开放列表为空。
根据本公开的第二方面,提供一种轨迹规划装置,包括:获取模块,用于在地图中获取起点和终点,确定所述起点的状态参数;建立模块,用于建立开放列表,以记录其中每个节点的状态参数,将所述起点加入所述开放列表,每个节点的状态参数包括该节点对应的时刻;扩展模块,用于依次以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数,并将所述子节点加入所述开放列表,将所述父节点从所述开放列表移除;连接模块,用于当将所述终点加入所述开放列表时,从所述终点开始,依次连接子节点及其父节点,直到连接到所述起点,得到目标路径;生成模块,用于根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹。
可选的,所述状态参数还包括姿态和速度。
可选的,所述扩展模块,用于基于所述父节点的状态参数,分别计算通过每种状态变量组合运动所述预设间隔时间后,所到达的子节点,并计算所述子节点的状态参数。
可选的,所述状态变量包括速度变量和转向变量。
可选的,所述扩展模块包括代价计算单元,用于通过执行以下步骤确定所述子节点的代价值:根据所述子节点的父节点的第一子代价值,以及所述子节点及其父节点之间的状态参数之差,确定所述子节点的第一子代价值;将所述子节点到所述终点的代价值确定为所述子节点的第二子代价值;根据所述子节点的第一子代价值和第二子代价值确定所述子节点的代价值;其中,所述起点的第一子代价值为0。
可选的,所述代价计算单元,还用于通过执行以下步骤确定任一节点到所述终点的代价值:计算所述任一节点到所述终点的Reeds-Shepp曲线的长度,作为第一长度;通过A星算法确定所述任一节点到所述终点的路径的长度,作为第二长度;将所述第一长度和所述第二长度中较大的长度值作为所述任一节点到所述终点的代价值。
可选的,所述扩展模块包括节点更新单元,用于在将所述子节点加入所述开放列表时,通过执行以下步骤确定所述子节点的父节点:当所述子节点已经位于所述开放列表中时,获取所述子节点的原父节点;基于所述原父节点计算所述子节点的原代价值,以及基于当前父节点,计算所述子节点的当前代价值;当所述原代价值小于所述当前代价值时,保留所述子节点和所述原父节点之间的关系;当所述原代价值大于所述当前代价值时,将所述子节点的父节点更新为所述当前父节点。
可选的,所述装置还包括:曲线规划模块,用于在所述扩展模块将所述子节点加入所述开放列表后,执行以下步骤:当满足预设条件时,生成所述子节点到所述终点的Reeds-Shepp曲线;基于所述子节点的状态参数,计算采用匀速移动时,所述Reeds-Shepp曲线中各节点的状态参数;根据所述Reeds-Shepp曲线中各节点的状态参数判断所述Reeds-Shepp曲线是否与所述地图中的障碍物发生碰撞;当确定所述Reeds-Shepp曲线与所述地图中的障碍物发生碰撞时,调度所述扩展模块继续以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数;当确定所述Reeds-Shepp曲线不与所述地图中的障碍物发生碰撞时,从所述子节点开始,依次连接子节点及其父节点,直到连接到所述起点,并结合所述Reeds-Shepp曲线,得到目标路径;根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹。
可选的,所述扩展模块,还用于在将所述子节点加入所述开放列表时,移除其中位于封闭列表中的子节点,将剩余的子节点加入所述开放列表。
可选的,所述扩展模块,还用于在将所述父节点从所述开放列表移除后,将所述父节点加入所述封闭列表。
可选的,所述扩展模块,还用于当根据所述子节点的状态参数确定所述子节点与所述地图中的障碍物发生碰撞时,将所述子节点加入所述封闭列表。
可选的,所述扩展模块,还用于在将所述父节点从所述开放列表移除后,如果所述开放列表为空,则判断轨迹规划失败。
可选的,在所述建立模块将所述起点加入所述开放列表前,所述开放列表为空。
根据本公开的第三方面,提供一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现上述任意一种轨迹规划方法。
根据本公开的第四方面,提供一种电子设备,包括:处理器;以及存储器,用于存储所述处理器的可执行指令;其中,所述处理器配置为经由执行所述可执行指令来执行上述任意一种轨迹规划方法。
本公开的技术方案具有以下有益效果:
根据上述轨迹规划方法、轨迹规划装置、计算机可读存储介质与电子设备,建立开放列表,依次以开放列表中代价值最小的节点为父节点进行扩展,将扩展得到的子节点加入开放列表,直到将终点加入开放列表时,按照子节点和父节点的关系,从终点反向连接到起点,并根据其中每个节点的状态参数,生成从起点到终点的目标轨迹。一方面,每个节点记录状态参数,不仅包括节点的位置,还包括节点对应的时刻,基于时刻信息可以判断节点是否与地图中的移动障碍物发生碰撞,从而实现在动态场景中的轨迹规划。另一方面,子节点的状态参数是通过父节点的状态参数以及预设间隔时间的状态变量计算得到的,该过程符合物体的运动学特性,使得所规划的路径具有更好的实用性。
应当理解的是,以上的一般描述和后文的细节描述仅是示例性和解释性的,并不能限制本公开。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本公开的实施例,并与说明书一起用于解释本公开的原理。显而易见地,下面描述中的附图仅仅是本公开的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1示出本示例性实施例中一种轨迹规划方法的流程图;
图2示出本示例性实施例中一种轨迹规划方法的子流程图;
图3示出本示例性实施例中另一种轨迹规划方法的子流程图;
图4示出本示例性实施例中另一种轨迹规划方法的流程图;
图5示出本示例性实施例中一种轨迹规划装置的结构框图;
图6示出本示例性实施例中一种用于实现上述方法的计算机可读存储介质;
图7示出本示例性实施例中一种用于实现上述方法的电子设备。
具体实施方式
现在将参考附图更全面地描述示例实施方式。然而,示例实施方式能够以多种形式实施,且不应被理解为限于在此阐述的范例;相反,提供这些实施方式使得本公开将更加全面和完整,并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中,提供许多具体细节从而给出对本公开的实施方式的充分理解。然而,本领域技术人员将意识到,可以实践本公开的技术方案而省略所述特定细节中的一个或更多,或者可以采用其它的方法、组元、装置、步骤等。在其它情况下,不详细示出或描述公知技术方案以避免喧宾夺主而使得本公开的各方面变得模糊。
此外,附图仅为本公开的示意性图解,并非一定是按比例绘制。图中相同的附图标记表示相同或类似的部分,因而将省略对它们的重复描述。附图中所示的一些方框图是功能实体,不一定必须与物理或逻辑上独立的实体相对应。可以采用软件形式来实现这些功能实体,或在一个或多个硬件模块或集成电路中实现这些功能实体,或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。
轨迹规划是指在运动物体(如车辆、机器人等)的起始状态与终止状态之间计算出符合内部/外部约束条件的行驶轨迹,其中内部约束主要包括运动学特性,外部约束主要包括碰撞躲避或其他用户自定义添加的限制。
本公开的示例性实施方式首先提供一种轨迹规划方法,下面结合图1做具体说明。在下述说明中,将以车辆行驶轨迹规划的场景为例,而对于机器人移动轨迹规划、无人车行进轨迹规划等其他场景,方法的过程和原理是相同的。
如图1所示,轨迹规划方法可以包括以下步骤S110至S150:
步骤S110,在地图中获取起点和终点,确定起点的状态参数。
地图可以是包括起点和终点的局部地图,并包含足够的空间,以便于在其中规划轨迹;地图中还可以包括道路、障碍物等其他信息。起点和终点的位置可以通过地图中的位置坐标,或者GPS(Global Positioning System,全球定位***)数据等形式表示,本公开对此不做限定。
轨迹规划和一般的路径规划不同之处在于,轨迹规划需要记录状态信息,例如规划车辆行驶到A点,除了记录A点的位置外,还应当记录车辆行驶到A点的状态。本示例性实施方式中,对于地图中的节点,通过状态参数表示车辆到达节点时的状态。在状态参数中,必要的数据包括:时刻,即车辆到达该节点的时刻。此外,状态参数还可以包括:姿态,包括车辆到达节点时的方向、转向角(即车辆本身处于直行还是转向的状态,以及转向的角度)等;速度,即车辆到达节点时的行驶速度。在开始轨迹规划时,可以先确定起点的状态参数,其中,可以将当前时刻或零时刻作为起点的时刻,将当前车辆行驶状态作为起点对应的姿态和速度,例如车辆起始为静止状态,则起点对应的转向角和速度均为0。
步骤S120,建立开放列表,以记录其中每个节点的状态参数,将起点加入开放列表。
开放列表(Open List)可以看作是轨迹规划中候选节点的集合,将车辆可能行驶到的位置节点加入到开放列表中,以进行后续的筛选处理。开放列表可以记录其中每个节点的状态参数,包括每个节点对应的时刻,例如将起点加入开放列表时,在开放列表中记录起点的位置、时刻,还可以记录车辆的姿态、速度等。在一种可选的实施方式中,在将起点加入开放列表前,初始的开放列表可以为空,则起点是加入开放列表的第一个节点。
步骤S130,依次以开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定该父节点的子节点的状态参数,并将子节点加入开放列表,将该父节点从开放列表移除。
其中,从父节点确定子节点的过程可称为“扩展”。本示例性实施方式实质上是从起点开始,通过不断扩展到达终点的过程,扩展的轨迹即最终规划的轨迹。对于每个节点,可以计算其代价值,代价值是对行驶轨迹成本的度量,表示由起点行驶到该节点,再行驶到终点的成本,包括路径长度、所需时间、是否发生碰撞、是否频繁切换速度方向等因素。关于如何具体计算代价值,将在下文具体说明。
需要说明的是,步骤S130是循环执行的步骤,每执行一次为一次扩展,即车辆从当前父节点出发,经过预设间隔时间的行驶,所能到达的节点为子节点;简而言之,每次扩展表示车辆“走一步”。预设间隔时间是每次扩展的周期性时间,表示较小的时间单位,例如可以是5秒、10秒等,预设间隔时间越短,所规划的轨迹越精细,因此可以根据实际需求确定预设间隔时间。状态变量是指预设间隔时间内车辆所能实现的运动状态变化,例如加速、减速、转向等,由于预设间隔时间较短,在预设间隔时间内的状态变量可视为恒定值。
本示例性实施方式以父节点的状态参数为基础,通过预设间隔时间内的状态变量,计算子节点的状态参数。计算过程可以采用前向运动学方程或其他运动学模型,使得父节点和子节点之间满足运动学特性,例如在预设间隔时间内车辆能够完成一定程度的转弯,但无法实现掉头,因此子节点不会位于父节点的道路对向位置。
和起点的状态参数相同,子节点的状态参数也可以包括子节点的位置、时刻以及姿态、速度等其他信息。在确定子节点的状态参数后,将其加入开放列表。由于父节点已经完成了扩展,可以将其从开放列表移除,以防止重复扩展。
步骤S140,当将终点加入开放列表时,从终点开始,依次连接子节点及其父节点,直到连接到起点,得到目标路径。
将终点加入开放列表后,无需再进行扩展。从终点开始,连接终点和终点的父节点,再以该父节点为子节点,连接该子节点的父节点……以此类推,相当于将扩展过的节点逆向逐个连接,直到连接到起点,获得了连接起点和终点的目标路径。
步骤S150,根据目标路径中每个节点的状态参数,生成从起点到终点的目标轨迹。
其中,每个节点的状态参数包括每个节点的时刻,即根据规划,车辆行驶到每个节点的时刻。由此,可以将状态参数添加到目标路径中,例如将每个节点的时刻标注到目标路径中,得到从起点到终点的目标轨迹。目标轨迹包括每个节点的时刻信息,当地图中存在移动障碍物时,可以推算出车辆是否和移动障碍物发生碰撞,从而在动态场景中实现有效的轨迹规划,满足实际需求。
为了使扩展的过程更加符合运动学特性,在一种可选的实施方式中,步骤S130中,可以具体通过以下步骤确定父节点的子节点的状态参数:
获取每种状态变量组合;
基于父节点的状态参数,分别计算通过每种状态变量组合运动预设间隔时间后,所到达的子节点,并计算子节点的状态参数。
其中,状态变量可以包括多个维度的参数,例如速度变量、转向变量等。不同的状态变量可以形成多种组合,代表了车辆在预设间隔时间内可能采用的各种运动方式。举例来说,若状态变量包括速度变量和转向变量,分别以Δv和ΔΦ表示,在预设间隔时间Δt内,Δv有三种情况:v0、0和-v0,分别表示加速(速度增加v0)、保持匀速和减速;ΔΦ也有三种情况:Φ0、0和-Φ0,分别表示转向轮向左转动Φ0、保持直行和向右转动Φ0(Φ0也可以表示向右转,-Φ0向左转)。那么状态变量组合一共有3*3=9种,分别为:
Δv=v0,ΔΦ=Φ0,加速左转;
Δv=v0,ΔΦ=0,加速直行;
Δv=v0,ΔΦ=-Φ0,加速右转;
Δv=0,ΔΦ=Φ0,匀速左转;
……(不一一列举)
车辆处于当前父节点的状态参数下,采用上述9种方式运动预设间隔时间后,所可能到达的位置和状态都是不同的,因此该父节点存在9个子节点。可以通过运动推算的方式,从父节点的状态参数,分别计算在上述9种状态变量组合下,所得到的子节点及其状态参数。
可以将每个节点的状态参数表示为数组,例如当前父节点的状态参数S(Pcur)=[x(Pcur),y(Pcur),θ(Pcur),v(Pcur),t(Pcur)],分别表示当前父节点Pcur的x坐标、y坐标、姿态角、速度和时刻。对于子节点的状态参数S(Pchild)=[x(Pchild),y(Pchild),θ(Pchild),v(Pchild),t(Pchild)],其中x(Pchild)、y(Pchild)和θ(Pchild)可以通过前向运动学方程计算得到,其余部分可以通过下式(1)和(2)计算:
v(Pchild)=v(Pcur)+Δv (1)
t(Pchild)=t(Pcur)+Δt (2)
Δt为预设间隔时间。
在得到子节点Pchild的状态参数后,可以记录其和父节点的关系,如下:
Parent(Pchild)=Pcur (3)
需要补充的是,起点作为一个特殊节点,可以看作是祖先节点,即起点的父节点为空(Null)。
在一种可选的实施方式中,在将子节点加入开放列表时,可以确定其代价值,与其状态参数一同进行保存,以便于后续进行扩展时从开放列表中选取代价值最小的节点。参考图2所示,可以通过以下步骤S210至S230确定子节点的代价值:
步骤S210,根据子节点的父节点的第一子代价值,以及子节点及其父节点之间的状态参数之差,确定该子节点的第一子代价值;
步骤S220,将该子节点到终点的代价值确定为第该子节点的二子代价值;
步骤S230,根据子节点的第一子代价值和第二子代价值确定子节点的代价值。
其中,第一子代价值表示从起点到子节点的成本,第二子代价值表示从子节点到终点的成本。将这两部分相加或加权,可以得到该子节点最终的代价值。示例性的,以f(·)表示代价函数,g(·)表示第一子代价函数,h(·)表示第二子代价函数,子节点Pchild的代价值为:
f(Pchild)=g(Pchild)+h(Pchild) (4)
g(Pchild)=g(Pcur)+diff(Pcur,Pchild) (5)
g(Pcur)为子节点Pchild的父节点Pcur的第一子代价值;diff(Pcur,Pchild)表示子节点Pchild的父节点Pcur的状态参数之差,其具体计算方法取决于状态参数的具体信息,例如:状态参数包括位置和时刻的情况下,由于任意一组父节点和子节点之间的时刻差都是预设间隔时间Δt,可以忽略时刻差,将位置差,即距离(可以是欧式距离或曼哈顿距离)作为状态参数之差;状态参数包括位置、姿态、速度和时刻的情况下,忽略时刻差,分别计算位置差、姿态角度差、速度差,再将三组差值加权,得到状态参数之差,表示移动距离越大,运动状态的改变越大,代价值越大。此外,还可以在g(·)函数中添加惩罚项,以对一些特殊的行驶状态,例如倒车、频繁切换速度方向等,追加计算惩罚的代价值。
由式(5)可知,第一子代价值的计算是个递推的过程,子节点的第一子代价值依赖于其父节点的第一子代价值,其父节点的第一子代价值又依赖于祖父节点(父节点的父节点)的第一子代价值。因此,需要确定第一个父节点的第一子代价值。本示例性实施方式中,第一个父节点为起点,可以设定起点的第一子代价值为0,表示从起点到起点的成本为0。
以上介绍了g(·)函数,即第一子代价值的计算方法。下面通过几个具体方案说明h(·)函数,即第二子代价值的计算方法:
方案一、参考图3所示,任一节点到终点的代价值通过以下步骤S310至S330确定:
步骤S310,计算该节点到终点的Reeds-Shepp曲线的长度,作为第一长度;
步骤S320,通过A星算法确定该节点到终点的路径的长度,作为第二长度;
步骤S330,将第一长度和第二长度中较大的长度值作为该节点到终点的代价值。
其中,Reeds-Shepp曲线(RS曲线)是一种路径规划方法,可以采用曲线路径的模型,通过求解约束列方程得到。Reeds-Shepp曲线考虑车辆的运动学特性,但不考虑碰撞躲避;A星算法考虑静态碰撞躲避,但不考虑车辆的运动学特性。因此得到的第一长度和第二长度均是对该节点到终点的成本估计,本示例性实施方式可以采用其中较大的长度值作为该节点到终点的代价值。数学表示如下:设置函数hRS(·)与hA*(·),其中hRS(·)代表示第一长度的函数,hA*(·)表示第二长度的函数,据此可以定义:
h(·)max(hRS(·),hA*(·)) (6)
方案二、将上述方案一进行简化,可以采用其中的第一长度或第二长度作为节点到终点的代价值。
方案三、采用更为简化的方式,计算子节点到终点的距离,可以是欧氏距离或曼哈顿距离(视地图类型而定),作为其第二子代价值。
在轨迹规划的过程中,可能出现父节点不唯一的情况,例如车辆可以从节点A行驶到节点C,也可以从节点B行驶到节点C,则节点C的父节点可以是节点A或节点B。基于此,在一种可选的实施方式中,在将子节点加入开放列表时,可以执行以下步骤:
当该子节点已经位于开放列表中时,获取该子节点的原父节点;
基于原父节点计算该子节点的原代价值,以及基于当前父节点,计算该子节点的当前代价值;
当原代价值小于当前代价值时,保留该子节点和原父节点之间的关系;
当原代价值大于当前代价值时,将该子节点的父节点更新为当前父节点。
其中,开放列表中的节点,除了起点以外,均是某一节点的子节点。因此,若发现当前需要加入开放列表的子节点(以Pchild表示)已经位于开放列表中,说明Pchild在之前的扩展中,以其他节点的子节点身份加入了开放列表。即Pchild已经存在一个父节点,称为原父节点Pori,本轮扩展中的父节点称为当前父节点Pcur。基于Pori计算Pchild的代价值,表示从起点到Pori、再到Pchild的代价值,称为原代价值;基于Pcur计算Pchild的代价值,表示从起点到Pcur、再到Pchild的代价值,称为当前代价值;由于轨迹不同,行驶的成本一般不同,原代价值和当前代价值一般是不同的,因此可以通过比较代价值,选取一个更优的父节点。如下所示:
f(Pchild)=g(Pcur)+diff(Pcur,Pchild)+h(Pchild) (7)
f′(Pchild)=g(Pori)+diff(Pori,Pchild)+h(Pchild) (8)
f(Pchild)为当前代价值,f′(Pchild)为原代价值。具体计算方法可以参考上述式(4)~(6)的说明内容。
若f(Pchild)>f′(Pchild),则确定Parent(Pchild)=Pori;若f(Pchild)<f′(Pchild),则确定Parent(Pchild)=Pcur。此外,还可能存在f(Pchild)=f′(Pchild)的情况,说明两个父节点是等效的,则选择任一个皆可,也可以保留两个父节点。
图4示出了本示例性实施方式的另一种流程,其中,在执行步骤S110至S130后,判断终点是否加入了开放列表,若是,则执行步骤S140,若否,则继续判断是否满足预设条件;若不满足预设条件,则继续执行步骤S130,若满足预设条件,则执行以下步骤S131至S135:
步骤S131,当满足预设条件时,生成子节点到终点的Reeds-Shepp曲线。该子节点是当前加入开放列表的子节点,可以采用曲线路径的模型,通过求解约束列方程得到子节点到终点的Reeds-Shepp曲线。预设条件可以包括:每当进行了N次扩展,例如每扩展3次,执行步骤S131;该子节点位于终点周围的预设范围内,便于进行Reeds-Shepp曲线的规划;等等。
步骤S132,基于该子节点的状态参数,计算采用匀速移动时,Reeds-Shepp曲线中各节点的状态参数。例如可以采用当前速度,也可以采用预设速度(可以是正常行驶过程的平均速度),计算沿着Reeds-Shepp曲线匀速行驶时,每行驶预设间隔时间,所到达的节点以及各节点的状态参数,相当于将Reeds-Shepp曲线进行了等分。
步骤S133,根据Reeds-Shepp曲线中各节点的状态参数判断Reeds-Shepp曲线是否与地图中的障碍物发生碰撞。由于在生成Reeds-Shepp曲线时,通常已经考虑过躲避静态障碍物,此处的碰撞判断主要是动态碰撞判断:确定地图中移动障碍物的轨迹,如果障碍物轨迹和Reeds-Shepp曲线不相交,则不会发生碰撞;如果障碍物轨迹和Reeds-Shepp曲线相交,则判断障碍物轨迹中交点的时刻和Reeds-Shepp曲线中交点的时刻是否相距较近;如果两时刻相距较近(在一阈值范围内),则会发生碰撞;如果两时刻相距较远,则不会发生碰撞。
步骤S134,当确定Reeds-Shepp曲线与地图中的障碍物发生碰撞时,说明Reeds-Shepp曲线无法作为行驶轨迹,可以继续执行步骤S130,从开放列表中选取父节点进行扩展。
步骤S135,当确定Reeds-Shepp曲线不与地图中的障碍物发生碰撞时,从该子节点开始,依次连接子节点及其父节点,直到连接到起点,并结合Reeds-Shepp曲线,得到目标路径。目标路径由两部分组成,前一部分为起点到子节点之间,通过扩展所得到的路径,该路径的连接方法可以参照步骤S140;后一部分为Reeds-Shepp曲线的路径。
步骤S150,根据目标路径中每个节点的状态参数,生成从起点到终点的目标轨迹。在步骤S135确定的目标路径中,前一部分路径的节点为通过扩展所得到的节点,在扩展时计算了节点的状态参数,后一部分路径的节点在步骤S132中计算了状态参数。由此,目标路径中每个节点的状态参数已知,可以生成目标轨迹。
基于图4所示的流程,可以在扩展的过程中,通过RS曲线的方式直接生成后一部分的轨迹,从而加速轨迹规划的处理过程。
在一种可选的实施方式中,为了提高效率,可以建立封闭列表(ClosedList),用于存放已经探索过的节点,以防止重复处理。在步骤S130中,将子节点加入开放列表时,可以移除其中位于封闭列表中的子节点,将剩余的子节点加入开放列表。举例说明,当前扩展时,以父节点A确定子节点B、C、D,其中若子节点D位于封闭列表中,说明子节点D为已经处理过的节点,无需再加入开放列表重复处理,因此可以将子节点B、C加入开放列表。
初始的封闭列表可以为空,在扩展过程中,可以通过以下方式维护封闭列表:
在步骤S130中将父节点从开放列表移除后,可以将其加入封闭列表,例如上述将父节点A的子节点B、C加入开放列表后,可以将父节点A从开放列表移除,并加入封闭列表,后续如果在扩展其他节点时,探索到节点A作为该其他节点的子节点,也无需再加入开放列表。
或者在步骤S130中根据预设间隔时间内的状态变量确定父节点的子节点的状态参数后,当根据子节点的状态参数确定子节点与地图中的障碍物发生碰撞时,可以将该子节点加入封闭列表。其中,子节点的碰撞判断可以包括两方面:其一是静态障碍物判断,判断子节点是否位于静态障碍物的位置即可;其二是动态障碍物判断,根据动态障碍物的移动轨迹,当该轨迹经过子节点时,判断二者的时刻是否相距较近,若相距较近,则发生碰撞。通常将可能发生碰撞的节点称为无效节点或非法节点,在规划轨迹时需要避开这些节点,因此将加入封闭列表。
需要补充的是,轨迹规划可能存在规划失败的情况,包括但不限于:
在步骤S130中将父节点从开放列表移除后,如果开放列表为空,则判断轨迹规划失败。这说明所扩展得到的子节点均位于封闭列表中,没有新的子节点加入开放列表,无法继续扩展。该情况通常发生在地图中存在难以逾越的障碍物时;
扩展达到特定次数,仍然未能得到目标路径,说明处理过程可能发生了异常,该特定次数可以和起点与终点之间的距离相关;
开放列表中的节点的代价值均较高,例如均高于特定值,说明前期的扩展过程可能存在异常。
如果当前规划失败,可以立即重新开始一次规划,或者对地图、起点或终点的信息进行更新后再次规划,本公开对此不做限定。
综上所述,本示例性实施方式中,建立开放列表,依次以开放列表中代价值最小的节点为父节点进行扩展,将扩展得到的子节点加入开放列表,直到将终点加入开放列表时,按照子节点和父节点的关系,从终点反向连接到起点,并根据其中每个节点的状态参数,生成从起点到终点的目标轨迹。一方面,每个节点记录状态参数,不仅包括节点的位置,还包括节点对应的时刻,基于时刻信息可以判断节点是否与地图中的移动障碍物发生碰撞,从而实现在动态场景中的轨迹规划。另一方面,子节点的状态参数是通过父节点的状态参数以及预设间隔时间的状态变量计算得到的,该过程符合物体的运动学特性,使得所规划的路径具有更好的实用性。
本公开的示例性实施方式还提供一种轨迹规划装置。如图5所示,该轨迹规划装置500可以包括:获取模块510,用于在地图中获取起点和终点,确定起点的状态参数;建立模块520,用于建立开放列表,以记录其中每个节点的状态参数,将起点加入开放列表,每个节点的状态参数包括该节点对应的时刻;扩展模块530,用于依次以开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定该父节点的子节点的状态参数,并将子节点加入开放列表,将该父节点从开放列表移除;连接模块540,用于当将终点加入开放列表时,从终点开始,依次连接子节点及其父节点,直到连接到起点,得到目标路径;生成模块550,用于根据目标路径中每个节点的状态参数,生成从起点到终点的目标轨迹。
在一种可选的实施方式中,状态参数还可以包括姿态和速度。
在一种可选的实施方式中,扩展模块530,可以用于基于父节点的状态参数,分别计算通过每种状态变量组合运动预设间隔时间后,所到达的子节点,并计算子节点的状态参数。
在一种可选的实施方式中,状态变量可以包括速度变量和转向变量。
在一种可选的实施方式中,扩展模块530可以包括代价计算单元,用于通过执行以下步骤确定子节点的代价值:根据子节点的父节点的第一子代价值,以及子节点及其父节点之间的状态参数之差,确定子节点的第一子代价值;将子节点到终点的代价值确定为子节点的第二子代价值;根据子节点的第一子代价值和第二子代价值确定子节点的代价值;其中,起点的第一子代价值为0。
在一种可选的实施方式中,上述代价计算单元,还可以用于通过执行以下步骤确定任一节点到终点的代价值:计算任一节点到终点的Reeds-Shepp曲线的长度,作为第一长度;通过A星算法确定任一节点到终点的路径的长度,作为第二长度;将第一长度和第二长度中较大的长度值作为任一节点到终点的代价值。
在一种可选的实施方式中,扩展模块530可以包括节点更新单元,用于在将子节点加入开放列表时,通过执行以下步骤确定子节点的父节点:当子节点已经位于开放列表中时,获取子节点的原父节点;基于原父节点计算子节点的原代价值,以及基于当前父节点,计算子节点的当前代价值;当原代价值小于当前代价值时,保留子节点和原父节点之间的关系;当原代价值大于当前代价值时,将子节点的父节点更新为当前父节点。
在一种可选的实施方式中,轨迹规划装置500还可以包括:曲线规划模块,用于在扩展模块将子节点加入开放列表后,执行以下步骤:当满足预设条件时,生成子节点到终点的Reeds-Shepp曲线;基于子节点的状态参数,计算采用匀速移动时,Reeds-Shepp曲线中各节点的状态参数;根据Reeds-Shepp曲线中各节点的状态参数判断Reeds-Shepp曲线是否与地图中的障碍物发生碰撞;当确定Reeds-Shepp曲线与地图中的障碍物发生碰撞时,调度扩展模块530继续以开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定父节点的子节点的状态参数;当确定Reeds-Shepp曲线不与地图中的障碍物发生碰撞时,从子节点开始,依次连接子节点及其父节点,直到连接到起点,并结合Reeds-Shepp曲线,得到目标路径;根据目标路径中每个节点的状态参数,生成从起点到终点的目标轨迹。
在一种可选的实施方式中,扩展模块530,还可以用于在将子节点加入开放列表时,移除其中位于封闭列表中的子节点,将剩余的子节点加入开放列表。
在一种可选的实施方式中,扩展模块530,还可以用于在将父节点从开放列表移除后,将父节点加入封闭列表。
在一种可选的实施方式中,扩展模块530,还可以用于当根据子节点的状态参数确定子节点与地图中的障碍物发生碰撞时,将子节点加入封闭列表。
在一种可选的实施方式中,扩展模块530,还可以用于在将父节点从开放列表移除后,如果开放列表为空,则判断轨迹规划失败。
在一种可选的实施方式中,在建立模块520将起点加入开放列表前,开放列表为空。
上述装置中各模块/单元的具体细节在方法部分实施方式中已经详细说明,未披露的细节内容可以参见方法部分的实施方式内容,因而不再赘述。
所属技术领域的技术人员能够理解,本公开的各个方面可以实现为***、方法或程序产品。因此,本公开的各个方面可以具体实现为以下形式,即:完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等),或硬件和软件方面结合的实施方式,这里可以统称为“电路”、“模块”或“***”。
本公开的示例性实施方式还提供了一种计算机可读存储介质,其上存储有能够实现本说明书上述方法的程序产品。在一些可能的实施方式中,本公开的各个方面还可以实现为一种程序产品的形式,其包括程序代码,当程序产品在电子设备上运行时,程序代码用于使电子设备执行本说明书上述“示例性方法”部分中描述的根据本公开各种示例性实施方式的步骤。
参考图6所示,描述了根据本公开的示例性实施方式的用于实现上述方法的程序产品600,其可以采用便携式紧凑盘只读存储器(CD-ROM)并包括程序代码,并可以在电子设备,例如个人电脑上运行。然而,本公开的程序产品不限于此,在本文件中,可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行***、装置或者器件使用或者与其结合使用。
程序产品可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的***、装置或器件,或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了可读程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。可读信号介质还可以是可读存储介质以外的任何可读介质,该可读介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。
可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于无线、有线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言的任意组合来编写用于执行本公开操作的程序代码,程序设计语言包括面向对象的程序设计语言—诸如Java、C++等,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。在涉及远程计算设备的情形中,远程计算设备可以通过任意种类的网络,包括局域网(LAN)或广域网(WAN),连接到用户计算设备,或者,可以连接到外部计算设备(例如利用因特网服务提供商来通过因特网连接)。
本公开的示例性实施方式还提供了一种能够实现上述方法的电子设备。下面参照图7来描述根据本公开的这种示例性实施方式的电子设备700。图7显示的电子设备700仅仅是一个示例,不应对本公开实施方式的功能和使用范围带来任何限制。
如图7所示,电子设备700可以以通用计算设备的形式表现。电子设备700的组件可以包括但不限于:至少一个处理单元710、至少一个存储单元720、连接不同***组件(包括存储单元720和处理单元710)的总线730和显示单元740。
存储单元720存储有程序代码,程序代码可以被处理单元710执行,使得处理单元710执行本说明书上述“示例性方法”部分中描述的根据本公开各种示例性实施方式的步骤。例如,处理单元710可以执行图1至图4中任意一个或多个方法步骤。
存储单元720可以包括易失性存储单元形式的可读介质,例如随机存取存储单元(RAM)721和/或高速缓存存储单元722,还可以进一步包括只读存储单元(ROM)723。
存储单元720还可以包括具有一组(至少一个)程序模块725的程序/实用工具724,这样的程序模块725包括但不限于:操作***、一个或者多个应用程序、其它程序模块以及程序数据,这些示例中的每一个或某种组合中可能包括网络环境的实现。
总线730可以为表示几类总线结构中的一种或多种,包括存储单元总线或者存储单元控制器、***总线、图形加速端口、处理单元或者使用多种总线结构中的任意总线结构的局域总线。
电子设备700也可以与一个或多个外部设备800(例如键盘、指向设备、蓝牙设备等)通信,还可与一个或者多个使得用户能与该电子设备700交互的设备通信,和/或与使得该电子设备700能与一个或多个其它计算设备进行通信的任何设备(例如路由器、调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口750进行。并且,电子设备700还可以通过网络适配器760与一个或者多个网络(例如局域网(LAN),广域网(WAN)和/或公共网络,例如因特网)通信。如图所示,网络适配器760通过总线730与电子设备700的其它模块通信。应当明白,尽管图中未示出,可以结合电子设备700使用其它硬件和/或软件模块,包括但不限于:微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID***、磁带驱动器以及数据备份存储***等。
通过以上的实施方式的描述,本领域的技术人员易于理解,这里描述的示例实施方式可以通过软件实现,也可以通过软件结合必要的硬件的方式来实现。因此,根据本公开实施方式的技术方案可以以软件产品的形式体现出来,该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM,U盘,移动硬盘等)中或网络上,包括若干指令以使得一台计算设备(可以是个人计算机、服务器、终端装置、或者网络设备等)执行根据本公开示例性实施方式的方法。
此外,上述附图仅是根据本公开示例性实施方式的方法所包括的处理的示意性说明,而不是限制目的。易于理解,上述附图所示的处理并不表明或限制这些处理的时间顺序。另外,也易于理解,这些处理可以是例如在多个模块中同步或异步执行的。
应当注意,尽管在上文详细描述中提及了用于动作执行的设备的若干模块或者单元,但是这种划分并非强制性的。实际上,根据本公开的示例性实施方式,上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之,上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。
本领域技术人员在考虑说明书及实践这里公开的发明后,将容易想到本公开的其他实施方式。本申请旨在涵盖本公开的任何变型、用途或者适应性变化,这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施方式仅被视为示例性的,本公开的真正范围和精神由权利要求指出。
应当理解的是,本公开并不局限于上面已经描述并在附图中示出的精确结构,并且可以在不脱离其范围进行各种修改和改变。本公开的范围仅由所附的权利要求来限。
Claims (13)
1.一种轨迹规划方法,其特征在于,包括:
在地图中获取起点和终点,确定所述起点的状态参数;
建立开放列表,以记录其中每个节点的状态参数,将所述起点加入所述开放列表,每个节点的状态参数包括该节点对应的时刻、姿态和速度;
依次以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数,并将所述子节点加入所述开放列表,将所述父节点从所述开放列表移除;
当将所述终点加入所述开放列表时,从所述终点开始,依次连接子节点及其父节点,直到连接到所述起点,得到目标路径;
根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹;
其中,在将所述子节点加入所述开放列表时,通过以下方式确定所述子节点的代价值:
根据所述子节点的父节点的第一子代价值,以及所述子节点及其父节点之间的状态参数之差,确定所述子节点的第一子代价值;所述起点的第一子代价值为0;
将所述子节点到所述终点的代价值确定为所述子节点的第二子代价值;
根据所述子节点的第一子代价值和第二子代价值确定所述子节点的代价值;
其中,任一节点到所述终点的代价值通过以下方式确定:
计算所述任一节点到所述终点的Reeds-Shepp曲线的长度,作为第一长度;
通过A星算法确定所述任一节点到所述终点的路径的长度,作为第二长度;
将所述第一长度和所述第二长度中较大的长度值作为所述任一节点到所述终点的代价值。
2.根据权利要求1所述的方法,其特征在于,所述根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数,包括:
获取每种状态变量组合;
基于所述父节点的状态参数,分别计算通过所述每种状态变量组合运动所述预设间隔时间后,所到达的子节点,并计算所述子节点的状态参数。
3.根据权利要求1或2所述的方法,其特征在于,所述状态变量包括速度变量和转向变量。
4.根据权利要求1所述的方法,其特征在于,在将所述子节点加入所述开放列表时,所述方法还包括:
当所述子节点已经位于所述开放列表中时,获取所述子节点的原父节点;
基于所述原父节点计算所述子节点的原代价值,以及基于当前父节点,计算所述子节点的当前代价值;
当所述原代价值小于所述当前代价值时,保留所述子节点和所述原父节点之间的关系;
当所述原代价值大于所述当前代价值时,将所述子节点的父节点更新为所述当前父节点。
5.根据权利要求1所述的方法,其特征在于,在将所述子节点加入所述开放列表后,所述方法还包括:
当满足预设条件时,生成所述子节点到所述终点的Reeds-Shepp曲线;
基于所述子节点的状态参数,计算采用匀速移动时,所述Reeds-Shepp曲线中各节点的状态参数;
根据所述Reeds-Shepp曲线中各节点的状态参数判断所述Reeds-Shepp曲线是否与所述地图中的障碍物发生碰撞;
当确定所述Reeds-Shepp曲线与所述地图中的障碍物发生碰撞时,继续执行以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数的步骤;
当确定所述Reeds-Shepp曲线不与所述地图中的障碍物发生碰撞时,从所述子节点开始,依次连接子节点及其父节点,直到连接到所述起点,并结合所述Reeds-Shepp曲线,得到目标路径;
根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹。
6.根据权利要求1所述的方法,其特征在于,在将所述子节点加入所述开放列表时,移除其中位于封闭列表中的子节点,将剩余的子节点加入所述开放列表。
7.根据权利要求6所述的方法,其特征在于,在将所述父节点从所述开放列表移除后,所述方法还包括:
将所述父节点加入所述封闭列表。
8.根据权利要求6所述的方法,其特征在于,在根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数后,所述方法还包括:
当根据所述子节点的状态参数确定所述子节点与所述地图中的障碍物发生碰撞时,将所述子节点加入所述封闭列表。
9.根据权利要求6所述的方法,其特征在于,在将所述父节点从所述开放列表移除后,如果所述开放列表为空,则判断轨迹规划失败。
10.根据权利要求1所述的方法,其特征在于,在将所述起点加入所述开放列表前,所述开放列表为空。
11.一种轨迹规划装置,其特征在于,包括:
获取模块,用于在地图中获取起点和终点,确定所述起点的状态参数;
建立模块,用于建立开放列表,以记录其中每个节点的状态参数,将所述起点加入所述开放列表,每个节点的状态参数包括该节点对应的时刻、姿态和速度;
扩展模块,用于依次以所述开放列表中代价值最小的节点为父节点,根据预设间隔时间内的状态变量,确定所述父节点的子节点的状态参数,并将所述子节点加入所述开放列表,将所述父节点从所述开放列表移除;
连接模块,用于当将所述终点加入所述开放列表时,从所述终点开始,依次连接子节点及其父节点,直到连接到所述起点,得到目标路径;
生成模块,用于根据所述目标路径中每个节点的状态参数,生成从所述起点到所述终点的目标轨迹;
其中,所述扩展模块包括代价计算单元,用于通过执行以下步骤确定所述子节点的代价值:
根据所述子节点的父节点的第一子代价值,以及所述子节点及其父节点之间的状态参数之差,确定所述子节点的第一子代价值;所述起点的第一子代价值为0;
将所述子节点到所述终点的代价值确定为所述子节点的第二子代价值;
根据所述子节点的第一子代价值和第二子代价值确定所述子节点的代价值;
其中,所述代价计算单元,还用于通过执行以下步骤确定任一节点到所述终点的代价值:
计算所述任一节点到所述终点的Reeds-Shepp曲线的长度,作为第一长度;
通过A星算法确定所述任一节点到所述终点的路径的长度,作为第二长度;
将所述第一长度和所述第二长度中较大的长度值作为所述任一节点到所述终点的代价值。
12.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至10任一项所述的方法。
13.一种电子设备,其特征在于,包括:
处理器;以及
存储器,用于存储所述处理器的可执行指令;
其中,所述处理器配置为经由执行所述可执行指令来执行权利要求1至10任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911114431.XA CN110836671B (zh) | 2019-11-14 | 2019-11-14 | 轨迹规划方法、轨迹规划装置、存储介质与电子设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911114431.XA CN110836671B (zh) | 2019-11-14 | 2019-11-14 | 轨迹规划方法、轨迹规划装置、存储介质与电子设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110836671A CN110836671A (zh) | 2020-02-25 |
CN110836671B true CN110836671B (zh) | 2021-09-14 |
Family
ID=69575115
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911114431.XA Active CN110836671B (zh) | 2019-11-14 | 2019-11-14 | 轨迹规划方法、轨迹规划装置、存储介质与电子设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110836671B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111369066B (zh) * | 2020-03-09 | 2022-06-24 | 广东南方数码科技股份有限公司 | 路径规划方法、装置、电子设备及可读存储介质 |
CN111397624A (zh) * | 2020-03-27 | 2020-07-10 | 湖南大学 | 一种基于JPS和Hybrid A*的全局路径规划方法 |
CN112181999B (zh) * | 2020-09-21 | 2022-01-07 | 北京京东乾石科技有限公司 | 一种轨迹更新方法及装置、设备、存储介质 |
CN112783166A (zh) * | 2020-12-30 | 2021-05-11 | 深兰人工智能(深圳)有限公司 | 局部轨迹规划方法、装置、电子设备和存储介质 |
CN115083139B (zh) * | 2021-03-12 | 2023-11-24 | 上海交通大学 | 多车辆调度方法 |
CN114379584B (zh) * | 2021-12-17 | 2023-12-22 | 阿波罗智联(北京)科技有限公司 | 车辆控制方法、装置、车辆、设备和存储介质 |
CN114475658B (zh) * | 2022-02-23 | 2023-08-25 | 广州小鹏自动驾驶科技有限公司 | 一种自动驾驶速度的规划方法及装置、车辆和存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106441303A (zh) * | 2016-09-30 | 2017-02-22 | 哈尔滨工程大学 | 一种基于可搜索连续邻域a*算法的路径规划方法 |
CN108073176A (zh) * | 2018-02-10 | 2018-05-25 | 西安交通大学 | 一种改进型D*Lite车辆动态路径规划方法 |
CN108444488A (zh) * | 2018-02-05 | 2018-08-24 | 天津大学 | 基于等步采样a*算法的无人驾驶局部路径规划方法 |
CN109540146A (zh) * | 2018-11-29 | 2019-03-29 | 珠海格力智能装备有限公司 | 路径规划方法及装置 |
CN109947120A (zh) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | 仓储***中的路径规划方法 |
CN110274596A (zh) * | 2019-04-14 | 2019-09-24 | 西安烛龙智能科技有限公司 | 一种基于时间节点的智能避障方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106092111B (zh) * | 2016-06-03 | 2019-09-24 | 山东师范大学 | 一种车辆路径动态规划方法、服务器及导航*** |
CN108510095B (zh) * | 2017-02-23 | 2020-12-22 | 北京京东乾石科技有限公司 | 一种拣货路径的确定方法及装置 |
US10663966B2 (en) * | 2017-03-29 | 2020-05-26 | Mitsubishi Electric Research Laboratories, Inc. | Vehicle motion control system and method |
-
2019
- 2019-11-14 CN CN201911114431.XA patent/CN110836671B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106441303A (zh) * | 2016-09-30 | 2017-02-22 | 哈尔滨工程大学 | 一种基于可搜索连续邻域a*算法的路径规划方法 |
CN108444488A (zh) * | 2018-02-05 | 2018-08-24 | 天津大学 | 基于等步采样a*算法的无人驾驶局部路径规划方法 |
CN108073176A (zh) * | 2018-02-10 | 2018-05-25 | 西安交通大学 | 一种改进型D*Lite车辆动态路径规划方法 |
CN109540146A (zh) * | 2018-11-29 | 2019-03-29 | 珠海格力智能装备有限公司 | 路径规划方法及装置 |
CN110274596A (zh) * | 2019-04-14 | 2019-09-24 | 西安烛龙智能科技有限公司 | 一种基于时间节点的智能避障方法 |
CN109947120A (zh) * | 2019-04-29 | 2019-06-28 | 西安电子科技大学 | 仓储***中的路径规划方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110836671A (zh) | 2020-02-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110836671B (zh) | 轨迹规划方法、轨迹规划装置、存储介质与电子设备 | |
Li et al. | Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles | |
Cui et al. | Deep kinematic models for kinematically feasible vehicle trajectory predictions | |
US10459441B2 (en) | Method and system for operating autonomous driving vehicles based on motion plans | |
CN111830979B (zh) | 一种轨迹优化方法和装置 | |
KR20210074366A (ko) | 자율주행 차량 계획 및 예측 | |
CN109434831B (zh) | 机器人运行方法、装置、机器人、电子设备及可读介质 | |
CN112595337B (zh) | 避障路径规划方法、装置、电子装置、车辆及存储介质 | |
JP2023547745A (ja) | 車両の障害物回避方法、装置、電子機器、記憶媒体 | |
Hu et al. | Plug and play distributed model predictive control for heavy duty vehicle platooning and interaction with passenger vehicles | |
CN110782092A (zh) | 无人配送车在非结构化场景中的轨迹规划方法及装置 | |
CN112325898B (zh) | 路径规划方法、装置、设备和存储介质 | |
Altché et al. | Partitioning of the free space-time for on-road navigation of autonomous ground vehicles | |
Nakamura et al. | Online update of safety assurances using confidence-based predictions | |
Malayjerdi et al. | Practical path planning techniques in overtaking for autonomous shuttles | |
CN113442908A (zh) | 自动泊车路径规划方法及***、泊车控制设备 | |
WO2017122718A1 (ja) | 駐車支援システム、駐車支援方法及びプログラム | |
CN112272805B (zh) | 连接两条平滑参考线的基于多点增强的拼接方法 | |
WO2023201952A1 (zh) | 确定车辆最优行驶轨迹方法和装置 | |
JPWO2020157990A1 (ja) | 経路計画装置、経路計画方法、及びプログラム | |
CN115657675A (zh) | 一种车辆运动路径生成方法、***及存储介质 | |
CN115097826A (zh) | 一种车辆掉头轨迹规划方法及装置 | |
Li et al. | Decision making for autonomous vehicles | |
Gratzer et al. | Flatness-Based Mixed-Integer Obstacle Avoidance MPC for Collision-Safe Automated Urban Driving | |
CN111832142A (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 |