In real-world robotics, path planning remains to be an open challenge; not only robots are asked to move through unexplored environments, but also the motion of robots is constrained by their dynamics. At the same time, such dynamics typically suffer from uncertainties, which should be taken into account for completely ensuring the feasibility of the path and the robot’s safety.

The state-of-the-art usually addresses those issues separately. Planning online requires being able to quickly update the path according to the incremental knowledge of the environment. Such prescription is hard to be satisfied when considering the system dynamics and its uncertainty because a policy over the entire belief space must be constructed.

This work proposes an incremental mapping-planning framework that jointly addresses these challenges for achieving fast replanning. The framework is threefold: (1) the environment is represented as a collection of local maps, for each of which the system has a relative uncertainty so (2) the probability of colliding with the environment can be probabilistically checked and (3) the feasibility of the path is ensured by considering the kinodynamic constraints of the system.

The proposed framework is evaluated with the Sparus II AUV, a torpedo-shaped vehicle suffering from nonholonomic constraints. The experiments are conducted in simulated and real-world environments, such as a breakwater structure and a natural passage. Results show the potential of the method for planning under motion and probabilistic constraints in uncertain environments while being suitable for systems with limited computational power.