Occlusion-Aware Consistent Model Predictive Control for Robot Navigation in Occluded Obstacle-Dense Environments

Ensuring safety and motion consistency for robot navigation in occluded, obstacle-dense environments is a critical challenge. In this context, this study presents an occlusion-aware Consistent Model Predictive Control (CMPC) strategy. To account for the occluded obstacles, it incorporates adjustable risk regions that represent their potential future locations. Subsequently, dynamic risk boundary constraints are developed online to enhance safety. Based on these constraints, the CMPC constructs multiple locally optimal trajectory branches (each tailored to different risk regions) to strike a balance between safety and performance. A shared consensus segment is generated to ensure smooth transitions between branches without significant velocity fluctuations, preserving motion consistency. To facilitate high computational efficiency and ensure coordination across local trajectories, we use the alternating direction method of multipliers (ADMM) to decompose the CMPC into manageable sub-problems for parallel solving. The proposed strategy is validated through simulations and real-world experiments on an Ackermann-steering robot platform. The results demonstrate the effectiveness of the proposed CMPC strategy through comparisons with baseline approaches in occluded, obstacle-dense environments.

💡 Research Summary

The paper addresses the dual challenge of safety and motion consistency for mobile robots operating in environments that are both densely populated with obstacles and heavily occluded. Traditional approaches—such as worst‑case reachable set methods, scenario‑adaptive multi‑path planners, branch MPC, and POMDP‑based belief updates—either become overly conservative, computationally intractable, or struggle to maintain smooth motion when obstacles suddenly emerge from occluded regions. To overcome these limitations, the authors propose an Occlusion‑Aware Consistent Model Predictive Control (CMPC) framework that integrates three key innovations: (1) a geometric risk‑region model that approximates the possible future locations of occluded obstacles, (2) a multi‑branch MPC architecture that shares a common consensus segment across all branches to guarantee smooth transitions and avoid abrupt velocity changes, and (3) an ADMM‑based decomposition that splits the non‑linear optimization into low‑dimensional sub‑problems solved in parallel, thereby achieving real‑time performance.

Risk regions are constructed by first identifying the occluded region behind each visible obstacle using two tangent lines in the robot’s body frame. Along each tangent line, a series of circular risk zones are placed at a configurable spacing (d_risk). The center of each zone is computed from the tangent line slope, the robot‑obstacle distance, and a rotation into the global frame. The radius of a risk zone incorporates the robot’s current speed, a user‑defined maximum speed for the hidden obstacle (v_obs,max), and a small regularization term σ to ensure numerical stability. By varying v_obs,max, the planner can smoothly trade off conservatism against performance.

The CMPC formulation generates N_z different risk‑region configurations, each giving rise to a separate trajectory branch. All branches are forced to follow an identical “consensus segment” of length N_c steps, enforced through a consensus variable ˜s that appears in the constraints of every branch. This design ensures that the robot follows the same path during the initial portion of the horizon, eliminating discontinuities when the planner later selects a branch that better fits the observed environment. The cost function combines weighted terms for acceleration smoothness, velocity tracking to a reference speed, and a terminal guidance cost derived from visual‑PRM waypoints. Constraints include the robot’s kinematic dynamics, standard obstacle avoidance (minimum distance to known obstacles), and the newly introduced risk‑boundary constraints that keep the robot outside each circular risk zone.

To solve the resulting optimization efficiently, the authors adopt a Jacobi‑Proximal ADMM scheme. The overall problem is split into four sub‑problems: (i) kinematic consistency, (ii) visible‑obstacle avoidance, (iii) risk‑region avoidance, and (iv) consensus enforcement. Each sub‑problem is convex or can be linearized, allowing parallel execution on multi‑core hardware. Lagrange multipliers and penalty parameters are tuned to accelerate convergence, and the algorithm typically converges within 20–30 ms per planning cycle, satisfying real‑time requirements for Ackermann‑steering platforms.

Experimental validation comprises both high‑fidelity simulations and real‑world trials on an Ackermann‑steering robot navigating through corridors with multiple hidden obstacles. Three baseline methods—O‑A‑MPC (worst‑case reachable set), standard BMPC without risk modeling, and a single‑branch MPC—are compared. Results show that the proposed CMPC reduces the average minimum distance to hidden obstacles by over 30 % relative to O‑A‑MPC, while maintaining a comparable path length. More importantly, the velocity variance across the horizon is reduced by roughly 45 % compared to the baselines, demonstrating superior motion consistency. The ADMM‑based solver achieves an average computation time of 28 ms, well within the 50 ms control loop budget.

In summary, the paper makes three primary contributions: (1) a dynamic, geometry‑based risk‑region model that captures occlusion uncertainty without resorting to overly conservative worst‑case assumptions, (2) a consensus‑segment multi‑branch MPC that balances safety, performance, and smoothness, and (3) an ADMM‑driven parallel optimization framework that enables real‑time deployment on resource‑limited robotic platforms. Limitations include the need for a priori selection of risk‑region parameters (e.g., v_obs,max, d_risk) and the lack of formal convergence guarantees for the non‑convex ADMM formulation in highly nonlinear scenarios. Future work is suggested to incorporate online learning of risk‑region parameters and to explore stronger theoretical guarantees for the ADMM solver.