Egalitarian versus prioritarian approach in multiple task motion planning for nonholonomic systems

In this paper, two different concepts of multiple task motion planning algorithm for nonholonomic systems are considered. The egalitarian approach treats all the tasks equivalently and tries to solve all the tasks simultaneously. In contrast, the prioritarian approach arranges the tasks with decreasing priorities in such a way that the solution of the lower order task should not influence the solution of the higher order task. This paper contains the derivation of the egalitarian motion planning algorithm and the prioritarian motion planning algorithm. Moreover, the definitions of the three types of basic subtasks are also included. The efficiency of the egalitarian and prioritarian algorithms is presented with the simulation of the nonholonomic model of the unmanned surface vessel. The simulation results provide the data to perform a comparison of the egalitarian versus the prioritarian approach.


Introduction
A solution of the motion planning problem in nonholonomic systems provides the control function which drives the system from an initial to a desired point. The multiple task motion planning problem is an extension of the classical motion planning problem. The multiple task motion planning algorithm solves the proper motion planning task along with one or more additional tasks, named subtasks. These subtasks may be diverse and may depend on the control, state space or task space variables.
The idea of multiple task motion planning in robotics comes from the theory for redundant manipulators. According to [6], there are several ways to solve the inverse kinematics problem for manipulators with joint redundancy. In general, the inverse kinematics problem for redundant manipulators has more than one solution, and the main issue is how to choose the desired one. On the other hand, this fact could be used to solve some additional tasks beyond the inverse kinematics problem. Two main techniques have been developed to solve the inverse kinematics problem with additional subtasks. The first one based on the extended Jacobian was proposed in [2] and revised in [4]. This technique uses specific extending functions which produce the square extended Jacobian matrix, finally inverted to obtain a solution of the inverse kinematics problem. A very similar approach is presented in [16], where the augmented Jacobian is defined. This approach, either extended Jacobian or augmented Jacobian, treats the main task (the proper motion planning) equivalently with additional subtasks, thus we will call it an egalitarian approach. The second approach used to solve the multiple task problem is the task priority approach [5,11]. This method arranges the subtasks in accordance with decreasing priorities. As long as the manipulator is redundant, the algorithm's design can be based on the fact that the kernel of the Jacobian matrix is non empty. This strategy results in that the solution of a task with lower priority does not influence the solution of the higher priority task. For the sake of simplicity, we will refer to that method as a prioritarian approach.
This work focusses on the egalitarian multiple task motion planning algorithm and the prioritarian multiple task motion planning algorithm for nonholonomic systems. These two algorithms are derived within the endogenous configuration space approach [19]. This approach introduces many analogies between the holonomic robotic manipulators and the nonholonomic robotics systems. Relying on this analogy, we shall introduce two multiple task motion planning algorithms. Some applications of the multiple task motion planning algorithm based on the endogenous configuration space approach have already been published. For example, the work [9] introduces the augmented Jacobian algorithm. In the paper [20], the extended Jacobian is used to approximate another Jacobian algorithm. The prioritarian approach could be found in [13,14].
The primary contribution of this work is the design of two multiple task motion planning algorithms. The paper [13] introduces the prioritarian multiple task motion planning algorithm. Contrary to [13], in the presented paper, the derivation of the prioritarian algorithm is provided in more systematic way. In addition, this paper also includes the derivation of the egalitarian multiple task motion planning algorithm. Moreover, the three types of basic subtasks are defined relaying on the theory introduced in [14]. These subtasks may be treated as a base for the construction of more complicated subtasks. The paper also presents a discussion about numerical aspects and the guidelines concerning the algorithms implementation. Additional contribution is the comparison between the egalitarian approach and the prioritarian one based on the simulation results.
A secondary contribution of this paper is the implementation of both multiple task algorithms by means of the nonparametric and higher order differential equation solver [15]. The paper [15] presents the vari-ous approaches to numerical computation of the single task motion planning algorithm. In this paper, this approaches are extended to the multiple task motion planning algorithms.
Up to now, there is no existing work which introduces the systematic derivation of the egalitarian and prioritarian motion planning algorithms. Moreover, the previous works did not discuss the numerical aspects of the implementation of both algorithms with nonparametric approach.
The remaining parts of this work is arranged in the following way. Section 2 introduces preliminaries about the endogenous configuration space. The definitions of both multiple task Jacobian motion planning algorithms are presented in Sect. 3. In Sect. 4 we define three types of subtasks. Computer simulation results and the comparison of the egalitarian and the prioritarian algorithm are collected in Sect. 5. Section 6 contains conclusions.

