FORMATION CONTROL OF MULTIPLE UNICYCLE-TYPE ROBOTS USING LIE GROUP

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 rooted directed acyclic graph and fixed. Some numerical simulations using Matlab are made to validate our results.


Introduction
The various ways to control and coordinate widely a group of mobile robots have been studied in recent years and brought a breadth of innovation, providing a considerable attention for the potential applications, such as flocking systems control, surveillance, search and rescue, cooperative construction, distributed sensor fusion, etc.When comparing the mission outcome of a multi-robot system (MRS) to that of a single robot, it is clear that multiple robots cooperation can perform complex tasks that would otherwise be impossible for one 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][2][3].In formation control, different control topologies can be adopted depending on the specific environment and tasks.Theoretical views of MRS behavior are divided between centralized and decentralized 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 the 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][5][6][7][8][9]).Some of these results have been used to control vehicles (holonomic, nonholonomic mobile robots,. . .).In this paper, our goal is to control a group of unicycle robots to achieve a desired formation.Motivated by the references [10][11][12][13][14], we focus on the rigid body with kinematics evolving on Lie groups which is based on regarding the set of rigid body posture as the Lie group SE(2) which leads to a set of kinematic equations that are expressed in terms of standard coordinated invariant linear operators on the Lie algebra se (2).This approach allows a global description of rigid body motion which does not suffer from singularities, and provides a geometric description of rigid motion which greatly simplifies the analysis of mechanisms [10].The work [1] proposed an elegant control law based on Lie algebra theory for consensus of multi-agent system which 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 leaderfollower approach and multi-leader case is not considered.In this paper, Lie group method is used to control multiple unicycle-type robots.The communication topology is defined as 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.

Lie algebra associated with 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: A Lie algebra g is said to be commutative (or abelian) if [x, y] = 0 for all x, y ∈ g.We can define ad A B = [A, B] = AB − BA where A, B ∈ gl(n, R) which is the vector space of all n × n real matrices, 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 3 × 3 matrix of the form ξ = ω v 0 0 , where The exponential map: exp : T e G → G is a local diffeomorphism from a neighborhood of zero in g onto a 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) where sin ωt .

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) = {v 1 , . . ., v N }, the set of edges E(G) ⊂ V × V, and an adjacency matrix A = (a ij ) N ×N .If for all (v i , v j ) ∈ E, (v j , v i ) ∈ E as well, the graph is said to be undirected, otherwise it is called directed.Here, each node v i in V corresponds to a robot-i, and each edge (v i , v j ) ∈ 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 (v i , v j ) ∈ E denotes that each robot can communicate with the other one.The adjacency matrix A of a digraph G is represented as , where a ij is the weight of link (v i , v j ) and a ii = 0 for any rooted graph is a graph in which one vertex is distinguished as the root.

Problem statement
A unicycle-type mobile robot is composed of two independent 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, caster, Swedish or spherical wheel) which are not controlled 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 = (a ij ) N ×N , where a ii = 0 and The purpose is to design the strategy of control applied to each robot in order that this group of mobile robots could execute a predefined task of formation control.

Kinematic model on Lie group
In order to describe the kinematic properties of the unicycle-type robot, we consider a reference point O R at the mid-distance of the two actuated wheels.Then we define two frames: is an arbitrary inertial basis on the plane as the global reference frame and  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 1where ω = 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 translation.A general motion could also be described by an exponential map exp : se(2) → SE( 2 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 The kinematic model of the unicycle-type robot is given by 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: where r w is the wheels' radius, L the distance between the two actuated wheels, and ω 1 and ω 2 are respectively the angular velocity of the right and left wheel.
We differentiate the matrix given in Equation 3, and obtain the kinematic model of unicycle-type robot on Lie group: ġ(t) = ξg(t) (5) where ξ is the control input matrix given by ξ = This is the kinematic model on Lie group for the unicycle-type robot.For one robot with certain pose (x I , y I , θ I ), a control vector (u, ω) results in a unique control input matrix ξ to update the robot's motion.

Controller design
We consider N unicycle-type mobile robots, and use ) to denote respectively the current configuration and the desired configuration of each robot.In fact g i is the representation of the robot frame F R shown in Figure 1 relative to the spatial frame F I .As introduced in the previous section, the evolution of the system g i can be expressed by ġi = ξi g i (7) where ξi ∈ se(2) is the control input matrix.Let g ij be the configuration of the robot-j frame relative to the robot-i frame, then we have Thus g ij = g −1 i g j .We can use ḡij to represent the desired configuration of robot-j frame in the robot-i frame.Then the robots achieve a desired formation if their configurations satisfy the following equation for ḡki ∈ SE( 2) is defined according to the task requirements and is often used to identify the geometric configuration of the formation.We study the movement of g i relative to g j , so here we can consider provisionally g j = ḡj , then ḡij could be written as ḡ−1 i g j .Thus we have which gives ḡi = g j (ḡ kj ) −1 ḡki .Then for robot-i (in the local frame g i ), the needed transformation of robot-i from current configuration to the desired configuration while considering the current configuration of robot-j is gi_j Youwei Dong, Ahmed Rahmani http://robotics.fel.cvut.cz/demur15/

