CN111208814A - 用于自动车辆的、利用动态模型的、基于记忆的最优运动规划 - Google Patents

Abstract

描述了用于车辆的运动规划的方法和装置。运动规划器使用包括在运动图树中的先前运动图数据和查找表(LUT)来生成(一个或多个)候选轨迹。在终止条件时，利用与(一个或多个)候选轨迹相关联的运动图数据来更新运动图树。从候选轨迹中选择轨迹，并且利用该轨迹更新控制器以控制车辆。路径规划器使用配置图树中的先前配置图数据来生成(一个或多个)候选路径。在终止条件时，利用与(一个或多个)候选路径相关联的配置图数据来更新配置图树。从(一个或多个)候选路径中选择路径。速度规划器算法根据路径确定速度。LUT用于辅助速度确定。利用路径和速度更新控制器以控制车辆。

Description

用于自动车辆的、利用动态模型的、基于记忆的最优运动规划

相关申请的交叉参考

本申请要求2018年11月16日提交的美国临时申请No.62/768,425和2018年12月21日提交的美国专利申请No.16/230,094的权益，它们的全部公开内容通过引用并入本申请。

技术领域

本公开涉及自动驾驶车辆。更具体地，本公开涉及用于自动驾驶车辆的控制器、控制器系统和控制器方法。

背景技术

在常见的自动驾驶车辆(AV)系统中所使用的运动规划算法具有很强的局限性，在复杂的城市道路环境下并不能够快速的生成一条优化的可行驶的轨迹。因此期望有新的运动规划方案以解决这样的问题。

发明内容

本申请公开了基于记忆的运动规划的实现。基于记忆的运动规划可以包括一个或多个动态模型。

描述了用于车辆的运动规划的方法和装置。该方法包括初始化运动图树。运动规划器使用包括在运动图树中的先前运动图数据和查找表(Look-up Table，LUT)来生成(一个或多个)候选轨迹。当终止条件已经发生时，利用与(一个或多个)候选轨迹相关联的运动图数据来更新运动图树。从(一个或多个)候选轨迹中选择轨迹，并且利用该轨迹更新控制器以控制车辆。一种方法可以包括初始化配置图树。路径规划器使用配置图树中的先前配置图数据来生成(一个或多个)候选路径。当终止条件已经发生时，利用与(一个或多个)候选路径相关联的配置图数据来更新配置图树。从(一个或多个)候选路径中选择路径。速度规划器算法被应用于该路径并且使用LUT来确定速度。利用该路径和该速度更新控制器以控制车辆。

一种方法可用于车辆的运动规划。车辆可以是自动驾驶车辆(AV)并且可以被称为主机车辆。一种用于自动驾驶车辆(AV)中的运动规划的方法可以包括初始化图。该方法可以包括确定是否已经发生终止条件。该方法可以包括在终止条件已经发生的情况下，存储图数据并搜索图。在终止条件尚未发生的情况下，该方法可包括针对图中的每个开放顶点(open vertex)以及针对控制集中的每个控制，将相应控制应用于相应开放节点以获得新状态。该方法可包括确定从开放顶点到新状态的运动是否有效。在运动有效的情况下，该方法可以包括将新的顶点和边添加到图。该方法可以包括采样目标。该方法可以包括选择图中的顶点。该方法可以包括计算从运动/配置状态到运动/配置状态目标的运动的控制输入。该方法可以包括获得新状态。在新状态有效的情况下，该方法可以包括将新状态作为新的顶点和新的边添加到图。

AV控制器可包括运动规划组件。运动规划组件可以被配置为：初始化图树；确定终止条件是否已经发生；以及在已经发生终止条件的情况下，存储图数据并搜索图。运动规划组件还可被配置成在终止条件尚未发生的情况下，针对树中的每个开放顶点以及针对控制集中的每个控制，将相应控制应用于相应开放节点以获得新状态。运动规划组件还可被配置成确定从开放顶点到新状态的运动是否有效。在运动有效的情况下，运动规划组件可以被配置为将新的顶点添加到树。运动规划组件还可被配置为将所存储的图数据添加到当前图数据。运动规划组件还可以被配置为：采样目标；选择图中的顶点；计算从顶点的运动状态到目标的运动的控制输入；获得新状态；以及在新状态有效的情况下，将新状态作为新的顶点和新的边添加到图。

附图说明

当结合附图阅读时，根据以下详细描述可以最佳理解本公开。需要强调的是，根据惯例，附图的各种特征不是按比例的。相反，各种特征的尺寸为了清楚而任意放大或缩小。

图1是根据本公开的实施例的车辆的示例的图；

图2是图1所示的控制系统的示例的图；

图3是根据本公开的实施例的车辆控制系统的示例的图；

图4是根据本公开的实施例的包括车辆控制系统的车辆的侧视图的示例的图；

图5是根据本公开的实施例的车辆控制系统的示例的图；

图6是根据本公开的实施例的自动驾驶车辆运动规划方法的示例的图；

图7是根据本公开的实施例的数值优化方法的示例的图；

图8是根据本公开的实施例的基于模型预测控制的方法的示例的图；

图9是根据本公开的实施例的基于采样中的网格规划器的方法的示例的图；

图10A-10M是根据本公开的实施例的示例网格规划器图序列的图；

图11是根据本公开的实施例的基于采样中的快速扩展随机树(RRT)的方法的示例的图；

图12A-12O是根据本公开的实施例的示例的基于采样的规划器图序列的图；

图13是根据本公开的实施例的路径速度分解方法的示例的图；

图14是根据本公开的实施例的路径速度分解方法中的RRT路径规划器的示例的图；

图15是根据本公开的实施例的路径速度分解方法中的网格规划器的示例的图；

图16是根据本公开的实施例的路径速度分解方法中的速度规划的示例的图。

具体实施方式

现在更详细参照本发明的优选实施方式，其示例在附图中示出。如可能，在附图和说明书中使用相同的附图标记来指代相同或相似的部分。

这里使用的术语“计算机”或“计算设备”包括能够执行本申请公开的任意方法或方法的任意一个或多部分的任意单元或单元组合。

这里使用的术语“处理器”指示一个或多个处理器，例如一个或多个专用处理器、一个或多个数字信号处理器、一个或多个微处理器、一个或多个控制器、一个或多个微控制器、一个或多个应用处理器、一个或多个中央处理单元(CPU)、一个或多个图处理单元(GPU)、一个或多个数字信号处理器(DSP)、一个或多个专用集成电路(ASIC)、一个或多个专用标准产品、一个或多个场可编程门阵列、任意其他类型的集成电路或组合、一个或多个状态机、或这些的任意组合。

这里使用的术语“存储器”指示能够包含、存储、通信或传输可以被任意处理器使用或结合任意处理器使用的任意信号或信息的任意计算机可使用或计算机可读的介质或设备。例如，存储器可以是一个或多个只读存储器(ROM)、一个或多个随机存取存储器(RAM)、一个或多个寄存器、低功率双数据率(LPDDR)存储器、一个或多个缓存存储器、一个或多个半导体存储器设备、一个或多个磁性介质、一个或多个光介质、一个或多个磁光介质、或这些的任意组合。

这里使用的术语“指令”可以包括用于执行本申请公开的任意方法或方法的任意一个或多个部分的指示或表达，并可以被实现为硬件、软件或这些的任意组合。例如，指令可以被实施为信息，例如计算机程序，其存储在存储器中可以由处理器执行以执行本申请描述的各自的方法、算法、方面或这些任意的组合。指令或其部分可以被实施为专用处理器或电路，其可以包括用于执行本申请描述的方法、算法、方面或这些的组合的任意的专用硬件。在一些实施中，指令的部分可以被分配在单个设备、多个设备上的多个处理器，可以直接通信或在网络(例如局域网、广域网、因特网或这些的组合)上通信。

这里使用的术语“确定”和“识别”或这些的任意变体包括选择、确认、计算、查找、接收、确定、建立、得到或其他以无论什么使用本申请示出并描述的设备和方法的一者或多者的任意方式识别或确定。

这里使用的术语“示例”、“实施方式”、“实施”、“方面”、“特征”或“元素”指示用作示例、实例或例证。除非另有指明，任何示例、实施方式、实施、方面、特征或元素与其他示例、实施方式、实施、方面、特征或元素彼此独立并可以与任意其他示例、实施方式、实施、方面、特征或元素组合使用。

这里使用的术语“或”旨在表示包括性的“或”而不是排他性的“或”。也就是说，除非另有指明，或从上下文中明确的，“X包括A或B”是旨在指示自然包容性的排列项中的任意。也就是说，如果X包括A；X包括B；或X包括A和B，那么在上述实例的任意的情况下满足“X包括A或B”。此外，在本申请和附加的权利要求中使用的冠词“一”应当一般理解为“一个或多个”，除非另有指明或从上下文中清楚指向为单数形式。

此外，为了简化说明，虽然这里的附图和描述可以包括步骤或阶段的序列或系列，但是本申请公开的方法的元素可以以不同顺序或同时发生。此外，本申请公开的方法的元素可以在有本申请没有明确存在或描述的其他元素的情况下发生。此外，可以不需要本申请描述的方法的所有元素来实施根据本公开的方法。虽然在本申请中以特定组合描述了方面、特征和元素，但是每个方面、特征或元素可以独立被使用或者以与或不与其他方面、特征和元素的各种组合的方式被使用。

