# Development of Task-Space Nonholonomic Motion Planning Algorithm Based on Lie-Algebraic Method
## 2 Model and Algorithm  
Nonholonomic systems modeled at a kinematic level are described by the control-affine equation [14]  
\[\dot{\mathbf{q}}=\sum_{i=1}^{m}\mathbf{g}_{i}(\mathbf{q})u_{i},\quad\dim(\mathbf{q})=n>m, \tag{1}\]  
where \(\mathbb{Q}\
i\mathbf{q}\) is a configuration, \(\mathbf{u}=(u_{1},\ldots,u_{m})^{T}\) are controls, and \(\mathbf{g}_{i}(\mathbf{q})\) are (smooth) vector fields (generators) that span a null space of nonholonomic constraints. By definition, nonholonomic systems satisfy the Lie algebra rank condition. Thus, according to the [PERSON]'s theorem [15], they are small time locally controllable. In order to shorten notations, two-input systems, \(m=2\), will be considered with \(\mathbf{g}_{1}=\mathbf{X}\) and \(\mathbf{g}_{2}=\mathbf{Y}\). Two input systems are the most challenging as for the systems it is quite easy to generate difficult motion planning tasks by increasing a configuration space dimension. In many practical applications not all components of a configuration vector are important (for example wheel angles of mobile robots do not impact obstacle collisions), thus an output map is introduced  
\[\mathbf{x}=\mathbf{k}(\mathbf{q}),\quad\dim(\mathbf{x})=r \tag{2}\]  
and usually \(r<n\). Now, a motion planning task can be stated as follows: given an initial configuration \(\mathbf{q}_{0}\) and a goal point, \(\mathbf{x}_{f}\) find controls \(\mathbf{u}(\cdot)\) which, applied to the system (1), generates an end-point of the resulting trajectory mapped with Equation (2) into the desired location \(\mathbf{x}_{f}\). In Lie-algebraic methods, locally, around a current configuration \(\mathbf{q}_{c}\), possible directions of motion of the system (1) are described by vector fields derived from generators \(\mathbf{X},\mathbf{Y}\). To avoid redundancy, it is a common practice to take only a basis of the Lie algebra spanned by the generators. There are at least three such bases, but probably the most frequently used is the Ph. Hall basis. It is composed of an infinite number of Lie monomials, interpreted as vector fields within the scope of the control system (1). The very first elements of the basis are given below  
\[\mathbf{H}=(\mathbf{X},\mathbf{Y},[\mathbf{X},\mathbf{Y}],[\mathbf{X},[\mathbf{X},\mathbf{Y}]],[\mathbf{Y},[\mathbf{X}, \mathbf{Y}]],\ldots) \tag{3}\]  
where \([\mathbf{A},\mathbf{B}]\) denotes a Lie bracket of Lie monomials (for vector fields \([\mathbf{A},\mathbf{B}]=(\partial\mathbf{B}/\partial\mathbf{q})\mathbf{A}-(\partial\mathbf{A}/ \partial\mathbf{q})\mathbf{B}\)). The basis is composed of generators (degree 1 Lie monomials) and higher degree monomials derived from generators and their descendants. The system (1) is a nonholonomic one, thus a finite (and usually small) number of initial Ph. Hall basis elements, \(\mathbf{H}_{trunc}\), can be taken to satisfy the Lie algebra rank condition [15]  
\[\forall\mathbf{q}_{c}\in\mathbb{Q}\quad\text{rank}\,\mathbf{H}_{trunc}(\mathbf{q}_{c})=n. \tag{4}\]  
Consequently, the system (1) is a small time locally controllable and it can evolve at any configuration in any direction. The condition (4) is quite easy to check in off-line mode with the assistance of symbolic computation packages. However, a very demanding problem is how to generate a motion in any direction with a small number of controls. Fortunately, the generalized Campbell-Baker-Hausdorff-Dynkin formula [16] answers this question constructively. It states that a configuration shift \(\mathbf{z}(t)(\mathbf{q}_{c})\) at a current configuration \(\mathbf{q}_{c}\), depends on controls \(\mathbf{u}\)[17] as follows  
\[ \begin{split}\mathbf{z}(t)(\mathbf{q}_{c},\mathbf{u})=&\alpha_ {1}(t)\mathbf{X}(\mathbf{q}_{c})+\alpha_{2}(t)\mathbf{Y}(\mathbf{q}_{c})+\alpha_{3}(t)[\mathbf{X}, \mathbf{Y}](\mathbf{q}_{c})+\\ &+\alpha_{4}(t)[\mathbf{X},[\mathbf{X},\mathbf{Y}]](\mathbf{q}_{c})+\alpha_{5}(t )[\mathbf{Y},[\mathbf{X},\mathbf{Y}]](\mathbf{q}_{c})+\ldots=\mathbf{H}_{trunc}(\mathbf{q}_{c})^{T} \mathbf{\alpha}(\mathbf{u}),\end{split}