Endogenous configuration space
The endogenous configuration space approach [19] will be adopted to a control affine system of the form where q ∈ R n is a state space vector, u ∈ R m represents control vector and y ∈ R r is a vector of coordinates in the task space. G(q) is a n × m control matrix, f (q) is a drift term and k(q) denotes an output function. We assume that all the vectors and functions appearing in (1) are smooth. The admissible control functions u(t) belong to the control space U named the endogenous configuration space. The endogenous con- and the norm With every control function u(t) ∈ U we associate the corresponding state trajectory q(t) = ϕ q 0 ,t (u(·)) and the task space trajectory y(t) = k(ϕ q 0 ,t (u(·))), where ϕ q 0 ,t (u(·)) denotes the flow of the system (1) at the moment t, initialized in q 0 and driven by the control function u(·). The endogenous configuration space approach establishes an analogy between the holonomic redundant manipulators and the nonholonomic robots. Following this line of reasoning, we can define the kinematics as the end point map K q 0 ,T : U → R r of the system (1) as This map computes the final point y(T ) of the output of (1) under the influence of the control u(·). Still following the analogy to the manipulation robots, we can differentiate the end point map to obtain the Jacobian J q 0 ,T : U → R r [19] ∂q come from the associated linear system, which is actually the linear approximation to (1) along a control-trajectory u(·), q(·) pair and Φ(t, s) is the fundamental matrix of (5) solving the partial differential equation ∂Φ(t,s) ∂t = A(t)Φ(t, s) with the boundary condition Φ(s, s) = I n [17]. The Jacobian (4) determines how the output y(T ) will change in response to infinitesimal changes v(·) of the control function u(·).
Before we define the Jacobian motion planning algorithm, we have to introduce the Jacobian inverse and the adjoint Jacobian. Let us begin with the Jacobian inverse. As we have already mentioned, the Jacobian (4) transforms the variations v(·) ∈ U of the endogenous configuration into variations η ∈ R r in the task space, so we can write the Jacobian equation As long as we stay within the region of regular configurations, i.e., the J q 0 ,T u(·) is surjective, we can solve (6) with respect to v(·) and obtain a Jacobian inverse. Using the least squares method and the Lagrange multiplier technique, one can derive the Jacobian pseudoin- where is the Gram matrix which can be used to distinguish regular and singular configurations. If the Gram matrix has full rank then the configuration is regular and the system (1) is locally (along the control-trajectory pair) controllable. The Gram matrix could be computed by solving the Lyapunov differential equation with the initial condition M(0) = 0, and then substituting G q 0 ,T (u(·)) = C(T )M(T )C T (T ). Finally, we introduce the adjoint Jacobian J * q 0 ,T (u(·)) : (R r ) * → (U) * , which transforms the dual space to the task space into the dual configuration space in accordance with the formula where ·, · denotes the inner product (2). Using the formula (10) we derive the adjoint Jacobian [19] Invoking the definition of the adjoint Jacobian (11), the Gram matrix (8) can be rewritten as and the Jacobian pseudoinverse (7) as Observe that (13) resembles the classic formula In the redundant manipulators, the number of additional tasks is limited by the degree of redundancy (the difference between the number of joints and the dimension of the task space). In the endogenous configuration space approach, the Jacobian (4) operates on the infinite-dimensional endogenous configuration space. This means that in the case of the nonholonomic systems the so-called "degree of redundancy" is infinite and the number of additional subtasks could be theoretically unlimited.

