Traditional path-planning methods for mobile robots typically focus on avoiding obstacles but often fall short when obstacles block the path to the goal. This paper addresses the challenge of Navigation Among Movable Obstacles (NAMO), where a single robot can reposition obstacles to create previously inaccessible pathways. We introduce SVG-MPPI, a novel approach that integrates semantics to incorporate continuous movability into both path planning and local control strategies, allowing the robot to navigate cluttered environments by moving obstacles as needed to reach its goal.

SVG-MPPI refines the traditional Visibility Graph (VG) method by introducing the Semantic Visibility Graph (SVG). In this advanced approach, additional nodes are placed near movable obstacles, with the movability of these obstacles assessed based on their estimated mass—a key semantic property defining movability. This enables the modeling of potential passages through these barriers. The cost of push actions needed to navigate to these additional nodes is incorporated into the path-finding algorithm, eliminating the need for explicit obstacle placement and reducing the overhead of custom task planning. The local control strategy employs Model Predictive Path Integral (MPPI), which uses the IsaacGym physics engine to simulate robot and obstacle state transitions. MPPI minimizes trajectory contact forces as part of its objective, thereby reducing the push actions executed by the robot. The system supports optional replanning by continuously evaluating obstacle movability and adjusting the path if actual conditions deviate from initial estimates.

Our solution was evaluated through both qualitative and quantitative experiments. Qualitative experiments demonstrated the algorithm’s success in a simulated environment, where it effectively handled path execution and replanning scenarios. In real-world testing, an omnidirectional robot successfully pushed an obstacle and established a path to a goal. Quantitative analyses compared SVG-MPPI with its unaltered planning method (VG), which has no notion of movability, and Rapidly-exploring Random Tree (RRT) adapted to include binary movability. Each of these path planning methods was equipped with MPPI, which had a similar notion of movability as the planner. Results showed that SVG-MPPI consistently outperformed both VG and RRT in terms of path planning success rate and execution success rate. Furthermore, SVG-MPPI exhibited lower cumulative contact forces over the trajectory, indicating that the integration of continuous movability allowed the algorithm to select paths of least resistance and navigate around obstacles where possible.

Overall, SVG-MPPI represents a significant advancement in NAMO planning by offering a cohesive solution that addresses the complexities of navigating random, cluttered environments with movable obstacles. By prioritizing direct progress toward the goal while repositioning obstacles, SVG-MPPI introduces a novel, integrated approach with promising results and substantial potential for future research and development.

Spatial object detection and environmental understanding are fundamental aspects of autonomous driving in mobile robots. In this work, a stochastic approach to 2D LiDAR and stereo camera sensor fusion for object detection is presented. By tracking LiDAR clusters 360° around the robot and continuously updating knowledge of their corresponding object types using Bayesian inference when they are within the camera’s view, uncertainties in the detection algorithm are addressed. Additionally, localisation and sensor uncertainties are accounted for by using Gaussian distributions to model the locations of entities. By estimating the state of these entities, dynamic objects can be tracked throughout the environment, while remembering their type from when they were last visible in the camera’s view. The results demonstrate correct linking of camera detections with LiDAR clusters, with an average positional inaccuracy of objects of 0.12m in simulation, and 0.25m across different experiments on the robot, as well as the ability to track humans even when they move out of the camera’s field of view with an average tracking error of 0.33m in simulation.