CN112703147B - 用于控制车辆的移动的***和方法 - Google Patents

Abstract

一种用于控制车辆从车辆的初始状态和车辆的目标状态的移动的***构造具有多个节点的图形并且确定通过图形将初始节点与目标节点连接的路径，所述节点定义车辆的状态并且包括定义车辆的初始状态的初始节点和定义车辆的目标状态的目标节点。该***使用具有起源于初始节点的节点的初始树和起源于目标节点的节点的目标树的双树构造来确定图形。双树构造被配置为基于可扩展节点的成本在初始树或目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展图形，使得子节点的成本小于可扩展节点的成本。

Description

用于控制车辆的移动的***和方法

技术领域

本发明总体上涉及车辆的路径规划，更具体地，涉及用于以确定性方式控制车辆从初始状态向目标空间中的移动的路径规划方法。

背景技术

车辆(无论自主车辆还是以自主驾驶模式执行的车辆)所采用的多个控制***预测车辆的未来、安全运动或路径，以便不仅避开诸如其它车辆或行人的障碍物，而且优化关联到车辆的操作的一些标准。目标状态可以是固定位置、移动位置、速度向量、区域或其组合。诸如道路边缘、行人和其它车辆的周围环境通过车辆的传感器来感测和/或通过先验给定信息至少部分地获知。

在诸如自主车辆、机器人、制造和计算机视觉应用的各种应用中出现了路径规划。已提出了许多规划算法，包括(仅举几个例子)基于图形的A*和D*方法、涉及导航功能的连续方法和潜在领域；最近，诸如概率路线图(PRM)、扩展空间树、快速探索随机树(RRT)、最优变体RRT*和PRM*以及随时RRT的基于采样的方法。

用于控制以自主驾驶模式执行的自主或半自主车辆的任务之一自动地将车辆停放到停放位置和取向(本文中称为目标状态)。停车任务可表述如下。给定车辆动力学、停车空间的地图、表示车辆的起始位置和取向的初始状态以及表示车辆的目标停放位置和取向的目标状态确定车辆从初始状态到目标状态的期望路径或运动，然后控制车辆的致动器(例如，车辆的油门踏板和方向盘)，以确保车辆遵循期望路径或运动。然而，由于对车辆运动的非完整约束以及停车空间(例如，停车库或停车场)中的通常狭窄的自由空间，用于自动地停放车辆的路径规划有挑战性。

各种基于图形的方法搜索与机器人的配置空间近似的预定义图形。从各个节点和边缘分别表示相同的配置和连接路径的意义上讲，图形有点是静态的。在搜索算法的探索期间，图形的节点和边缘二者被标记为无冲突或被占用。诸如A*和D*的完善算法实现了分辨率完整性，在某些情况下具有最优性保证。由于预定义图形的复杂度随配置空间的维度呈指数增长，所以对于高维***，这些方法变得实际上不可行。

基于采样的方法可克服维数问题，并且在高维***上得以普及。根据基于采样的方法，代替使用预定义图形，在运行中构造图形，其中其节点从配置空间随机地抽取。这些方法的有效性取决于图形构造(或等同地空间探索)的效率。

然而，基于采样的方法(例如，RRT方法)在计算上昂贵。为此，许多工作开发了有偏采样方案以改进计算效率。例如，美国申请15/402353通过一组航路点来使采样偏倚，这些航路点表示中间但里程标配置。然而，对于一些应用，即使有偏采样在计算上也不实际。

发明内容

一些实施方式的目的是公开一种允许实时路径生成以用于自动车辆控制的运动规划方法。一些实施方式的另一目的是公开一种例如适合于自动停车***和方法的允许实时自动车辆控制的运动规划方法。一些实施方式的另一目的是提供一种降低由探索车辆的状态空间的采样方法的概率性本质导致的路径规划的计算成本的方法。一些实施方式的另一目的是提供以降低的计算复杂度和/或存储器要求用于车辆的状态空间的确定性探索的***和方法。

一些实施方式基于这样的认识：用于控制车辆从车辆的初始状态和车辆的目标状态的移动的自动车辆控制***可包括运动规划器，其被配置为构造具有定义车辆的状态的多个节点的图形。运动规划器可使用这种图形来探索车辆的状态空间并且选择用于移动车辆的控制动作。例如，图形的节点包括定义车辆的初始状态的初始节点和定义车辆的目标状态的目标节点。图形中的各对节点利用使车辆在连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接。基元运动由定义基元运动的类型和范围的控制动作和积分时间的组合形成。

有了该图形，运动规划器可确定通过图形将初始节点与目标节点连接的路径，使得该路径定义使车辆从初始状态移动到目标状态的基元运动序列，以使用该基元运动序列来控制车辆的移动。然而，这种***的挑战在于以适合于实时路径生成和控制的高效计算方构造图形。

一些实施方式基于这样的理解：基于采样的路径规划方法可用于寻找通过具有任意布局的状态空间的期望路径。然而，基于采样的路径规划方法(例如，快速探索随机树(RRT)方法及其变体)应用于自动控制问题可导致大量浪费的计算，使得此任务对于实时应用在计算上不实际。例如，由于可将目标状态与车辆的初始(例如，当前)状态分离的许多障碍物，通往目标状态的路径可具有不同的形式和形状。为此，为了寻找这种路径，可能需要对整个状态空间进行采样并测试路径可行性。

结果，基于概率采样的路径规划方法可未必采样所有未探索的状态空间，导致对于实时车辆控制应用不实际的解决方案。另外，由于不必要的采样，概率规划器可能输出与人的直觉不一致的路径。对于人驾驶者，这种路径可能不舒服和令人困惑。为此，与计算效率的目的另外或另选地，一些实施方式的目的是输出与人操作者可能选择的路径类似的路径。

一些实施方式基于从对人操作者选择用于驾驶车辆通过具有障碍物的空间的路径的行为和/或基本原理的观察搜集的多个认识。这些观察共同地或单独地得到自动路径规划和控制的一些实施方式。

首先，一个实施方式基于这样的认识：空间的探索应该是确定性的，即，非概率性的，就像人操作者将如何执行一样。一些实施方式通过以确定性方式构造连接车辆的初始状态和目标车辆的图形将此认识结合到规划和控制***中。例如，一个实施方式通过迭代地选择节点并利用预定义基元运动的有限集合扩展所选节点来构造图形(或树)。此方法的挑战在于确定如何确定性地识别要扩展的节点和/或如何确定性地选择用于扩展节点的运动基元以确保方法的计算效率。

其次，一个实施方式基于这样的认识：人操作者不仅使用局部信息，而且使用全局信息来执行空间探索。例如，人操作者将评估车辆的当前(即，初始)状态周围的空间和车辆的目标状态周围的空间，以关于车辆的路径做出决策。一些实施方式基于这样的认识：通过构造形成图形的两个树而非一个树，即，通过构造起源于与车辆的初始状态对应的初始节点的节点的初始树并且通过构造起源于与车辆的目标状态对应的目标节点的节点的目标树，可将全局信息结合到基于确定性图形的路径规划方法中。初始树表示车辆周围的局部信息，目标树表示车辆目标周围的局部信息，并且初始树与目标树之间的关系提供关于全局信息的启示。

另外，该实施方式基于通过许多仿真和实验确认的直觉：通过构造从封闭空间到自由空间的“外树”，而非通过构造从自由空间朝着封闭空间的“内树”，可更高效地避开围绕空间的障碍物。在诸如自主停车***的许多应用中，停车空间通常比车库或停车场中通往停车空间的空间更封闭。为此，一些实施方式基于这样的认识：双树空间探索可在计算上比单树空间探索更高效并且可更遵循人的直觉。

一些实施方式通过在初始树或目标树上选择节点扩展来利用双树空间探索的计算效率。具体地，一些实施方式基于可扩展节点的成本在初始树或目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展图形，使得子节点的成本小于可扩展节点的成本。以这种方式，实现树的确定性选择和扩展。

另外，如本文所使用的，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本(考虑初始状态周围的局部信息)、通过目标树的节点的第二路径的第二成本(考虑目标状态周围的局部信息)以及初始树和目标树的节点之间的第三路径的第三成本(考虑初始状态与目标状态之间的关系)。可扩展节点的这种选择允许考虑从两个树的构造搜集的信息，使得图形延伸在计算上更高效。例如，许多实验证明，双树图形构造确定具有比单树图形构造更少的节点的图形。

第三，一个实施方式基于这样的认识：除了节点选择之外，节点扩展也可按确定性方式执行，以减少以所有可能的基元运动探索节点周围的空间的需求。为此，实施方式选择降低可扩展节点的成本的第一无碰撞基元运动，即，无需测试后续基元运动。为了进一步减轻计算负担，一些实施方式基于各个基元运动与定义通往可扩展节点的图形边缘的基元运动的相似度来对基元运动集合进行排序，并按该顺序测试基元运动。这些实施方式促进人操作者想要的车辆移动的平稳性，并且实际上有助于降低所构造的图形的复杂度。

