Dynamic optimization for the trajectory planning of robot manipulators in the presence of obstacles

Dynamic Optimization for the Trajectory Planning of Robot Manipulators in the Presence of Obstacles

Sezimária F. P. Saramago

Valder Steffen Júnior

Universidade Federal de Uberlândia

Departamento de Matemática

Departamento de Engenharia Mecânica

Campus Santa Mônica - 38400-089

Uberlândia, MG Brazil

Abstract

This paper presents an approach to the solution of moving a robot manipulator with minimum cost along a specified geometric path in the presence of obstacles. The main idea is to express obstacle avoidance in terms of the distances between potentially colliding parts. The optimal traveling time and the minimum mechanical energy of the actuators are considered together to build a multiobjective function. A simple numerical example involving a Cartesian manipulator arm with two-degree-of-freedom is described.

Keywords: Robot Manipulator, Trajectory Planning, Obstacle avoidance.

Introduction

A large number of robotic applications such as inspection, welding, painting, assembling, and moving objects, involve repetitive processes. This technological characteristic justifies off-line trajectory planning. It is also very common that the robots may have obstacles in their workspace due to the presence of industrial equipment or other robots.

This paper considers a solution to the problem of moving a robot manipulator with minimum cost along a specified geometric path in the presence of obstacles (Gilbert and Johnson,1985). The optimal traveling time and the minimum mechanical energy of the actuators are considered together to build a multiobjective function and the results depend on the associated weighting factors (Richard et al.,1993).

The optimization problem is subject to physical constraints, input torque/force constraints and obstacle avoidance. The distances between potentially colliding parts are understood as obstacle avoidance. The developed methodology is applicable to very general mechanisms including tridimensional workspaces, but a heavy computation work is required.

The robot model is based on the parameters of Denavit-Hartenberg. The dynamic model for the manipulator is obtained by writing the Euler-Lagrange’s equation using the Lagrange’s energy function. The resulting equations describe the motion in terms of the joint variables and the structural parameters of the manipulator. The mathematical model takes into account the manipulator nonlinear coupled equations of motion.

Sequential Unconstrained Minimization Techniques (SUMT) have been used for the optimization. For this purpose the pseudo-objective function is written using the Augmented Lagrange Multiplier Method. The trajectory for robot manipulators is obtained through off-line computation using spline functions (Steffen and Saramago, 1996). Polynomial functions are chosen to describe the spline functions and the path planning is done in the joint coordinates. Numerical results are presented to illustrate the developed methodology.

Formulation of the Problem – a Review

In a previous research work the authors developed a general methodology to study the optimization of the trajectory planning of robot manipulators (see Saramago and Steffen, 1998). In that paper the dynamics of the system was taken into-account and the procedure is now revisited adding special constraints to represent obstacles avoidance.

Let a manipulator with N d.o.f., considers that j represents the joints and m represents the knots used to construct the trajectories. The task is to move the robot in the workspace avoiding the obstacles Kl, while minimizing the traveling time and the energy consumed in the move, subject to physical constraints and actuator limits.

The optimum traveling time for the robot corresponds to the minimization of a set of time intervals

.This technique leads to the maximization of the operation speed. The N joints of the robot must be considered simultaneously.

The optimal traveling time and the minimum mechanical energy of the actuators are considered together to build a performance index such as the optimization problem is defined as follows:

Minimize:

(1)

where,h_j e R^m are the design variables (time intervals);

- generalized velocity;

u_i - generalized forces;

a₁ e a₂ - weighting factors.

Subject to:

for j=1,2,...,N and i=1,2,...,m-1 (2)

for j=1,2,...,N and i=1,2,...,m-1 (3)

for j=1,2,...,N and i=1,2,...,m-1 (4)

for j=1,2,...,N and i=1,2,...,m-1 (5)

for (l, q) e I (6)

h_i^(L)£ h_i £hi^(U) for i=1,2,...,m-1 (7)

where VC_j is the velocity constraint, WC_j is the acceleration constraint, JC_j is the jerk constraint, and UC_j is the force/torque constraint for joint j. The obstacle avoidance is given by d_lq(t)r_oⁱ, where I represents the set of possibly colliding pairs of parts. Eq. (7) represents the side constraints.

Kinematic and Dynamic Model

Consider q_i, d_i, a_i, ai as the link parameters of Denavit-Hartenberg for the robot manipulator. A homogeneous transformation matrix (Paul, 1982) describing the relative translation and rotation between the link i coordinate system and the link i-1coordinate system is:

(8)

Given the transformation matrix, the kinematic motion of a robot manipulator with n degrees-of-freedom (d.o.f.) is completely described by

(9)

The Lagrangian function L is defined as the difference between the kinetic energy K and the potential energy P of the system :

L= K - P(10)

The dynamic equations for conservative systems in terms of the Lagrangian function are given by:

(11)

where,

q_i - generalized coordinates of the system (q_ifor rotational joints and d_ifor prismatic joints);

