PaperlayerView exact revision on arXiv

Source-linked AI summary

Model Predictive Contouring Control for Collision Avoidance in Unstructured Dynamic Environments

Bruno Brito, Boaz Floor, Laura Ferranti, Javier Alonso-Mora

arXiv:2010.10190v1cs.RO

TL;DR

The paper addresses local motion planning for robots navigating unstructured environments with static and moving obstacles. It extends MPCC with online convex free-space regions and conservative moving-obstacle collision bounds, producing onboard trajectories that avoid obstacles while tracking a reference. Experiments report successful navigation, real-time scaling to six pedestrians, and applicability to simulated autonomous cars.

  • Problem

    AGVs navigating closely with humans need to avoid static and moving obstacles while progressing toward goals, but planning and control are typically treated separately.

  • Method

    The method integrates MPCC-based local planning and control with polyhedral free-space constraints for static obstacles and a closed-form collision bound for ellipsoidal moving obstacles.

  • Results

    The method navigated safely in all experiments with static and two pedestrians, scaled to six pedestrians with low collision probability in real time, and outperformed the evaluated baselines.

  • Takeaways & Limitations

    The lightweight integrated design runs fully onboard and extends beyond the mobile robot to a simulated autonomous-car model.

Abstract

from arXiv · show

This paper presents a method for local motion planning in unstructured environments with static and moving obstacles, such as humans. Given a reference path and speed, our optimization-based receding-horizon approach computes a local trajectory that minimizes the tracking error while avoiding obstacles. We build on nonlinear model-predictive contouring control (MPCC) and extend it to incorporate a static map by computing, online, a set of convex regions in free space. We model moving obstacles as ellipsoids and provide a correct bound to approximate the collision region, given by the Minkowsky sum of an ellipse and a circle. Our framework is agnostic to the robot model. We present experimental results with a mobile robot navigating in indoor environments populated with humans. Our method is executed fully onboard without the need of external support and can be applied to other robot morphologies such as autonomous cars.

I. INTRODUCTION

The paper combines local motion planning and control for AGVs navigating static and moving obstacles in complex environments. It extends MPCC with free-space constraints and moving-obstacle approximations for real-time onboard operation.

  • I. INTRODUCTION: The method combines motion planning and control in one constrained-optimization module that generates kinematically feasible local trajectories with fast replanning cycles.It uses MPC to compute optimal control commands directly for the controlled system.
  • I. INTRODUCTION: The approach applies MPCC to local motion planning and control in unstructured environments with static and dynamic obstacles, rather than assuming a precomputed collision-free path.MPCC explicitly penalizes contouring and lateral path deviation while accommodating additional constraints.
  • B. Contribution: The paper constrains predicted robot positions to polyhedral approximations of collision-free space around the robot.These convex free-space regions can be larger than safety bubbles.
  • B. Contribution: It introduces a closed-form bound for conservatively approximating collision constraints caused by ellipsoidal moving obstacles.The bound addresses the collision region arising from the Minkowski sum of a moving-obstacle ellipse and the robot’s circular approximation.
  • B. Contribution: The integrated MPCC runs in real time onboard with onboard perception and is evaluated against the dynamic window, classical MPC, and a socially aware planner.Experiments use a mobile robot in indoor environments with static and moving obstacles, with additional simulated-car results.

II. PRELIMINARIES

The preliminaries model the robot as a discrete-time nonlinear system and approximate its occupied body by a union of circles. Static obstacles come from an updating occupancy-grid map, while tracked dynamic obstacles are removed from that map.

  • II. PRELIMINARIES: The AGV is modeled on the plane W = R2 with discrete-time nonlinear dynamics.The system uses a state z(t) and input u(t) at time t.
  • II. PRELIMINARIES: For the mobile robot, the state is its configuration in C = R2×S; for the car, the state space additionally includes speed.This formulation supports different robot morphologies.
  • II. PRELIMINARIES: The robot footprint B(z) is approximated by a union of nc circles whose centers depend on robot position, orientation, and body-frame circle centers.The circle decomposition represents the occupied area in the inertial frame.
  • II. PRELIMINARIES: Static obstacles are represented in an occupancy-grid map that is continuously updated locally from current sensor readings.A global map is used primarily for localization, while the local map captures the current environment.
  • II. PRELIMINARIES: Recognized and tracked dynamic obstacles are removed from the static map and modeled separately as moving obstacles.This separates static-map avoidance from dynamic-obstacle handling.

C. Dynamic obstacles

