Tracking Error Based On-line Trajectory Time Scaling

Tracking Error Based On-line Trajectory Time Scaling Emese Sz´adeczky-Kardoss

B´alint Kiss

Dept. of Control Engineering and Information Technology Dept. of Control Engineering and Information Technology Budapest University of Technology and Economics Budapest University of Technology and Economics Magyar Tud´osok krt. 2, H-1117, Budapest Magyar Tud´osok krt. 2, H-1117, Budapest Hungary Hungary [email protected] [email protected]

such that sufficient margin is left for the feedback controller. Our concept uses the tracking error instead of the torque bounds in order to modify the time scaling of the reference path. This method changes the traveling time of the motion according to the values of the tracking error. If the movement is not accurate enough, the robot should move slower, and if the errors are small, higher speed can be achieved. The remaining part of this work is organized as follows. Section II presents a short overview of existing time scaling methods. Our new concept, the time scaling method based on tracking error, is detailed in Section III. Special time scaling functions are presented in Section IV. An algorithm, which uses the new concept and some results of simulations are also shown in Section V. Finally, a short summary concludes the paper.

Abstract – This paper presents an on-line time scaling concept for robotic manipulators. The task is to ensure the fastest possible motion along a trajectory with a given geometry in the robot workspace. The other requirement is tracking accuracy. Robots should move not only with high speed but with high precision as well. The speed of the motion cannot be arbitrary fast. It is delimited by the saturation of the actuators and by some other (e.g. security) constraints. Off-line time scaling solutions change the speed of the motion without changing its geometry, thus these constraints are not violated. If the exact torque bounds are not known in advance, another method should be used. This work gives an on-line time scaling algorithm which is based on the tracking error. Simulations demonstrate the usefulness of the algorithm proposed.

I I NTRODUCTION Nowadays several robots are used for different tasks. The main reason of their application is that they can work faster and more accurately than people. One wishes robots to perform the prescribed tasks with high precision and high speed which are usually conflicting requirements. Fast motions may cause tracking errors, especially if the load is unknown or the path designed off-line for the task is calculated using wrong parameter values. Slow movements are generally more accurate, since they leave more reaction margin for the controllers. Several researchers developed algorithms to find the time-optimal trajectories for robot manipulators. It is an important question, because robot actuators cannot generate unlimited torques or forces. Thus the minimum time has to be found such that the limits on the actuator torques are not violated. These algorithms are usually applied off-line, before the tracking is started. The optimal trajectory, which needs the least time to finish the motion, requires at least one actuator to be on the saturation boundary at a given time. The methods for timeoptimal motion planning [7] find the switching points where the actuator which has operated in its boundary changes the torque or force, and another actuator goes into saturation. Time scaling methods can also be used to find this timeoptimal solution. This calculation is usually also off-line; at the start of the motion the algorithm already determined the optimal path. The problem with these off-line methods is that sufficient torque or force reserve is not always assured for the closed loop control during the tracking. Other algorithms use online trajectory scaling to change the actuator boundaries 1-4244-9708-8/06/$20.00 ©2006 IEEE.

II T IME SCALING METHODS A short summary of the off-line trajectory scaling is given in Subsection II-A. Subsection II-B presents some on-line time scaling methods. A Off-line trajectory scaling The off-line methods perform the time scaling prior to the start of the motion. This concept of time scaling was introduced by John M. Hollerbach in [2]. His method determines whether the tracking of the planned path violates the actuator torque limits. If the constraints are violated, the time scaling algorithm modifies the traveling time along the trajectory such that the actuator constraints are respected along the path. This algorithm modifies the motion speed along the trajectory without the need for complete dynamics recalculations. Let us consider a fully actuated mechanical structure with an open kinematic chain (e.g. a robot manipulator). Suppose that a trajectory planner has produced the time functions of the joint angles θ(t) = [θ1 (t), θ2 (t), . . . , θn (t)]T for an ndof manipulator (t represents the time, 0 ≤ t ≤ tf and tf is the traveling time for the trajectory). Using the inverse dynamics, the joint torques τ (t) = [τ1 (t), τ2 (t), . . . , τn (t)]T can be calculated for all t ∈ [0, tf ]. A time scaling function r = r(t) can be defined with the following properties: • r(t) is an increasing function of time, because time cannot be reversed, 80

