MARC：一种安全又高效的时空联合规划的方法

论文标题：
MARC: Multipolicy and Risk-aware Contingency Planning for Autonomous Driving

论文作者：
Tong Li, Lu Zhang, Sikang Liu, Shaojie Shen

导读： 论文来自港科大的研究成果，主要讲时空联合contingency planning问题。是《EPSILON: An Efficient Planning System for Automated Vehicles in Highly Interactive Environments》工作的进一步扩展研究。©️【深蓝AI】编译

1 论文概览

在高度交互的环境中完成安全和高效的规划任务是一项具有挑战的任务，这是由于交通参与者的随机性以及它们与自车的隐式互动，无法直接观测它们的多模态的意图。对于这种交互决策问题，一般有两种主流方法：第一种是马尔可夫决策过程（MDP：Markov decision process）和 部分可观察马尔可夫决策过程（POMDP： partially observable Markov decision process），这种方法存在一个缺点，随着问题规模的增加（也就是参与交互的障碍物越多），计算量剧增；为了减少计算量，可以使用规则进行剪枝，将原始问题分解成有限数量的闭环策略评估。然而，剪枝的缺点是，很难设计出好的规则去适应所有的场景。

第二种方法是数值优化方法（如：contigency planning），它是在运动规划层为多种可能的未来生成应急计划。通过优化树状（tree-structured）轨迹来处理环境不确定性，其中每个分支都好考虑了潜在的风险，但contigency planning 对周围障碍物的选择是一个问题。

而为了解决上述问题，本文提出的MARC算法，结合前面两种方法的优点，将行为和运动规划层进行组合。首先利用前向模拟（Forward Simulations）生成基于语义级别的关键场景集（具有动态分支点的场景树，使用分叉点减少相同部分的计算量），然后使用Risk-aware Contiency Planning(使用线性规划器LP和迭代线性二次调节器iLQR)进行优化，生成考虑多样化风险和未来场景的轨迹树，最后进行cost计算，选择出合适的场景和轨迹。

2 相关工作

不确定性下的交互规划问题：虽然POMDP提供了一个理论上完备的框架，但由于维度灾难，实时解决POMDP在复杂环境中不可行。为了实现实时决策，基于多策略的方法利用自动驾驶的特定知识简化POMDP过程（如树结构剪枝）。尽管目前会考虑多策略驱动模型（MPDM）这一思路（具体实现如： EUDM， EPSILON, TPP），但仍存在策略一致性和多模态信息丢失的问题。

Motion Planning with Contingency: 基于优化方法的Contingency planning，为多个未来场景生成确定性动作，然后利用模型预测控制（MPC）来解决一个共享状态的分支轨迹树，以跟踪期望路径并考虑潜在的风险。类似地，可以利用iLQR生成灵活且安全的行为。然而现有方法在在低计算成本下生成有效的复杂环境中的交互方面存在限制，主要是由于缺乏决策层的指导。此外，这些方法的固定或预定义的情景树在实际应用中的泛化能力不强。

MARC正是系统的结合多策略决策和应急规划的来解决以上问题。

3 技术要点

3.1 Syetem Overview

如图1所示为MARC框架的规划流程，首先基于历史观察和环境背景预测附近车辆的未来轨迹（作者这里通过基于神经网络的运动预测器来预测生成）和语义级别意图（即, 保持速度， 加速和减速灯纵向的意图；以及参考车道序列的横向意图），然后ego policy 前向推演获得很多场景，然后生成场景树，在这基础上采用RCP（Risk-aware Contigency Planning）进行优化，然后轨迹评估出最优策略。

图1｜MARC算法框架©️【深蓝AI】编译

MARC算法流程见图2所示。

图2｜MARC算法流程©️【深蓝AI】编译

3.2 Policy-conditioned Scenario Tree

通过树结构来构建场景，可以构建动态场景树，以实现场景的泛化能力。场景的生成包括了自车的策略以及周围障碍物的意图预测。根据构建好的场景树，然后使用剪枝的方法剪去不必要的障碍物，然后使用前向模拟建立轨迹，但是这种方法缺乏对极端环境下的泛化能力。通过FRSs(forward reachable sets)来开环泛化场景，然后再对FRSs的结果进行闭环反馈，最后再用树结构对场景进行安全保障。具体来说，通过直接计算多模态预测的时空占用，并根据时间戳收集占用联合以形成FRSs。

图3｜场景树推演过程©️【深蓝AI】编译

有了各个推演出去的场景，来生成场景树（如图3(a)所示），通过最小分叉阈值进行生成（在时间$t$时，给定情景对$(s_i,s_j)$, 并通过限制最大分支时间${\tau }_{max}$共享部分的长度）：

