- Email: [email protected]
Motion Control of Flexible Space Robots Using Optimal Trajectory Planning and Stable Inversion
Ib-03 5
Copyright © 19% IFAC 13th Triennial World Congress, San Francisco, USA
MOTION CONTROL OF FLEXIBLE SPACE ROBOTS USING OPTIMAL TRAJECTORY PLANNING AND STABLE INVERSION 1 Hongchao Zhao
Degang Chen'
• Dept. of EledNcal and Computer Engineering Iowa State Univt1'Sity
Ames, lA 50011 , USA
Abstract. A space robot is typically controlled to move extremely slowly in order to avoid structural vibratio ns, to minimize interference with the spacecraft, and to reduce energy co nsumption, This paper investigates a new motion controL stra.tegy to achieve the fastest motion without violating these goals, First, the dynamics eq uation of a flexible space robot is developed using the assumed modes method . Then , the optimal motion control problem is formulated as a two-stage functional optimization problem. The inner stage is solved by a newly developed stable inversion theory. The outer stage then becomes an optimal trajectory planning problem that is further simplified into a finite dimensional optimization problem. Simulation results are finally presented, Keywords. Inversion , Trajectory Pla.nning, Flexible Arms, Space Robotks
1. INTRODUCTION
A typical task for a robot manipulator mounted on a space station would be to grasp an object, say a satellite. from space and put it into the space station, or vice Versa. Because of high fuel costs of bringing the robot into space and limited solar energy supply by space stations, space manipulatofl:; are always flimsy and flexible, a.nd therefore often exhibit extremely under~damped vibration modes. The structural flexibility and limited solar energy impose great challenges to the precise manipulator motion control. First, it is dear that any control strategy has to result in a minimum energy consumption because of limited resou rce, Secondly, any movement of the manipulator arm would transmit an undesirable interference force from th e ann to the space station. Finally, any control forces or disturbances applied to the 1 T his work is partially supported by Nationa.l Science Founda-
t ion under Grant No. ECS-9410646 and by Iowa Space Grant COMor'ium.
manipulator are very likely to excite structural vibrations in the arm as well as in the space station . Therefore, a. good motion control design for a space manipulator should have the following properties: 1) Achieving a desired. motion with the shortest possible time; 2) Not exciting structural vibrations; 3) Using a minimum amount of energy; and 4) Producing the minimum interference on the space station. Though robotics ha...;; been an active research area for the past few decades, applications are concerned primarily with massive earth-bound industrial robots. On the other hand, investigations concerning space robots ha.ve been mostly carried out by considering the rigid links assumption (NUY92. ). By assuming relatively small elastic vibrations, a perturbation approach is utilized to design separate motion controllers for the rigid and the flexible parts (ML94.) . Using reaction wheels or attitude control jets (VD87 , ). th e effect of interference from manipUlator motion on the space station can be compensated. Another method to reduce the interference is
91
to include the space station in the trajectory planning or to use kinematic redundancy to optimize the robot trajectory (MC92, ). All the methods either lead to slow motion in order to keep down energy consumption and vibrations excitation, or neglect the transient impact on the space station.
A fundamentally different approach to the tracking control of flexible structures is by using noncausal inversion. The idea was first presented by Bayo (Bay87, ) for solving the inverse dynamics for one-link flexible robots. Since one-link robots are linear systems, the fast Fourier transform method worked successfully. By using feedback linearization and locally exponenLially stable joint controllers, the method was extended to the nonlinear cases, the multi-link flexible robots. Successful experimental results demonstrated the efficiency of the approach (PCLB93, ). With these results, the stable inversion concept was first mtroduced to design exact and stable output tracking c,ootrol for a general class of nonlinear nonminimum phase systems (CP92, ). An iterative linearization algorithm was also developed for consLructing such stable inverses (Che93, ). Furthermore, it has been recently proved that th~ stable inverses have a nice minimum energy property (ZC94, ). Specifically, the stable inversion can achieve a given reference trajectory using a minimum amount of control energy and causing a minimum amount of internal vibrations.
are assumed to be slender such that the Euler- Bernoulli beam assumption is valid. Planar maneuver is assumed so that we neglect the out-of-plane deflections. We also neglect any possible effect from the sun and the earth, which means there is no external force on the system. For convenience, part of the notations are summarized in the following list: Xo
Yo 00
0, O2 'If;
v 8 Q U
I; Wi
In this paper! we investigate a new motion control strategy by using the stable inversion approach for space manipulators with all flexible links and no control jets or reaction wheels. The problem is attacked in two steps in this study. First, for a given trajectory the stable inversion theory is used to find the optimal control strategy that guarantees precise output tracking and at the same Lime ensures minimal structural vi brations and interference on the space station. Furthermore, it is also guaranteed that the control uses a minimum amount of energy. In the second step, an optimization problem is solved that searches for the best trajectory which leads to the fastest motion but still keeps at the minimum level the energy consumption, vibrations, and interference.
Ii Ei lb, mo mi
ex ey €Wl
€Wl
C B
displacement of platform's mass center along X-inertial axis displacement of platform's mass center along Y -inertial axis rotation of platform's xy-body frame with respect to XY -inertial frame rotation of link 1 from platform's V-body axis to undeformed link 1 position rotation of link 2 from undeformed link 1 position to undeformed link 2 position (XO,YO, 00, 01,8 2 ,Ql1, Q12, Q::n, Q22)T, the generalized coordinates for the system (xo, Yo, ( 0)T, the generalized coordinates for the space station (8 O,)T, the rigid modes of two links " (ql11 Q12, Q21, Q22)T, the flexible modes of two links (Ul, U2?' vector of joint torque length of the ith link, i 1, 2 flexible displacement of link i area moment of inertia of link i Young's modulus of link i inertia of rigid hub of link i mass of the space station mass of link i unit vector along platform's x-body axis unit vector along platform's y-body axis unit vector perpendicular to undeformed link I position unit vector perpendicular to undeformed link 2 position damping matrix torque distribution matrix
=
By the assumed modes method, the flexible displacements can be approximated by
2. FORWARD DYNAMICS For the purpose of fixing notations, a very brief derivation of system dynamics of a simplified space robot is provided in this section. Figure 1 defines the reference coordinate frames for the whole system that consists of a rigid platform, representing the space station, and a robot arm with two flexible links. Both joints of the arms are considered to be revolute and input torque is applied at these joints. The flexible members of each link
n,
W,(Zi,t)
= 2:>'i;(Zi)Qi;(t) = "Tq"
Vi
= 1,2,
(1)
j=l
where 0'1j(Z') and 0'2;(Z,) are the jth admissible functions of link 1 and link 2 respectively. Two flexible modes are assigned to each link in this study. The absolute velocity vectors of a point on the platform, link 1 and link 2 can be expressed in terms e~, €Y' €Wl
92
,, 8, Body Frame x
,
w
y (X ."
Y,, )
Inertial Frame
Fig. I. Flexible Space Robot and
ew~
1
components respectively as follows:
Vo(x],yil
= (cos90xn+sin 90Yo -
2",'pT M(,p),'p
'
1
= LT;,
and 2".pT K,p
= LP;. 2
i=O
x]00)e.+
i=l
Then, system equation of motion is given by
+(-8inOo%0 + cosOoYO + y]Oo)e, where
V,(z,)
= (cosOoxo+ sinOoyo -
hOo)e. +
H(.p ,,'p) d~ M,'p -
+( - sin 90ro + co,Boyo + d9 0 )e, + +(z]OO
II("" ,'p), which represents the centrifugal and the
Cariolis forces, is defined by
+ ,,0] + wl(,il)e w )
~ a(,'pT ,:,/.p),'p)
The angular coordinates for the end-point of each link are defined to be the system's output vector
[8 + I
V2('2) = V,.e. + V"e, + (/,0 0 + 1]0, + w,(I,»e w ) +(Z2 90 + Z,Ol + z, 8, + w,(z,))e w ,·
+
With these! t he kinetic energy of the platform and each link is given, respectively. by
T.1=2!
Jv.. . v..
(~o)
--
arctan (
Vi - 1" 2, 3
I
8, + 8, + arctan(
]
~2 »
1V212,t
.
(3)
Wi(I;,t» l;
~
(Wi(/;, t» li
1
Vi=1,2.
Thus, we can write output equation as
j=O
where ho := O. The potential energy in each link due to elasticity is
are t an ( w,(/"t» (
For small elastic deformation
2
+ !/b - -J ' 1dm· 12 ,L
m.
y-
y
= D!/;,
(4)
where matrix D is defined accordingly.
r.
Pi
= 2"1
J "
Edi( wi (z;,i)) 2 dz; . Vi
= 1,2,
3. OPTIMAL MOTION CONTROL
o where w;' is the second order partial derivative with respect to z,. Define the mass/inertia m a.trix M(fjJ ) and the stiffness matrix J( by
The mission of spacecra.ft usually consists of taking its manipulator from an init.ial configuration to a final desired configuration. Trajectory planning is usually first.
93
designed to provide a planned path to realize the change of configurations. The planned referen ce t raj ectory together with the system dynamics then determines the I:t.b8olutc best that an ideal controller can do. This is measured in terms of the amount of control effort\ the maneuvering time, the structural vibration , and the interference from the arm to the space station. Consequently, optimizing the reference trajectory is expected to lead to overall better motion control performance. Thus, the optimal motion control design caD be characterized by the following optimization problem with the initial and the final config urations specified by Yo and Yt respectively. For notational convenience , we define the energy part of the performance index as +00
J oo " " =
+00
J
IIq(t)II, dt
-
+
J
lIu (t)1I2 dt.
(5)
-00
Optimal Motion Control: min t, u
to
+ J~Yler9 !1
subject to Vi $ io,
y(t)=yO,
and
y(i)=Yf ,
W;:::if
Ilq(t)11 $ '" Ilu(t)11 $ 'u, Ilv(I)11 $ '" Ilv(i)11$ ';, and system dynamics:
M(1/J) ~
+ H( 1/J,,j, ) + C,j, + /( .p = Bu , y=D1/J,
and to is given.
Here 11 . 11 in constraints is taken to be the componentwise infinity norm, and to and t, are defined as t he initial and the final time of maneuver reRpectively,
min If - io
••
+
1 s,
min u
t. y(i)
= Yd(t),
subject to
IIq(t)1I $ '., lIu(i)1I $ '. , IIv(t)1I $ eO. 1I,;(t)1I $ '. , and system dynamics:
M(1/J)~ + H(1/J,,j,) + C,j, Yd
+ /(t/J =
Bu,
= D~'.
and tu, Yd(t O) and Yd(tJ) are give n,
Notice that the only constraint 011 the inner optlmizar tion is an output tracking requirement and all other constraints are left to the outer optimization. Hence, the inner opt.imization i!; actually an unconstrained exact output tracking control problem with energy minimization, the vibrations energy and control energy. The newly developed stable inversion theory provides a solution that precisely addresses these issues. For any given reference trajectory, it has been shown t hat the stable inversion leads to a control strategy that guarantees exactly outpu t tra.cking with internal stability. Furthermore, among all controls which generate the desired referen ce trajectory, the one being the solution of stable inversion has the minimum energy, and the corresponding internal dynamics (q and v) are also of minimum energy, Thus, the inner optimization is a.utomatically solved by using stable inversion , and the two-stage optimal motion control problem lS therefore reduced to the outer stage optimal trajectory planning problem . In order to apply the stable inversion theorYl It IS required that the system should ha.ve a well-defined relative degree and its zero dynamics should have a hyperbolic equilibrium point at the origln. The first condition is well satisfied since the two components of the output chosen uniformly have relative degree 2. Howe\'er, the hyperbolicity condition is not satisfied in this flexible space robot case. Fortun ately! this problem can be avoided when pursuing only suboptimal solutions as discussed in the next section.
Notke t.hat the hard (active) constraint.s make the above optimization problem difficult to handle. Following a common practice in th e m anipulator control area, wc attack the optimal motion control problem in two steps. In this approach, a trajectory planning is fir~t carried out to find out a reference trajectory satisfying the hard co nstraints required on t he initial an d the final time. Then. a motion cootrol strategy i.s designed to realize the reference trajed ory. Optimization over the control stra.tegy as well as over t he referenee traject.ory will both be carried out.
Let q.(t, Vd) , v.(t, Yd), v.(t, Vd), and u.(I, Vd) denote the solutions of stable inversion which solve the inner-stage optimization problem . The above two-stage optimal motio n control problem is then Tc:du ced to the following optimal trajectory planning problem : Optimal Trajectory Planning:
Following this idea, it is easy to see that the a.bove opti-
mint J - to
mal motion control problem is equiva.lent to the following two-stage optimization problem. Two-Stage Optimal Motion Control:
.,
subject to
94
=
II v.(t , Yd)1I :os <., 1I ';.(t, Yd)1! :os <., and to , Yd(tO) and Yd(tj) are given_
Noticed that in the performance index, we have used the Cad that stable inversion automatically solves the minimum energy property of q and u.
4_ A SUBOPTIMAL SOLUTION 1t is clear that the above optimal trajectory planning problem is an infinite dimensional search. It is first simplified by taking the desired output reference trajectory Yd(t) with specified initial and final values as a linear combina.tion of some base time functions. Hence, by searching over a finite coefficient space, we can simplified the optimization problem into a finite dimensional one . It can be easily verified that choosi ng sinusoidal ba..~ fun ctions as follows is a valid parameterization provided that t he sum of the coefficients equals to one
= Vd,(tO) + [Yd, (tj) - Yd,(tO)]-
(6)
Now, the stable inverses need to be found through stable inversion . This is done by linearizing the zero dynamics at any equilibrium point and solving it a.nalytically. The solution is obtained by decoupling t.he hyperbolic and nonhyp erbolic parts of the linearized zero dynamics. By doing t his, only a suboptimal solution will be achieved in the trajectory planning. To obtain the optimal solution , the original motjon con~ trol problem has to be modified s uch that the inner stage optimization on ly involves the hyperbolic part of t he zero dynamics since the flexible space robot system has a non hyperbolic zero dynamics. But due to the nonlinearity, both pttrts of the zero dynamics are lightly coupJed. Thus, this op timal solution approach needs further study. From system's forward dynamics, t.he. linearized zero dyna.mics can be derived as follows:
Mu ii + M ,2 q + M 15 Yd M21 i!
=
= 0,
+ M"ij + M 23 q+ M,.q + M'5Yd
iJr
From the requirement of stable inversion we know that the hyperbolic part of the zero dyn amics lies in unstable manifold at time tOl and in stable manifold at time l" that is,
ii. (to) = 0,
and q,(t j)
= O.
(ll)
Solving the above two-point boundary value problem ( 10)-( 11) leads to solution q. = T,(C), where C denotes the set containing coefficients Cilt for all i = 1, ... , nand k 1,2_ Int,egTations of equation (7) will bring us
=
" . := T.(C) ,
and ,
v.:=T.(C) _
(1 2)
Through forward dynami cs, the mapping from reference output to optimal control input can also be obtained as u. := Tu(C )_
Ya.(CI." ... ,cnk,t)
~ -sm co, - (2 71"%--- t - to )] t --to - - - L.. [ t f - to i= t 21ri t j - to
where ii (qT, qT?, and A and B are defined accordingly_ Then, dynamics (9) is decoupled into Btable and unstable parts
(7)
= 0,
(8)
=
where Yd (yr , ,iiD'F , ry (uT , qT f , and the coefficients are all constant matrices. Eliminating v from the above two equatio ns the hyperbo lic part of t he zero dynamics with sta.te space (o rm caD be obtained as follows
With all these derivations, (see (ZC96, ) for more details), our optimal trajectory planning problem is further simplified to the following version: SUboptimal Trajectory Planning: min tf - to
(13)
c
subject to
1IT,(C) II :os '"
IITu(C)II:OS 'u ,
liT. (C)II :os '., IIT. (C)II :os '., and t o is given.
Thus, by applying stable inversion theory, the onglna.l extremely difficult optimal motion control problem, whic.h is a two-stage functi onal optimization problem, has been reduced to the above suboptimal trajectory planning problem which is a fini te dimensional problem. This problem may be solved easily by evoking some software package available at present, such as the Ma.tlab Optimization Toolbox. The solution provided by any algorithm is globally optimal which is guaranteed by the convexity of the performance index on the parameters C for fixed n_ This
Yd(tO)
(9)
95
= [ -450
1,
and,
1
400 . Yd(tj)= [ 1400
minimum amount of control effort, and t akes t he shortest possible time to achieve the desired change of configurations. A simulation result is also presented to demonstrate the effectiveness of the method to achi eve the above goals.
~~o===-~~--~~----=~~--=w~--~,oo~--~,~~-" IIirM (MOOnd)
6 . REFERENCES
""
r= ..
~ Ba
" "
-'~~--':""'-'--:..~----O.:;;-"----:,,::::-,,'---;,= ..~.j lime(s.aond]
Fig. 2. Suboptimal Trajectory Planned The: Ruooptimal trajectory is found in Figure 2. A comparison study is made which is summarized in table l. The first row corresponds t.o the suboptimal trajectory_ The second corresponds to a heuristic trajectory with sinusoidal acceleration which fultills the desired configuration change. Stable inversion is used for con trol. The third corresponds to the same trajectory as the second with a PD controller implemented . It is seen that the heuristic trajecto ry requires much more con trol energy and exhibits more struct ural vibrations. More simulation results are reported in (ZC96, ).
Table 1. A Com parison on Energy
Trajedory
1
Control
IlIq(t)lh IlIu(t)lh
Subopt.
Stable Inv.
0.0060
0.5338
Heuristic
Stable Inv.
0.0195
1.6711
Heuristic
PO Control
0.0212
1.8121
1
5. CONCLUSION This paper presents a new optimal motion control strategy for flexible space manipulators. The motion control des ign is divided into two parts. First, a two-stage functiona] optimization problem is formulated to characterize the optimal m otion control problem , and it is then simplified into a finite dimensional trajectory planning opti mization problem by applying the stable inversion theory. The motion control with so generated optimal reference trajectory minimizes t.he excited structural vibrations of the flexible links and the interference on the space station during the maneuvp.c. It also requires a
[Bay87] E. Bayo. A finite-element approach to control the end-point motion of a single-link flexible robot. 1. Robotic Systems, 4(1):63- 75, 1987. [C he93] D. Chen. An iterative solution to stable inversion of non linear Donrninimum phase systems. Proceedings of American Control Conference, pages 2960-2964 , 1993. [CP92] D. Chen and B. Paden . Stable inversion of noulinear nonminimum phase systems. Proceedings of Japan/USA Symposium on Flexible Automation, pages 791-797, 1992. [MC921 L. Meirovitch and Y. Chen. Trajectory and control optimi:6ation for flexible space robots. Proceedings oJ AJAA Guidance, Navigation and Control Conference, pages 1625-1638, 1992. [ML94] L. Meirovitch and S. Lim. Maneuvering "nd control of flexible space robots. Journal of Guidance, Control and Dynamics, 17(3):520527, 1994. INUY92] D. Nenchev, Y. Umetani, and K. Yoshida. Analysis of a redundant free-flying spacecraft/manipulator system. IEEE Trons. on Robotics and Automation, 8(1) :1-6, 1992. [PCLB93] B. Paden, D. Chen, R. Ledesma, and E. Bayo. Exponentially stable tracking control for multi-link flexible manipulators. ASME J. of Dynamics, Measureme.nt and Controll 115(1):53 59, 1993. [V087] Z. Vafa and S. Ooubowsky. On the dynamics of manipulators in space using the virtual manipulator approach . Proceedings of IEEE Intent. Con! on Robotics and Automation, 1987. [ZC94] H. Zhao and O. Chell. Minimum-energy approach to stable inversion of nonminimum phase systems. Proceedings DJ American Control Conference, pages 2705-2709, 1994 . [ZC96] H. Zhao and D. C hen. Optimal motion planning for flexible space robots. To be presented at th e IEEE Intern. Conf. on Robotics and Automation, 1996.
96
Copyright © 19% IFAC 13th Triennial World Congress, San Francisco, USA
MOTION CONTROL OF FLEXIBLE SPACE ROBOTS USING OPTIMAL TRAJECTORY PLANNING AND STABLE INVERSION 1 Hongchao Zhao
Degang Chen'
• Dept. of EledNcal and Computer Engineering Iowa State Univt1'Sity
Ames, lA 50011 , USA
Abstract. A space robot is typically controlled to move extremely slowly in order to avoid structural vibratio ns, to minimize interference with the spacecraft, and to reduce energy co nsumption, This paper investigates a new motion controL stra.tegy to achieve the fastest motion without violating these goals, First, the dynamics eq uation of a flexible space robot is developed using the assumed modes method . Then , the optimal motion control problem is formulated as a two-stage functional optimization problem. The inner stage is solved by a newly developed stable inversion theory. The outer stage then becomes an optimal trajectory planning problem that is further simplified into a finite dimensional optimization problem. Simulation results are finally presented, Keywords. Inversion , Trajectory Pla.nning, Flexible Arms, Space Robotks
1. INTRODUCTION
A typical task for a robot manipulator mounted on a space station would be to grasp an object, say a satellite. from space and put it into the space station, or vice Versa. Because of high fuel costs of bringing the robot into space and limited solar energy supply by space stations, space manipulatofl:; are always flimsy and flexible, a.nd therefore often exhibit extremely under~damped vibration modes. The structural flexibility and limited solar energy impose great challenges to the precise manipulator motion control. First, it is dear that any control strategy has to result in a minimum energy consumption because of limited resou rce, Secondly, any movement of the manipulator arm would transmit an undesirable interference force from th e ann to the space station. Finally, any control forces or disturbances applied to the 1 T his work is partially supported by Nationa.l Science Founda-
t ion under Grant No. ECS-9410646 and by Iowa Space Grant COMor'ium.
manipulator are very likely to excite structural vibrations in the arm as well as in the space station . Therefore, a. good motion control design for a space manipulator should have the following properties: 1) Achieving a desired. motion with the shortest possible time; 2) Not exciting structural vibrations; 3) Using a minimum amount of energy; and 4) Producing the minimum interference on the space station. Though robotics ha...;; been an active research area for the past few decades, applications are concerned primarily with massive earth-bound industrial robots. On the other hand, investigations concerning space robots ha.ve been mostly carried out by considering the rigid links assumption (NUY92. ). By assuming relatively small elastic vibrations, a perturbation approach is utilized to design separate motion controllers for the rigid and the flexible parts (ML94.) . Using reaction wheels or attitude control jets (VD87 , ). th e effect of interference from manipUlator motion on the space station can be compensated. Another method to reduce the interference is
91
to include the space station in the trajectory planning or to use kinematic redundancy to optimize the robot trajectory (MC92, ). All the methods either lead to slow motion in order to keep down energy consumption and vibrations excitation, or neglect the transient impact on the space station.
A fundamentally different approach to the tracking control of flexible structures is by using noncausal inversion. The idea was first presented by Bayo (Bay87, ) for solving the inverse dynamics for one-link flexible robots. Since one-link robots are linear systems, the fast Fourier transform method worked successfully. By using feedback linearization and locally exponenLially stable joint controllers, the method was extended to the nonlinear cases, the multi-link flexible robots. Successful experimental results demonstrated the efficiency of the approach (PCLB93, ). With these results, the stable inversion concept was first mtroduced to design exact and stable output tracking c,ootrol for a general class of nonlinear nonminimum phase systems (CP92, ). An iterative linearization algorithm was also developed for consLructing such stable inverses (Che93, ). Furthermore, it has been recently proved that th~ stable inverses have a nice minimum energy property (ZC94, ). Specifically, the stable inversion can achieve a given reference trajectory using a minimum amount of control energy and causing a minimum amount of internal vibrations.
are assumed to be slender such that the Euler- Bernoulli beam assumption is valid. Planar maneuver is assumed so that we neglect the out-of-plane deflections. We also neglect any possible effect from the sun and the earth, which means there is no external force on the system. For convenience, part of the notations are summarized in the following list: Xo
Yo 00
0, O2 'If;
v 8 Q U
I; Wi
In this paper! we investigate a new motion control strategy by using the stable inversion approach for space manipulators with all flexible links and no control jets or reaction wheels. The problem is attacked in two steps in this study. First, for a given trajectory the stable inversion theory is used to find the optimal control strategy that guarantees precise output tracking and at the same Lime ensures minimal structural vi brations and interference on the space station. Furthermore, it is also guaranteed that the control uses a minimum amount of energy. In the second step, an optimization problem is solved that searches for the best trajectory which leads to the fastest motion but still keeps at the minimum level the energy consumption, vibrations, and interference.
Ii Ei lb, mo mi
ex ey €Wl
€Wl
C B
displacement of platform's mass center along X-inertial axis displacement of platform's mass center along Y -inertial axis rotation of platform's xy-body frame with respect to XY -inertial frame rotation of link 1 from platform's V-body axis to undeformed link 1 position rotation of link 2 from undeformed link 1 position to undeformed link 2 position (XO,YO, 00, 01,8 2 ,Ql1, Q12, Q::n, Q22)T, the generalized coordinates for the system (xo, Yo, ( 0)T, the generalized coordinates for the space station (8 O,)T, the rigid modes of two links " (ql11 Q12, Q21, Q22)T, the flexible modes of two links (Ul, U2?' vector of joint torque length of the ith link, i 1, 2 flexible displacement of link i area moment of inertia of link i Young's modulus of link i inertia of rigid hub of link i mass of the space station mass of link i unit vector along platform's x-body axis unit vector along platform's y-body axis unit vector perpendicular to undeformed link I position unit vector perpendicular to undeformed link 2 position damping matrix torque distribution matrix
=
By the assumed modes method, the flexible displacements can be approximated by
2. FORWARD DYNAMICS For the purpose of fixing notations, a very brief derivation of system dynamics of a simplified space robot is provided in this section. Figure 1 defines the reference coordinate frames for the whole system that consists of a rigid platform, representing the space station, and a robot arm with two flexible links. Both joints of the arms are considered to be revolute and input torque is applied at these joints. The flexible members of each link
n,
W,(Zi,t)
= 2:>'i;(Zi)Qi;(t) = "Tq"
Vi
= 1,2,
(1)
j=l
where 0'1j(Z') and 0'2;(Z,) are the jth admissible functions of link 1 and link 2 respectively. Two flexible modes are assigned to each link in this study. The absolute velocity vectors of a point on the platform, link 1 and link 2 can be expressed in terms e~, €Y' €Wl
92
,, 8, Body Frame x
,
w
y (X ."
Y,, )
Inertial Frame
Fig. I. Flexible Space Robot and
ew~
1
components respectively as follows:
Vo(x],yil
= (cos90xn+sin 90Yo -
2",'pT M(,p),'p
'
1
= LT;,
and 2".pT K,p
= LP;. 2
i=O
x]00)e.+
i=l
Then, system equation of motion is given by
+(-8inOo%0 + cosOoYO + y]Oo)e, where
V,(z,)
= (cosOoxo+ sinOoyo -
hOo)e. +
H(.p ,,'p) d~ M,'p -
+( - sin 90ro + co,Boyo + d9 0 )e, + +(z]OO
II("" ,'p), which represents the centrifugal and the
Cariolis forces, is defined by
+ ,,0] + wl(,il)e w )
~ a(,'pT ,:,/.p),'p)
The angular coordinates for the end-point of each link are defined to be the system's output vector
[8 + I
V2('2) = V,.e. + V"e, + (/,0 0 + 1]0, + w,(I,»e w ) +(Z2 90 + Z,Ol + z, 8, + w,(z,))e w ,·
+
With these! t he kinetic energy of the platform and each link is given, respectively. by
T.1=2!
Jv.. . v..
(~o)
--
arctan (
Vi - 1" 2, 3
I
8, + 8, + arctan(
]
~2 »
1V212,t
.
(3)
Wi(I;,t» l;
~
(Wi(/;, t» li
1
Vi=1,2.
Thus, we can write output equation as
j=O
where ho := O. The potential energy in each link due to elasticity is
are t an ( w,(/"t» (
For small elastic deformation
2
+ !/b - -J ' 1dm· 12 ,L
m.
y-
y
= D!/;,
(4)
where matrix D is defined accordingly.
r.
Pi
= 2"1
J "
Edi( wi (z;,i)) 2 dz; . Vi
= 1,2,
3. OPTIMAL MOTION CONTROL
o where w;' is the second order partial derivative with respect to z,. Define the mass/inertia m a.trix M(fjJ ) and the stiffness matrix J( by
The mission of spacecra.ft usually consists of taking its manipulator from an init.ial configuration to a final desired configuration. Trajectory planning is usually first.
93
designed to provide a planned path to realize the change of configurations. The planned referen ce t raj ectory together with the system dynamics then determines the I:t.b8olutc best that an ideal controller can do. This is measured in terms of the amount of control effort\ the maneuvering time, the structural vibration , and the interference from the arm to the space station. Consequently, optimizing the reference trajectory is expected to lead to overall better motion control performance. Thus, the optimal motion control design caD be characterized by the following optimization problem with the initial and the final config urations specified by Yo and Yt respectively. For notational convenience , we define the energy part of the performance index as +00
J oo " " =
+00
J
IIq(t)II, dt
-
+
J
lIu (t)1I2 dt.
(5)
-00
Optimal Motion Control: min t, u
to
+ J~Yler9 !1
subject to Vi $ io,
y(t)=yO,
and
y(i)=Yf ,
W;:::if
Ilq(t)11 $ '" Ilu(t)11 $ 'u, Ilv(I)11 $ '" Ilv(i)11$ ';, and system dynamics:
M(1/J) ~
+ H( 1/J,,j, ) + C,j, + /( .p = Bu , y=D1/J,
and to is given.
Here 11 . 11 in constraints is taken to be the componentwise infinity norm, and to and t, are defined as t he initial and the final time of maneuver reRpectively,
min If - io
••
+
1 s,
min u
t. y(i)
= Yd(t),
subject to
IIq(t)1I $ '., lIu(i)1I $ '. , IIv(t)1I $ eO. 1I,;(t)1I $ '. , and system dynamics:
M(1/J)~ + H(1/J,,j,) + C,j, Yd
+ /(t/J =
Bu,
= D~'.
and tu, Yd(t O) and Yd(tJ) are give n,
Notice that the only constraint 011 the inner optlmizar tion is an output tracking requirement and all other constraints are left to the outer optimization. Hence, the inner opt.imization i!; actually an unconstrained exact output tracking control problem with energy minimization, the vibrations energy and control energy. The newly developed stable inversion theory provides a solution that precisely addresses these issues. For any given reference trajectory, it has been shown t hat the stable inversion leads to a control strategy that guarantees exactly outpu t tra.cking with internal stability. Furthermore, among all controls which generate the desired referen ce trajectory, the one being the solution of stable inversion has the minimum energy, and the corresponding internal dynamics (q and v) are also of minimum energy, Thus, the inner optimization is a.utomatically solved by using stable inversion , and the two-stage optimal motion control problem lS therefore reduced to the outer stage optimal trajectory planning problem . In order to apply the stable inversion theorYl It IS required that the system should ha.ve a well-defined relative degree and its zero dynamics should have a hyperbolic equilibrium point at the origln. The first condition is well satisfied since the two components of the output chosen uniformly have relative degree 2. Howe\'er, the hyperbolicity condition is not satisfied in this flexible space robot case. Fortun ately! this problem can be avoided when pursuing only suboptimal solutions as discussed in the next section.
Notke t.hat the hard (active) constraint.s make the above optimization problem difficult to handle. Following a common practice in th e m anipulator control area, wc attack the optimal motion control problem in two steps. In this approach, a trajectory planning is fir~t carried out to find out a reference trajectory satisfying the hard co nstraints required on t he initial an d the final time. Then. a motion cootrol strategy i.s designed to realize the reference trajed ory. Optimization over the control stra.tegy as well as over t he referenee traject.ory will both be carried out.
Let q.(t, Vd) , v.(t, Yd), v.(t, Vd), and u.(I, Vd) denote the solutions of stable inversion which solve the inner-stage optimization problem . The above two-stage optimal motio n control problem is then Tc:du ced to the following optimal trajectory planning problem : Optimal Trajectory Planning:
Following this idea, it is easy to see that the a.bove opti-
mint J - to
mal motion control problem is equiva.lent to the following two-stage optimization problem. Two-Stage Optimal Motion Control:
.,
subject to
94
=
II v.(t , Yd)1I :os <., 1I ';.(t, Yd)1! :os <., and to , Yd(tO) and Yd(tj) are given_
Noticed that in the performance index, we have used the Cad that stable inversion automatically solves the minimum energy property of q and u.
4_ A SUBOPTIMAL SOLUTION 1t is clear that the above optimal trajectory planning problem is an infinite dimensional search. It is first simplified by taking the desired output reference trajectory Yd(t) with specified initial and final values as a linear combina.tion of some base time functions. Hence, by searching over a finite coefficient space, we can simplified the optimization problem into a finite dimensional one . It can be easily verified that choosi ng sinusoidal ba..~ fun ctions as follows is a valid parameterization provided that t he sum of the coefficients equals to one
= Vd,(tO) + [Yd, (tj) - Yd,(tO)]-
(6)
Now, the stable inverses need to be found through stable inversion . This is done by linearizing the zero dynamics at any equilibrium point and solving it a.nalytically. The solution is obtained by decoupling t.he hyperbolic and nonhyp erbolic parts of the linearized zero dynamics. By doing t his, only a suboptimal solution will be achieved in the trajectory planning. To obtain the optimal solution , the original motjon con~ trol problem has to be modified s uch that the inner stage optimization on ly involves the hyperbolic part of t he zero dynamics since the flexible space robot system has a non hyperbolic zero dynamics. But due to the nonlinearity, both pttrts of the zero dynamics are lightly coupJed. Thus, this op timal solution approach needs further study. From system's forward dynamics, t.he. linearized zero dyna.mics can be derived as follows:
Mu ii + M ,2 q + M 15 Yd M21 i!
=
= 0,
+ M"ij + M 23 q+ M,.q + M'5Yd
iJr
From the requirement of stable inversion we know that the hyperbolic part of the zero dyn amics lies in unstable manifold at time tOl and in stable manifold at time l" that is,
ii. (to) = 0,
and q,(t j)
= O.
(ll)
Solving the above two-point boundary value problem ( 10)-( 11) leads to solution q. = T,(C), where C denotes the set containing coefficients Cilt for all i = 1, ... , nand k 1,2_ Int,egTations of equation (7) will bring us
=
" . := T.(C) ,
and ,
v.:=T.(C) _
(1 2)
Through forward dynami cs, the mapping from reference output to optimal control input can also be obtained as u. := Tu(C )_
Ya.(CI." ... ,cnk,t)
~ -sm co, - (2 71"%--- t - to )] t --to - - - L.. [ t f - to i= t 21ri t j - to
where ii (qT, qT?, and A and B are defined accordingly_ Then, dynamics (9) is decoupled into Btable and unstable parts
(7)
= 0,
(8)
=
where Yd (yr , ,iiD'F , ry (uT , qT f , and the coefficients are all constant matrices. Eliminating v from the above two equatio ns the hyperbo lic part of t he zero dynamics with sta.te space (o rm caD be obtained as follows
With all these derivations, (see (ZC96, ) for more details), our optimal trajectory planning problem is further simplified to the following version: SUboptimal Trajectory Planning: min tf - to
(13)
c
subject to
1IT,(C) II :os '"
IITu(C)II:OS 'u ,
liT. (C)II :os '., IIT. (C)II :os '., and t o is given.
Thus, by applying stable inversion theory, the onglna.l extremely difficult optimal motion control problem, whic.h is a two-stage functi onal optimization problem, has been reduced to the above suboptimal trajectory planning problem which is a fini te dimensional problem. This problem may be solved easily by evoking some software package available at present, such as the Ma.tlab Optimization Toolbox. The solution provided by any algorithm is globally optimal which is guaranteed by the convexity of the performance index on the parameters C for fixed n_ This
Yd(tO)
(9)
95
= [ -450
1,
and,
1
400 . Yd(tj)= [ 1400
minimum amount of control effort, and t akes t he shortest possible time to achieve the desired change of configurations. A simulation result is also presented to demonstrate the effectiveness of the method to achi eve the above goals.
~~o===-~~--~~----=~~--=w~--~,oo~--~,~~-" IIirM (MOOnd)
6 . REFERENCES
""
r= ..
~ Ba
" "
-'~~--':""'-'--:..~----O.:;;-"----:,,::::-,,'---;,= ..~.j lime(s.aond]
Fig. 2. Suboptimal Trajectory Planned The: Ruooptimal trajectory is found in Figure 2. A comparison study is made which is summarized in table l. The first row corresponds t.o the suboptimal trajectory_ The second corresponds to a heuristic trajectory with sinusoidal acceleration which fultills the desired configuration change. Stable inversion is used for con trol. The third corresponds to the same trajectory as the second with a PD controller implemented . It is seen that the heuristic trajecto ry requires much more con trol energy and exhibits more struct ural vibrations. More simulation results are reported in (ZC96, ).
Table 1. A Com parison on Energy
Trajedory
1
Control
IlIq(t)lh IlIu(t)lh
Subopt.
Stable Inv.
0.0060
0.5338
Heuristic
Stable Inv.
0.0195
1.6711
Heuristic
PO Control
0.0212
1.8121
1
5. CONCLUSION This paper presents a new optimal motion control strategy for flexible space manipulators. The motion control des ign is divided into two parts. First, a two-stage functiona] optimization problem is formulated to characterize the optimal m otion control problem , and it is then simplified into a finite dimensional trajectory planning opti mization problem by applying the stable inversion theory. The motion control with so generated optimal reference trajectory minimizes t.he excited structural vibrations of the flexible links and the interference on the space station during the maneuvp.c. It also requires a
[Bay87] E. Bayo. A finite-element approach to control the end-point motion of a single-link flexible robot. 1. Robotic Systems, 4(1):63- 75, 1987. [C he93] D. Chen. An iterative solution to stable inversion of non linear Donrninimum phase systems. Proceedings of American Control Conference, pages 2960-2964 , 1993. [CP92] D. Chen and B. Paden . Stable inversion of noulinear nonminimum phase systems. Proceedings of Japan/USA Symposium on Flexible Automation, pages 791-797, 1992. [MC921 L. Meirovitch and Y. Chen. Trajectory and control optimi:6ation for flexible space robots. Proceedings oJ AJAA Guidance, Navigation and Control Conference, pages 1625-1638, 1992. [ML94] L. Meirovitch and S. Lim. Maneuvering "nd control of flexible space robots. Journal of Guidance, Control and Dynamics, 17(3):520527, 1994. INUY92] D. Nenchev, Y. Umetani, and K. Yoshida. Analysis of a redundant free-flying spacecraft/manipulator system. IEEE Trons. on Robotics and Automation, 8(1) :1-6, 1992. [PCLB93] B. Paden, D. Chen, R. Ledesma, and E. Bayo. Exponentially stable tracking control for multi-link flexible manipulators. ASME J. of Dynamics, Measureme.nt and Controll 115(1):53 59, 1993. [V087] Z. Vafa and S. Ooubowsky. On the dynamics of manipulators in space using the virtual manipulator approach . Proceedings of IEEE Intent. Con! on Robotics and Automation, 1987. [ZC94] H. Zhao and O. Chell. Minimum-energy approach to stable inversion of nonminimum phase systems. Proceedings DJ American Control Conference, pages 2705-2709, 1994 . [ZC96] H. Zhao and D. C hen. Optimal motion planning for flexible space robots. To be presented at th e IEEE Intern. Conf. on Robotics and Automation, 1996.
96