自动驾驶车辆(AV)是一种成熟的技术，其具有通过增强汽车运输的安全性、可达性、效率性和便利性来改造机动性的潜力。可由AV执行的安全关键任务包括穿过与其他车辆和行人共享的动态环境的运动规划，以及该运动规划经由反馈控制的稳健执行。AV可以包括处理器，该处理器包括运动规划层。运动规划层可以被配置为计算从车辆的当前配置到由决策制定层级的行为层提供的目标配置的安全、舒适和动态可行的轨迹。取决于上下文，目标配置可以不同。例如，目标位置可以是沿行驶方向而向前数米的当前车道的中心点、下一交叉口处的停止线的中心或者下一期望的停车点。控制器可以包括运动规划组件，其接收关于车辆周围的静态和动态障碍物的信息，并且生成满足对车辆运动的动态和运动学约束的无冲突轨迹。运动规划组件可以计算对于控制器执行来说安全且舒适的轨迹。所计算的轨迹可以基于定位、车辆状态(位置、速度、加速度、底盘)、地图、路线、感知、预测或它们的任何组合。AV运动规划的一个方面是处理时间的限制。运动规划必须实时运行以对环境变化具有快速响应，从而确保安全。

为了增加运动规划的速度，本申请公开的实施例可以应用用于动态计算的先前规划数据、查找表或这两者。对于AV运动规划，每个规划周期中的环境变化相对较小。不是丢弃所有先前规划周期计算，而是可以配置新的规划周期以利用该信息来减少处理时间。利用先前规划数据的方法可以取决于规划器的类型。另一计算成本时间是计算车辆的动态运动。通过离线地预先计算动态运动，并且在需要时使用查找表(LUT)，可以减少处理时间，然而，这可能牺牲在效率和计算之间折衷的计算精度。

图1是根据本公开的实施例的车辆1000的示例的图。车辆1000可以是自动驾驶车辆(AV)或半自动驾驶车辆。如图1所示，车辆1000包括控制系统1010。控制系统1010可以称为控制器。控制系统1010包括处理器1020。处理器1020被编程以命令应用到预定转向力矩值和应用到预定净非对称制动力值中的一者。每个预定力被选择以实现预定车辆横摆力矩，其至多是由转向系统1030导致的第一最大横摆力矩和由制动系统导致的第二最大横摆力矩中的较小者。

转向系统1030可以包括转向致动器1040，其是电动助力转向致动器。制动系统可以包括耦合到车辆1000的各自车轮1060的一个或多个制动器1050。此外，处理器1020可以被编程以命令制动系统通过使每个制动器1050施加一个不同于其他制动器1050的制动力来实现净非对称制动力。

处理器1020还可以被编程以命令制动系统响应当转向系统1030的故障时产生制动力，例如净非对称制动力。附加或可替换地，处理器1020可以被编程以响应于转向系统1030的故障向驾乘人员提供报警。转向系统1030可以是电动转向控制模块。控制系统1010可以包括转向系统1030。此外，控制系统1010可以包括制动系统。

转向系统1030可以包括是电动助力转向的转向致动器1040。制动系统可以包括耦合到车辆1000相对两侧的各自的车轮1060的两个制动器1050。此外，方法可以包括命令制动系统通过应用不同制动力的每个制动器1050来应用净非对称制动力。

如果转向系统1030和制动系统中的一者在车辆1000执行转弯时故障，则控制系统1010允许转向系统1030和制动系统中的另一者接管转向系统1030和制动系统中的一者。转向系统1030和制动系统不管哪一个保持可操作，都能够将足够的横摆力矩应用到车辆1000以继续转弯。车辆1000因此不太可能影响物体，例如另一车辆或道路障碍，且车辆1000的任何驾乘人员不太可能受伤。

车辆1000可以在一个或多个自动驾驶车辆操作等级操作。为了本公开的目的，自动模式被定义为车辆1000的驱动力(例如经由包括电机和/或内燃机的动力总成)、制动以及转向的每一者由处理器1020控制的一种模式；在半自动模式中，处理器1020控制车辆1000的驱动力、制动和转向的一者或两者。因此，在一个示例中，非自动操作模式可以涉及SAE等级0-1，部分自动或半自动操作模式可以涉及SAE等级2-3，以及全自动操作模式可以涉及SAE等级4-5。

参考图2，控制系统1010包括处理器1020。处理器1020被包括在车辆1000中用于执行包括如本申请所述的各种操作。处理器1020是一般包括处理器和存储器的计算设备，存储器包括一种或多种形式的计算机可读介质，以及存储处理器可执行的用于执行包括如本申请公开的各种操作的指令。处理器1020的存储器一般还存储经由各种通信机制接收的远程数据；例如，处理器1020一般被配置用于车辆1000内的通信网络上的通信。处理器1020还可以具有到车载诊断连接器(OBD-II)的连接。虽然为了便于图示，在图2中示出了一个处理器1020，可以理解处理器1020可以包括一个或多个计算设备，且该一个或多个计算设备可以执行本申请描述的各种操作。处理器1020可以是控制模块，例如电动转向控制模块，或可以包括其他计算设备的控制模块。

控制系统1010可以通过通信网络传送信号，该通信网络可以是控制器局域网(CAN)总线、以太网、局域互联网络(LIN)、蓝牙和/或通过任意其他有线或无线通信网络。处理器1020可以与动力系统2010、转向系统1030、制动系统2020、传感器2030和/或用户界面2040还有其他组件通信。

继续参考图2，车辆1000的动力系统2010生成能量并将能量转换成车辆1000的运动。动力系统2010可以是已知的车辆动力子系统，例如常规的动力总成，其包括耦合到传动装置(transmission)的内燃机，传动装置将转动传递到车轮1060；电力动力总成，包括电池、电机以及将驱动力传递到车轮1060的传动装置；混合动力总成，包括传统动力总成和电力动力总成的元件；或任意其他类型的驱动系统。动力系统2010与处理器1020通信并从处理器1020和驾驶员接收输入。驾驶员可以经由例如油门踏板和/或变速杆(未示出)控制动力系统2010。

参考图1和2，转向系统1030典型地是已知的车辆转向子系统并控制车轮1060的转弯。转向系统1030与方向盘1070和处理器1020通信并从中接收输入。转向系统1030可以是具有经由转向致动器1040的电动辅助转向的齿条齿轮系统、线控转向系统(这两者都是本领域已知的)或任意其他合适的系统。转向系统1030可以包括安装到耦合到转向齿条1090的转向柱1080的方向盘1070。

参考图1，转向齿条1090可调地耦合到车轮1060，例如以铰接四连杆的方式。转向齿条1090的平移运动致使车轮1060转向。转向柱1080可以经由齿条齿轮耦合到转向齿条1090，也就是说，齿轮与齿条(未示出)之间的啮合。

转向柱1080将方向盘1070的转动传递到转向齿条1090的运动。转向柱1080可以是例如将方向盘1070连接到转向齿条1090的轴。转向柱1080可以内安装扭矩传感器和离合器(未示出)。

方向盘1070允许操作员通过将方向盘1070的转动传送到转向齿条1090的运动来操纵车辆1000。方向盘1070可以是例如固定附着到转向柱1080的刚性圈，例如已知的方向盘。

继续参考图1，转向致动器1040耦合到转向系统1030，例如转向柱1080，以致使车轮1060转向。例如，转向致动器1040可以是可转动耦合到转向柱1080的电机，也就是说耦合以能够将转向力矩应用到转向柱1080。转向致动器1040可以与处理器1020通信。

转向致动器1040可以给转向系统1030提供助力。换句话说，转向致动器1040可以在驾驶员转动方向盘1070的方向提供力矩，允许驾驶员使用更少的力量转动方向盘1070。转向致动器1040可以是电动辅助转向致动器。

参考图1和图2，制动系统2020典型地是已知的车辆制动子系统并阻碍车辆1000的运动以由此减速和/或停止车辆1000。制动系统2020包括耦合到车轮1060的制动器1050。制动器1050可以是摩擦制动器，例如盘式制动器、鼓式制动器、带式制动器等；可以是再生制动器；可以是任意其他合适类型的制动器；或可以是这些的组合。制动器1050可以耦合到例如车辆1000的相对两侧的车轮1060。制动系统2020与处理器和驾驶员通信并从处理器1020和驾驶员接收输入。驾驶员可以经由例如制动踏板(未示出)控制制动。

参考图2，车辆1000可以包括传感器2030。传感器2030可以检测车辆1000的内部状态，例如轮转速、轮朝向以及发动机和传动变量。传感器2030可以检测车辆1000的位置或朝向，例如全球定位系统(GPS)传感器；加速度计，例如压电或微机电系统(MEMS)；陀螺仪，例如速率、环形激光器或光纤陀螺仪；惯性测量单元(IMU)；以及磁力计。传感器2030可以检测外部世界，例如雷达传感器、扫描激光测距仪、光检测与测距(LIDAR)设备以及图像处理传感器(例如摄像头)。传感器2030可以包括通信设备，例如车到基础设施(V2I)设备、车到车(V2V)设备或车到一切(V2X)设备。

