Layer, Lie algebraic method of motion planning for nonholonomic systems

In this paper a layer, Lie algebraic method of motion planning for nonholonomic systems is presented. It plans locally a motion towards a goal by searching for optimal directions in equi-cost spaces. The spaces are easy to determine via exploiting Lie algebraic properties of vector fields that define the controlled system. The method was illustrated on the unicycle robot and the inverted pendulum.