adaptive leader-follower formation in cluttered ...€¦ · namic environment which is tracked...

HAL Id: hal-01712911https://hal.archives-ouvertes.fr/hal-01712911

Submitted on 23 Feb 2018

HAL is a multi-disciplinary open accessarchive for the deposit and dissemination of sci-entific research documents, whether they are pub-lished or not. The documents may come fromteaching and research institutions in France orabroad, or from public or private research centers.

L’archive ouverte pluridisciplinaire HAL, estdestinée au dépôt et à la diffusion de documentsscientifiques de niveau recherche, publiés ou non,émanant des établissements d’enseignement et derecherche français ou étrangers, des laboratoirespublics ou privés.

Adaptive Leader-Follower Formation in ClutteredEnvironment Using Dynamic Target Reconfiguration

Jose Miguel Vilca Ventura, Lounis Adouane, Youcef Mezouar

To cite this version:Jose Miguel Vilca Ventura, Lounis Adouane, Youcef Mezouar. Adaptive Leader-Follower Formationin Cluttered Environment Using Dynamic Target Reconfiguration. 12th International Symposium onDistributed Autonomous Robotic Systems (DARS 2014), Nov 2014, Daejeon, South Korea. pp.237-254, 10.1007/978-4-431-55879-8_17. hal-01712911

https://hal.archives-ouvertes.fr/hal-01712911

https://hal.archives-ouvertes.fr

Adaptive Leader-Follower Formation inCluttered Environment using Dynamic Target

Reconfiguration

Jose Vilca, Lounis Adouane and Youcef Mezouar

Abstract This paper presents a control architecture for safe and smooth navigation

of a group of Unmanned Ground Vehicles (UGV) while keeping a specific forma-

tion. The formation control is based on Leader-follower and Behavioral approaches.

The proposed control architecture is designed to allow the use of a single control law

for different multi-vehicle contexts (navigation in formation, transition between dif-

ferent formation shapes, obstacle avoidance, etc.). The obstacle avoidance strategy

is based on the limit-cycle approach while taking into account the dimension of

the formation. A new Strategy for Formation Reconfiguration (SFR) of the group

of UGVs based on suitable smooth switching of the set-points (according, for in-

stance, to the encountered obstacles or the new task to achieve) is proposed. The

inter-vehicles collisions are avoided during the SFR using a penalty function act-

ing on the vehicle velocities. Different simulations on cluttered environments show

the performance and the efficiency of the proposal, to obtain fully reactive and dis-

tributed control strategy for the navigation in formation of a group of UGVs.

1 Introduction

In the last decades, research interest in control and coordination of multiple robots

has increased significantly. Different tasks that may be performed by a single com-

plex robot can be performed with more flexibility and efficiency by a group of el-

ementary cooperative robots. These cooperatives robots join their capacities and

information to improve the task achievement.

Exploration [16], management and platooning of autonomous vehicles [5], map-

ping of unknown locations [19], coverage of unknown area [9], transport of heavy

objects [3], rendez-vous of multiple agents [21] are some examples of multi-robot

tasks.

J. Vilca, L. Adouane and Y. Mezouar

Institut Pascal, Blaise Pascal University – UMR CNRS 6602, Clermont-Ferrand, France

e-mail: [email protected]

This work was supported by the French National Research Agency through the Safeplatoon project.

1

The 12th International Symposium on Distributed Autonomous Robotic Systems (DARS 2014)

- 182 -

2 Jose Vilca, Lounis Adouane and Youcef Mezouar

Fig. 1 Autonomous naviga-

tion in formation of a group

of UGVs in an urban envi-

ronment (Clermont-Ferrand,

France).

Nonetheless, the coordination of multi-robot navigation is among the most chal-

lenging tasks, due notably, to its implication for instance for public transportation

[15]. This paper addresses the navigation of a group of vehicles in formation (i.e.,

when a group of mobile robots has to navigate and to keep a desired relative po-

sitions to each other or to a reference). Mostly, three approaches have been inves-

tigated to deal with this problem: behavior-based [22], virtual structure [10] and

Leader-follower [12] approaches. In this work, the proposed control architecture is

based on Leader-follower and behavior-based approaches for the formation control

problem.

In the Leader-follower approach, the leader is the reference for the desired con-

figuration of the followers. Different works exploit graph theory to describe the

inter-vehicle communications [5, 8, 18, 20]. Different formation cases (leader reas-

signment, robot adding and control saturation) were presented in [18]. The authors

proposed a formation control law based on the combination of Linear Matrix In-

equalities and hybrid system. The case of dynamic formation, i.e., the formation

shape changes to another (e.g. from square to triangle), and obstacle avoidance was

