Decentralized Trajectory Tracking with Collision Avoidance Control for Teams of Unmanned Vehicles with Constant Speed Erick J. Rodr´ıguez-Seda Abstract— In this paper, we present a decentralized trajectory tracking control with collision avoidance for an arbitrarily large group of heterogeneous, nonholonomic vehicles. We consider vehicles with different constant speeds and bounded turning rates (e.g., airplanes), for which their avoidance maneuverability is limited. The proposed controller is divided in two parts: an auxiliary system and a local heading control. The auxiliary system is in charge of guiding the vehicle toward the desired trajectory while guaranteeing collision avoidance with other agents at all times. The local heading control is designed such that the vehicle remains continuously within a bounded, short distance of the auxiliary system. To enforce the convergence of the vehicles to a proximity of their desired trajectories and avoid deadlocks (a common issue within most decentralized navigation methods), an alternative discontinuous control strategy is presented. Finally, a numerical example is provided to illustrate the performance of the proposed control strategy.
I. I NTRODUCTION Recent technological advances in electronics, computing, and communications have enabled the use of autonomous vehicles in a wide range of applications. Nowadays, autonomous vehicles are used for law enforcement [1], transportation [2], household chores [3], military operations [4], and scientific exploration [5], [6]. Certainly, the level of accuracy and complexity expected from the vehicles varies among tasks. Yet, all applications demand a common objective from the vehicles: to satisfactorily complete the desired task while avoiding collisions with obstacles and the environment. Based on this common objective, several autonomous navigation control solutions have been studied. They varied by methodology (e.g., prescribed, optimization, and force fields methods, as categorized by Kuchar and Yang [7]) and by levels of guarantees and robustness. A comprehensive review of these methodologies is out of the scope of this paper and the interested reader can consult with [7]–[10]. A successful control solution for the operation of an unmanned vehicle must take into consideration the vehicle’s kinematic and dynamic limitations. For instance, many of the currently employed unmanned vehicles, such as car-like systems and airplanes, have nonholonomic constraints. By nonholonomic constraints we mean that not every direction of motion is admissible (e.g., move sideways) and that there is no smooth time-invariant state feedback control law capable to asymptotically stabilize the system at any given point [11]. Therefore, conventional linear control techniques E. J. Rodr´ıguez-Seda is with the Department of Weapons and Systems Engineering, United States Naval Academy, Annapolis, MD 21402, USA.
[email protected]
cannot be directly applied to regulate the motion of the vehicle. Additionally, the control solution must take into account the potential interaction of the vehicle with other agents,1 that is, it must react safely to potential conflicts. Some examples of safe control strategies for teams of unmanned nonholonomic vehicles are those presented in [12]–[18]. In addition to the nonholonomic nature of most vehicles, many of them possess restrictions on their velocities and turning rates. For example, airplanes must maintain a minimum linear speed to generate enough lift to keep the vehicle in the air. Similarly, some underwater vehicles and marine vessels cannot decelerate and accelerate (i.e., change their velocity vector) arbitrarily fast due, in part, to their large inertia. That means that the vehicles cannot stop or turn sideways immediately. These vehicles are typically modeled as nonholonomic systems with bounded speed and, in a more conservative approach, with constant speed.2 Based on this assumption, several safe control methods have been reported. Examples include the work of [10], [19], where collision-free strategies for group of nonholonomic vehicles with bounded speeds are developed using the concept of velocity obstacles [20], and the work of [21], where control strategies are proposed based on the use of navigation or potential field functions. Similarly, a hybrid approach with a predefined protocol that each vehicle must follow in order to guarantee collision avoidance is presented in [22] for vehicles with constant speed and bounded turning rates. Other successful strategies for vehicles with similar constraints are presented in [23], [24]. In this paper, we introduce a new cooperative control strategy that guarantees trajectory tracking and collision avoidance for an arbitrarily large group of nonholonomic vehicles with constant speeds and bounded turning rates. In contrast to the work of [19], [22], the vehicles considered herein can be heterogeneous, meaning that the proposed control strategy can be directly applied to vehicles with different constant speeds and turning rates. The proposed controller is divided in two parts: an auxiliary control system and a local heading control. The auxiliary system is in charge of tracking the desired trajectory while maintaining a safe distance from other vehicles at all times. The local heading control is designed such that the nonholonomic vehicle follows the auxiliary system within some predefined, bounded error. Overall, the control law has the advantage 1 By
agents we mean other vehicles and/or obstacles. solution for a constant speed vehicle can be applied to a nonconstant speed vehicle by restricting the velocity of the latter. The reverse is not true. 2A
of being 1) decentralized, meaning that only information of nearby agents is required, which in turns reduces the computational complexity of the control algorithm and adds reliability to the multi-vehicle network; 2) reactive, implying that the control inputs to the vehicles are computed online as other vehicles are detected; and 3) independent of other agents’ velocity information (as opposed to [10]) and/or control intentions. Moreover, we discuss the liveness of the multi-vehicle system under the developed control law and propose a modified stable solution that guarantees, for at least the case of two vehicles, that each agent converges to its desired target configuration. An example with eight vehicles is finally presented to illustrate the performance of the proposed control strategy. II. P ROBLEM F ORMULATION A. Modeling of the Vehicles We consider a team of N unmanned, heterogeneous nonholonomic vehicles with constant speed vi and with motion governed by the following kinematic equation x˙ i (t) vi cos φi (t) y˙ i (t) = vi sin φi (t) , for i ∈ {1, · · · , N } (1) ωi (t) φ˙ i (t)
where zi (t) = [xi (t), yi (t)]T , φi (t), and ωi (t) denote the position, orientation (i.e., heading), and control input (turning rate), respectively, for the ith vehicle. We assume that the vehicles’ turning rates are bounded, that is, kωi (t)k ≤ ω ¯i ∀t ≥ 0 and for some ω ¯ i > 0. The latter assumption implies that the vehicles have bounded curvature with radius RCi ≥ vi ω ¯ i . Note that, in contrast to the work in [19], [22], we do not require the vehicles to have the same linear speeds and bounded curvatures. To guarantee a safe interaction between all vehicles, we further assume that the agents can bi-directionally communicate their positions (or alternatively, sense each other) whenever they are at a close distance. Without loss of generality, we assume the each agent has a circular communication (or sensing) region of radius R > 0. That is, the ith vehicle can detect the (relative) position of the jth vehicle if their distance is smaller than R. B. Control Objective The main objective is to safely drive a group of nonholonomic vehicles with constant speeds and bounded turning rates along their desired trajectories. Specifically, we would like to design a control policy ωi (t) such that zi (t) converges to B(zdi (t), rBi ), where zdi (t) = [xdi (t), yid (t)]T denotes the desired trajectory and B(zdi (t), rBi ) is a bounded circle centered at zdi (t) with radius rBi to be defined (see
Fig. 1). We assume that the desired trajectories satisfy z˙ di (t) ≤ vid < vi ∀t ≥ 0. The latter condition is required to guarantee that the vehicles can reach B(zdi (t), rBi ) in finite time. We assume that the desired vehicles’ headings are unspecified. Simultaneously, we would like the vehicles to maintain a safe inter-agent distance at all times. Mathematically, we mean that ∀i, j ∈ {1, · · · , N }, i 6= j and ∀t ≥ 0,
kzi (t) − zj (t)k > rij , where rij is the minimum safe distance between the ith and jth vehicles. Without loss of generality, we will assume that rij = r > 0 for all vehicles. We will further assume that R − r > 4 · maxi {rBi } to accommodate for the detection of the vehicles’ convergence regions, i.e., B(zdi (t), rBi ). Remark 2.1: Note that convergence to any desired trajectory zdi with arbitrarily small error (i.e., rBi → 0) is in general unfeasible due to the vehicles’ velocity and turning rate constraints. For example, the vehicles cannot be stabilized at a stationary point nor can turn sideways arbitrarily fast. C. Preliminaries In this section we introduce some definitions that will help us to develop the control strategies for the team of vehicles. Let v and w be two vectors in ℜn and b > a > 0 two constant parameters. We define the following attractive potential function T : ℜn × ℜn → [0, ∞) as 1 2 kv − wk . (2) 2 Similarly, we define the following repulsive potential function (also called avoidance function) A : ℜn ×ℜn ×(0, ∞)× (0, ∞) → [0, ∞) as )!2 ( 2 kv − wk − b2 . (3) A(v, w, a, b) = min 0, 2 kv − wk − a2 T (v, w) =
The reader can easily verify that A is almost everywhere continuously differentiable and that ∂T (v, w)T ∂T (v, w)T =− =v−w (4) ∂v ∂w ∂A(v, w, a, b)T ∂A(v, w, a, b)T =− ∂w ∂v 0 if kv − wk ≥ b 4(b2 −a2 )(kv−wk2 3−b2 ) (v − w) if a < kv − wk < b (kv−wk2 −a2 ) = . undefined if kv − wk = a 0 otherwise (5) Similar avoidance functions have been successfully used in [12], [25]–[27]. Now, our main objective is to guarantee that the vehicles remain at a safe distance from each other at all times. Geometrically, it means that there is a region around the ith vehicle that no other vehicle should enter. Accordingly, for any pair of vehicles we define an avoidance set as Ωrij = {z : z ∈ ℜ2N , kzi − zj k ≤ r}
(6)
where z = [zT1 , · · · , zTN ]T . We say that a collision between the ith and jth vehicles occurs if for some time tc ≥ 0, z(tc ) ∈ Ωrij . Similarly, for each pair of vehicles we define their communication or Detection Region as R Dij = {z : z ∈ ℜ2N , kzi − zj k ≤ R}.
(7)
α
Ωrij R Dij
α
B
R Dij
zα i
Ωrij
Fig. 1. Illustration of the Detection Regions for the ith vehicle and its R and D Rα ), their Avoidance Regions (i.e., Ωr auxiliary system (i.e., Dij ij ij α and Ωrij ), and the bounded convergence region for zi (i.e., B(zdi , rBi )).
The latter implies that the ith and jth vehicles can commuR . nicate or detect their positions at time td ≥ 0 if z(td ) ∈ Dij The Avoidance and Detection Regions for the ith vehicle are represented in Fig. 1 by the smallest and largest circular regions, respectively. III. C ONTROL F RAMEWORK To achieve our control objective, namely trajectory tracking with collision avoidance, we propose a control framework for each vehicle comprised of an auxiliary system and a local heading control. An illustration of the control framework is depicted in Fig. 2. The goal of the auxiliary system is to track the vehicle’s desired trajectory while maintaining a safe distance from any other vehicle, whereas the objective of the local heading control is to maintain the nonholonomic vehicle and the auxiliary system within a bounded distance from each other. A. Auxiliary System The kinematic equations of the auxiliary system are given by3 α o x˙ i x˙ i o α = uα (8) = u , z ˙ = z˙ oi = i i i y˙ iα y˙ io α T o α where [xoi , yio , xα i , yi ] is the state vector and ui and ui are two control strategies to be designed. We will interpret the α state variables xα i and yi as the Cartesian coordinates (i.e., position) of the auxiliary systems and the state variables xoi and yio as their targets or desired position goals. The control strategy uoi will be designed to enforce the tracking objective, 3 In what follows we will omit time dependence of signal except when considered necessary.
whereas uai will guarantee collision avoidance. With this in mind, we propose the first control strategy to be given by ! o d T o ∂T (z , z ) /∂z i i
i uoi = z˙ di − κoi
∂T (zo , zd )T /∂zo + σ i i i ( ) P α α α 2 j∈Ni A(zα i , zj , r , R ) × max 0, 1 − P (9) α α α α j∈Ni A(zi , zj , r , R ) + σh
where κoi ∈ (0, vi − vid ) is a design parameter, Ni = {1, · · · , N }−{i} defines the neighborhood of the ith vehicle, 2 2 2 2 σ > 0 is a constant, and σh = (hα − Rα )2 (hα − rα )−2 for some control parameters R > Rα > hα > rα > r > 0. The parameters Rα and rα represent the detection and avoidance radii, respectively, for the auxiliary systems and will be defined in Section V. The radius hα denotes the distance at which the vehicles activate their avoidance strategy uai (defined next) and shut-down the trajectory tracking control uoi . Note that uoi is a continuous law, assuming that the desired trajectory and the vehicles’ trajectories are also continuous. Analogous to the previous definitions of Avoidance (6) and Detection Regions (7) for the ith and jth vehicles, we α Rα , for define the Avoidance, Ωrij , and Detection Regions, Dij their auxiliary systems as
α α ≤ rα } (10) Ωrij ={zα : zα ∈ ℜ2N , zα i − zj
α α α R α α 2N α (11) Dij ={z : z ∈ ℜ , zi − zj ≤ R }
αT T where zα = [zαT 1 , · · · , zN ] . For the second control equation, we propose the following control law
where
˙ oi − κα uα i =z i o Vi = T (zα i , zi ) +
∂ViT /∂zα
i
+σ ∂ViT /∂zα i
X
α α α A(zα i , zj , r , h )
(12)
(13)
j∈Ni d o for some κα i ∈ (0, vi − vi − κi ). It is worth to mention that the number of nonzero avoidance functions and, therefore, the number of computations in (9) and (12) is bounded by the number of agents (Ni ) that can safely interact with the ith vehicle at any given time. This number is determined by the size of the vehicles’ Detection and Avoidance Regions and is given by [28] ! r Rα − r α 1 π . + Ni ≤ rα 2 3rα2 sin−1 α α 2R −r
Remark 3.1: Note that we implicitly assumed that the vehicles can communicate the coordinates of their auxiliary systems–instead of (xi , yi )–whenever the vehicles are within each other’s Detection Region. Also note that, from (9), we α have that if zα ≤ hα for some j, then z˙ oi = uoi = 0. i − zj This means that the ith vehicle will give priority to the avoidance task over the trajectory tracking control if it detects a collision threat.
Desired trajectory zdi , z˙ di Information from nearby vehicles zα j
Auxiliary System
Fig. 2.
if D(˜ xi , y˜i ) ≥ rφi and
zα i
Heading Control
xi , y˜i ) + 3rφi 2 xi , y˜i )2 − 3rφi D(˜ x ˜i y˜˙ i − y˜i x ˜˙ i D(˜ φ˙ α i = D(˜ xi , y˜i ) rφi 3 y˜i (˜ xi , y˜i ) − 6rφi ) ˜˙ i + y˜i y˜˙ i )(3D(˜ xi x + atan2 3 x ˜i rφi y˜i 3(˜ xi x ˜˙ i + y˜i y˜˙ i ) + atan2 (17) xi , y˜i ) x ˜i rφi D(˜
ωi
Proposed control framework.
zdi
Bi zα i zdj
otherwise. Using (16), (17), and the Cauchy–Schwarz Inequality we can bound φ˙ α i as
zα j Bj
1 α Fig. 3. The auxiliary control is designed such that B(zα i , 2 r ) and 1 α B(zα , r ), represented by the blue-gray and red-gray circles, follow their j 2 desired trajectories without any overlapping between both regions. The local heading control is designed such that the nonholonomic vehicles, denoted 1 α α 1 α by the blue and red airplanes, remain inside B(zα i , 2 r ) and B(zj , 2 r ), respectively, at all times.
Remark 3.2: To illustrate the purpose of the auxiliary system, let us assume that the ith vehicle, zi , remains at all times inside a bounded circle of radius 21 rα and α 1 α center zα i , denoted as B(zi , 2 r ) (see Fig. 3). Then, the overall objective of the auxiliary system is to drive the d α 1 α center of B(z T i , 2αr 1) αtoward zi while guaranteeing that α 1 α B(zi , 2 r ) B(zj , 2 r ) = ∅ for any pair i 6= j, where ∅ is the empty set. The latter condition implies
that no
two circles α − z should overlap at any given time (i.e., zα should be i j equal or greater than rα ) as illustrated in Fig. 3. Since each vehicle remains inside its circular region, the latter condition also implies that no two vehicles should collide. In Section III-B we will design the heading control law to guarantee 1 α α that zi (t) ∈ B(zα i (t), rBi ) ⊂ B(zi (t), 2 r ), ∀t ≥ 0. B. Heading Control We design the heading control such that the ith nonholonomic vehicle remains at a close distance of zα i . Accordingly, we propose the heading control equation to be given by ωi = ki
φα i − φi + φ˙ α i − φ k 1 + kφα i i
(14)
for some ki > 0 and xi , y˜i ) − rφi )3 + rφi3 y˜i min rφi3 , (D(˜ α φi =atan2 (15) x ˜i rφi 3 ˜ i = xα − xi and where rφi > 0 is a design parameter, x ip α xi , y˜i ) = x y˜i = yi −yi are the error signals, and D(˜ ˜2i + y˜i2 is the Euclidean distance between the auxiliary system and the position of the ith vehicle. Note that (15) is continuously differentiable and that x ˜i y˜˙ i − y˜i x ˜˙ i φ˙ α i = D(˜ xi , y˜i )2
(16)
v + vd
˙ α i i xi , y˜i ) + 3rφi 2 D(˜ xi , y˜i )2 − 3rφi D(˜
φ i < 3 rφi
vi + vid
3D(˜ xi , y˜i ) + 3rφi 2 xi , y˜i )2 − 6rφi D(˜ +π 3 rφi ≤3(vi + vid )rφi −1 (1 + π)
˜˙ i , y˜˙ i ]T < vi + vid and that where we used the fact that [x the bound on (17) is larger than the bound on (16). From the above equation we have that the ith vehicle’s turning rate constraint is satisfied if ki and rφi are chosen such that ¯i. ki + 3(1 + π)(vi + vid )rφi −1 ≤ ω
(18)
Note that we can always set ki sufficiently small and rφi sufficiently large such that (18) holds. IV. T RAJECTORY T RACKING In this section we will show that 1) the ith vehicle remains within a bounded, predefined distance rBi ≤ 12 rα of its auxiliary system at all times and that 2) the auxiliary system effectively tracks the desired trajectory when there is no threat of a collision. The synthesis of both results implies that the ith vehicle converges to B(zdi , rBi ). Theorem 4.1: Assume ∃t0 ≥ 0 such that φα i (t0 ) = φi (t0 ) . = r ) for r (t ), r and zi (t0 ) ∈ B(zα φi Then zi (t) ∈ Bi Bi i 0 ) ∀t ≥ t . (t), r B(zα 0 Bi i Proof: We first show that if φα i (t0 ) = φi (t0 ), then 1 α 2 φα i (t) = φi (t) ∀t ≥ t0 . Consider Vφ = 2 (φi − φi ) . Taking its time derivative yields 2
kφα i − φi k ˙α V˙ φ = (φα ≤ 0. i − φi )(φi − ωi ) = −ki 1 + kφα i − φi k Since V is positive-definite and V˙ φ < 0 ∀φα i 6= φi , we conclude that φi (t) → φα (t) as t → ∞. Therefore, if for i α (t) = φ (t) ∀t ≥ t . (t ) = φ (t ), then φ some t0 , φα i 0 i 0 i i 0 assume that at t = t0 , φα i (t0 ) = φi (t0 ) and
Now
[˜ xα ˜iα (t0 )]T ≤ rBi (the first assumption implies that i (t0 ), y ∗ φi (t) ≡ φα yi /˜ xi ) and let i (t) ∀t ≥ t0 ). Define φi = atan2(˜ rBi = rφi . Using (15) we have that ∗ φi =φα i = φi + ∆φ ( 0, ∆ = φ
(D(˜ xi ,˜ yi )−rφi )3 +rφi 3 rφ i 3
− 1 φ∗i ,
if D(˜ xi , y˜i ) ≥ rφi otherwise
Take VB = T (zα i , zi ) as our Lyapunov-candidate function. Computing its time derivative we obtain that cos(φ∗i + ∆φ ) α T α ˙ (19) VB =(zi − zi ) z˙ i − vi sin(φ∗i + ∆φ )
which ∀ zi ∈ / B(zα i (t), rBi ) yields
d V˙ B ≤ kzα i − zi k (vi − vi ) < 0.
Since VB is decreasing outside of the set B(zα i (t), rBi ), we conclude that all solutions of (1) converge to B(zα i (t), rBi ). Therefore, any solution starting in B(zα i (t0 ), rBi ) remains in B(zα i (t), rBi ) ∀t ≥ t0 . Remark 4.1: Note that the results in Theorem 4.1 depend on the initialization of the controller, i.e., φα i (t0 ) = φi (t0 ) and kzi (t0 ) − zα i (t0 )k ≤ rBi , for which the designer has complete authority. Theorem 4.1 establishes that any trajectory zi (t) emaα nating from the set B(zα i (t), rBi ) remains in B(zi (t), rBi ) α ∀t. The next theorem will show that B(zi (t), rBi ) → B(zdi (t), rBi ) as t → ∞.
α α
Theorem 4.2: If ∃t⋆ ≥ 0 such that zα i (t) − zj (t) ≥ R d α ∀ j ∈ Ni and t ≥ t⋆ , then zi (t) → zi (t) as t → ∞. Proof: Consider the kinematic equations for the ith vehicle’s auxiliary system (8) and assume that for some time
α
≥ Rα ∀ j ∈ Ni , t ≥ t⋆ (i.e., t⋆ ≥ 0, zα i (t) − zj (t) o no collision threat). Let VT = T (zoi , zdi ) + T (zα i , zi ) be our Lyapunov-candidate function. Taking its time-derivative yields o ∂T (zα ∂T (zoi , zdi ) o i , zi ) α d ˙ ˙ ) + − z ( z (z˙ i − z˙ oi ) V˙ T = i i ∂zoi ∂zα i
o 2 o d
o ∂T (zi , zi )/∂zi = − κi ∂T (zo , zd )T /∂zo + σ i
−
i i α o α 2 k∂T (z , z )/∂z i i i k κα i α o α T k∂T (zi , zi ) /∂zi k +
σ
.
T d T T o o T Since V˙ T < 0 ∀ [(zα i − zi ) , (zi − zi ) ] 6= [0, 0, 0, 0] , d o α we can conclude that zi (t) → zi (t) → zi (t) as t → ∞ and the proof is complete. A consequence of Theorems 4.1 and 4.2 is that zi (t) remains in B(zα i (t), rBi ) which in turns converges to B(zdi (t), rBi ) as t → ∞ if there is no collision threat. In Section VI we will discuss the convergence of the ith vehicle to the desired trajectory taking into account collision threats.
V. D ECENTRALIZED C OLLISION AVOIDANCE We now show that the proposed control strategy guarantees the safe interaction of all vehicles as long as the vehicles start from a safe distance. We will show that the vehicles’ auxiliary systems do not enter each other’s auxiliary Avoidance Regions at any time, which in turns implies that the vehicles remain within a safe distance regardless of their desired trajectories. Theorem 5.1: Consider a team of N cooperative vehicles with kinematic equations given by (1) and with control law as described in Section III. Define rα = r + 2 · maxi {rBi }, Rα = R − 2 · maxi {rBi }, and hα ∈ (rα , Rα ) and assume
S α α that for some t0 ≥ 0, zα (t0 ) ∈ / Ωr = i,j,i6=j Ωrij . Then, for all i and all t ≥ t0 , we have that zi (t) − zoi (t) remains α bounded and zα (t) ∈ / Ωr . Proof: Consider the following positive-definite function N X X 1 α α α o T (zα A(zα VC = i , zj , r , h ) . (20) i , zi ) + 2 i j∈Ni
Taking its time derivative yields that V˙ C =
N X ∂T (zα , zo ) i
i
+
i
∂zα i
˙ oi ) (z˙ α i −z
N α α α 1 X X ∂A(zα i , zj , r , h ) α (z˙ i − z˙ α j ). 2 i ∂zα i j∈Ni {z } | PN P i
j∈Ni
(21)
∂A(zα ,zα ,r α ,hα ) i j z˙ α i ∂zα i
P Using the fact that z˙ oi ≡ 0 whenever j∈Ni Aij 6= 0, we have that (21) reduces to4
∂Ti 2 ∂Ti P ∂A N
∂zα + ∂zα j∈Ni ∂zαij X i i α ˙
i κi VC = − ∂AT
∂Ti P ij
∂zα + j∈Ni ∂zα + σ i i i
2
P T P
∂T ∂A ∂Aij ij i N +
X j∈Ni ∂zα j∈Ni ∂zα ∂zα i i i
κα − T i ∂A
∂Ti P
∂zα + j∈Ni ∂zαij + σ i i i 2 P ∂A ∂T ij i N + j∈Ni ∂zα X ∂zα i i
κα =− ≤0 (22) i P ∂AT ∂Ti ij
∂zα + j∈Ni ∂zα + σ i i
i
Since V˙ C ≤ 0, we can integrate (22) and obtain that VC (t) ≤ V (0) ≤ ∞, which implies that zoi − zi remains bounded. Now assume that for some i and j 6= i, zα → Ωα ij . The latter implies that Aij → ∞ ⇒ VC → ∞. However, VC is bounded which means that we reach a contradiction. Since α the solutions of (1) are continuous, zα must never enter Ωr .
The above theorem establishes that if α α α α α (t) − z (t) k > r (t ) − z (t ) k > r , then kz kzα j j 0 i i 0 ∀t ≥ t0 and ∀i, j regardless of the size of the group. Since α Ωrij ⊂ Ωrij , the latter implies that kzi (t) − zj (t)k > r ∀t and ∀i, j. It also demonstrates that the error between zoi and zi remains bounded. It however can not prove if the vehicles successfully track their desired trajectories. In fact, for some initial configurations and desired trajectories, the agents may never reach their targets. In the next section we will modify the auxiliary control law with a discontinuous perturbation that will guarantee at least the convergence of two vehicles to their desired trajectories. VI. L IVENESS We now evaluate the liveness of the multi-vehicle network. Specifically, we will evaluate the existence of unwanted 4 To avoid cluttering of equations, let ∂T (zα , zo )/∂zα = ∂T /∂zα and i i i i i α α α α α ∂A(zα i , zj , r , h )/∂zi = ∂Aij /∂zi .
local minima (i.e., deadlocks) which is a fundamental problem of most potential (and avoidance) field function based navigation methods [29]. We say that a deadlock occurs when an auxiliary system (and therefore, a vehicle) stays in an equilibrium other than the desired trajectory. As the next theorem will imply, a deadlock is a consequence of symmetries between their trajectory control P and avoidance α T = 6 0 for /∂z ∂A → − laws, i.e., when ∂TiT /∂zα i ij i j∈Ni some i. To break the symmetries, we propose the following piecewise, continuous modification to the auxiliary control (9) if [c1 , c2 ]˙zoi ≥ 0 [−c2 , c1 ]T , γi p 2 and [c1 , c2 ]T 6= 0 c1 + c22 uoi = o o d T o κi ∂T (zi , zi ) /∂zi d
z˙ i −
∂T (zo , zd )T /∂zo + σ , otherwise i i i (23)
where γi is a positive constant and [c1 (t), c2 (t)]T = P T α j∈Ni ∂Aij /∂zi . The difference between the control law in (9) and the new piecewise continuous law in (23) is that (23) does not necessarily interrupt the trajectory tracking process of the ith vehicle when another vehicle enters its Detection Region. Instead, the auxiliary control continues to track the desired trajectory as long as it does not conflict with the avoidance control that the tracking function P (note remains in effect if zoT ∂ATij /∂zα i i ≤ 0, which implies that zoi moves opposite to the collision threat). If the motion of the auxiliary state zoi moves toward the collision threat, then the control law (23) chooses a motion perpendicular to the collision vector. In fact, this motion is a rotation of the multi-vehicle network in a clockwise direction. By moving the network in a clockwise direction, the vehicles should eventually come to a configuration where their tracking and avoidance control laws do not conflict with each other, that is, if their desired trajectories are sufficiently apart. Theorem 6.1: Consider a team of N cooperative vehicles with kinematic equations given by (1) and with control law described by (12), (14), and (23). Define rα = r + 2maxi {rBi }, Rα = R − 2maxi {rBi }, and hα ∈ (rα , Rα ). α Assume that ∃t0 ≥ 0 such that zα (t0 ) ∈ / Ωr . Then, α zα (t) ∈ / Ωr ∀t ≥ t0 . Proof: Consider the candidate Lyapunov function in (20). Taking its time derivative yields
2
∂Vi N N κα
X X ∂ATij X α i ∂z oT
T i ≤ 0 (24) z ˙ + V˙ C = − i
∂Vi ∂zα i α + σ i i ∂zi
j∈Ni
where we used the fact that the last term is always nonpositive. From (24) we can conclude (similar to the proof of α Theorem 5.1) that zα (t) ∈ / Ωr ∀t ≥ t0 . Corollary 6.1: Consider the statement in Theorem 6.1. Let N = 2 and assume that ∃t0 such that zd1 (t) − zd2 (t) ≥ o d R for all t ≥ t0 . Then, zα i (t) → zi (t) → zi (t) as t → ∞. Proof: Consider the candidate Lyapunov function in (20) and its time derivative (24), which is nonpositive. Define oT T ˙ α , z˙ o } are bounded and, zoT = [zoT 1 , · · · , zN ] . Since {z
TABLE I C ONTROL AND S YSTEM PARAMETERS Vehicle Parameter vi (m/s) ω ¯ i (rad/s) vid (m/s) Rα (m) r α (m) rφi (m) rBi (m) κoi (m/s) κα i (m/s) ki (rad/s) γi (m/s)
i ∈ {1, 2, 3, 4}
i ∈ {5, 6, 7, 8}
1.50 π 1.00 24.86 19.14 10.00 6.75 0.35 0.15 2.39 0.02
2.00 2π 1.81 24.86 19.14 8.00 7.57 0.13 0.06 4.85 0.02
consequently, {zα , zo } are absolutely continuous, we can apply LaSalle’s Invariance Principle for Nonsmooth Systems [30] and conclude that ζ = [zαT , zoT ]T approaches the ∂Aij o invariance set S = {ζ : ζ ∈ ℜ4N , ∂ViT /∂zα ·z˙ i = i = 0, ∂zα i 0, ∀i}. A solution of ζ ∈ S means that either P ∂ATij /∂zα 1) ∂TiT /∂zα i ≡ 0 ∀i, or i ≡− j∈N Pi T α 2) ∂Ti /∂zi ≡ − j∈Ni ∂ATij /∂zα 6= 0 and i P ∂Aij o · z ˙ = 0. i j∈Ni ∂zα i
o d The former implies that zα Theoi → zi → ziα (by applying
α h rems 4.1 and 4.2) and that z (t) ∈ / Dij since zdi − zdj ≥ R, which proves the claim. Therefore, let us consider the latter case. ≡ Let N = 2 and assume that ∂TiT /∂zα i P T 6= 0 for i, j ∈ {1, 2}. Then, − j∈Ni ∂Aij /∂zα i o hα kzα 6 0 and zα ∈ D12 . Moreover, assume that i − zi k = ∂Aij o = 0, which implies that zoi moves perpendicu· z ˙ α i ∂zi ˙ oi 6= 0 by definition (23). Then, lar to ∂ATij /∂zα i since z ˙ oi and therefore, zα using (12) we have that z˙ α i also i = z ˙ oi ˙α moves perpendicular to ∂ATij /∂zα i = z i . Likewise, z T α implies that ∂Ti /∂zi must remain constant. However, T α α α since ∂AT12 /∂zα 1 = −∂A21 /∂z2 , we have that z1 and z2 must move in opposite directions, away from each other,
which in turns implies that ∂ATij /∂zα i must decrease.
≡ Since of ζ ∈ S requires that ∂TiT /∂zα i
T a solution
∂A /∂zα , the latter results in a contradiction. Therefore, ij i thePonly solution of ζ that can remain in S is ∂TiT /∂zα i ≡ α − j∈Ni ∂ATij /∂zα i ≡ 0, for which we conclude that zi → zoi → zdi . Remark 6.1: Although the control in (23) is discontinuous, the solutions for xi , yi , and φi are continuous since x˙ i , y˙ i , and φ˙ i are all piecewise continuous and bounded. α ˙α Moreover, the discontinuity jumps in x˙ α i , y˙ i , and φi can be made arbitrarily small by appropriately tuning γi and κoi .
VII. S IMULATION E XAMPLE To illustrate the performance of the proposed control law, we present a numerical example with a team of eight heterogeneous aerial vehicles. We restrict the motion of the vehicles to the horizontal plane and assume that their motion is governed by (1). In addition, we assume that the vehicles
z4
z5
2R
z8
2R
R z1
0
z3
y (m)
y (m)
R
0
−R
−R
z6
−2R
z7
z2
−2R
−R
0 x (m)
R
−2R
2R
−2R
Fig. 4. Initial configuration for the multi-vehicle system. The orientation of the vehicles are indicated by the nose of the aircraft. Desired trajectories are delineated by solid lines colored according to the vehicles’ colors. The vehicles are scaled according to their avoidance radii.
0 x (m) (a) t ∈ [0 s, 90 s]
−R
R
2R
6R
4R
if i ∈ {1, 2, 3, 4} and
=
"
zd6 (t) = − zd8 (t) =
"
zd5 (t)
=−
zd7 (t)
√1 t − 2R + R cos 2 2 − √12 t + 2R + R2 sin − √12 t + 2R + R2 cos − √12 t + 2R + R2 sin
#
4 5R t 4 5R t
# 4 5R t 4 5R t
,
otherwise. The initial positions for the vehicles are set to zi (0) = zdi (0) with orientation φi ∈ {0.97, 0.85, −2.40, 0.85, 1.29, −3.11, 2.21, −1.44} rad. Fig. 4 depicts the initial configuration for each vehicle with their corresponding desired trajectories traced by the line of the same vehicle’s color. Note that this is a symmetric scenario with all desired trajectories passing through or near the origin simultaneously at t ≈ 2R, an event that maximizes the opportunity for a collision. The results from the simulation are presented in Fig. 5 to 7. Observe, from Fig. 5(a), that the vehicles start tracking the desired trajectories until they come to the proximity of each other before t = 90 s. While the agents are within each other’s Detection Regions, the vehicles stop tracking the desired trajectory and start moving in a clockwise direction to resolve the conflict in accordance to their control law (23). Once the conflicts have been resolved, the agents return to their desired trajectories, as seen in Fig. 5(b). Fig. 6 illustrates the minimum distance between any pair of vehicles. Note that no pair of vehicles come closer than the
2R y (m)
have a communication range of R = 40 m and that the minimum safety distance among vehicles is r = 4 m. The complete list of system parameters is provided in Table I. The control objective of each vehicle is to follow a desired trajectory given by (t − 2R) cos π2 (i − 1) zdi (t) = (t − 2R) sin π2 (i − 1)
0
−2R
−4R
−6R −6R
−4R
0 2R x (m) (b) t ∈ [90 s, 300 s] −2R
4R
6R
Fig. 5. Sequential motion of the multi-vehicle system for t ∈ [0 s, 90 s] and t ∈ [90 s, 300 s]. The system trajectories are colored according to the vehicles’ colors. The desired trajectories (zdi ) are indicated by solid lines, while the trajectories for the auxiliary systems (zα i ) are traced by the fine dotted lines. The trajectories of the vehicles (zi ) are marked by small solid circles and spaced by 5 s in the first plot and by 10 s in the second plot. The avoidance regions for the auxiliary systems at the end of each simulation interval are delimited by the larger circles around the vehicles.
minimum safety distance r. Likewise, the auxiliary systems remain at a distance larger than rα at all times. Finally, Fig. 7 illustrates the tracking error for each vehicle. Observe that the error signals reach a peak value while the agents are in conflict. However, once the conflict is resolved, the error signals converge to the ball of radius rBi . VIII. C ONCLUSION A decentralized trajectory tracking with cooperative collision avoidance strategy for multi-vehicle systems has been presented. We considered the general case of an arbitrarily large team of nonholonomic, heterogeneous vehicles with constant speed and bounded turning rate. We made the as-
Distance (m)
mini,j ||zi − zj ||
α mini,j ||zα i − zj ||
R rα r 0
20
40
60
80
100 t (s)
120
140
160
180
Fig. 6. Minimum reached distance between pairs of auxiliary systems and pairs of vehicles.
||zdi − zi || (m)
100
i=1 i=2 i=3 i=4
50
0
0
100
Fig. 7.
200
300 t (s)
400
i=5 i=6 i=7 i=8
500
600
Tracking errors for all vehicles.
sumption that each vehicle can communicate its position exclusively to agents that are within its bounded sensing region (a number that is also bounded). Based on this assumption, we then developed a continuous trajectory tracking control with a reactive cooperative collision avoidance strategy that guarantees, using Lyapunov-based analysis, the collision-free transit of all agents (regardless of the group’s size) if the vehicles start from a safe configuration (specifically, outside of each other’s safety regions). We then modified the overall strategy with an arbitrarily small, piecewise continuous control perturbation that is activated whenever the trajectory tracking control and avoidance control are in conflict. This perturbation is shown to break the symmetries between the tracking and avoidance functions and to guarantee the convergence to the desired trajectories for at least two vehicles. Simulation results with eight vehicles confirmed our claims. R EFERENCES [1] T. Theodoridis and H. Hu, “Toward intelligent security robots: A survey,” IEEE Trans. Syst., Man, Cybern., vol. 42, no. 6, pp. 1219– 1230, Nov. 2012. [2] M. B. M. D. Tuan Le-Anh, “A review of design and control of automated guided vehicle systems,” Eur. J. Oper. Res., vol. 171, no. 1, pp. 1–23, May 2006. [3] H. S¸ahin and L. G¨uvenc¸, “Household robotics: Autonomous devices for vacuuming and lawn mowing,” IEEE Control Syst. Mag., vol. 27, no. 2, pp. 22–96, Apr. 2007. [4] D. Voth, “A new generation of military robots,” IEEE Intell. Syst., vol. 19, no. 4, pp. 2–3, 2004. [5] N. E. Leonard, D. A. Paley, F. Lekien, R. Sepulchre, D. M. Fratantoni, and R. E. Davis, “Collective motion, sensor networks, and ocean sampling,” Proc. IEEE, vol. 95, no. 1, pp. 48–74, Jan. 2007. [6] R. Bogue, “Robots for space exploration,” Ind. Robot, vol. 39, no. 4, pp. 323–328, 2012. [7] J. K. Kuchar and L. C. Yang, “A review of conflict detection and resolution modeling methods,” IEEE Trans. Intell. Transp. Syst., vol. 1, no. 4, pp. 179–189, 2000. [8] R. M. Murray, “Recent research in cooperative control of multivehicle systems,” J. Dyn. Syst. Meas. Control, vol. 129, no. 5, pp. 571–583, Sept. 2007.
[9] D. M. Stipanovi´c, “A survey and some new results in avoidance control,” in Proc. Int. Workshop Dyn. Control, Tossa de Mar, Spain, 2009, pp. 166–173. [10] E. Lalish and K. A. Morgansen, “Distributed reactive collision avoidance,” Autonomous Robots, vol. 32, no. 3, pp. 207–226, 2012. [11] R. W. Brockett, “Asymptotic stability and feedback stabilization,” in Differential Geometric Control Theory, R. W. Brockett, R. S. Millman, and H. J. Sussmann, Eds. Boston: Birkhauser, 1983, pp. 181–191. [12] S. Mastellone, D. M. Stipanovi´c, C. R. Graunke, K. A. Intlekofer, and M. W. Spong, “Formation control and collision avoidance for multiagent non-holonomic systems: Theory and experiments,” Int. J. Robot. Res., vol. 27, no. 1, pp. 107–126, Dec. 2008. [13] C. De La Cruz and R. Carelli, “Dynamic model based formation control and obstacle avoidance of multi-robot systems,” Robotica, vol. 26, pp. 345–356, 2008. [14] K. D. Do, “Formation tracking control of unicycle-type mobile robots with limited sensing ranges,” IEEE Trans. Control Syst. Technol., vol. 16, no. 3, pp. 527–538, May 2008. [15] ——, “Output-feedback formation tracking control of unicycle-type mobile robots with limited sensing ranges,” Robot. Autonomous Syst., vol. 57, no. 1, pp. 34–47, Jan. 2009. [16] O. M. Palafox and M. W. Spong, “Bilateral teleoperation of a formation of nonholonomic mobile robots under constant time delay,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., St. Louis, MO, Oct. 2009, pp. 2821–2826. [17] W. Kowalczyk, K. Kozlowski, and J. K. Tar, “Trajectory tracking for multiple unicycles in the environment with obstacles,” in Proc. RAAD, Budapest, Hungary, June 2010, pp. 451–456. [18] E. J. Rodr´ıguez, C. Tang, , M. W. Spong, and D. M. Stipanovi´c, “Trajectory tracking with collision avoidance for nonholonomic vehicles with acceleration constraints and limited sensing,” Int. J. Robot. Res., 2014, submitted. [19] E. Lalish, K. A. Morgansen, and T. Tsukamaki, “Decentralized reactive collision avoidance for multiple unicycle-type vehicles,” in Proc. Am. Control Conf., Seattle, WA, June 2008, pp. 5055–5061. [20] P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,” Int. J. Robot. Res., vol. 17, no. 7, pp. 760– 772, 1998. [21] S. G. Loizou and K. J. Kyriakopoulos, “Navigation of multiple kinematically constrained robots,” IEEE Trans. Robot., vol. 24, no. 1, pp. 221–231, Feb. 2008. [22] L. Pallottino, V. G. Scordio, A. Bicchi, and E. Frazzoli, “Decentralized cooperative policy for conict resolution in multi-vehicle systems,” IEEE Trans. Robot., vol. 23, no. 6, pp. 1170–1183, Dec. 2007. [23] A. S. Oikonomopoulos, S. G. Loizou, and K. J. Kyriakopoulos, “Coordination of multiple non-holonomic agents with input constraints,” in Proc. IEEE Int. Conf. Robot. Autom., Kobe, Japan, May 2009, pp. 869–874. [24] A. Krontiris and K. E. Bekris, “Using minimal communication to improve decentralized conict resolution for non-holonomic vehicles,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., San Francisco, CA, Sept. 2011, pp. 3235–3240. ˇ [25] D. M. Stipanovi´c, P. F. Hokayem, M. W. Spong, and D. Siljak, “Cooperative avoidance control for multiagent systems,” J. Dyn. Syst. Meas. Control, vol. 129, pp. 699–707, Sept. 2007. [26] E. J. Rodr´ıguez-Seda, D. M. Stipanovi´c, and M. W. Spong, “Collision avoidance control with sensing uncertainties,” in Proc. Am. Control Conf., San Francisco, CA, June 2011, pp. 3363–3368. [27] ——, “Lyapunov-based cooperative avoidance control for multiple Lagrangian systems with bounded sensing uncertainties,” in Proc. IEEE Conf. Decision Control, Orlando, FL, Dec. 2011, pp. 4207– 4213. [28] E. J. Rodr´ıguez-Seda and M. W. Spong, “Guaranteed safe motion of multiple lagrangian systems with limited actuation,” in Proc. IEEE Conf. Decision Control, Maui, HI, Dec. 2012, pp. 2773–2780. [29] Y. Koren and J. Borenstein, “Potential field methods and their inherent limitations for mobile robot navigation,” in Proc. IEEE Int. Conf. Robot. Autom., Sacramento, CA, Apr. 1991, pp. 1398–1404. [30] D. Shevitz and B. Paden, “Lyapunov stability theory of nonsmooth systems,” IEEE Trans. Automat. Control, vol. 39, no. 9, pp. 1910– 1914, Sept. 1994.