节点扩展的减少降低了所构造的图形的稀疏度，这一方面改进了计算效率，另一方面减少了空间探索的选项。为了解决由一些实施方式的确定性节点扩展造成的该问题，一些实现方式在图形的不同部分处变化图形的稀疏度。例如，一个实现方式将潜在可扩展节点的列表维持在运动规划器的存储器中并且基于可扩展节点列表的长度来选择稀疏度的值。稀疏度的值可由形成基元运动的控制动作的积分时间和可扩展节点的子节点到图形中的最近节点之间的最小允许距离之一或组合定义。

因此，一个实施方式公开了一种用于控制车辆从车辆的初始状态和车辆的目标状态的移动的***。该***包括运动规划器，该运动规划器包括处理器，其被配置为构造具有定义车辆的状态的多个节点的图形，其中，节点包括定义车辆的初始状态的初始节点和定义车辆的目标状态的目标节点，其中，图形中的各对节点以使车辆在连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接，确定通过图形将初始节点与目标节点连接的路径，使得该路径定义使车辆从初始状态移动到目标状态的基元运动序列；并且将基元运动序列发送到车辆的控制器，该控制器被配置为使用基元运动序列来控制车辆的移动。

运动规划器被配置为通过构造起源于初始节点的节点的初始树并且通过构造起源于目标节点的节点的目标树来确定图形，其中，为了构造初始树或目标树，运动规划器被配置为基于可扩展节点的成本在初始树或目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展图形，使得子节点的成本小于可扩展节点的成本，其中，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本、通过目标树的节点的第二路径的第二成本以及初始树和目标树的节点之间的第三路径的第三成本。

另一实施方式公开了一种用于控制车辆从车辆的初始状态和车辆的目标状态的移动的方法，其中，该方法使用与所存储的实现该方法的指令联接的处理器，其中，所述指令在由处理器执行时执行方法的步骤，这些步骤包括：构造具有定义车辆的状态的多个节点的图形，其中，节点包括定义车辆的初始状态的初始节点和定义车辆的目标状态的目标节点，其中，图形中的各对节点以使车辆在连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接，其中，图形包括起源于初始节点的节点的初始树和起源于目标节点的节点的目标树，其中，为了构造初始树或目标树，处理器被配置为：基于可扩展节点的成本在初始树或目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展图形，使得子节点的成本小于可扩展节点的成本，其中，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本、通过目标树的节点的第二路径的第二成本以及初始树和目标树的节点之间的第三路径的第三成本；确定通过图形将初始节点与目标节点连接的路径，使得该路径定义使车辆从初始状态移动到目标状态的基元运动序列；并且将基元运动序列发送到车辆的控制器，该控制器被配置为使用基元运动序列来控制车辆的移动。

另一实施方式公开了一种非暂时性计算机可读存储介质，其上具体实现有可由处理器执行以用于执行一种方法的程序，该方法包括：构造具有定义车辆的状态的多个节点的图形，其中，节点包括定义车辆的初始状态的初始节点和定义车辆的目标状态的目标节点，其中，图形中的各对节点以使车辆在连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接，其中，图形包括起源于初始节点的节点的初始树和起源于目标节点的节点的目标树，其中，为了构造初始树或目标树，处理器被配置为：基于可扩展节点的成本在初始树或目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展图形，使得子节点的成本小于可扩展节点的成本，其中，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本、通过目标树的节点的第二路径的第二成本以及初始树和目标树的节点之间的第三路径的第三成本；确定通过图形将初始节点与目标节点连接的路径，使得该路径定义使车辆从初始状态移动到目标状态的基元运动序列；并且将基元运动序列发送到车辆的控制器，该控制器被配置为使用基元运动序列来控制车辆的移动。

附图说明

[图1A]图1A示出由一些实施方式解决的停车情景的示例。

[图1B]图1B示出根据一些实施方式的车辆的几何表示的示例示意图。

[图1C]图1C示出与图1A的停车情景对应的停车空间的地图。

[图2A]图2A示出根据一个实施方式的自动车辆控制***的框图。

[图2B]图2B示出根据一个实施方式的运动规划***的一般结构的框图。

[图3A]图3A示出根据一些实施方式的由运动规划器执行的方法的框图。

[图3B]图3B示出根据一个实施方式的使用双树构造确定图形的示意图。

[图3C]图3C示出根据一些实施方式的用于运动规划方法的方法的框图。

[图4A]图4A示出根据一个实施方式的图形构造方法的框图。

[图4B]图4B示出根据一些实施方式的表示图形的数据结构的示例。

[图4C]图4C示出根据一个实施方式的节点的数据结构。

[图5A]图5A示出根据一些实施方式的原理的调整图形生长参数的方法的流程图。

[图5B]图5B示出根据另一实施方式的调整图形生长参数的方法的流程图。

[图5C]图5C示出根据一些实施方式的从可扩展节点的列表选择可扩展节点的方法的框图。

[图5D]图5D示出根据一个实施方式的节点扩展的框图。

[图5E]图5E示出一些实施方式用来终止迭代图形构造的多个停止标准的框图。

[图6A]图6A示出根据一个实施方式的当目标树仅包括目标节点时确定节点X的成本F(X)的方法的示意图。

[图6B]图6B示出根据一个实施方式的当目标树包括多个节点时确定节点X的成本F(X)的方法的示意图。

[图6C]图6C示出例示了不同实施方式的益处的示例的示意图。

[图7A]图7A示出例示了根据一些实施方式的与控制动作

和积分时间

的集合关联的基元运动的示意图。

[图7B]图7B示出一些实施方式用来扩展节点的基元运动的有序列表。

[图8A]图8A示出根据一个实施方式的确定运动基元或运动基元的子集以扩展所选节点的方法的框图。

[图8B]图8B示出根据另一实施方式的在可扩展节点的扩展期间选择运动基元的方法的框图。

[图9]图9示出根据一个实施方式的***的示意图。

[图10]图10示出根据一些实施方式的自动停车***的框图。

具体实施方式

一些实施方式的目的是公开一种允许实时路径生成以用于自动车辆控制的路径和/或运动规划方法。一些实施方式的另一目的是公开一种例如适合于自动停车***和方法的允许实时自动车辆控制的运动规划方法。仅出于清晰目的，关于自动停车情景描述一些实施方式。然而，关于停车情景说明的原理由其它自动车辆控制应用中的替代实施方式使用。

图1A示出通过一些实施方式解决的停车情景的示例。在此示例中，停车空间150的边界由大小为L×H的矩形表示。车辆100具有初始状态101并且需要停放在由目标状态102定义的停车位。各个状态(例如，初始状态和目标状态)定义车辆的位置和取向。停车空间150包括障碍物。障碍物可以是停车空间的布局的一部分，即，诸如停车空间的墙壁和/或立柱的永久障碍物103。永久障碍物的尺寸通常是已知的，或者可在合理范围内估计。图1A描绘了这些尺寸的非限制性示例。

另外地或另选地，障碍物可包括其它停放或移动的车辆104。车辆104和要停放的车辆100的几何尺寸可基于车辆的类型来确定。为了清晰，本公开考虑了前轮驱动车辆；但是不同的实施方式应用于包括后轮驱动和全轮驱动车辆的其它车辆。

在一些实施方式中，在停车空间的地图上指定停车空间的布局和障碍物103的位置。这种地图可预定或者在停车任务期间或之前实时构造。各种实施方式确定连接初始状态和目标状态的运动路径105，使得运动路径可行且无碰撞。

图1B示出根据一些实施方式的车辆的几何表示的示例示意图。在此示例中，车辆被抽象为矩形115。车辆状态包括表示其后轮轴线的中点的位置(x,y)110以及表示车身轴线与水平轴线之间的角度的取向θ120。

路径规划是要求解以下问题。给定初始配置

目标配置

和***(2)，寻找无碰撞运动学可行的路径

其

开始于X₀并且结束于X_f；

处于无碰撞配置空间

中；

满足***运动学(2)。

设J(·)是向各个非平凡路径指派非负成本的成本函数。最优路径规划是要寻找使J(·)最小的无碰撞运动学可行的路径

不同实施方式的运动规划使用车辆100的运动学和/或动力学模型来确定车辆如何沿着路径移动。例如，车辆的动力学模型考虑了车辆状态随时间的改变。动力学模型通常由微分方程表示。在一个实施方式中，车辆的动力学模型是五阶微分方程