E. Szádeczky-Kardoss, B. Kiss • Tracking Error Based On-line Trajectory Time Scaling

r(0) = 0, because the movement must start at the same point, • r(t1 ) = tf for some t1 > 0, the movement must finish in some point. ˜ Using this time scaling function a new trajectory θ(t) ˜ = θ(r). Applying the time can be defined such that θ(t) scaling function r(t), the robot arm can be moved along the path with the same geometry, but with different time distribution. The dynamics of the n-dof fully actuated robot arm considering the joint i (i = 1 . . . n) read

Using this time scaling function, (8) yields the following simple form τ˜a (t) = c2 τa (ct). (9)

•

The task is now to find the time scaling value c that brings the trajectory within the actuator torque limits. This algorithm is presented in [2]. A discretized time scaling method was also developed in [6] to find the minimum-time trajectory for a robot arm. B On-line time scaling Tracking the minimum time trajectories results at least one joint being at this torque limit. The problem is that there is no control authority left for these joints to take care of disturbances or modeling errors. On-line trajectory scaling methods [1], [3] reduce the assumed torque bounds to leave room for the closed-loop control action. The path can be parameterized by its length denoted by a scalar parameter s(t). The input torque boundaries can be converted to bounds on the path speed s˙ and path acceleration s¨. The resulting constraints can be illustrated by plotting the boundary cases for s˙ admissible as a function of s. The dynamic equation of the robot and the equation of the controller are also parameterized by s. To keep the input torque boundaries, the time scaling is done on-line by modifying the path parameter s and its derivatives, taking into account the current value of the torque required by the controller.

˙T

¨ + θ (t)Ci (θ(t))θ(t) ˙ + gi (θ(t)), (1) τi (t) = Ii (θ(t))θ(t) where τi (t) stands for the torque of the joint i, and Ii (θ(t)) for the n-dimensional inertia vector of the manipulator considering the joint i. The term Ci (θ(t)) is the n × ndimensional matrix for the formulation of the Coriolis and centripetal torques, and gi (θ(t)) stands for the influence of the gravity on the joint i. For the sake of simplicity, the following notation is used to have only one equation instead of the n equations in the form (1) if all the n joints are considered ¨ + θ(t)C(θ(t)) ˙ ˙ + g(θ(t)). τ (t) = I(θ(t))θ(t) θ(t)

(2)

Since time scaling does not influence the gravity terms, the acceleration and velocity dependent torques can be treated separately: τa (t) = [τa1 (t), τa2 (t), . . . , τan (t)], such that τ (t) = τa (t) + g(θ(t)).

(3)

III A NOVEL CONCEPT FOR ON - LINE TIME SCALING The methods presented in the previous section give solutions for path planning with known input torque bounds. But we have a new problem, if we do not know the torque constraints exactly, which may arise from the unknown load of the robot. Our proposal is to use the time scaling method depending on the tracking errors as it is also suggested in [5]. (We consider the tracking error e as the absolute value of the error, so it is non-negative.) This method uses a trajectory geometry pre-computed by a path designer. The manipulator will move along this trajectory, the geometry of this path is fixed. Only the velocity profile along the trajectory is determined by the time scaling algorithm, which revaluates regularly the velocity profile with a given period during the tracking. At each evaluation, the method changes the traveling time of the motion, if it is necessary, based on the value of the tracking error. If the error is too large, then the traveling time should be increased. In the case of small errors, the traveling time may be reduced in order to guarantee a more time-optimal motion. The new velocity profile can be determined depending on the modified traveling time of the motion. Based on the previous discussion, the proposed time scaling function depends on the (always non-negative) tracking error e:

Time derivatives of the joint angles should be determined, because they are required to calculate the dynamics of the arm for the new trajectory: ˜ dθ(r) dr dθ(t) ˜˙ = = θ′ (r)r, ˙ θ(t) = dt dr dt