DEMUR'15
To simplify the notations, we note gij instead of gi_j .In the work [1], noting xij = log gij , a control law for agents, which have holonomic constraints, is proposed as where a ij is the element in the adjacency matrix A and a i = N j=1 a ij .However in our MRS, nonholonomic constraints are associated with the unicycle-type robots, so we develop a new nonlinear control approach.From the matrix gij , we could know the position error and orientation error xij , ỹij , θij .We suppose that the relative configuration ḡi with respect to the robot frame g i is denoted by ḡii which could be obtained by the the mean function which means to get the weighted arithmetic mean of all the arguments, that is, if we note where j = 1, . . ., N and j = i, then xii , ỹii , θii are given by: where a ij is the element of adjacency matrix A, a i = n j=1 a ij and c > 0 is a proportional gain of the control input which does not influence the analysis of stability.Here we choose c = 1.We take the inverse of the matrix g−1 ii which represents the relative configuration of g i with respect to the desired configuration ḡi when the predefined communication topology is considered.Let us consider the 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−1 ii , 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 C B , then propose a control law to drive the robot on to this circle and move to the origin finally, at the same time, the orientation θ should converge to 0.
The absolute value |r| is always positive, and it is supposed appropriately according to the initial conditions.r is signed: when the robot is located in the lower half-plane, r = −|r| and thus the angle α is also negative.The coordinate r is determined according to the following rules: where the function "sign" is defined as: We note β = arcsin(sin( β)), then the local control law is proposed as follows: where λ is a positive constant.From the proposed law, we have u i and ω i , then the control input matrix of robot i is obtained from Equation 6.

Stability analysis
From the previous section, we know that gii is the representation of ḡi in the frame g i , while its inverse g−1 ii is the representation of g i in the frame ḡi .To explain the convergence of g i to ḡi , we just need to prove that ḡ−1 ii converge to the origin which is also the identity matrix I. To prove that, with the help of the notations depicted in Figure 2, we will divide the movement of each robot into three phases.
Because sin(α) = r/l and l ≥ 2r, so dE β dβ < 0. Then we can say that E β is a monotonically decreasing function about β and We have and the control law is 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 the point A.
Theorem 1: Suppose that one robot, with the velocity defined by the proposed control law (Equation 13), moves towards the origin along the tangent of the circle C B (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. Proof: We consider first the case where r > 0. In this case, l = 2r, (x, y) satisfies the equation x 2 + (y − r) 2 =

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 DEMUR'15 http://robotics.fel.cvut.cz/demur15/Formation Control of Multiple Unicycle-type Robots Using Lie Group K m denote the set of the nodes (robots) to which there is a directed path from the root and this path consists of at most m edges.Then K 0 has only one elementthe root robot, denoted by v 0 .The configuration of this robot in the fixed frame is denoted by g 0 = ḡ0 .Then we use the mathematical induction method.
For K 1 : suppose that there are n 1 elements in K 1 .One element is denoted by v 1i where 1 ≤ i ≤ n 1 , and the configuration of v 1i is denoted by g 1i .Because v 1i receives information only from v 0 , according to the lemmas above and theorem 1 we know that lim t→∞ g −1 1i g 0 = ḡ1i,0 .For K m : the elements in this set are denoted by v mi , 1 ≤ i ≤ c m where c m is the cardinality of this set.v mi receives information from the nodes which are elements of K n,n≤m−1 and have achieved the desired configurations.We use j to denote the index numbers of these robots, that is, v nj j ∈ K nj ⊂ K n,n≤m−1 .Then with control law, v mi will converge to the desired configuration relative to v nj j , so The topology graph is a finite graph, so all the robots will converge to the desired configuration relative to v 0 .Then for any v i , v j , we have Then the formation in Equation 9 is achieved.

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 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 the global frame and the angle θ indicates the orientation of the robot.The six initial poses are given by p 1 = (0, 0, 0), p 2 = (5, 3, 0), p 3 = (−1, 6, π/6), p 4 = (6, −5, −π/2), p 5 = (0, −5, π/3), p 3 = (−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: the robot-1 has no information source, so it remains static.Other robots perform the trajectories according to the posture of their information source robots.The robots   4, 5 and 6 achieve the desired configurations after 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 X-axis of the global frame.We see that the six angles turn to a same value after some regulations of the configurations which indicates the coordination of the robots' orientations.The rotation velocities become 0 at the end.

Conclusions
In this paper, we study the problem of formation control for a group of unicycle-type robots using 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, the behavior of the system is discussed.Some simulations of a 6-robot system validate the proposed control laws.The problem of avoiding dynamic and static obstacles was not considered, and the communication topology was supposed fixed.The case of

Figure 1 .
Figure 1.Representation of the frames.