JERK BOUNDED TRAJECTORY PLANNING FOR NON-HOLONOMIC MOBILE MANIPULATOR

Atef A. Ata
Department of Engineering Mathematics and Physics Faculty of Engineering, Alexandria University Alexandria 21544, Egypt
[email protected]

Amr El Zawawi Department of Electrical Engineering Faculty of Engineering, Alexandria University Alexandria (21544), Egypt
[email protected]

Mostafa A.E.Razek Department of Mechatronics Engineering Alexandria Institute of Engineering and Technology, AIET, Alexandria, Egypt
[email protected]

ABSTRACT Mobile manipulator systems comprising of a mobile platform with one or more manipulators are of great interest in a number of applications. This paper presents a modeling of the system without violating the nonholonomic constraints of the platform. Trajectory planning by soft motion and seventh order polynomial to control jerk acceleration, and velocity, is also presented. The hub torque required to move the manipulator according to a prescribed trajectory and the platform motor torque are also calculated for each trajectory. INTRODUCTION Mobile manipulator is nowadays a widespread term referring to robot systems built from a robotic manipulator arm mounted on a mobile platform. Such systems combine the advantages of mobile platforms and robotic manipulator arms and reduce their drawbacks. A mobile manipulation system offers the dual advantages of mobility offered by a mobile platform and dexterity offered by the manipulator. The mobile platform offers unlimited workspace to the manipulator. The extra degrees of freedom of the mobile platform also provide the user with more choices. Papadopoulos and Poulakakis (2000) presented a methodology for computing actuator commands for such systems that allow them to follow desired end-effector and platform trajectories without violating the non-holonomic constraints. They used orthogonal complements and Lagrangian methodology to obtain equations of motion. They applied a third order polynomial trajectory for the joints and a second order polynomial trajectory for the motion of the platform and used a fifth order polynomial trajectory for each link of the manipulator. Mohri et al. (2001) derived the dynamics of the mobile manipulator considering it as the combined system of the manipulator and the mobile platform by Lagrange method. Trajectory planning problem is formulated and optimal control problem with some constraints is solved numerically by using the concept of the order of priority and the gradient function.
Proceedings 27th European Conference on Modelling and Simulation ECMS Webjrn Rekdalsbakken, Robin T. Bye, Houxiang Zhang (Editors) ISBN: 978-0-9564944-6-7 / ISBN: 978-0-9564944-7-4 (CD)

Chung and Velinsky (1999) developed a dynamic model of a mobile manipulator subject to wheel slip for high load and high speed applications. First, the Newton-Euler method was used to derive the dynamic equations of motion in which Dugoffs tire friction model is utilized. First order polynomial and cyclic motion trajectories are used in this model. Omreen et al. (2003) used the Lagrangian dynamics to express the dynamic model of the complete system. A torque compensation control approach is proposed for the robust motion control of the robot by Chi-wu and Ke-fei (2009). They considered the coupling disturbance between the platform and the manipulator trajectory tracking curve of mobile platform. In all the above mentioned research work, jerk and acceleration problems at start and end of manipulator motion are not considered. On the other hand, (Mostafa et al. 2010) applied fifth order polynomial trajectory which can control the acceleration at start of manipulator motion. Broqu'ere et al. (2009) applied the soft motion trajectory planner which takes into account both the point to point motion and the transition motion. For each axis, these cubic trajectories share the same time intervals. Due to the direct computation of cubic parameters, the planner is fast enough to be used on-line. The soft motion trajectory planner limits jerk, acceleration and velocity in Cartesian space using quaternion. Tawfik et al. (2011) studied the effect of the trajectory planning method on the dynamic response of six degrees of freedom micro-robot intended for surgery applications. The kinematic equations of motion were obtained using Denavit-Hartemberg representation. The trajectory planning was derived using two different methods: the fifth-order polynomial and the soft motion trajectory planning. A comparison of the dynamic response was carried out to choose the best method that gives the smoother trajectory and better performance of the robot under investigation. Jerk bounded trajectory for a non-holonomic mobile manipulator is investigated in this paper. The equations of motion of the system are obtained using the Lagrange multipliers technique. Two jerk bounded trajectories are applied and the hub torques of the manipulator are

calculated for each trajectory for comparison. The paper is organized as follows: Section (1) is introductory in nature, while section (2) focuses on the mobile manipulator kinematics modeling using the Jacobian to calculate the velocity of the end-effector. Section (3) presents the dynamic modeling using Lagrange multipliers taking into account the non-holonomic constraints in the platform wheels. In section (4) trajectory planning by soft motion and 7th order polynomial according to maximum allowed jerk, acceleration and, velocity are presented. Section (5) displays the simulation results for the platform and the links to calculate right and left wheel torques and also hub torque at joint 1 and joint 2 respectively.

