CC BY
0J Intell Robot Syst
DOI 10.1007/s10846-016-0390-8
CrossMark
Point-to-Point Collision-Free Trajectory Planning for Mobile Manipulators
Grzegorz Pajak • Iwona Pajak
Received: 14 December 2015 / Accepted: 24 May 2016
© The Author(s) 2016. This article is published with open access at Springerlink.com
Abstract The collision-free trajectory planning method subject to control constraints for mobile manipulators is presented. The robot task is to move from the current configuration to a given final position in the workspace. The motions are planned in order to maximise an instantaneous manipulability measure to avoid manipulator singularities. Inequality constraints on state variables i.e. collision avoidance conditions and mechanical constraints are taken into consideration. The collision avoidance is accomplished by local perturbation of the mobile manipulator motion in the obstacles neighbourhood. The fulfilment of mechanical constraints is ensured by using a penalty function approach. The proposed method guarantees satisfying control limitations resulting from capabilities of robot actuators by applying the trajectory scaling approach. Nonholonomic constraints in a Pfaffian form are explicitly incorporated into the control algorithm. A computer example involving a mobile manipulator consisting of nonholonomic platform (2,0) class and 3DOF RPR type holonomic manipulator operating in a three-dimensional task space is also presented.
Keywords Mobile manipulator • Point-to-point task • Trajectory planning • Control constraints
G. Pajak (E) • I. Pajak
Faculty of Mechanical Engineering, University of Zielona Gora, 65-246 Zielona Gora, Poland e-mail: g.pajak@iizp.uz.zgora.pl
1 Introduction
A mobile manipulator is a robotic system consisting of a mobile platform and a manipulator mounted on top. Each of these subsystems has its advantages and disadvantages. Manipulators are characterized by high accuracy which allows them to perform precise tasks, but their operational space is relatively small. Mobile platforms can accomplish tasks in large workspaces, but their movements are subject to nonholonomic constraints and they have the lower positioning accuracy. The mobile manipulator combining the mobility of the platform and the dexterity of the manipulator can replace several stationary manipulators moving between multiple production workstations. In this application the main task of the mobile manipulator is to place the end-effector in a specified point which will enable it to perform a task on a given workstation. In this case, the trajectory of the end-effector is not significant. It is important to achieve a particular point in the workspace avoiding possible collisions. Moreover, the configuration obtained by a holonomic manipulator after reaching the end-effector position is also significant. Achieving the configuration with a high manipulability measure will allow to perform a task on a given workstation without the necessity of the reconfiguration. Additionally, such an approach results in minimising platform movements which are undesirable during the task execution since it leads to increase in the end-effector tracking error [1-3].
Published online: 16 June 2016
Ö Springer
Combining the mobility of the platform with the dexterous capability of the manipulator makes the mobile manipulator gains the kinematic redundancy. The redundant degrees of freedom render it possible to accomplish complex tasks in complicated workspaces with obstacles, but equally the redundancy causes the solution of the mobile manipulator task to be difficult because of its ambiguity. In the literature, different approaches to solving such problems have been developed. Some of the existing researches address the problem by using only kinematics. In [4] Seraji has proposed an approach which treats nonholonomic constraints of the mobile platform and the kinematic redundancy of the manipulator arm in a unified manner to obtain the desired end-effector motion. Bayle et al. in the works [5, 6] have proposed an algorithm based on a pseudo-inverse scheme taking into account a geometric constraint on the end-effector motion. Secondary tasks have been used for resolving the redundancy of the mobile manipulator. A solution to the inverse kinematic problem for a mobile manipulator by applying an endogenous configuration space have been presented in [7] by Tchon and Jakubiak. This approach has defined an endogenous configuration that drives the end-effector to a desirable position and orientation in the task space. In the work [8] Papadopoulus et al. have used polynomial functions to obtain collision-free paths between two given configurations. Both stationary and non-stationary obstacles moving along known trajectories have been considered. A solution at the kinematic level to the inverse kinematic problem to solve the point-to-point problem in a workspace with obstacles has been formulated by Galicki in [9]. Fruchard et al. in the work [10] have presented a kinematic control method based on the transverse function approach. The realisation of the manipulation task has been set as the prime objective. A secondary cost function has been used for obtaining motion of the platform which enables the performance of the primary task.
In the above works, solutions at the kinematic level to the motion planning problem for a mobile manipulator have been given. In this approach it is possible to obtain a correct task execution if the mobile manipulator moves at low speed. At high velocities neglecting the dynamic capabilities of the system may lead to the solutions which can be impossible to use in real cases. To overcome this disadvantage, the dynamics of the
mobile manipulator should be taken into account. The dynamic interactions between the manipulator and the mobile platform during tracking the end-effector trajectory have been considered by Yamamoto and Yun in [11]. To solve the point-to-point problem Desai and Kumar [12] have presented a method based on the calculus of variations. This approach requires knowledge of the final mobile manipulator configuration and the shapes of obstacles. Chung et al. in [13] have proposed the kinematic redundancy resolution scheme decomposing the mobile manipulator into two subsystems: the mobile platform and the manipulator. They have designed two independent controllers for these subsystems based on their dynamic characteristics. Nevertheless, the trajectory found in this approach is not optimal in any sense. Coordinated motion planning considering both the stability and manipulability has been discussed in [14] by Huangh et al. The authors have formulated the vehicle motion problem taking into account dynamics, workspace, and system stability for mobile manipulators equipped with relatively small size platform. Egerstedt and Hu in the paper [15] have proposed error-feedback control algorithms in which the trajectory for the mobile platform is planned in such a way that the end-effector trajectory is reachable for the manipulator arm. Mohri et al. [16] have presented the sub-optimal trajectory planning method. The task has been formulated as an optimal control problem and solved by using an iterative algorithm based on the gradient function synthesized in a hierarchical manner considering the order of priority. In [17] Tan et al. have proposed integrated task planning and a control approach for manipulating a nonholonomic cart by a mobile robot consisting of a holonomic platform and on-board manipulator. The kinematic redundancy and dynamic properties of the platform and the manipulator arm have been considered. The manipulator dexterity has been preserved. Mazur and Szakiel [18] have presented the solution to the path following control problem decomposed into two separated tasks defined for the end-effector of the manipulator and the nonholonomic platform. This solution can be applied to mobile manipulators with fully known dynamics. In the work [19] Galicki has presented the solution at the torque/force level. The class of controllers, fulfilling state equality and inequality constraints and generating a collision free mobile manipulator trajectory with instantaneous minimal energy, has been proposed. Nevertheless, in these
solutions control constraints have not been taken into consideration.
This paper presents a sub-optimal point-to-point trajectory planning method for mobile manipulators operating in the workspace including obstacles. The proposed approach can be used for a mobile manipulator consisting of a holonomic manipulator and any class of a nondegenerated nonholonomic platform with the configuration of the motorisation ensuring a full robot mobility. The robot's trajectory is planned in a manner to maximise the manipulability measure to avoid manipulator singularities. In addition, the constraints imposed on mechanical limits and mobile manipulator controls are taken into account. Boundary conditions resulting from the task to be performed are also considered. In the proposed solution, the motion planning problem is transformed into an optimization problem with holonomic and nonholonomic equality constrains, and inequality constraints resulting from mechanical limitations. Collision avoidance is accomplished by perturbing the manipulator motion close to obstacles. The resulting trajectory is scaled to fulfil the constraints imposed on the controls. To the best of the authors' knowledge, no research takes into account control constraints solving the problem formulated in the above manner. Contrary to similar approaches the presented method incorporates explicitly nonholonomic constraints in a Pfaffian form to the control algorithm, so it does not require transformation to a driftless control system (which is not unique). The method guarantees to stop the platform and consequently enables the accomplishment of precise end-effector motions on the desired workstation. Lyapunov stability theory is used to prove the effectiveness of the proposed collision avoidance method as well as the convergence of the found solution.
The paper is organised as follows. Section 2 formulates the point-to-point trajectory planning problem. Section 3 contains the solution of the mobile manipulator task without collision avoidance conditions and control constraints. Then, this section presents the use of acceleration perturbation to plan a collisionfree motion and finally, the application of a trajectory scaling method to fulfil control constraints. Section 4 shows computer simulations for a mobile manipulator consisting of a nonholonomic (2,0) class platform and a 3DOF RPR type holonomic manipulator operating in a three-dimensional workspace. Additionally, a numerical comparison of the method presented in
this paper to the algorithm described in [9] is carried out in this section. The conclusions are formulated in Section 5.
2 Problem Formulation
The robotic task is to move the end-effector in the m--dimensional task space from the initial point resulting from a given robot configuration to the final point Pf e in such a way as to avoid manipulator singularities. The robot motion has to take into account mechanical limits, boundary constraints coming of the task and collision-free conditions. An exemplary robot considered in the Numerical example section and its workspace is shown in Fig. 1.
A mobile manipulator composed of the holonomic manipulator with kinematic pairs of the 5th class and a nondegenerated nonholonomic platform with the configuration of the motorisation ensuring full robot mobility [20] is considered to plan a trajectory. It is described by the vector of generalised coordinates:
q = (qp qr f , (1)
where q e is the vector of the generalised coordinates of the whole mobile manipulator, qp e is the vector of the coordinates of the nonholonomic platform, qr e №r is the vector of joints coordinates of the holonomic manipulator, p is the number of coordinates describing the nonholonomic platform, r is the number of kinematic pairs of the holonomic manipulator, and n = p + r.
At the initial moment of motion the manipulator is (by assumption) in the collision-free configuration q0 with zero velocity:
q (0) = qo, q (0) = 0. (2)
At the final moment of motion T , the end-effector of the manipulator has to reach the final point with zero velocity:
Pf = f (q (T)), q (T) = o, (3)
where function f : ^ denotes m -dimensional mapping, which describes the position and orientation of the end-effector in the workspace.
The platform motion is subject to nonholonomic constraints which can be described in the Pfaffian
Fig. 1 An exemplary task of the mobile manipulator
A (qp) qp = 0,
where A (qp) is (k x p) Pfaffian full rank matrix and k is the number of independent nonholonomic constraints.
The conditions resulting from the mechanical limits and constraints connected with the obstacles existing in the workspace for the manipulator configuration q can be written as a set of inequalities:
Vi e [0, T] {d (q (t)) > 0} , i = 1,..., L1,
Vi e [0,T] jcl1 (q (t)) > 0} ,
i = 1,...,L
where cj (■ ) is a scalar function which involves the fulfilment of the constraints imposed by the robot mechanical limits, Lj denotes a total number of mechanical constraints, cj1 (■ ) is a scalar function which describes collision-free conditions of the manipulator with the obstacles and L11 stands for a total number of collision avoidance conditions.
Additionally, the constraints of control resulting from the physical abilities of the actuators are also assumed:
< u < u„
лшт < " <
where u is (n — k) -dimensional vector of controls (torques/forces) and um!n, umax are (n — k)-dimensional vectors denoting lower and upper limits on u, respectively.
In order to determine controls, it is necessary to know the mobile robot dynamic equations, given in a general form, as:
M (q) q + F (q, q) + ATX = Bu,
where M (q) is (n x n) positive definite inertia matrix, F (q, q) is n-dimensional vector representing Corio-lis, centrifugal, viscous, Coulomb friction and gravity forces, A = ^A (qp) 0kxr j, 0kxr denotes (k x r) zero matrix, X is k-dimensional vector of the Lagrange multipliers corresponding to nonholonomic constraints (4) and B is (n x (n — k)) full rank matrix (by definition) describing which state variables of the mobile manipulator are directly driven by the actuators.
In practice, the configuration of mobile manipulators joints should be far away from singular configurations. For this purpose, the instantaneous performance index is minimised (maximising the manipulability measure [21]):
I (q) = —det
where j = df (q) /dq is (m x n)-dimensional Jaco-bian matrix of the manipulator.
The solution of the problem formulated above is the trajectory of the mobile manipulator q (t) that satisfies constraints (2)-(7) and reaches the minimum value of the performance index (9) in each time instant.
3 Solution
The solution of the problem defined by dependencies (2)-(9) uses a penalty function approach and a redundancy resolution at the acceleration level. It is based on the trajectory planning methods for mobile manipulators tracking an end-effector path described in [22, 23].
3.1 Point-to-Point Trajectory Planning
First, the trajectory planning task without collision avoidance conditions (6) is considered. The approach is to use penalty functions which cause the inequality constraints (5) to be satisfied. In this case the instantaneous performance index (9) can be expressed as:
I(q) = 7(q) + J2 "I (V (q))>
where kI (■) is the penalty function which associates a penalty with a violation of a constraint.
In order to find mobile manipulator motion minimising the performance index (10), first let us consider the problem of finding an optimal configuration at the final pose Pf. For this purpose, the task of searching minimum of the performance index (10) with constraints (3) and (4) should to be solved. Following the method presented in the work [24] for stationary manipulators, the necessary conditions for minimum of function (10) with equality constraints (3) and (4) have been derived for a mobile manipulator in [23] and they take the following form:
JR (q)"
- 1n-m-k I 'q (q) = 0,
where J (q) = ^ jT (q) AT (q) } is ((m + k) x n)-
dimensional nonsingular matrix, J* (q) is ((m + k) x (m + k)) square matrix constructed from (m + k) linear independent columns of J (q), JF (q) is ((m + k) x (n — m — k)) matrix obtained by excluding J* (q) from J (q), (m + k) < n, 1 n—m—k denotes ((n — m — k) x (n — m — k)) identity matrix and Iq (q) = dl/dq is n-dimensional vector.
The equation (11), called the transversality conditions, introduces (n — m — k) dependencies which in combination with the conditions (3) and (4) allow
finding an optimal configuration for a given final point Pf. As it has been shown in [23], the full rank of the matrix J^ depends on the rank of the Jacobian matrix of a holonomic manipulator, a full rank of this matrix is a consequence of maximising the manipulability measure (9). The results of simulations presented in Section 4 show that the application of the proposed method leads to an increase of the holonomic manip-ulability measure during the task accomplishment.
In order to find a point-to-point trajectory of the mobile manipulator, based on the solution presented in [23] and using the transversality conditions (11), the mapping E (q, q) is introduced:
E(q, q) =
f(q) - Pf JR(q)-1 JF(q))r - 1, A (q) q
— m. — tf
The dependency (12) may be interpreted as a measure of error between a current configuration q (t) and unknown final configuration q (T). The m-first components of E are responsible for reaching the given final point Pf, the next (n — m — k) dependencies are responsible for the fulfilment of constraints (5) as well as for maximising manipulability measure (9) and the k-last items are responsible for the fulfilment of constraints (4). For simplicity of further calculations the following notations are introduced:
E' (q) =
f(q) - Pf
((JR(q)-1 JF(q))r - 1n-m-k)'q(q)
EII(q, q) = A (q) q,
so the dependency (13) can be rewritten in a simplified form:
E (q, q) =
E' (q) E'' (q, q)
To find the trajectory of the mobile manipulator q (t) from the initial configuration qo to the final point Pf the following dependencies are proposed:
a've ' + a'le' E '' + al'e''
where aL and A'
L are ((n — k) x (n — k)) diagonal matrices with positive coefficients AV AL i ensuring the stability of the first equation, AIL is (k x k)
diagonal matrix with positive coefficients A!L!i ensuring the stability of the second equation.
The proposed form of dependency (14) results from the necessity to determine a trajectory at the acceleration level. Hence, the second order differential equation with respect to q is needed (E1, E11 are dependent on q and q , respectively). Using Lyapunov stability theory it is possible to show the above form of the system of differential equations ensures that its solution is asymptotically stable for positive coefficients A!v i, AL i and A^i. The lower equation (14) is trivial asymptotically stable for AILIi > 0. In order to show the stability of the upper equation (14), let us consider Lyapunov function candidate of the following form:
v (ei,E 1 ) = 2 (E I,E1 ) + 2 (ei,aLei ) (15)
It is easy to see that V (E1, E^ is non-negative for positive coefficients AL i. After differentiating and substituting E1 determined from (14), V can be written as:
V (ei,ÈL') = -(EI,AIViL')
As it is seen, for positive coefficients AV i, V (EI,iL1 ) < 0. It can be shown that V (E1, E1} = 0 for E1 = 0 . Additionally, if E1 = 0 then E1 = 0 and from the upper equation (14) it follows that E1 = 0. Based on La Salle's invariance principle it can be concluded that the solution of the system of differential equations (14) is asymptotically stable for positive coefficients AV i, AL i and A^i. This property implies the fulfilment of conditions (3) associated with the final moment of the task performance, e.g. f (q (T)) ^ Pf and q (T) ^ 0 as T ^ œ.
Moreover, if AV i > AL i, the solution is also a strictly monotonic function, so for the initial nonsingular configuration q0, fulfilling initial conditions (2) and mechanical constraint (5) robotic motion is free of singularities, fulfils constraints (4) and (5) during the movement to the final point P f.
The equation (14) is a system of homogeneous differential equations with constant coefficients. In order to solve it and find the trajectory of the mobile manipulator (2n — k) consistent dependencies should be given. These dependencies are obtained from E for
t = 0 taking into account conditions (2), especially zero initial velocity:
E/=0 = (E0,1 • • • E0,n-k) , Et=0 = 0 ,
= 0, El=0 = 0^
Finally, the trajectory of the mobile manipulator can be determined by simple transformations from the equation (14) as:
q (t) = -
(dEl/dt) q + aVeI q + aLe1
ElIq + AILIEI1
where El = dÈ11 ■
dE1 /dq, El1 = dE
/dq, Ell =
'/d q.
3.2 Collision-Free Trajectory Planning
The trajectory of the mobile manipulator (18) depends on the choice of the gain coefficients A!V, A!L and A^1. It can be shown that m-first elements A!V ; and A!L ; determine the end-effector motion. Since the motion of the end-effector proceeds along the curve determined by gain coefficients the mobile manipulator cannot avoid the obstacle if it is located on this curve. For this reason, in the case of trajectory planning with collision avoidance conditions it is not possible to apply for mechanical constraints the same approach as presented in Section 3.1. Therefore, in this paper collision avoidance is accomplished by perturbing the manipulator motion close to obstacles [25].
For the proposed method, scalar functions cj1 (■), which describe collision-free conditions (6), should specify distances between a manipulator and obstacles. In general, checking whether two objects do not collide is a common equivalent to a non-linear programming problem. But this approach cannot be used for on-line trajectory planning due to the large computational burden. In the presented paper the method of obstacles enlargement with a simultaneous discretization of the mobile manipulator has been proposed [26]. In this method it is assumed that the platform and manipulator links are described by parametrised two-dimensional surfaces. Moreover, the surface of each obstacle S;- is defined by smooth function Sj (P), as follows:
S; = {P : Sj(P) < 0},
where Sj defines geometry of j-th obstacle, P is a point belonging to the obstacle and Sj (■) denotes a smooth function describing the obstacle surface.
In order to obtain a finite number of constraints describing collision avoidance conditions, each obstacle is enlarged by certain positive value 5. For the assumed value 5 it is possible to determine the discretization of the surface describing the platform and manipulator links to ensure collision avoidance. In this way, the mobile manipulator is reduced to a discrete set of points, so the collision test leads to checking a finite number of inequalities (6) and scalar functions cI1 (■) can be expressed as:
c'' (q) = Sj (P{) - S > 0,
where P; denotes l-th point from a discrete set of the points approximating the mobile manipulator.
The proposed method allows to take into account solids obtained by rotation of two-dimensional figures with smooth borders around certain fixed axes. In this way, it is also possible to obtain non-convex solids with smooth surfaces. Therefore, the presented approach allows to take into account non-convex obstacles.
For collision-free trajectory planning, each obstacle is surrounded by a safety zone in which the acceleration of the mobile manipulator is disturbed by continuous perturbation pushing the robot out of this zone:
qo(0 = -£(bk!'/Bq) - (£«!') q,
. i = 1
where qo (t) stands for perturbation of the accelerations in the obstacles neighbourhood, k?1 is assumed to be a non-negative continuous scalar penalty function, equals 0 outside the neighbourhood of the i-th obstacle, increasing if the manipulator approaches the obstacle. An exemplary form of penalty function k?1, used in the numerical example presented in Section 4., is expressed as:
(c" (q)) = f p {c" (q) - 2 for c'' (q) < ef
V i / 0 otherwise
where p denotes the constant positive coefficient determining the strength of penalty and ei is the constant positive coefficient determining the threshold value which activates the i-th constraint.
It is worth noting that the magnitude of perturbation described by dependency (21) is small when the mobile manipulator enters a safety zone and increases as it approaches the obstacle. Moreover, the magnitude of perturbation can be tuned by a suitable choice of coefficient p in penalty function (22). Additionally, the first component of the dependency (21) is responsible for pushing the robot away from the obstacles. The second one reduces the velocity of the mobile manipulator in the obstacles neighbourhood.
Using Lyapunov stability theory it is possible to show that perturbation (21) forces the mobile manipulator to escape from the obstacles neighbourhood. For this purpose, let us consider the following local Lya-punov function candidate (acting only in the obstacles neighbourhood):
V!(q) = 2 <q, q> + Y,«'',
where (■} is the scalar product of vectors. The time derivative of this function can be calculated as:
V (q) = (q, q} + ^ (K/dq) , qj . (24)
Inserting right-hand side from (21) into (24), the following formula is obtained after simple calculation:
vi(q) = - IE'
<q, q> •
From (25) it is seen that V (q) < 0. The derivative of V; (q) equals 0 either for k? 1 = 0or (j = 0. Because in the obstacles neighbourhood (Sk?? /dq) = 0, nonzero perturbation tjo acts on the mobile manipulator, so Vi (q) = 0 only for k?1 = 0. From the definition of penalty function k j1 it follows that k j1 = 0 outside of the neighbourhood of the i-th obstacle, so perturbation of the acceleration (21) forces the manipulator to escape from the obstacles neighbourhood.
Finally, the trajectory q (t) that satisfies boundary conditions (2)-(3), nonholonomic constraints (4), mechanical and collision avoidance constraints (5)-(6) and minimises the performance index (9), can be
obtained from (18) extended by perturbation (21) as follows:
Introducing (n x (n — k))-dimensional full rank matrix:
q <" =— I %
q+ A'yEl q + A^E1
EIJq + A'j'E11
+ (i„ — a#A) q0,
where A# = AT (AA7) 1 is the pseudoinverse of matrix A, matrix (1„ — A#A) is chosen to fulfil nonholonomic constraints (4) and q0 is a vector of accelerations perturbations (21).
Let us note that the presented method has a local character so it is possible to stuck in the local minima or saddle points. This disadvantage can be overcome by applying small perturbation at the acceleration level if the mobile manipulator stops before reaching the final point. As can be seen in [27] such an approach ensures leaving a saddle point. If the use of perturbation does not provide the expected results, the mobile manipulator is stuck in the local minima. In this case, a global method has to be applied to escape from this point. An example of such a method using a real-time version of the algorithm based on A* and hill-climbing for collision-free trajectory planning of redundant manipulators is presented in [28]. The detailed discussion of these issues is beyond the scope of this work.
3.3 Control Constraints
In order to ensure fulfilment of constraints (7) an idea of trajectory scaling is used. As shown below, it is possible to choose gain coefficients AjV and AjL to scale the trajectory of the mobile robot (26) in such a way as to fulfil control constraints (7). To determine values of controls which are required to realise the trajectory it is necessary to express the model of dynamics (8) using auxiliary velocities for the mobile platform. For this purpose the nonholonomic constraints are expressed by an analytic driftless dynamic system:
qp = N (qp) v,
where N (qp) is (p x (p — k))-dimensional matrix satisfying the relationship A (qp) N (qp) = 0 and v is (p — k)-dimensional vector denotes scaled angular auxiliary velocities of the platform driving wheels, presented approach is not dependent on the selection of the vector v.
N (q) =
N (qp) 0pxr
0rx(p—k) lrxr
and multiplying the dynamic equation (8) by NT (q) the vector of Lagrange multipliers X is eliminated and finally the new form of dynamic model is obtained:
NT (q) M (q) q + NT (q) F (q, q) = NT (q) Bu. (29)
To determine controls u from (29) the assumption of nonsingularity of the matrix NT (q) B is needed. Writing the matrix B in the form of:
]rx(p—k) lrxr
where B is (p x (p — k)) matrix describing which state variables of the mobile platform are directly driven by actuators, NTB can be determined as:
NT (q) B =
NT (qp) B 0{p—k)x
0r x(p—k) lr xr
NTB is full rank matrix if NTB if full rank matrix. As shown in the work [20], any nondegenerate non-holonomic mobile platform belonging to one of four classes: (2,0), (2,1), (1,1), (1,2) (fifth class (3,0) refers to an omnidirectional holonomic platform). According to the analysis carried out in the work cited above, it is possible to choose the configuration of the motorisation which provides full platform mobility and ensures a full rank of the matrix NT B. For the mobile platform of (2, 0) class, considered in Numerical example section, matrices N and B take the following form:
cos (ff) sin (ff) 1/a 2/r 0
cos (0) sin (0) —1/a 0 2/r
03x2 "
where a is a one half of the distance between platform wheels and r denotes the wheel radius. Hence, NTB is a nonsingular diagonal matrix and NTB is nonsingular too.
In order to find controls fulfilling constraints resulting from limitations of mobile robot actuators, u is
determined from (29) and substituted into conditions (7), so the following system of inequalities is obtained:
Umin < (nT (q) B —1 NT (q) M (q) q
+ (NT (q) B — 1 NT (q) F (q, q) < Umax. (30) Introducing the following substitutions:
a (q, q) = -N#M
E' EEq'q'' Eq
diag (Eq q) , diag (E1
0kx2(n-k)
b (q, q) = -N#M
E' Eq E''
(dE'/dt) q E''q + A'L'E''
+N#M (ln - A# A qo + N#F,
(NT B 1 NT, (A'v1••
L, 1 • •
system of inequalities (30) can be rewritten as: umin < a (q, q)A + b (q, q) < umax. (31)
As is seen, this system is linear with respect to A. The dependency (31) introduces 2 (n — k) inequalities, whereas dim (A) = 2 (n — k), hence, assuming the full rank of the matrix a (q, P) it is possible to determine 2 (n — k) gain coefficients A ensuring fulfilment of constraints (7) at each time instant.
Finally, the solution of equation (26) with suitable parameters A gives an suboptimal trajectory satisfying inequality constraints on state variables (5) and
(6), control constraints (7) and boundary conditions (2) and (3).
4 Numerical Example
In the numerical example a mobile manipulator, shown in Fig. 2, consisting of a nonholonomic platform of (2,0) class and a 3DOF RPR type holo-nomic manipulator working in a three-dimensional task space is considered.
The mobile manipulator is described by the vector of generalised coordinates:
q = (xc, yc, 6,01,02, q1, q2, q3)T,
where (xc, yc) denotes the platform centre location and 6 is the platform orientation, 01, 02 are angles of driving wheels, qi, q2, q3 are angles and offset of the manipulator joints.
The platform works in XBYB plane of the base coordinate system. The coordinate system OpXpYp Zp is attached to the mobile platform at the midpoint of the line segment connecting the two driving-wheels. The holonomic manipulator is connected to the platform at the point (xr,yr, 0)T of OPXPYPZP system. The kinematic equation of mobile manipulator is given as:
f(q) =
cg (kcc +/2c1 +Xr)-sg (I3S1 c3 +I2S1 +yr)+Xc se (hc1c3+hc1 +Xr)+cg (hs1c3+/2S1 +yr)+yc
q2 -I3S3
where c6 = cos (6), c1 = cos (q1), c3 = cos (q3), s6 = sin (6), s1 = sin (q1), s3 = sin (q3), l2 and l3 are the lengths of the second and the last arm of manipulator.
Fig. 2 Kinematic scheme of the mobile manipulator
The motion of the platform is subject to one holo-nomic and two nonholonomic constraints, describing roll without lateral slipping [29], so constraints (4) in this case take the following form:
' & - 2a<Pi + ¿Д "
Xc - 2сеф1 - 2Св<ф2
Ус - 2se<p1 - 2se(p2
where r is the radius of driving wheels, and a is half-distance between the wheels.
The kinematic parameters of the mobile manipulator are given as (all physical values are expressed in the SI system):
l2 = 0.3, l3 = 0.2, a = 0.2, r = 0.075, xr = 0.2, yr = 0.
The masses of the mobile manipulator's elements amount to:
mp = 94, m2 = 20, тз = 20,
where mp is the total mass of the platform, m2, m3 are the masses of the manipulators arm.
At the initial moment of motion the mobile manipulator is in the nonsingular, the collision-free configuration:
q0 = (0, 0, n/2, 0, 0, 0, 0.5, 0.3)T ,
which corresponds to the initial end-effector position:
P0 = (0,0.69, 0.44)T.
The task of the robot is to move the end-effector to the final point:
Pf = (3.5,2.0,1.0)T.
To preserve mechanical constraints generalised coordinates of holonomic manipulator should not exceed limits:
qrmin = (-П, 0.5, -n/2)T , qmax = (n, 2.0, 3n/2)T .
It is also assumed that limitations on controls are equal to:
umin = (-25, -25, -30, 0, 0)T , Umax = (25,25, 30,450, 20)T.
There are six obstacles in the workspace. Four are approximated by cylinders with radius equal to 0.25, the other two obstacles are represented by spheres with radius 0.25. The centre points of the obstacles are placed at (0.7, 0.4, 0)T, (1.0,1.65, 0.0)T, (2.25,1.0, 0.0)T, (2.1,1.95, 0.0)T, (0.5,1.3, 0.7)T
and (2.7,1.9, 0.8)T, respectively. Each obstacle is surrounded by a safety zone with a radius 0.5. The height of the cylinders is set to 0.2, so only the platform can collide with them. On the other hand, the holonomic manipulator can collide with the spheres placed at a height 0.7 and 0.8, respectively. In order to obtain collision avoidance constraints (20) coefficient S has been taken as 0.005, so the discretization of the surfaces of the platform and manipulator links has been determined as 0.1. The workspace of the mobile manipulator is presented in Fig. 1.
Three cases of performing above task are considered. The first one is collision-free motion when control constraints are not taken into account. In the second case mobile manipulator operates in the same workspace and control constraints are considered. The last experiment shows how the method performs in a "real case" - Coulomb and viscous friction as well as noise are included. Finally, a numerical comparison of method presented in this paper to an algorithm described in [9] is also carried out in this section.
In the first simulation gain coefficients AjV and AjL have been determined based on the inequality (31) in such a way as to control constraints have not been fulfilled. The coefficients AL do not affect the values of controls and in all simulations have been taken as the identity matrix.
A'V = diag (3.6, 3.6, 3.6, 3.6, 3.6),
A[ = diag (3, 3, 3, 3, 3), , и
A'L = diag (1,1,1).
In this case the final time of task execution T is obtained as 23.97 [s]. Figure 3a displays the minimum distance between the mobile manipulator and all obstacles in each time instant. The distance to a single obstacle is defined as the minimum norm of the difference between the centre of this obstacle and all points approximating the platform and manipulator links. The dashed and dotted grey lines represent the radius of the obstacles and their neighbourhood, respectively. The mobile manipulator, except for a short time at the beginning and end of the motion, remains in the safety zones of the obstacles but it does not collide with any of them. Figure 3b shows the potential collisions when collision avoidance conditions are not taken into account. At the beginning of the motion (t e [2.5,13.5]), the mobile manipulator collides successively with the first three obstacles:
„ 0.5
10 15 20 25 0 5 10 15
t [s] t [s]
(a) collision avoidance conditions are taken into ac- (b) collision avoidance conditions are not taken into count account
Fig. 3 Distances between the mobile manipulator and the centres of the obstacles
the platform collides with the first and second cylinder and the holonomic manipulator collides with the first sphere. In the final phase of the motion, the robot enters the cluttered environment. For t e [18.5, 22.0], the platform collides with the third and fourth cylinder and the manipulator collides with second sphere.
The mobile manipulator controls obtained in the first simulation are shown in Fig. 4. The controls are continuous functions of the time but they exceed the assumed constraints for gain coefficients given above. At the beginning of the motion, control connected with the left wheel of the platform exceeds both upper and lower limits. Similarly, controls connected with the first and second joint of the holonomic manipulator
exceed the lower and upper constraint, respectively. Furthermore, during the task accomplishment control connected with the prismatic joint reaches a value higher than the upper limit. It results from the necessity of raising the arm to avoid collision with the second spherical obstacle.
The second simulation presents the solution of the same task as the first one, but control constraints (7) are considered. As shown in Section 3.3 controls are linearly dependent on gain coefficients A!V, A!L and the lower values of gain coefficients lead to lower values of controls. In order to determine values of AV, aL the system of inequalities (31) is solved. It gives maximum allowed values AV*;, A^ i for which
10 15 t [s]
(a) the wheels torques of the platform
10 15 t [s]
480 400 320
240 S,
160 80
Fig. 4 The controls of the mobile manipulator for the first simulation
constraints (7) are satisfied. As gain coefficients, min-
i * V, i'
Al*- are accepted, which
imum values from A ensures that control constraints are satisfied for all actuators. Finally, due to uncertainty in models, the found values are rounded down in order to leave free room for on-line feedback control. According to this approach, A!V, A!L for the second simulation have been chosen as:
A'v = diag (2.2, 2.2, 2.2,2.2,2.2), AL = diag (1.1,1.1,1.1,1.1 ,1.1).
For this solution, the final time T is increased and it equals 34.82 [s], but the determined controls do not exceed the assumed constraints as can be seen in Fig. 5. In this case the way of task execution is similar to the first simulation - the mobile manipulator penetrates the safety zones of each obstacles but does not collide with them.
From a practical point of view, a significant advantage of the presented method is to ensure a high holonomic manipulator dexterity. Kinematic equation fr (qr) of RPR type holonomic manipulator, which is part of mobile robot considered in this section, expressed in the Op Xp Yp Zp system is given as:
fr (qr) =
/3C1C3 + I2C1 + Xr I3S1C3 + I2S1 + yr
q2 - I3S3
The Jacobian matrix jr (qr) of this manipulator takes the following form:
-si (¡2 + ¡3C3) 0 -¡3C1S3 jr (qr) = ci (¡2 + ¡3C3) 0 -¡3S1S3 0 1 -¡3C3
so after simple calculations manipulability measure of holonomic part can be determined as:
wr (qr) = \hs3 (¡2 + ¡3C3)| •
As it is seen, in this case the manipulability measure depends on configuration angle q3 only. Taking into account kinematic parameters of the manipulator considered in this section, it is possible to show that wr reaches the maximum value wrmax = 0-0697 for q3 = i-1314.
The changes of the manipulability measure of the holonomic manipulator are presented in Fig. 6a. It reaches a relatively low value (less than half the maximum value) for initial configuration q0, but minimisation of the performance index (9) leads to a steady increase of its value. This way of the task accomplishment is an essential point of the proposed method, because it ensures that the holonomic part is far from singular configurations which guarantees the existence of matrix (J^ 1 in dependency (11). Moreover, at the final moment of the motion, the manipulability measure reaches a value close to the maximum wrmax, so the manipulator attains the high dexterity and it can perform the next task without the necessity of the reconfiguration.
? If V
—V -~U2
5 10 15 20 25 30 t [s]
(a) the wheels torques of the platform
f " ^ ' " V
U3-~ ~U4.......U5
480 400 320
240 3L 160 80
15 20 25 30 t [s]
Fig. 5 The controls of the mobile manipulator for the second simulation
(a) second simulation (b) third simulation
Fig. 6 The holonomic manipulability measure
Finally, Fig. 7 illustrates the collision-free robot motion taking into account mechanical and control constraints and maximising the manipulability measure. Additionally, at [30] the animation presenting the results of this simulation is available. Configuration angle q3, which determines the holonomic manipula-bility measure, increases during the task performance and reaches the value close to the maximum at the
final point. Furthermore, the animation shows reduction in speed of the robot in the obstacles neighbourhood as a consequence of the influence of the second term of the dependency (21).
The last simulation verifies the method in a "real case". The real conditions are simulated by adding to the mobile manipulator dynamic equation (8) an additional component D (q , t) including the viscous and
-60 -600
-Ui--u2
5 10 15 20 25 30 35 t [s]
(a) the wheels torques of the platform
75 50 25 0
-25 -50 -75.
U3-~-U4.......u5
480 400 320
240 2,
160 80
0 5 10 15 20 25 30 35 t [s]
(b) the joints torques and force of the manipulator
Fig. 8 The controls of the mobile manipulator for the third simulation
Coulomb friction as well as bounded time-dependent uncertainty term:
D (q, t) = ciq + C2Sign (q)
03x1 0.2 sin (t) 0.2 sin (t + n/6) 10.0 sin (t + n/3) 50.0 sin (t + n/4) 10.0 sin (t + n/2)
where c1, c2 are viscous and Coulomb friction coefficients, respectively. The changes of controls and holonomic manipulability measure obtained in the last simulation are shown in Figs. 8 and 6b. As can be seen,
+cos (250
the method is robust against disturbing the dynamic equations and the mobile manipulator accomplishes the task correctly.
Finally, in order to compare the results presented above to an algorithm described in [9], the additional simulation using the controller (32) has been carried out.
^ d q , ^ d q
where Vc = 2 <e, e) + A (q) + B (q), A (qr) =
H (wr (qr) - wrmax)2 for wr (qr) <
wmax and
-60 -600
— ui---u2
15 20 25 30 t [s]
(a) the wheels torques of the platform
Fig. 9 The controls of the mobile manipulator for the controller (32) £ Springer
75 50 25 0
-25 -50 -75
1 1 j. 111 1 '
-4 i
I 1 1 u I
u3-~-u4.......u5
600 500 400
300 2,
200 100
15 20 25 30 35 t [s]
u = —a
A (qr) = 0 otherwise, n denotes positive constant coefficient (strength of penalty), B (q) =
Ei=0 A1 (cF (q)), e = f(q) - P/,«e [0-5, i] is a
positive coefficient ensuring the stability of (32).
In this case, the mobile manipulator performs the same task as in previous simulations. In order to obtain the comparable results coefficient a has been chosen as 0.23. This value guarantees that the time of the task execution is comparable to the time obtained in the second simulation. The other coefficients have been selected as: \x = 10, a = 1.0. Figures 9 and i0 display controls and the holonomic manipulability measure obtained in this simulation. Although execution times are similar, the torques generated by control law (32) are clearly larger than those provided by the presented method. However, the method proposed in the work [9] does not allow to take into account control constraints, as shown by results of the second simulation, it is possible to accomplish the task within a given time in such a way that assumed constraints are fulfilled. The torques generated by control law (32) significantly exceed the limitations imposed on controls, what is particularly evident in the initial phase of the motion. Moreover, the torques obtained by using our method are much smoother and can be used directly to control a real robot. Additionally, as shown in Figs. 6 and 10, the holonomic manipulability measure obtained by using control law (32) increases much slower and does not reach the assumed value wmax which may be important for the performance of the next task of the mobile manipulator.
Fig. 10 The holonomic manipulability measure for the controller (32)
5 Conclusion
This paper presents a method of trajectory planning in the case of the mobile manipulator has to reach a specified end-effector position in the workspace including obstacles. Such a task is important in the case when a mobile manipulator operates on several workstations and has to move between them, so it has to move from a current configuration to a given end-effector position.
Presented approach guarantees fulfilment of mechanical constraints, collision avoidance conditions and it provides continuous, limited controls. Nonholonomic constraints in a Pfaffian form are explicitly incorporated to the control algorithm, so it does not require transformation to a driftless control system. Furthermore, minimisation of the proposed performance index leads to maximisation of the manipulability measure during the task execution and consequently, this measure reaches a high value in the final point of the motion. After the end of the movement, therefore, the mobile manipulator is in the configuration that allows the execution of a next task without the necessity of the reconfiguration.
The problem has been solved by using penalty functions and a redundancy resolution at the acceleration level. The collision avoidance is accomplished by local perturbation of the mobile manipulator motion in the obstacles neighbourhood pushing the robot out of these zones. The resulting trajectory has been scaled to fulfil the constraints imposed on the controls. The property of asymptotic stability of the proposed solution implies fulfilment of all the constraints imposed. The proposed approach to trajectory planning is computationally efficient and robust against disturbances. The effectiveness of the solution is confirmed by the results of computer simulations, future work will focus on experimental verification of the algorithm.
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.
References
1. Galicki, M.: Two-stage constraint control of mobile manipulators. Mech. Mach. Theory 54, 18-40 (2012)
2. Pajak, G.: End-effector vibrations reduction in trajectory tracking for mobile manipulator. J. Vibroeng 17(1), 101— 111 (2015)
3. Pajak, G., Pajak, I.: Mobile manipulators collision-free trajectory planning with regard to end-effector vibrations elimination. J. Vibroeng 17(6), 2896-2906 (2015)
4. Seraji, H.: A unified approach to motion control of mobile manipulators. Int. J. Robot. Res 17(2), 107-118 (1998)
5. Bayle, B., Fourquet, J.Y., Renaud, M.: A coordination strategy for mobile manipulation. In: Proc. 6th Int. Conf. Intell. Auton. Syst. (IAS-6), pp. 981-988. Venice (2000)
6. Bayle, B., Fourquet, J.Y., Renaud, M.: Manipulability of wheeled mobile manipulators: application to motion generation. Int. J. Robot. Res 22(7-8), 565-581 (2003)
7. 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)
8. Papadopoulos, E., Papadimitriou, I., Poulakakis, I.: Polynomial-based obstacle avoidance techniques for non-holonomic mobile manipulator systems. Int. J. Robot. Autonom. Syst. 51, 229-247 (2005)
9. Galicki, M.: Control-based solution to inverse kinematics for mobile manipulators using penalty functions. J. Intell. Robot. Syst 42(3), 213-238 (2005)
10. Fruchard, M., Morin, P., Samson, C.: A framework for the control of nonholonomic mobile manipulators. Int. J. Robot. Res 25(8), 745-780 (2006)
11. Yamamoto, Y., Yun, X.: Effect of the dynamic interaction on coordinated control of mobile manipulators. IEEE Trans. Robot. Autom 12(5), 816-824 (1996)
12. Desai, J., Kumar, V.: Nonholonomic motion planning for multiple mobile manipulators. Proc. IEEE Int. Conf. Robot. Autom 4, 3409-3414 (1997)
13. Chung, J., Velinsky, S., Hess, R.: Interaction control of a redundant mobile manipulator. Int. J. Robot. Res 17(12), 1302-1309 (1998)
14. Huang, Q., Tanie, K., Sugano, S.: Coordinated motion planning for a mobile manipulator considering stability and manipulation. Int. J. Robot. Res 19(8), 732-742 (2000)
15. Egerstedt, M., Hu, X.: Coordinated trajectory following for mobile manipulation. In: Proc. IEEE Int. Conf. Robot. Autom., vol. 4, pp. 3479-3484. San Francisco (2000)
16. Mohri, A., Furuno, S., Iwamura, M., Yamamoto, M.: Suboptimal trajectory planning of mobile manipulator. In: Proc. IEEE Int. Conf. Robot. Autom., pp. 1271-1276 (2001)
17. Tan, J., Xi, N., Wang, Y.: Integrated task planning and control for mobile manipulators. Int. J. Robot. Res. 22(5), 337354 (2003)
18. Mazur, A., Szakiel, D.: On path following control of non-holonomic mobile manipulators. Int. J. Appl. Math. Comp. 19(4), 561-574 (2009)
19. Galicki, M.: Collision-free control of mobile manipulators in a task space. Mech. Syst. Signal Pr. 25(7), 2766-2784 (2011)
20. Campion, G., Bastin, G., D'Andrea-Novel, B.: Structural properties and classification of kinematic and dynamic models of wheeled mobile robots. IEEE Trans. Robot. Autom 12, 47-62 (1996)
21. Yoshikawa, T.: Manipulability of robotic mechanisms. Int. J. Robot. Res 4(2), 3-9 (1985)
22. Pajak, G., Pajak, I.: Motion planning for mobile surgery assistant. Acta Bioeng. Biomech 16(2), 11-20 (2014)
23. Pajak, G., Pajak, I.: Sub-optimal trajectory planning for mobile manipulators. Robotica 33(6), 1181-1200 (2015)
24. Galicki, M.: Optimal planning of collision-free trajectory of redundant manipulators. Int. J. Robot. Res 11(6), 549-559 (1992)
25. Galicki, M.: Inverse kinematics solution to mobile manipulators. Int. J. Robot. Res 22(12), 1041-1064 (2003)
26. Galicki, M.: The Selected Methods of Manipulators' Optimal Trajectory Planning (in Polish). WNT Publisher, Warsaw (2000)
27. Khansari-Zadeh, S.M., Billard, A.: A dynamical system approach to real-time obstacle avoidance. Auton. Robot 32(4), 433-454 (2012)
28. Galicki, M., Morecki, A.: Finding collision-free trajectory for redundant manipulator by local information available. In: Proc. 9th CISM-IFToMM Symposium on Theory and Practice of Robots and Manipulators, pp. 61-71. Udine (1992)
29. Siciliano, B., Sciavicco, L., Villani, L., Oriolo, G.: Robotics, Modelling, Planning and Control. Springer, London (2009)
30. Results of simulation, http://www.uz.zgora.pl/~gpajak/ rtoolbox/jint15
Grzegorz Pajak received PhD degree in Automatics and Robotics from Poznan University of Technology, Poland in 1998. Now he works at University of Zielona Gora. His current research interests include optimal control of dynamic systems with special focus on trajectory planning and control of mobile manipulators and robots.
Iwona Pajak received PhD degree in Automatics and Robotics from Poznan University of Technology, Poland in 2000. Now she works at University of Zielona Gora. Her current research interests include real time motion planning of mobile manipulators and robots.
