Time-energy optimal control of articulated systems ... - Semantic Scholar

6 downloads 95 Views 490KB Size Report
Automotion, Vol. 7, No. 6, December 1991, pp. 785-797. Shiller, Z., and Lu, H.H., 1992, “Computation of. Path Constrained Time Optimal Motions along Specified.
Time-Energy Optimal Control of Articulated Systems with Geometric Path Constraints Zvi Shiller Department of Mechanical, Aerospace and Nuclear Engineering University of California Los Angeles Los Angeles, CA 90024 [email protected] Abstract

1988). The energy term effectively convexifies the cost function and the Hamiltonian with respect to the control, creating a minimum that is not necessarily on the control boundaries. The continuity of this minimum along the optimal trajectory ensures the continuity of the optimal control. Previous efforts for solving the energy optimization problem include a dynamic programming search in the state space for point to point motions (Vukobratovic and Kircanski 1982) and for motions along specified paths (Shin and McKay 1986, Singh and Leu 1987, Pfeiffer and Johanni 1987). The dynamic programming approach yields non-smooth trajectories due to the discrete grid representation. It also offers no a priori insights into the structure of the optimal solution. This paper considers a time-energy cost function for articulated systems moving along specified paths. Using the Pontryagin’s maximum principle, the optimal trajectory is shown to be continuous in the acceleration along the path, resulting in smooth velocity profiles. The optimal trajectory is computed by numerically solving a fourth order two point boundary value problem. The method is demonstrated numerically for a two link manipulator, and experimentally for the UCLA Direct Drive Arm. The time-energy optimal trajectory is demonstrated experimentally to yield smaller tracking errors than the time optimal trajectory, when used as the reference input to the PD joint controllers.

A method is presented for optimizing the motions of articulated systems along specified paths, minimizing a time-energy cost function. Using a transformation to path variables, the optimization problem is formulated in a reduced two dimensional state space. The necessary conditions for optimality, stated for the reduced problem, lead to a compact two point boundary value problem that requires the iterations of only one boundary condition. The optimal control obtained for this problem is smooth, as opposed to the typically discontinuous time optimal control. The method is demonstrated numerically for a two link planar manipulator, and experimentally for the UCLA Direct Drive Arm. The smoother time-energy optimal trajectory is shown to result in smaller tracking errors than the time optimal trajectory.

1

Introduction

