Trajectory Generation for Mobile Manipulators

This book presents the most recent research advances in robot manipulators. It offers a complete survey to the kinematic and dynamic modelling, simulation, computer vision, software engineering, optimization and design of control algorithms applied for robotic systems. It is devoted for a large scale of applications, such as manufacturing, manipulation, medicine and automation. Several control methods are included such as optimal, adaptive, robust, force, fuzzy and neural network control strategies. The trajectory planning is discussed in details for point-to-point and path motions control. The results in obtained in this book are expected to be of great interest for researchers, engineers, scientists and students, in engineering studies and industrial sectors related to robot modelling, design, control, and application. The book also details theoretical, mathematical and practical requirements for mathematicians and control engineers. It surveys recent techniques in modelling, computer simulation and implementation of advanced and intelligent controllers.


Introduction
Mobile robot navigation has stood as an open and challenging problem over decades. Despite the number of significant results obtained in this field, people still look for better solutions. Some mobile robots are subject to constraints of rolling without slipping and thus belong to non-holonomic systems. Mobile robots also are subject to navigate in environments cluttered with obstacles. Now, in case the mobile robot presents a nonholonomic constraint, the problem consists of finding a path taking into account constraints imposed both by the obstacles and the non-holonomic constraints. Since non-holonomy make path planning more difficult, many techniques have been proposed to plan and generate paths. May be the most popular is the method of potential field (Khatib, 1986). However, this method may present some problems such as sticking to local minima. Moreover, the kinematic constraint is the other problem that can face trajectory planning. This can make time derivatives of some configuration variables non-integrable and hence, a collision free path in the configuration space not achievable by steering control.. Some researchers worked to find feasible path using different methodologies (Sundar & Shiller, 1997), (Laumond et al, 1994), (Reeds & Shepp, 1990). To deal with obstacles, some researchers decomposed the dynamic motion to static paths and velocity-planning problem , (Tilbury et al, 1995). In the work of (Qu et all, 2004), the authors treated the problem as a family of curves where the optimal path is found by adjusting a certain polynomial parameter. This idea was raised in many references including (Kant & Zucker, 1988) and (Murray & Sastry,1993), where trajectories are represented by sinusoidal, polynomial or piecewise constant functions. In our recent work, and based on intelligent control, we proposed a fuzzy control methodology to navigate a mobile robot in a cluttered environment with the aim to reach the goal while avoiding static and/or dynamic obstacles (Abdessemed et al, 2004). Concerning robot arm path planning and trajectory generation, considerable efforts have been devoted to make these mechanical systems succeeding in their tasks. If we consider a robot arm with n-joints that move independently, the robot's configuration can be described by a 3-dimentional coordinate: (xe, ye, ze) for the location of the end effector. These coordinates characterize the workspace representation, since they represent exactly the same coordinates of the object it intends to manipulate or to avoid. Although the workspace is well suited for collision avoidance, it happens that we are still facing some other problems. In fact the task is usually expressed in the workspace coordinates and the question is how to map this space into configuration space. This problem finds its solution in the inverse kinematics. However, calculating the inverse kinematics is hard, and the problem becomes much harder if the robot has many DOFs. Moreover, for a particular workspace coordinates, many distinct configurations are possible. The other problem that may arise when using configuration space is the presence of obstacles. Within the scope of all these problems, many methods to path planning emerged. Among the techniques found in the literature, we state the cell decomposition methods (Russel & Norvig, 1995), Skeletonization methods (e.g. Voronoi graph (Okabe et al, 2000)), potential field. Due to some problems of the applied techniques already mentioned, such as local minima and uncertainties, probabilistic and robust methods have been explored. Demonstration of robustness of fuzzy logic and genetic algorithm encouraged many researchers to use these concepts for path planning and obstacle avoidance. We note that robust methods assume a bounded amount of uncertainties and do not assign any probabilities to values. The robots could replace good number of specific machine tools and could continue to supplant the man in a lot of complex tasks. In spite of all, the robot manipulators on fixed base have a lot of limitations. This is why like a man who has the faculty to move to achieve some tasks at different places or to do continuous tasks requiring a work of the arm during the displacement, the mobile manipulators have been considered. Mobile manipulators received particular attentions these last decades (Zhao et al, 1994); (Pin & Culioli, 1992); (Pin et al, 1996); (Lee & Cho, 1997); (Seraji, 1995). This is mainly due to their analytic problems and their various applications. A mobile manipulator consists of an arm fixed on a mobile platform. Such a configuration leads us toward a kinematically redundant system. Although we can construct non redundant mobile manipulators but, there are some good reasons to make us thinking of these systems as for example: increasing the working space of the arm, avoiding static or dynamic obstacles or, to avoid the robot singularities. From these observations, which allow to increase the working space by the mobility of the platform that a number of applications have been appeared. When these systems are devoted to indoor tasks, they are often equipped with wheels. The arrangement of the wheels and their actuation device determine the holonomic or non-holonomic nature of its locomotion system (Campion et al, 1996). Whereas some wheeled mobile manipulators built from an omni-directional platform are holonomic. Extensive research efforts have led people to plan the collision free paths for mobile manipulators. Many techniques have been proposed for path planning and trajectory generation. A desired task is usually specified in the work space. The first type has been a subject of our previous work [Abdessemed et al] where as the second type considers the plat form to be holonomic [Djebrani et al]. However, trajectory following control is easily performed in the joint space. Therefore, it is essential to obtain the desired joint space trajectory given the desired Cartesian space trajectory. This is accomplished using the inverse kinematics transformation. Controlling such systems is hard and the problem is more difficult to solve in case where the mobile platform contains some non-holonomic kinematical constraints. However, one should know that such a constraint does not decrease the configuration space reachable by the mobile platform but decreases the velocity space. Therefore, the mobile platform moves only along trajectories having a certain shape. On the other hand, if we choose a holonomic platform, the control would be much easier. In fact, A holonomic platform robot is an omni-directional robot whose mechanical structure enables it to change its displacement at any direction, without waiting for the reconfiguration of its rolling parts. One of the consequences of the omnidirectionality is that the orientation of a robot becomes independent of the trajectory performed, provided that each "wheel" of the robot has the 3 degrees of mobility (2 translations and 1 rotation). In this book chapter, we try to see the two case studies, namely a holonomic and a non-holonomic mobile manipulator. The approach presented describes the development of the complete kinematic representation of a mobile manipulator. In this case, we present the analysis of the whole mechanical system constituted of a mobile platform over which a robot manipulator is mounted, forming thus the mobile manipulator (Fig. 1); the arm and the mobile platform are considered as a unique system. In this part of analysis, the mobile manipulator is considered as a unique entity. In order to have an overall study, we consider the two types of mobile platforms. The mobility introduced by the mobile platform is exploited to solve problems like collision avoidance and joints saturations. The results obtained demonstrate the effectiveness of the approaches for simple situations as well as for complex situations when obstacles are encountered.