用户界面2040向车辆1000的驾乘人员呈现信息并从驾乘人员接收信息。用户界面2040可以位于例如车辆1000的客舱(未示出)中的仪表盘上，或驾乘人员容易看到的任何地方。用户界面2040可以包括刻度盘、数字读出器、屏幕、扬声器等以用于输出，即提供信息给驾乘人员，例如包括诸如已知的元件的人机界面(HMI)。用户界面2040可以包括按键、把手、键盘、触摸屏、麦克风等以用于接收来自驾乘人员的输入，即信息、指令等。

图3是根据本公开的实施例的车辆控制系统3000的示例的图。车辆控制系统3000可以包括依据特定实施的要求的各种组件。在一些实施例中，车辆控制系统3000可以包括处理单元3010、图像获取单元3020、位置传感器3030、一个或多个存储器单元3040、3050、地图数据库3060、用户界面3070、以及无线收发信机3072。处理单元3010可以包括一个或多个处理设备。在一些实施例中，处理单元3010可以包括应用处理器3080、图像处理器3090或任意其他合适的处理设备。类似地，图像获取单元3020依据特定应用的要求可以包括任意数量的图像获取设备和组件。在一些实施例中，图像获取单元3020可以包括一个或多个图像捕获设备(例如，摄像头、CCD或任意其他类型的图像传感器)，例如图像捕获设备3022、图像捕获设备3024以及图像捕获设备3026。系统3000还可以包括通信地将处理单元3010连接到图像获取单元3020的数据接口3028。例如，数据接口3028可以包括任意一个或多个有线和/或无线链路，以用于将图像获取单元3020获取的图像数据传送到处理单元3010。

无线收发信机3072可以包括一个或多个设备，被配置成通过使用射频、红外频率、磁场或电场来通过空中接口将传输交换到一个或多个网络(例如蜂窝网络、因特网等)。无线收发信机3072可以使用已知的标准来传送和/或接收数据(例如，Wi-Fi、蓝牙、蓝牙智能、802.15.4、ZigBee等)。这种传输可以包括从主机车辆到一个或多个位于远程的服务器的通信。这种传输还可以包括主机车辆与在主机车辆环境中的一个或多个目标车辆之间的通信(单向或双向)(例如，以促进主机车辆相对于主机车辆环境中的目标车辆的导航协调或与目标车辆一起的导航协调)，或甚至是至进行传输的车辆附近的未指定接收方的广播传输。

应用处理器3080和图像处理器3090两者可以包括各种类型的基于硬件的处理设备。例如，应用处理器3080和图像处理器3090的任一者或两者可以包括微处理器、预处理器(例如图像预处理器)、图处理器、中央处理单元(CPU)、支持电路、数字信号处理器、集成电路、存储器、或适用于运行应用和用于图像处理和分析的任意其他类型的设备。在一些实施例中，应用处理器180和/或图像处理器190可以包括任意类型的单核或多核处理器、移动设备微控制器、中央处理单元等。

在一些实施例中，应用处理器3080和/或图像处理器3090可以包括具有本地存储器和指令集的多个处理单元。这样的处理器可以包括用于从多个图像传感器接收图像数据的视频输入，并还可以包括视频输出能力。在一个示例中，处理器可以使用工作在332Mhz的90纳米-微米(nm-micron)技术。

本申请公开的任意处理设备可以被配置成执行某些功能。配置处理设备(例如描述的处理器、其他控制器或微处理器中的任意)来执行某些功能可以包括对计算机可执行指令编程并使得这些指令可用于在处理设备操作期间供处理设备执行。在一些实施例中，配置处理设备可以包括使用架构指令直接对处理设备编程。在其他实施例中，配置处理设备可以包括在处理设备在操作期间可访问的存储器上存储可执行指令。例如，处理设备在操作期间可以访问处理器以得到并执行存储的指令。在任一情况中，被配置成执行本申请公开的感测、图像分析和/或导航功能的处理设备代表基于专用硬件的系统，其控制主机车辆的多个基于硬件的组件。

虽然图3示出了在处理单元3010中包含两个分开的处理设备，但是可以使用更多或更少的处理设备。例如，在一些实施例中，单个处理设备可以用于实现应用处理器3080和/或图像处理器3090的任务。在其他实施例中，这些任务可以由两个以上的处理设备执行。此外，在一些实施例中，车辆控制系统3000可以包括一个或多个处理单元3010，不包括其他组件，例如图像获取单元3020。

处理单元3010可以包括各种类型的设备。例如，处理单元3010可以包括各种设备，例如控制器、图像预处理器、中央处理单元(CPU)、支持电路、数字信号处理器、集成电路、存储器或用于图像处理和分析的任意其他类型的设备。图像预处理器可以包括视频处理器，用于捕获、数字化并处理来自图像传感器的影像。CPU可以包括任意数量的微控制器或微处理器。支持电路可以是一般本领域公知的任意数量的电路，包括缓存器、电源、时钟和输入输出电路。存储器可以存储软件，该软件在被处理器执行时控制系统操作。存储器可以包括数据库和图像处理软件。存储器可以包括任意数量的随机存储器、只读存储器、闪存、磁盘驱动器、光存储器、带存储器、可移除存储器以及其他类型的存储器。在一种实例中，存储器可以与处理单元3010分开。在其他实例中，存储器可以集成到处理单元3010。

每个存储器3040、3050可以包括软件指令，其在被处理器(例如应用处理器3080和/或图像处理器3090)执行时可以控制车辆控制系统3000的各种方面的操作。举例而言，这些存储器单元可以包括各种数据库和图像处理软件以及训练系统，例如神经网络或深度神经网络。存储器单元可以包括随机存取存储器、只读存储器、闪存、磁盘驱动器、光存储器、带式存储器、可移除存储器和/或任意其他类型的存储器。在一些实施例中，存储器单元3040、3050可以与应用处理器3080和/或图像处理器3090分开。在其他实施例中，这些存储器单元可以集成到应用处理器3080和/或图像处理器3090。

位置传感器3030可以包括适用于确定与车辆控制系统3000的至少一个组件相关联的位置的任意类型的设备。在一些实施例中，位置传感器3030可以包括GPS接收器。这样的接收器能够通过处理全球定位系统卫星广播的信号来确定用户位置和速度。来自位置传感器3030的位置信息对于应用处理器3080和/或图像处理器3090可用。

在一些实施例中，车辆控制系统3000可以包括例如用于测量车辆1000的速度的速度传感器(例如速度计)的组件。车辆控制系统3000还可以包括用于沿一个或多个轴测量车辆1000的加速度的一个或多个加速度计(单轴或多轴)。

存储器单元3040、3050可以包括数据库，或以任意其他形式组织的数据，其指示已知地标的位置。环境的传感信息(例如图像、雷达信号、来自激光雷达或两个或更多图像的立体处理的深度信息)可以与位置信息(例如GPS坐标、车辆自主运动等)一起被处理以确定相对于已知地标的车辆当前位置，并修正该车辆位置。

用户界面3070可以包括适用于提供信息给车辆控制系统3000的一个或多个用户或从一个或多个用户接收输入的任意设备。在一些实施例中，用户界面3070可以包括用户输入设备，包括例如，触摸屏、麦克风、键盘、指针设备、轨迹轮、摄像头、把手、按键等。使用这样的输入设备，用户能够通过键入指令或信息，提供语音命令，使用按键、指针或眼跟踪能力选择屏幕上的菜单选项，或通过用于将信息传递到车辆控制系统3000的任意其他合适的技术来向车辆控制系统3000提供信息输入或命令。

用户界面3070可以被装配有一个或多个处理设备，该处理设备被配置成提供信息给用户和从用户接收信息，并处理该信息以供例如应用处理器3080使用。在一些实施例中，这样的处理设备可以执行用于识别和跟踪眼部运动，接收和解译语音命令，识别和解译在触摸屏上进行的触摸和/或手势，响应于键盘输入或菜单选择等的指令。在一些实施例中，用户界面3070可以包括显示器、扬声器、触觉设备和/或用于提供输出信息给用户的任意其他设备。

地图数据库3060可以包括用于存储有用于车辆控制系统3000的地图数据的任意类型的数据库。在一些实施例中，地图数据库3060可以包括关于在参考坐标系中各种项的位置的数据，包括道路、水景、地理特征、商业、兴趣点、餐厅、加油站等。地图数据库3060不仅可以存储这些项的位置，还可以存储关于这些项的描述符，包括例如与任意存储的特征相关联的名称。在一些实施例中，地图数据库3060在物理上可以位于车辆控制系统3000的其他组件。可替换地或附加地，地图数据库3060或其部分可以相对于车辆控制系统3000的其他组件(例如处理单元3010)位于远程。在这样的实施例中，来自地图数据库3060的信息可以通过到网络的有线或无线数据连接(例如通过蜂窝网络和/或因特网等)被下载。在一些情况中，地图数据3060可以存储稀疏数据模型，包括某些道路特征(例如车道标记)或主机车辆的目标轨迹的多项式表示。地图数据库3060还可以包括存储的各种识别的地标的表示，其可以用于确定或更新关于目标轨迹的主机车辆的已知位置。地标表示可以包括数据字段，例如地标类型、地标位置以及其他可能的标识符。