其中v是前轮的速度，ζ是前轮与车辆取向之间的角度，a_v是平移加速度，a_s是转向角速度，b是(x,y)与前轮的中点之间的距离。

在一些实施方式中，根据动力学模型(1)来生成路径。动力学可行的路径是动力学模型(1)的解。因此，车辆状态为X＝(x,y,θ,v,ζ)。由于车辆通常以零速度和转向角开始和结束，因此动力学模型(1)的初始状态和目标状态分别为X₀＝(x₀,y₀,θ₀,0,0)和X_f＝(x_f,y_f,θ_f,0,0)。

另外地或另选地，一些实施方式使用车辆的运动学模型来进行运动规划，其描述车辆的运动而不考虑车辆的质量或导致运动的力。在一个实施方式中，考虑以下运动学模型

其中u₁＝cos(ζ)v是后轮轴线的中点的速度(以车辆的纵向速度命名)，u₂＝tan(ζ)/b。不失一般性，u₂被称为(归一化)转向角。

根据此实施方式，如果路径是运动学模型(2)的解，则该路径是运动学可行的。只有当位于X处的车辆不与任何障碍物碰撞并且完全处于边界105内时，车辆状态X＝(x,y,θ)才是无碰撞的。所有无碰撞车辆状态构成无碰撞配置空间

初始状态101被缩写为X₀＝(x₀,y₀,θ₀)，目标状态102由X_f＝(x_f,y_f,θ_f)表示。对于具有由矩形L×H表示的停车空间的特定停车任务，车辆状态总是属于状态空间χ：[0,L)×[0,H)×[0,2π)。

以这种方式，不同的实施方式可应用于运动学模型和动力学模型二者。不失一般性，以下描述基于运动学模型来进行。

树

是节点集合

与边缘集合ε的并集，即，

在不引起混淆的情况下，节点、配置和车辆状态下面可互换使用。边缘E(X_i,X_j)∈ε表示节点X_i和X_j之间的无碰撞运动学可行的路径。对于有限集合

表示其元素的数量。

一些实施方式基于停车空间的几何表示(也称为停车空间的地图)来确定车辆状态是否无碰撞。通过将停车空间的所有障碍物和边界近似为简单几何形状来推导停车空间的地图。在一个实施方式中，环境(等同于停车空间)中的障碍物103可被近似为矩形，其通过构造各个障碍物的最小边界框来推导。利用停车空间的障碍物和边界的几何近似，停车空间或环境可完全由几何对象的列表描述。

图1C示出与图1A的停车情景对应的停车空间的地图130。使用矩形表示将两种类型的障碍物合并为一种类型的永久障碍物103。障碍物可由多个几何形状近似，这取决于用于路径规划的机载处理器的计算能力。例如，在另一实施方式中，如果计算能力不足，则可通过为各个障碍物构造边界圆来近似障碍物。另选地，障碍物可由多面体近似，然而这可能增加路径规划的计算负担。另外，停车空间中的障碍物可能不由相同的几何形状近似。在一些实施方式中，环境地图被分解为一组单元，各个单元对应于具有特定体积的配置及其邻域。单元可具有不同的体积。停车空间中的障碍物可由一组单元表示。

图2A示出根据一个实施方式的自动车辆控制***299的框图。环境映射和定位块201构造或更新空间(例如，停车空间)的地图，并且通过感测环境和车辆操作条件来确定车辆的当前位置。例如，惯性测量单元(IMU)(可包括3轴加速度计、3轴陀螺仪和/或磁力计)可用于感测车辆操作。全球定位***传感器可用于提供车辆的位置和速度。感测环境200的传感器可以是捕获包括其它车辆、行人和建筑物的障碍物的视频相机、检测车辆与障碍物之间的距离的超声/雷达和LiDAR传感器等。在一个实施方式中，环境地图被进一步处理以生成停车空间的几何表示，如图1C所示。

目标状态选择202通过识别停车场候选来为停车位选择目标状态以停放车辆，并且将目标状态提交给运动规划块203。在一个实施方式中，通过与停车库的管理关联的单独***跟踪可用停车位。另外地或另选地，可使用自动***的传感器230来检测停车位。在一个实施方式中，运动规划块203检查以确定目标状态是否可停车(即，是否存在到停车位的可行路径)，并且将检查结果通知给目标状态选择块202。如果目标状态不可停车，则目标选择块202选择另一目标状态进行评估。在另一实施方式中，目标状态选择块202还可评估目标状态是否可停车，并且仅将可停车的目标状态发送到运动规划块。

如果目标状态可停车，则运动规划203发起完整运动规划过程以基于车辆模型210、车辆的初始状态和目标状态和停车空间的地图之一或组合来确定参考轨迹241。参考轨迹是车辆的基元运动序列。例如，在一个实施方式中，参考轨迹定义车辆速度和转向角随时间的轮廓。在另一实施方式中，参考轨迹定义车辆状态(x,y,θ)随时间的轮廓。在大多数情况下，各种形式的参考轨迹是等效的，一个轨迹经由车辆模型定义另一轨迹。

运动规划器将由基元运动序列确定的参考轨迹发送到车辆的控制器204。给定参考轨迹241，车辆控制器和致动器204确定并施加控制命令以强制车辆状态跟踪其参考轨迹241。在一个实施方式中，控制命令可以是油门踏板压力和/或转向扭矩。车辆控制器/致动器可使用信号243和244来确定控制命令。信号243可以是使方向盘或油门踏板移动的电机的测量的转向角或测量的电流、IMU的输出以及由定位块201估计的车辆的当前状态。

图2B示出根据一个实施方式的运动规划***203的一般结构的框图。运动规划***203包括用于执行运动规划***203的模块的至少一个处理器270。处理器270连接271到存储几何信息281(例如，车辆的几何形状和停车空间的地图)的存储器280。存储器280还可存储车辆282的模型(例如，车辆的运动学模型和/或车辆的动力学模型)。存储器280还可存储运动规划器的内部信息283，包括(但不限于)车辆的初始状态、停放车辆的目标状态、成本函数、各个计算的状态的值、导致各个状态的运动、几何图形、运动学图形、航路点、参考轨迹。在一些实施方式中，存储器280可包括存储的实现用于自动车辆控制的方法的指令，其中，这些指令在由处理器270执行时执行方法的至少一些步骤。

图3A示出根据一些实施方式的由***299的运动规划器203执行以用于控制车辆从车辆的初始状态和车辆的目标状态的移动的方法的框图。运动规划器被配置为构造310具有定义车辆的状态的多个节点的图形。节点包括定义车辆的初始状态的初始节点和定义车辆的目标状态的目标节点。图形中的各对节点以使车辆在连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接。

运动规划器还被配置为：确定330通过图形将初始节点与目标节点连接的路径，使得该路径定义使车辆从初始状态移动到目标状态的基元运动序列；并且将基元运动序列发送335到车辆的控制器，该控制器被配置为使用基元运动序列来控制车辆的移动。有了该图形，可使用优化路径的参数(例如，其成本和平稳性之一或组合)的各种优化技术来确定路径。

在一些实施方式中，运动规划器使用双树构造来构造310图形。具体地，运动规划器通过构造起源于初始节点的节点的初始树并且通过构造起源于目标节点的节点的目标树来确定图形。为了构造初始树或目标树，运动规划器被配置为基于可扩展节点的成本在初始树或目标树中选择315可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展320图形，使得子节点的成本小于可扩展节点的成本。

如本文所使用的，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本、通过目标树的节点的第二路径的第二成本以及初始树和目标树的节点之间的第三路径的第三成本。在各种实施方式中，节点的成本指示车辆通过节点的状态从初始状态到目标状态的行驶时间、车辆通过节点的状态从初始状态行驶到目标状态的距离、车辆通过节点的状态从初始状态行驶到目标状态的移动模式的变化率之一或组合。

一些实施方式基于通过许多仿真和实验确认的直觉：通过构造从封闭空间到自由空间的“外树”，而非通过构造从自由空间朝着封闭空间的“内树”，可更高效地避开围绕空间的障碍物。在诸如自主停车***的应用的许多应用中，停车空间通常比车库或停车场中通往停车空间的空间更封闭。为此，一些实施方式基于这样的认识：双树空间探索可在计算上比单树空间探索更高效并且更遵循人的直觉。

一些实施方式通过在初始树或目标树上选择节点扩展来利用双树空间探索的计算效率。具体地，一些实施方式基于可扩展节点的成本在初始树或目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展图形，使得子节点的成本小于可扩展节点的成本。

图3B示出根据一个实施方式的使用双树构造来确定图形的示意图。在此示例中，使用具有与初始状态101对应的初始节点311的初始树321和具有与目标状态102对应的目标节点312的目标树322从两端构造图形。初始树具有节点集合和边缘集合。目标树也具有节点集合和边缘集合。

