Geodesic motion planning on 3D-terrains satisfying the robot´s kinodynamic constraints

In this article, a robot motion planning scheme for 3D-terrains is developed. Given the terrain profile and various obstacles on it, a navigation function is created. A geodesic based shortest path algorithm is developed to find the optimal lengthwise path towards the goal position. The path is then converted into a continuous smooth trajectory via an optimization scheme relying on a Bézier curve parametrization that satisfies the robot´s kinodynamic constraints. The efficacy of the proposed method is tested in various simulation studies.