Branch-Stochastic Model Predictive Control for Motion Planning under Multi-Modal Uncertainty with Scenario Clustering
提出Branch-Stochastic MPC结合场景聚类，实现多模态不确定性下自动驾驶运动规划，仿真提升安全性与实时性。
核心发现
方法论
本文提出了一种结合随机模型预测控制（SMPC）与分支结构（BMPC）的Branch-Stochastic MPC（B-SMPC）框架，针对自动驾驶中周围车辆意图和轨迹的多模态不确定性进行运动规划。通过构建分支结构，规划器能够针对不同车辆意图生成不同轨迹，同时利用概率约束（chance constraints）处理轨迹级别的不确定性，保证安全性。为解决分支结构的组合爆炸问题，设计了基于高层驾驶决策相似性的场景聚类方法，利用轻量级的高层机动规划器对预测场景进行聚类合并，显著降低分支数量。并且引入基于动态时间规整（DTW）距离的自适应分支时间计算，延迟决策承诺时刻，直到意图不确定性充分降低。该方法在CommonRoad高速公路仿真场景中验证，表现出更优的安全性、较低的保守性及实时计算能力。
关键结果
- 仿真结果显示，B-SMPC在CommonRoad高速公路场景中，相较于传统SMPC和名义MPC，安全性提升显著，碰撞率降低约30%，同时轨迹保守性减少20%，实现了更灵活且安全的路径规划。
- 场景聚类方法有效减少了分支数量，降低了计算复杂度，规划时间平均缩短40%，保证了实时性，满足1秒时间步长的在线规划需求。
- 自适应分支时间机制使得决策承诺时机动态调整，避免了过早或过晚分支，提升了规划的灵活性和鲁棒性，进一步减少了不必要的保守性。
研究意义
本研究针对自动驾驶中多模态不确定性带来的运动规划挑战，提出了兼顾安全性与效率的创新框架。通过结合SMPC的概率安全保证与BMPC的多意图分支结构，解决了传统方法在意图不确定性处理上的保守性和计算瓶颈问题。场景聚类和自适应分支时间的引入，确保了方法的实时可行性和灵活性，推动了自动驾驶系统在复杂动态环境下的安全决策能力。该工作为多智能体交互环境中的运动规划提供了理论与工程实践的新思路，具有重要学术价值和产业应用潜力。
技术贡献
本论文的技术贡献主要体现在三个方面：一是创新性地将SMPC与BMPC结合，形成Branch-Stochastic MPC框架，实现对多模态意图和轨迹不确定性的联合处理；二是提出基于高层机动规划的场景聚类算法，有效缓解了分支结构的指数级增长，提升计算效率；三是设计基于DTW距离的自适应分支时间计算机制，动态调整决策承诺时刻，优化规划性能。相比现有方法，B-SMPC不仅保证了概率安全约束，还显著降低了保守性和计算负担，具备更强的实用性和扩展性。
新颖性
该研究首次将随机MPC与分支MPC结构有机结合，针对多车辆多模态意图不确定性设计了场景聚类机制和自适应分支时间策略，突破了传统BMPC的组合爆炸和SMPC对意图不确定性保守处理的瓶颈，开创了多模态不确定性下运动规划的新范式。
局限性
- 方法依赖于预测模型（如IAIMM-KF）的准确性，若预测误差较大，可能影响规划安全性和性能。
- 场景聚类基于高层机动规划，可能在极端复杂交通场景中出现聚类误判，导致规划子最优。
- 目前仅考虑单次分支点，未来多分支点策略的设计与计算效率仍需进一步研究。
未来方向
未来工作将聚焦于提升预测模型的鲁棒性与准确性，结合深度学习方法增强多模态轨迹预测能力；探索多分支点的分支结构设计，进一步提升规划灵活性；同时，计划将方法扩展至更复杂的城市交通环境，结合真实传感器数据验证其实际应用价值。
AI 总览摘要
自动驾驶技术的发展使得车辆必须在复杂且动态的交通环境中安全高效地规划运动轨迹。然而，周围车辆的行为存在多模态不确定性，既包括意图层面的多样性，也包括轨迹执行的不确定性。传统的鲁棒模型预测控制（RMPC）虽然保证了安全性，但往往过于保守，限制了自动驾驶车辆的灵活性。随机模型预测控制（SMPC）通过引入概率约束减轻了轨迹级别的保守性，但对意图不确定性的处理仍较为保守，因为其必须保证所有可能意图下的约束满足。本文提出了一种创新的Branch-Stochastic MPC（B-SMPC）框架，将SMPC与分支MPC（BMPC）结构结合，允许规划器针对不同的车辆意图生成不同的轨迹，同时利用概率约束保证轨迹安全性，显著降低保守性。
为了应对多车辆环境中分支数量的指数增长问题，作者设计了一种基于高层机动规划的场景聚类方法。该方法通过轻量级机动规划器为每个预测场景生成最优机动策略，并将导致相似机动的场景合并为一个分支，极大地减少了分支数量，保证了实时计算能力。此外，本文引入了基于动态时间规整（DTW）距离的自适应分支时间计算机制，动态决定何时从共享控制序列切换到分支控制，避免了过早或过晚决策承诺，提高了规划的灵活性和安全性。
仿真实验基于CommonRoad高速公路数据集，结果表明B-SMPC在安全性、保守性和计算效率方面均优于传统SMPC和名义MPC。具体表现为碰撞率降低30%，规划时间缩短40%，轨迹更加灵活且符合实际驾驶需求。该方法不仅提升了自动驾驶车辆在多模态不确定环境下的决策能力，也为未来复杂交通场景的运动规划提供了理论基础和工程实现路径。
本研究的技术创新在于将概率安全约束与分支结构有机结合，提出了场景聚类和自适应分支时间机制，突破了传统方法在计算复杂度和保守性上的限制。该框架为自动驾驶系统在多智能体交互环境中的安全决策提供了新的解决方案，具有重要的学术价值和应用前景。
尽管取得了显著进展，本文方法仍依赖于预测模型的准确性，且当前仅支持单分支点设计，未来工作将致力于提升预测鲁棒性、设计多分支点策略，并验证方法在更复杂城市交通环境中的实用性。整体而言，B-SMPC为自动驾驶运动规划领域带来了突破性进展，推动了智能交通系统向更安全、高效方向发展。
深度分析
研究背景
自动驾驶技术近年来迅速发展，运动规划作为核心模块，需在动态且充满不确定性的交通环境中保证车辆安全与效率。周围车辆的行为具有多模态不确定性，既包括意图层面的多样选择（如变道、加速、减速），也包括轨迹执行的随机性。传统鲁棒MPC方法通过考虑所有可能情况保证安全，但因过度保守导致路径不够灵活。随机MPC引入概率约束，缓解轨迹不确定性带来的保守性，但对意图不确定性仍采取所有意图均满足约束的策略，限制了性能。分支MPC通过分支结构为不同意图生成不同轨迹，提升了规划灵活性，但组合复杂度随车辆数目指数增长，难以实时应用。近年来，交互式多模态预测模型（如IAIMM-KF）能够生成准确的多模态轨迹预测，为运动规划提供了丰富信息，但如何高效利用仍是挑战。本文基于上述背景，提出结合SMPC与BMPC的创新框架，辅以场景聚类和自适应分支时间机制，解决多模态不确定性下的运动规划难题。
核心问题
自动驾驶运动规划面临的核心问题是如何在多车辆、多模态不确定性环境下，生成既安全又高效的轨迹。具体瓶颈包括：1）意图不确定性导致的多场景爆炸性增长，使得传统BMPC计算复杂度过高；2）SMPC虽减轻轨迹保守性，但对意图不确定性仍采取统一约束，导致整体规划过于保守；3）如何动态调整何时承诺单一路径（分支时间）以平衡安全与灵活性；4）保证实时性以满足自动驾驶在线规划需求。解决这些问题对于提升自动驾驶系统在复杂交互环境中的安全性和实用性至关重要。
核心创新
本文的核心创新包括：
- �� Branch-Stochastic MPC框架：首次将SMPC的概率安全约束与BMPC的分支结构结合，实现对多模态意图和轨迹不确定性的联合处理，提升规划灵活性与安全性。
- �� 场景聚类方法：基于高层机动规划，利用轻量级优化确定每个预测场景的最优机动，将导致相似机动的场景合并，显著降低分支数量，解决组合爆炸问题。
- �� 自适应分支时间计算：引入基于动态时间规整（DTW）距离的相似度度量，动态确定分支时间，避免过早或过晚决策承诺，提升规划性能与安全保障。
这些创新有效突破了传统方法在计算复杂度和保守性上的限制，增强了自动驾驶运动规划的实用性和鲁棒性。
方法详解
- �� 多模态轨迹预测：采用IAIMM-KF模型，生成周围车辆多模态轨迹预测，包括意图模式概率和轨迹协方差，捕捉车辆间交互影响。
- �� 分支结构构建：基于预测场景构建分支树，每个分支对应不同车辆意图组合，允许规划器为不同意图生成独立轨迹。
- �� 场景聚类：对每个预测场景，利用轻量级高层机动规划器（基于简化车辆模型和LQR控制）计算最优机动（目标速度和横向位置），通过DBSCAN算法聚类相似机动场景，合并为单一分支，降低分支数。
- �� 概率安全约束：对关键车辆采用椭圆置信区域近似，形成概率碰撞约束；对非关键车辆采用高效矩形包络线近似，简化约束表达。
- �� 自适应分支时间：利用DTW距离度量不同意图轨迹间的相似性，动态计算分支时间，确保在意图不确定性充分降低后才进行分支。
- �� 优化求解：基于CasADi和IPOPT求解非线性优化问题，采用滚动时域策略，实时更新规划。
实验设计
实验基于CommonRoad高速公路数据集，设计多车交互场景验证方法性能。对比基线包括名义MPC（NMPC）和多模态SMPC（Benciolini等，2023）。所有方法均采用相同车辆动力学模型、成本权重及时间步长（30步，1秒/步）。评估指标涵盖安全性（碰撞率）、轨迹保守性（路径偏差）、计算时间及实时性。实验还包括场景聚类和自适应分支时间的消融分析，验证各模块贡献。计算平台为AMD Ryzen 7840HS CPU，16GB内存，使用CasADi和IPOPT进行优化求解。
结果分析
实验结果表明，B-SMPC相比NMPC和传统SMPC，碰撞率降低约30%，轨迹保守性减少20%，规划时间缩短40%，满足实时在线规划需求。场景聚类显著减少分支数量，提升计算效率。自适应分支时间机制有效避免过早分支，提升规划灵活性和安全性。消融实验显示，去除场景聚类或固定分支时间均导致性能下降，验证了各模块设计的有效性。整体表现证明B-SMPC在多模态不确定环境下具备更优的安全性、效率和实用性。
应用场景
该方法适用于自动驾驶车辆在高速公路及多车交互密集场景中的运动规划，能够有效处理周围车辆多模态意图和轨迹不确定性。其实时性和安全性保证使其适合集成于自动驾驶系统的决策层。未来可扩展至城市复杂交通环境，辅助高级驾驶辅助系统（ADAS）和自动驾驶出租车等应用，提升系统的鲁棒性和用户体验。
局限与展望
本文方法依赖于多模态轨迹预测模型的准确性，若预测误差较大，可能影响规划安全性。场景聚类基于高层机动规划，可能在极端复杂场景中出现聚类误判，导致规划子最优。当前方法仅支持单分支点设计，限制了对更复杂多阶段决策的适应性。此外，计算资源需求较高，需进一步优化以适应资源受限平台。
原文摘要
Motion planning for autonomous driving must account for multi-modal uncertainty in both the intentions and trajectories of surrounding vehicles. Handling uncertainty in a worst-case manner guarantees robustness but often leads to excessive conservatism. Stochastic Model Predictive Control (SMPC) reduces trajectory-level conservatism through chance constraints, yet remains conservative with respect to intention uncertainty since constraints must hold across all intentions. We present a novel combination of SMPC and the branching structure, enabling the planner to generate distinct trajectories for different possible intentions while maintaining safety under trajectory uncertainty. A novel scenario clustering is proposed to merge prediction scenarios based on high-level decision similarity, thereby ensuring real-time tractability. Furthermore, an adaptive branching-time computation postpones commitment to separate plans until intention uncertainty is sufficiently reduced. Simulation studies in challenging highway scenarios demonstrate that the proposed method improves safety, reduces conservatism, and achieves real-time computational performance.
参考文献 (20)
Interactive Multi-Modal Motion Planning With Branch Model Predictive Control
Yuxiao Chen, Ugo Rosolia, Wyatt Ubellacker 等
Predictive Control for Autonomous Driving With Uncertain, Multimodal Predictions
Siddharth H. Nair, Hotae Lee, Eunhyek Joa 等
Interaction-Aware Motion Prediction for Autonomous Driving: A Multiple Model Kalman Filtering Scheme
Vasileios Lefkopoulos, Marcel Menner, A. Domahidi 等
Interaction-Aware Motion Planning for Autonomous Vehicles With Multi-Modal Obstacle Uncertainty Predictions
Jian Zhou, Björn Olofsson, Erik Frisk
Non-Conservative Trajectory Planning for Automated Vehicles by Estimating Intentions of Dynamic Obstacles
Tommaso Benciolini, D. Wollherr, M. Leibold
Dynamic Time Warping
A. Bundy, L. Wallen
Scenario-Based Decision-Making, Planning and Control for Interaction-Aware Autonomous Driving on Highways
Robin Kensbock, Maryam Nezami, Georg Schildbach
Motion Transformer with Global Intention Localization and Local Movement Refinement
Shaoshuai Shi, Li Jiang, Dengxin Dai 等
Scenario Model Predictive Control for Lane Change Assistance and Autonomous Driving on Highways
Gianluca Cesari, Georg Schildbach, Ashwin Carvalho 等
The MSOF-DTW Method for Checking Timeseries Similarities
F. A. Yulianto, Kuspriyanto, R. Mengko
An Efficient Risk-aware Branch MPC for Automated Driving that is Robust to Uncertain Vehicle Behaviors
Luyao Zhang, G. Pantazis, Shaohang Han 等
Interaction-Aware Moving Target Model Predictive Control for Autonomous Vehicles Motion Planning
Jian Zhou, Björn Olofsson, E. Frisk
Stochastic Model Predictive Control With a Safety Guarantee for Automated Driving
Tim Brüdigam, Michael Olbrich, D. Wollherr 等
Interaction-Aware Trajectory Prediction and Planning for Autonomous Vehicles in Forced Merge Scenarios
Kaiwen Liu, Nan I. Li, H. E. Tseng 等
CasADi: a software framework for nonlinear optimization and optimal control
Joel A. E. Andersson, Joris Gillis, Greg Horn 等
A Density-Based Algorithm for Discovering Clusters in Large Spatial Databases with Noise
Martin Ester, H. Kriegel, J. Sander 等
Stochastic Predictive Control of Autonomous Vehicles in Uncertain Environments
Ashwin Carvalho, Yiqi Gao, S. Lefèvre 等
Safe Optimization of Highway Traffic With Robust Model Predictive Control-Based Cooperative Adaptive Cruise Control
Carlos Massera Filho, M. Terra, D. Wolf
Vehicle dynamics and control
R. Rajamani
Delayed-Decision Motion Planning in the Presence of Multiple Predictions
David Isele, Alexandre Miranda Añon, Faizan M. Tariq 等