在当前迭代期间，基于其成本来选择来自初始树或目标树的节点313。在此示例中，节点313选自初始树。例如，在一个实现方式中，来自初始树或目标树的节点可互换选择。在初始树的所有节点当中，节点313具有从初始节点通过节点313到达目标节点的最小成本。该节点旨在以子节点扩展，因此，该节点被称为可扩展节点。

只有当存在无碰撞运动基元，可通过添加以该无碰撞基元运动所定义的边缘连接到可扩展节点的子节点来扩展初始树时，节点313才被选为可扩展节点。否则，节点313不可扩展。为此，在一些实施方式中，用于选择可扩展节点的运动规划器被配置为按照其相应成本的顺序测试多个节点，并且选择所测试的节点中的具有降低所选节点的成本的无碰撞基元运动的第一个节点作为可扩展节点。

例如，如果可降低节点313的成本的所有基元运动均通往障碍物103，则节点313不可扩展，并且实施方式选择成本大于节点313的成本的另一节点(例如，节点331)来扩展。如果节点331可扩展，即，存在降低可扩展节点331的成本的无碰撞运动基元(例如，336)，则实施方式使用由该无碰撞运动基元定义的边缘336来向初始树321添加成本小于节点331的成本的子节点337。

接下来，在一些实现方式中，实施方式例如从节点341扩展目标树。例如，一些实施方式另选地(即，一个接一个)构造初始树和目标树。图形构造继续，直至达到终止条件。例如，在一个实施方式中，当初始树和目标树利用无碰撞Reed-Shepp路径连接时，运动规划器停止构造图形。

以这种方式，实现树的确定性选择和扩展。另外，如本文所使用的，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本(考虑初始状态周围的局部信息)、通过目标树的节点的第二路径的第二成本(考虑目标状态周围的局部信息)以及初始树和目标树的节点之间的第三路径的第三成本(考虑初始状态和目标状态之间的关系)。可扩展节点的这种选择允许考虑从两个树的构造搜集的信息，使得图形延伸在计算上更高效。例如，许多实验证明，双树图形构造确定具有比单树图形构造更少的节点的图形。

一些实施方式基于这样的附加认识：除了节点选择之外，节点扩展也可按确定性方式执行，以减少以所有可能的基元运动探索节点周围的空间的需求。为此，一个实施方式选择降低可扩展节点的成本的第一无碰撞基元运动。为了进一步减轻计算负担，一些实施方式基于各个基元运动与定义通往可扩展节点的图形边缘的基元运动的相似度来对基元运动集合进行排序，并按该顺序测试基元运动。这些实施方式促进人操作者想要的车辆移动的平稳性，并且实际上有助于降低所构造的图形的复杂度，因为如果通往节点的运动基元无碰撞，则可能甚至很有可能起源于该节点的相同运动基元也无碰撞。

例如，边缘336由定义通往可扩展节点331的图形边缘的相同基元运动创建。即使存在甚至可进一步降低节点331的成本的其它基元运动，也选择第一个测试的基元运动以扩展节点。

图3C示出根据一些实施方式的用于运动规划方法的方法的框图。以车辆的几何形状、停车空间的地图以及初始状态和目标状态开始，该方法首先将图形

初始化345并输出初始图形350。基于初始图形和基元运动355，通过重复两个步骤来构造360图形：使用优先控制动作的节点选择和节点扩展。结果，获得连接初始状态和目标状态或生长到目标状态的吸引区域中的图形365，并用于确定370运动学或动力学路径375。如果图形包括距目标节点ε距离内的节点，则可以说该图形生长到目标状态的吸引区域中。路径可包括驱动车辆从初始状态到目标状态的基元运动序列。在一些实施方式中，基于车辆380的运动学路径和动力学模型来确定385参考轨迹。在另一实施方式中，通过基于动力学模型380使动力学路径375平滑来确定参考轨迹。参考轨迹390被馈送到控制模块以基于感测的信号391(例如，车辆的当前状态)来控制395车辆的运动，使得车辆运动遵循参考轨迹。

图4A示出根据一个实施方式的图形构造方法的框图。基于初始和/或目标状态将图形初始化345，首先根据多个度量的组合调节401预设图形生长参数。例如，生长参数可包括运动基元的集合、图形的稀疏度、选择用于节点扩展的运动基元的顺序之一或组合。生长参数允许构造具有非均匀分布的节点的图形，在图形的不同部分处具有不同的稀疏度值。在一些实施方式中，运动规划器基于可扩展节点列表的长度来选择稀疏度值。稀疏度的值由形成基元运动的控制动作的积分时间和子节点到图形中的最近节点之间的最小允许距离之一或组合定义。

在一些实施方式中，运动规划器在存储器中存储可扩展节点的一个或更多个列表。如果所有可扩展节点列表均为空，则图形构造停止以选择不同的生长参数，或者该方法返回失败。否则，从可扩展节点列表选择402可扩展节点441。进一步扩展403可扩展节点441，其中应用优化控制动作以实现节点扩展。基于停车空间的地图，节点扩展403可能确定可扩展节点的零个、一个或一组无碰撞子节点，以及相应地可扩展节点与其子节点之间的零个、一个或一组无碰撞边缘。这些无碰撞子节点和边缘被添加到图形。如果新构造的图形满足特定停止标准，则构造算法终止；否则，重复调整401、选择402和扩展403。

图4B示出根据一些实施方式的表示图形的数据结构的示例。例如，在一个实施方式中，图形

至少包括节点集合431和边缘集合432，其中基于车辆X的特定无碰撞状态来创建节点。边缘表示两个节点之间的无碰撞运动学/动力学可行的路径。在一个实施方式中，图形至少包括可扩展节点列表

421以及树

422。图形初始化345基于初始状态创建初始节点，并且将初始节点添加到可扩展列表

为了简化表示法而不引起混淆，此描述可互换地使用节点和状态。

在一个实施方式中，状态的吸引区域是其邻域，包括距该状态特定距离内的所有可能状态。基元运动由一对预定义控制动作A_i及其积分时间t_fi表示，或者通过将该对(A_i,t_fi)应用于车辆模型而产生状态序列。在一些实施方式中，对车辆动力学模型应用诸如加速度的控制动作以获得状态。在另一实施方式中，对车辆运动学模型应用诸如速度和转向角的控制动作以生成对应状态。

在一些实施方式中，对车辆的预定义控制动作是转向角和纵向速度的组合。为了简单，我们将转向角和纵向速度归一化以使得二者均取[-1,1]内的值。因此，将在域[-1,1]×[-1,1]上定义车辆的控制动作。为了应用本发明中教导的方法，该域需要近似以生成预定义控制动作。在一个实施方式中，通过将转向角的范围[-1,1]离散化为多个(例如，5个)可能的动作{-1,-0.5,0.,0.5,+1}来获得域的近似；而纵向速度的范围被离散化为多个(例如，2个)可能的动作{-1,+1}。离散化导致包括预定义控制动作的预定义动作集合。例如，在一个实施方式中，预定义动作集合A包括10个元素：{(-1,1),(-0.5,1),(0,1),(+0.5,1),(1,1),(-1,-1),(-0.5,-1),(0,-1),(+0.5,-1),(1,-1)}。预定义控制动作A_i可以是A的任何元素。

类似地，积分时间是有限正实数。在一个实施方式中，积分时间也被归一化，使得其域为[0,1]。例如，一个实施方式将其域离散化为多个值。例如，域被均匀地离散化为五个元素的有限集合TF＝{0.2,0.4,0.6,0.8,1}，并且从该有限集合TF选择t_fi的值。

给定动作集合A和积分时间集合TF，运动基元与对(Ai,tfi)关联。在一些实施方式中，控制动作集合被预定义为

并且积分时间集合被表示为

因此，定义

基元运动，其中第i基元运动对应于对(A_i,t_fi)。

在一些实施方式中，各个动作Ai可具有相同的积分时间。即，TF仅包含一个元素。可相应地定义运动基元。

在另一实施方式中，图形包括用于初始树

426的可扩展节点

列表425和用于目标树

428的另一可扩展节点列表

427。图形初始化345基于目标状态来创建目标节点，并且将初始节点和目标节点分别添加到列表

和

图4C示出根据一个实施方式的节点的数据结构。节点441具有状态441a、节点成本441b、运动基元的碰撞概率441c、运动基元的优先级441d和具有最高优先级的运动基元441e的性质。例如，状态441a描述车辆配置或位置；成本441b描述经过该节点并连接初始状态和目标状态的路径的估计的成本；运动基元的碰撞概率指定所估计的该运动基元导致碰撞的可能性；优先级441d确定哪一基元最有可能导致成本降低。

