Local Reactive Control for Mobile Manipulators with Whole-Body Safety in Complex Environments

TL;DR

This paper addresses the challenge of safely and efficiently controlling high-DOF mobile manipulators in cluttered environments. It introduces a local reactive controller that reframes a single-step control problem as a finite-horizon spatial trajectory optimization along the kinematic chain, with per-link states $\boldsymbol{q}_i=[\boldsymbol{p}_i,\boldsymbol{r}_i]\in\mathbb{R}^7$ and controls $u_i\in\mathbb{R}$ over a horizon $\bar{N}$, propagated via $q_{k+1}=f_k(\cdot)$. Per-link safety is enforced by containment constraints ${}^{\mathcal{W}}_k\subseteq {}^{\mathcal{F}}_k$ and collision avoidance via polytopic free regions ${}^{s_k}\mathcal{F}_k(\alpha_k)$ with $\alpha_k$ computed through SOS programming, all integrated into an augmented Lagrangian differential dynamic programming (AL-DDP) solver. The approach decouples link constraints, preserves sparsity through chain propagation, and demonstrates substantial improvements in safety, efficiency, and task completion in simulations and real-world experiments, with an open-source release. These results offer a robust pathway for real-time, whole-body safe navigation and manipulation of mobile manipulators in narrow and dynamic environments.

Abstract

Mobile manipulators typically encounter significant challenges in navigating narrow, cluttered environments due to their high-dimensional state spaces and complex kinematics. While reactive methods excel in dynamic settings, they struggle to efficiently incorporate complex, coupled constraints across the entire state space. In this work, we present a novel local reactive controller that reformulates the time-domain single-step problem into a multi-step optimization problem in the spatial domain, leveraging the propagation of a serial kinematic chain. This transformation facilitates the formulation of customized, decoupled link-specific constraints, which is further solved efficiently with augmented Lagrangian differential dynamic programming (AL-DDP). Our approach naturally absorbs spatial kinematic propagation in the forward pass and processes all link-specific constraints simultaneously during the backward pass, enhancing both constraint management and computational efficiency. Notably, in this framework, we formulate collision avoidance constraints for each link using accurate geometric models with extracted free regions, and this improves the maneuverability of the mobile manipulator in narrow, cluttered spaces. Experimental results showcase significant improvements in safety, efficiency, and task completion rates. These findings underscore the robustness of the proposed method, particularly in narrow, cluttered environments where conventional approaches could falter. The open-source project can be found at https://github.com/Chunx1nZHENG/MM-with-Whole-Body-Safety-Release.git.

Figures (6)

• Figure 1: Overview of the proposed local reactive controller design approach. We formulate the single-step reactive control problem as a finite-horizon spatial trajectory optimization problem along the kinematic chain. Using the robot states at $t$, we construct the kinematic chain propagation function to formulate the link-specific constraints. The collision avoidance constraints are formulated as implicit constraints, with the values and gradients determined using SOS programming. The spatial trajectory optimization problem is solved using the AL-DDP algorithm, where all link-specific constraints are integrated using the augmented Lagrangian.
• Figure 2: Illustration of spatial kinematic propagation of base and manipulator's links. (a). The motion process (the purple lines) generated by $\boldsymbol{u}_{base}$ of the mobile base can be decoupled into three independent stages (the dashed red lines). (b). Motion process of the manipulator's link. The dashed line link, located at $\{q_{k+1}^{t+1}\}$, is rotated from its position at $\{q_{k+1}^t\}$ along the red axis.
• Figure 3: (a). The free region (depicted in blue) for each link is generated along the robot's kinematic chain. (b). The midline of each link is illustrated as red dashed lines. The mobile manipulator is described as several polytopic regions (the light green regions).
• Figure 4: Navigation of the mobile manipulator in the random forest environment. Three trajectories of the base from different start points are represented as green, red, and orange lines, respectively. Key frames (A, B, C, D, E, F) are extracted from rviz and each key frame shows the most representative free region among all the free regions with the current robot configuration.
• Figure 5: Visualization of the manipulation task in the unstructured environment. The top figure shows the whole trajectory of the manipulation task, where red circles indicate key frames (A, B, C, D). A: The robot encounters a floating bar. The mobile base continues moving forward while the manipulator adjusts to avoid the bar. B: The robot is positioned under the floating bar without any collisions. C: The end-effector is tracking the target position and orientation at the top layer of the bookcases, which has a small clearance. D: The end-effector moves to the second target located at the middle layer of the bookcases.