Analysis
In this case a mobile manipulator with non-holonomic platform is viewed. Figure. 2 shows the four main reference axes: The stationary reference axis, the reference axis attached to the mobile platform, the reference axis attached to the base of the robot manipulator, and finally the reference axis attached to the end point of the effector. The homogeneous matrix found by a successive multiplication of the three homogeneous sub matrices can obtain the position and the orientation of the terminal point of the end effector with respect to the stationary reference axis: Such that the matrix o T p is determined by a certain matrix A(q), p T m is a fixed matrix and m T e is determined by the joint variable vector T θ = θ , θ , ..., θ n 1 2 m     , n m represents the degree of freedom of the arm manipulator.
www.intechopen.com The equations of the geometric model are found to be: (2) (x A , y A, ) represent the mobile platform coordinates and its orientation in the world frame. ( 1 ,  2,  3 ) are the three angles of the arm, and l 1 , l 2 and l 3 its lengths. (x , y ,z ) e e e are the coordinates of the end effector in the world frame. As we can see, the position vector  of the end effector E with respect to the world coordinate W is a nonlinear function of the configuration vector: , (n=3+n m ). Now, if the vector d X e is the vector of the wanted task then If we derive this equation, we get the kinematic equation of the model where J m (q) is the mxn Jacobienne matrix of the mobile manipulator. This equation represents a holonomic kinematic constraint since it can be written as If the mobile platform is non holonomic and without slip, then the following kinematic constraint is true: Equivalently, we can write the non-holonomic constraint (5) as: , this means that the system is non holonomic. Equations (4) and (8) are combined to give the differential kinematic model of the system including the mobile platform and the robot manipulator [1].
. In a compact form, equation (9) can be written as: where: If we assume that the speed of the end-effector is X d  , we need then to solve the following differential equation: The term redundancy designates the determination of admissible control signal for this redundant system. However, the system being undetermined, it is necessary to use some criteria that allow determining one of the infinite solutions of the problem. according to (Liégeois, 1997), the general formulation of the inverse kinematics is expressed by the following equation Where , J # is the generalized inverse of the Jacobian matrix. The first term of the equation (12) represents the particular solution used to achieve the desired velocity of the end effector, and the term # (I -J J) is an operator of projection that projects an arbitrary vector into the null space of J. Therefore, the term # (I -J J)z is the general solution of the homogeneous equation: The homogeneous solution contributes only in a motion within the joint space of the mechanical system named the self-motion. In order to find the optimal solutions, let us mention the techniques most commonly used for serial-chain redundant arms:  Minimization of joint velocities  Minimization of joint acceleration  Minimization of kinetic energy Some of these techniques can present some problems as: nonzero joint velocities corresponding to zero end-effector velocities, and instability of the motion. Otherwise, if the mobile manipulator is brought to evolve in an environment cluttered of obstacles, then the goal consists to find the solutions to the problem that must take them into consideration, consequently, one can write the relation as a certain function f such that, q = f(q, x, obstacles)  

