[Paper Review] Interactive multi-modal motion planning with Branch Model Predictive Control

Yuxiao Chen, Ugo Rosolia+3 more|arXiv (Cornell University)|Sep 10, 2021

Real-time simulation and control systems6 citations

TL;DR

This paper proposes a Branch Model Predictive Control (MPC) framework that plans interactive, multi-modal motions for autonomous agents by modeling uncontrolled agents' reactive behaviors through a finite set of feedback policies. It constructs a scenario tree over these policies and solves for a trajectory tree with the same topology, using coherent risk measures like CVaR to balance performance and robustness, achieving human-like, safe, and efficient behaviors in simulation and real-world quadruped experiments.

ABSTRACT

Motion planning for autonomous robots and vehicles in presence of uncontrolled agents remains a challenging problem as the reactive behaviors of the uncontrolled agents must be considered. Since the uncontrolled agents usually demonstrate multimodal reactive behavior, the motion planner needs to solve a continuous motion planning problem under these behaviors, which contains a discrete element. We propose a branch Model Predictive Control (MPC) framework that plans over feedback policies to leverage the reactive behavior of the uncontrolled agent. In particular, a scenario tree is constructed from a finite set of policies of the uncontrolled agent, and the branch MPC solves for a feedback policy in the form of a trajectory tree, which shares the same topology as the scenario tree. Moreover, coherent risk measures such as the Conditional Value at Risk (CVaR) are used as a tuning knob to adjust the tradeoff between performance and robustness. The proposed branch MPC framework is tested on an overtake and lane change task and a merging task for autonomous vehicles in simulation, and on the motion planning of an autonomous quadruped robot alongside an uncontrolled quadruped in experiments. The result demonstrates interesting human-like behaviors, achieving a balance between safety and performance.

Motivation & Objective

• To address the challenge of planning safe and efficient trajectories for autonomous agents interacting with uncontrolled agents exhibiting multimodal, reactive behaviors.
• To overcome the conservatism of traditional robust planning by incorporating future reactivity of uncontrolled agents into the motion planning process.
• To enable tractable motion planning under uncertainty by approximating continuous reactive behaviors with a finite set of feedback policies.
• To improve robustness against predictive model inaccuracies using coherent risk measures such as Conditional Value at Risk (CVaR).
• To demonstrate the framework's effectiveness in both autonomous vehicle tasks and real-world quadruped robot motion planning.

Proposed method

• Construct a scenario tree from a finite set of feedback policies representing the multimodal reactive behaviors of uncontrolled agents, with probabilities derived from a predictive model.
• Formulate a branch MPC problem that computes a trajectory tree with identical topology to the scenario tree, optimizing over feedback policies.
• Use the expectation of cost over all branches, weighted by their probabilities, as the primary objective function.
• Incorporate Conditional Value at Risk (CVaR) as a risk measure to tune the tradeoff between performance and robustness under model uncertainty.
• Apply sequential quadratic programming (SQP) to solve the resulting optimization problem, with dual formulation of CVaR for computational tractability.
• Enable event-triggered, asynchronous branching to improve realism, though current implementation uses fixed-interval branching.

Experimental results

Research questions

• RQ1 Can a motion planning framework effectively model and leverage the multimodal reactive behaviors of uncontrolled agents in real-time?
• RQ2 How can feedback policies be synthesized over a scenario tree to achieve robust and high-performing motion plans?
• RQ3 To what extent can coherent risk measures like CVaR improve robustness when the predictive model of uncontrolled agents is inaccurate?
• RQ4 Can the proposed branch MPC framework generate human-like, balanced behaviors that are both safe and performance-oriented in interactive environments?
• RQ5 How does the framework scale with multiple uncontrolled agents, and can joint behavior modeling reduce computational complexity?

Key findings

• The branch MPC framework successfully generates motion plans that balance safety and performance by anticipating future reactivity of uncontrolled agents.
• The use of CVaR as a tuning knob allows the planner to adjust robustness, reducing over-conservatism compared to standard robust MPC.
• In autonomous vehicle simulations, the method achieved safe and efficient overtake and lane change maneuvers while respecting human-like interaction patterns.
• The framework demonstrated real-world feasibility by enabling a quadruped robot to navigate alongside an uncontrolled quadruped in physical experiments.
• The resulting behaviors were observed to be human-like, avoiding excessive caution while maintaining safety margins.
• The method shows promise for extension to event-triggered branching and belief-based observation integration, though current implementation assumes fixed branching intervals and no prior observation history.