Jacobian motion planning algorithm
In this section, we shall introduce the motion planning algorithm based on the previously defined Jacobian. Firstly, we provide a derivation of the proper motion planning algorithm. Then, we expand the approach and define two multiple task algorithms which will plan the motion simultaneously with one or more additional tasks.
Let us introduce some nomenclature. The proper motion planning algorithm solves the proper motion planning problem which consists only of the motion planning task. The multiple task motion planning algorithm can solve the proper motion planning problem as well as one or more additional tasks. These additional tasks will be called subtasks. For the clarity of presentation, we assume that the subtask with index 0 is the proper motion planning task, and the total number of subtasks is denoted with s + 1. Each subtask will be defined by its task map i K q 0 ,T (u(·)), i = 0, 1, . . . , s. For the proper motion planning (i = 0), the task map is defined by (3). The other task maps as well as the corresponding Jacobians, their inverses, and the subtask errors will be defined later on.
The definition of the proper motion planning problem for system (1) is as follows: Find a control function u(·) ∈ U which drives the system (1) from a certain initial space configuration q(0) to desired point y d ∈ R r in the task space in a prescribed time interval [0, T ]. With such defined problem, we can associate the error formula as e(u(·)) = K q 0 ,T (u(·)) − y d = 0 K q 0 ,T (u(·)) − y d , where K q 0 ,T (u(·)) is defined in (3). We want to solve such a problem using the continuation method [18]. For this reason, we choose in the control space U a smooth curve u ϑ (·) ∈ U parametrized by ϑ ∈ R, passing through a certain initial configuration (control) u 0 (·). For that curve, we define the motion planning error as Let us assume that the error should decrease exponentially along the curve with a decay rate γ > 0 The substitution of the (14) into (15) yields the Ważewski-Davidenko equation To solve equation (16) we can use any right Jacobian inverse. If we use the Jacobian pseudoinverse (7) then the dynamic system of the proper motion planning algorithm can be expressed as The solution of the motion planning problem is the control function obtained as a limit lim ϑ→∞ u ϑ (·) of the resultant trajectory of (17). In the following subsections, we shall present two ways of modifying the proper motion planning algorithm in order to solve the motion planning problem with additional tasks.

Multiple task motion planning
The multiple task motion planning algorithms can be twofold. An egalitarian algorithm treats all subtasks equivalently, while a prioritarian algorithm arranges the subtasks with decreasing priorities. The next two subsections introduce these two types of algorithms.

Egalitarian algorithm
As was already mentioned, the egalitarian approach to motion planning treats the proper motion planning task as well as all the additional subtasks as equally important. To derive the egalitarian algorithm, let us begin with the definition of the collective error where 0 e(ϑ) is the proper motion planning task error (14) and i e(ϑ) = i K q 0 ,T (u ϑ (·)) ∈ R r i for i = 1, 2, . . . , s stand for the errors of the subtasks. Assuming that the error should decrease exponentially with the decay rate γ , we can write the Ważewski-Davidenko equation where is a collective Jacobian, 0 J q 0 ,T (u(·)) is defined by (4), and i J q 0 ,T (u(·))v(·) = D i K q 0 ,T (u(·))v(·), i = 1, 2, . . . , s is the Jacobian for the i-th subtask. Using the pseudoinverse of the collective Jacobian (20) where the collective adjoint Jacobian J * q 0 ,T (u(·)) is defined analogously to (10), one can solve equation (19) and obtain the egalitarian motion planning algorithm as a dynamic system where E = blockdiag{ i I r i } is a block diagonal weight matrix, which allows us to scale the influence exerted by the particular subtask on the problem solution, by tunning the i value. The control function which solves the egalitarian motion planning problem is computed as a limit lim ϑ→∞ u ϑ (·) of the solution of the system (22).

Prioritarian algorithm
Differently to the egalitarian approach, the prioritarian approach assigns to each subtask a priority with decreasing order. The subtask with a higher priority is taken into consideration before the subtasks with lower priorities. It also means that in case when the solution of lower priority subtask would influence the solution of the higher priority task, the lower priority task may not be solved. Let us assume that the subtask index indicates also the priority (0 means the highest priority).
For the i-th subtask, we can write Assuming that the error should decrease exponentially, one can write the algorithm which solves (23) using the Jacobian generalized inverse [3,19] du where is a projection of U onto ker i J q 0 ,T (u ϑ (·)), i μ ϑ (·) ∈ U and id U is the identity map. Following [5], let us reformulate the derivation of the prioritarian multiple task motion planning algorithm for nonholonomic systems. For any two subtasks, say subtasks 1 and 2, the control function u ϑ (·) has to be the same, so we can write from (24) Using the projection property of: idempotence i P q 0 ,T (u ϑ (·)) i P q 0 ,T (u ϑ (·)) = i P q 0 ,T (u ϑ (·)), symmetry i P q 0 ,T (u ϑ (·)) = i P T q 0 ,T (u ϑ (·)), and the annihilation i P q 0 ,T (u ϑ (·)) i J # q 0 ,T (u ϑ (·)) = 0, the (26) could be rearranged as follows Substituting (27) into (24) with i = 1 we arrive at the algorithm which, if we set 2 μ(·) = 0, is the prioritarian algorithm for two subtasks. The endogenous configuration 2 μ(·) could be used to expand the algorithm for more subtasks. Finally, the prioritarian algorithm for s-subtasks takes the form of the dynamic system [13] du where −1 P q 0 ,T u ϑ (·) = id U . In terms of [1] this algorithm is a successive inverse-based projection method. Similar to the previous algorithms, the solution of the motion planning problem is a control function obtained as a limit lim ϑ→∞ u ϑ (·) of the trajectory of (29). In the prioritarian algorithm, the significance of a particular subtask is controlled by the corresponding decay rate i γ .