(4)

where θ′ (r) denotes dθ(r)/dr. ¨˜ θ(t) = θ′′ (r)r˙ 2 + θ′ (r)¨ r

(5)

˜ For the trajectory after time scaling θ(t), we have ˜ ¨θ(t) ˜ + θ(t) ˜˙ · C(θ(t)) ˜ ˜˙ τ˜a (t) = I(θ(t)) · θ(t).

(6)

Using (4) and (5) one receives τ˜a (t)

=

[I(θ(r))θ′′ (r) + θ′ (r)C(θ(r))θ′ (r)] r˙ 2 + I(θ(r))θ′ (r)¨ r.

(7)

Rearranging and using (2) and (3) one obtains τ˜a (t) = r˙ 2 τa (r) + r¨I(θ(r))θ′ (r).

(8)

The new torque time function τ˜a (t) is related to the old τa (r) by the scaling factor r˙ 2 plus a term proportional to the generalized momentum I(θ(r))θ′ (r) of the manipulator. ˜ The gravity torque g(θ(t)) = g(θ(r)) is not scaled since it is position dependent only. Consider now a simple example by taking r(t) = ct as a time scaling function for some constant c > 0. In this case r(t) ˙ = c and r¨(t) = 0. If c > 1 the movement is accelerated, if c < 1 the velocity of the motion is reduced.

r

= r(t, e),

r˙ = r¨ = 81

rt′ + re′ e, ˙ ′′ ′′ 2 rt + re e˙

(10) +

re′ e¨.

(11) (12)

INES 2006 – 10th International Conference on Intelligent Engineering Systems

rt′ is positive as at the usual time scaling. Note that re′ is negative since larger errors result slower motion and consequently the value of r should be decreased. The new reference values are then: ˜ θ(t) = θ(r) ˙˜ θ(t) = θ′ (r)r˙ = θ′ (r)(rt′ + re′ e) ˙ ¨˜ ′′ 2 ′ θ(t) = θ (r)r˙ + θ (r)¨ r=

B Hyperbolic time scaling functions The other special time scaling function is has the form: eok t, (21) e where eok is the normal error, for e < eok the motion could be faster, for e > eok the movement should be slowed down (see Fig. 2). The derivative of r has the following form in this case: eok eok r˙ = p + p′ et ˙ = − 2 et. ˙ (22) e e V A N ALGORITHM FOR USING THE ON - LINE TIME r = p(e)t =

(13) (14)

= θ′′ (r)(rt′ +re′ e) ˙ 2 + θ′ (r)(rt′′ +re′′ e˙ 2 +re′ e¨).(15) IV S PECIAL TIME SCALING FUNCTIONS We have seen that the derivatives of the general form of the time scaling functions for the new concept are involved in (10)-(15). To get simpler equations, we study a special class of time scaling functions r(t, e), where the time t is multiplied by an error dependent value: r = r(t, e) = p(e)t.

SCALING

An algorithm using the semi-constant time scaling function is presented. First, an initial motion planning is done. The geometry of the trajectory is fixed, it does not change during the motion. If the error is too large or too small, the traveling time tf will be increased or reduced. The steps of the algorithm are the following: • The algorithm calculates the reference values of the motion from the parameters of the trajectory. This is done before the tracking starts. Let Torig denote the obtained traveling time. • The method changes the time of the motion (i.e. the distribution of the time along the path) if it is necessary because of tracking errors. It modifies the time using a linear transformation:

(16)

In this case the derivatives (11)-(12) read r˙ = rt′ + re′ e˙ = p + p′ et, ˙ ′ ′′ 2 ′ r¨ = p e˙ + p e˙ t + p e¨t,

(17) (18)

where p′ = dp de . Two examples are presented for this special class of time scaling functions. A Semi-constant time scaling functions The first example uses a time scaling function similar to the constant scaling of Hollerbach. The time can be multiplied by three different constants; the error magnitude determines which constant to be used (see Fig. 1):   c1 · t if e < e1 1 · t if e1 ≤ e < e2 , (19) r=  c2 · t if e2 ≤ e

