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

Nonlinear Dynamics, May 2017

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.

A PDF file should load here. If you do not see its contents the file may be temporarily unavailable at the journal website or you do not have a PDF plug-in installed and enabled in your browser.

Alternatively, you can download the file locally and open with any standalone PDF reader:

https://link.springer.com/content/pdf/10.1007%2Fs11071-017-3342-3.pdf

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

Egalitarian versus prioritarian approach in multiple task motion planning for nonholonomic systems Adam Ratajczak 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. Nonholonomic system; Jacobian motion planning; Multiple task; Egalitarian algorithm; Prioritarian algorithm; Unmanned surface vessel - 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 various 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. 2 Endogenous configuration space The endogenous configuration space approach [19] will be adopted to a control affine system of the form where q ∈ Rn is a state space vector, u ∈ Rm represents control vector and y ∈ Rr 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 configuration space U ⊂ L2m [0, T ] is a space of Lebesque square integrable functions defined on the time interval t ∈ [0, T ]. Moreover, the space L2m [0, T ] is a Hilbert space with the inner product u1(·), u2(·) = and the norm u(·) 2U = u1T(t )u2(t ) dt, uT(t )u(t ) dt. 0 With every control function u(t ) ∈ U we associate the corresponding state trajectory q(t ) = ϕq0,t (u(·)) and Jq0,T u(·) v(·) = C (T ) As long as we stay within the region of regular configurations, i.e., the Jq0,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 pseudoinverse (Moore–Penrose) Jq#0,T u(·) : Rr → U as [19] Jq0,T u(·) η (t) = BT(t)ΦT(T , t)CT(T )Gq−01,T u(·) η, # = C (T ) the task space trajectory y(t ) = k(ϕq0,t (u(·))), where ϕq0,t (u(·)) denotes the flow of the system (1) at the moment t , initialized in q0 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 Kq0,T : U → Rr of the system (1) as Kq0,T u(·) = y(T ) = k ϕq0,T u(·) . 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 Jq0,T : U → Rr [19] Jq0,T u(·) v(·) = D Kq0,T u(·) v(·) T = C (T ) where the matrices A(t ) = ∂( f (q)∂+qG(q)u) , B(t ) = G(q), C (t ) = ∂k∂(qq) 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 ∂Φ∂(tt,s) = A(t )Φ(t, s) with the boundary condition Φ(s, s) = In [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 η ∈ Rr in the task space, so we can write the Jacobian equation 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 = B(t )BT(t ) + A(t )M (t ) + M (t ) AT(t ) with the initial condition M (0) = 0, and then substituting Gq0,T (u(·)) = C (T )M (T )C T(T ). Finally, we introduce the adjoint Jacobian Jq∗0,T (u(·)) : (Rr )∗ → (U )∗, which transforms the dual space to the task space into the dual configuration space in accordance with the formula Jq∗0,T u(·) η, v(·) = ηT Jq0,T u(·) v(·), where ·, · denotes the inner product (2). Using the formula (10) we derive the adjoint Jacobian [19] Jq∗0,T u(·) η (t ) = BT(t )ΦT(T , t )C T(T )η. Invoking the definition of the adjoint Jacobian (11), the Gram matrix (8) can be rewritten as Gq0,T u(·) = Jq0,T u(·) Jq∗0,T u(·) , and the Jacobian pseudoinverse (7) as # −1 Jq0,T u(·) = Jq∗0,T u(·) Gq0,T u(·) . Observe that (13) resembles the classic formula J # = J T( J J T)−1. 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. 3 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 iKq0,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 yd ∈ Rr in the task space in a prescribed time interval [0, T ]. With such defined problem, we can associate the error formula as e(u(·)) = Kq0,T (u(·)) − yd = 0Kq0,T (u(·)) − yd , where Kq0,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) u0(·). For that curve, we define the motion planning error as e(ϑ ) = Kq0,T uϑ (·) − yd . 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 Waz˙ewski–Davidenko equation Kq0,T uϑ (·) − yd d = Jq0,T uϑ (·) dϑ uϑ (·) = −γ e(ϑ ). 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. 3.1 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. 3.1.1 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 e(ϑ ) = 0e(ϑ ), 1e(ϑ ), . . . , se(ϑ ) , where 0e(ϑ ) is the proper motion planning task error (14) and ie(ϑ ) = iKq0,T (uϑ (·)) ∈ Rri 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 Waz˙ewski–Davidenko equation is a collective Jacobian, 0Jq0,T (u(·)) is defined by (4), and iJq0,T (u(·))v(·) = D iKq0,T (u(·))v(·), i = 1, 2, . . . , s is the Jacobian for the i -th subtask. Using the pseudoinverse of the collective Jacobian (20) # Jq0,T u(·) = Jq∗0,T u(·) Jq0,T u(·) Jq∗0,T u(·) −1 3.1.2 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] = iJq0,T uϑ (·) = −iγ iJq#0,T uϑ (·) ie(ϑ ) (t ) is a projection of U onto ker iJq0,T (uϑ (·)), iμϑ (·) ∈ U and idU 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) − 1γ 1Jq#0,T uϑ (·) 1e(ϑ) + 1Pq0,T uϑ (·) 1μϑ (·) (t) − 2γ 2Jq#0,T uϑ (·) 2e(ϑ) + 2Pq0,T uϑ (·) 2μϑ (·) (t). where the collective adjoint Jacobian Jq∗0,T (u(·)) is defined analogously to (10), one can solve equation (19) and obtain the egalitarian motion planning algorithm as a dynamic system # = −γ Jq0,T uϑ (·) E e(ϑ ) (t ), where E = blockdiag{ i Iri } 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). Using the projection property of: idempotence iPq0,T (uϑ (·))iPq0,T (uϑ (·)) = iPq0,T (uϑ (·)), symmetry iPq0,T (uϑ (·)) = iPqT0,T (uϑ (·)), and the annihilation iPq0,T (uϑ (·))iJ # q0,T (uϑ (·)) = 0, the (26) could be rearranged as follows = −2γ 1Pq0,T uϑ (·) 2Jq#0,T uϑ (·) 2e(ϑ ) (t ) Substituting (27) into (24) with i = 1 we arrive at the algorithm duϑ (t ) = −1γ 1 # dϑ Jq0,T uϑ (·) 1e(ϑ ) (t ) − 2γ 1Pq0,T uϑ (·) 2Jq#0,T uϑ (·) 2e(ϑ ) (t ) 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] = − i=0 j=0 j−1Pq0,T uϑ (·) where −1Pq0,T uϑ (·) = idU . 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γ . 3.2 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 Jq#0,T : Rr¯ → U where r¯ = is=0 ri . On the other hand, in the prioritarian algorithm the multiple inverse operators of the form iJq#0,T : Rri → 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 Pq0,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. 4 Subtasks As was already written, the subtasks are described by the task maps iKq0,T (u(·)) : U → Rri . For the construction of the multiple task motion planning algorithm, it will also be necessary to define the subtask Jacobian iJq0,T (u(·)), its inverse (iJq#0,T (u(·)))(t ), and the subtask’s error ie(ϑ ). Let us assume that the i -th task map have the following form iKq0,T u(·) = 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] iJq0,T u(·) v(·) = and its inverse computed in [14] is The function iβ(t ) may be computed by solving the following differential equation = − A(t )T i b(t ) − with the final condition i b(T ) = 0, and then substituting the solution of (34) into ∂u For every subtask with i = 1, 2, . . . , s we assume that the subtask error is determined by the subtask end point map, i.e., ie(ϑ ) = iKq0,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 BT(t) tT ΦT(s, t)CT(s) ∂h(y(s)) Tds ∂y 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. 5 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. 5.1 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 ) = u0(t ) ∈ U . 2. Apply the control uϑ (t ) to control affine system (1) to obtain the error values. 3. If the motion planning error 0e(ϑ ) (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 ⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧ G∂ddqΦqMdϑ0dϑϑt,∂(tT(t(tT)t(),u=t=)(·=)f)B(ϑ=q−ϑ(ΦtC()tϑϑB)()(ϑTTT+(,t)t)GM)+A(ϑqϑ(AϑT(ϑ(t)t)(C),t))ϑTuM(ϑTϑ(t)(),t,) + Mϑ (t) AϑT (t), ⎪⎪⎪⎪⎪⎨⎪⎪⎪ Aϑ (t) = ∂( f (qϑ (t))+∂Gq(qϑ (t))uϑ (t)) , ⎪⎪⎪⎪⎪⎪⎪⎪ Bϑ (t) = G(qϑ (t)), Cϑ (t) = ∂k(q∂ϑq(t)) , ⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩⎪⎪⎪⎪ i0dβebϑ(dϑϑt((tt)))===K−BqϑT0A,(ϑTt)((btuϑ)ϑb((ϑt·)()t)+)−−y∂di,α∂ϑiαiϑqeϑ(qϑ(tϑ))∂(,tyu=)ϑ∂,y(qitϑ)K(,tuq)ϑ0,u,(Ttϑ)((tu)ϑT (,T·),), with initial conditions qϑ (0) = q0, Mϑ (0) = 0, the final condition bϑ (T ) = 0 and the boundary condition Φ(T , T ) = In. 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 (emax). 5.2 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] Fig. 1 Unmanned surface vessel 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 uu and control torque in yaw ur 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 prioritarian algorithm is marked with “P” and the index “E” reffers to the egalitarian algorithm. 5.2.1 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 0T uT(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 u2(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 u2(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 1e(ϑ ) (see Fig. 5). On the Table 2 Motion planning with control energy minimization— simulation parameters ⎛ 00 ⎞ 0 ⎝ 00 ⎠ 0 ⎛ 55 ⎞ 0 ⎝ 00 ⎠ 0 5 4 3 y2 Fig. 2 Motion planning with control energy minimization— path in X Y plane Fig. 3 Motion planning with control energy minimization— control u2(t) (t 1 ) 2 u ) (ϑ10 0 e Fig. 4 Motion planning with control energy minimization— convergence of 0e(ϑ) other hand, the proper motion planning task is solved more accurately by the prioritarian algorithm (the error 0e(ϑ ) 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 1e(ϑ ) on the 0e(ϑ ) results in higher value of 0e(ϑ ) than for the prioritarian approach (see Fig. 4). The saturation of the 0e(ϑ ) 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 Fig. 5 Motion planning with control energy minimization— convergence of 1e(ϑ) level, but on the other hand, it increases the computation time. 5.2.2 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 0T qT(t )σ (t )q(t ) dt . Table 3 collects all parameters used in the simulation. We want to minimize only the state variable q5(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 X Y 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 0e(ϑ ), and the additional subtask error 1e(ϑ ) is plotted in Fig. 9. The value Table 3 Motion planning with state minimization—simulation parameters ⎛ 00 ⎞ 0 ⎝ 00 ⎠ 0 ⎛ 22 ⎞ π ⎝ 00 ⎠ 0 Fig. 6 Motion planning minimization—path in X Y plane of the secondary task error 1e(ϑ ) for both multiple task algorithms is of the same order of magnitude; however, for the egalitarian algorithm, the value of 1e(ϑ ) reaches a lower value than for the prioritarian algorithm. The convergences of the main task error 0e(ϑ ) for both multiple task algorithms are comparable. The value of the main task error 0e(ϑ ) 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. 5.2.3 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 ) (ϑ10 0 e Fig. 7 Motion planning with state minimization—trajectory of q5(t ) = νv (t ) Fig. 8 Motion planning with minimization—convergence of 0e(ϑ ) 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 h(y) = where operation y2 means the element-wise power of vector y, i = 1, 2, 3 is the number of point-type obstacles, determined by the mass (radius) mi = 10 and the position yoi = {(1, 1); (1, 4); (4, 1)}, yc = (2.5, 2.5) describes the center of the restrictive rectangle and ) (ϑ10 1 e Fig. 9 Motion planning with minimization—convergence of 1e(ϑ ) Fig. 11 Motion planning with obstacle avoidance—path in X Y plane Fig. 10 Motion planning with obstacle avoidance—obstacles location Table 4 Motion planning with obstacle avoidance—simulation parameters ⎛ 00 ⎞ 0 ⎝ 00 ⎠ 0 ⎛ 55 ⎞ 0 ⎝ 00 ⎠ 0 yd = {3, 3} 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 X Y plane, together with the contour of h(y) function is presented in Fig. 11. One can see that both algorithms, egalitarian and priorFig. 12 Motion planning with obstacle avoidance—function h(q(t )) over the motion trajectory itarian, solve the planning problem. The resultant path reaches the desired point and avoids the obstacles. For comparison, we show that the trajectory obtained from the single task algorithm passes through an obstacle. The efficiency of the obstacle avoidance can be also observed in Fig. 12, where are depicted the values of the obstacle functions h(y(t )) along the trajectories of all three algorithms. The algorithms convergences are plotted in Figs. 13 and 14. Figure 14 shows that the prioritarian algorithm, for small values of ϑ , allows the error 1e(ϑ ) to increase, while the main task error 0e(ϑ ) decreases. On the contrary, in the egalitarian approach both errors decrease simultaneously. Another observation is that for the egalitarian algorithm the presence of ) (ϑ10 1 e Fig. 13 Motion convergence of 0e(ϑ ) Fig. 15 Algorithm comparison—egalitarian algorithm solutions over ascending ϑ Fig. 14 Motion convergence of 1e(ϑ ) Fig. 16 Algorithm comparison—prioritarian algorithm solutions over ascending ϑ the additional task affects the proper motion planning task solution. It can be observed that the final value of 0e(ϑ ) 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. 5.3 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 Table 5 The number of steps and the computation time of one step for each algorithm (Fig. 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 u0(t ). 6 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. Acknowledgements The author is deeply indebted to prof. Krzysztof Tchon´ for providing many constructive suggestions for improving the text and to anonymous reviewers whose remarks allowed him to improve this paper. This research was supported by the National Science Centre, Poland, under the Grant Decision No DEC-2013/09/B/ST7/02368. Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made. 1. Antonelli , G. : Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems . In: IEEE International Conference on Robotics and Automation , 2008 . ICRA 2008 , pp. 1993 - 1998 ( 2008 ) 2. Baillieul , J. : Kinematic programming alternatives for redundant manipulators . In: 1985 IEEE International Conference on Robotics and Automation. Proceedings , vol. 2 , pp. 722 - 728 ( 1985 ) 3. Ben-Israel , A. , Greville , T.N. : Generalized inverses: theory and applications CMS books in mathematics . Springer, New York ( 2003 ) 4. Chang , P. : A closed-form solution for inverse kinematics of robot manipulators with redundancy . IEEE J. Robot. Autom . 3 ( 5 ), 393 - 403 ( 1987 ) 5. Chiaverini , S. : Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators . IEEE Trans. Robot. Autom . 13 ( 3 ), 398 - 410 ( 1997 ) 6. Chiaverini , S. , Oriolo , G. , Walker , I.D. : Springer handbook of robotics, chap kinematically redundant manipulators . Springer, Berlin ( 2008 ) 7. Elmokadem , T. , Zribi , M. , Youcef-Toumi , K. : Trajectory tracking sliding mode control of underactuated AUVs . Nonlinear Dyn . 84 ( 2 ), 1079 - 1091 ( 2016 ) 8. Fantoni , I., Lozano , R. : Non-linear control for underactuated mechanical systems . Springer, London ( 2002 ) 9. Janiak , M. , Tchon´ , K. : Towards constrained motion planning of mobile manipulators . In: 2010 IEEE International Conference on Robotics and Automation (ICRA) , pp. 4990 - 4995 ( 2010 ) 10. Khaled , N. , Chalhoub , N.G. : A self-tuning guidance and control system for marine surface vessels . Nonlinear Dyn . 73 ( 1 ), 897 - 906 ( 2013 ) 11. Nakamura , Y. , Hanafusa , H. , Yoshikawa , T.: Task-priority based redundancy control of robot manipulators . Int. J. Robot. Res . 6 ( 2 ), 3 - 15 ( 1987 ) 12. Pettersen , K.Y. , Egeland , O. : Exponential stabilization of an underactuated surface vessel . In: 35th IEEE Conference on Decision and Control in Proceedings. Kobe, Japan , pp. 967 - 971 ( 1996 ) 13. Ratajczak , A. , Karpin´ska, J. , Tchon´ , K. : Task-priority motion planning of underactuated systems: an endogenous configuration space approach . Robotica 28 , 885 - 892 ( 2010 ) 14. Ratajczak , A. , Tchon´ , K. : Multiple-task motion planning of non-holonomic systems with dynamics . Mech. Sci . 4 ( 1 ), 153 - 166 ( 2013 ) 15. Ratajczak , A. , Tchon´ , K. : Parametric and non-parametric Jacobian motion planning for non-holonomic robotic systems . J. Intell. Robot. Syst . 77 ( 3 ), 445 - 456 ( 2015 ) 16. Sciavicco , L. , Siciliano , B. : A solution algorithm to the inverse kinematic problem for redundant manipulators . IEEE J. Robot. Autom . 4 ( 4 ), 403 - 410 ( 1988 ) 17. Sontag , E.D. : Mathematical control theory: deterministic finite dimensional systems, 2nd edn . Springer, New York ( 1998 ) 18. Sussmann , H.J. : A continuation method for nonholonomic path-finding problems . In: Proceedings of the 32nd IEEE Conference on Decision and Control , 1993 , vol. 3 , pp. 2718 - 2723 ( 1993 ) 19. Tchon´ , K. , Jakubiak , J. : Endogenous configuration space approach to mobile manipulators: a derivation and performance assessment of Jacobian inverse kinematics algorithms . Int. J. Control 76 ( 14 ), 1387 - 1419 ( 2003 ) 20. Tchon´ , K. , Karpin´ska , J. , Janiak , M. : Approximation of Jacobian inverse kinematics algorithms . Int. J. Appl. Math. Comput. Sci . 19 ( 4 ), 519 - 531 ( 2009 ) 21. Zadarnowska , K. : Switched modeling and task-priority motion planning of wheeled mobile robots subject to slipping . J. Intell. Robot. Syst. ( 2016 ). doi:10.1007/ s10846- 016 - 0397 -1


This is a preview of a remote PDF: https://link.springer.com/content/pdf/10.1007%2Fs11071-017-3342-3.pdf

Egalitarian versus prioritarian approach in multiple task motion planning for nonholonomic systems, Nonlinear Dynamics, 2017, DOI: 10.1007/s11071-017-3342-3