- Email: [email protected]
Minimum cost trajectory planning for industrial robots
European Journal of Mechanics A/Solids 23 (2004) 703–715
Minimum cost trajectory planning for industrial robots T. Chettibi a,∗ , H.E. Lehtihet a , M. Haddad a , S. Hanchi b a Mechanical Laboratory of Structures, E.M.P., B.E.B., BP 17, 16111, Algiers, Algeria b Mechanical Laboratory of Fluids, E.M.P., B.E.B., BP 17, 16111, Algiers, Algeria
Received 18 August 2003; accepted 24 February 2004 Available online 27 March 2004
Abstract We discuss the problem of minimum cost trajectory planning for robotic manipulators. It consists of linking two points in the operational space while minimizing a cost function, taking into account dynamic equations of motion as well as bounds on joint positions, velocities, jerks and torques. This generic optimal control problem is transformed, via a clamped cubic spline model of joint temporal evolutions, into a non-linear constrained optimization problem which is treated then by the Sequential Quadratic Programming (SQP) method. Applications involving grasping mobile object or obstacle avoidance are shown to illustrate the efficiency of the proposed planner. 2004 Elsevier SAS. All rights reserved. Keywords: Robotic manipulators; Motion planning; Obstacles avoidance; Grasping mobile objects; Non-linear optimization
1. Introduction Due to their great ability of speed and precision and their cost-effectiveness in repetitive tasks, industrial robots have been used for decade in place of human workers in automatic production lines. But these powerful machines are hardly autonomous in the sense that they require preliminary actions, such as calibration or motion planning, to achieve specified tasks. In general, robotic manipulators are used at their limit capacities for obvious reasons of productivity. This leads, however, to quite significant joint torque and velocity magnitudes which can be harmful to the system state. In order to increase the manipulator performances, it is highly desirable to control the system dynamic taking into account technological, geometrical and environmental constraints as well as any other constraints inherent both to the robot design and to the nature of the task to be executed. Since many different ways are possible to perform the same task, this freedom of choice can be exploited judiciously to optimize a given performance criterion. During the past decades, a great deal of attention has been given to the problem of motion planning and control. The complexity of the problem made researchers divide the robot control structure into two levels (Fig. 1) (Dombre and Khalil, 1988; Bessonnet, 1992; Angeles, 1997; Chettibi, 2001): the upper level, called path or trajectory planning, and the lower level, called path tracking or path control. Path control is the process that makes the robot actual position and velocity match some desired values provided to the controller by the trajectory planner. The trajectory planner receives a description of the path from which it computes a time history of desired positions and velocities. Then, the path tracker compensates for any deviation. In many publications dealing with motion planning of robotic manipulators, authors state a variety of problems and suggest a large diversity of solution schemes. Several regrouping can be done according to execution modes, adopted models for the robot behaviour and proposed numerical methods of treatment. * Corresponding author.
E-mail address: [email protected] (T. Chettibi). 0997-7538/$ – see front matter 2004 Elsevier SAS. All rights reserved. doi:10.1016/j.euromechsol.2004.02.006
704
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 1. Synoptic diagram of optimal motion planner for robotic manipulators.
Among the tasks which robot manipulators are devoted to, a first distinction can be made in regards to the desired motion nature. Depending on the robot task, it might be necessary to specify the end effector trajectory in the work space. For example, if the effector’s tool acts without interruption according to a predefined path (gluing, arc welding, laser cutting operations, etc.), the planner (or optimization process) defines optimal tracking modalities of the imposed path. On the other hand, in point to point motions (pick-and-place operations, point to point welding), the end effector is free to move between two extremal positions. In this case, the planner tries to define the optimal trajectory and the corresponding controls. A second distinction can be made according to the type of model used. Earlier works (Kahn and Roth, 1971; Luh and Walker, 1977; Luh and Lin, 1981) use kinematic models where the imposed trajectory is geometrically defined in the work space in such a way that the manipulator avoids existing obstacles. The main preoccupation being obstacle avoidance, the optimization problem is synthesized using the inter-penetration distance between elements in collision. Of course, this type of trajectory planning can produce very high execution velocities, particularly when transfer time is minimized. It induces also excessive torque amplitudes which can be harmful for the system state. For these reasons, dynamic models were later incorporated for a more realistic control of the robot dynamic behavior (Bobrow et al., 1985; Pfeiffer and Rainer, 1987; Eltimsahy and Yang, 1988; Yamamoto and Ozaki, 1988; Jaques et al., 1989; Bessonnet, 1992; Chettibi, 2000). However, the problem of motion planning becomes quite complex and requires specific schemes for its treatment. Fig. 2 shows examples of such schemes, some of which are widely used. Two main families can be distinguished: direct and indirect methods (Hull, 1997; Betts, 1998). Indirect methods are based, in particular, on Pontryagin Maximum Principle (PMP) (Pontryagin et al., 1965) which was first used to define the optimal control. The corresponding states may be obtained by another method. For example, the phase plane method is among early techniques taking into account the robot dynamic model and bounds on joint torques (Bobrow et al., 1985; Kang and McKay, 1986; Shin and Mckay, 1986; Pfeiffer and Rainer, 1987; Jaques et al., 1989). It was first used to solve minimum time motion problems along specified paths. Then, it was extended to handle free motions as well (Shiller and Dubowsky, 1986). In this minimum time trajectory planner, it is assumed that the desired path is given in a parameterized form (e.g., using curvilinear abscissa) which is substituted into the manipulator dynamic equations to give a set of second order differential equations in terms of the path parameter. Consequently, bounds on individual joint torques are converted into bounds on parametric accelerations. The allowable sets of second derivatives (one set per joint) are intersected to give a single allowable set. Using the fact that the minimum time solution will be bang–bang in the acceleration, it is possible to construct phase plane plots (Bobrow et al., 1985; Shin and Mckay, 1986; Pfeiffer and Rainer, 1987; Chettibi and Yousnadj, 2001) which give the optimal trajectory in terms of the trajectory parameter and its derivatives. Despite the fact that this technique is elegant and solves the optimal time motion planning problem, the discontinuous nature (bang-bang) of joint torques thus obtained creates many practical problems. In fact, the controller must work in saturation for long periods. Hence, the optimal control leaves no control authority to compensate for tracking errors caused by either unmodelled dynamic or delays introduced by the on-line feedback controller. In addition, this method only takes into account constraints that can be formulated in terms of the trajectory parameter and its time derivatives. Therefore, it spans a small field of possible constraints.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
705
Fig. 2. Methods used for solving optimal free motions planning problem.
PMP is used also to treat directly the optimal dynamic motion planning problem (Bessonnet, 1992; Gaudin and Bessonnet, 1995; Lazrak, 1996; Danes, 1998). The optimality conditions for transfer modalities are expressed as a set of differential equations (Pontryagin et al., 1965). This boundary values problem is solved by specific numerical techniques such as shooting or relaxation methods (Hull, 1997; Betts, 1998). Although this approach has been used successfully in many applications, it tends to be abandoned in recent years. The primary reasons are that the convergence domain is relatively small and that it is difficult to incorporate inequality terms in path constraints. Direct methods were introduced to reduce somewhat the problem complexity. They are based on a discretisation of dynamic variables (states, controls) leading to a parameter optimization problem. Then, non-linear optimization (Richard et al., 1993; Martin and Bobrow, 1997, 1999; Chettibi, 2000, 2002; Bobrow et al., 2001), evolutionary (Nearchou and Aspragathos, 1997; Mao and Hsia, 1997) or classical stochastic (Chettibi and Lehtihet, 2002) techniques are applied to obtain optimal values of the parameters. A common characteristic of these methods is to try to solve directly the problem by adjusting parameters to minimize some cost function while taking into account imposed constraints. Other ways of discretisation can be found (Tan and Potts, 1988; Richard et al., 1993; Chettibi, 2000). However, these techniques suffer from numerical explosion when treating high dimension problems. In the present paper, a different direct method is used for optimal free motion planning problems. The proposed scheme is shown in highlighted arrows in Fig. 2. The basic idea is to parameterize directly the joint evolution vector q(t). The optimal control problem becomes a parametric constrained optimization problem which is to be solved for the unknown transfer time T and the unknown parameters of the chosen model for q(t). Several choices are made here that can be summarized as follows. First, we use for q(t) a convenient clamped cubic spline model with free nodes uniformly distributed along the time scale. Second, we use the SQP method to solve the optimization problem with the unknowns being now the position of spline nodes as well as the transfer time T . Also, the cost function F , which can take many forms, is chosen as a weighted balance of transfer time, mean average of actuators, power, etc. Furthermore, in order to make the resulting motion physically feasible, F is minimised under position, velocity, acceleration, jerk and torque constraints. The problem is formulated in such a way that the same approach can be easily extended to handle
706
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
complex situations. Three examples are treated here: the first one concerns a pick-and-place operation with a six d.o.f robot; the second one deals with a grasping operation of a moving object by a planar redundant robot. The last example takes into account the presence of obstacles in the workspace.
2. Problem statement Let us consider a n d.o.f serial manipulator robot. Motion equations for this system can be derived using Lagrange’s equations or Newton–Euler formalism (Dombre and Khalil, 1988; Angeles, 1997). They are simply given by: M(q)q¨ + Q(q, q) ˙ + G(q) = τ,
(1a)
where M is the inertia matrix, Q is the vector of centrifugal and Coriolis forces in which joints velocities appear under a quadratic form, G is the vector of gravitational forces and τ ∈ n is the vector of actuators efforts. In the presence of external forces Fext applied on the end effector, relation (1a) becomes: M(q)q¨ + Q(q, q) ˙ + G(q) + J T (q)Fext = τ,
(1b)
where J T is the transpose of the robot Jacobian matrix. The robot is required to move freely (without following a specified path) from an initial to a final state, P0 and Pf , respectively. The problem is to find, in addition to actuators efforts vector τ (t) and final time T , the joint time evolution vector q(t) that matches initial and final states, satisfies given constraints and minimizes a given cost function F . 2.1. Constraints Constraints represent physical limitations imposed on the robot kinematic and dynamic performances. They are given by the following inequalities: τi (t) τ max , i = 1, . . . , n, (2a) Joint torques: i ... ...max q q (2b) Joint jerks: i (t) i , i = 1, . . . , n, max (2c) Joint accelerations: q¨i (t) q¨i , i = 1, . . . , n, q˙i (t) q˙ max , i = 1, . . . , n, (2d) Joint velocities: i qi (t) q max , i = 1, . . . , n. (2e) Joint positions: i Of course, non-symmetrical bounds on the above physical quantities can be handled without any difficulties. Relations (2a–e) traduce the fact that not all motions are tolerable, that power resources are limited and must be used rationally in such a way that the robot dynamic behavior is correctly controlled. Jerk constraints are introduced due to the fact that joint position tracking errors increase when jerk increases, and also to limit excessive wear on the robot so that the robot life-span is extended (Piazzi and Visioli, 1998; Macfarlane and Croft, 2001). 2.2. Objective function The motion quality will depend strongly on the expression adopted for the objective function F . In fact, F represents the transfer cost between initial and final postures. Hence, it would be judicious to include significant physical parameters related directly to the global behavior of the robot and to the productivity of the industrial process. The following general expression is a balance between transfer time T and quadratic average of both actuator efforts A and corresponding power B: (1 − µ) αA + (1 − α) B , (3a) F = µT + 2 where 2 τi dt, max τi 0 i=1 2 T n q˙i τi dt, B= q˙imax τimax 0 i=1
A=
T n
A and B have the dimension of a time.
(3b)
(3c)
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
707
µ and α are weighting coefficients (0 µ 1, 0 α 1). The case µ = 1 corresponds to the optimal time motion problem. There are many situations where it is desirable and appropriate to give more importance to some links relatively to the others. In that case, we could just define additional normalized weighting coefficients wi (i = 1, . . . , n) inside expressions (3b) and (3c).
3. Optimal control via direct parameter optimization of joint positions The planning of optimal dynamic motion is a generic optimal control problem. It is transformed here into a parametric optimization problem as follows: joint variables are expressed as a function of a set of parameters. These parameters are varied inside a non-linear optimization program until an optimum satisfying constraints has been reached. Joint trajectories are represented here by clamped cubic spline functions with nodes uniformly distributed along time scale (Fig. 3). This choice can be justified by the well-known characteristics of cubic spline functions; continuity of second order is guarantied and their lower order greatly limits oscillations and enables us to compute easily extremum values between two consecutive nodes. By parameterizing the joint trajectories, variables of the problem become the transfer time and the position of cubic spline nodes. Constraints that must be handled are those defined by relations (2a–e). In constrained optimization, the general aim is to transform the problem into an easier sub-problem that can be then solved. It is used as the basis of an iterative process (Powell, 1984; Fletcher, 1987). A common characteristic of a large class of earlier methods is the translation (using a penalty function) of the constrained problem to a basic unconstrained one. A sequence of parameterized unconstrained optimizations is performed which may converge to the solution of the constrained problem. These methods are now considered relatively inefficient and have been overwhelmed by methods based on the resolution of the Kuhn– Tucker (KT) equations which are the necessary conditions of optimality for a constrained optimization problem (Barclay et al., 1997). The solution of the KT equations forms the basis to many non-linear programming algorithms. These methods attempt to compute directly the Lagrange multipliers. Constrained quasi-Newton methods guarantee superlinear convergence by accumulating second order information regarding the KT equations using a quasi-Newton updating schedule. These methods are commonly referred to as Sequential Quadratic Programming (SQP) and represent the state-of-the-art in non-linear programming methods (Powell, 1984; Schittowski, 1985; Fletcher, 1987). For example, (Schittowski, 1985), has implemented and tested a version that outperforms, over a large number of benchmark problems, every other tested method in terms of efficiency, accuracy and frequency of successful convergences. This method is applied here to solve our new parametric optimization problem. (Martin and Bobrow, 1997, 1999; Bobrow et al., 2001) have already made use of SQP to solve a parametric optimization problem. But, instead of clamped cubic splines these authors have used B-splines to model joint variables. Also, their cost function corresponds to the special case (µ = 0, α = 1), that is a minimum-effort problem. However, no bounds equivalent to inequalities (2a–e) are given. In addition, in contrast to (3a) their cost function does not include normalizing factors such as τimax , i = 1, . . . , n. In many practical cases, these factors as well as bounds (2a–e) are in fact necessary. Otherwise, the optimization process may lead either to a solution that is not physically feasible (violation of a technological constraint) or to a biased solution in the case of a problem involving a large difference in torque magnitudes from one actuator to the other.
Fig. 3. Cubic spline approximation of joint position temporal evolution.
708
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
4. Results: pick and place operations We present a simulation study of the standard six d.o.f. PUMA 560 robot (Fig. 4) whose geometric and inertial arm parameters are listed in Appendix A according to those reported in Armstrong et al. (1986). The robot is asked to carry out transfer between two configurations defined in Table 1. Initial and final joint velocities are null. The transfer must be done without violating bounds on kinematic and dynamic performances (given in Appendix B). Computations are carried out for various cases using a cubic spline function with four nodes. Solutions are obtained in few minutes on a 1.6 GHz PC. Table 2 summarizes numerical results for sample values of weighting coefficients. Fig. 5 shows an example of profiles of joints positions, velocities, accelerations, jerks and torques as well as a 3D representation of the corresponding motion. Graphical results are given in the following units: position (rad), velocity (rad/s), acceleration (rad/s2 ), jerk (rad/s3 ), and torque (N m). The time scale is given in seconds. Transfer between initial and final postures is executed in a smooth way and, of course, without violating imposed constraints. Varying the weighting coefficients leads to different types of motions. In general, for fixed α, increasing µ leads to lower transfer time and higher torques and velocities. On the other hand, for fixed µ, increasing α leads to higher transfer time and lower torques and velocities. Hence, the power term in the objective function acts as a regulator for torques and velocities profiles.
Fig. 4. PUMA 560 robot shown here in its zero angle pose.
Table 1 Configurations to be linked
Table 2 Numerical results
Joint N ◦
Initial
Final
µ
α
Cost function
Transfer time
A
B
1 2 3 4 5 6
0 −π/6 0 −π/3 0 0
2π/3 π/6 π/4 π/3 −π/4 π/6
1 0.75 0.75 0.5 0.5 0.5 0.25 0.25 0.25
N/A 0.25 1 0 0.5 1 0.5 0.75 1
1.159 1.376 1.404 0.608 0.944 1.370 0.763 0.752 1.167
1.159 1.819 1.845 1.159 1.934 2.675 2.865 2.788 4.465
0.200 0.172 0.171 0.199 0.156 0.147 0.165 0.174 0.136
0.115 0.075 0.072 0.113 0.066 0.061 0.078 0.061 0.078
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 5. Optimal motion obtained for (µ = 0.5, α = 1).
709
710
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 6. To pick and place mobile objects.
5. Results: grasping mobile objects Among tasks to which robots are devoted, there are those involving grasping mobile objects. It is the case, for example, when transferring objects from a moving belt to a loading palette. In general, grasp and placement sites are fixed and perfectly defined. The transfer cycle is divided into two phases. The approach phase (phase A) corresponds to the robot motion between the initial posture to an unknown grasping position. The removal phase (phase R) corresponds to moving the object to its final placement. The optimal motion problem aims here to optimize the dynamics of the grasp and the transfer operation by determining the optimal place and the optimal grasp time without interrupting the dynamic of the robot. Phase A, governed by relation (1a), leads to a dynamic problem with an initial posture entirely defined and a final posture characterized by local constraints at grasping time. Phase R, governed by relation (1b), corresponds to a dynamic problem with initial and final states entirely defined. The particularity here is the phase A final posture Pf which must satisfy some conditions at the grasp time tA . They are defined by the following relationships: posture constraint:
φp (tA ) = 0,
(4a)
velocity constraint:
φV (tA ) = 0.
(4b)
The first constraint indicates that, at t = tA , the position of robot end effector’s must be the same as that of the target object. Furthermore, its orientation must meet some desired values depending on the shape and nature of the manipulated object. The second constraint is imposed in order to make the operational velocity of the end effector the same as that of the target object. Example of pick operation of a mobile object using a planar redundant robot We consider here a redundant planar robot constituted of three links connected by revolute joints. The corresponding characteristics are listed in Appendix C. The robot is asked to grasp a mobile object. At time t = 0, the object moving on the belt, starts from the position (x0 = 1.2 m, y0 = 2.5 m) with a constant velocity (vx = −0.8 m s−1 ; vy = −0.2 m s−1 ). At the same time, the robot begins motion from the initial configuration (0, −π/3, −π/10) with null velocity (Fig. 7(d)). At time t = tA (unknown) the robot must grasp the object at orientation π . Then, it must place the object at the final configuration (π, 0, 0) with null velocity (Fig. 7(e)). Bounds on kinematic and dynamic performances (Appendix D) must be satisfied during all the operation.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 7. Optimal pick and place motion.
Fig. 8. Example of ball modelling of robot and obstacles.
711
712
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 9. Optimal robot motion among static obstacles.
Numerical results are obtained with µ = 0.3, α = 0.5 and using a cubic spline function with four nodes. Fig. 7 shows the evolution of the system during the approach and removal phases. The first is achieved in 2.633 s while the second takes 3.131 s. The bounds imposed on kinematics and dynamics performances are respected. Fig. 7(a)–(c) show the continuity of positions, velocities and torques during the transition from one phase to the other. Hence, the whole operation is done smoothly.
6. Results: motion planning in the presence of obstacles When obstacles are present in the robot work space, motion must be planned in such a way collision is avoided between robot links and obstacles. In consequence, we need to model appropriately obstacles and links, then measure the distance between obstacles and links. For example, here we use a finite number of spheres to cover obstacles and links. Different radii are used to cover different shapes of links and objects (Fig. 8), so that more accurate coverage is possible without increasing complexity. Similar methods are proposed in references (Nearchou and Aspragathos, 1997; Mao and Hsia, 1997; Glass et al., 1995). Such a geometric simplification allows stating explicitly constraints due to the presence of obstacles. We use here the same robot as in Section 5. It is asked to move among three static obstacles disposed in it’s work space. The robot begins at (0, −π/3, −π/10) and stops at (π/2, 0, 0). Boundary velocities are null. The obtained optimal motion is presented in Fig. 9. Numerical results are obtained with µ = 0.2, α = 1 and using a four node cubic spline function. The task is achieved in about 3.695 s and no violation of limit values of robot characteristics is recorded. The robot folds up first the third arm then joins the final posture. This is a natural action otherwise the transfer will be impossible.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
713
7. Conclusion We have presented a trajectory planner to deal with the problem of optimal dynamic free motion planning for robotic arms. It computes minimum cost trajectories taking into account the main constraints imposed on the robot kinematics and dynamics performances. The problem is highly non-linear. This is due, firstly, to the complexity of the robot dynamic model that must be verified during the transfer and secondly to the non-linearity of the cost function and constraints. The optimal motion planning problem is originally an optimal control one. It has been converted into a non-linear optimization problem by parameterezing joint temporal variations via clamped cubic spline functions with nodes uniformly distributed along time scale. The proposed planner uses the powerful SQP optimization method. The optimal motions were obtained by minimizing a cost function containing significant motion parameters such as transfer time, quadratic average of joint torques and consumed power. The cost function was written in a convenient weighted form which allows giving a relative importance for minimizing transfer time, joint efforts or power. Smooth motions were obtained by choosing adequate weighting factors. Through the numerical examples we have shown the great ability of the proposed method to handle various kinds of problems involving redundant robots, obstacles avoidance or grasping mobile objects. However, as any non-linear optimization method, SQP can be attracted by local minima. One way to overcome this problem is to start the process from various initial guesses or to use a stochastic technique to scan the solution space and to approach the global minimum. Also, SQP involves computing the gradient and the Hessian of objective function and constraints. That is, continuity of second order must be ensured. However, physical models sometimes include discontinuous terms (e.g., frictions). In that case only stochastic techniques can be used. In consequence to these observations, we have studied, in parallel to this method, the possibility to use a stochastic technique to overcome previous drawbacks. In this case and according to Fig. 2, the main difference in the new scheme is the numerical technique. Of course, several arrangements have to be done in the problem formulation in order to make it propitious to be solved by a stochastic technique while taking into account frictions and obstacles (Chettibi and Lehtihet, 2002). Encouraging results have been already obtained in the treatment of discontinuous physical models (Rebai, 2002) and handling obstacles (Hentout, 2004). In a future paper, we intend to present a complete comparative study between these two methods and we will show how it is possible to combine them to get better results and to treat more complex problems.
Appendix A. Geometric and inertial parameters of PUMA 560 (Armstrong et al., 1986) Joint N◦
1
α (rad) di (m) ri (m) M (kg) px (m) py (m) pz (m) Ixx (kg m2 ) Iyy (kg m2 ) Izz (kg m2 ) Ixy (kg m2 ) Iyz (kg m2 ) Ixz (kg m2 )
π/2 0 0 – 0 0 0 0 0.35 0 0 0 0
2
3 −π/2 0.0203 0.15005 4.8 −0.0203 −0.0141 0.0700 0.066 0.086 0.0125 0 0 0
0 0.4318 0 17.4 −0.3638 0.006 0.2275 0.13 0.524 0.539 0 0 0
4
5
6
π/2 0 0.4318 0.82 0 0.19 0 1.8e−3 1.3e−3 1.8e−3 0 0 0
−π/2 0 0 0.34 0 0 0 0.3e−3 0.4e−3 0.3e−3 0 0 0
0 0 0 0.09 0 0 0.032 0.15e−3 0.15e−3 0.04e−3 0 0 0
Appendix B. limit performances used for PUMA 560 Joint N◦
1
2
3
4
5
6
qmax (rad) q˙max (rad/s) q¨max (rad/s2 ) ... q max (rad/s3 ) τmax (N m)
π 8 10 30 97.6
3π/4 10 12 40 186.4
3π/4 10 12 40 89.4
π 5 8 20 24.2
3π/4 5 8 20 20.1
3π/4 5 8 20 21.3
714
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Appendix C. Geometric and inertial parameters of the planar three degrees of freedom robot Joint N◦
1
2
3
di (m) M (kg) px (m) Izz (kg m2 )
0.7 7 0.35 0.8
0.5 5 0.5 0.5
0.5 5 0.5 0.5
Appendix D. Limit performances of the planar three degrees of freedom robot Joint N◦
1
2
3
qmax (rad) q˙max (rad/s) q¨max (rad/s 2 ) ... q max (rad/s3 ) τmax (N m)
π 3 8 10 30
3π/4 3 8 15 25
3π/4 3 8 20 25
References Angeles, J., 1997. Fundamentals of Robotic Mechanical Systems. Theory, Methods, and Algorithms. Springer. Armstrong, B., Khatib, O., Burdick, J., 1986. The explicit dynamic model and inertial parameters of PUMA 560 arm. In: IEEE Int. Conf. on Rob. & Aut., San Francisco, pp. 510–518. Barclay, A., Gill, P.E., Rosen, J.B., 1997. SQP Methods and their application to numerical optimal control. Report NA97-3, Department of Mathematics, University of California, San Diego. Bessonnet, G., 1992. Optimisation dynamique des mouvements point à point de robots manipulateurs. Thèse d’état, Université de Poitiers, France. Betts, J.T., 1998. Survey of numerical methods for trajectory optimization. J. Guidance Cont. Dyn. 21 (2), 193–207. Bobrow, J.E., Dubowsky, S., Gibson, J.S., 1985. Time optimal control of robotic manipulators along specified paths. Int. J. Rob. Res. 4 (3), 3–15. Bobrow, J.E., Martin, B.J., Sohl, G., Wang, E.C., Park, F.C., Kim, J., 2001. Optimal robot motions for physical criteria. J. Rob. Syst. 18 (12), 785–795. Chettibi, T., 2000. Contribution à l’exploitation optimale des bras manipulateurs. Mémoire de Magister, EMP, Algiers, Algeria. Chettibi, T., 2001. Optimal motion planning of robotic manipulators. Maghrebine Conference of Electric Engineering, Constantine, Algeria. Chettibi, T., 2002. Research of optimal free motions of manipulators robots by non-linear optimisation. In: Séminaire international de génie mécanique, Oran, Algeria, pp. 317–325. Chettibi, T., Lehtihet, H.E., 2002. A new approach for point to point optimal motion planning problems of robotic manipulators. In: 6th Biennial Conf. on Engineering Syst. Design and Analysis (ASME), Turkey. APM10. Chettibi, T., Yousnadj, A., 2001. Optimal motion planning of robotic manipulators along specified geometric path. Int. Conf. on Productic, Algiers, Algeria. Danes, F., 1998. Critères et contraintes pour la synthèse optimale des mouvements de robots manipulateurs. Application à l’évitement d’obstacles. Thèse d’état, Université de Poitiers. Dombre, E., Khalil, W., 1988. Modélisation et commande des robots, First ed. Hermes. Eltimsahy, A.H., Yang, W.S., 1988. Near minimum time control of robotic manipulator with obstacles in the workspace. CH25551/88/000/IEEE, pp. 358–363. Fletcher, R., 1987. Practical methods of Optimization, Second ed. Wiley. Gaudin, H., Bessonnet, G., 1995. From identification to motion optimisation of a planar manipulator. Robotica 13, 123–132. Glass, K., Colbaugh, R., Lim, D., Seradji, H., 1995. Real time collision avoidance for redundant manipulators. IEEE Trans. Rob. Aut. 11 (3), 448–457. Hentout, A., 2004. Planification de trajectoires sans collision pour les robots manipulateurs. Mémoire de Magister, EMP, Algiers, Algeria. Hull, D.G., 1997. Conversion of optimal control problems into parameter optimization problems. J. Guidance Cont. Dyn. 20 (1), 57–62. Jaques, J., Soltine, E., Yang, H.S., 1989. Improving the efficiency of time-optimal path following algorithms. IEEE Trans. Rob. Aut. 5 (1). Kahn, M.E., Roth, B., 1971. The near minimum time control of open loop articulated kinematic chains. ASME J. Dyn. Syst. Meas. Contr. 11, 164–172. Kang, G.S., McKay, D.N., 1986. Selection of near minimum time geometric paths for robotic manipulators. IEEE Trans. Aut. Contr. AC-31 (6), 501–512.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
715
Lazrak, M., 1996. Nouvelle approche de commande optimale en temps final libre et construction d’algorithmes de commande de systèmes articulés. Thèse d’état, Université de Poitiers, France. Luh, J.Y.S., Lin, C.S., 1981. Optimum path planning for mechanical manipulators. Trans. ASME J. Dyn. Syst. Meas. Contr. 102, 142–151. Luh, J.Y.S., Walker, M.W., 1977. Minimum time along the path for mechanical manipulators. In: IEEE Conf. Decision & Cont., New Orleans, LA, pp. 755–759. Macfarlane, S., Croft, E.A., 2001. Design of jerk bounded trajectories for on-line industrial robot applications. In: Proc. IEEE Int. Conf. Rob. & Aut., pp. 979–984. Mao, Z., Hsia, T.C., 1997. Obstacle avoidance inverse kinematics solution of redundant robots by neural networks. Robotica 15, 3–10. Martin, B.J., Bobrow, J.E., 1997. Minimum effort motions for open chain manipulators with task dependent end-effector constraints. In: Proc. IEEE Int. Conf. Rob. & Aut., Albuquerque, NM, pp. 2044–2049. Martin, B.J., Bobrow, J.E., 1999. Minimum effort motions for open chain manipulators with task dependent end-effector constraints. Int. J. Rob. Res. 18 (2), 213–224. Nearchou, A.C., Aspragathos, N.A., 1997. A genetic path planning for redundant articulated robots. Robotica 15, 213–224. Pfeiffer, F., Rainer, J., 1987. A concept for manipulator trajectory planning. IEEE J. Rob. Aut. Ra-3 (3), 115–123. Piazzi, A., Visioli, A., 1998. Global minimum time trajectory planning of mechanical manipulators using internal analysis. Int. J. Cont. 71 (4), 631–652. Pontryagin, L., Boltianski, V., Gamkrelidze, R., Michtchenko, E., 1965. Théorie mathématique des processus optimaux. Mir. Powell, M.J., 1984. Algorithm for non-linear constraints that use Lagrangian functions. Math. Programming 14, 224–248. Rebai, S., 2002. Contribution à la planification optimale des mouvements libres des robots manipulateurs. Mémoire de Magister, EMP, Algiers, Algeria. Richard, M.J., Dufourn, F., Tarasiewicz, S., 1993. Commande des robots manipulateurs par la programmation dynamique. Mech. Mach. Theory 28 (3), 301–316. Schittowski, K., 1985. NLQPL: a FORTRAN-subroutine solving constrained nonlinear programming problems. Ann. Oper. Res. 5, 485–500. Shiller, Z., Dubowsky, S., 1986. Robot path planning with obstacles, actuators, gripper and payload constraints. Int. J. Rob. Res. 8 (6), 3–18. Shin, G.H., Mckay, D.N., 1986. A dynamic programming approach to trajectory planning of robotic manipulators. IEEE Trans. Aut. Contr. AC31 (6), 450–491. Tan, H.H., Potts, R.B., 1988. Minimum time trajectory planner for the discrete dynamic robot model with dynamic constraints. IEEE J. Rob. Aut. 4 (2), 174–185. Yamamoto, M., Ozaki, H., 1988. Planning of manipulator joint trajectories by an iterative method. Robotica 6, 101–105.
Minimum cost trajectory planning for industrial robots T. Chettibi a,∗ , H.E. Lehtihet a , M. Haddad a , S. Hanchi b a Mechanical Laboratory of Structures, E.M.P., B.E.B., BP 17, 16111, Algiers, Algeria b Mechanical Laboratory of Fluids, E.M.P., B.E.B., BP 17, 16111, Algiers, Algeria
Received 18 August 2003; accepted 24 February 2004 Available online 27 March 2004
Abstract We discuss the problem of minimum cost trajectory planning for robotic manipulators. It consists of linking two points in the operational space while minimizing a cost function, taking into account dynamic equations of motion as well as bounds on joint positions, velocities, jerks and torques. This generic optimal control problem is transformed, via a clamped cubic spline model of joint temporal evolutions, into a non-linear constrained optimization problem which is treated then by the Sequential Quadratic Programming (SQP) method. Applications involving grasping mobile object or obstacle avoidance are shown to illustrate the efficiency of the proposed planner. 2004 Elsevier SAS. All rights reserved. Keywords: Robotic manipulators; Motion planning; Obstacles avoidance; Grasping mobile objects; Non-linear optimization
1. Introduction Due to their great ability of speed and precision and their cost-effectiveness in repetitive tasks, industrial robots have been used for decade in place of human workers in automatic production lines. But these powerful machines are hardly autonomous in the sense that they require preliminary actions, such as calibration or motion planning, to achieve specified tasks. In general, robotic manipulators are used at their limit capacities for obvious reasons of productivity. This leads, however, to quite significant joint torque and velocity magnitudes which can be harmful to the system state. In order to increase the manipulator performances, it is highly desirable to control the system dynamic taking into account technological, geometrical and environmental constraints as well as any other constraints inherent both to the robot design and to the nature of the task to be executed. Since many different ways are possible to perform the same task, this freedom of choice can be exploited judiciously to optimize a given performance criterion. During the past decades, a great deal of attention has been given to the problem of motion planning and control. The complexity of the problem made researchers divide the robot control structure into two levels (Fig. 1) (Dombre and Khalil, 1988; Bessonnet, 1992; Angeles, 1997; Chettibi, 2001): the upper level, called path or trajectory planning, and the lower level, called path tracking or path control. Path control is the process that makes the robot actual position and velocity match some desired values provided to the controller by the trajectory planner. The trajectory planner receives a description of the path from which it computes a time history of desired positions and velocities. Then, the path tracker compensates for any deviation. In many publications dealing with motion planning of robotic manipulators, authors state a variety of problems and suggest a large diversity of solution schemes. Several regrouping can be done according to execution modes, adopted models for the robot behaviour and proposed numerical methods of treatment. * Corresponding author.
E-mail address: [email protected] (T. Chettibi). 0997-7538/$ – see front matter 2004 Elsevier SAS. All rights reserved. doi:10.1016/j.euromechsol.2004.02.006
704
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 1. Synoptic diagram of optimal motion planner for robotic manipulators.
Among the tasks which robot manipulators are devoted to, a first distinction can be made in regards to the desired motion nature. Depending on the robot task, it might be necessary to specify the end effector trajectory in the work space. For example, if the effector’s tool acts without interruption according to a predefined path (gluing, arc welding, laser cutting operations, etc.), the planner (or optimization process) defines optimal tracking modalities of the imposed path. On the other hand, in point to point motions (pick-and-place operations, point to point welding), the end effector is free to move between two extremal positions. In this case, the planner tries to define the optimal trajectory and the corresponding controls. A second distinction can be made according to the type of model used. Earlier works (Kahn and Roth, 1971; Luh and Walker, 1977; Luh and Lin, 1981) use kinematic models where the imposed trajectory is geometrically defined in the work space in such a way that the manipulator avoids existing obstacles. The main preoccupation being obstacle avoidance, the optimization problem is synthesized using the inter-penetration distance between elements in collision. Of course, this type of trajectory planning can produce very high execution velocities, particularly when transfer time is minimized. It induces also excessive torque amplitudes which can be harmful for the system state. For these reasons, dynamic models were later incorporated for a more realistic control of the robot dynamic behavior (Bobrow et al., 1985; Pfeiffer and Rainer, 1987; Eltimsahy and Yang, 1988; Yamamoto and Ozaki, 1988; Jaques et al., 1989; Bessonnet, 1992; Chettibi, 2000). However, the problem of motion planning becomes quite complex and requires specific schemes for its treatment. Fig. 2 shows examples of such schemes, some of which are widely used. Two main families can be distinguished: direct and indirect methods (Hull, 1997; Betts, 1998). Indirect methods are based, in particular, on Pontryagin Maximum Principle (PMP) (Pontryagin et al., 1965) which was first used to define the optimal control. The corresponding states may be obtained by another method. For example, the phase plane method is among early techniques taking into account the robot dynamic model and bounds on joint torques (Bobrow et al., 1985; Kang and McKay, 1986; Shin and Mckay, 1986; Pfeiffer and Rainer, 1987; Jaques et al., 1989). It was first used to solve minimum time motion problems along specified paths. Then, it was extended to handle free motions as well (Shiller and Dubowsky, 1986). In this minimum time trajectory planner, it is assumed that the desired path is given in a parameterized form (e.g., using curvilinear abscissa) which is substituted into the manipulator dynamic equations to give a set of second order differential equations in terms of the path parameter. Consequently, bounds on individual joint torques are converted into bounds on parametric accelerations. The allowable sets of second derivatives (one set per joint) are intersected to give a single allowable set. Using the fact that the minimum time solution will be bang–bang in the acceleration, it is possible to construct phase plane plots (Bobrow et al., 1985; Shin and Mckay, 1986; Pfeiffer and Rainer, 1987; Chettibi and Yousnadj, 2001) which give the optimal trajectory in terms of the trajectory parameter and its derivatives. Despite the fact that this technique is elegant and solves the optimal time motion planning problem, the discontinuous nature (bang-bang) of joint torques thus obtained creates many practical problems. In fact, the controller must work in saturation for long periods. Hence, the optimal control leaves no control authority to compensate for tracking errors caused by either unmodelled dynamic or delays introduced by the on-line feedback controller. In addition, this method only takes into account constraints that can be formulated in terms of the trajectory parameter and its time derivatives. Therefore, it spans a small field of possible constraints.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
705
Fig. 2. Methods used for solving optimal free motions planning problem.
PMP is used also to treat directly the optimal dynamic motion planning problem (Bessonnet, 1992; Gaudin and Bessonnet, 1995; Lazrak, 1996; Danes, 1998). The optimality conditions for transfer modalities are expressed as a set of differential equations (Pontryagin et al., 1965). This boundary values problem is solved by specific numerical techniques such as shooting or relaxation methods (Hull, 1997; Betts, 1998). Although this approach has been used successfully in many applications, it tends to be abandoned in recent years. The primary reasons are that the convergence domain is relatively small and that it is difficult to incorporate inequality terms in path constraints. Direct methods were introduced to reduce somewhat the problem complexity. They are based on a discretisation of dynamic variables (states, controls) leading to a parameter optimization problem. Then, non-linear optimization (Richard et al., 1993; Martin and Bobrow, 1997, 1999; Chettibi, 2000, 2002; Bobrow et al., 2001), evolutionary (Nearchou and Aspragathos, 1997; Mao and Hsia, 1997) or classical stochastic (Chettibi and Lehtihet, 2002) techniques are applied to obtain optimal values of the parameters. A common characteristic of these methods is to try to solve directly the problem by adjusting parameters to minimize some cost function while taking into account imposed constraints. Other ways of discretisation can be found (Tan and Potts, 1988; Richard et al., 1993; Chettibi, 2000). However, these techniques suffer from numerical explosion when treating high dimension problems. In the present paper, a different direct method is used for optimal free motion planning problems. The proposed scheme is shown in highlighted arrows in Fig. 2. The basic idea is to parameterize directly the joint evolution vector q(t). The optimal control problem becomes a parametric constrained optimization problem which is to be solved for the unknown transfer time T and the unknown parameters of the chosen model for q(t). Several choices are made here that can be summarized as follows. First, we use for q(t) a convenient clamped cubic spline model with free nodes uniformly distributed along the time scale. Second, we use the SQP method to solve the optimization problem with the unknowns being now the position of spline nodes as well as the transfer time T . Also, the cost function F , which can take many forms, is chosen as a weighted balance of transfer time, mean average of actuators, power, etc. Furthermore, in order to make the resulting motion physically feasible, F is minimised under position, velocity, acceleration, jerk and torque constraints. The problem is formulated in such a way that the same approach can be easily extended to handle
706
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
complex situations. Three examples are treated here: the first one concerns a pick-and-place operation with a six d.o.f robot; the second one deals with a grasping operation of a moving object by a planar redundant robot. The last example takes into account the presence of obstacles in the workspace.
2. Problem statement Let us consider a n d.o.f serial manipulator robot. Motion equations for this system can be derived using Lagrange’s equations or Newton–Euler formalism (Dombre and Khalil, 1988; Angeles, 1997). They are simply given by: M(q)q¨ + Q(q, q) ˙ + G(q) = τ,
(1a)
where M is the inertia matrix, Q is the vector of centrifugal and Coriolis forces in which joints velocities appear under a quadratic form, G is the vector of gravitational forces and τ ∈ n is the vector of actuators efforts. In the presence of external forces Fext applied on the end effector, relation (1a) becomes: M(q)q¨ + Q(q, q) ˙ + G(q) + J T (q)Fext = τ,
(1b)
where J T is the transpose of the robot Jacobian matrix. The robot is required to move freely (without following a specified path) from an initial to a final state, P0 and Pf , respectively. The problem is to find, in addition to actuators efforts vector τ (t) and final time T , the joint time evolution vector q(t) that matches initial and final states, satisfies given constraints and minimizes a given cost function F . 2.1. Constraints Constraints represent physical limitations imposed on the robot kinematic and dynamic performances. They are given by the following inequalities: τi (t) τ max , i = 1, . . . , n, (2a) Joint torques: i ... ...max q q (2b) Joint jerks: i (t) i , i = 1, . . . , n, max (2c) Joint accelerations: q¨i (t) q¨i , i = 1, . . . , n, q˙i (t) q˙ max , i = 1, . . . , n, (2d) Joint velocities: i qi (t) q max , i = 1, . . . , n. (2e) Joint positions: i Of course, non-symmetrical bounds on the above physical quantities can be handled without any difficulties. Relations (2a–e) traduce the fact that not all motions are tolerable, that power resources are limited and must be used rationally in such a way that the robot dynamic behavior is correctly controlled. Jerk constraints are introduced due to the fact that joint position tracking errors increase when jerk increases, and also to limit excessive wear on the robot so that the robot life-span is extended (Piazzi and Visioli, 1998; Macfarlane and Croft, 2001). 2.2. Objective function The motion quality will depend strongly on the expression adopted for the objective function F . In fact, F represents the transfer cost between initial and final postures. Hence, it would be judicious to include significant physical parameters related directly to the global behavior of the robot and to the productivity of the industrial process. The following general expression is a balance between transfer time T and quadratic average of both actuator efforts A and corresponding power B: (1 − µ) αA + (1 − α) B , (3a) F = µT + 2 where 2 τi dt, max τi 0 i=1 2 T n q˙i τi dt, B= q˙imax τimax 0 i=1
A=
T n
A and B have the dimension of a time.
(3b)
(3c)
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
707
µ and α are weighting coefficients (0 µ 1, 0 α 1). The case µ = 1 corresponds to the optimal time motion problem. There are many situations where it is desirable and appropriate to give more importance to some links relatively to the others. In that case, we could just define additional normalized weighting coefficients wi (i = 1, . . . , n) inside expressions (3b) and (3c).
3. Optimal control via direct parameter optimization of joint positions The planning of optimal dynamic motion is a generic optimal control problem. It is transformed here into a parametric optimization problem as follows: joint variables are expressed as a function of a set of parameters. These parameters are varied inside a non-linear optimization program until an optimum satisfying constraints has been reached. Joint trajectories are represented here by clamped cubic spline functions with nodes uniformly distributed along time scale (Fig. 3). This choice can be justified by the well-known characteristics of cubic spline functions; continuity of second order is guarantied and their lower order greatly limits oscillations and enables us to compute easily extremum values between two consecutive nodes. By parameterizing the joint trajectories, variables of the problem become the transfer time and the position of cubic spline nodes. Constraints that must be handled are those defined by relations (2a–e). In constrained optimization, the general aim is to transform the problem into an easier sub-problem that can be then solved. It is used as the basis of an iterative process (Powell, 1984; Fletcher, 1987). A common characteristic of a large class of earlier methods is the translation (using a penalty function) of the constrained problem to a basic unconstrained one. A sequence of parameterized unconstrained optimizations is performed which may converge to the solution of the constrained problem. These methods are now considered relatively inefficient and have been overwhelmed by methods based on the resolution of the Kuhn– Tucker (KT) equations which are the necessary conditions of optimality for a constrained optimization problem (Barclay et al., 1997). The solution of the KT equations forms the basis to many non-linear programming algorithms. These methods attempt to compute directly the Lagrange multipliers. Constrained quasi-Newton methods guarantee superlinear convergence by accumulating second order information regarding the KT equations using a quasi-Newton updating schedule. These methods are commonly referred to as Sequential Quadratic Programming (SQP) and represent the state-of-the-art in non-linear programming methods (Powell, 1984; Schittowski, 1985; Fletcher, 1987). For example, (Schittowski, 1985), has implemented and tested a version that outperforms, over a large number of benchmark problems, every other tested method in terms of efficiency, accuracy and frequency of successful convergences. This method is applied here to solve our new parametric optimization problem. (Martin and Bobrow, 1997, 1999; Bobrow et al., 2001) have already made use of SQP to solve a parametric optimization problem. But, instead of clamped cubic splines these authors have used B-splines to model joint variables. Also, their cost function corresponds to the special case (µ = 0, α = 1), that is a minimum-effort problem. However, no bounds equivalent to inequalities (2a–e) are given. In addition, in contrast to (3a) their cost function does not include normalizing factors such as τimax , i = 1, . . . , n. In many practical cases, these factors as well as bounds (2a–e) are in fact necessary. Otherwise, the optimization process may lead either to a solution that is not physically feasible (violation of a technological constraint) or to a biased solution in the case of a problem involving a large difference in torque magnitudes from one actuator to the other.
Fig. 3. Cubic spline approximation of joint position temporal evolution.
708
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
4. Results: pick and place operations We present a simulation study of the standard six d.o.f. PUMA 560 robot (Fig. 4) whose geometric and inertial arm parameters are listed in Appendix A according to those reported in Armstrong et al. (1986). The robot is asked to carry out transfer between two configurations defined in Table 1. Initial and final joint velocities are null. The transfer must be done without violating bounds on kinematic and dynamic performances (given in Appendix B). Computations are carried out for various cases using a cubic spline function with four nodes. Solutions are obtained in few minutes on a 1.6 GHz PC. Table 2 summarizes numerical results for sample values of weighting coefficients. Fig. 5 shows an example of profiles of joints positions, velocities, accelerations, jerks and torques as well as a 3D representation of the corresponding motion. Graphical results are given in the following units: position (rad), velocity (rad/s), acceleration (rad/s2 ), jerk (rad/s3 ), and torque (N m). The time scale is given in seconds. Transfer between initial and final postures is executed in a smooth way and, of course, without violating imposed constraints. Varying the weighting coefficients leads to different types of motions. In general, for fixed α, increasing µ leads to lower transfer time and higher torques and velocities. On the other hand, for fixed µ, increasing α leads to higher transfer time and lower torques and velocities. Hence, the power term in the objective function acts as a regulator for torques and velocities profiles.
Fig. 4. PUMA 560 robot shown here in its zero angle pose.
Table 1 Configurations to be linked
Table 2 Numerical results
Joint N ◦
Initial
Final
µ
α
Cost function
Transfer time
A
B
1 2 3 4 5 6
0 −π/6 0 −π/3 0 0
2π/3 π/6 π/4 π/3 −π/4 π/6
1 0.75 0.75 0.5 0.5 0.5 0.25 0.25 0.25
N/A 0.25 1 0 0.5 1 0.5 0.75 1
1.159 1.376 1.404 0.608 0.944 1.370 0.763 0.752 1.167
1.159 1.819 1.845 1.159 1.934 2.675 2.865 2.788 4.465
0.200 0.172 0.171 0.199 0.156 0.147 0.165 0.174 0.136
0.115 0.075 0.072 0.113 0.066 0.061 0.078 0.061 0.078
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 5. Optimal motion obtained for (µ = 0.5, α = 1).
709
710
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 6. To pick and place mobile objects.
5. Results: grasping mobile objects Among tasks to which robots are devoted, there are those involving grasping mobile objects. It is the case, for example, when transferring objects from a moving belt to a loading palette. In general, grasp and placement sites are fixed and perfectly defined. The transfer cycle is divided into two phases. The approach phase (phase A) corresponds to the robot motion between the initial posture to an unknown grasping position. The removal phase (phase R) corresponds to moving the object to its final placement. The optimal motion problem aims here to optimize the dynamics of the grasp and the transfer operation by determining the optimal place and the optimal grasp time without interrupting the dynamic of the robot. Phase A, governed by relation (1a), leads to a dynamic problem with an initial posture entirely defined and a final posture characterized by local constraints at grasping time. Phase R, governed by relation (1b), corresponds to a dynamic problem with initial and final states entirely defined. The particularity here is the phase A final posture Pf which must satisfy some conditions at the grasp time tA . They are defined by the following relationships: posture constraint:
φp (tA ) = 0,
(4a)
velocity constraint:
φV (tA ) = 0.
(4b)
The first constraint indicates that, at t = tA , the position of robot end effector’s must be the same as that of the target object. Furthermore, its orientation must meet some desired values depending on the shape and nature of the manipulated object. The second constraint is imposed in order to make the operational velocity of the end effector the same as that of the target object. Example of pick operation of a mobile object using a planar redundant robot We consider here a redundant planar robot constituted of three links connected by revolute joints. The corresponding characteristics are listed in Appendix C. The robot is asked to grasp a mobile object. At time t = 0, the object moving on the belt, starts from the position (x0 = 1.2 m, y0 = 2.5 m) with a constant velocity (vx = −0.8 m s−1 ; vy = −0.2 m s−1 ). At the same time, the robot begins motion from the initial configuration (0, −π/3, −π/10) with null velocity (Fig. 7(d)). At time t = tA (unknown) the robot must grasp the object at orientation π . Then, it must place the object at the final configuration (π, 0, 0) with null velocity (Fig. 7(e)). Bounds on kinematic and dynamic performances (Appendix D) must be satisfied during all the operation.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 7. Optimal pick and place motion.
Fig. 8. Example of ball modelling of robot and obstacles.
711
712
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Fig. 9. Optimal robot motion among static obstacles.
Numerical results are obtained with µ = 0.3, α = 0.5 and using a cubic spline function with four nodes. Fig. 7 shows the evolution of the system during the approach and removal phases. The first is achieved in 2.633 s while the second takes 3.131 s. The bounds imposed on kinematics and dynamics performances are respected. Fig. 7(a)–(c) show the continuity of positions, velocities and torques during the transition from one phase to the other. Hence, the whole operation is done smoothly.
6. Results: motion planning in the presence of obstacles When obstacles are present in the robot work space, motion must be planned in such a way collision is avoided between robot links and obstacles. In consequence, we need to model appropriately obstacles and links, then measure the distance between obstacles and links. For example, here we use a finite number of spheres to cover obstacles and links. Different radii are used to cover different shapes of links and objects (Fig. 8), so that more accurate coverage is possible without increasing complexity. Similar methods are proposed in references (Nearchou and Aspragathos, 1997; Mao and Hsia, 1997; Glass et al., 1995). Such a geometric simplification allows stating explicitly constraints due to the presence of obstacles. We use here the same robot as in Section 5. It is asked to move among three static obstacles disposed in it’s work space. The robot begins at (0, −π/3, −π/10) and stops at (π/2, 0, 0). Boundary velocities are null. The obtained optimal motion is presented in Fig. 9. Numerical results are obtained with µ = 0.2, α = 1 and using a four node cubic spline function. The task is achieved in about 3.695 s and no violation of limit values of robot characteristics is recorded. The robot folds up first the third arm then joins the final posture. This is a natural action otherwise the transfer will be impossible.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
713
7. Conclusion We have presented a trajectory planner to deal with the problem of optimal dynamic free motion planning for robotic arms. It computes minimum cost trajectories taking into account the main constraints imposed on the robot kinematics and dynamics performances. The problem is highly non-linear. This is due, firstly, to the complexity of the robot dynamic model that must be verified during the transfer and secondly to the non-linearity of the cost function and constraints. The optimal motion planning problem is originally an optimal control one. It has been converted into a non-linear optimization problem by parameterezing joint temporal variations via clamped cubic spline functions with nodes uniformly distributed along time scale. The proposed planner uses the powerful SQP optimization method. The optimal motions were obtained by minimizing a cost function containing significant motion parameters such as transfer time, quadratic average of joint torques and consumed power. The cost function was written in a convenient weighted form which allows giving a relative importance for minimizing transfer time, joint efforts or power. Smooth motions were obtained by choosing adequate weighting factors. Through the numerical examples we have shown the great ability of the proposed method to handle various kinds of problems involving redundant robots, obstacles avoidance or grasping mobile objects. However, as any non-linear optimization method, SQP can be attracted by local minima. One way to overcome this problem is to start the process from various initial guesses or to use a stochastic technique to scan the solution space and to approach the global minimum. Also, SQP involves computing the gradient and the Hessian of objective function and constraints. That is, continuity of second order must be ensured. However, physical models sometimes include discontinuous terms (e.g., frictions). In that case only stochastic techniques can be used. In consequence to these observations, we have studied, in parallel to this method, the possibility to use a stochastic technique to overcome previous drawbacks. In this case and according to Fig. 2, the main difference in the new scheme is the numerical technique. Of course, several arrangements have to be done in the problem formulation in order to make it propitious to be solved by a stochastic technique while taking into account frictions and obstacles (Chettibi and Lehtihet, 2002). Encouraging results have been already obtained in the treatment of discontinuous physical models (Rebai, 2002) and handling obstacles (Hentout, 2004). In a future paper, we intend to present a complete comparative study between these two methods and we will show how it is possible to combine them to get better results and to treat more complex problems.
Appendix A. Geometric and inertial parameters of PUMA 560 (Armstrong et al., 1986) Joint N◦
1
α (rad) di (m) ri (m) M (kg) px (m) py (m) pz (m) Ixx (kg m2 ) Iyy (kg m2 ) Izz (kg m2 ) Ixy (kg m2 ) Iyz (kg m2 ) Ixz (kg m2 )
π/2 0 0 – 0 0 0 0 0.35 0 0 0 0
2
3 −π/2 0.0203 0.15005 4.8 −0.0203 −0.0141 0.0700 0.066 0.086 0.0125 0 0 0
0 0.4318 0 17.4 −0.3638 0.006 0.2275 0.13 0.524 0.539 0 0 0
4
5
6
π/2 0 0.4318 0.82 0 0.19 0 1.8e−3 1.3e−3 1.8e−3 0 0 0
−π/2 0 0 0.34 0 0 0 0.3e−3 0.4e−3 0.3e−3 0 0 0
0 0 0 0.09 0 0 0.032 0.15e−3 0.15e−3 0.04e−3 0 0 0
Appendix B. limit performances used for PUMA 560 Joint N◦
1
2
3
4
5
6
qmax (rad) q˙max (rad/s) q¨max (rad/s2 ) ... q max (rad/s3 ) τmax (N m)
π 8 10 30 97.6
3π/4 10 12 40 186.4
3π/4 10 12 40 89.4
π 5 8 20 24.2
3π/4 5 8 20 20.1
3π/4 5 8 20 21.3
714
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
Appendix C. Geometric and inertial parameters of the planar three degrees of freedom robot Joint N◦
1
2
3
di (m) M (kg) px (m) Izz (kg m2 )
0.7 7 0.35 0.8
0.5 5 0.5 0.5
0.5 5 0.5 0.5
Appendix D. Limit performances of the planar three degrees of freedom robot Joint N◦
1
2
3
qmax (rad) q˙max (rad/s) q¨max (rad/s 2 ) ... q max (rad/s3 ) τmax (N m)
π 3 8 10 30
3π/4 3 8 15 25
3π/4 3 8 20 25
References Angeles, J., 1997. Fundamentals of Robotic Mechanical Systems. Theory, Methods, and Algorithms. Springer. Armstrong, B., Khatib, O., Burdick, J., 1986. The explicit dynamic model and inertial parameters of PUMA 560 arm. In: IEEE Int. Conf. on Rob. & Aut., San Francisco, pp. 510–518. Barclay, A., Gill, P.E., Rosen, J.B., 1997. SQP Methods and their application to numerical optimal control. Report NA97-3, Department of Mathematics, University of California, San Diego. Bessonnet, G., 1992. Optimisation dynamique des mouvements point à point de robots manipulateurs. Thèse d’état, Université de Poitiers, France. Betts, J.T., 1998. Survey of numerical methods for trajectory optimization. J. Guidance Cont. Dyn. 21 (2), 193–207. Bobrow, J.E., Dubowsky, S., Gibson, J.S., 1985. Time optimal control of robotic manipulators along specified paths. Int. J. Rob. Res. 4 (3), 3–15. Bobrow, J.E., Martin, B.J., Sohl, G., Wang, E.C., Park, F.C., Kim, J., 2001. Optimal robot motions for physical criteria. J. Rob. Syst. 18 (12), 785–795. Chettibi, T., 2000. Contribution à l’exploitation optimale des bras manipulateurs. Mémoire de Magister, EMP, Algiers, Algeria. Chettibi, T., 2001. Optimal motion planning of robotic manipulators. Maghrebine Conference of Electric Engineering, Constantine, Algeria. Chettibi, T., 2002. Research of optimal free motions of manipulators robots by non-linear optimisation. In: Séminaire international de génie mécanique, Oran, Algeria, pp. 317–325. Chettibi, T., Lehtihet, H.E., 2002. A new approach for point to point optimal motion planning problems of robotic manipulators. In: 6th Biennial Conf. on Engineering Syst. Design and Analysis (ASME), Turkey. APM10. Chettibi, T., Yousnadj, A., 2001. Optimal motion planning of robotic manipulators along specified geometric path. Int. Conf. on Productic, Algiers, Algeria. Danes, F., 1998. Critères et contraintes pour la synthèse optimale des mouvements de robots manipulateurs. Application à l’évitement d’obstacles. Thèse d’état, Université de Poitiers. Dombre, E., Khalil, W., 1988. Modélisation et commande des robots, First ed. Hermes. Eltimsahy, A.H., Yang, W.S., 1988. Near minimum time control of robotic manipulator with obstacles in the workspace. CH25551/88/000/IEEE, pp. 358–363. Fletcher, R., 1987. Practical methods of Optimization, Second ed. Wiley. Gaudin, H., Bessonnet, G., 1995. From identification to motion optimisation of a planar manipulator. Robotica 13, 123–132. Glass, K., Colbaugh, R., Lim, D., Seradji, H., 1995. Real time collision avoidance for redundant manipulators. IEEE Trans. Rob. Aut. 11 (3), 448–457. Hentout, A., 2004. Planification de trajectoires sans collision pour les robots manipulateurs. Mémoire de Magister, EMP, Algiers, Algeria. Hull, D.G., 1997. Conversion of optimal control problems into parameter optimization problems. J. Guidance Cont. Dyn. 20 (1), 57–62. Jaques, J., Soltine, E., Yang, H.S., 1989. Improving the efficiency of time-optimal path following algorithms. IEEE Trans. Rob. Aut. 5 (1). Kahn, M.E., Roth, B., 1971. The near minimum time control of open loop articulated kinematic chains. ASME J. Dyn. Syst. Meas. Contr. 11, 164–172. Kang, G.S., McKay, D.N., 1986. Selection of near minimum time geometric paths for robotic manipulators. IEEE Trans. Aut. Contr. AC-31 (6), 501–512.
T. Chettibi et al. / European Journal of Mechanics A/Solids 23 (2004) 703–715
715
Lazrak, M., 1996. Nouvelle approche de commande optimale en temps final libre et construction d’algorithmes de commande de systèmes articulés. Thèse d’état, Université de Poitiers, France. Luh, J.Y.S., Lin, C.S., 1981. Optimum path planning for mechanical manipulators. Trans. ASME J. Dyn. Syst. Meas. Contr. 102, 142–151. Luh, J.Y.S., Walker, M.W., 1977. Minimum time along the path for mechanical manipulators. In: IEEE Conf. Decision & Cont., New Orleans, LA, pp. 755–759. Macfarlane, S., Croft, E.A., 2001. Design of jerk bounded trajectories for on-line industrial robot applications. In: Proc. IEEE Int. Conf. Rob. & Aut., pp. 979–984. Mao, Z., Hsia, T.C., 1997. Obstacle avoidance inverse kinematics solution of redundant robots by neural networks. Robotica 15, 3–10. Martin, B.J., Bobrow, J.E., 1997. Minimum effort motions for open chain manipulators with task dependent end-effector constraints. In: Proc. IEEE Int. Conf. Rob. & Aut., Albuquerque, NM, pp. 2044–2049. Martin, B.J., Bobrow, J.E., 1999. Minimum effort motions for open chain manipulators with task dependent end-effector constraints. Int. J. Rob. Res. 18 (2), 213–224. Nearchou, A.C., Aspragathos, N.A., 1997. A genetic path planning for redundant articulated robots. Robotica 15, 213–224. Pfeiffer, F., Rainer, J., 1987. A concept for manipulator trajectory planning. IEEE J. Rob. Aut. Ra-3 (3), 115–123. Piazzi, A., Visioli, A., 1998. Global minimum time trajectory planning of mechanical manipulators using internal analysis. Int. J. Cont. 71 (4), 631–652. Pontryagin, L., Boltianski, V., Gamkrelidze, R., Michtchenko, E., 1965. Théorie mathématique des processus optimaux. Mir. Powell, M.J., 1984. Algorithm for non-linear constraints that use Lagrangian functions. Math. Programming 14, 224–248. Rebai, S., 2002. Contribution à la planification optimale des mouvements libres des robots manipulateurs. Mémoire de Magister, EMP, Algiers, Algeria. Richard, M.J., Dufourn, F., Tarasiewicz, S., 1993. Commande des robots manipulateurs par la programmation dynamique. Mech. Mach. Theory 28 (3), 301–316. Schittowski, K., 1985. NLQPL: a FORTRAN-subroutine solving constrained nonlinear programming problems. Ann. Oper. Res. 5, 485–500. Shiller, Z., Dubowsky, S., 1986. Robot path planning with obstacles, actuators, gripper and payload constraints. Int. J. Rob. Res. 8 (6), 3–18. Shin, G.H., Mckay, D.N., 1986. A dynamic programming approach to trajectory planning of robotic manipulators. IEEE Trans. Aut. Contr. AC31 (6), 450–491. Tan, H.H., Potts, R.B., 1988. Minimum time trajectory planner for the discrete dynamic robot model with dynamic constraints. IEEE J. Rob. Aut. 4 (2), 174–185. Yamamoto, M., Ozaki, H., 1988. Planning of manipulator joint trajectories by an iterative method. Robotica 6, 101–105.