在一些实施方式中，碰撞概率在介于0和1之间的连续域上取实值；并且优先级概率在有界连续域上取实值。在另一实施方式中，碰撞概率从包含两个元素的有限集合{0,1}|取离散值；并且优先级从包含两个元素的有限集合{0,1}取离散值。

在一些实施方式中，当新子节点通过将基元应用于父节点而生成时，该节点初始从其父节点继承运动基元的碰撞概率和优先级。在通过应用运动基元扩展父节点之后，根据基元的结果来更新所应用的基元的碰撞概率和优先级。例如，如果基元通往新节点并且新节点与父节点之间的路径碰撞，则父节点的基元的碰撞概率增加。在极端情况下，碰撞概率被设定为1。如果基元通往给予低于父节点的成本的新节点，则父节点的基元的优先级增加，否则降低。在一些情况下，根据新节点的成本，优先级被增加至1或降低至0。以这种方式，利用对各个可扩展节点的运动基元的概率估计的确定性方法降低了图形构造的计算复杂度。

在一个实施方式中，运动基元的优先级增加或降低的量与父节点和新节点的成本之间的差成线性比例。此实施方式增加了其它运动基元相对于父运动基元的相对优先级，以更好地考虑确定性图形扩展中的各种选项。

一些实施方式基于这样的认识：确定性图形构造360导致可扩展节点的数量减少。继而，节点扩展的减少增加了所构造的图形的稀疏度，这一方面改进了计算效率，另一方面减少了空间探索的选项。为了解决由确定性节点扩展造成的该问题，一些实施方式在图形的不同部分处变化图形的稀疏度。例如，一个实现方式将潜在可扩展节点的列表维持在运动规划器的存储器中并且基于可扩展节点列表的长度来选择稀疏度的值。例如，稀疏度的值由形成基元运动的控制动作的积分时间和可扩展节点的子节点到图形中的最近节点之间的最小允许距离之一或组合定义。

图5A示出根据一些实施方式的原理的针对图形365的单树

调整图形生长参数401的方法的流程图。根据可扩展节点列表502的长度来调节503定义车辆状态的邻居的稀疏度值。具体地，如果可扩展节点列表

的长度低于特定阈值(下界)，则减小定义到车辆状态的邻居的距离的稀疏度以避免图形构造的过早终止(意味着没有路径可行)。在一些实施方式中，成比例地且递归地减小稀疏度。如果节点长度超过另一阈值(上界)，则成比例地且递归地增大稀疏度。当节点列表的长度介于下界和上界之间时，稀疏度维持恒定。

图5B示出根据一些实施方式的针对双树图形

调整图形生长参数402的方法的流程图。这些实施方式的一些原理类似于图5A的稀疏度调节。只有当两个可扩展节点列表

均为空时，块402才返回失败。各个树可具有生长参数的相同或不同值，这取决于其相应可扩展节点列表的状态。

图5C示出根据一些实施方式的从可扩展节点列表选择402可扩展节点的方法的框图。通常，一些实施方式在可扩展节点列表中选择具有最低估计成本的节点。如果可扩展节点已被完全扩展(已尝试所有控制动作)，则从列表移除523节点，并且该算法应该从新的可扩展节点列表寻找最低成本节点。

图5D示出根据一个实施方式的节点扩展403的框图。利用所选可扩展节点X_best，选择541并应用具有高优先级的控制动作A_i或控制动作的子集

以使用与所选控制动作对应的基元运动来确定542子节点候选X。在一些实施方式中，可通过在预定义时间段[0,t_fi]内以初始条件X_best和所选控制动作A_i对车辆模型进行积分来获得子节点候选。要注意的是，对于不同的控制动作，t_fi可不同。

在一些实施方式中，基元运动指定应用于车辆模型的控制动作A_i和时间段t_fi。基元运动以对(A_i,t_fi)或者从对应控制动作和时间段得到的状态序列的形式存储在存储器中。通过以零初始条件和对应基元运动对车辆运动学或动力学模型进行积分来获得状态序列。

如图5D所示，使用地图信息130，只有当作为X_best和X之间的基元运动的结果的路径无碰撞时，子节点候选X和边缘(X_best,X)才分别被添加到图形的节点集合和边缘集合。子节点X的节点成本被估计为F(X)，并且检查544其是否低于X_best的节点成本。如果是，则节点扩展退出；否则(X_best仍具有最低节点成本的情况)，选择另一控制动作或控制动作的子集以继续节点X_best的扩展。

图5E示出一些实施方式用来404终止迭代图形构造的多个停止标准的框图。在一个实施方式中，进行551测试以检查添加的子节点是否足够接近X_f(到达X_f的预定义邻居)。在另一实施方式中，尝试在添加的子节点与X_f之间构造552标准Reeds-Shepp路径并验证552该Reeds-Shepp路径是否无碰撞。如果是，则该Reeds-Shepp路径被添加到图形的边缘集合，并且图形构造停止。如果节点的数量超过阈值(在这种情况下该方法未能在X₀和X_f之间找到可行路径)，则图形构造可终止。

为了执行树的确定性选择和扩展，一些实施方式通过利用双树图形构造的优势来确定要扩展的节点的成本。在一些实施方式中，节点的成本是从初始节点通过该节点到达目标节点的最小成本，并且包括通过初始树的节点的第一路径的第一成本(考虑初始状态周围的局部信息)、通过目标树的节点的第二路径的第二成本(考虑目标状态周围的局部信息)以及初始树和目标树的节点之间的第三路径的第三成本(考虑初始状态和目标状态之间的关系)。可扩展节点的这种选择允许考虑从两个树的构造搜集的信息，使得图形延伸在计算上更高效。例如，许多实验证明，双树图形构造确定具有比单树图形构造更少的节点的图形。

在一些实施方式中，图形包括两个树

和

其中前者具有根节点X₀，后者具有根节点X_f。在511中另选地选择树

和

进行扩展。即，块511通过交替地选择列表

和列表

来选择可扩展节点列表。在一些实施方式中，对于当前节点X，不同地确定其到达成本(第一成本)和估计的行驶成本(第二成本)。

在一个实施方式中，通过适当定义的函数F(·)向各个节点(状态)X指派成本：

F(X)＝g(X₀,X)+h(X,X_f)，

其中g(X₀,X)表示从初始节点(状态)的到达成本或从X₀到X的第一成本；并且h(X,X_f)表示从X到X_f的估计的行驶成本或第二成本。节点X的F值是在经过该节点的同时从X₀到X_f的潜在路径的估计成本。

在一个实施方式中，到达成本g(X₀,X)对从X₀到X的所有边缘的成本求和，其中边缘的成本是表示X₀和X之间的无碰撞运动学/动力学可行的路径的长度的路径成本l(X₀,X)。在另一实施方式中，到达成本另外包括惩罚沿着从X₀到X的路径控制动作的改变的控制成本c(u(X₀),u(X))，其中u(X)表示通往X的动作。

图6A示出根据一个实施方式的当目标树仅包括目标节点时确定节点X的成本F(X)的方法的示意图。在图6A中，X₀和X_f之间的路径由边缘(X₀,X₁),…,(X_best,X)表示，因此路径成本可由这些边缘的长度精确地确定，即，

l(X₀,X)＝l(X₀,X₁)+…+l(X_best,X)。

类似地，控制成本为

c(u(X₀),u(X))＝c(0,u(X₁))+…+c(u(X_best),u(X))。

在一些实施方式中，行驶成本h(X,X_f)精确地捕获表示X和X_f之间的无碰撞运动学/动力学可行的路径的长度的路径成本l(X,X_f)。由于所构造的图形还未达到目标X_f，所以无法找到将X连接到X_f的一系列边缘，这意味着l(X,X_f)未知。因此，所估计的行驶成本h(X,X_f)代替l(X,X_f)。在一些实现方式中，h(X,X_f)是在未考虑障碍物的情况下表示X和X_f之间的运动学/动力学可行的路径的估计的路径成本l_e(X,X_f)以及考虑障碍物强加于估计的路径成本的影响的碰撞成本l_c(X,X_f)。

例如，假设存在X和X_f之间运动学/动力学可行的路径601及其成本l_e(X,X_f)。事实上，路径601与圆形障碍物103碰撞，因此l_e(X,X_f)不是真实行驶成本。相反，真实行驶成本对应于行驶成本高于l_e(X,X_f)的路径602。l_c(X,X_f)用于近似差l(X,X_f)-l_e(X,X_f)。

