Luis Rodolfo

García Carrillo

Gerardo R Flores Colunga

Guillaume Sanahuja
email: gsanahuj@hds.utc.fr

Rogelio Lozano
email: rlozano@hds.utc.fr

L R García Carrillo

R Flores Colunga

Quad Rotorcraft Switching Control: An Application for the Task of Path Following

This paper addresses the problem of road following using a quad rotorcraft equipped with an onboard image processing system. The main objective consists of estimating and tracking the road without a priori knowledge of the path to be tracked. Special interest is also given to the development of efficient estimation and control strategies for dealing with situations when the road is not detected in the camera image. Aiming at this goal, two operational regions are defined: one for the case when the road is detected, and another for the case when it is not. A switching between the measurements of imaging and inertial sensors enables estimation of the required vehicle parameters in both operational regions.

Additionally, for dealing with both aforementioned cases, a switching control strategy for stabilizing the vehicle lateral position is proposed. The system stability is verified not only in the two operational regions, but also in the switching boundaries between them. The performance of the switching estimation and control strategies is tested in numerical simulations and real time experiments, successfully validating the effectiveness of the proposed approaches.

I. INTRODUCTION

position stabilization, but also for successfully executing a more complex predefined mission.

Enabling a UAV to perform a task in a completely autonomous fashion is perhaps one of the most important and challenging control problems. Furthermore, estimating the vehicle position with sufficient accuracy is one of the important problems related with this subject.

The task addressed in this paper consists of performing an autonomous navigation mission.

The objective is to enable a UAV to estimate and track a road with no prior knowledge of the path that must be followed. In addition, the mission must be successfully performed in the presence of external disturbances, which are present not only in the attitude angles, but also in the vehicle translational dynamics. To further improve the autonomy of the vehicle, the mission is complemented with an autonomous take-off, as well as with a landing at some specific point near the end of the road. With the purpose of simplifying the inherent complexity of the whole aforementioned task, the complete mission is divided into five modes of operation: take-off, ψ alignment, lateral position (y-state) alignment, road tracking, and landing.

The problem of stabilizing a quad rotorcraft using visual feedback has been addressed by several research groups, see for example [START_REF] Altug | Control of a quadrotor helicopter using visual feedback[END_REF], [START_REF] Zingg | Mav navigation through indoor corridors using optical flow[END_REF], [START_REF] Cabecinhas | Vision-based quadrotor stabilization using a pan and tilt camera[END_REF] and the references therein. From previous experiences performed, it has been found that proportional derivative (PD) controllers work efficiently, in practice, for stabilizing the UAV attitude dynamics [START_REF] García Carrillo | Stabilization and trajectory tracking of a quad rotor using vision[END_REF]. In addition, these kind of controllers have proved to be robust enough for applications having objectives similar to the one being addressed in the present research [START_REF] Rondón | Vision-based altitude, position and speed regulation of a quad-rotor rotorcraft[END_REF]. In both previously mentioned approaches, the imaging system was used for estimating the vehicle translational dynamics. However, the strategies proposed there are not robust for dealing with external disturbances which may cause the imaging system to temporarily lose the artificial landmarks used for obtaining the vehicle states.

Switching system ideas have been previously implemented to solve UAV-related problems. The implementation of different controllers with different gains is addressed in [START_REF] Hoffmann | Quadrotor helicopter flight dynamics and control: Theory and experiment[END_REF], this approach is adopted by the fact that the sole use of a PD controller in the position dynamics is not enough for attenuating the disturbances caused by, for example, wind gusts. In [START_REF] Hespanha | Linear parametrically varying systems with brief instabilities: An application to integrated vision/imu navigation[END_REF], the authors propose a switching strategy for estimating the states of a UAV equipped with imaging, inertial and air data sensing systems, capable of dealing with instants of time when the visual detection of the landmark fails. Following a similar reasoning, in [START_REF] Oishi | Switching in nonminimum phase systems: Application to vstol aircraft[END_REF] the authors present the analysis and [START_REF] Liberzon | Switched systems[END_REF] DRAFT control of a vertical take-off and landing (VTOL) aircraft at simulation level. By using a common Lyapunov function, the stability of the complete system, divided into three modes, is proved.

In this paper, we aim at estimating and controlling the vehicle relative position, orientation, and velocity with respect to (w.r.t.) a road that must be followed. An onboard camera allows estimation of the vehicle heading angle w.r.t. the longitudinal orientation of the road. Similarly, the imaging sensor is used for stabilizing the lateral distance of the vehicle in order to navigate exactly over the road. With the objective of developing a switching control strategy for estimation and tracking purposes, two operational regions are defined: one region for the situation when the road is in the camera field of view (FOV), and another region for when this is not the case.

The system stability is shown not only in the two operational regions, but also in the switching boundaries between them. The performance of the switching control was tested in real time experiments, successfully validating the effectiveness of the proposed approach.

The paper is organized as follows. Section II presents the problem statement. The methods proposed to estimate the vehicle states are described in Section III. The switching control strategy and a discussion of the system stability are presented in Section IV. Section V presents the quad rotorcraft experimental platform. Numerical simulations, as well as the performance of the UAV during real-time experiments are shown in Section VI. Finally, Section VII presents the conclusions.

II. PROBLEM STATEMENT

Fig. 1. Road following setup: The quad rotorcraft UAV under consideration is equipped with inertial sensors, an ultrasonic range finder, and a calibrated camera. The objective is to enable the UAV to estimate and track the road, with no prior knowledge of the path that must be followed.