- generalized velocities (angular velocity for rotational joints and linear velocity for prismatic joints);

u_i - generalized forces

Figure 1 represents a point r_i described with respect to link i. In base coordinates position r_oⁱ is:

(12)

It's velocity is:

(13)

Where,

(14)

A simple notation defined by Alves (1988) is used to write the derivative terms of the transformation matrix in a compact form:

(15)

thus,

(16)

This way it can be written:

(17)

and the kinetic energy of link i is:

(18)

The integral is known as the inertia matrix J_i. Given the moments of inertia and the products of inertia, J_i can be expressed as:

(19)

and the total kinetic energy is:

(20)

The potential energy of a link with its center of mass described by a vector is represented by:

(21)

where, is the acceleration due to gravity with respect to the base coordinate system. The total potential energy of the manipulator is:

(22)

From Eqs. (20) and (22), the Lagrangian L= K - P is:

(23)

thus,

(24)

and

(25)

Using Eq. (11):

(26)

that can be rewritten as:

(27)

where:

(28)

(29)

(30)

D_ij is the inertial system matrix, C_ijkis the Coriolis and centripetal forces matrix and G_iis the gravity loading vector. The generalized forces u_i calculated by Eq. (27) will be used in the optimization problem (1) and in the force/torque constraint equations given by Eq. (5).

Formulation of Trajectories

The robot model is based on the parameters of Denavit - Hartemberg. Figure 2 represents the gripper of a manipulator.

The position and orientation of the gripper can be represented by the following matrix:

(31)

where p is the position vector of the hand; v_n, v_o, and v_a are the unit normal, slide and approach vectors, respectively. Joint values corresponding to H(t) can be solved using the kinematic transformation equation. To construct the joint trajectories, for manipulator with N d.o.f., m knots are first transformed into joint vectors ,, , where q_ji is the displacement of joint j at knot i corresponding to. Cubic polynomials are chosen to describe the trajectories (Steffen and Saramago, 1996). The trajectory is constructed for each joint in such a way that it fits the sequence . As the same procedure is repeated for each joint the joint number j is neglected.

Let be an ordered time sequence. Let v₁ and v_m,a₁ and a₁ , be joint velocities and joint accelerations, respectively, at the initial time t₁ and terminal time t_m.

A cubic polynomial function Q_i(t) is defined on the time interval . It is assumed that the displacement, velocity and acceleration are continuous on this time interval. The second time derivative W_i(t) can be written as:

(32)

where, h_i = t_i+1-t_i