在一些实施方式中，根据由|X_f-X|_p表示的向量X-X_f的p范数来计算l_e(X,X_f)。在另一实施方式中，l_e(X,X_f)是X和X_f之间的Reeds-Shepp路径的长度。在一些实施方式中，l(X,X_f)-l_e(X,X_f)是碰撞度量f_c(·)的单调函数。碰撞度量可被构造为一组高斯混合模型，其中各个障碍物的所有顶点用于构造高斯模型。对于节点，其碰撞度量可被评估为f_c(X)。f_c(X)越高，X和X_f之间的中性路径越有可能以碰撞告终。

图6B示出根据一个实施方式的当目标树包括多个节点时确定节点X的成本F(X)的方法的示意图。如图6B所示，扩展X₀导致具有下标s的节点的树

而扩展X_f导致具有下标g的节点的树

假设选择

进行生长，具体地，选择节点X_bestg进行扩展，因为它在

当中具有最低成本。其以控制动作A_g的扩展导致子节点X_g。节点X_g的成本由其到达成本和估计的行驶成本之和给出：

F(x_g)＝g(X_f,X_g)+h(X_g,X₀)。

要注意的是，估计的行驶成本近似于X_g和X₀之间的路径成本。

在一些实施方式中，h(X_g,X₀)如上被近似为l_e(X_g,X₀)和l_c(X_g,X₀)。在另一实施方式中，h(X_g,X₀)近似如下

h(X_g,X₀)＝h(X_g,X_bests)+g(X₀,X_bests)，其中X_bests是列表

中具有最低成本的节点，g(X₀,X_bests)是从X₀到X_bests的到达成本(第三成本)并且确切已知，并且h(X_g,X_bests)＝l_e(X_g,X_bests)+l_c(X_g,X_bests)是X_g和X_bests之间的行驶成本的估计。估计的行驶成本的该定义显著优于先前估计的行驶成本：h(X_g,X₀)＝l_e(X_g,X₀)+l_c(X_g,X₀)，因为前者仅估计X_g和X_bests之间的路径成本。

在一个实施方式中，X_bests是树

的最接近X_g的节点。在另一实施方式中，X_bests是通过求解以下优化问题而获得的节点列表

其中X_nears是如下定义的集合

要注意的是，γ是正有限常数，并且d(X_i,X_j)是任意两个节点X_i和X_j之间的距离函数。X_nears包含

上距X_gγ距离内的所有节点。X_bests表示树

上给出最小估计行驶成本h(X₀,X_g)的节点。

如图6B所示，X_g和X_bests之间的运动学/动力学可行的路径612有更高的概率比X₀和X_g之间的运动学/动力学可行的路径611短(因此可由l_e(X_g,X_bests)更好地近似)，因此X_bests和X_g之间的运动学/动力学可行的路径更不可能与障碍物碰撞，并且提供行驶成本的更好的估计。类似地，树

上的节点X_s的成本可估计如下

F(X_s)＝g(X₀,X_s)+h(X_s,X_bestg)+g(X_bestg,X_f)，

其中X_bestg是列表

中具有最低成本的节点，g(X_bests,X_f)是从X_f到X_bestg的到达成本(第三成本)并且确切已知，并且h(X_s,X_bestg)是从X_s到X_bestg的估计的行驶成本。

在一个实施方式中，X_bestg是树

的最接近Xs的节点。在另一实施方式中，X_bestg是通过求解以下优化问题而获得的节点列表

其中X_nearg是

上的节点的集合，使得

X_nearg包括

上距X_sγ距离内的所有节点。X_bestg表示树

上给出最小估计行驶成本h(X_s,X_f)的节点。

图6C示出例示了不同实施方式的益处的示例的示意图。在这些示例中，车辆需要在包括各种障碍物103的环境内从初始状态101移动到目标状态102。根据仿真，没有优先控制动作的方法导致包括6339个节点的密集且复杂的树621。基于优先运动基元进行确定性节点扩展的实施方式以仅具有2465个节点的更简单的树622实现期望的运动。使用双树构造的实施方式以仅1325个节点的甚至更简单的树623实现期望的运动。

图7A示出例示了根据一些实施方式的与控制动作

和积分时间

的集合关联的基元运动的示意图。给定车辆模型的初始条件X_o701，应用与具有曲率的左转弯移动对应的基元运动751产生子节点711。类似地，应用与不同移动(例如，直线移动或具有不同曲率的转弯移动)对应的基元75i产生子节点71i。所有控制动作

的集合和积分时间

的集合被设计为使得给定可扩展节点的所有节点候选之间的距离至少大于定义图形的稀疏度的特定阈值δ。在一些实施方式中，与状态X_i和X_j对应的两个节点之间的距离函数被定义为向量X_i-X_j：|X_i-X_j|_p的加权p范数。

在一些实施方式中，运动学车辆模型用于基于基元运动生成节点候选，其中基元运动包括控制动作A_i：(s_i,v_i)和积分时间t_fi。具体地，s_i和v_i分别是车辆方向盘的转向角和车辆的纵向速度。在一些实施方式中，

在[-1,1]上均匀分布；并且v_i仅在{-1,+1}上取值。

另选地，动力学车辆模型可用于基于基元运动生成节点候选，其中基元运动包括控制动作A_i：(a_si,a_vi)和积分时间t_fi。具体地，a_si和a_vi分别是车辆方向盘的转向速度和车辆的纵向加速度。

除了节点选择之外，一些实施方式以确定性方式执行节点扩展，以减少以所有可能的基元运动探索节点周围的空间的需求。为此，实施方式选择降低可扩展节点的成本的第一无碰撞基元运动，即，无需测试后续基元运动。为了进一步减轻计算负担，一些实施方式基于各个基元运动与定义通往可扩展节点的图形边缘的基元运动的相似度来对基元运动集合进行排序，并按该顺序测试基元运动。这些实施方式促进人操作者想要的车辆移动的平稳性，并且实际上有助于降低所构造的图形的复杂度。

图7B示出一些实施方式用来扩展节点的基元运动的有序列表720。一些实施方式基于这样的理解：不需要测试所有可能的基元运动，仅一个无碰撞并且降低我们想要扩展的节点的成本。另外，如果一个控制动作降低父节点的成本，则相似控制动作可能也将降低子节点的成本。为此，如果父节点的右前基元运动无碰撞，则有意义的是按照由与通往父节点的控制动作的相似度控制的顺序开始测试运动基元。

为此，一个实施方式从通往可扩展节点的边缘的基元运动开始测试基元运动。另外地或另选地，为了增加图形的空间感知，一个实施方式从包括通往可扩展节点的边缘的基元运动的驱动模式的基元运动开始测试基元运动。例如，驱动模式可以是向前驱动模式或向后驱动模式，其中向前驱动模式包括定义向前方向上的直线移动、右转弯移动和左转弯移动的向前基元运动，向后驱动模式包括定义向后方向上的直线移动、右转弯移动和左转弯移动的向后基元运动。另外地或另选地，驱动模式可以是直线驱动模式或转弯驱动模式，其中，转弯驱动模式包括用于车辆以不同曲率转弯移动的不同基元运动。

图8A示出根据一个实施方式的确定541运动基元或运动基元的子集以扩展所选节点X_best的方法的框图。实施方式根据未尝试运动基元、未尝试运动基元的优先级803和未尝试运动基元的碰撞概率804来选择802运动基元或运动基元的子集。可从节点441c和441d的性质容易地推断信息803和804，如图4C所示。

图8B示出根据另一实施方式的在可扩展节点X_best的扩展期间选择运动基元的方法的框图。实施方式在未尝试运动基元801当中选择812运动基元，使得运动基元与具有最高优先级的运动基元441e之间的距离最小。此规则基于这样的认识：具有最高优先级的运动基元441e更有可能生成具有最低成本的子节点。即：最高优先级被指派给所有尝试的运动基元当中给出最低节点成本的运动基元。在没有尝试的运动基元的情况下，从其父节点继承X_best的具有最高优先级的运动基元441e。

如果节点是树的根节点，则所有运动基元的碰撞概率为零(意味着所有运动基元均被假设为无碰撞)，并且运动基元的优先级441d被初始化为1(意味着所有运动基元均具有相同的优先级)。对于子节点，初始从其父节点继承其优先级和碰撞概率。

在一些实施方式中，根据对其子节点候选的碰撞检查的结果来更新543可扩展节点的运动基元的碰撞概率。类似地，根据从尝试的运动基元得到的节点成本来更新544可扩展节点的运动基元的优先级。例如，在可扩展节点X_best的所有子节点当中，运动基元的优先级被定义为所得子节点的成本的单调函数：子节点的成本越低，运动基元的优先级越高。

图9示出根据一个实施方式的***的示意图。该***包括车辆901，其包括配置用于执行自动停车950的处理器902。该车辆还包括诸如LIDAR 910和/或相机920的至少一个传感器。LIDAR传感器910是低分辨率第一传感器，并且相机920是高分辨率第二传感器。传感器910和/或920在操作上连接到处理器902并且被配置用于感测指示停车空间的至少一部分的几何形状的信息。使用此信息，处理器902确定和/或更新停车空间的地图130。为此，处理器902使用地图130执行自动停车950。

