VISUAL SERVOING WITH ORIENTATION LIMITS OF A X4-FLYER

Transcription

1 VISUAL SERVOING WITH ORIENTATION LIMITS OF A X4-FLYER Najib Metni,Tarek Hamel,Isabelle Fantoni Laboratoire Central es Ponts et Chaussées, LCPC-Paris France, najib.metni@lcpc.fr Cemif-Sc FRE-CNRS 2494, Evry France, thamel@iup.univ-evry.fr Heuiasyc UMR-CNRS 6599,Compiègne France, Isabelle.Fantoni@hs.utc.fr Keywors: Unmanne Aerial Vehicle, Homography estimation, Visual Servoing, Nonlinear control. Abstract In this paper, we stuy the ynamics an the control using visual features of a four rotor vertical take-off an laning (VTOL) vehicle known as the X4-flyer while stabilizing with quasistationary flight above a planar target. A new control strategy is presente using the homography matrix, it is base on saturation functions for bouning the orientation of the UAV (unmanne air vehicle) in orer to keep the target in the camera s fiel of view. 1 Introuction Unmanne air vehicles (UAV) are becoming of a major interest in moern control theories. Several authors have contribution in the evelopment of ynamic moelling [8, 6]. Their highly couple ynamics an their small size provie an ieal testing groun for many complex control techniques. But the problem that arises in this kin of applications is the ifficulty of measuring non-inertial variables as position, orientation, an linear velocity. One way of overcoming the problem is the use of vision sensor. Typically, a vision system onboar a UAV inclues a Global Positioning System (GPS), an Inertial Navigation Sensor (INS) an a high-tech camera. So, almost all control strategies are built aroun a vision sensor. Inee some works in the omain of control esign, eicate to helicopters, propose to use stereovision systems in the laning maneuvers of an UAV for the purpose of estimating the location an orientation of the helicopter laning pa. The vehicle consiere in this paper is an autonomous hovering system, capable of quasi-stationary, vertical take-of an laning in near hover flight conitions. All visual servoing techniques that involve reconstruction of the target pose with respect to the camera are calle: position base visual servoing (PBVS). This kin of techniques lea to a Cartesian motion planning problem. Its main rawback is the nee of a perfect knowlege of the target geometric moel. The secon class known as image base visual servoing (IBVS) aims to control the ynamics of features in the image plane irectly [7]. Classical IBVS methos have the avantage of being robust with respect to calibration errors, however they suffer from the high coupling ynamics between translation an rotational motion which makes the cartesian trajectory uncontrollable. Recently, in [5] a new algorithm for visual servoing of an uneractuate ynamic rigi boy system, such a helicopter, base on exploiting the passivity-like properties of rigi boy motion has been propose. In this paper we use the metho presente in [9] (2 1 2D visual servoing) that consists of combining visual features obtaine irectly from the image, an features expresse in the Eucliean space. More precisely, a homography matrix is estimate from the planar feature points extracte from the two images (corresponing to the current an esire poses). From the homography matrix, we will estimate the relative position of the two views. The purpose of this paper is to stuy how we can use the approach given in [9] to control an autonomous hovering system. First, we propose a simplifie ynamic moel of a four-rotor vertical take-off an laning (VTOL) vehicle known as the X4- flyer. Then we propose a control esign base on separating the translational from the rotational rigi boy (airframe) ynamics [6]. The control strategy is new because it takes into account a limit for the orientation of the X4-flyer in orer to keep the target in the camera s fiel of view. We will prove the stability of such strategy base on saturation functions. 2 The X4-flyer ynamic moel The X4-flyer is a system consisting of four iniviual electric fans, linke to a rigi cross frame as shown in Figure 1. It operates as an omniirectional UAV. Vertical motion is controlle by collectively increasing or ecreasing the power of all four motors. Lateral motion is achieve by controlling ifferentially the motors generating a pitch/roll motion of the airframe that inclines the collective thrust an leas to lateral acceleration. Yaw control is erive from the reactive couple applie to the airframe ue to rotor rag. Diagonally opposite rotors turn in opposite irections (cf. Fig. 1) leaing to a balance torque istribution in hover conitions. To apply yaw control, the spee of a pair of iagonal motors is increase while it is ecrease in the opposing pair. This generates a torque aroun the vertical axis while maintaining the total thrust. Let F = {E x, E y, E z } enote a right-han inertial or worl frame such that E z enotes the vertical irection ownwars into the earth. Let ξ = (x, y, z) enote the position of the centre of mass of the object in the frame F relative to a fixe origin in F. Let F = {E a 1, E a 2, E a 3 } be a (right-han) boy fixe frame for the airframe. The orientation of the airframe is given by a rotation R : F F, where R SO(3) is an orthogonal

