Path planning for simple wheeled robots: Sub-Riemannian and elastic curves on SE(2)

This paper presents a motion planning method for a simple wheeled robot in two cases: (i) where translational and rotational speeds are arbitrary, and (ii) where the robot is constrained to move forwards at unit speed. The motions are generated by formulating a constrained optimal control problem on the Special Euclidean group SE(2). An application of Pontryagin's maximum principle for arbitrary speeds yields an optimal Hamiltonian which is completely integrable in terms of Jacobi elliptic functions. In the unit speed case, the rotational velocity is described in terms of elliptic integrals, and the expression for the position is reduced to quadratures. Reachable sets are defined in the arbitrary speed case, and a numerical plot of the time-limited reachable sets is presented for the unit speed case. The resulting analytical functions for the position and orientation of the robot can be parametrically optimised to match prescribed target states within the reachable sets. The method is shown to be easily adapted to obstacle avoidance for static obstacles in a known environment.