图像捕获设备3022、3024以及3026每一个可以包括适用于从环境捕获至少一个图像的任意类型的设备。此外，任意数量的图像捕获设备可以用于获取用于输入到图像处理器的图像。一些实施例可以包括仅单个图像捕获设备，而其他实施例可以包括两个、三个或甚至四个或更多的图像捕获设备。下面将参考图4进一步描述图像捕获设备3022、3024和3026。

一个或多个摄像头(例如图像捕获设备3022、3024和3026)可以是包括在车辆上的感测块的部分。各种其他传感器可以被包括在感测块中，且可以依赖任意或所有这些传感器来开发车辆的感测导航状态。除了摄像头(前面、侧面、后面等)，在感测块中还可以包括其他传感器，例如雷达、激光雷达以及声音传感器。此外，感测块可以包括一个或多个组件，被配置成通信和传送/接收与车辆环境有关的信息。例如，这样的组件可以包括无线收发信机(RF等)，其可以从相对于主机车辆远程设置的源接收关于主机车辆环境的基于传感器的信息或任意其他类型的信息。这样的信息可以包括传感器输出信息或从主机车辆以外的车辆系统接收的相关信息。在一些实施例中，这样的信息可以包括从远程计算设备、中央服务器等接收的信息。此外，摄像头可以采用多种不同配置：单摄像头单元、多摄像头、摄像头群集、长FOV、短FOV、广角、鱼眼等。

图4是根据本公开的实施例的包括车辆控制系统3000的车辆1000的侧视图的示例的图。例如，车辆1000可以被装配处理单元3010和如上参考图3所描述的车辆控制系统3000的任意其他组件。虽然在一些实施例中车辆1000可以被装配仅单个图像捕获设备(例如摄像头)，但是在其他实施例中，可以使用多个图像捕获设备。例如，如图4所示，车辆1000的图捕获设备3022和3024任一者可以是高级驾驶辅助系统(ADAS)成像设备的部分。

包括在车辆1000上的作为图像获取单元3020的部分的图像捕获设备可以设置在任意合适位置。在一些实施例中，图像捕获设备3022可以位于后视镜附近。该位置可以提供类似于车辆1000的驾驶员视线的视线，这可以帮助确定驾驶员可看见和看不见的东西。图像捕获设备3022可以设置在后视镜附近的任意位置，但是将图像捕获设备3022放置在镜子的驾驶员侧可以进一步帮助得到代表驾驶员视场和/或视线的图像。

也可以使用针对图像获取单元3020的图像捕获设备的其他位置。例如，图像捕获设备3024可以位于车辆1000的保险杠上或中。这种位置尤其可以适合具有广视场的图像捕获设备。位于保险杠的图像捕获设备的视线可以不同于驾驶员的视线，且由此，保险杠图像捕获设备和驾驶员可以不总是看到相同的物体。图像捕获设备(例如图像捕获设备3022、3024和3026)也可以位于其他位置。例如，图像捕获设备可以位于车辆1000的外后视镜的一个或两个的上面或者集成在外后视镜上，在车辆1000的车顶上，在车辆1000的引擎盖上，在车辆1000的后备箱上，在车辆1000的侧面，安装在车辆1000的任意窗户上、设置在窗户后，或设置在窗户前面，以及安装在车辆1000的前面和/或后面的灯具中或附近。

除了图像捕获设备之外，车辆1000还可以包括车辆控制系统3000的各种其他组件。例如，车辆1000上可以包括处理单元3010，与车辆的发动机控制单元(ECU)集成或分开。车辆1000还可以装配有位置传感器3030，例如GPS接收器，且还可以包括地图数据库3060和存储器单元3040和3050。

如之前所述，无线收发信机3072可以通过一个或多个网络(例如蜂窝网络、因特网等)传送和/或接收数据。例如，无线收发信机3072可以将车辆控制系统3000收集的数据上传到一个或多个服务器，以及从一个或多个服务器下载数据。经由无线收发信机3072，车辆控制系统3000可以接收例如针对存储在地图数据库3060、存储器3040和/或存储器3050中的数据的周期性或按需更新。类似地，无线收发信机3072可以将来自车辆控制系统3000的任意数据(例如图像获取单元3020捕获的图像，位置传感器3030或其他传感器、车辆控制系统等接收的数据)和/或处理单元3010处理的任意数据上传到一个或多个服务器。

车辆控制系统3000可以基于隐私等级设置将数据上传到服务器(例如，上传到云)。例如，车辆控制系统3000可以实施隐私等级设置来管控或限制发送到服务器的数据类型(包括元数据)，其可以唯一标识车辆和/或车辆的驾驶员/车主。这种设置可以由用户经由例如无线收发信机3072来设置，可以通过工厂默认设置初始化，或由通过无线收发信机3072接收的数据初始化。

图5是根据本公开的实施例的车辆系统架构5000的示例的图。车辆系统架构5000可以被实现为主机车辆5010的一部分。

参考图5，车辆系统架构5000包括导航设备5090、决策单元5130、物体检测器5200、V2X通信5160和车辆控制器5020。导航设备5090可以由决策单元5130用来确定主机车辆5010到目的地的行驶路径。例如，行驶路径可以包括行进路线或导航路径。导航设备5090、决策单元5130和车辆控制器5020可共同用于确定沿道路将主机车辆5010转向到何处，使得主机车辆5010相对于例如车道标记、路缘、交通标志、行人、其它车辆等适当地定位在道路上，或者导航设备5090、决策单元5130和车辆控制器5020可共同用于基于数字地图5120确定指示主机车辆5010跟随以到达目的地的路线，或这两者。

为了确定主机车辆5010位于数字地图5120上的何处，导航设备5090可包括定位设备5140，例如GPS/GNSS接收器和惯性测量单元(IMU)。摄像头5170、雷达单元5190、声呐单元5210、激光雷达单元5180或它们的任何组合可用于检测在数字地图5120上指示的主机车辆5010附近的相对永久物体，例如交通信号、建筑物等，并且确定相对于那些物体的相对位置以便确定主机车辆5010位于数字地图5120上的何处。该过程可以被称为地图定位。导航设备5090的功能、由导航设备5090提供的信息或这两者可全部或部分地通过V2I通信、V2V通信、车辆到行人(V2P)通信或它们的组合，其可一般标记为V2X通信5160。

在一些实现方式中，物体检测器5200可以包括声纳单元5210、摄像头5170、激光雷达单元5180和雷达单元5190。物体检测器5200可以用于检测另一实体的相对位置，并且确定另一实体将与主机车辆5010的行驶路径相交的交点。为了确定交点以及主机车辆5010和另一实体何时将到达该交点的相对定时，车辆系统架构5000可以使用物体检测器5200来确定例如相对速度、另一实体与主机车辆5010的分离距离或者这两者。物体检测器5200的功能、由物体检测器5200提供的信息或者这两者可以全部或者部分地通过V2I通信、V2V通信、V2P通信或者它们的组合，其可以一般地被标记为V2X通信5160。因此，车辆系统架构5000可以包括收发信机以实现这样的通信。

车辆系统架构5000包括与物体检测器5200和导航设备5090通信的决策单元5130。通信可以是通过但不限于有线、无线通信或光纤的方式。决策单元5130可以包括(一个或多个)处理器，诸如微处理器或其他控制电路，例如模拟电路、数字电路或这两者，包括用于处理数据的专用集成电路(ASIC)。决策单元5130可以包括存储器，包括非易失性存储器，例如用于存储一个或多个例程(routine)、阈值、捕获的数据或其组合的电可擦除可编程只读存储器(EEPROM)。决策单元5130可至少包括任务规划器5300、行为规划器5310和运动规划器5320，它们共同确定或控制主机车辆5010的路线或路径规划、局部驾驶行为和轨迹规划。

车辆系统架构5000包括与决策单元5130通信的车辆控制器或轨迹跟踪器5020。车辆控制器5020可以通过将诸如转向、油门、制动等运动的适当的车辆命令施加到诸如转向器、加速器、制动器等沿着几何路径引导车辆的物理控制机构来执行限定的几何路径(其可以由运动规划器5320或决策单元5130提供)。车辆控制器5020可包括(一个或多个)处理器，诸如微处理器或其他控制电路，例如模拟电路、数字电路或两者，包括用于处理数据的专用集成电路(ASIC)。车辆控制器5020可包括存储器，包括非易失性存储器，例如用于存储一个或多个例程、阈值、捕获的数据或其组合的电可擦除可编程只读存储器(EEPROM)。

主机车辆5010可以以自动模式操作，该模式下不需要操作人员来操作车辆5010。在自动模式中，车辆控制系统5000(例如使用车辆控制器5020、决策单元5130、导航设备5090、物体检测器5200和其它描述的传感器和设备)自主地控制车辆5010。替代地，主机车辆可以以手动模式操作，该模式下自动化程度或水平可能比向操作人员提供转向建议稍多。例如，在手动模式中，车辆系统架构5000可以根据需要辅助操作人员到达所选择的目的地，避免与另一实体的干扰或冲突，或者这两者，其中另一实体可以是另一车辆、行人、建筑物、树、动物或车辆5010可能遇到的任何其他物体。