The method represents moving obstacles as ellipses, predicts their future positions and uncertainties, and optimizes collision-free motion over a finite horizon while tracking a reference path.

  • Dynamic obstacle representation: Moving obstacles are modeled as ellipses whose number can vary over time.Each ellipse is defined by its semi-major axis, semi-minor axis, and rotation matrix.
  • Motion prediction: A constant-velocity model with Gaussian acceleration noise represents dynamic-obstacle motion.Future positions and uncertainties are estimated from measured positions using a linear Kalman filter.
  • Reference path: The reference path may be a straight route to the goal, a preferred-motion line, or a global planner path composed of cubic path segments.The path is defined by way-points and segments parameterized by approximate traveled distance.
  • Reference-path tracking: The reference need not be collision-free, so the robot may deviate from it to avoid collisions.The optimization penalizes deviations from the reference while generating collision-free motion.
  • Receding-horizon optimization: Over N prediction steps, the optimizer generates admissible robot states and controls that guide the robot along the reference path while avoiding static and moving obstacles.The formulation uses predicted progress along the path and obtains a locally optimal command sequence.

III. METHOD

Each planning loop updates static free-space constraints, predicts dynamic obstacles, and solves a modified MPCC formulation for mobile robots.

  • Static collision avoidance: Each planning loop searches the updated static map for a collision-free region centered on the robot.The control problem constrains the robot to remain inside this region.
  • Dynamic collision avoidance: The method predicts dynamic-obstacle positions and applies a corrected collision bound to enforce dynamic collision avoidance.This step is performed in every planning loop.
  • Optimization: A modified MPCC formulation is solved for mobile robots after the static and dynamic constraints are constructed.The three steps are executed repeatedly within the planning loop.
  • Convex free-space regions: The static map is represented by convex four-sided polygons in free space, which can provide larger collision-free areas than circle-based approximations.These regions are computed around prediction points along the horizon.
  • Convex free-space regions: The previous optimal trajectory is shifted, extrapolated at its endpoint, and used to define one convex region around each prediction point.Each region separates its prediction point from the closest obstacles using four linear constraints.
  • Geometric representation: Figure 2 depicts the prediction horizon, convex free-space regions, inflated static environment, dynamic collision ellipses, and vehicle representing discs.The regions are shown around the prediction steps, while the vehicle is represented by blue discs.

B. Dynamic collision avoidance

The method models moving obstacles as ellipses and enlarges them with a rigorously computed bound for the robot’s circular footprint, ensuring the resulting constraint ellipsoid contains the collision space.

  • Moving obstacles are represented by ellipses, and each robot disc is constrained not to intersect their occupied elliptical regions.
  • The collision region is approximated by the Minkowski sum of an obstacle ellipse and a robot-bounding circle.The enlarged ellipse uses semi-axes α and β to include the original ellipse and circle.
  • The common approximation α = a + rdisc and β = b + rdisc is incorrect because collisions can still occur.
  • The smallest safe enlargement δ is found by applying a quadratic-distance lemma and solving for the value satisfying minj λ(j) = r2.The first two polynomial roots have multiplicity two, and the minimum distance is the square root of the minimal positive root.
  • The resulting semi-axes α = a + δ and β = b + δ guarantee that the constraint ellipsoid entirely bounds the collision space.

C. Model Predictive Contouring Control

MPCC is adapted for mobile-robot path following by constructing a local reference from nearby global path segments while preserving sufficient horizon coverage.

  • MPCC is tailored to path-following problems and is modified here for mobile robots navigating unstructured environments with on-board perception.
  • The closest path segment m and the previously predicted path parameter determine the initial local-reference parameter through a neighborhood line search.
  • Only η ≤ M path segments are selected from the global reference to reduce computational load.
  • The local segment count must remain large enough for the robot to follow the reference path across the prediction horizon.The minimum depends on horizon length, segment lengths, and robot speed.

3) Maintaining continuity over the local reference path:

The controller forms a differentiable local reference, penalizes contour and lag errors alongside speed, clearance, and input costs, and solves the resulting constrained problem repeatedly online.

  • Maintaining continuity over the local reference path:: The selected reference segments are concatenated into a differentiable local path for tracking by LMPCC.
  • Maintaining continuity over the local reference path:: Sigmoid activations provide continuous transitions between path segments, enabling the solver to compute gradients.
  • 4) Cost function:: Contour and lag errors are combined into an error vector used to track the reference path.
  • 4) Cost function:: The speed term penalizes deviation from a higher-level planner’s reference velocity, which may vary across path segments.
  • 4) Cost function:: The repulsive cost increases obstacle clearance and improves robustness to localization uncertainties.
  • 4) Cost function:: The stage cost combines tracking, speed, repulsive-obstacle, and input penalties, while the terminal cost retains tracking and repulsive terms.
  • At each control iteration, the algorithm estimates the path parameter, selects local segments, predicts dynamic obstacles, solves the optimization, and applies the optimal input.

