the helicopter is given by a fixed rotation R c : C!A. It is assued that the anti-torque associated with the rotor air resistance is known and differe

Transcription

1 Visual Servoing For A Scale Model Autonoous Helicopter A. CHRIETTE y, T. HAMEL y and R. MAHONY z y CEMIF z Dep. of Elec. and Cop. Sys. Eng. Université d'evry. Monash University, Clayton, Victoria, 4 rue du Pelvoux, Evry, France. 8, Australia. Abstract A control design to stabilise a reduced scale autonoous helicopter equipped with a caera is presented. The proposed algorith is otivated by recent work in iage-based visual servoing control for under-actuated dynaic systes. This work is extended byconsidering odels that contain weakly noniniu phase zero dynaics. This is an iportant class of systes since an offset between the caera and the centre of ass for a typical 'dynaic' autonoous vehicle will result in zero dynaics occurring in the iage dynaics. In this paper we propose a siplified odel of dynaics of the helicopter and show that by placing soe constraints in the choice of the position of the caera we can iniise the effect of the zero dynaics. Introduction Unanned Air Vehicles (UAVs) are becoing an exciting new application area for odern non-linear control theory. Several authors in this last decade have contributed to the study and developentofdynaics odels which are dedicated to the UAV[,4]. Their sall size, highly coupled dynaics and low cost ipleentation provide an ideal testing ground for sophisticated control techniques. One ajor proble that arises in the control of such vehicles is the difficulty of easuring non-inertial variables such as position, orientation and velocity. Acceleration and angular velocity ay be easured using accelerations and rate gyros. To overcoe the proble of regulating the position of a UAV without expensive ground based systes, a nuber of authors have considered using visual feedback. This kind of control is expressed by `Visual Servoing'. Early ipleentations of visual servoing algoriths were used to position a robotic anipulator relative to a fixed visual target []. More recently, visual servoing has been proposed for applications in obile robotics, for exaple in the control of a car-like vehicle [9, 6]. In all approaches feedback control is used to regulate an error function easured directly in the task space and derived fro the control objective. This approach does not generalise well to the case where the dynaics of the syste are iportant. Most existing applications exploit a high gain or feedback linearization (coputed torque) design to reduce the syste to a controllable kineatic odel, for which the iage-based visual servoing techniques were developed []. Recently Hael and Mahony [] presented a novel new algorith for visual servoing of an under-actuated dynaic rigid body syste based on exploiting the passivity-like properties of rigid body otion. In this paper we consider a siilar proble to that solved in []. However, the odel considered is extended to incorporate the possibility of zero dynaics in the syste resulting fro an offset between the caera and the centre of ass of the UAV. In practice, this effect is unavoidable and, since the zero dynaics of a Hailtonian syste are Hailtonian, ay result in sall non-decaying oscillations in the otion of the UAV. An idealized odel of an autonoous odel helicopter is considered as a base for the theoretical developent undertaken. The principal contribution of the paper is to show thatbychoos- ing a suitable position for the caera, it is possible to liit the effects of the zero dynaics. The paper is divided into five sections including the present overview. In Section an idealised odel of an autonoous helicopter is presented based on the Newton-Euler equations. In section the iage dynaics are derived. Section 4 shows how to derive the proposed control law using a robust backstepping techniques. Finally, in the section 5, we give soe siulations and results. Helicopter Model In this section a dynaic odel of autonoous helicopter equipped with a caera is proposed. The odel is derived fro Newton-Euler equations by assuing that the helicopter body is rigid. For details of the dynaic equations, please refer to [4] or [7]. Denote the body airfrae by letter A. let I = fe x ;E y ;E z g denote a right-hand inertial frae stationary with respect to the earth. Let the vectors ο = (x; y; z) T and ο c = (x c ;y c ;z c ) T denote respectively the position of the centre of ass of the helicopter and the focal point of the caera relative to the frae I: Let A = fe a ;Ea ;Ea g be a (right-hand) body fixed frae for A, andc = fe c ;Ec ;Ec g denote the caera fixed frae of the caera, (see fig. ). In addition, let R SO() be an orthogonal rotation atrix which give the orientation of the helicopter relative to the inertial frae R : A!I. The orientation of the caera relative to the body fixed frae of