In order to perform the road-following task, the quad rotorcraft must be capable of measuring October 3, 2013 DRAFT its angular dynamics, as well as its relative 3-dimensional position and heading w.r.t. the road.

An image describing the scenario considered in the present research is shown in Figure 1. The quad rotorcraft UAV platform is equipped with inertial sensors, an ultrasonic range finder, and a calibrated camera. The inertial system provides the vehicle angular dynamics, allowing the development of a control strategy for attitude stabilization. The ultrasonic range finder points downwards, directly measuring the altitude of the aerial vehicle during flight. The camera is installed on-board pointing downwards, in a manner that the plane formed by the helicopter (B x , B y ) plane and the camera (C x , C y ) image plane are parallel and have the same orientation, see Figure 2. The camera and the aerial vehicle move together as a rigid body, therefore, by using the image provided by the camera, vision-based strategies could be implemented for estimating the vehicle states required during the navigation mission.

The present study addresses the problem of stabilizing the quad rotorcraft UAV in 6 degrees of freedom (DOF) during a road-following task. Such a procedure can be detailed as follows. In the first stage, the vehicle performs an autonomous take-off, reaching the desired altitude z d over the road. At this point, the heading of the vehicle, expressed by ψ, is driven to yield a parallel positioning between the helicopter x-axis (represented by B x ) and the longitudinal direction of the road (expressed by R x ). The helicopter forward speed, expressed by ẋ, is kept to a constant value while the distance between B x and R x , expressed by B e y (see Figure 2), is regulated and kept to a minimum value, achieving a flight path well aligned and centered w.r.t. the road.

The objective of this paper is to design a road following strategy based on computer vision and switching controllers, with the purpose of stabilizing the vehicle altitude, heading angle, and lateral position (z, ψ, and y states, respectively) w.r.t. the road, while traveling at constant forward speed ( ẋ = c). Overcoming external disturbances while performing the navigation task is also a subject of interest. In general, the traveling speed may be time-varying. However, for simplicity we will only consider here the case of constant speed.

III. STATES ESTIMATION USING A VISION SENSOR

Suppose the camera-equipped quad rotorcraft is flying over a road composed by straight sections and smooth curves. If the distance between the road and aerial vehicle (altitude state) is appropriate, the road will be projected in the camera image plane as a group of straight lines, October 3, 2013 DRAFT Fig. 2. Scheme of the road following mission: while flying at an appropriate altitude, the road is projected onto the image plane as a group of straight lines. see Figure 2. A straight line in the image plane can be seen as a segment of infinite length, whose center of gravity belongs to the straight line [START_REF] Chaumette | Image moments: a general and useful set of features for visual servoing[END_REF]. A scheme of such a representation can be seen in Figure 3. By implementing the Hough transform method for line detection, a straight line can be represented as [START_REF] Shapiro | Computer Vision[END_REF]:

ρ = x cos θ + y sin θ (1)
The center of gravity (x g , y g ) of each straight line detected can be computed as

x i g = cos(θ)ρ ; y i g = sin(θ)ρ (2)
October 3, 2013 DRAFT where the super-index i stands for line i. Notice that it is possible to assign initial and final bounds to the line. Let us define (x i I , y i I ) as the initial point of the line, located in a place below the image lower margin. Similarly, let (x i F , y i F ) be the final point of the line, located in a place above the image upper margin. If the line has a parameter ρ > 0, the coordinates (x i I , y i I ) and (x i F , y i F ) will be defined as

x i I = x g + Γ(-sin(θ)) ; y i I = y g + Γ cos(θ) (3)
x i F = x g -Γ(-sin(θ)) ; y i F = y g -Γ cos(θ) (4)
where Γ is a known constant defining the bounds of the line. In the case where ρ < 0, the point

(x i F , y i F )
will be computed as in equation ( 3), while the point (x i I , y i I ) will be computed as in equation ( 4). The set of lines obtained from the projection of the road on the image are grouped together with the purpose of obtaining just an average line. This average line will uniquely represent the road in the image with a single pair of initial and final coordinates

x I = 1 n n i=1 x i I ; y I = 1 n n i=1 y i I (5)
x

F = 1 n n i=1 x i F ; y F = 1 n n i=1 y i F ( 6
)
where n is the number of lines grouped together, (x I , y I ) represents the initial (lowermost) road coordinate, and (x F , y F ) represents the final (uppermost) road coordinate.

Wrong line detections may occur due to unexpected situations (e.g., changes in the scene illumination), therefore, a method for identifying false detections is required. In the developed algorithm, every line i detected must satisfy a safety condition in order to be considered as a member of the road. To achieve this, the final and initial coordinates of line i are compared with respect to the average initial and final coordinates computed in equation ( 5) and [START_REF] Hoffmann | Quadrotor helicopter flight dynamics and control: Theory and experiment[END_REF]. If the coordinates of line i are far from the average values, line i is considered as a false detection and it is removed from further computations.

A. Computing the heading angle

The angle between the camera C y axis and the line going from (x I , y I ) to (x F , y F ) can be computed using the coordinates (x I , y I ) and (x F , y F ) in the two argument arctangent function as October 3, 2013 DRAFT follows

B ψ r = arctangent(y F -y I , x F -x I ) (7)
The angle B ψ r is used for obtaining the desired heading angle B ψ d which will align the vehicle

x-axis (B x ) with the road longitudinal axis (R x ), see Figure 2. The desired heading angle B ψ d can be expressed as