a · tnow + b = told ,

where c1 ≥ 1 and c2 ≤ 1, e1 and e2 define the limits for normal error. If the error is smaller than e1 , the motion can be faster. If the error is larger or equal to e2 , slower motion is required. If e ∈ [e1 , e2 ) the velocity of the motion should be left unchanged. Using this time scaling function, the derivative of r is quite simple, because p′ = 0 if e 6= e1 and e 6= e2 : r˙ = p(e).

•

•

(20)

where tnow stands for the current time (read from the clock), and told means the moment based on the original time scaling (see Fig. 3). (At the beginning of the motion a = 1 and b = 0 so tnow = told .) The values of the errors are checked with a period Tcheck . At the end of each period Tcheck , after using the timeconversion (23), the algorithm checks if the errors are lower or higher than the desired error-margin. The following steps are done: 1) Determination of the reference values for the current moment of time.

An example for p(e)

Other example for p(e)

p(e)

p(e)

c_1

1

c_2 1

0

0

e_1

0 0

e_2 e

e_ok e

Figure 1

Figure 2

First example for special p(e)

Second example for special p(e)

82

(23)

E. Szádeczky-Kardoss, B. Kiss • Tracking Error Based On-line Trajectory Time Scaling

Torig

t0

c1 : rate of acceleration in case of small errors; c2 : rate of retardation in case of large errors. An optional property is that the algorithm checks the position error at the beginning of the motion. This is useful in such cases where the initial position of the robot and the starting point of the reference trajectory are different. It is possible to increase the time of the motion before the movement starts, thus the initial error in the position can be corrected better while moving slower. (At the beginning of the motion, the algorithm may increase the time of the motion if it is necessary, but it does not decrease it.)

Figure 3

B Simulations

Path with original velocity profile c old

t

current point

•

end point

•

told a.tnow+b=told Tnew

t0

c tnow tnow

Path with the scaled velocity profile

start point

Sketch for the time-transformation of a path

The simulations are done using a 2-dof positioning tool equipped with a gas spring in order to reduce the effects of the gravitational force. The 2-dof positioning mechanism is composed of two rigid bodies (gas spring excluded) as it is shown in Fig. 4 (together with gas spring). The column (rigid body no.1) is moving vertically and the arm (rigid body no.2) is rotating around a horizontal axis, hence all motions are subject to gravitational forces. The coordinates of the centers of mass of the two segments are denoted by xi and yi for i = 1, 2 whereas the corresponding masses are denoted by m1 and m2 . The mechanism has two joints, both are actuated by DC motors: there is a prismatic joint between the ground and the column, the corresponding linear displacement is denoted by θ1 . A revolute joint connects the column to the arm, where θ2 denotes the corresponding angular displacement. The input of the model is the vector of generalized forces [τ1 , τ2 ] exerted by the motors. The gas spring itself is modeled by two additional rigid bodies linked with a prismatic joint. The gas spring is connected to the column and to the arm by two revolute joints, respectively. The coordinates of the centers of mass of the two additional segments of the gas spring are denoted by xi and yi for i = 3, 4 and the corresponding masses are denoted by m3 and m4 .

2) The errors are calculated using the deviations from the reference values. 3) The traveling time (T = tf ) is changed depending on the tracking error: If the error is too large, then T should be increased. In case of small errors T should be reduced. The calculations are based on (19). Let us denote by Told the traveling time to be updated and by Tnew the updated traveling time.   c1 · Told if e < e1 Told if e1 ≤ e < e2 Tnew = (24)  c2 · Told if e2 ≤ e, where

c1 ≤ 1, c2 ≥ 1.

(25)

4) If Tnew 6= Told , then the parameters of the timetransformation in (23) have to be recalculated. The values of the parameters a and b should be chosen according to the following conditions: – Equation (23) should transform the current tcnow to tcold (see Fig. 3) using a · tcnow + b = tcold .

(26)

– The updated traveling time Tnew should be mapped to the original Torig (see Fig. 3) as a · Tnew + b = Torig .