Most of the recent work on optimizing the motions of articulated systems has focused on minimizing motion time of rigid body systems, treating the actuator torques/forces as the control inputs (see for example Bobrow et a1 1985, Shiller and Dubowsky 1989). However, the resulting bang-bang strategies are physically unrealizable due to the typical discontinuities at the switching times and the non-negligible actuator dynamics. In addition, such strategies are undesirable due t o the structural vibrations induced by the control discontinuities, and due to the damage to the permanent magnets of the electric motors caused by the abrupt changes in the motor current (Kenjo and Nagamori 1985). Some of these practical difficulties can be addressed by modeling the actuator dynamics (Tarkiainen and Shiller 1993). This, however, increases the dimensionality of the model and the required computational effort. Alternatively, the control can be smoothed by adding a quadratic energy term to the cost function (Jacobson and Speyer 1971, Chen and Desrochers

2

We wish to solve the following problem:

2.1

where

2680

1050-4729B4%03.00Q 1994 IEEE

Problem Formulation

Problem I

L‘(T,8 , t ) = 1 + e2T2

g2(x, U) = Umin(X) - U I 0

subject to system dynamics (neglecting the gravity forces for simplicity)

M(e)e + e T C ( e ) e= T

(12)

and the state inequality constraints

(3)

h(x) = x z - smar(X 1) L 0

the boundary conditions

(13)

where, using (7), the integrand L(x, U) becomes

e(o)= eo,e ( t f )= et,, e(o) = e ( t f )= o

(4)

L(x, U ) = 1 + c2(bTbx;+ 2mTbx;u

+ mTmu2) (14)

and path constraints

q(e)= o ;

e

E R " - ~ ; E R"

The inequality constraints g = [S1,g2IT express the upper, U,, and lower, umin,bounds on the acceleration along the path, SI (Shiller and Lu 1992)

(5)

where t f is the free final time, 6 is a weighting factor, T E Rn is the vector of actuator efforts, 8 E Rn is a vector of the joint displacements, M(B) is the inertia matrix, and C ( 8 ) is an array of the coefficients of the centrifugal and Coriolis forces. Assuming for simplicity constant actuator limits, the set of the admissible actuator efforts, R, is defined as

R = {Tl'&,in I

r 5 Zm,, ; i = 1 , 2 , . .. ,n}

-

Ti b . x 2 umar(x)= m i n { m a x ( L > }i = 1 , . . . ,n t

+ b(s)s2= T

(17) We now determine the structure of the optimal control of Problem I1 from the first order necessary optimality conditions for the case of state-dependent control constraints and a free final time (Bryson and Ho 1975). We ignore the state constraint (13) since it can be shown that, excluding singular points, the state constraint is active only when the control constraints are active (Shiller and Lu 1992). It is assumed that singularity points are avoided either by appropriately choosing the path or by convexifying the set of admissible controls (Shiller 1992).

(7)

3

1

tf

~ ( xu)dt ,

(8)

H(X*,A,U*,t)= 0

subject to system dynamics j,

= f(x,U) = [ X Z , U]T

Structure of The Optimal Control

The solution to Problem 11, u * ( t ) ,x*(t), must satisfy the following necessary conditions (Bryson and HO 1975) HU(X*,A,U*,t) =0 (18)

Problem I1 min J =

5 [S,s .'lT

(15)

The scalar inequality constraint, h , represents the upper bound on the velocity along the path, 8:

(6)

where m and b are the coefficients of the acceleration and Coriolis forces expressed in path coordinates (Shiller and Lu 1992). The transformed dynamics (7) represent a linear mapping from T to s. Since this mapping is unique (due to the linearity of (7) in both T and s), we can use s as the control input, and thus reduce the n dependent control variables to an independent scalar control. This also reduces the 2n dimensional state space to the two independent states s and S. 5, we Denoting x = { X ~ , X G ~ }{ s~ , S } ~ and U can now restate Problem I in the reduced state space. 2.2

mi

- b.2' u,in(x) = m + x { m i n ( L ) } , i = l , . . - , n . (16) t T, mi

The n-1 path constraints (5) can be used to rewrite system dynamics (3) in terms of the velocity s and acceleration s along the path (Bobrow et a1 1985, Shiller and Lu 1992) m(s)s

T,

(19)

for all t E [to,!f] where (omitting the time argument) the Hamiltonian, H(x, A, U), is defined as

(9)

2681

state inequality constraint (13). The Hamiltonian is said to be regular if it has a unique minimum with respect to u (Jacobson et al 1971). The state constraint is said to be first order if its first time derivative is explicit in the control (Bryson and Ho 1975). The interior arcs are gradual transitions between the boundary arcs, with slopes depending on the weighting factor E. As E approaches zero, the transitions become sharper, and the time-energy optimal trajectory approaches the time optimal solution (Jacobson and Speyer 1971, Chen and Desrochers 1988).

Substituting (9) and (14) into (20) and (21) yields

+ 2 m T b z i u + mTmu2) + X z u + plgl + pzgz = 0 H,, = 2c2(mTbz; + m T m u ) + X z + g T p = 0 H = 1 + E2(bTbxi

(22)

+XlZZ

xi

i z

+

= - 2 ~ ~ { m ~ m bTb,xi , u ~ (mTb mTb,)z:u} - g:p

+

+

= -4c2(bTbz;

(23) (24)

+ mTbzzu) - A 1 - gz2d25) 4

where the subscripts s and x2 denote partial derivatives with respect to 2 1 E s and xz E S, respectively. The multipliers p = b1,pZlT must be positive if the associated constraint is active, and zero otherwise, to ensure that at the optimum, the cost function can be reduced only by violating the constraints. The nonzero multipliers are determined from the optimality condition (23). Solving (23) for p , using glu = 1 and gZu = -1, we get p1 p2

To solve the two point boundary value problem stated by the state equations (9), (24), (25), and the boundary conditions (lo), we first solve for the initial condition Xz(0) using (22). This reduces the problem to selecting the initial condition X l ( 0 ) so as to satisfy the terminal conditions (10). The selection of Xl(0) is essentially a line optimization problem that can be solved efficiently. Using the initial condition zz(0) = 0 and assuming interior control, (28),we use (22) to solve for Xz(0):

= -2c2mTbz; - 2c2mTmu - Xz ifgl = 626) = 2 ~ ' m ~ b x ; 2 ~ ~ m ~ Xz m u ifgz =(Q7)

+

Numerical Solution

+

Xz(0) = 2J"-

Since the Hamiltonian for this problem is convex with respect to U , solving (23) for u yields the global unconstrained (interior) optimal control, uint:

(31)

The negative sign is selected to ensure an initial positive control. Substituting (31) into (28) and applying the initial conditions, we obtain the initial unconstrained optimal control

The control uint is the optimal control if the control constraints are inactive, or If uint(0) from (32) satisfies (29), then the optimal trajectory starts with an interior arc u'(0) = uint(0). Otherwise, the optimal trajectory starts with a constrained arc, u*(O) = UmaZ(O), or ~ ' ( 0 ) = umin(0). The initial condition Xz(0) is then given by

If (29) is not satisfied at some t = t,, then due to the convexity of H with respect to U, the optimal control switches to the upper or lower bounds uma2(ts),or umin(ts), respectively. The optimal control then consists of constrained arcs, determined from (15) or (16), or interior arcs, determined from (28). The structure of the optimal control U * @ ) is thus given by

The trajectory switches to an interior arc at some time, t,, when uint(ts) (28) no longer violates the actuator constraints (1 1) or (12). The initial condition on Xl(0) is determined iteratively by satisfying the terminal conditions (10). The initial value Al(0) is the only unknown in this problem since all multipliers are continuous across the junction points between interior and boundary arcs. The iteration on a single parameter is computationally quite inexpensive compared to the dynamic programming search used in (Shin and McKay 1986, Singh and Leu 1987, Vukobratovic and Kircanski 1982).

The optimal control is continuous at the junction points between interior and constrained arcs due to the continuity of x and A. It follows that p is also continuous (and zero) across the junction points. The smoothness of the control across the junction points extends also to the state constraint due to the regularity of the Hamiltonian and the low (first) order of the

2682

s

Table 1: Parameters of the Two-Link Manipulator

I

L 1.0m 1.Om

I IC 0.5m 0.5m

I I at C.G I T-,, ...”- I, 1 kg I 0.08 Kg-m2 40 N-m 1 kn I 0.08 Kg-mZ 20 N-m

I m

I

S

5

Examples

Figure 2: Time-energy optimal trajectories in the phase plane, s - SIfor various values of E .

The method was implemented for two planar manipulators. It is first demonstrated numerically for a two link manipulator, followed by an experimental example for the UCLA Direct Drive Arm.

5.1

A Two link Planar Manipulator

In this example, the task is to move the two link manipulator, with the parameters given in Table l, along the specified path shown in Figure 1. Figure 2 shows a family of time-energy optimal trajectories in the phase plane, s - S, for various values of E . Also shown in Figure 2 is the time optimal trajectory. Clearly, as E approaches zero, the corresponding trajectory approaches the time optimal one. Figure 3 shows the variations in the motion time as a function of 6’. From Figure 3 it is clear that trajectories for a small E can be computed by starting with a large E , then solving the problem repeatedly for progressively smaller E’S, using the solution for XI(0) at each iteration as the initial guess for the next iteration. This approach was used to compute the solutions shown in Figure 2.

m

.&I

.Ol

.oo1

1

*

e2

Figure 3: Motion time vs.

5.2

6’

Experiments with a Planar Direct Drive Manipulator

This example demonstrates the time-energy optimal trajectory for the UCLA Direct Drive Arm shown in Figure 4. The control system of this manipulator consists of two independent joint P D controllers, each wrapped around a PD velocity control loop. The optimal trajectory (position as a function of time), computed off-line on a Silicon Graphics, IRIS-4D, was used as a reference trajectory to the position controllers. Modeling the driver as a current amplifier, we used the control signal generated by the velocity loop to indicate the applied actuator torques. The task for this example was to move the tip of the manipulator along the path shown in Figure 5, using the time optimal and the time-energy optimal ( e 2 = 0.005) trajectories. Figure 6 shows the nominal and actual tip velocities for both trajectories, with the corresponding times of te--opl = 0.628s and t t - o p t = 0.547s, an increase of 15% for the time-energy solution. As expected, the time-energy optimal trajectory is smooth, and stays below the time optimal trajectory. Figure 7 shows the tracking errors along the path for each trajectory. The nominal and actual actuator torques are shown in Figures 8 and 9. The noted bias between the nominal and the actual torque

Figure 1: A two link manipulator a t the end points of a specified path.

2683

Figure 4: The UCLA Direct Drive Arm.

Figure 7: The tracking errors along the path for the time-optimal and time-energy optimal trajectories.

is due to the unmodeled motor friction and motor dynamics. P a t h in Workspace

50

1

-50 50

0

100 0

[cml

0.2

0.4

Time

0.9

'

-201 0

0.8

I 0.2

[-I

0.4

0.6

0.8

rime [see]

Figure 5: The specified path in the workspace.

n

Figure 8: The nominal and actual actuator torques for the time-energy optimal trajectory.

Yeloel1

N&----

;

Time-Energy 0 0

IO

20

30

40

50

60

70

80

Figure 6 : The nominal and actual tip velocities for the time-optimal and time-energy optimal trajectories. The time optimal trajectory is difficult to follow due to the discontinuities in the actuator torques and the uncompensated transient dynamics of the position controller. As a result, the actual time optimal velocity profile overshoots around s = 45cm, resulting in the peak tracking error of 3.9 mm, compared to the maximum error of 2.7 mm for the time-energy trajectory, a reduction of 45%. It is important to note that the error for the time optimal trajectory would have been larger if the actuator torques were not allowed to exceed their theoretical limits. Limiting the actuator torques was not practical since the velocity loop is internal t o the motor driver. Comparing Figures 6 and 9. we can see that the large tracking errors for

::F/ ;F/

; I

-

0

Y

P

P

-10 -20 0

0.2

-

0.4

f-1

B

-10

-20 0.6

0.8

0

0.2

-

0.4

0.8

OB

[-I

Figure 9: The nominal and actual actuator torques for the time-optimal trajectory.

the time optimal trajectory resulted from the inability

Chen, Y., and Desrochers, A. A., 1988, “Time-Optimal Control of Two-Degree of Freedom Robot Arms,” Proc. of IEEE Conf. on Robotics and Automation, Philadelphia, PA, April 1988, pp. 1210-1215. Jacobson, D.H., Lele, M.M., and Speyer, J.L., 1971, “New Necessary Conditions of Optimality for Control Problems with State-Variable Inequality Constraints,” J . of Mathematical Analysis and Applications Vol. 35, No. 2, August, pp. 255-284. Jacobson, D.H., and Speyer, J.L., 1971, “Necessary and Sufficient Conditions of Optimality for Singular Control Problems: A Limit Approach,” J. of Mathematical Analysis and Applications Vol. 34, No. 2, May, pp. 239-266. Kenjo, T., and Nagamori, S., 1985, Permanent-Magnet and Brushless D C Motors, Clarendon Press, Oxford. Pfeiffer F., and Johanni R., “A Concept for Manip ulator Trajectory Planning,” IEEE Trans. Robotics and Automation, Vol. RA-3, no 3, pp. 115-123. Singh, S., Leu, M.C., 1987, “Optimal Trajectory Generation for Robotic Manipulators Using Dynamic Programming,” Journal of Dynamic Systems, Measurement and Control, Vol. 109, June 1987, pp. 88-96. Shiller, Z., 1992, “On Singular Time-Optimal Control Along Specified Paths,” ASME Winter Annual Meeting, Anaheim, CA, November 1992. Also accepted in IEEE

of the feedback controller to follow the sharp drop in the nominal actuator torques due to its limited bandwidth. The lower control effort required and the smaller tracking errors resulting from the smooth transitions between the actuator extremes clearly demonstrate the utility of the time-energy optimal trajectories. For most applications, the reduction in the tracking error is generally well worth the slight increase in motion time. Further, the lower control effort required to follow the time-energy optimal trajectories, and consequently, the lower motor temperature, can potentially extend the operational life of the electric motors in repetitive motions.

6 Conclusions A method for computing time-energy optimal trajectories along specified paths has been presented, considering the full nonlinear system dynamics and actuator constraints. The optimization problem is formulated as a fourth order two point boundary value problem, and is solved by iterating over the initial value of one Lagrange multiplier. The optimal control is shown to be continuous, in contrast to the typically discontinuous time optimal control. The smooth trajectories produced by the timeenergy cost function are demonstrated to yield better transient response and smaller tracking errors than the time optimal trajectories, when used as reference inputs to PD position controllers. In fact, the time optimal trajectory is impossible to implement without feedforward control, or preshaping of the nominal trajectory (Shiller and Chang 1993), due to the finite bandwidth of the feedback controller. The slight increase in motion time for the timeenergy optimal trajectory seems insignificant compared to the gains in motion accuracy. The timeenergy optimal trajectories are also expected to have a positive effect on the operational life of the electric motors by commanding lower currents and thus reducing motor temperature. This method is applicable to a wide class of articulated systems with rigid body dynamics, including industrial robots and autonomous vehicles.

7

Trans. on Robotics and Automation. Shiller Z., and Chang, H., 1993, “Trajectory Preshaping for High-speed Articulated Systems,” Submitted to ASME J . of Dynamic Systems, Measurement and Control, June 1993. Shiller, Z., and Dubowsky, S. 1989, ‘Time Optimal

Path Planning for Robotic Manipulators in the Presence of Obstacles, With Actuator, Gripper and Payload, Constraints,” Int’l J . Robotics Research, December 1989, pp 3-18.

Shiller, Z., and Dubowsky, S., 1991, “On Computing the Global Time-Optimal Motions of Robotic Manipulators in the Presence of Obstacles,” IEEE Trans. on Robotics and Automotion, Vol. 7, No. 6, December 1991, pp. 785-797. Shiller, Z., and Lu, H.H., 1992, “Computation of Path Constrained Time Optimal Motions along Specified Paths,” ASME Journal of Dynamic Systems, Measurement and Control, Vol. 114, No. 3, pp 34-40. Shin, K. G. and McKay, N. D. 1986, “A Dynamic Programming Approach to Trajectory Planning of Robotic Manipulators,” IEEE Trans. Automatic Control, Vol. AC31, no 6, pp. 491-500. Tarkiainen, M., and Shiller, Z., 1993, “Time Optimal Motions with Actuator Dynamics,” IEEE Conf. on Robotics and Automation, Atlanta, GA, May 1993. Vukobratovic, M., and Kircanski, M., 1982, “A Method for Optimal Synthesis of Manipulation Robot Trajectories,” Journal of Dynamic Systems, Measurement and Control, Vol. 104, June 1982, pp. 188-193.

References

Bobrow, J. E., Dubowsky, S., and Gibson, J. S., 1985, “Time-Optimal Control of Robotic Manipulators,” Int’l J . of Robotics Research, vo1.4, No. 3, 1985. Bryson, A.E., and Ho, Y.C., 1975, Applied Optimal Control, Optimization, Estimation and Control, Hemisphere Publishing, New York, 1975.

2685