CLF-based Tracking Control for UAV Kinematic Models with ... - BYU

CDC03-IEEE1045

CLF-based Tracking Control for UAV Kinematic Models with Saturation Constraints Wei Ren Randal W. Beard1 Department of Electrical and Computer Engineering Brigham Young University Provo, Utah 84602, {weiren,beard}@ee.byu.edu. Abstract This paper considers the trajectory tracking problem for unmanned air vehicles (UAVs). We assume that the UAV is equipped with an autopilot which reduces the twelve degree-of-freedom (DOF) model to a six DOF model with altitude, heading, and velocity command inputs. In this paper we restrict our attention to planar motion. One of the novel features of our approach is that we explicitly account for heading and velocity input constraints. For a UAV, the velocity is constrained to lie between two positive constants, and therefore presents particular challenges for the control design. We propose a control Lyapunov function (CLF) approach. We first introduce a CLF for the input constrained case, and then construct the set of all constrained inputs that render the CLF negative. The control input is then selected from this “feasible” set. The proposed approach is then applied to a simulation scenario, where a UAV is assigned to transition through several targets in the presence of multiple dynamic threats. 1 Introduction The stabilization and tracking of dynamical systems with nonholonomic constraints has received recent attention in the literature. Reference [1] provides a nice overview of the developments in control of nonholonomic systems. An inherent challenge, identified by Brockett’s well-known necessary condition for feedback stabilization [2], is that nonholonomic systems cannot be stabilized via smooth time-invariant state feedback. A simple but classical example of a nonholonomic system is a mobile robot which serves as an interesting topic for stabilization [3] and tracking [3, 4, 5, 6]. Unmanned aerial vehicles (UAVs) equipped with low-level altitude-hold, velocity-hold, and heading-hold autopilots can be modeled by kinematic equations of motion that are similar to the kinematic equations of motion of mobile robots. The input constraints, however are 1 Corresponding

author.

quite different. While mobile robots and UAVs have similar angular velocity constraints, the linear velocity constraints are quite different. In particular, UAVs have a minimum velocity constraint that is greater than zero, due to the stall conditions of the aircraft. On the other hand, mobile robots may have negative linear velocity, greatly simplifying the tracking problem. This paper deals with the issue of tracking control for UAV kinematic models with physically motivated heading rate and velocity constraints. We approach the problem using control Lyapunov functions (CLFs) [7, 8, 9]. While our approach is designed for UAVs in particular, it is also valid for mobile robot kinematic models with input constraints. Current approaches to tracking control of mobile robots includes linear model approaches [10, 11], the sliding mode approach [5], backstepping [12, 4, 6, 13], and passivity based approaches [14], to name a few. Reference [5] proposes a robust tracking strategy for nonholonomic wheeled mobile robots using sliding modes. The approach asymptotically stabilizes the mobile robot to a desired trajectory and is robust to bounded external disturbances. In [4], a tracking control methodology using time-varying state feedback based on the backstepping technique is proposed for both a kinematic and simplified dynamic model of a two-degree-of-freedom mobile robot, where local and global tracking problems are solved under certain conditions. Using the backstepping technique and the LaSalle’s invariance principle, [13] proposed a controller with saturation constraints which can simultaneously solve both the tracking and regulation problems of a unicycle-modeled mobile robot. With their approach, mobile robots can globally follow any path specified by a straight line, a circle, or a path approaching the origin using a single controller. Reference [14] developed a model-based control design strategy via passivity and normalization approaches to deal with the problem of global stabilization and global tracking control for the kinematic model of a wheeled mo-