B ψ d = B ψ r - π 2 (8)
In equation ( 8) the term π 2 is subtracted because the angle B ψ r calculated using equation ( 7) has its origin in the position corresponding to 3:00 hours on the clock. If π 2 is not subtracted, the heading measurement obtained when the heading angle of the vehicle is aligned with the direction of the road would be equal to +90 • . With this subtraction we adjust such measurement to a value of 0 • .

B. Computing the relative lateral position

Consider an image-based distance C e y located between the road center of gravity projection (x g , y g ) and the vehicle center of gravity projection (x 0 , y 0 ) = (C W /2, C H /2), where C W and C H represent the image width and height respectively, in pixel coordinates (see Figure 2). For the case, when x I > x F , one has

C e y = x I -x F 2 + x F - C W 2 (9)
On the other hand, for the case when x I < x F , x I must be replaced by x F and vice-versa. The lateral position of the aerial vehicle w.r.t. the road can be estimated from C e y as e y = z C e y α y [START_REF] Shapiro | Computer Vision[END_REF] where z represents the altitude of the rotorcraft w.r.t. the road, and α y represents the camera focal length, in terms of pixel dimensions, in the direction of C y .

C. Translational velocities

The vehicle translational velocities in the forward and lateral directions can be estimated by implementing an optical flow algorithm in the camera image. In our work, the algorithm of Shi and Tomasi [START_REF] Shi | Good features to track[END_REF] is used to identify the mesh of points allowing an efficient feature tracking, October 3, 2013 DRAFT i.e., the best textured points. After this step, the selected features are used by the pyramidal implementation of the Lucas-Kanade algorithm [START_REF] Bouguet | Pyramidal implementation of the Lucas-Kanade feature tracker[END_REF] to estimate the optical flow.

Consider the camera-vehicle arrangement moving in a 3-dimensional space w.r.t. a rigid scene.

The optical flow computed at an image point (x i , y i ) comprises translational and rotational parts expressed as

  OF xi OF yi   = T i OF + R i OF ( 11
)
where the translational part is

T i OF = 1 z   -α x 0 x i 0 -α y y i        ẋc ẏc żc      (12)
and the rotational part is

R i OF =   x i y i αx -(α x + (x i ) 2 αx ) y i (α y + (y i ) 2 αy ) -x i y i αy -x i        ω x ω y ω z      (13)
The terms OF xi and OF yi are the optical flow components in the x and y coordinates, respectively, of the (x i , y i ) feature. The terms ( ẋc , ẏc , żc ) and (w x , w y , w z ) represent the camera translational velocities and rotation rates, respectively, while α x and α y express the camera focal lengths.

During the optical flow computation process the (x i , y i ) features share the same movement (rigid scene assumption). By using all the tracked features, the mean values for the optical flow in the forward an lateral direction can be expressed as

OF x = V OFx + K x V OFz + R OFx ( 14
)
OF y = V OFy + K y V OFz + R OFy ( 15
)
where OF x and OF y are the mean values of the optical flow (sensed in the image coordinate system), V OFz represents the relative depth, and K x and K y are known scale factors depending on the intrinsic parameters of the camera. Applying a similar approach to the one presented October 3, 2013 DRAFT in [START_REF] Rondón | Vision-based altitude, position and speed regulation of a quad-rotor rotorcraft[END_REF], the rotational optical flow terms R OFx and R OFy are compensated and the pseudo-speeds

(V OFx , V OFy , V OFz ) are deduced. This procedure yields -z V OFx α x = ẋ (16) -z V OFy α y = ẏ ( 17
)
zV OFz = ż (18)
where the fact that the camera and the vehicle share the same movement has been used, i.e., the velocity of the camera is equal to the velocity of the vehicle, or ( ẋc , ẏc , żc ) = ( ẋ, ẏ, ż).

D. Computing the heading and the lateral position when the road is not detected

The computation of the desired heading angle and lateral position of the vehicle are compromised by the sensitivity of the Hough transform method for line detection. In addition to this, it is not possible to estimate such parameters when the vehicle is flying over a region where the road is out of the camera FOV. Then, a realistic scenario suggests that the image of the road can be temporarily lost by the onboard camera. For this reason, it is crucial to extend the results presented in Sections III-A and III-B with the main objective of dealing with time events where the road is out of sight. Aiming at this goal, let us define a binary signal s : [0, ∞) → {0, 1} as

s(t) :=    0 no line detection at time t 1 camera detects line at time t (19)
In equation [START_REF] Hespanha | Stability of switched systems with average dwell-time[END_REF], s(t) allows switching between two different methods for computing the vehicle states. Furthermore, for a given binary signal s and t > τ > 0, denote by T s (τ, t) the amount of time in the interval (τ, t) for wich s = 0. Formally, T s (τ, t)

:= t τ (1 -s(l))dl.
Let us consider first the desired heading angle ψ d . Using equation ( 19) it is possible to define

ψ s (t) = s(t)ψ d (t) + (1 -s(t))ψ IMU (T s (τ )) (20)
where ψ d (t) is obtained from equation ( 8), ψ IMU (T s (τ )) is the heading angle measured by the IMU at the time when the binary signal s changes from 0 to 1, and ψ s (t) represents the desired heading angle that will be used in the helicopter control algorithm. Given the lack of visual information when s = 0, we decided to keep the yaw angle at a fixed value as long as the road is not seen. Equation [START_REF] Khalil | Nonlinear Systems[END_REF]