IV. RESULTS

The evaluation studies parameter settings and compares the planner with state-of-the-art methods in static and dynamic environments using a mobile robot.

  • Experiments and simulations cover three mobile-robot scenarios, including parameter studies and comparisons in static and dynamic environments.

A. Experimental setup

The experiments run fully onboard on a Clearpath Jackal, using onboard localization, perception, planning, and control. The setup supports static-map updates, dynamic-obstacle tracking, and real-time nonlinear optimization.

  • Experimental platform: The platform is a fully autonomous Clearpath Jackal with onboard localization, perception, motion planning, and control.An Intel i5 runs localization and motion planning, while an Intel i7 NUC runs pedestrian tracking.
  • Reference generation: Global paths are built from waypoints using clothoids, then sampled and connected with third-order polynomials for local references.
  • Static obstacle handling: Static avoidance uses an online-updated sensor map and vehicle-aligned rectangles that search free space in 0.05 m steps up to 2 m.The rectangle expansion runs in parallel with the LMPCC solver using the latest available information.
  • Dynamic obstacle handling: Dynamic obstacles are detected and tracked with the open-source SPENCER pedestrian tracker and two-dimensional laser data.Detected pedestrians are modeled as moving ellipses.
  • Optimization: The nonconvex LMPCC problem is solved online with ACADO, multiple shooting, fourth-order Gauss–Legendre integration, qpOASES, and a 50 ms sampling time.

B. Parameter evaluation

The parameter study varies reference speed and prediction horizon, then compares LMPCC with classical MPC and Dynamic Window in static-obstacle navigation. LMPCC maintains safe clearance and completes scenarios where classical MPC fails.

  • Parameter evaluation: The evaluation tests reference speeds of 1, 1.25, and 1.5 m/s with prediction horizons of 1, 3, and 5 s.The robot follows a figure-8 path while avoiding two elliptical pedestrians and remaining inside collision-free rectangles.
  • Parameter evaluation: Figure 6 summarizes online LMPCC computation time across velocity references and horizon lengths using medians, quartiles, and outliers.
  • Parameter evaluation: Short horizons produce lower safety distances, while the method maintains safe clearance from dynamic obstacles.
  • Parameter selection: A reference speed of 1.25 m/s and a 3 s horizon are selected for subsequent experiments.
  • Static collision avoidance: In static-obstacle navigation, classical MPC fails to proceed toward the next waypoint, whereas LMPCC and Dynamic Window complete the task.
  • Static collision avoidance: LMPCC and Dynamic Window travel nearly identical distances, but goal time is 41 s for LMPCC versus 50 s for Dynamic Window.
  • Static collision avoidance: LMPCC traverses around a malfunctioning door, while Dynamic Window enters a deadlock state at the narrow opening.

D. Dynamic collision avoidance

Dynamic-environment experiments evaluate LMPCC against Dynamic Window and CADRL with simulated and real pedestrians, and also test an autonomous-car model. The results report low-failure operation up to six pedestrians, earlier avoidance, onboard execution, and applicability beyond mobile robots.

  • Simulation results: The simulation compares LMPCC with Dynamic Window and CADRL while the robot follows a corridor path around pedestrians modeled with constant-velocity predictions.
  • Simulation results: Failures increase with agent count, but the method scales to six pedestrians with low collision probability and real-time performance.For larger crowds, it selects the six closest pedestrians.
  • Experimental results: In a real two-pedestrian scenario, LMPCC reacts in advance and produces larger clearance, while Dynamic Window reacts late and pedestrians actively avoid the robot.
  • Experimental results: LMPCC navigates safely in all reported experiments involving static obstacles and two pedestrians.
  • Autonomous-car validation: A kinematic bicycle-model simulation shows the planner following a road path while respecting road boundaries and avoiding a cyclist and crossing pedestrian.
  • Conclusions: The approach combines MPCC with a bounded ellipse–circle Minkowski-sum approximation for dynamic obstacles and convex free-space regions for static obstacles.
  • Conclusions: The implementation runs onboard, scales to six agents, and is demonstrated in simulation with an autonomous-car model.
Loading 2010.10190v1…