Simultaneous Velocity and Position Estimation via Distance-only Measurements with Application to Multi-Agent System Control
SSimultaneous Velocity and Position Estimation viaDistance-only Measurements with Application toMulti-Agent System Control
Bomin Jiang, Mohammad Deghat,
Member, IEEE andBrian D.O. Anderson,
Life Fellow, IEEE
Abstract —This paper proposes a strategy to estimate the velocity andposition of neighbor agents using distance measurements only. Sincewith agents executing arbitrary motions, instantaneous distance-onlymeasurements cannot provide enough information for our objectives,we postulate that agents engage in a combination of circular motionand linear motion. The proposed estimator can be used to developcontrol algorithms where only distance measurements are available toeach agent. As an example, we show how this estimation method canbe used to control the formation shape and velocity of the agents ina multi agent system. Simulation results are provided to illustrate theperformance of the proposed algorithm.
I. I
NTRODUCTION
The performance of multi-agent systems in various tasks, e.g.consensus [1], [2], formation shape control [3], [4], cooperativegeolocalization [5], etc. has been studied with increasing intensityover recent years. These tasks are usually required to be performedin a decentralised way [6] and using limited information, i.e. eachagent should individually identify possible actions, and while suchactions are required to achieve the final goal of the formation, eachagent can communicate only with its neighboring agents. Examplesof these tasks are retrieving information from an area covered by asensor network (where the agents are sensors deployed in the area),or moving together in a desired formation shape from one point toanother where the agents are ground or aerial vehicles.In a formation control problem, which is the focus of this paper,each agent tries to contribute to achieve the global goal of theformation using measurements of, typically, relative position andvelocity of its neighbors. Examples of such problems are given in[7]–[10]. These problems become more challenging when the agentscannot instantaneously measure all the information required to applymotion corrections to achieve the final goal of the formation andhave to estimate some of this information using their measurements.An example of such a challenging problem is given in [11], wherea formation (shape and translation motion) control method, calledstop-and-go, has been devised to control the agents not able tomeasure the relative positions (both distance and angle) of theirneighbors, but only able to measure the distances to their respectiveneighbors. This measurement restriction makes the control problemsignificantly harder. Beyond that, this paper makes an assumptionthat the formation has a leader agent whose velocity is constant,and the followers take up positions while moving with the samevelocity as the leader. *This work is supported by National ICT Australia, which is funded by theAustralian Research Council through the ICT Centre of Excellence program.Bomin Jiang and Brian D. O. Anderson are with ResearchSchool of Engineering, The Australian National UniversityandNational ICT Australia. Mohammad Deghat is with the Schoolof Engineering and Information Technology, the University ofNew South Wales and used to be with the ANU and NICTA. (e-mail: { U5225976,Brian.Anderson } @anu.edu.au,[email protected]) This paper treats a related problem. Agents are required toestimate the relative position and velocity of their neighbor agentsusing only distance measurements to the neighbors, and achieveboth velocity consensus and formation shape control. The key isto postulate that the motion of each agent comprises two parts: atranslation and a circular motion. The circular motion is arounda moving center, and it is the centers of each agent’s motion,rather than the agents themselves, which achieve velocity consensus.The purpose of the superimposed circular motion is to allow inter-agent localization and velocity estimation, not using instantaneousmeasurements, but using distance measurements collected over aninterval. We postulate that neighbor agents remain in communicationeven if they initially have different velocities.The notion of using deliberate motions of agents to assist inlocalization was suggested in [12], in relation to sensor networklocalization. The idea in [12] is that if each node in a sensornetwork moves in a small neighbourhood of its original position, it ispossible to infer direction information from distance measurements.Our idea is similar; however, the motions in [12] are randomwhile this paper studies the localization problem using distance-only measurements when agents are executing independent circularmotions and it further discusses the situation where agents areperforming a combination of circular motion and linear motion, withthe linear motion components required to achieve velocity consen-sus. In addition, the idea of introducing sinusoidal perturbation information control problems is not wholly novel: in [13], the authorshave introduced sinusoidal perturbations to the usual gradient basedcontrol algorithm in order to achieve a different objective. Anadvantage of having a combination of linear and circular motionover only linear motion as in [11] is that the agents are less likelyto travel out of communication range during the localization process.An abbreviated conference version of this paper has been pre-sented in [14]. The novel contributions of the paper, in comparisonto the conference paper [14], are as follows (a) proposing adiscrete time control algorithm to achieve velocity consensus andsimultaneous formation shape control, which can be used whendistance-only measurements are available, and (b) introducing animprovement by adaptively adjusting the circular motion radius.The rest of this paper is organised as follows. Section II givesa solution to the location and velocity estimation problem us-ing distance-only measurements when each agent is executing acombination of linear motion and circular motions. Section IIIdiscusses an improvement of the algorithm derived in the previoussection involving an adaptively adjusting the circular motion radius.Section IV discusses a discrete time control algorithm to achievevelocity consensus and formation shape control with distance-onlymeasurements. Simulation is included in each section. Concludingremarks and directions for future research are given in Section V. a r X i v : . [ c s . S Y ] M a y I. R
ELATIVE POSITION AND VELOCITY ESTIMATION USINGSINUSOIDAL PERTURBATION
In the conference version of this paper [14], we gave detailedexplanation on how to infer neighbouring agents’ relative positionand velocity. We have also discussed special cases very carefully.Here we just give a brief introduction to the ideas.
A. Problem statement
Consider two point agents, 1 and 2. Each agent performs acombination of circular and rectilinear motion, so each has a certainradius, direction and angular velocity for the circular motion andvelocity for the rectilinear motion. Agent 1 knows its own radius,angular velocity and the translational velocity of its circle centreand can only measure (continuously) the distance but not bearing ofagent 2. Conversely, agent 2 knows its radius, angular velocity andthe velocity of its circle centre and can only measure the distanceof agent 1. The goal is for both agents to localize and sense thevelocities of each other for velocity consensus purposes.
𝐴𝑔𝑒𝑛𝑡 1
𝐴𝑔𝑒𝑛𝑡 2 𝑟 ,𝜔 𝑟 ,𝜔 𝑥 𝑦 𝑣 𝑥 𝑣 𝑦 𝜙 𝜙 Fig. 1. Set up a coordinate system with respect to agent 1’s circle centre
As shown in Fig. 1, we set up (for analysis purposes by us) aglobal coordinate system with origin at agent 1’s circle centre andagent 2’s circle centre on the x axis when t = 0 . Suppose r i isthe radius of agent i ’s motion, ω i is the angular velocity of agent i , z ( t ) is the distance at time t between agent 1 and 2 and d isthe distance between the two circle centres. The coordinate systemis defined by the agent pair, and is used for analysis purposes byus. Its orientation with respect to agent 1’s local coordinate basisis not known by agent 1 at this stage though the orientation canbe obtained after that agent learns φ . In addition, let v c i be thevelocity of agent i ’s circle centre, v ji be the relative velocity ofagent j ’s circle centre with respect to agent i ’s circle centre, v x bethe x component of the velocity v , and v y be the y componentof the velocity v . The positive direction of angular velocities iscounter-clockwise.We assume in this paper that v x and v y are constant for kT
In the system comprising a pair of agents 1 and 2, without loss ofgenerality, we only show how agent 1 can localize and estimate therelative velocity of agent 2. The first step is for agent 1 to identifythe angular velocity of agent 2, using a Fourier representation of z ( t ) for t ∈ [0 , T ] . When (cid:107) v (cid:107) is sufficient small in comparisonto r , r and d , four distinct peaks will show up at 0, | ω | , | ω | and | ω − ω | in frequency domain. This allows agent 1 to pick upthe angular velocity of agent 2. More insights about this assumptionwill be discussed in Section III; detailed explanation about how toidentify the angular velocity is given in [14].In order to identify the value of d , φ , v x and v y , we allow agent1 to measure the distance between the two agents z ( t ) and analysethe Fourier series of the periodic extension of z ( t ) . Lemmas 1 and2 show the Fourier series of some summands arising in (2) andLemma 3 will provide the tool to show that these summands arelinearly independent and can be identified separately. Theorem 1gives details of the procedure to identify d , φ , v x and v y . Lemma 1.
Suppose d , v , ω and T are positive constants with T = k πω for some positive integer k and f ( t ) = ( d + vt ) cos ( ω t ) ∀ t ∈ [0 , T ] . Define f (cid:48) ( t ) to be the periodic extension of f ( t ) such that ∀ t ∈ [0 , T ) , f (cid:48) ( t ) = f ( t ) and ∀ t ∈ ( −∞ , ∞ ) , f (cid:48) ( t ) = f (cid:48) ( t + T ) . Let c n be the coefficients of its Fourier series f (cid:48) ( t ) = (cid:80) ∞ n = −∞ c n e j πnt/T Then, if n = k , there holds c k = 12 ( d + 12 vT ) + vT πk j (3) and if n (cid:54) = k and n > there holds c n = vT j π ( 1 n − k + 1 n + k ) (4) Proof.
The lemma above can be proved in a straightforward mannerby calculating the value of c n = 1 T (cid:90) T ( d + vt ) cos ( ω t ) e − jn πT t dt (5) Lemma 2.
Suppose a , b and T are positive constants (with T not necessarily a multiple of π/ω ). Define f ( t ) = at + bt ∀ t ∈ [0 , T ] . Note the domain of definition of f ( t ) is bounded.Define f (cid:48) ( t ) to be the periodic extension of f ( t ) such that ∀ t ∈ [0 , T ) , f (cid:48) ( t ) = f ( t ) and ∀ t ∈ ( −∞ , ∞ ) , f (cid:48) ( t ) = f (cid:48) ( t + T ) . Let c n be the coefficients of the Fourier series f (cid:48) ( t ) = ∞ (cid:88) n = −∞ c n e i πnt/T Then, for all n = ± , ± , · · · , there holds c n = aT π n + aT + Tb πn j ( j = − )Proof. The lemma can be proved in a straightforward manner bycalculating the value of c n = 1 T (cid:90) T ( at + bt ) e − jn πT t dt (6) Lemma 3.
Suppose n , n , n , n , k and k are six differentpositive integers. Then the matrix n n n − k + n + k n − k + n + k n n n − k + n + k n − k + n + k n n n − k + n + k n − k + n + k n n n − k + n + k n − k + n + k (7) s full rank.Proof. The lemma can be proved in a straightforward manner bycalculating the value of the matrix determinant.In the following theorem, we show that each agent can estimatethe position and translational velocity of the other agent usingdistance-only measurements over an interval of time T . For nowwe assume that the angular velocities of agents 1 and 2 arecommensurate. We later explain what happens if ω and ω areincommensurate. Theorem 1.
For a pair of point agents in R , if each agent isexecuting a combination of circular motion and linear motion andthe associated angular frequencies are commensurate, each agentcan find the position and translational velocity of the other agentby distance-only measurements over an interval.Proof. The definitions of r , r , ω , ω , d , z , v x , v y , φ and φ are the same as in Section II-A. We choose T so that thereexist integers k , k defining the multiple which T represents of theperiods associated with the two angular velocities, i.e. k = ω T π and k = ω T π . The existence of k and k relates to the conceptof commensurable numbers, see Remark 3 in [14].Suppose one continuously measures z for a time period T andfinds the Fourier series of the periodic extension of z . Consider (2)and suppose c n are the coefficients of Fourier series of the periodicextension of z , s n are the coefficients of Fourier series of theperiodic extension of ( d x + d y + r + r ) , u n are the coefficients ofFourier series of the periodic extension of − d x r cos ( ω t + φ ) − d y r sin ( ω t + φ ) and w n are the coefficients of Fourier series ofthe periodic extension of d x r cos ( ω t + φ )+2 d y r sin ( ω t + φ ) .From (2) we know that for any n > ∩ n (cid:54) = | k − k | thereholds c n = s n + u n + w n (8)Note the coefficients of the Fourier series of the term − r r cos [( ω − ω ) t + ( φ − φ )] in (2) are all zero except forthe index n = | k − k | .Define constants: U = r ( j v x T π + v y T π ) e j ( φ + π ) (9)and W = r ( j v x T π + v y T π ) e j ( φ ) (10)Suppose further that R = ( v x + v y ) T π (11)and I = ( v x + v y ) T + 2 v x dT π (12)From Lemma 1, Lemma 2 and (2) we know for any n > ∩ n (cid:54) = k , k or | k − k | there holds c n = 1 n R + 1 n Ij + ( 1 n − k + 1 n + k ) · U + ( 1 n − k + 1 n + k ) · W (13)From (13) and Lemma 3 we know that if we have four values of c n , n > ∩ n (cid:54) = k , k or | k − k | , we are able to find the uniquesolutions of R , I , U and W . Because d , v x and v y are all realnumbers, ideally R and I should also be real numbers. Sometimes due to noise or error, the R and I obtained from matrix operationsmay be complex numbers, but this will not affect the process below.Now we have the value of U and W and can obtain u k .Furthermore, from Lemma 1 and (9) we know that u k − U k = r d + 12 v x T − v y T j ) e j ( φ + π ) (14) U = r ( j v x T π + v y T π ) e j ( φ + π ) (15)and d , v x , v y and φ can be found from these equations.The solutions for d and φ are given by d = 2 r | u k − U k + πjU | (16) φ = arg ( u k − U k + πjU ) + π (17)and the solutions for v x and x y are given by v x = Im( 4 πUT r e j ( φ + π ) ) (18) v y = Re( 4 πUT r e j ( φ + π ) ) (19) Remark 1.
In the special situation where there are no rotations andboth agents are executing linear motion, the absolute value of therelative velocity and distance between these agents can be obtainedfrom the Fourier series of the term ( d x + d y + r + r ) in (2) but thedirection cannot be found. This result is the same as the situationdescribed in Section 5.2.1 of the previous paper [11]. Remark 2.
When ω and ω are incommensurate , then z ( t ) in (2) is an almost periodic function [15] and one cannot have T = k πω = k πω with k /k a rational number. Thus at least one ormaybe both of k and k are not integers. Now T should be chosen(and, as guaranteed by the theory of almost periodic functions, itcan be so chosen by taking it sufficiently large) to ensure that both k , k are close to integers (and indeed one may be an integer).Then the Fourier coefficients in Lemma 1 and Lemma 2 are different;their expressions have extra additive terms which are small if thedeviation of k and k from integer numbers are small. Thus inTheorem 1 we can still find d , v x , v y and φ with some error whichis also small if the deviations of k and k from integer numbersare small. The longer T is, the more accurate the results are. III. A
SSIGNING R ADIUS A DAPTIVELY
In the above sections, we let each agent infer the position andrelative velocity information of neighbouring agents by 1) carryingout a Fourier transform and then 2) identifying peaks to estimate ω of neighbouring agents 3) solving the set of linear equations (13).In step 2) if (cid:107) v (cid:107) T is sufficiently small in comparison to r , r ,we can show that there are always peaks at k and k . Lemma 4.
Adopt the hypothesis in Theorem 1 and consider (8) .There exists a positive real number α such that if d > r , r >α · (cid:107) v (cid:107) , then c n (regarded as a function of the integer n ) haspeaks at n = k and n = k . Two non-zero real numbers a and b are said to be commensurable if a/b is a rational number. roof. When n (cid:54) = 0 , k , k , | k − k | , the dependence of (cid:107) c n (cid:107) on (cid:107) v (cid:107) can be expressed as follows (cid:107) c n (cid:107) = (cid:13)(cid:13)(cid:13) h ( k , k , T ) (cid:107) v (cid:107) + h ( k , k , T ) (cid:107) v (cid:107) d + h ( k , k , T ) (cid:107) v (cid:107) r + h ( k , k , T ) (cid:107) v (cid:107) r (cid:13)(cid:13)(cid:13) . (20)On the other hand, when n = k , there holds (cid:107) c n (cid:107) = (cid:13)(cid:13)(cid:13) h ( k , k , T ) (cid:107) v (cid:107) + h ( k , k , T ) (cid:107) v (cid:107) d + h ( k , k , T ) (cid:107) v (cid:107) r + h ( k , k , T ) dr (cid:13)(cid:13)(cid:13) (21)where h , , ··· , ( k , k , T ) are all bounded functions for integer k , k and T > .Now compare (20) and (21). For a large enough α the term h ( k , k , T ) dr will be dominant and thus there will be peaksrecognised at n = k . Similarly there will also be peaks recognisedat n = k .According to Lemma 4, the proposed algorithm works if d >r , r > α · (cid:107) v (cid:107) . In reality, d > r , r is automatically satisfiedif we aim to avoid collision. Furthermore, in order to ensure that r , r > α ·(cid:107) v (cid:107) holds for each agent pair, we propose an adaptiveradius algorithm whereby r i of each agent re-set at the end of each T second intervals as follows r i (cid:16) ( k + 1) T (cid:17) = α · max j {(cid:107) v ij ( kT ) (cid:107)} (22)where j denotes the indeces of neighbouring agents of i and α is asufficient large value. Note that because r i only changes at the endof each interval T , the radius is fixed within each interval.The adaptive radius law will ensure that r , r > α · (cid:107) v (cid:107) holds for each agent pair. Furthermore, as velocity consensus isbeing achieved, (cid:107) v ij ( kT ) (cid:107) will approach zero and so will r i .It is noticeable that the accuracy of estimation of (cid:107) v ij ( kT ) (cid:107) isindependent of the value of radius. Each agent can estimate the norm of velocities of neighbours’ circle centres via R according to(11), even if no peaks are identified. This phenomenon is consistentwith the paper [11], which shows that without circular motions, foragents only doing linear motions, it is possible to estimate the normof relative velocities of neighbours, even though the directions areleft unknown.When there are sudden changes in velocities of agents due to e.g.wind or deliberate change of course by a leader agent, an alreadyachieved consensus and formation may be broken. In this case, evenif the radius of the circle of each agent has already approached tozero, each agent can still obtain a good estimate of the absolutevalue of velocities of its neighbours’ circle centres. This can resultin an increase of radius of circular motions in response to the brokenconsensus, which allows the agents to achieve velocity consensusand formation shape control again.A simple demonstration of this idea is shown in the figure below.Similarly to the setting in Section V-B in [14], consider a multi-agentsystem shown in Fig. 2, suppose ω i is the angular velocity of agent i , T is the sampling time interval, ( v xi , v yi ) is the translationalvelocity of agent i and ( p xi , p yi ) is the position of circle centreof agent i . In the simulation, we set ω = ω = 5 , ω = − , T = 2 π . When t = 0 , ( v x , v y ) = ( − , , ( v x , v y ) = (3 , − , ( v x , v y ) = (2 , , ( p x , p y ) = (70 , , ( p x , p y ) = (0 , , ( p x , p y ) = (0 , and ε = 0 . . All the parameters are in SIunits.Fig. 3 and Fig. 4 show a simulation where there is a suddenchange in velocity of agent 2 at t = 20 T . In Fig. 3, it is clearthat the radius of circular motions of agents increases in response Fig. 2. A example of multi-agent system to the broken consensus. In Fig. 4, it is shown that the consensusof translational velocities is re-achieved after this sudden change. −40 −20 0 20 40 60 80 100 120050100150200250 position (m) po s i t i on ( m ) agent 1agent 2agent 3 Fig. 3. Re-achieved consensus v e l o c i t y ( m / s ) velocity of agent 1velocity of agent 2velocity of agent 3 0 100 200 300−3−2−101234 velocities in y directiontime (sec) v e l o c i t y ( m / s ) velocity of agent 1velocity of agent 2velocity of agent 3 Fig. 4. Velocities of circle centres in re-achieved consensus
IV. C
OMBINING V ELOCITY C ONSENSUS AND F ORMATION S HAPE C ONTROL
A. Stability of discrete time control algorithm
In [16], an algorithm is derived to combine velocity consensuswith formation shape control. The algorithm deals with a continuoustime problem and is in the form ˙ p i = v i ˙ v i = (cid:88) j ∈N i ( v j − v i ) + 2 (cid:88) j ∈N i ( d ∗ ij − d ij )( p i − p j ) (23)where p i is the position of the i th agent, i = 1 , · · · , N , v i is thevelocity of the i th agent and N i is the set of neighbouring agentsof agent i . Further, d ∗ ij is the desired distance between agent i and j and d ij is the current distance between agent i and j . In ourcontext, agent positions and velocities refer to the centre of theircular motion. The system equations (23) can be written in thematrix form ˙ p = v ˙ v = − ( L ⊗ I ) v + f ( p ) (24)where p ∈ R N is the vector of all p i stacked together, L denotesthe Laplacian matrix which is positive semi-definite and has onezero eigenvalue when the graph is connected and undirected, and f ( p ) is a vector with the entries (cid:80) j ∈N i ( d ∗ ij − d ij )( p i − p j ) , i = 1 , · · · , N .In our case, the algorithm cannot be implemented directly becausewe let each agent measure distance for a time period T and thenmake a velocity adjustment at the end of each such interval. Adiscrete version of (23) for our use is given by ˙ p i = v i v i (cid:0) ( k + 1) T (cid:1) = v i ( kT ) + (cid:15) T (cid:88) j ∈N i (cid:16) v j ( kT ) − v i ( kT ) (cid:17) + 2 (cid:15) T (cid:88) j ∈N i (cid:16) d ∗ ij − d ij ( kT ) (cid:17)(cid:16) p i ( kT ) − p j ( kT ) (cid:17) (25) where (cid:15) , (cid:15) are suitably small positive constants; more informationis given below. Note that the first equation remains in continuoustime while the second equation is discretised. However, since v i ( t ) is constant over an interval T , it follows that the discretisation ofthe first equation, viz. p i (cid:0) ( k + 1) T (cid:1) = p i ( kT ) + T v i ( kT ) exactlyinterpolates the continuous function p i ( t ) for t = kT with integer k . To show the stability of (25), we start with the continuous-timesystem and make the following transformation ¯ p r = Rp, ¯ v r = Rv (26)where R is an orthonormal matrix whose first two rows are ( ⊗ I ) (cid:62) / √ N , ¯ p r := [ p (cid:62) ¯ p (cid:62) ] (cid:62) with p ∈ R and v := [ v (cid:62) ¯ v (cid:62) ] (cid:62) with v ∈ R . Then ˙ v = 0 , that is the position of the center of mass ofthe agents in ¯ p -coordinates is constant, and the system equations in ¯ p and ¯ v are ˙¯ p = ¯ v ˙¯ v = L ¯ v + ¯ f (¯ p ) (27)where L is the (2 N − × (2 N − nonzero block of − R ( L⊗ I ) R (cid:62) which is negative definite and ¯ f (¯ p ) contains the nonzero entries of Rf ( R (cid:62) ¯ p ) = Rf ( p ) .Our approach to show the stability of (25) is as follows: firstwe define a Malkin structure in Definition 1. After that we showin Lemma 5 that (27) has Malkin structure. Then we develop inTheorem 2 a discrete-time version of the continuous-time Malkin’stheorem as invoked by Krick [17]. Finally, we use these results andshow in Theorem 3 that (25) is stable for sufficiently small valuesof (cid:15) and (cid:15) . Definition 1 (Malkin structure) . A system has Malkin structure ifit is in the form ˙ r = (cid:20) A (cid:21) r + g ( θ, ρ ) , r = (cid:20) θρ (cid:21) , g = (cid:20) Θ( θ, ρ ) P ( θ, ρ ) (cid:21) (28) where A has eigenvalues with negative real parts. Furthermore, g ( θ, ρ ) is a second order term with the following conditions i) g ( θ,
0) = 0 , ii) there exists h ( θ ) = lim ρ → Θ( θ, ρ ) (cid:107) ρ (cid:107) , h ( θ ) = lim ρ → P ( θ, ρ ) (cid:107) ρ (cid:107) , There are minor differences in the definition of Malkin structure indifferent references. We use the definition in [17] here. b = (cid:40) Θ( θ,ρ ) (cid:107) ρ (cid:107) if ρ (cid:54) = 0 h ( θ ) if ρ = 0 , b = (cid:40) P ( θ,ρ ) (cid:107) ρ (cid:107) if ρ (cid:54) = 0 h ( θ ) if ρ = 0 such that b and b are bounded smooth functions and b (0) = 0 ; Lemma 5.
The system equations in (27) can be transformed toa Malkin structure through a local diffeomorphism around theequilibrium point of (27) . The proof is provided in Appendix I.
Theorem 2.
Consider the time-discretized version of Malkin struc-ture in Definition 1, where θ k and ρ k are the k th sample of thequantities θ and ρ in Definition 1. Then there exists a sufficientlysmall sampling time interval (cid:15) (certainly with (cid:15) < ), and asufficiently small open ball V around the origin such that if ( θ , ρ ) lies in this open ball, then ( θ k , ρ k ) lies in the ball for all k and ρ k → exponentially fast. The proof is provided in Appendix II.
Theorem 3.
Consider the system of equations in (25) and supposethe graph associated with the velocity measurements is connectedand undirected. Then (25) is stable for sufficiently small (cid:15) and (cid:15) .Proof. To show the stability of (25) for suitable (cid:15) i , we initiallystudy certain variants on (27) and examine their stability. First, ifthe second equation of (27) is replaced for some positive α, β by ˙¯ v = αL ¯ v + β ¯ f (¯ p ) (29)the convergence properties are unaffected. Of course, the speed ofconvergence is changed.Second, if (29) is replaced for any (cid:15) > by ˙ˆ p = ˆ v ˙ˆ v = (cid:15)αL ˆ v + (cid:15) β ¯ f (ˆ p ) (30)or alternatively by ˙ˆ p = (cid:15) ˆ w ˙ˆ w = (cid:15)αL ˆ w + (cid:15)β ¯ f (ˆ p ) (31)then any solution of (29) gives rise to solutions of (30) and (31)and vice versa through (cid:20) ¯ p ( (cid:15)t ) (cid:15) ¯ v ( (cid:15)t ) (cid:21) = (cid:20) ˆ p ( t )ˆ v ( t ) (cid:21) = (cid:20) ˆ p ( t ) (cid:15) ˆ w ( t ) (cid:21) (32)The discrete-time equation with which we are working in (25)is a discretisation of (30) (after some transformation and withappropriate identification of (cid:15) , (cid:15) ). Now if the original equation(29) is approximated by a difference equation with sampling interval h , this is equivalent to sampling (31) with sampling interval h/(cid:15) ,or sampling (30) with the same sampling interval. In particular,if h is such that discretisation of (29) gives solutions whichconverge exponentially fast to the center manifold associated withthat equation, then with discretisation interval h/(cid:15) , solutions of thediscretised version of (30) or (31) will also converge exponentiallyfast to the center manifold. In particular, if (cid:15) is chosen so that h/(cid:15) = T , then for that value of (cid:15) and with the sampling interval T , the desired convergence will occur. In summary, if α, β areprescribed, and if a sampling interval h is chosen so that thediscretised version of (29) converges to the centre manifold, thentaking (cid:15) = h/T, (cid:15) = α(cid:15), (cid:15) = β(cid:15) will be satisfactory in (25). Ofcourse, α = β = 1 is legitimate; with (cid:15) small, the values of (cid:15) , (cid:15) will be such that velocity consensus is effectively achieved beforethe correct shape. This is intuitively reasonable.o the question arises as to whether discretisation of (29) with asufficiently small sampling interval will give convergence. Lemma5 shows that (27) can be transformed to a Malkin structure andtherefore (29). Furthermore, Theorem 2 shows that the discretizationof Malkin structure with a sufficiently small sampling intervalwill give convergence. We further show in Appendix III that theoperations of coordinate basis change through a diffeomorphism toa Malkin equation and time-discretization commute. Therefore, thetheorem is proved. B. Simulation Results combining Velocity Consensus and formationshape control
Consider a three-agent system where each agent can measure itdistance to the other two agents. The goal is to achieve velocityconsensus and form a triangular formation. Suppose ω i is theangular velocity of agent i , T is the sampling time interval, ( v xi , v yi ) is the translational velocity of agent i and ( p xi , p yi ) is theposition of circle centre of agent i . In the simulation, we set ω = 5 , ω = − , ω = 7 , T = 2 π . When t = 0 , ( v x , v y ) = ( − , . , ( v x , v y ) = (3 , − . , ( v x , v y ) = (2 , . , ( p x , p y ) =(100 , , ( p x , p y ) = (0 , , ( p x , p y ) = (0 , , (cid:15) = 5 × − and (cid:15) = 7 × − . The desired distance between each pair of agentsin the formation is 20. The trajectories of the agents are shown inFig. 5 and the velocities of agents are shown in Fig. 6. −50 0 50 100 150020406080100120140160 position (m) po s i t i on ( m ) agent 1agent 2agent 3 Fig. 5. Results of combining velocity consensus and formation shape controlwith adaptive radius setting: The trajectories of agents
V. C
ONCLUSION
In this paper, we proposed a strategy to achieve velocity con-sensus and formation control using distance-only measurements formultiple agents. Given the fact that for agents to execute arbitrarymotions, instantaneous distance-only measurements cannot provideenough information for achieving velocity consensus and formationcontrol, we studied agents performing a combination of circularmotion and linear motion.In further research, we are looking to achieve formation controland velocity consensus using agents’ perturbations, such that agentsare not limited to perform a combination of circular motion andlinear motion. In addition, it appears very likely that the samestrategy as we proposed in this paper can be used in velocityconsensus using bearing-only measurements.A
PPENDIX I Proof of Lemma 5:
Suppose there are N agents in a formation.Consider the formation control system ˙ p = f ( p ) (33) v e l o c i t y ( m / s ) velocity of agent 1velocity of agent 2velocity of agent 3 0 100 200 300−4−3−2−101234 velocities in y directiontime (sec) v e l o c i t y ( m / s ) velocity of agent 1velocity of agent 2velocity of agent 3 Fig. 6. Results of combining velocity consensus and formation shape controlwith adaptive radius setting: The translational velocities of agents where f ( p ) is a vector with entries (cid:88) j ∈N i ( d ∗ ij − d ij )( p i − p j ) , i = 1 . · · · , N and d ij , d ∗ ij , p i and p j are as defined in (23). It is shown in [17]that there is a local diffeomorphism around the equilibrium pointthat transforms (33) to a Malkin structure.Because there is a linear mapping between this p and the ¯ p defined in (26) and the text below (26), we know there is alsoa diffeomorphism r = φ (¯ p ) , ¯ p = ψ ( r ) (34)which transforms ˙¯ p = ¯ f (¯ p ) , where ¯ f ( · ) is defined in the text below(27), to a Malkin structure. ˙ r = (cid:20) A (cid:21) r + g ( θ, ρ ) , r = (cid:20) θρ (cid:21) , g = (cid:20) Θ( θ, ρ ) P ( θ, ρ ) (cid:21) (35)where A has eigenvalues with negative real parts and g ( θ, ρ ) fulfillsthe conditions of second order term g ( · ) in Definition 1. Let n θ denote the number of elements in θ and n ρ denote the number ofelements in ρ .Observe ˙ r = ∂φ∂ ¯ p ˙¯ p = ∂φ∂ ¯ p ¯ f (¯ p ) = (cid:16) ∂ψ∂r (cid:17) − ¯ f ( ψ ( r )) . (36)Now the right side of (35) and (36) are the same. This is becauseequation (35) is the formation control system after applying a localdiffeomorphism. As a result, we obtain a further equation linkingthe function ¯ f ( · ) , g ( · , · ) , φ ( · ) and the matrix A . ∂φ∂ ¯ p ¯ f (¯ p ) = (cid:20) A (cid:21) r + g ( θ, ρ ) (37)Note the above equation reflects a property regarding ¯ f ( · ) and thediffeomorphism (34). Therefore, it is available as to draw on inconsidering the transformation of the second order system.Now we are going to show that the velocity and formation shapecontrol problem in (27) ¨¯ p = L ˙¯ p + ¯ f (¯ p ) , L = L T < (38)is also transformed to a Malkin structure by the same diffeomor-phism stated in (34). In the second order system, we are using thesame diffeomorphism stated in (34), but we need to additionallyknow how ˙¯ p is mapped. It is easy to obtain r = ∂φ∂ ¯ p ˙¯ p (39)and then ¨ r = Φ(¯ p, ˙¯ p ) + ∂φ∂ ¯ p ¨¯ p (40)where the row i column j entry of Φ(¯ p, ˙¯ p ) takes the form (cid:88) k ∂ φ i ∂ ¯ p j ∂ ¯ p k ˙¯ p j ˙¯ p k . Combining (40) and (38), we obtain ¨ r = Ψ( r, ˙ r ) + ∂φ∂ ¯ p L ˙¯ p + ∂φ∂ ¯ p ¯ f (¯ p ) (41)where Ψ( r, ˙ r ) = Φ (cid:0) ψ ( r ) , ∂ψ∂r ˙ r (cid:1) is O ( (cid:107) ˙ r (cid:107) ) . Together with (37),which as noted above remains valid, we obtain ¨ r = Ψ( r, ˙ r ) + (cid:16) ∂ψ∂r (cid:17) − L (cid:16) ∂ψ∂r (cid:17) ˙ r + (cid:20) A (cid:21) r + g ( θ, ρ ) . (42)Now we have the system equation ddt (cid:20) r ˙ r (cid:21) = C (cid:20) r ˙ r (cid:21) + (cid:20) r, ˙ r ) + h · ˙ r + g ( θ, ρ ) (cid:21) (43)where C = I (cid:20) A (cid:21) (cid:16) ∂ψ∂r (cid:17) ∗− L (cid:16) ∂ψ∂r (cid:17) ∗ and h = − (cid:16) ∂ψ∂r (cid:17) ∗− L (cid:16) ∂ψ∂r (cid:17) ∗ + (cid:16) ∂ψ∂r (cid:17) − L (cid:16) ∂ψ∂r (cid:17) where ∗ denotes the value at system equilibrium. Observe that C takes the following form C = (cid:20) D E (cid:21) (44)with E a ( n θ + 2 n ρ ) × ( n θ + 2 n ρ ) nonsingular square matrix.Suppose there is a nonsingular similarity transformation T = (cid:20) I − DE − I (cid:21) and define (cid:20) ¯ r ˙ r (cid:21) = T (cid:20) r ˙ r (cid:21) . Note ¯ r = (cid:20) ¯ θ ¯ ρ (cid:21) .There holds ddt ¯ θ (cid:20) ¯ ρ ˙ r (cid:21) = (cid:20) E (cid:21) ¯ θ (cid:20) ¯ ρ ˙ r (cid:21) + T · o (¯ r, ˙ r ) (45)with (cid:20) E (cid:21) = T CT − .Krick points out the general conclusion in [17] that first applyinga diffeomorphism r = φ (¯ p ) and then linearizing the transformedsystem is equivalent to first linearizing the system and then applyingthe diffeomorphism. While this idea was applied to the singleintegrator system, it remains valid for the double integrator system.It is shown in [18] that the system (38) is locally exponentiallystable on a centre manifold, therefore the linearization of the systemequation (38) at a point on the center manifold (cid:34) I (cid:16) ∂ ¯ f∂ ¯ p (cid:17) ∗ L (cid:35) haseigenvalues with non-positive real parts. Furthermore, because thelocal diffeomorphism is smooth, its linearization around the equi-librium (cid:34) ∂φ∂p ∂φ∂p (cid:35) is a non-singular similarity transformation.Therefore, the linearization of the system equation after applyingthe diffeomorphism C = (cid:34) ∂φ∂p ∂φ∂p (cid:35) (cid:32)(cid:34) I (cid:16) ∂ ¯ f∂ ¯ p (cid:17) ∗ L (cid:35)(cid:33) (cid:34) ∂φ∂p ∂φ∂p (cid:35) − also has eigenvalues with non-positive real parts. With (44) and thefact that A and L are both full rank, we know E is a non-singularsquare matrix. Thus E has eigenvalues with negative real parts.Define l (¯ θ, ¯ ρ, ˙ r ) = o (¯ r, ˙ r ) = Ψ( r, ˙ r ) + h · ˙ r + g ( θ, ρ ) because1) Ψ is O ( (cid:107) ˙ r (cid:107) ) h = 0 when ¯ ρ = 0 and ˙ r = 0 g ( θ, ρ ) fulfills the conditions of second order term g ( · ) inDefinition 1we can conclude (in relation to the double integrator system) that l (¯ θ, ¯ ρ, ˙ r ) fulfills the conditions for the second order term g ( · ) ofDefinition 1. Therefore, we have completed the proof.A PPENDIX II Proof of Theorem 2:
The time discretized version of Malkin structure takes the fol-lowing form (cid:20) θ k +1 ρ k +1 (cid:21) = (cid:18) (cid:15) (cid:20) A (cid:21)(cid:19) (cid:20) θ k ρ k (cid:21) + (cid:15) (cid:20) Θ( θ k , ρ k ) P ( θ k , ρ k ) (cid:21) (46)where A has eigenvalues with negative real parts, Θ( θ,
0) = 0 and P ( θ,
0) = 0 . Define h ( θ k ) = lim ρ k → Θ( θ k , ρ k ) (cid:107) ρ k (cid:107) , h ( θ k ) = lim ρ k → P ( θ k , ρ k ) (cid:107) ρ k (cid:107) ,b = (cid:40) Θ( θ k ,ρ k ) (cid:107) ρ k (cid:107) if ρ k (cid:54) = 0 h ( θ k ) if ρ k = 0 , b = (cid:40) P ( θ k ,ρ k ) (cid:107) ρ k (cid:107) if ρ k (cid:54) = 0 h ( θ k ) if ρ k = 0 Because lim ρ → P (0 ,ρ ) (cid:107) ρ (cid:107) = 0 , we know that b (0) = 0 . Since A has eigenvalues with negative real parts, for all sufficiently small τ > , the matrix A d := I + τ A will have eigenvalues insidethe unit circle. Without loss of generality, we may assume (using anonsingular similarity transformation T if necessary, correspondingto a replacement of ρ k by T ρ k ) that for some γ > , there holds I − A (cid:62) d A d ≥ γI (47)Now set V ( ρ k ) = ρ (cid:62) k ρ k . Also, note that given any σ > ,there exists η ( σ ) and a closed ball ¯ B η , without loss of generalitycontained in V , such that || b ( ρ k , θ k ) || ≤ σ ∀ ( ρ k , θ k ) ∈ ¯ B η (48)Now observe that for ( ρ k , θ k ) ∈ B η there holds V ( ρ k +1 ) − V ( ρ k ) (49) = ρ (cid:62) k ( I − A (cid:62) d A d ) ρ k + 2 τ ρ (cid:62) k A (cid:62) d P ( ρ k , θ k ) + τ || P ( ρ k , θ k ) || ≤ − γρ (cid:62) k ρ k + 2 τ || A d |||| ρ k || || b ( ρ k , θ k ) || + τ |||| ρ k || || b ( ρ k , θ k ) || ≤ ( − γ + 2 τ σ + τ σ ) || ρ k || ≤ ( − γ + 2 σ + σ ) || ρ k || Restrict σ to be small enough that σ + σ < γ/ . Then we achieve: V ( ρ k +1 ) − V ( ρ k ) ≤ − ( γ/ V ( ρ k ) (50)and || ρ k +1 || ≤ (1 − ( γ/ || ρ k || (51)rovided that the sequence ( ρ k , θ k ) remains in B η , exponentialconvergence to zero of ρ k is achieved. We shall now argue that thiscan be assured through appropriate selection of the initial condition.Suppose to obtain a contradiction that there exists a finite K suchthat ( ρ k , θ k ) ∈ B η ∀ k ∈ [0 , K ] but the condition fails for k = K +1 .Suppose that the function b , which is continuous, attains an upperbound of ¯ m on ¯ B η . Observe that for all k ∈ [0 , K ] , || Θ( ρ k , θ k ) || = || ρ k |||| b ( ρ k , θ k ) || (52) ≤ ¯ m || ρ || (1 − ( γ/ k which implies by summation that || θ k +1 || ≤ ¯ m || ρ || − ( γ/
2) + || θ || (53)Now restrict the initial condition ( ρ , θ ) to lie-in a smaller ballthan B η . Define a η < η as a positive quantity satisfying η + ¯ m − ( γ/ η + η < η (54)and suppose that ( ρ , θ ) ∈ B η . Then while the trajectory ( ρ k , θ k ) remains in B η , ie. for all k ∈ [0 , K ] with K maximal, we knowusing (51), (53) that || ( ρ k +1 , θ k +1 ) || ≤ || ρ k +1 || + || θ k +1 || (55) ≤ η + ¯ m − ( γ/ η + η < η This shows that ( ρ K +1 , θ K +1 ) ∈ B η , and that K is not maximal,i.e. there cannot be a finite K . Hence exponential convergence ofthe sequence ρ k and convergence of the sequence θ k is established.A PPENDIX
III
Lemma 6.
Consider a differential equation ˙ p = f ( p ) , with theproperty that a coordinate change through the diffeomorphism r = φ ( p ) produces a differential equation set in Malkin form. Supposethat this set is then time-discretized to obtain a discrete-time Malkinequation. Consider also the time-discretization of the equation ˙ p = f ( p ) followed by use of the diffeomorphic coordinate change r = φ ( p ) . Then the transformed discrete-time equation is the same asthat obtained as the discrete-time Malkin equation referred to above,i.e. the operations of diffeomorphic coordinate change to a Malkinequation and time-discretization commute.Proof. Consider the Malkin structure in (28). Let ψ ( r ) = p be theinverse transformation to r = φ ( p ) . All points ρ = 0 are equilibriumpoints, and therefore all points p = ψ ( θ, are equilibrium pointsof the equation for p . It follows that f ( ψ ( θ, . Now considerthe following discretisation of the differential equation for p : p k +1 = p k + (cid:15)f ( p k ) (56)with sufficiently small (cid:15) . Under the mapping r = φ ( p ) , with J φ theJacobian of φ ( p ) , we have r k +1 = φ ( p k + (cid:15)f ( p k )) = φ ( p k ) + (cid:15)J φ f ( p k ) + o ( (cid:15) )= r k + (cid:15)J φ f ( p k ) + o ( (cid:15) ) (57)where o ( (cid:15) ) denotes higher order terms of (cid:15) . We must show this is ofa Malkin form. It is straightforward to conclude that the linear partof the discrete time equation is of a Malkin form. In order to showthat the nonlinear part also has this property, what we must show isthat if ρ k = 0 , then r k +1 = r k . This will happen if and only if thehigher order terms on the right of the difference equation go to zerowhen ρ k = 0 . Accordingly, suppose ρ k = 0 . Then we know that p k = ψ ( θ k , is an equilibrium point of the differential equation for p , and so f ( p k ) = 0 . It follows that the difference equation forwhich r k +1 = φ ( p k + (cid:15)f ( p k )) (58)actually has r k +1 = φ ( p k + 0) = φ ( p k ) = r k . Hence in (57), theremainder terms of higher order in (cid:15) all go to zero when ρ k goesto zero, therefore we have completed our proof.R EFERENCES [1] W. Ren, R. W. Beard, and E. M. Atkins, “Information consensus inmultivehicle cooperative control,”
IEEE Control Systems , vol. 27, no. 2,pp. 71–82, 2007.[2] R. Olfati-Saber, J. A. Fax, and R. M. Murray, “Consensus and coop-eration in networked multi-agent systems,”
Proceedings of the IEEE ,vol. 95, no. 1, pp. 215–233, 2007.[3] B. D. O. Anderson, C. Yu, B. Fidan, and J. Hendrickx, “Rigidgraph control architectures for autonomous formations,”
IEEE ControlSystems , vol. 28, no. 6, pp. 48–63, 2008.[4] W. Ren and Y. Cao,
Distributed coordination of multi-agent networks:emergent problems, models, and issues . Springer, 2010, vol. 83.[5] H. Wymeersch, J. Lien, and M. Z. Win, “Cooperative localization inwireless networks,”
Proceedings of the IEEE , vol. 97, no. 2, pp. 427–450, 2009.[6] G. Foderaro and S. Ferrari, “Necessary conditions for optimality fora distributed optimal control problem,” in . IEEE, 2010, pp. 4831–4838.[7] B. D. O. Anderson and C. Yu, “Range-only sensing for formation shapecontrol and easy sensor network localization,” in
Chinese Control andDecision Conference (CCDC) . IEEE, 2011, pp. 3310–3315.[8] L. Krick, M. E. Broucke, and B. A. Francis, “Stabilization of infinitesi-mally rigid formations of multi-robot networks,”
International Journalof Control , vol. 82, no. 3, pp. 423–439, 2009.[9] T. H. Summers, C. Yu, S. Dasgupta, and B. D. O. Anderson, “Controlof minimally persistent leader-remote-follower and coleader formationsin the plane,”
IEEE Transactions on Automatic Control , vol. 56, no. 12,pp. 2778–2792, 2011.[10] D. V. Dimarogonas, E. Frazzoli, and K. H. Johansson, “Distributedevent-triggered control for multi-agent systems,”
IEEE Transactionson Automatic Control , vol. 57, no. 5, pp. 1291–1297, 2012.[11] M. Cao, C. Yu, and B. D. O. Anderson, “Formation control usingrange-only measurements,”
Automatica , vol. 47, no. 4, pp. 776–781,2011.[12] Y. Zhu, S. Gortler, and D. Thurston, “Sensor network localization usingsensor perturbation,”
ACM Transactions on Sensor Networks (TOSN) ,vol. 7, no. 4, p. 36, 2011.[13] Y.-P. Tian and Q. Wang, “Global stabilization of rigid formations inthe plane,”
Automatica , vol. 49, no. 5, pp. 1436–1441, 2013.[14] B. Jiang, M. Deghat, and B. Anderson, “Translational velocity consen-sus using distance-only measurements,” in , Dec 2013, pp. 2746–2751.[15] C. Corduneanu and V. Barbu,
Almost periodic functions . ChelseaPublishing Company, Incorporated, 1989.[16] B. D. Anderson, Z. Lin, and M. Deghat, “Combining distance-basedformation shape control with formation translation,”
Developments inControl Theory Towards Glocal Control, Eds. L. Qiu, J. Chen, T.Iwasaki and H. Fujioka, , pp. 121–130, 2012.[17] L. Krick, “Application of graph rigidity in formation control of multi-robot networks,” Master’s thesis, University of Toronto, 2007.[18] M. Deghat, B. D. O. Anderson, and Z. Lin, “Combined flocking anddistance-based shape control of multi-agent formations,”