where e y (t) is obtained as in equation ( 10), e y (T s (τ )) is the lateral position measurement at the time when the binary signal s changes from 1 to 0, and ẏ(t) represents the vehicle lateral velocity obtained from equation [START_REF] Slotine | Applied Nonlinear Control[END_REF]. Notice that equation [START_REF] Peleties | Asymptotic stability of m-switched systems using Lyapunov-like functions[END_REF] allows switching between two different approaches for estimating the vehicle lateral position w.r.t. the road. In the first approach (when s = 1) the lateral position is directly obtained from the detection of the road in the camera image (equation ( 10)). The second approach (when s = 0) makes use of the lateral position measurement at the time when the binary signal s changes from 1 to 0, in combination with the integral of the translational velocity ẏ during the time the road is not being detected by the vision algorithm, i.e., the amount of time in the interval (τ, t) that s = 0.

IV. CONTROL STRATEGY

A normalized mathematical model for the quad-rotor is implemented aiming at developing the navigation control strategy [START_REF] García Carrillo | Quad Rotorcraft Control: Vision-Based Hovering and Navigation[END_REF]:

ẍ = -u 1 (cos φ sin θ cos ψ + sin φ sin ψ) ÿ = -u 1 (cos φ sin θ sin ψ -sin φ cos ψ) z = 1 -u 1 (cos φ cos θ) θ = u 2 φ = u 3 ψ = u 4 (22)
October 3, 2013 DRAFT

The quad-rotor model in equation ( 22) can be written in a state-space form by introducing Ẋ = (x 1 , ..., x 6 ) T ∈ R 6 , Ż = (z 1 , ..., z 6 ) T ∈ R 6 , with states defined by

x 1 = x z 1 = θ x 2 = ẋ z 2 = θ x 3 = y z 3 = φ x 4 = ẏ z 4 = φ x 5 = z z 5 = ψ x 6 = ż z 6 = ψ (23)
Using the non-linear model in equation ( 22) and the notation from equation ( 23), the following

approximation is obtained ẋ1 = x 2 ż1 = z 2 ẋ2 = -z 1 u 1 ż2 = u 2 ẋ3 = x 4 ż3 = z 4 ẋ4 = z 3 u 1 ż4 = u 3 ẋ5 = x 6 ż5 = z 6 ẋ6 = 1 -u 1 ż6 = u 4 (24)
In general, the vehicle never operates in areas where |θ| ≥ π/2 and |φ| ≥ π/2. For this reason, the approximation in (24) is chosen for implementation instead of its non-linear version. This kind of operation is satisfied even in research works where the non-linear model is used together with a feedback control law [START_REF] Bouabdallah | Backstepping and sliding-mode techniques applied to an indoor micro quadrotor[END_REF].

A. Operating modes for the road following mission

We divide the road following mission into four different stages which are described next:

1) Take-off mode: The objective is to achieve the desired altitude z d , while avoiding drift in the x-y plane.

2) ψ-alignment mode (ψ AL): In this mode the vehicle has already achieved the desired altitude. The task to be accomplished here is to align the vehicle heading angle w.r.t. the road direction.

3) y-alignment mode (y AL):

The vehicle is required to maintain a flying path well aligned w.r.t. the center of the road.

October 3, 2013 DRAFT 4) Tracking mode: In this mode, the forward displacement of the vehicle is stabilized in order to navigate at constant velocity.

5) Landing mode:

When the vehicle has reached a previously defined position near to the end of the road, the altitude is controlled in order to perform an autonomous landing. In the current implementation of the algorithm, the end of the road is not detected autonomously, i.e., a signal for activating the "landing" mode is manually generated.

B. Control Laws for each Operating Mode

The control strategy proposed in all the operating modes is based on the idea that the global system presented in equation (24) comprises two subsystems; the attitude dynamics and the position dynamics, with a time-scale separation between them [START_REF] Kokotović | Singular Perturbation Methods in Control: Analysis and Design[END_REF]. From this approach it is possible to propose a hierarchical control scheme where the positioning controller provides the reference attitude angles (θ d , φ d and ψ d ), which are the angles that must be tracked by the orientation controllers. For the complete system analysis, the error dynamics of the model in equation ( 24) are represented by the error terms xi = x i -x i d and zi = z i -z i d , with i ∈ {1, ..., 6}.

1) Attitude Control:

For the present study, the attitude dynamics controller is the same in all modes of operation An integral sliding mode control (ISMC) is proposed and implemented on the platform. This kind of controller was chosen due to its insensitivity to model errors, parametric uncertainties, and diverse disturbances [START_REF] Xu | Sliding mode control of a class of underactuated systems[END_REF]. Specifically, for the pitch dynamics case the error equation is defined as z1 = z 1 -z 1 d . As shown in [START_REF] Slotine | Applied Nonlinear Control[END_REF], let us select the following switching function

s(z, t) = ż1 + 2λz 1 + λ 2 t 0 z1 (τ ) dτ (25)
which depends on the pitch dynamics states. The parameter λ in equation ( 25) is the slope of the sliding curve, which should be greater than zero to ensure the asymptotic stability of the sliding mode. Computing the time derivative of equation ( 25) one has

ṡ = u 2 + 2λz 2 + λ 2 z1 (26)
Considering the sliding mode condition ṡ = 0 and using equation (26), one finds the equivalent control

u 2eq = -2λz 2 -λ 2 z1 (27)
October 3, 2013 DRAFT

In order to obtain a control law such that the state vector z1 remains on the sliding surface s(z, t) = 0, ∀ t > 0, a Lyapunov function candidate is defined as