Integrating Eq. (32) for the given points Q_i(t_i=q_i e Q(t_i+1)=q_i+1the following interpolating functions are obtained:

(33)

and

(34)

Two extra knots q₂and q_m-1 are not fixed and are used to add two new equations to the system in such a way that it can be solved. The joint displacements of these two knots are written as

(35)

(36)

Using the continuity condition on velocities and accelerations, a system of (m-2) linear equations solving for (m-2) unknowns W_i(t_i), i=2,3,...,m-1 is obtained as Lin,1983 :

(37)

where,

(38)

where,

, (39)

and, matrix B given by

(40)

The solution uniqueness of Eq. (37) is guaranteed once that matrix A is nonsingular. Thus, adopting the time interval h_i, the acceleration is obtained solving the system (37), the velocity is given by Eq. (33) and the displacement is obtained using Eq. (34). The velocity will be further used in the optimization problem (1).

Constraints Formulation

To calculate the velocity constraints it is considered that the maximum absolute value of velocity exists at the extreme points t_i and t_i+1 or at t_i where W_ji(t_i)=0 in this interval. The velocity is calculated using Eq. (33). The velocity constraints become

j=1,2,...,N and i=1,2,...,m-1 (41)

The acceleration is the solution of system (37).To calculate the acceleration constraints it is considered that they are linear functions of time and the maximum absolute value exists at either t_i or t_i+1:

, j=1,2,...,N (42)

As the jerk is the rate of change of acceleration the corresponding constraints are represented by

, j=1,2,...,N and i=1,2,...,m-1 (43)

The Eqs. (41) to (43) will be used such as constraints (Eqs. (2) to (4)) of the optimal control problem.

To this problem we add special constraints corresponding to obstacle avoidance. These constraints require

for (l, q) e I (44)

where K_l(t) are sets in R² or R³ and Æ is the empty set. Let

(45)

The sets K_l(t) describe the space occupied by the parts of the manipulator(s) and the working environment. The set of index pairs I corresponds to potentially colliding pairs of parts, and (44) states that they shall not collide. The sets C₁ characterize the shape of the rigid parts, while T_l(t) and p_l(t) describe the rotation and translation of the parts. Note that if l denotes a "part" of the fixed environment, K_l is not dependent on time, then K_l=C_l.

It is convenient to describe the requirement (44) by the distance between the sets:

(46)

for z_leK_l(t), z_qe K_q(t)

To make sure (44) holds with some margin of error, the following conditions are imposed:

for (l, q) e I (47)

where represents a given tolerance.

The properties of d_lq depend on the properties of C_l or C_q. Gilbert (1985) proved that, if either C_l or C_q is strictly convex, then d_lq is continuously differentiable on the domain.

Equation (47) can be used to represent the obstacle avoidance given by Eq. (6) in the optimal control problem.

Initialization of the optimization process

Let h be defined as the vector of design variables . To initialize the optimization process it is considered that:

(48)

As two extra knots are needed they are initially taken as

(49)

Remark: The optimization procedure is such that infeasible projects are automatically avoided.

Optimization strategy

The program DOT- Design Optimization Tools Program developed by Vanderplaats (1995) was used to perform the sequential optimization method. For this purpose the pseudo-objective function is written using the Augmented Lagrange Multiplier Method. The unconstrained minimization is performed by the Davidon-Fletcher-Powell (Vanderplaats, 1984) method utilizing a combination of the Golden Section and polynomial one-dimensional search procedures.

The optimization program had to be coupled to the robot trajectory analysis program in order to obtain an automated design procedure.

For optimization purposes a general analysis code developed by the authors (involving the dynamic model and the trajectory planning of the robot) was coupled with the non-linear optimization program DOT as shown in Fig. 3.

Numerical Application

The numerical example considers a Cartesian Robot (PP) with two prismatic joints represented in Fig. 4.The Denavit-Hartemberg parameters are given in Table 1.

Knots from the Cartesian path of the hand are given in Table 2 and it’s dynamic characteristics are given in Table 3

The robot is at rest initially, and comes to a full stop at the end of the trajectory. The velocity, acceleration, jerk and force constraints are given in Table 4. The optimization results are given in Table 5.

The graphic results of the joint trajectories Q(t), velocities V(t), and accelerations W(t) for each joint corresponding to the optimal solution for a₁=a₂=0.5 are shown in Fig. 5.

The task is to move along the trajectory K1, avoiding the obstacles K2, K3, K4 and K5. The nature of the obstacle constraints is such that, in general, several local minimal exist which can be easily observed by the numerical procedure. However these results show that significant reductions are obtained for the objective function and the trajectories come closer to the obstacles.

Conclusion

A large class of path planning of robot manipulators has been formulated in terms of an optimal-control problem. To perform the optimization of the system a performance index was defined combining the total traveling time of the gripper of the robot and the mechanical energy involved in the system operation. A non-linear optimization problem was defined taking into account kinematic and dynamic constraints. Special constraints that ensure obstacle avoidance were also included. These obstacle constraints are expressed in terms of the distance between potentially colliding parts. This modeling of colliding parts has been presented and has proven to be successful for a Cartesian manipulator in a two-dimensional workspace. The problem formulation and the approach to its solution, as presented, are directly applicable to tridimensional workspaces.

Presented at DINAME 97 - 7th International Conference on Dynamic Problems in Mechanics, 3 - 7 March 1997, Angra dos Reis, RJ, Brazil. Technical Editor: Agenor de Toledo Fleury.

• Alves, J. B. M., 1988, "Controle de Robô ", Câmara Brasileira do Livro, São Paulo, Brasil, pp 80-82.
• Gilbert, E.G., Johnson, D.W., 1985, " Distance Functions and Their Application to Robot Path Planning in the Presence of Obstacles", IEEE Journal of Robotics and Automation, Vol. RA-1, No. 1, March, pp. 21-30.
• Lin, C.S., et al., 1983, " Formulation and Optimization of Cubic Polynomial Joint Trajectories for Industrial Robots", IEEE Trans. Automat. Contr., vol. AC-28, Dec, pp. 1066-1073.
• Paul, R. P., 1982, "Robot Manipulators: Mathematics, Programming and Control"; MIT Press.
• Richard, M.C., et al., 1993, "Commande des Robots Manipulateurs par la Programation Dynamique"; Mech. Mach. Theory, Vol. 28, no. 3, pp. 301- 316.
• Steffen, Jr.V., Saramago, S.F.P, 1997, "Optimization Techniques for Off - line Trajectories Planning of Robot Manipulators ", In Non - linear Dynamics, Chaos, Control and their Applications in Engineering Sciences, publishing by AAM- ABCM, ISBN: 85-900351-1-5,Vol. 1, pp.363-368.
• Saramago, S. F. P, Steffen, Jr.V., 1998, "Optimization of the Trajectory Planning of Robot Manipulators Taking Into-Account The Dynamics of the System", Mechanism and Machine Theory, Vol. 33, No. 7, pp. 883-894.
• Vanderplaats, G., 1984, "Numerical Optimization Techniques for Engineering Design"; McGraw-Hill, USA.
• Vanderplaats, G., 1995, " DOT - Design Optimization Tools Program", Vanderplaats Research & Development, Inc, Colorado Springs.

Publication Dates

• Publication in this collection
  20 Nov 2002
• Date of issue
  Sept 1999