Motion planning and Collision Avoidance using Non-Gradient Vector Fields. Technical Report
IIEEE TRANSACTIONS ON 1
Motion Planning and Collision Avoidanceusing Non-Gradient Vector Fields
Dimitra Panagou
Abstract
This paper presents a novel feedback method on the motion planning for unicycle robots inenvironments with static obstacles, along with an extension to the distributed planning and coordinationin multi-robot systems. The method employs a family of 2-dimensional analytic vector fields, whoseintegral curves exhibit various patterns depending on the value of a parameter λ . More specifically, foran a priori known value of λ , the vector field has a unique singular point of dipole type and can beused to steer the unicycle to a goal configuration. Furthermore, for the unique value of λ that the vectorfield has a continuum of singular points, the integral curves are used to define flows around obstacles.An almost global feedback motion plan can then be constructed by suitably blending attractive andrepulsive vector fields in a static obstacle environment. The method does not suffer from the appearanceof sinks (stable nodes) away from goal point. Compared to other similar methods which are free of localminima, the proposed approach does not require any parameter tuning to render the desired convergenceproperties. The paper also addresses the extension of the method to the distributed coordination andcontrol of multiple robots, where each robot needs to navigate to a goal configuration while avoidingcollisions with the remaining robots, and while using local information only. More specifically, basedon the results which apply to the single-robot case, a motion coordination protocol is presented whichguarantees the safety of the multi-robot system and the almost global convergence of the robots to theirgoal configurations. The efficacy of the proposed methodology is demonstrated via simulation resultsin static and dynamic environments. I. I
NTRODUCTION
Motion planning, coordination and control for robotic systems still remains an active researchtopic in many respects. The primary motivation has been the computation of safe, collision-free
Dimitra Panagou is with the Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI, USA; [email protected] . October 22, 2014 DRAFT a r X i v : . [ c s . R O ] O c t EEE TRANSACTIONS ON 2 trajectories for robotic agents, mechanisms and autonomous vehicles which operate in constrainedand/or uncertain environments. Research within the robotics community has attributed variousformulations and methodologies on the motion planning problem, often specialized based on thecontrol objectives and the characteristics of the problems at hand. These methodologies rangefrom Lyapunov-based control methods, to sampling-based planning, to combinatorial planning,to formal methods [1]–[3]. Multi-robot systems have attracted the interest of the control systemscommunity as well. Emphasis has been given in consensus, flocking and formation controlproblems for multiple agents [4].Avoiding obstacles and inter-agent collisions is a requirement of highest priority in motionplanning and coordination problems. Recently, significant interest has been paid to the high-level task planning under complex goals, where the problem for an autonomous robot hastransitioned from the classical motion planning formulation (i.e., move from point A to point B ) to the consideration of complex goals under temporal specifications; such specifications aretypically described as: “visit region A , and then visit either region B or region C ”. Despite thetremendous and elegant contributions in this area, which provide elegant solutions to the high-level mission synthesis with rigorous guarantees under certain assumptions on the consideredenvironments [2], [3], the interconnection of high-level tasking with the physical layer/systemis still an open problem in many respects. One issue is the consideration of multiple agents indynamic environments and the associated complexity in finding provably correct solutions in thepresence of nonlinearities, arbitrary constraints, and uncertainty.The scope of this paper is to provide a solution to the motion planning problem for singleand multiple nonholonomic agents in dynamic environments, where agents have local sensingand communication capabilities and which may be populated by dynamic (moving) obstacles.Our goal is to provide a feedback synthesis of low-level planning controllers along with certainguarantees, which can later on be combined with high-level tasks, such as dynamic coverage[5], towards provably correct feedback solutions for a specific class of dynamical systems indynamic environments. The technical tools which we use towards this goal are set-invariancemethods, which have been proved efficient in constrained control problems of a class of nonlinear,under-actuated systems [6].The spirit of the proposed solutions is similar, but not identical to , Lyapunov-like scalarfunctions, such as the Avoidance Functions in [7] and the Artificial Potential Fields (APF) in October 22, 2014 DRAFTEEE TRANSACTIONS ON 3 [8], [9]. More specifically: It is well-known that, although scalar functions offer the merit ofLyapunov-based control design and analysis, yielding thus solutions in closed-form with certainguarantees [10], they suffer from the drawback of possible local minima away from the goalpoint, i.e., of points in the state space other than the desired equilibrium at which the gradientvector vanishes; this in principle results in system trajectories which get stuck away from thegoal point. Certain forms of potential functions may overcome this limitation; namely, navigationfunctions [11] and harmonic functions [12], [13], but under some cost: the caveat in the formercase is that the Morse property which guarantees the non-existence of local minima is renderedafter a tuning parameter exceeds a lower bound, which is not a priori known. In the lattercase, harmonic functions may be constructed with either discrete or continuous approaches, butthe computational cost of discrete methods is quite demanding. Continuous approaches whichemploy the analogies of Laplace equation with fluid mechanics yield closed-form solutionsfor certain dynamic environments [14]. Stream functions [15] combine the local-minima-freeproperty of harmonic functions along with hydrodynamic concepts to yield streamlines whichmay be preferable for second order systems. The method of vortex fields [16] uses the anti-gradient of a scalar function to define flows around obstacles.Now, let us note that one common ground in this class of solutions is the resulting gradientvector field which is employed in the control synthesis. In this respect, the idea of directly definingvector fields encoding obstacle avoidance has been studied for robot motion planning problems.In [17], for instance, simple smooth vector fields are locally constructed in given convex celldecompositions of polygonal environments, so that their integral curves are by constructioncollision-free and, in a sequential composition spirit, convergent to a goal point. The method,nevertheless, presumes the existence of a high-level discrete motion plan which determines thesuccessive order of the cells from an initial to a final configuration. Recent work employingvector fields for vehicles’ navigation is presented also in [18] and in [19]. The approach withvelocity vector fields in [20] is also relevant to the context. However, these contributions addressonly the position control of the robot, while the orientation is not guaranteed to converge to adesired value.Stepping now a little further away from single-agent problems: when it comes to multipleagents, their motion towards goal configurations defines a dynamic environment and poseschallenges to the planning, coordination and control design, even in the absence of static physical
October 22, 2014 DRAFTEEE TRANSACTIONS ON 4 obstacles. At the same time, limitations in the available sensing and communication platformsimpose additional constraints to the multi-agent system. Given a pair ( i, j ) of agents i and j ,agents typically make decisions on their actions based on available information, which can beeither locally measured using onboard sensors, or transmitted and received across the nodes of themulti-agent system via wireless communication links. Thus, information flow between two agentscan be either bidirectional (undirected) or unidirectional (directed). During the past ten years,research efforts have achieved the formalization of problems such as consensus and formationcontrol in multi-agent networks using tools and notions from graph theory, matrix theory andLyapunov stability theory [21]–[25]. The case of directed information exchange has recentlyattracted increased interest [26]–[30], motivated in part by the fact that undirected informationflow is not always a realistic and practical assumption, due to bandwidth limitations in thenetwork, anisotropic sensing of the agents etc. Extending consensus algorithms to nonlinearsystems has also become popular, see for instance [31], [32].Nevertheless, despite that consensus, flocking, and formation control algorithms achieve colli-sion avoidance in multi-vehicle systems by carefully selecting initial conditions and controllingrelative distance and heading, they are typically not used in encoding problems such as navigationto specific goal locations for each one of the agents. In this respect, the development of planningand coordination algorithms for the motion of multiple agents along with safety and performanceguarantees is an open problem in many respects. A. Overview
This paper presents a novel method on the motion planning and coordination in environmentswith static and/or dynamic obstacles, which results in feedback motion plans for unicycle robotsalong with collision avoidance guarantees. The method employs a family of two-dimensionalanalytic vector fields, originally introduced in [33], given as: F ( r ) = λ ( p T r ) r − p ( r T r ) , (1)where λ ∈ R is a parameter to be specified later on, r = [ x y ] T the position vector with respectto (w.r.t.) a global cartesian frame and p = [ p x p y ] T , with p (cid:54) = . The role the vector p ∈ R plays in the properties of the vector field (1) becomes evident later on in Theorem 2. October 22, 2014 DRAFTEEE TRANSACTIONS ON 5
In [33] the family of vector fields (1) was employed in the control design for steering kinematic,drift-free systems in chained form in obstacle-free environments.In this paper we first show that, except for a known value of the parameter λ , the vectorfield (1) has a unique singular point on R . More specifically: ( i ) For λ > the pattern of theintegral curves around the unique singular point is dipolar [34]. Such vector field can be usedfor steering a unicycle to a goal configuration. ( ii ) For λ = 1 the vector field has a continuumof singular points and can be used to define tangential flows around circular obstacles. ( iii ) For λ < the pattern of the integral curves is suitable for defining repulsive flows away from lines,and as thus, away from polygonal obstacles. A preliminary example is given in the Appendixof [35].We then consider the single-agent case in a static environment of circular obstacles and proposea blending mechanism between attractive and repulsive vector fields, which yields almost globalfeedback motion plans. In other words, we construct vector fields whose integral curves areconvergent to a goal configuration, except for a set of initial conditions of Lebesgue measurezero, and collision-free by construction. This in turn results in simple feedback control laws,which force the system to flow along the vector field.We finally consider the extension of the methodology to the distributed coordination andcontrol for multiple nonholonomic agents. Based on the results for the single-agent case in staticobstacle environments, we propose a coordination protocol for multiple agents which need toconverge to specific goal configurations, using local information only. The proposed protocolyields collision-free and almost globally convergent trajectories for the multi-agent system. B. Contributions and Organization
When it comes to the single-agent case, i.e., to a robot operating in a known, static environmentof circular obstacles, the proposed method does not suffer from the appearance of sinks (stablenodes) away from goal point. Furthermore, compared to similar feedback methods which relyon scalar (potential) functions, such as [11], the main difference and advantage of the proposedapproach is that:(i) no parameter tuning is needed in order to render the desired convergence properties; thevalues of the parameter λ of the vector field are known a priori .Compared to similar methods which rely on vector fields, such as [17], the proposed method: October 22, 2014 DRAFTEEE TRANSACTIONS ON 6 (ii) requires neither the computation of a cell decomposition of the free space, nor the existenceof a high-level discrete motion plan, and as thus it is free of any computational complexityissues,(iii) addresses the motion planning and collision avoidance for multiple agents in dynamicenvironments, and is scalable as the number of agents increases.Finally, compared to other similar vector field based methods, such as [18]–[20], the proposedmethod:(iv) guarantees the convergence of the orientation trajectories of the robots to any predefinedvalue.
Remark 1:
While here we consider circular, not polygonal, obstacle environments, preliminaryresults reveal that the method can be used for defining repulsions around polygonal obstacles aswell, see the Appendix in [35].When it comes to the multi-agent case, i.e., to multiple agents moving towards goal configu-rations while avoiding collisions, the proposed method:(v) offers the flexibility to directly impose the minimum allowable clearance among agents,something which typically is not the case with gradient-based solutions. This character-istic might be desirable, for instance, when considering multi-robot systems in confinedenvironments.(vi) being a non-gradient vector field approach, the technical developments are based on setinvariance concepts rather than Lyapunov-based methods. This in principle provides lessconservative solutions, while it might desirable in extending the method to more complicateddynamical models.Compared to our earlier work, the vector field construction presented here is not the samewith the one in [36]. Furthermore, the proposed construction, coordination protocol and technicaldevelopments are not the same with the ones in [37]. Moreover, since it offers feedback solutionswith certain convergence guarantees, it can be used as a basis in constrained model predictivecontrol designs [38], which are appropriate for uncertain environments. The case of mixedenvironments, i.e., of multiple agents operating among physical obstacles under uncertainty,are not considered in this paper and this topic is left open for future research.
October 22, 2014 DRAFTEEE TRANSACTIONS ON 7
Part of this work has appeared in [39]. The current paper additionally includes: (i) a detailedpresentation of the overall method both for the static and the dynamic case, along with the proofswhich have been omitted in the conference version in the interest of space, (ii) more simulationresults which demonstrate the efficacy of the method in static and dynamic environments.The paper is organized as follows: Section II includes a brief overview of the notions regardingthe topology of two-dimensional vector fields that are used throughout the paper. Section IIIcharacterizes the singular points of our vector fields w.r.t. the parameter λ , while section IVpresents the blending mechanism among vector fields, the construction of the almost globalfeedback motion plans and the underlying control design, along with simulation results instatic obstacle environments. Section V presents the extension of the method to the distributedcoordination and collision-free motion of multiple agents under various sensing/communicationpatterns. Our conclusions and thoughts on future work are summarized in Section VI.II. S INGULAR POINTS OF VECTOR FIELDS
This section provides an overview of notions from vector field topology. For more informationthe reader is referred to [34], [40], [41].
Definition 1:
A vector field on an open subset U ⊂ R n is a function which assigns to eachpoint p ∈ U a vector X p ∈ T p ( R n ) . A vector field on R n is C ∞ (smooth) if its componentsrelative to the canonical basis are C ∞ functions on U . Definition 2:
Given a C ∞ vector field X on R n , a curve t → F ( t ) defined on an open interval J of R is an integral curve of X if d F d t = X F ( t ) on J . Definition 3:
A point p of U at which X p = 0 is called a singular, or critical, point of thevector field. Center-type and non-center type singularities:
Singular points are typically distinguishedto those that are reached by no integral curve (called center type) and those that are reachedby at least two integral curves (called non-center type). In the case of a center type singularity,one can find a neighborhood of the singular point where all integral curves are closed, insideone another, and contain the singular point into their interior. In the case of non-center typesingularities, one has that at least two integral curves converge to the singular point. The localstructure of a non-center type singularity is analyzed by considering the behavior of all theintegral curves which pass through the neighborhood of the singular point. This neighborhood
October 22, 2014 DRAFTEEE TRANSACTIONS ON 8 is made of several curvilinear sectors. A curvilinear sector is defined as the region bounded bya circle C of arbitrary small radius, and two integral curves, S and S (cid:48) , which both converge(for either t → + ∞ , or t → −∞ ) to the singular point. The integral curves passing through theopen sector g (i.e., the integral curves except for S , S (cid:48) ) determine the following three possibletypes of curvilinear sectors [42]: (i) Elliptic sectors: all integral curves begin and end at thecritical point. (ii)
Parabolic sectors: just one end of each integral curve is at the critical point.(iii)
Hyperbolic sectors: the integral curves do not reach the critical point at all. The integralcurves that separate each sector from the next are called separatrixes, see also Fig. 1.
Fig. 1. A typical isolated critical point. Image taken from [34].
First-order and high-order singularities:
A singular point p of a vector field X on R iscalled a first-order singular point if the Jacobian matrix J X ( · ) of the vector field X does notvanish (i.e., is nonsingular) on p , i.e., if: det ( J X ( p )) (cid:54) = 0 ; otherwise the singular point is called high-order singular point. October 22, 2014 DRAFTEEE TRANSACTIONS ON 9
III. N
AVIGATION VIA VECTOR FIELDS
Consider the motion of a robot with unicycle kinematics in an environment W with N staticobstacles. The equations of motion read: ˙ x ˙ y ˙ θ = cos θ θ
00 1 uω , (2)where q = (cid:2) r T θ (cid:3) T is the configuration vector, r = [ x y ] T is the position and θ is the orientationof the robot w.r.t. a global frame G , and u , ω are the linear and the angular velocity of the robot,respectively. The robot is modeled as a closed circular disk of radius (cid:37) , and each obstacle O i is modeled as a closed circular disk of radius (cid:37) oi centered at r oi = [ x oi y oi ] T , i ∈ { , . . . , N } .Denote O i = { r ∈ R | (cid:107) r − r oi (cid:107) ≤ (cid:37) oi } . A. A family of vector fields for robot navigation
We consider the class of vector fields F : R → R given by (1). The vector field components F x , F y read: F x = ( λ − p x x + λp y xy − p x y , (3a) F y = ( λ − p y y + λp x xy − p y x . (3b) Theorem 1:
The origin r = is the unique singular point of the vector field F (1) if andonly if λ (cid:54) = 1 . Proof:
It is straightforward to verify that r = is a singular point of F . Let us write thevector field components (3) of F in matrix form as: F x F y = ( λ − x − y λxyλxy ( λ − y − x (cid:124) (cid:123)(cid:122) (cid:125) A ( λ, r ) p x p y . (4)The determinant of the matrix A ( λ, r ) is: det( A ( λ, r )) = − ( λ − x + y ) . This implies that A ( λ, r ) is nonsingular away from the origin r = if and only if λ (cid:54) = 1 . Therefore, for λ (cid:54) = 1 and r (cid:54) = , one has F = if and only if p = . Since p (cid:54) = by definition, if follows that thevector field F is nonsingular everywhere but the origin r = , as long as λ (cid:54) = 1 . October 22, 2014 DRAFTEEE TRANSACTIONS ON 10
Theorem 2:
The line l : y = tan ϕ x, where tan ϕ (cid:44) p y p x , is an axis of reflection, or mirrorline, for F (1). Proof:
Consider two points A , B of equal distance and on opposites sides w.r.t. the line l (Fig. 2). Their position vectors r A = [ x A y A ] T , r B = [ x B y B ] T w.r.t. G read: x A = R cos a, y A = R sin a, (5a) x B = R cos(2 ϕ − a ) , y B = R sin(2 ϕ − a ) , (5b)where ( R, a ) , ( R, (2 ϕ − a )) are the polar coordinates of A , B , respectively. We need to prove α ββ φ y = ta n φ xx A y A x B y B υ ο υ ’ ο υ p υ p AB x g y g x l y l Fig. 2. The line l : y = tan ϕ x , where ϕ = arctan( p y p x ) , is a reflection (or mirror) line for the vector field F . that the vector F ( r A ) , denoted F A , reflects to the vector F ( r B ) , denoted F B , w.r.t. the line l : y = tan ϕ x . Recall that the reflection matrix about the considered line l is: H (2 ϕ ) = cos 2 ϕ sin 2 ϕ sin 2 ϕ − cos 2 ϕ . (6) October 22, 2014 DRAFTEEE TRANSACTIONS ON 11
Substituting (5a) into (4) and after some standard algebra yields: F A = ( λ − R (cid:107) p (cid:107) cos ϕ sin ϕ (cid:124) (cid:123)(cid:122) (cid:125) v p + λR (cid:107) p (cid:107) cos( ϕ − a ) − sin( ϕ − a ) (cid:124) (cid:123)(cid:122) (cid:125) v o , (7)where (cid:107) p (cid:107) = (cid:112) p x + p y . Similarly, substituting (5b) into (4) yields: F B = ( λ − R (cid:107) p (cid:107) cos ϕ sin ϕ (cid:124) (cid:123)(cid:122) (cid:125) v p + λR (cid:107) p (cid:107) cos 2 ϕ sin 2 ϕ sin 2 ϕ − cos 2 ϕ cos( ϕ − a ) − sin( ϕ − a ) (cid:124) (cid:123)(cid:122) (cid:125) v (cid:48) o . (8)One has: F A = v p + v o and F B = v p + v (cid:48) o . Out of (7), (8) one gets that v (cid:48) o = H (2 ϕ ) v o , i.e., v (cid:48) o is the reflection of the vector v o about the line l . Thus, one may write v o = v lox ˆ x l + v loy ˆ y l and v (cid:48) o = v lox ˆ x l − v loy ˆ y l , where ˆ x l , ˆ y l are the unit vectors along the axes x l , y l , respectively, see Fig. 2. Furthermore, v p is parallel to the vector p , i.e., parallel to the candidate reflection line l . Consequently, one maywrite: v p = v lpx ˆ x l + 0 ˆ y l . It follows that: F A = ( v lox + v lpx ) ˆ x l + v loy ˆ y l , F B = ( v lox + v lpx ) ˆ x l − v loy ˆ y l , i.e., that the vector F B is a reflection of vector F A about the line l . This completes the proof. Remark 2:
The Jacobian matrix of F is singular at r = , which implies that r = is ahigh-order singularity. Thus, one may expect that the pattern of the integral curves around thesingular point will be more complicated compared to those around a first-order singularity, i.e.,around nodes, saddles, foci or centers. Theorem 3:
The equation of the integral curves of F for p = [1 0] T is given as: ( x + y ) λ = c y ( λ − , c ∈ R . (9) October 22, 2014 DRAFTEEE TRANSACTIONS ON 12
Proof:
Consider the polar coordinates ( r cos φ, r sin φ ) of a point ( x, y ) where: r = (cid:112) x + y , cos φ = xr , sin φ = yr . (10)After substituting (10) and p x = 1 , p y = 0 into (4) the vector field components read: F x = r (cid:0) ( λ −
1) cos φ − sin φ (cid:1) , (11a) F y = r ( λ cos φ sin φ ) . (11b)An integral curve of (1) is by definition the solution of the system of ordinary differentialequations: dxdt = F xdydt = F y , which further reads: dxdy = F x F y , (12)while the differentials between Cartesian and polar coordinates satisfy the formula: drrdφ = cos φ sin φ − sin φ cos φ dxdy . (13)Plugging (13), (11) into (12) results in: r dr = ( λ −
1) cos φ sin φ dφ, while integrating by parts yields: ln( r ) = ( λ −
1) ln(sin φ ) + ln( c ) ⇒ ln( r ) = ln (cid:0) c sin ( λ − φ (cid:1) ⇒ r = c sin ( λ − φ ⇒ r = c y ( λ − r ( λ − ⇒ r λ = c y ( λ − ⇒ ( x + y ) λ = c y ( λ − , where c ∈ R . This completes the proof.
Remark 3:
It is straightforward to verify that: • For λ = 0 , (9) reduces to y = c , i.e., the integral curves are straight lines parallel to p = [1 0] T . • For λ = 1 , (9) reduces to (cid:112) x + y = c , i.e., the integral curves are circles of radius √ c ,where c > , centered at the origin ( x, y ) = (0 , . October 22, 2014 DRAFTEEE TRANSACTIONS ON 13
B. Attractive vector fields
Let us consider the case λ = 2 . Take for simplicity p = [1 0] T and write the vector fieldcomponents as: F x = x − y , (14a) F y = 2 xy (14b)Following [34], the singular point r = of (14) is a dipole. More specifically, the vector field(14) has two elliptic sectors, with the axis y = 0 serving as the separatrix. This implies thatall integral curves begin and end at the singular point, except for the separatrix y = 0 . Theseparatrix converges to r = for x < and diverges for x > (Fig. 3). Out of Theorem 2, theseparatrix y = 0 is the reflection line for the vector field (14). - - - - Fig. 3. The integral curves of (1) for λ = 2 , p x = 1 , p y = 0 . Furthermore, Theorem 2 implies that the axis the vector p (cid:54) = lies on is, in general, a October 22, 2014 DRAFTEEE TRANSACTIONS ON 14 reflection line for (1). This means that the resulting integral curves are symmetric w.r.t. thevector p ∈ R . In that sense, any of the integral curves of F offers a path to r = , while at thesame time the direction of the vector p dictates the symmetry axis of the integral curves w.r.t.the global frame G .Therefore, defining a feedback motion plan for steering the unicycle to a goal configuration q g = (cid:2) r Tg θ g (cid:3) T has been based in earlier work of ours’ [33] on the following simple idea: Picka vector field F out of (1) in terms of ( r − r g ) , with λ = 2 and p = [ p x p y ] T , so that thedirection of the vector p coincides with the goal orientation: ϕ (cid:44) arctan( p y p x ) = θ g . Then, theintegral curves serve as a reference to steer the position trajectories r ( t ) to the goal position r g ,and the orientation trajectories θ ( t ) to the goal orientation θ g . C. Repulsive vector fields
Let us consider the case λ = 1 , i.e., the case when the vector field (1) has multiple singularpoints. The vector field components read: F x = p y xy − p x y , (15a) F y = p x xy − p y x . (15b)The vector field (15) vanishes on the set V = { r ∈ R | p y x − p x y = 0 } . Out of Theorem 2, thesingularity set V coincides with the reflection line of the vector field (15). The equation of theintegral curves can be computed for p y x − p x y (cid:54) = 0 as: dxdy = y − x ⇒ x + y = c , where c ∈ R ,which implies that the integral curves are circles centered at the origin r = , see Fig. 4.The signum of x (in general, of p iT r ) dictates whether the integral curves escape the singularityset V (see the half-plane x > ) or converge to the singularity set V (see the half-plane x < ).We say that the singular point r = of the vector field (15) is of center type; this means that no integral curve reaches the singular point. Thus, one may employ (15) to define tangential vector fields locally around circular obstacles. This is to have the unique singular point of F coinciding with the desired position r g . Characterizing this particular singularity as of center type is slightly inconsistent with standard notation, since in this casethe singular point r = is not isolated. October 22, 2014 DRAFTEEE TRANSACTIONS ON 15 - - - - Fig. 4. The vector field F for λ = 1 and p x = 1 , p y = 0 . IV. A
LMOST GLOBAL FEEDBACK MOTION PLANS
Given the class of attractive and repulsive vector fields, the idea on defining an almost globalfeedback motion plan F (cid:63) on the collision-free space F is now simple: we pursue to combine anattractive-to-the-goal vector field F g with (local) repulsive vector fields F oi around each obstacle O i , so that the integral curves of F (cid:63) : 1) converge to the goal q g , and 2) point into the interiorof F on the boundaries of the obstacles O i . The vector field F (cid:63) can then serve as a feedbackmotion plan on W . Remark 4:
Combining the vector fields F g , F oi should be done carefully so that the resultingvector field F (cid:63) does not have any undesired singularities on F . For this reason, we consider the October 22, 2014 DRAFTEEE TRANSACTIONS ON 16 normalized unit vector fields: F ng = F g (cid:107) F g (cid:107) , for r (cid:54) = ; , for r = . (16a) F noi = F oi (cid:107) F oi (cid:107) , for r / ∈ V i ; , for r ∈ V i . (16b)respectively, when defining the blending mechanism, see later on in Section IV-C. A. Attractive vector field to the goal
Without loss of generality we assume that q g = . An attractive-to-the-goal vector field F g may be taken out of (1) for λ = 2 , p g = [1 0] T , which yields the vector field (14). Thecomponents of the normalized vector field F ng taken out of (16a) for x (cid:54) = 0 , y (cid:54) = 0 read: F ngx = x − y x + y , F ngy = 2 xyx + y . B. Repulsive vector field w.r.t. a circular obstacle
Consider an obstacle O i and the region Z i : (cid:8) r ∈ R | (cid:107) r − r oi (cid:107) ≤ (cid:37) Z i (cid:9) , where (cid:37) Z i = (cid:37) oi + (cid:37) + (cid:37) ε , see Fig. 5. The parameter (cid:37) ε ≥ is the minimum distance that the robot is allowed tokeep w.r.t. the boundary of the obstacle.A repulsive vector field w.r.t. the point r oi can be picked out of (15) for p i = [ p xi p yi ] T ,where p xi = cos φ i , p yi = sin φ i , φ i = atan2( − y oi , − x oi ) + π as: F oxi = p yi ( x − x oi )( y − y oi ) − p xi ( y − y oi ) , (17a) F oyi = p xi ( x − x oi )( y − y oi ) − p yi ( x − x oi ) . (17b)Note that the vector p i is picked such that it lies on the line connecting the center r oi of theobstacle with the goal point r g = . Therefore, the singularity set V i of (17) lies by constructionon this line, which is also the reflection axis of the vector field (17). Denote A i = { r ∈Z i | p iT ( r − r oi ) ≥ } , B i = { r ∈ Z i | p iT ( r − r oi ) < } , where Z i = A i (cid:83) B i , and considerthe behavior of the integral curves around the singularity set V i . The integral curves depart fromthe singularity set V i in the region A i (see the red vectors around V i in Fig. 5), and converge tothe singularity set V i in the region B i (the corresponding vectors have not been drawn in Fig.5). The integral curves in region A i render safe, tangential reference paths around the obstacle October 22, 2014 DRAFTEEE TRANSACTIONS ON 17 o i ρ i p ρ i O ε ρ g x B i A i λ = 1 λ = 0 Ζ i λ = 2 V i g y Fig. 5. Defining a repulsive vector field F oi around the obstacle O i . Note that we take the vector field (1) with λ = 1 inregion A i and with λ = 0 in region B i . O i . However, their pattern in region B i is undesirable, since it may trap the system trajectories r ( t ) away from r g . To overcome this, in region B i we define a vector field out of (1) for λ = 0 and p i as before, whose vector field components read: F oxi = − p xi ( x − x oi ) − p xi ( y − y oi ) , (18a) F oyi = − p yi ( x − x oi ) − p yi ( y − y oi ) . (18b)This vector field is co-linear with p i and vanishes at the unique singular point r = r oi . Remark 5:
The transition of the integral curves between regions A i , B i is smooth, since thevectors at the points where p iT ( r − r oi ) = 0 coincide. October 22, 2014 DRAFTEEE TRANSACTIONS ON 18
In summary, the vector field F oi around a circular obstacle O i is picked out of the family ofvector fields (1) as: F oi = F ( λ =1) ( δr i ) , for p iT ( δr i ) ≥ F ( λ =0) ( δr i ) , for p iT ( δr i ) < , (19)where δr i (cid:44) r − r oi , φ i (cid:44) atan2( − y oi , − x oi ) + π , p i = [cos φ i sin φ i ] T . The normalized vectorfield then reads: F noi = F ( λ =0) ( δr i ) (cid:107) F ( λ =0) ( δr i ) (cid:107) , for p iT ( δr i ) < F ( λ =1) ( δr i ) (cid:107) F ( λ =1) ( δr i ) (cid:107) , for p iT ( δr i ) ≥ and r / ∈ V i ; , for p iT ( δr i ) ≥ and r ∈ V i ; (20) C. Blending attractive and repulsive vector fields
Define the obstacle function β i ( · ) : R → R as: β i ( r , r oi , (cid:37) oi ) = (cid:37) oi − (cid:107) r − r oi (cid:107) , (21)which is positive in the interior Int( O i ) of the obstacle, zero on the boundary ∂ O i of the obstacle,and negative everywhere else. Denote the value of the constraint function β i on the boundary ∂ Z i of the region Z i as β i Z = − (cid:37) oi ( (cid:37) + (cid:37) ε ) − ( (cid:37) + (cid:37) ε ) . The repulsive vector field F noi is then locally defined on the set: ( Z i \ Int( O i )) = { r ∈ R | β i Z ≤ β i ( r ) ≤ } . At the same time, the attractive vector field F ng should be defined exteriorto Z i , i.e., for β i ( r ) < β i Z . To encode this, define the smooth bump function σ i ( · ) : R → [0 , : σ i = , for β i ( r ) ≤ β i F ; aβ i + bβ i + cβ i + d, for β i F < β i ( r ) < β i Z ;0 , for β i Z ≤ β i ( r ); (22)where β i Z is the value of (21) at distance (cid:37) Z i w.r.t. r oi , β i F is the value of (21) at some distance (cid:37) F i > (cid:37) Z i w.r.t. r oi , and the coefficients a , b , c and d are computed as: a = 2( β i Z − β i F ) , b = − β i Z + β i F )( β i Z − β i F ) ,c = 6 β i Z β i F ( β i Z − β i F ) , d = β i Z ( β i Z − β i F )( β i Z − β i F ) , October 22, 2014 DRAFTEEE TRANSACTIONS ON 19 so that (22) is a C function. Having this at hand, and inspired by [17], one may now define thevector field: F i = σ i F ng + (1 − σ i ) F noi . (23) Lemma 1:
The vector field (23) is:( i ) Attractive to the goal q g for (cid:107) r − r oi (cid:107) ≥ (cid:37) F i , i.e., for β i ( r ) ≤ β F i where σ i = 1 , via theeffect of F ng .( ii ) Repulsive w.r.t. O i for (cid:37) oi ≤ (cid:107) r − r oi (cid:107) ≤ (cid:37) Z i , i.e., for β Z i ≤ β i ( r ) where σ i = 0 , via theeffect of F noi .( iii ) Nonsingular in the region (cid:37) Z i < (cid:107) r − r oi (cid:107) < (cid:37) F i , i.e., for β F i < β i ( r ) < β Z i where < σ i < .( iv ) Safe w.r.t. the obstacle O i and convergent to the goal q g for almost all initial conditions. Proof:
The first two arguments have been proved in the previous section. To verify the thirdargument, consider the norm of vector field F i in the blending region D i : { r ∈ R | (cid:37) Z i < (cid:107) r − r oi (cid:107) < (cid:37) F i } , which reads: (cid:107) F i (cid:107) = (cid:112) − σ i (1 − σ i ) + 2 σ i (1 − σ i ) cos α, where α the angle between the vectors F ng , F noi at some point r ∈ D i . Then, for r / ∈ V i one hasthat (cid:107) F i (cid:107) vanishes at the points where σ i is the solution of: − cos α ) σ i − − cos α ) σ i + 1 = 0 . The discriminant reads ∆ = − − cos α ) , which implies that there are no real solutions, i.e.,that the vector field F i is nonsingular for r / ∈ V i . Moreover, for r ∈ V i one has F noi = , andtherefore: (cid:107) F i (cid:107) = σ i (cid:54) = 0 .Finally, to verify the fourth argument, consider first that the integral curves which do not intersectwith the blending region D i are convergent by construction to r g . Consider now the boundary S i : { r ∈ R | (cid:107) r − r oi (cid:107) − (cid:37) F i = 0 } of the region D i and let us analyze the behavior of the integral curves on the manifolds: S − i : { r ∈ R | (cid:107) r − r oi (cid:107) = (cid:37) F i + δ(cid:37) } ,S + i : { r ∈ R | (cid:107) r − r oi (cid:107) = (cid:37) F i − δ(cid:37) } , October 22, 2014 DRAFTEEE TRANSACTIONS ON 20 with δ(cid:37) > arbitrarily small. After some calculations: ∇ S i F i = 2( r − r oi ) T F ng , ∇ S − i F i = 2( r − r oi ) T F ng . For ∇ S + i F i , consider the following cases:Case 1. The vector field F noi satisfies: ( r − r oi ) T F noi = 0 , and therefore: ∇ S + i F i = 2 σ i ( r − r oi ) T F ng . Then: ( ∇ S − i F i )( ∇ S + i F i ) > , which implies that the integral curves cross the switchingsurface S i and enter A i . Consider now the behavior of the integral curves in A i . Assumethat ∇ S + i F i = 2 σ i ( r − r oi ) T F ng > ; this would imply that ∇ S i F i > as well, i.e., thatthe integral curves did not cross S i , a contradiction. Then: ∇ S + i F i = 2 σ i ( r − r oi ) T F ng < , which yields that the integral curves approach the boundary T i : { r ∈ R | (cid:107) r − r oi (cid:107) − (cid:37) Z i = 0 } of the blending region D i . Denote T − i : { r ∈ R | (cid:107) r − r oi (cid:107) = (cid:37) Z i + δ(cid:37) } and note that: ∇ T − i F i = ∇ S + i F i < , and that ∇ T i F i = 0 , since on T i one has σ i = 0 .Then, F i (cid:54) = is tangent to T i , which means that the integral curves slide along T i , untilreaching region B i . Remark 6:
The integral curves are not defined on the (unique) point on T i where F i = .This further implies that system trajectories which either start or reach this point get stuckaway from the goal configuration. Let us now consider the pattern of the integral curves in the vicinity of the singularity andcharacterize the set of initial conditions from which the system trajectories end there.
Itwas shown in the previous section that the integral curves around the singularity set V i aredeparting the set, except for one integral curve which converges to V i . For this condition tooccur the goal orientation θ g should be co-linear with the line the singularity set V i lies on .To see why, recall that the vector field in the blending region reads: F i = σ i F ng , and that October 22, 2014 DRAFTEEE TRANSACTIONS ON 21 the vector field F ng should point to the singularity set V i . Consequently, this condition arisesif and only if the obstacle is positioned such that the direction of the vector p i coincideswith the direction of the vector p g . Therefore, the set of initial conditions from which theintegral curves of F i converge to the singularity set V i is of Lebesgue measure zero. Notealso that if the direction of p g does not coincide with the direction of p i , then the singularpoints of F i are confined in Z i on a line segment of length (cid:37) ε , correspond to the initialconditions from which solutions are not defined, and are reached by no integral curve.Case 2. In region B i one may follow a similar analysis to conclude that the integral curves exit B i .In summary, the vector field (23) is safe and globally convergent almost everywhere, i.e., exceptfor a set of initial conditions of measure zero. D. Motion plan in static obstacle environmentsTheorem 4:
Assume a workspace W of N circular obstacles O i , i ∈ { , . . . , N } , positionedsuch that the inter-obstacle distances d ij = (cid:107) r oi − r oj (cid:107) satisfy: d ij ≥ (cid:37) Z i + (cid:37) Z j , ∀ ( i, j ) , j ∈ { , . . . , N } , j (cid:54) = i. (24)Then, the vector field F (cid:63) : R → R , given as: F (cid:63) = N (cid:89) i =1 σ i F g + N (cid:88) i =1 (1 − σ i ) F oi , (25)where F g is the normalized attractive vector field (16a), F oi is the normalized repulsive vectorfield (20) around an obstacle O i , and σ i is the bump function (22) defined in terms of the obstaclefunction β i given by (21), is a safe, almost global feedback motion plan in F , except for a setof initial conditions of measure zero. Proof:
By construction, the first term in (25) cancels the effect of the attractive vectorfield F g where at least one of the bump functions σ i = 0 , i.e., in the corresponding region Z i around obstacle O i . At the same time the second term shapes the corresponding vector field F oi in Z i . Thus, the attractive vector field F g is activated through (25) only when β i < β Z i ∀ i ∈ { , . . . , N } , i.e., outside the regions Z i . Furthermore, setting the inter-obstacle distance d ij ≥ (cid:37) Z i + (cid:37) Z j implies that the repulsive flows around obstacles do not overlap, and therefore October 22, 2014 DRAFTEEE TRANSACTIONS ON 22 are both safe and almost globally convergent to the goal, as proved in Lemma 1. This completesthe proof.
Remark 7:
The condition (24) reads that the minimum distance among the boundaries of theobstacles should be at least (cid:37) + (cid:37) ε ) . This clearance is not conservative or restrictive in practice,since the parameter (cid:37) ε can be chosen arbitrarily close to zero, or even equal to zero, in case therobot is allowed to touch the obstacle. E. Control design and simulation results
Having (25) at hand, the control design for the unicycle (2) is now straightforward. We usethe control law: u = k u tanh (cid:0) (cid:107) r − r g (cid:107) (cid:1) , (26a) ω = − k ω ( θ − ϕ ) + ˙ ϕ, (26b)where ϕ (cid:44) arctan( F (cid:63)y F (cid:63)x ) is the orientation of the vector field F (cid:63) at a point ( x, y ) , with its timederivative reading: ˙ ϕ (2) = (cid:16)(cid:16) ∂ F (cid:63)y ∂x cθ + ∂ F (cid:63)y ∂y sθ (cid:17) F (cid:63)x − (cid:16) ∂ F (cid:63)x ∂x cθ + ∂ F (cid:63)x ∂y sθ (cid:17) F (cid:63)y (cid:17) u, with the linear velocity u given by (26a), see in [35], and k ω > , k u > . Then, the orientation θ of the unicycle is Globally Exponentially Stable (GES) to the safe orientation ϕ , and the robotflows along the integral curves of F (cid:63) until converging to r g .To demonstrate the efficacy of the proposed navigation and control design we consider themotion of a robot in an environment with N = 10 static obstacles (Fig. 6), where the goalposition is r g = [ − . . T . The radii of the obstacles are set equal to (cid:37) oi = 0 . . Theblending zone D i around each obstacle O i is illustrated between the boundary surfaces S i (blackline) and T i (red line), respectively. The resulting collision-free path under the control law (26),with the control gains picked equal to k u = 0 . , k r = 2 . , are depicted in blue color. Remark 8:
The integral curves of F oi in the region A i around an obstacle O i forces the robotto perform a sharp maneuver in order to follow the tangential direction and avoid collision. Thisin practice is plausible for unicycle-type vehicles (e.g. differentially driven mobile robots), yetit may not be desirable for input-constrained vehicles, such as car-like vehicles and aircraft. Ourcurrent work focuses in encoding curvature constraints via (1). October 22, 2014 DRAFTEEE TRANSACTIONS ON 23 −0.6 −0.5 −0.4 −0.3 −0.2 −0.1 0−0.4−0.200.2 12 3 4567 89 10
Fig. 6. The path of a unicycle in a obstacle environment.
V. E
XTENSION TO DYNAMIC ENVIRONMENTS
Consider N agents i ∈ { , , . . . , N } of unicycle kinematics which are assigned with the taskto converge to goal configurations q gi while avoiding collisions.Each agent i has a circular communication/sensing region C i of radius R c centered at r i = (cid:104) x i y i (cid:105) T , denoted as: C i : { r ∈ R | (cid:107) r i − r (cid:107) ≤ R c } , and can reliably exchange information with any agent j (cid:54) = i which lies within its communicationregion C i . In other words, we say that a pair of agents ( i, j ) is connected, or equivalently, thatagent j is neighbor to agent i , as long as the inter-agent distance d ij = (cid:107) r i − r j (cid:107) ≤ R c . Denotethe set of neighbors j (cid:54) = i of agent i with N i . October 22, 2014 DRAFTEEE TRANSACTIONS ON 24
Agents j (cid:54) = i serve as dynamic (moving) obstacles to agent i . Navigating safely to an assignedgoal q gi is then reduced into finding a feedback motion plan F (cid:63)i such that its integral curves:(i) point into the interior of the collision-free space F i on the boundaries of the agents j (cid:54) = i ,and (ii) converge to the goal q gi . Towards this end, we would like to employ a vector field F (cid:63)i for each agent i as: F (cid:63)i = (cid:89) j ∈N i σ ij F gi + (cid:88) j ∈N i (1 − σ ij ) F ioj , (27)where the attractive term F gi is taken out of (16a), the bump function σ ij is defined later on,and the repulsive term F ioj around each each agent j (cid:54) = i is replaced with a normalized repellingnode , given out of: F ixoj = x i − x j (cid:112) ( x i − x j ) + ( y i − y j ) , (28a) F iyoj = y i − y j (cid:112) ( x i − x j ) + ( y i − y j ) . (28b)In order to utilize the (almost global) convergence and safety guarantees applying to the staticcase, we need to ensure that the repulsive flows around agents do not overlap, for any pair ofagents ( i, j ) . Recall from the static case that this condition equivalently reads as that the minimumdistance d m between any pair of (moving, in the dynamic case) agents ( i, j ) is d m = 2(2 (cid:37) + (cid:37) (cid:15) ) ,or equivalently, that the minimum clearance between any pair of agents is (2 (cid:37) + (cid:37) (cid:15) ) , where (cid:37) (cid:15) > arbitrarily small and (cid:37) is the radius of the agents.In that respect, the bump function σ ij in (27) is defined as: σ ij = , for d m ≤ d ij ≤ d r ; a d ij + b d ij + c d ij + d, for d r < d ij < d c ;0 , for d ij ≥ d c ; (29)where the coefficients a, b, c, d have been computed as: a = − d r − d c ) , b = 3( d r + d c )( d r − d c ) ,c = − d r d c ( d r − d c ) , d = d c (3 d c − d r )( d r − d c ) , The tangential repulsive vector field (19) defined for static obstacles is not a suitable choice for the dynamic case; the reasonis that the repulsive integral curves of the vector field (25) of agent i around agent j are rendered an invariant set under theproposed velocity coordination protocol, forcing thus the trajectories r i ( t ) , r j ( t ) of a pair of agents i , j converge to undesiredlocations away from the goal locations r gi , r gj , see also the analysis in [ ? ]. October 22, 2014 DRAFTEEE TRANSACTIONS ON 25 so that (22) is a C function, d m ≥ (cid:37) + (cid:37) (cid:15) ) , d m < d r < d c . Remark 9:
The communication/sensing range of each agent i should be R c ≥ d c . A. Control design
Each agent i moves under the control law: u i = max (cid:26) , min j ∈N i | J j < u i | j (cid:27) , d m ≤ d ij ≤ R c , u ic , R c < d ij ; , (30a) ω i = − k ωi ( θ i − ϕ i ) + ˙ ϕ i , (30b)where: ϕ i is the orientation of the vector field F (cid:63)i at a point ( x, y ) , the vector field F (cid:63)i is givenby (27), u i | j is the safe velocity of agent i w.r.t. an agent j lying in the communication regionof C i of agent i , given as: u i | j = u ic d ij − d m R c − d m + ε i u is | j R c − d ij R c − d m , (31)with the terms in (31) defined as: u ic = k ui tanh( (cid:107) r i − r gi (cid:107) ) , u is | j = u j r jiT η j r jiT η i , η i = cos ϕ i sin ϕ i , J j = r jiT η i , r ji = r i − r j , and ε i > , k ui , k ωi > . Theorem 5:
Consider N agents i ∈ { , . . . , N } assigned to converge to goal configurations q gi . Then, under the control law (30) each agent safely converges to its goal configuration almostglobally, except for a set of initial conditions of measure zero. Proof:
The closed loop trajectories of each agent i are forced to flow along the vector field(27). If d ij ( t ) > R c , ∀ t ≥ and ∀ j ∈ { , . . . , N } , then σ ij ( t ) = 1 , implying that agent i flowssafely along (16a) and converges to q gi .Let us now assume that at some time t ≥ the distance d ij ( t ) between a pair of agents ( i, j ) is d ij ( t ) ≤ R c . By definition agent i lies in the sensing/communication region of agent j and viceversa, which implies that they exchange information on their current positions r i ( t ) , r j ( t ) and October 22, 2014 DRAFTEEE TRANSACTIONS ON 26 velocities ν i ( t ) , ν j ( t ) . Consider the blending region D i : { r j ∈ R | d r < (cid:107) r i − r j (cid:107) < d c } andthe surfaces: S i ( t ) : { r i ( t ) , r j ( t ) ∈ R | (cid:107) r i ( t ) − r j ( t ) (cid:107) − d c = 0 } ,T i ( t ) : { r i ( t ) , r j ( t ) ∈ R | (cid:107) r i ( t ) − r j ( t ) (cid:107) − d r = 0 } . Lemma 2:
Agent i avoids collision with any of its neighbor agents j ∈ N i . Proof:
Collision-free motion is realized as ensuring that d ij ( t ) ≥ (cid:37) , ∀ t ≥ , for any pair ( i, j ) . Let us consider the time derivative of inter-agent distance function, which after somecalculations reads: ddt d ij = ( x i − x j )( ˙ x i − ˙ x j ) d ij + ( y i − y j )( ˙ y i − ˙ y j ) d ij (2) = u i r jiT η i − u j r jiT η j d ij . (32)The control law (30) renders the value of the time derivative (32) positive when the value of thedistance function is d ij = d m , implying thus that the inter-agent distance is forced to increase.Since d m > (cid:37) , this further implies that collisions are avoided.In order to draw conclusions about the convergence of the agents’ trajectories to their goalconfigurations we need to examine the behavior of the integral curves around the switchingsurfaces S i ( t ) , T i ( t ) . With the vector fields F (cid:63)i , F (cid:63)j well-defined everywhere in the correspondingblending regions, and the linear velocities u i , u j vanishing only at the goal locations r gi , r gj , weare interested in identifying conditions under which the system trajectories r i ( t ) , r j ( t ) are forcedto get stuck on S i ( t ) , or on T i ( t ) , for infinite amount of time. This can be seen as identifyingsufficient conditions of the appearance of (chattering) Zeno behavior, or Zeno points [43]. Asufficient condition on the appearance of Zeno points is given in [44], Theorem 2. Based onthis result, we study under which conditions the system (i.e., agents’) trajectories converge toa Zeno point. Consider the case with N = 2 agents. Denote the dynamics of the k -th agent as ˙ q k = f k ( q k ) , k ∈ { i, j } , q = (cid:2) q iT q jT (cid:3) T , r = (cid:2) r iT r jT (cid:3) T , and take: ∇ S i f ( q ) = 2 u i r ijT cos θ i sin θ i − u j r ijT cos θ j sin θ j , (33)where r ij = r i − r j . Note that the control law (30b) renders the orientation θ k of the k -th agentGES to the orientation ϕ k of the vector field F (cid:63)k . Thus, the unit vector [cos θ k sin θ k ] T coincides October 22, 2014 DRAFTEEE TRANSACTIONS ON 27 with the vector field F (cid:63)k ( r k ) , evaluated at r k ∈ R . With this at hand and after some algebraiccalculations one has: ∇ S − i f ( q ) = 2 u i r ijT F gi − u j r ijT F gj (cid:124) (cid:123)(cid:122) (cid:125) A , ∇ S + i f ( q ) = σ ij (cid:0) u i r ijT F gi − u j r ijT F gj (cid:1) + (1 − σ ij ) (cid:0) u i r ijT F ioj − u j r ijT F joi (cid:1)(cid:124) (cid:123)(cid:122) (cid:125) B . The set of Zeno points is: Z i = { r ∈ R N | ∇ S − i f ( q ) = ∇ S + i f ( q ) = 0 } , which reads: A = σ ij A + (1 − σ ij ) B = 0 ⇒ A = B = 0 ⇒ u i r ijT ( F gi − F ioj ) = u j r ijT ( F gj − F joi ) . Not surprisingly, the set Z i is depended on the current positions r i , r j and the goal locations q gi , q gj . The Zeno condition reduces to ( F gi + F gj ) = , which corresponds to current positions r i ( t ) , r j ( t ) and goal locations r gi , r gj lying on the same line. Then, the set of initial conditions(positions) from which agents’ trajectories converge to the set Z i is confined on R , i.e., on alower dimensional manifold, and as thus is of measure zero. The same analysis holds alongthe switching surface T i ( t ) , yielding exactly the same condition as before regarding on theappearance of Zeno points.The case of N > agents can be treated accordingly. Consider an agent i lying at distance d im ≤ d c w.r.t. M ≤ ( N − agents m (cid:54) = i . The vector field F (cid:63)i includes the repulsive effect F iom of all M connected agents. To check whether undesired singularities appear, one needs toconsider the norm (cid:107) F i (cid:107) in the blending region D i . The analytical expression is more involvedcompared to the N = 2 case. To make the probability of more than one agents lying in theblending region D i as low as possible, one can define the width d c − d r → . Define also: S im ( t ) : { r i ( t ) , r m ( t ) ∈ R | (cid:107) r i ( t ) − r m ( t ) (cid:107) − d c = 0 } the M ≤ ( N − switching surfaces of agent i w.r.t. its neighbors m . The conditions on theappearance of Zeno points around each switching surface read: ∇ S − im f ( r i , r m ) = ∇ S + im f ( r i , r m ) = 0 , ∀ m ∈ { , . . . , N } . October 22, 2014 DRAFTEEE TRANSACTIONS ON 28
This results in NM switching surfaces, since for any pair of agents ( i, m ) it holds that: S im = S mi ,and N M
Zeno conditions.Now, note that the NM Zeno conditions S − im f ( r i , r m ) = 0 introduce NM = N M unknownterms of the form r imT F gi , r imT F gm .In the same spirit, the NM Zeno conditions S + im f ( r i , r m ) = 0 additionally introduce NM ( M − N M ( M − unknown terms of the form r imT F iok , r imT F mok .In total, one has N M unknown terms and N M equations. Given that the N goal locations r gi , r gm , . . . , are known, the number of unknown terms reduces to N M − N M = N M ( M − .To have as many equations as unknown terms, it should hold that M − ⇒ M = 2 . Thisimplies that each agent i ∈ { , . . . , N } is connected with at most M = 2 agents; note that thisis irrespective of the total number of agents N . Then, the geometric conditions which result inZeno points are given as the solutions of the resulting linear system; these solutions express N M relations of the form r Tim F iok , r Tim F mok , which dictate the Zeno points, i.e., the Zeno positionsamong the N agents. Then, the set of initial conditions from which the agents converge to theseZeno positions are confined to a lower dimensional manifold, since they correspond to initialpositions confined on a line, and to a specific initial orientation for each agent, and as thus areof measure zero.Finally, let us note that the case of M > neighbors is not of interest for the proposed algorithm,as each agent i makes the avoidance decision w.r.t. the worst-case neighbor agent, i.e., w.r.t. theagent which is more susceptible to collision. This is realized via considering the safe velocity u i | m w.r.t. each neighbor agent m and taking the minimum over safe velocities in the definitionof the linear velocity control law (30a). The maximum function is defined to ensure that eachagent i will never be forced to move with negative linear velocity, i.e., backwards; this is toensure that there is no possibility of back-to-back colliding agents.In summary, the motion of each agent i remains collision-free w.r.t. its neighbor agents j ∈ N i under the control law (30), and each agent i converges to its goal location q gi almost globally,except for a set of initial configurations of measure zero. This completes the proof. Remark 10:
Theorem 5 justifies that the set of initial conditions for which the multi-robotsystem exhibits Zeno trajectories (chattering across a switching surface for infinite amount oftime) which result in robots getting stuck away from their goals, is of measure zero. To avoidsliding along a switching surface, which can be seen as “finite-time chattering”, one can employ
October 22, 2014 DRAFTEEE TRANSACTIONS ON 29 hysteresis logics [45].
B. Simulation Results
We consider N = 30 agents which are moving towards their goal locations (depicted withsquare markers) starting from goal positions (depicted with cross markers) while avoiding colli-sions, see the resulting paths in Fig. ?? . The goal locations are defined sufficiently far apart sothat the communication regions do not overlap when agents lie on their goal locations.VI. C ONCLUSIONS
This paper presented a novel methodology for the motion planning of unicycle robots inenvironments with obstacles, with extensions to the collision avoidance in multi-agent systems.The method is based on a family of vector fields whose integral curves exhibit attractive orrepulsive behavior depending on the value of a parameter. It was shown that attractive-to-the-goal and repulsive-around-obstacles vector fields can be suitably blended in order to yield almostglobal feedback motion plans in environments with circular obstacles. The case of collisionavoidance under local sensing/communication in multi-agent scenarios was also treated. Noparameter tuning is needed in order to avoid local minima, as needed in similar methods whichare based on scalar (potential) functions. Current work focuses on the definition of vector fieldsencoding input constraints, such as curvature bounds, which may be more appropriate for aircraftand car-like vehicles. R
EFERENCES [1] L. E. Parker, “Path planning and motion coordination in multiple mobile robot teams,” in
Encyclopedia of Complexity andSystem Science , R. A. Meyers, Ed. Springer, 2009.[2] H. Kress-Gazit, G. E. Fainekos, and G. J. Pappas, “Temporal-logic-based reactive mission and motion planning,”
IEEETransactions on Robotics , vol. 25, no. 6, pp. 1370–1381, Dec. 2009.[3] A. Bhatia, M. R. Maly, L. E. Kavraki, and M. Y. Vardi, “A multi-layered synergistic approach to motion planning withcomplex goals,”
IEEE Robotics and Automation Magazine , vol. 18, no. 3, pp. 55–64, 2011.[4] W. Ren and Y. Cao, “Overview of recent research in distributed multi-agent coordination,” in
Distributed Coordination ofMulti-agent Networks , ser. Communications and Control Engineering. Springer-Verlag, 2011, ch. 2, pp. 23–41.[5] D. Panagou, D. M. Stipanovi´c, and P. G. Voulgaris, “Vision-based dynamic coverage control for nonholonomic agents,”in
Proc. of the 53rd IEEE Conference on Decision and Control , Los Angeles, CA, Dec. 2014, p. to appear.[6] D. Panagou and K. J. Kyriakopoulos, “Viability control for a class of underactuated systems,”
Automatica , vol. 49, no. 1,pp. 17–29, Jan. 2013.
October 22, 2014 DRAFTEEE TRANSACTIONS ON 30
Fig. 7. Collision-free motion of 30 nonholonomic agents under the proposed control strategy.
October 22, 2014 DRAFTEEE TRANSACTIONS ON 31
Fig. 8. Collision-free motion of 30 nonholonomic agents under the proposed control strategy (Continued).[7] G. Leitmann and J. Skowronski, “Avoidance control,”
Journal of Optimization Theory and Applications , vol. 23, pp.581–591, Dec. 1977.[8] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,”
The International Journal of RoboticResearch , vol. 5, no. 1, pp. 90–98, Spring, 1986.[9] E. G. Hernandez-Martinez and E. Aranda-Bricaire, “Convergence and collision avoidance in formation control: A surveyof the artificial potential functions approach,” in
Multi-Agent Systems - Modeling, Control, Programming, Simulations andApplications , F. Alkhateeb, E. A. Maghayreh, and I. A. Doush, Eds. InTech, 2011, ch. 6, pp. 103–126.[10] D. M. Stipanovi´c, C. J. Tomlin, and G. Leitmann, “Monotone approximations of minimum and maximum functions andmulti-objective problems,”
Applied Mathematics and Optimization , vol. 66, pp. 455–473, 2012.[11] E. Rimon and D. Koditschek, “Exact robot navigation using artificial potential functions,”
IEEE Transactions on Roboticsand Automation , vol. 8, no. 5, pp. 501–518, Oct. 1992.[12] C. I. Connolly, J. B. Burns, and R. Weiss, “Path planning using Laplace’s equation,” in
Proc. of the 1990 IEEE Int. Conf.on Robotics and Automation , May 1990, pp. 2102–2106.[13] P. Szulczy´nski, D. Pazderski, and K. Kozłowski, “Real-time obstacle avoidance using harmonic potential functions,”
Journalof Automation, Mobile Robotics and Intelligent Systems , vol. 5, no. 3, pp. 59–66, 2011.[14] H. J. S. Feder and J.-J. E. Slotine, “Real-time path planning using harmonic potentials in dynamic environments,” in
Proc.of the 1997 IEEE Int. Conf. on Robotics and Automation , Albuquerque, New Mexico, Apr. 1997, pp. 874–881.[15] S. Waydo and R. M. Murray, “Vehicle motion planning using stream functions,” in
Proc. of the 2003 IEEE Int. Conf. onRobotics and Automation , Taipei, Taiwan, Sep., pp. 2484–2491.[16] A. D. Luca and G. Oriolo, “Local incremental planning for nonholonomic mobile robots,” in
Proc. of the 1994 IEEE Int.Conf. on Robotics and Automation , May 1994, pp. 104–110.[17] S. R. Lindemann and S. M. LaValle, “Simple and efficient algorithms for computing smooth, collision-free feedback lawsover given cell decompositions,”
The International Journal of Robotics Research , vol. 28, no. 5, pp. 600–621, 2009.[18] T. Liddy, T.-F. Lu, P. Lozo, and D. Harvey, “Obstacle avoidance using complex vector fields,” in
Proc. of the 2008Australasian Conference on Robotics and Automation , Canberra, Australia, Dec. 2008.[19] E. G. Hern´andez-Mart´ınez and E. Aranda-Bricaire, “Multi-agent formation control with collision avoidance based ondiscontinuous vector fields,” in
Proc. of the 35th Annual Conf. of IEEE Industrial Electronics , Nov. 2009, pp. 2283 –2288.
October 22, 2014 DRAFTEEE TRANSACTIONS ON 32 [20] W. Dixon, T. Galluzo, G. Hu, and C. Crane, “Adaptive velocity field control of a wheeled mobile robot,” in
Proc. of FifthInt. Workshop on Robot Motion and Control , Jun. 2005, pp. 145–150.[21] M. Mesbahi and M. Egerstedt,
Graph Theoretic Methods in Multiagent Networks . Princeton University Press, 2010.[22] W. Ren, R. W. Beard, and E. M. Atkins, “Information consensus in multi-vehicle cooperative control,”
IEEE ControlSystems Magazine , vol. 27, no. 2, pp. 71–82, apr 2007.[23] R. Olfati-Saber, J. A. Fax, and R. M. Murray, “Consensus and cooperation in networked multi-agent systems,”
Proceedingsof the IEEE , vol. 97, no. 1, pp. 215–233, 2007.[24] D. V. Dimarogonas and K. J. Kyriakopoulos, “Connectedness preserving distributed swarm aggregation for multiplekinematic robots,”
IEEE Transactions on Robotics , vol. 24, no. 5, pp. 1213–1223, Oct. 2008.[25] S. G. Loizou and K. J. Kyriakopoulos, “Navigation of multiple kinematically constrained robots,”
IEEE Transactions onRobotics , vol. 24, no. 1, pp. 221–231, Feb. 2008.[26] P. Lin, Y. Jia, and L. Li, “Distributed robust h ∞ consensus control in directed networks of agents with time-delay,” Systems & Control Letters , vol. 57, pp. 643–653, 2008.[27] J. Mei, W. Ren, and G. Ma, “Distributed containment control for lagrangian networks with parametric uncertainties undera directed graph,”
Automatica , vol. 48, pp. 653–659, 2012.[28] Z. Qu, C. Li, and F. Lewis, “Cooperative control with distributed gain adaptation and connectivity estimation for directednetworks,”
International Journal of Robust and Nonlinear Control , 2012.[29] J. liang Zhang, D. lian Qi, and M. Yu, “A game theoretic approach for the distributed control of multi-agent systemsunder directed and time-varying topology,”
International Journal of Control, Automation, and Systems , vol. 12, no. 4, pp.749–758, 2014.[30] Z. Li, G. Wen, Z. Duan, and W. Ren, “Designing fully distributed consensus protocols for linear multi-agentsystems with directed graphs,”
IEEE Transactions on Automatic Control, to appear , 2014. [Online]. Available:http://arxiv.org/abs/1312.7377v2[31] W. Ren, “On consensus algorithms for double-integrator dynamics,” in
Proc. of the 46th Conference on Decision andControl , New Orleans, LA, USA, Dec. 2007, pp. 2295–2300.[32] K. Liu, G. Xie, W. Ren, and L. Wang, “Consensus for multi-agent systems with inherent nonlinear dynamics under directedtopologies,”
Systems & Control Letters , vol. 62, no. 2, pp. 152–162, Feb. 2013.[33] D. Panagou, H. G. Tanner, and K. J. Kyriakopoulos, “Control of nonholonomic systems using reference vector fields,”in
Proc. of the 50th IEEE Conf. on Decision and Control and European Control Conf. , Orlando, FL, Dec. 2011, pp.2831–2836.[34] M. Henle,
A Combinatorial Introduction to Topology
IEEE Transactions on Robotics , vol. 30, no. 4, pp. 831–844, Aug. 2014.[37] D. Panagou, D. M. Stipanovi´c, and P. G. Voulgaris, “Multi-objective control for multi-agent systems using Lyapunov-like barrier functions,” in
Proc. of the 52nd IEEE Conference on Decision and Control , Florence, Italy, Dec. 2013, pp.1478–1483.[38] S. Maniatopoulos, D. Panagou, and K. J. Kyriakopoulos, “A MPC scheme for the navigation of a nonholonomic vehicle withfield-of-view constraints,” in
Proc. of the 2013 American Control Conf. , Washington DC, USA, Jun. 2013, pp. 3967–3972.
October 22, 2014 DRAFTEEE TRANSACTIONS ON 33 [39] D. Panagou, “Motion planning and collision avoidance using navigation vector fields,” in
Proc. of the 2014 IEEEInternational Conference on Robotics and Automation , Hong Kong, China, Jun. 2014, pp. 2513–2518.[40] W. M. Boothby,
An Introduction to Differentiable Manifolds and Riemannian Geometry - 2nd Edition . Academic Press,1986.[41] J. M. Lee,
Introduction to Smooth Manifolds . Springer, 2002.[42] X. Tricoche, G. Scheuermann, and H. Hagen, “A topology simplification method for 2D vector fields,” in
Proc. ofVisualization 2000 , Salt Lake City, UT, USA, Oct., pp. 359–366.[43] A. D. Ames and S. Sastry, “Characterization of Zeno behavior in hybrid systems using homological methods,” in
Proc. ofthe 2005 American Control Conf. , Portland, OR, USA, Jun. 2005, pp. 1160–1165.[44] F. Ceragioli, “Finite valued feedback laws and piecewise classical solutions,”
Nonlinear Analysis , vol. 65, no. 5, pp.984–998, 2006.[45] D. Liberzon,
Switching in Systems and Control . Birkhauser Boston, 2003. A PPENDIX