dealt in [5, 8, 20]. In [5], the leader generates a free-collision trajectory in a dy-

namic environment which is tracked using a formation control law based on neural

network, Lyapunov function and dynamic model of the robot. The stability of the dy-

namic formation and dynamic topology (adjacency matrix) are also demonstrated.

In [8], switches between different formation shapes are exploited (from triangle

to line) to avoid encountered obstacles in the environment. The formation control

law is based on input-output feedback linearization and vision sensors (omnidirec-

tional camera) are embedded in each robot for localization and navigation purpose.

A strategy to modify the formation configuration by scaling the distance between

the vehicles is proposed in [20]. Obstacle avoidance is dealt with using potential

fields. In the already presented works, interesting solutions for formation control

problem are proposed. Nevertheless, they are based on predefined trajectories and

do not address the issues related to the constraints of the formation shape and to

the dimension of the vehicles (large vehicles need large space for navigation and

obstacle avoidance). In this paper, the main proposal is to present a fully reactive

and distributed navigation in formation of a fleet of UGVs. Additionally, we make

a focus on the reconfiguration phase based on a suitable smooth switches between

the formation shapes while considering the follower configurations.

In this paper, the dynamics of the followers’ set-points are given by the specific

dynamic of the leader (Leader-follower approach). This strategy allows a good flex-

ibility proprieties for the formation shape [7]. The behavior-based approach allows

to use different elementary controllers to perform several sub-tasks (cf. Fig. 2). The

- 183 -

Adaptive Leader-Follower Formation using Dynamic Target Reconfiguration 3

obstacle avoidance controller for the whole formation is based on limit-cycle prin-

ciple [1]. This control architecture is designed in order that each follower tracks

safely the assigned configuration (given by the leader) while using an appropriate

dynamic target-reaching controller. The proposed strategy for formation reconfigu-

ration takes into account the presence of obstacles in the environment as well as the

inter-vehicles distance to avoid collision between the UGVs.

The rest of this paper is organized as follows: in the next section, the control

architecture for the navigation of UGVs in formation is introduced. The model of the

UGV and its controllers are also detailed. In Section 3, the navigation in formation

based on Leader-follower approach is described. Moreover, this section presents

also the reconfiguration method for the multi-robot formation. Simulations showing

the efficiency of the proposed strategy are detailed in Section 4. Finally, conclusion

and future works are given in Section 5.

2 Control architecture

The control architecture for the navigation in formation of a group of UGVs is

shown in Fig. 2. This control architecture is designed for a group of UGVs mod-

eled as tricycle robots. This architecture aims to manage the interactions among

elementary controllers while guaranteeing the stability of the overall control [2]. It

allows to obtain safe and smooth navigation of the formation (cf. section 3). The

global navigation in formation framework is operated by the Formation parameters

block that sends to each elementary controller (Dynamic target reaching and Obsta-

cle avoidance) its desired set-points. Each elementary controller (cf. Fig. 2) provides

as output a Control Input CI to the Control law block through a Hierarchical action

selection block which selects between CIT or CIO.

In this work, a single control law for the UGV (tricycle robot) is used. It considers

the vehicle poses and velocities. This control law allows the UGV to reach a static

or dynamic target with a desired orientation and velocity (cf. subsection 2.2.3). The

inputs of the control law (pose errors between the vehicle and its assigned target)

are provided by the elementary controllers (cf. subsection 2.2). The control law is

synthesized according to Lyapunov theorem (more details are given in [23]). The

main blocks of the architecture are detailed below.

The Perceptions and communication block incorporates the propriocetive and

exteroceptive sensors such as range sensor, cameras, odometers and RTK-GPS. Its

goal is to capture information related to the robot environment, mainly potential

obstacles [6, 8]. The communication is related with the vehicle capability to send

and to receive information from other vehicles. In the sequel, we assume a stable

communication between UGVs without latency) and that each UGV has a RTK-

GPS and a range sensor LIDAR.

The Formation parameters block determines the desired configuration for the

group of UGVs to keep a specific distance and orientation between them. The

Leader-follower approach is used to obtain the formation configuration of the UGVs

- 184 -


according to the Leader configuration (cf. Section 3). Moreover, a reconfiguration

strategy between the formation shapes according to the context of the environment

(dynamic and/or cluttered or not) is described in subsection 3.2.

The control architecture uses a Hierarchical action selection mechanism to

manage the switches between the two elementary controllers (Behavior-based ap-

proach), Dynamic target reaching and Obstacle avoidance blocks, according to the

formation parameters and environment perception. The hierarchical action selection

mechanism activates the Obstacle avoidance block as soon as it detects at least one

obstacle which can hinder the future vehicle movement toward its dynamic virtual

target (more details are given in [1]). It allows to anticipate the activation of obsta-