Geometric solution with no obstacles
For this redundant system the matrix J is of dimension mxn, with m<n. We seek a solution to equation (12), which guarantees a minimum value for the norm, in addition to a number of solutions in the null space of J. The solutions in the null space can be used to optimize some tasks as for example: to avoid some obstacles or to warn saturations of the joints. However, it is not recommended in practice to use directly the solution with minimal norm. Indeed, to avoid big velocities values, one can impose a weighting matrix for the linear and angular velocities, in order to minimize the sum of such that: -(q ) and b max b , b , , b max max n i i i min 1 2 Thus, the optimal solution is the one that optimises the norm defined by the following expression: While noticing that the matrix W is diagonal, it could be split into two diagonal matrices such that: In this case the norm could be written as : and relation (11) becomes: www.intechopen.com To find q  , one should solve equation (21), which minimizes the norm (18). The problem becomes an optimization problem, where we consider -1 JW o as a system matrix and the product W q o  as the vector whose norm is to be minimized; the solution is thus: knowing that: The first term of the equation (22), represent the optimal solution and the second term the homogeneous solution. The vector z is an arbitrary vector that is projected by the matrix H in the null space of J. It can be used to prevent saturation of the manipulator joints or to avoid unforeseen obstacles. To correct any drift of the trajectory of the space of the task, we introduce the error that measures the difference between the wanted vector and the one measured to yield: However, if we try to solve the equation (24), one can fall on a problem of numeric instability. To overcome this problem, we propose to use the singular value decomposition as a solution. This algorithm is a stable numerical procedure based on the decomposition theorem.

Theorem
Given a matrix A of size mxn, it can be written as a product of three matrices as: The matrices U et V are orthogonal matrices and  is a diagonal matrix for which the elements on the diagonal are the singular values of the A matrix. If one applies this theorem to the matrix: In this case, the complete solution becomes: . The complete solution makes intervene also the matrix H representing the projection of an arbitrary vector z in the null space of the Jacobian matrix J.

Geometric solution with obstacle avoidance
Now, if we consider a smooth function g(q), representing a certain criterion to be minimized, then the vector z of the general solution given by equation (22) can be defined as follows: is the gradient of g, and the homogeneous solution is obtained by projecting z in the null space of J. However, we can use any function as far as it can be reduced to an expression that involves only terms of generalized joint variables. The method of the gradient is of a very vast use because it allows an easy incorporation of the different performance criteria in the control algorithm. This technique is used in our case to satisfy two objectives; first avoiding obstacles and second avoiding joint limits. Thus, the vector z is composed of two terms: . V1 is the potential associated with the joint limits such that: and V2 is the potential associated with the obstacle presence.
q i and q i represent respectively the upper and lower joint limits. Whereas V 2 is the potential associated with the obstacle presence such that: Such that www.intechopen.com k 1 denotes some adjusting coefficient, d and d 0 represent respectively the distance of a certain point on the robot to the obstacle and the minimal security distance.

Simulation results
A series of simulation were conducted in order to illustrate the performance of the method. Fig. 3, shows the terminal point of the end-effector following a reference trajectory in a free obstacle environment in the case where we consider the whole system as unique. In this case, one notes, according to Fig. 4, that the mobile platform follows a non-deformable trajectory in the x-y plane. The curves presented in Figs. 5, 6, 7 and 8 shows the evolution of the corresponding mobile manipulator joints. However, if an obstacle is put on the path of the mobile platform then, one can see that the mobile platform succeeds in getting around the obstacle while maintaining the terminal point on the reference trajectory; this is depicted in Fig. 9, and the new trajectory of the mobile platform in the x-y plane is clearly shown in Fig. 10. The curves presented in Figs. 11, 12, 13 and 14 show the evolution of the new corresponding mobile manipulator joints.

Analysis
The other type of mobile platforms that we intend to present in this section is the one with omnidirectional wheels. These particular types of wheels are used to develop a holonomic mobile robot. They enable the robot to move in any direction at any orientation. There is no need to change the orientation of the platform while moving in an arbitrary trajectory. The direction of the linear velocity is independent from the orientation of the mobile platform. We used the particular concept of a wheel formed with 2 truncated spheres intermechanically dependent developed in Mourioux and his colleagues, (Mourioux et al., 2006). Two parallel planes truncate each sphere. An axis enables each sphere to turn on it freely. This axis is maintained by a fork, which can rotate by using a DC motor. We consider here the mobile manipulator shown in Figure 15. The location of the platform is given by a vector [ , , ]    T x y p ; which defines the position and the orientation of the platform in the fixed frame The position of the point O 4 in the fixed frame is thus given by its Cartesian coordinates  1,  2, and  3.