m a x   τ ∈ { 0 , … , τ m a x } s . t .   ∀ M ( s i , s j , τ ) < θ ,    ( s i , s j ) ∈ { S × S } \begin{align} max \ {\tau} \in \{ 0, \dots, \tau_{max}\} \\ s.t.\ \forall M(s_i,s_j, \tau) \lt \theta, \ \ (s_i,s_j) \in \{S \times S\} \end{align} max τ∈{0,…,τmax​}s.t. ∀M(si​,sj​,τ)<θ,  (si​,sj​)∈{S×S}​​左右滑动查看完整公式

3.3 Risk-aware Contingency Planning

Contingency Planning目的是获得一个考虑所有可能周围演变的轨迹树，将安排$K$个contigency planing的规划问题定义为一个受约束的非线性优化问题（如图3(b)所示的轨迹树）：

min ⁡ U = ∑ j ∈ I s l j ( x j , u j ) + ∑ k ∈ K ∑ j ∈ I k l j ( x j , u j ) s . t .    x i = f ( p r e ( x i ) , u i ) ,     ∀ i ∈ I    { 0 } h i ( x i , u i ) ≤ 0 , ∀ i ∈ I \begin{align} \min_{U} = \sum\limits_{j \in I_s}l_j(x_j, u_j) + \sum\limits_{k \in K}\sum\limits_{j \in I_k}l_j(x_j, u_j) \\ s.t. \ \ x_i = f(pre(x_i), u_i), \ \ \ \forall_{i} \in I \ \ \{0\} \\ h_i(x_i, u_i) \le 0, \forall_{i} \in I \end{align} Umin​=j∈Is​∑​lj​(xj​,uj​)+k∈K∑​j∈Ik​∑​lj​(xj​,uj​)s.t.  xi​=f(pre(xi​),ui​),   ∀i​∈I  {0}hi​(xi​,ui​)≤0,∀i​∈I​​左右滑动查看完整公式

增加两层次的风险保障策略，框架种引入条件风险价值（CVaR）作为风险评估的措施，即给定一个具有关联概率 { p k ∣ k ∈ K } \{p_k \mid k \in K\} {pk​∣k∈K}和风险成本 { ξ k ∣ k ∈ K } \{ξ_k|k \in K\} {ξk​∣k∈K}的离散随机变量集$K$。

$$\begin{array}{r}C{V}_a{R}_{\alpha }=\underset{Q}{\max }\sum q_k{q}_k{\xi }_k\\ s.t.0\le q_k\le (1-\alpha {)}^{-1},k\in K\\ \sum q_k{p}_k=1,\end{array}$$左右滑动查看完整公式

然后将CVaR和Contingency planning相结合，得到RCP的优化目标函数（如下公式），通过改变$\alpha$（用户的风险的容忍水平），从而获得多样化的驾驶行为（如图3©所示）。

max ⁡ Q min ⁡ U = ∑ j ∈ I s l j ( x j , u j ) + ∑ k ∈ K ∑ j ∈ I k ( p k q k l j s a f e ( x j , u ) j ) + l j − s a f e ( x j , u j ) ) s . t .    x i = f ( p r e ( x i ) , u i ) ,     ∀ i ∈ I    { 0 } h i ( x i , u i ) ≤ 0 , ∀ i ∈ I , 0 ≤ q k   ≤ α − 1 , k ∈ K ∑ q k p k = 1 , \begin{align} \max_{Q}\min_{U} = \sum\limits_{j \in I_s}l_j(x_j, u_j) + \\ \sum\limits_{k \in K}\sum\limits_{j \in I_k}(p_kq_kl_j^{safe}(x_j, u)j) + l_j^{-safe}(x_j, u_j)) \\ s.t. \ \ x_i = f(pre(x_i), u_i), \ \ \ \forall_{i} \in I \ \ \{0\} \\ h_i(x_i, u_i) \le 0, \forall_{i} \in I, \\ 0 \le q_k \ \le \alpha^{-1}, k \in K \\ \sum\limits_{}q_kp_k = 1, \end{align} Qmax​Umin​=j∈Is​∑​lj​(xj​,uj​)+k∈K∑​j∈Ik​∑​(pk​qk​ljsafe​(xj​,u)j)+lj−safe​(xj​,uj​))s.t.  xi​=f(pre(xi​),ui​),   ∀i​∈I  {0}hi​(xi​,ui​)≤0,∀i​∈I,0≤qk​ ≤α−1,k∈K∑​qk​pk​=1,​​左右滑动查看完整公式