m2 x2, y2

(27)

From these two equations the values of a and b can be calculated: Torig − tcold a = , (28) Tnew − tcnow Tnew · tcold − Torig · tcnow . (29) b = Tnew − tcnow

m4 m1 x4, y4 x1, y1

Thus, the new reference values can be calculated.

x3, y3 m3

A Parameters of the algorithm This algorithm can be implemented in many ways. There are some parameters in the implementation used in the next subsection where we present simulation results. • Tcheck : period for the recalculation of the traveling time; • e1 : lower margin for normal error in percent; • e2 : upper margin for normal error in percent;

Figure 4 The 2-dof model with the gas spring

83

INES 2006 – 10th International Conference on Intelligent Engineering Systems

The dynamics of the robot (1) can be obtained using the Lagrange formalism [4] (i = 1, 2): τi =

∂K ∂P d ∂K ˙ − + + fgsi (θ, θ), dt ∂ θ˙i ∂θi ∂θi

shows the change of the motion’s time at multiples of Tcheck . If the first check is made at the beginning of the motion, the robot starts the movement with lower speed and it is able to track the reference path more accurately. At the end of the motion the errors are very small, the robot is able to finish its task faster, thus the time scaling algorithm reduces the time of the motion in one step.

(30)

where τi is the torque or force on the i-th joint, K stands for kinetic, P for potential energy, θi is the joint variable of i-th joint and fgsi gives the contribution of the gas spring to the generalized force of the given joint. The kinetic energy K and the potential energy P are K P

= =

4 X 1

i=1 4 X

2

mi vi2 =

mi ghi =

i=1

4 X 1

i=1 4 X

2

mi (x˙ 2i + y˙ i2 )

C Modification of the algorithm In special cases, if it is not necessary to follow the whole path and the goal is only to reach the final position a modification of the algorithm is available. The path can be modified if the current position of the robot is nearer to another path point than to the current reference point. The reference values for the further motion can be selected in the following way. The current reference point is the point of the trajectory, which is the nearest one to the current position of the robot. (So if the robot left its desired position, it should not go back, if it is moving in the right direction.) Thus the current value of tcold in (26) becomes the moment of the nearest reference point accordingly to the primary path planning. This modification is also useful in such cases where the initial position of the robot and the starting point of the reference trajectory are different. The robot can find the nearest reference path point at the beginning of the motion. The path planning can modify the reference trajectory such that the new initial point is the nearest path point. Thus the movement needs less time and the errors can be reduced. This modification should not be used if it is necessary to follow the whole path or the robot has to travel through some path points more than once. The results of the two time scaling algorithms are compared to the tracking without time scaling case at the second simulation. (The parameters of the algorithm for this simulation are: Tcheck = 0.05 sec, e1 = 0.5%, e2 = 10%, c1 = 1.05, c2 = 0.77.) Fig. 7 shows how the path of the center of gravity of the robot arm shapes in case of different methods. Using the

(31)

mi gyi ,

(32)

i=1

respectively, where vi stands for the velocity of the rigid body no. i, hi is the height of the center of mass of segment i, and g corresponds to the gravitational acceleration. The position feedback law used in the simulation is based on the computed torque method which can be seen as a linearizing state feedback together with a PID like linear tracking controller, see [4] for details. The first simulation demonstrates the effectiveness of the time scaling algorithm. The center of gravity of the robot arm has to move along a circular trajectory. There is some viscous friction on the revolute joint excluded from the model equations in order to generate tracking errors in the simulations. The effect of an initial time scaling operation is also shown in Fig. 5. The initial position of the robot differs from the reference point at the start of the movement. (The parameters of the algorithm for this simulation are: Tcheck = 0.5 sec, e1 = 0.5%, e2 = 10%, c1 = 1.05, c2 = 0.77.) In Fig. 5 the path of the center of gravity of the robot arm is depicted. At the beginning of the motion, there is a large error in the position. The robot is not able to track the trajectory accurately. But the use of the time scaling algorithm results a more accurate but slower motion. Fig. 6

