Acta Polytechnica doi:10.14311/APP.2016.56.0001 Acta Polytechnica 56(1):1–9, 2016 © Czech Technical University in Prague, 2016 available online at http://ojs.cvut.cz/ojs/index.php/ap FORMATION CONTROL OF MULTIPLE UNICYCLE-TYPE ROBOTS USING LIE GROUP Youwei Dong∗, Ahmed Rahmani Ecole Centrale de Lille, Cité Scientifique – CS20048, 59651 Villeneuve d’Ascq Cedex, France ∗ corresponding author: youwei.dong@ec-lille.fr Abstract. In this paper the formation control of a multi-robots system is investigated. The proposed control law, based on Lie group theory, is applied to control the formation of a group of unicycle-type robots. The communication topology is supposed to be a rooted directed acyclic graph and fixed. Some numerical simulations using Matlab are made to validate our results. Keywords: formation control; multi-robot system; unicycle-type robot; Lie group; Lie algebra. 1. Introduction The various ways to control and coordinate a group of mobile robots widely have been studied in recent years. This has brought a breadth of innovation, providing considerable attention for the potential applications, such as flocking systems control, surveillance, search and rescue, cooperative construction, distributed sen- sor fusion, etc. When comparing the mission out- come of a multi-robot system (MRS) to that of a single robot, it is clear that cooperation among multi- ple robots can perform complex tasks that it would otherwise be impossible for a single powerful robot to accomplish. The fundamental idea behind multi- robotics is to allow the individuals to interact with each other to find solutions of complex problems. Each of them senses the relative positions of his neighbors, and achieves the desired formation by controlling the relative positions [1–3]. In formation control, various control topologies can be adopted, depending on the specific environment and tasks. Theoretical views of MRS behavior are divided into centralized and decen- tralized systems. In a centralized system, a powerful core unit makes decisions and communicates with the others. In the decentralized approach, the robots can communicate and share information with each other [4]. We will focus on distributed system control due to its advantages, such as feasibility, accuracy, robustness, cost, and so on. Many studies have been devoted to the control and coordination of multi-agent systems and multi-robot systems (e.g. [1, 4–9]). Some of these results have been used to control vehicles (holonomic, nonholo- nomic mobile robots, etc.). In this paper, our goal is to control a group of unicycle robots to achieve a desired formation. Motivated by references [10–14], we focus on a rigid body with kinematics evolving on Lie groups. This is based on regarding the set of rigid body posture as the Lie group SE(2), which leads to a set of kinematic equations. These equations are expressed in terms of standard coordinated invari- ant linear operators on the Lie algebra se(2). This approach allows a global description of rigid body motion which does not suffer from singularities. It provides a geometric description of rigid motion which greatly simplifies an analysis of the mechanisms [10]. Paper [1] proposed an elegant control law based on Lie algebra theory for consensus of a multi-agent system. It has the holonomic constraints, while nonholonomic constraints are not considered. In [12], Lie algebra is used to study the path following control of one mobile robot. In [15], distributed formation control of multi-nonholonomic robots is studied. However the the control law is a leader-follower approach, and the multi-leader case is not considered. In this paper, a Lie group method is used to control multiple unicycle- type robots. The communication topology is defined as a rooted directed acyclic graph (DAG). Due to the nonholonomic property of this type of robot, a new local control law is proposed to make the nonlinear system converge to the desired formation. The outline of this paper is as follows. In Section 2, some preliminary results are summarized and the formation control problem for a group of unicycle-type robots is stated. In Section 3, a formation control strategy is proposed and the stability is analyzed. The simulation and results are given in Section 4. Concluding remarks are finally provided in Section 5. 2. Preliminary and problem statement 2.1. Lie groups Definition 1 [10]. A manifold of dimension of n is a set M, which is locally homeomorphic to Rn. A Lie group is a group G which is also a smooth manifold and for which the group operations (g,h) 7→ gh and g 7→ g−1 are smooth. Left action of G on itself Lg : G → G is defined by Lg(h) = gh, and right action is defined the same way. Adjoint action Adg : G → G is Adg(h) = ghg−1. Here are two examples, the special orthogonal group SO(n) = {R ∈ GL(n,R) : RRT = I,detR = +1} and the special Euclidean group SE(n) = {(p,R) : p ∈ Rn,R ∈ SO(n)} = Rn ×SO(n). 1 http://dx.doi.org/10.14311/APP.2016.56.0001 http://ojs.cvut.cz/ojs/index.php/ap Youwei Dong, Ahmed Rahmani Acta Polytechnica 2.2. Lie algebra associated with a Lie group A Lie algebra g over R is a real vector space g together with a bilinear operator [, ]: g×g (called the bracket) such that for all x,y,z ∈ g, we have: • Anti-commutativity: [x,y] = −[y,x]; • Jacobi identity: [[x,y],z] + [[y,z],x] + [[z,x],y] = 0. A Lie algebra g is said to be commutative (or abelian) if [x,y] = 0 for all x,y ∈ g. We can define adAB = [A,B] = AB − BA where A,B ∈ gl(n,R), which is the vector space of all n × n real matri- ces, gl(n,R) forms a Lie algebra. Clearly, we have [x,x] = 0. The Lie algebra of SO(2), denoted by so(2), may be identified with a 2×2 skew-symmetric matrix of the form ω̂ = [ 0 −ω ω 0 ] with the bracket structure [ω̂1, ω̂2] = ω̂1ω̂2−ω̂2ω̂1, where ω̂1, ω̂2 ∈ so(2). The Lie algebra of SE(2), denoted by se(2), can be identified with a 3 × 3 matrix of the form ξ̂ = [ ω̂ v 0 0 ] , where ω ∈ R,v ∈ R2, with the bracket [ξ̂1, ξ̂2] = ξ̂1ξ̂2 − ξ̂2ξ̂1. The exponential map: exp : TeG → G is a local diffeomorphism from the neighborhood of zero in g onto the neighborhood of e in G. The mapping t → exp(tξ̂) is the unique one-parameter subgroup R → G with tangent vector ξ̂ at time 0. For ω̂ ∈ so(2) and ξ̂ = (ω̂,v) ∈ se(2), we have exp ω̂t = [ cos ωt −sin ωt sin ωt cos ωt ] , (1) exp ξ̂t = [ exp ω̂t A(ω)v 0 1 ] , (2) where A(ω) = 1 ω [ sin ωt −(1 − cos ωt) (1 − cos ωt) sin ωt ] . 2.3. Graph theory The communication topology among N robots will be represented by a graph. Let G = (V,E,A) be a graph of order N with the finite nonempty set of nodes V(G) = {v1, . . . ,vN}, the set of edges E(G) ⊂V ×V, and an adjacency matrix A = (aij)N×N. If for all (vi,vj) ∈ E, (vj,vi) ∈ E as well, the graph is said to be undirected, otherwise it is called directed. Here, each node vi in V corresponds to a robot-i, and each edge (vi,vj) ∈E in a directed graph corresponds to an information link from robot-i to robot-j, which means that robot-j can receive information from robot-i. In contrast, the pairs of nodes in an undirected graph are unordered, where an edge (vi,vj) ∈E denotes that each robot can communicate with the other one. The adjacency matrix A of a digraph G is represented as A =   a11 a12 · · · a1n a21 a22 · · · a2n ... ... ... ... an1 an2 · · · ann   , where aij is the weight of link (vi,vj) and aii = 0 for any vi ∈ V, aij > 0 if (vi,vj) ∈ E and aij = 0 otherwise. A of a weighted undirected graph is de- fined by analogy, except that aij = aji,∀i 6= j [16]. A directed path from node vi to vj is a sequence of edges (vi,vj1), (vj1,vj2), . . . , (vjl,vj) in a directed graph G with distinct nodes vjk,k = 1, . . . , l. A directed graph is called acyclic if it contains no directed cycle. A rooted graph is a graph in which one vertex is distin- guished as the root. 2.4. Problem statement A unicycle-type mobile robot is composed of two in- dependent actuated wheels on a common axle which is rigidly linked to the robot chassis. In addition, there are one or several passive wheels (for example, a caster, Swedish or spherical wheel) which are not con- trolled and just serve for sustentation purposes [17]. We study the formation control problem of a group of such robots and each one is equipped with a local controller for deciding the velocities. We consider each robot as a node of a directed graph G, then the communication topology of a group of N robots could be expressed by an adjacency matrix A = (aij)N×N, where aii = 0 and aij = 1 if (vi,vj) ∈ E or 0 other- wise. The purpose is to design the strategy of the control applied to each robot in order that this group of mobile robots could execute a predefined task of formation control. 2.5. Kinematic model on a Lie group In order to describe the kinematic properties of the unicycle-type robot, we consider a reference point OR at the mid-distance of the two actuated wheels. Then we define two frames: FI = {O,X,Y} and FR = {OR,XR,YR}, as shown in Figure 1. FI = {O,X,Y} is an arbitrary inertial basis on the plane as the global reference frame and FR = {OR,XR,YR} is a frame attached to the mobile robot with its origin located at OR, and the basis {XR,YR} defines two axes relative to OR on the robot chassis and is thus the robot’s local reference frame. The position of the robot in the global reference is specified by coordinates xI and yI, and the angular difference between the global and local reference frames is given by θI. Then the pose of the robot could be described as an element of the Lie group SE(2): g = [ R p 0 1 ] =  cos θI −sin θI xIsin θI cos θI yI 0 0 1   , where p = [xI,yI]T denotes the position of the robot in the global reference frame, and R = [ cos θI −sin θI sin θI cos θI ] is the rotation matrix of the frame FR relative to frame FI. Then the motion of a robot could be described by g(t), which is a curve parameterized by time t in SE(2). 2 vol. 56 no. 1/2016 Formation Control of Multiple Unicycle-type Robots Using Lie Group X Y XRYR θ ω2 ω u O OR ω1 Figure 1. Representation of the frames. Each pure rotational motion of a robot on a plane can be given by a 2×2 orthogonal matrix R ∈ SO(2). Let ω ∈ R be the rotation velocity of the robot’s chassis and then the exponential map exp : so(2) → SO(2), ω̂ → exp(ω̂t) which is defined by Equation 1 where ω̂ = [ 0 −ω ω 0 ] ∈ so(2) correspond to the robot chassis rotation. This map represents the rotation from the initial (t = 0) configuration of the robot to its final configuration with the rotation velocity ω. The rigid motions consist of rotation and transla- tion. A general motion could also be described by an exponential map exp : se(2) → SE(2), ξ̂ → exp(ξ̂t) defined in Equation 2, where ξ̂ = [ ω̂ v 0 0 ] ∈ se(2) rep- resents the velocities of movement, and v = −ω̂p+ṗ =[ ωyI + ẋI −ωxI + ẏI ] where (xI,yI) is the position of the robot and v represents the velocity of a (possibly imaginary) point on the rigid body which is moving through the origin of the world frame. exp(ξ̂t) is a mapping from the initial configuration of the robot to its final configuration. That is, if we suppose that the initial configuration of the robot is g(0), then the final configuration is given by g(t) = eξ̂tg(0). (3) The kinematic model of the unicycle-type robot is given by   ẋI = u cos θI, ẏI = u sin θI, θ̇I = ω, where u characterizes the robot’s longitudinal velocity. The variables u and ω are related to the angular velocity of the actuated wheels via the one-to-one transformation:[ u ω ] = [ rw/2 rw/2 rw/L −rw/L ][ ω1 ω2 ] (4) where rw is the wheels’ radius, L is the distance be- tween the two actuated wheels, and ω1 and ω2 are respectively the angular velocity of the right wheel and the left wheel. We differentiate the matrix given in Equation 3, and obtain the kinematic model of a unicycle-type robot on the Lie group: ġ(t) = ξ̂g(t) (5) where ξ̂ is the control input matrix given by ξ̂ =  0 −ω ωyI + u cos θIω 0 −ωxI + u sin θI 0 0 0   . (6) This is the kinematic model on the Lie group for a unicycle-type robot. For one robot with a certain pose (xI,yI,θI), a control vector (u,ω) results in a unique control input matrix ξ̂ to update the robot’s motion. 3. Formation control law on SE(2) 3.1. Controller design We consider N unicycle-type mobile robots, and use gi ∈ SE(2) and ḡi ∈ SE(2) (i = 1, · · · ,N) to denote respectively the current configuration and the desired configuration of each robot. In fact, gi is the repre- sentation of the robot frame FR shown in Figure 1 relative to the spatial frame FI. As introduced in the previous section, the evolution of system gi can be expressed by ġi = ξ̂igi (7) where ξ̂i ∈ se(2) is the control input matrix. Let gij be the configuration of the robot-j frame relative to the robot-i frame, then we have gj = gigij. (8) Thus gij = g−1i gj. We can use ḡij to represent the desired configuration of the robot-j frame in the robot- i frame. Then the robots achieve a desired formation if their configurations satisfy the following equation for any k = 1, · · · ,N,k 6= i lim t→∞ g−1k gi = ḡki, i = 1, · · · ,N. (9) ḡki ∈ SE(2) is defined according to the task re- quirements and is often used to identify the geometric configuration of the formation. We study the move- ment of gi relative to gj, so here we can consider provisionally gj = ḡj, then ḡij could be written as ḡ−1i gj. Thus we have ḡ−1i gj = ḡ −1 i ḡj = ḡ −1 i (ḡkḡ −1 k )ḡj = (ḡ−1k ḡi) −1(ḡ−1k ḡj) = (ḡki) −1ḡkj which gives ḡi = gj(ḡkj)−1ḡki. Then for robot-i (in the local frame gi), the needed transformation of robot-i 3 Youwei Dong, Ahmed Rahmani Acta Polytechnica from the current configuration to the desired configu- ration while considering the current configuration of robot-j is g̃i_j = g−1i gj(ḡkj) −1ḡki. (10) To simplify the notations, we note g̃ij instead of g̃i_j. In [1], noting x̃ij = log g̃ij, a control law for agents, which have holonomic constraints, is proposed as ξ̂i = c ai N∑ j=1 aijx̃ij, i = 1, · · · ,N where aij is the element in the adjacency matrix A and ai = ∑N j=1 aij. However, in our MRS, nonholonomic constraints are associated with unicycle-type robots, so we develop a new nonlinear control approach. From the matrix g̃ij, we could know the position error and orientation error x̃ij, ỹij, θ̃ij. We suppose that the relative configuration of ḡi with respect to the robot frame gi is denoted by ḡii. Then ḡii could be obtained by the the mean function M : SE(2) ×···×SE(2)︸ ︷︷ ︸ N−1 → SE(2), (g̃i1, · · · , g̃i,i−1, g̃i,i+1, · · · , g̃iN ) 7→ g̃ii. This function means to get the weighted arithmetic mean of all the arguments, that is, if we note g̃ij =  cos θ̃ij −sin θ̃ij x̃ijsin θ̃ij cos θ̃ij ỹij 0 0 1   j=1,··· ,N,j 6=i , then x̃ii, ỹii, θ̃ii are given by: ∆ii = 1 ai N∑ j=1 aij∆ij, j 6= i, ∆ = x̃, ỹ, θ̃ (11) where aij is the element of adjacency matrix A and ai = ∑n j=1 aij. When x̃ii, ỹii and θ̃ii are obtained, g̃ii can be rewritten as g̃ii =  cos θ̃ii −sin θ̃ii x̃iisin θ̃ii cos θ̃ii ỹii 0 0 1   . We take the inverse of the matrix g̃−1ii which repre- sents the relative configuration of gi with respect to the desired configuration ḡi when the predefined com- munication topology is considered. Let us consider Figure 2, where the unknowns are annotated in the list of symbols after the article. O′X′Y ′ is the frame of the desired configuration of robot i, and (A,θ), related to g̃−1ii , is the current pose of robot i in the frame O′X′Y ′. In this frame, we assume a circle of radius |r|, denoted by CB, and then we propose a control law to drive the robot to the origin with the help of this circle. The absolute value |r| is always positive, and it is supposed appropriately according to the initial con- ditions. r is signed: when the robot is located in the lower half-plane, r = −|r| and thus the angle α is also A θ B O′ X′ Y′ Pr φ l P′ B′ O″ β α ψ d _ Figure 2. Geometrical relations between the robot actual configuration and the desired configuration. negative. The coordinate r is determined according to the following rules:  r is chosen arbitrarily without changing sign if |r| ≤ l/2, r = −y+sgn y √ 4y2+3x2 3 if |r| > l/2, (12) where the function “sgn” is defined as: sgn x = { 1, x ≥ 0, −1, x < 0. We denote β = arcsin sin β̄, then the local control law is proposed as follows:{ u = −sgn cos β̄λl ω = u|r|(β −α) (13) where λ is a positive constant. From the proposed law, we have ui and ωi, then the control input matrix of robot i is obtained from Equation 6. 3.2. Stability analysis From the previous section, we know that g̃ii is the representation of ḡi in frame gi, while its inverse g̃−1ii is the representation of gi in frame ḡi. To explain the convergence of gi to ḡi, we just need to prove that ḡ−1ii converges to the origin, which is also the identity matrix I. To prove this, with the help of the notations depicted in Figure 2, we will divide the movement of each robot into three phases. Phase 1. l ≥ 2|r|,β −α 6= 0. Lemma 1. If we choose a convenient r which satisfies l ≥ 2|r|, then the angle between the direction of movement and one tangent of the circle CB converges to 0, that is δ = |β −α|→ 0. 4 vol. 56 no. 1/2016 Formation Control of Multiple Unicycle-type Robots Using Lie Group Proof. If r > 0, we have δ = |β − α| = √ (β −α)2, then δ̇ = β −α√ (β −α)2 (β̇ − α̇) = sgn(β −α)(β̇ − α̇). Because β = arcsin sin β̄ = arcsin sin θ −ϕ, so β̇ = 1 1 − sin2(θ −ϕ) cos(θ −ϕ) (θ̇ − ϕ̇) = sgn cos β̄ (θ̇ − ϕ̇). Consider the coordinate transformation into polar coordinates, we have ϕ̇ = u sin β/l and l̇ = u cos β. We distinguish three cases: (1.) Case β̄ ∈ [−π2 , π 2 ]. In this case, the control law is u = −λl,ω = −λl(β −α)/r. And sin α = r l ⇒ α̇ = − rl̇ l2 cos α = λr cos β l cos α = λ cos β tan α. Then we have δ̇ = sgn(β −α) ( ω − u sin β l −λ cos β tan α ) = sgn(β −α)λ [ − l r (β −α) + sin β − cos β tan α ] . Suppose Eβ = − lr (β −α) + sin β − cos β tan α. Be- cause sin α = r/l and l ≥ 2r, so dEβ dβ < 0. Then we can say that Eβ is a monotonically decreasing function about β, and β = α is the unique zero value point of Eβ. Hence: • if −π2 ≤ β < α, then Eβ > 0, β − α < 0 and δ̇ ≤ 0; • if α ≤ β ≤ π2 , then Eβ ≤ 0, β −α > 0 and δ̇ ≤ 0. So δ converges monotonically to 0. (2.) Case β̄ ∈ (π2 ,π]. We have β = π−β̄ ∈ (0, π 2 ], and the control law is u = λl, ω = λl r (π − β̄ −α). In this case, we get δ̇ = sgn(β −α)λ [ sgn cos β̄ ( l r (π − β̄ −α) − sin β̄ ) + cos β̄ tan α ] . Suppose Eβ = sgn cos β̄[ lr (π − β̄ − α) − sin β̄] + cos β̄ tan α, then dEβ dβ = − l r + cos β + sin β tan α < 0. α = β = π − β̄ is the equilibrium point of Eβ, so δ converge monotonically to 0. (3.) Case β̄ ∈ [−π,−π2 ). We have β = −π − β̄, we can get a similar result to case 2. If r < 0, the same calculus leads to the same results. Hence we have the conclusion δ = |β −α|→ 0. A θ P′ O′ X′ Y′ B r φ l P Figure 3. Movement in phase 3. Phase 2. l ≥ 2|r|,δ = β −α = 0. Because of the regulation of phase 1, in this phase, the robot will moves towards the origin along the tangent of circle CB, thus δ = 0 and ω = u(β − α)/|r| = 0. Lemma 2. Suppose d = |O′A| and the Lyapunov function is chosen as V = 12d 2 + 12θ 2. If the robot moves towards the origin along the tangent of circle CB, then V̇ < 0. Proof. Consider the polar coordinates, we have ḋ = u cos(β̄ −ψ) sgn x sgn y = cos(β̄ −ψ)sxsy, where sx is sgn x for short. We find that if u < 0, then 0 < |β̄| ≤ π6 ; if u > 0, then 5π 6 ≤ |β̄| < π. Angle ψ is always positive. If l = 2|r|, ψ is maximal: ψmax = arccos(7/8) ≈ 0.5054, then no matter what sign x and y have, we have ḋ = −sgn cos β̄ λl cos(β̄ −ψsxsy) < 0. In this phase, δ = 0, so θ̇ = ω = 0. Hence V̇ = dḋ + θθ̇ < 0. The lemma is proved. Phase 3. l = 2|r|. In this phase, we have always l = 2r and β = α = π 6 (shown in Figure 3). We use y(x) to represent the movement of the robot and suppose r ≥ 0. The case r < 0 could be studied in the same way and the same conclusion will be obtained. (x,y) is the position of point A. Theorem 1. Suppose that one robot, with the veloc- ity defined by the proposed control law (Equation 13), moves towards the origin along the tangent of the circle CB (Figure 3) of which the radius |r| satisfies l = 2|r| and r is determined by rule (Equation 12), then both d and θ asymptotically converge to 0. 5 Youwei Dong, Ahmed Rahmani Acta Polytechnica Proof. We consider first the case where r > 0. In this case, l = 2r, (x,y) satisfies the equation x2 +(y−r)2 = 4r2. Then we get r = −y+ √ 3x2+4y2 3 (the negative solution is omitted). Using BP⊥PA, we could have y′ = dy dx = − 3x + √ 3(4y − √ 3x2 + 4y2) −3 √ 3x + 4y − √ 3x2 + 4y2 . This is an homogeneous differential equation. We suppose y = zx and differentiate it about x: dy dx = z + x dz dx = 3x + √ 3(4y − √ 3x2 + 4y2) 3 √ 3x− 4y + √ 3x2 + 4y2 = 3 + √ 3(4z − √ 3 + 4z2) 3 √ 3 − 4z + √ 3 + 4z2 . Simplify this result and get dx x = 3 √ 3 − 4z + √ 3 + 4z2 3 + √ z + 4z2 − ( √ 3 + z) √ 3 + 4z2 dz. Integrate it and get ln |x| = ∫ 3 √ 3 − 4z + √ 3 + 4z2 3 + √ z + 4z2 − ( √ 3 + z) √ 3 + 4z2 dz = √ 3 z ( − √ 1 + 43z 2 − 1 ) − 1 2 ln(z2 + 1) + arctanh z √ 3 + 4z2 + const, where “const” is a constant and its value is determined by the initial conditions. When x > 0, x = l cos ϕ, so ẋ = l̇ cos ϕ− lϕ̇ sin ϕ = −λl cos β − lλ sin β sin ϕ = − λl 2 ( √ 3 + sin ϕ) < 0. l = 0 is the equilibrium point, so x → 0. This is also the conclusion for x < 0. So we just need to consider the right neighborhood of the origin, hence x = exp (√ 3 z ( − √ 1 + 43z 2 − 1 ) − 1 2 ln(z2 + 1) + arctanh z √ 3 + 4z2 + const ) . Thus dx dz = x (−z3 + √ 3z2 + √ 3) √ 3 + 4z2 + 4z2 + 3 z2(z2 + 1) √ 3 + 4z2 . Solve the equation dx dz = 0, get z0 = 1 + √ 3. And when z < z0, dxdz > 0; when z > z0, dx dz < 0. So in the right neighborhood of the origin, dx dz < 0. Hence if z → 0, then x → 0, and we have the approximate relation between x and z: ln x = − 2 √ 3 z − z √ 3 − z2 2 + O(z3). Now z is very close to 0, so the terms of higher orders could be omitted and we get ln x = − 2 √ 3 z = − 2 √ 3x y . Thus y = −2 √ 3x ln x → 0 when x → 0. The inclination also converges to 0, because tan θ = dy dx ∼ d dx ( −2 √ 3 x ln x ) = −2 √ 3 ( 1 ln x − 1 ln2 x ) , when x → 0, tan θ → 0, then θ → 0. From the proposed control law, we know that l = 0 is the equilibrium point, and here it is demonstrated that when l → 0, the limit of d and θ are both 0. In the polar coordinate frame, we have l̇ = u cos β̄ = −sgn cos β̄ λl cos β̄ = −λl|cos β̄|, hence l → 0. With the trigonometric relations, we could prove that V̇ = dḋ + θθ̇ < 0, and the gain λ does not affect the stability. When r < 0, the same reasoning can be made. This completes the proof. Suppose D to be a length that has the same order of the workspace of the system and satisfies l ≤ D for any t ∈ R and any robot. The two wheel velocities are ω1 = 2u + ωL 2rw ,ω2 = 2u−ωL 2rw , and satisfy |ω1| ≤ ωmax, |ω2| ≤ ωmax, then we get the range of λ: 0 < λ ≤ λmax = rwωmax D + 2π3 µRchassis where Rchassis = L/2 is the radius of the robot chassis, and µ is a convenient number that satisfies l/|r| ≤ µ for all t. 3.3. Stability of formation control Because of the nonholonomic constraints, if there is a bidirectional path between any two unicycle-type robots which are equipped with this local control law, the system will not converge, so we propose a rooted directed acyclic graph as the communication topology of the multi-robot system and the theorem below. Theorem 2. If the communication topology between N unicycle-type robots is a rooted directed acyclic graph, then the system (Equation 7) will achieve the desired formation (Equation 9) under the local control law (Equation 11, 13). Especially, each robot, in phase 3, converges to the desired formation asymptotically. Proof. There is no directed circle, so the root node (robot) will not receive any information and will be static. Let Km denote the set of the nodes (robots) 6 vol. 56 no. 1/2016 Formation Control of Multiple Unicycle-type Robots Using Lie Group 1 2 3 4 5 6 Figure 4. Communication topology of simulation. to which there is a directed path from the root and this path consists of at most m edges. Then K0 has only one element – the root robot, denoted by v0. The configuration of this robot in the fixed frame is denoted by g0 = ḡ0. Then we use the mathematical induction method. (1.) For K1, suppose that there are n1 elements in K1. One element is denoted by v1i where 1 ≤ i ≤ n1, and the configuration of v1i is denoted by g1i. Because v1i receives information only from v0, according to the lemmas above and theorem 1 we know that limt→∞g−11i g0 = ḡ1i,0. (2.) For Km, the elements in this set are denoted by vmi, 1 ≤ i ≤ cm where cm is the cardinality of this set. vmi receives information from the nodes which are elements of ⋃ Kn,n≤m−1 and have achieved the desired configurations. We use j to denote the index numbers of these robots, that is, vnjj ∈ Knj ⊂⋃ Kn,n≤m−1. Then with the control law, vmi will converge to the desired configuration relative to vnjj, so lim t→∞ g−1mig0 = lim t→∞ g−1mignjjg −1 njj g0 = ( lim t→∞ g−1miḡnjj ) (ḡ−1njjḡ0) = ḡ−1(njj),miḡ(njj),0 = ḡmi,0. The topology graph is a finite graph, so all the robots will converge to the desired configuration rela- tive to v0. Then for any vi,vj, we have lim t→∞ g−1i gj = lim t→∞ g−1i g0g −1 0 gj = ḡ −1 0i ḡ0j = ḡij. Then the formation in Equation 9 is achieved. 4. Simulation Let us consider a group of 6 unicycle-type robots which are located in a global frame, and we suppose that each robot could know its own position and orientation in the frame via GPS or via a camera which is installed above the work area. The initial pose of each is p = (x,y,θ) where (x,y) represents the robot position in Figure 5. Trajectories of the 6 robots. the global frame and angle θ indicates the orientation of the robot. The six initial poses are given by p1 = (0, 0, 0), p2 = (5, 3, 0), p3 = (−1, 6,π/6), p4 = (6,−5,−π/2), p5 = (0,−5,π/3), p3 = (−5,−4,−π/2). and the desired formation is a regular hexagon with side length of 2. Let c = 1, the sample time is 0.1 s, and the maximum angular velocity of the wheels is ωmax = 5π/s. The communication topology is given in Figure 4. Using Matlab, the results are obtained as shown in Figure 5 and 6. We observe that the six robots achieve the desired hexagonal formation: robot-1 has no information source, so it remains static. Other robots perform the trajectories according to the pos- ture of their information source robots. Robots 4, 5 and 6 achieve the desired configurations after robots 2 and 3 because of the communication topology shape. Figure 6 shows the evolution of the angles between the forward direction of each robot and the X-axis of the global frame. We see that the six angles turn to the same value after some regulations of the con- figurations, which indicates the coordination of the robots’ orientations. The rotation velocities become 0 at the end. 5. Conclusions In this paper, we have studied the problem of forma- tion control for a group of unicycle-type robots using a Lie group. A local control law based on SE(2) for the robots is proposed, and the stability is analyzed. The problem is investigated under a rooted directed acyclic communication topology for a group of unicycle-type robots, and the behavior of the system is discussed. Some simulations of a 6-robot system validate the 7 Youwei Dong, Ahmed Rahmani Acta Polytechnica Figure 6. Orientation of the 6 robots. proposed control laws. The communication topology was supposed fixed. The case of switching topology, avoiding obstacles and an experiment on real robots will be studied in our future work. List of symbols u robot’s longitudinal velocity [m/s] ω robot’s chassis instantaneous velocity of rotation [rad/s] ω1,ω2 angular velocity of right and left wheel [rad/s] rω radius of wheel [m] L distance between the two actuated wheels [m] p = [xI,yI ]T position of robot in frame FI gi ∈ SE(2) configuration of local frame attached to robot- i relative to frame FI gij configuration of robot-j relative to the local frame attached to robot-i ḡij desired configuration of robot-j relative to robot-i B = (0,r) the centre of a circle of which the radius is |r| A = [x,y]T position of robot gi in frame O′X′Y ′ θ orientation of robot gi relative to axis O′X′ ϕ angle ∠ABX′ ∈ [−π,π] l distance between A and B d distance between A and O′ β̄ angle formed by −→ BA and robot’s orientation, ∈ [−π,π] α angle arcsin(r/l) ∈ [−π/2,π/2] ψ angle ∠BAO′ ∈ [0,π/2] Acknowledgements Youwei Dong was sponsored by the China Scholarship Council. Many thanks to members of the Math Stack Ex- change community, anonymous and otherwise, for helpful comments and suggestions. References [1] R. Dong, Z. Geng. Consensus based formation control laws for systems on lie groups. in Systems & Control Letters 62:104–111, 2013. doi:10.1016/j.sysconle.2012.11.005. [2] A. Gautam, S. Mohan. A review of research in multi-robot systems. In Industrial and Information Systems(ICIIS), pp. 1–5. Chennai, Aug 2012. doi:10.1109/ICIInfS.2012.6304778. [3] K.-K. Oh, H.-S. Ahn. Formation control of mobile agents based on distributed position estimation. in IEEE Transactions on Automatic Control 58(3):737– 742, March 2013. doi:10.1109/TAC.2012.2209269. [4] H. Mehrjerdi, J. Ghommam, M. Saad. Nonlinear coordination control for a group of mobile robots using a virtual structure. in Mechatronics 21:1147–1155, 2011. doi:10.1016/j.mechatronics.2011.06.006. [5] W. Ren. Consensus based formation control strategies for multi-vehicle systems. In Proceedings of the 2006 American Control Conference, pp. 4237–4242. Minnesota, June 2006. doi:10.1109/ACC.2006.1657384. [6] S. Mastellone, J. S. Mejía, D. M. Stipanović, M. W. Song. Formation control and coordinated tracking via asymptotic decoupling for lagrangian multi-agent systems. in Automatica 47:2355–2363, 2011. doi:10.1016/j.automatica.2011.08.030. [7] R. Olfati-Saber, R. M. Murray. Distributed cooperative control of multiple vehicle formations using 8 http://dx.doi.org/10.1016/j.sysconle.2012.11.005 http://dx.doi.org/10.1109/ICIInfS.2012.6304778 http://dx.doi.org/10.1109/TAC.2012.2209269 http://dx.doi.org/10.1016/j.mechatronics.2011.06.006 http://dx.doi.org/10.1109/ACC.2006.1657384 http://dx.doi.org/10.1016/j.automatica.2011.08.030 vol. 56 no. 1/2016 Formation Control of Multiple Unicycle-type Robots Using Lie Group structural potential functions. In IFAC World Congress. Barcelona, 2002. [8] K. D. Listmann, M. V. Masalawala, J. Adamy. Consensus for formation control of nonholonomic mobile robots, in robotics and automation. In Mathematical Physics, pp. 3886–3891. Kobe, May 2009. doi:10.1109/ROBOT.2009.5152865. [9] P. Morin, C. Samson. Control of nonholonomic mobile robots based on the transverse function approach. in Transactions on Robotics 25(5):1058–1073, Oct 2009. doi:10.1109/TRO.2009.2014123. [10] R. M. Murray, Z. Li, S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [11] F. W. Warner. Foundations of Differentiable Manifolds and Lie Groups. London: Scott, Foresman and Company, 1983. [12] P. Coelho, U. Nunes. Lie algebra application to mobile robot control: a tutorial. in Robotica 21:483–493, 2003. doi:10.1017/S0263574703005149. [13] J. F. Cariñena, J. Clemente-Gallardo, A. Ramos. Motion on lie groups and its applications in control theory. in Mathematical Physics 51:159–70, Jul 2003. doi:10.1016/S0034-4877(03)80010-1. [14] G. S. Chirikjian. Information theory on lie groups and mobile robotics applications. In IEEE International Conference on Robotics and Automation, pp. 2751–2757. Alaska, 2010. doi:10.1109/ROBOT.2010.5509791. [15] Z. Peng. Formation Control of Multi Nonhonomic Wheeled Mobile Robots. Ph.D. thesis, Ecole Centrale de Lille, june 2013. [16] G. Wen. Distributed Cooperative Control for Multi-Agent Systems. Ph.D. thesis, Ecole Centrale de Lille, October 2012. [17] B. Siciliano, O. Khatib. Handbook of Robotics. Springer, 2008. 9 http://dx.doi.org/10.1109/ROBOT.2009.5152865 http://dx.doi.org/10.1109/TRO.2009.2014123 http://dx.doi.org/10.1017/S0263574703005149 http://dx.doi.org/10.1016/S0034-4877(03)80010-1 http://dx.doi.org/10.1109/ROBOT.2010.5509791 Acta Polytechnica 56(1):1–9, 2016 1 Introduction 2 Preliminary and problem statement 2.1 Lie groups 2.2 Lie algebra associated with a Lie group 2.3 Graph theory 2.4 Problem statement 2.5 Kinematic model on a Lie group 3 Formation control law on SE(2) 3.1 Controller design 3.2 Stability analysis 3.3 Stability of formation control 4 Simulation 5 Conclusions List of symbols Acknowledgements References