Comparison remarks
Having defined the two multiple task motion planning algorithms, namely the egalitarian (22) and the prioritarian (29), we can provide some comparison remarks. The analysis of the algorithm formulas shows that in the egalitarian approach we have to compute the inverse operator J # q 0 ,T : Rr → U wherer = s i=0 r i . On the other hand, in the prioritarian algorithm the multiple inverse operators of the form i J # q 0 ,T : R r i → U must be found. Taking these two facts into account, one can observe that the prioritarian approach is a kind of decomposition of the larger problem while the egalitarian approach tries to solve the entire problem at once. However, the prioritarian algorithm is usually more difficult to implement, for example due to the projection P q 0 ,T calculation.
The construction of the two multiple motion planning algorithms imposes the usage for specific problems. If the subtasks can be arranged with decreasing priorities, then the prioritarian approach should be preferred. As it was already written, the prioritarian algorithm may return the solution which does not solve all of the subtasks. The egalitarian algorithm works differently, it tries to solve all subtasks simultaneously. However, when the solution of all subtasks does not exists at all, or the subtasks compete with each other, then the egalitarian algorithm will fail. In such case, the prioritarian approach will return the best possible solution. As long as the subtasks are equivalent in the egalitarian algorithm, it is possible that one of the subtasks (let us say less important) will dominate the solution. It is even possible to obtain some oscillations, when the subtasks start to compete. When the error of one subtask dominates the other one, then the algorithm tries to reduce that error. When it becomes smaller, then the other one starts to dominate. Such situation leads to no solution. When the prioritarian algorithm is applied the oscillations are not possible.