Geometric solution with no obstacles
The control of the mobile manipulator is given by T T T u = [u , u ] = [q , q , q , x, y, ] a p a1 a2 a3  3  2  2  1  3   2  2  23  3  1  2  2  23  3  1  2   2  2  23  3  1  2  2  23  3 From (32), we get the instantaneous location kinematics model: Where; The kinematic control problem is aimed to find the control vector   of the end effector in such a way that the error (t) (t) * e(t) ξ ξ   approaches zero. Since the system is redundant ( δ m < m ), we set m add additional tasks (Seraji, 1998)  . Equation (34) and (35) are combined to give the differential kinematics model Such that: The problem is now to regulate the actual value of ξ t to * *T *T T ξ =[ξ ,ξ ] t add . Let square. If we suppose that J t is of full rank and if r = rank(J ) t , then r = min (m+m add )x m .
The control vector u is computed by solving the linear system ξ t = J u t  (Bayle et al., 2002).
Where W t is an (m + m add )-order definite positive matrix and * * ξ (t)=ξ (t) t d denotes the desired motion. www.intechopen.com

Geometric solution with obstacles
Up to now we have supposed the path of the robot clear from any obstacles.. However, in case of presence of obstacles some modifications have to be done. In this case, we use an approach based on virtual impedance model (Arai & Ota, 1996). This model can be seen as an extension of the potential field concept (Khatib, 1986). This model determines the motion of a robot by means of a desired trajectory * ξ t modified by a sum of different forces. These forces consist of three parts: an attractive force named F target , generated to attract the robot toward the objective, a repulsive force generated between the robot and the obstacles F obs , and a third force generated between the platform and the carried arm manipulator F man , (see Fig 16). In this work, only the first two forces are considered. The closed loop dynamical equation is expressed as equation (37).
Where F ext represents all the forces exerted on the mobile robot, such that: From equation (37) If e d approaches zero, then equation (38) is realized. The realized trajectory in equation (41) can be seen as the sum of the desired trajectory * ξ t and the force correction F ext Z d to give what we call the desired modification motion: Note that F ext =0 in free motion.
www.intechopen.com  The magnitude F obs is chosen to be (Borenstein & Koren, 1991): obs obs where a obs and b obs are positive constants satisfying the min obs obs , d max is the maximum distance between the robot and the detected obstacle that causes a nonzero repulsive force, d min represents the minimum distance accepted between the robot and the obstacle and d(t) is the distance measured between the robot and the obstacle d min < d(t) < d max ( Fig. 17). Note that the bound d max characterizes the repulsion zone. Which is inside the region where the repulsion force has a non-zero value. Desired interaction impedance is defined as the linear dynamic relationship Z d = B d s + K d where B d and K d are positive constants simulating the damping and the spring effects, respectively, involved in the robot obstacle interaction inside the repulsion zone.

Simulation results
Simulations are conducted in order to show the performance of the proposed methodology. The numerical example supposes the lengths of the arm are such that a = 0.6, a = 0.4, a = 0.3 1 2 3 and the initial configuration of the mobile manipulator is such that: ξ = (0. Furthermore, we imposed the following additional tasks to the mobile platform * * * * ξ (t) = (x (t), y (t), (t)) = (t, t, π/4) p   . Fig. 18 shows the stance of the whole system when the end effector tracks the reference trajectory. The resulting trajectory of the end effector as well as that of the mobile plat form is depicted in Fig. 19. Figures 20, 21, 22 and 23describe the evolution of the angles of the arm and the orientation of the platform respectively. If the robot finds an obstacle at less than d = 1m max the impedance control is activated, and the collision is avoided as it can be seen in Fig. 24.

Conclusion
This work proposed two different methodologies to generating desired joint trajectories for both holonomic and non-holonomic mobile manipulators given prespecified operational tasks. The first part considers a non-holonomic platform where the generalized inverses in the resolution of a redundant system are used. The additional degrees of freedom are exploited to avoid unforeseen obstacles and joint limits. In the second part of the work a holonomic platfrom is used. In this case, the trajectory is generated using a reactive approach based on virtual impedance and additional tasks. When the robot task is about a stationary point, the mobile manipulator showed a good tracking for the manipulator. As perspective an estimate procedure must be conducted in order to estimate the contact forces and the unknown holonomic mobile manipulator parameters driving the system Computer www.intechopen.com simulations have validated to show the effectiveness of the two approaches. The reference values obtained by the two methods can be used as inputs to controllers for real mtion.