Embedded Hierarchical MPC for Autonomous Navigation

TL;DR

This work addresses the challenge of real-time, collision-free autonomous navigation with nonlinear dynamics on embedded hardware. It introduces a hierarchical MPC (HMPC) framework that co-designs a long-horizon planning MPC (PMPC) with a fast tracking MPC (TMPC), both using the same nonlinear robot model, and proves collision avoidance and recursive feasibility in static environments. The TMPC relies on offline terminal ingredients obtained via a semidefinite program and a tightened, ellipsoidal terminal set, while the PMPC generates obstacle-avoiding reference trajectories using an Iterative-DecompUtil-based constraint generator, ensuring online feasibility. The approach yields a fivefold increase in planning horizon compared to a single-layer MPC, demonstrates real-time feasibility on an embedded quadrotor platform, and shows robust performance under model mismatch, with open-source code and ROS integration for reproducibility and practical deployment.

Abstract

To efficiently deploy robotic systems in society, mobile robots must move autonomously and safely through complex environments. Nonlinear model predictive control (MPC) methods provide a natural way to find a dynamically feasible trajectory through the environment without colliding with nearby obstacles. However, the limited computation power available on typical embedded robotic systems, such as quadrotors, poses a challenge to running MPC in real time, including its most expensive tasks: constraints generation and optimization. To address this problem, we propose a novel hierarchical MPC scheme that consists of a planning and a tracking layer. The planner constructs a trajectory with a long prediction horizon at a slow rate, while the tracker ensures trajectory tracking at a relatively fast rate. We prove that the proposed framework avoids collisions and is recursively feasible. Furthermore, we demonstrate its effectiveness in simulations and lab experiments with a quadrotor that needs to reach a goal position in a complex static environment. The code is efficiently implemented on the quadrotor's embedded computer to ensure real-time feasibility. Compared to a state-of-the-art single-layer MPC formulation, this allows us to increase the planning horizon by a factor of 5, which results in significantly better performance.

Figures (15)

• Figure 1: The closed-loop trajectory of (a) the baseline single-layer MPC (SMPC) scheme and (b) the proposed hierarchical MPC (HMPC) scheme. Whereas SMPC solves the planning and tracking tasks in a single-layer formulation, HMPC decouples the tasks using a planning MPC and a tracking MPC that use the same nonlinear model but run at different frequencies. SMPC uses the same nonlinear model as PMPC and TMPC and is sampled at the same rate as TMPC to ensure reliable operation. Both schemes fly from start to goal position and avoid the obstacles. Compared to SMPC, HMPC has a significantly longer planning horizon, given the limited computational resources on the onboard computer, and does not have to balance planning and tracking tasks. Therefore, HMPC reaches the goal faster, is less sensitive to model mismatch, and maintains altitude better than SMPC.
• Figure 2: HMPC framework overview. In the offline phase, the terminal ingredients of the TMPC, including the terminal set, cost, and tightening constants, are computed. During online operation, the PMPC plans a trajectory based on goal position $\bm{p}^\mathrm{g}$ and grid map $\mathcal{M}$, and the TMPC tracks this trajectory using state feedback $\bm{x}$.
• Figure 3: Quadratic Lyapunov function $\mathcal{J}^\mathrm{f,t}(\bm{x}{-}\bm{x}^\mathrm{r})$ for two state tracking error dimensions. The blue ellipsoid is the terminal set $\mathcal{X}^\mathrm{f,t}$ intersecting $\mathcal{J}^\mathrm{f,t}(\bm{x}{-}\bm{x}^\mathrm{r})$ at value $\alpha^2$.
• Figure 4: 2D visualization of map pre-processing and I-DecompUtil, given occupied grid cells $\tilde{\mathcal{O}}$ and the last optimized plan $\bm{x}_{\tau|t}^*$. (a) The obstacles are inflated by half of the robot radius (orange arrows). (b) To construct the obstacle avoidance constraints around a specific line segment, first, a subset of the grid map $\mathcal{M}$ is selected such that the bounding box $\mathcal{B}$ with any orientation fits in this subset. (c) The obstacle avoidance constraints $\mathcal{F}_{\tau|t+T^\mathrm{s,p}}$ are constructed according to the SFC method in liu2017planning by growing an ellipsoid around the line segment, creating the tangential lines and clipping them to $\mathcal{B}$. Furthermore, $\mathcal{F}_{\tau|t+T^\mathrm{s,p}}$ are tightened by the other half of the robot radius, such that the robot does not collide with the obstacles if its center satisfies $\mathcal{F}_{\tau|t+T^\mathrm{s,p}}$. (d) The tightened obstacle avoidance constraints $\bar{\mathcal{F}}_{\tau|t+T^\mathrm{s,p}}$ are constructed by tightening $\mathcal{F}_{\tau|t+T^\mathrm{s,p}}$ with $c^\mathrm{o} \alpha$ according to \ref{['eq:property_ref_traj_obst_constraints_tightened']}, visualized using terminal set $\mathcal{X}^\mathrm{f,t}$ in this figure. Note that (b)-(d) are repeated for all line segments.
• Figure 5: Visualization of a scenario in which the robot would crash into the obstacle if the constraints are not tightened. The obstacle is inflated, here represented by $\mathcal{O}$, with corresponding occupied grid cells $\tilde{\mathcal{O}}$ and the point representation of $\tilde{\mathcal{O}}$ in the code. While the ellipsoid does not intersect the grid points, it overlaps with the obstacle region, i.e., $\mathcal{F} \cap \mathcal{O} \neq \emptyset$.

Theorems & Definitions (10)

• Remark 1
• Proposition 1: Terminal ingredients
• proof
• Remark 2
• Remark 3
• Theorem 1
• Theorem 2
• Corollary 1
• proof
• proof