图10示出根据一些实施方式的自动停车***1000的框图。***1000可被实现于车辆901内部。另外地或另选地，***1000可在通信上连接到车辆901。

***1000可包括相机1010、惯性测量单元(IMU)1030、处理器450、存储器1060、收发器1070和显示器/屏幕1080之一或组合，其可在操作上通过连接1020联接到其它组件。连接1020可包括总线、线路、光纤、链路或其组合。

例如，收发器1070可包括能够经由一种或更多种类型的无线通信网络发送一个或更多个信号的发送器以及接收经由一种或更多种类型的无线通信网络发送的一个或更多个信号的接收器。收发器1070可允许基于诸如(但不限于)毫微微小区、可基于IEEE 802.11系列标准的Wi-Fi网络或无线局域网(WLAN)、诸如蓝牙、近场通信(NFC)的无线个域网(WPANS)、基于IEEE 802.15x系列标准的网络和/或诸如LTE、WiMAX等的无线广域网(WWAN)的各种技术与无线网络通信。***1000还可包括用于经由有线网络通信的一个或更多个端口。

在一些实施方式中，***1000可包括诸如CCD或CMOS传感器、激光器和/或相机1010的图像传感器，其以下称为“传感器1010”。例如，传感器1010可将光学图像转换为电或数字图像并且可将获取的图像发送到处理器450。另外地或另选地，传感器1010可感测从场景中的目标对象反射的光并将所捕获的光的强度提交给处理器450。

例如，传感器1010可包括提供“颜色信息”的彩色或灰度相机。如本文所使用的术语“颜色信息”是指颜色和/或灰度信息。通常，如本文所使用的，彩色图像或颜色信息可被视为包括1至N个通道，其中N是取决于用于存储图像的颜色空间的某一整数。例如，RGB图像包括三个通道，红色、蓝色和绿色信息各一个信道。

例如，传感器1010可包括用于提供“深度信息”的深度传感器。可使用深度传感器按各种方式获取深度信息。术语“深度传感器”用于表示可用于独立地和/或结合一些其它相机获得深度信息的功能单元。例如，在一些实施方式中，深度传感器和光学相机可以是传感器1010的一部分。例如，在一些实施方式中，传感器1010包括RGBD相机，当深度传感器被启用时，其除了彩色(RGB)图像之外还可捕获每像素深度(D)信息。

作为另一示例，在一些实施方式中，传感器1010可包括3D飞行时间(3DTOF)相机。在具有3DTOF相机的实施方式中，深度传感器可采取联接到3DTOF相机的频闪灯的形式，其可对场景中的对象照明，并且反射光可由传感器1010中的CCD/CMOS传感器捕获。深度信息可通过测量光脉冲传播到对象并返回到传感器所花费的时间来获得。

作为另一示例，深度传感器可采取联接到传感器1010的光源的形式。在一个实施方式中，光源将结构化或纹理化的光图案(可包括一个或更多个窄带光)投影到场景中的对象上。通过利用由对象的表面形状导致的投影图案的几何失真来获得深度信息。一个实施方式从立体传感器(例如，配准到RGB相机的红外结构光投影仪和红外相机的组合)确定深度信息。

在一些实施方式中，传感器1010包括立体相机。例如，深度传感器可形成无源立体视觉传感器的一部分，其可使用两个或更多个相机来获得场景的深度信息。在捕获的场景中两个相机共同的点的像素坐标可连同相机姿态信息和/或三角测量技术一起用于获得每像素深度信息。

在一些实施方式中，***1000可在操作上连接到多个传感器1010(例如，双前置相机和/或前置和后置相机)，其还可包含各种传感器。在一些实施方式中，传感器1010可捕获静止图像和视频图像二者。在一些实施方式中，传感器1010可包括能够以例如30帧/秒(fps)捕获图像的RGBD或立体视频相机。在一个实施方式中，由传感器1010捕获的图像可为原始未压缩格式，并且可在被处理和/或存储在存储器1060中之前被压缩。在一些实施方式中，可由处理器450使用无损或有损压缩技术来执行图像压缩。

在一些实施方式中，处理器450还可从IMU 1030接收输入。在其它实施方式中，IMU1030可包括3轴加速度计、3轴陀螺仪和/或磁力计。IMU 1030可向处理器450提供速度、取向和/或其它位置相关信息。在一些实施方式中，IMU 1030可与传感器1010的各个图像帧的捕获同步地输出所测量的信息。在一些实施方式中，处理器450部分地使用IMU 1030的输出来融合传感器测量和/或进一步处理融合的测量。

***1000还可包括渲染图像(例如，彩色和/或深度图像)的屏幕或显示器1080。在一些实施方式中，显示器1080可用于显示由传感器1010捕获的实时图像、融合图像、增强现实(AR)图像、图形用户界面(GUI)和其它程序输出。在一些实施方式中，显示器1080可包括和/或容纳有触摸屏以允许用户经由虚拟键盘、图标、菜单或其它GUI、用户手势和/或输入装置(例如，铁笔和其它书写工具)的一些组合输入数据。在一些实施方式中，显示器1080可使用液晶显示(LCD)显示器或发光二极管(LED)显示器(例如，有机LED(OLED)显示器)来实现。在其它实施方式中，显示器1080可以是可穿戴显示器。在一些实施方式中，融合的结果可被渲染在显示器1080上或提交给不同的应用(可在***1000内部或外部)。

示例性***1000还可按照与本公开一致的方式以各种方式进行修改，例如通过添加、组合或省略所示的一个或更多个功能块。例如，在一些配置中，***1000不包括IMU1030或收发器1070。此外，在某些示例实现方式中，***1000包括各种其它传感器(未示出)，例如环境光传感器、麦克风、声传感器、超声传感器、激光测距仪等。在一些实施方式中，***1000的部分采取一个或更多个芯片组等的形式。