2 the helicopter is given by a fixed rotation R c : C!A. It is assued that the anti-torque associated with the rotor air resistance is known and differentiable. The control input w is decoposed into two parts w = ~w + ~w where ~w is the coponent of the torque input that counter-acts air resistance of the rotors and ~w is the part that contributes directly to the control of the helicopter. To counter-act the anti-torque Q M e due to the ain rotor, choose ~w = QM l T. Let (I I )=". Thus, one has I _Ω = Ω Ω " + l T ~w Figure : Coponents of lift forces and offsets. In the present paper, it is preferable to work directly with the force inputs decoposed into orthogonal coponents and assue that each of these inputs is an independent control. As shown on figure (), the ain rotor lift is decoposed into three coponents ( w ;w ; u) Aorientated in the directions fe a ;Ea ;Ea g. Furtherore, the force due the tail rotor is always oriented in the second axis direction E a and is written (; w ; ) A. As shown in [8], by applying Newton's equations of otions, the dynaics of an autonoous helicopter are _ ο = v () _v = ure + ge w Re +(w w )Re () _R = Rsk(Ω) () I _Ω = Ω IΩ+(l M w l T w )e + l M w e + l T w e + Q M e + Q T e : (4) In the above dynaics, the two torque controls governing roll and pitch, Eq.(), generate sideways forces on the airfrae which introduce zero dynaics into the syste. Theses forces are known as sall body forces. In this odel, it is assued that l M = ; ; lm A, where l M > and l T = l T ; ; lt : A Reduced Model In this paper, it is proposed to find the optial position for the caera that will reduced the effects of the sall body forces. Assuing that the inertia atrix I is diagonal, then Eq.(4) ay be rewritten explicitly as follows: I _Ω = Ω Ω (I I )+(l M w l T w ); (5) I _Ω = Ω Ω (I I )+Q T + l M w ; (6) I _Ω = Ω Ω (I I )+Q M + l T w : (7) Assue that Ω () = (otherwise initially stabilise the yaw before coencing the control design) then by choosing ~w =Ω Ω " l T (8) one ensures that Ω foralltie. Choosing ~w = QT l, cancels the effect of the antitorque Q T (t). It is also necessary to cancel the contri- M bution to the first coponent torque due to the input ~w and non-zero offset lt. Set ~w = l T lm w = l T lt Q M ; (9) l M Based on the above choices, the equation of otion Eqn's -4 ay be written in a reduced for : _v = ure ~w Re +(~w ~w )Re () + ge + l M l T lm Q M Re + Q T l T lm Re I _Ω = (lm ~w lt ~w )e + lm ~w e : () The inputs for this syste are u, ~w and ~w. In addition, the control input ~w is used to stabilize Ω (t) at. All the ters in Eq. () directly independent of the control input are written separately in brackets. These ters for the disturbance to the translation dynaics, doinate by the gravitational force g, but also depending on the rotation atrix R as well as the rotor drag ters. A consequence of the perturbation ters in Eq.() is that the stationary point of the dynaics Eqn's (,, and ) corresponding to the helicopter hovering requires a non-trivial rotation R. Indeed, in stationary flight any helicopter ust fly with slight peranent inclination of the principal lift force ure to copensate for the sall body force resulting fro the torque control necessary to cancel rotor drag. It is iportant for control design in visual servoing to represent the dynaics of the helicopter in the caera fixed frae. Let v c be the velocity of focal point of the caera relative to the inertial frae, and let V c its representation in the caera fixed frae.