bile robot in the presence of input saturations. Saturated, Lipschitz continuous, and time-varying feedback laws are obtained in their approach. However, in all of these approaches, negative velocities are allowed with exclude them from being used on UAVs. The CLF approach introduced by Artstein [7] and Sontag [8] can be used to find asymptotically stabilizing controllers for nonlinear systems. Artstein’s theorem [7] states that the existence of a differentiable CLF implies (and is also implied by) the existence of a continuous feedback stabilizer [15]. Based on a known CLF, universal formulas are found to generate explicit feedback stabilizing control laws for cases of unbounded controls [9], bounded controls [16], and positive controls [17]. In Ref. [18], a class of pointwise min-norm control laws are defined based on robust CLF, which minimize the instantaneous control effort while maintaining some desired negativity of the worst-case Lyapunov derivative. In [19], the satisficing approach is developed to design and analyze state-feedback control laws based on CLFs. The satisficing framework parameterizes a set of asymptotically stabilizing control laws that obey an instantaneous cost-benefit inequality. Under certain definitions and parameterizations, these control laws guarantee inverse-optimality and desirable stability margins. This paper proposes a different approach to the input constrained tracking control than those papers mentioned above. Instead of introducing a specific control law and then proving its stability, we propose a valid CLF for tracking control of the kinematic model with input constraints, and derive a state-dependent, timevarying set of “feasible” control values from which different controllers can be instantiated. In this sense, the approach is similar to the satisficing approach proposed in [19]. To our knowledge the CLF proposed in this paper has not appeared previously in the literature. Selection from the “feasible control set”, guarantees accurate tracking as well as satisfaction of the saturation constraints. Different control strategies can be derived by selection from the feasible control set according to some auxiliary performance index. This approach introduces a great deal of flexibility to the tracking control problem. It is worthwhile to mention that the universal formulas introduced in [16, 17] are not feasible in the UAV case due to the velocity constraints. The salient features of our approach are as follows. First, under the proposed tracking CLF framework with input constraints, we allow the reference velocity and angular velocity to be piecewise continuous while other papers for tracking control (e.g. [4, 14, 13]) constrain them to be uniformly continuous in order to apply Barbalat’s lemma. This in turn relaxes the potential restrictions for the other approaches. Sec-

ond, we apply the tracking control approach with input constraints to a UAV scenario, where the UAV is assigned to transition through several targets in the presence of dynamic threats. Instead of following a simple path like straight lines and circles (e.g. [4, 14, 13]), the UAV tracks a real-time trajectory generated dynamically from a trajectory generator according to the current situation. The remainder of the paper is organized as follows. In Section 2, we formally state the tracking control problems that will be addressed in the paper. In Section 3, we propose a valid CLF for tracking control with input constraints. A nonlinear tracking control is then instantiated from the feasible control set and is presented in Section 4. In Section 5, simulation results are given. Finally Section 6 contains our conclusion. 2 Problem Statement Following [20], we assume that each UAV is equipped with standard autopilots for heading hold and Mach hold. In order to focus on the essential issues, we will assume that altitude is held constant. Let (x, y), ψ, and v denote the inertial position, heading angle, and velocity for the UAV respectively. Then the resulting kinematic equations of motion are x˙ = v cos(ψ) y˙ = v sin(ψ) ψ˙ = αψ (ψ c − ψ)

(1)

v˙ = αv (v c − v)

where ψ c and v c are the commanded heading angle and velocity to the autopilots, and αψ and αv are positive constants [20]. In addition, we assume that each UAV has the constraints that vmin ≤ v ≤ vmax and −ωmax ≤ ψ˙ ≤ ωmax , where vmin > 0 and ωmax > 0 is the saturated heading rate. Assuming that αv is large compared to αψ , Eq. (1) reduces to x˙ = v c cos(ψ) y˙ = v c sin(ψ) ψ˙ = αψ (ψ c − ψ). Letting ψ c = ψ +

1 αψ ω

(2)

and v c ≈ v, Eq. (2) becomes

x˙ = v cos(ψ) y˙ = v sin(ψ) ψ˙ = ω

(3)

with input constraints that vmin ≤ v ≤ vmax and −ωmax ≤ ω ≤ ωmax . Note that if vmin = −vmax , then Eq. (3) is the kinematic model for a mobile robot with input constraints.

In this paper we will assume the existence of a reference trajectory (xr , yr , ψr , vr , ωr ) which satisfies x˙ r = vr cos(ψr ) y˙ r = vr sin(ψr ) ψ˙ r = ωr

(4)

under the constraints that vr and ωr are piecewise continuous and satisfy the constraints vmin + ²1 ≤vr ≤ vmax − ²1 −ωmax + ²2 ≤ωr ≤ ωmax − ²2 ,

(5)