正如上面公式，变成了maxmin 问题（对偶形式）。它采用了双层优化方法，通过使用iLQR和LP迭代地解决两个子问题来获得原始问题的解决方案。具体的，这里面的部分去固定的Q的iLQR问题，而外面的部分需要求去最大化Q的最优化U问题，具体的求解是用LP去求解。iLQR 利用泰勒展开式来获得近似解。

给定固定的风险相关变量$Q$，我们可以得到控制序列U的近似子问题如下：

min ⁡ U ∑ j ∈ I s ( 1 2 x j T R i x j + r j T x j + 1 2 u j T S j u j + s j T u j ) + ∑ k ∈ K ∑ j ∈ I k p k q k ( 1 2 x j T R j s a f e T x j + r j s a f e T x j ) + ∑ k ∈ K ∑ j ∈ I k ( 1 2 x j T R j − s a f e x j + r j − s a f e T x j + 1 2 u j T S j u j + s j T u j ) s . t .    x i = A i p r e ( x i ) + B i u i ,     ∀ i ∈ I { 0 } \begin{align} \min_{U} \sum\limits_{j \in I_s }(\frac{1}{2}x_j^{T}R_ix_j + r_j^{T}x_j + \frac{1}{2}u_j^{T}S_ju_j + s_j^{T}u_j) + \\ \sum\limits_{k \in K} \sum\limits_{j \in I_k}p_kq_k(\frac{1}{2}x_j^{T}R_j^{safe^{T}}x_j + r_j^{safe^{T}}x_j) + \\ \sum\limits_{k \in K} \sum\limits_{j \in I_k}(\frac{1}{2}x_j^{T}R_j^{-safe}x_j + r_j^{-safe^{T}}x_j + \frac{1}{2}u_j^{T}S_ju_j + s_j^{T}u_j) \\ s.t. \ \ x_i = A_ipre(x_i) + B_iu_i, \ \ \ \forall_{i} \in I\{0\} \end{align} Umin​j∈Is​∑​(21​xjT​Ri​xj​+rjT​xj​+21​ujT​Sj​uj​+sjT​uj​)+k∈K∑​j∈Ik​∑​pk​qk​(21​xjT​RjsafeT​xj​+rjsafeT​xj​)+k∈K∑​j∈Ik​∑​(21​xjT​Rj−safe​xj​+rj−safeT​xj​+21​ujT​Sj​uj​+sjT​uj​)s.t.  xi​=Ai​pre(xi​)+Bi​ui​,   ∀i​∈I{0}​​左右滑动查看完整公式

最后，我们可以这样理解：对于任何固定的控制序列U，原始问题都会退化为一个LP。解决LP会在额外的等式约束下找到具有最优变量Q的CVaR。对于任何固定的Q，原始问题可以通过iLQR进行近似。解决iLQR子问题可以获得轨迹树的最优控制U。由于这两个子问题都是凸问题，我们可以推导出一个具有收敛保证的双层优化问题。

$$\begin{array}{r}{U}^{k+1}=\arg \underset{U}{\min }iLQR(X^k,U^k,Q^k)\\ Q^{k+1}=\arg \underset{Q}{\max }LP(X^{k+1},U^{k+1},Q^k)\end{array}$$左右滑动查看完整公式

对于iLQR种的目标函数设计，包括了四个部分（如: safety cost, target cost, kinematic cost 和 comfort cost）：

$$\begin{array}{r}{l}_i=l_i^{safe}+l_i^{tar}+l_i^{kin}+l_i^{comf}\end{array}$$

1）Safety cost

$$\begin{array}{r}{l}_i^{safe}=\stackrel{drivablearea}{\overbrace{{\mathcal{G}}_{dri}((1-1_{{\mathcal{P}}_d}){\mathcal{S}}_{\beta <0}(\mathcal{D}({\mathcal{P}}_d)))}}+\\ \stackrel{boundingbox}{\overbrace{\sum _{j\in n_p}{\mathcal{G}}_{bb}(1_{{\mathcal{P}}_{b_j}}{\mathcal{S}}_{\beta <0}(\mathcal{D}({\mathcal{P}}_{b_j})))}}+\stackrel{reachableset}{\overbrace{\sum _{j\in n_p}{\mathcal{G}}_{rs}(1_{{\mathcal{P}}_{r_j}}{\mathcal{S}}_{\beta <0}(\mathcal{D}({\mathcal{P}}_{r_j})))}}\end{array}$$左右滑动查看完整公式

2）arget cost

