# Development of Task-Space Nonholonomic Motion Planning Algorithm Based on Lie-Algebraic Method
## 1 Introduction  
A large number of contemporary and practically important robots can be described as nonholonomic systems. Despite different sources of nonholonomy and nonholonomic manipulators, wheel mobile robots [1] and free-floating space robots [2] belong to this class. Even when modeled at the kinematic level, nonholonomic systems are difficult to control because the number of controls is smaller than the dimension of their configuration spaces. Thus, sophisticated maneuvers have to be performed to get a desired location, clearly exemplified by the task of parking a car (especially towing trailers). For some special cases of nonholonomic systems (like Dubbins [3] and Reeds-Shepp cars [4] or systems in a chain form [5]), dedicated methods were proposed to plan their motions. However, there is a constant need to develop general purpose methods applicable to any nonholonomic system. Two main classes of general purpose methods can be distinguished: global [6, 7] and local ones [8]. Global methods try to get a whole output trajectory at once (although in an iterative manner), while local ones construct an output trajectory from sub-trajectories derived from local planning. Global methods are rooted in various paradigms. Using a linearization of a nonholonomic system along the trajectory corresponding to current controls, [PERSON] and coworkers [7] reformulated a nonholonomic motion planning task into an inverse task solved with classical methods. [PERSON] [9] applied the Volterra series expansion of a nonholonomic system with an output function to get highly oscillatory controls that were able to trace a desired trajectory with an arbitrary precision. [PERSON] and coworkers [10] adapted a variant of a wave front technique to design the fast marching square path planning method of nonholonomic motion planning.  
Generally, algorithms derived from global methods are computationally involved and very sensitive to geometric constraints due to obstacles. Consequently, they cannot beapplied in dynamic or partially unknown environments. Local methods do not suffer from these drawbacks.  
In this paper, a Lie-algebraic local method of motion planning will be studied. There are three main goals of the paper:  
1. to extend applicability of the method to plan a motion also within a task-space, and to provide a complete algorithm for implementing the method,
2. to show through simulations how data parameterizing the algorithm impacts its efficiency,
3. to compare planning within a configuration and a task-space, pointing out similarities and differences.  
It is worth noticing that local and global planning tasks belong to a classic triple: geometrical planning, motion planning, and control. Recently, some attempts have been made to combine the last two tasks into one planning-control loop [11; 12]. A vast majority of motion planning algorithms are based on discretization of control/configuration spaces to transform continuous tasks, described by differential equations and static output functions, into a graph domain. In this discrete space, graph-searching algorithms, like RRT or its numerous variants [13], can be used to solve the planning task.  
The paper is organized as follows: In Section 2 a model of nonholonomic systems is introduced supplemented with an output function, then a motion planning task is defined and, finally, a Lie algebraic algorithm to solve the task is presented. In Section 3 an extensive simulation study is provided to evaluate performance of the algorithm for various data that impact its behavior. Then, some observations are formulated based on the simulation results. Section 4 concludes the paper.