2 rotation matrix. Figure 1: A prototype X4-flyer. Let V F enote the linear velocity an Ω F enote the angular velocity of the airframe both expresse in the boy fixe frame. Let m enote the mass of the rigi object an let I R 3 3 be the constant inertia matrix aroun the centre of mass (expresse in the boy fixe frame F ). Newton s equations of motion yiel the following ynamic moel for the motion of a rigi object: ξ = RV (1) m V = mω V + F (2) Ṙ = Rsk(Ω), (3) I Ω = Ω IΩ + Γ. (4) The notation sk(ω) enotes the skew-symmetric matrix such that sk(ω)v = Ω v for the vector cross-prouct an any vector v R 3. The vector forces an the vector torques are escribe as follows F = mgr T e 3 u 4 e 3 (5) Γ = u 1 e 1 + u 2 e 2 + u 3 e 3 (6) In the above notations, g is the acceleration ue to gravity. The inputs (u 1, u 2, u 3, u 4 ) are erive from the iniviual motor control signals (cf. [4]). an thus the projecte points obey 1 ( ) p i = R T + tn T p i, i = 1,..., k. (9) ( ) The projective mapping H := R T + tn T is calle a homography matrix, it relates the images of points on a target plane when viewe from two ifferent poses (efine by the coorinate systems F an F ). More etails on the homography matrix coul be foun in [4]. The homography matrix contains the pose information (R, ξ) of the camera. However, since a projective relationship exists between the image points an the homography, it is only possible to etermine H (using only image points equations) up to a scale factor. There are numerous approaches for etermining H, up to this scale factor, cf. for example [1]. Extracting R an t from H can be quite complex [9, 13, 12, 3]. However, one quantity r = can be calculate easily an irectly: r = 1 + nt t = et(h) = et(rt + tn T ). There are certain special cases where it is relatively straight forwar to compute important parameters from an unscale estimate of the homography matrix H. An important case is where the target plane is perpenicular to the line of sight of the worl frame (n = (0, 0, 1) T ). In this case, the first two columns of H are scale versions of the first two columns of R. This special case is particularly useful in stabilizing a UAV over a laning pa, as long as the camera is mounte beneath the vehicle so as the line of sight is vertically ownwar. This case will be use in the simulation (section 4). 3 Motion Estimation 3.1 Camera Projection an Planar Homography Visual ata is obtaine via a projection of real worl images onto the camera image surface. This projection is parameterize by two sets of parameters: intrinsic (i.e., those internal parameters of the camera such as the focal length, the pixel aspect ratio etc.) an extrinsic (the pose - position an orientation of the camera). The pose of the camera etermines a rigi boy transformation from the worl or inertial frame F to the camera fixe frame F (an vice-versa). One has P = RP + ξ (7) as a relation between the coorinates of the same point in boy fixe frame (P F ) an in the worl frame (P F ). Let p is the image of the point P an p is the image of the same point viewe when the camera is aligne with frame F (see fig.2). When all target points lie in a single planar surface one has P i = R T P i + tn T P i, i = 1,..., k, (8) Figure 2: Camera projection iagram showing the esire (F ) an the current (F ) frames 3.2 Visual servoing control strategy In this section, visual servoing is base on Lyapunov control esign an on saturation functions. By exploiting equations (1)- 1 Most statements in projective geometry involve equality up to a multiplicative constant enote =.