Subtasks
As was already written, the subtasks are described by the task maps i K q 0 ,T (u(·)) : U → R r i . For the construction of the multiple task motion planning algorithm, it will also be necessary to define the subtask Jacobian i J q 0 ,T (u(·)), its inverse ( i J # q 0 ,T (u(·)))(t), and the subtask's error i e(ϑ).
Let us assume that the i-th task map have the following form where i α(q(t), y(t), u(t)) ≥ 0 is smooth and will be denoted for short as i α(t). For such a task map the corresponding Jacobian takes the form [14] i and its inverse computed in [14] is The function i β(t) may be computed by solving the following differential equation with the final condition i b(T ) = 0, and then substituting the solution of (34) into For every subtask with i = 1, 2, . . . , s we assume that the subtask error is determined by the subtask end point map, i.e., i e(ϑ) = i K q 0 ,T u ϑ (·) . According to the above formulas, to define any subtask it is necessary to define the functions i α(t) and i β(t). In this paper we shall introduce three different subtasks.
1. Control energy minimization-is a subtask which minimizes the total control energy. 2. State variable minimization-in this subtask the value of one or more state variables should be as close to 0 as possible during the motion time. 3. Obstacle/Singularity avoidance-this subtask introduces an obstacle function h(y) which describes the locations and shapes of the obstacles. If the obstacles are defined in the task space, we have "classical" obstacle avoidance problem. If the obstacles are defined in the state space then the problem could be treated as a kind of the singularity avoidance problem. Table 1 collects all necessary functions to completely define these three types of subtasks, that could be treated as basic.
The first one represents the function which depends on control signals u(t), the second one depends on the ∂ y T ds state trajectory q(t), and the third function depends on the output trajectory y(t). The matrix σ (s) is a diagonal weight matrix which allows us to control the influence on the solution of the particular components of the vector u(t) or q(t). Obviously, one can define any other subtask by combining the basic ones. The example of control energy minimization is presented in [13]. The singularity avoidance could be found in [14]. The work [21] introduces a subtask corresponding to a reduction of the wheels slip.

Numerical example
Now, we shall shift our attention toward the numerical aspects of the multiple task motion planning algorithms. All computations were accomplished in the MATLAB environment. We shall describe our computation methods and make some comments on the organization of computation. Later on, we shall present some simulation results and assess the algorithms efficiency.

Computational aspects
The algorithm equation (22) as well as (29) is a specific functional differential equation. To solve this kind of equation a specific computation methodology needs to be adopted. Till now [9,13,19,20], the solution of motion planning algorithm with endogenous configuration space approach has been mostly obtained by control function parametrization (truncated series parametrization) and by the algorithm discretization (fixed-step-size Euler method). The recent research [15] shows that the computations could be made in a more effective way. In this work, the multiple task algorithms are implemented in the nonparametric version, assisted with higher order algorithms of differential equations integration. As mentioned before, the equation underlying the motion planning algorithms is a functional differential equation depending on two variables t and ϑ. To provide the necessary computations, we proceed along the following lines: 1. For the initial value ϑ = 0 choose an arbitrary initial control function u ϑ=0 (t) = u 0 (t) ∈ U. 2. Apply the control u ϑ (t) to control affine system (1) to obtain the error values. 3. If the motion planning error 0 e(ϑ) (14) and the errors of the other subtasks i e(ϑ) are below the assumed limits then the problem is solved, otherwise proceed to 4. 4. Compute a new control function u ϑ (t) from the algorithm equation (22) or (29) for the next value of ϑ and return to 2.
Proceedings as above, especially with respect to item 4, for the computation of the control function u ϑ (t) as long as it depends on ϑ, it is necessary to solve for every ϑ the following differential-algebraic system of equations with initial conditions q ϑ (0) = q 0 , M ϑ (0) = 0, the final condition b ϑ (T ) = 0 and the boundary condition Φ(T, T ) = I n . The resultant trajectories from (36) could be used to write every ingredient of the egalitarian multiple task motion planning algorithm (22) or the prioritarian multiple task motion planning algorithm (29). To summarize, the computations are organized in the form of two nested differential equation solvers. The inner solver computes the solution of the system (36) for t ∈ [0, T ], while the outer solver determines the resultant trajectory of the algorithm equation (22) or (29) for ϑ → +∞. An outline of the pseudocode illustrating the above procedure may look as follows These two solvers are built-in MATLAB variable step methods (e.g., ode45), thus the computation accuracy could be easily controlled. In the pseudocode this fact is denoted with the function OPTIMAL, which returns the optimal step length to achieve the demanded accuracy. In theory, the solution of the multiple task motion planning algorithm is obtained as a limit lim ϑ→∞ u ϑ (·), practically we stop the computation when the errors drop below the assumed levels (e max ).

Simulation results
As a testbed we have chosen a simplified model of the underacutated surface vessel, often called unmanned surface vessel (USV) [10] which is a subclass of autonomous underwater vehicle (AUV) [7]. Such a system could be regarded as a hovercraft model.
If we assume that the body of the vessel has the disk shape, and the propellers are located at the center of mass (Fig. 1) then the dynamic equation of motion takes the form [8,12] where the q = (x, y, θ, ν u , ν v , ν r ) is the state space vector. The x, y and θ represent the position and orientation, ν u , ν v and ν r denote the linear surge velocity, linear sway velocity and angular yaw velocity, respectively. We assume that only the control force in surge u u and control torque in yaw u r are available. The system (37) can be transformed into the affine control system (1) with the following vector fields To assess efficiency of the two multiple task motion planning algorithms, we create three simulation scenarios. In each scenario, a different type of subtasks is considered. In every case, the problem is solved using first the egalitarian and then the prioritarian algorithm. For the comparison, we also add a solution of the motion planning problem obtained by the single task (only proper motion planning) algorithm. In all figures in the following subsection, we use the corresponding indexes to distinguish the algorithms. So, the single task algorithm is denoted with "S", the priori-tarian algorithm is marked with "P" and the index "E" reffers to the egalitarian algorithm.

Motion planning with control energy minimization
In the first problem we want to reach a destination point together with the control energy minimization as a minimization of the integral T 0 u T (t)σ (t)u(t) dt. The simulation parameters are collected in Table 2.
The selected form of matrix σ (t) means that we are interested only in reducing the energy of the second control u 2 (t), namely the yaw torque. The simulation results are depicted in Figs. 2, 3, 4 and 5. The motion path is presented in Fig. 2. It can be observed that both multiple task algorithms return a wider motion path than the path from the single task motion planning algorithm. The result of the control energy minimization could also be seen in Fig. 3 where the plots of the control function u 2 (t) are shown. It can be observed, that using the egalitarian approach the amplitude of control function is smaller than in the prioritarian case. Figures. 4 and 5 present the convergence of the proper motion planning task error and of the control minimization task error, respectively. Here also, one can see that the egalitarian approach produces smaller error of the control minimization task 1 e(ϑ) (see Fig. 5). On the Table 2 Motion planning with control energy minimizationsimulation parameters other hand, the proper motion planning task is solved more accurately by the prioritarian algorithm (the error 0 e(ϑ) in Fig. 4 takes smaller values for the prioritarian approach than the egalitarian). Figures 4 and 5 present also the main difference between these two approaches. The prioritarian algorithm allows the additional subtask error to temporary increase as long as the first task error convergence is not interrupted. Differently, in the egalitarian approach both errors have to decrease simultaneously and the influence of the 1 e(ϑ) on the 0 e(ϑ) results in higher value of 0 e(ϑ) than for the prioritarian approach (see Fig. 4). The saturation of the 0 e(ϑ) error decrease, which can be seen in Fig. 4, depends on the computation accuracy. The improvement of the computation accuracy results in decreasing the saturation level, but on the other hand, it increases the computation time.

Motion planning with state variable value minimization
The second problem consists in finding a control function which drives the model from an initial to a desired point, together with the minimization of a state variable in the meaning of the minimization of the integral T 0 q T (t)σ (t)q(t) dt . Table 3 collects all parameters used in the simulation. We want to minimize only the state variable q 5 (t) = ν v (t), by choosing a suitable σ (t) matrix. It means that in the whole motion the linear sway velocity should be as small as possible. Figures from 6 to 9 present the results of the simulation. The path in XY plane (Fig. 6) shows the contribution of the additional subtask to the motion planning problem solution. The resultant paths obtained from both algorithms, egalitarian and prioritarian, are almost the straight lines. At the beginning of the motion, the vessel changes its orientation, next it goes ahead toward the desired point, and finally the orientation is changed again, so the linear sway velocity is close to zero during the whole motion time, as can be observed in Fig. 7. For comparison, the solution of the single task algorithm is also depicted. It is obvious that both the egalitarian and the prioritarian multiple task algorithms outperform the single task algorithm. Figure 8 displays the convergence of the main task error 0 e(ϑ), and the additional subtask error 1 e(ϑ) is plotted in Fig. 9. The value Table 3 Motion planning with state variable value minimization-simulation parameters of the secondary task error 1 e(ϑ) for both multiple task algorithms is of the same order of magnitude; however, for the egalitarian algorithm, the value of 1 e(ϑ) reaches a lower value than for the prioritarian algorithm. The convergences of the main task error 0 e(ϑ) for both multiple task algorithms are comparable. The value of the main task error 0 e(ϑ) reached by the single task algorithm shows that the additional task influences the minimum value of the main task error reached by both multiple task algorithms. As was already said, also here the saturation and the fluctuation of the plots come from the inaccuracy of numeric computations.

Motion planning with obstacle/singularity avoidance
The last simulation shows a result of the motion planning composed with the avoidance of an obstacle. To determine this problem, it is necessary to define the obstacle function h(y). This function h(y) should have small values in the obstacle free regions, and take high values where the obstacles exist. Such a definition of the obstacle function provides that the algorithm finds control functions producing system trajectory which is repelled from the obstacle. The definition of the obstacle function h(y) refers to the potential field theory. In the presented case, the obstacle function was selected as  Table 4 Motion planning with obstacle avoidance-simulation parameters represents the lengths of the rectangle edges, so in this case it is a square. Figure 10 presents the plot of the obstacle function h(y). The remaining simulation parameters are collected in Table 4.
The results of the simulation are depicted in Figs. 11, 12, 13 and 14. The path in XY plane, together with the contour of h(y) function is presented in Fig. 11. One can see that both algorithms, egalitarian and prior-  Figure 14 shows that the prioritarian algorithm, for small values of ϑ, allows the error 1 e(ϑ) to increase, while the main task error 0 e(ϑ) decreases. On the contrary, in the egalitarian approach both errors decrease simultaneously. Another observation is that for the egalitarian algorithm the presence of the additional task affects the proper motion planning task solution. It can be observed that the final value of 0 e(ϑ) is greater for the egalitarian than the prioritarian algorithm. Again, the fluctuations of the error convergences could be refined by increasing the computation accuracy, however such a modification may have impact on the computation time.

Algorithms efficiency comparison
In this section, we shall make the efficiency comparison between the egalitarian and the prioritarian algorithms. From the quantitative point of view, the egal- itarian algorithm usually reaches smaller error values for the additional subtasks than the prioritarian algorithm. However, this fact usually affects the value of main task error which is often higher than in the prioritarian approach. From the qualitative point of view the main difference between the egalitarian and the proritarian algorithms is in the way of reaching the solution. We shall explain the differences by reference to Figs. 15 and 16, where the solution of the obstacle avoidance problem is presented for another obstacles placement.
With increasing ϑ parameter the egalitarian approach (Fig. 15) provides trajectories which simultaneously reach the desired point and guarantee the obstacle avoidance. The solution of the prioritarian algorithm  16) very quickly (for small values of ϑ) reaches the destination point and then with increasing ϑ, begins to modify the trajectory in order to solve the additional subtask. Very often, the prioritarian approach allows the additional subtask error to increase (for small values of ϑ) as long as the main task error decreases. Table 5 collects the number of steps necessary to solve the previously presented problems together with the time needed to compute single step of the particular algorithm. One can observe, that the prioritarian approach always needs more time to compute the single step than the egalitarian. This is due to the prioritarian algorithm is more difficult in implementation. On the other hand, as it can be seen in Table 5, the egalitarian approach requires more number of steps to obtain the solution than the prioritarian algorithm.
To sum up, from the quantitative point of view, both the algorithms return similar error values, so the choice of the right algorithm should be done by taking into consideration the qualitative point of view. If the two or more task have to be treated with the same weight then the egalitarian algorithm should be used. The prioritarian approach will have better efficiency for problems where the additional subtasks are essentially less important than the main task. The differences between the egalitarian algorithm and the prioritarian algorithm are important when there is not enough time to make long-term computations. As long as the evaluation time of one algorithm step for the egalitarian algorithm, as well as for the prioritarian algorithm is comparable, we have to decide if the computation could be stopped early. Let us assume that we cannot wait until ϑ reach large values. If someone chooses the egalitarian algorithm then for some small values of ϑ the system output y(T ) (proper motion planning) is still far from the desired point, obviously is should be closer to the desired point than the initial point. Also, the additional subtask is solved simultaneously, and finally, after a short computation time, one can obtain some temporary results where the system output is closer to the desired point and for example, the obstacles are avoided. If we repeat the above line of reasoning for the prioritarian algorithm then after a short-term computation the system output y(T ) could be much closer to desired point than it was in the egalitarian case. Nevertheless, the solution of the additional subtask could be poor, and the value of the additional subtask error could be even greater than it was for the initial control u 0 (t).