Where, : Angular velocity of the left wheel. : Angular velocity of the right wheel. r : Radius of the left and right wheels. C and S : represent cos and sin of any angle respectively From Eqn. (2) the base velocity without steering can be given as:
+ =

+ =
.

The differential kinematic equation relating the wheel angular to the linear velocities of point F and the platform rate of rotation, in matrix form, is given by

(2)

KINEMATIC ANALYSIS There are two configurations of mobile manipulators based on various mobile platform designs. The first design uses differential drive, where The platform moves by driving the two wheels as show in Figure (1). The two driven wheels do not slip sideways and the resulting velocity constraint, for point F, is given by [Papadopoulos and Poulakakis (2000)]:

(3)

It can be seen from Eqn. (3) that if (lG = 0) the manipulator is mounted on the axis that connects the wheels centers, then the second matrix in Eqn. (3) will be singular. Consequently one degree of freedom will be lost to remove this problem, manipulator must be mount away from the axis connecting wheels centers. To obtain the end-effector velocity we used the Jacobain method as follows:
= + (4) Y = Y + Y (5) sin + = [ cos + cos( + )] sin + [ sin( + ) sin( + )] cos (6) cos + = [ sin + + )] cos cos( + + cos( + )] sin (7)

In which Figure 1: Mobile Manipulator System on a DifferentiallyDriven Platform

: is the distance between G and F F : is the velocity component in the x-direction F : is the velocity component in the y-direction : is the steering angle of platform Equation (1) represents a non-holonomic constraint which cannot be integrated analytically. The kinematic equations of mobile manipulator are divided into two parts the kinematics of the platform and the robot arm kinematics. Kinematic Equations of Platform

Where,

Substituting from Equations (6) and (7) into (4) and (5) and making use of Eqn. (3) we can write the end-effector velocity in matrix form as:
= + + (8)

: Velocity component in X-axis of the manipulator relative to point F. : Velocity component in Y-axis of the manipulator relative to point F. : Length of upper arm. : Length of fore arm. , and : joint variables of the manipulator

, are the x and y velocity components of Where and the end-effector, and the Jij (i, j=1,2) terms are the elements of fixed-base Jacobian of the manipulator and are given by:
J11 = sin sin( + ) J12 = sin( + ) (9a) (9b)