3 (4), we erive a control strategy for limiting the robot orientation. To simplify the erivation, it is assume that the camera fixe frame coincies with the boy fixe frame F. Let P enote the observe point of reference of the planar target, an P be the representation of P in the camera fixe frame at the esire position (Figure 2). The ynamics associate with the stabilization of the camera aroun the esire position P fully etermine two egrees of freeom (pitch an roll) in the attitue of the airframe. The yaw of the airframe must be separately assigne. In this paper we use the classical yaw, pitch an roll Euler angles (φ, θ, ψ) commonly use in aeroynamic applications [10]. Although these angles are not globally efine they provie a suitable local representation for all quasi-stationary manoeuvres unertaken by an X4-flyer. The yaw angle trajectory is specifie irectly in terms of the angle φ, the esire yaw angle. The relationship between the Euler angles use an the rotation matrix is 2 R = c θc φ s ψ s θ c φ c ψ s φ c ψ s θ c φ + s ψ s φ c θ s φ s ψ s θ s φ + c ψ c φ c ψ s θ s φ s ψ c φ. (10) s θ s ψ c θ c ψ c θ The visual servoing problem consiere is: Fin a smooth state feeback (u 1, u 2, u 3, u 4 ) epening only on the measurable states (the observe point p,the homography matrix H, the translational an angular velocities (V, Ω), an the estimate parameters (R,r) from the homography matrix (H) which provie a partial pose estimation), such that the following error is asymptotically stable. (ɛ = R(P R T P ), σ = φ φ ) Note: ɛ an σ are not efine in terms of visual information. In the sequel, these errors will be transforme to quantities that are expresse in terms of visual information. Recall that the current an esire position P an P of the observe point are not known an the only information that we have are given by their projections, the estimate matrix R an the ratio, r, between the istances an which is given by the eterminant of the homography matrix H. Following [9], the camera can be controlle in the image space an in the Cartesian space at the same time. They propose the use of three inepenent visual features, such as the image coorinates of the target point associate with the ratio r elivere by eterminant of the homography matrix. Consequently, let us consier the reference point P lying in the reference plan π an efine the scale cartesian coorinates using visual information as follow: P r = n T p n T p rp 2 The following shorthan notation for trigonometric function is use: c β := cos(β), s β := sin(β), t β := tan(β). Knowing that T p P P = n n T p r, it follows that we can reformulate the error ɛ in terms of available information Let us efine ɛ 1 = R ( n T p n T p rp RT p ) (11) From the above iscussion an equations escribing the system ynamics, the full ynamics of the error ɛ 1 may be rewritten as Define ɛ 1 = 1 P v (12) m v = u 4 Re 3 + mge 3 (13) Ṙ = Rsk(Ω) (14) I Ω = Ω IΩ + Γ (15) δ := ɛ 1 + v (16) Let S 1 be the first storage function for the backstepping proceure. It is chosen for the full linear ynamics Eqn s S 1 = 1 2 δ v 2 (17) Taking the time erivative of S 1 an substituting for Eq. 12 an 13 yiels where ρ = 1 P. t S 1 = ρδ T v + (δ + v) T (mge 3 u 4 Re 3 ) (18) Applying classical backstepping one woul assign a virtual vectorial control for 1 m (u 4Re 3 ) u 4 (Re 3 ) := mge 3 + mv + mδ (19) This choice is sufficient to stabilize S 1 if the term (u 4 Re 3 ) were available as a control input. If u 4 Re 3 =(u 4 Re 3 ) then is negative efinite ρ < 1. Ṡ 1 = δ 2 (2 ρ)δ T v v 2 Note that the vectorial input can be split into its magnitue u 4, that is linke irectly to the motor torques, an its virtual (or esire) irection R e 3, that efines two egrees of freeom in the airframe attitue ynamics (Eqn s 14-15). In this case, the magnitue an its virtual (or esire) irection of the vectorial term become:

