SPF-EMPC Planner: A real-time multi-robot trajectory planner for complex environments with uncertainties

TL;DR

An extended state model predictive control planner with a safe probability field to address the multi-robot navigation problem in complex, dynamic, and uncertain environments and can accurately track generated trajectories while considering the robots' inherent model constraints and state uncertainty, thus ensuring the practical feasibility of the planned trajectories.

Abstract

In practical applications, the unpredictable movement of obstacles and the imprecise state observation of robots introduce significant uncertainties for the swarm of robots, especially in cluster environments. However, existing methods are difficult to realize safe navigation, considering uncertainties, complex environmental structures, and robot swarms. This paper introduces an extended state model predictive control planner with a safe probability field to address the multi-robot navigation problem in complex, dynamic, and uncertain environments. Initially, the safe probability field offers an innovative approach to model the uncertainty of external dynamic obstacles, combining it with an unconstrained optimization method to generate safe trajectories for multi-robot online. Subsequently, the extended state model predictive controller can accurately track these generated trajectories while considering the robots' inherent model constraints and state uncertainty, thus ensuring the practical feasibility of the planned trajectories. Simulation experiments show a success rate four times higher than that of state-of-the-art algorithms. Physical experiments demonstrate the method's ability to operate in real-time, enabling safe navigation for multi-robot in uncertain environments.

Figures (9)

• Figure 1: The ground mobile robots' navigation in a complex environment using the proposed algorithm. The upper shows the three-wheeled omnidirectional mobile robot used for the real-world experiment. The bottom presents the navigation in a physical environment with static, moving obstacles (the one in the red circle marked in green) and other robots.
• Figure 2: System overview. When perceiving the dynamic obstacle, a probability field is generated in real-time based on the obstacle's state. Then, the local trajectory planner iteratively generates the safe trajectory based on the local environment and probability field. Finally, the trajectory with the requisite time is tracked by the extended state model predictive controller. Throughout this process, the perception and localization modules regularly provide collision detection for each robot.
• Figure 3: Illustration of the safety probability field. The field is generated based on the dynamic obstacle's current shape, position, and velocity, with different collision probabilities for different regions. The transparent circle surrounded by a dashed line represents the state of the obstacle at the next moment during deterministic motion.
• Figure 4: Illustration of trajectory optimization based on safety probability field. For the trajectory that detects collision risks (the green curve), the collision probability values of dangerous path points (the light green dot) and the gradient (the black solid line) away from collisions can be obtained based on the safety probability field. Subsequently, the current dangerous path point ${\bf{p}}_i^r$ is to be advanced to the safe guidance point ${\bf{p}}_{ij}^g$ (the blue dot) along the gradient. Furthermore, by establishing a symmetrical line (the purple dashed line) through the index of path points entering and exiting the risk area, it is possible to obtain a candidate trajectory (the blue dashed line) on the other side of the dynamic obstacle.
• Figure 5: Robot model and relative relationship between the coordinate systems, where ${\rm{L=0.04m}}$, ${\theta _{\rm{bw}}}$ is the angle between the corresponding coordinate systems. The positive directions of the velocities are given.