本申请描述的导航系统被配置为确定主机车辆的行驶路径。本申请描述了用于运动规划的方法和系统。可以使用各种方法来执行运动规划。例如，运动规划可以包括一个或多个数值优化方法，诸如基于模型预测控制的方法。运动规划可以包括基于采样的方法，例如点阵和快速扩展随机树(RRT)。运动规划可以包括路径速度分解方法。

在本申请公开的实施例中，运动图可以包括顶点和边。每个顶点可以包含运动状态以及必要的信息，诸如时间戳、行驶距离、汽车状态等，并且边是两个顶点之间的连接。运动图可以以初始运动状态为根，并且通过将顶点和边添加到图中来扩展。如果顶点具有连接到图中的任何顶点的边，则将该顶点添加到图。运动状态(x，y，θ，v，a，δ)可以包括配置空间(x，y，θ，v)和控制空间(a，δ)，在配置空间(x，y，θ，v)中，位置是x和y、方向是θ以及速度是v，在控制空间(a，δ)中，加速度是a以及转向角是δ。开放顶点可以是先前未被选择来扩展的顶点。控制集可以是预定义的控制输入集，其可以用于应用到运动状态以获得新运动状态。在一种实现方式中，有效运动可以包括但不限于以下条件：无冲突的运动，运动导致可以满足状态约束的新运动状态。在一种实现方式中，有效运动状态可包含但不限于以下条件：不处于冲突中的运动状态、满足所有运动状态约束的运动状态或这两者。项dt可以是时间间隔，其中在dt期间，控制输入可以被应用到运动状态以获得新运动状态，dt可以是小的时间间隔，例如100ms。可以利用一个或多个终止条件，例如，处理时间>时间限制，图中的顶点数量>最大数量，覆盖区域>最大区域，或它们的任何组合。运动图用于搜索(一个或多个)运动轨迹。

配置图可以包括顶点和边。每个顶点可以包含配置状态以及必要的信息，诸如时间戳、行驶距离、汽车状态等。在一种实现方式中，配置包括与车辆相关或相关联的位置(x，y)和方向(θ)。边是两个顶点之间的连接。配置图可以以初始配置状态为根，并且通过将顶点和边添加到图中来扩展。配置状态可以包括配置空间(x，y，θ)，其中位置是x和y，并且方向是θ。开放顶点可以是先前未被选择来扩展的顶点。在一种实现方式中，有效配置可以包括但不限于以下条件：无冲突的配置状态、满足配置约束的配置状态或这两者。例如，配置约束可以是x_min＜x＜x_max，y_min＜y＜y_max；θ_min＜θ_max等。在一种实现方式中，有效连接可以包括但不限于以下条件：无冲突的连接。配置图被用来搜索路径。速度规划器用于从路径生成轨迹。

图6是根据本公开的实施例的自动驾驶车辆运动规划方法或技术6000的示例的图。技术6000包括：接收初始化帧6005；使用初始化帧执行运动规划6010；以及计算车辆轨迹6015。在一种实现方式中，技术6000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行。

方法6000包括接收初始化帧6005，其中初始化帧可以包括与定位、车辆状态(位置、速度、加速度、底盘)、地图、路线、感知、预测、行为决策或它们的任何组合相关联的信息。

方法6000包括基于初始化帧中的信息执行运动规划6010。

方法6000包括基于定位、车辆状态(位置、速度、加速度、底盘)、地图、路线、感知、预测、行为决策或它们的任何组合来计算车辆轨迹6015。

图7是根据本公开的实施例的数值优化方法和系统7000的示例的图。在一种实现方式中，系统7000可以包括优化器7005。优化器7005被配置成基于接收的车辆动态模型7010和每个接收的帧7015来确定约束和成本函数。优化器7005可被配置为基于约束、成本函数或这两者来确定车辆轨迹7020。车辆轨迹7020和相关信息可作为下一个规划周期的输出数据7025存储在存储器中，例如图3所示的存储器单元3040、3050。在一种实现方式中，输出数据7025的最后结果7030可用作优化器7005的初始输入。在一种实现方式中，方法和系统7000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

图8是根据本公开的实施例的基于模型预测控制的方法和系统8000的示例的图。预测模型8005可以基于车辆动态模型8010和初始输入帧8015来确定。系统8000可以包括优化器8020。优化器8020可基于预测模型8005确定约束和成本函数。优化器8020可基于约束、成本函数或这两者来确定车辆轨迹8025。车辆轨迹8025和相关信息可以作为下一个规划周期的输出数据8030存储在存储器中，例如图3所示的存储器单元3040、3050。在一种实现方式中，输出数据8030的最后结果8035可用作优化器8030的初始输入。在一种实现方式中，方法和系统8000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

图9是根据本公开的实施例的基于采样的方法9000中的网格规划器的示例的图。方法9000包括：初始化运动图9005；添加先前运动图数据9010；检查终止条件9015；如果终止条件为真，则存储运动图数据9020；生成候选轨迹9025；选择轨迹解9030；如果终止条件为假，则针对运动图中的每个开放节点9035以及针对控制集中的每个控制输入9040，将该控制输入应用到运动状态9045；判断运动状态的有效性9050；如果运动状态有效，则生成新的顶点和新的边，并将新的顶点和新的边添加到运动图中9055；以及如果运动状态无效，则返回检查终止条件9015。在一种实现方式中，方法9000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

方法9000包括初始化运动图9005。在一种实现方式中，运动图T可以用处于初始运动状态s_init的根顶点v_init来初始化。

方法9000包括添加先前运动图数据9010。在实施例中，存储的运动图数据可以在初始化时被添加到运动图T。

方法9000包括检查终止条件9015。在一种实现方式中，终止条件可以包括处理时间大于预定时间限制的情形。

方法9000可以包括如果终止条件为真，则存储运动图数据9020。在一种实现方式中，当终止条件为真时，运动图数据可以被存储以用于下一规划周期。例如，从初始状态，先前运动规划图中的每个顶点可以连接到运动图T，并且可以丢弃任何不可行的运动。

方法9000包括生成候选轨迹9025。在一种实现方式中，所存储的运动图可被用于生成一个或多个候选轨迹。运动规划目标可以由如图5所示的任务和行为规划来指定。满足运动规划目标的运动图中的顶点集可以被选择为目标运动。可以通过级联(concatenate)与将根运动连接到目标运动的边相关联的运动来生成轨迹集。

方法9000包括选择轨迹解9030。选择轨迹解9030可以包括选择具有最佳轨迹成本的轨迹作为轨迹解。轨迹成本可以包括但不限于行驶距离、平滑度、舒适度、安全性或它们的任何组合。

方法9000包括如果终止条件为假，则针对运动图中的每个开放节点9035以及针对控制集中的每个控制输入9040，将控制输入应用到运动状态9045。控制输入可以在时间间隔dt上被应用到运动状态，以获得新的运动状态。在一种实现方式中，如果终止条件为假，则针对运动图中的每个开放顶点v_i以及针对控制集中的每个控制u_i，可将控制u_i应用于v_i的运动状态通过时间步长dt以获得新的运动状态snew。在一种实现方式中，当应用控制输入以获得新的运动状态以减少计算时间时，可以使用查找表(LUT)。在一种实现方式中，针对应用于时间间隔dt的初始状态(例如车辆运动状态)s＝(x,y,θ,v,δ)_init和输入控制

更新的状态(x,y,θ,v,δ)_updated可以被计算并存储在LUT中，其中x和y是位置，θ是前进方向，v是速度，a是加速度，δ是转向角，以及

是转向速率。在一种实现方式中，对LUT的访问可以使用车辆运动状态和输入控制。在一个实现方式中，对LUT的访问可以使用车辆运动状态和输入控制参数的组合。在一种实现方式中，LUT可以在运动规划执行时被预先计算和存储。在一种实现方式中，LUT可在速度规划器被执行时被填入。在一种实现方式中，LUT可以被填入以用于速度或运动更新处理。

方法9000包括确定运动状态的有效性9050。在一种实现方式中，确定9050确定从v_i到s_new的运动状态的运动是否有效。

方法9000包括如果运动状态有效，则生成新的顶点和新的边9055。在一种实现方式中，如果运动有效，则生成9055可以包括生成具有运动状态s_new和指示从v_i到v_new的连接的边e_new的新的顶点v_new，然后将v_new和e_new添加到运动图T。

方法9000包括如果运动状态无效，则返回到检查终止条件9015。如果运动规划组件确定运动无效，则其可以继续确定终止条件是否适用。

图10A-10M是根据本公开的实施例的示例网格规划器图序列的图。图10A示出了用单个顶点(示为椭圆)初始化的运动图，其中顶点包含初始车辆状态下的运动状态(x,y,θ,v,a,δ)以及其它信息，如时间戳、车辆状态等。顶点具有初始运动状态(x1,y1,θ1,v1,a1,δ1)和设置为零的时间戳和行驶距离。

图10B-10C示出了针对每个开放节点(之前从未被选择的节点)，应用控制集中的每个控制输入(在该示例中示出了3个控制)来生成新的运动状态(即，运动状态(x2,y2,θ2,v2,a2,δ2)，其中时间戳被设置为0.1，并且行驶距离被设置为1)。如果到新运动状态(黑线)的运动有效(无冲突)，则将新运动状态添加到运动图作为新的顶点(椭圆)和新的边(黑线)。