4 u 4 = mge 3 + mv + mδ ; R e 3 = mge 3 + mv + δ u 4 (20) Now, to etermine the fully esire rotation matrix R we have to fin the constraint of the yaw parameter using another vector. Let e 1 be the esire orientation. We efine the vector σ which belongs to the plane built by the two vectors e 1 an R e 3 (σ span{r e 3, e 1 }) an we impose that σ = R e 1 (by this way, σ will be perpenicular to R e 3 ). We obtain σ = e 1 + αr e 3 e 1 + αr e 3 ; with σt R e 3 = 0 (21) Here α is a real number obtaine by solving the two above equations. The final equation for the esire matrix R can be efine as: R = [σ σ (R e 3 ) R e 3 ] (22) Limiting The X4-flyer Orientation In the theoretical evelopments base on the backstepping (see [5]), the propose law of control assures an exponential convergence towars the esire position. It seems to us, however, that this type of convergence is not recommene when the vehicle is initially far from this position. Inee, the ynamic moel base on quasi-stationary conitions (hover conitions) is not vali anymore, because the ynamics of such a convergence will provoke a ifferent flight moe. Moreover, the target image may leave the fiel of view of the camera uring the evolution of the vehicle. To avoi such situations, it is necessary to insure that the focal axis of the camera is close to the gravity irection. In the sequel, we propose to use small gains technique (for example the technique of saturation functions presente by Teel in [11]). This technique seems well aapte to our problem. Inee, if the orientation is saturate, we can insure that the X4-flyer will remain in quasi-stationary manoeuvres uring all the operation. The orientation R e 3 is a function of the terms v an δ. In orer to limit the orientation, we a a saturation on the two terms v an δ. Therefore, Eq. 19 becomes u 4 R e 3 = mge 3 + m Sat 2 (v + Sat 1 (δ)) (23) where Sat(x) is a continuous, nonecreasing saturation function satisfying: x T Sat i (x) > 0 for all x 0. Sat i (x) = x when the components of the vector x are smaller than L i ( x (.) L i. Sat i (x) M i for all x IR. Proposition 3.1 The following choice of the saturation functions [11] M i < 1 2 L i+1; 1 ρ 2 L i+1 L i M i ensures global stabilization of the linear ynamics when equation 23 is use as control input of the translational ynamics Proof Recalling Eq. 13 an Eq. 23, it yiels v = Sat 2 (v + Sat 1 (δ)) Consier the storage function S v = 1 2 v 2. The erivative of S v is given by Ṡ v = v T Sat 2 (v + Sat 1 (δ)) Using conitions on Sat i couple with the fact that M 1 L 2, it follows that Ṡ v < 0 ( v (.) 1 2 L 2) (v (.) represents a component of the vector v). Consequently, it exist a finite time T 1 after which all components of the linear velocity vector v (.) 1 2 L 2 ( t T 1 ). The control law Eq. 23 becomes then u 4 R e 3 = mge 3 + m(v + Sat 1 (δ)), t T 1 Now consier the evolution of the term δ for t T 1. Let S δ the storage function associate with the term δ (S δ = 1 2 δ 2 ). Deriving S δ it yiels S δ = δ T ((ρ 1)v Sat 1 (δ)) Using the secon conition of the proposition, one can observe that the components of the vector δ become smaller than M 1 after a finite time T 2. After T 2, the control law becomes u 4 R e 3 = mge 3 + m(v + δ), t T 2 insuring exponential stability after the time T 2. Using the saturate control law (Eq. 23), the erivative of the first storage function becomes Ṡ 1 = δ 2 (2 ρ)δ T v v 2 (δ +v) T u 4 s ( R I)R e 3 where R = RR T ; an u 4 s = mge 3 + m Sat 2 (v + Sat 1 (δ)) Accoring to the above proposition, the system with such a saturate input is globally asymptotically stable if the new error term R I converges to zero. Now, it only remains to control the attitue ynamics involving the error R I Attitue ynamics control The next step of the control esign involves the control of the attitue ynamics such that the error R I converges exponentially to zero. We will use a quaternion representation of the rotation to obtain a smooth control for R. The attitue eviation R is parameterize by a rotation γ aroun the unit vector k. Using Rorigues formula ([10]) one has R = I + sin( γ)sk( k) + (1 cos( γ))sk( k) 2

5 The quaternion representation escribing the eviation R is given by [2] The time erivative of ν becomes ν = k 2 η 0R T η k 2 η 0 2 ν (32) η := sin γ 2 k, η 0 := cos γ 2 ; with η 2 + η 2 0 = 1 The eviation matrix R is then efine as follows R = ( η 2 0 η 2 )I + 2 η η T + 2 η 0 sk( η) (24) The attitue control control objective is to achieve when R = I. From Eqn 24 this is equivalent to η = 0 an η 0 = 1. Inee, it may be verifie that R I F = tr((r I) T ( R I)) = 2 2 η (25) Base on this result, the attitue control objective is to rive η to zero. Differentiating (η, η 0 ) yiels (see [10]) η = 1 2 ( η 0I + sk( η)) Ω, η 0 = 1 2 ηt Ω (26) where Ω enotes the error angular velocity Ω = R (Ω Ω ) (27) an Ω represents the esire angular velocity. In orer to fin the esire angular velocity, we have to consier the time erivative of the esire orientation R e 3 Ṙ = R sk(ω ); Ṙ e 3 = R e 3 sk(ω ) (28) Since ifferentiating the expression of R e 3 is quite complex, so we will esign a control law with a high gain virtual control Ω v. In this way we can neglect the time erivative of R e 3. Then, by choosing the virtual control as Ω v Ω v = 2k η 0 η with parameter k chosen high enough to neglect Ω. With the above choice, we then have where η = k 2 η 0 2 η + k 2 η 0R ν + ksk( η)r ν an its time erivative ν is given by where ν := 1 k Ω + RT η 0 η (29) ν = Ω k + χ (30) χ = 1 2 RT η T R Ω η + R T η 0 ( 1 2 η 0I + sk( η))r Ω (31) Let us efine the Lyapunov function caniate for the attitue eviation : S 2 = 1 2 η ν 2. (33) Taking the time erivative of S 2 an using (32), we obtain Ṡ 2 = k 2 η 0 2 η 2 k 2 η 0 2 ν 2 (34) This completes the control esign for the attitue ynamics, since the time erivative of the storage function in (34) is efinite negative. Then the input of the new control law (eq. 23) limiting the orientation ensures the exponential stability of the system. 4 Simulation results In orer to evaluate the efficacy of the propose servoing technique with orientation limits, simulation results which concerns the above X4-flyer moel are presente. The experiment consiers a basic stabilization. The target is five points: four on the vertices of a planar square an one on its center (see the initials points on fig 4). The available signals are the pixel coorinates of the five points observe by the camera. For this experiment, it is assume that the plane is perpenicular to the line of sight (i. e. the unit vector normal to the target plane is equal to the irection of the gravity n = e 3 ) As iscusse in the en of section 3.1 the homography matrix has two columns of a rotation matrix R T as the first two columns. The esire image feature is chosen such that the camera set point is locate some meters above the square. Using the above specification, the error ɛ 1 (eq.11) is efine as follows ɛ 1 = 1 n T p rp RT e 3 The parameters use for the ynamic moel are m = 0.6, I = iag[0.4, 0.4, 0.6], g = 10, = 0.25 an κ = Initially, the X4-flyer is assume to hover at some meters above the groun with thrusts corresponing to necessary forces to maintain stationary flight u 4 mg. For the sake of the simulation, the initial position of the X4- Flyer is x = 10, y = 15, z = 12. Its orientation is fixe as the ientity matrix I 3 an it is consiere also in hover conitions (Ω = 0). The simulation is presente to valiate the propose control esign. It simulates the behavior of the X4-flyer ynamics in the ieal case (extraction of the homography matrix without isturbance). We will compare the results of the new control law (with orientation limits) versus the evolution of the states in the control law (without orientation limits) evelope in [4] (Figure 3). One can notice that the time of convergence

6 for the states following the new law of control is longer than the previous law, but instea we see that the variation of the Euler angles are restricte to small values (in this case, in the orer of 10 3 ra). So the new control law which limits the X4-flyer orientation ensures small values for Euler angles, therefore we are sure that the ynamics of the flying vehicle are applicable to the hover conitions (quasi-stationary manoeuvres) an the object will remain in the fiel of view of the camera. Finally, Figure 4 shows us the evolution of the target from the initial position till the esire position (highlighte by the ashe square). Figure 3: Evolution of the X4-flyer states (the 3 Euler Angles [raian] an the 3 coorinates [meter]) in the 2 control laws: without limiting its orientation (Dashe Lines-Angles in 10 1 ra), an the new control law (Full Lines-Angles in 10 3 ra) 5 Conclusion In this paper, we presente a control strategy for the autonomous flight of the X4-flyer taking into account an orientation limit in orer to keep the image in the camera s view fiel. Figure 4: Evolution of the target points in the image surface for the new control law. Points evolving from initial position till the esire position (ashe square) The control strategy is base on separating the airframe ynamics from the motor ynamics. This strategy only requires that the system is able to measure with a vieo camera the image plane mapping features on a planar surface (such as a laning pa). References [1] B. Couapel an B. Bainian. Stereo vision with the use of a virtual plane in the space. Chinese Journal of Electronics, 4(2):32 39, Avril [2] O. Egelan, M. Dalsmo, an O.J. Soralen. Feeback control of a nonholonomic unerwater vehicle with constant esire configuration. Int. Journal of Robotics Research, 15:24 35, [3] O. Faugeras an F. Lustman. Motion ans structure from motion in a piecewise planar environment. International Journal of Pattern Recognition an Artificial Intelligence, 2(3): , [4] T. Hamel, D.Suter, an R. Mahony. Visual servoing base on homography estimation for the stabilization of an x4- flyer [5] T. Hamel an R. Mahony. Visual servoing of uneractuate ynamic rigi boy system : An image space approach. In 39th Conference on Decision an Control, [6] T. Hamel, R. Mahony, R. Lozano, an J. Ostrowski. Dynamic moelling an configuration stabilization for an x4- flyer. In 15th IFAC Worl Congress, [7] S. Hutchinson, D. Ager, an P.I. Cake. A tutorial on visual servo control. In IEEE Trans. on Robotics an Automation, volume 12, Octobre [8] Y. Ma, S. Sastry, an J. Kosecka. View recovery from image sequences: Discrete viewpoint vs. ifferential viewpoint. Electronic research laboratory memoranum, UC, Berkeley, [9] E. Malis. Contribution à la moélisation et à la commane en asservissement visuel. PhD thesis, Université e Rennes, [10] R.M. Murray, Z. Li, an S. Sastry. A mathematical introuction to robotic manipulation. CRC Press, [11] A.R. Teel. Global stabilization an restricte tracking for multiple integrators with boune controls. System an Control Letters, 18: , [12] J. Weng, T.S. Huang, an N. Ahuja. Motion an structure from line corresponences: Close-form solution, uniqueness, an optimization [13] Z. Zhang an A.R. Hanson. Scale eucliean 3 reconstruction base on externally uncalibrate cameras. In IEEE Symposium on Computer Vision, Coral Glabes, Florie, 1995.