Coordinated Path Following Control of Fixed-wing Unmanned Aerial Vehicles

1 Coordinated P ath F ollo wing Control of Fix ed-wing Unmanned Aerial V ehicles Hao Chen, Y irui Cong, Xiangk e W ang, Xin Xu, and Lincheng Shen This work has been submitted to the IEEE for possible publication. Copyright may be transferred without notice, after which this version may no longer be accessible. Abstract —In this paper , we in vestigate the problem of coordi- nated path following for ﬁxed-wing U A Vs with speed constraints in 2D plane. The objective is to steer a ﬂeet of U A Vs along the path(s) while achieving the desired sequenced inter -U A V arc distance. In contrast to the previous coordinated path follo wing studies, we are able through our proposed hybrid control law to deal with the f orward speed and the angular speed constraints of ﬁxed-wing U A Vs. More speciﬁcally , the hybrid control law makes all the U A Vs work at two different levels: those U A Vs whose path follo wing errors ar e within an in variant set (i.e., the designed coordination set) work at the coordination level; and the other U A Vs work at the single-agent level. At the coordination level, we pro ve that even with speed constraints, the proposed control law can make sure the path follo wing errors reduce to zero, while the desired arc distances con verge to the desired value. At the single- agent level, the conv ergence analysis for the path f ollowing error entering the coordination set is pro vided. W e dev elop a hardwar e- in-the-loop simulation testbed of the multi-U A V system by using actual autopilots and the X-Plane simulator . The effectiveness of the proposed approach is corroborated with both MA TLAB and the testbed. Index T erms —Coordinated path follo wing, hybrid control, speed constraints, multi-U A V systems. I . I N T RO D U C T I O N A. Motivation Coordinated path following control of multiple ﬁxed-wing unmanned aerial vehicles (UA Vs) has attracted signiﬁcant attention in recent years, due to its increasing demands in civil and military uses [1]. It studies how to steer a group of ﬁxed- wing UA Vs moving along given/planned paths while forming a desired formation pattern based on local interactions. Different from multirotor U A Vs or ground vehicles, a ﬁxed- wing UA V cannot move backwards and is actually constrained by a minimum forward speed which generates suf ﬁciently large lift to support the UA V in ﬂight. It should be noted that with the minimum forward speed constraint, the coordinated path following control behav es very dif ferently: • During path following, the maximum angular speed is not negligible an y more, since it combined with the minimum forward speed determines the minimum turning radius. As a result, one U A V can only follow the class of paths (curves) with limited curvature, which is determined by The authors are with the College of Intelligence Science and T echnology , National Uni versity of Defense T echnology , Changsha 410073, China. (e-mail: xkwang@nudt.edu.cn, xinxu@nudt.edu.cn). the minimum forward speed and the maximum angular speed; while the existing studies in the coordinated path following control problems did not take the speed-related curvature into account. • During the formation pattern forming process, one UA V cannot completely stop or become unacceptably slow to wait for another U A V . Consequently , some coordinating U A Vs can quit the group formation, if the minimum forward speed and the maximum angular speed are not properly considered when designing the control law . 1 Therefore, the coordinated path follo wing control of ﬁxed- wing UA Vs is very different from that without minimum forward speed and maximum angular speed constraints in the literature, and cannot be solved by the existing methods (e.g., [2]). It is necessary to design an efﬁcient control law to solve the coordinated path follo wing control problem for ﬁxed-wing UA Vs. B. Related W ork The coordinated path following control problem consists of two core components: path follo wing and multi-vehicle coordination. Path follo wing problem has a long history dating back to the end of last century [3]. For ﬁxed-wing UA V path following, some early work mainly focused on the straight line following and circle follo wing [4]–[7], and a comparison of existing methods dealing with these two kinds of path follo wing problem can be found in [8]. In recent years, general curved path following problem has received increasing attention, and typical approaches are largely based on proportional-integral- deriv ativ e (PID) controller [9], vector ﬁeld method [10], [11], sliding mode controller [12], backstepping controller [13], adaptiv e controller [14], nested saturation and control L ya- punov function based method [15], etc. In terms of the multi-vehicle coordination, typical methods include leader-follo wer approach [16]–[18], virtual structure approach [19]–[21], behavior -based approach [22], [23]. W e recommend the surve ys in [1], [24], [25] to readers who are interested in the existing coordinated control results. Some methods have been validated by the formation ﬂight of ﬁxed- wing U A Vs: in [26], a vision-aided close formation ﬂight of two U A Vs was conducted based on leader-follo wer control; the work in [27] considered several formation design approaches, and performed ﬁeld experiments of two U A Vs with a combined 1 W e have prov ed that the con ventional coordinated path following control laws in [2] cannot solve the coordinated path following control problem for ﬁxed-wing UA Vs if the constraints are not properly considered. 2 controller; as a big advance in terms of the scale, autonomous ﬂight of 50 ﬁxed-wing platforms was presented in [28], but details of their control law were not provided. T o combine the path follo wing controller with the co- ordinated control architecture is the k ey factor to achiev e coordinated path following. There ha ve been a large number of studies dealing with the coordinated path following control of multirotor U A Vs [16], [29], [30] and ground vehicles [20], [31]. Howe ver , most of these results cannot be applied to ﬁxed-wing UA Vs, since they are constrained by the minimum forward speed, which is a hard constraint in this kind of vehi- cles for certain missions. Note that the speed constraints have been partially considered in the coordinated path following problems for ﬁxed-wing U A Vs: in [32], a centralized strategy was in vestigated for a group of ﬁxed-wing U A Vs to follow closed intersecting curves, where the coordination refers to the collision av oidance among the U A Vs; in [33], a time-critical coordinated path following control problem was studied, and the proposed algorithm can steer a ﬂeet of ﬁxed-wing U A Vs along gi ven paths while arriving at their own ﬁnal destinations at the same time; the cooperativ e moving path following prob- lem was introduced in [34], with sufﬁcient conditions deriv ed under which the closed-loop system is asymptotically stable. Nev ertheless, in these studies, the forward speed constraints and the angular speed constraints are not considered as a whole. C. Our Contributions In this work, we focus on the coordinated path following control problem for a group of ﬁxed-wing UA Vs. T o solve this problem, we propose a novel distributed hybrid control law by extending the hybrid controller provided in [2]. It should be noted that most of the existing work use L yapunov analysis to conclude the stability of the hybrid systems [35]. Howe ver , in this paper , the stability of the ov erall system is concluded by employing the similar technique with [2], which can provide a complete dynamical analysis of the ov erall systems. The main contributions of this paper are listed as follows: • W e propose a hybrid control framew ork based on an in- variant set, the coordination set , to solve the coordinated path following problem of ﬁx ed-wing U A Vs. Different from the existing results, the coordination set is designed to guarantee the con ver gence of the path following error and coordination error while satisfying both the forward speed constraints and the angular speed constraints. • W e propose a coordinated path following control law inside the coordination set, and theoretically prove that ev en with the speed constraints of ﬁxed-wing U A Vs, the proposed control law can make the path following errors reduce to zero, while the desired arc distances conv erge to the desired value. • W e propose a single-agent level control law outside the coordination set by using optimal control, and the con- ver gence analysis for the UA Vs entering the coordination set is provided. • W e dev elop a hardware-in-the-loop simulation testbed of the multi-U A V system by using actual autopilots and the X-Plane simulator , and validate the proposed coordinated path following approach with the testbed. D. P aper Or ganization The paper is organized as follows. Section II presents the system model as well as the problem description, and analyzes the difﬁculty from speed constraints. Then, in the next two sections, we present our control law to tackle the coordinated path following problem with speed constraints: at coordination lev el when the path following error is within a coordination set (see Section III); and at single-agent level when it is outside this coordination set (see Section IV). In Section V we show the simulation results, including the simulation with MA TLAB and the hardw are-in-the-loop simulation with the X- Plane simulator; and ﬁnally the concluding remarks are gi ven in Section VI. E. Notation A curve is said to be C k -smooth, if it admits an analytic expression Γ( x, y ) , whose k th deriv ativ e exists and is contin- uous. The curv ature of a path at any point p on the curve is denoted as κ ( p ) . T ( p ) denotes the unit tangent vector of the curve at point p , and the curvature of the curve at point p is deﬁned as κ ( p ) = d T ( p ) / d s , where s is the natural parameter of the curve representing the length of the curve. I I . C O O R D I NAT E D P AT H F O L L OW I N G P RO B L E M F O R F I X E D - W I N G UA V S In this section, we formulate the 2D coordinated path following problem for a group of n homogeneous ﬁxed-wing U A Vs. The aim is to design a control law such that each U A V follows a predeﬁned curved path while the sequenced inter- U A V arc distances conv erge to the desired constant. A. System Model and Problem Description Consider a group of ﬁxed-wing U A Vs ﬂying at the same altitude. Then, the state of the i th U A V can be represented by the conﬁguration vector q i = ( x i , y i , θ i ) T ∈ R 2 × [ − π , π ) , where ( x i , y i ) is the i th U A V’ s position deﬁned in an inertia coordinate frame W , and θ i is the orientation of the i th U A V with respect to the x -axis of W . The kinematic model of the i th U A V with pure rolling and non-slipping is given as ˙ q i =   ˙ x i ˙ y i ˙ θ i   =   cos θ i 0 sin θ i 0 0 1    v i ω i  , (1) where control inputs v i and ω i stand for the forward speed and angular speed of the i th U A V , respectiv ely . For ﬁxed-wing UA Vs, the speed constraints must be con- sidered in (1). On the one hand, the forward speed of a U A V is constrained with saturation and dead zone, i.e., each ﬁxed- wing U A V has the maximum and minimum forward speed constraints. On the other hand, the angular speed is also constrained with saturation. Mathematically , for the i th U A V , the speed constraints are: ( 0 < v min ≤ v i ≤ v max , | ω i | ≤ ω max , (2) 3 where v min and v max are the minimum and maximum for- ward speeds, while ω max is the maximum angular speed. W e consider all the U A Vs follow a directed curved path Γ ∈ C 2 . Assumption 1: Γ is globally known to each UA V , and the absolute value of the curv ature of Γ at any point p is less than a constant κ 0 , i.e., | κ ( p ) | < κ 0 . W e note that due to the constraints on the forward and angular speeds, κ 0 must be not larger than ω max /v min . This implies each UA V has a minimum turning radius (numerically equals v min /ω max ) when following a path. W e use T ( p ) to denote the tangent vector of the path Γ at point p . The point p = ( p x , p y ) T on the path is said to be a projection of the i th U A V , if the vector t = ( x i − p x , y i − p y ) T is orthogonal to T ( p ) . W e deﬁne φ i = ( ρ i , ψ i ) as the path following error of the i th U A V with respect to the path Γ (see Fig. 1), where ρ i ∈ R is the distance from the i th U A V to its closest projection p i on Γ with a sign: ρ i > 0 when the i th U A V is on the left side of Γ in the direction of the path, and ρ i < 0 when it is on the other side. For example, in Fig. 1, we hav e ρ i < 0 for the i th U A V , and ρ j > 0 for the j th U A V . ψ i is the heading of the i th U A V with respect to T ( p i ) at its projection p i . In this way , ρ i can be seen as the location differ ence of the i th U A V with respect to the path Γ ; and ψ i ∈ [ − π , π ) is the orientation differ ence between the heading of the i th U A V and T ( p i ) . Remark 1: If | ρ i | < R 0 , where R 0 = 1 κ 0 , the closest projection p i is unique [3]. The dynamics (1) thus can be re written in the form of ˙ φ i = f ( φ i ) with the speed constraints represented by (2): ( ˙ ρ i = v i sin ψ i , ˙ ψ i = ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i , i = 1 , ..., n. (3) U A V i asymptotically follo ws the path Γ if and only if φ i → 0 .  i  i p ij l i UAV j UAV () i p T j p i Fig. 1. Path Γ with a direction. Points p i and p j are the projections of the i th and the j th U A Vs, respectively . φ i = ( ρ i , ψ i ) is the path following error of the i th U A V , and arc distance l ij is the length along the path from p i to p j . In this case, the j th U A V is the pre-neighbor of the i th U A V . In our coordinated path following problem, it not only requires all the U A Vs move along the path Γ , but also guarantees the distance between any two adjacent U A Vs is a desired constant, say L , in the sense of arc length. T o describe adjacent U A Vs, we introduce the deﬁnition of pre-neighbor as follows. Deﬁnition 1: The j th U A V is the i th U A V’ s pre-neighbor if i) | ρ i | < R 0 , | ρ j | < R 0 ; ii) The projection point p j of the j th U A V on the path Γ is in front of the i th U A V’ s projection point p i (see Fig. 1), without any other UA V’ s projection in the middle. The two conditions in Deﬁnition 1 imply that: (i) the pre- neighbor deﬁnition only works for the UA Vs close enough to the path Γ (i.e., with | ρ i | < R 0 ); and (ii) each U A V can at most hav e one pre-neighbor . Remark 2: When there are two or more UA Vs with the same projection point on Γ , we deﬁne the pre-neighbor in ascending order with respect to the U A Vs’ labels to avoid ambiguity . Since the desired inter-U A V arc distance is between adjacent U A Vs, the i th U A V just needs to focus on the arc distance l ij to its pre-neighbor . T o simplify the description, we denote the arc distance between the i th U A V and its pre-neighbor as ζ i . The coordination error for the i th U A V is thus L − ζ i . Now we can formally deﬁne the coordinated path following problem for ﬁxed-wing UA Vs as follo ws. Pr oblem 1 (Coor dinated P ath F ollowing): Giv en n ﬁxed- wing U A Vs, each is modeled as (1) with speed constraints represented by (2), design control law such that φ i → 0 , and L − ζ i → 0 , i = 1 , . . . , n , B. The Challenge fr om Speed Constraints When ignoring the speed constraints, we can extract a set S as S = { ( ρ, ψ ) : ρ ∈ [ − R 0 , R 0 ] , ψ ∈ [ − π , π ) } , which is illustrated in Fig. 2, as deﬁned in [2], where S is partitioned into two parts S 1 and S 2 . S 1 is the coordination set, deﬁned as S 1 := { ( ρ, ψ ) ∈ S : | ρ | ≤ R 0 , | ψ | ≤ a, | aρ + R 0 ψ | ≤ aR 0 } , and S 2 := S \ S 1 . P arameter a satisﬁes 0 < a < min { ( π / 2) , R 0 } . It has been pro ved that, by applying an  0 R  a     2 S a R 1 S 0 R 1 S a Fig. 2. Set S with its two partitioned subsets S 1 and S 2 in [2]. appropriate hybrid control law , the path following error φ i of the i th U A V , whose dynamics are represented by (3) without constraints represented by (2), con verges to 0 . T o be more speciﬁc, if φ i is in S 2 initially , then it cannot leav e S and will enter S 1 in a ﬁnite time, and ﬁnally conv erge to 0 , with the arc distance ζ i approaching L as t → ∞ . In this process, when φ i ( t ) ∈ S 2 , the i th U A V only works at single-agent lev el (i.e., without any coordination), and the coordination algorithm gets to work only after φ i gets into the coordination set. Unfortunately , when considering the speed constraints represented by (2), the abov e results cannot be guaranteed. Remark 3: W e can always characterize a small region S e , such that when φ i ( t 0 ) ∈ S e , gi ven any control input ( v i , ω i ) ∈ [ v min , v max ] × [ − ω max , ω max ] , there exists a time t 1 > t 0 such that φ i ( t 1 ) / ∈ S . W ithout loss of generality , we assume the U A Vs follo w a path with the curvature κ ( p i ) ∈ ( − κ 0 , 0] . W e deﬁne S e = { φ i ∈ S : v min ( ψ i −  0 ) sin  0 ω max + ρ i − R 0 > 0 , ρ i ∈ [0 , R 0 ] , ψ i ∈ [0 , π / 2] } , where 0 <  0 < π / 2 . W e note that S e is not an empty set, otherwise, v min ( ψ i −  0 ) sin  0 ω max + ρ i − R 0 ≤ 0 should hold for all ρ i ∈ [0 , R 0 ] and ψ i ∈ [0 , π / 2] , which can be veriﬁed to be impossible by taking ρ i = R 0 and ψ i = π / 2 . According to (3), when ψ i ∈ [  0 , π / 2] , ˙ ψ i = ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i ≥ − ω max . Suppose t ∗ is the minimum time 4 it takes to satisfy ψ i ≤  0 , then t ∗ ≥ ψ i ( t 0 ) −  0 ω max . When t ∈ [ t 0 , t 0 + t ∗ ] , we hav e ˙ ρ i = v i sin ψ i ≥ v min sin  0 > 0 , and thus ˙ ρ i · ψ i ( t 0 ) −  0 ω max ≥ v min ( ψ i ( t 0 ) −  0 ) sin  0 ω max > R 0 − ρ i ( t 0 ) , meaning φ i will leav e S before t 0 + t ∗ . Therefore, it is impossible to use the results in [2] to solve the speed constrained coordinated path following problem deﬁned in Problem 1, since not all the points in S can be guaranteed always within S e ven under any possible control input. T o solve Problem 1, we have to further partition set S to specify the in variant subsets and design the control laws accordingly . T o be more speciﬁc, we deﬁne a ne w coordination set and propose the corresponding control law in Section III; and for the points outside the coordination set, we design a single-agent lev el control law in Section IV. Before we proceed, the following lemma is necessary throughout the paper . Lemma 1 (P ages 61-62 of [36]): Consider a simple closed contour deﬁned by g ( x ) = 0 , with g ( x ) < 0 enclosed by the contour , where g ( x ) is a continuously differentiable function. The vector ﬁeld f ( x ) at a point x on the coutour points inward if the inner product of f ( x ) and the gradient vector 5 g ( x ) is negati ve, i.e., f ( x ) · 5 g ( x ) < 0 ; and the vector ﬁeld points outward if f ( x ) · 5 g ( x ) > 0 ; and it is tangent to the contour if f ( x ) · 5 g ( x ) = 0 . The trajectory can leave the set enclosed by the contour , only if the vector ﬁeld points outward at some point on its boundary , i.e., ∃ x such that g ( x ) = 0 and f ( x ) · 5 g ( x ) > 0 . I I I . C O O R D I NAT E D C O N T R O L L AW I N C O O R D I NAT I O N S E T In this section, we discuss how to control the U A Vs, whose path following errors are within a given set (called the coordination set S 1 ), to mo ve along the path Γ in a coordination manner . W e formulate the coordination set as S 1 = { ( ρ, ψ ) : | ρ | ≤ R 1 , | ψ | ≤ a, | aρ + R 1 ψ | ≤ aR 1 } , where R 1 < R 0 . Since κ 0 = 1 R 0 , we hav e κ 0 R 1 < 1 . W e note that this newly deﬁned coordination set is relatively smaller compared to that without considering the speed constraints (see Fig. 2). W ith speed constraints, parameters a and R 1 should be properly selected. Thus, we ﬁrst illustrate the parameter selection (Section III-A) before designing the control law (Section III-B). A. P arameter Selection of S 1 The selection of parameters in S 1 follows two basic princi- ples, which are illustrated one by one as follows. 1) The F irst Principle: W e need to guarantee the existence of a proper control law making S 1 an in variant set (see [36], page 127), i.e., if the initial path follo wing error φ i ( t 0 ) ∈ S 1 at time t 0 , then φ i ( t ) ∈ S 1 for any t > t 0 . In that way , for each φ i ( t 0 ) / ∈ S 1 , we only need to design the control law to guarantee φ i ( t ) entering S 1 . Remark 4: Denote the set ∂ S 1 , as the intersection of S 1 with the following set: { ( ρ, ψ ) : | ψ | = a } ∪ { ( ρ, ψ ) : | aρ + R 1 ψ | = aR 1 } . It can be seen that ∂ S 1 is a subset of the boundary of S 1 . According to Lemma 1, the ﬁrst principle to guarantee φ i ( t ) will not leave S 1 is equivalent to guarantee φ i ( t ) will nev er get across | aρ + R 1 ψ | = aR 1 in the ﬁrst and the third quadrants, and | ψ | = a in the second and fourth quadrants. Since v i > 0 , it is certain that φ i ( t ) will nev er leav e S 1 from | ρ | = R 1 in the second and the fourth quadrants, and that is why we do not include set { ( ρ, ψ ) : | ρ | = R 1 } in ∂ S 1 . Then, we hav e the following lemma: Lemma 2: A suf ﬁcient condition under the ﬁrst principle is that, for any φ i ∈ ∂ S 1 , there exist v i and ω i satisfying (2), such that one of the four conditions [i.e., one of the four inequalities (4)-(7)] holds: ( v i ( a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i ) + R 1 ω i + R 1 α ≤ 0 , ρ i ≥ 0 , ψ i ≥ 0; (4) ( ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i + α ≤ 0 , ρ i < 0 , ψ i > 0; (5) ( v i ( a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i ) + R 1 ω i − R 1 α ≥ 0 , ρ i ≤ 0 , ψ i ≤ 0; (6) ( ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i − α ≥ 0 , ρ i > 0 , ψ i < 0; (7) where α is a small positiv e number . Lemma 2 can be easily deriv ed by using Lemma 1. W ith Lemma 2, we can get an important inequality for the parameter selection, giv en in the following lemma. Lemma 3: If there exists v m ∈ ( v min , v max ] making (8) and (9) hold, then for any φ i ∈ ∂ S 1 , there exist v i and ω i satisfying (2), such that one of the four inequalities (4)-(7) holds. r ( a R 1 ) 2 + κ 2 0 + α v m ≤ ω max v m , (8) κ 0 1 − κ 0 R 1 + α v m ≤ ω max v m . (9) Pr oof: See Appendix A. 2) The Second Principle: W e need to guarantee the e x- istence of a proper control law , which ensures the sequence of the U A Vs along the path to be ﬁxed, once all the UA Vs enter S 1 . More speciﬁcally , if U A V i and its pre-neighbor U A V j satisfy φ i ( t 0 ) , φ j ( t 0 ) ∈ S 1 at time t 0 , then there will be no any other k th U A V ( k 6 = i, k 6 = j ) turning to be the pre-neighbor of the i th U A V from then on. Mathematically , if φ i ( t 0 ) ∈ S 1 , ∀ i = 1 , . . . , n , then ζ i ( t ) > 0 , ∀ t > t 0 . 2 Lemma 4: The second principle holds if 1 1 − κ 0 R 1 v min + c ≤ cos a 1 + κ 0 R 1 v m , (10) where c > 0 . Pr oof: See Appendix B. Besides inequalities (8)-(10) (deduced from Lemma 3 and Lemma 4, respectiv ely), there are some additional constraints for parameters:      0 < a < π / 2 , 0 < R 1 < R 0 , v min < v m ≤ v max , (11) 2 W e note that when ζ i ( t ) is reduced to zero, it means the i th U A V is being overtak en by another U A V , or is ov ertaking another U A V at time t . 5 where v min , v max and ω max are determined by the dynamics of the U A Vs; κ 0 is determined by the path for the UA Vs to follow; c is chosen by the designers. Then, there are three parameters in (11) left unknown, namely , a , R 1 and v m . The selection of a , R 1 and v m can be regarded as an optimization problem. Its objectiv e is to make S 1 as large as possible, since a larger S 1 contains more path follo wing errors, which means more situations can be directly 3 dealt with our proposed coordination control law in Section III-B. This optimization problem is described by maximize aR 1 (12) s . t . inequalities (8)-(11) hold. If constraints (8)-(11) are satisﬁed (see Lemma 5), we can guarantee the existence of proper control law satisfying the aforementioned two principles. By solving the optimization problem (12), we can select the optimized parameters a and R 1 to design S 1 , and we can also obtain v m , which is an important parameter in the control law proposed in Section III-B. Lemma 5: A sufﬁcient condition to guarantee the existence of feasible solutions of (12) is ( κ 0 ≤ ω max v max , v min + c ≤ v max . (13) Remark 5: Lemma 5 can be veriﬁed easily . From it, we can get one guideline to choose the user-determined value of parameter c in (12) as c ≤ v max − v min . It is obvious from (10) that the smaller c is, the larger the coordination set will become, since we will get a larger feasible region in (12). Howe ver , in terms of the control law designs, c is not the smaller the better , because c is also correlated to the con vergence rate (more details are giv en in Section III-B), and a trade-off should be made between the coordination set’ s size and the con ver gence rate. Abov e completes the parameter selection process for co- ordination set S 1 . W ith the parameter selection, we cannot only guarantee the existence of the control law satisfying the proposed two principles, but also design a proper coordination set S 1 . Next, we will design the control law , and show the importance of the two principles to the con ver gence of the path following error and the sequenced inter-U A V arc distance. B. Contr ol Law in S 1 Now , we propose the coordinated path following control law in S 1 , which is included in Algorithm 1. W e assume ρ i , ψ i and κ ( p i ) can be calculated by the UA V itself, since the path is globally av ailable to the UA V . Besides, ζ i can be measured by the UA V itself or through communication with its pre-neighbor , and thus the multi-UA V coordination can be achie ved through sensing or communication. When the i th U A V does not hav e a pre-neighbor , we artiﬁcially set ζ i = L , i.e., the coordination error L − ζ i is set to be zero. 3 W e note that if a path following error is outside S 1 , we have to make it enter S 1 , before the coordination control law in Section III-B is applied. Algorithm 1 Coordinated Path Follo wing Control Law in S 1 Input: ρ i , ψ i , κ ( p i ) , ζ i Output: v i , ω i 1: procedure C O OR D C O NT RO L ( ρ i , ψ i , κ ( p i ) , ζ i ) 2: Set the forward speed as v i = Sat  1 − κ ( p i ) ρ i cos ψ i χ ( ζ i ) , v min , v max  ; 3: Set the angular speed as ω i = Sat( ω d , − ω max , ω max ) , where ω d = v i  − k 1 ϑ i k 2 + κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i  − α · sign( ϑ i ) , and k 1 > 0 , k 2 , k 3 ≥ 1 and a ≤ R 1 k 1 < ak 2 ; 4: v i ← R E S E T V A L U E ( v i , ω i , ρ i , ψ i , κ ( p i ) , ζ i ) 5: retur n v i , ω i 6: end procedure 7: procedure R E S E T V A L UE ( v i , ω i , ρ i , ψ i , κ ( p i ) , ζ i ) 8: if φ i ∈ S 1 1 and inequality (4) does not hold then 9: v i = − h a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i i − 1 R 1 ( ω i + α ) 10: end if 11: if φ i ∈ S 2 1 and inequality (5) does not hold then 12: v i = 1 − κ ( p i ) ρ i κ ( p i ) cos ψ i ( ω i + α ) 13: end if 14: if φ i ∈ S 3 1 and inequality (6) does not hold then 15: v i = − h a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i i − 1 R 1 ( ω i − α ) 16: end if 17: if φ i ∈ S 4 1 and inequality (7) does not hold then 18: v i = 1 − κ ( p i ) ρ i κ ( p i ) cos ψ i ( ω i − α ) 19: end if 20: if φ i ∈ S 5 1 and ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i − α < 0 then 21: v i = 1 − κ ( p i ) ρ i κ ( p i ) cos ψ i ( ω i − α ) 22: end if 23: if φ i ∈ S 6 1 and ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i + α > 0 then 24: v i = 1 − κ ( p i ) ρ i κ ( p i ) cos ψ i ( ω i + α ) 25: end if 26: retur n v i 27: end procedure Before illustrating our control algorithm, we introduce a continuous function χ ( ζ i ) , which is used in Line 2 with the following properties: i) χ ( ζ i ) = 1 1 − κ 0 R 1 v min , when ζ i ∈ [0 , L − δ 1 ) , where 0 < δ 1 < L ; ii) χ ( L ) = λ 1 − κ 0 R 1 v min + (1 − λ ) cos a 1+ κ 0 R 1 v m , where 0 < λ < 1 ; iii) χ ( ζ i ) is non-decreasing when ζ i ∈ [0 , ∞ ) , and is strictly increasing in [ L − δ 2 , L + δ 2 ] , where 0 < δ 2 ≤ δ 1 . W e also use a saturation function Sat( · ) in Lines 2 and 3, which is deﬁned as follows (suppose a < b ): Sat( x, a, b ) = a if x ≤ a , Sat( x, a, b ) = x if a < x ≤ b , and Sat( x, a, b ) = b if x > b . The main procedure of the coordinated path following algo- rithm in S 1 , C O O R D C O N T R O L , can be described as follows: in Line 2, we set the value of v i ∈ [ v min , v max ] based on the coordination error . W e calculate ω i ∈ [ − ω max , ω max ] in Line 3, which is a function of v i deriv ed in Line 2. After calculating v i and ω i , we check whether some properties (see procedure R E S E T V A L U E ) hold with the deriv ed v i and ω i , and if not, we will recalculate v i again. The checking- recalculating process is depicted by procedure R E S E T V A L U E . 6 In this procedure, we di vide S 1 into six subsets, and reset the value of v i by using different rules for different subsets. Let ϑ i = k 1 ρ i + k 2 ψ i + k 3 sin ψ i , then ϑ i = 0 is a nearly- straight curve passing through the origin, ϑ i > 0 is on the upper-right side of the curve, and ϑ i < 0 on its lower -left side. ϑ i = 0 together with ρ -axis and ψ -axis di vides S 1 into six subsets as shown in Fig. 3, which are deﬁned as follows: S 1 1 = { φ i ∈ S 1 : ρ i > 0 , ψ i ≥ 0 , ϑ i > 0 } ; S 2 1 = { φ i ∈ S 1 : ρ i ≤ 0 , ψ i ≥ 0 , ϑ i ≥ 0 } ; S 3 1 = { φ i ∈ S 1 : ρ i < 0 , ψ i ≤ 0 , ϑ i < 0 } ; S 4 1 = { φ i ∈ S 1 : ρ i ≥ 0 , ψ i ≤ 0 , ϑ i ≤ 0 } ; S 5 1 = { φ i ∈ S 1 : ρ i < 0 , ψ i > 0 , ϑ i < 0 } ; S 6 1 = { φ i ∈ S 1 : ρ i > 0 , ψ i < 0 , ϑ i > 0 } . W e note that according to the abov e partition, the origin is contained both in S 2 1 and in S 4 1 . a a  1 R  1 R 0 i   1 1 S 2 1 S 3 1 S 4 1 S 5 1 S 6 1 S  Fig. 3. Six subsets of S 1 divided by ϑ i = 0 , ρ -axis, and ψ -axis. In R E S E T V A L U E , we check whether one of the four in- equalities (4)-(7) holds when φ i ∈ S 1 1 ∪ S 2 1 ∪ S 3 1 ∪ S 4 1 . If not, we will recalculate v i . It is easy to ﬁnd that, the recalculated v i can ﬁnally make one of the inequalities (4)-(7) hold when φ i ∈ S 1 1 ∪ S 2 1 ∪ S 3 1 ∪ S 4 1 . W e also note that there is no need to check inequalities (4)-(7) when φ i ∈ S 5 1 ∪ S 6 1 , since φ i will not leav e S 1 from | ρ i | = R 1 (see Remark 4). But we will make sure ˙ ψ i ≥ 0 when φ i ∈ S 5 1 , and ˙ ψ i ≤ 0 when φ i ∈ S 6 1 . There is an important property of procedure R E S E T V A L U E . Lemma 6: If v i is changed in R E S E T V A L U E , denote v i 1 as the forward speed calculated in Line 2, and v i is the ﬁnal returned value from R E S E T V A L U E , then v m ≤ v i < v i 1 . Pr oof: See Appendix C. According to the deﬁnition of function Sat( · ) , we hav e v i 1 ≤ v max . Therefore, Lemma 6 implies that, control inputs v i and ω i calculated by Algorithm 1 satisfy (2). W e no w sho w that the ﬁrst principle is guaranteed by using Algorithm 1, which means S 1 is made an in variant set. Theor em 1 (V alidation for the F irst Principle): According to Algorithm 1, v i and ω i make S 1 an inv ariant set, i.e., if φ i ( t 0 ) ∈ S 1 , then φ i ( t ) ∈ S 1 , ∀ t > t 0 . Pr oof: It is easy to check that procedure R E S E T V A L U E can guarantee one of the four inequalities (4)-(7) hold when φ i ∈ S 1 1 ∪ S 2 1 ∪ S 3 1 ∪ S 4 1 . According to the deﬁnition of these sets, ∂ S 1 ( S 1 1 ∪S 2 1 ∪S 3 1 ∪S 4 1 . W ith Lemma 2, we can conclude that S 1 is an in variant set. Remark 6: W e get the following results with Algorithm 1. i) If ϑ i > 0 , then ˙ ψ i ≤ − α ; if ϑ i < 0 , then ˙ ψ i ≥ α . ii) If φ i ∈ S 1 1 ∪ S 3 1 , and ψ i 6 = 0 , then ˙ ψ i ˙ ρ i ≤ − a R 1 . In terms of the second principle proposed in Section III-A, Theorem 2 illustrates that Algorithm 1 guarantees the sequence of U A Vs becoming ﬁxed, once all the U A Vs enter S 1 . Theor em 2 (V alidation for the Second Principle): Suppose φ i ( t 0 ) ∈ S 1 , ∀ i = 1 , . . . , n . By executing Algorithm 1, if ζ i ( t 0 ) > 0 , then ζ i ( t ) > 0 holds, ∀ t ≥ t 0 . Pr oof: Firstly , each UA V’ s speed along the path v r i is not smaller than χ (0) with Algorithm 1. This is obvious if the v alue of v i is not changed in R E S E T V A L U E by us- ing the deﬁnition of χ ( · ) . If the v alue of v i is changed in R E S E T V A L U E , according to Lemma 6, v i ≥ v m , then v r i ≥ v m cos a 1+ κ 0 R 1 > χ (0) . Now let the j th U A V be the pre-neighbor of the i -th U A V , then ˙ ζ i = v r j − v r i . Suppose initially 0 < ζ i ( t 0 ) ≤ L − δ 1 , and thus v r i = χ (0) . Using similar analyses with Lemma 4, it can be concluded that ζ i ( t ) > 0 , ∀ t ≥ t 0 . Now we show the importance of the two principles to the con vergence of the path follo wing error and sequenced inter- U A V arc distance in S 1 . In terms of the path follo wing error , we hav e the follo wing theorem stating that all the U A Vs will ﬁnally move on the desired path if they are all in S 1 initially . Theor em 3 (Con verg ence of P ath F ollowing Error): By ex ecuting Algorithm 1, if φ i ( t 0 ) ∈ S 1 , then lim t →∞ φ i ( t ) = 0 . Pr oof: Firstly , recall i) in Remark 6, ˙ ψ i ≤ − α < 0 when ϑ i > 0 ; and ˙ ψ i ≥ α > 0 when ϑ i < 0 . W e note that S 1 is an in variant set according to Theorem 1, and | ψ i | ≤ a when φ i ∈ S 1 . Therefore, for any φ i (0) ∈ S 1 , there exists a ﬁnite time t 0 ≤ 2 a α , such that ϑ i ( t 0 ) = 0 . Secondly , by i) in Remark 6, we can show that for any φ i ( t 0 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , φ i will not go to S 5 1 or S 6 1 directly , since ϑ i ˙ ϑ i = ϑ i ( k 1 ˙ ρ i + k 2 ˙ ψ i + k 3 ˙ ψ i cos ψ i ) < 0 when φ i ∈ S 5 1 ∪ S 6 1 . Thus if φ i ( t 0 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , the possibilities for the movement of φ i after t 0 can be partitioned into the following three cases. Case 1: φ i ( t ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } holds for any t ≥ t 0 . Since ϑ i = k 1 ρ i + k 2 ψ i + k 3 sin ψ i , it can be re written as ϑ i = k 1 ρ i + k 2 arcsin ˙ ρ i v i + k 3 ˙ ρ i v i . Denote h ( x ) = k 2 arcsin x v i + k 3 x v i , x ∈ [ − v i sin a, v i sin a ] . Obviously , h ( x ) is an odd func- tion, and its inv erse function h − 1 ( x ) exists. Thus if ϑ i ( t ) ≡ 0 holds for all t ≥ t 0 , then ˙ ρ i = − h − 1 ( k 1 ρ i ) . Since h ( x ) is a Lipschitz function, there exists a positi ve constant c 1 such that | h ( x ) | ≤ | c 1 x | . Then | ρ i ( t ) | ≤ | ρ i ( t 0 ) | exp[ − k 1 c 1 ( t − t 0 )] , meaning ρ i ( t ) → 0 as t → ∞ . Moreov er , ψ i ( t ) → 0 since ϑ i ( t ) = k 1 ρ i + k 2 ψ i + k 3 sin ψ i ≡ 0 holds for all t ≥ t 0 . Case 2: φ i ( t ) / ∈ { ( ρ i , ψ i ) : ϑ i = 0 } for some t > t 0 , but φ i ( t ) ∈ S 2 1 ∪ S 4 1 , ∀ t > t 0 . In this case, since ˙ ψ i ≥ α if φ i ∈ S 4 1 \ { ( ρ i , ψ i ) : ϑ i = 0 } , and ˙ ψ i ≤ − α if φ i ∈ S 2 1 \ { ( ρ i , ψ i ) : ϑ i = 0 } . Therefore, | ψ i | is non-increasing when φ i ( t ) / ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , and the total time duration that φ i ( t 0 ) / ∈ { ( ρ i , ψ i ) : ϑ i = 0 } is ﬁnite, which is no more than a α . Besides, | ρ i | is also non-increasing when φ i ( t ) / ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , and by combining the con vergence results in Case 1 , we get | ρ i ( t ) | ≤ | ρ i ( t 0 ) | exp[ − k 1 c 1 ( t − t 0 − a α )] , and thus lim t →∞ ρ i ( t ) = 0 . Since | ψ i | is non-increasing and bounded, then lim t →∞ | ψ i | exists. Note that the total time duration that φ i ( t 0 ) / ∈ { ( ρ i , ψ i ) : ϑ i = 0 } is ﬁnite and lim t →∞ ρ i ( t ) = 0 , it leads to lim t →∞ ψ i ( t ) = 0 , and thus lim t →∞ φ i ( t ) = 0 . Case 3: there exists φ i ( t 0 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } and φ i ( t ) / ∈ S 2 1 ∪ S 4 1 for some t > t 0 . Since φ i will not go to S 5 1 and S 6 1 directly , then φ i must hav e entered S 1 1 ∪ S 3 1 through 7  1 R  1 R a  1 0 | ( ) | i a R t  0 i   0 0 ( ( ) , ( ) ) ii t t 1 1 ( ( ) , ( ) ) ii t t a Fig. 4. An illustration of the state trajectory in Case 3 when initially φ i ( t 0 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } . the ψ -axis. W ithout loss of generality , we assume ρ i ( t 0 ) > 0 and ψ i ( t 0 ) < 0 , as shown in Fig. 4. After φ i enters S 3 1 , since ˙ ψ i ≥ α > 0 holds in S 3 1 and S 5 1 , there exists a ﬁnite time t 1 ≤ t 0 + 2 a α such that φ i ( t 1 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , with ρ i ( t 1 ) ≤ 0 and ψ i ( t 1 ) ≥ 0 . W e use ( − r 1 , 0) to denote the intersection of the trajectory of φ i with the ρ -axis, since ˙ ψ i ≥ α > 0 when φ i ∈ S 4 1 \ { ( ρ i , ψ i ) : ϑ i = 0 } , and inequality (6) holds when φ i ∈ S 3 1 , which means r 1 < R 1 a | ψ i ( t 0 ) | . Moreover , since ˙ ρ i = v i sin ψ i > 0 in S 5 1 . Therefore, | ρ i ( t 1 ) | < r 1 < R 1 a | ψ i ( t 0 ) | . W e note that | ψ i ( t 0 ) | ≤ k 1 k 2 | ρ i ( t 0 ) | and | ψ i ( t 1 ) | ≤ k 1 k 2 | ρ i ( t 1 ) | . Thus | ρ i ( t 1 ) | < R 1 k 1 ak 2 | ρ i ( t 0 ) | and | ψ i ( t 1 ) | < R 1 k 1 ak 2 | ψ i ( t 0 ) | . Let σ := R 1 k 1 ak 2 , and we have σ < 1 . Therefore, | ρ i ( t ) | ≤ | ρ i ( t 0 ) | , | ψ i ( t ) | ≤ | ψ i ( t 0 ) | when t ∈ [ t 0 , t 1 ] . By symmetry , when φ i ( t 1 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , there are three possibilities for the movement of φ i after t 1 , corresponding to the three cases we listed here. W e only consider Case 3 , since the ﬁrst two cases implies the path following error conv erges to zero by our analysis above. Now if φ i enters S 1 1 after t 1 and then reaches { ( ρ i , ψ i ) : ϑ i = 0 } again at time t 2 , it follows that | ρ i ( t ) | ≤ σ | ρ i ( t 0 ) | , | ψ i ( t ) | ≤ σ | ψ i ( t 0 ) | when t ∈ [ t 1 , t 2 ] . Proceeding forward, we get | ρ i ( t ) | ≤ σ m − 1 | ρ i ( t 1 ) | , and | ψ i ( t ) | ≤ σ m − 1 | ψ i ( t 1 ) | , for t ∈ [ t m , t m +1 ] , where t m corresponds to the m -th time that φ i leav es S 2 1 ∪S 4 1 and reaches { ( ρ i , ψ i ) : ϑ i = 0 } again. Since σ m − 1 → 0 as m → ∞ , we get lim t →∞ | ρ i ( t ) | = 0 and lim t →∞ | ψ i ( t ) | = 0 , i.e., lim t →∞ φ i ( t ) = 0 . Combining the above cases together , we conclude that lim t →∞ φ i ( t ) = 0 always holds. Remark 7: According to the proof of Theorem 3, if φ i ( t 0 ) ∈ { ( ρ i , ψ i ) : ϑ i = 0 } , there are three possibilities for the mo vement of φ i after t 0 . Ho we ver , in a small neighborhood of the origin, it satisﬁes k 1 v max | sin ψ i | ≤ k 2 α + k 3 α cos ψ i . As a result, ϑ i ˙ ϑ i ≤ 0 alw ays holds in this neighborhood, and ϑ i ˙ ϑ i = 0 if and only if ϑ i = 0 , meaning φ i can only slide on ϑ i = 0 once it reaches { ( ρ i , ψ i ) : ϑ i = 0 } in this neighborhood. W e also note that since the signum function is adopted in the control law , ω i is not continuous at ϑ i = 0 , but φ i is continuous. Thus, the solution should be understood in the sense of Filippov . T o eliminate chattering caused by the signum function, a high-slope saturation function can be used to replace it [36]. Remark 8: It should be noted that the L yapunov method can be employed in each subset of S 1 , but we still need to prov e the overall stability by considering the six subsets and their boundaries altogether , which is actually what we did in our current proof of Theorem 3. T o be more speciﬁc, consider the following L yapunov function: V =              1 2 ( k 1 ρ i + k 2 ψ i + k 3 sin ψ i ) 2 , φ i ∈ S 1 1 ∪ S 3 1 ; 1 2 ( k 2 ψ i + k 3 sin ψ i ) 2 , φ i ∈ S 2 1 ∪ S 4 1 ; 1 2 ( k 1 ρ i ) 2 , φ i ∈ S 5 1 ∪ S 6 1 . It can be found that V is a continuous function of φ i , and ˙ V < 0 in the interior of each subset. Howe ver , V is not differentiable with respect to φ i on the boundary of these subsets. Thus it calls for the similar technique to the above proof to analyze the boundaries in order to conclude the asymptotic stability . Now we hav e demonstrated the path following stability in S 1 . In terms of the coordination, we have the following claim that the coordination error will also con verge to zero. Theor em 4 (Con ver gence of Coor dination Err or): Suppose φ i ( t 0 ) ∈ S 1 , ∀ i = 1 , . . . , n , by executing Algorithm 1, lim t →∞ ζ i ( t ) = L for all i . Pr oof: It follows from Theorem 2 that the pre-neighbor of each U A V does not change. Without loss of generality , suppose U A V 1 is the pre-neighbor of U A V 2, and U A V 2 is the pre-neighbor of U A V 3, and so on. Since UA V 1 does not hav e a pre-neighbor, it is artiﬁcially set as ζ 1 = L . For U A V 2, consider V 2 = (1 / 2)( ζ 2 − L ) 2 , thus ˙ V 2 = ( ζ 2 − L ) ˙ ζ 2 = ( ζ 2 − L )( v r 1 − v r 2 ) = − ( ζ 2 − L )( v r 2 − χ ( L )) . According to the deﬁnition of χ ( · ) and Lemma 6, ( v r i − χ ( L )) · ( ζ i − L ) ≥ 0 , and the equality holds if and only if ζ i = L . Thus, we obtain lim t →∞ ζ 2 ( t ) = L by using LaSalle’ s in variance principle [36]. As a result, lim t →∞ v r 2 = χ ( L ) . Since ˙ ζ 3 = ( v r 2 − χ ( L )) − ( v r 3 − χ ( L )) , and the system described by equation ˙ ζ i = − ( v r i − χ ( L )) will con verge to lim t →∞ ζ i = L (we get this claim by using the same analyses for UA V 2), with the Limiting Equation Theorem [37], we get lim t →∞ ζ 3 = L . Proceeding forward, we get lim t →∞ ζ 1 ( t ) = . . . = lim t →∞ ζ n ( t ) = L . Remark 9: W e hav e completed the control law design in the coordination set S 1 . W e demonstrate that the designed control law can drive the UA Vs whose path following errors initially in the coordination set to mov e onto the predeﬁned path, with the inter-U A V arc distances conv erging to the desired value. Recall that in Remark 5, we hav e proposed a guideline for the user-determined value c , that is, the smaller c is, the larger the coordination set would become, since we will get a larger feasible region for (12). Ho wev er , the value of c is not the smaller , the better . Now , we brieﬂy analyze it from the perspectiv e of con vergence rate of the coordination error . Assume the j th U A V is the pre-neighbor of the i th U A V , and the coordination error of the j th U A V is already zero, i.e., ζ j = L . If ζ i < L , then | ˙ ζ i | = | v r i − v r j | = | χ ( ζ i ) − χ ( ζ j ) | ≤ (1 − λ )[ cos a 1 + κ 0 R 1 v m − 1 1 − κ 0 R 1 v min ] . (14) Recall that (10) is a precondition for (12), then with a greater c , we are more likely to ha ve a greater v alue of cos a 1+ κ 0 R 1 v m − 1 1 − κ 0 R 1 v min from (12), then we can get a greater 8 upper-bound for the conv ergence rate of the coordination error in this case. Therefore, the determination of value c is a trade- off between enlarging the coordination set and increasing the con vergence rate of the sequenced inter-U A V arc distance. I V . S I N G L E - A G E N T L E V E L C O N T R O L L A W O U T S I D E C O O R D I NAT I O N S E T In Section III, we hav e designed the control law for those U A Vs whose path following errors are within S 1 , such that they follo w the path in a coordination manner . Now we discuss how to control the UA Vs whose path follo wing errors are outside S 1 . In that case, those UA Vs do not follow the path cooperativ ely , but only adjust their path follo wing errors at the single-agent lev el, in order to enter S 1 . W e label the set containing those path following errors outside S 1 as S 2 . T o describe S 2 , we deﬁne a uni verse S which giv es the scope for our designed control law 4 : S = { ( ρ, ψ ) : ρ ∈ [ − R 2 , R 2 ] , ψ ∈ [ − π , π ) } . (15) Then, set S 2 is deﬁned as S 2 := S \ S 1 . W e further divide S 2 into four subsets: S 1 2 , S 2 2 , S 3 2 and S 4 2 , as shown in Fig. 5. The mathematical descriptions of these subsets are as follows: S 1 2 =  { ( ρ, ψ ) : ψ > 0 } ∩ S 2 \ S 2 2  [ { ( ρ, ψ ) : R 1 < ρ ≤ R 2 , ψ = 0 } , S 2 2 = { ( ρ, ψ ) : − R 2 ≤ ρ < − R 1 , 0 < ψ ≤ a } , S 3 2 =  { ( ρ, ψ ) : ψ < 0 } ∩ S 2 \ S 4 2  [ { ( ρ, ψ ) : − R 2 ≤ ρ < − R 1 , ψ = 0 } , S 4 2 = { ( ρ, ψ ) : R 1 < ρ ≤ R 2 , − a ≤ ψ < 0 } .  1 R  a a     1 2 S 1 R 4 2 S 3 2 S 1 S 2 2 S 2 R 2 R  4 2 S Fig. 5. S 1 and partition of S 2 . In the rest of this section, we design the control laws for these four subsets: In Section IV -A, we prov e the existence of the control law which makes any path following error in sets S 2 2 and S 4 2 enter S 1 , and a near time optimal control law for sets S 2 2 and S 4 2 is designed. In Section IV -B, we design a robust control law for sets S 1 2 and S 3 2 . A. Near T ime Optimal Contr ol Law in S 2 2 and S 4 2 Firstly , we prov e the existence of control laws which makes the path following error within S 2 2 ∪ S 4 2 enter S 1 . Theor em 5 (Dynamics in S 2 2 and S 4 2 ): If R 2 < 1 κ 0 − v min ω max , then for any φ i ( t 0 ) ∈ S 2 2 ∪ S 4 2 , there exists time t 1 , and control 4 For the path following errors outside S , they are distant away from the path Γ , and we can design Dubins paths to driv e the errors into S . ( v i ( t ) , ω i ( t )) , t ∈ [ t 0 , t 1 ] , satisfying constraint (2), such that φ i ( t 1 ) ∈ S 1 . Pr oof: W ithout loss of generality , we consider the condi- tion in S 4 2 . The condition in S 2 2 can be deduced similarly . In S 4 2 , since ψ i < 0 , then ˙ ρ i < 0 . According to Lemma 1, ∀ φ i ( t 0 ) ∈ S 4 2 would not leav e S through ρ = R 2 , thus it can only enter S 1 through ρ = R 1 , or enter S 3 2 through ψ = − a , or enter S 1 2 through ψ = 0 , or remain in S 4 2 forev er . W e sho w that by applying appropriate control law , the last three cases can be made impossible. Otherwise, suppose φ i will enter S 3 2 , then there exists φ i = ( ρ i , ψ i ) such that ˙ ψ i = ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i < 0 (16) holds for all v i and ω i satisfying (2). Let ω i = ω max , v i = v min , and (16) becomes: ω max − κ ( p i ) v min cos ψ i 1 − κ ( p i ) ρ i < 0 . (17) If φ i ∈ S 4 2 , then cos ψ i > 0 and 1 + κ ( p i ) ρ i > 0 hold, then ω max − κ ( p i ) v min cos ψ i 1 − κ ( p i ) ρ i ≥ ω max − κ 0 v min cos ψ i 1 − κ 0 ρ i ≥ ω max − κ 0 v min 1 − κ 0 R 2 ≥ 0 (18) which is contrary to (17), meaning there exist v i and ω i satisfying (2), such that φ i will not enter S 3 2 . In the same way , we conclude that there exist v i and ω i such that φ i will not enter S 1 2 if ( ρ i ( t 0 ) , ψ i ( t 0 ) ∈ S 4 2 , and additionally , it can be made that ψ i ( t ) ≤ ψ i ( t 0 ) for all t ≥ t 0 before φ i leav es S 4 2 . Thus there exists t 1 ≤ R 1 − ρ i ( t 0 ) v min sin ψ i ( t 0 ) such that φ i ( t 1 ) ∈ S 1 . Since all the state errors in S 2 2 and S 4 2 can enter S 1 when R 2 < 1 κ 0 − v min ω max , it is necessary to make φ i enter S 1 as soon as possible. Suppose t f is the minimum time instant such that φ i ( t f ) ∈ S 1 , then the time optimal control objective is to minimize t f , which can be formulated as follows. (P1) minimize J 1 = t f , s . t . φ i ( t 0 ) ∈ S 2 2 ∪ S 4 2 , φ i ( t f ) ∈ S 1 , and inequality (2) holds. In general, it is difﬁcult to deriv e the optimal solution to P1. Here we adopt a greedy strategy to transform P1 into a near optimal control problem. Let d = inf φ f ∈S 1 k φ i ( t ) − φ f k denote the point-to-set Euclidean distance from φ i ( t ) to S 1 . In our greedy strategy , the control objectiv e is to minimize ˙ d at ev ery time instant 5 , i.e., to driv e φ i ( t ) as close as possible tow ards S 1 . Thus P1 is transformed to the follo wing problem. (P2) minimize J 2 = ˙ d, where d = inf φ f ∈S 1 k φ i ( t ) − φ f k , s . t . φ i ( t ) ∈ S 2 2 ∪ S 4 2 , and inequality (2) holds. When φ i ( t ) ∈ S 4 2 , d = ρ i − R 1 , then J 2 = ˙ ρ i = v i sin ψ i . 5 This is why we say this strategy is “greedy”. 9 W e can see that J 2 is af ﬁne with respect to v i . In S 4 2 , ψ i < 0 , so we need to set v i = v max . In terms of ω i , we hav e ∂ J 2 ∂ ω i = ∂ J 2 ∂ ψ i · ∂ ψ i ∂ ω i = ∂ J 2 ∂ ψ i · ∂ ˙ ψ i ∂ ω i · dt = v i cos ψ i · dt > 0 . (19) Therefore, to minimize J 2 , we choose ω i = − ω max , and the control law in S 4 2 becomes v i = v max , ω i = − ω max . (20) W ith control law (20), we hav e ˙ ψ i = ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i = − ω max − κ ( p i ) v min cos ψ i 1 − κ ( p i ) ρ i ≤ − ω max + κ 0 v min 1 + κ 0 R 1 ≤ 0 . (21) According to Lemma 1, by applying this control law , φ i will not enter S 1 2 through the ρ -axis. Howe ver , it may lead φ i to enter S 3 2 . T o prev ent it, we set up a threshold  0 , where 0 <  0  a . When − a ≤ ψ i < − a +  0 , we switch to a new control mode. There are two principles for this new mode. Firstly , we need ˙ ψ i ≥ 0 such that φ i will not enter S 3 2 . Secondly , we need to minimize ˙ d , i.e., v i sin ψ i . Thus, the optimization problem P2 becomes (22) when − a ≤ ψ i < − a +  0 : minimize v i sin ψ i , s . t . ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i ≥ 0 , and (2) holds. (22) Then, our near time optimal control law in S 4 2 can be described as follows: i) If ψ i ≥ − a +  0 , the control law is (20). ii) If − a ≤ ψ i < − a +  0 , the control law is the solution of (22), i.e., • if ω max − κ ( p i ) v max cos ψ i 1 − κ ( p i ) ρ i ≥ 0 , then v i = v max , ω i = max  − ω max , κ ( p i ) v max cos ψ i 1 − κ ( p i ) ρ i  ; • if ω max − κ ( p i ) v max cos ψ i 1 − κ ( p i ) ρ i < 0 , then v i = ω max (1 − κ ( p i ) ρ i ) κ ( p i ) cos ψ i , ω i = ω max . The control law in S 2 2 can be designed in the same way as follows: i) If ψ i ≤ a −  0 , the control law is (23); v i = v max , ω i = ω max . (23) ii) If a −  0 < ψ i ≤ a , the control la w is the solution of (24), maximize v i sin ψ i , s . t . ω i − κ ( p i ) v i cos ψ i 1 − κ ( p i ) ρ i ≤ 0 , and (2) holds. (24) It is easy to verify that the proposed near time optimal control law is the desired control in Theorem 5, i.e., φ i will enter S 1 when initially in S 2 2 ∪ S 4 2 by ex ecuting the control law . B. Rob ust Contr ol Law in S 1 2 and S 3 2 By symmetric property , we only analyze the control law design in S 1 2 , and the methods can be applied to S 3 2 . It is obvious by Lemma 1 that S 1 2 is not an in variant set. Speciﬁcally , there are ﬁve situations that φ i ( t ) leav es S 1 2 : i) φ i ( t ) passes through ψ = a, − R 1 ≤ ρ < 0 , or aρ + R 1 ψ = aR 1 , 0 ≤ ρ ≤ R 1 , and enters S 1 directly; ii) φ i ( t ) passes through ψ = a, − R 2 ≤ ρ < − R 1 , and enters S 2 2 ; iii) φ i ( t ) passes through ψ = 0 , R 1 < ρ ≤ R 2 , and enters S 4 2 ; iv) φ i ( t ) passes through ψ = π , and enters S 3 2 ; v) φ i ( t ) passes through ρ = R 2 , 0 ≤ ψ ≤ π , and leav es S . Among all these situations, situation i) is the case that φ i ( t ) enters S 1 directly . For situations ii) and iii), φ i ( t ) enters S 2 2 or S 4 2 , which can ﬁnally enter S 1 (see Theorem 5). For situation v), φ i ( t ) leav es S . F or situation iv), φ i ( t ) enters S 3 2 , which has the same property as that in S 1 2 due to the symmetric property . W e note that for situation i v), it still has the possibility to make φ i ( t ) leave S . Hence, we consider a robust control scheme to av oid the last two situations. More speciﬁcally , our objective is to maximize the possibility of the trajectory belonging to the ﬁrst three situations. Denote the ratio of ˙ ψ i and ˙ ρ i as β i , i.e., β i = ˙ ψ i ˙ ρ i . W e note that a smaller β i can make φ i ∈ S 1 2 hav e a higher possibility of belonging to the ﬁrst three situations. This is because β i determines the tangent to the state trajectory , and a smaller β i corresponds to a steeper slope towards the ρ -axis, which maximizes the possibility of the trajectory entering S 1 ∪ S 2 2 ∪ S 2 4 . In this way , the control problem in S 1 2 is formulated as minimize ω i v i sin ψ i − κ ( p i ) cot ψ i 1 − κ ( p i ) , s . t . φ i ( t ) ∈ S 1 2 , and (2) holds. and the solution is v i = v min , ω i = − ω max . (25) Similarly , the control law in S 3 2 can be deriv ed as v i = v min , ω i = ω max . (26) Then, we provide a sufﬁcient condition for states in S 1 2 entering S 1 ﬁnally . Theor em 6 (Dynamics in S 1 2 ): Consider the system de- scribed by state equations (27) and (28), where the state vari- ables are denoted as ˜ φ i = ( ˜ ρ i , ˜ ψ i ) . If ˜ φ i ( t 0 ) = φ i ( t 0 ) ∈ S 1 2 , and the state trajectory of ˜ φ i has an intersection with the ˜ ρ - axis, denoted as ( R ∗ , 0) , where R ∗ ≤ R 2 < 1 κ 0 − v min ω max , then for any φ i ( t 0 ) ∈ S 1 2 , by applying control law (25), φ i will get into S 1 ∪ S 2 2 ∪ S 4 2 in a ﬁnite time. ( ˙ ˜ ρ i = v min sin ˜ ψ i , ˙ ˜ ψ i = − ω max − κ 0 v min cos ˜ ψ i 1 − κ 0 ˜ ρ i , π / 2 ≤ ˜ ψ i < π (27) ( ˙ ˜ ρ i = v min sin ˜ ψ i , ˙ ˜ ψ i = − ω max + κ 0 v min cos ˜ ψ i 1+ κ 0 ˜ ρ i , 0 ≤ ˜ ψ i < π / 2 (28) 10 Pr oof: The state trajectory of ˜ φ i after t 0 can be described by g ( φ i ) = 0 , with the gradient vector 5 g ( φ i ) = ( − ˙ ˜ ψ i , ˙ ˜ ρ i ) . Since g ( φ i ) = 0 has an intersection with the ˜ ρ -axis at ( R ∗ , 0) , where R ∗ ≤ R 2 , then g ( φ i ) = 0 lies in S when ˜ ψ i ∈ [0 , π ) . By applying control la w (25), ˙ ρ i = v min sin ψ i , ˙ ψ i = − ω max − κ ( p i ) v min cos ψ i 1 − κ ( p i ) ρ i . Suppose g ( φ i ( t )) = 0 at time t , where t ≥ t 0 , and φ i ( t ) ∈ S 1 2 , it can be veriﬁed that f ( φ i ) · 5 g ( φ i ) ≤ 0 holds. W ith Lemma 1, we conclude that the state trajectory of φ i is bounded by g ( φ i ) = 0 when φ i ∈ S 1 2 , i.e., φ i will not leav e S . Let α 1 = ω max − κ 0 v min 1 − κ 0 R 2 , and we hav e α 1 > 0 . By applying control law (25), ˙ ψ i ≤ − α 1 < 0 when φ i ∈ S 1 2 . Thus there exists a ﬁnite time t 1 ≤ t 0 + π α 1 , such that φ i ( t 1 ) ∈ S 1 ∪ S 2 2 ∪ S 4 2 . For states in S 3 2 , we hav e a similar result. Theor em 7 (Dynamics in S 3 2 ): Consider the system de- scribed by state equations (29) and (30), where the state vari- ables are denoted as ˜ φ i = ( ˜ ρ i , ˜ ψ i ) . If ˜ φ i ( t 0 ) = φ i ( t 0 ) ∈ S 3 2 , and the state trajectory of ˜ φ i has an intersection with the ˜ ρ - axis, denoted as ( − R ∗ , 0) , where R ∗ ≤ R 2 < 1 κ 0 − v min ω max . then for any φ i ( t 0 ) ∈ S 3 2 , by applying control law (26), φ i will get into S 1 ∪ S 2 2 ∪ S 4 2 in a ﬁnite time. ( ˙ ˜ ρ i = v min sin ˜ ψ i , ˙ ψ i = ω max + κ 0 v min cos ˜ ψ i 1+ κ 0 ˜ ρ i , − π ≤ ˜ ψ i < − π / 2 (29) ( ˙ ˜ ρ i = v min sin ˜ ψ i , ˙ ψ i = ω max − κ 0 v min cos ˜ ψ i 1 − κ 0 ˜ ρ i , − π / 2 ≤ ˜ ψ i ≤ 0 (30) After φ i enters S 2 2 ∪ S 4 2 , it is certain that following our proposed control law in these two subsets (see Section IV -A), φ i can ﬁnally enter S 1 . W e have the following theorem to conclude the stability and con ver gence of the ov erall closed- loop system. Theor em 8: Consider a ﬂeet of ﬁxed-wing U A Vs following a C 2 -smooth path under Assumption 1, with the path following error equation described by (3) with constraints (2). If R 2 < 1 κ 0 − v min ω max , and each U A V is in one of the following cases: • Case 1 : φ i ( t 0 ) ∈ S 1 ; • Case 2 : φ i ( t 0 ) ∈ S 2 2 ∪ S 4 2 ; • Case 3 : φ i ( t 0 ) ∈ S 1 2 , and the state trajectory of ˜ φ i following (27) and (28) has an intersection with the ˜ ρ - axis at ( R ∗ , 0) , where R ∗ ≤ R 2 , when ˜ φ i ( t 0 ) = φ i ( t 0 ) ; • Case 4 : φ i ( t 0 ) ∈ S 3 2 , and the state trajectory of ˜ φ i following (29) and (30) has an intersection with the ˜ ρ -axis at ( − R ∗ , 0) , where R ∗ ≤ R 2 , when ˜ φ i ( t 0 ) = φ i ( t 0 ) ; then by ex ecuting Algorithm 1 when φ i ( t ) ∈ S 1 , control law (20) and (22) when φ i ( t ) ∈ S 4 2 , control law (23) and (24) when φ i ( t ) ∈ S 2 2 , control law (25) when φ i ( t ) ∈ S 1 2 , and control law (26) when φ i ( t ) ∈ S 3 2 , we will ﬁnally get lim t →∞ φ i = 0 , lim t →∞ ζ i = L, ∀ i . Note that the proposed controller in Theorem 8 for the ov erall sysem is hybrid, as the controller is not continuous at the boundary of the subsets. For example, when φ i enters S 1 from S 2 4 at time t a , and ψ i ( t a ) ≥ − a +  0 , then according to (20), lim t → t − a v i = v max , and lim t → t − a ω i = − ω max . Ho wev er , it does not necessarily return v i = v max and ω i = ω max with Algorithm 1 at time t a . No w , it is the position to analyse the stability of Theorem 8. Pr oof: The conv ergence of the closed-loop system can be concluded by using the similar technique with Theorem 3.5 in [2]. In each of the last three cases, φ i will enter the coordination set, i.e., satisﬁes the condition in Case 1 , within a ﬁnite time: • for Case 2 , by Theorem 5, there exists a time t 1 ≥ t 0 such that φ i ( t 1 ) ∈ S 1 ; • for Case 3 and Case 4 , by Theorem 6 and Theorem 7, there exists a time t 2 ≥ t 0 such that φ i ( t 2 ) ∈ S 1 ∪S 2 2 ∪S 2 4 ; if φ i ( t 2 ) ∈ S 2 2 ∪ S 2 4 , by Theorem 5, there exists a time t 3 ≥ t 2 ≥ t 0 such that φ i ( t 3 ) ∈ S 1 ; while for Case 1 , by Theorem 1, φ i remains in S 1 thereafter . Consequently , there exists a time t ∗ ≥ t 0 such that φ i ( t ∗ ) ∈ S 1 holds for all i . Thus, follo wing Theorem 3, we get lim t →∞ φ i = 0 , and following Theorem 4, we get lim t →∞ ζ i = L . Remark 10: W e note that collisions between UA Vs can be av oided if the path has no intersection points and the U A Vs are inside the coordination set, since no overtaking will occur according to Theorem 2. Howe ver , when the U A Vs are outside the coordination set, since they are executing the single-agent lev el control law , collision av oidance is not guaranteed. In real applications, some collision av oidance algorithms such as [38] can be employed. V . S I M U L A T I O N R E S U LT S In this section, simulations are giv en to corroborate the effecti veness of our control strategy for coordinated path following. The simulation consists of two parts: the simulation with MA TLAB, and the Hardware-In-the-Loop (HIL) simula- tion with the X-Plane simulator . A. MA TLAB Simulation Firstly , we validate the algorithm with a typical path follow- ing problem, the cyclic pursuit on a circle. The control objec- tiv e is to distribute all the U A Vs uniformly on a circle. The U A Vs are with the constraints v min = 10m / s , v max = 25m / s , and ω max = 0 . 2rad / s . The desired path is a circle centered at (0 , 0) , with the radius of r = 1000m . W e are employing n = 6 U A Vs in the simulation, then the desired arc distance is L = 2 π r/n = 1000 π / 3m . W e take κ 0 = 0 . 002 , then the optimized parameters for the coordination set are a = 0 . 6303 , R 1 = 122 . 1297 . W e take control parameters k 1 = 1 , k 2 = R 1 /a + 1 , k 3 = 1 ,  0 = 0 . 05 . The initial positions and the orientations of the six UA Vs are (600 , 0 , 0 . 6 π ) T , (200 , 580 , − π ) T , (650 , − 160 , 0 . 3 π ) T , (1100 , 0 , − 0 . 25 π ) T , ( − 1100 , − 80 , 0 . 75 π ) T , and ( − 200 , 1000 , − 0 . 25 π ) T . χ ( ζ i ) is deﬁned as χ ( ζ i ) =      v r min , when ζ i < L − 6; 0 . 475( ζ i − L + 6) + v r min , when | ζ i − L | ≤ 6; 0 . 95( ζ i − L ) + v r min , otherwise . where v r min = 1 1 − κ 0 R 1 v min , The trajectories of the six UA Vs under our hybrid control law are shown in Fig. 6. The wedges in the ﬁgure not only 11 -1000 -500 0 500 1000 X (m) -1000 -500 0 500 1000 Y (m) 1: at t=0 1: at t=24.67 1: at t=400 2: at t=0 2: at t=23.74 2: at t=400 3: at t=0 3: at t=16.55 3: at t=400 4: at t=0 4: at t=18.45 4: at t=400 5: at t=0 5: at t=19.22 5: at t=400 6: at t=0 6: at t=8.94 6: at t=400 Fig. 6. Trajectories of six U A Vs following a circle, with the green wedges indicating the initial positions and headings for each U A V , blue ones indicating the states when the UA Vs enter S 1 , black ones indicating the ﬁnal states. indicate the positions of the UA Vs, but also their headings. The green wedges represent the initial positions and headings for the U A Vs. Initially , φ 1 (0) , φ 2 (0) , φ 6 (0) ∈ S 1 2 , φ 3 (0) ∈ S 4 2 , φ 4 (0) , φ 5 (0) ∈ S 3 2 . By executing the single-agent level control law in these subsets, φ i , i = 1 . . . 6 all enter the coordination set S 1 . The blue wedges represent the positions and headings at the time when the U A Vs enter S 1 . Besides, by executing the coordinated path following control law in S 1 , we ﬁnd that φ i ( t ) , i = 1 . . . 6 con ver ge to zero, as sho wn in Fig. 7(a) and Fig. 7(b), respectively . The positions and headings for the U A Vs at t = 400s are shown by the black wedges in Fig. 6, and we can see that each successfully follows the path with a desired arc distance from its pre-neighbor . The arc distances between every two adjacent UA Vs are shown in Fig. 7(c), which ﬁnally conv erge to the desired con- stant L after all the U A Vs enter S 1 , 6 meaning the coordination errors conv erge to 0, and all the UA Vs are ev entually e venly spaced with the desired distance between the adjacent U A Vs, while moving along the path. W e should note that there is a jump of the arc distances to 0 for U A V 3 and 4 before all the U A Vs enter the coordination set S 1 , which means an ov ertaking occurs, and their pre-neighbors are changed. Howe ver , after all the U A Vs enter S 1 , there are no such jumps of arc distances, meaning that each U A V will not change its pre-neighbor , and there are no overtakings any more. Fig. 8 shows the control inputs of each U A V . It can be observed that the control inputs are bounded but not continuous. It should be noted that the existing methods for the co- ordinated path following control without considering speed constraints cannot solve our problem. The trajectories of six U A Vs with speed constraints using the method in [2] are shown in Fig. 9. All the U A Vs are placed at the same initial positions as in Fig. 6. It can be seen that only U A V 6 ﬁnally con verges to the desired path, while the other ﬁv e U A Vs do not, demonstrating that our proposed control law has extended the work in [2] and solved the problem of coordinated path following with speed constraints. B. HIL Simulation T o further validate the proposed algorithm, an HIL sim- ulation en vironment is constructed, which consists of four 6 All the U A Vs are in S 1 after 24.67s. computers running the X-Plane ﬂight simulator, four auto- pilots , and a gr ound contr ol station , as shown in Fig. 10. W e use ethernet networks for the communications among the three parts. The plane chosen in our HIL simulation is the Great Planes PT -60 RC plane [11], [15]. With all these facilities, we have conducted two typical HIL simulations, i.e., cyclic pursuit and parallel path following. 1) Cyclic Pursuit: In this setting, ﬁrstly , three U A Vs are ex ecuting the coordinated path following algorithm, and trying to follo w the path while being distrib uted evenly on the orbit. After the system becomes stable, the fourth U A V joins. The orbit and the control parameters are set as the same as those in Section V -A. The location dif ferences and orientation differences are sho wn in Fig. 11(a) and Fig. 11(b), respecti vely . It can be seen that the location differences of the four U A Vs con verge to 0, and in terms of the orientation difference, though they do not strictly con ver ge to 0 , each only has a ± 5 ◦ bias at the steady state. W e can also see that the joining of the fourth U A V has no inﬂuence on the stability of the location difference and orientation difference of the former three UA Vs. This demonstrates the scalablity of our algorithm. T o achie ve coordination, we set p 0 = (1000 , 0) T as a reference point, and we use l i to denote the arc distance from p 0 to the i th U A V’ s closest projection point p i . Each U A V broadcasts its arc distance l i with respect to p 0 to other U A Vs. After the i th U A V receiving the nearby U A Vs’ arc distances with respect to p 0 , it judges which U A V is its pre-neighbor , and ζ i is calculated as ζ i = l j − l i , (suppose the j th U A V is the i th U A V’ s pre-neighbor). The arc distances between U A Vs are shown in Fig. 11(c). W e can see that the arc distances can be steered to around the desired value whether there are three or four U A Vs. W e can also ﬁnd that there are cyclic ﬂuctuations of arc distances. The ﬂuctuations are caused by two main reasons. The ﬁrst reason is that our control law is based on the ﬁrst order system, with the acceleration period ignored in our model. The second reason is attributed to the establishment of the cyclic interaction topology in this scenario, in which each UA V has a pre-neighbor to follow . W e ha ve already shown that the proposed control law can stabilize the arc distances for the ﬁrst order U A V model in Fig. 7(c). Later we will show in another example that with a tree interaction topology , the cyclic ﬂuctuation can be eliminated or reduced. W e also note that the fewer U A Vs, the smaller ﬂuctuations would be, as shown in Fig. 11(c), where the ﬂuctuation for 3- U A V coordination is smaller than that for 4-U A V coordination. 2) P arallel P ath F ollowing: With a little bit of modiﬁcation, we sho w our proposed hybrid control law can steer a ﬂeet of U A Vs to move on a set of parallel paths and achieve a desired “in-line” formation pattern. In this case, each U A V has its own target path. Still, the interaction topology is not pre-established but formed when all the U A Vs enter S 1 . In order to achiev e an “in-line” formation pattern, we set L = 0 , and χ ( · ) is deﬁned as χ ( ζ i ) = 0 . 475 ζ i + v r min . W e employ a cubic B-Spline curve [15] to obtain a continuous and non-constant curvature path. W e select sev en points as sho wn in T able I to generate the cubic B-Spline for U A V 1 to follow . In T able I, the Lon and the Lat represent the longitude and latitude, respecti vely . 12 0 100 200 300 400 Time (s) -200 0 200 400 600 Location Difference (m) UAV 1 UAV 2 UAV 3 UAV 4 UAV 5 UAV 6 (a) 0 100 200 300 400 Time (s) -3 -2 -1 0 1 2 3 Orientation Difference (rad) UAV 1 UAV 2 UAV 3 UAV 4 UAV 5 UAV 6 (b) 0 100 200 300 400 Time (s) 0 1000 2000 3000 4000 Arc Distance (m) Time when all the UAVs enter the coordination set. UAV 1 UAV 2 UAV 3 UAV 4 UAV 5 UAV 6 (c) Fig. 7. Con vergence performance in MA TLAB simulation: (a) location difference ρ i of each UA V ; (b) orientation difference ψ i of each UA V ; (c) arc distance ζ i between ev ery two adjacent UA Vs. T ABLE I P O SI T I O NS O F W AYP O I N TS . W aypoint 1 2 3 4 5 6 7 Lon/deg 113.2167 113.2371 113.2167 113.1963 113.2167 113.2371 113.2167 Lat/deg 28.2029 28.2209 28.2390 28.2570 28.2751 28.2931 28.3112 x /m 0 2006.43 4013.47 6019.83 8026.83 10033.19 12040.19 y /m 0 1996.54 0 -1997.26 0 1997.87 0 0 100 200 300 400 Time (s) 10 15 20 25 v i (m/s) UAV 1 UAV 2 UAV 3 UAV 4 UAV 5 UAV 6 0 100 200 300 400 Time (s) -0.2 0 0.2 ω i (rad/s) Fig. 8. Control inputs of each U A V . -1500 -1000 -500 0 500 1000 1500 X (m) -1000 -500 0 500 1000 Y (m) 1: at t=0 1: at t=400 2: at t=0 2: at t=400 3: at t=0 3: at t=400 4: at t=0 4: at t=400 5: at t=0 5: at t=400 6: at t=0 6: at t=400 Fig. 9. T rajectories of six UA Vs follo wing a circle with the method in [2], while v i and ω i are constrained. Only U A V 6 is on the desired path at t = 400 . Besides, we establish a north-east coordinate with the origin positioned at the ﬁrst waypoint of U A V 1, such that all the U A Vs’ states can be represented in an xy -plane. The positions of the waypoints in this new coordinate are also provided in T able I. The simulation results of the parallel path following are shown in Fig. 12. The path for UA V 1 is generated by B- spline, then by moving along the y − direction for 100m , 200m , U A V 1 U A V 2 Ground Control Station U A V 3 U A V 4 Switch X - Plane Sim ula to r UDP UDP Auto - Pilots E ac h au t o - p i l ot c on s i s t s of : T w o C or t e x - M 4 A R M : 168 MH z ; D oubl e 100 M ne t w or k i nt e r f a c e s ; 8 M S R A M , 6 U A R T , 3 S P I , 2 C A N , a nd 16 A D c onve r t or s w i t h 12 bi t s . UDP Fig. 10. The hardware-in-the-loop simulation environment. and 300m , we get the planned paths for U A V 2, U A V 3, and U A V 4, respecti vely , which are shown by the dashed curves in Fig. 12. W e can see that all the UA Vs ﬂy along the planned paths while achie ving the desired “in-line” formation pattern during the ﬂight. The arc distances between adjacent U A Vs are shown in Fig. 13. Contrary to the cyclic pursuit case with a cyclic interaction topology , the interaction topology established in this scenario is a tree, with U A V 1 as the global leader of the formation. W e can see that the cyclic ﬂuctuations are eliminated in Fig. 13. V I . C O N C L U S I O N In this paper , we have inv estigated the problem of steering a ﬂeet of ﬁxed-wing UA Vs with speed constraints along any C 2 -smooth path with maximum curvature κ 0 ≤ ω max /v min , while achieving sequenced desired inter-U A V arc distances. W e hav e proposed the hybrid control law based on the deﬁned coordination set: for each UA V , if its path following error is within this coordination set, then the U A V follows the path in a coordination manner with its pre-neighbor; otherwise, the U A V works at the single-agent level which individually controls the path following error towards the coordination set. T o handle the speed constraints from ﬁxed-wing U A Vs, we transform the parameter selection problem for the coordination set to an optimization problem, while satisfying the speed constraints of ﬁxed-wing U A Vs, as well as guaranteeing the con ver gence of 13 0 200 400 600 800 1000 Time(s) -2500 -2000 -1500 -1000 -500 0 500 1000 1500 Location Difference (m) Time when UAV 4 joins UAV 1 UAV 2 UAV 3 UAV 4 (a) (b) 0 200 400 600 800 1000 Time(s) 0 1000 2000 3000 4000 Arc Distance (m) Time when UAV 4 joins UAV 1 UAV 2 UAV 3 UAV 4 (c) Fig. 11. Con vergence performance in HIL simulation of cyclic pursuit: (a) location difference ρ i of each UA V ; (b) orientation difference ψ i of each UA V ; (c) arc distance ζ i between ev ery two UA Vs. t = 0 s t = 1 0 0 s t = 2 0 0 s t = 3 0 0 s t = 4 0 0 s t = 5 0 0 s t = 6 0 0 s Fig. 12. Parallel path Follo wing of four U A Vs, while achieving ”in-line” formation pattern. 0 200 400 600 800 1000 1200 Time(s) 0 200 400 600 800 1000 Arc Distance (m) UAV 1 UAV 2 UAV 3 UAV 4 Fig. 13. Arc distance ζ i between ev ery two UA Vs in the HIL simulation of parallel path follo wing. both the path following error and the coordination error . W e hav e also calculated the admissible set for these two errors reducing to zero when the U A Vs are executing our proposed control law . The algorithm is validated using MA TLAB and the HIL simulation, respecti vely , demonstrating the effecti ve- ness of the proposed approach. The proposed approach can scale up to handle different ve- locity bounds for heterogeneous ﬁxed-wing UA Vs by design- ing dif ferent coordination sets, provided that all the U A Vs have common feasible speed. Future work includes extending the proposed approach to the 3D case, considering communication delay , loss of communication, and wind disturbances. A P P E N D I X A P RO O F O F L E M M A 3 When φ i is in the ﬁrst quadrant, with (8), we hav e 0 ≥ v m ( a sin ψ i + R 1 κ 0 cos ψ i ) − R 1 ω max + R 1 α ≥ v m ( a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i ) − R 1 ω max + R 1 α. (31) Thus (4) is deriv ed when v i ∈ [ v min , v m ] and ω i = − ω max . When φ i is in the second quadrant, with (9), we hav e − α ≥ − ω max + v m κ 0 1 − κ 0 R 1 ≥ − ω max − v m κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i . Thus (5) is deriv ed when v i ∈ [ v min , v m ] and ω i = − ω max . Inequalities (6) and (7) can be concluded in the same way . A P P E N D I X B P RO O F O F L E M M A 4 Denoting the speed that the i th U A V moves along the path as v r i , then v r i = cos ψ i 1 − κ ( p i ) ρ i v i . Let the j th U A V be the pre-neighbor of the i -th U A V , and φ i , φ j ∈ S 1 , then ˙ ζ i = v r j − v r i . Suppose initially 0 < ζ i ( t 0 ) ≤ L − δ 1 , where 0 < δ 1 < L , if (10) holds, we can choose v i ∈ [ v min , v m ] and v j ∈ [ v min , v max ] , such that v r i = 1 1 − κ 0 R 1 v min , v r j ≥ 1 1 − κ 0 R 1 v min , then ˙ ζ i ≥ 0 holds for t ≥ t 0 , as a result, ζ i > 0 alw ays holds, i.e., there exists proper control law such that no ov ertaking occurs. A P P E N D I X C P RO O F O F L E M M A 6 W e take φ i ( t ) ∈ S 1 1 as an example. If ω i is not saturated in Line 3, i.e. ω i = ω d , then the v alue of v i will not be changed in R E S E T V A L U E , since the left-hand of inequality (4) becomes v i 1  a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i  + R 1 ω i + R 1 α = av i 1 sin ψ i − k 1 R 1 v i 1 k 2 ( k 1 ρ i + k 2 ψ i + k 3 sin ψ i ) ≤ av i 1 sin ψ i − k 1 R 1 v i 1 ψ i ( a ) ≤ 0 . where inequality (a) is caused by k 1 R 1 ≥ a and 0 < ψ i < π / 2 . Therefore, inequality (4) holds when ω i = ω d . As a result, v i will not be changed in R E S E T V A L U E . 14 Now suppose ω i = ω max , which means ω d ≥ ω max , the left-hand of inequality (4) becomes v i 1  a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i  + R 1 ω max + R 1 α ≤ v i 1  k 1 R 1 k 2 ( k 1 ρ i + k 2 ψ i + k 3 sin ψ i ) − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i  + R 1 α + R 1 ω max = R 1 ( − ω d + ω max ) ≤ 0 . Inequality (4) holds, implying that v i will not be changed. Finally , if ω i = − ω max and v i is changed in R E S E T V A L U E , then the returned value is v i =  a sin ψ i − R 1 κ ( p i ) cos ψ i 1 − κ ( p i ) ρ i  − 1 R 1 ( ω max − α ) ≥ R 1 ( ω max − α ) a sin ψ i + R 1 κ 0 cos ψ i ( b ) ≥ v m , where inequality ( b ) follo ws from (31). Clearly , v i < v i 1 , otherwise inequality (4) will hold and v i does not need to be changed. Thus, if φ i ∈ S 1 1 , and v i is changed in R E S E T V A L U E , then v m ≤ v i < v i 1 . Results for the other ﬁv e subsets are deduced similarly . R E F E R E N C E S [1] S.-J. Chung, A. Paranjape, P . Dames, S. Shen, and V . Kumar , “ A survey on aerial swarm robotics, ” IEEE Tr ans. Robot. , vol. 34, no. 4, pp. 837– 855, 2018. [2] Y . Lan, G. Y an, and Z. Lin, “Synthesis of distributed control of coordinated path following based on hybrid approach, ” IEEE T rans. Autom. Control , vol. 56, no. 5, pp. 1170–1175, 2011. [3] C. Samson, “Path follo wing and time-varying feedback stabilization of a wheeled mobile robot, ” in Pr oc. ICARCV’92 , 1992, pp. R O–13.1.1– R O–13.1.5. [4] D. R. Nelson, D. B. Barber , T . W . Mclain, and R. W . Beard, “V ector ﬁeld path following for miniature air vehicles, ” IEEE T rans. Robot. , vol. 23, no. 3, pp. 519–529, 2007. [5] S. Park, J. Deyst, and J. P . How , “Performance and Lyapunov stability of a nonlinear path following guidance method, ” J. Guid. Contr ol and Dynam. , vol. 30, no. 6, pp. 1718–1728, 2007. [6] G. Ambrosino, M. Ariola, U. Ciniglio, and F . Corraro, “Path generation and tracking in 3D for U A Vs, ” IEEE T rans. Contr ol Syst. T echnol. , vol. 17, no. 4, pp. 980–988, 2009. [7] S. Lee, A. Cho, and C. Kee, “Integrated waypoint path generation and following of an unmanned aerial vehicle, ” Aircr . Eng. Aer osp. T echnol. , vol. 82, no. 5, pp. 296–304, 2010. [8] P . B. Sujit, S. Saripalli, and J. Borges Sousa, “Unmanned aerial vehicle path following: A survey and analysis of algorithms for ﬁxed-wing unmanned aerial vehicles, ” IEEE Contr ol Syst. Mag. , vol. 34, no. 1, pp. 42–59, 2014. [9] I. Rhee, S. Park, and C.-K. Ryoo, “ A tight path following algorithm of an U AS based on PID control, ” in Proc. SICE Annu. Conf. , 2010, pp. 1270–1273. [10] Y . Q. Liang and Y . M. Jia, “Combined vector ﬁeld approach for 2D and 3D arbitrary twice differentiable curved path following with constrained U A Vs, ” J. Intell. Robot. Syst. , vol. 83, no. 1, pp. 133–160, 2016. [11] S. Zhao, X. W ang, Z. Lin, D. Zhang, and L. Shen, “Integrating vector ﬁeld approach and input-to-state stability curved path following for unmanned aerial vehicles, ” IEEE Tr ans. Syst., Man, and Cybern., Syst. , DOI: 10.1109/TSMC.2018.2839840. [12] S. Jackson, J. T isdale, M. Kamgarpour, B. Basso, and J. K. Hedrick, “T racking controllers for small UA Vs with wind disturbances: Theory and ﬂight results, ” in Pr oc. 47th IEEE Conf. Decision and Contr ol , 2008, pp. 564–569. [13] M. Ahmed and K. Subbarao, “Nonlinear 3-D trajectory guidance for unmanned aerial vehicles, ” in Pr oc. 11th Inter . Conf. Contr ol Autom. Robot. V ision , 2010, pp. 1923–1927. [14] C. Cao, N. Hovakimyan, V . V . Patel, I. Kaminer , and V . Dobrokhodov , “Stabilization of cascaded systems via L1 adaptive controller with application to a UA V path following problem and ﬂight test results, ” in Pr oc. American Control Conf. , 2007, pp. 1787–1792. [15] S. Zhao, X. W ang, D. Zhang, and L. Shen, “Curved path following control for ﬁxed-wing unmanned aerial vehicles with control constraint, ” J. Intell. Robot. Syst. , vol. 89, no. 1-2, pp. 107–119, 2018. [16] V . Roldao, R. Cunha, D. Cabecinhas, C. Silvestre, and P . Oliv eira, “ A leader-following trajectory generator with application to quadrotor formation ﬂight, ” Robot. Auton. Syst. , vol. 62, no. 10, pp. 1597–1609, 2014. [17] S.-M. Hung and S. N. Givigi, “ A Q-learning approach to ﬂocking with U A Vs in a stochastic environment, ” IEEE Tr ans. Cybern. , vol. 47, no. 1, pp. 186–197, 2017. [18] X. Jin, “Nonrepetitiv e leader-follower formation tracking for multiagent systems with LOS range and angle constraints using iterativ e learning control, ” IEEE T rans. Cybern. , vol. 49, no. 5, pp. 1748–1758, 2019. [19] N. H. Li and H. H. Liu, “Formation U A V ﬂight control using virtual structure and motion synchronization, ” in Proc. American Contr ol Conf. , 2008, pp. 1782–1787. [20] J. Ghommam, H. Mehrjerdi, M. Saad, and F . Mnif, “Formation path following control of unicycle-type mobile robots, ” Robot. Auton. Syst. , vol. 58, no. 5, pp. 727–736, 2010. [21] Q. Li and Z. P . Jiang, “Pattern preserving path following of unicycle teams with communication delays, ” Robot. Auton. Syst. , vol. 60, no. 9, pp. 1149–1164, 2012. [22] G. Antonelli, F . Arrichiello, and S. Chiaverini, “Flocking for multi-robot systems via the null-space-based behavioral control, ” Swarm Intell. , vol. 4, no. 1, p. 37, 2010. [23] J. W erfel, K. Petersen, and R. Nagpal, “Designing collective behavior in a termite-inspired robot construction team, ” Science , vol. 343, no. 6172, p. 754, 2014. [24] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “ A survey of multi-agent formation control, ” Automatica , vol. 53, pp. 424–440, 2015. [25] X. W ang, Z. Zeng, and Y . Cong, “Multi-agent distributed coordination control: De velopments and directions via graph viewpoint, ” Neur ocom- puting , vol. 199, pp. 204–218, 2016. [26] D. B. W ilson, A. H. Goktog an, and S. Sukkarieh, “V ision-aided guidance and navigation for close formation ﬂight, ” J. F ield Robot. , vol. 33, no. 5, pp. 661–686, 2016. [27] Z. Sun, H. G. de Marina, G. S. Seyboth, B. D. Anderson, and C. Y u, “Circular formation control of multiple unicycle-type agents with non- identical constant speeds, ” IEEE T rans. Contr ol Syst. T echnol. , vol. 27, no. 1, pp. 192–205, 2019. [28] T . H. Chung, M. R. Clement, M. A. Day , K. D. Jones, D. Davis, and M. Jones, “Live-ﬂy , large-scale ﬁeld experimentation for large numbers of ﬁxed-wing uavs, ” in Pr oc. IEEE Int. Conf. Robot. and Autom., ICRA , 2016, pp. 1255–1262. [29] A. Kushleye v , D. Mellinger , C. Powers, and V . Kumar, “T owards a swarm of agile micro quadrotors, ” Auton. Robot. , vol. 35, no. 4, pp. 287–300, 2013. [30] V . Cichella, R. Choe, S. B. Mehdi, and E. Xargay , “Safe coordinated maneuvering of teams of multirotor unmanned aerial vehicles: A coop- erativ e control framework for multi vehicle, time-critical missions, ” IEEE Contr ol Syst. Mag . , vol. 36, no. 4, pp. 59–82, 2016. [31] L. A. V . Reyes and H. G. T anner , “Flocking, formation control, and path following for a group of mobile robots, ” IEEE T rans. Control Syst. T echnol. , vol. 23, no. 4, pp. 1358–1372, 2015. [32] V . M. Gonalves, L. C. A. Pimenta, C. A. Maia, and G. A. S. Pereira, “Coordination of multiple ﬁxed-wing U A Vs trav ersing intersecting pe- riodic paths, ” in Pr oc. IEEE Int. Conf. Robot. and Autom., ICRA , 2013, pp. 849–854. [33] E. Xargay , I. Kaminer , A. Pascoal, N. Hovakimyan, V . Dobrokhodov , V . Cichella, A. P . Aguiar, and R. Ghabcheloo, “Time-critical cooperati ve path following of multiple unmanned aerial vehicles ov er time-varying networks, ” J. Guid. Control and Dynam. , vol. 36, no. 2, pp. 499–516, 2013. [34] Y . W ang, D. W ang, and S. Zhu, “Cooperative moving path following for multiple ﬁxed-wing unmanned aerial vehicles with speed constraints, ” Automatica , vol. 100, pp. 82–89, 2019. [35] D. Liberzon, Switching in systems and control . Boston, MA, USA: Springer Science & Business Media, 2003. [36] H. K. Khalil, Nonlinear systems . Upper Saddle Riv er , NJ, USA: Prentice-Hall, 2002. [37] I. Barkana, “Defending the beauty of the inv ariance principle, ” Int. J. Contr ol , vol. 87, no. 1, pp. 186–206, 2014. [38] J. Y ang, D. Y in, and L. Shen, “Reciprocal geometric conﬂict resolution on unmanned aerial vehicles by heading control, ” J. Gui. Contr ol and Dynam. , vol. 40, no. 10, pp. 2511–2523, 2017.

Coordinated Path Following Control of Fixed-wing Unmanned Aerial Vehicles

Original Paper

Comments & Academic Discussion

Leave a Comment

Original Paper

Related Papers

Comments & Academic Discussion

Leave a Comment