图10D-10E示出重复选择开放节点和应用控制输入的过程。如果到新运动状态(粗线)的运动无效(冲突)，则不将新运动状态添加到运动树。

图10F示出重复该过程直到满足终止条件。

图10G示出了在满足终止条件之后选择候选轨迹(虚线)。

图10H示出了选择轨迹解(黑线)。

图10I示出了具有(一个或多个)新障碍物的下一规划周期，其中运动图用新的初始运动状态(当前车辆状态)初始化。

图10J-10K示出了将先前规划数据添加到新的运动图中，并且丢弃所有无效运动。

图10L示出了重复该过程以扩展运动图并获得候选轨迹(虚线)。

图10M示出了轨迹解的选择。

图11是根据本公开的实施例的基于采样的方法11000中的快速扩展随机树(RRT)及其变体(RRT类型)的示例的图。方法11000包括：初始化运动图11005；添加先前运动图数据11010；检查终止条件11015；如果终止条件为真，则存储运动图数据11020；生成候选轨迹11025；选择轨迹解11030；如果终止条件为假，则采样目标11035；选择运动图中的顶点11040；计算控制输入11045；将控制输入应用到顶点的运动状态以生成新运动状态11050；确定新运动状态的有效性11055；如果新运动状态有效，则生成新的顶点和新的边，并将新的顶点和新的边添加到运动图11060，并返回到检查终止条件11015；如果新运动状态无效，则返回检查终止条件11015。在一种实现方式中，方法11000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

方法11000包括初始化运动图11005。在一种实现方式中，运动图T可以用在初始运动状态s_init的根顶点v_init来初始化。

方法11000包括添加先前运动图数据11010。在一种实现方式中，存储的运动图数据可以在初始化时被添加到运动图T。

方法11000包括检查终止条件11015。在一种实现方式中，终止条件可以包括处理时间大于预定时间限制的情形。

方法11000包括如果终止条件为真，则存储运动图数据11020。在一种实现方式中，可以存储运动图数据以用于下一规划周期。例如，从初始状态，先前运动规划图中的每个顶点可以连接到运动图T，并且可以丢弃任何不可行的运动。

方法11000包括生成候选轨迹11025。在一种实现方式中，所存储的运动图可用于生成一个或多个候选轨迹。运动规划目标可以由如图5所示的任务和行为规划来指定。满足运动规划目标的运动图T中的顶点集可以被选择为目标运动。

方法11000包括选择轨迹解11030。在一种实现方式中，可通过级联与将根运动连接到目标运动的边相关联的运动来生成轨迹集。在一种实现方式中，选择轨迹解11030可以包括选择具有最佳轨迹成本的轨迹作为轨迹解。轨迹成本可以包括但不限于行驶距离、平滑度、舒适度、安全性或它们的任何组合。

方法11000包括如果终止条件为假，则采样目标11035。在一种实现试中，如果终止条件为假，则可以对目标s_sample进行采样；s_sample必须满足配置约束。例如，可以利用(x,y,θ)对配置目标进行采样，其中x、y、θ满足配置约束(x_min<x<x_max；y_min<y<y_max；θ_min<θ<θ_max)。

方法11000包括选择顶点11040。在一种实现方式中，可以在运动图中选择一个顶点v_i。

方法11000包括计算控制输入11045。在一种实现方式中，计算控制输入u以生成从所选顶点的运动状态到s_sample的运动。

方法11000包括将控制输入应用到运动状态11050。控制输入可以在时间间隔dt上被应用到运动状态，以获得新的运动状态。在一种实现方式中，控制u可以被应用于v_i的运动状态通过时间步长dt以获得新的运动状态s_new。在示例中，当应用控制输入以获得新的运动状态以减少计算时间时，可以使用LUT。在一种实现方式中，当应用控制输入以获得新的运动状态以减少计算时间时，可以使用查找表(LUT)。在一种实现方式中，针对应用于时间间隔dt的初始状态(例如，车辆运动状态)s＝(x,y,θ,v,δ)_init和输入控制

可以计算更新的状态(例如，更新的车辆运动状态)(x,y,θ,v,δ)_updated并存储在LUT中，其中x和y是位置，θ是前进方向，v是速度，a是加速度，δ是转向角以及

是转向速率。在一个实现方式中，对LUT的访问可以使用车辆运动状态和输入控制。在一个实现方式中，对LUT的访问可以使用车辆运动状态和输入控制参数的组合。在一种实现方式中，可以在运动规划执行时预先计算并存储LUT。在一种实现方式中，LUT可在速度规划器被执行时被填入。在一种实现方式中，LUT可以被填入以用于速度或运动更新处理。

方法11000包括确定运动状态的有效性11055。在一种实现方式中，确定11055可以确定新的运动状态s_new是否有效。

方法11000包括如果运动状态有效，则生成新的顶点和新的边11060，并且返回到检查终止条件11015。在一种实现方式中，如果s_new有效，则生成11060可以生成新的顶点v_new，其具有运动状态s_new和边e_new，边e_new指示从v_i到v_new的连接，然后将v_new和e_new添加到运动图T。

方法11000包括如果运动状态无效，则返回到检查终止条件11015。

图12A-12O是根据本公开的实施例的基于采样的规划器图序列的示例的图。图12A示出了用单个顶点(示为椭圆)初始化的运动图，其中顶点包含运动状态(x,y,θ,v,a,δ)以及其它信息，如时间戳、车辆状态等。顶点具有初始运动状态(x_init,y_init,θ_init,v_init,a_init,δ_init)。

图12B示出了在(x_sample,y_sample,θ_sample)处的采样目标和在运动图中选择顶点以从其扩展。

图12C示出了对控制u的计算，以生成从所选顶点的运动状态到采样目标的运动。如果到新运动状态的运动(黑线)有效(无冲突)，则将新运动状态(具有x₂,y₂,θ₂,v₂,a₂,δ₂)添加到运动图作为新的顶点(椭圆)和新的边(黑线)。

图12D示出重复采样目标和选择运动图中的顶点、生成从所选顶点的运动状态到采样目标的运动的过程。

图12E-12F示出了如果运动无效则不将运动添加到采样目标。

图12G示出重复该过程直到满足终止条件。

图12H示出了当终止条件满足时选择候选轨迹(虚线)。

图12I示出了从候选轨迹中选择轨迹解(粗黑线)。

图12J示出了具有(一个或多个)新障碍物的下一规划周期，其中运动图用新的初始运动状态(当前车辆状态)初始化。

图12K-12L示出了将先前规划数据添加到新的运动图中，并且丢弃所有无效运动。

图12M-12N示出重复采样随机目标、选择顶点以扩展运动图以及选择候选轨迹(虚线)的过程。

图12O示出了选择轨迹解。

图13是根据本公开的实施例的路径速度分解方法13000的示例的图。方法13000包括：接收帧13005；使用来自规划帧的信息执行路径规划13010；执行路径上的速度规划13015；以及基于路径和速度确定车辆轨迹13020。在一种实现方式中，方法13000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

方法13000包括接收帧13005。在一种实现方式中，该帧可以是运动规划帧。

方法13000包括使用来自该帧的信息执行路径规划13010。在一种实现方式中，执行13010可对运动规划帧执行路径规划以生成无冲突路径。

方法13000包括在路径上执行速度规划13015。在一种实现方式中，执行13015可执行速度规划(即，速率规划)以生成沿从路径规划生成的路径的最优速度配置文件(profile)。在一种实现方式中，执行13015可考虑用户参考、交通规则、操纵要求或它们的任何组合。

方法13000包括确定车辆轨迹13020。在一种实现方式中，确定13020可以基于路径规划和速度规划。

图14是根据本公开实施例的路径速度分解方法14000中的RRT路径规划器的示例的图。方法14000包括：初始化配置图14005；将先前配置图添加到当前配置图14010；检查终止条件14015；如果终止条件为真，则存储配置图数据14020；产生候选路径14025；选择路径解14030；如果终止条件为假，则采样目标14035；确定目标配置状态的有效性14040；如果配置状态有效，则选择顶点14045；检查连接是否有效14050；如果连接有效，则生成新的顶点和新的边14055，并返回到检查终止条件14015；如果配置状态无效，则返回检查终止条件14015；如果连接无效，则返回检查终止条件14015。在一种实现方式中，方法14000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

方法14000包括初始化配置图14005。在一种实现方式中，可以用处于初始配置状态s_init的根来初始化配置图T。

方法14000包括将先前配置图添加到当前配置图14010。在实施例中，可以在初始化时将先前配置图数据添加到配置图T。例如，从初始状态，可以添加先前配置图数据，并且可以丢弃任何不可行的数据。

方法14000包括检查终止条件14015。例如，终止条件可以包括处理时间大于预定时间限制的情形。

方法14000包括如果终止条件为真，则存储配置图数据14020。在一种实现方式中，可以存储配置图以用于下一个规划周期。

方法14000包括生成候选路径14025。在一种实现方式中，所存储的配置图可以用于生成一个或多个候选路径。路径规划目标可以由如图5所示的任务和行为规划来指定。可以选择配置图中满足路径规划目标的顶点集作为目标配置。在一种实现方式中，可以通过级联与将根配置连接到目标配置的边相关联的配置来生成路径集合。