v(s) = 1 2 s 2 (28)
A sufficient condition for the stability of the pitch sub-system can be satisfied if one can ensure that the following condition holds

v(s) = 1 2 d dt s 2 ≤ -η|s|, η ≥ 0 (29)
Then, the system remains on the sliding surface and the states converge to the origin. From this, s ṡ ≤ -η|s| and the controller must be chosen such that

u 2 = u 2eq -Ksign(s) ( 30
)
where K is a positive real number. Following a similar approach, it is possible to obtain the controllers for the yaw and roll angle dynamics.

2) Position Control: As previously explained, the position control has well defined objectives for each operating mode. Even though the control objectives are different, the same control law structure is applied for all the operating modes.

Motion in the x -y plane is accomplished by orientating the vehicle thrust vector in the direction of the desired displacement. As a consequence, the angles θ d and φ d act as virtual controllers for the position dynamics. The control laws proposed for the z and x dynamics, respectively, are expressed by the following equations

u 1 = k pz (x 5 -x 5 d ) + k vz (x 6 -x 6 d ) -1 (31)
θ d = k px (x 1 -x 1 d ) + k vx (x 2 -x 2 d ) u 1 ( 32
)
where k vx , k px , k pz and k vz are positive real numbers. Notice that, in the present application, we are not making use of the term x 1 d , which represents a desired x position. As previously explained, we are only interested on regulating the forward velocity, therefore, equation (32) can be reduced to

θ d = k vx (x 2 -x 2 d ) u 1 (33)
In order to avoid singularities, the initial error between the quad-rotorcraft altitude state and its corresponding altitude reference should be small. It is worth mentioning that, when the altitude October 3, 2013 DRAFT error converges to zero, the value of u 1 approaches the value -1. This means that the error converges only locally, avoiding the singularity.

The lateral position stabilization makes use of two PID controllers, one for the case when the road is being detected and the other for the case when it is not. Although both PID controllers are similar, they are designed having different parameters. When the road is inside the FOV, the gains of the control algorithm are tuned to behave as a PD controller since the vehicle needs to regulate its y coordinate at a minimum value (y ≈ 0) as fast as possible [START_REF] Hoffmann | Quadrotor helicopter flight dynamics and control: Theory and experiment[END_REF]. On the other hand, when the camera loses sight of the road, a switching strategy allows using a different method for measuring the vehicle ψ angle and y position. Additionally, the parameters of the positioning controller switch to alternative values emulating a PI controller. In both cases, the control objective is to regulate the x 3 state to the origin, i.e. x 3d = 0.

• Control scheme implemented when the road is detected: the feedback control law is

z 3 = 1 u 1 (-k L3 x 3 -k L4 x 4 -k LI ξ) (34)
where k L3 , k L4 and k LI are positive real numbers. Here, the additional state ξ has been introduced, whose dynamics are given by ξ = x 3d -x 3 = -x 3 . Using the control in equation ( 34), the closed-loop system of the lateral dynamics is

ẋ3 = x 4 ẋ4 = -k L3 x 3 -k L4 x 4 -k LI ξ ξ = -x 3 (35)
Equation ( 35) can be represented as ėy = A L e y where e y = (x 3 , x 4 , ξ) T and

A L =      0 1 0 -k L3 -k L4 k LI -1 0 0      (36)
• Control scheme implemented when the road is not detected: the control scheme proposed is the same as given by equation (34), with the only difference being a set of different gains.

The closed-loop system is represented as ėy = A N L e y , where

A N L =      0 1 0 -k N L3 -k N L4 k N LI -1 0 0      (37)
October 3, 2013 DRAFT and k N L3 , k N L4 and k N LI are positive real numbers.

Remark 1:

The controller parameters of both operational regions were adjusted independently following an heuristic approach consisting of two main steps which are described next. During the first step, special care is taken to stabilize the vehicle fast dynamics, which corresponds to the angular behavior. The parameters associated with the angular rate are adjusted first, until an appropriate angular rate response is obtained. Following a similar method, the next parameter to be adjusted corresponds to the attitude control parameter. After both parameters have been adjusted properly, an autonomous hover flight experiment is performed to verify the effectiveness of the tuning procedure. The second part of the adjustment procedure is devoted to stabilizing the vehicle translational dynamics, which involves the implementation of visual feedback in the control strategy. First, the parameter corresponding to the translational velocity is adjusted until the vehicle translational drift is almost eliminated. Next, the parameter associated with the translational position is tuned to obtain an appropriate behavior of the controller for stabilizing the 3-dimensional position of the vehicle over road area. The main objective behind the procedure just described is to minimize the tracking error, in order to obtain the most appropriate angular and translational behavior. For the first case (road detected), the gains were selected as k L3 = 1000, k L4 = 850 and k LI = 0.1, while in the second case (no road detected) as k N L3 = 1000, k N L4 = 1 and k N LI = 9.5. In this way, the system presents a switched-system characteristic.

C. Stability Analysis of the Lateral Position Control

We now present a study concerning the stability of the system across switching boundaries, i.e., in the regions where the control parameters of equation (34) switch to different values.

In fact, it is possible to find a common Lyapunov function for the closed-loop system of the two controllers proposed for the lateral dynamics [START_REF] Liberzon | Switching in Systems and Control[END_REF]. Following such approach, similar pole locations have to be chosen for both cases, i.e., when the road is detected and when it is not.

However, this is not the present situation since different gain values are being applied.