处理器450可使用硬件、固件和软件的组合来实现。处理器450可表示可配置为执行与传感器融合有关的计算过程或处理的至少一部分和/或用于进一步处理融合的测量的方法的一个或更多个电路。处理器450从存储器1060检索指令和/或数据。处理器450可使用一个或更多个专用集成电路(ASIC)、中央和/或图形处理单元(CPU和/或GPU)、数字信号处理器(DSP)、数字信号处理器件(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、控制器、微控制器、微处理器、嵌入式处理器核、电子装置、被设计为执行本文所描述的功能的其它电子单元或其组合来实现。

存储器1060可被实现在处理器450内和/或处理器450外部。如本文所使用的，术语“存储器”是指任何类型的长期、短期、易失性、非易失性或其它存储器，不限于任何特定类型的存储器或存储器的数量、或者存储有存储器的物理介质的类型。在一些实施方式中，存储器1060保存方便自动停车的程序代码。

例如，存储器1060可存储传感器的测量，例如静止图像、深度信息、视频帧、程序结果以及由IMU 1030和其它传感器提供的数据。存储器1060可存储存储器，其存储车辆的几何形状、停车空间的地图、车辆的运动学模型和车辆的动力学模型。通常，存储器1060可表示任何数据存储机制。例如，存储器1060可包括主存储器和/或辅存储器。例如，主存储器可包括随机存取存储器、只读存储器等。尽管在图10中示出为与处理器450分离，但是应该理解，主存储器的全部或部分可设置在处理器450内或以其它方式与处理器450共同定位和/或联接。

例如，辅存储器可包括与主存储器和/或一个或更多个数据存储装置或***相同或相似类型的存储器，例如闪存/USB存储器驱动器、存储卡驱动器、盘驱动器、光盘驱动器、磁带驱动器、固态驱动器、混合驱动器等。在某些实现方式中，辅存储器可在操作上接受或以其它方式配置到可移除介质驱动器(未示出)中的非暂时性计算机可读介质。在一些实施方式中，非暂时性计算机可读介质形成存储器1060和/或处理器450的一部分。

本发明的上述实施方式可按众多方式中的任一种来实现。例如，实施方式可使用硬件、软件或其组合来实现。当以软件实现时，软件代码可在任何合适的处理器或处理器集合(无论设置在单个计算机中还是分布于多个计算机)上执行。这些处理器可被实现为集成电路，在集成电路组件中具有一个或更多个处理器。但是，处理器可使用任何合适格式的电路来实现。

另外，本发明的实施方式可被具体实现为一种方法，已提供该方法的示例。作为该方法的一部分执行的动作可按照任何合适的方式排序。因此，可构造以与所示不同的次序执行动作的实施方式，其可包括同时执行一些动作，尽管在例示性实施方式中作为顺序动作示出。

在权利要求中使用诸如“第一”、“第二”的序数词修饰权利要求元素本身并不暗示一个权利要求元素相比于另一权利要求元素的任何优先或次序或者方法动作执行的时间次序，而是仅用作标签以将具有特定名称的一个权利要求元素与具有相同名称(但使用序数词)的另一元素相区分，以区分权利要求元素。

Claims (20)

1.一种用于控制车辆从所述车辆的初始状态到所述车辆的目标状态的移动的***，该***包括：

包括处理器的运动规划器，该处理器被配置为：

构造具有定义所述车辆的状态的多个节点的图形，其中，所述节点包括定义所述车辆的所述初始状态的初始节点和定义所述车辆的所述目标状态的目标节点，其中，所述图形中的各对节点以使所述车辆在所连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接，

确定通过所述图形将所述初始节点与所述目标节点连接的路径，使得该路径定义使所述车辆从所述初始状态移动到所述目标状态的基元运动序列；并且

将所述基元运动序列发送到所述车辆的控制器，该控制器被配置为使用所述基元运动序列来控制所述车辆的移动；

其中，所述运动规划器被配置为通过构造起源于所述初始节点的节点的初始树并且通过构造起源于所述目标节点的节点的目标树来确定所述图形，其中，为了构造所述初始树或所述目标树，所述运动规划器被配置为基于可扩展节点的成本在所述初始树或所述目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到所述可扩展节点的子节点来扩展所述图形，使得所述子节点的成本小于所述可扩展节点的成本，其中，节点的成本是从所述初始节点通过所述节点到达所述目标节点的最小成本，并且包括通过所述初始树的节点的第一路径的第一成本、通过所述目标树的节点的第二路径的第二成本以及所述初始树和所述目标树的节点之间的第三路径的第三成本，

所述基元运动由一对所述车辆的控制动作以及所述控制动作的积分时间表示，或者通过将所述对应用于车辆模型而得到的状态序列表示。

2.根据权利要求1所述的***，其中，所述节点的成本指示所述车辆从所述初始状态通过所述节点的状态到所述目标状态的行驶时间、所述车辆从所述初始状态通过所述节点的状态行驶到所述目标状态的距离中的一个或组合。

3.根据权利要求1所述的***，其中，为了选择所述可扩展节点，所述运动规划器被配置为按照其相应成本的顺序测试多个节点，并且选择所测试的节点中的具有降低所选节点的成本的所述无碰撞基元运动的第一个节点作为所述可扩展节点。

4.根据权利要求1所述的***，其中，为了扩展所述可扩展节点，所述运动规划器被配置为：

基于基元运动的集合中的各个基元运动与定义通往所述可扩展节点的所述图形的边缘的基元运动的相似度来对所述集合进行排序，以生成基元运动的有序集合；

从基元运动的所述有序集合选择降低所述可扩展节点的成本的第一无碰撞基元运动；并且

利用所述第一无碰撞基元运动来扩展所述图形。

5.根据权利要求4所述的***，其中，所述运动规划器从通往所述可扩展节点的边缘的基元运动开始测试所述基元运动。

6.根据权利要求4所述的***，其中，所述运动规划器从包括通往所述可扩展节点的边缘的基元运动的驱动模式的基元运动开始测试所述基元运动，其中，所述驱动模式是向前驱动模式、向后驱动模式、直线驱动模式或转弯驱动模式。

7.根据权利要求4所述的***，其中，所述运动规划器从具有最高优先级的所述可扩展节点的基元运动开始测试所述基元运动。

8.根据权利要求7所述的***，其中，如果所述基元运动通往成本低于所述可扩展节点的新子节点，则所述可扩展节点的所述基元运动的所述优先级增加；否则，所述可扩展节点的所述基元运动的所述优先级降低。

9.根据权利要求8所述的***，其中，所述优先级的增加或减少与所述可扩展节点和所述新子节点的成本差异线性成比例。

10.根据权利要求7所述的***，其中，如果所述基元运动通往成本低于所述可扩展节点的新子节点，则所述可扩展节点的所述基元运动的所述优先级被设定为1；否则，所述可扩展节点的所述基元运动的所述优先级被设定为比所述1的所述优先级低的0。

11.根据权利要求1所述的***，其中，所述图形的所述节点非均匀地分布，在所述图形的不同部分处具有不同的稀疏度的值。

12.根据权利要求11所述的***，其中，所述运动规划器基于可扩展节点的列表的长度来选择所述稀疏度的值，其中，所述稀疏度的值由形成所述基元运动的控制动作的积分时间和所述子节点到所述图形中的最近节点之间的最小允许距离中的一个或组合来定义。

13.根据权利要求1所述的***，其中，通过从所述目标树的可扩展节点列表选择其端节点来确定所述第二路径，其中，所述端节点具有最低成本。

14.根据权利要求1所述的***，其中，通过从所述目标树选择其端节点来确定所述第二路径，其中，所述端节点最靠近所述第一路径的端节点。

15.根据权利要求1所述的***，其中，通过从所述目标树选择其端节点来确定所述第二路径，使得所述第二成本和第三成本之和最小。

16.根据权利要求1所述的***，其中，当所述初始树和所述目标树以Reed-Shepp路径无碰撞地连接时，所述运动规划器停止构造所述图形。

17.根据权利要求1所述的***，其中，所述目标状态定义所述车辆的停车空间。

18.根据权利要求1所述的***，该***还包括：

导航***，该导航***包括至少一个传感器以估计所述车辆的所述初始状态和所述车辆的所述目标状态。

19.一种用于控制车辆从所述车辆的初始状态到所述车辆的目标状态的移动的方法，其中，该方法使用与所存储的实现该方法的指令联接的处理器，其中，所述指令在由所述处理器执行时执行该方法的步骤，该方法包括以下步骤：

构造具有定义所述车辆的状态的多个节点的图形，其中，所述节点包括定义所述车辆的初始状态的初始节点和定义所述车辆的目标状态的目标节点，其中，所述图形中的各对节点以使所述车辆在所连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接，其中，所述图形包括起源于所述初始节点的节点的初始树和起源于所述目标节点的节点的目标树，其中，为了构造所述初始树或所述目标树，所述处理器被配置为基于可扩展节点的成本在所述初始树或所述目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到所述可扩展节点的子节点来扩展所述图形，使得所述子节点的成本小于所述可扩展节点的成本，其中，节点的成本是从所述初始节点通过所述节点到达所述目标节点的最小成本，并且包括通过所述初始树的节点的第一路径的第一成本、通过所述目标树的节点的第二路径的第二成本以及所述初始树和所述目标树的节点之间的第三路径的第三成本；

确定通过所述图形将所述初始节点与所述目标节点连接的路径，使得该路径定义使所述车辆从所述初始状态移动到所述目标状态的基元运动序列；以及

将所述基元运动序列发送到所述车辆的控制器，该控制器被配置为使用所述基元运动序列来控制所述车辆的移动，

所述基元运动由一对所述车辆的控制动作以及所述控制动作的积分时间表示，或者通过将所述对应用于车辆模型而得到的状态序列表示。

20.一种具体实现有程序的非暂时性计算机可读存储介质，该程序能够由处理器执行以用于执行一种方法，该方法包括以下操作：

构造具有定义车辆的状态的多个节点的图形，其中，所述节点包括定义所述车辆的初始状态的初始节点和定义所述车辆的目标状态的目标节点，其中，所述图形中的各对节点以使所述车辆在所连接的节点的状态之间移动的无碰撞基元运动之一所定义的边缘连接，其中，所述图形包括起源于所述初始节点的节点的初始树和起源于所述目标节点的节点的目标树，其中，为了构造所述初始树或所述目标树，所述处理器被配置为基于可扩展节点的成本在所述初始树或所述目标树中选择可扩展节点，并且通过添加以无碰撞基元运动所定义的边缘连接到所述可扩展节点的子节点来扩展所述图形，使得所述子节点的成本小于所述可扩展节点的成本，其中，节点的成本是从所述初始节点通过所述节点到达所述目标节点的最小成本，并且包括通过所述初始树的节点的第一路径的第一成本、通过所述目标树的节点的第二路径的第二成本以及所述初始树和所述目标树的节点之间的第三路径的第三成本；

确定通过所述图形将所述初始节点与所述目标节点连接的路径，使得该路径定义使所述车辆从所述初始状态移动到所述目标状态的基元运动序列；以及

将所述基元运动序列发送到所述车辆的控制器，该控制器被配置为使用所述基元运动序列来控制所述车辆的移动，

所述基元运动由一对所述车辆的控制动作以及所述控制动作的积分时间表示，或者通过将所述对应用于车辆模型而得到的状态序列表示。

Images