J21 = cos + cos( + ) J22 = cos( + ) 0 0 0 0 = 0 0 0 0 2 2 + ) ( + ) ( + 2 2

(9c) (9d)

Combining Eqn. (8) and (2), the forward differential kinematics of the system is obtained as:

0 0 0 0

Where [ = ]T is the generalized coordinate, is the Lagrange multiplier that corresponds to the constraint force, and represents externally applied forces. The columns of from a non-normalized base for these forces. Expressing Eqn. (12) in terms of the generalized coordinates and substituting the result into Eqn. (13), the system equation of motion are obtained in the form: Where M(q) is the 5 5 inertia matrix, V(q, ) is the vector of velocity-dependent forces, = [ , , , ]T is the 4-dimentional torque vector, E(q) is a 5 4 input transformation matrix, comprised of the left and right wheel torques and the first and second manipulator joint torques, and is Lagrange multiplier.
M(q)q + V(q, q ) = E(q) A (q)

(14)

(10)

DYNAMICAL ANALYSIS Lagrange's equation cannot be applied directly to find the equations of motion for mechanical system due to presence of the non-holonomic constraint. The system under consideration is subject to a single non-holonomic constraint, which is described in matrix form as [Papadopoulos and Poulakakis (2000)]: )( = 0 Where, (11)

TRAJECTORY PLANNING Trajectory planning refers to the way that a robot moves from one location to another in a controlled manner. The sequence of movements for a controlled movement between motion segments, in straight-line motion or in sequential motions. Normally the jerk of the desired trajectory has undesired effects on the performance of the tracking control algorithms for robotic manipulators. Controlling the jerk of an industrial manipulator results in improved path tracking and reduced wear on the robot. Soft Motion Trajectory Planner We consider the planning of a trajectory defined by a set of points generated by path planning techniques that the end effector must follow in Cartesian space. We propose a soft motion trajectory planner that limits jerk, acceleration and velocity for service robot applications [7]. The trajectory consists of seven cubic polynomials starting by identifying the jerk as follows:
J(t) = J (t) = + J t (t) = + t + J t

[ = )(sin cos 0 0] and = [ ]T To derive the equations of motion for the mobile manipulator system, an alternative approach which can accommodate the non-holonomic constraint is the Lagrange multipliers. The Lagrangian L(q, ) of the system assuming that the mass and the moments of inertia of the casters and the driving wheels are negligible, is given by:
) ) = ( + + + ( + + + ) + ) + ( + + ( +

(12)

1st, 3rd, 5th and 7th segments (15a) (15b) (15c) (15d)
(16a) (16b) (16c) (16d) (17a) (17b) (17c) (17d)

Where; : mass of platform. : mass of first link. : mass of second link. : moment of inertia of the platform. : moment of inertia of the first link. : moment of inertia of the second link. , , , , , are the x, y components of the velocities of the centre of mass of the platform, and the first and the second link respectively Adding constraint forces as input terms forms the equation of motion of the constrained system. These forces are responsible for not allowing the wheels to slip sideways. The constrained dynamics of the system can be described by [1]:

(t) = + t + t + J t

J(t) = 0 (t) = (t) = + t (t) = + t + t

2nd and 6th segments

+ )( =

J(t) = 0 (t) = 0 (t) = (t) = + t

4th segment

(13)

Where J,, , and are the jerk, angular acceleration, angular velocity and joint angle, respectively.

The soft motion trajectory of the mobile manipulator is shown in Fig. 2 where the maximum allowable jerk is 0.5 m/s3, maximum allowable acceleration is 0.75 m/s2 and, maximum allowable velocity is 1 m/s. On the other hand Platform trajectory is planned by: = 0.1, = 0.1 and, =
.

= 0, = 0.5 , = 0 , = 0 , = 0 , = 0 , = 0.5, and = 0.5.

Figure 3: Seventh Order Polynomial Trajectory for Both Joints NUMERICAL SIMULATION AND DISCUSSION Figure 2: Soft Motion Trajectory for Both Joints Seventh Order Polynomial Trajectory
By designing the joint trajectory as a third order polynomial, it is only possible to control the initial and final velocities. Increasing the order of the polynomial allows the designer to control the initial and final acceleration as well as the jerk. The seventh order polynomial used to control the initial and final position, velocity, acceleration, and jerk is given by: ( = ) + + + + + + +

In order to illustrate the validity of the modeling of mobile manipulator and effectiveness of the motion planning, we perform the simulation in MATLAB. The total time was chosen equal to 3.5s. The kinematic and dynamic parameters of manipulator and platform of simulated robot are given in table 1and 2. Table 1: Kinematic Parameters of The System Description Platform width Platform length Wheel radius Parameter b
2

Where , , , , , , , and are the coefficients to be determined from the initial and final conditions. These coefficients can be determined by solving the following equation:
1 2 4 3 0 1 12 0 0 2 6 24 0 0 0 6 1 0 1 3 4 0 0 1 6 12 24 0 0 0 6 = J J 6 5 30 20 120 60 30 20 5 6 7 42 210 7 42 210

(18)

Value 0.6 1 0.2 0.5 0.25 0.5 0.25

Unit m m m m m m m

Length of link 1 Length of center link 1

Table 2: Dynamic Parameters of The System Description Platform mass Mass of link 1 Mass of link 2 Platform Moment Inertia moment of link 1 Inertia moment of link 2 Parameter Value 50.0 4.0 3.5 1.417 0.030 0.036 Unit kg kg kg kg.m2 kg.m2 kg.m2

60 120

(19)

The simulation procedure is executed as follows: 1. For joint (1) and joint (2) we proposed soft motion trajectory Eqns. (15), (16) and (17) or seventh order polynomial (18) for the platform trajectories, the

Where t0 is the initial time and tf is the final time. Fig. 3 shows seventh order polynomial trajectory planned for the two joints. The initial and final values are:

2.

By substituting the trajectory equations in manipulator equations of motion we can compute the first and second joints torque, similarly substitute trajectory equations in platform equations of motion we can compute left and right wheels torque. The simulated results for the joints torques for the joint 1 and joint 2 for both trajectories are shown on Figures (4) and (5) respectively, while the driving torque for the right and left wheel motors are shown on Figures (6) and (7) respectively. Figure 7: Left Wheel Torque It can be seen from the simulated results of the driving torques for both joints that there is no considerable change of the torque values using both trajectories and the driving torques have almost the same trend. It should be noticed that the drive torque for joint 2 using the seventh order polynomial is smoother than the torque using the soft motion trajectory. For the right and left wheel motors using either trajectory give the same results. The soft motion trajectory enables the designer to control the jerk throughout the time interval while the seventh order trajectory allow controlling the jerk at the start and end of trajectory only. On the other hand the seventh order trajectory is represented by one polynomial from the start to the end of the time interval.

= 0.1 and, =

following polynomials are proposed: = 0.1,

Figure 4: Torque for Joint 1

CONCLUSION This paper focuses on the mobile manipulator design using kinematic and dynamic modeling using Jacobian method to calculate the velocity of end-effector and dynamics modeling by Lagrange multiplier due to nonholonomic constraint in platform wheels. The equations of motion for platform and links are then calculated to find right and left wheel torque and also joint 1 and joint 2 torques to achieve desired trajectory. A comparison between two jerk bounded trajectories and their effect on the hub torque of each joint is also investigated.

Figure 5: Torque for Joint 2

REFERENCES Broqu'ere, X.; D. Sidobre; and I. Herrera. 2009. Soft Motion Trajectory Planner for Service Manipulator Robot. IEEE Trans. on Robotics, (Apr),281291. Chi-Wu, B. and X. Ke-fei. 2009. Robust Control of Mobile Manipulator Service Robot Using Torque Compensation. Proc. International Conference on Information Technology and Computer Science, Trondheim. Norway, (May), 69-72. Chung, J. and A. Velinsky. 1999. Robust Control of a Mobile Manipulator - Dynamic Modeling Approach. Proc. American Control Conference, San Diego, California, (June), pp. 2435-2439. Khaled T. Mohamed; Atef A. Ata; and Bassuny M. ElSouhily. 2011 Dynamic analysis algorithm for a micro-robot for surgical applications. International

Figure 6: Right Wheel Torque

Journal of Mechanics and Materials in Design, Volume 7, (NOV), 17-28. Mohri, A.; S. Furuno; and M. Yamamoto.2001. Trajectory Planning of Mobile Manipulator with End- Effectors Specified Path. Intelligent Robots and Systems, Maui (USA), (Nov), 2264-2269. Mostafa, S.; G. Mostafa; and M. Masoud. 2010. Optimal Trajectory Planning of a Mobile Robot with Spatial Manipulator for Obstacle Avoidance. International Conference on Control, Automation and Systems, Gyeonggi-do, Korea, (Oct), 314-318. Omr'cen, D.; B. Nemec; and L. Zlajpah. 2003 TorqueVelocity Control Algorithm for On-Line Obstacle Avoidance for Mobile Manipulators. Proc. Of the ICIT 2003 Maribor, Slovenia, (April), 784-789. Papadopoulos, E. and J. Poulakakis. 2000. Planning and Model-Based Control for Mobile Manipulators. Intelligent Robots and Systems, IROS 2000 (July), Japan,1810-1815.

Mustafa Abdel Razek was born in Alexandria Egypt in 1986. He got his B. Sc. In Mechatronics Engineering from Alexandria Institute of Engineering and Technology (AEIT) in 2009 with Honor. He is now performing his M. Sc. And his area of interest is robot dynamics and Control. His E-mail is [email protected]

Author Biographies
Atef A. Ata was born in Alexandria, Egypt in 1962 and received his B. Sc. Degree with Honor in Mechanical Engineering from Alexandria University in Egypt in 1985. After his graduation he obtained his M. Sc. Degree in Engineering Mathematics (Hydrodynamics) in 1990 from Alexandria university. In 1996 he obtained his Ph. D in Engineering Mathematics (Robotics) as a Joint-Venture between University of Miami, Florida, USA and Alexandria University in Egypt. Currently he is a Professor of engineering mechanics at the Faculty of Engineering, Alexandria University, Egypt. Dr. Atef is a member of IEEE robotics and Automation Society and senior member of IACSIT . His research interest includes Dynamic and Control of Flexible Manipulators, Trajectory Planning, Genetic Algorithms and Modelling and Simulation of Robotic Systems. His E-mail address is [email protected]

Amr M. O. EL Zawawi was born in Alexandria, Egypt. He obtained his B.Sc. and M.Sc. in Electrical Engineering on 1972 and 1976 respectively. He also obtained a higher studies diploma DEA on 1977 and a Ph.D. in Electrical Engineering from the ENSIEG of the INPG, France on 1980. His main fields of interest are power electronics, artificial intelligence and industrial automation. His e-mail address is [email protected].