Point Cloud-Based Control Barrier Functions for Model Predictive Control in Safety-Critical Navigation of Autonomous Mobile Robots

TL;DR

The paper tackles safety-critical navigation for autonomous mobile robots in environments with both static and dynamic obstacles by fusing point-cloud perception with planning. It introduces a point-cloud–based CBF-NMPC framework that builds a forward-time-domain map, predicts dynamic obstacle motion via a Kalman filter, and identifies static and dynamic risk points to generate hard CBF constraints within NMPC. A novel static/dynamic risk-point identification strategy and a YOLO-Fusion–driven perception pipeline enable far-sighted, robust obstacle avoidance. Experimental results in both Gazebo simulations and real indoor scenarios demonstrate improved safety and robustness over two baselines, with the authors releasing the source code for community use.

Abstract

In this work, we propose a novel motion planning algorithm to facilitate safety-critical navigation for autonomous mobile robots. The proposed algorithm integrates a real-time dynamic obstacle tracking and mapping system that categorizes point clouds into dynamic and static components. For dynamic point clouds, the Kalman filter is employed to estimate and predict their motion states. Based on these predictions, we extrapolate the future states of dynamic point clouds, which are subsequently merged with static point clouds to construct the forward-time-domain (FTD) map. By combining control barrier functions (CBFs) with nonlinear model predictive control, the proposed algorithm enables the robot to effectively avoid both static and dynamic obstacles. The CBF constraints are formulated based on risk points identified through collision detection between the predicted future states and the FTD map. Experimental results from both simulated and real-world scenarios demonstrate the efficacy of the proposed algorithm in complex environments. In simulation experiments, the proposed algorithm is compared with two baseline approaches, showing superior performance in terms of safety and robustness in obstacle avoidance. The source code is released for the reference of the robotics community.

Figures (8)

• Figure 1: Experimental results of navigation in an indoor environment with onboard sensing. Top left: an overview of the experimental setup and trajectory results. Top right: onboard color image. Bottom left: depth image. Bottom right: the optimized trajectory (green curve) through the hollow area inside the blackboard.
• Figure 2: The proposed collision avoidance framework. The perception pipeline processes sensory data into occupancy voxel map. This occupancy voxel map is subsequently utilized to compute a forward-time-domain (FTD) map, which identifies static and dynamic risk points based on the nonlinear model predictive control (NMPC) prediction results. The points with the highest risk of collision (static risk points in blue and dynamic risk points in purple) are identified through collision detection. These high-risk points dynamically define safety constraints within the NMPC framework via control barrier functions, enabling safe collision-free navigation.
• Figure 3: Illustration of the YOLO-Fusion detector. The RGB image is first processed to obtain the 2D YOLO detection results, from which the corresponding bounding box on the depth image is derived. Using this 2D bounding box information, the proposed filtering method calculates the 3D bounding box for accurate object localization.
• Figure 4: FTD map structure.
• Figure 5: Static risk point identification methodology. Forward detection and inverse collision detection leverage the FTD map data and NMPC predictions to identify static risk points.