Let d c be defined as the distance measured from the vehicle center of gravity projection (in the road plane) to the point where the camera loses the image of the road (see Figure 4). From this, a state-dependent switched linear system can be defined, which will be given by the closed-loop October 3, 2013 DRAFT system (36)-( 37), together with the following switching conditions

ėy =          A N L e y if x 3 ≤ -d c or s = 0 A L e y if -d c < x 3 < d c and s = 1 A N L e y if x 3 ≥ d c or s = 0 (38)
A switching event can occur in system (38) every time the trajectory crosses a switching surface.

For the case under consideration, the switching surfaces are encountered when the system changes from ėy = A L e y to ėy = A N L e y , and viceversa. In general, there is no need to associate each subsystem in (38) with a global Lyapunov function. In fact, it is enough to require that each function V i decrease along solutions of the i-th subsystem in the region where this system is active.

The stability of the overall system (38) is accomplished when: (i) individual subsystems are stable, and (ii) the overall system remains stable when a switching surface is crossed. Indeed, it is possible to verify that individual subsystems of the switching system (38) are globally exponentially stable. Furthermore, if the switching between individual subsystems is sufficiently slow, the entire system (38) will be globally exponentially stable [START_REF] Hespanha | Stability of switched systems with average dwell-time[END_REF]

∂V i ∂e y A i e y ≤ - c i b i V i (e y ) (41)
This implies that

V i (e y (t 0 + τ d )) ≤ e - c i b i τ d V i (e y (t 0 )) (42)
Let us consider two switching times {t 1 , t 2 }, and let us also assume that subsystem ėy = A L e y is active on the interval t ∈ [t 0 , t 1 ), while subsystem ėy = A N L e y is active on ∈ [t 1 , t 2 ). From inequalities (39)-( 42) it follows that

V L (t 2 ) ≤ b L a N L V N L (t 2 ) ≤ b L a N L e - c NL b NL τ d V N L (t 1 ) (43)
and also

V L (t 2 ) ≤ b N L b L a L a N L e - c L b L + c NL b NL τ d V L (t 0 ) (44)
In order to find the lower bound on τ d it is sufficient to ensure that (see Theorem 1 of [START_REF] Peleties | Asymptotic stability of m-switched systems using Lyapunov-like functions[END_REF])

V L (t 2 ) -V L (t 0 ) ≤ -p e y (t 0 ) 2 (45)
where p > 0. It is now straightforward to compute an explicit lower bound on τ d , which guarantees that the switched system (38) is globally asymptotically stable [START_REF] Liberzon | Switched systems[END_REF]

τ d > b L + b N L c L + c N L log b L b N L a L a N L (46)
The lower bound on τ d can be satisfied by adjusting a i , b i , and c i , for i = {L, NL}, which depend on the controller gains and the Lyapunov function.

October 3, 2013 DRAFT Fig. 5. UAV experimental platform: The quad-rotor equipped with imaging, inertial and altitude sensing systems.

V. EXPERIMENTAL PLATFORM

A. Quad-rotor experimental vehicle

The quad-rotor used during the real time application is shown in

C. Ground station

The rotorcraft is wirelessly linked to a ground station PC, where a graphical user interface (GUI) based on the QT application allows monitoring and controlling the vehicle. The user can visualize, in real time, graphics representing the measurements from the on-board sensors, as well as graphics representing the control law computation. It is also possible to modify (online) the parameters of the control laws, as well as the implemented filters. The rotorcraft can also be manually controlled by a human pilot using a PlayStation R 3 joystick, which is linked to the ground station by means of a BlueTooth TM link.

October 3, 2013 DRAFT

VI. SIMULATIONS AND REAL TIME EXPERIMENTS

A. Numerical Simulations

The control strategy developed in Section IV has been designed for the quad rotorcraft model approximation in equation ( 24). In order to show that the obtained controller can also be used to control the non-linear plant in [START_REF] Liberzon | Switched systems[END_REF] as long as the pitch and roll angles are small, we have performed a set of numerical simulations. The simulation scenario follows the procedure previously explained in Subsection IV-A. A synthetic road having two smooth turns has been designed, and was used as the desired reference that the vehicle has to follow during navigation. The desired altitude is set at 0.70 m over the road. A ramp profile was used for modifying the desired velocity from ẋ = 0.0 m/s to ẋ = 0.3 m/s during the first stages of the navigation, and also for modifying the desired velocity from ẋ = 0.3 m/s to ẋ = 0.0 m/s before landing.

As can be seen in the set of set of Figures 7891011, the controller performs well in spite of the non-linearities of the plant. Figure 9 shows a disturbance in the y state reference signal (blue line) at t = 18 seconds. Notice how the proposed controller maintains stability of the system even in the presence of these unexpected behaviors. From Figure 11 it can be observed that the pitch and roll angles are always kept at very small values, and also that the induced disturbance produces only small effects on them. how the system remains stable despite a disturbance in the y state reference signal (dashed blue line) at t = 18 seconds. This demonstrates that the proposed controller is effective for dealing with these unexpected behaviors.

B. Real Time Experiments

