利用具有预测和运动规划的集成策略优化自动泊车系统

全自动泊车仍然具有挑战性，尤其是在具有多个独立代理的动态环境中，不仅因为它涉 及在狭小空间内的运动规划，而且因为自动驾驶车辆 (AV) 应该对周围的障碍物车辆 (OV)

做出智能反应。与在公路或高速公路上驾驶相比，泊车区域的车辆运动没有一套明确的规则 可循，很大程度上取决于驾驶员的意图甚至技能水平。这些使得泊车环境预测具有较大的 挑战性，因此，要想对整个自动泊车控制有一个好的控制水平，也需要有一个集成预测和规 划的自主泊车系统。自动泊车过程中，运动预测至关重要，因为它决定了规划模块的安全约束，从而决定了 运动规划的可行性和平滑性。这其中的规划过程包括了短期规划和长期规划两个方面。特别 是，准确的短期运动预测使智驾车辆能够对障碍物做出安全的计划和反应；短期预测的一个 主要挑战是估计障碍物的转向角。通常可以使用扩展卡尔曼滤波器 (EKF) 来重建障碍物的 速度，然后求助于自适应观测器进行转向估计。而长期计划/模式预测可以确保智驾车辆在 运行过程中更有效、更顺利地进行规划处合理的行驶路径。长期运动通常尽管很难预测准确的，但可以观察到，由于驾驶习惯，司机通常会遵循一 些路线（例如，在中国汽车应该靠右行驶）。此外，车辆在整个停车/离开过程中的运动可 以通过几种“模式”来捕捉（例如，机动进出狭窄空间和在过道上巡航）。基于这两个先 验，可以使用成本图来捕获这些路线，结合短期预测来确定障碍物的模式，并进行长期预测。本文针对自动泊车预测与规划提出了一种基于模型的混合预测器，通过观察障碍物的姿态来执行短期和长期的模式预测。

典型的自动泊车规划控制 Profile

智驾车辆的自动泊车运动规划是自动泊车场景中的一个主要挑战。一般运动规划算法 无法直接适用于存在障碍物的地方泊车，因为这需要对复杂的驾驶操作进行快速重新规划。另一方面，专门从事自动泊车的运动规划要么未能将短期规划与长期规划相结合，要么无法 将在线路径修复纳入动态环境中的新障碍。实施多种策略的场景感知规划器在计算时间方面 可能是有效的，从而导致重新规划率较高，才可能保证安全。总体来讲，自动泊车的规控模块主要包含 4 个部分：障碍物车辆轨迹预测，可行驶区域 选择，局部轨迹规划，车辆控制。

如上图所示为一种典型的泊车路径规划的软件分布模式图。其中，预测可以使用交互多模型滤波器 Interacting multiple model (IMM) filter，输入自车位置、障碍物车的 heading angle, velocity, acceleration, yaw-rate，输出的 intention 分 2 种：直行，规避对向来车。“可行驶区域”选择采用势场法，障碍物预测轨迹、道路边沿、静态障碍物的位姿都可 以作为可行驶边界的输入，由此来构造势能函数，以评估危险性以及是否可行驶。“局部路径规划”则可以采用 RRT 算法，采用了 2 个小技巧来提升速度：将可行驶区 域划分为 seeding surfaces，在这些 surfaces 上随机采样；使用前述的势能函数的倒数作为采 样的概率密度函数，以减少采样点数。在采样点中寻找代价最小路径作为动态规划（DP） 方法。输出的轨迹可以用 B-spline（piecewise polynomial）进行平滑。

本文优化的泊车规划算法

本文算法以前序基础的泊车算法为基础进行了有效信息的优化。首先使用双向 A 搜索 引导树 (BIAGT) 生成长期运动参考。然后，基于混合环境预测器结果的战略运动规划器实 施三种策略：基于模型预测控制（MPC）的安全控制器，如果参考对环境仍然有效，则用于 轨迹跟踪；基于搜索的避撞路径规划，可在紧急情况下快速找到避撞路径；以及当参考失效 时基于优化的修复可用路径规划。基于以上操作我们可以实现基于模型的混合环境预测器对前面提到的预测短期运动和 长期运动模式进行规划。由此，制定的一种策略型运动规划器，可以实现在不同情况下进行 有效规划。