cle avoidance controller and to decrease the time to reach the assigned target (static

or dynamic). In order to provide the enough overall details of the presented con-

trol architecture, the following subsections present the UGV model and elementary

controllers.

2.1 Vehicle and target set-point modeling

We assume that the UGVs evolves in asphalt road and in cluttered urban environ-

ment with relatively low speed (less than vmax = 2 m/s). Hence, the use of kinematic

model (which relies on pure rolling without slipping) of the UGV is sufficient. The

kinematic model of the UGV is based on the well-known tricycle model [17]. The

two front wheels are replaced by a single virtual wheel located at the center between

the front wheels. The equations of UGV model can be written as (cf. Fig. 3):

x = vcos(θ); y = vsin(θ); θ = v tan(γ)/lb (1)

where (x,y,θ) is the UGV pose in the global reference frame XGYG. v and γ are

respectively the linear velocity and the orientation of the vehicle front wheel. lb is

the wheelbase of the vehicle.

!" # $ %&'%()* +,-.,/ '+001,* '.)* +, 2 3 45 6 78 9( ):; < = ; < => w

Fig. 2 Proposed control architecture em-

bedded in each UGV navigating in the for-

mation.

? @A @B CD ED

F @G H I HA JK

L M N O P QR ST UP V

P W CC R UP XDY Z Z CN Z R R [ UP [ UB UK U

TFig. 3 UGV and dynamic target configuration vari-

ables in (local and global) Cartesian reference

frames.

- 185 -


2.2 Elementary controllers

Each elementary controller generates the control inputs CI (pose errors (ex,ey,eθ )and velocities vT ) of the Control law block (cf. Fig. 2).

2.2.1 Dynamic target reaching controller

The target set-point modeling is defined by the formation shape (Formation param-

eters block) and it is computed according to the leader configuration (cf. Section 3).

The target is defined by (xT ,yT ,θT ) and vT which are respectively the target poses

and linear velocity in the global reference frame. Indeed, an RTK-GPS embedded

in each vehicle allows to estimate its current configuration.

Before to introduce the control law, let us describe the following notations (cf.

Fig. 3):

• Icc is the instantaneous center of curvature of the vehicle trajectory, rc = lb/ tan(γ)is the radius of curvature and cc = 1/rc is the curvature.

• (ex,ey,eθ ) are the errors w.r.t local frame (XmYm) between the vehicle and the

target poses.

• θRT and d are respectively the angle and distance between the target and vehicle

positions.

• eRT is the error related to the vehicle position (x,y) w.r.t the target orientation.

This controller guides the vehicle towards the dynamic target. It is based on thepose control of the UGV w.r.t. the target (represented by errors variables (ex,ey,eθ )in Fig. 3). These errors are computed w.r.t the local reference frame XmYm and theyare given by:

ex = cos(θ)(xT − x) +sin(θ)(yT − y)ey = −sin(θ)(xT − x) +cos(θ)(yT − y)eθ = θT −θ

(2)

The error function eRT is added to the canonical error system (2) (cf. Fig. 3). Letus now write d and θRT as (cf. Fig. 3):

d =√

(xT − x)2 +(yT − y)2 (3)

θRT = arctan((yT − y)/(xT − x)) if d > ξθRT = θT if d ≤ ξ

(4)

where ξ is a small positive value (ξ ≈ 0). The error eRT is defined as (cf. Fig. 3):

eRT = θT −θRT (5)

Furthermore, the velocity set-points of the dynamic target (in global frame XGYG)

are computed according to the leader velocities and formation shape (cf. Section 3).

Finally the pose errors and velocities (ex,ey,eθ ,vT ) are the input of the Control law

block (cf. subsection 2.2.3).

- 186 -


2.2.2 Obstacle avoidance controller

Different methods can be found in the literature for obstacle avoidance [14, 24]. One

of them is the limit-cycle method, the UGV avoids reactively the obstacle if it tracks

accurately limit-cycle trajectories as detailed in [1] (ellipse of influence). The main

ideas behind this controller are briefly detailed below:The differential equations of the elliptic limit-cycles are:

xs = m(Bys +0.5Cxs)+ xs(1−Ax2s −By2

s −Cxsys) (6)

ys = −m(Axs +0.5Cys)+ ys(1−Ax2s −By2

s −Cxsys) (7)

with m=±1 according to the avoidance direction (clockwise or counter-clockwise).(xs,ys) corresponds to the position of the UGV according to the center of the ellipse.The variables A, B and C are given by:

A =(sin(Ω)/blc)2 +(cos(Ω)/alc)

2 (8)

B =(cos(Ω)/blc)2 +(sin(Ω)/alc)

2 (9)

C =(1/a2lc −1/b2

lc)sin(2Ω) (10)

where alc and blc characterize respectively the major and minor elliptic semi-axes

and c gives the ellipse orientation when it is not equal to 0.

Let us now extend this method, initially proposed for the navigation of a mono-

robot, to the case of a group of UGVs. In section 3, the formation is defined by

longitudinal hi and lateral li coordinates (15) (cf. Fig 4). At this aim, the dimensions

of the ellipse (alc,blc) are increased according to the maximum lateral coordinate

of the formation shape limax, i.e., the dimension of the ellipse to avoid will be (alc +limax,blc + limax). The advantage of the proposed method is to maintain the shape of

the whole formation even when obstacles hinder the formation navigation, instead

of each robot avoids locally the obstacles [2].

In our case, the controller can be written as an orientation control. We consider

thus ex = 0 and ey = 0 in (2) (cf. Fig. 3), i.e, the vehicle position is at each sample

time in the desired position. The limit-cycle propriety allows to avoid the obstacles.

The desired vehicle orientation is given by the differential equation of the limit-cycle

(6) and (7):

θd = arctan(ys/xs) (11)

Furthermore, the linear velocities of the UGVs are decreased for safe avoidance

when the obstacle avoidance controller is activated, e.g, vT = vmax/2.

2.2.3 Control law

The used control law is designed according to Lyapunov stability analysis [23]. The

desired vehicle’s linear velocity v and its front wheel orientation γ that make the

errors (ex,ey,eθ ) converge always to zero can be chosen as:

- 187 -


v = vT cos(eθ )+Kx (Kdex +Kld sin(eRT )sin(eθ )+Ko sin(eθ )cc) (12)

γ = arctan(lb[

r−1cT

cos−1(eθ )+ cc

]

) (13)

where cc is given by:

cc =d2Kl sin(eRT )cos(eRT )

rcTKo sin(eθ )cos(eθ )

+Kθ tan(eθ )+Kdey −Kld sin(eRT )cos(eθ )

Ko cos(eθ )+

KRT sin2(eRT )

sin(eθ )cos(eθ )(14)

K = (Kd ,Kl ,Ko,Kx,KRT ,Kθ ) is a vector of positive constants which must be de-

fined by the designer. Kd , Kl and Ko are respectively related to the desired conver-

gence of the distance, lateral and angular errors w.r.t. the assigned target. Kx, KRT

and kθ are related to the maximum linear and angular velocities (more details are

given in [23]).

3 Navigation in formation

We consider a group of N UGVs with the objective of reaching and keeping their

assigned configuration according to the desired formation and leader configuration

[8, 25]. The proposed strategy consists on controlling each UGV (follower) to track

its assigned virtual dynamic target (cf. subsection 3.1 and Fig. 4). The strategy of

formation reconfiguration (cf. subsection 3.2) is based on a suitable smooth switch

of these virtual dynamic targets while considering the inter-vehicle collisions.

3.1 Leader-follower approach

Leader-follower approach allows to maintain a rigid geometric shape (e.g. a triangle

in Fig. 4). The formation is defined in this case w.r.t. the Cartesian frame (local

frame of the leader) (cf. Fig 4). The proposed formation, based on Leader-follower

approach, is defined by:

• A leader (UGVL in Fig. 4); its pose (xL,yL,θL) and its linear velocity vL deter-

mine the dynamic of the formation (cf. Fig. 4).

• The formation structure is defined with as much nodes as necessary to obtain

the desired formation shape. Each node i is a virtual dynamic target (Tdi). The

formation is defined as F = fi, i = 1 · · ·N, where fi are the coordinates (hi, li)T

of the dynamic target Tdiw.r.t. the leader local reference frame (cf. Fig. 4).

The position and orientation of each node (virtual target) are computed from the

leader configuration. The leader position determines the nodes positions according

to the formation shape. The instantaneous center of curvature IccLof the formation

is determined by the leader according to its movements (cf. Fig. 4). IccLallows to

compute the desired orientation of the nodes according to the formation shape (cf.

- 188 -


Fig. 4). The leader turns around IccL(positioned perpendicularly to its rear wheel),

then the other target set-points Tdimust also turn around IccL

to maintain the rigid

formation. Thus, the target velocity vTimust be tangent to the circle which has IccL

as center and the distance between Tdiand IccL

as radius rcTi.

The idea behind this strategy is to eliminate the dependency of each UGV to a

global reference frame. A straightforward transformation can be applied to obtain

the set-point w.r.t. a local reference frame attached to the leader. The polar coor-

dinates (ri,Φi) can also be used by applying a straightforward transformation. An

important advantage of the used Leader-follower approach is that it does not depend

here on any reference trajectory and the formation is fully defined by the instanta-

neous dynamic of the leader. An important advantage of the proposed formation

definition based on Leader-follower approach is that it takes, in addition to the tar-

get positions (xT ,yT ), the heading θT of the virtual targets, which allows to have

even more accurate formation navigation (cf. section 4). Furthermore, the proposed

approach is more reactive in the sense that it takes at each sample time only the

current configuration and velocity of the Leader, instead of using the trajectory of

the Leader as a reference for the formation [5, 20].

One important consideration to take into account to achieve the presented nav-

igation of formation strategy, is that the followers have to know, as accurately as

possible, the leader state (pose and velocity). As mentioned before, we assume that

the leader sends its state by stable Wi-Fi communication without latency. However,

cameras and/or LIDAR sensors embedded in each follower, can be used to estimate

the leader state [8, 11].

In the sequel, fi is given in Global Cartesian frame to homogenize the notation

of the equations. The pose of the virtual target Tdiw.r.t the leader pose in the Global

reference frame can be written as (cf. Fig. 4):

xTi= xL +hi cos(θL)− li sin(θL)

yTi= yL +hi sin(θL)+ li cos(θL)

θTi= θL +βi

(15)

where (xL,yL,θL) is the current pose of the leader and βi is the Tdiorientation w.r.t.

the leader pose. It is given by:

Fig. 4 Formation definition in

mobile Cartesian frame linked

to the Leader.

\]^_` ab c cdef

ghij klmn

ob( )p pq q qr s q

( )t tu u uv v vw x q

yb z | ~ ~ ¡¢£ ¤q ¥h¦§

¨© ª « ¬ ®¯°±²³ ´µ¶

( )·¸ ¸ ¸¹ º=

»

- 189 -


βi = arctan(hi/(rcL− li)) (16)

where rcLis the radius of curvature of the leader. Differentiating (15), the veloc-

ities of each Tdiare given thus by:

vTi=√

(vL − liωL)2 +(hiωL)2 (17)

ωTi=ωL + βi (18)

where vL and ωL are respectively the linear and angular velocities of the leader, βi

is computed as:

βi =−hircL/(

(rcL− li)

2 +(hi)2)

(19)

One can note from (19) that when βi is equal to zero, the formation has a constant

radius of curvature rcLand the angular velocities of the virtual targets are equal to

the angular velocity of the leader (ωTi= ωL) (18).

3.2 Proposed strategy for formation reconfiguration

Different methods dealing with formation reconfiguration for a group of UGV have

been proposed in the literature [4, 5, 8]. Many methods exploit Model Predictive

Control (MPC) based on time horizon and optimization of a cost function [4]. These

methods are generally time consuming due to predictive computation w.r.t. a time

horizon. Moreover, they were applied to small unicycle robots and they are based

on predefined trajectories computed along a time horizon. This subsection proposes

a new Strategy for Formation Reconfiguration (SFR) based on suitable smooth

switches between different virtual target configurations (cf. subsection 3.2.1). This

strategy allows to obtain a fully reactive architecture in the sense that the UGV fol-

lowers track the instantaneous state (pose and velocity) of its virtual targets (thus,

without any use of a reference trajectory or a trajectory planning process). Addition-

ally to the reconfiguration process, one should manage potential collisions between

UGVs and allocation of virtual targets to UGVs (cf. Subsection 3.2.2).

Different algorithms optimizing target assignment can be easily integrated in our

multi-block control architecture (cf. Fig. 2) (refer to [2, 25]). Nonetheless, this pa-

per is focused on the control strategy for formation reconfiguration. Therefore, the

allocation of virtual targets to UGVs is achieved using elementary rules when a for-

mation reconfiguration is required (cf. section 4). These rules assign a label Hi of

the virtual target Tdito the UGVi at the beginning of the experiments. This label is

kept by each UGV along of the reconfiguration process (cf. Fig. 5).

3.2.1 Reconfiguration method

A typical example of application of formation reconfiguration is when the formationdetects a narrow tight corridor, therefore the formation have to adapt to the corridor

- 190 -


dimension to continue the navigation. The proposed strategy for formation recon-figuration is based on suitable smooth switching of the virtual target configurations.The new virtual targets defined on the new formation shape must be ahead to theUGVs to guarantee the stability of the overall system (the vehicle must not go backto reach the new virtual target). If this condition is not satisfied then the former for-mation will be adapted by increasing smoothly and contentiously the longitudinalcoordinates hi until that all UGVs will be positioned in the right configuration (21).The error between the coordinates of the former and the new formation e fi(ehi

,eli)is defined as:

e fi = fni − f

fi (20)

where ffi (h

fi , l

fi ) and fn

i (hni , l

ni ) are respectively the coordinates of the former forma-

tion and new desired formation (cf. Fig. 4 and 5).The reconfiguration process between the different formation shapes is given by:

fi =

hi = hni − ehi

e−kr(t−tr), li = lni ; if ehi

< 0

hi = hni , li = ln

i ; if ehi≥ 0

(21)

where fi(hi, li) are the coordinates of the current virtual target Tdito be tracked by

the follower UGVi. ehiis the longitudinal coordinate of e fi that allows to detect if the

virtual target is ahead to its respective follower (ehi≥ 0). The adaptation function

when ehi< 0 (virtual target behind to followeri) is set as proportional to the error

between formation shapes, where kr is a real positive constant designed according to

the dynamic of the leader and tr > 0 is the initial time for the reconfiguration process.

An accurate analysis of this adaptive reconfiguration function will be developed in

future works.

3.2.2 Collision between UGV

Collision between UGVs can occur during the reconfiguration phase of the groupof UGVs. To address this collision risk, we use a penalty function acting on thelinear velocity of the UGVs. Moreover, this reduced velocity of UGVs allows toobtain a smooth and less oscillating vehicles’ movements (cf. section 4). Each UGVis enclosed by two circle Cint and Cext with respectively radius Rint and Rext (Rint <Rext ). The collision occurs when the distance di j between UGVi and UGVj are less

than Rint . Hence, the penalty function ψj

i for the UGVi w.r.t. the UGVj is defined as:¼½¾¿ÀÁÂÃÄ Å Æ Ç Å ÈÉ È Æ Ê Å Ë É Ì È Í ÎÏÐÑÒÓ Ô Õ Ö × Õ ØÙÚÛÜÝÞßà áâãä åæçèéêëìí î ï ð ñ ï òóôõ=

ö÷ø=ùúû

=

üýþ=ÿ

==

Fig. 5 Formation reconfiguration between, for instance, triangular and

linear formation shapes.

! "# $ " % ! & ' ( !) ! *+ $ ( ' ( ),- . / - . /0 d

( )12 3 4 2 3 45 d6 7897:;< =>?@>A 9=BBC>< 9@;< => D EFFig. 6 Integration of

the penalty function in

the proposed architec-

ture.

- 191 -


ψj

i =

1 if di j ≥ Rext

(di j −Rint i)/(Rext −Rint i

) if Rint i< di j < Rext

0 if di j ≤ Rint i

(22)

The modified linear velocity of the UGVi is then given by:

v j = v jψj

i (23)

Using the definition of Rint i(where Rint i

6= Rint j), it is guaranteed that two UGVs

do not stop simultaneously. Indeed, if the UGVs have the same Rint iwe can observe

local minima in certain configurations, in fact, when di j < Rint ithen ψ

ji = ψ i

j = 0

and the robots are stopped at the same time.vRext is designed according to commu-

nication constraints (latency) and localization errors (GPS). This penalty function

can be straightforward integrated to our control architecture (cf. Fig. 2) by adding a

block after the output of the Control law block (cf. Fig. 6).

4 Simulations

This section shows the navigation of a group of N = 3 UGVs in a cluttered en-

vironment using the proposed control architecture. The reconfiguration strategy

(SFR) between the formation shapes is also analyzed. All simulations were made

in MATLABr software. The physical parameters of the used UGV are based on

the urban vehicle VIPALAB from Apojee company [13]. The UGV constraints are

minimum velocity vmin = 0.1 m/s, maximum velocity 1.5 m/s, maximum steering

angle γmax =±30 and maximum acceleration 1.0 m/s2. We consider that the sam-

ple time is 0.01 s. Each UGV has a range sensor (LIDAR) with a maximum detected

range equal to Dmax = 10 m and a stable communication network.

The controller parameters are set to K = (1,2.2,8,0.1,0.01,0.6) (cf. subsec-

tion 2.2.3). These parameters were chosen to obtain a safe and smooth trajectory,

fast response and velocity value within the limits of the vehicle capacities. The ra-

dius for non-collision between UGVs are selected as RintL= 1.8 m, Rint1

= 2.2 m,

Rint2= 2.0 m and Rext = 2.7 m. For each simulation the vehicles start at the same

configuration and must reach the same final configuration. The initial positions of

the vehicles have an offset (∆x,∆y) = (1, 0.5) m from the initial position of their

assigned virtual targets.

The simulations given in Fig. 7 to 11 are selected from several conclusive simu-

lations because they make a focus on the proposed reconfiguration method between

the two formation shape (triangular and linear shapes) while navigating in a clut-

tered environment (this simulation can be found online1). We consider that the ini-

tial formation coordinates are defined by F = (f1, f2), with f1 = (−4,−2)T m and

f2 = (−4,2)T m (triangular shape). Therefore, the group of UGVs must keep the

formation while moving in a cluttered environment. A static target is defined in the

1 http://maccs.univ-bpclermont.fr/uploads/Profiles/VilcaJM/FormationReconfiguration.mp4

- 192 -


environment, the leader (and thus the formation) must go toward it while avoiding

the hinder obstacle. The new targeted formation is defined as straight line with the

following coordinates Fn = (fn1, f

n2), with fn

1 = (−6,0)T m and fn2 = (−3,0)T m.

At the beginning of the simulation (cf. Fig. 9), the navigation of the group of

UGVs is in triangular formation F. When the leader detects an obstacle with ade-

quate range to allow the formation reconfiguration, then the leader avoids the ob-

stacle using the limit-cycle method (limit-cycle is increased by R f = 2 m to allow

a safe navigation (cf. subsection 2.2.2) and sends the new desired formation Fn to

the other UGVs (followers) to modify the configuration of the formation. The for-

mation returns to triangular shape F, when the leader does not detect obstacles that

can hinder the other UGVs movement and the last follower left behind the avoided

obstacle. The adaptation phase allows to have the virtual target always ahead to the

followers to obtain a suitable adaptive formation reconfiguration (cf. Fig. 7 and 11).

Figure 7 shows the values of errors d and eθ between each UGV and its virtual

target. At first reconfiguration, it can be observed that the follower 1 wait until its

assigned virtual target is ahead (cf. Subection 3.2.1). Moreover, it is noted some

small peaks that are related to the fast dynamic change of the leader (the dynamic of

the formation increased and the saturation occurs in the followers when the leader

curvature increased). Fig. 8 shows the distance between each UGV of the formation.

This last figure shows clearly non-collision between the vehicles in the formation,

i.e., di j > Rint12(cf. Subsection 3.2.2). The figures show some peaks which are re-

lated to the adaptation and reconfiguration phase between formations.

Figures 9 and 10 show respectively the trajectories and velocities of the UGVs. It

can be noted that the vehicles trajectories are smooth along the navigation and there

is not neither collisions with the obstacles nor inter-vehicle collisions. The reconfig-

uration strategy was designed to reduce the peaks of the control commands of each

UGV when the transition between the formation occurs (cf. Fig. 10). The proposed

control architecture allows thus to adapt the formation according to the environment

context. Fig. 11 shows the evolution of the formation coordinates (hi, li) (virtual tar-

get positions). It can be observed that adaptation phase of hi when the follower is

always ahead of its new assigned virtual target (21) which attest on the efficiency of

the strategy for formation reconfiguration.

Fig. 7 Distance and orientation errors of the

UGVs w.r.t. their virtual targets.Fig. 8 Distance among the UGVs.

- 193 -


Fig. 9 Navigation with reconfiguration in formation for a group of N = 3 UGVs.

Fig. 10 Velocities commands of the UGVs. Fig. 11 Progress of the set-point definition

fi according to the proposed SFR.

5 Conclusions and prospects

This paper presented an overall control architecture to cope with the navigation in

formation of a group of UGVs in cluttered environment. A single Control law em-

bedded in each UGV is used in the proposed architecture which allows the simpli-

fication of the overall control strategy for the navigation in formation. The obstacle

avoidance based on the limit-cycle trajectories allows to keep the desired formation

shape during the navigation even in cluttered environments. In the proposed forma-

tion definition based on Leader-follower approach, the leader reference path is not

taken into account, only its current pose and dynamic has to be known by the follow-

ers. It allows thus more accurate and flexible formation navigation. A fully reactive

reconfiguration strategy between the UGVs based on suitable smooth switching of

the virtual target configurations was also proposed. This strategy avoids the use of

predefined trajectories and it can be applied for different situations when the forma-

tion has to be modified according to the environment context (dynamic, cluttered,

etc.). Furthermore, this strategy takes into account the probable collisions between

vehicles as well as the vehicle constraints to ensure safe navigation to reach the new

desired formation. Different accurate simulations using a tricycle vehicles show the

efficiency and the flexibility of the proposed strategy for multi-robot navigation.

In future works, formation reconfiguration strategy even in uncertain environ-

ments (for instance, w.r.t. the vehicle’s perception/localization) will be addressed.

- 194 -


References

1. Adouane, L., Benzerrouk, A., Martinet, P.: Mobile robot navigation in cluttered environment

using reactive elliptic trajectories. In: 18th IFAC World Congress (2011)2. Benzerrouk, A., Adouane, L., Martinet, P.: Altruistic distributed target allocation for stable

navigation in formation of multi-robot system. In: 10th International IFAC Symposium on

Robot Control (SYROCO’12). Dubrovnik - Croatia (2012)3. Chaimowicz, L., Kumar, R.V., Campos, M.F.M.: A mechanism for dynamic coordination of

multiple robots. Autonomous Robots 17, 7 – 21 (2004)4. Chao, Z., Zhou, S.L., Ming, L., Zhang, W.G.: UAV formation flight based on nonlinear model

predictive control. Mathematical Problems in Engineering 2012, 1–15 (2012)5. Chen, X., Li, Y.: Smooth formation navigation of multiple mobile robots for avoiding moving

obstacles. International Journal of Control, Automation 4(4), 466 – 479 (2006)6. Clark, J., Fierro, R.: Mobile robotic sensors for perimeter detection and tracking. ISA

Transactions 46(1), 3 – 13 (2007)7. Consolini, L., Morbidi, F., Prattichizzo, D., Tosques, M.: Leader-follower formation control

of nonholonomic mobile robots with input constraints. Automatica 44(5), 1343 – 1349 (2008)8. Das, A., Fierro, R., Kumar, V., Ostrowski, J., Spletzer, J., Taylor, C.: A vision-based formation

control framework. IEEE Transaction on Robotics and Automation 18(5), 813–825 (2002)9. Dasgupta, P., Whipple, T., Cheng, K.: Effects of multi-robot team formations on distributed

area coverage. International Journal of Swarm Intelligent Research 2(1), 44–69 (2011)10. Desai, J., Ostrowski, J., Kumar, V.: Modeling and control of formations of nonholonomic

mobile robots. IEEE Transaction on Robotics and Automation 17(6), 905 – 908 (2001)11. El-Zaher, M., Contet, J.M., Gechter, F., Koukam, A.: Echelon platoon organisation: a dis-

tributed approach based on 2-spring virtual links. In: 15th International Conference on Artifi-

cial Intelligence: Methodology, Systems, Applications (AIMSA). Germany (2012)12. Ghommam, J., Mehrjerdi, H., Saad, M., Mnif, F.: Formation path following control of

unicycle-type mobile robots. Robotics and Autonomous Systems 58(5), 727 – 736 (2010)13. The Institut Pascal Data Sets: (2013). URL http://ipds.univ-bpclermont.fr.14. Khatib, O.: Real-time obstacle avoidance for manipulators and mobile robots. The Interna-

tional Journal of Robotics Research 5, pp.90–99 (1986)15. Levinson, J., Thrun, S.: Robust vehicle localization in urban environments using probabilistic

maps. In: IEEE International Conference on Robotics and Automation. Alaska, USA (2010)16. Lozenguez, G., Adouane, L., Beynier, A., Mouaddib, A., Martinet, P.: Map partitioning to

approximate an exploration strategy in mobile robotics. MAGS - Multiagent and Grid Systems

(2012)17. Luca, A.D., Oriolo, G., Samson, C.: Feedback control of a nonholonomic car-like robot. In:

J.P. Laumond (ed.) Robot Motion Planning and Control, pp. 171–253. Springer-Verlag (1998)18. Mesbahi, M., Hadaegh, F.: Formation flying control of multiple spacecraft via graphs, matrix

inequalities, and switching. In: Control Applications, 1999. Proceedings of the 1999 IEEE

International Conference on, vol. 2, pp. 1211–1216 vol. 2 (1999)19. Miner, D.: Swarm robotics algorithms: A survey (2007)20. Shames, I., Deghat, M., Anderson, B.: Safe formation control with obstacle avoidance. In:

IFAC World Congress. Milan - Italy (2011)21. Sinha, A., Ghose, D.: Generalization of linear cyclic pursuit with application to rendezvous of

multiple autonomous agents. IEEE Transactions on Automatic Control 51, 1819–1824 (2006)22. Tang, H., Song, A., Zhang, X.: Hybrid behavior coordination mechanism for navigation of

reconnaissance robot. In: International Conference on Intelligent Robots and Systems (2006)23. Vilca, J., Adouane, L., Mezouar, Y., Lebraly, P.: An overall control strategy based on target

reaching for the navigation of an urban electric vehicle. In: IEEE/RSJ International Confer-

ence on Intelligent Robots and Systems (IROS’13). Tokyo - Japan (2013)24. Zapata, R., Cacitti, A., Lepinay, P.: Dvz-based collision avoidance control of non-holonomic

mobile manipulators. JESA, European Journal of Automated Systems 38(5), 559–588 (2004)25. Ze-su, C., Jie, Z., Jian, C.: Formation control and obstacle avoidance for multiple robots sub-

ject to wheel-slip. International Journal of Advanced Robotic Systems 9, 1–15 (2012)

- 195 -

adaptive leader-follower formation in cluttered ...€¦ · namic environment which is tracked...

Documents