Whole-Body MPC and Dynamic Occlusion Avoidance: A Maximum Likelihood Visibility Approach

TL;DR

This paper presents a probabilistic visibility-based approach integrated into whole-body MPC for dynamic occlusion avoidance, enabling real-time, onboard implementation on mobile robots.

Contribution

It introduces a likelihood maximization formulation of visibility constraints using a shadow field, with algorithms for 2D and 3D, and demonstrates real-time applicability.

Findings

High update rates achieved due to linear complexity

Successful real-time hardware experiments in 3D

Effective occlusion avoidance in simulation with quadrupedal robot

Abstract

This paper introduces a novel approach for whole-body motion planning and dynamic occlusion avoidance. The proposed approach reformulates the visibility constraint as a likelihood maximization of visibility probability. In this formulation, we augment the primary cost function of a whole-body model predictive control scheme through a relaxed log barrier function yielding a relaxed log-likelihood maximization formulation of visibility probability. The visibility probability is computed through a probabilistic shadow field that quantifies point light source occlusions. We provide the necessary algorithms to obtain such a field for both 2D and 3D cases. We demonstrate 2D implementations of this field in simulation and 3D implementations through real-time hardware experiments. We show that due to the linear complexity of our shadow field algorithm to the map size, we can achieve high update rates, which facilitates onboard execution on mobile platforms with limited computational power. Lastly, we evaluate the performance of the proposed MPC reformulation in simulation for a quadrupedal mobile manipulator.