arXiv:1610.04391v2 [cs.SY] 7 Feb 2017 sesetadtafi o oes(IAVTRM)”. models flow traffic on-roa through and driving S assessment Traffic automated project of Cooperative benefits the to the under Vehicles Predicting Netherlands, Automated the Individual TTW, P “From NWO A. by of work supported The is 307207). (ERCStG- Council Research European yaisgnrlymk tipsil otakareference precis a more track accuracy; zero predefined to arbitrary unstable impossible with [10], it trajectory in make wit shown generally dealing As dynamics systems. when non- nonlinear e.g. especially be general limitations, An can fundamental that [9]. unav has paths able it method [8], this of However, is self-intersecting. systems range approach or broad smooth nonlinear tracking a trajectory of to the applicability class of broad solv advantage and obvious very theory control a in studied for extensively been has that niern fRsinAaeyo cecs(PERS n IT and RAS) E-mail: (IPME Russia. Sciences [email protected] Petersburg, of [email protected], Problem Academy St. for Institute Russian University, University with of Delft also at and Engineering Control Proskur Netherlands, and V. The Systems Anton Delft, for Center Netherlands. Delft The the Groningen, of University hc saseilcs fthe of following case reference-point special to a reduced poi is then reference which some is of steering functio trajectory a Path the as equivalently, treated or, thus time is path the time-parametrization; c the and categories. differ, similar major several are may in control classified following robots be path [7]. marine their of [6], of [5] models cars principles surface autonomous mathematical as and and robots such Although [4] ground robots, central underwater wheeled mobile the crafts, [3], of of vehicles one guidance is aerial automatic predefined following in a Path [2]. accurately problems [1], following path of geometric functionality the upon robots. applicabi wheeled real its with demonstrate experiments converg curve and by global performance integral algorithm establish and We an our path. such for desired along the conditions to robot it i the bringing controller steers motion lev which nonlinear t cur A a proposed integral and trajectory. whose is, the function GVF, to a this that converge design Using form, we function. model, implicit smooth kinematic its robot’s predefined a in of curve set smooth arbitrary an oo,mto oto,nniersystems nonlinear control, motion robot, olwn oto ftennoooi oierbtbsdon based the robot mobile of nonholonomic idea the of control following udn etrfil loih o ahfollowing path for algorithm field vector guiding A .Kptnu n .Coakoldeteprilspotfro support partial the acknowledge Cao M. and Kapitanyuk Y. uiKptnu n igCoaewt h NE nttt tt at institute ENTEG the with are Cao Ming and Kapitanyuk Yuri n praht te otedsrdpt st xits fix to is path desired the to steer to approach One built is robots mobile and industrial in applications Many Terms Index Abstract I hspprw rps nagrtmfrpath- for algorithm an propose we paper this —In udn etrfield vector guiding Pt olwn,vco edgiac,mobile guidance, field vector following, —Path oto fnnoooi oierobots mobile nonholonomic of control .I I. NTRODUCTION GF.Tedsrdpt a be may path desired The (GVF). uiA aiayk no .Pokrio n igCao Ming and Proskurnikov V. Anton Kapitanyuk, A. Yuri eeec tracking reference [email protected], ua behaviour human d fMechanical of s fTechnology, of aaeet- Management io swith is nikov roskurnikov TW#13712 problem then s the m ence of n oid- ely, MO lity ves the nt. an ed he he el h s , , oaodteedfclishsbe ugse n[5 n is and [15] way in possible suggested its A been or trajectory. has path desired difficulties the these the of avoid from to point far self-intersection is e.g a general, position when passes In opti- be determined error. robot uniquely real-time either be cross-track the cannot solve should the point or projection robot find the [7] the to problems sensors unique, mization special is with point equipped nearest Ev algorithm. the path-following the if on limitations of number a ah aea rf ftevhceadatao saturations actuator and t vehicle of the geometry of non-trivial drift lateral the path, by caused uncertainties, with with Pro cars [14]. tractors as based agricultural such and vehicles [13] fo unmanned trailers algorithms multiple complex to of leads pat control approach the the this along cross-tr speed of the vehicle’s development desired eliminate Further the that maintain [12] and autonomo [7], error the for [6], to controllers al vehicles following representation vector wheeled path This normal efficient point. design and projection to tangent the at the Th trajectory of error”). Serret-Fr “cross-track consisting the the in distance frame, represented as the is measure to model mathematical to referred robot’s capability (sometimes robot’s it the to and path, a on oe o a-ieweldrbt 6,[2,asmsthe assumes [12], [6], robots the wheeled of car-like existence for posed disturbances. fixed unpredictable perfectly to due point, a reference it with the “overtake” to may motion close robot precisel keep the a to even Attempting at direction. or following to path speed, instance, due either constant For guarantee “irregular” point. to quite reference impossible the be robo around can the oscillations trajectory practice its the in independ along Furthermore, positive motion design. uniformly controller is the error of tracking integral the goerc otolr teigterbtt h eie p desired the to f robot the separated steering usually controller, is “geometric” speed, longitudinal the maintaining b can designed tracking carefully trajectory by of removed limitations performance these oc eed nterbtssed The speed. is lif robot’s which the the profile, where on vehicles, speed depends this aerial constant force desired for [11]; a e.g. other maintain important variable” any extremely to control or allows speed additional freedom of forward an of function as degree time-free us a additional law some to than “flexible timing or becomes equations rather a thus implicit algorithm curve The its geometric parametrization. treat with a control dealing following as time, path path approach, the tracking the Unlike h eest omauetedsac otetakimposes track the to distance the measure to necessity The ieyue praht ahfloig rgnlypro- originally path-following, to approach used widely A [11] paper influential the in illustrated clearly been has As ldn mode sliding projection loihs[] 1]aecpbet cope to capable are [14] [7], algorithms on,ta s h lss point closest the is, that point, ahfollowing path dynamic algorithms. controller, jection- . lows enet tis it rom ting ath. ack the ent he en t’s us h. y e e e s 1 r . , 2 referred to as the “virtual target” approach. The Serret-Frenet frame, assigned to the projection point in the algorithm from [6], [12], can be considered as the body frame of a virtual target vehicle to be tracked by the real robot. A modification of this approach, offered in [15], allows the virtual target to have its own dynamics, taken as one of the controller’s design parameter. The design from [15] is based on the path following controller from [12], using the model representation Fig. 1. The structure of motion control systems in the Serret-Frenet frame and taking the geometry of the path into account. However, the controller from [15] implicitly involves target tracking since the frame has its own dynamics. integrator model a general vector-field algorithm for navigation Avoiding the projection problem, the virtual target approach along a general smooth curve in n-dimensional space has been thus inherits disadvantages of the usual target tracking. In discussed in [33]. However, for more realistic nonholonomic presence of uncertainties the robot may slow down and turn vehicles the vector-field algorithms have been studied mainly back in order to trace the target’s position [3]. for straight lines and circular paths [30], [31], [34], where they A guidance strategy of a human helmsman inspired another demonstrate better, in several aspects, performance compared path following algorithm, referred to as the line-of-sight (LOS) to other approaches [3]. Unlike [3], [30], [31], in this paper method [3], [5], [16] which is primarily used for air and marine we propose and analyze rigorously a vector-field algorithm crafts. Maintaining the desired speed of the robot, the LOS for guidance of a general nonholonomic robot along a general algorithm steers its heading along the LOS vector which starts smooth planar path, given in its implicit form. at the robot’s center of gravity and ends at the target point. The paper is organized as follows. The path following prob- This target is located ahead of the robot either on the path lem is set up in Section II. In Section III we design the guiding [3] or on the line, tangent to the path at the projection point vector field and discuss the properties of its integral curves. [5]. Unlike the virtual target approach, the target is always Section IV offers the path following control algorithm and chosen at a fixed prescribed distance from the robot, referred establishes its main properties. This algorithm is practically to as the lookahead distance. The maneuvering characteristics validated by experiments with wheeled robots, described in substantially depend on the lookahead distance: the shorter Section V. In Section VI we give a detailed comparison of our distance is chosen, the more “aggressive” it steers. A thorough algorithm with several alternative approaches to path following mathematical examination of the LOS method has been carried controllers design and also discuss its possible extensions. out in the recent paper [5], establishing the uniform semi- global exponential stability (USGES) property. Differential-geometric methods for invariant sets stabiliza- II. PROBLEM SETUP tion [8], [9], [17] have given rise to a broad class of “set- A widely used paradigm in path following control is to based” [18] path following algorithms. Treating the path as a decompose the controller into an “inner” and an “outer” geometric set, the algorithm is designed to make it invariant feedback loop [35]–[37] as illustrated in Fig. 1. The “inner” and attractive (globally or locally). Typically the path is dynamic controller is responsible for maintaining the vector considered as a set where some (nonlinear) output of the of generalized velocities (e.g. the longitudinal speed and turn system vanishes, and thus the problem of its stabilization boils rate) by controlling the vector of forces and moments. The down to the output regulation problem. To solve it, various design of the dynamic controller is based on the robot’s linearization techniques have been proposed [17], [19]–[25]. mathematical model and may include rejection of external For stabilization of a closed strictly convex curve an elegant disturbances, e.g. adaptive drift compensators [38]. Assuming passivity-based method has been established in [18]. the dynamic controller to be sufficiently fast and precise, one In this paper we develop a path following strategy, based may consider the speed and the turn rate to be new control on the idea of reference vector field. Vector field algorithms inputs, describing thus the robot with a simpler kinematic are widely used in collision-free navigation and extremum model. The path following algorithm is typically implemented seeking problems [26]–[29]; their efficiency in path following in the “outer” kinematic controller, steering the simplified problems has been recently demonstrated in [3], [30]–[32]. kinematic model to the prescribed path. A vector field is designed in a way that its integral curves The kinematic model of a mobile robot can be holonomic approach the path asymptotically. Steering the robot along the or nonholonomic. A holonomic robot is not restricted by integral curves, the control algorithm drives it to the desired its angular orientation and able to move in any direction, path. Unlike many path following algorithms, providing con- as exemplified by helicopters, wheeled robots with omni- vergence only in a sufficiently small vicinity of the desired directional wheels [2] and fully actuated marine vessels at low path, the vector field algorithm guarantees convergence of any speeds [16]. For nonholonomic robots only some directions of trajectory, which does not encounter the “critical” points where motion are possible. The simplest model of this type is the the vector field is degenerate. In particular, in any invariant unicycle, which can move only in the longitudinal direction domain without critical points the convergence to the path can while the lateral motion is impossible. Examples of robots, be proved. reducing under certain conditions to the unicycle-type kine- For holonomic robots described by a single or double matics, include differential drive wheeled mobile robots [13], 3
[39], fixed wing aircraft [30], [31], [36], marine vessels at As illustrated in Fig. 2, the plane R2 is covered by the cruise speeds [35] and car-like vehicles, whose rear wheels are disjoint level sets of the function ϕ, namely the sets where not steerable [13], [20], [39]. The most interesting are unicycle ϕ(x, y) = c = const. If (3) holds, then the vector n¯(x, y) is models where the speed is restricted to be sufficiently high, the normal vector to the corresponding level set at the point making it impossible to reduce the unicycle to a holonomic (x, y). The path P is one of the level sets, corresponding to model via the feedback linearization [40]. The lift force of a c =0; the value ϕ(x(t),y(t)) can be considered as a (signed) fixed-wing UAV, the rudder’s yaw moment of a marine craft “distance” from the robot to the path (differing, as usual, from and the turn rate of a car-like robot depend on the longitudinal the Euclidean distance), or the tracking error [17], [22]. More speed; at a low speed their manoeuvrability is very limited. generally, choosing an arbitrary strictly increasing function ψ ∈ C1(R → R) with ψ(0) = 0 (and thus ψ(s)s > 0 for A. The robot’s model any s 6=0), one may define the tracking error as follows In this paper, we consider the unicycle-type model where e(x, y) =∆ ψ[ϕ(x, y)] ∈ R. (4) the longitudinal velocity ur > 0 is a predefined constant By definition, e = 0 if and only if (x, y) ∈ P. Our goal is x˙ 2 cos α r¯˙ = = urm¯ (α) ∈ R , m¯ (α)= , to design a path following algorithm, i.e. a causal feedback y˙ sin α (1) law (x(·),y(·), α(·)) 7→ ω(·), which eliminates the tracking α˙ = ω. error |e(t)| −−−→ 0, bringing thus the robot to the predefined t→∞ Here r¯ is the position of the robot’s center of gravity C in the path P. Upon reaching the desired path, the algorithm should inertial Cartesian frame of reference 0XY , α is the robot’s “stabilize” the robot on it, which means that the robot’s orientation in this frame and m¯ (α) is the unit orientation vector heading is steered along the tangent vector to the path. (see Fig. 2). The only control input to the system (1) is the The mapping ψ(·) in (4) is a free parameter of the algo- angular velocity ω, and hence the system is underactuated. rithm. Formally one can get rid of this parameter, replacing ϕ by the composition ψ ◦ ϕ. However, it is convenient to distinguish between the path-defining function ϕ(x, y) and the tracking error, depending on the choice of ψ(s). The path m¯ n¯ representation is usually chosen as simple as possible: for α instance, dealing with a straight line, it is natural to choose τ¯ ϕ(x, y) = ϕ(x∗, y∗) linear ϕ(x, y), while the circular path is naturally described 2 2 by ϕ(x, y) = (x − x0) + (y − y0) . At the same time, y∗ ϕ(x, y) = const > 0
δ some mathematical properties of the algorithm, in particular, the region where convergence of the algorithm is guaranteed,
m¯ d depend on the way the tracking error is calculated. As will
Y P be discussed, it may be convenient to choose ψ(·) bounded : ϕ(x, y) = 0 ′ p v¯ with |ψ (s)| −−−−→ 0, for instance, ψ(s) = arctan s or ¯ |s|→∞ r∗ ψ(s)= |s|p sign s/(1 + |s|p) with p ≥ 1. The mentioned two − ¯ knen ϕ(x, y) = const < 0 functions, as well as the simplest function ψ(s) = s satisfy the condition, which is henceforth assumed to be valid sup ψ′(ψ−1(u)) < ∞. (5) u∈R
x∗ 0 C. Technical assumptions X Henceforth the following three technical assumptions are adopted, excluding some “pathological” situations. Our first Fig. 2. The robot orientation and level sets of the function ϕ(x,y) assumption enables one to use of the tracking error e(x, y) as a “signed distance” to the path P and implies that the asymptotic vanishing of the error e(¯r(t)) −−−→ 0 entails the convergence t→∞ B. The description of the desired path to the path in the usual Euclidean metric1 dist(¯r(t), P) → 0. The desired curvilinear path is the zero set of a function P Assumption 1. For an arbitrary constant κ > 0 one has ϕ ∈ C2(R2 → R), i.e. it is described by the implicit equation inf{|e(¯r)| : dist(¯r, P) ≥ κ} > 0. (6) P =∆ {(x, y): ϕ(x, y)=0}⊂ R2. (2) The same curve P may be represented in the implicit form Our second assumption provides the regularity condition (3) (2) in many ways. The principal restrictions imposed by our in a sufficiently small vicinity of P. approach is regularity [17]: in some vicinity of P one has 1 As usual, the distance from a point r¯0 to the path P A is dist(¯r0, P)= ⊤ r r r . More generally, the distance between sets A, B is ∆ ∂ϕ(x,y) ∂ϕ(x,y) inf{|¯0 − ¯| : ¯ ∈ P} n¯(x, y) = ∇ϕ(x, y)= ∂x ; ∂y 6=0. (3) defined as dist(A, B) = inf{|r¯1 − r¯2| :r ¯1 ∈ A, r¯2 ∈ B}. h i 4
Assumption 2. The set of critical points, where n¯ vanishes, A. Properties of the integral curves ∆ 2 We notice first that the vector field is 1-smooth, which C0 = {(x, y) ∈ R : ∇ϕ(x, y)=0}, v¯ C implies the local existence and uniqueness of solutions of the is separated from P by a positive distance dist(C0, P) > 0. system (10). Obviously, any point (x0,y0) ∈ C0 corresponds In the most typical cases where either P is a closed curve to the equilibrium of (10), being a trivial single-point integral or C0 is compact, Assumption 2 boils down to the condition curve. The uniqueness property implies that non-constant C0 ∩ P = ∅. Our final assumption is similar in spirit to integral curves are free of the critical points; the corresponding Assumption 1 and guarantees that the asymptotic vanishing solutions, however, may converge to C0 asymptotically. In of the normal vector n¯(r(t)) −−−→ 0 is possible only along a general, a solution may also escape to infinity in finite time. t→∞ The following lemma establishes the principal dichotomy trajectory, converging to C0, i.e. dist(¯r(t), C0) → 0. property of the integral curves, stating that any curve leads Assumption 3. For an arbitrary constant κ one has > 0 either to the desired path P or to the critical set C0. κ inf{|n¯(¯r)| : dist(¯r, C0) ≥ } > 0. (7) Lemma 2. Let ξ¯(t), t ∈ [0; t∗) be a maximally prolonged Assumption 3 implies the following useful technical lemma. solution to (10). Then two situations are possible 1) either dist(ξ¯(t), P) −−−→ 0, that is, the solution con- Lemma 1. Consider a Lipschitz vector-function r¯ : [0; ∞) → t→t∗ 2 verges to the desired path, R such that r¯(t) does not converge to C0 as t → ∞, 2) or t∗ = ∞ and dist(ξ¯(t), C0) −−−→ 0. that is, lim sup d(t) > 0 where d(t) = dist(¯r(t), C0). Then t→∞ t→∞ ∞ p Proof: The proof is based on the Lyapunov function 0 |n¯(¯r(t))| dt = ∞ for any p> 0. 1 R Proof: It can be easily noticed that d(·) is a Lipschitz V (x, y)= e(x, y)2 ≥ 0. (11) function, since as can be easily shown, |d(t1) − d(t2)| ≤ 2 |r¯(t1) − r¯(t2)|≤ M|t1 − t2| ∀t1,t2 ≥ 0, where M > 0 is the A straightforward computation shows that ∇e(ξ¯) = Lipschitz constant for r¯(·). Since lim sup d(t) > 0, a number ψ′(ϕ(ξ¯)) ∇ϕ(ξ¯)= ψ′(ψ−1(e(ξ¯)))|n¯(ξ¯)| and hence V is non- t→∞ ¯ ε> 0 and a sequence tk → ∞ exist such that d(tk) ≥ 2ε and, increasing along the trajectory ξ(t) since its derivative is −1 therefore, d(t) ≥ ε for t ∈ ∆k = [tk; tk + M ε]. Passing ′ −1 ⊤ (9) to subsequences, one may assume without loss of generality V˙ (ξ¯)= e(ξ¯)ψ (ψ (e(ξ¯)))¯n (ξ¯)¯v(ξ¯) = (12) −1 (9) that tk + M ε
convergence dist(ξ¯(t), C0) −−−→ 0, obviously, means that the The following lemma is dual to Lemma 2. t→∞ ∆ solution converges to a critical point: ξ¯∗ = lim ξ¯(t) ∈C0. Lemma 4. Let r¯(t), t ∈ [0; t∗) be a maximally prolonged t→∞ solution to (15). Then two situations are possible ¯ Definition 1. Let ξ∗ ∈ C0 be an equilibrium point of (10). 1) either dist(¯r(t), C0) −−−→ 0, t→t∗ The stable manifold of ξ¯∗, denoted by W (ξ¯∗), is the set of all 2) or t∗ = ∞ and dist(¯r(t), P) −−−→ 0 that is, the solution points ξ¯0 such that the solution of (10), starting at ξ(0) = ξ0, t→∞ ∆ converges to the desired path; exists for all t ≥ 0 and ξ¯∗ = lim ξ¯(t). t→∞ Proof: If t∗ < ∞ then the limit r¯∗ =r ¯(t∗ − 0)=r ¯(0)+ We will use the following corollary of the central manifold t∗ m¯ d(¯r(t))dt exists and, since the solution is maximally theorem (see e.g. Theorem 4.1 and Proposition 4.1 in [41]). 0 prolonged, one obviously has |n¯(¯r∗)| = 0, i.e. r¯∗ ∈ C0 andR statement 1 holds. The case of t = ∞ is considered Lemma 3. If both eigenvalues of the Jacobian matrix J(ξ¯∗)= ∗ ∂ ¯ ¯ ¯ similar to the proof of Lemma 2. Introducing the Lyapunov ¯v¯(ξ∗) are strict unstable Re λ1,2J(ξ∗) > 0, then W (ξ∗)= ∂ξ function (11), its derivative is shown to be {ξ¯∗}. If J(ξ¯∗) has at least one strictly unstable eigenvalue, ¯ ′ −1 ⊤ (9) then W (ξ) is a set of zero measure. V˙ (¯r)= ure(¯r)ψ (ψ (e(¯r)))¯n (¯r)¯md(¯r) = 2 Lemma 3 in turn has the following important corollary. (9) urkne(¯r) = − ψ′(ψ−1(e(¯r))) |n¯(¯r)|≤ 0. (16) 2 2 1+ kne(¯r) Corollary 1. Let the set C0 be finite and for any of its points ¯ ¯ ¯ 3 ξ∗ ∈ C0 the matrix e(ξ∗)H(ξ∗) has a negative eigenvalue. p Ψ(e(¯r)) Then the maximally prolonged solution of (10) (possibly, There exists| the limit e∗ {z= limt→∞ |e(¯r}(t))|. If e∗ = 0, existing on finite interval only) converges to P for almost all statement 2 holds thanks to Assumption 1. Otherwise, e > 0 ¯ ¯ ¯ ∗ initial conditions ξ(0). If e(ξ∗)H(ξ∗) < 0 for any ξ∗ ∈ C0, and Ψ(e(¯r(t))) is uniformly positive. In view of (16), one has this convergence takes place whenever ξ¯(0) 6∈ C0. ∞ 0 |n¯(¯r(t))|dt < ∞. Lemma 1 entails now statement 1. Proof: A straightforward computation shows that if RRemark 1. The equation (16) implies that V˙ ≈−knV |n¯|θ(e), ∇ϕ(ξ¯∗)=0 then J(ξ¯∗) = (E − kne(ξ¯∗))H(ξ¯∗) and where θ(e) → θ0 > 0 as e → 0. Assumptions 2 and 3 imply that κ as . Therefore, the desired path Tr J(ξ¯∗)= −kne(ξ¯∗) Tr H(ξ¯∗), |n¯(r)| ≥ e ≈ 0 P is locally exponentially attractive: if |e(0)| ≤ ε, where ε is det J(ξ¯ )=(1+ k2 e(ξ¯ )2) det H(ξ¯ ). ∗ n ∗ ∗ sufficiently small, then |e(t)| ≤ e−knur βt|e(0)|, where β > 0 It can be easily shown that a symmetric 2 × 2 matrix M is a constant, depending on ψ(·) and the normal vector n¯(x, y) is non-negatively definite M ≥ 0 if and only det M = in the vicinity of P. The coefficient kn is responsible for the λ1(M)λ2(M) ≥ 0 and Tr M = λ1(M) + λ2(M) ≥ 0. attraction to P. In the limit case kn = 0 the path P is not Here λi(M) stand for the eigenvalues of M. Since eH(e) asymptotically stable; the higher kn > 0 one chooses, the is not non-negatively definite, either det J < 0 or Tr J > stronger is the attraction to the path P in its small vicinity. 0, det J ≥ 0. In both situations, the matrix J(ξ¯ ) has at least ∗ Obviously, the integral curves of (15) are in one-to-one one strictly unstable eigenvalue. Furthermore, if eH(e) < 0 correspondence with non-equilibrium integral curves of (10). then Tr J > 0, det J > 0, i.e. both eigenvalues of J are strictly Corollary 1 can now be reformulated as follows. unstable. The statement now follows from Lemma 3. Corollary 2. Let the set C0 be finite and for any of its points ξ¯ ∈ C the matrix e(ξ¯ )H(ξ¯ ) has a negative eigenvalue. B. The GVF and the “ideal” motion of the robot ∗ 0 ∗ ∗ Then for almost all initial conditions r¯(0) the solutions of (15) The main idea of the path following controller, designed in can be prolonged up to ∞ and converge to P. If e(ξ¯∗)H(ξ¯∗) < the next section, is to steer the robot to the integral curve of 0 for any ξ∗ ∈C0, this holds for any r¯(0) 6∈ C0. the field (9). In other words, when the robot is passing a point r¯ = (x, y) ∈ R2, its desired orientation is Note that the conditions of Corollary 2 always hold for strictly convex function ϕ(x, y) (that is, H(x, y) > 0 at 1 any point) such that since the critical point (if it exists) is m¯ d(x, y)= v¯(x, y). (14) |v¯(x, y)| the unique point of global minimum. Therefore, at this point (x∗,y∗) one has ϕ(x∗,y∗) < 0 and thus e< 0, which implies The field of unit vectors m¯ d(x, y), henceforth referred to as the guiding vector field (GVF), is defined at any regular point that eH < 0. This is exemplified by the function 2 2 2 2 (x, y), where n¯ 6=0 and thus |v¯| = 1+ kne |n¯| 6=0. Fig. 2 (x − x0) (y − y0) ϕ(x, y)= 2 + 2 − 1, illustrates the relation between the vectors r¯, m¯ , τ¯, n¯, v¯, m¯ d. a b p Consider the desired motion of the robot, “ideally” oriented defining the elliptic path. Fig. 3 demonstrates the correspond- along the integral curves of the GVF at any point. Its position ing GVF with the unique critical point at the center of ellipse. vector r¯(t) obeys the differential equation This critical point is “repulsive” and no trajectory of (15) converges to it. Fig. 4 illustrates the GVF for the Cassini oval, ˙ r¯(t)= urm¯ d(¯r(t)), t ≥ 0. (15) being the zero level set of the non-convex function
3 4 4 2 2 2 4 4 Recall that H is a symmetric matrix, so its eigenvalues are real (x − x0) + (y − y0) − 2a ((x − x0) − (y − y0) )+ a − b . 6
⊤ multiplier ωd(t) can be found from ωd(t)= −m¯˙ d(t) Em¯ d(t). 700 It remains to prove that ωd(t) in fact depends only on the tra- 600 jectory (x(t),y(t), α(t)), and this dependence is continuous.
500 Introducing the vector field (9) v¯(t)=¯v(x(t),y(t)) and the tracking error e(t)= e(x(t),y(t)) along the robot’s trajectory, 400 a straightforward computation shows that
300 ⊤ d v¯ I2 v¯v¯ 200 m¯˙ = = − v,¯˙ d dt kv¯k kv¯k kv¯k3 100 (18) v¯˙ = ur[E − kneI2]H(x, y)¯m(α) − kne˙ n¯(x, y), 0 ′ ⊤ 0 200 400 600 800 1000 1200 e˙ = urψ (ϕ(x, y))n ¯(x, y) m¯ (α).
Here I2 is 2 × 2 identity matrix and H is the Hessian (13). Fig. 3. The GVF for an elliptic path 2 Since ϕ ∈ C , m¯˙ d and m¯ d continuously depend on the triple ⊤ (x(t),y(t), α(t)), the same holds for ωd = −m¯˙ d Em¯ d. To clarify the meaning of the function ωd, suppose for the 700 moment that the robot’s speed is ur =1. If the robot is moving 600 strictly along the integral curves of the GVF, then ωd is the signed curvature of the robot’s trajectory at its current position. 500 In general, ωd can be treated as the “desired” curvature of the 400 robot’s trajectory, which may differ from its real curvature.
300
200 B. The algorithm of GVF steering 100 As was discussed in the foregoing, the idea of the path
0 0 200 400 600 800 1000 1200 following algorithm is to steer the robot’s orientation along the guiding vector field m¯ d =m ¯ d(x, y). We introduce the directed angle δ = δ(x, y, α) ∈ (−π; π] between m¯ d and m¯ (see Fig. 4. The GVF for a Cassini oval. Fig. 2). The function δ is thus defined at any point (x, y, α) 1 and C -smooth at the points where m¯ (α) 6= −m¯ d(x, y). The orientation vector’s derivative along the trajectory is The corresponding set C0 consists of two “locus points” (x ± a,y ) and the “center” (x ,y ). In all these points d − sin α(t) 0 0 0 0 m¯˙ (t)= m¯ (α(t)) = ω(t) = −ω(t)Em¯ (t). e < 0. At the locus points one has H > 0, whereas at the dt cos α(t) center H has one positive and one negative eigenvalue, so the (19) robot potentially can be “trapped” at the center, but the set of On the other hand, m¯ may be decomposed (Fig. 2) as corresponding initial conditions has zero measure. m¯ = (cos δ)¯md − (sin δ)Em¯ d = [(cos δ)I2 − (sin δ)E]¯md, (20) IV. THE PATH FOLLOWING CONTROLLER DESIGN At any point where δ(t) < π and thus δ˙(t) exists, one obtains In this section, we design the controller, steering the robot to the GVF (14). This controller uses the GVF at (14) m¯ d(x, y) −ωEm¯ = m¯˙ = −δ˙ [(sin δ)I2 + (cos δ)E]¯md + the current robot’s position and the function ωd(x, y, α), mea- suring the GVF’s “rotation rate” along the robot’s trajectory. =Em¯ (17) ˙ + [(cos δ)I2 − (sin| δ)E]m¯˙ d {z= −δEm¯ −}
A. Preliminaries − ωd [(cos δ)I2 − (sin δ)E]Em¯ d = −(δ˙ + ωd)Em,¯ We start with the following technical lemma. =Em¯
Lemma 5. Let m¯ d(t)=m ¯ d(x(t),y(t)) stand for the GVF entailing the| following principal{z relation} between δ, ω and ωd along a trajectory of the robot. Then its derivative m¯˙ d(t) is δ˙ = ω − ωd. (21) m˙ d(t)= −ωd(x(t),y(t), α(t))Em¯ d(t), (17) 3 Furthermore, at any time the following equality is valid where ωd : R → R is continuous and uniquely determined by the functions ϕ(·), ψ(·) and the constant kn from (9). d d ⊤ sin δ = − m¯ Em¯ d = (ω − ωd)cos δ. (22) 2 dt dt Proof: Differentiating the equality |m¯ d(t)| = 1, one ⊤ shows that m¯˙ d(t) m¯ d(t) = 0, that is, m¯˙ d(t) ⊥ m¯ d(t). Supposing that δ(t0) = π and ω(t) − ωd(t) ≤ 0 for t ≈ ˙ π Therefore, the vector m¯ d(t) is proportional to the unit vector t0, equation (22) entails that sin δ ≥ 0 and hence δ = 2 + Emd(t), so that m¯˙ d(t) = −ωd(t)Em¯ d(t) and the scalar arcsin(sin δ) for t ∈ [t0; t0 + ε), where ε > 0 is sufficiently 7
small. In this situation, the function δ(x(t),y(t), α(t)) has the We are going to show that r¯∗ ∈C0, i.e. n¯(¯r∗)=0. Suppose, 4 + right derivative δ˙ = D δ, satisfying (21) as t ∈ [t0; t0 + ε). on the contrary, that |n¯(¯r∗)| > 0; therefore, |v¯(x(t),y(t))| We are now in a position to describe our path-following is uniformly positive on ∆∗. Using (18), this implies that algorithm, employing the GVF “rotation rate” ωd and the angle m¯˙ d(t), and hence ωd(x(t),y(t), α(t)) and ω(t) are uniformly of discrepancy between the GVF and the robot’s orientation δ bounded on ∆∗. Thus there exists the finite limit
t∗ ω(t)= ωd(x(t),y(t), α(t)) − kδδ(x(t),y(t), α(t)). (23) α∗ = lim α(t)= α(0) + ω(t)dt, t→t∗ Here kδ > 0 is a constant, determining the convergence rate. Z0 When δ(x(t),y(t), α(t)) < π, equality (21) holds and thus enabling one to define the solution at t = t∗ and then to prolong it to [t∗,t∗ + ε), i.e. the solution is not maximally δ˙ = ω − ωd = −kδδ. (24) prolonged. This contradiction proves that n¯(¯r∗)=0. Furthermore, even for δ(x(0),y(0), α(0)) = π one has Note that if δ(0) = 0, that is, the robot was perfectly ω − ωd = −kδδ < 0 as t ≈ t0 and hence (24) retains its oriented along the GVF at the starting moment, (21) implies validity at t = 0, treating δ˙ as the right derivative D+f. that δ(t) ≡ 0 so that the robot follows the integral curve of Thus, considering δ˙ as a new control input, the algorithm (21) the GVF. As was shown in Section III, in this “ideal” situation is equivalent to a very simple proportional controller (24), the robot either approaches the desired path P or is driven to providing, in particular, that δ(x(t),y(t), α(t)) < π ∀t> 0. one of the critical points. The latter situation is practically impossible if the conditions of Corollary 2 are valid since the C. Local existence and convergence of the solutions set of integral curves, leading to the set C0, has zero measure. In this subsection we examine the properties of the solutions One may consider (25) as a system with slow-fast dynam- of the closed-loop system (1), (23), rewritten as follows ics. Informally, the controller (24) provides the exponential convergence of the robot to an integral curve of the GVF; x˙(t)= ur cos α(t), y˙(t)= ur sin α(t), (25) after this “fast” transient process, the robot “slowly” follows α˙ (t)= ωd(x(t),y(t), α(t)) − kδδ(x(t),y(t), α(t)). this integral curve and approaches the desired trajectory, The right-hand side of (25) is continuous at any point unless it is “trapped” in a critical point. Ignoring the “fast (x0,y0, α0), where n¯(x0,y0) 6=0 and δ0 = δ(x0,y0, α0) < π. dynamics”, one may suppose that the statement of Lemma 4 However, the discontinuity at the points where δ0 = π makes remains valid for a general solution of the system (25). This the usual existence theorem [42] inapplicable. To avoid this argument, however, is not mathematically rigorous. Recalling problem, we consider the equivalent “augmented” system the proofs of Lemmas 2 and 4, one may notice that the central argument was the non-increasing property of the Lyapunov x˙(t)= u cos α(t), y˙(t)= u sin α(t), r r function (11). Although the deviation of the robot from the α˙ (t)= ωd(x(t),y(t), α(t)) − kδδ∗(t), (26) integral curve exponentially decreases due to (24), the non- δ˙∗(t)= −kδδ∗(t). increasing property, in general, fails; as will be shown below, As was discussed in the previous subsection, any solution even if the robot is positioned very close to the desired path, the tracking error may increase. However, this effect does not of (25) satisfies (26) with δ∗(t) = δ(x(t),y(t), α(t)), and destroy the dichotomy property (any solution converges either vice versa: choosing a solution (x(t),y(t), α(t),δ∗(t)), where to P or to C0) under the following assumption, which usually δ∗(0) = δ(x(0),y(0), α(0)) ∈ (−π; π], one has δ∗(t) = δ(x(t),y(t),z(t)) for any t ≥ 0 due to (24). Unlike (25), the holds in practice, being valid e.g. if ϕ(·) is a polynomial. right-hand side of (26) is a 1-smooth function of C (x,y,α,δ∗) Assumption 4. There exist θ ∈ (0; kδ) and C > 0 such that at any point where n¯(x, y) 6= 0. The standard existence and uniqueness theorem [42] implies the following lemma. |n¯(¯r)|≤ Ceθ|r¯| as |r¯| → ∞. (27)
Lemma 6. For any point ζ0 = (x0,y0, α0), such that The latter condition can be relaxed in the case of a closed n¯(x0,y0) 6= 0, there exists the unique solution ζ(t) = path; however, we adopt it to consider both bounded and (x(t),y(t), α(t)) with ζ(0) = ζ0. Extending this solution to unbounded paths in a unified way. Note that if ϕ(x, y) is a the maximal existence interval , one either has [0; t∗) t∗ = ∞ polynomial function, then kδ > 0 can be arbitrarily small. or t∗ < ∞ and (x(t),y(t)) −−−→ (x∗,y∗) ∈C0. t→t∗ Henceforth all Assumptions 1-4 are supposed to be valid. Proof: Reducing the Cauchy problem for the closed-loop The following theorem, extending Lemma 4 to the case system (25) to the Cauchy problem for (26), one shows that the where δ(0) 6=0, is our first main result. solution exists locally and is unique [42]. Let its maximally Theorem 1. For any maximally prolonged solution ∆ prolongable solution (x(t),y(t), α(t)) be defined on ∆∗ = (x(t),y(t), α(t)) of (25), defined for t ∈ [0; t∗), one of [0; t∗) with t∗ < ∞. Since |r¯˙(t)| = ur, the limit exists the following statements holds: t∗ 1) either t∗ = ∞ and dist(¯r(t), P) −−−→ 0, or t r¯∗ = lim r¯(t)=¯r(0) + r¯˙(t)dt. →∞ t→t∗ 2) dist(¯r(t), C ) → 0 as t → t . Z0 0 ∗ 4 By definition, the right derivative of a function f(t) at t = t0 (written In other words, for any initial condition the algorithm drives ′ + + f(t)−f(t0) f (t0 + 0) or D f(t0)) is defined by D f(t0)= lim − . the robot to either the desired path P or C . t→t0+0 t t0 0 8
Proof: In the case where t∗ = ∞ statement 2 holds criterion requires the initial condition (x(0),y(0), α(0)) to due to Lemma 6. Suppose that t∗ = ∞. Differentiating the belong to some invariant set, free of critical points. Similar function (11) along the trajectories, it can be shown that restrictions arise in most of the path following algorithms; for example, in the projection-based algorithms the convergence ˙ ′ −1 ⊤ (20) V = ureψ (ψ (e))¯n m¯ = can be rigorously proved only in some region of attraction (20) ′ −1 ⊤ (14) where the projection to the desired curve is well defined [14]. = ureψ (ψ (e))¯n [¯md cos δ − Em¯ d sin δ] = ′ −1 Assumptions 1 and 2 imply the uniform positivity of the (14) ureψ (ψ (e)) ⊤ (9) = n¯ [¯v cos δ − Ev¯ sin δ] = error on C0, that is, the following inequality holds |v| (28) ′ −1 e = inf{|e(x, y)| : (x, y) ∈C } > 0. (30) (9) ureψ (ψ (e)) c 0 = |n¯|(−kne cos δ + sin δ)= 2 2 1+ kne Consider the following set = Φ(e)|n¯|(−kne cos δ + sin δ). p M = {(x, y, α):¯n 6=0, |δ| < arctan(knec), |e|
D. An invariant set, free of critical points |δ(x(0),y(0), α(0))| < arctan(knec) < π/2. (33)
In this subsection we give a sufficient condition, guaran- Since ec > 0, (33) is valid for sufficiently large kn whenever teeing that a solution of (25) does not converge to C0. This |δ| < π/2. In other words, Theorem 2 guarantees convergence 9
to the path from any starting position with |e(0)| < ec and |δ(0)| < π/2, choosing large kn. Furthermore, if the desired path P is closed and the direction of circulation along it is unimportant, the condition |δ(0)| < π/2 can be provided by reverting the vector field m¯ d 7→ −m¯ d (which corresponds to the replacement of ϕ 7→ −ϕ and |δ| 7→ π − |δ|) unless δ(0) = ±π/2 (in practice it is never possible). Remark 3. Even if (33) is violated at the starting time −1 t = 0, it obviously holds when t>t0 = kδ [ln |δ(0)| − ln arctan(knec)] thanks to (24). Thus, if one is able to prove that the solution is prolongable up to t0 and |e(x(t0),y(t0))| < ec, Theorem 2 provides the convergence Fig. 5. The E-puck robot with marker on top, used in experiments of the robot’s position to the desired path. Remark 3 suggests the way to relax the restriction on the initial robot’s orientation (33). Since the robot moves at the Condition (36) can often be provided under an appropriate 2 constant speed u > 0, it covers the distance u t until its choice of ψ(·). For instance, when ϕ(x, y) = (x−x0) +(y − r r 0 2 2 orientation satisfies (33). The path following is guaranteed if y0) − R (circular path), one can choose ψ(s) = arctan s so ′ 2 β u t is less than the viability distance of the initial position, that ψ (s)=1/(1 + s ). More generally, if |ϕ(¯r)| ≥ C1|r¯| r 0 γ and |∇ϕ(r)| ≤ C2|r¯| as |r¯| → ∞, then ∇e is globally i.e. the distance from it to the set where |e|≥ ec. bounded; we choose ψ(s) = arctan(sp), where p > 0 is Definition 2. Given a point r¯0 = (x0,y0) with |e(¯r0)| < ec, sufficiently large so that p − 1+ γ ≤ 2βp. the number d0 = inf{dist(¯r0, r¯): e(¯r) ≥ ec} > 0 is said to be its viability distance. V. EXPERIMENTAL VALIDATION Theorem 2 and Remark 3 yield in the following. In this section the experiments with real robots are reported. In one of these experiments, the desired path is an ellipse, Corollary 4. Let the initial position of the robot r¯(0) = and the other experiments deals with the more sophisticated (x(0),y(0)) with e(¯r(0)) < ec have the viability distance Cassini oval (see Section III). d0 > 0. If this viability distance satisfies the condition We test the results using the E-puck mobile robotic plat- ur |δ(x(0),y(0), α(0))| form [43]. The experimental setup consists of the differential d0 > ln , (34) kδ arctan(knec) wheeled E-puck robot, a server computer, an overhead camera and a communication module. The robot is identified by a then the solution of (25) gets into the set M in finite time, data matrix marker on its top (see Fig. 5). The position and is prolongable up to ∞ satisfying dist(¯r(t), P) −−−→ 0. t→∞ of the central point and the orientation of the marker are Note that (34) holds for any α(0) under the assumption recognized by a vision algorithm running at a control computer ur π connected to the overhead camera. The available workspace d0 > ln . (35) is a planar area of 2.6x2 meters covered by the image of kδ arctan(knec) 1280x720 Px (henceforth the acronym Px is used for pixels). The condition (35) gives an estimate for the region, starting in For convenience we operate with pixels as coordinates. The which the robot necessarily converges to the desired path P. PC runs real-time calculation of the control actions based on Taking the original orientation relative to the field into account, the pose information of the robot and computes the control this estimate can be tightened by using (34). Typically d is 0 inputs for the robot. The results of computation as the desired uniformly positive in the vicinity of P (this holds, for instance, angular and linear velocities of the robot are translated into when P is a closed curve). Thus (35) guarantees the algorithm the commands for its left and right wheels. The commands to converge in the vicinity of P, choosing k /u sufficiently δ r from the control computer to the robot are sent via Bluetooth large. As was mentioned, practical experiments with natural at the fixed frequency of 20 Hz. trajectories, satisfying the assumptions of Corollary 2, show We verify the guidance algorithm for two closed trajectories, that the robot is always attracted to the desired path, although passed by the robot in the clockwise direction. In both cases the mathematical proof of this remains a non-trivial task. the forward velocity was set to ur = 50 Px/s, and we set the Remark 4. Although explicit calculation of the viability dis- parameters of the algorithm kn = 3 and kδ = 2. We choose tance is complicated, conditions (34) and (35) in fact require ψ(s) = s, so that e(x, y) = ϕ(x, y). Below we describe the only its lower bound. Such a bound can be explicitly obtained, trajectories and show the numerical data from the experiments. for instance, if the error is Lipschitz A. Circulation along the ellipse |∇e(¯r)| = ψ′(ϕ(¯r))|n¯(¯r)|≤ c = const ∀x, y. (36) For our first experiment, we choose the elliptic trajectory, If the condition (36) holds, then |e(¯r) − e(¯r0)|≤ c|r¯− r¯0| and defined by the function hence the viability distance of r¯0 is estimated as follows 2 2 (x − x0) (y − y0) 2 ϕ(x, y)= ks + − R . (37) d0 ≥ (ec − e(x0,y0))/c. p2 q2 10
Obviously, the level curves of ϕ are ellipses. The path P the ellipse and sufficiently far from it, then it enters into the corresponds to the ellipse with semiaxes pR and qR, centered domain (39) with a “proper” orientation, satisfying (33), and at the point (x0,y0). thus the problem of path following is solved. The practical For the experiment we choose x0 = 600Px, y0 = 350Px, experiments, however, show that the robot converges to the R = 400Px, and the semiaxes scale factors p =1, q =0.5 (see path from any point, except for the ellipse’ center, independent Fig. 6). To provide ϕ(x, y) ∈ [−5; 5] in the working area, we of the initial robot’s orientation, whereas the mathematically −5 choose the scaling factor ks = 10 . The robots’s trajectories rigorous proof remains an open problem. (labeled a, b, c and d) under four different initial conditions are shown in Fig. 6. The respective starting conditions are B. Circulation along the Cassini oval a : (x = 472,y = 311, α =0.0768), For our second experiment the path chosen is referred to as b : (x = 30,y = 555, α =0.0278), (38) the Cassini oval (Fig. 8) and determined by the function c : (x = 408,y = 369, α =2.1515), 2 ϕ(x, y)= k ∆x2 + ∆y2 − 2q2 ∆x2 − ∆y2 − p4 + q4 , d : (x = 78,y = 133, α =4.0419) s h i (the coordinates x, y are in pixels, and α is in radians). Fig. 7 ∆x = (x − x0), ∆y = (y − y0). illustrates the dynamics of the tracking error ϕ(x(t),y(t)). −10 We choose here ks = 10 , p = 330Px and q = 300Px. The oval is centered at (x0,y0) = (600, 350)Px. Fig. 8 illustrates four trajectories (labeled , , and ), 700 a b c d a corresponding to the initial conditions b 600 c d a : (x = 233,y = 184, α =2.9287), 500 P b : (x = 106,y = 202, α =4.2487), 400 (40) c : (x = 355,y = 343, α =5.4071), Y, Px 300 d : (x = 503,y = 619, α =0.1022). 200 (the coordinates x, y are in pixels, and α is in radians). The 100 corresponding tracking errors are displayed in Fig. 9. 0 0 200 400 600 800 1000 1200 X, Px
700 Fig. 6. Elliptical path: the vector field and robot’s trajectories. a b 600 c d 500 P
400
Y, Px 300
200
100
0 0 200 400 600 800 1000 1200 X, Px
Fig. 8. Cassini oval: the vector field and robot’s trajectories.
Fig. 7. Elliptical path: the dynamics of the tracking error e = ϕ(x,y).
In this example the set of critical points is C0 = {(x0,y0)}, 2 2 which corresponds to e(x0,y0)= −ksR and hence ec = R . The set {(x, y): |e(x, y)| As discussed in Section III, the set of critical points is C0 = The second path following algorithm to be compared with 2 2 2 {(x0 ± q,y0), (x0,y0)}. If q < p < 2q , such inequalities the GVF method is the “nonlinear guidance law” (NGL) [3], being valid in our example, then it can be checked that ec = [44]. At the current robot position r¯, draw a circle of radius 4 4 ′ |e(x0,y0)| = p − q , and the set {(x, y): |e(x, y)| 500 400 ωR = −kR (α − αR) , 300 The constants R> 0, kR > 0 are the algorithm’s parameters. 200 For the sake of comparison, we will use the same elliptic 100 path, determined by the function (37) and the same parameters 0 of the GVF algorithm as in Subsect. V-A. The initial position -100 and heading of the robot are 0 200 400 600 800 1000 1200 x = 200; y = 450; α =0.0278. Fig. 10. Cassini oval: the set {(x,y): |e(x,y)| ωLOS = c(P )ur − kLOS (α − αLOS) , where is a constant gain and is the curvature Fig. 11. Geometric objects, employed by the LOS (red) and the NGL (blue) kLOS > 0 c(P ) path following algorithms of the curve P at the projection point P . The parameters of the algorithm are the lookahead distance ∆ and the gain kLOS. 12 40 35 Guiding Vector Field 30 LOS Path Following 25 Nonlinear Guidance Law 20 15 10 5 0 -5 0 5 10 15 20 25 Fig. 12. The dynamic of the euclidean distance d B. The previous works on GVF algorithms Fig. 13. The desired path as the intersection of two surfaces The vector-field path following algorithms have been mainly considered for straight lines and circular paths [3], [31]. The GVF designed for these paths is the same as constructed in Section III; the only difference is the path following controller: been reported in [24], [30]. The desired curvilinear path unlike [31], in this paper we do not consider sliding-mode P in the three dimensional space can be describing as an algorithms and confine ourselves to simple linear controllers. intersection of two surfaces (Fig. 13) However, a general approach to the Lyapunov-based GVF de- ∆ R3 sign has been suggested in [30]. The desired path is supposed P = {(x,y,z): ϕ1(x,y,z)=0 ∧ ϕ2(x,y,z)=0}⊂ , to be the set of global minima of some predefined Lyapunov 2 R3 R function; e.g. for a path (2), our usual Lyapunov function (11) where ϕi ∈ C ( → ),i = 1, 2. The main design idea can be chosen. Departing from this Lyapunov function, a is that if the robot simultaneously approaches both surfaces, broad class of GVF has been suggested, including (14) as eventually it will be brought to the prescribed path P. Thus, a special case. The paper [30] also offers a methodology of we may define the guiding vector field by proving the convergence of integral curves to the desired path v¯ =τ ¯ − k e n¯ − k e n¯ , τ¯ =n ¯ × n¯ (considered in our Section III) and design of path following n1 1 1 n2 2 2 1 2 controllers. For the case of unicycle-like robot the control where n¯i = ∇ϕi and ei = ϕi(x,y,z) are the “tracking errors” algorithm, suggested in [30, Eq. (35)] is equivalent to our (i = 1, 2). Subsequent design of the controller, steering the controller (24). Describing a general framework, the paper [30] robot to the GVF, is similar to the planar case (two angles are focuses however on some specific problems of flight control, controlled instead of a single angle). The technical analysis namely, following a loiter circle (planar circle at the fixed of this algorithm requires however some extra assumptions of altitude) or some other planar path, which can be obtained non-degeneracy and is beyond the scope of this paper; the from a loiter circle by a smooth “warping” transformation, convergence rate of the path following algorithm appears to preserving the vector field’s properties. Many assumptions be very sensitive to the choice of ϕ1 and ϕ2. adopted in [30] are restrictive, e.g. the Lyapunov function Third, the results can be extended to time-varying vector should be radially unbounded, excluding straight lines and fields [30], allowing to work with some types of transfor- other unbounded paths from the consideration. The analysis mations (such as translation, rotation and scaling) of the of the closed-loop system in [30] is not fully rigorous as it predefined trajectory and enabling to use the GVF approach completely ignores the problem of the solution existence up for stand-off tracking of slowly moving targets [45]. to ∞, which becomes non-trivial when C 6= ∅. 0 Fourth, it is important to consider external disturbances, Unlike the previous papers [3], [30], [31], we consider a unavoidable in practice and leading, in particular, to the lateral general smooth path (2), which can be closed or unbounded; drift of the robot. Some preliminary results on using draft for closed trajectories, we do not restrict the Lyapunov func- compensators in parallel with the path following controller tion to be radially unbounded. We provide a mathematically are reported in the conference papers [34], [46]. rigorous analysis of our algorithm and specify an explicit set of initial conditions, starting at which the robot evades the critical points and converges to the desired trajectory. VII. CONCLUSIONS In this paper, we offer a new algorithm for path following C. Further extensions of our GVF method control of nonholonomic robots exploiting the idea of a The results presented in the paper can be extended in several guiding vector field. Unlike the existing results, the desired directions. First, the idea of GVF steering can be applied path can be an arbitrary smooth curve in its implicit form, i.e. to more general models than the simplest unicycle robot, the zero set of a smooth function. We examine mathematical considered in this paper. properties of the algorithm and give global conditions for fol- Second, the algorithm can be extended to the three- lowing asymptotically the desired path. The results have been dimensional case. Some special results in this direction have experimentally validated, using the E-puck wheeled robots. 13 REFERENCES [26] D. Panagou and V. Kumar, “Cooperative visibility maintenance for leader-follower formations in obstacle environments,” IEEE Trans. [1] T. Kurfess, Ed., Robotics and Automation Handbook. CRC Press, 2005. Robotics, vol. 30, no. 4, pp. 831–844, 2014. [27] M. Hoy, A. Matveev, and A. V. Savkin, “Algorithms for collision- [2] B. Siciliano and O. Khatib, Eds., Springer Handbook of Robotics. Springer, 2008. free navigation of mobile robots in complex cluttered environments: a survey,” Robotica, vol. 33, no. 3, pp. 463–497, 2015. [3] P. Sujit, S. Saripalli, and J. Borges Sousa, “Unmanned aerial vehicle path [28] A. S. Matveev, K. S. Ovchinnikov, and A. V. Savkin, “A method of following: A survey and analysis of algorithms for fixed-wing unmanned reactive 3d navigation for a tight surface scan by a nonholonomic mobile aerial vehicless,” IEEE Control Systems, vol. 34, no. 1, pp. 42–59, 2014. robot,” Automatica, vol. 75, pp. 119–126, 2017. [4] E. Peymania and T. I. Fossen, “Path following of underwater robots using [29] A. S. Matveev, M. C. Hoy, and A. V. Savkin, “Extremum seeking Lagrange multipliers,” Robotics and Autonomous Systems, vol. 67, pp. navigation without derivative estimation of a mobile robot in a dynamic 44–52, 2015. environmental field,” IEEE Transactions on Control Systems Technology, [5] T. I. Fossen and K. Y. Pettersen, “On uniform semiglobal exponential vol. 24, no. 3, pp. 1084–1091, 2016. stability (USGES) of proportional line-of-sight guidance laws,” Auto- [30] D. A. Lawrence, E. W. Frew, and W. J. Pisano, “Lyapunov vector fields matica, vol. 50, no. 11, pp. 2912 – 2917, 2014. for autonomous unmanned aircraft flight control,” Journal of Guidance, [6] C. Samson, “Path following and time-varying feedback stabilization of Control, and Dynamics, vol. 31, no. 5, pp. 1220–1229, 2008. a wheeled mobile robot,” in Proc. of the ICARCV’92, Singapore, 1992. [31] D. Nelson, D. Barber, T. McLain, and R. Beard, “Vector field path [7] J. Ackermann, J. Guldner, W. Sienel, R. Steinhauser, and V. I. Utkin, following for miniature air vehicles,” IEEE Trans. Robotics, vol. 23, “Linear and nonlinear controller design for robust automatic steering,” no. 3, pp. 519–529, June 2007. IEEE Trans. Control Syst. Tech., vol. 3, no. 1, pp. 132–143, 1995. [32] L. Valbuena Reyes and H. Tanner, “Flocking, formation control, and [8] A. Isidori, Nonlinear Control Systems, 3rd ed., M. Thoma, E. D. Sontag, path following for a group of mobile robots,” IEEE Trans. Control Syst. B. W. Dickinson, A. Fettweis, J. L. Massey, and J. W. Modestino, Eds. Tech., vol. 23, no. 4, pp. 1268–1282, July 2015. Secaucus, NJ, USA: Springer-Verlag New York, Inc., 1995. [33] V. Goncalves, L. Pimenta, C. Maia, B. Dutra, and G. Pereira, “Vector [9] L. Marconi and A. Isidori, “Mixed internal model-based and feedforward fields for robot navigation along time-varying curves in n-dimensions,” control for robust tracking in nonlinear systems,” Automatica, vol. 36, IEEE Trans. Robotics, vol. 26, no. 4, pp. 647–659, Aug 2010. no. 7, pp. 993 – 1000, 2000. [34] R. Beard, J. Ferrin, and J. Humpherys, “Fixed wing UAV path following [10] M. M. Seron, J. H. Braslavsky, P. V. Kokotovic, and D. Q. Mayne, in wind with input constraints,” IEEE Trans. Control Syst. Tech., vol. 22, “Feedback limitations in nonlinear systems: From Bode integrals to no. 6, pp. 2103–2117, Nov 2014. cheap control,” IEEE Trans. Autom. Control, vol. 44, no. 4, pp. 829–833, [35] T. I. Fossen, Guidance and control of ocean vehicles. John Wiley & 1999. Sons, Ltd, 1994. [11] A. Aguiar, J. Hespanha, and P. Kokotovic, “Path-following for nonmin- [36] R. W. Beard and T. W. McLain, Small unmanned aircraft: Theory and imum phase systems removes performance limitations,” IEEE Trans. practice. Princeton university press, 2012. Autom. Control, vol. 50, no. 2, pp. 234–239, 2005. [37] A. Tsourdos, B. White, and M. Shanmugavel, Cooperative Path Plan- [12] A. Micaelli, C. Samson et al., “Trajectory tracking for unicycle-type and ning of Unmanned Aerial Vehicles, ser. Aerospace Series. Wiley, 2010. two-steering-wheels mobile robots,” Tech. Rep. No. 2097, INRIA, 1993. [38] T. I. Fossen, K. Y. Pettersen, and R. Galeazzi, “Line-of-sight path [13] C. Samson, “Control of chained systems application to path following following for Dubins paths with adaptive sideslip compensation of drift and time-varying point-stabilization of mobile robots,” IEEE Trans. forces,” IEEE Trans. Control Syst. Tech., vol. 23, no. 2, pp. 820–827, Autom. Control, vol. 40, no. 1, pp. 64–77, 1995. 2015. [14] A. S. Matveev, M. Hoy, J. Katupitiya, and A. V. Savkin, “Nonlinear [39] P. Morin and C. Samson, “Trajectory tracking for non-holonomic sliding mode control of an unmanned agricultural tractor in the presence vehicles: overview and case study,” in Proc. of 4th Int. Workshop on of sliding and control saturation,” Robotics and Autonomous Systems, Robot Motion and Control RoMoCo’04., June 2004, pp. 139–153. vol. 61, no. 9, pp. 973 – 987, 2013. [40] G. Oriolo, A. D. Luca, and M. Vendittelli, “WMR control via dynamic [15] D. Soetanto, L. Lapierre, and A. Pascoal, “Adaptive, non-singular path- feedback linearization: Design, implementation, and experimental vali- following control of dynamic wheeled robots,” in Proc. of 42nd IEEE dation,” IEEE Trans. Control Syst. Tech., vol. 10, no. 6, pp. 835–852, Conf. on Decision and Control, vol. 2, Dec 2003, pp. 1765–1770 Vol.2. 2002. [16] M. Breivik and T. Fossen, “Principles of guidance-based path following [41] R. Potrie and P. Monzo’n, “Local implications of almost global stability,” in 2D and 3D,” in Proc. of 44th IEEE Conf. on Decision and Control Dynam. Syst., vol. 24, no. 1, pp. 109–115, 2009. and European Control Conf. CDC-ECC’05., Dec 2005, pp. 627–634. [42] H. Khalil, Nonlinear systems. Englewood Cliffs, NJ: Prentice-Hall, 1996. [17] A. Fradkov, I. Miroshnik, and V. Nikiforov, Nonlinear and Adaptive [43] F. Mondada, M. Bonani, X. Raemy, J. Pugh, C. Cianci, A. Klaptocz, Control of Complex Systems, ser. Mathematics and Its Applications. S. Magnenat, J.-C. Zufferey, D. Floreano, and A. Martinoli, “The e- Springer, 1999. puck, a robot designed for education in engineering,” in Proc. of the 9th [18] M. I. El-Hawwary and M. Maggiore, “Case studies on passivity-based Conf. on Auton. Robot Syst. and Competitions, 2009, pp. 59–65. stabilisation of closed sets,” International Journal of Control, vol. 84, [44] S. Park, J. Deyst, and J. P. How, “Performance and lyapunov stability no. 2, pp. 336–350, 2011. of a nonlinear path following guidance method,” Journal of Guidance, [19] A. Hladio, C. Nielsen, and D. Wang, “Path following for a class of Control, and Dynamics, vol. 30, no. 6, pp. 1718–1728, 2007. mechanical systems,” IEEE Trans. Control Syst. Tech., vol. 21, no. 6, [45] H. Oh, S. Kim, H. Shin, and A. Tsourdos, “Coordinated standoff tracking pp. 2380–2390, Nov 2013. of moving target groups using multiple UAVs,” IEEE Transactions on [20] A. Akhtar, C. Nielsen, and S. Waslander, “Path following using dy- Aerospace and Electronic Systems, vol. 51, no. 2, pp. 1501–1514, April namic transverse feedback linearization for car-like robots,” IEEE Trans. 2015. Robotics, vol. 31, no. 2, pp. 269–279, April 2015. [46] H. G. de Marina, Y. A. Kapitanyuk, M. Bronz, G. Hattenberger, and [21] S. Kolyubin and A. Shiriaev, “Dynamics-consistent motion planning for M. Cao, “Guidance algorithm for smooth trajectory tracking of a fixed underactuated ships using virtual holonomic constraints,” in Proc. of wing UAV flying in wind flows,” accepted to IEEE Int. Conf. Robotics IEEE OCEANS 2014, Sept 2014, pp. 1–7. and Automation (ICRA), 2017. [22] Y. Kapitanyuk and S. Chepinsky, “Control of mobile robot following a piecewise-smooth path,” Gyroscopy and Navigation, vol. 4, no. 4, pp. 198–203, 2013. [23] Y. Kapitanyuk, S. Chepinskiy, and A. Kapitonov, “Geometric path following control of a rigid body based on the stabilization of sets,” in Proc. of the 19th IFAC World Congress, Cape Town, South Africa, 2014, pp. 7342–7347. [24] J. Wang, I. A. Kapitaniuk, S. A. Chepinskiy, D. Liu, and A. J. Krasnov, “Geometric path following control in a moving frame,” IFAC- PapersOnLine, vol. 48, no. 11, pp. 150 – 155, 2015. [25] R. Gill, D. Kuli´c, and C. Nielsen, “Spline path following for redundant mechanical systems,” IEEE Trans. Robotics, vol. 31, no. 6, pp. 1378– 1392, 2015.