Conclusion
This paper has presented two different approaches to the multiple task motion planning problem for nonholonomic systems, which consists of the proper motion planning problem and one or more additional subtasks.
The equation of the egalitarian algorithm as well as of the prioritarian algorithm derived with the endogenous configuration space approach are functional differential equations. This work has shown a result of the multiple task algorithm computations involving the nonparametric version of the algorithms and the higher order, variable step-size differential equation solver. Such a numerical approach is characterized on a greater accuracy than the parametric and fixed-step method.
Both algorithms, the egalitarian and the prioritarian, have successfully solved three exemplary multiple task problems. The algorithms efficiency comparison has been done with respect to the control energy minimization, the state variable value minimization and the obstacle avoidance.
It is worth to mention that in some situation the prioritarian algorithm could provide the solution which does not solve all the subtasks. In such case the prioritarian approach leads to best possible solution. On the other hand, the egalitarian algorithm in difficult cases may not return any solution.
The accuracy of the computation could be tuned with built-in MATLAB functionality. However, the increase in computational accuracy results in increasing the computational time. The time needed by a 3.2 GHz processor for one evaluation of the outer differential equation solver (i.e., for a particular value of ϑ) in the presented simulations is almost always below 0.5 s.
The presented subtasks constitute a base for the creation of more complicated subtasks. We have shown how to involve the control trajectory u(t), the state space trajectory q(t) and the output trajectory y(t) in the additional subtask. In this paper we solve the problems with only two subtasks: the proper motion planning and one additional subtask. Both algorithms, the egalitarian and the prioritarian, can be easily expanded to solve problems with more than two subtasks.