3 Let d R be a translation vector between (A) and (C), one has ο c = ο + Rd () Considering only a displaceent in the E a axis, that is d = d e =(; ;d ) T, the rotational atrix R c = I, the full linear dynaics of a helicopter expressed in the caera fixed frae are given by _ ο c = RV c ; () Vc _ = Ω (V c Ω d) ue (4) +gr T l e + M lt lm Q M e + Q T l T lm e + B@ ~w + d (~w ~w ) d I I l M ~w ~w l M ~w lt CA Fro this equation it is clear that the displaceent d ay bechosen such that the first entries of the right hand side of Eq.(5) are independent of the control action ~w. Set: d = I l M (5) In order, to cancel the centrifugal forces Ω (Ω d) introduced by the offset of the point ο c, define the following odified control μu μu = u + d (Ω ) +(Ω ) (6) By this choice and using Eq.(5), the reduced linear odel dynaics ay be written : _ο c = RV c ; (7) _ Vc = sk(ω) c V c μue + gr T e + where X = X + l M l T l M l T " ~w + l T I lm I I lm ~w e (8) Q M e + QT l M e and we assue that the control algorith has access to _X and Ẍ: In Eq.(8), the presence of the sall body forces, which couple torque inputs to the translational dynaics are only present in the second axis direction E a. The zero dynaics are generated by the additional control inputs ~w and ~w that are not exactly cancelled by the choice of caera position. In practice, the presence of the sall body forces destroysthe design of a robust control, in particular the backstepping procedure which use to give our control design. In the present paper and as proposed in [8, 4], to avoid this proble, we ignore the control ters contributing to the zero dynaics effect. Furtherore, as proposed in [8], we can give an analysis which shows that as long as the sall body forces are sufficiently sall the proposed control design in this paper will ensure the sae properties for the full syste. 4 Iage Dynaics Letasetofn points Pi of a fixed target, relativeto the inertial frae (I) and observed fro the caera, and Let P i the representation of Pi in the caera fixed frae, then P i is given by P i = R T (P i ο c) A backstepping control design has passivity-like properties fro virtual input to the backstepping error [5]. In a recent paper [] itwas shown that these structural passivity-like properties are present inthe iage space dynaics if and only if the spherical projection of an observed point is used. Denoting the spherical projection of an iage point P i by p i the iage dynaics are given by (see [] for ore details) _p i = sk(ω) c p i + ß p V i r i (9) Here r i = jp i j and V i Cis the observed velocity of the target point represented in the body fixed frae of the caera. The atrix ß p =(I p i p T i ) is the projection onto the tangent space of the spherical iage surface at the point p i. If the inertial velocity of the target point Pi is equal to zero then V i = V c, where V c is the velocity of the caera relative to the caera fixed frae. Thus Eq. (9) becoes: _p i = Ω p i ß p i r i V c Here the focal length of the spherical caera has been noralized to unity and it is assued that the target points are stationary in the inertial frae. We wish to stablize the caera to a desired pose (position and orientation) with respect to a target of interest. Let p Λ i denote the desired visual features, fixed relative to the inertial frae. The iage based error vector chosen is Let ffi =vect(p i p Λ i ) R n Π= B@ r (I p p T ). r n (I p n p T n ) CA : Coputing the first order dynaics of ffi, it yields _ffi = diag(sk(ω))ffi ΠV c : A coon approach taken in iage based visual servoing is to average the available visual inforation contained in the full error ffi. This is achieved by introducing a cobination atrix C that `codes' the averaging process and defining a reduced error ffi r = Cffi; C R n ()