The ultimate test for the proposed estimation and control strategies consists on verifying its proper operation when used during real-time experiments. Aiming at this goal, a road model having two smooth curves was built and installed in our experimental area. The experimental procedure can be described as follows. The rotorcraft starts its mission on the ground, exactly positioned over the road. Next, an autonomous take-off is performed, achieving a desired altitude of 0.70 m over the road. Once at this height, the embedded camera detects the road. The imaging information allows heading and lateral position stabilization for achieving a flight path well aligned w.r.t. the road. The vehicle is then required to navigate forward at a desired speed of ẋ = 0.3 m/s, while regulating its heading angle ψ and lateral position y w.r.t. the road. In order to achieve a smooth transition from a forward velocity of ẋ = 0.0 m/s to a forward velocity of ẋ = 0.3 m/s, a ramp velocity profile is implemented, in such a way that the velocity slowly increases after the take-off task. Once the vehicle is near to the end of the road segment, the landing is performed autonomously, and the rotorcraft descends slowly to the ground. Again, a ramp velocity profile is implemented, in such a way that the forward velocity slowly decreases from ẋ = 0.3 m/s to ẋ = 0.0 m/s before the landing task is engaged.

During the execution of the autonomous navigation experiment, an external force (disturbance) was applied in the lateral dynamics of the vehicle. The disturbance coerces the quad-rotor to displace arbitrarily in the y direction, making the imaging system unable to detect the road for an inertial coordinate frame I, whose origin is located at the quad rotorcraft initial position.

It can be seen that the vehicle performs the road following mission inside a rectangular area of 6×6 meters. The projection of the rotorcraft displacement in the (x, y) plane can be seen in Figure 13. It is worth mentioning that the forward displacement (relative to the body fixed frame) is obtained by integrating the optical flow measured in the forward direction. On the other hand, the rotorcraft lateral position is obtained from two methods. The first of them directly applies equation [START_REF] Shapiro | Computer Vision[END_REF], which provides the lateral position of the vehicle relative to the road. The second method corresponds to the integration of the optical flow, in the lateral direction, during the October 3, 2013 DRAFT instants of time when the road is not detected by the vision system.

The inertial coordinates shown in Figure 12 and Figure 13 are obtained after rotating both the (x, y) states around the z-axis using a gymbal like rotation matrix. Specifically, the rotation matrix makes use of the heading angle provided by the onboard IMU. The computation, which is described in equations ( 47) and (48), generates the 3-dimensional trajectory of the vehicle with respect to the inertial frame I.

I x = t 0 cos(ψ(t)) ẋ(t)dt -sin(ψ(t))y(t) (47)
I y = t 0 sin(ψ(t)) ẋ(t)dt + cos(φ(t))y(t) (48)
The The switching between the two different state estimation approaches is illustrated in Figure 16.

The variable s (the graphic in the middle) shows the instants of time when the road is being detected (s = 1) and when it is not being detected (s = 0). The upper graphic represents the desired heading angle that will align the rotorcraft heading with the road direction, as obtained from equation [START_REF] Khalil | Nonlinear Systems[END_REF]. The lower graphic represents the lateral position error as computed from equation [START_REF] Peleties | Asymptotic stability of m-switched systems using Lyapunov-like functions[END_REF]. A zoom to the region where the switching occurs is shown in Figure 17. rotorcraft in order to handle the two curves of the road. This heading angle is represented w.r.t.

the inertial reference frame whose origin is at the quad-rotor initial position.

Figure 19 shows an image of the rotorcraft in the experimental area during the real-time tests.

In addition, a video showing the quad-rotor while performing the road following experiment can be seen at http://youtu.be/tmiApYi3tT4

VII. CONCLUSION

The problem of road following using a quad rotorcraft equipped with a fully embedded imaging and control system was addressed. The goal of this research consists of estimating and tracking a road without a priori knowledge of the path to be tracked, as well as of deriving efficient estimation and control strategies for dealing with situations when the road is not detected by the vision system. Two main operational regions were defined: one for the case when the road is detected, and the other for the case when it is not. A switching between measurements coming from imaging and inertial sensors was used in order to estimate the vehicle parameters required in both regions of operation. In addition to this, a switching control strategy for stabilizing the vehicle lateral position was proposed. The system stability was verified not only in the two October 3, 2013 DRAFT operational regions, but also in the switching boundaries between them. The performance of the switching strategies for sensing and control was tested in a numerical simulation and a real time but also different control strategies.

Fig. 3 .

 3 Fig. 3. Scheme of the Hough transform method for line detection.

Fig. 4 .

 4 Fig. 4. The distance measured from the vehicle center of gravity projection in the image plane to the point where the camera loses the image of the road is defined as dc. The distance between the vehicle center of gravity projection and the road is defined as x3.

Figure 5 .Fig. 6 .

 56 Fig. 6. Image obtained from the onboard camera while the UAV is flying above the road: (a) the raw image provided by the camera; (b) gray scale image used for optical flow computation; (c) black and white images used in Hough transform algorithm; (d) the post-processed image where the detection of the road has been highlighted.

Fig. 7 .

 7 Fig. 7. Simulation of the road-following mission: The 3-dimensional trajectory flown by the vehicle. The black line represents the vehicle displacement.

Fig. 8 .

 8 Fig. 8. A projection on the (x, y) plane of the trajectory flown by the rotorcraft.

Fig. 9 .

 9 Fig. 9. The (x, y, z) states during the simulation test. The solid black line represents the behavior of the rotorcraft. Notice

Fig. 10 .

 10 Fig. 10. Translational velocities during the simulation test. The solid black line represents the behavior of the rotorcraft. The forward velocity ẋ is kept at 0.3 m/s, while the lateral velocity ẏ and altitude velocity ż are both kept around zero. Ramp profiles were used for increasing-decreasing the ẋ reference value. The variation in ẏ at t ≈ 18 s is caused by the induced disturbance.