$$\begin{array}{r}{l}_i^{tar}={\mathcal{G}}_{ref}({\mathcal{S}}_{\beta <0}(\mathcal{D}({\mathcal{L}}_{ref})))+{\mathcal{G}}_{des}({\mathcal{S}}_{\beta <0}(\mathcal{D}({\mathcal{P}}_{des})))\end{array}$$左右滑动查看完整公式

3）Kinemetic cost

$$\begin{array}{r}{l}_i^{kin}={\mathcal{G}}_{xub}(max(x_t-x_{ub},0))+{\mathcal{G}}_{xlb}(max(x_{lb}-x_t,0))\\ +{\mathcal{G}}_{uub}(max(u_t-u_{ub},0))+{\mathcal{G}}_{ulb}(max(u_{lb}-u_t,0))\end{array}$$左右滑动查看完整公式

4）Comfort cost

$$l_i^{comf}={\mathcal{G}}_{lon}(x_t)+{\mathcal{G}}_{lat}(x_t)$$

最后在策略评估部分，总策略奖励基于情景背景和风险感知应急规划进行评估。具体而言，策略的奖励被定义为几个组成部分的负加权和。

$$R=-({\lambda }_1{F}_s+{\lambda }_2{F}_e+{\lambda }_3{F}_n+{\lambda }_4{F}_r+{\lambda }_5{F}_u)$$左右滑动查看完整公式

其中，安全成本$F_s$由自我车辆轨迹与其他车辆轨迹之间的距离衡量。效率成本$F_e$由应急规划的平均速度与期望速度之间的差距计算得到。导航成本$F_n$由路由和目标车道序列之间的差异得到。风险成本$F_r$由RCP的成本获得，不确定性成本$F_u$则由分支时间和不同应急规划之间的差异度衡量。

4 实验结果

4.1 定量分析

1）与Epsilon对比的结果：

如图4所示的三种场景：

●场景(I)是一个非合作的变道场景，旨在测试处理极端情况的能力；

●场景(II)是一个噪声s注入测试，在测试中，相邻车辆在保持车道的同时，向自我车辆的预测模块注入噪声，增加了变道预测；

●场景(III)是一个具有多个自主代理的高速公路场景，旨在评估在密集不确定交互下的规划能力。

图4｜实验对比场景©️【深蓝AI】编译

如表1所示，方法MARC在所有指标上均优于Epsilon方法。

表1｜与EPSILON的实验结果对比©️【深蓝AI】编译

图5显示了三种场景的动态轨迹，以进行并排比较。MARC在面对多个预测时更加谨慎，在场景I中加速更加平滑，在时间内减速。在场景II中，MARC在嘈杂的预测下执行更一致的策略，而EPSILON切换策略导致突然减速。在场景III中，MARC比EPSILON更好地处理不确定交互，减少了不必要的减速，并且平均速度更高，提高了舒适度和效率。

图5｜三种场景的动态轨迹对比©️【深蓝AI】编译

2）RCP消融实验结果：

表2｜消融实验结果©️【深蓝AI】编译

在图4（a）场景种，通过修改场景进行消融实验，实验结果表明动态分支优于固定分支，因为它更好地适应了场景。将风险措施与动态分支结合起来，带来了更为保守但更平滑的驾驶风格，这在各种方法中表现最佳，证明了RCP的有效性。

4.2 定性分析

我们在高度互动的场景（如无信号的左转和绕行对向车流）上进行定性测试，其中在自建平台上进行无信号的左转测试，而在CARLA上进行随机交通的场景测试，以进一步验证我们方法的泛化性能。

图6｜定性实验结果©️【深蓝AI】编译

**1）无信号的左转（如图6(a-d)所示）：**结果显示MARC在处理不确定意图并在交涉过程中生成合理行为的能力。

**2）绕行对向车流（如图(e-h)所示）：**结果显示了MARC在冲突情况下在时空约束下进行有效行为生成的能力。

**3）在CARLA上进行定性测试(如图7所示)：**MARC在测试中生成了类似人类的驾驶行为，例如在谈判中进行的防御性左转和绕行，证明了它在各种场景中的泛化能力。

图7｜在CARLA环境中实验结果©️【深蓝AI】编译

5 总结

本文提出的MARC算法，结合了多策略的场景树和风险感知的Contingency Planning的优点, 考虑了风险等级的容忍度和不确定性来生成安全且互动性的驾驶行为。它的主要特点是场景和规划都是基于树结构，并且能够充分利用到多策略和多模式信息，使得获得的最优策略是针对所有场景的最优结果。

编译｜巴巴塔

审核｜Los

移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态。