1、预测器

车辆运动预测的研究对于自动驾驶系统的有效性控制有比较大的贡献，其中涉及短期运 动预测方法和长期规划/模式预测方法。可以说，这一领域的大多数研究都与道路驾驶有关。相比之下，泊车系统中车辆运动预测研究较少。交互多模型 (IMM) 滤波器可以用于预测泊 车中的短期轨迹。如果着眼于长期预测，首先需要训练轨迹聚类分类器，然后获取分类聚类 的均值轨迹。分类器识别的驾驶员意图和车辆历史姿态可以用于通过长期记忆网络，从而生 成长期运动预测。对于这类泊车轨迹规划而言纯数据驱动的方法不可取，原因有二：1) 缺乏性能保证。它们的性能在很大程度上取决于训练数据集，如果数据集选择不当， 它们可能会出现较大的预测误差。2) 预测网络过度拟合也可能是一个问题。目前还没有对融合短期和长期停车预测的预 测因子进行广泛的研究。

2、规划器

流行的运动规划方法分为三类：基于搜索的、基于采样和基于优化的方法。基于抽样的 规划器由于其不确定性，可能会引起对风险敏感任务的过度关注，而基于优化的规划器只是 局部最优的，并且经常需要与全局规划器一起工作。自动驾驶汽车广泛采用各种基于搜索的 运动规划器，因为它们具有精心选择的运动原语和启发式算法来提升计算效率。如果泊车环境发生变化，初始的长期轨迹可能需要修复。实时轨迹修复方法包括在线启 发式更新，基于采样的搜索结构进行修剪和重新连接，以及基于样条的运动动力学搜索。启 发式更新方法不能直接应用于在 BIAGT 中基于树的搜索结构，并且修剪对于停车场景效率 较低。而基于样条的运动动力学搜索相对有效，但未利用原始轨迹。另一方面，我们观察到修复场景中的备选可行解通常与原始轨迹属于同类型。因此，基于对备选路径的可行解进行 有效优化会生成修复现有路径的合适候选路径。

自动泊车系统实现

对于自动泊车的系统级策略来说，当前已有在静态环境中的自动泊车系统中采用交互多 模型 IMM 进行预测，同时采用基于抽样的方法进行规划。该方法首先预测障碍物信息，然后 选择自动驾驶汽车的导航策略。对于智驾车辆而言，需要穿过停车场的道路，但不执行停车 操作。相反，在采用更完整的综合泊车系统方法后，可以实现对环境进行短期和长期预测， 便可将其用于自主泊车战略规划。

A. 运动规划定义

考虑车辆动力学的规划问题可表示为：X’= f(X) + g(X; u); (1) 其中 X = [x;y;θ] T 表示二维坐标和车辆航向，u = [δ; v] T 是包括纵向速度和转向 角的控制输入。无碰撞配置空间 Cfree ⊂ R nc 是车辆与障碍物没有交叉点的配置集。本文考虑的运动规划问题可以定义为给定初始配置 X0 free、目标配置 Xf Cfree 和公 式(1)，找到可行轨迹 Pt，其 (1) 起始于 0 到 Xf 结束，同时满足（1）(2) 在于无碰撞配 置空间 Cfree。使用常见的自行车模型来表示车辆运动。通过欧拉离散化得到离散时间模型如下：

（2）

其中 Ts 是采样时间。

B. 系统架构

下图显示了所提出自动泊车系统架构。混合环境预测器和战略运动规划器是两个主要组 成部分。在运行时，中央控制首先处理停车场地图 Mmap 并生成初始长期轨迹 Pref。这里使用 搜索引导树 BIAGT，规划生成一条轨迹，使本自动驾驶车辆准确地到达目标，这是在狭窄空 间停车的一个重要特征。