Fig. 11 .

 11 Fig. 11. The rotorcraft Euler angles during the simulation test. The solid black line represents the behavior of the rotorcraft.Notice that the pitch angle (θ) and roll angle (φ) are both kept small. The heading angle (ψ) is represented w.r.t. the inertial reference, and describes the heading that the vehicle must track in order to handle the two curves of the road. From the (ψ) graphic, notice that the difference between the actual behavior (black line) and expected behavior (dashed blue line) is very small.

  three translational states are shown individually in Figure 14. Here, x = 10 m represents the longitudinal dimension of the road, which is also the total distance flown by the quad-rotor in the forward direction. This measurement was based on the integration of the optical flow sensed by the camera in the x direction. The y state represents the lateral position of the quad-rotor w.r.t. the road. An external perturbation in the y direction can be seen at t = 25 seconds. The rotorcraft translational velocities are shown in Figure 15. Notice that, during navigation, the forward speed ẋ is maintained at 0.3 m/s, while the lateral ẏ and altitude ż velocities are kept near zero.

Finally, theFig. 12 .

 12 Fig. 12. Road-following mission: 3-dimensional reconstruction of the path navigated during the real-time experiment. The dashed blue line plotted in the x-y plane represents the position of the road model. The black line represents the actual vehicle displacement.

Fig. 13 .

 13 Fig. 13. The trajectory described by the rotorcraft, projected in the (x, y) plane. The dashed blue line represents the position of the road model. The black line represents the actual vehicle displacement.

Fig. 14 .

 14 Fig. 14. The (x, y, z) rotorcraft states: The perturbation in the lateral dynamic (y state) is shown at t = 25 seconds.

  application. The experiment allowed to validate the effectiveness of the proposed methods for autonomously performing the task of road following in the presence of external disturbances and unexpected failures of the imaging system. Future work will concern a hybrid scheme for switching not only different controller gains October 3

Fig. 15 .

 15 Fig. 15. Translational velocities during the experiment: The forward velocity ẋ is kept at 0.3 m/s, while the lateral velocity ẏ and altitude velocity ż are both kept around zero. The variation in ẏ at around 25 s is caused by the external perturbation.

Fig. 16 .

 16 Fig.[START_REF] Xu | Sliding mode control of a class of underactuated systems[END_REF]. Behavior of the switching signals ψ d , s and eys. The graphic in the middle represents the instants of time when the line is being detected (s = 1) and when the line is not being detected (s = 0). During the take-off and landing phases the switching signal s is kept fixed at a value of s = 1. Once the desired height is reached, the signal s varies according to the situation encountered, i.e., s = 1 when the road is detected, and s = 0 for when the road is not detected.

Fig. 17 .

 17 Fig.17. A zoom to the region where the disturbance occurs: The upper graphic shows how the s signal changes between 1 and 0. The lower graphic shows how the switching strategy influences the lateral position measurement.

Fig. 18 .

 18 Fig. 18. The rotorcraft Euler angles as obtained directly from the embedded IMU: The pitch angle (θ) is kept to a value which ensures forward displacement at the required velocity. The roll angle (φ) is kept small. The heading angle (ψ) is represented w.r.t. the inertial frame I.

Fig. 19 .

 19 Fig. 19. Road-following mission: The rotorcraft flying in the experimental area during real-time tests.

  algorithm, and (ii) a desired heading temporarily determined by the IMU heading measured at the time when the binary signal s changes from 1 to 0.Consider now the computation of the lateral position of the vehicle when the road is out of

	sight. Equation (19) allows us to compute
	t
	e ys (t) = s(t)e y (t) + (1 -s(t)) e y (T s (τ )) +	ẏ(t)dt
	τ

allows switching between: (i) a desired heading provided by

[START_REF] Liberzon | Switched systems[END_REF]

DRAFT the imaging

  As previously mentioned, individual subsystems are globally exponentially stable, then there exist Lyapunov functions V L , V N L which, for some positive constants a i , b i , and c i satisfy[START_REF] Khalil | Nonlinear Systems[END_REF] a i e y 2 ≤ V i (e y ) ≤ b i e y

			2	(39)
	and
	∂V i ∂e y	A i e y ≤ -c i e y	2	(40)
	for i = {L, NL}. Combining (39) and (40) we have
	October 3, 2013			DRAFT

. Aiming at this goal, let us define a dwell time τ d satisfying t n+1 -t n ≥ τ d , where t n (for n = 1, 2, 3, . . .) represents a switching time. Next, it is required to investigate the lower bound on τ d from the exponential decay bounds of the subsystems as follows.

October 3, 2013 DRAFT

This work is motivated by the Unmanned Aerial Vehicle (UAVThis work was partially supported by the Mexican National Council for Science and Technology (CONACYT) and the Institute for Science & Technology of Mexico City (ICyTDF).

B. Embedded vision system

The vehicle is equipped with a PlayStation R Eye camera whose focal length is (f x , f y ) = (270.87, 271.76) pixels. The camera is capable of providing 120 images per second, at a resolution of 320×240 pixels. The camera points downwards which allows observing the scene below the vehicle. The images provided by the camera are processed by computer vision algorithms in order to estimate the helicopter translational velocity in the x -y plane, the heading angle, as well as the lateral position w.r.t. the road direction. The translational velocity in the x-y plane is obtained from an optical flow algorithm, which is based on the pyramidal Lucas-Kanade method.

For this purpose, the algorithm uses two pyramid levels, searching for 64 characteristic features