方法14000包括选择路径解14030。选择14030可包括选择具有最佳路径成本的路径作为路径解。路径成本可以包括但不限于行驶距离、平滑度、舒适度、安全性或它们的任何组合。

方法14000包括如果终止条件为假，则采样目标14035。在一种实现方式中，采样14035可包括对目标s_sample进行采样。

方法14000包括确定目标配置状态的有效性14040。在一种实现方式中，确定14040可包括确定目标s_sample是否有效。

方法14000包括如果目标配置状态有效，则选择顶点14045。在一种实现方式中，选择14045可以包括如果s_sample有效则选择配置图中的顶点v_i。

方法14000包括检查连接是否有效14050。在一种实现方式中，检查14050可以包括确定从v_i到s_sample的配置状态的连接是否有效。在一种实现方式中，有效连接是无冲突的连接。

方法14000包括如果连接有效，则生成新的顶点和新的边14055，并且返回到检查终止条件14015。在一种实现方式中，生成14055可以包括生成新的顶点v_new，其具有配置s_sample和边e_new，且边e_new指示从v_i到v_new的连接，然后如果该连接有效，则将v_new和e_new添加到配置图T。

图15是根据本公开的实施例的路径速度分解方法15000中的网格路径规划器的示例的图。该方法15000包括：初始化配置图15005；添加先前配置图数据15010；检查终止条件15015；如果终止条件为真，则存储配置图数据15020；生成候选路径15025；选择15030路径解；如果终止条件为假，则针对配置图中的每个开放节点15035，针对每个样本目标15040，确定连接是否有效15045；如果连接有效，则生成新的顶点和新的边15050，并将新的顶点和边加入连接图；如果连接无效，则返回到检查终止条件15015。在一种实现方式中，方法15000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

方法15000包括初始化配置图15005。在一种实现方式中，可以用处于初始配置状态s_init的根来初始化配置图T。

方法15000包括添加先前配置图数据15010。在实施例中，可以在初始化时将先前的配置图数据添加到配置图T。例如，从初始状态，可以添加先前配置图数据，并且可以丢弃任何不可行的数据。

方法15000包括检查终止条件15015。例如，终止条件可以包括处理时间大于预定时间限制的情形。

方法15000包括如果终止条件为真，则存储15020配置图数据。在一种实现方式中，可以存储配置图数据以用于下一个规划周期。

方法15000包括生成候选路径15025。路径规划目标可以由如图5所示的任务和行为规划来指定。可以选择配置图中满足路径规划目标的顶点集作为目标配置。在一种实现方式中，可以通过级联与将根配置连接到目标配置的边相关联的配置来生成路径集合。

方法15000包括选择路径解15030。在一种实现方式中，选择15030可包括选择具有最佳路径成本的路径作为路径解。路径成本可以包括但不限于行驶距离、平滑度、舒适度、安全性或它们的任何组合。

方法15000包括，如果终止条件为假，则针对配置图中的每个开放节点15035，针对每个样本目标15040，确定连接是否有效15045。在一种实现方式中，确定15045可包括，针对配置图中的每个开放顶点v_i以及针对v_i的每个样本目标s_sample，确定从v_i到s_sample的配置的连接是否有效。

方法15000包括如果连接有效，则生成新的顶点和新的边15050，并且将新的顶点和边添加到连接图。在一种实现方式中，生成15050可以包括生成具有配置状态s_sample和边e_new的新的顶点v_new，其指示从v_i到v_new的连接，然后将v_new和e_new添加到配置图T。

图16是根据本公开的实施例的路径速度分解方法16000中的速度规划的示例的图。方法16000包括：接收路径数据16005；将路径数据应用到运动控制器16010；以及生成车辆轨迹16015。在一种实现方式中，方法16000可以由决策单元5130、运动规划器5320、处理单元3010、处理器1020等执行和实现。

方法16000包括接收路径数据16005。

方法16000包括将路径数据应用到运动控制器16010。在一种实现方式中，运动控制器可以具有动态模型和一个或多个约束。

方法16000包括生成车辆轨迹16015。在一种实现方式中，运动控制器可以根据路径数据到动态模型的应用和一个或多个约束来生成车辆轨迹。

一种用于自动驾驶车辆(AV)中的运动规划的方法。该方法包括：初始化运动图树；至少使用运动图树中的先前运动图数据和查找表(LUT)来执行运动规划器算法，以生成至少一个候选轨迹；当终止条件已经发生时，利用与所述至少一个候选轨迹相关联的运动图数据来更新所述运动图树；从所述至少一个候选轨迹中选择轨迹；以及用该轨迹更新控制器以控制所述AV。在一种实现方式中，该方法还包括在终止条件尚未发生的情况下，针对运动图树中的每个开放节点以及针对控制集中的每个控制，将相应控制应用于相应开放节点以获得新运动状态。在一种实现方式中，该方法还包括确定从开放节点到新运动状态的运动是否有效；在所述运动有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树；以及在所述运动是无效运动的情况下丢弃运动图数据。在一种实现方式中，开放节点是从未被选择用于运动图树扩展的节点。在一种实现方式中，运动图树包括顶点和边，每条边是两个顶点之间的连接，并且该方法还包括通过级联与将根运动连接到目标运动的边相关联的运动来生成至少一个候选轨迹。在一种实现方式中，由任务和行为规划器指定运动规划目标，并且选择满足运动规划目标的运动图树中的顶点集作为目标运动。在一种实现方式中，所选择的轨迹是具有最佳成本的候选轨迹。在一种实现方式中，最佳包括行驶距离、平滑度、舒适度和安全性中的至少一者。在一种实现方式中，运动图树包括顶点和边，每条边是两个顶点之间的连接，并且该方法还包括在终止条件尚未发生的情况下：采样目标；在所述运动图树中选择顶点；计算从所述顶点到所述目标的运动的控制输入；获取新运动状态；以及在所述新运动状态有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树。在一种实现方式中，该方法还包括将控制输入应用于顶点的运动状态通过时间步长以生成运动。在一种实施方式中，LUT填入有基于车辆状态和输入控制的更新的运动数据。

一种用于自动驾驶车辆(AV)中的运动规划的方法。该方法包括：初始化配置图树；至少使用所述配置图树中的先前配置图数据来执行路径规划器算法，并且生成至少一个候选路径；当终止条件已经发生时，利用与所述至少一个候选路径相关联的配置图数据来更新所述配置图树；从所述至少一个候选路径中选择路径；使用所选择的路径和查找表(LUT)来执行速度规划器算法以确定速度；以及用所述路径和所述速度更新控制器以控制所述AV。在一种实现方式中，该方法还包括在终止条件尚未发生的情况下，针对所述配置图树中的每个开放顶点以及针对目标集合中的每个样本目标：确定从所述开放节点到样本目标的连接是否有效；在所述连接有效的情况下，将与所述样本目标相关联的配置图数据添加到所述配置图树；以及在连接无效的情况下，丢弃配置图数据。在一种实现方式中，配置图树包括顶点和边，每条边是两个顶点之间的连接，并且该方法还包括通过级联与将根配置连接到目标配置的边相关联的连接来生成至少一个候选路径。在一种实现方式中，由任务和行为规划器指定路径规划目标，并且选择配置图树中满足路径规划目标的顶点集作为目标配置。在一种实现方式中，配置图树包括顶点和边，每条边是两个顶点之间的连接，并且该方法还包括在终止条件尚未发生的情况下：采样目标；在配置图树中选取顶点；将所述顶点连接到所述目标以生成新配置状态；以及在所述新配置状态有效的情况下，将与所述新配置状态相关联的配置图数据添加到所述配置图树。

自动驾驶车辆(AV)控制器，其包括运动规划器，所述运动规划器被配置为：初始化运动图树；至少使用所述运动图树中的先前运动图数据和查找表(LUT)来执行运动规划器算法，以生成至少一个候选轨迹；当终止条件已经发生时，利用与所述至少一个候选轨迹相关联的运动图数据来更新所述运动图树；从所述至少一个候选轨迹中选择轨迹；以及用所述轨迹更新控制器以控制所述AV。在一种实现方式中，运动规划器还被配置为：在终止条件尚未发生的情况下，针对所述运动图树中的每个开放节点以及针对控制集中的每个控制，将相应控制应用于相应开放节点以获得新运动状态；确定从所述开放节点到所述新运动状态的运动是否有效；在所述运动有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树；以及在运动是无效运动的情况下，丢弃运动图数据。在一种实现方式中，运动图树包括顶点和边，每条边是两个顶点之间的连接，并且运动规划器还被配置为在终止条件尚未发生的情况下：采样目标；选择所述运动图树中的顶点；计算从所述顶点到所述目标的运动的控制输入；获取新运动状态；以及在所述新运动状态有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树。在一种实施方式中，LUT填入有基于车辆状态和输入控制的更新的运动数据。