混合环境预测器监视环境并预测障碍物的移动。根据预测，战略运动规划器检查自动驾 驶车辆是否违反了安全边际。如果不是，它会检查初始长期预测轨迹 Pref 是否由于障碍物 的运动需要被修复。如果发生任何这些情况，Pref 将被更新。最后，基于模型预测控制器 MPC 规划一个无碰撞运动，以跟踪动态环境中的最新的长期运动轨迹 Pref。如果修复规划器无法 成功优化可行驶的自车运动轨迹，则会请求中央域控更新地图并重新生成参考轨迹。

C. 混合环境预测器

混合环境预测器包含三个主要部分：障碍物的运动估计、运动预测和模式估计。接下来 对其主要的几个特性进行说明。

1）级联运动估计：

运动估计在基于独轮车模型从 (x; y) 的测量中重建状态 X。这样的处理对于涉及频繁 改变移动方向和转向动作的泊车过程是不够的。为了准确预测障碍物的短期运动，重构控制 输入 u 是有利的。可以将其视为未知输入估计问题或使用控制输入增加障碍物车辆 OV 的 系统状态并解决状态估计问题。假设障碍物运动按照模型 (2) 演化。通过假设控制输入 (δ; v) 是分段常数来获得 OV 的增广模型，并估计增广状态 [x;y;θ;δ;v] T。给定非线性增强模型，很自然地应用完善的非线性状态估计器（例如扩展的卡尔曼滤波 EKF 或粒子滤波器）进行状态估计。通过调整 EKF 来准确估计转向角其实并不简单，这部 分归因于涉及未测量状态乘法的项 v tan(δ)的存在多带来的。粒子滤波器虽好，但是其繁 重的计算也可能阻碍实际的运算。

短期运动预测：

短期运动预测更加关注计算效率和实时性，我们假设障碍物车辆 OV 的短期运动完全由 状态 Xcc 的平均值及其协方差捕获。通过向前传播估计状态为 Xcc，k 并获得短期预测 XH ;k = [X1 ,kT ,......,XH ,kT ] T 用于时间范围的未来 H 个步骤。同理，根据扩展卡尔曼滤波器 EKF 的前向预测公式进行前向传播得到协方差矩阵 PmH，k ={Pmk+1 ,......,Pmk+H }，这些信息将有助于 长期预测，并用于确定每个未来时间步长的安全裕度。

长期模式预测：

长期运动所关注的障碍物车辆 OV 在一段时间内的运动依赖于它的历史状态、动态模型 及其相对于环境的运动。其中前两个因素在某种程度上被短期运动预测捕获。为了利用相对 环境运动，可以引入成本图（如下图所示混合预测器预测短期 OV 轨迹（绿线）并将其与模 式预测结合使用以生成 h = 1 和 h = H 的安全裕度和安全边界。），以此来捕捉障碍物车 辆 OV 可能的长期运动。

同时，也可以使用路线规划器构建成本图路径 Mroute，其中成本图包含障碍物车辆 OV 所 采用的可能路线。

其中 Xmk;i 是模式中路线的第 i 个航路点； ，W1 和 W2 为权重矩阵。函数 f(mk ; Xcc;k ) 与 OV 转向角的大小以及 OV 航向角与路线最终航向角的 偏差成正比。最后，对进行归一化，得到 b(mk )，取最大置信度的 mk的值为 m^k。此外，停车场中的车辆通常以两种模式运行，“手动模式”和“巡航模式”。处于手动 模式的车辆有可能会频繁改变转向并偏离成本图中的路线（上图中的黑色虚线）以停放或离 开狭窄的停车位。处于巡航模式的车辆具有较小或稳定的转向角，并且通常遵循其中一条路 线。车辆首次进入停车场并接近停车位时或车辆离开停车位并离开停车场时处于该模式。