4 Following the approach of Hael and Mahony [], the reduced error is only three diensional and it is ipossible to derive full pose inforation fro the reduced error. However, the reduced error does suffice to fully specify UAVs position and yields passivelike behaviour suitable for application of backstepping control design as long as the atrix C satisfies CΠ =Q>andCdiag( sk(ω)) = sk(ω)c; 8Ω R A atrix respecting these conditions will be of the for C ff =[ ff I ff n I ]; ff i ; i=;:::;n. Note that the property C ff Π=Q ff > relies on the fact that r i >. The reduced error vector considered is ffi = C ff ffi and the full dynaics of ffi are given by _ffi = sk(ω)ffi Q ff V c : () The exact value of Q ff is unknown, however, it is possible to obtain bounds on the eigenvalues of Q ff fro upper and lower bounds on distance of the UAV to the target [] and these are used in the control design. 5 Visual servoing for helicopter In this section, a control design robust backstepping techniques [5] is proposed for visual servoing of the idealised autonoous helicopter dynaics proposed earlier. Fro Eq.() and the reduced rigid body dynaics Eqn's (8, and ), the full dynaics of the error ffi ay be written: _ffi = sk(ω):ffi Q ff V c () _ Vc = Ω V c μue + gr T e + X () _R = Rsk(Ω) (4) I _Ω = (l M ~w l T ~w )e + l M ~w e (5) Define the first storage function S by: S = jffi j Deriving S and recalling Eq. () yields: _S = ffi T Q ffv c (6) If the velocity V c were available as a control input, then the choice of V c = ffi is sufficient to stabilise S. Thus the virtual control chosen for this step is V v c = k ffi (7) where k is a positive constant. Since the atrix Q ff is positive definite and if Vc v V c then S _ = k ffit Q ffffi is negative definite in ffi. Thus, Eq.() becoes _ffi = sk(ω)ffi k Q ffffi k Q ffffi ; (8) where ffi is a new error ter given by ffi = k V c ffi ; (9) the difference between the true and the desired velocity. The derivative ofs ay be written _S = k ffit Q ffffi k ffit Q ffffi () The second storage function considered is S = S + jffi j Taking the derivative of S Eq. (), yields _S = k ffit Q ffffi + k ffit Q ffffi and substituting for + k ffi T ( μue + gr T e + X) () Let ( μue + gr T e ) v the virtual control chosen for this step ( μue +gr T e ) v = k k ffi X. With this choice the derivative offfi is given by _ffi = sk(ω)ffi + k Q ffffi k (k I Q ff )ffi + k k ffi where ffi represents the third error ter ffi = k k ( μue + gr T e )+ k k ffi + X () In the foral processof backstepping the new error ffi would now be differentiated. This requires a foral tie derivative of the input μu. To provide this the input μu is dynaically extended μu =~u () The otivation for adding a double integrator here is to ensure that the relative degree of the new input ~u with respect to the position co-ordinates ο c is the sae as the relative degree of ~w, ~w and ~w with respect to the position. In the present derivation, all inputs are chosen to have relative degree four with respect to the position ο c. The derivative of the second storage function is: _S = k ffit Q ffffi k ffit (k I Q ff )ffi + k k ffit ffi (4) where k is chosen such thatk > ax (Q ff ). The backstepping process continued by taking the derivative offfi _ffi = sk(ω)ffi + k Q ffffi k (k I Q ff )ffi + k k ffi + _X k k k k ( _μue + sk(ω c )(μue X))

5 The virtual control chosen in this step is ( _μue +sk(ω)(μue X)) v = k k (k + k k ) ffi + _X: Using this choice yields _ffi = sk(ω)ffi + k Q ffffi (5) k (k I Q ff )ffi k ffi (k + k k ) ffi 4 : Here ffi 4 is the final error ter introduced in the design procedure ffi 4 = ( _μue + sk(ω)(μue X) _X) k k (k + k k ) ffi (6) Let S be a storage function associated with this step of the procedure S = S + jffi j (7) Deriving S and recalling Eq. (4) one obtains _S = k ffit Q ffffi k ffit (k I Q ff )ffi + k ffit Q ffffi + k ffit Q ffffi (k + k k ) ffi T ffi 4 k ffit ffi (8) The derivative offfi 4 is _ffi 4 = sk(ω)ffi k Q ffffi + k (k I Q ff )ffi + k ffi + (k + k k ) ffi 4 k k (k + k k ) Ẍ + k k (k + k k ) μue + sk( _Ω)(μue X) + k k (k + k k ) sk(ω)( _μue _X) At this stage the actual control inputs enter into the equations through μu = ~u and _Ω via Eq. (5). The final stage of the backstepping process is choose a control input. The following vector equation is solved for the control inputs μu and ~w. ~ue + _Ω c ( μue + X) = sk(ω c )( _μue + _X) Ẍ + k k (k + k k ) ( sk(ω c )ffi k k + k + k 4 ffi 4 + k k ffi ) (9) Given the abovechoice the dynaics of ffi 4 becoes _ffi 4 = k Q ffffi + k (k I Q ff )ffi + (k k + k ) k 4 ffi 4 (4) ffi The last storage function in the backstepping procedure is the Lyapunov candidate function S 4 = S + jffi 4j Deriving S 4 and recalling Eq.(8) one obtains _S 4 = k ffit Q ffffi k ffit (k I Q ff )ffi k ffit ffi + k ffit 4 (k I Q ff )ffi + k ffit Q ffffi + k ffit Q ffffi k 4 ffit 4 ffi 4 k ffit 4 Q ffffi Recall that the dynaics of the angular velocity of the caera expressed in the body fixed frae are given by Eq. (). Expanding the vector cross product and collecting ters then the left hand side of Eq. (9) ay be written as: B@ lm I ~w μu l M ~w l T ~w I μu ~u lm I ~w e T X + l M ~w l T ~w I e T X CA (4) Thus, as long as μu 6= there is a unique control input (~u; ~w ; ~w ; ~w ) that solves Eq. (9). In hover conditions, the condition μu 6= will always hold since μu corresponds to the heave force used to copensate gravitational acceleration. A second interesting observation is that the presence of the drag ters X introduce parasitic ters sk(ω)x which are cancelled by the input ~u. Finally, as indicated in the subsection., the control design is coplete when the the third input ~w free to fix Ω at zero. Proposition [] Consider the dynaics given by Eqn's (-5). Let the vector controller given by Eq.(9) and recover the original inputs fro Eq.(4). Then the ain objective ffi converges to zero if the control gains satisfy the following constraints : k > k > ax (Q ff ) k > k ax (Q ff ) in (Q ff ) + k in (Q ff ) ax (Q ff ) k 4 > k in (Q ff ) + k in (Q ff k ax (Q ff ) where in (Q ff ) and ax (Q ff ) represent the lower and upper bounds of Q ff. The proof of this proposition is a direct application of the principals of the backstepping based on the developent leading up the proposition stateent. 6 Siulation and results In this section we present a siulation with the ai to validate the proposed control design. The task considered is to position a caera relative to the planar square target. The target is odelled by four