虽然本申请的一些实施方式涉及方法，但是本领域技术人员可以理解实施方式也可以被实施为系统或计算机程序产品。因此，本发明的方面可以采用完全硬件实施方式的形式、完全软件实施方式的形式(包括固件、常驻软件、微代码等)或结合软件和硬件方面的实施方式的形式，这在本申请中可以一般都称为“处理器”、“设备”或“系统”。此外，本发明的方面可以采取在其上具有计算机可读程序代码的一个或多个计算机可读介质中实施的计算机程序产品的形式。可以使用一个或多个计算机可读介质的任意组合。计算机可读介质可以是计算机可读信号介质或计算机可读存储介质。计算机可读存储介质可以是例如但不限于电、磁、光、电磁、红外或半导体系统、装置或设备，或前述的任意合适的组合。计算机可读存储介质的更具体的示例(非穷举)包括以下：具有一个或多个电线的电连接、可携带计算机磁盒、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或闪存)、光纤、可携带压缩盘只读存储器(CD-ROM)、光存储设备、磁存储设备或前述的任意合适的组合。在本申请上下文中，计算机可读存储介质可以是任意有形介质，能够包含或存储用于指令执行系统、装置或设备使用或与其关联的程序。

计算机可读信号介质可以包括在其中实施计算机可读程序代码的传播数据信号，例如在基带中，或作为载波的部分。这样的传播信号可以采用任意多种形式，包括但不限于，电磁、光或这些的任意合适组合。计算机可读信号介质可以是不是计算机可读存储介质且能够通信、传播或传输供指令执行系统、装置或设备使用或与之关联的程序的任意计算机可读介质。

在计算机可读介质实施的程序代码可以使用任意合适的介质来传送，包括但不限于，CD、DVD、无线、有线、光纤电缆、RF等，或前述的任意合适组合。

用于执行本发明方面的操作的计算机程序代码可以以一种或多种编程语言的任意组合被写入，包括面向对象的编程语言，例如Java、Smalltalk、C++等，以及常规程序编程语言，例如“C”编程语言或类似的编程语言。程序代码可以完全在用户的计算机上执行，部分在用户的计算机上执行，作为独立软件包执行，部分在用户的计算机上且部分在远程计算机上或完全在远程计算机或服务器上执行。在后者场景中，远程计算机可以通过任意类型的网络连接到用户的计算机，包括局域网(LAN)和广域网(WAN)，或可以到外部计算机的连接(例如通过使用因特网服务供应商的因特网)。

下面参考根据本发明实施例的方法、装置(系统)和计算机程序产品的流程图图示和/或框图描述本发明的方面。可以理解流程图图示和/或框图的每一个框以及流程图图示和/或框图中的框的组合可以由计算机程序指令来执行。

这些计算机程序指令可以被提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器以产生机器，由此经由计算机或其他可编程数据处理装置的处理器执行的指令创建用于执行在流程图和/或框图的一个或多个框中指定的功能/动作。这些计算机程序指令也可以被存储在计算机可读介质中，其可以指示计算机、其他可编程数据处理装置或其他设备已特定方式发挥功能，由此存储在计算机可读介质中的指令产生一项产品，包括执行在流程图和/或框图的一个或多个框中指定的功能/动作。

计算机程序指令也可以被加载到计算机、其他可编程数据处理装置或其他设备，以使得在计算机、其他可编程装置或其他设备上执行一系列操作步骤，从而产生计算机执行的过程，由此在计算机或其他可编程装置上执行的指令提供用于执行在流程图和/或框图的一个或多个框中指定的功能/动作的过程。

附图中的流程图和框图示出了根据本发明的各种实施方式的系统、方法和计算机程序产品的可能实施的架构、功能和操作。在这一点而言，流程图或框图中的每个框可以代表代码模块、片段或部分，其包括用于执行指定的一个或多个逻辑功能的一个或多个可执行指令。还应当注意，在一些可替换实施中，框中注释的功能可以不按照图中标示的顺序发生。

虽然结合某些实施方式描述了本公开，但是可以理解本公开不限于公开的实施方式，而是意在覆盖包括在权利要求书的范围内的各种修改、组合和等同设置，范围符合最宽解释由此专利法允许的所有这样的修改和等同结构。

Claims (20)

1.一种用于自动驾驶车辆(AV)中的运动规划的方法，其中，所述方法包括：

初始化运动图树；

至少使用所述运动图树中的先前运动图数据和查找表(LUT)来执行运动规划器算法，以生成至少一个候选轨迹；

当终止条件已经发生时，利用与所述至少一个候选轨迹相关联的运动图数据来更新所述运动图树；

从所述至少一个候选轨迹中选择轨迹；以及

利用所述轨迹更新控制器以控制所述AV。

2.根据权利要求1所述的方法，其中，所述方法还包括：

在终止条件尚未发生的情况下，针对所述运动图树中的每个开放节点以及针对控制集中的每个控制，将相应控制应用于相应开放节点以获得新运动状态。

3.根据权利要求2所述的方法，其中，所述方法还包括：

确定从所述开放节点到所述新运动状态的运动是否有效；

在所述运动有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树；以及

在所述运动是无效运动的情况下，丢弃运动图数据。

4.根据权利要求3所述的方法，其中，开放节点是从未被选择用于运动图树扩展的节点。

5.根据权利要求1所述的方法，其中，所述运动图树包括顶点和边，每条边是两个顶点之间的连接，并且所述方法还包括：

通过级联与将根运动连接到目标运动的边相关联的运动来生成所述至少一个候选轨迹。

6.根据权利要求5所述的方法，其中，运动规划目标由任务和行为规划器指定，并且满足所述运动规划目标的所述运动图树中的顶点集被选择为目标运动。

7.根据权利要求1所述的方法，其中，所选择的轨迹是具有最佳成本的候选轨迹。

8.根据权利要求7所述的方法，其中，所述最佳包括行驶距离、平滑度、舒适度和安全性中的至少一者。

9.根据权利要求1所述的方法，其中，所述运动图树包括顶点和边，每条边是两个顶点之间的连接，并且所述方法还包括：

在终止条件尚未发生的情况下：

采样目标；

在所述运动图树中选择顶点；

计算从所述顶点到所述目标的运动的控制输入；

获得新运动状态；以及

在所述新运动状态有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树。

10.根据权利要求9所述的方法，其中，该方法还包括：

将所述控制输入应用于所述顶点的运动状态通过时间步长以生成所述新运动状态。

11.根据权利要求1所述的方法，其中，所述LUT填入有基于车辆状态和输入控制的更新的运动数据。

12.一种用于自动驾驶车辆(AV)中的运动规划的方法，其中，所述方法包括：

初始化配置图树；

至少使用所述路径规划器中的先前配置图数据来执行路径规划器算法，并生成至少一个候选路径；

当终止条件已经发生时，利用与所述至少一个候选路径相关联的配置图数据来更新所述配置图树；

从所述至少一个候选路径中选择路径；

使用所选择的路径和查找表(LUT)来执行速度规划器算法以确定速度；以及

利用所述路径和所述速度更新控制器以控制所述AV。

13.根据权利要求12所述的方法，其中，该方法还包括：

在终止条件尚未发生的情况下，针对所述配置图树中的每个开放顶点以及针对目标集合中的每个样本目标：

确定从所述开放节点到样本目标的连接是否有效；

在所述连接有效的情况下，将与所述样本目标相关联的配置图数据添加到所述配置图树；以及

在所述连接无效的情况下，丢弃配置图数据。

14.根据权利要求12所述的方法，其中，所述配置图树包括顶点和边，每条边是两个顶点之间的连接，并且所述方法还包括：

通过级联与将根配置连接到目标配置的边相关联的连接来生成所述至少一个候选路径。

15.根据权利要求14所述的方法，其中，路径规划目标由任务和行为规划器指定，并且满足所述路径规划目标的所述配置图树中的顶点集被选择为目标配置。

16.根据权利要求12所述的方法，其中，所述配置图树包括顶点和边，每条边是两个顶点之间的连接，并且所述方法还包括：

在终止条件尚未发生的情况下：

采样目标；

在所述配置图树中选择顶点；

将所述顶点连接到所述目标以生成新配置状态；以及

在所述新配置状态有效的情况下，将与所述新配置状态相关联的配置图数据添加到所述配置图树。

17.一种自动驾驶车辆(AV)控制器，其中，该AV控制器包括：

运动规划器，其被配置为：

初始化运动图树；

至少使用所述运动图树中的先前运动图数据和查找表(LUT)执行运动规划器算法，以生成至少一个候选轨迹；

当终止条件已经发生时，利用与所述至少一个候选轨迹相关联的运动图数据来更新所述运动图树；

从所述至少一个候选轨迹中选择轨迹；以及

利用所述轨迹更新控制器以控制所述AV。

18.根据权利要求17所述的AV控制器，其中，所述运动规划器还被配置为：

在终止条件尚未发生的情况下，针对所述运动图树中的每个开放节点以及针对控制集中的每个控制，将相应控制应用于相应开放节点以获得新运动状态；

确定从所述开放节点到所述新运动状态的运动是否有效；

在所述运动有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树；以及

在所述运动是无效运动的情况下，丢弃运动图数据。

19.根据权利要求17所述的AV控制器，其中，所述运动图树包括顶点和边，每条边是两个顶点之间的连接，并且所述运动规划器还被配置为：

在终止条件尚未发生的情况下：

采样目标；

选择所述运动图树中的顶点；

计算从所述顶点到所述目标的运动的控制输入；

获得新运动状态；以及

在所述新运动状态有效的情况下，将与所述新运动状态相关联的运动图数据添加到所述运动图树。

20.根据权利要求17所述的AV控制器，其中，所述LUT填入有基于车辆状态和输入控制的更新的运动数据。

Images