where ²1 and ²2 are positive constants. The inclusion of ²i in the constraints of the reference trajectory generator, guarantees that there is sufficient control authority to track the trajectory. We will see that as ²i approach zero, the feasible control set will vanish. The control objective is to find feasible control inputs v and ω such that |xr − x| + |yr − y| + |ψr − ψ| → 0 as t → ∞. Transforming the tracking errors expressed in the inertial frame to the robot frame, the error coordinates [21] can be denoted as      cos(ψ) sin(ψ) 0 xr − x xe  ye  =  − sin(ψ) cos(ψ) 0   yr − y  ψe 0 0 1 ψr − ψ (6) Accordingly, the tracking error model can be represented as x˙ e = ωye − v + vr cos(ψe )

(7)

x˙ 0 = u0 x˙ 1 = (ωr − u0 )x2 + vr sin(x0 )



   x0 ψe  x 1  =  ye  x2 −xe u0 u1

¸

=

·

ωr − ω v − vr cos(x0 )

(8)

(9)

¸

.

The input constraints under the transformation become −²2 ≤ u0 ≤ ²2

Obviously, Equations (6) and (9) are invertible transformations, which means (x0 , x1 , x2 ) = (0, 0, 0) is equivalent to (xe , ye , ψe ) = (0, 0, 0) and (xr , yr , ψr ) = (x, y, ψ). Therefore, the original tracking control objective is converted to a stabilization objective, that is, our goal is to find feasible control inputs u0 and u1 to stabilize x0 , x1 , and x2 . Note from Equation (8) that when both x0 and x2 go to zero, that x1 becomes uncontrollable. To avoid this situation we introduce another change of variables. Let x ¯0 = mx0 +

x1 , π1

(11)

4 p where m > 0 and π1 = x21 + x22 + 1. Accordingly, x1 . Obviously, (¯ x0 , x1 , x2 ) = (0, 0, 0) is x0 = x¯m0 − mπ 1 equivalent to (x0 , x1 , x2 ) = (0, 0, 0). Therefore it is sufficient to find control inputs u0 and u1 to stabilize x ¯0 , x1 , and x2 . With the same input constraints (10), Equation (8) can be rewritten as

x ¯˙ 0 = (m − (ax2 − bx1 ))u0 + (ax2 − bx1 )ωr

+ avr sin x0 + bu1 x˙ 1 = (ωr − u0 )x2 + vr sin(x0 ) x˙ 2 = −(ωr − u0 )x1 + u1 , 1+x22 π13

(12)

−x1 x2 , and where it is obvious π13 1 < 2 , and −2 < ax2 − bx1 < 2.

and b =

that 0 < a ≤ 1, − 12 < b

In this section, we find a valid CLF for tracking control with input constraints. Consider the following class of affine nonlinear time-varying systems

x˙ 2 = −(ωr − u0 )x1 + u1 ,

·

vmin − vmax + ²1 ≤vmin − vr cos(x0 ) ≤ vmin + vmax − ²1 ²1 ≤vmax − vr cos(x0 ) ≤ 2vmax − ²1 .

3 CLF for Tracking Control with Saturation Constraints

Following [13], equation (7) can be simplified as

and

(10)

It is also easy to see that

where a =

y˙ e = −ωxe + vr sin(ψe ) ψ˙ = ωr − ω.

where

vmin − vr cos(x0 ) ≤ u1 ≤ vmax − vr cos(x0 ).

x˙ = f (x, t) + g(x, t)u,

(13)

where x ∈ IRn , u ∈ IRm , and f : IRn × IR+ → IRn and g : IRn × IR+ → IRn×m are locally Lipschitz in x and piecewise continuous in t. Definition 1 A continuously differentiable function V : IRn × IR+ → IR is a control Lyapunov function (CLF) for the system (??) with input constraints u ∈ U ⊂ IRm if it is positive definite, decrescent, radially unbounded in x, and satisfies ¾ ½ ∂V ∂V + (f (x, t) + g(x, t)u) ≤ −W3 (x), inf u∈U ∂t ∂x (14)

∀x 6= 0 and ∀t ≥ 0 where W3 (x) is a continuous positive definite function. In order to find a CLF for systems with bounded input constraints, we prefer the partial derivative of V to be bounded. Accordingly, we have the following lemma. √ Lemma 2 If W (x) = xT x + 1 − 1, then W (x) is continuously differentiable, radially unbounded, positive ° ° ° ≤ 1. definite, and ° ∂W ∂x Proof: Trivial.

Lemma 2 will be used to construct a CLF for system 12. The following lemma defines a valid CLF for tracking control with input constraints.

