Development of Task-Space Nonholonomic Motion Planning Algorithm Based on Lie-Algebraic Method

Abstract

In this paper, a Lie-algebraic nonholonomic motion planning technique, originally designed to work in a configuration space, was extended to plan a motion within a task-space resulting from an output function considered. In both planning spaces, a generalized Campbell–Baker–Hausdorff–Dynkin formula was utilized to transform a motion planning into an inverse kinematic task known for serial manipulators. A complete, general-purpose Lie-algebraic algorithm is provided for a local motion planning of nonholonomic systems with or without output functions. Similarities and differences in motion planning within configuration and task spaces were highlighted. It appears that motion planning in a task-space can simplify a planning task and also gives an opportunity to optimize a motion of nonholonomic systems. Unfortunately, in this planning there is no way to avoid working in a configuration space. The auxiliary objective of the paper is to verify, through simulations, an impact of initial parameters on the efficiency of the planning algorithm, and to provide some hints on how to set the parameters correctly.

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, Tchon and coworkers [7] reformulated a nonholonomic motion planning task into an inverse task solved with classical methods. Jakubczyk [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. Arismendi 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 be applied 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:

• 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,
• to show through simulations how data parameterizing the algorithm impacts its efficiency,
• 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.

2. Model and Algorithm

Nonholonomic systems modeled at a kinematic level are described by the control-affine equation [14]

$$\dot{\mathit{q}}=\sum _{i=1}^m{\mathit{g}}_i\left(\mathit{q}\right)u_i,dim\left(\mathit{q}\right)=n>m,$$

(1)

where $\mathbb{Q}\ni \mathit{q}$ is a configuration, $\mathit{u}={(u_1,\dots ,u_m)}^T$ are controls, and ${\mathit{g}}_i\left(\mathit{q}\right)$ 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 Chow’s theorem [15], they are small time locally controllable.

In order to shorten notations, two-input systems, $m=2$, will be considered with ${\mathit{g}}_1=\mathit{X}$ and ${\mathit{g}}_2=\mathit{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

$$\mathit{x}=\mathit{k}\left(\mathit{q}\right),dim\left(\mathit{x}\right)=r$$

(2)

and usually $r<n$.

Now, a motion planning task can be stated as follows: given an initial configuration ${\mathit{q}}_0$ and a goal point, ${\mathit{x}}_f$ find controls $\mathit{u}(·)$ which, applied to the system (1), generates an end-point of the resulting trajectory mapped with Equation (2) into the desired location ${\mathit{x}}_f$.

In Lie-algebraic methods, locally, around a current configuration ${\mathit{q}}_c$, possible directions of motion of the system (1) are described by vector fields derived from generators $\mathit{X},\mathit{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

$$\mathit{H}=(\mathit{X},\mathit{Y},[\mathit{X},\mathit{Y}],[\mathit{X},[\mathit{X},\mathit{Y}]],[\mathit{Y},[\mathit{X},\mathit{Y}]],\dots )$$

(3)

where $[\mathit{A},\mathit{B}]$ denotes a Lie bracket of Lie monomials (for vector fields $[\mathit{A},\mathit{B}]=(\partial \mathbf{B}/\partial \mathit{q})\mathit{A}-(\partial \mathit{A}/\partial \mathit{q})\mathit{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, ${\mathit{H}}_{trunc}$, can be taken to satisfy the Lie algebra rank condition [15]

$$\forall {\mathit{q}}_c\in \mathbb{Q}\mathrm{rank}{\mathit{H}}_{trunc}\left({\mathit{q}}_c\right)=n.$$

(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 $\mathit{z}\left(t\right)\left({\mathit{q}}_c\right)$ at a current configuration ${\mathit{q}}_c$, depends on controls $\mathit{u}$ [17] as follows

$$\begin{array}{cc}\hfill \mathit{z}\left(t\right)({\mathit{q}}_c,\mathit{u})=& {\alpha }_1\left(t\right)\mathit{X}\left({\mathit{q}}_c\right)+{\alpha }_2\left(t\right)\mathit{Y}\left({\mathit{q}}_c\right)+{\alpha }_3\left(t\right)[\mathit{X},\mathit{Y}]\left({\mathit{q}}_c\right)+\hfill \\ \hfill & +{\alpha }_4\left(t\right)[\mathit{X},[\mathit{X},\mathit{Y}]]\left({\mathit{q}}_c\right)+{\alpha }_5\left(t\right)[\mathit{Y},[\mathit{X},\mathit{Y}]]\left({\mathit{q}}_c\right)+\dots ={\mathit{H}}_{trunc}{\left({\mathit{q}}_c\right)}^T\mathit{\alpha }\left(\mathit{u}\right),\hfill \end{array}$$

(5)

where control-dependent coefficients $\mathit{\alpha }={({\alpha }_1,{\alpha }_2,\dots )}^T$ pre-multiplying Lie monomials (vector fields) are expressed as

$$\begin{array}{c}{\alpha }_1\left(t\right)={\int }_0^t{u}_1\left(s_1\right)d{s}_1,{\alpha }_2\left(t\right)={\int }_0^t{u}_2\left(s_1\right)d{s}_1,\hfill \\ {\alpha }_3\left(t\right)=\frac{1}{2}{\int }_0^t{\int }_0^{s_2}(u_1\left(s_1\right)u_2\left(s_2\right)-u_2\left(s_1\right)u_1\left(s_2\right))d{s}_{1}d{s}_2,\dots \hfill \end{array}$$

(6)

and they become more and more complex as the integration is performed over the k-dimensional simplex, where k is the degree of a Lie monomial the coefficient corresponds to. It is a common practice to express controls as linear combinations of time-dependent functions [7]

$$u_i\left(t\right)=\sum _{j=0}^{N_i-1}{\varphi }_j\left(t\right)p_i^j,i=1,\dots ,m,t\in [0,T],$$

(7)

where ${\varphi }_j\left(t\right)$ are elements of any functional basis (polynomials, harmonic functions) and $N_i$ is the number of variables required to describe the i-th control. Thus, a vector $\mathit{p}$ collects all variables $p_i^j$ and uniquely describes controls $\mathit{u}$ and also energy of controls. Moreover, the shift (5) can be expressed as a function of $\mathit{p}$ and resembles forward kinematics (but at a velocity level when considered on a fixed time horizon)

$$\mathit{z}\left(t\right)({\mathit{q}}_c,\mathit{p})=\mathit{F}\left(\mathit{p}\right).$$

(8)

The shift (8) from the configuration space is mapped into the task-space, and within this space a desired motion towards the goal ${\mathit{x}}_f$ can be computed using the Newton algorithm. A complete algorithm to plan a motion of the system (1) with the output function (2), either in a configuration or a task-space, is presented in Algorithm 1.

Algorithm 1 nonholonomic motion planning within a task-space

Step 1. Read-in initial data: the system (1) together with the output function (2), the initial configuration ${\mathit{q}}_0$ and the goal point ${\mathit{x}}_f$, desired accuracy of reaching the goal $ϵ$, the initial value of ${\mathit{p}}_c$. Step 2. Initialize empty resulting trajectory ${\mathit{q}}_{res}(·)$, and set the current configuration ${\mathit{q}}_c\leftarrow {\mathit{q}}_0$. Step 3. Check the stop condition: iff $$\parallel {\mathit{x}}_f-\mathit{k}\left({\mathit{q}}_c\right)\parallel <ϵ,$$ (9) then stop computations and output resulting trajectory ${\mathit{q}}_{res}(·)$. Otherwise, progress with Step 4. Step 4. Select a parameterization of controls (7), a time horizon T, and derive mapping (8). Step 5. At a current point in the task-space ${\mathit{x}}_c=\mathit{k}\left({\mathit{q}}_c\right)$, select the planned shift $$\Delta \mathit{x}\leftarrow \xi ({\mathit{x}}_f-{\mathit{x}}_c)$$ (10) by setting a positive coefficient $\xi$. Step 6. Using the Newton algorithm, Ref. [18] find ${\mathit{p}}^{★}$ that solves the inverse kinematic task $$\Delta \mathit{x}=\mathit{J}\left({\mathit{q}}_c\right)\mathit{F}\left(\mathit{p}\right),$$ (11) where $\mathit{J}\left(\mathit{q}\right)=\partial \mathit{k}\left(\mathit{q}\right)/\partial \mathit{q}$ is the Jacobi matrix of the mapping (2). Step 7. Check whether the solution is acceptable: $$\parallel {\mathit{x}}_f-\mathit{k}\left({\mathit{q}}^{★}\left(T\right)\right)\parallel <\parallel {\mathit{x}}_f-{\mathit{x}}_c\parallel ,$$ (12) where ${\mathit{q}}^{★}\left(T\right)$ is the end-point of the trajectory ${\mathit{q}}^{★}(·)$ initialized at ${\mathit{q}}_c$ and generated by the system (1) with controls ${\mathit{u}}^{★}\left(t\right)$, $t\in [0,T]$, corresponding to the vector of parameters ${\mathit{p}}^{★}$, cf. Equation (7). Step 8. If Condition (12) is satisfied, then: append the trajectory ${\mathit{q}}^{★}(·)$ to the resulting trajectory ${\mathit{q}}_{res}(·)\leftarrow {\mathit{q}}_{res}(·)\cup {\mathit{q}}^{★}(·)$, update the current configuration ${\mathit{q}}_c\leftarrow {\mathit{q}}^{★}\left(T\right)$ and go back to Step 4. Otherwise, decrease the coefficient $\xi$ and go to Step 5.

Comments on Algorithm 1:

• A nonholonomic motion planning in a configuration space is achieved by setting the identity output function (2).
• A selected parameterization (7), Step 4, can vary from one iteration to another. Usually, one or at most a few parameterizations can be exploited, and vectors $\alpha \left(\mathit{u}\right(\mathit{p}\left)\right)$ from Equation (5) can be computed in an off-line mode.
• In Step 7, a minimal requirement was formulated, i.e., a new end-point of trajectory in the task-space should be closer to the goal than it was in the previous iteration. More restrictive requirement assumes that it should also be close enough to the planned sub-goal ${\mathit{x}}_c+\Delta \mathit{x}$.
• Singularities in solving (11) result from a rank deficiency of the Jacobi matrix, derived from the right hand side of (11)

  $$\widehat{\mathit{J}}\left(\mathit{p}\right)=\frac{\partial \left(\mathit{J}\left({\mathit{q}}_c\right)\mathit{F}\left(\mathit{p}\right)\right)}{\partial \mathit{p}}=\mathit{J}\left({\mathit{q}}_c\right)\frac{\partial \mathit{F}\left(\mathit{p}\right)}{\partial \mathit{p}}.$$

  (13)

  Thus, singularities may arise
  • either due to a rank deficiency of $\mathit{J}\left({\mathit{q}}_c\right)$ (they are avoidable by a small variation of controls which results in a small shift of ${\mathit{q}}_c$)
  • or a rank deficiency of the Jacobi matrix $\partial \mathit{F}\left(\mathit{p}\right)/\partial \mathit{p}$ at the current ${\mathit{p}}_c$. Equivalent to the matrix $\partial \alpha \left(\mathit{u}\right(\mathit{p}\left)\right)/\partial \mathit{p}$, cf. Equations (5), (7), and (8) as the full rank matrix ${\mathit{H}}_{trunc}\left({\mathit{q}}_c\right)$ cannot introduce singularities at all, cf. Equation (4).
• In Step 5, the positive coefficient $\xi$ should not be too large, as values of matrices $\mathit{J}\left({\mathit{q}}_c\right)$, ${\mathit{H}}_{trunc}{\left({\mathit{q}}_c\right)}^T$ are computed at the current configuration ${\mathit{q}}_c$ and neglecting some vector fields, while truncation $\mathit{H}\to {\mathit{H}}_{trunc}$, is justified only locally. Its value cannot be too small either, as many iterations might be required to complete the algorithm.
• In motion planning within a task-space with $r<n$, it is impossible to avoid planning within a configuration space, as the mapping (2) is not unique in this case.

3. Simulations

In order to evaluate Algorithm 1, some tests were performed using two models of mobile robots depicted in Figure 1. The unicycle is described by equations

$$\dot{\mathit{q}}=\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\left[\begin{array}{cc}cos\left(q_3\right)& 0\\ sin\left(q_3\right)& 0\\ 0& 1\end{array}\right]\mathit{u}=\left[\begin{array}{c}cos\left(q_3\right)\\ sin\left(q_3\right)\\ 0\end{array}\right]u_1+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]u_2=\mathit{X}\left(\mathit{q}\right)u_1+\mathit{Y}\left(\mathit{q}\right)u_2.$$

(14)

The configuration vector $\mathit{q}$ is composed of position $(q_1,q_2)$ and orientation $q_3$ of the vehicle on a plane, while $u_1,u_2$ denote controls interpreted as angular and linear velocities.

The model of a kinematic car is given as

$$\dot{\mathit{q}}=\left[\begin{array}{cc}Lcos\left(q_3\right)cos\left(q_4\right)& 0\\ Lsin\left(q_3\right)cos\left(q_4\right)& 0\\ sin\left(q_4\right)& 0\\ 0& 1\end{array}\right]\mathit{u}=\left[\begin{array}{c}Lcos\left(q_3\right)cos\left(q_4\right)\\ Lsin\left(q_3\right)cos\left(q_4\right)\\ sin\left(q_4\right)\\ 0\end{array}\right]u_1+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]u_2=\mathit{X}\left(\mathit{q}\right)u_1+\mathit{Y}\left(\mathit{q}\right)u_2,$$

(15)

where configuration $\mathit{q}$ has got an extra component $q_4$ describing the steering wheel angle. For simplicity, it was assumed that $L=1$. Both models are nonholonomic ones because generators $\mathit{X},\mathit{Y}$ supplemented with the Lie bracket $[\mathit{X},\mathit{Y}]$ for the unicycle

$$[\mathit{X},\mathit{Y}]=\left[\begin{array}{c}sin\left({\mathit{q}}_3\right)\\ -cos\left({\mathit{q}}_3\right)\\ 0\end{array}\right],$$

(16)

and

$$[\mathit{X},\mathit{Y}]=\left[\begin{array}{c}Lcos\left({\mathit{q}}_3\right)sin\left({\mathit{q}}_4\right)\\ Lsin\left({\mathit{q}}_3\right)sin\left({\mathit{q}}_4\right)\\ -cos\left({\mathit{q}}_4\right)\\ 0\end{array}\right],[\mathit{X},[\mathit{X},\mathit{Y}]]=\left[\begin{array}{c}-Lsin\left({\mathit{q}}_3\right)\\ Lcos\left({\mathit{q}}_3\right)\\ 0\\ 0\end{array}\right],[\mathit{Y},[\mathit{X},\mathit{Y}]]=\mathit{X}.$$

(17)

for the kinematic car, satisfy Condition (4).

For both models, the position on the xy plane was selected as the output function

$$\mathit{k}\left(\mathit{q}\right)={(q_1,q_2)}^T.$$

(18)

If a robot is inscribed in a circle of sufficient radius, its orientation does not influence the possibility of collisions with obstacles. For the kinematic car another output function was used, coupling position on the xy plane and orientation of the vehicle

$$\mathit{k}\left(\mathit{q}\right)={(q_1,q_2,q_3)}^T,$$

(19)

In practice, the orientation of the steering axle is not relevant. In order to check the motion planning Algorithm 1 within a configuration space, the identity output function was selected for both mobile robots.

In all tested cases, an initial configuration was set to the vector composed of zeroes only

$${\mathit{q}}_0=\mathbf{0}.$$

(20)

In the tests, a side-way motion maneuver on the $xy$-plane was planned for both models and all output functions. The maneuver is challenging as it requires a motion along higher (than generators $\mathit{X},\mathit{Y}$) degree vector fields. Depending on a dimension of a task-space, goal points were selected as

$${\mathit{x}}_f=(0,\delta ),{\mathit{x}}_f=(0,\delta ,0),\delta \in \{0.05,0.1,0.2,0.5,0.7,1\}.$$

(21)

Any single task was run using three parameterizations of controls. Each control was composed of a few very first items of the orthonormal Fourier basis

$$\begin{array}{cc}\hfill & u_1=p_1^1\rho +p_1^2\sqrt{2}\rho sin\left(\omega t\right)+p_1^3\sqrt{2}\rho cos\left(\omega t\right),\hfill \\ & u_2=p_2^1\rho +p_2^2\sqrt{2}\rho sin\left(\omega t\right)+p_2^3\sqrt{2}\rho cos\left(\omega t\right)\hfill \end{array}$$

(22)

$$u_1=p_1^1\rho +p_1^2\sqrt{2}\rho sin\left(\omega t\right),u_2=p_2^1\rho +p_2^3\sqrt{2}\rho cos\left(\omega t\right)$$

(23)

$$u_1=p_1^1\rho +p_1^3\sqrt{2}\rho cos\left(\omega t\right),u_2=p_2^1\rho +p_2^2\sqrt{2}\rho sin\left(\omega t\right)$$

(24)

where $\rho =\sqrt{1/T}$ and $\omega =2\pi /T$. Later on, it was assumed that $T=1$. The total energy expenditure on controls is equal to

$$energy\left(\mathit{p}\right)=\sum _{i=1}^2\sum _{j=1}^{\#N_i}{\left(p_i^j\right)}^2,$$

(25)

where $\#N_i$ is the number of the Fourier basis elements contributing to the i-th control. All tasks, with the data collected in

Table 1. Data for tasks.

Task No.	Model	Output Function	Controls	$\mathit{\delta }$
1	unicycle	identity	Equation (22)	$0.5$
2	unicycle	Equation (18)	Equation (22)	$0.5$
3	unicycle	Equation (18)	Equation (24)	$0.2$
4	kinematic car	identity	Equation (22)	$0.1$
5	kinematic car	Equation (18)	Equation (22)	$0.05$
6	kinematic car	Equation (19)	Equation (23)	$0.1$

, were solved using Algorithm 1. In Step 6 of the algorithm, two versions of the Newton algorithm were tested: the basic one and with the energy (25) optimization within the null-space of the Jacobi matrix (13). In order to obtain statistically significant results, Step 6 was repeated 100 times for each task, with initial values of $\mathit{p}$ varied (each component of $\mathit{p}$ was generated randomly with the uniform distribution on the interval $[-1,1]$). Programs used in simulations were written in Wolfram Mathematica, version 11.3, and run on a computer with Intel^® Core™ i5-8400 CPU and 24 GiB RAM memory. In Figure 2 and Figure 3, some characteristic paths (projected on the xy-plane and with a target point marked with an asterisk) were visualized for the unicycle and the kinematic car, respectively.

Some numeric data describing the quality of solutions were collected in

Table 2. Accuracy, numeric failures, and computing time for the unicycle tasks.

			The Newton Algorithm
			Basic				Null-Space Optimization
Output Function	Control Param.	$\mathbf{\delta }$	b.a.	a. [%]	f. [%]	Ctime [s]	b.a.	a. [%]	f. [%]	Ctime [s]
ident.	(22)	0.05	$0.000$	100	0	$1.7$	$0.001$	100	0	$1.7$
		0.1	$0.001$	100	0	$1.6$	$0.001$	100	0	$1.6$
		0.2	$0.002$	98	0	$1.5$	$0.003$	100	0	$1.5$
		0.5	$0.009$	49	0	$1.5$	$0.011$	62	0	$1.5$
		0.7	$0.017$	43	0	$1.4$	$0.020$	42	0	$1.4$
		1	$0.037$	29	0	$1.2$	$0.040$	31	0	$1.3$
	(23)	0.05	$0.000$	100	0	$0.6$	$0.000$	100	0	$0.6$
		0.1	$0.000$	100	0	$0.5$	$0.001$	100	0	$0.5$
		0.2	$0.000$	100	0	$0.4$	$0.002$	100	0	$0.5$
		0.5	$0.006$	100	0	$0.4$	$0.010$	100	0	$0.5$
		0.7	$0.012$	100	0	$0.4$	$0.020$	100	0	$0.4$
		1	$0.028$	100	0	$0.4$	$0.040$	100	0	$0.4$
	(24)	0.05	$0.002$	100	0	$0.5$	$0.005$	100	0	$0.6$
		0.1	$0.010$	99	0	$0.5$	$0.017$	100	0	$0.5$
		0.2	$0.032$	82	0	$0.4$	$0.049$	100	0	$0.6$
		0.5	$0.157$	0	0	$0.4$	$0.195$	0	0	$0.5$
		0.7	$0.262$	0	0	$0.4$	$0.322$	0	0	$0.5$
		1	$0.454$	0	0	$0.4$	$0.546$	0	0	$0.4$
(18)	(22)	0.05	$0.001$	6	5	$42.3$	$0.006$	1	0	$42.4$
		0.1	$0.004$	7	3	$43.1$	$0.018$	1	0	$43.2$
		0.2	$0.007$	4	0	$38.5$	$0.051$	1	1	$38.5$
		0.5	$0.136$	1	0	$42.7$	$0.172$	0	0	$42.8$
		0.7	$0.155$	2	2	$40.3$	$0.257$	0	0	$40.3$
		1	$0.328$	0	3	$39.2$	$0.381$	0	0	$39.2$
	(23)	0.05	$0.005$	10	15	$13.7$	$0.042$	0	0	$13.8$
		0.1	$0.016$	2	20	$13.0$	$0.083$	0	1	$13.2$
		0.2	$0.095$	0	21	$13.0$	$0.164$	0	0	$13.4$
		0.5	$0.388$	0	5	$12.6$	$0.406$	0	1	$12.8$
		0.7	$0.546$	0	3	$12.4$	$0.567$	0	2	$12.5$
		1	$0.783$	0	2	$12.6$	$0.809$	0	1	$12.7$
	(24)	0.05	$0.009$	7	11	$15.6$	$0.005$	68	2	$16.6$
		0.1	$0.033$	0	13	$15.0$	$0.017$	62	4	$15.2$
		0.2	$0.069$	0	3	$15.0$	$0.037$	43	7	$15.1$
		0.5	$0.103$	1	5	$15.3$	$0.207$	0	8	$15.4$
		0.7	$0.296$	0	2	$15.0$	$0.329$	0	17	$15.1$
		1	$0.553$	0	4	$14.7$	$0.539$	0	11	$14.8$

and

Table 3. Accuracy, numeric failures, and computing time for the kinematic car tasks.

			The Newton Algorithm
			Basic				Null-Space Optimization
Output Function	Control Param.	$\mathbf{\delta }$	b.a.	a. [%]	f. [%]	Ctime [s]	b.a.	a. [%]	f. [%]	Ctime [s]
ident.	(22)	0.05	$0.001$	8	14	$31.8$	$0.001$	6	8	$32.4$
		0.1	$0.021$	1	18	$30.8$	$0.004$	9	8	$31.5$
		0.2	$0.077$	0	18	$30.5$	$0.022$	3	9	$31.3$
		0.5	$0.017$	3	14	$28.3$	$0.005$	7	4	$29.4$
		0.7	$0.292$	0	19	$30.6$	$0.104$	4	3	$31.5$
		1	$0.446$	0	25	$30.6$	$0.030$	7	4	$31.7$
	(23)	0.05	$0.001$	7	0	$10.0$	$0.001$	12	3	$10.4$
		0.1	$0.001$	4	8	$9.2$	$0.001$	12	31	$9.5$
		0.2	$0.244$	0	10	$9.5$	$0.000$	11	27	$9.9$
		0.5	$0.507$	0	22	$8.4$	$0.003$	7	40	$8.8$
		0.7	$0.679$	0	23	$10.0$	$0.011$	7	36	$10.3$
		1	$0.940$	0	34	$10.0$	$0.015$	7	35	$10.5$
	(24)	0.05	$0.001$	47	53	$12.0$	$0.001$	48	52	$12.8$
		0.1	$0.001$	52	48	$11.1$	$0.001$	53	47	$12.3$
		0.2	—	0	100	—	$0.000$	46	54	$14.7$
		0.5	—	0	100	—	$0.002$	40	60	$15.0$
		0.7	—	0	100	—	$0.003$	36	64	$14.8$
		1	—	0	100	—	$0.008$	42	58	$14.1$
(18)	(22)	0.05	$0.001$	6	5	$42.3$	$0.006$	1	0	$42.4$
		0.1	$0.004$	7	3	$43.1$	$0.018$	1	0	$43.2$
		0.2	$0.007$	4	0	$38.5$	$0.051$	1	1	$38.5$
		0.5	$0.136$	1	0	$42.7$	$0.172$	0	0	$42.8$
		0.7	$0.155$	2	2	$40.3$	$0.257$	0	0	$40.3$
		1	$0.328$	0	3	$39.2$	$0.381$	0	0	$39.2$
	(23)	0.05	$0.005$	10	15	$13.7$	$0.042$	0	0	$13.8$
		0.1	$0.016$	2	20	$13.0$	$0.083$	0	1	$13.2$
		0.2	$0.095$	0	21	$13.0$	$0.164$	0	0	$13.4$
		0.5	$0.388$	0	5	$12.6$	$0.406$	0	1	$12.8$
		0.7	$0.546$	0	3	$12.4$	$0.567$	0	2	$12.5$
		1	$0.783$	0	2	$12.6$	$0.809$	0	1	$12.7$
	(24)	0.05	$0.009$	7	11	$15.6$	$0.005$	68	2	$16.6$
		0.1	$0.033$	0	13	$15.0$	$0.017$	62	4	$15.2$
		0.2	$0.069$	0	3	$15.0$	$0.037$	43	7	$15.1$
		0.5	$0.103$	1	5	$15.3$	$0.207$	0	8	$15.4$
		0.7	$0.296$	0	2	$15.0$	$0.329$	0	17	$15.1$
		1	$0.553$	0	4	$14.7$	$0.539$	0	11	$14.8$
(19)	(22)	0.05	$0.002$	58	7	$43.4$	$0.005$	94	0	$43.5$
		0.1	$0.005$	53	5	$43.2$	$0.018$	93	1	$43.3$
		0.2	$0.015$	32	4	$43.2$	$0.052$	63	3	$43.3$
		0.5	$0.070$	30	1	$42.2$	$0.175$	0	1	$42.3$
		0.7	$0.142$	17	1	$42.4$	$0.260$	0	0	$42.5$
		1	$0.210$	6	2	$43.0$	$0.384$	0	0	$43.2$
	(23)	0.05	$0.007$	24	0	$13.9$	$0.007$	29	0	$14.0$
		0.1	$0.024$	19	0	$13.8$	$0.024$	28	0	$13.9$
		0.2	$0.073$	0	0	$13.7$	$0.074$	0	0	$13.8$
		0.5	$0.303$	0	0	$13.5$	$0.304$	0	1	$13.6$
		0.7	$0.496$	0	2	$13.7$	$0.496$	0	0	$13.8$
		1	$0.788$	0	0	$13.4$	$0.815$	0	0	$13.4$
	(24)	0.05	$0.005$	46	1	$16.5$	$0.005$	59	0	$17.1$
		0.1	$0.020$	10	0	$15.1$	$0.019$	61	0	$15.2$
		0.2	$0.074$	0	0	$14.2$	$0.056$	63	1	$14.2$
		0.5	$0.228$	0	0	$13.9$	$0.208$	0	0	$13.9$
		0.7	$0.349$	0	0	$13.7$	$0.330$	0	0	$13.8$
		1	$0.555$	0	0	$13.7$	$0.540$	0	3	$13.8$

. The abbreviations used:

 b.a.

—the best end-point accuracy obtained;

 a.

—how many generated paths were accurate enough,

 f.

—the percentage of the Newton algorithm runs which caused numerical problems due to an inversion of near-singularity matrices,

 ctime

—the average computation time. It is worth noticing that the time includes all stages of running each task, i.e., a symbolic generation of the Ph. Hall basis, kinematic-like mapping, and Jacobians and, finally, a truly numeric trajectory generation.

A path is considered to be accurate if an end-point of generated trajectory ${\mathit{x}}_{freal}$ in the task-space is close enough to the target point ${\mathit{x}}_f$, i.e.,

$$\parallel {\mathit{x}}_f-{\mathit{x}}_{freal}\parallel <\eta \parallel {\mathit{x}}_f-{\mathit{x}}_0\parallel ,$$

(26)

where ${\mathit{x}}_0=\mathit{k}\left({\mathit{q}}_0\right)$—the starting point and $\eta$— an accuracy coefficient. In Table 2 and Table 3, data were collected for $\eta =0.3$. The most accurate path satisfies

$$min\parallel {\mathit{x}}_f-{\mathit{x}}_{freal}\parallel .$$

(27)

In

Table 4. Path length and energy of motion of the unicycle.

			The Newton Algorithm
Output Function	Control Param.	$\mathbf{\delta }$	Basic		Null-Space Optimization
			Length	Energy	Length	Energy
ident.	(22)	0.05	$0.62÷2.63$	$0.27÷1.24$	$0.62÷0.69$	$0.42÷0.54$
		0.1	$1.26÷3.02$	$0.51÷1.35$	$1.24÷1.25$	$0.71÷0.71$
		0.2	$2.52÷3.95$	$0.79÷1.68$	$2.50÷2.51$	$1.01÷1.01$
		0.5	$6.30÷7.55$	$1.29÷2.08$	$6.27÷6.28$	$1.59÷1.60$
		0.7	$8.80÷9.70$	$1.57÷2.31$	$8.78÷8.79$	$1.89÷1.89$
		1	$12.60÷13.58$	$2.14÷2.73$	$12.55÷12.55$	$2.26÷2.26$
	(23)	0.05	$0.62÷1.66$	$0.23÷1.14$	$0.62÷0.63$	$0.47÷0.51$
		0.1	$1.26÷2.67$	$0.36÷1.23$	$1.24÷1.26$	$0.71÷0.71$
		0.2	$2.51÷3.86$	$0.61÷1.59$	$2.50÷2.51$	$1.01÷1.01$
		0.5	$6.28÷7.26$	$1.22÷2.10$	$6.27÷6.28$	$1.59÷1.60$
		0.7	$8.80÷9.75$	$1.50÷2.35$	$8.78÷8.79$	$1.89÷1.89$
		1	$12.57÷13.61$	$1.84÷2.69$	$12.55÷12.56$	$2.26÷2.26$
	(24)	0.05	$0.62÷2.92$	$0.23÷1.53$	$0.62÷0.63$	$0.50÷0.50$
		0.1	$1.25÷2.27$	$0.43÷1.30$	$1.24÷1.26$	$0.71÷0.71$
		0.2	$2.51÷3.67$	$0.84÷1.60$	$2.50÷2.51$	$1.01÷1.01$
(18)	(22)	0.05	$0.41÷3.03$	$0.18÷1.04$	$0.36÷0.62$	$0.35÷0.50$
		0.1	$0.76÷3.01$	$0.36÷1.18$	$0.72÷1.28$	$0.50÷0.70$
		0.2	$1.65÷4.07$	$0.60÷1.27$	$1.83÷1.99$	$0.86÷0.90$
		0.5	$4.35÷6.28$	$1.21÷1.63$	$4.80÷4.80$	$1.39÷1.39$
		0.7	$6.45÷9.79$	$1.40÷1.88$	—	—
		1	$9.00÷13.46$	$1.72÷2.23$	—	—
	(23)	0.05	$0.38÷1.91$	$0.18÷0.90$	$0.36÷0.41$	$0.34÷0.41$
		0.1	$0.73÷2.07$	$0.33÷0.91$	$0.72÷0.73$	$0.54÷0.54$
		0.2	$1.51÷2.89$	$0.61÷0.98$	—	—
		0.5	$4.31÷5.12$	$1.13÷1.30$	—	—
		0.7	$6.51÷6.78$	$1.44÷1.47$	—	—
	(24)	0.05	$0.63÷2.18$	$0.22÷1.13$	$0.62÷0.67$	$0.48÷0.60$
		0.1	$1.26÷2.88$	$0.45÷1.39$	$1.24÷1.28$	$0.68÷0.71$
		0.2	$2.52÷4.33$	$0.65÷1.42$	$2.50÷2.51$	$1.01÷1.01$
		0.5	$6.37÷7.91$	$1.26÷1.94$	—	—
		0.7	$9.04÷10.42$	$1.56÷2.32$	—	—
		1	$12.88÷14.18$	$1.90÷2.65$	—	—

and

Table 5. Path length and energy of motion for the kinematic car.

			The Newton Algorithm
Output Function	Control Param.	$\mathbf{\delta }$	Basic		Null-Space Optimization
			Length	Energy	Length	Energy
ident.	(22)	0.05	6912 ÷ 49,083	$74÷199$	$5166÷5392$	$64÷66$
		0.1	18,281 ÷ 18,281	$121÷121$	$9788÷$ 10,735	$89÷93$
		0.2	—	—	18,057 ÷ 19,637	$120÷126$
		0.5	44,864 ÷ 135,750	190 ÷ 331	41,005 ÷ 45,333	182 ÷ 191
		0.7	—	—	59,707 ÷ 75,423	219 ÷ 247
		1	—	—	82,748 ÷ 87,747	258 ÷ 266
	(23)	0.05	21,332 ÷ 45,418	131 ÷ 191	4049 ÷ 24,218	57 ÷ 140
		0.1	76,600 ÷ 79,107	249 ÷ 253	8310 ÷ 22,968	82 ÷ 136
		0.2	—	—	18,926 ÷ 137,973	123 ÷ 334
		0.5	—	—	40,082 ÷ 162,380	180 ÷ 362
		0.7	—	—	55,316 ÷ 113,002	211 ÷ 302
		1	—	—	80,542 ÷ 197,181	255 ÷ 399
	(24)	0.05	21,146 ÷ 123,282	130 ÷ 316	4020 ÷ 86,003	57 ÷ 264
		0.1	86,087 ÷ 163,986	264 ÷ 364	8229 ÷ 105,744	81 ÷ 292
		0.2	—	—	17,406 ÷ 176,427	118 ÷ 378
		0.5	—	—	54,373 ÷ 142,871	209 ÷ 340
		0.7	—	—	76,363 ÷ 211,192	248 ÷ 413
		1	—	—	106,666 ÷ 219,503	294 ÷ 421
(18)	(22)	0.05	2.63 ÷ 5.96	0.94 ÷ 1.70	5.28 ÷ 5.28	1.40 ÷ 1.40
		0.1	4.43 ÷ 10.33	1.20 ÷ 1.61	9.00 ÷ 9.00	1.71 ÷ 1.71
		0.2	8.17 ÷ 17.87	1.55 ÷ 1.93	15.87 ÷ 15.87	2.11 ÷ 2.11
		0.5	37.16 ÷ 37.16	2.55 ÷ 2.55	—	—
		0.7	39.00 ÷ 49.05	1.08 ÷ 2.93	—	—
	(23)	0.05	2.20 ÷ 3.39	0.88 ÷ 1.09	—	—
		0.1	4.19 ÷ 4.80	1.07 ÷ 1.12	—	—
	(24)	0.05	5.31 ÷ 5.65	1.10 ÷ 1.34	5.22 ÷ 5.29	1.40 ÷ 1.40
		0.1	—	—	8.95 ÷ 9.01	1.70 ÷ 1.71
		0.2	—	—	15.90 ÷ 15.94	2.11 ÷ 2.19
		0.5	35.86 ÷ 35.86	3.19 ÷ 3.19	—	—
(19)	(22)	0.05	5.38 ÷ 13.11	0.98 ÷ 2.50	5.23 ÷ 6.80	1.40 ÷ 1.73
		0.1	9.44 ÷ 27.03	1.35 ÷ 3.79	8.95 ÷ 10.80	1.70 ÷ 2.04
		0.2	16.93 ÷ 29.52	1.71 ÷ 3.57	15.83 ÷ 15.87	2.11 ÷ 2.11
		0.5	36.59 ÷ 54.22	2.22 ÷ 3.36	—	—
		0.7	49.38 ÷ 57.04	2.51 ÷ 3.03	—	—
		1	66.52 ÷ 76.19	2.92 ÷ 3.62	—	—
	(23)	0.05	6.77 ÷ 7.24	1.43 ÷ 1.95	6.75 ÷ 6.80	1.72 ÷ 1.73
		0.1	10.81 ÷ 11.14	1.95 ÷ 2.33	10.73 ÷ 10.79	2.04 ÷ 2.04
	(24)	0.05	5.30 ÷ 5.93	1.01 ÷ 1.62	5.22 ÷ 5.29	1.40 ÷ 1.41
		0.1	9.04 ÷ 9.36	1.40 ÷ 1.68	8.96 ÷ 9.01	1.70 ÷ 1.71
		0.2	—	—	15.90 ÷ 15.94	2.12 ÷ 2.12

, statistical data (length and energy ranges for paths satisfying Condition (26)) were collected. The case when no path meets Condition (26) is marked with a dash (or not included at all when both versions of the Newton algorithm failed).

Based on results of the tests, some observations can be formulated:

• Length and energy of resulting paths strongly depend on initial values of ${\mathit{p}}_c$, especially when the basic Newton algorithm is used. The Newton algorithm, being local and iterative, has to generate trajectory-dependent results (the next iteration output depends on the output of the current iteration) and it tends to get into a desired location as fast as possible. It may happen that the basic algorithm provides better results than the optimized one. However, a smaller variance of results was observed in the energy-optimized version. In this case, the search space is searched more thoroughly.
• Trajectories generated with the optimized version of the Newton algorithm usually form a few clusters corresponding to local minima. This feature was not observed in the basic algorithm, as trajectories are not constrained by the energy minimization.
• Although the null-space optimization Newton algorithm does not necessarily guarantee the best accuracy, still much more often it promptly reaches the target.
• The failure ratio, cf. Equation (26), is much bigger for non-redundant parameterizations. What is interesting, in some tasks, parameterizations with the same number of parameters, cf. (23) and (24), generate significantly different results. Sometimes a task is solvable in one parameterization, while it is not in the other.
• The optimization within the null space significantly increases computational costs. In order to decrease the costs, this version of the Newton algorithm might be run after the target point was reached using the basic algorithm.
• For the unicycle robot and the identity output function, the quality of the solutions was generally better than that for tasks with smaller dimensional task-spaces, likely due to simplicity of the model and a small number of local minima. However, the length of a path and an energy of motion is smaller for tasks with low-dimensional output functions as it is less restricted.
• Obviously, the higher dimensional output space is, the more difficult task is generated. Additionally, a high redundancy in a parameter space is more computationally costly, but gives more flexibility as well. Thus, a compromise between flexibility and costs has to be found.
• Computational costs are smaller for the identity output function than for other output functions, as there is no need to transfer data between configuration and task spaces.
• Short length steps, characterized by small values of $\delta$, are easier to control.
• There are a few sources of inaccuracies in motion planning using Lie-algebraic methods: the truncation of the Lie series (5), evaluation of vector fields at $q_c$ although a trajectory surrounds the point (quite well visible for long-range motions) numeric properties of the Newton algorithm. All of them can be controlled by the right choice of parameters.
• A planning accuracy varies substantially with the current configuration $q_c$, as vector fields in the Lie-algebraic methods are evaluated at the point, cf. Equation (5), and vector fields exhibit different variability around different ${\mathit{q}}_c$.
• In almost all xy-projection plots one can notice characteristic cusps, which are typical in Lie-algebraic motion planning. At a close vicinity of a cusp a tangent line to the trajectory remains almost the same, while direction of motion is changed. It means that due to particular values of controls and generators, some components of velocity $\dot{q}$ in Equation (1) change their signs while passing through zero. An illustrative example of such behavior can be constructed by setting: $\mathit{X}$ long, $\mathit{Y}$ short, $u_2$ small, and $u_1$ control passing though zero. Therefore, one can expect that the number of cusps strongly and positively correlates with frequencies of controls. For mobile robots tested the explanation is even simpler, as the cusps result from linear velocity sign changes.
• Many trajectory cusps result in a zigzag motion, which is especially useful in obstacle-cluttered environments as the volume of maneuvers can be controlled and kept small. However, it is also visible that generating a relatively short motion along high degree vector fields requires a relatively long path to follow.
• Results for the kinematic car are the most interesting, especially for the identity output function. In this case, a hard to meet condition $q_4=0$ forces extensive movement. This condition is particularly difficult to satisfy for non-redundant parameterizations. Therefore, in some sub-figures of Task 4, cf. Figure 3, scales of axes were extended to present the whole paths. Nevertheless, the point ${\mathit{x}}_{freal}$ (cf. Figure 3 b,c,e,f) is close to the desired point ${\mathit{x}}_f$ as required.
• It should be noted that planning a motion of the kinematic car is qualitatively more difficult than of the unicycle, as a high degree (equal to 3) of vector fields have to be involved, comparing to degree 2 for the unicycle, cf. (16), (17).
• It may look strange that in Task 6, cf. Figure 3e, the projected path with the biggest energy is relatively short. In this case, the major contribution to the energy consumption was caused by control ${\mathit{u}}_2$, responsible for reorientation of the robot.

4. Conclusions

In this paper, the Lie-algebraic method of motion planning for nonholonomic systems, originally designed to plan within the configuration space, has been extended to also work in a task-space. There are some advantages of planning within the task-space:

• a smaller than the configuration space dimension either increases redundancy and facilitates optimizing a motion or allows to decrease the number of parameters of controls,
• it allows to define and solve many practical tasks, especially in obstacle-cluttered environments when some components of the configuration vector are not important.

However, there are also some disadvantages:

• a new source of singularities introduced by the output mapping,
• the impossibility to plan only within the task-space, without referring explicitly to the configuration space (usually the task-space dimension is smaller than the configuration space dimension, thus no unique mapping between the two spaces exists).

Some general observations based on simulation results:

• Depending on the planning purpose, the Newton algorithm, extensively used in the proposed algorithm, can be applied with or without optimization. The first version (low computational costs) should be used for tasks with no extra requirements or as a first try solution.
• A small redundancy in setting the search space size ($=dim\left(\mathit{p}\right)$) is advised not to cause a significant increase of computational costs, while preserving flexibility and solvability of planning tasks.
• An initial value of the parameter vector $\mathit{p}$ highly impacts solvability of the planning task and also the resulting trajectory shape. Therefore, the multi-start technique is recommended, especially when planning is performed in the off-line mode.

Our future research will concentrate on applications of the Lie-algebraic method in difficult environments (narrow passages, non-convex obstacles) in the task-space. In such environments, the planning should be performed through selection of sub-goals on the way to the target position. The main difficulty is the sub-goal selection is caused by the fact that nonholonomic systems are not governed by the Euclidean geometry, but rather the sub-Riemannian one [19]. The Lie-algebraic method is local and general-purpose, thus it is particularly well suited for solving tasks in partially known or dynamic environments.