1.6

1.4

4

Time of the motion (sec)

1.2

1

0.8

0.4

path without time scaling path with time scaling and checking at the beginning path with time scaling reference path

0.2 −0.2

−0.15

0.6

3.5

without time scaling with time scaling with time scaling and checking at the beginning

3

2.5

2 −0.1

−0.05

0

0.05

0.1

0.15

0.2 0

50

100

150

200 250 Time (*0.01 sec)

300

350

400

Figure 5 Figure 6 Path of the arm’s center of mass in the first example (the reference path is a circle, the initial position for the simulation is outside of the circle)

The change of the motion’s time in the first example

84

450

E. Szádeczky-Kardoss, B. Kiss • Tracking Error Based On-line Trajectory Time Scaling

improved method we get a shorter path, where the largest errors are smaller than at the other methods. Since the path is shorter and the largest errors are smaller, the robot can track the trajectory faster. Fig. 8 shows the traveling time in different cases. The use of the improved method yields a shorter path, but it cannot be used in every application. If it is important to track the whole trajectory then the normal time scaling algorithm should be applied.

without time scaling with time scaling with improved time scaling

Time of the motion (sec)

4

3.5

3

2.5

VI C ONCLUSION Using time scaling in the motion planning of a manipulator is a useful off-line method to avoid torque constraints violation. Using this algorithm a near-minimum time motion can be achieved. Our method which uses the tracking error to redistribute the time along the trajectory without changing its geometry can be also useful for other systems, where the exact bounds of input torques are not known. The choice of the time scaling function is discussed in details and some solutions were proposed. The semi constant time scaling method is detailed in this work. The results of simulations showed that the use of this algorithm can result a more accurate motion. If the movement is accurate enough the method allows the robot to track the trajectory faster.

2 0

150

200 250 Time (*0.01 sec)

300

350

400

450

The change of the motion’s time in case of modified algorithm

This time scaling method has some special options. These are also detailed in the work. They can make the motion of the robot more effective. ACKNOWLEDGMENT The research was partially founded by the Hungarian Science Research Fund under grant OTKA 042634 and by the Advanced Vehicles & Vehicle Control Knowledge Center under grant RET 04/2004. R EFERENCES

6

[1] O. Dahl and L. Nielsen, “Torque-Limited Path Following by On-Line Trajectory Time Scaling”, IEEE Trans. on Robotics and Automation, Vol. 6, No. 5, Oct. 1990, pp. 658-669 [2] J. M. Hollerbach, “Dynamic Scaling of Manipulator Trajectories”, Trans. of the ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 106, No. 1, 1984, pp. 102-106 [3] A. Kumagai and D. Kohli and R. Perez, “Near-Minimum Time Feedback Controller for Manipulators Using On-Line Time Scaling of Trajectories”, Trans. of the ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 118, No. 2, Jun. 1996, pp. 300-308 [4] B. Lantos and J. Soml´o and P. T. Cat, Advanced Robot Control, Hungarian Academic Press, Budapest, 1997. [5] J. L´evine, “On the Synchronization of a Pair of Independent Windshield Wipers”, IEEE Trans. on Control System Technology, Vol. 12, No. 5, Sept. 2004, pp. 787-795 [6] G. Sahar and J. M. Hollerbach, “Planning of Minimum-Time Trajectories for Robot Arms”, International Journal of Robotics Research, Vol. 5, Issue 3, Fall 1986, pp. 90-100 [7] M. Zefran, Review of the Literature on Time-Optimal Control of Robotic Manipulators, GRASP Laboratory, Department of Computer and Information Science, University of Pennsylvania, MS-CIS-94-30, GRASP LAB 375, 1994.

5

4

3

2

0 −0.2

100

Figure 8

7

1

50

path without time scaling path with improved time scaling path with time scaling reference path

−0.15

−0.1

−0.05

0

0.05

0.1

0.15

0.2

Figure 7 Path of the arm’s center of mass in case of modified algorithm (the reference path is a circle, the initial position for the simulation is outside of the circle)

85