− ²1 − 2²2 + γ0 ,

and where M1 > 0 and M2 > 0 will be specified in the proof, then W3 (x) is a continuous positive definite function and V is a constrained CLF for system (12) with input constraints (10). Proof: Obviously V is positive definite, decrescent, and radially unbounded, therefore it remains to show that Equation (14) is satisfied. Using straightforward calculations, it is easy to show that under the hypothesis, W3 (x) is continuous and positive definite. Differentiating V we obtain the following expression after some algebraic manipulation: V˙ + W3 (x) = σ1 + σ2 + σ3 u0 + σ4 u1 + σ5 ,

where k1 > the function

1 2

(15)

where x ¯0 2 ) π2 µ ¶ x1 x1 α1 = k1 γ1 (vmin + ²1 ) sin π1 mπ1 µ ¶ µ ¶2 x ¯ 0 x1 x2 α 2 = γ 2 k1 − 2 π2 π1 π1 ¶ ¶ µ µ x1 − vmin , · (vmin + ²1 ) cos mπ1 p ¯20 , and γ0 , γ1 , γ2 ∈ (0, 1). and where π2 = 1 + x α0 = γ 0 (

If

) ( √ d2 √ d1 1 4 , 2 + 2, 2 + 2, , m > max vmin ²2 ²2 acos( vmin +²1 ) π where µ

¶ 1 d1 = vmax + (γ1 + 1) k1 + vmin 2 µ ¶ 3 + 2ωmax + (γ1 + γ2 − 2) k1 − ²1 2 3 2k1 + 2

¶

µ

− 2²2 + γ0 , µ ¶ 1 d 2 = k 1 M1 + k 1 + M2 + vmax + 2ωmax 2

x1 vr sin(x0 ) + α1 π1

x ¯0 ((ax2 − bx1 ) ωr + avr sin (x0 )) + α0 π2 x ¯0 σ3 = (m − (ax2 − bx1 )) π2 ¶ µ x ¯ 0 x1 x2 σ 4 = k1 − π2 π12 π1 σ2 =

and W (x) is defined in Lemma 2, and W3 (x) = α0 + α1 + α2 ,

(17)

where σ1 = k 1

Lemma 3 Consider the function µ ¶ x1 V = k1 W + W (¯ x0 ) , x2

(16)

σ5 = α 2 .

The goal is to show that we can always find a control satisfying (10) to drive V˙ + W3 (x) ≤ 0. Three cases will be considered with respect to x ¯0 . Case 1: |¯ x0 | ≥ 1. From (17), we know that σ1 < k1 (vmax − ²1 + γ1 (vmin + ²1 )), σ2 < 2(ωmax − ²2 ) + (vmax − ²1 ) + γ0 , |σ4 | < k1 + 21 , and σ5 < k1 γ2 ²1 . When x2 < 0, we can choose u1 = vmax − vr cos(x0 ), which is always positive. Thus σ4 u1 < 0. When x2 ≥ 0, we can choose u1 = vmin − vr cos(x0 ). Thus σ4 u1 < (k1 + 21 )(vmin +vmax −²1 ). Combining these two cases, we know that σ4 u1 < (k1 + 21 )(vmin + vmax − ²1 ). We can also choose u0 = −²2 sgn(¯ x0 ). As a result, V˙ + W3 (x) < − |¯πx02 | (m − (ax2 − bx1 ))²2 + d1 , where d1 is defined in (16). Therefore, after some manipulation, we know that if √ m > 2 d²21 + 2, V˙ ≤ −W3 (x). Case 2: 0 < |¯ x0 | < 1. x1 | < π4 , and In this case, we know that | x¯m0 | < π4 , | mπ 1 |x0 | < π2 since m > π4 . We also know that σ1 ≤ β1 vr , x1 where β1 = k1 πx11 (sin(x0 ) + γ1 sin( mπ )). When |¯ x0 | → 1 1, obviously β1 /|¯ x0 | has a finite positive upper bound. When |¯ x0 | → 0, it can be easily verified that β1 ≤ 0. Accordingly, we know that (β1 /|¯ x0 |) ≤ 0 when |¯ x0 | → 0. In fact, in the case of x1 6= 0, |¯xβ10 | → −∞ when

|¯ x0 | → 0. In addition, β1 /|¯ x0 | is continuous when 0 < |¯ x0 | < 1, which means that it has a finite positive upper bound. Thus |¯σx01 | has a finite positive upper bound k1 M1 when 0 < |¯ x0 | < 1, where M1 can be easily specified by numerical methods. Also, when x2 < 0, we can choose u1 = vmax − vr cos(x0 ), which is greater than or equal to ²1 from section 2. In this case, it is easy to see that σ4 u1 + σ5 ≤ 0. When x2 ≥ 0, we can choose u1 = vmin − vr cos(x0 ). In this case, since cos(x0 ) > 0, we know that (σ4 u1 + σ5 ) ≤ β2 , where β2 = (k1 − x ¯ 0 x1 x2 ) (v − (vmin + ²1 ) cos(x0 )) + σ5 . When |¯ x0 | → π2 π12 π1 min 1, it is obvious that β2 /|¯ x0 | has a finite positive upper bound. When |¯ x0 | → 0, it can be verified that β2 ≤ 0 since m > acos( 1vmin ) . Accordingly, we know that

¯ represents unconstrained control valλ0 u0 + λ 1 u1 ≤ λ ues that ensure that V˙ ≤ −W3 (x). The input constraints (10) produce a time-varying rectangle in the u0 –u1 plane. One interpretation of Lemma 3, is that it guarantees that there is always a nonempty intersec¯ tion between the areas described by λ0 u0 + λ1 u1 ≤ λ and the input constraints (10). We call this nonempty intersection the “feasible control set” and denote it by F(t, x). We have the following lemma. Lemma 4 If the time-varying feedback control law k(t, x) satisfies

vmin +²1

β2 /|¯ x0 | ≤ 0 when |¯ x0 | → 0. In fact, in the case of x2 6= 0, β2 /|¯ x0 | → −∞ when x ¯0 → 0. In addition, β2 /|¯ x0 | is continuous when 0 < |¯ x0 | < 1, which means that it has a finite positive upper bound. Thus (σ4 u1 + σ5 )/|¯ x0 | has a finite positive upper bound (k1 + 12 )M2 , where M2 can also be easily specified by numerical methods. We can also choose u0 = −²2 sgn(¯ x0 ). It is also easy to see that σ2 ≤ (2(ωmax − ²³2 ) + (vmax − ²1 ) + γ0 ) |¯ x0 |.´ As a ²2 ˙ result, V + W3 (x) ≤ d2 − (m − (ax2 − bx1 )) |¯ x0 |, π2

where d2 is defined in (16). √ Therefore, if m > 2 d²22 + 2, V˙ ≤ −W3 (x).

Case 3: x ¯0 = 0. In this case, (17) can be written as V˙ + W3 (x) = σ1 + x1 | < π4 since σ4 u1 + σ5 . Similarly, we know that | mπ 1 m > π4 . Since x ¯0 = 0, it is easy to see that σ1 ≤ 0. When x2 ≥ 0, we can choose u1 = vmin − vr cos(x0 ). When x2 < 0, we can choose u1 = vmax − vr cos(x0 ). Thus it can be verified that σ4 u1 + σ5 ≤ 0 since m > 1 . As a result, we know that V˙ ≤ −W3 (x). vmin ) acos(

1. k(t, 0) = 0, 2. k(t, x) ∈ F(t, x),

3. k(t, x) is locally Lipschitz in x and piecewise continuous in t, ∀x 6= 0 and ∀t ≥ 0, then this control solves the tracking problem with input constraints, that is, |xr − x| + |yr − y| + |ψr − ψ| → 0 as t → ∞. Proof: Straightforward using standard Lyapunov stability theory for time-varying systems. As a preliminary result, the following lemma gives a simple control law chosen from the feasible control set. Lemma 5 If

vmin +²1

Therefore, V is a valid CLF for system (12) under saturation constraints (10). Note that a very conservative upper bound is found for m in each case for simplicity of the proof. In reality, m can be much smaller than the upper bound specified above. 4 Nonlinear Tracking Control based on CLF With the CLF given in Lemma 3, our goal in this section is to find a family of feasible tracking control laws based on this CLF. Motivated by equation (17), we consider the following expression ¯ λ0 u0 + λ1 u1 = λ, (18) ¯ = −σ1 − σ2 − σ5 . where λ0 = σ3 , λ1 = σ4 , and λ In the u0 –u1 plane, Equation (18) denotes a line which separates the control space into two halves: λ0 u0 + ¯ and λ0 u0 + λ1 u1 > λ. ¯ The half plane where λ1 u1 ≤ λ

∀x 6= 0,

ksat (t, x) =

µ

¶ ¯0 ) −satu0 (η0 x , satu1 (−η1 x2 )

where (

x, |x| ≤ ²2 (19) sgn(x)²2 , |x| > ²2   vmin − vr cos(x0 ), x < vmin − vr cos(x0 ) satu1 (x) = vmax − vr cos(x0 ), x > vmax − vr cos(x0 ) ,   x, otherwise (20)

satu0 (x) =

then there exist positive constants η0 and η1 such that ksat (t, x) satisfies the conditions of Lemma 4. Proof: Obviously ksat (t, x) satisfies the first and third conditions in Lemma 4. We will show that they also stay in the feasible control set F(t, x). In the proof for Lemma 3, we use discontinuous signum like functions for u0 and u1 . Here we use two saturation functions instead. When x ¯0 and x2 are large, these two sets of

functions are the same, which means the proposed control law stays in the feasible control set F(t, x). When x ¯0 and x2 are close to zero, we can see that u0 = −η0 x ¯0 and u1 = −η1 x2 . By properly tuning η0 and η1 , which may depend on other parameters in Equation (17), the proposed control law can be made to guarantee that V˙ + W3 (x) ≤ 0 based on the property of W3 (x) in Equation (15). We have second order items ( πx¯02 )2 and |¯ x0 | π2

|¯ x2 | π1

and in α0 and ( πx¯21 )2 instead of first order items α2 respectively in W3 (x) in order that besides discontinuous sigum like functions the proposed saturation functions for u0 and u1 can also make σ3 u0 + σ4 u1 dominate the other terms in Equation (17) to guarantee V˙ + W3 (x) ≤ 0 when x ¯0 and x2 are small. Since the proof follows a similar line as that in Lemma 3, a detailed proof is omitted here. In Lemma 5 we used a simple control law that stays in the feasible control set. Other continuous saturation functions like atan, tanh are also possible as long as they stay in the feasible control set. In the case of vr and ωr being uniformly continuous, it is also possible to use geometrical strategies to find feasible control laws (e.g. choose the geometrical center of the feasible control set F(t, x) as feasible controls). One advantage of the CLF-based approach used in this paper is that it only requires vr and ωr to be piecewise continuous instead of being uniformly continuous, which results in wider potential applications than other approaches which requires uniform continuity. Note that if we go back to the original system defined by (3), we can see that v = satu1 (−η1 x2 ) + vr cos(x0 ) and ¯0 ), which is piecewise continuous ω = ωr + satu0 (η0 x in t since vr and ωr are piecewise continuous in t. The other advantage is that it provides the possibility to use other advanced strategies to choose feasible controls from F(t, x). For example, at each time t, a feasible control may be generated from F(t, x) while optimizing some performance index function or minimizing some cost function at the same time, which introduces more flexibility and benefits to the tracking control problem than specifying a fixed control law in advance. In addition, it is also possible to propose a suboptimal controller from F(t, x) based on the combination of model predictive techniques and the tracking CLF (see e.g. [22]). 5 Simulation Results In this section, we simulate a scenario where a UAV is assigned to transition through several known targets in the presence of dynamic threats. The overall system architecture is shown in Figure 1. The design of the Waypoint Path Planner (WPP) and Dynamic Trajectory Smoother (DTS) are described in [23, 24] and [25, 26], respectively. The WPP produces waypoint paths that change in accordance with the dynamic threat envi-

Waypoint Path Planner J ­ Dynamic Trajectory Smoother J ­ Trajectory Tracker J ­ Low-Level Autopilot J ­ UAV Figure 1: Path Planning Architecture. ronment. The DTS smoothes through these waypoints and produces a feasible time-parameterized trajectory that satisfies Equations (4) and (5). The parameters used in this paper are given in Table 5. Parameter vmin vmax ωmax ²1 ²2 vr ωr αψ αv m k1 γ0 γ1 γ2 η0 η1

Value 1.0 (m/s) 2.0 (m/s) 1.7 (rad/s) 0.2 (m/s) 0.2 (rad/s) ∈ [1.2, 1.8] (m/s) ∈ [−1.5, 1.5] (rad/s) 5 50 100 2 0.5 0.5 0.5 10 10

Table 1: Parameter values used in simulation. In Figure 2 shows the problem scenario. The dots are threat locations to be avoided. The waypoint path planner described in [23, 24] generates the waypoint path which is shown in green. The dynamic trajectory smoother described in [25, 26] generates the reference trajectory, which is shown in red. The actual trajectory is shown in blue. The trajectory tracking errors for position and head-

1.8

vr (m/s)

1.65 1.5 1.35 1.2

0

5

10

15

20

25

15

20

25

time (s)

1.5

ωr (rad/s)

0.75

0

−0.75

−1.5

0

5

10 time (s)

Figure 2: The simulation scenario:

waypoint path (green), smoothed reference trajectory (red), and actual trajectory (blue).

Figure 4: The reference control inputs vr and ωr . 2

1.75 v (m/s)

0

r

x −x (m)

1

1.25

−1 −2

1.5

0

5

10

15

20

1

25

0

5

10

time (s)

0

5

10

15

20

25

time (s)

r

ψ −ψ (rad)

25

15

20

25

0.85 0

0.5

−0.85

0

−1.7 0

−0.5 −1

20

1.7

−1 ω (rad/s)

r

y −y (m)

−0.5

−1.5

15 time (s)

0

5

10 time (s)

0

5

10

15

20

25

time (s)

Figure 5: The control inputs v and ω. Figure 3: The trajectory tracking errors expressed in the inertial frame.

ing angle are plotted in Figure 3. We can see that the tracking error for heading angle converges faster than that for x and y coordinate, which is due to the weighting factor m in the definition of x ¯0 . The reference control inputs vr and ωr are plotted in Figure 4. Obviously, vr and ωr are only piecewise continuous instead of uniformly continuous. The reference control inputs generated by the trajectory generator satisfy their constraints respectively, that is, vr ∈ [1.2, 1.8] m/s and ωr ∈ [−1.5, 1.5] rad/s. The control inputs v and ω are plotted in Figure 5. We can see that v and ω are within the range [1, 2] m/s and [−1.7, 1.7] rad/s respectively, which satisfies the input constraints. The original heading rate plot ω is somewhat chattering due to the abrupt change of the reference heading rate ωr , discrete implementation of the system with a sample rate of 100 Hz, and the high gain m in the definition of x ¯0 . The chattering phe-

nomenon is overcome by adding a low pass filter after the control signal ω. 6 Conclusion A tracking CLF for a UAV kinematic models with input constraints is derived. Based on this CLF, a feasible control set is formed. This feasible control set facilitates the generation of a variety of feasible control strategies that not only guarantee accurate tracking but also optimize auxiliary performance functions. A simple saturation control strategy generated from the feasible control set was used and applied to a nontrivial simulation scenario. Acknowledgments This work was funded by AFOSR grants F49620-011-0091 and F49620-02-C-0094, and by DARPA grant NBCH1020013. References [1] I. Kolmanovsky and N. H. McClamroch, “Developments in nonholonomic control problems,” IEEE

Control Systems Magazine, vol. 15, pp. 20–36, December 1995.

and J. C. Willems, eds.), Communications and Control Engineering, ch. 40, pp. 211–216, Springer, 1999.

[2] R. W. Brockett, “Asymptotic stability and feedback stabilization,” in Differential Geometric Control Theory (R. S. Millman and H. J. Sussmann, eds.), pp. 181–191, Birkha¨ user, 1983.

[16] Y. Lin and E. D. Sontag, “Universal formula for stabilization with bounded controls,” Systems and Control Letters, vol. 16, pp. 393–397, June 1991.

[3] C. C. de Wit, B. Siciliano, and G. Bastin, Theory of Robot Control. London: Spriner-Verlag, 1996. [4] Z.-P. Jiang and H. Nijmeijer, “Tracking control of mobile robots: A case study in backstepping,” Automatica, vol. 33, pp. 1393–1399, 1997. [5] J.-M. Yang and J.-H. Kim, “Sliding mode motion control of nonholonomic mobile robots,” Control Systems Magazine, vol. 19, pp. 15–23, April 1999. [6] T. Fukao, H. Nakagawa, and N. Adachi, “Adaptive tracking control of a nonholonomic mobile robot,” IEEE Transactions on Robotics and Automation, vol. 16, pp. 609–615, October 2000. [7] Z. Artstein, “Stabilization with relaxed controls,” Nonlinear Analysis, Theory, Methods, and Applications, vol. 7, no. 11, pp. 1163–1173, 1983. [8] E. D. Sontag, “A Lyapunov-like characterization of asymptotic controllability,” SIAM Journal on Control and Optimization, vol. 21, pp. 462–471, May 1983. [9] E. D. Sontag, “Smooth stabilization implies coprime factorization,” IEEE Transactions on Automatic Control, vol. 34, pp. 435–443, April 1989. [10] A. M. Bloch, N. H. McClamroch, and M. Reyhanoglu, “Controllability and stabilizability properties of a nonholonomic control system,” in Proceedings of the 29th Conference on Decision and Control, (Honolulu, HI), pp. 1312–1314, December 1990.

[17] Y. Lin and E. Sontag, “Control-lyapunov universal formulas for restricted inputs,” Control-Theory and Advanced Technology, vol. 10, pp. 1981–2004, December 1995. [18] R. A. Freeman and P. V. Kokotovic, Robust Nonlinear Control Design: State-Space and Lyapunov Techniques. Systems & Control: Foundations & Applications, Birkhauser, 1996. [19] J. W. Curtis and R. W. Beard, “Satisficing: A new approach to constructive nonlinear control,” IEEE Transactions on Automatic Control, (in review). Available at http://www.ee.byu.edu/~beard/papers/preprints/CurtisBeard01b.pdf. [20] A. W. Proud, M. Pachter, and J. J. D’Azzo, “Close formation flight control,” in Proceedings of the AIAA Guidance, Navigation, and Control Conference and Exhibit, (Portland, OR), pp. 1231–1246, American Institute of Aeronautics and Astronautics, August 1999. Paper no. AIAA-99-4207. [21] Y. J. Kanayama, Y. Kimura, F. Miyazaki, and T. Noguchi, “A stable tracking control scheme for an autonomous mobile robot,” in Proceedings of the IEEE International Conference on Robotics and Automation, pp. 384–389, 1990. [22] B. Kouvaritakis and M. Cannon, Nonlinear Predictive Control: Theory and Practice. London, United Kingdom: The Institution of Electrical Engineers, 2001.

[11] A. W. Divelbiss and J. T. Wen, “Trajectory tracking control of a car-trailer system,” IEEE Transactions on Control Systems Technology, vol. 5, pp. 269– 278, May 1997.

[23] R. W. Beard, T. W. McLain, M. Goodrich, and E. P. Anderson, “Coordinated target assignment and intercept for unmanned air vehicles,” IEEE Transactions on Robotics and Automation, vol. 18, pp. 911– 922, December 2002.

[12] R. Fierro and F. Lewis, “Control of a nonholonomic mobile robot: backstepping kinematics into dynamics,” in Proceedings of the 34th Conference on Decision and Control, (New Orleans, LA), pp. 3805–3810, December 1995.

[24] T. McLain and R. Beard, “Cooperative rendezvous of multiple unmanned air vehicles,” in Proceedings of the AIAA Guidance, Navigation and Control Conference, (Denver, CO), August 2000. Paper no. AIAA-2000-4369.

[13] T.-C. Lee, K.-T. Song, C.-H. Lee, and C.-C. Teng, “Tracking control of unicycle-modeled mobile robots using a saturation feedback controller,” IEEE Transactions on Robotics and Automation, vol. 9, pp. 305–318, March 2001.

[25] E. P. Anderson and R. W. Beard, “An algorithmic implementation of constrained extremal control for UAVs,” (Monterey, CA), August 2002. AIAA Paper No. 2002-4470.

[14] Z.-P. Jiang, E. Lefeber, and H. Nijmeijer, “Saturated stabilization and track control of a nonholonomic mobile robot,” Systems and Control Letters, vol. 42, pp. 327–332, 2001. [15] E. D. Sontag, “Control-lyapunov functions,” in Open Problems in Mathematical Systems and Control Theory (V. D. Blondel, E. D. Sontag, M. Vidyasagar,

[26] E. P. Anderson, R. W. Beard, and T. W. McLain, “Real time dynamic trajectory smoothing for uninhabited aerial vehicles,” IEEE Transactions on Control Systems Technology, (in review).