4）安全裕度和安全界限：

由于障碍物车辆 OV 在机动模式下的运动很难预测，因此预测器需要生成一个自车运动 的安全边界（OV 历史姿态的凸形边界，上图中的橙色线）导致规划模型将更加保守，确保 使自车远离难以预测的障碍物车辆 OV。请注意，考虑到运动学模型和路线信息，安全裕度 和安全边界也可以应用于其他移动障碍物，例如行人或摩托车。

5）策略运动规划器

使用参考轨迹 Pref，运动策略规划器运行的主要模块包括基于 MPC 的安全控制器和两 个支持模块：避撞轨迹规划器和修复轨迹规划器。如果自车当前位置和参考轨迹分别因自车 的运动而无效，则如上两个规划器都会被激活。A、基于 MPC 的安全控制器：安全控制器在给定安全余量和安全界限的情况下跟踪参考轨迹 Pref。借助 Pref，使用优 化基础规划器的方式来计算 MPC 框架中的跟踪轨迹。假设 Xref;k 为 Pref 的片段，设置时间 步长 k 进行跟踪。外部参照 k 将被选择和修剪，确保不会违反在所有模式要求的安全裕度 或者在“机动”模式下安全边界。整个轨迹跟踪问题可以表述为非凸优化问题的解决方式， 这里可以使用非线性规划求解器（例如 IPOPT）进行求解，以参考路径作为热启动，平均求 解时间约为 0:06 秒。B、避障规划：当自车停留在原始参考轨迹上可能会被认为不安全，此时需要启动避障规划程序。这种 情况可能发生在障碍物 OV 驶向自车时，其运动与之前的预测有很大不同——可能违反安全 裕度并造成安全威胁。因此，自车需要寻路避险。通常不建议采取后退运动，因为自车没有 安全目标。相反，探索前向可行驶环境来找到最佳目标才是可选的，因此我们提出了一种基 于搜索的避障规划器，它可以探索空间并快速找到避障轨迹。

6）轨迹修复规划器：

障碍物车辆 OV 的动作可能会使自车的参考轨迹无效。下图说明了这样一种情况，其中 由蓝色框表示的障碍物车辆 OV，停在自车的参考轨迹上，用浅蓝色线表示。

当前方区域不可行时，安全控制器将命令自车停在参考轨迹上。除非接收到新的参考轨 迹，否则安全控制器将控制自车停止运动并等待障碍物 OV 消失 - 如果 OV 长时间停止则认 为此次轨迹规划不合理或效率较低。此时应该更新参考轨迹 Pref，这样安全规划器就可以指 导自车绕过 OV 并合并回原来的路径。我们注意到修复后的轨迹通常与原始轨迹位于同一类 轨迹集合，这样可以直接通过基于优化当前轨迹来进行安全隐患修复生成可执行的轨迹。为 了快速获得修复路径，就需要在二维空间上进行轨迹规划的修复，即设置轨迹为 Xrepair = [x; y] T ,并相应地修改约束。由此产生的路径尽管没有碰撞，但不能确保循迹始终准确，从而可能引起自动驾驶汽车撞上障碍物。因此，只有当它通过运动学可行性检查时，才会被认可为 已验证的路径并接受为修复后的轨迹。如果修复失败，则通知中控接管修复任务。

总结

本文介绍了一种用于在存在其他移动车辆的情况下自动驾驶车辆 (AV) 泊车集成运动 规划系统。这一自动泊车系统包括预测周围车辆运动的混合环境预测器和对预测做出反应的 战略运动规划器。战略运动规划器包括三种：

用于轨迹跟踪的基于模型预测控制的安全控制器；

基于搜索的避撞规划器，用于在紧急情况下寻找避撞路径；

基于优化的修复规划器，用于在原始路径失效时规划新路径。

基于此，混合环境预测器通过扩展卡尔曼滤波器和自适应观察器执行短期预测。它还将短期预测与驾驶员行为成本图相结合，以做出长期预测。