6 z points on the vertices of the square. Naturally for this task, the signals available are the pixel co-ordinates of the four points observed by the caera, denoted f(u ;v ); (u ;v ); (u ;v ); (u 4 ;v 4 )g. The siulated spherical co-ordinates (x i ;y i ;z i ) of the four points are respectively given by x i = ui v, y i = i and p f +v i p f +v i p z i = f u i v i. For ore details the reader is referred to []. The desired iage feature are chosen such that the caera is located several eters above the square. It is defined by : f( a; a); (a; a); (a; a); ( a; a)g where a represents the ratio between thevertex length and the final desired range. In this siulation the paraeter a has been chosen equal to.485 corresponding to a location of six eters of the caera above the target. The agnitude of the initial force input is chosen to be u = g ß 77 corresponding to the fact that the helicopter is initially in hover flight. The initial position is: ο = T ; _ ο = and ffi = _ ffi =: The center of the target ^ο is chosentobe: ^ο = 6 T the choice of the values of in (Q ff ) and ax (Q ff ) depends respectively on initial conditions and the desired location, it follows that : in (Q ff )=:6 and ax (Q ff )=:686: Fro the above choice, we have used the following control gains: k =:; k =;k =;k 4 =,that satisfies conditions given in Proposition. Siulations results of the helicopter syste control algorith are given on figures and Figure : Evolution of the iage features in the iage plan. [] Hael, T., Mahony, R., Visual servoing of underactuated dynaic rigid body syste : An iage space approach", In Proceedings of the 9th Conference on Decision and Control,. [] Hutchinson, S., Hager, G., Cork, P., A tutorial on servo control". IEEE Transactions on Robotics and Autoation,, 5, (996), pp [4] Koo, J.T., Sastry, S., Output tracking control design of a helicopter odel based on approxiate linearization", In Proceedings of the 7th Conference on Decision and Control, Florida, Deceber 998. [5] Krstic, M., Kanellakopoulos, I., Kokotoviv, P.V, Nonlinear and adaptive control design", Aerican Matheatical Society, Rhode Islande, USA, 995. [6] Ma, Yi., Kosecka, J., Sastry, S., Vision guided navigation for a nonholonoic obile robot", IEEE Transactions on Robotics and Autoation, june (999) [7] Mahony, R., Hael, T., Dzul, A., Hover control via approxiate Lyapunov control for a odel helicopter", In Proceedings of the 999 Conference on Decision and Control, Phoenix, Arizona, U.S.A, y x 4 6 [8] Mahony, R., Lozano, R., Exact Tracking Control for an Autonoous Helicopter in Hover-like Manouvers", International Conference on Robotics and Autoation, 999. Figure : Positioning of the helicopter with respect to the target. References [] Espiau, B., Chauette, F., Rives, B., A new approach to visual servoing in Robotics", IEEE Transactions on Robotics and Autoation, 8, (99), pp. -6. [9] Pissard, G., Rives, P., Applying visuel servoing to control of a obile hand-eye syste", In Proceedings of the International Conference on Robotics and Autoation, Nagasaki, JAPAN, 995. [] Shaufelberger, W., Geering, H., Case study on helicopter control", Invited session in Control of Coplex Systes, (COSY), October 998.