[START_REF] Samson | Control of chained systems. application to path following and timevarying point-stabilization of mobile robots[END_REF]) and (Chained system [START_REF] Laumond | La Robotique Mobile[END_REF]). .

GLOSSARY

• AUV: Autonomous Underwater Vehicle.

• EKF: Extended Kalman Filter.

• GDI-VIPA: Graphical Data Interface for VIPALAB.

• GPS: Global Positioning System.

• MD: Mahalanobis Distance.

• MRF: Multi-Robot Formation.

• MRS: Multi-Robot System.

• OMWS: Optimal Multi-criteria Waypoint Selection.

• PAVIN: Plate-forme d'Auvergne pour Véhicules INtelligents.

• SFR: Strategy for Formation Reconfiguration.

• SMPA: Sense-Model-Plan-Act.

• UAV: Unmanned Aerial Vehicle.

• UGV: Unmanned Ground Vehicle.

• VIPALAB: Véhicule Individuel Public et Autonome.

GENERAL INTRODUCTION

"Coming together is a beginning. Keeping together is progress. Working together is success."

Henry Ford Founder of the Ford Motor Company Nowadays, different robotic systems are more and more present in many fields such as industries (e.g., robotic arms and manipulators), home (e.g., cleaning-robots and robotic toys), military (e.g., unmanned aerial, ground and underwater vehicles), space (e.g., robotic spacecraft and satellites), medical (e.g., robotic surgery) and more. All these systems have solved, improved or optimized the specific tasks which they were designed for. Some convincing examples of the capabilities of robotic systems are the Curiosity rover [307] for the Mars-expedition and the Philae robot for landing on a comet [376].

In recent years, one of the most interesting research corresponds to autonomous navigation of a single or a group of vehicles in structured environment. It is an important topic of research around the world due to its advantages in different domains such as ecology (reduction of pollution), transport time (efficient time schedule), safety (reduction of road accidents) and health (human comfort) among others [START_REF] Litman | Autonomous Vehicle Implementation Predictions: Implications for Transport Planning[END_REF][START_REF] Vine | Autonomous cars: The tension between occupant experience and intersection capacity[END_REF]. Nevertheless, many challenges remain in different domains such as in the perception in which climatological factors (rain, fog, darkness, lightness, etc.) disturb the environmental perceived data [START_REF] Ackerman | Korean competition shows weather still a challenge for autonomous cars[END_REF], as well as, in the control to design a robust and fault tolerant systems to deal with the errors and uncertainties of the sensors, in the communication in which network delays can produce erroneous decision, etc. [START_REF] Fagnant | Preparing a nation for autonomous vehicles: Opportunities, barriers and policy recommendation[END_REF].

Furthermore, the coordination of a group of robots to perform a task shows important benefits and large applications (agriculture, military or transport sector) [START_REF] Murray | Recent research in cooperative control of multivehicle systems[END_REF][START_REF] Parker | Distributed intelligence: Overview of the field and its application in multi-robot systems[END_REF]. Tasks that may be performed by a single complex robot can be performed more efficiently and cheaply by a group of elementary cooperating robots. These cooperative systems can be fault tolerant (one robot can be replaced or joined for more complex tasks) [START_REF] Arrichiello | A decentralized fault tolerant control strategy for multi-robot systems[END_REF][START_REF] Khan | Autonomous and market-based fault tolerant algorithms for multi-robot cooperation[END_REF] and can improve certain criteria related to the time, the robustness or the flexibility of the task to achieve [START_REF] Lian | Cooperative task planning of multi-robot systems with temporal constraints[END_REF][START_REF] Zlot | Market-based multirobot coordination for complex tasks[END_REF]. Nonetheless, some difficult problems need to be solved to achieve a suitable coordination. The numerous interactions between robots themselves and their environment, as well as the complexity of task to be achieved do not permit the direct use of neither classical perception nor control techniques [START_REF] Liu | Containment control of general linear multiagent systems with multiple dynamic leaders: A fast sliding mode based approach[END_REF][START_REF] Petkovi Ć | Input displacement neuro-fuzzy control and object recognition by compliant multi-fingered passively adaptive robotic gripper[END_REF].

In this context, the Safeplatoon project [309], to which this PhD thesis is associated, studies the problem of autonomous vehicles navigating in convoy for application in urban, military and agricultural environments. The innovative characteristics of this project is related to the design and development of the movement capabilities in a large and robust convoy. The aim of our work is the development of a control architecture for navigation in formation of a group of vehicles in structured environments.

This Multi-Robot Formation (MRF) is defined as a group of vehicles with an appropriate coordination between them in order to keep a pre-defined geometric configuration and avoiding hindering obstacles during the autonomous navigation. The strategy for the development of proposed safe and flexible control architecture is to break up the complex task (navigation in formation) in adequate elementary behaviors [START_REF] Adouane | Hybrid and safe control architecture for mobile robot navigation[END_REF][START_REF] Benzerrouk | Architecture de Contrôle Hybride pour les Systèmes Multi-robots Mobiles[END_REF]. Therefore, the desired flexibility of the control architecture can be obtained, for instance, a new controller can be added or modified to perform another complex task while keeping the initial elementary behaviors. The desired safety will be analyzed for each controller in the sense of performance, system constraints, convergence, etc. Furthermore, this control architecture is embedded in each vehicle instead of using a main central control system.

This manuscript begins with a survey of autonomous navigation for a single vehicle. The proposed control architecture has been developed for this task. The elementary controllers were designed to reach a target or successive targets while avoiding the detected obstacles. This obstacle avoidance is based on reactive methods (limit-cycle trajectories) using only the measurements of the local sensor of each vehicle [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]. This control architecture is validated in simulations and experiments. The formation control is based on the Leader-follower and behavior-based approaches where the leader sends its instantaneous pose and velocity to each follower. This approach avoids the followers' necessity of knowing a priori the whole leader trajectory. In addition, this control architecture is complemented with a stable formation reconfiguration (i.e., the formation shape changes), for example from line to triangle shape. This overall control architecture is validated in simulation and experiments which allows to conclude on the reliability and effectiveness of our work.

This manuscript is organized in two parts, the first part contains two chapters which correspond to the state of the art and bibliographical review:

• Chapter 1 contains a summary of the state of the art in mobile robotics, with a main focus on autonomous ground vehicles. Different control architectures for autonomous navigation of a single robot are presented.

• Chapter 2 contains a summary of the state of the art in cooperative robotics, with a main focus on autonomous navigation of a fleet of vehicles. Different control architectures for autonomous navigation of a group of robots are detailed. Particularly, a description of the main approaches for navigation in formation is presented.

The second part corresponds to different proposals developed during this PhD thesis. This part contains three chapters:

• Chapter 3 is related to autonomous navigation of a single vehicle in cluttered environments. The composition of the proposed control architecture for a single robot's navigation is described. A specific block related to the environmental perception is detailed. Its role, advantages and drawbacks are also presented. Mainly, this chapter describes the proposed control law which allows to perform safe autonomous navigation. Its stability and safety are demonstrated. Simulations and experiments validate the proposed control architecture for safe navigation.

• Chapter 4 describes the proposed navigation strategy through successive waypoints which allows to perform safe and flexible autonomous navigation. The proposed methods to select the optimal set of waypoints in structured environments are presented. Its advantages and drawbacks w.r.t other methods from the literature are explained. In addition, an extension to multi-robot formation is presented. Simulations and experiments are performed to demonstrate the application of the proposed strategy.

• Chapter 5 deals with the multi-robot navigation in formation. The main literature approaches are given and our proposal is explained. Mainly, the proposed strategy for reconfiguration of the formation is presented with different examples. An exhaustive analysis of the system using different criteria is detailed in simulations and experiments.

This manuscript ends with a general conclusion summarizing the main contributions of this PhD thesis and the future works.

An overall scheme about the general organization of the manuscript is shown in Fig. 1 CONTRIBUTIONS

The goal of this work is developing a safe and flexible Multi-Robot System (MRS) control architecture for the navigation in formation of a group of vehicles. Before to design the MRS control architecture, we start by autonomous navigation of a single vehicle based on a Hybrid architecture which allows respectively a high-level planning and reactive navigation (cf. Subsection 1.3.3) [START_REF] Vilca | A novel safe and flexible control strategy based on target reaching for the navigation of urban vehicles[END_REF]. Moreover, this architecture allows to add some other behaviors. Our navigation strategy for a single vehicle consists in reaching successive waypoints instead of tracking a predefined trajectory (cf. Section 4.2) [4,[START_REF] Vilca | Optimal multi-criteria waypoint selection for autonomous vehicle navigation in structured environment[END_REF]. The idea is allowing the vehicle to perform different maneuvers between the waypoints such as obstacle avoidance before reaching another waypoints. The proposed control law is designed to converge to its assigned waypoints while guaranteeing stable and smooth vehicle's trajectories. Moreover, the advantage of this control law w.r.t others is performing different behavior such as trajectory tracking, point stabilization, dynamic target reaching [4,[START_REF] Vilca | A novel safe and flexible control strategy based on target reaching for the navigation of urban vehicles[END_REF] (cf.

Glossary

Subsection 3.5). The obstacle avoidance is based on reactive method (as developed in [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]) using limit-cycle. We propose an on-line obstacle detection method which allows to enclose the dangerous obstacle with an ellipse using range sensors [START_REF] Vilca | On-line obstacle detection using data range for reactive obstacle avoidance[END_REF][START_REF] Vilca | Reactive navigation of a mobile robot using elliptic trajectories and effective online obstacle detection[END_REF]. The cognitive part is related to the selection of the set of waypoint in structured environment. We propose an algorithm to obtain the optimal set of waypoint while taking into account the environmental constraints, the vehicle's kinematics and uncertainties. The advantages of this algorithm are the different considered criteria and the convergence time to obtain a solution [START_REF] Vilca | Optimal multi-criteria waypoint selection for autonomous vehicle navigation in structured environment[END_REF].

Once the single vehicle navigation is tested and validated via simulation and experimental results, the extension to MRS is analyzed. The MRS control architecture is based on centralized/decentralized architecture. The centralized part is dedicated to the formation management, i.e., the leader sends the follower position w.r.t. it (such as the virtual structure). The decentralized part is related to the tracking of the desired pose (virtual target) by the follower. Moreover, each follower can perform different behaviors (such as obstacle avoidance [5] (cf. chapter 5)) according to the environment perception.

The advantage of this MRS architecture is to allow the dynamic formation reconfiguration (e.g., line to triangle shape, or diamond to square shape) using a virtual structure definition (cf. Section 5.2). The leader decides when a reconfiguration is necessary according to the perceived environment (e.g., when a hinder obstacle is detected). The advantages of the designed method w.r.t. others is that it takes into account the formation shape dimensions and vehicle constraints while guaranteeing the shape convergence (cf. Section 5.4). Moreover, to evaluate the performance of the MRS system, system errors, convergence time, formation shape along the navigation are presented. The proposed MRS control architecture (cf. Chapter 5) is validated via simulation and experimental results showing its safety (no collision), feasibility (easy implementation) and flexibility (add more vehicles/behavior) for a group of vehicles.

I CONTEXT AND STATE OF THE ART 1 MOBILE ROBOTS FOR AUTONOMOUS NAVIGATION

This chapter describes the state of the art in mobile robots, particularly applied to autonomous navigation of a single vehicle. An important part of the chapter is dedicated to several control architectures of mobile robot system designed to perform autonomous navigation.

Robotics is a branch of technology such as computer science, mechanical and electronic engineering that deals with the design, construction, operation and application of different robots [START_REF] Dictionaries | [END_REF]. A robot is a machine (mechanical) capable of carrying out a complex series of actions or tasks automatically. The word robot was, as it is well known, introduced by the Czech writer Karel Čapek [START_REF] Čapek | Rosumovi univerzálni roboti (r.u.r)[END_REF]. It comes from the Slavic word robota which means labor or work. The word robotics (derived from robot) was first used by the science fiction writer Isaac Asimov in the science fiction short history titled "Liar!" [START_REF] Asimov | Liar! Astounding Science Fiction[END_REF].

(a) Industrial robot (FANUC ) for car manufacturing [START_REF] Brogårdh | Robot control overview: An industrial perspective[END_REF].

(b) Quadruped robot (Boston Dynamics ) for soldier assistance in rough terrains [START_REF] Springer | Military Robots and Drones: A Reference Handbook[END_REF].

(c) Wheelchair robot (Genny ) for transportation of disabled people [START_REF] Takahashi | Developing a mobile robot for transport applications in the hospital domain[END_REF].

(d) Suction robots (Maytronics ) for cleaning of swimming pool [START_REF] Güttler | Towards a future robotic home environment: A survey[END_REF].

(e) Space robot (ESA ) for comet surface lander [376].

(f) Kangaroo robot (Festo ), Insect fly robot (Harvard) and Salamander robot (EPFL) [START_REF] Trivedi | Soft robotics: Biological inspiration, state of the art, and future research[END_REF].

MOBILE ROBOTS FOR AUTONOMOUS NAVIGATION

The robotic systems have been applied to different domains, for instance: industries (car manufacturing (cf. Fig. 1.1(a))) [START_REF] Brogårdh | Robot control overview: An industrial perspective[END_REF][START_REF] Krüger | Cooperation of human and machines in assembly lines[END_REF], military (assistance, reconnaissance, guidance and weaponry (cf. Fig. 1.1(b))) [START_REF] Voth | A new generation of military robots[END_REF][START_REF] Springer | Military Robots and Drones: A Reference Handbook[END_REF], transportation (large heavy objects and elderly or disabled humans (cf. Fig. 1.1(c))) [START_REF] Takahashi | Developing a mobile robot for transport applications in the hospital domain[END_REF][START_REF] Urdiales | Collaborative Assistive Robot for Mobility Enhancement[END_REF], domestic (house cleaning, toys and human interaction (cf. Fig. 1.1(d))) [START_REF] Goodrich | Human-robot interaction: A survey[END_REF][START_REF] Güttler | Towards a future robotic home environment: A survey[END_REF], space (orbiters, rovers and landers (cf. Fig. 1.1(e))) [START_REF] Alfraheed | Exploration and cooperation robotics on the moon[END_REF][START_REF] Schwendner | Space robotics: An overview of challenges, applications and technologies[END_REF] , and other applications as given in [START_REF] Pfeifer | Self-organization, embodiment, and biologically inspired robotics[END_REF][START_REF] Trivedi | Soft robotics: Biological inspiration, state of the art, and future research[END_REF] (cf. Fig. 1.1(f)).

1.1/ MOBILE ROBOT SYSTEMS

A mobile robot is an automatic machine not fixed to one physical location and capable to move in its environment [START_REF] Siegwart | Introduction to Autonomous Mobile Robots[END_REF]. Mobile robots can be classified according to their work environments as follows:

• Land robots are related to the navigation in the ground surface (e.g., desert or mountain areas) [START_REF] Djapic | Land vehicle control using a command filtered backstepping approach[END_REF][START_REF] Berns | Autonomous Land Vehicles: Steps towards Service Robots[END_REF] (cf. Fig. 1.2(a)). They are commonly called Unmanned Ground Vehicles (UGVs).

• Aerial robots are related to the navigation in the air [START_REF] Michael | Cooperative manipulation and transportation with aerial robots[END_REF][START_REF] Gupte | A survey of quadrotor unmanned aerial vehicles[END_REF] (cf. Fig. 1.2(b)). They are usually named Unmanned Aerial Vehicles (UAVs).

• Underwater robot are related to the underwater navigation [START_REF] Moore | SCIENCE, DESIGN & FABRICATION[END_REF][START_REF] Moreno | A modular and reconfigurable underwater robot for task optimization[END_REF] (cf. Fig. 1.2(c)). They are commonly called Autonomous Underwater Vehicles (AUVs).

• Polar robots are related to the navigation in icy areas [START_REF] Stansbury | Survivability, mobility, and functionality of a rover for radars in polar regions[END_REF][START_REF] Gifford | Mobile robots for polar remote sensing[END_REF] (cf. Fig. 1.

2(d))

.

The presented work is dedicated to UGVs. The objective of these robots can vary according to the application, e.g., human or area surveillance [START_REF] Borja | Integration of service robots in the smart home by means of upnp: A surveillance robot case study[END_REF][START_REF] Memon | Fully autonomous flammable gases (methanegas) sensing and surveillance robot[END_REF] (cf.

MOBILE ROBOT SYSTEMS

mapping of unknown environments [START_REF] Lategahn | Visual slam for autonomous ground vehicles[END_REF][START_REF] Fernández-Madrigal | Simultaneous Localization and Mapping for Mobile Robots: Introduction and Methods[END_REF] (cf. Fig. 1.3(b)), human search and rescue [START_REF] Murphy | A decade of rescue robots[END_REF][START_REF] Simpkins | Rescue robotics [on the shelf[END_REF] (cf. Fig. 1.3(c)), space exploration [START_REF] Dirk | The bio-inspired scorpion robot: design, control & lessons learned[END_REF][START_REF] Seeni | Robot mobility systems for planetary surface exploration-state-of-the-art and future outlook: A literature survey[END_REF] (cf. Fig. 1.3(d)), autonomous navigation [START_REF] Lamon | The smartter-a vehicle for fully autonomous navigation and mapping in outdoor environments[END_REF][START_REF] Luettel | Autonomous ground vehicles Ůconcepts and a path to the future[END_REF] among others [START_REF] Mondada | The e-puck, a robot designed for education in engineering[END_REF][START_REF] Araújo | Integrating arduinobased educational mobile robots in ros[END_REF]. Furthermore, the UGV can use different devices to move such as legs (bio-inspired robot) (cf. Fig. 1.1(b) and 1.3(d)) [START_REF] Zhou | A survey of bio-inspired compliant legged robot designs[END_REF][START_REF] Geonea | Dynamic modelling of a four legged robot[END_REF] wheels (car-like robot) (cf. Fig. 1.1(c) and 1.3(b)) [START_REF] Chan | Review of modelling and control of two-wheeled robots[END_REF][START_REF] Jazar | Vehicle Dynamics: Theory and Application[END_REF] and tracks (trunk-like robot) (cf. Fig. 1.2(a) and 1.3(c)) [START_REF] Saffiotti | Platforms for rescue operations[END_REF][START_REF] Wu | A tracked robot for complex environment detecting[END_REF].

In this thesis, wheeled robots, referred as UGVs, will be used with final main objective of autonomous navigation in formation. The proposed navigation strategy is divided into two phases:

(a) Navigation of a single UGV: Safe and flexible navigation has been developed. This single robot must navigate in cluttered and structured environments while avoiding the detected obstacles (cf. Chapters 3 and 4).

(b) Navigation in formation: The proposed navigation is extended to a group of UGVs, of which the leader performs a safe navigation and the other robots follow virtual dynamic targets defined according to the leader configuration. The proposed formation reconfiguration acts when one UGV detects a possible collision of the formation with any dangerous obstacle (cf. Chapter 5).

The next sections provide a brief review of the different works of autonomous navigation for a single UGV (cf. Section 1.2).

1.2/ SINGLE ROBOT NAVIGATION: OVERVIEW

A brief overview of the evolution of the UGV's structures is detailed in Fig. 1.4. The first mobile robots has been built by W. Grey Walter in 1949. They were turtle-like robots with three wheeled, light sensor, touch sensor, drive and steering motors and two vacuum tube analog computer [START_REF] Walter | The Living Brain[END_REF][START_REF] Holland | Grey walter: The pioneer of real artificial life[END_REF]. He named them Elsie and Elmer turtles (ELectro MEchanical Robots, Light Sensitive) (cf. Fig. 1.4). These robots (Machina Speculatrix) used one vacuum tube to imitate two interconnected neurons. These simple amplifier circuits connected the light and touch sensors to two motors. The first sensor (photocell) was connected to the drive and steering motors. The second sensor (contact switch) indicates that the turtle's shell had collided with an object. Consequently, the sensor sent the vacuum-tube amplifiers into oscillation and changed the robot's direction. This system allowed the turtles to wander in a room and return to a recharging station.

In the 1980s, the largest robot car project in the field of driverless car, named Eureka PROMETHEUS Project (PROgraMme for a European Traffic of Highest Efficiency and Unprecedented Safety), was launched with the cooperation of several European companies such as Ernst Dickmanns' team of UniBW, Daimler-Benz, Jaguar, BMW and others [START_REF] Williams | Prometheus-the european research programme for optimising the road transport system in europe[END_REF][START_REF] Hellåker | Prometheus-strategy[END_REF]80]. The final Mercedes prototypes, the autonomous W140 S-Class called VaMP (Versuchsfahrzeug für autonome Mobilität Passenger which means "test vehicle for autonomous mobility Passenger") and VITA-2 (VIsion Technology Application) (cf. Fig. 1.5(a)), used saccadic computer vision, sixty transputers (a type of parallel computers), and probabilistic approaches (Kalman filter) to react in real time to road and traffic conditions [START_REF] Dickmanns | The seeing passenger car 'vamors-p[END_REF][START_REF] Ulmer | Vita ii-active collision avoidance in real traffic[END_REF][START_REF] Thomanek | Autonomous road vehicle guidance in normal traffic[END_REF][START_REF] Behringer | Autonomous road vehicle guidance from autobahnen to narrow curves[END_REF] (cf. Fig. 1.4). The steering, throttle and brakes were controlled through computer commands based on a real time processing of image sequences taken by four cameras with different focal lengths for each hemisphere. This system allowed to track other vehicles and read road markings automatically.

In the 1990s, National Aeronautics and Space Administration (NASA) launched the Mars Pathfinder mission, with the objective of landing of a first robotic rover (Sojourner) to the Mars surface [START_REF] Lindemann | Mars exploration rover mobility assembly design, test and performance[END_REF][START_REF] Bajracharya | Autonomy for mars rovers: Past, present, and future[END_REF] (cf. Fig. 1.4). Sojourner was built with six-wheel instead of fourwheel since it allows more stability and obstacle-crossing capabilities [START_REF] Cooper | Driving on the surface of mars using the rover control workstation[END_REF][START_REF] Volpe | Navigation results from desert field tests of the rocky 7 mars rover prototype[END_REF]. The control commands were operated from Earth using a graphical interface of Rover Control Software. The software allowed to Sojourner to capture its surrounding from various angles, carrying the analysis of terrain features or locating waypoints [START_REF] Matijevic | Sojourner: The mars pathfinder microrover flight experiment[END_REF][START_REF] Matijevic | The mission and operation of the mars pathfinder microrover[END_REF].

In the 2000s, DARPA [START_REF] Challenges | [END_REF] (Defense Advanced Research Projects Agency) and ELROB [START_REF][END_REF] (European Land-Robot Trial) challenges motivated the development of new vehicle technologies for different complex environments in military, rural and urban areas [START_REF] Iagnemma | Editorial for journal of field robotics Ůspecial issue on the darpa grand challenge[END_REF][START_REF] Lamon | The smartTer for ELROB 2006-a vehicle for fully autonomous navigation and mapping in outdoor environments[END_REF][START_REF] Thrun | The robot that won the darpa grand challenge[END_REF][START_REF] Buehler | The DARPA urban challenge: autonomous vehicles in city traffic[END_REF][START_REF] Schneider | Professional ground robotic competitions from an educational perspective: A consideration using the example of the european land robot trial (elrob)[END_REF]. In the last years, the development of fully autonomous vehicle for transportation field has received more attention in different countries [START_REF] Burns | Sustainable mobility: a vision of our transport future[END_REF]. The objective of autonomous vehicles is to improve quality of life, as pollution reduction and accidents prevention, etc. [START_REF] Fagnant | Preparing a nation for autonomous vehicles: Opportunities, barriers and policy recommendation[END_REF][START_REF] Litman | Autonomous Vehicle Implementation Predictions: Implications for Transport Planning[END_REF][START_REF] Vine | Autonomous cars: The tension between occupant experience and intersection capacity[END_REF]. One of the UGVs that has drawn the public's attention is the Google driverless car [START_REF] Markoff | Google cars drive themselves[END_REF][START_REF] Thrun | GoogleŠs driverless car[END_REF] (cf. Fig. 1.5(b)). This vehicle has a LIDAR mounted on the top which allows a view of 360 • . This data is used to generate a high-resolution 3D map of its environment (accurate to about 11 cm). Currently, the system needs to have a prior map of the area (pre-defined map) where the vehicle is supposed to drive, including traffics lights, telephone poles, crosswalks and static infrastructure [START_REF] Thrun | GoogleŠs driverless car[END_REF]. This map allows to quickly identify dynamic objects (e.g., pedestrians and cyclists) and to improve the accuracy of the localization system based on GPS, gyro-scopes, altimeters and tachymeters. This information is merged by the on-board software to analyze the traffic situation and generate a safe vehicle's path. The on-board software Dickmanns builds the first real robot car (VaMoRS) using saccadic vision and probabilistic methods. x 1987: Hughes Research Laboratory (HRL) generates the software system for the first autonomous navigation of land vehicles in cross-country Terrain using digital maps and sensor-based operation.

x 2002: iRobot trades domestic autonomous mobile robots (Roomba) using mechanical bumpers and omnidirectional infrared sensors. x 2004: First DARPA Grand Challenge is organized with no traffic, dirt roads, driven by a predefined route and obstacle types known in advance. None of the vehicles finished the route. x 2006: DARPA presents the armed and unmanned vehicles, MDARS-I and MDARS-E, which can navigate autonomously in complicated environments. x 2007: New mobile robots for different applications, e.g. Tug, Speci-Minder, Seekur, PatrolBot; and new challenges by ELROB and DARPA for off-road and urban autonomous vehicles.

x 2010: University of Parma drives a small electric car Piaggio from Parma to Shangai (VIAC) using several sensors such as cameras, laser, GPS, IMU, etc. x 2010: Apojee and Institut Pascal presents VIPALab, a compact platform with a set of integrated sensors (GPS, LIDAR, cameras, IMU, etc.) for research on autonomous vehicles. system controls brake, gas and steering systems. Other vehicle manufacturers developing the autonomous vehicle are Mercedes (Mercedes F015), Audi (Audi A7) and BMW (BMW M235i) among others [START_REF] Volkswagen | Autonomous vehicles[END_REF][START_REF] Vine | Autonomous cars: The tension between occupant experience and intersection capacity[END_REF].

This thesis focuses on the autonomous navigation task (level 4/5 according to [359,[START_REF] Sae) | Taxonomy and definitions for terms related to on-road motor vehicle automated driving systems[END_REF] (cf. Fig. 1.6), i.e., driverless vehicle which performs a safe navigation while solving different critical situations). This task is defined as the displacement of a single and multiple UGVs from a started point to an end point (goal or target) in a structured area (with static and dynamic obstacles, roads, corners, roundabouts and others).

The following section describes the main control architectures allowing autonomous navigation of UGVs.

No Automation

The full-time performance by the human driver of all aspects of the dynamic driving tasks, even when enhanced by warning or intervention systems.

0

Human driver

Human driver

Human driver n/a Driver Assistance

The driving mode-specific execution by a driver assistance system of either steering or acceleration/deceleration using information about the driving environment and with the expectation that the human driver perform all remaining aspects of the dynamic driving task.

1

Human driver and system

Human driver

Human driver

Some driving modes Driver Assistance

The driving mode-specific execution by one or more driver assistance systems of both steering and acceleration/deceleration using information about the driving environment and with the expectation that the human driver perform all remaining aspects of the dynamic driving task.

2

System

Human driver

Human driver

Some driving modes Conditional Automation

The driving mode-specific performance by an automated driving system of all aspects of the dynamic driving task with expectation that the human driver will respond appropriately to a request to intervene.

3

System

System

Human driver

Some driving modes High Automation

The driving mode-specific performance by an automated driving system of all aspects of the dynamic driving task, even if a human driver does not respond appropriately to a request to intervene.

4

System

System System Some driving modes Full Automation the full-time performance by an automated driving system of all aspects of the dynamic driving task under all roadway and environmental conditions that can be managed by a human driver.

1.3/ CONTROL ARCHITECTURES FOR UGVS

Autonomous control of an elementary single UGV basically consists of four main processes (cf. Fig. 1.7) which are described below:

(a) Perception: Different sensors are used to obtain an accurate data from the UGV and its environment:

• Proprioceptive sensor: It obtains information about the current robot's state (position, orientation, velocity, battery level, etc.). Some examples are: odometers, gyroscope and Inertial Measurement Unit (IMU).

• Exteroceptive sensor: It obtains information from the environment. Some examples are Global Positioning System (GPS), camera, range sensors (LIDAR, sonar and infrared).

One of the task of the perception system is to detect different obstacles (static or dynamic) in the environment. It is a crucial process since the localization and decision process depend upon it. This process must provide a good estimation of the obstacle positions in the near surroundings of the vehicle and this with low latency and a high update frequency. The main approaches use range sensor [START_REF] Kamon | A new range-sensor based globally convergent navigation algorithm for mobile robots[END_REF][START_REF] Rejas | A varying density 3d laser scanner for unmanned ground vehicles mapping and obstacle detection[END_REF] and cameras [START_REF] Erbs | Moving vehicle detection by optimal segmentation of the dynamic stixel world[END_REF][START_REF] Wu | Research progress of obstacle detection based on monocular vision[END_REF]. The low-cost of the camera has motivated many works in this area such as stereo-vision, omnidirectional camera, monocular, etc. [START_REF] Matthies | Obstacle detection for unmanned ground vehicles: A progress report[END_REF][START_REF] Barth | Vehicle Tracking and Motion Estimation Based on Stereo Vision Sequences[END_REF][START_REF] Mattoccia | A compact, lightweight and energy efficient system for autonomous navigation based on 3d vision[END_REF].

The range sensors have an accurate measurement in different environmental conditions (fog, rain, etc.) [START_REF] Mckinney | Components of an autonomous all-terrain vehicle[END_REF][START_REF] Wei | Towards a viable autonomous driving research platform[END_REF]. This thesis dedicates a short part for this process using range sensors (LIDAR) (cf. Section 3.4).

(b) Localization:

This process determines the vehicle's pose w.r.t. its work environment. It is a critical process since most of other processes depend upon it.

The main localization methods are based on proprioceptive sensors (odometers [START_REF] Doh | Accurate relative localization using odometry[END_REF][START_REF] Rahok | Odometry correction with localization based on landmarkless magnetic map for navigation system of indoor mobile robot[END_REF] and gyroscope [START_REF] Barshan | Evaluation of a solid-state gyroscope for robotics applications[END_REF][START_REF] Myung | Mobile robot localization with gyroscope and constrained kalman filter[END_REF]) and exteroceptive sensors (GPS [START_REF] Bulusu | Gps-less low-cost outdoor localization for very small devices[END_REF][START_REF] Lenain | A new nonlinear control for vehicle in sliding conditions: Application to automatic guidance of farm vehicles using RTK GPS[END_REF],

Figure 1.7: Reference control scheme for UGV's autonomous navigation.

camera [START_REF] Zhou | Mobile robot self-localization based on global visual appearance features[END_REF][START_REF] Tomono | 3-d localization and mapping using a single camera based on structurefrom-motion with automatic baseline selection[END_REF], range sensors [START_REF] Elfes | Sonar-based real-world mapping and navigation[END_REF][START_REF] Kim | Robust geometric-based localization in indoor environments using sonar range sensors[END_REF]). Commonly, a data fusion from the mentioned sensors is required to provide an accurate estimation of the vehicles' pose. A great extent of research focus on the data fusion using Extended Kalman Filter (EKF) [START_REF] Thrapp | Robust localization algorithms for an autonomous campus tour guide[END_REF], Markov localization [START_REF] Gutmann | An experimental comparison of localization methods[END_REF], particle filter [START_REF] González | Mobile robot localization based on ultra-wideband ranging: A particle filter approach[END_REF] or robust filter dealing with data uncertainties [START_REF] Inoue | Robust state-space estimation for mobile robot localization[END_REF]. Nevertheless, some drawbacks are still existing in real application in which some elements can influence the localization such as luminosity for camera or material type for range sensor or indoor environment for GPS, as well as the time processing for data fusion of several sensors, and accuracy of sensors [START_REF] Boukerche | Vehicular ad hoc networks: A new challenge for localization-based systems[END_REF][START_REF] Li | Localization for intelligent vehicle by fusing mono-camera, low-cost gps and map data[END_REF]. Moreover, different approaches from literature have been developed to contribute to Perception and Localization processes such as Simultaneous Localization and Mapping (SLAM) [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF][START_REF] Bosse | Map matching and data association for large-scale twodimensional laser scan-based slam[END_REF][START_REF] Lategahn | Visual slam for autonomous ground vehicles[END_REF][START_REF] Avanzini | Using monocular visual SLAM to manually convoy a fleet of automatic urban vehicles[END_REF]. SLAM can be defined as the problem of building a map of an unknown environment by a mobile robot while navigating at the same time in the environment. The main challenging issue of SLAM is that a good map is needed for accurate localization while an accurate pose estimation is needed to build a map [START_REF] Siegwart | Introduction to Autonomous Mobile Robots[END_REF][START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF]. This thesis focuses in the development of the control system. We assume an accurate localization system allowing to perform all simulation results. In experiments, a data fusion using EKF with RTK-GPS and gyrometer is implemented to obtain an accurate pose.

(c) Decision: This process is the outcome based on the different elements such as the assigned task, the perceived environment from perception process, the vehicle's pose and the priority of the system. For instance, the vehicle navigates towards its goal, an obstacle is detected, therefore, the decision process has to choose if the vehicle stops or the vehicle must maneuver to avoid it according to the task to accomplish. Different architectures are designed to take into account the decision elements such as LAAS architecture (LAAS Architecture for Autonomous Systems) [START_REF] Alami | An architecture for autonomy[END_REF][START_REF] Ingrand | An architecture for dependable autonomous robots[END_REF], which is organized in three hierarchical levels named decision, execution and functional levels. The decision level includes the task planner and is also supervising its execution. The execution level receives tasks which transforms into procedures composed of elementary robot actions and supervises its execution while being reactive to the environment. The functional level embeds a set of elementary robot functions implementing control loops (motion control, perception, etc.).

Another example is CLARAty architecture (Coupled Layer Architecture for Robotic Autonomy) [START_REF] Volpe | The claraty architecture for robotic autonomy[END_REF][START_REF] Nesnas | An architecture for reusable robotic software[END_REF] which consists of two distinct layers named Functional and Decision Layer. The Functional layer defines the various actions of the system and adapts the action components to real devices. The Decision layer is in charge of high-level autonomy using the information from the Functional layer, the global resources and mission constraints. Furthermore, other architectures are based on artificial intelligence such as multi-agent systems where the interaction between virtual agents and the sensor data provides a merged command for execution [START_REF] Dafflon | Adaptive autonomous navigation using reactive multi-agent system for control law merging[END_REF], [START_REF] Dafflon | Making decision with reactive multi-agent systems: A possible alternative to regular decision processes for platoon control issue[END_REF]. In this work, the decision process is an elementary process where the noncollision of the vehicle with any obstacle is the highest priority (cf. Chapter 3).

(d) Action:

This process applies the commands generated by the decision process to the robot. The control law generates the commands to apply to the actuators (motor) according to the sensor information, decision process (maneuvers) and vehicle model. This command law can be very complex according to the system modeling and the desired task. For instance in navigation application, the common design of the command law is oriented to guide the vehicle along a trajectory [START_REF] Fang | Trajectory tracking control of farm vehicles in presence of sliding[END_REF][START_REF] Blazic | Four-state trajectory-tracking control law for wheeled mobile robots[END_REF]. Moreover the control law can take into account the uncertainties of the sensors and actuators (robust control [START_REF] Inoue | Robust state-space estimation for mobile robot localization[END_REF][START_REF] Inoue | Robust recursive control of a skid-steering mobile robot[END_REF]), and can even deal with fault in the system (fault-tolerant control [START_REF] Wang | Fault-tolerant control for electric ground vehicles with independently-actuated in-wheel motors[END_REF][START_REF] Zhang | Fault tolerant control scheme based on active fault diagnosis for the path tracking control of a 4wd electric vehicle[END_REF]). In this thesis, the designed control law is used for navigation while taking into account the UGV's modeling and its constraints (cf.

Chapter 3).

In the sequel, three kind of control architectures, Deliberative, Reactive and Hybrid architectures, for autonomous navigation are described according to the process detailed above (cf. Fig. 1.7) [START_REF] Adouane | Architectures de controle comportementales et reactives pour la cooperation d'un groupe de robots mobiles[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF][START_REF] Benzerrouk | Architecture de Contrôle Hybride pour les Systèmes Multi-robots Mobiles[END_REF].

1.3.1/ DELIBERATIVE ARCHITECTURES

This kind of architecture is also named Sense-Model-Plan-Act (SMPA). It is characterized by sending the perceived information to several processing stages (Decision process) based on modeling and planning methods to obtain a consistent plan for current situation. Then, the plan is successively decomposed by other function modules until the desired actions can be directly executed by the actuators. The straightforward way to depict that is horizontal (or lateral) decomposition (cf. Fig. 1.8). This SMPA architecture using three functional elements: sensing (camera), planning and executing systems was implemented in Shakey robot of Stanford University [START_REF] Nilsson | Principles of Artificial Intelligence[END_REF]. Another direct implementation of the SMPA architecture is the NASREM architecture [START_REF] Albus | Nasa/nbs standard reference model for telerobot control system architecture (nasrem)[END_REF][START_REF] Albus | Outline of a theory of intelligence[END_REF].

The Deliberative approach for autonomous navigation contains localization and decision modules (cf. Fig. 1.8). This approach assumes a complete or quasi-complete world modeling between sensing and acting [START_REF] Nilsson | A mobile automaton: An application of ai techniques[END_REF][START_REF] Nilsson | Principles of Artificial Intelligence[END_REF]. This world modeling can be used for localization w.r.t. a map of the environment using the collected sensor data. The world modeling requires more effort to be built (it requires accurate sensors and fast processing time) allowing an architecture that can successfully navigate in different environments.

The drawbacks of this approach is to design an accurate and reliable world modeling for internal representation of the robot. If the model diverges from reality (i.e., if the map is wrong), then the robot's navigation may be dangerous and incorrect due to possible misguided commands. Another drawback of this approach is that the several stages, through which all sensor information must be processed, can generate an inevitable high delay in the loop between input and action [START_REF] Nilsson | A mobile automaton: An application of ai techniques[END_REF][START_REF] Nilsson | Principles of Artificial Intelligence[END_REF]. This delay can be reduced by improving the processing capabilities of the modules.

1.3.2/ REACTIVE ARCHITECTURES

This approach is based on stimuli-response, e.g., the control system reacts to its sensors and drives its actuators. It was proposed in opposition to the Deliberative architectures to avoid to rely upon world modeling. This approach uses minimal or any state information which implies that the behaviors may not directly accomplish the goal since the robot could not have any feedback related to current state of its assigned task (cf. Fig. 1.9). Reactive approach was inspired by biological systems which have evolved patterns of behavior without requiring neither exact models for control nor detailed planning before they can act [START_REF] Arkin | Behavior-Based Robotics[END_REF].

Main advantage of this approach correspond to its very simple computational implementation which ensures fast response to external events enabling the robot's navigation in very dynamic environments [START_REF] Arkin | Behavior-Based Robotics[END_REF][START_REF] Adouane | Architectures de controle comportementales et reactives pour la cooperation d'un groupe de robots mobiles[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]. Different approaches have been developed to create reactive systems with diverse levels of abstraction . An example is Subsumption architecture developed by Brooks [START_REF] Brooks | A robust layered control system for a mobile robot[END_REF]. Each module is an instance of a fairly simple computational machine named Behaviors. This approach proposes a vertical decomposition and the sensor data is used to select actions in an intimate and bottom-up way (cf. Fig. 1.9). Other examples are behavioral architecture [START_REF] Nicolescu | A hierarchical architecture for behavior-based robots[END_REF][START_REF] Faisal | Behavior based mobile for mobile robots navigation and obstacle avoidance[END_REF] and some approaches based on neural network [START_REF] Rusu | Behaviorbased neuro-fuzzy controller for mobile robot navigation[END_REF] or fuzzy logic [START_REF] Seraji | Behavior-based robot navigation on challenging terrain: A fuzzy logic approach[END_REF].

Main disadvantages of reactive architecture are the lack of state information, in particular world models. This lack of information makes it difficult to enable look ahead planning and goal-directed behavior (i.e., the robot has to reached a specific position named goal or target). Another drawback is the elementary behaviors that have to be thoughtfully designed to produce the desired actions. This behavior/process design may be timeconsuming and might heavily rely on the specific robot hardware and its environment [START_REF] Pfeifer | Self-organization, embodiment, and biologically inspired robotics[END_REF]. Moreover, this approach can have several active behaviors at any time which produce several switches between multiple behaviors damaging the robot's hardware. The addition of each new behavior forces the robot designer to reanalyze the whole existing architecture to ensure its accurate overall stability [START_REF] Adouane | Hybrid and safe control architecture for mobile robot navigation[END_REF].

1.3.3/ COMPARISON AND COMBINATION OF ARCHITECTURES

One of the most important discussion in the domain of control systems for autonomous robots is the Deliberative versus Reactive architectures [START_REF] Orebäck | Evaluation of architectures for mobile robotics[END_REF]. One problem of Reactive approach is the extremely difficult design of the primitive behaviors for all situations even for unforeseen situations [START_REF] Simmons | Monitoring and error recovery for autonomous walking[END_REF]. As the higher-level behaviors are designed with a knowledge of the characteristics of the lower-level ones, a modification to a primitive behavior may produce modifications to several other behaviors. Furthermore, the modeling of intelligence based on behavior does not scale to human-like problems and performance [START_REF] Tsotsos | Behaviorist intelligence and the scaling problem[END_REF]. For instance, the ability to search images to find stimuli on which to act is a necessary component of any intelligent agent. Another drawback of reactive approach is that the robot does not have an action plan, its global behavior is constructed from the interactions between the limited behaviors which may be insufficient to perform more complex behaviors since these complex behavior can require other behaviors which are not embedded in the robot [START_REF] Chatila | Deliberation and reactivity in autonomous mobile robots[END_REF].

Reactive approaches may have more limited abilities since most of the strictly Reactive robots are not based on complex sensors. Nevertheless, the deliberative architectures based on artificial intelligence (AI) may not suitable for real-time application in dynamic world [START_REF] Brooks | New approaches to robotics[END_REF].

The main problem of the selection of a kind of architecture is to find the right equilibrium between the desired intelligent behavior and the necessary reactivity, since:

• The existence of a specialized agent deliberating about the access to the environment usually improves the quality of the robot's response to changes according to its environment. However, it can also introduce time delays between the decision and the execution of the actions.

• A central module with a global world modeling generally leads the system to make better choices about the actions to accomplish, however it is a potential communication bottleneck.

Therefore, the selection of the appropriate architecture is a technological problem for which the designer must consider the required degrees of reactivity and intelligent behavior, efficiency to perform the task and the related implementation costs [START_REF] Simmons | Structured control for autonomous robots[END_REF]. For instance, if a reactive approach is used, the reactivity of the robot can be easily obtained, however some work is necessary to ensure that the current robot behavior corresponds to the desired action in all situations. On the other hand, with a Deliberative approach the robot responses are more predictable, but an excessive complexity of the policy of access to the environment model can dangerously increase its reaction time.

Several architectures, named hybrid architectures have been designed to overcome the drawbacks related to traditional deliberative and reactive architectures, i.e. world modeling, intelligence and reactive mechanisms. Three layer architectures are developed to integrate both architectures [START_REF] Alami | An architecture for autonomy[END_REF][START_REF] Ingrand | An architecture for dependable autonomous robots[END_REF]. These layers uses three levels of abstraction, a planning, an executive and a reactive layers (cf. Fig. 1.10). They are described below:

• The Planning layer correspond to strategic planning as well as eventual plan modification. This level relies on very abstract knowledge, highly complex reasoning methods and the typical domain of AI planners.

• The Executive layer correspond to reactive planner which selects and executes appropriate strategies with regards to the designed rules. A strategy is an ordered set of actions, i.e. hierarchies of actions. Execution of strategy leads finally to activation/deactivation of reactive layer behaviors.

• The Reactive layer performs the transition from planning to numerical control and the combination of separate behaviors.

An advantage of the hybrid approach is the elimination of some levels of hierarchy according to the considered application. For instance, an implementation without the higher planning level (only decisional and functional levels), since the control architecture is addressed to short planning based on pre-written ordered sets of actions is presented in in [START_REF] Medeiros | Priorities and data abstraction in hierarchical control architecture for autonomous robots[END_REF]. Moreover, the authors demonstrate the communication and interaction between the modules applied to the real world. The LAAS architecture is an example of a hybrid architectures [START_REF] Alami | An architecture for autonomy[END_REF][START_REF] Ingrand | An architecture for dependable autonomous robots[END_REF]. The different elements to sense (perception modules), model (mapping modules), plan (planners) and act (actuator modules) indicates the presence of deliberative approach. The hierarchy of levels is implemented in vertical decomposition. The modules, under certain circumstances, can operate in parallel to directly enter predefined sensors and actuators, indicating the presence of a reactive approach. Other examples are Hughes Research laboratory software for planning and control requirements of an autonomous land vehicle [START_REF] Payton | An architecture for reflexive autonomous vehicle control[END_REF][START_REF] Olin | Autonomous cross-country navigation: an integrated perception and planning system[END_REF], Blackboard architecture [START_REF]A blackboard architecture for control[END_REF], control architecture for CaRINA Intelligent Robotic Car [START_REF] Fernandes | Carina intelligent robotic car: Architectural design and applications[END_REF], SOAR architecture [START_REF] Hanford | Control of mobile robots using the soar cognitive architecture[END_REF], Task-Control Architecture (TCA) [START_REF] Simmons | Monitoring and error recovery for autonomous walking[END_REF][START_REF] Simmons | Structured control for autonomous robots[END_REF] and others [START_REF] Orebäck | Evaluation of architectures for mobile robotics[END_REF].

Different applications, works and control architecture of UGVs are shown in the previous sections. The following chapter extends this concept for a group of UGVs whose cooperate to achieve the assigned task.

1.4/ CONCLUSION

This chapter provided an overall review of mobile robots research with a special focus on autonomous navigation field. Due to the large amount of works, only some relevant works for single navigation with its architecture are described. This description aims to present related works and justify the choice of the proposed architecture. The described control architecture were classified according to the decision capacity of their components, the reactive system keeps the sensor and/or actuators connected to all modules, and the deliberate system requires a processing and a management of a data information by a planner. The navigation of a single vehicle is addressed (cf. Section 1.2), the proposed architecture allows reactive navigation using behaviors and local information. The cognitive part allows the planning of the navigation through the optimal set of waypoints instead of using any trajectory planning. The proposed architecture benefits of the main advantages of the deliberative and reactive architectures described in Section 1.3.

COOPERATIVE MULTI-ROBOT

NAVIGATION

This chapter describes the state of the art in mobile multi-robot systems with a particular focus on autonomous navigation of a group of vehicles. An important part of the chapter is dedicated to several control architectures of multi-robot system designed to perform navigation in formation. Various formation control approaches are also detailed.

The basis of the cooperative robotics can be defined by "Given some task specified by a designer which may be performed by a single complex robot, a group of elementary robots displays cooperative behavior if, due to join their capacities and information, there is an increase in the total utility of the system such as more robustness, more flexibility and more efficiency w.r.t. the performance of a single complex robot" [START_REF] Cao | Cooperative mobile robotics: Antecedents and directions[END_REF]. (d) Two cooperating rovers carrying long container by NASA [START_REF] Schenker | Planetary rover developments supporting mars exploration, sample return and future human-robotic colonization[END_REF].

(e) Group of quadracopters building a tower by GRASP lab (University of Pennsylvania-USA) [START_REF] Lindsey | Construction with quadrotor teams[END_REF].

CHAPTER 2. COOPERATIVE MULTI-ROBOT NAVIGATION

These cooperative robotic systems can be classified in two categories [START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]:

• Collective swarm systems: Each cooperative robot executes its assigned task with minimal information related to the other robots. Some multi-robot application fields are exploration in hazardous environments [START_REF] Leitner | Multi-robot cooperation in space: A survey[END_REF][START_REF] Lozenguez | Map partitioning to approximate an exploration strategy in mobile robotics[END_REF][START_REF] Cabrera-Mora | Fleet size of multi-robot systems for exploration of structured environments[END_REF], entertainment [START_REF] Siegwart | Human-robot swarm interaction for entertainment: from animation display to gesture based control[END_REF][START_REF] Baltes | Robocup humanoid league rule developments 2002-2014 and future perspectives[END_REF] (cf. Fig. 2.1(a)), mapping of unknown locations [START_REF] Miner | Swarm robotics algorithms: A survey[END_REF][START_REF] Jessup | Robust and efficient multi-robot 3d mapping with octree based occupancy grids[END_REF][START_REF] Li | Multivehicle cooperative local mapping: A methodology based on occupancy grid map merging[END_REF], management and platooning of autonomous vehicles [START_REF] Daviet | Platooning for small public urban vehicles[END_REF][START_REF] Chen | Smooth formation navigation of multiple mobile robots for avoiding moving obstacles[END_REF][START_REF] El-Zaher | Echelon platoon organisation: a distributed approach based on 2-spring virtual links[END_REF] (cf. Fig. 2.4), rendezvous of multiple agents [START_REF] Sinha | Generalization of linear cyclic pursuit with application to rendezvous of multiple autonomous agents[END_REF][START_REF] Valdastri | Micromanipulation, communication and swarm intelligence issues in a swarm microrobotic platform[END_REF][START_REF] Becerra | Visual navigation of wheeled mobile robots using direct feedback of a geometric constraint[END_REF] and coverage of unknown area [START_REF] Dasgupta | Effects of multi-robot team formations on distributed area coverage[END_REF].

• Intentionally cooperative systems: Each cooperative robot has information related to the other robot, and the global task is performed according to their current configurations and/or abilities. Some multi-robot application fields are search and rescue [START_REF] Kitano | Robocup rescue: search and rescue in large-scale disasters as a domain for autonomous agents research[END_REF]336,[START_REF] Pellenz | Robocup rescue robot league. THE 18TH ANNUAL ROBOCUP INTERNATIONAL SYMPOSIUM[END_REF] (cf. Fig. 2.1(b)), cooperative manipulation [START_REF] Tinós | A fault tolerance framework for cooperative robotic manipulators[END_REF][START_REF] Bischoff | The kuka-dlr lightweight robot arm -a new reference platform for robotics research and manufacturing[END_REF][START_REF] Siqueira | Robust control of cooperative manipulators[END_REF] (cf. Fig. 2.1(c)), transport of heavy objects [START_REF] Chien | A comparison of coordinated planning methods for cooperating rovers[END_REF][START_REF] Schenker | Planetary rover developments supporting mars exploration, sample return and future human-robotic colonization[END_REF][START_REF] Chaimowicz | A mechanism for dynamic coordination of multiple robots[END_REF] (cf. Fig. 2.1(d)), construction [START_REF] Lindsey | Construction with quadrotor teams[END_REF][START_REF] Werfel | Designing collective behavior in a termite-inspired robot construction team[END_REF] and reconfigurable robots [START_REF] Dorigo | a novel concept for the study of heterogeneous robotic swarms[END_REF][START_REF] Vagaš | The chosen aspects of selfreconfigurable robots[END_REF][START_REF] Zhu | A simplified approach to realize cellular automata for ubot modular self-reconfigurable robots[END_REF].

Over the last decades, research in cooperative robotics has been developed (cf. Fig. 2.2), mainly for its advantages w.r.t. single robot system such as:

• Achievement of the task by a group of simple robots instead of a single complex robot [START_REF] Triannia | Cooperative hole avoidance in a swarmbot[END_REF][START_REF] Pellegrinelli | Multi-robot spot-welding cell design: Problem formalization and proposed architecture[END_REF] since the task is distributed among robots [START_REF] Ulusoy | Robust multi-robot optimal path planning with temporal logic constraints[END_REF][START_REF] Yasuda | Distributed coordination architecture for cooperative task planning and execution of intelligent multi-robot systems[END_REF].

• Parallelism solutions of the group of robots instead of a sequence of solution of a single robot [START_REF] Kheddar | Parallel multi-robots long distance teleoperation[END_REF][START_REF] Carpin | On parallel rrts for multi-robot systems[END_REF].

• Robustness of the group of robots (one can be changed or added related to a redundant system) [START_REF] Hazon | Redundancy, efficiency and robustness in multi-robot coverage[END_REF][START_REF] Huang | Robust multi-robot coordination in pick-and-place tasks based on part-dispatching rules[END_REF]. Moreover, each robot has an elementary design and construction instead of a single powerful well-equipped robot [START_REF] Laird | Design and validation of a centimeter-scale robot collective[END_REF][START_REF] Mclurkin | A robot system design for low-cost multi-robot manipulation[END_REF].

2.1/ OVERVIEW

A brief overview of the evolution of the multi-robot systems is detailed in Fig. 2.2. In the 1970s, Distributed Artificial Intelligence (DAI), in contrast to earlier AI, emerged to deal with the coordination and interaction problems involving multiple intelligent software agents [START_REF] Hare | Foundations of distributed artificial intelligence[END_REF][START_REF] Bond | Readings in distributed artificial intelligence[END_REF]. The two main approaches are multi-agent systems and distributed problem solving [START_REF] Bond | Readings in distributed artificial intelligence[END_REF].

The rapid progress of research in cooperative robotics has begun in the 1980s mainly in the field of mobile robots [START_REF] Cao | Cooperative mobile robotics: Antecedents and directions[END_REF][START_REF] Parker | Current state of the art in distributed autonomous mobile robotics[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]. Commonly, some projects were developed in Cellular (or reconfigurable) robot systems (CEBOT project [START_REF] Fukuda | Approach to the dynamically reconfigurable robotic system[END_REF][START_REF] Fukuda | Dynamically reconfigurable robotic system[END_REF] and SWARM project [START_REF] Beni | The concept of cellular robotic system[END_REF][START_REF] Beni | Swarm intelligence in cellular robotic systems[END_REF]), in Self-organization of agents [START_REF] Steels | Cooperation between distributed agents through self-orcamsation[END_REF], in MRS architectures (ACTRESS project [START_REF] Asama | Design of an autonomous and distributed robot system: Actress[END_REF]) and in MRS motion planning (traffic control [START_REF] Premvuti | Consideration on the cooperation of multiple autonomous mobile robots. In Intelligent Robots and Systems '90[END_REF], formations [START_REF] Arai | Collision avoidance among multiple robots using virtual impedance[END_REF][START_REF] Wang | Navigation strategies for multiple autonomous mobile robots moving in formation[END_REF][START_REF] Wang | Navigation strategies for multiple autonomous mobile robots moving in formation[END_REF] and GOFER project [START_REF] Caloud | Indoor automation with many mobile robots[END_REF]).

In the 1990s, the fast development of reliable communication systems have increased largely different work in MRS (cf. Fig. 2.2). Many works were based on behavior-based approaches [START_REF] Brooks | New approaches to robotics[END_REF] and implemented in physical cooperative robots [START_REF] Cai | Hierarchical control architecture for cellular robotic system-simulations and experiments[END_REF][START_REF] Mataric | Cooperative multi-robot box-pushing. In Intelligent Robots and Systems 95[END_REF]. In 1994, the European PROMETHEUS project [START_REF] Williams | Prometheus-the european research programme for optimising the road transport system in europe[END_REF][START_REF] Hellåker | Prometheus-strategy[END_REF]80] tested the twin robot vehicles VaMP and VITA-2 driving more than 1000 km on a Paris multi-lane highway in standard heavy traffic at maximum velocity of 130 km/h (cf. Fig. 2.2). They demonstrated the use of visionbased control for autonomous driving in free lanes, convoy driving, automatic tracking of other vehicles and lane changes with autonomous passing other cars [START_REF] Dickmanns | 4-d dynamic vision for intelligent motion control[END_REF][START_REF] Maurer | A system architecture for autonomous visual road vehicle guidance[END_REF][START_REF] Hofmann | Radar and vision data fusion for hybrid adaptive cruise control on highways[END_REF][START_REF] Corke | Robotics, Vision and Control: Fundamental Algorithms in MATLAB[END_REF].

In 1997, California's Partners for Advanced Transit and Highways (PATH) project [419] demonstrated multiple automobiles (Honda-PATH vehicles) capable of platoon driving and safe maneuvers such as lane changes [START_REF] Rajamani | Design and experimental implementation of control for a platoon of automated vehicles[END_REF][START_REF] Tan | Demonstration of an automated highway platoon system[END_REF] (cf. Fig. 2.3(a)). The communication system is used to send the required information to the controllers of each vehicle (lateral (steering) and longitudinal (spacing and speed) controllers) [START_REF] Shladover | Automated vehicle control developments in the path program[END_REF][START_REF] Shladover | Automated vehicle control developments in the path program[END_REF].

In the 2000s, the cooperative systems were extended to cooperative control of multiple UAVs which has became a more attractive research area around the world [START_REF] Chandler | Uav cooperative control[END_REF][START_REF] Bertuccelli | Robust planning for coupled cooperative uav missions[END_REF][START_REF] Ryan | An overview of emerging results in cooperative uav control[END_REF][START_REF] Jeyaraman | Formal techniques for the modelling and validation of a co-operating uav team that uses dubins set for path planning[END_REF]. The multi-robot systems inspired by biological systems [START_REF] Dorigo | Ant colony optimization theory: A survey[END_REF] for large number of robots such as swarm intelligence [START_REF] Liu | Biomimicry of social foraging bacteria for distributed optimization: models, principles, and emergent behaviors[END_REF], flocking and schooling [START_REF] Nagpal | Programmable self-assembly using biologically-inspired multiagent control[END_REF][START_REF] Jadbabaie | Coordination of groups of mobile autonomous agents using nearest neighbor rules[END_REF] were more developed. In 2004, the Centibots project funded by DARPA [START_REF] Ortiz | Task inference and distributed task management in the centibots robotic system[END_REF][START_REF] Konolige | Centibots: Very large scale distributed robotic teams[END_REF] demonstrated the use of 100 robots (Amigobots and Pioneer 2 AT from ActivMedia Robotic) for mapping,

x 1970s: AI based on multiple software agents is used to solve coordination and interaction problems. x 1980s: Several projects (CEBOT, ACTRESS, GOFER and SWARM ) uses a few group of robots to demonstrate the advantages of cooperative robots. Cruise Control (CACC) system using a group of Toyota PRIUS vehicles. The ACC system is a driver assistance system considering the traffic flow characteristics. x 2011: Grand Cooperative Driving Challenge (GCDC) is presented with the objective of driving a group of different autonomous vehicles in platoon (longitudinal control and inter-vehicle communication). x 2011: PATH's project experiments an automated truck platooning using three tractor-trailer trucks. x 2012: Google's fleet of seven autonomous Toyota Prius drives more than 224 000 km in California State. x 2014: Harvard University presents the TERMES project, inspired by termites, which consists in developing a swarm construction system. Another Harvard's swarm project is the Kilobots project designed for very large number of small robot (Kilobot) to experiment collective artificial intelligence.

x 1991: EPFL develops the small differential wheeled mobile robots (Khepera) for research purposes such as swarm robotics. tracking and surveillance of an urban area. This project exploited the existing research in collaborative multilayer architecture and distributed robotics to apply on very large group of robots [START_REF] Konolige | Centibots: Very large scale distributed robotic teams[END_REF] (cf. Fig. 2.2).

In the last years, some of the most important research in cooperative robotics has been focused on the urban transportation system [START_REF] Chan | Sartre automated platooning vehicles[END_REF][START_REF] Gacovski | Different control algorithms for a platoon of autonomous vehicles[END_REF][START_REF] Ali | The flatbed platoon towing model for safe and dense platooning on highways[END_REF], cooperative air and ground vehicles for military applications [START_REF] Inoue | Controle robusto descentralizado de movimentos coordenados de robôs heterogêneos[END_REF][START_REF] Cognetti | Cooperative control of a heterogeneous multi-robot system based on relative localization[END_REF][START_REF] Ponda | Cooperative mission planning for multi-uav teams[END_REF] and mobile sensor networks [START_REF] Elakkiya | Performance evaluation of mobile sensor network[END_REF][START_REF] Vijayan | Improving network coverage and life-time in a cooperative wireless mobile sensor network[END_REF]. In 2011, a group of automated trucks was driven in platoon in PATH's project [START_REF] Bergenhem | Overview of platooning systems[END_REF]419] (cf. Fig. 2.3(b)). The experiment used three tractor-trailer trucks to demonstrate the energy savings in constant velocity and different maneuvers (to join and split from the platoon) and traveling up and down grades [START_REF] Tsugawa | An automated truck platoon for energy saving[END_REF][START_REF] Tsugawa | An overview on an automated truck platoon within the energy its project[END_REF]. The trucks were coupled electronically using vehicle to vehicle (V2V) communication and forward sensors to keep a specific inter-vehicle distance (10 m) [START_REF] Tsugawa | An automated truck platoon for energy saving[END_REF][START_REF] Tsugawa | An overview on an automated truck platoon within the energy its project[END_REF]. In 2012, Google presented a fleet of seven autonomous Toyota Prius hybrids driving more than 224000 km in California State [START_REF] Wirasingha | Plug-in hybrid electric vehicle developments in the us: trends, barriers, and economic feasibility[END_REF][START_REF] Moore | Autonomous vehicles for personal transport: A technology assessment[END_REF][START_REF] Prendi | Sustainability issues for vehicles and fleet vehicles using hybrid and assistive technologies[END_REF]. The vehicles combine data from Google Street View with cameras and LIDAR to obtain the vehicle's localization and to navigate safely in urban environments [START_REF] Wirasingha | Plug-in hybrid electric vehicle developments in the us: trends, barriers, and economic feasibility[END_REF][START_REF] Moore | Autonomous vehicles for personal transport: A technology assessment[END_REF].

Nevertheless, some of the specific challenges in MRS are still existing such as [START_REF] Murray | Recent research in cooperative control of multivehicle systems[END_REF][START_REF] Olson | Multi-robot systems and communications limits[END_REF][START_REF] Simonin | Multi-robot systems: from software needs to human monitoring[END_REF]:

• Accurate evaluation criteria for the performance of the multi-robot system.

• Scaling to larger numbers of robots (e.g., thousands) for cooperation tasks.

• Extensions to heterogeneous multi-robots (i.e., coordination of underwater, ground and aerial robots).

• Developing online replanning to deal with highly dynamic environment.

• Creating and implementing correct interaction approaches between the robots and human-robot.

• Design and incorporating robot motion and sensor constraints.

• Integrating the developed theoretical methods in real robots.

• Uncertainty, delay and fault-tolerant communication among the robots.

This thesis deals with the problem of MRS navigating in formation (i.e., each UGV of the group keeps a predefined position relative to each other or relative to a reference), mainly, applied to a group of UGVs for urban (private vehicles [START_REF] Klein Wolterink | Automated merging in a cooperative adaptive cruise control (cacc) system[END_REF][START_REF] Meyer | Research and innovation for automated driving in germany and europe[END_REF]) , agriculture (tractors and farm equipments [START_REF] Guillet | Adaptable robot formation control: Adaptive and predictive formation control of autonomous vehicles[END_REF][START_REF] Lu | Automated truck platoon control and field test[END_REF]) and military (fleet of military vehicle [START_REF] Healey | Application of formation control for multi-vehicle robotic minesweeping[END_REF][START_REF] Burgard | Coordinated multirobot exploration[END_REF]) fields.

In this context, a brief review of different projects around the world on platooning of MRS are presented in [START_REF] Grumert | Cooperative systems: an overview[END_REF][START_REF] Meyer | Research and innovation for automated driving in germany and europe[END_REF] such as:

• SARTRE project (SAfe Road TRains for the Environment) for automated closeformation vehicle platoons (Volvo car and trucks) in public traffic environments [START_REF] Dávila | Safe road trains for the environment[END_REF][START_REF] Chan | Cooperative control of sartre automated platoon vehicles[END_REF]. This project was funded by European Commission; from September 2009 to September 2012.

• CityMobil project for public transportation of automated transport shuttles Cybercars [START_REF] Fraichard | Cybercar: l'alternative à la voiture particulière[END_REF][START_REF] Parent | Past, present and future of the technology[END_REF] driving a low velocity in restricted well-known environments with few pedestrian [START_REF] Resende | A cooperative personal automated transport system -a citymobil demonstration in rocquencourt[END_REF][START_REF] Van Dijke | advanced transport for the urban environment[END_REF]. This project was funded by the European Commission; from May 2006 to December 2011.

• Connect & Drive project for vehicle platoon (Toyota Prius vehicles) testing the cooperative Adaptive Cruise Control (ACC) system [START_REF] Klein Wolterink | A dynamic geocast solution to support cooperative adaptive cruise control (cacc) merging[END_REF][START_REF] Klein Wolterink | Automated merging in a cooperative adaptive cruise control (cacc) system[END_REF] which use an algorithm designed to dampen traffic shock waves to decide whether the vehicles may accelerate, decelerate, or maintain their current velocity [START_REF] Heijenk | On the use of cooperative adaptive cruise control to increase traffic stability and efficiency[END_REF][START_REF] Ploeg | Connect & drive: design and evaluation of cooperative adaptive cruise control for congestion reduction[END_REF]. This project was funded by High Tech Automotive System (HTAS), Dutch Ministry of Economic Affairs, Agriculture and Innovation, and ING Car Lease; from January 2009 to Mars 2011.

• SafePlatoon project [309,[START_REF] Adouane | Safeplatoon: Sûreté de convois de véhicules autonomes[END_REF] studies the problem of autonomous vehicle navigating in convoy for application for urban, military and agriculture environments (cf. Fig. 2.4). The innovative characteristics lie in the design and development of the movement capabilities in an extensive and robust convoy. The project takes into account several geometric convoy configurations (linear,triangular, front line, etc.). It also integrates the ability of dynamic adaptation of the convoy configuration. An important aspect of the SafePlatoon project lies in the fact that the decision algorithms and proposed control/command will be verified and validated in actual experiment. This project was funded by Agence National de Recherche (ANR) -France , from Mars 2011 to September 2014.

Next section shows the main control architectures used for MRS navigation in formation.

2.2/ MULTI-ROBOT CONTROL ARCHITECTURES

Concerning the navigation in formation, the MRS architecture has the same essential elements as a single robot architecture, perception, localization, decision and action (cf. Subsection 1.3). In addition, MRS architecture has Communication process. This process deals with the following questions: "What do I have to do in the formation?". Furthermore, an architecture has to include elements dealing with the robot interaction and group behavior to achieve the navigation in formation.

The design of the MRS architecture determines the properties of the system such as robustness, flexibility, fault-tolerant, scalability, etc. [START_REF] Parker | Current state of the art in distributed autonomous mobile robotics[END_REF][START_REF] Alami | Multi-robot cooperation : Architectures and paradigms[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF].

In the sequel, the most common approaches for the MRS architecture based on management of the control (to centralize the control in a single robot/entity or to decentralize the control among the robots [START_REF] Adouane | Architectures de controle comportementales et reactives pour la cooperation d'un groupe de robots mobiles[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF][START_REF] Benzerrouk | Architecture de Contrôle Hybride pour les Systèmes Multi-robots Mobiles[END_REF]) are described below.

2.2.1/ CENTRALIZED ARCHITECTURES

In this architecture, a single central unit of control (one supervisor robot [START_REF] Yuta | Coordinating autonomous and centralized decision making to achieve cooperative behaviors between multiple mobile robots[END_REF] or entity [START_REF] Khoshnevis | Centralized sensing and control of multiple mobile robots[END_REF]) coordinates the whole group of robot to execute the assigned task. This centralized controller has the necessary sensors to observe the robots and their environment, then it can compute the set-point (pose and velocities) to send to each robot to accomplish the assigned global task [START_REF] Alami | Multi-robot cooperation in the martha project[END_REF]. One advantage is that the centralized controller can easily broadcast a command message (e.g. emergency stop) for all robots.

The factors of motivation for this architecture are [START_REF] Khoshnevis | Centralized sensing and control of multiple mobile robots[END_REF]:

• Use of elementary robots, e.g., the robots have basic elements to achieve the task (wheels, motors, communication) and the perception sensors and processing data are made by the centralized controller.

• The centralization of the sensor information allows a better interpretation of the environments which can improve the decision process instead of rely on only the local perception of each robot (without merging process).

Main characteristics of this architecture are a high time-consuming in term of computation and processing of the data with a suitable frequency of communication of the whole system with the data (very important for real-time) [START_REF] Yuta | Coordinating autonomous and centralized decision making to achieve cooperative behaviors between multiple mobile robots[END_REF][START_REF] Khoshnevis | Centralized sensing and control of multiple mobile robots[END_REF][START_REF] Camacho | Roboskeleton: An architecture for coordinating robot soccer agents[END_REF]. Main drawbacks are the good performance of the centralized controller even when the group of robots are larger and the vulnerability of one controller in the case of fails or system errors.

An example of centralized architecture is proposed in [START_REF] Camacho | Roboskeleton: An architecture for coordinating robot soccer agents[END_REF]. This architecture named RoboSkeleton architecture (cf. Fig. 2.5) is based on the three layer architecture (cf. subsection 1.3.3) and it contains an agent that manages the other agents (cf. Fig. 2.5). If the controlling agent (CoachAgent) fails, the whole system could readily fail.

2.2.2/ DECENTRALIZED ARCHITECTURES

This architecture is characterized by the distribution of controller resources among the group of robots (e.g., perception sensor, processing unit, etc.) [START_REF] Feddema | Decentralized control of cooperative robotic vehicles: theory and application[END_REF][START_REF] Dimarogonas | Decentralized motion control of multiple holonomic agents under input constraints[END_REF]. This characteristic is required for multi-robot teams in which each robot takes decision based on its local information and communicates the progress to the others of their assigned task [START_REF] Baras | Decentralized control of autonomous vehicles[END_REF][START_REF] Kobayashi | Design of decentralized capturing behavior by multiple mobile robots[END_REF]. There are two types of decentralized architectures [START_REF] Cao | Cooperative mobile robotics: Antecedents and directions[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]:

• Distributed architecture: all robots have the same control [START_REF] Adouane | Behavioral and distributed control architecture of control for minimalist mobile robots[END_REF][START_REF] Ren | Distributed coordination architecture for multi-robot formation control[END_REF]. Some examples are ACTRESS (ACtor-based Robots and Equipments Synthetic System) [START_REF] Asama | Design of an autonomous and distributed robot system: Actress[END_REF][START_REF] Asama | A concept of distributed architecture for maintenance robot systems[END_REF], SWARM systems [START_REF] Beni | Swarm intelligence in cellular robotic systems[END_REF][START_REF] Jin | Stability of synchronized distributed control of discrete swarm structures[END_REF][START_REF] Matari Ć | Issues and approaches in the design of collective autonomous agents[END_REF], GOFER [START_REF] Caloud | Indoor automation with many mobile robots[END_REF][START_REF] Le Pape | A combination of centralized and distributed methods for multi-agent planning and scheduling[END_REF] and ALLIANCE [START_REF] Parker | Alliance: An architecture for fault tolerant, cooperative control of heterogeneous mobile robots. In Intelligent Robots and Systems' 94[END_REF][START_REF] Parker | Alliance: An architecture for fault tolerant multirobot cooperation[END_REF].

• Hierarchical architecture: this architecture is locally centralized [START_REF] Medeiros | Priorities and data abstraction in hierarchical control architecture for autonomous robots[END_REF][START_REF] Nicolescu | A hierarchical architecture for behavior-based robots[END_REF], i.e., the robots performs independently its assigned tasks, however, they send to a central entity information about the state of tasks. Some examples are CEBOT (CEllular roBOTics System) architecture [START_REF] Fukuda | Dynamically reconfigurable robotic system[END_REF][START_REF] Ueyama | Self-organization of cellular robots using random walk with simple rules[END_REF][START_REF] Iritani | Construction mechanism of group behavior with cooperation[END_REF], CAMPOUT (Control Architecture for Multirobot Planetary OUTposts) [START_REF] Pirjanian | Development of campout and its further applications to planetary rover operations: A multirobot control architecture[END_REF][START_REF] Huntsberger | Campout: A control architecture for tightly coupled coordination of multirobot systems for planetary surface exploration[END_REF] and UM-PRS (University of Michigan Procedural Reasoning System) [START_REF] Lee | Um-prs: An implementation of the procedural reasoning system for multirobot applications[END_REF][START_REF] Lee | Reactive-system approaches to agent architectures[END_REF] The advantages of this decentralized architecture are the robustness to system errors and failure (since no robot commands or depends on any other robot), possible parallel solution, reliable and scalable implementation [START_REF] Cao | Cooperative mobile robotics: Antecedents and directions[END_REF][START_REF] Feddema | Decentralized control of cooperative robotic vehicles: theory and application[END_REF][START_REF] Fierro | A framework and architecture for multi-robot coordination[END_REF][START_REF] Ren | Distributed coordination architecture for multi-robot formation control[END_REF]. Moreover, some complex tasks which require parallel processing can be solved using this architecture [START_REF] Défago | Distributed computing on the move: From mobile computing to cooperative robotics and nanorobotics[END_REF].

The drawbacks are a hard level of coordination since the assigned tasks of each robot are embedded in the local control and if any change in the task occurs then a global reconfiguration in the robots's tasks may be difficult without a supervisor.

A typical example is ALLIANCE/L-ALLIANCE architecture developed by Parker [START_REF] Parker | Alliance: An architecture for fault tolerant, cooperative control of heterogeneous mobile robots. In Intelligent Robots and Systems' 94[END_REF][START_REF] Parker | Task-oriented multi-robot learning in behavior-based systems[END_REF][START_REF] Parker | Alliance: An architecture for fault tolerant multirobot cooperation[END_REF] (cf. Fig. 2.6) for heterogeneous robots. Robots have information about their actions and the actions of the group through the top-broad communication topology. This architecture uses behavior-based controllers which depend on the assigned robot's task (motivational behaviors). L-ALLIANCE [START_REF] Parker | Task-oriented multi-robot learning in behavior-based systems[END_REF] is an extension of ALLIANCE which uses reinforcement learning to adjust the activation parameters of the behaviors.

2.2.3/ CENTRALIZED/DECENTRALIZED ARCHITECTURES

The Centralized/Decentralized architecture combines the high level controller with the local control of each robot (Centralized/Decentralized [START_REF] Noreils | Toward a robot architecture integrating cooperation between mobile robots: Application to indoor environment[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]). Therefore, the main advantages of both architecture are used such as central planner as high-level control over the autonomous robots, the robustness to errors, reprogramming of the global task/control, elimination of some hierarchical levels [START_REF] Medeiros | Priorities and data abstraction in hierarchical control architecture for autonomous robots[END_REF].

One example of Centralized/Decentralized architecture is DIRA (DIstributed Robot Architecture) or Layered Multirobot Architecture (LMA) developed by Simmons [START_REF] Simmons | First results in the coordination of heterogeneous robots for large-scale assembly[END_REF] (cf. Fig.

2.7)

. It is based on the three layer architecture (cf. subsection 1.3.3) and it allows coordination between robots (layer communication) and autonomy action of each one (behavior and sequencing layer). Other examples in the literature that use this architecture are Syndicate architecture [START_REF] Heger | Results in sliding autonomy for multi-robot spatial assembly[END_REF], Emotion-Based Control architecture [START_REF] Murphy | Emotion-based control of cooperating heterogeneous mobile robots[END_REF] and Multi-agent control architecture [START_REF] Vidal | Multi-agent probabilistic pursuit-evasion games with unmanned ground and aerial vehicles[END_REF].

This thesis uses the Centralized/Decentralized architecture since it is more suitable for our application: navigation in formation. The centralized approach allows to the central entity (Leader) to manage the configuration of the desired formation even for formation reconfiguration. The decentralized approach allows to each UGV (follower) locally generate its commands to track its assigned virtual target. This virtual target is related to the received leader's information according to the formation shape. The following section exposes the different approaches to keep a specific formation shape. A central entity uses the information from all sensors to localize the robots in the environment and to compute the commands to sent to them.

Each robot execute the command sent by the central entity.

Decentralized Each robot uses local information from its sensors to localize itself and to generate and execute its commands to accomplish the assigned task.

2.3/ FORMATION CONTROL

Many research laboratories have been focused in navigation in formation applications, as the current thesis work, since they can be used in transport, agricultural and military fields [START_REF] Klein Wolterink | Automated merging in a cooperative adaptive cruise control (cacc) system[END_REF][START_REF] Meyer | Research and innovation for automated driving in germany and europe[END_REF] (cf. Subsection 2.1).

The formation control is the ability to keep the position of a group of UGVs relative to each other or relative to a reference, i.e., the group of robots will maintain a desired geometric formation [START_REF] Sugihara | Distributed motion coordination of multiple mobile robots[END_REF][START_REF] Healey | Application of formation control for multi-vehicle robotic minesweeping[END_REF][START_REF] Murray | Recent research in cooperative control of multivehicle systems[END_REF]. The main existing complications in the navigation in formation of a group of UGV include:

• What is the desired formation?

• How do robots to determine their desired position in the formation?

• How do robots to determine their actual position in the formation?

• How do robots to move to ensure that the formation is maintained?

• What should robots do if there are obstacles?

• How do the robot formation performance is evaluated?

Different works have been developed to reply to these questions using different approaches such as Leader-follower, Virtual Structure, Behavior-based, Potential fields and Optimization-based [START_REF] Chen | Formation control: a review and a new consideration[END_REF][START_REF] Murray | Recent research in cooperative control of multivehicle systems[END_REF] which are described below.

2.3.1/ LEADER-FOLLOWER APPROACH

The formation is defined by one robot (leader) and the others (followers) define their positions according to the leader or another guide robot using their local information from their sensor (camera, LIDAR, etc.) [START_REF] Chen | Distributed motion coordination of multiple robots. In Intelligent Robots and Systems '94[END_REF][START_REF] Das | A vision-based formation control framework[END_REF][START_REF] El-Zaher | Echelon platoon organisation: a distributed approach based on 2-spring virtual links[END_REF]. The followers can use three different aspects of information to keep the desired formation shape [START_REF] Desai | Controlling formations of multiple mobile robots[END_REF][START_REF] Chen | Formation control: a review and a new consideration[END_REF]:

• Distance and orientation control (l -ψ): The follower has to keep a certain distance l j and orientation ψ j from the leader or guide robot (cf. the UGV j in Fig. 2.8(a)).

• Distance and distance control (l-l): The follower has to keep certain distances w.r.t. two other or more robots (cf. the UGV i in Fig. 2.8(a)). This (ll) control is more flexible than (l -ψ) since (ll) control can use as reference one robot with another one or even with an object. For instance, this control can be extended to the case of obstacle avoidance where a virtual robot (in the obstacle border) can be used to keep a specific distance to the obstacle (cf. UGV i in Fig. 2.8(b)).

The advantages of this approach are the implementation using decentralized architecture where each robot takes its own decision according to its local information [START_REF] Chen | Distributed motion coordination of multiple robots. In Intelligent Robots and Systems '94[END_REF][START_REF] Das | A vision-based formation control framework[END_REF][START_REF] Feddema | Decentralized control of cooperative robotic vehicles: theory and application[END_REF]. Some works use the control graph theory to represent the formation interaction or communication [START_REF] Mesbahi | Formation flying control of multiple spacecraft via graphs, matrix inequalities, and switching[END_REF][START_REF] Das | A vision-based formation control framework[END_REF][START_REF] Das | Control graphs for robot networks[END_REF][START_REF] Chen | Smooth formation navigation of multiple mobile robots for avoiding moving obstacles[END_REF][START_REF] Shames | Safe formation control with obstacle avoidance[END_REF]. This theory allows to prove the suitable performance and convergence of the formation under certain conditions [START_REF] Feddema | Decentralized control of cooperative robotic vehicles: theory and application[END_REF][START_REF] Tanner | Leader-to-formation stability[END_REF]. The formation shape is maintained according to the links between the robots which can vary dynamically according to robot interaction constraints, e.g., fault communication, no perception of the leader, etc. [START_REF] Fierro | Hybrid control of formations of robots[END_REF][START_REF] Vilca | Formation control with leadership alternation for obstacle avoidance[END_REF]. In [START_REF] Das | A vision-based formation control framework[END_REF], three non-holonomic robots with omnidirectional cameras for vision-based control were used for different formation applications such as scouting, object transportation and formation reconfiguration (from triangle to line shape when an obstacle is detected). Different formation cases (leader reassignment, robot adding and control saturation) were presented in [START_REF] Mesbahi | Formation flying control of multiple spacecraft via graphs, matrix inequalities, and switching[END_REF]. The authors proposed a formation control law based on the combination of Linear Matrix Inequalities (LMI) and hybrid system.

The Leader-follower approach in column formation (one UGV (Leader or follower) always visible for the followers) is described in [START_REF] Klan Čar | A control strategy for platoons of differential drive wheeled mobile robot[END_REF]. The followers track the leader's trajectory by using only local information (camera or laser). Another representation of the Leaderfollower approach is based on a spring-damper system (cf. Fig. 2.9) [START_REF] Macarthur | Compliant formation control of a multi-vehicle system[END_REF][START_REF] El-Zaher | Echelon platoon organisation: a distributed approach based on 2-spring virtual links[END_REF]. This method proposes using two springs interaction model which allows easy implementation and transition between formation shape according to the attachment points of both springs (e.g., echelon (formation in which the UGVs are positioned diagonally) to column).

The main drawbacks of the Leader-follower approach are the dependency on the Leader (such as in the centralized architecture), e.g., any problem in the leader can stop the navigation or can produce a collision among the robots. Sometimes, the leader can not supervise the state of all followers, therefore any fault in the followers are not managed by the leader in the navigation in formation [START_REF] Benzerrouk | Architecture de Contrôle Hybride pour les Systèmes Multi-robots Mobiles[END_REF].

Figure 2.9: Leader-follower based on spring-damper system.

2.3.2/ VIRTUAL STRUCTURE APPROACH

This approach defines a virtual structure or rigid body, in which the group of robots in formation is considered as a single rigid body with its dynamic properties [START_REF] Tan | Virtual structures for high-precision cooperative mobile robotic control[END_REF][START_REF] Antonelli | Experiments of formation control with collisions avoidance using the null-space-based behavioral control[END_REF][START_REF] Benzerrouk | Stable navigation in formation for a multi-robot system based on a constrained virtual structure[END_REF]. This approach may be derived in three steps [START_REF] Chen | Formation control: a review and a new consideration[END_REF]:

• The dimension, shape and dynamic of the virtual rigid body are defined according to the number and to the robots' characteristics.

• The motions of each virtual agent or node are computed according to the desired motion of the rigid body (commonly, a predefined trajectory is generated for the virtual structure).

• Each robot tracks its assigned agent/node.

The advantages of the virtual structure approach are relative simple formation definition (robots follows the center of the body instead of another robot), highly flexible for the different kinds of formation shapes and robust of the robots [START_REF] Chang | A formation control framework based on lyapunov approach[END_REF][START_REF] Benzerrouk | Architecture de Contrôle Hybride pour les Systèmes Multi-robots Mobiles[END_REF][START_REF] Chao | UAV formation flight based on nonlinear model predictive control[END_REF]. Different works use this approach, mainly applied to spacecraft or small satellite formation flying control [START_REF] Beard | A feedback architecture for formation control[END_REF][START_REF] Ren | Decentralized scheme for spacecraft formation flying via the virtual structure approach[END_REF]. The desired formation is achieved by having all members of the formation tracking assigned nodes of the virtual structure which moves into desired configuration through a pre-defined reference trajectory. In [START_REF] Beard | A coordination architecture for spacecraft formation control[END_REF], the virtual structure method is combined with a leader-following method and behavioral approach (cf. Subsection 2.3.3) to formation control of multiple spacecraft interferometer in the deep space. A similar idea was applied for spacecraft formation flying control in [START_REF] Ren | Decentralized scheme for spacecraft formation flying via the virtual structure approach[END_REF]. Each spacecraft uses a synchronized control law based on tracking of the desired state of the virtual structure to achieve the desired formation patterns. In [START_REF] Do | Nonlinear formation control of unicycle-type mobile robots[END_REF], the virtual structure is used to define the followed trajectory. The dynamic model of the robot is used to design the nonlinear formation control to the trajectory synchronized tracking.

The drawbacks of the virtual structure, as the centralized approaches, is the central control unit to define and supervise the formation keeping (high time consuming due to predictive computation of the system in a finite time) [START_REF] Belta | Optimal motion generation for groups of robots: a geometric approach[END_REF]. Moreover, the constraints of the rigid structure according to the dimension of the vehicle and dynamic structure have to be considered.

2.3.3/ BEHAVIOR-BASED APPROACH

This approach is based on elemental behaviors (controllers) in order to generate the desired emergent behavior [START_REF] Arkin | Behavior-Based Robotics[END_REF][START_REF] Balch | Behavior-based formation control for multirobot teams[END_REF][START_REF] Dorigo | Ant colony optimization theory: A survey[END_REF] (cf. Subsection 1.3.2). The global task is decomposed in different elemental behaviors which can be selected or processed to generate the desired action according to the following action selection methods:

• Competitive selection: One behavior is activated according to the perception system (cf. Fig. 2.11(a)). The design is based on hierarchy in which one behavior has the highest priority w.r.t. the others. One of the drawbacks is the stability analysis during the switching phase [START_REF] Schöner | Dynamics of behavior: Theory and applications for autonomous robot architectures[END_REF][START_REF] Arkin | Behavior-Based Robotics[END_REF].

• Cooperative selection: Two or more behaviors are activated and the action (commands) are fused (mean, artificial intelligent fuzzy logic, multi-agent system) to obtain a final command to the robot [START_REF] Antonelli | Experiments of formation control with collisions avoidance using the null-space-based behavioral control[END_REF] (cf. Fig. 2.11(b)). One drawback is the partial accomplishment on the assigned task due to command fusion. Some works deal with this problem [START_REF] Arkin | Behavior-Based Robotics[END_REF][START_REF] Antonelli | The nsb control: a behaviorbased approach for multi-robot systems[END_REF]. Moreover, as in the last case, the stability analysis (transition phase) is a very complex problem [START_REF] Schöner | Dynamics of behavior: Theory and applications for autonomous robot architectures[END_REF][START_REF] Arkin | Behavior-Based Robotics[END_REF]. The advantages of the behavior-based approach are the flexibility, extension to more complex tasks and easy implementation in real robots. Different works using behaviorbased approach in real robot are found in the literature. In [START_REF] Mataric | Designing emergent behaviors: From local interactions to collective intelligence[END_REF][START_REF] Balch | Behavior-based formation control for multirobot teams[END_REF], the authors use a cooperative selection in which the local control rules are defined by the designer. In [START_REF] Fredslund | A general algorithm for robot formations using local sensing and minimal communication[END_REF], the robots use only local information to keep a certain distance and orientation from another robot (friend) within its its field of view. The authors show several formation shapes such as diamond, triangle, arrowhead, wedge and hexagon. The problem of keeping four marching robots in a side-by-side formation is presented in [START_REF] Parker | Designing control laws for cooperative agent teams[END_REF]. The authors use a cooperative selection for merging global and local information which allows to perform obstacle avoidance or other maneuvers. In [START_REF] Antonelli | Experiments of formation control with collisions avoidance using the null-space-based behavioral control[END_REF], the collision avoidance is taken into account by the behavior-based approach. The authors propose a Null-Space-Based (NSB) behavioral control (defined as competitive-cooperative approach) to coordinate the formation while performing different tasks. The null space of an elementary behavior is the state space region where the whole system can operate without affecting the actions of this elementary behavior. This approach is based on task-priority kinematic control approach for ground fixed manipulators to manage the possible conflicts between tasks [START_REF] Nakamura | Task-priority based redundancy control of robot manipulators[END_REF].

2.3.4/ OTHER APPROACHES OF FORMATION CONTROL

2.3.4.1/ POTENTIAL FIELDS APPROACH

This approach uses virtual bodies to model the group of robots and formation dynamic. The potential fields of these bodies depends on robot role (leader or follower), robot dynamics (velocity), distance between the robots (repulsive force if vehicles are very close and attractive force if vehicles are far enough within certain distance) and distance to the obstacles (repulsive force) [START_REF] Olfati-Saber | Distributed cooperative control of multiple vehicle formations using structural potential functions[END_REF][START_REF] Ogren | Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment[END_REF]. The asymptotic stability of the formation is guaranteed [START_REF] Ogren | Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment[END_REF]. Commonly, a "virtual leader" is defined to guide the movements of the group of robots [START_REF] Leonard | Virtual leaders, artificial potentials and coordinated control of groups[END_REF][START_REF] Ogren | A control lyapunov function approach to multi-agent coordination[END_REF][START_REF] Ogren | Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment[END_REF]. In [START_REF] Olfati-Saber | Distributed cooperative control of multiple vehicle formations using structural potential functions[END_REF], the potential fields is defined according to the desired formation while considering its dimension constraints. The decentralized implementation allows each vehicle of the formation to perform a collision-free movements. In [START_REF] Ge | Queues and artificial potential trenches for multirobot formations[END_REF], the authors present artificial potential trenches which represent the formation structure by queues and formation vertices, instead of nodes. The artificial potential trench (associated with a queue) attracts the robot to the bottom of the valley created by the potential trench. This approach improves the scalability and flexibility of robot formations for formation reconfiguration and obstacles avoidance. One drawback of this Potential fields approach is the local minima problem and the modeling of the robot and the environment (obstacles) [START_REF] Koren | Potential field methods and their inherent limitations for mobile robot navigation[END_REF][START_REF] Chen | Formation control: a review and a new consideration[END_REF].

2.3.4.2/ OPTIMIZATION-BASED APPROACH

This approach is based on optimization method using a suitable cost function [START_REF] Cortes | Coverage control for mobile sensing networks[END_REF][START_REF] Spletzer | Optimal positioning strategies for shape changes in robot teams[END_REF]. This cost function takes into account the error between a vehicle and its neighbors, distance to the obstacle, desired velocity, and other constraints (penalty function) [START_REF] Dunbar | Receding horizon control of multi-vehicle formations: A distributed implementation[END_REF][START_REF] Chao | UAV formation flight based on nonlinear model predictive control[END_REF].

The optimization problem can be solved using either centralized [START_REF] Chao | UAV formation flight based on nonlinear model predictive control[END_REF] or a distributed methods [START_REF] Dunbar | Distributed receding horizon control for multivehicle formation stabilization[END_REF]. Commonly, these methods uses receding horizon Model Predictive Control (MPC) [START_REF] Mayne | Constrained model predictive control: Stability and optimality[END_REF]. The MPC method rely on an optimization of predicted model response with respect to plant input to determine the best input changes for a given state. The MPC method is suitable for nonlinear and time-variant models with hard constraints (that cannot be violated) or soft constraints (that can be violated but with some penalty) [START_REF] Ge | Autonomous mobile robots: sensing, control, decision making and applications[END_REF].

In [START_REF] Dunbar | Receding horizon control of multi-vehicle formations: A distributed implementation[END_REF][START_REF] Dunbar | Distributed receding horizon control for multivehicle formation stabilization[END_REF], the problem is distributed for each robot which obtains the previous optimal control trajectory at each horizon time. In [START_REF] Chao | UAV formation flight based on nonlinear model predictive control[END_REF], the authors use non-linear MPC as penalty function (distance to the virtual target, obstacle) and a priority strategy (according to the distance to the object) to obtain a safe navigation of formation of a group of UAV.

A simple dual-mode MPC algorithm using local leader-follower controller was presented in [START_REF] Michalska | Robust receding horizon control of constrained nonlinear systems[END_REF][START_REF] Scokaert | Suboptimal model predictive control (feasibility implies stability). Automatic Control[END_REF].

The drawbacks of the Optimization-based approach are the time consuming due to predictive computation w.r.t. the horizon time which can originate delay in the system output, the adequate selection of the horizon time to obtain fast and suitable solution, and the limited region of convergence of the system to obtain stable and feasible solutions.

In this thesis, the formation problem is addressed by the combination of the main advantages of different approaches such as Leader-follower, virtual structure and behaviorbased to obtain complex formation shapes which can be extended or reconfigured dynamically according to the environment context/configuration.

2.4/ CONCLUSION

This chapter provided an overall review in the navigation in formation field. As in the case of single robot navigation, only some works among the huge number of research are described. The common MRS architecture are presented to motivate the proposed MRS control architecture based on centralized/decentralized architecture. The proposed control architecture uses the centralized approach to allow the central entity (Leader) to manage the configuration of the desired formation even for formation reconfiguration. The decentralized approach allows to each UGV (follower) to locally generate its commands to track its assigned virtual target. Moreover, the different approaches of formation control have been detailed. The proposed formation control uses a combination of Leaderfollower and behavior-based to obtain complex navigation in formation.

SAFE HYBRID CONTROL ARCHITECTURE FOR REACTIVE AUTONOMOUS VEHICLE'S NAVIGATION IN CLUTTERED ENVIRONMENTS

Fully autonomous vehicle's navigation is a complex problem of major interest for the research community. Systems capable of performing efficient and robust autonomous navigation are unquestionably useful in many robotic applications such as manufacturing technologies [START_REF] Sezen | Modeling automated guided vehicle systems in material handling. Otomatiklestirilmi Rehberli Arac Sistemlerinin Transport Tekniginde Modellemesi[END_REF], urban transportation [START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and clothoid based methods and its simulation[END_REF], assistance to disabled or elderly people [START_REF] Martins | Assistive mobility devices focusing on smart walkers: Classification and review[END_REF] and surveillance [START_REF] Stoeter | A robot team for surveillance tasks: Design and architecture[END_REF]. Even if a lot of progress has been made, some specific technologies have to be improved for real applications. This chapter is particularly focused on the problem of the autonomous navigation for UGVs in a cluttered environment (cf. Fig.

3.1).

An important issue for successful UGV's navigation is the obstacle avoidance. This function permits to prevent UGV's collision and to ensure UGV's safety. One part of the literature considers that the robot's control is entirely based on path planning methods while involving the total knowledge of its environment. All obstacles configurations are taken thus into account in the planning step. Some examples are Voronoï diagrams and visibility graphs [START_REF] Latombe | Robot Motion Planning[END_REF] or artificial potential fields functions [START_REF] Rimon | Exact robot navigation using artificial potential fields[END_REF]. Moreover, the planning methods can also deal with dynamic environment while regularly replanning the robot's path [START_REF] Fraichard | Trajectory planning in a dynamic workspace: a "state time" approach[END_REF]200]. Nevertheless, planning and replanning require a significant computational time and complexity.

Another part of the community is concentrated on reactive methods to deal with the obstacle avoidance, where only local sensors information is used rather than a prior knowledge of the environment [START_REF] Egerstedt | A hybrid control approach to action coordination for mobile robots[END_REF][START_REF] Toibero | Switching control of mobile robots for autonomous navigation in unknown environments[END_REF][START_REF] Adouane | Hybrid and safe control architecture for mobile robot navigation[END_REF]. In [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF], the author proposes a real-time obstacle avoidance approach based on the principle of artificial potential fields. In this work, it is assumed that the robot actions are guided by the sum of attractive and repulsive fields.

In [START_REF] Arkin | Motor schema-based mobile robot navigation[END_REF], the author extends Khatib's approach while proposing specific schema motors for mobile robots navigation. Another interesting approach, based on a reflex behavior reaction, uses the Deformable Virtual Zone (DVZ) concept, in which a robot movement depends on risk zone surrounding the robot [START_REF] Zapata | Dvz-based collision avoidance control of non-holonomic mobile manipulators[END_REF]. If an obstacle is detected, it will deform the DVZ and the approach consists of minimizing this deformation by modifying the control vector. This method deals with any obstacle shape, however, it suffers as schema motors from local minima problem. In general, reactive methods do not require high com-putational complexities since robot's actions must be given in real-time according to the perception [START_REF] Arkin | Behavior-Based Robotics[END_REF].

Several works proposes reactive methods to deal with the obstacle avoidance problem such as obstacles avoidance using vortex fields [START_REF] De Luca | Local incremental planning for nonholonomic mobile robots[END_REF] and orbital trajectories [START_REF] Kim | A real-time limit-cycle navigation method for fast mobile robots and its application to robot soccer[END_REF]. The last approach is build on circular limit-cycle differential equations in [START_REF] Kim | A real-time limit-cycle navigation method for fast mobile robots and its application to robot soccer[END_REF][START_REF] Jie | Real time obstacle avoidance for mobile robot using limit-cycle and vector field method[END_REF][START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF]. The circular limit-cycles are more stable than the vortex fields and always converge to periodic orbit (cf. Appendix E.3). In [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF], the authors extend this limit-cycles method to elliptical trajectories. Therefore, more generic and efficient obstacle avoidance is performed for different obstacle shapes even for long walls. Indeed, an ellipse fits better this kind of obstacles than a circle (cf. Fig. 3.

2).

This thesis exploits the adaptive elliptical limit-cycle method (cf. Subsection 3.2) for reactive obstacle avoidance using only robot's pose information and uncertain range data. This method was selected because it is stable and robust while using only local information from range sensors. It considers that each obstacle is surrounded by an ellipse. The ellipse parameters are computed on-line using the sequence of uncertain range data (cf. Section 3.4).

The problem of obstacle avoidance and the considered scenario during the UGV's navigation is presented in the next section. The proposed control architecture to deal with this problem is described in Section 3.3. Sections 3.4 and 3.5 describe two important blocks of the control architecture. Several simulations are performed to demonstrate the stability, reliability, flexibility and advantages of our strategy w.r.t. the other methods given in Section 3.5.3. In Section 3.6, our navigation strategy is implemented and experimented in a real urban electric vehicle. Finally, Section 3.7 provides the conclusion of this chapter.

3.1/ PROBLEM STATEMENT

The objective of the navigation task is to lead a UGV towards a specific target in a cluttered environment. This task must be achieved while avoiding static and dynamic obstacles O which can have different shapes. The following scenario is considered (cf. Fig.

3.1):

• Each obstacle O i in the environment can be surrounded by an elliptical box (cf. Fig.

3.1)

. The elliptical box is represented by the conic form of an ellipse (cf. Appendix A):

Ā (x -h) 2 + B(x -h)(y -k) + C (y -k) 2 = 1 (3.1)
where (h, k) ∈ R 2 are the ellipse center coordinates. The Ā, B and C terms are given by:

⎧ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎩ Ā = b -2 sin 2 (Ω) + a -2 cos 2 (Ω) B = a -2 -b -2 sin(2Ω) C = b -2 cos 2 (Ω) + a -2 sin 2 (Ω) (3.2)
where a, b ∈ R + characterize respectively major and minor elliptical semi-axes and Ω gives the ellipse's orientation (cf. Fig. 3.1).

• The robot and target are represented by circles C R and C T with radius R R and R T respectively (cf. Fig. 3.1).

• D RO i is the minimal distance between the robot and the obstacle O i [START_REF] Eberly | Distance from a point to an ellipse in 2d[END_REF]. • Ellipse of influence (E f i ) is an ellipse that has the same center (h, k) and orientation Ω as the ellipse which surround the obstacle O i (cf. eq. (3.1)). Nevertheless, its major and minor semi-axes, a lc and b lc , are defined as follows:

a lc b lc = a b + R R + δ E R R + δ E (3.3)
where δ E represents safe tolerance margins which include perception uncertainties, feasible control commands and data sensor accuracy. This section focus on reactive navigation, i.e, the ellipse of influence enclosing the detected obstacle is obtained online by the robot.

• l f as the line passing through the center of C R and C T . The intersection points between l f and E f i allows to identify if O i is a dangerous obstacle (which can hinder the robot's future movements) (cf. Fig. 3.1).

The assumption of ellipse boxes instead of circle is to obtain a generic and flexible means to surround accurately different kind of obstacle shapes [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]. One example of shape, which can be properly fit by an ellipse instead of a circle, is a wall (or in general, longitudinal shapes). This example is shown in Fig. 3.2 using perception with uncertainty taken by range sensors on the left side of the wall. Indeed, if this wall is surrounded by a circle, it will have a large radius which can produce more robot's path distance to avoid safely the obstacle [START_REF] Kim | A real-time limit-cycle navigation method for fast mobile robots and its application to robot soccer[END_REF] (cf. Fig. 3.2(a)). Fig. 3.2(b) shows the ellipse which fits more accurately the dimension of the obstacle.

3.2/ OBSTACLE AVOIDANCE BASED ON LIMIT-CYCLE

In this approach, the robot has to follow accurately limit-cycle trajectories to not collide with obstacles, as detailed in [START_REF] Kim | A real-time limit-cycle navigation method for fast mobile robots and its application to robot soccer[END_REF][START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF]. These authors use a circular limit-cycle characterized by a circle of influence of R I radius. In [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF], it was proposed to extend this methodology for more flexible limit-cycle shape (an ellipse). In fact, the ellipse is the generalization of the circle which can be easily obtained by setting a = b in eq. (3.2). The main ideas of this approach are detailed below (cf. Appendix B.1, for more details)

The differential equations of the elliptical limit-cycles are given by:

ẋs = m( Clc y s + 0.5 Blc x s ) + μx s (1 -Ālc x 2 s -Blc x s y s -Clc y 2 s ) (3.4) ẏs = -m( Ālc x s + 0.5 Blc y s ) + μy s (1 -Ālc x 2 s -Blc x s y s -Clc y 2 s ) (3.5)
with m = ±1 according to the avoidance direction (clockwise or counter-clockwise) (cf. orientation θ d is given by the differential equations of the limit-cycle (cf. eq. (3.4) and (3.5)):

θ d = arctan ẏs ẋs (3.6)
The obstacle avoidance algorithm based on the limit-cycle approach is described in Appendix B.1 [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF][START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]. This algorithm guarantee the avoidance of the obstacle by the robot while performing smooth trajectories.

3.3/ PROPOSED CONTROL ARCHITECTURE FOR NAVIGATION IN CLUTTERED ENVIRONMENTS

This section presents the proposed control architecture for UGV's navigation in a cluttered environment (cf. Fig. 3.4). This control architecture is designed for a UGV modeled as a tricycle robot (cf. Subsection 3.3.1). This architecture aims to manage the interactions among elementary behaviors while guaranteeing the stability of the overall control [START_REF] Adouane | Hybrid and safe control architecture for mobile robot navigation[END_REF].

It allows to obtain safe and smooth UGV's navigation (cf. Section 3.6). This chapter focuses on the development of the Perception and Control law blocks which are extensively described and analyzed in Section 3.4 and 3.5.

Before describing each block of the control architecture (cf. Fig. 3.4), let us present the used models for the vehicle and target.

3.3.1/ MODELING OF VEHICLE AND TARGET: KINEMATIC MODEL

We assume that the UGV evolves in asphalt road and in cluttered urban environment with relatively low speed (less than 3 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 [START_REF] Luca | Feedback control of a nonholonomic carlike robot[END_REF]. The two front wheels are replaced by a single virtual wheel located at the center between the front wheels. The equations of UGV's model can be written as (cf. Fig. 3.5):

⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ ẋ = v cos(θ) ẏ = v sin(θ) θ = v tan(γ)/l b (3.7)
where (x, y, θ) is the UGV's pose in the global reference frame X G Y G . v and γ are respectively the linear velocity and the orientation of the vehicle front wheel. l b is the vehicle's wheelbase. I cc is the instantaneous center of curvature of the vehicle's trajectory,

r c = l b / tan(γ)
is the radius of curvature and c c = 1/r c is the curvature.

Let us consider a dynamic target modeled as a point (x T , y T , θ T ) with non-holonomic constraints (cf. Fig. 3.5). This model allows us to use the general model of robot motion and also to simplify the controller equation. Its kinematic characteristics are given by:

⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩ ẋT = v T cos(θ T ) ẏT = v T sin(θ T ) θT = ω T (3.8)
where v T and ω T are respectively the linear and angular velocities of the target. The radius of curvature is computed by r c T = v T /ω T . An important consideration for target reaching is that v T ≤ v max and r c T ≥ r c min , where v max and r c min are respectively the maximum linear velocity and the minimum radius of curvature of the vehicle, given by r c min = l b / tan(γ max ).

For static target reaching (point stabilization, i.e., to reach a specific point with a given orientation), ω T is considered equal to zero and v T is not necessarily equal to zero; v T is then considered as a desired velocity value for the vehicle when it reaches the desired target (x T , y T , θ T ).

3.3.2/ LOCALIZATION BLOCK

This block uses sensor information to estimate the current pose of the UGV. As mentioned in section 1.3, different methods according to the selected sensors can be used [START_REF] Boukerche | Vehicular ad hoc networks: A new challenge for localization-based systems[END_REF][START_REF] Inoue | Robust state-space estimation for mobile robot localization[END_REF][START_REF] González | Mobile robot localization based on ultra-wideband ranging: A particle filter approach[END_REF]. In this work, the localization is assumed accurate in simulation. For experiments, an RTK-GPS and IMU embedded in each vehicle are combined using EKF to allow accurate estimation of the current configuration (cf. Section 3.6).

3.3.3/ TARGET REACHING BLOCK

This block consists of generating the control variables to drive a UGV (cf. Fig. 3.5 and 3.6) towards a specific target configuration (static or dynamic). Before to introduce the control variables, let us describe the following notations (cf. Fig. 3.6):

• e x , e y , e θ are the errors w.r.t local frame X m Y m between the poses of the UGV and the target.

• d and θ RT are respectively the distance and the angle between the position of the target and the UGV.

• e RT is the error related to the UGV's position (x, y) w.r.t the target's orientation θ T .

The control variables are linked to the relative pose between the UGV and the target, represented by error state (e x , e y , e θ ) (cf.

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ e x e y e θ ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(θ) sin(θ) 0 -sin(θ) cos(θ) 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ x T -x y T -y θ T -θ ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (3.9)
The error function e RT is added to the canonical error state eq. (3.9) (cf.

d = (x T -x) 2 + (y T -y) 2 (3.10) ⎧ ⎪ ⎨ ⎪ ⎩ θ RT = arctan y T -y x T -x if d > ξ θ RT = θ T if d ≤ ξ (3.11)
where ξ is a small positive value (ξ ≈ 0).

The error e RT is defined as (cf. Fig. 3.6):

e RT = θ T -θ RT (3.12)
Furthermore, e RT can be written as a function of e x , e y and e θ as:

tan(e RT ) = tan (e θ -(θ RT -θ)) = tan(e θ ) -e y e -1
x 1 + tan(e θ )e y e -1

x = e x tan(e θ )e y e x + tan(e θ )e y (3.13) where tan(θ RT -θ) = e y e -1 x (cf. Fig. 3.6). Hence, e RT allows to consider an additional orientation error w.r.t. e x , e y and e θ , e.g., when e θ = 0 then e RT = -e y /e x . The stabilization of this error allows to decrease the lateral distance d l to zero eq. (3.32) (cf. Fig. 3.6), and to always have the UGV in the wake of the target.

Finally, the error state and velocity (e x , e y , e θ , v T ) are the input of the Control law block (cf. subsection 3.5).

3.3.4/ OBSTACLE AVOIDANCE BLOCK

This block generates the control variables to drive a UGV along limit-cycle trajectories to avoid an obstacle (cf. Subsection 3.2). The control variables are errors defined in the previous subsection (e x , e y , e θ ) (cf. Fig. 3.6). Their values are given by:

e x =0 (3.14) e y =0 (3.15) e θ =θ d -θ (3.16)
where θ d is the orientation of the limit-cycle trajectory (cf. eq. (3.6)). In this work, e x = e y = 0 means that UGV's position is at each sample time in the desired position w.r.t. limit-cycle trajectory.

The velocity set-point v T of limit-cycle trajectory for the control law can be computed as a polynomial function of distance to the obstacle D RO :

v T = f (D RO ) (3.17)
The coefficients of the polynomial function of degree 2 can be obtained using the UGV's constraints such as the maximum velocity v max , the minimum velocity of the UGV to start and to continue its movement v min and maximum acceleration a max .

3.3.5/ HIERARCHICAL ACTION SELECTION BLOCK

The control architecture uses a Hierarchical action selection block to manage the switches between two elementary behaviors, Target reaching and Obstacle avoidance blocks, according to the environmental perception (cf.

3.4/ PERCEPTION BLOCK FOR OBSTACLE DETECTION

The Perception block incorporates propriocetive and exteroceptive sensors such as range sensors, cameras, odometers and RTK-GPS. Its goal is to capture information related to the robot's environment, mainly potential obstacles [START_REF] Abuhadrous | Digitizing and 3d modeling of urban environments and roads using vehicle-borne laser scanner system[END_REF][START_REF] Clark | Mobile robotic sensors for perimeter detection and tracking[END_REF][START_REF] Rodríguez | Obstacle detection based on laser range data and segmentation for an autonomous vehicle[END_REF] (cf. Fig. 3.8). In [START_REF] Premebida | A lidar and vision-based approach for pedestrian and vehicle detection and tracking[END_REF], the authors propose a method for pedestrian detection using a LIDAR and a camera. The features from LIDAR and vision space are combined in a single vector for a posterior classification.

In this work, the UGV's perception uses data from a range sensor LIDAR (cf. Subsection 3.

3.4.1/ RANGE SENSOR MODEL

The sensor specifications and its real performance have significant differences [START_REF] Barshan | Active sonar for obstacle localization using envelope shape information[END_REF][START_REF] Burguera | Sonar sensor models and their application to mobile robot localization[END_REF].

Mainly, the accuracy of the range sensor, e.g., short range readings are more accurate than the long range ones. In [START_REF] Burguera | Sonar sensor models and their application to mobile robot localization[END_REF], the authors show how the mean and the standard deviation of the errors between the real and the measured range tend to increase with distance.

The model considers a set of n points in R 2 with coordinates r i = (x i , y i ) T (cf. Fig. 3.9). These points are computed using eq. (C.2) with range data of the UGV given in polar coordinates (D r i , β i ), where D r i is the distance between the center of the UGV's range sensor and its impact point and β i is the orientation with respect to the orientation of the center of UGV's range sensor (cf. Fig 3.9).

The reading data provided by the range sensor at each step-time is modeled by the Normal distribution D t r i = N( Dt r i , P t r i ), where Dt r i = Dr i , 0 T is the mean vector and

P t r i = diag σ 2 D r i , σ 2 β i
is the covariance defined by the model of the range and angular uncertainties. The range and angular uncertainties are respectively related to the accuracy of the sensor and its opening angle β i (cf. Fig. 3.9) [START_REF] Burguera | Sonar sensor models and their application to mobile robot localization[END_REF]. The representation of the range data in Cartesian frame is given by: where r i is the point with noise, ri is the point without noise, the noise v i is assumed Gaussian with mean

r i = ri + v i (3.18)
E[v i ] = 0 and mean E[v i v T i ] = R v i .
The covariance R v i is given by:

R v i = cos(β i ) -D r i sin(β i ) sin(β i ) D r i cos(β i ) ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ σ 2 D r i 0 0 σ 2 β i ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ cos(β i ) -D r i sin(β i ) sin(β i ) D r i cos(β i ) T (3.19)

3.4.2/ ENCLOSING DATA WITH AN ELLIPSE

The obtained data after processing the range data from LIDAR (elimination of noisy and outliers and segmentation using respectively Filtering and Segmentation blocks) is enclosed with an ellipse to execute the elliptical limit-cycle approach for reactive obstacle avoidance [START_REF] Vilca | Reactive navigation of a mobile robot using elliptic trajectories and effective online obstacle detection[END_REF].

Different techniques have been proposed in the literature to enclose data with an ellipse [START_REF] Welzl | Smallest enclosing disks (balls and ellipsoids)[END_REF][START_REF] Zhang | Parameter estimation techniques: A tutorial with application to conic fitting[END_REF][START_REF] De Maesschalck | The mahalanobis distance[END_REF]. In [START_REF] Welzl | Smallest enclosing disks (balls and ellipsoids)[END_REF], the author proposed a technique to obtain the smallest enclosing ellipse for a set of data using the Welzl's algorithm [START_REF] Welzl | Smallest enclosing disks (balls and ellipsoids)[END_REF] with linear increasing time with regards to data dimension. In [START_REF] Zhang | Parameter estimation techniques: A tutorial with application to conic fitting[END_REF], the author presents a summary of methods to fit a set of data with an ellipse. The presented methods are the least square fitting based on algebraic and Euclidean distance, Kalman filtering method and robust estimation. In [START_REF] De Maesschalck | The mahalanobis distance[END_REF], the authors constructed an ellipse using the mean and covariance of the data and the Mahalanobis distance to analyze the relationship among them. The maximum Mahalanobis distance is used for different purposes, namely for outliers detection.

This subsection proposes an online efficient heuristic method based on the Euclidean distance estimation [START_REF] Vilca | On-line obstacle detection using data range for reactive obstacle avoidance[END_REF][START_REF] Vilca | Robust on-line obstacle detection using range data for reactive navigation[END_REF][START_REF] Vilca | Reactive navigation of a mobile robot using elliptic trajectories and effective online obstacle detection[END_REF] to compute the ellipse parameters (cf. Subsection 3.4.2.1).

It is demonstrated that this process ensures that all range data are surrounded by the

3.4.2.1/ PROPOSED HEURISTIC APPROACH

The Heuristic approach is an efficient method to enclose data with an ellipse. This approach uses the distance between the points to obtain one of the axes and the obtained ellipse encloses all points regardless of the obstacle shape (cf. Fig. 3.10). An important condition of this approach is that it needs to start at least three different points.

Lemma 1 . Consider a set of n > 2 points in R 2 with coordinates r i (x i , y i ) with i = 1, . . . , n.
The parameters of the ellipse that enclose all points are computed as follow (cf. Fig.

3.10):

• Compute the distance between all points d i j = r ir j with i, j = 1, . . . , n; and select the maximum distance d max . This d max is not decreasing if more data points are added.

• The ellipse's center C E (h, k) is the middle point between the points with maximum distance and the orientation of the line that joins these two points define the ellipse's orientation Ω (cf. Fig 3.4.2.1). The first semi-axis is given by ā = d max /2.

• Transform n points to the new coordinates system X Y (cf. Fig. 3.4.2.1) using eq. (3.20) to obtain the second ellipse semi-axis b.

r i = cos(Ω) sin(Ω) -sin(Ω) cos(Ω) r i -C E (3.20)
Figure 3.11: Obtained ellipse using the proposed Heuristic approach.

r i (x i , y i ) are the coordinates in the new reference system.

• Compute the semi-axis b i from the standard form of the ellipse (cf. eq. (A.2)) using r i . The large value of b i is avoided using the points which y i is greater than a threshold E > 0. This threshold allows to eliminate points that are collinear with first axis ā and points in the perpendicular line to ā at their extremes. The second semi-axis is given by b = max {b i }.

• Finally, the semi-axes of the ellipse eq. (A.2), are obtained:

a = max ā, b b = min ā, b (3.21)
and, the orientation of the ellipse is Ω E = Ω+Π/2 when b is the major axis, otherwise

Ω E = Ω.
Proof. To demonstrate that the proposed Heuristic approach encloses all points, a data point r i which belongs to the set of n points is analyzed. This set of n points was used to compute parameters of the Ellipse. It is assumed that r i Ellipse then d i j = r ir j (r j belongs also to the set of n points) has two cases:

• d i j ≥ d max
, however, the first axis of ellipse is the maximum distance between all points. Therefore, d i j is the new first axis and r i ∈ Ellipse.

• d i j < d max , and the second axis satisfy b 2 i < b 2 . Using eq. (3.20) and (A.2), it is obtained:

b 2 i < b 2 ⇒ y 2 i 1 - x 2 i a 2 < b 2 x 2 i a 2 + y 2 i b 2 < 1 (3.22)
Therefore, r i satisfy the ellipse equation (r i is inside S E) for i = 1, . . . , n.

It is demonstrated that the point r i belongs to the Ellipse which was computed using the set of n points.

3.4.2.2/ COMPARATIVE STUDY

To demonstrate the efficiency of the proposed Heuristic approach to enclose an obstacle with an ellipse, a comparative survey w.r.t. other approaches based on Least Square and Covariance (cf. Appendix A.5) for identification of the closest surrounded ellipse is presented.

This experiment considers the data from a UGV with a frontal range sensor (LIDAR) which has maximum detected range equal to D L max = 10 m and scanning angle equal to 180 • (cf. Fig. 3.12). The maximum linear velocity of the UGV is 2.5 m/s and the sample time is 0.01 s.

In Fig. 3.12, Heuristic, Least square and covariance approaches for enclosing ellipse of all range data are shown. The UGV was drove around static obstacles. In Fig. 3.12(c), it is noted that the Least square approach has a large obtained ellipse and the proposed Heuristic approach encloses all points with a small area w.r.t. the others.

3.5/ CONTROL LAW BLOCK

Different control methods for trajectory tracking and path-following dedicated to wheeled mobile robots (unicycle, car-like robot, etc.) have been proposed in the literature [START_REF] Fang | Trajectory tracking control of farm vehicles in presence of sliding[END_REF][START_REF] Blazic | Four-state trajectory-tracking control law for wheeled mobile robots[END_REF][START_REF] Pesterev | Stabilizing control for a wheeled robot following a curvilinear path[END_REF][START_REF] Petrov | Modeling and nonlinear adaptive control for autonomous vehicle overtaking[END_REF]. In [START_REF] Kanayama | A stable tracking control method for an autonomous mobile robot[END_REF][START_REF] Blazic | Four-state trajectory-tracking control law for wheeled mobile robots[END_REF], [START_REF] Vilca | Reactive navigation of a mobile robot using elliptic trajectories and effective online obstacle detection[END_REF], nonlinear control laws for trajectory tracking are synthesized for a unicycle robot using Lyapunov stability analysis. The Lyapunov functions used in these studies are based only on distance and orientation errors. A trajectory tracking control for a farm vehicle, incorporating sliding in the kinematic model, is proposed in [START_REF] Fang | Trajectory tracking control of farm vehicles in presence of sliding[END_REF]. For the path-following problem, a control law for a tricycle robot is proposed in [START_REF] Samson | Control of chained systems. application to path following and timevarying point-stabilization of mobile robots[END_REF][START_REF] Pesterev | Stabilizing control for a wheeled robot following a curvilinear path[END_REF]. They are based on feedback linearization and chained form representation [START_REF] Laumond | La Robotique Mobile[END_REF]. The path-following controller allows thus to make the lateral and longitudinal control of the vehicle independent along the reference trajectory. Furthermore, the path-following controller allows smoother convergence to the desired path than the trajectory tracking controller (designed for a time-parametrized trajectory) [START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]. The trajectory tracking controller allows to track the trajectory with a desired velocity profile, while the path-following controller acts only on the orientation to drive it along the path. Both, path-following and trajectory tracking controllers require the pose of the closest point to the trajectory (w.r.t. robot configuration) and/or the value of curvature at this point (cf. Fig. 3.17) at each sample time [START_REF] Laumond | La Robotique Mobile[END_REF][START_REF] Blazic | Four-state trajectory-tracking control law for wheeled mobile robots[END_REF]. Although there exist a multitude of techniques to compute these parameters, they can add an error in certain situations thereby influencing negatively the mobile robot control [START_REF] Fang | Trajectory tracking control of farm vehicles in presence of sliding[END_REF][START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF].

In this section, the proposed control law based on a novel definition of control variables and Lyapunov function is presented. This Lyapunov function is based on distance and orientation errors and a new parameter related to the angle between the robot and the target position. The synthesized control law can perform either static or dynamic target reaching using only its current pose and velocity. Using dynamic target reaching, trajectory following can also be performed. The control law exhibits good flexibility properties and it could be adapted to diverse autonomous robotic applications such as multi-robot formation (cf. Chapter 5).

The control law generates the desired linear velocity v and front wheels' steering angle γ of the vehicle which lead the errors (e x , e y , e θ ) (cf. Fig. 3.6) to converge to zero. It is designed as follows:

v = v T cos(e θ ) + v b (3.23) γ = arctan(l b c c ) (3.24)
where v b and c c are given by:

v b =K x [K d e x + K l d sin(e RT ) sin(e θ ) + K o sin(e θ )c c ] (3.25) c c = 1 r c T cos(e θ ) + d 2 K l sin(e RT ) cos(e RT ) r c T K o sin(e θ ) cos(e θ ) + K θ tan(e θ ) + K d e y -K l d sin(e RT ) cos(e θ ) K o cos(e θ ) + K RT sin 2 (e RT ) sin(e θ ) cos(e θ ) (3.26) K = (K d , K l , K o , K x , K RT , K θ
) is a vector of positive constants which must be defined by the designer (cf. Subsection 3.5.2). K d , K l and K o are respectively related to the desired convergence of distance, lateral and angular errors w.r.t. the assigned target. K x , K RT and k θ are related to the maximum linear and angular velocities [4,[START_REF] Vilca | A novel safe and flexible control strategy based on target reaching for the navigation of urban vehicles[END_REF].

The stability of the proposed control law is proved in next subsection. An error boundary, enabling maximum error estimation according to the definition of the controller parameters, is computed in subsection 3.5.2. With this information, vehicle navigation is safe within a certain boundary. A brief description and comparison of some navigation methods from the literature are given in subsection 3.5.3.1.

3.5.1/ STABILITY OF THE CONTROL LAW

In this subsection, the stability of the derivatives of the errors (e x , e y , e θ ) ( 3.9 on page 54) and e RT ( 3.12 on page 54), which are computed using eq. (3.7), (3.8), (3.10) and (3.11), is analyzed. The differential error system is obtained by: These conditions guarantee that the target is ahead to the vehicle with respect to its orientation.

ėx = cos(θ)( ẋT -ẋ) + sin(θ)(ẏ T -ẏ) -sin(θ)(x T -x) θ + cos(θ)(y T -y) θ = -v + e y θ + v T [cos(θ T ) cos(θ) + sin(θ T ) sin(θ)] = -v + e y v tan(γ)l -1 b + v T cos(e θ ) (3.27) ėy = -sin(θ)( ẋT -ẋ) + cos(θ)(ẏ T -ẏ) -cos(θ)(x T -x) θ -sin(θ)(y T -y) θ = -e x θ -v T cos(θ T ) sin(θ) + v T sin(θ T ) cos(θ) = -e x v tan(γ)l -1 b + v T sin(e θ ) (3.28) ėθ = θT - θ = ω T -ω = v T r c T -v tan(γ)l -1 b (3.29) ėRT = θT -θRT = v T r c T - d dt arctan y T -y x T -x = v T r c T -v T sin(θ T )(x T -x) -cos(θ T )(y T -y) d 2 - -v sin(θ)(x T -x) + v cos(θ)(y T -y) d 2 = v T r c T - v T e
Theorem 1 . The control law given by eq. ( 3.23) and (3.24) ensures that the differential system (cf. eq. (3.27), (3.28), (3.29) and (3.30) is asymptotically stable according to Lyapunov-based analysis if the Assumption 1 is satisfied [START_REF] Khalil | Nonlinear Systems[END_REF].

Proof. The stability of the system is analyzed using Lyapunov method [START_REF] Khalil | Nonlinear Systems[END_REF]. The proposed Lyapunov function V, given by eq. (3.32), is a function of three parameters which depend on the distance d between the target and vehicle positions, the distance d l from the vehicle to the target line (line which passes through the target position with an orientation equal to the target orientation; this term is related to the Line of Flight and Sight of the target [START_REF] Siouris | Missile Guidance and Control Systems[END_REF]), and the orientation error e θ between the vehicle and the target (cf. Fig.

3.6).

The candidate Lyapunov function V is a positive-definite function [START_REF] Khalil | Nonlinear Systems[END_REF] when considering eq. (3.31) (Assumption 1). The Lyapunov function is given by:

V = 1 2 K d d 2 + 1 2 K l d 2 l + K o [1 -cos(e θ )] = 1 2 K d d 2 + 1 2 K l d 2 sin 2 (e RT ) + K o [1 -cos(e θ )] (3.32)
This Lyapunov function can be written with respect to e x , e y as follows:

V = 1 2 e 2 x + e 2 y [K d + K l sin 2 (e RT )] + K o [1 -cos(e θ )] (3.33)
To guarantee the stability of the system, V must be negative-definite [START_REF] Khalil | Nonlinear Systems[END_REF]. By taking the derivative of eq. ( 3.33) and using eq. ( 3.27), (3.28), (3.29), (3.30), (3.23) and (3.24), V can be written: Using eq. (3.13) in the first and last terms of eq. (3.35) and factorizing the common terms, it holds that:

V =(e x ėx + e y ėy )[K d + K l sin 2 (e RT )] + K l d 2 sin(e RT ) cos(e RT )ė RT + K o sin(e θ )ė θ =(e x e y vc c -v + v T cos(e θ ) + e y [v T sin(e θ ) -e x vc c ])[K d + K l sin 2 (e RT )] + K l d 2 sin(e RT ) cos(e RT ) v T r c T - v T e x sin(e θ ) d 2 + v T e y cos(e θ ) d 2 - e y v d 2 + K o sin(e θ ) v T r c T -vc c (3.
V =v T sin(e θ )[K d e y -K l d sin(e RT ) cos(e θ )] + v T r c T [d 2 K l sin(e RT ) cos(e RT ) + K o sin(e θ )] -v b [K d e x + K l d sin(e RT ) sin(e θ ) + K o sin(e θ )c c ] -v T K o sin(e θ ) cos(e θ )c c (3.36)
Finally, using eq. (3.25) and (3.26) in (3.36), it is obtained:

V = -K x [K d e x + K l d sin(e RT ) sin(e θ ) + K o sin(e θ )c c ] 2 -v T K o K θ sin 2 (e θ ) -v T K o K RT sin 2 (e RT ) ≤ 0 (3.37)
Equation (3.37) shows that the system is stable while the initial conditions eq. (3.31) are satisfied. To ensure the asymptotic stability of the proposed control law, V has to be a negative-definite function. Let us consider two cases, one where V = 0 with v T > ξ and another with v T = ξ, where ξ a constant value (ξ ≈ 0). Firstly, when v T > ξ and using the initial assumption K > 0, it is straightforward to show that e x , e θ , e RT are equal to zero to satisfy eq. (3.37); then according to eq. (3.11), (3.12) and (3.31), d is equal to zero (e x = e y = 0). Hence, V is equal to zero when v T > ξ, only if (e x , e y , e θ ) = (0, 0, 0).

Secondly, let us consider the case where v T = ξ. The initial assumption is identical. Therefore, the second and third terms of eq. (3.37) are equal to zero when v T = ξ. Additionally, when v T = ξ (quasi-static case) then it is considered r c T → ∞ (cf. Subsection 3.3.1); consequently the first term of V is equal to zero when:

K d e x + K l d sin(e RT ) sin(e θ ) + K o sin(e θ )c c = 0 (3.38)
Replacing eq. ( 3.26) with r c T → ∞ in (3.38), the following expression is obtained: In conclusion, if v T > ξ or v T = ξ, V is always strictly positive and V is always strictly negative while (e x , e y , e θ ) (0, 0, 0). Therefore, the system is asymptotically stable while the initial vehicle conditions eq. (3.31) are satisfied.

0 =K d e x + K l d

3.5.2/ SAFE TARGET REACHING

The synthesis of the proposed control law using a Lyapunov function enables to confirm its asymptotic stability (cf. Subsection 3.5.1). Nonetheless, it does not allow to obtain immediately the error values when the robot is in the immediate vicinity of the target to reach. These errors can be used to select an suitable next target to be accurately reached by the UGV and so on, i.e., a safe navigation through sequential targets placed in suitable position is performed (cf. Section 4.2).

The aim of this subsection is to determine a relation between the upper bound of the errors d and e θ , denoted respectively E dis and E ∠ (cf. Fig. 3.6 and 3.15) and the controller parameters K. These errors will be used as switch conditions between targets for the proposed navigation strategy in structured environments (cf. Subsection 4.2). The vehicle's dynamic and the localization of the vehicle and target are assumed to be always accurate (cf. Subsection 3.3.2). • The constant vector gain

K = (K d , K l , K o , K x , K θ , K RT )
which characterizes the control law parameters, known by advance. K is fixed while taking into account vehicle constraints such as maximum vehicle's velocity v max and minimum curvature radius r c min .

• The upper bound of the errors (E dis and E angle ) to satisfy at t = t f .

To simplify controller analysis, the orientation e θ and distance d errors are dealt separately. Firstly, the orientation error is computed considering enough initial distance vehicle-target d i (d i E dis ) to permit monotonous convergence of e θ towards zero. This consideration allows to estimate the minimum time t f to attain effectively e θ ≤ E angle (cf. Fig. 3.15). The subsequent analysis considers a static target ( ẋT = ẏT = 0 and r c T → ∞) and an extreme vehicle's configuration,

|e θ i | = π/2 -ζ (
where ζ is a positive value ≈ 0), hence the vehicle has initially the maximum admissible orientation error with respect to the target (cf. Fig. 3.15 and 3.21). The idea is to use the analysis of this initial limit vehicle's orientation error e θ i , which correspond to the slowest error convergence, to extrapolate thereafter the result for less critical vehicle's configuration |e θ 0 | π/2. Indeed, for less critical configuration, the convergence of e θ will be faster than the limit defined case (|e To solve the differential equation (3.41), let us introduce the following notations:

θ i | -→ π/2) (cf.
ėθ = v T r c T -v tan(γ) l b = - K x K d d cos(e θ ) + K x K o K θ sin 2 (e θ ) cos(e θ ) K d e y K o cos(e θ ) + K θ tan(e θ ) = - K x K o cos 2 (e θ ) K d d + K o K θ sin 2 (e θ ) [K d d sin(e θ ) + K o K θ sin(e θ )] = - K x (K d d + K o K θ ) K o cos 2 (e θ ) K d d + K o K θ sin 2 (e θ ) sin(e θ ) (3.41)
⎧ ⎪ ⎪ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ c 1 = K d d c 2 = K o K θ c 3 = (c 1 c -1 2 + 1) (3.42)
It is observed that the value of c 1 depends on the distance d. Since, this analysis focuses in the close proximity of the UGV to the target, the value of d is smaller w.r.t. the initial distance d i . Then, K d can be chosen as a function of the initial distance as K d = 1/d i , which allows to approximate the A 1 as a constant value. Hence, the analytic solution of eq. (3.41) (cf. Appendix D), while considering c 1 , c 2 and c 3 as constant values, has the following form:

ln ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ tan e θ 2 c 3 + cos(e θ ) c 3 -cos(e θ ) c 3 /2 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ e θ e θ i = - K x c 1 c 2 K o c 2 3 t t 0 (3.43)
From eq. (3.43), the upper bound of e θ can be written as:

e θ = f θ (t, K, e θ i ) (3.44)
where:

f θ = 2 tan e θ i 2 (c 3 + cos(e θ i ))(c 3 -1) (c 3 -cos(e θ i ))(c 3 + 1) c 3 /2 e -Kxc 1 c 2 Ko c 2 3 t (3.45)
This function approximates enough faithfully the evolution of the error e θ when the vehicle is close to the target (d di) at final time t f . From eq. (3.44), t f is obtained: Hence, taking e RT = 0 and e θ = 0, eq. (3.25) can be written as

t f = f -1 θ (E ∠ , K, e θ i ) (3
v b = K x K d d (3.47)
Introducing eq. (3.27), (3.28), (3.23) and (3.47) in the derivative of the distance, we obtain:

ḋ = e x ėx + e y ėy d = - e x v b d = -K x K d d (3.48)
Integrating eq. 3.48, we obtain:

d d i 1/d ∂d = t 0 -K x K d ∂t d = f d (t, K, d i ) (3.49)
where:

f d (t, K, d i ) = d i e -K x K d t (3.50)
From eq. (3.49) and (3.50), the convergence of the distance depends on K x , K d and the obtained time t f . Therefore, using equation 3.49 and while knowing that the objective here is to obtain d i (minimal distance covered by the UGV) which satisfy d = E dis at t = t f , the d i could be easily computed by:

d i = f -1 d (t f , K, E dis ) = E dis e K x K d t f (3.51)
Moreover, (3.44) and (3.51) show the relations between initial configuration, controller parameters and error of reaching the target. Therefore, for certain initial configuration and controllers parameters, the defined bounded of the errors can be obtained. This analysis is also conclusive for the dynamic target case, where the variation of the distance d between the UGV and target is slower than in the static target case. In the dynamic target case, the control law with the designed parameters K will have thus more time to converge the errors eq. (3.9) to zero.

3.5.3/ SIMULATION ANALYSIS

The following simulations show the advantages, stability and reliability of the proposed control law to drive a UGV towards a specific target.

3.5.3.1/ COMPARATIVE STUDY OF THE CONTROL LAW

In this subsection, two common approaches to follow a dynamic target by a tricycle are presented and compared with the proposed control law. The comparison is focused on the position and orientation errors and the convergence time. In the sequel, the dynamic target is assumed to be another identical UGV. These two strategies are briefly described below:

1. Approach based on a target model (Chained system): In [START_REF] Laumond | La Robotique Mobile[END_REF] and [START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF], a control law to track a reference vehicle (target) is proposed. A variable transformation to obtain the control is applied to the error system and commands. The control law is synthesized using a suitable Lyapunov function (more details can be found in [START_REF] Laumond | La Robotique Mobile[END_REF]). The desired steering angle is computed by integration. Nevertheless, the control law considers a non-zero linear target velocity, i.e, if the target is static then the commands sent to the vehicle are zero.

Approach based on a reference path (Frenet control):

In [START_REF] Daviet | Platooning for small public urban vehicles[END_REF], a method for following a vehicle based on a Frenet frame was developed (cf. Fig. 3.17). It exploits the use of chained systems to separate the lateral and longitudinal control. Therefore, each controller can be designed independently. The lateral control is obtained using chained transformation (more details are given in [START_REF] Samson | Control of chained systems. application to path following and timevarying point-stabilization of mobile robots[END_REF] and [START_REF] Laumond | La Robotique Mobile[END_REF]). The longitudinal control consists in keeping a specific curvilinear distance d s between the target and the UGV. One drawback of this approach is the dependency on a known reference path for the UGV, i.e., if the vehicle follows a dynamic target then the target trajectory must be accurately known by the UGV.

The approaches presented above are implemented in simulation and compared to our proposal. In order to do this, the main target (equivalent to a Leader robot in formation control [4,5]) tracks a sinusoidal trajectory and the followers must maintain a distance of 5 m w.r.t. this first robot, i.e., the secondary target to be reached is located at 5 m (curvilinear distance) from the main target. Moreover, the different controllers were tuned using the best performance for the task to achieve. The proposed control law was not designed to take into account the reference trajectory, however the obtained results are very close to those designed for trajectory tracking. In addition to its accuracy, the proposed control law has more flexibility (cf. Section 3.5) to perform the autonomous navigation of the UGVs. Indeed, it is only necessary to know the current pose and dynamics of the target instead of all recorded trajectory.

3.5.3.2/ VALIDATION OF SAFE TARGET REACHING

The following simulations verify the presented analysis in Subsection 3.5.2 and show the performance (safety, smoothness and convergence) of the control law to reach a desired final configuration (pose and velocity). The UGV starts at the same position with different initial distance and orientations.

The first simulation validates the analysis presented in subsection 3.5.2, where the minimum value of d i , obtained with a limit vehicle configuration e θ ≈ π/2, allows to satisfy the bound of the errors for other initial configurations. The controller parameters are K ≡ (d -1 i , 0.6, 10, 0.1, 0.3, 0.01) and the upper bound of the errors are E dist = 0.1 m and E ∠ = 5 • . These parameters were chosen to obtain a smooth trajectory, fast response and velocity within the UGV's velocity limits (cf. Subsection 3.5.2). The kinematic constraints of the UGV are maximum velocity equal to v max = 2.5 m/s and the minimum radius of curvature equal to r c min = 2.83 m. Hence, using eq.

3.6/ EXPERIMENTAL VALIDATION OF THE OVERALL CONTROL AR-CHITECTURE

This experiment focuses on the online UGV's navigation in cluttered environment using a LIDAR for online obstacle detection (cf. Fig. 3.24). The elliptical limit-cycle trajectories are applied for reactive obstacle avoidance using only position information and uncertain range data (cf. Appendix B.1). Each obstacle is surrounded by an appropriate ellipse (cf. Section 3.4.2) computed online using range data (cf. Fig. 3.25(a)). This experiment demonstrates the efficiency, feasibility, smoothness and safety of the proposed architecture and its elementary blocks (Perception, Target reaching, Obstacle avoidance and Control law) (cf. Section 3.3). This experiment can be found online 1 .

In this experiment, the used UGV is an urban electric car named VIPALAB (cf. Appendix G.1). The VIPALAB has a range sensor (LIDAR) with the maximum detected range equal to D max = 10 m (cf. Fig. 3.9). Moreover, the vehicle uses a combination of RTK-GPS and IMU using EKF to estimate its localization (current position and orientation) at a sample time of T s = 0.1 s (cf. Appendix G.3). The controller parameters K are K ≡ (1, 2.2, 8, 0.1, 0, 01, 0.6). These parameters were chosen to obtain a safe and smooth trajectory, fast response and velocity value within the limits of the vehicle, which are v max = 2.5 m/s and the minimum radius of curvature r c min = 2.83 m.

VALIDATION OF THE OVERALL CONTROL ARCHITECTURE

vehicle avoids the obstacles with a smooth trajectory and converges to its target after avoiding three obstacles (two boxes and one static vehicle). This trajectory was obtained while using the reactive obstacle avoidance algorithm [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF] (cf. Subsection 3.2) with the proposed control law (cf. 3.5) which takes its parameters from the online obstacle detection method (cf. Section 3.4).

(a) GDI-VIPA during the safe navigation ("v UGV" and "v Target" are respectively the current velocities of the UGV and the target).

(b) Vehicle's trajectory using the proposed control architecture (AT: Attraction to the Target and OA: Obstacle Avoidance). Fig. 3. 26(d) shows the minimum distance between the ellipse of influence (cf. Section 3.1) of each obstacle (obtained while knowing all the range data of the obstacles) and the vehicle's position along the limit-cycle trajectory (green line in Fig. 3.25(b)). The switching times between the obstacles in this figure were selected to show how the vehicle evolves along the elliptical limit-cycle. It can be noted that the UGV does never collide with any obstacle since there is not intersection with the ellipses of influence.

Therefore, the proposed control architecture is validated for real implementation to deal with cluttered environments.

3.7/ CONCLUSION

This chapter presented an overall control architecture to cope with the problem of navigation in a cluttered environment. The problem of obstacle avoidance is dealt with the use of online adaptive elliptic trajectory to perform smooth and safe UGV's navigation in a reactive way. These trajectories use the limit-cycle principle to obtain generic and flexible navigation in very encumbered environments. This elliptic limit-cycle trajectory is obtained while using the proposed Perception block which deals with the uncertain range data to obtain the ellipse parameters of each obstacle. This method was demonstrated and implemented online to enclose all range data. Furthermore, a single control law suitable for the proposed control architecture is described. It takes into account the position and orientation set-points to improve the performance and the safety of UGV's navigation. This proposed control law was synthesized using a suitable Lyapunov function, which takes into account the position, the angle between the robot and the target, and its orientation with respect to set-points. Moreover, it enables static and dynamic target reaching. The stability of the overall control architecture was proved using a suitable Lyapunov function based on a new set of variables. The relation between controller parameters and the upper bound on distance and orientation errors at final time t f allows to guarantee the safety of the UGV's trajectory towards its assigned target. Furthermore, different target-reaching methods from the literature were presented and their performances were compared with the proposed control law by simulations. These comparisons showed the interesting features of the proposed control law in term of stability and flexibility for different tasks. Experiments using a urban electric car validate the efficiency and the reliability of the proposed control architecture in cluttered environments.

SAFE AND FLEXIBLE AUTONOMOUS VEHICLE'S NAVIGATION USING SEQUENTIAL WAYPOINTS IN STRUCTURED ENVIRONMENTS

Different strategies for autonomous navigation in structured environments have been proposed in the literature [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF][START_REF] Gu | On-road motion planning for autonomous vehicles[END_REF][START_REF] Lee | A unified framework of the automated lane centering/changing control for motion smoothness adaptation[END_REF] (cf. Fig. 4.1). The most popular approaches are based on the following of a pre-defined reference trajectory [START_REF] Kanayama | A stable tracking control method for an autonomous mobile robot[END_REF][START_REF] Maalouf | A higher level path tracking controller for a four-wheel differentially steered mobile robot[END_REF][START_REF] Bonfè | Online trajectory generation for mobile robots with kinodynamic constraints and embedded control systems[END_REF]. These approaches link the control to a reference trajectory which can be obtained with a combination of path planning and trajectory generation techniques [START_REF] Lavalle | Planning Algorithms[END_REF] Typically, to obtain the reference path to be followed by the vehicle, arc-lines, B-splines or polynomial equations are used to interpolate points/waypoints [START_REF] Horst | Trajectory generation for an on-road autonomous vehicle[END_REF][START_REF] Connors | Manipulating b-spline based paths for obstacle avoidance in autonomous ground vehicles[END_REF][START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and clothoid based methods and its simulation[END_REF][START_REF] Lee | A unified framework of the automated lane centering/changing control for motion smoothness adaptation[END_REF]. This safe path can be a time-parametrized path or a path without temporal reference [START_REF] Lavalle | Planning Algorithms[END_REF]. The time-parametrized path can take into account different UGV's constraints and environmental characteristics [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF]. Nevertheless, obtaining this path requires more computational time than obtaining path without temporal reference [START_REF] Kuwata | Motion planning for urban driving using rrt[END_REF]. Most of the control laws linked to a reference path are dedicated to either trajectory tracking (to track a timeparametrized reference [START_REF] Blazic | Four-state trajectory-tracking control law for wheeled mobile robots[END_REF]) or path following (to follow a path without explicit temporal references [START_REF] Samson | Control of chained systems. application to path following and timevarying point-stabilization of mobile robots[END_REF]).

In [START_REF] Gu | On-road motion planning for autonomous vehicles[END_REF] a feasible path is obtained using a polynomial curvature spiral. In [START_REF] Bonfè | Online trajectory generation for mobile robots with kinodynamic constraints and embedded control systems[END_REF], the trajectory generation method generates a smooth path considering the kinodynamic constraints of the vehicle. In [START_REF] Courbon | Autonomous navigation of vehicles from a visual memory using a generic camera model[END_REF] straights line paths, defined by the position and orientation of a single waypoint, are considered. In this case, the orientation of the previous waypoint is not taken into account to simplify the implementation of the control law. In [START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and clothoid based methods and its simulation[END_REF], trajectories are built using user assigned points and interpolation functions such as cubic splines, trigonometric splines and clothoids. Moreover, velocity profiles along the trajectory are specified to improve the passengers comfort which is related to the acceleration. Nevertheless, trajectory generation presents some drawbacks, such as the necessity of a specific planning method, the proof of guarantee of continuity between different segments of the trajectory and the complexity for replanning.

A few works in the literature propose to use only specific set of waypoints in the environment to lead the robot toward its final objective. In [START_REF] Aicardi | Closed loop steering of unicycle like vehicles via lyapunov techniques[END_REF], the authors propose a navigation strategy via assigned static points for a unicycle robot. This strategy does not allow accurate navigation since the kinematic constraints of the robot (maximum velocity and steering), the orientation error and the velocity profile of the robot when it reaches the assigned point are not considered. Harmonic Potential Field (HPF) is used to guide an unmanned aerial vehicle (UAV) to a global waypoint with a position and a direction of arrival in [START_REF] Masoud | A harmonic potential approach for simultaneous planning and control of a generic uav platform[END_REF]. The author proposes a virtual velocity field which allows to consider the UAV model. Each vector component of the field is treated as an intermediate waypoint and the robot must comply with it in order to reach the global waypoint. Nonetheless, HPF requires a complex mathematical modeling for different shapes or dimension of the obstacles in the environment.

In this chapter, the proposed navigation strategy which avoids the pre-generation of any specific reference trajectory is presented. Vehicle movements are obtained according to the proposed control law while considering vehicle kinematic constraints and sequential waypoints to reach (defined by its position, orientation and velocity) (cf. Section 3.3.3).

The vehicle can thus perform different movements between waypoints without the necessity of replanning any reference trajectory. Moreover, it can also add or change the location of the successive waypoints according to the environment configuration or to the task to achieve. Thus, this strategy allows flexible navigation while taking into account appropriate waypoints suitably placed in the environment (cf. Section 4.3).

Next section describes the motivation and the considered structured environment for UGV's navigation. The proposed complete framework for autonomous vehicle navigation through successive waypoints is described and validated by simulation in Section 4.2. Furthermore, different algorithms from the literature to obtain the set of waypoints in the environment are detailed in Section 4.3. This sections also proposes an elementary waypoint selection method from a reference trajectory to perform safe and smooth trajectories and two methods for the selection of waypoint configurations in a well-known environment using a multi-criteria optimization techniques. The proposed algorithms are validated through several simulations to demonstrate their advantages, safety, reliability and flexibility. An extension to multi-robot system is also proposed in Subsection 4.3.4.5.

In Section 4.4, our navigation strategy is implemented and experimented in a real urban electric vehicle. Finally, Section 4.5 provides a conclusion of the chapter.

4.1/ PROBLEM STATEMENT

Typically, a human driver reactively guides his vehicle while performing a smooth trajectory within the roads limits until reaching the defined goal. To achieve similar behavior with a UGV, a target assignment strategy to enable vehicle navigation through successive waypoints in the environment is presented.

An important condition in the field of autonomous vehicles is to ensure safe and flexible vehicle navigation in a structured environment (cf. Fig. 4.1 and 4.2). In this work, safe navigation consists in not crossing over the road limits and bumping into obstacles while respecting the physical constraints of the vehicle. Flexible navigation consists in allowing different possible movements to achieve the task, while guaranteeing a smooth trajectory of the vehicle. The main idea of the proposed work is to guarantee both criteria simultaneously. The analysis of the control law presented in Subsection 3.5.2 allows the UGV to perform safe navigation within a certain boundary.

In this section, it is shown that it is not always important to follow with a high fidelity an imposed path for a vehicle, specifically in open or low-constrained environments. It will be demonstrated that only few waypoints will be sufficient to guarantee safe and flexible UGV navigation. A target assignment strategy is proposed to perform autonomous navigation through pre-defined waypoints (cf. Section 4.2). Moreover, it is also demonstrated that if we increase the number of these waypoints, the robot control performs as if we had applied common trajectory tracking control.

In what follows, the considered scenario for the UGV's navigation through sequential waypoints is presented (cf. Fig. 4.2):

• The environment is known throughout a map, containing the position of all obstacles and road dimensions. • The kinematic/dynamic model of the vehicle is known (with potential uncertainties).

• The vehicle starts at the initial pose P i and it has to reach the final pose P f (in certain cases, P i = P f ).

As presented in the beginning of Chapter 4 and according to the described scenario, a safe reference path in static environment can be obtained by different algorithms such as Voronoï diagram [START_REF] Latombe | Robot Motion Planning[END_REF], potential fields [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF] or others [START_REF] Lavalle | Planning Algorithms[END_REF]. In this work, specific key positions should be defined in the static environment, which are named waypoints (cf. Fig. 4.2). A waypoint corresponds to a specific key configurations (x p i , y p i , θ p i , v p i ) (where (x p i , y p i ) T , θ p i and v p i denote respectively the position, the orientation and the velocity of the waypoint p i ) in the environment as given in Fig. 4.3 (cf. Section 4.2).

UGV's navigation using only waypoints allows to avoid any path/trajectory planning which could be time-consuming and complex, mainly in cluttered and dynamic environment. Moreover, this kind of navigation does not require the pose of the closest point to any trajectory (w.r.t. the robot configuration) and/or the value of the curvature at this point [START_REF] Gu | On-road motion planning for autonomous vehicles[END_REF]. Consequently, the navigation problem is simplified to a target (waypoint) reaching problem, i.e, the UGV is guided by waypoints (cf. Fig. 4.3) instead of following a specific fixed path.

4.2/ NAVIGATION STRATEGY BASED ON SEQUENTIAL TARGET REACHING

The proposed strategy uses a sequence of sorted waypoints suitably disposed in the environment [4,[START_REF] Vilca | A novel safe and flexible control strategy based on target reaching for the navigation of urban vehicles[END_REF]. This sequence is obtained by a method which selects the optimal set of waypoints to perform a safe vehicle navigation in structured environment (cf. Section 4.3). In this section, the set of waypoints is assumed known to focus on the navigation strategy (cf. Fig. 4.4). Fig. 4.3 shows a set of successive waypoints. D j is the Euclidean distance between the waypoints p i-1 and p i . For simplicity, the orientation of the waypoint θ p i is represented as the orientation of the line that joins p i and p i+1 :

θ p i = arctan y p i+1 -y p j x p i+1 -x p i (4.1)
To navigate between successive waypoints (e.g. p i-1 and p i ), the distance d of the vehicle to the target (current waypoints to be reached) and the error e θ between the orientation of the vehicle and the target are used. Their maximum values (E dis and E ∠ respectively (cf. Fig. 4.3)) are imposed for the current waypoint p i to be reached (cf. Subsection 3.5.2). These values are related to the inaccuracies of the vehicle localization and/or the performance of the used control law. The maximum authorized values allow to keep a reliable vehicle control towards the target p i (cf. Fig. 4.3) while guaranteeing the appropriate vehicle configuration to reach the next target p i+1 .

4.2.1/ SEQUENTIAL TARGET ASSIGMENT Fig. 4.4 shows the extension of control architecture, proposed in section 3.3, for the navigation through successive waypoints. At this aim, a new block, Sequential target assignment is added. The proposed navigation strategy can be extended easily in order to deal with dynamic environments, notably using the limit-cycle approach [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF], [START_REF] Vilca | Reactive navigation of a mobile robot using elliptic trajectories and effective online obstacle detection[END_REF] (cf. section 3.1). This obstacle avoidance controller allows to modify locally the movement of the robot to avoid dangerous static or dynamic obstacles and to come back to its initial plan.

The flowchart of the proposed strategy to assign at each sample time the appropriate waypoint is shown in Fig. 4.5. The parameters of the control law (cf. Subsection 3.5) enable the vehicle to reach the next waypoint with an assigned velocity (3.23) (this velocity can be different from zero) while ensuring that the vehicle trajectory is smooth and always within the road limits (cf. Section 4.2.3).

The current waypoint is named the Target. The initial target is the first waypoint from the set of sorted waypoints. The error conditions (E dis and E ∠ ) are used to switch to the next waypoint in the list, when the vehicle position enters in the circle with a radius equal to E dis and center (x p i , y p i ). If the vehicle does not satisfy the distance and orientation error conditions w.r.t. the current waypoint p i then the perpendicular line L i (Y T axis) to the p i p i+1 line at the current waypoint is used to switch to the next waypoint when the vehicle crosses the line L i , i.e., x T ≥ 0 where x T is the coordinate of the vehicle in the local Target frame X T Y T ( cf. Fig. 4.3). The current target is updated with this next waypoint which should not intersect any obstacle (otherwise, a new next waypoint is selected until this condition is satisfied) (cf. Fig. 4.5). Finally, the vehicle starts the movement to reach this new current target.

4.2.2/ SMOOTH SWITCHING BETWEEN WAYPOINTS

When the vehicle switches from one waypoint to another (e.g., from p j to p j+1 as depicted in Fig. 4.3), the value of controller variables C v = (e x , e y , e θ , e RT , v T , r c T ) (cf. Section 3.5) can change abruptly. These hard switches could induce, in certain situations, the actuators jerk (v and γ from eq. 3.23 on page 62 and 3.24). This aspect could generate in certain applications, such transportation task, the discomfort of passengers.

SVC v (d c ) = C vi + (C v -C vi ) 1 + e -a s (d c -d 0 ) (4.2)
where C vi and C v are respectively the initial and current values of the controller variables. For example, for e x , an element of C v , when the target switches from p j to p j+1 , e xi is the value before to switch to p j+1 and e x is the current error w.r.t. the waypoint p j+1 ; d 0 is the value where the function has a half of its current value and a s is a constant value related to the slope of the sigmoid function. It is designed to attain the effective value (SVC v ≈ C v ) when d c = d s (cf. Fig. 4.6).

Before to detail the waypoints selection strategies, let us first evaluate the performance of the navigation through successive waypoints.

4.2.3/ SIMULATION EVALUATION OF THE NAVIGATION STRATEGY

This section presents simulations to demonstrate the efficiency of navigation through successive waypoints. As mentioned in Section 3.6, the physical parameters of the urban vehicle VIPALAB (cf. Section G.1), modeled using the tricycle kinematics (cf. Section 3. Secondly, the performance (safety, smoothness and convergence) of the control law to reach a desired target (static or dynamic) located at different distances. For each simulation the UGV starts at the same configuration but with different values of waypoints' velocities. 3), let us address , in this section, the methods to obtain the most appropriate configuration (number, poses, etc.) of these waypoints in the environment. The aim is to ensure in all cases the navigational safety, but in addition, the smoothness and rapidity of the navigation can be taken into account.

Waypoint selection consists of obtaining minimum number of points (waypoints) on the road to be successively reached by the vehicle to perform a safe navigation. These waypoints are selected considering a safe position on the road (as far as possible from the road limits) and the reliability of the obtained vehicle trajectory (smooth changes between the successive points). Before to present the proposed methods for the selection of waypoint configuration, in what follows, the state of the art related to waypoint planning is given.

4.3.1/ STATE OF THE ART

Diverse algorithms can be used to obtain these waypoints such as A * , D * [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF], Rapidly Random Tree (RRT) [START_REF] Kuwata | Motion planning for urban driving using rrt[END_REF], Sparse A * Search (SAS) [START_REF] Szczerba | Robust algorithm for real-time route planning[END_REF] and others [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF][START_REF] Lavalle | Planning Algorithms[END_REF]. A useful tools for these algorithm is the Configuration space (C-space) which is the space of all possible configuration of the robot [START_REF] Siciliano | Springer Handbook of Robotics, Part E-34[END_REF]. The C-space enables the identification of the safe area where the vehicle can navigate without a collision risk (free space C-space f ree ).

The C-space is also used to compute the minimum distance to C-space obst (obstacle) and/or C-space ext (road boundaries). Fig. 4.10 shows the C-space and its Voronoï diagram [START_REF] Latombe | Robot Motion Planning[END_REF] in gray scale w.r.t. the distance to the closest C-space obst and C-space ext (the whitest area represents the safest area).

Typically, algorithms based on grid map (e.g., A * or D * ) produce the shortest path by optimization of a criterion such as the distance to the goal, distance to the risk area, etc. [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF]. The algorithm begins generally at the final cell (final position) and traverses the cell's neighbors until to reach the initial position. The cost of traveling through the neighbor is added to the total cost, the neighbor with the lowest total cost is selected, and so on. The process terminates once the initial position is reached. The path is given through the cell positions of the grid map while backtracking the cells which have the lower path cost. Sometimes, a polynomial interpolation is used to obtain a smooth path [START_REF] Connors | Manipulating b-spline based paths for obstacle avoidance in autonomous ground vehicles[END_REF]. In [START_REF] Ziegler | Navigating car-like robots in unstructured environment using an obstacle sensitive cost function[END_REF], the authors present an A * algorithm using clothoid trajectories assuming constant velocity along them. Therefore, appropriate waypoints can be selected from this shortest path while only considering the cells where an orientation change occurs (w.r.t. its predecessor). Nonetheless, this algorithm does not consider neither former/initial vehicle orientation nor its kinematic constraints.

Instead of using grid map, it is possible also to obtain safe, feasible and smooth path using expanding tree algorithms (e.g., RRT, RRT* or SAS [START_REF] Szczerba | Robust algorithm for real-time route planning[END_REF][START_REF] Lavalle | Planning Algorithms[END_REF][START_REF] Kuwata | Motion planning for urban driving using rrt[END_REF][START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF]). This could be done by providing to the vehicle's model the commands to reach the successive selected nodes until its goal [START_REF] Szczerba | Robust algorithm for real-time route planning[END_REF][START_REF] Lavalle | Planning Algorithms[END_REF][START_REF] Kuwata | Motion planning for urban driving using rrt[END_REF]. The basic process of RRT consists in selecting, at each sample time, a random node q random in the C-space f ree . This selection considers generally only position q random = (x random , y random ) without any a priori UGV's orientation [START_REF] Lavalle | Planning Algorithms[END_REF]. Then, the commands (discrete values) are applied to vehicle (from its current position and orientation) during a constant time t exp . The vehicle's model and constant commands allow to predict the final vehicle position at the end of t exp . The commands that produce the closest position q chosen (a node which optimizes a dedicated task criterion [START_REF] Vaz | Kinodynamic motion planning of a skidsteering mobile robot using rrts[END_REF]) to current random node q random are selected and stored with q choose . A new expansion starts until to reach q random or to select a new random node q new random . Therefore, the waypoints can be selected, as in the case of grid map, while only considering the nodes where an orientation change occurs (w.r.t. its predecessor node). Algorithms based on RRT are very useful for motion planning because they can provide the commands (based on the kinematic/dynamic model of the vehicle) to reach the desired final configuration [START_REF] Kuwata | Motion planning for urban driving using rrt[END_REF][START_REF] Vaz | Kinodynamic motion planning of a skidsteering mobile robot using rrts[END_REF]. Moreover, RRT avoids the use of grid maps that can increase the computation time for large environment. In [START_REF] Szczerba | Robust algorithm for real-time route planning[END_REF], the authors use the expanding tree for trajectory planning introducing different constraints such as maximum turning angle and route distance. Nevertheless, this method does not consider neither the vehicle movements along the trajectory nor localization uncertainties. In [START_REF] Kallem | Sequential composition for navigating a nonholonomic cart in the presence of obstacles[END_REF], sequential composition of controllers (e.g., go to the landmark and wall following controller) are used to generate valid UGV's states q choose to the navigation function. This approach avoids to find a single globally attractive control law and allows to use some additional sensing capability of the robot when the landmark is unreachable (e.g. GPS-denied area). However, the obtained navigation function has a complex computational processing. The most important drawbacks of expanding tree algorithms are the slow convergence to cover all space until to reach the goal and that in most cases these algorithms do not provide the shortest path since the nodes are randomly selected [START_REF] Abbadi | Rrts review and options[END_REF]. Furthermore, it is important to underline that in RRT the control commands are maintained during a certain time, whereas in this thesis the UGV's movement takes into account the definition of the proposed control law (cf. Section 3.5) in addition to the UGV's model (cf. Subsection 4.3.3.2). A comparison with RRT and Voronoï approaches is shown in Subsection 4.3.4.2. Furthermore, a method to obtain the waypoints was proposed in [START_REF] Sharma | Autonomous waypoint selection for navigation and path planning: A navigation framework for multiple planning algorithms[END_REF]. The authors use agent's observation and the geometric characteristics of the environment to select the waypoints. These waypoints can be used to reduce the planning time of existing planners. However, the method is based only on the position; the orientation and UGV's model are not taken into account.

In this section, the proposed control architecture for navigation through successive waypoints (cf. Section 4.2) is extended to deal with vehicle planning/set-points definition. At this aim, a new block in the Planning layer, Waypoint configuration selection, is added. A new efficient and flexible algorithm to obtain the optimal set of waypoint configurations in the environment is proposed. Combining multi-criteria optimization and expanding tree allows safe, smooth and fast UGV's navigation (cf. Subsection 4.3.3.2). The method based on expanding tree to obtain the optimal waypoint configuration in a structured environment (cf. Fig. 4.2). This method allows to consider constraints such as the kinematic model and the used control law. Criteria to optimize are related to the kinematic constraints of the vehicle (non-holonomy, maximum velocity and steering angle) and localization uncertainties. To highlight the advantages and flexibility of the novel method,

4.3.2/ PRIMARY METHOD FOR THE SELECTION OF WAYPOINT CONFIGURATIONS FROM A REFERENCE TRAJECTORY

This method can be applied when a safe reference trajectory is already defined. This reference path could be obtained by different algorithms such as Voronoï diagram [START_REF] Latombe | Robot Motion Planning[END_REF], potential fields [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF] and others [START_REF] Lavalle | Planning Algorithms[END_REF], or by using a recorded vehicle trajectory [4]. The aim of the proposed method is to select an appropriate number of waypoints using this reference path (cf. Algorithm 1) to apply the navigation strategy based on sequential target reaching (cf. Section 4.2). Different criteria can be considered to obtain the minimum number of straight lines that closely fit the reference path. Criteria such as the euclidean or curvilinear distance, orientation or radius of curvature between waypoints can be used to fix the desired waypoints on the path. The discretized reference path r is composed of sorted pose r i = (x r i , y r i , θ r i ); where θ r i is the tangential orientation. The minimum number of straight line segments over the defined path is then computed while considering a constant threshold Δθ th for the orientation variation along the path Δθ (cf. Algorithm 1).

Algorithm 1 Waypoint selection based on existing reference path

Require: Reference path r = (x r , y r ) and Δθ th ∈ R + Ensure: Set of waypoints S p 1: Init j = 0, p j = r 0 (initial pose of r) 2: for r i ∈ r with i ≥ 1 (sorted set of path points) do end if 11: end for Fig. 4.12 shows the obtained waypoints using Algorithm 1 with Δθ th = 5 • (n p = 113), 15 • (n p = 34) and 30 • (n p = 16) respectively. Obviously, the switch between waypoints is smoother with a small value for Δθ th . Nevertheless, adding this step of pre-defined path planning (with all its possible drawbacks (cf. section 4.3.4)) before obtaining the set of waypoints, restricts considerably the C-space f ree to only a curvilinear line. Thus, the optimality of the obtained set of Waypoints is not guaranteed. Next section focus on optimization methods for the selection of waypoint configurations in any environment.

4.3.3/ OPTIMAL MULTI-CRITERIA WAYPOINT SELECTION (OMWS)

This section is dedicated to the selection of the discrete waypoints in structured environment (cf. Fig. 4.2) in order to perform safe and flexible vehicle navigation. The waypoints are obtained from an efficient and flexible methodology based on multi-criteria optimization using either grid map (cf. Subection 4.3.3.1) or expanding tree (cf. Subection 4.3.3.2).

In the both proposed Optimal Multi-criteria Waypoint Selection (OMWS) (i.e., based on Grid Map (GM) and Expanding Tree (ET)), waypoints are selected considering safe position on the road, feasibility of trajectories (smooth changes between the successive points and vehicle constraints) and localization uncertainties.

The waypoints assignment strategies (cf. Subsection 4.3.3.1 and 4.3.3.2) are formulated as an optimization problem and solved using dynamic programming [START_REF] Bellman | A markovian decision process[END_REF] (cf. Formulation 1).

Formulation 1 (Optimization problem). For each discrete state x k ∈ X where X is a nonempty and finite state space. The objective is to obtain the sequence of states to reach the final state x K while minimizing the following cost function:

C(x K ) = K k=1 g(Pred x k → x k ) + h(x K ) (4.3)
where Pred x k is the predecessor state of x k . g is the immediate traveling cost function to go from Pred x k to x k . h is the future traveling cost function (heuristic) to go from the current state to the final state x K . When the current state is the final state x K then h(x K ) is equal to zero. This function h contributes to improve the convergence of the suboptimal solutions towards the global optimal one [START_REF] Bertsekas | Dynamic Programming and Optimal Control[END_REF].

4.3.3.1/ OMWS BASED ON GRID MAP (GM)

Before describing the proposed algorithm and the criterion to optimize, let us provide some useful definitions. A grid map corresponds to a limited area of the environment decomposed generally on square cells [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF] (cf. Fig. 4. 13 and 4.14). Each cell of the grid map can be an obstacle or a free space (cf. Section 4.3 for definition of C-space obst and C-space f ree ). The exterior limits (C-space ext ) of the C-space f ree area are defined by the user, even for open environment (cf. Fig. 4.2). For simplification, the dimension of the cells in the grid map is chosen according to the vehicle's dimension. Therefore, the vehicle is contained, at each sample time, in only one cell [START_REF] Choset | Principles of Robot Motion: Theory, Algorithms, and Implementation[END_REF].

We consider the center of the cell (i, j) as its position. Each cell i j is defined by the following parameters:

• wij ∈ [0, 1] is related to the normalized distance d i j_T o_Obst to the closest C-space obst or C-space ext . wij is given by:

wij = 1 - d i j_T o_Obst d max_T o_Obst (4.4)
where d max_T o_Obst is the maximum value among all d cell_T o_Obst of all cells in the C-space f ree . For instance, Fig. 4.14 shows different distances of cells localized at (a, b), (i, j) and (m, n) to the C-space obst or C-space ext . d max_T o_Obst is equal, in this example, to the maximum distance d mn_T o_Obst .

• State is the cell state, which has three possible values, Init (Initialization), Open (when it is in the expansion queue) and Close (when it has already been expanded). • Its set of neighbors are defined by:

S N (cell i j ) = {(i ± Δi N , j ± Δ j N )|(Δi N , Δ j N ) (0, 0)} (4.5)
where Δi N , Δ j N = 1 . . . , N h . N h is the neighborhood value (cf. • Pred i j is the neighbor cell of i j which minimizes the total cost C(cell i j ) eq. ( 4.3) (cf. Fig. 4.13).

• g(Pred i j → i j) is the traveling cost from the predecessor cell until the current cell i j .

• h(i j) is the heuristic traveling cost from the current cell i j to the final cell. As conventional, this cost depends on the euclidean distance from the cell i j to the final cell.

The traveling cost function g(Pred i j → i j) = g(mn → i j), from mn to i j, is normalized

(g ∈ [0, 1]
). This function is also designed to take into account the variation of cell orientation (cf. Fig. 4.13). This variation allows to obtain an optimal path consisting of minimal number of straight lines. Therefore, a lowest possible number of waypoints in the safe area can be extracted from this optimal path. The cost function g(mn → i j) is given by:

g(mn → i j) = k g wij + (1 -k g ) α i j -α mn 2π (4.6)
where the first term of (4.6) is related to the safety of the obtained solution, and the real constant k g ∈ [0, 1] is used to increase or to decrease the significance of this term. The second term of (4.6) is related to the smoothness of the obtained solution, i.e, the path has a limited and minimum orientation change. The cell orientations α mn , α i j ∈] -π, π] are computed using the position of the current cell (i, j), its predecessor (m, n) and its probable successor (a, b) (cf. Fig. 4.13). They are computed as:

α i j = arctan a -i b -j (4.7) α mn = arctan i -m j -n (4.8)
The heuristic traveling cost h(i j) ∈ [0, 1] (refers to eq. ( 4.3)) is designed in function of the euclidean distance d i j from the cell i j to the final cell. This cost function is also used for OMWS-ET (cf. Subsection 4.3.3.2). The cost function h(i j) is given by:

h(i j) = k h 1 -e -d i j /k e (4.9)
where k h ∈ [0, 1] allows to tune the significance of h(i j) in the total cost function (cf. eq. (4.3)). The exponential function was chosen because it can be formulated to give values between 0 and 1 for positive values of d i j . The constant k e ∈ R + is used to scale the value of d i j according to the environmental dimensions. The value of h(i j) (cf. eq. (4.9)) decreases while the next selected cell goes closer to the final cell.

Algorithm 2 shows in pseudocode, the first proposed method to obtain the set of waypoints in a structured environment. It starts from the final vehicle's position (initial cell). The algorithm selects the cells that have the lower total cost C(i j) (4.3) until to reach the final cell. A discrete path r is obtained while tracking the predecessor cell of each selected cell which minimizes the total cost starting in p i until p f . Finally, the proposed Algorithm 1 is applied to the discrete path r to obtain the set of waypoints S p characterized by (x p i , y p i , θ p i , v p i ). Basically, this algorithm keeps only the sorted waypoints (pose, velocity and predecessor) which have an orientation changes w.r.t. its predecessors.

Algorithm 2

Waypoint selection based on a grid map Require: Initial position p i , final position p f and a Gridmap Ensure: Set of waypoints S p 1: Init State i j = INIT , g i j = 0 and Pred i j = , ∀ i j ∈ Gridmap 2: Init cell i j = p f and the set of neighbors S N (cell i j ) 3: while cell i j p i do Until to reach the initial position 4:

Set S tate i j = CLOSE 5:

for cell N ∈ S N (cell i j ) do 6:

if wcell N 0 then Only cells in the free space 7:

Obtain Pred cell i j = Predecessor(cell i j ) When cell i j p f 8:

Compute α i j (4. Get the first value of queue cell i j = Q( f irst) and remove it from Q 21: end while 22: Obtain the discrete path (straight lines) linking r = p i , Pred p i , Pred{Pred p i }, . . . , p f . 23: Apply Algorithm 1 using r to obtain the set of waypoint S p . ). The drawback of using a large number of neighbors is obviously the increase of processing time. When N h ≥ 1, it is mandatory to check if the current neighbor is blocked by some other forbidden neighbor (cf. Fig.

4.15(b))

. For an off-line planning, N h > 1 can always be used to obtain a coherent and optimal solution regardless of time consumption.

4.3.3.2/ OMWS BASED ON EXPANDING TREE (ET)

This subsection presents in details the proposed algorithm for optimal planning of the vehicle's path, using an appropriate expanding tree. The formulation of this expanding tree integrates the kinematic model of the vehicle as well as the proposed control law definition (cf. Section 3.5) and the vehicle's localization uncertainties.

Before describing the proposed method and the criterion to be optimized, let us present the definition of expanding tree. The expanding tree is composed by nodes and edges which have the following properties:

• Each node q j is defined by its pose (x q j , y q j , θ q j ) T , one predecessor node q i (except for the initial node) and a traveling cost values g(.) and h(.) (cf. eq. ( 4.3)).

• Each edge ξ i j corresponds to the link between q i to q j nodes.

• g(q i → q j ) = g(ξ i j ) is the traveling cost from q i to q j .

• h(q j ) is the heuristic traveling cost from the current node q j to the final node (final vehicle pose). It was defined by eq. (4.9) (cf. Subsection 4.3.3.1).

The traveling cost g(ξ i j ) ∈ [0, 1] is designed to obtain an appropriate balance among safe, smooth, feasible and fast vehicle's trajectory. It is defined as:

g(ξ i j ) = k 1 w j + k 2 Δv i j + k 3 Δγ i j + k 4 Δē l i j (4.10)
where k 1 , k 2 , k 3 and k 4 ∈ R + are constants which are defined by the designer to give the right balance (according to context of navigation, e.g., give more priority to the safety with regard to the smoothness) of each term of the criterion (cf. eq. (4.10)). To normalize the traveling cost, k i |i = 1, . . . , 4 must satisfy:

k 1 + k 2 + k 3 + k 4 = 1 (4.11)
The normalization of the individual criterion given in (4.10) allows to simplify the choice of k i to select the priority of a term w.r.t. the others according to the navigation context. Different set of values k 1 , k 2 , k 3 and k 4 will be considered for different scenarios in Subsection 4.3.4.

The first term of the cost function eq. (4.10) is related to the navigational safety (4.4).

The second and third terms are respectively related to the speed (4.12) and smoothness (4.15) of the trajectory. The fourth term is related to feasibility of the vehicle's trajectory while considering localization uncertainties, i.e., the risk to collide with an obstacle while considering inaccuracies in the UGV's pose (a detailed explanation of this term is given later in this subsection). This last term allows to consider the kinematic model of the UGV and the control law. The details of each term are given in the following:

• The term w j ∈ [0, 1] is related to the distance from the node q j to the closest C-space obst or C-space ext . It is given by eq. (4.4) (cf. Subsection 4.3.3.1).

• The term Δv i j ∈ [0, 1] is related to the velocity v i j from q i to q j . It is given by:

Δv i j = 1 - v i j v max (4.12)
where v max is the maximum velocity of the vehicle. v i j is estimated as a function of the curvature of the trajectory. The maximum velocity occurs when the curvature is zero (straight line) and the minimum velocity v min 0 occurs when the curvature is bigger than the value corresponding to γ max (cf. Fig. 3.5). This consideration allows the vehicle to maneuver without risk of collisions while enhancing the passenger comfort since the centripetal forces are limited [START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and clothoid based methods and its simulation[END_REF]. The minimum and maximum values of velocity and steering angle are defined by the designer according to vehicle's characteristics. The curvature is estimated using the orientation of the current node and its predecessor. Therefore, v i j is computed as:

v i j = v max -Δ θij (v max -v min ) (4.13)
where Δ θij ∈ [0, 1] is the normalized estimated curvature related to the variation of orientation between the current node q j and its predecessor q i . It is defined as:

Δ θij = |θ j -θ i | Δθ max (4.14)
where Δθ max is the maximum variation between a probable orientation of the current node w.r.t the orientation of its predecessor. This value is defined according to the steering vehicle's capability. θ j and θ i are computed using the node positions and eq. (4.7) and (4.8) (cf. Subsection 4.3.3.1). • The term Δγ i j ∈ [0, 1] is related to the variation of steering angle along the vehicle's trajectory from q i to q j . For instance, Fig. 4.17 shows a vehicle's trajectory between two nodes. It is given by:

Δγ i j = q j q i |Δγ i j | n q i j γ max (4.15)
where n q i j is the considered point number of the vehicle trajectory between q i and q j , and γ max is the maximum steering angle of the vehicle. This term Δγ i j (4.15) computes the sum of the Δγ i j to obtain the total variation of the steering angle along the vehicle's trajectory. Δγ i j uses the kinematic model and the control law to estimate the vehicle's trajectory.

• The term Δē l i j ∈ [0, 1] is the normalized maximum deviation of vehicle's trajectory w.r.t. the straight line that joins the positions (x q , y q ) of q i and q j (cf. Fig. 4.17). It is computed as:

Δē l i j = |Δe l i j | max{Δe l } (4.16)
where max{Δe l } is the maximum deviation of all trajectories from the node q i to the node q j while considering the position and orientation uncertainties ( d and θ respectively given in Fig. 4.17). This term Δē l i j allows to estimate the collision risk using the vehicle's trajectory that takes into account the kinematic model, the control law and localization uncertainties (position and orientation). Fig. 4.17 shows an illustration where the UGV has an ellipse of localization uncertainties with axes l d and t d . The UGV's trajectories start at ± l d in lateral distance (longitudinal distance is set to 0), and ± t d in longitudinal distance (lateral distance is set to 0) from the UGV's position with a ± θ from the presumed vehicle's orientation, i.e., we consider all extreme configurations to obtain, according to these maximum error configurations, the maximum lateral deviation (Δe l ). The trajectories are obtained using the kinematic model and the proposed control law in an offline simulated procedure.

Algorithm 3 shows in pseudocode, the proposed method which uses expanding tree to obtain the optimal waypoints configurations w.r.t. the optimized multi-criteria function eq. (4.10). Fig. 4.18 shows the first steps of the algorithm in which, for instance, the branch numbers of each node is n t = 3. Each branch orientation w.r.t. the vehicle orientation is given by: α = ±iΔα, i = 0, 1, . . . , (n t -1)/2; if n t is odd 1, 2, . . . , n t /2; if n t is even (4.17)

Algorithm 3

Waypoint selection based on expanding tree Require: Initial pose p i , final pose p f , branch number n t , edge distance ξ, branch orientation Δα, tolerable error distance and C-space f ree Ensure: Set of waypoints S p 1: Init the initial node q 0 = p i , g 0 = 0 and Pred q 0 = 2: Init the current node to expand q i = q 0 3: Init T ree(q i ) =Expansion_Tree (Procedure 4) with α = 0 Initial expansion 4: Set the new node to expand q i = r t where r t ∈ T ree(q i ) 5: Set Pred r t = q i and compute the total cost C(r t ) eq. ( 4.3) 6:

while |q i -p f | < do 7:
Compute the T ree(q i ) = Expansion_Tree Procedure 4 with a set of α = ±iΔα 8:

for r t ∈ T ree(q i ) do 9:

if r t ∈ C-space f ree then 10:

Compute the total cost C(r t ) eq. ( 4.3)

11:

Pred r t = q i 12:

Add r t to the queue Q Sort the queue Q in ascending order of total cost C 16:

Get the first value of queue q i = Q( f irst) and remove it from Q 17: end while 18: Obtain the discrete path (straight lines) linking r = p i , . . . , Pred{Pred p f }, Pred p f , p f . 19: Apply Algorithm 1 using r to obtain the set of waypoint S p .

where Δα is a constant angle defined according to the vehicle's characteristics.

The edge distance ξ is the Euclidean distance between two successive nodes and it depends on the environment dimensions, e.g., if the environment has a narrow passage then ξ must cope with this dimension. It is considered that the edge orientation is the vehicle's orientation at the current node position (cf. Fig. 4.18). Thus, at beginning the first expansion of q 0 is given with α = 0 because the vehicle starts at initial fixed pose (cf. line 3 -5 of Algorithm 3). This initial expansion is made to respect the kinematic constraints where the vehicle's rotation requires a displacement (linear velocity 0) of it. Therefore, the successive node q 1 has different possible orientation and so on (cf. Fig. 4.18). The algorithm selects the node which has the lower total cost C(q j ) (4.3). When two or more nodes have the same cost, the algorithm selects the last saved node. Fig. 4.18 shows the successive steps, the node q 4 was selected from the expansion of q 1 {q 2 , q 3 , q 4 } since it has the lower total cost value. The selection of the node with lower total cost (cf. Algorithm 3, line 15 -16) allows to avoid the deadlock areas because the successive branches from the nodes in this deadlock area will be in C-space obst (cf. Fig.

4.19(a)).

Procedure 4 Expansion_Tree Require: Current node q i , set of α S (α), edge distance ξ Ensure: Nodes of T ree(q i ) 1: Init T ree(q i ) = 2: for α t ∈ S (α) do 3:

Compute orientation θ r t = θ q i + α t

4:

Compute pose r t = q i + [ξ cos(θ r t ), ξ sin(θ r t ), α t ] T

5:

Add r t to T ree(q i ) 6: end for A discrete path r is obtained while tracking the predecessor cell of each selected cell which minimizes the total cost starting in p f until p i . Finally, the proposed Algorithm 1 is applied to the discrete path r to obtain the set of waypoints S p as detailed in Subsection 4.3.3.1. The smoothness of the UGV's trajectory depends on the number of branches of each tree n t , the maximum branch orientation α max = n t Δα/2 and the edge distance ξ (cf. Subection 4.3.4). The drawback of using a large number of n t is the increase of the processing time required to obtain the set of waypoints. The edge distance ξ is set to detect obstacles between the successive nodes. This method uses deterministic selection of expanding tree to obtain the optimal solution with lowest total cost. Nevertheless, a feasible solution can be obtained using a probabilistic selection of expanding tree to decrease the processing time (cf. Subsection 4.3.4), i.e., the branch orientation α and edge distance ξ are randomly selected in a fixed interval [START_REF] Lavalle | Planning Algorithms[END_REF]. Simulations in Subsection 4.3.4 show the case where these parameters are randomly chosen.

As described above, the traveling cost eq. (4.10) depends on four parameters (k i |i = 1, . . . , 4, which satisfy eq. (4.11)) related respectively to the safety, velocity, less steering and taking into account localization uncertainties. The values of these parameters are fixed according to the desired navigation and environment conditions. A pragmatical procedure to set these parameters consists in first identifying the main desired vehicle behavior and setting its parameter k i with a value greater than 0.5 (cf. Fig. 4.19). The other parameters will be tuned according to the designer's secondary priorities. Fig. 4. [START_REF] Payton | An architecture for reflexive autonomous vehicle control[END_REF] shows the set of waypoints when only the term with highest priority is considered in the traveling cost function. For instance, the priority is given to the safest and the shortest paths represented respectively in Fig. 4. 19(a) For these simulations, the physical parameters, constraints and controller parameters K of the UGV were described in Subsection 4.2.3.

4.3.4.1/ GRID MAP VERSUS EXPANDING TREE

These simulations show two set of waypoints obtained by two proposed methods based on grid map and expanding tree (cf. Algorithms 2 and 3 respectively). The maximum number of iteration is n I = 5000 to stop both algorithms, OMWS-GM (Algorithm 2) and OMWS-ET (Algorithm 3), when none solution can be obtained. For the grid map case, the cell has the UGV's dimension (2 m) and its neighborhood is N h = 1. The constant value of k g is 0.6 eq. (4.6) and k h is 0.1 eq. (4.9). The minimum set of waypoints has n p = 27 elements. An additional constraint is added for OMWS-GM (before line 9 of the Algorithm 2), the angle variation (second term of eq. (4.6) must be less than a threshold Δα th ). This constraint enables the processing time of the algorithm to be reduced since it considers only the cells with an orientation change, w.r.t the last cell orientation, less than Δα th (cf. Fig. 4.13).

For Expanding Tree case, the branch number n t is 5, the edge distance ξ is 2.5 m and Δα is 15 Table 4.1 shows different performance criteria to compare the set of waypoints where: n p is the number of waypoints, length is the sum of distance between two successive waypoints, d border is the sum of minimum distance to the road boundaries. Therefore, the method based on OMWS-ET is more safe, accurate and efficient than the one based on OMWS-GM, mainly when the criterion to optimize is related to the UGV's model (velocity and steering angle).

It is noted that the set of waypoint obtained by OMWS-ET is smaller (19 elements, cf.

4.3.4.2/ OMWS-ET VERSUS RRT*

To highlight the advantages and flexibility of the proposed OMWS-ET, a comparison with the popular RRT* algorithm [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF] is presented in this subsection. The RRT* is based on the RRT (Rapidly-exploring Random Tree) already described in Section 4.3 with an addition of the rewiring function which allows to reconnect the nodes to ensure that the edges have the path with minimum total cost (cf. Appendix B.2). RRT* provides thus an optimal solution with minimal computational and memory requirements [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF]. Moreover, RRT* is a sampling-based algorithm and the optimal solution depends on the number of iterations of the RRT* algorithm, i.e., more is the number of iterations (more samples in the C-space f ree ) closer is the obtained solution to the effective optimal global solution. Therefore, to compare the solutions obtained by the OMWS-ET with those obtained by the RRT* some little modifications in Algorithm 3 were made. The line 6 of Algorithm 3 was changed by a for loop from 0 to the maximum iteration number n I and the selection of the final pose at each iteration is obtained by the sampling in C-space f ree (q random ) as the RRT* Algorithm [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF]. It is to be noted that q random corresponds to a random sample (position) from a uniform distribution in the C-space f ree .

To compare OMWS-ET and RRT* algorithms, the safest obtained solution (which maximizes the distance to the border) is used as criterion. Therefore, the parameters of the cost function of OMWS-ET eq. ( 4.10) are fixed to:

k 1 = 1.0, k 2 = k 3 = k 4 = 0.0 and k h = 0.1.
In addition, the other parameters are fixed such as: the branch number n t = 5, the edge distance ξ = 2.5 m and Δα = 15 • .

The RRT* algorithm described in [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF] (cf. Appendix B.2) was also modified to obtain a cost function according to the safety wi (distance to the border) instead of an Euclidean distance between nodes. The kinematic model (cf. Fig. 3.5) with constant linear velocity and steering angles (v = 1.0 m/s and γ = -15, -7.5, 0, 7.5, 15 • ) respectively, during t exp = 2.5 s was used to produce the new nodes of the RRT*. The maximum number of iteration for both algorithm is fixed to n I = 5000. Fig. 4.22 shows the obtained path solutions according to RRT*, OMWS-ET and also Voronoï [START_REF] Latombe | Robot Motion Planning[END_REF] algorithms. The obtained Voronoï path (cf. Fig. 4.22(c)) is represented since it is the best reference w.r.t. the adopted comparison criterion (safety). Indeed, Voronoï path permits always to obtain the safest possible path [START_REF] Latombe | Robot Motion Planning[END_REF].

It can be noted that two obtained path using RRT* and OMWS-ET are generally enough far from the way borders (cf. Fig. 4.22(a) and 4.22(b)). Nevertheless, significant differences are observed in the obtained final results (cf. Fig. 4.22(c)). In fact, the obtained set of waypoint using RRT* are closer to the border which is due to the fact that RRT* expands its branches while adopting constant commands (v, γ, t exp ). These constant commands generate the next nodes with only a single possible orientation (for each node). Contrary to that, in the proposed OMWS-ET, each new obtained node q j has different possible orientations and velocities, thus, for the same position, more possible UGV's states (different orientations and velocity set-points) are taking into account in the optimization process.

Table 4.2 shows, as in the last subsection, different performance criteria to compare the obtained path. It is shown that the obtained path based on OMWS-ET is closer than the RRT* to the optimal obtained solution using Voronoï method. This result validates that the proposed OMWS-ET is more efficient than the RRT*, in the sens that OMWS-ET explores more possibilities in the state space of the UGV and environment.

Moreover, it is important to mention that the proposed OMWS-ET algorithm is related to . RRT* method is more suitable for navigation strategies based on trajectory following [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF]. This subsection shows the minimum set of obtained waypoints for different scenarios using the method based on expanding tree (Algorithm 3). Fig. 4. 23(a) shows the set of waypoints while considering the edge distance ξ = 10 m with an objective to obtain the fastest trajectory from the initial to the final positions, while not colliding with the road limits. The constant values are k 1 = 0.1, k 2 = 0.7, k 3 = 0.1, k 4 = 0.1 and k h = 0.1. The minimum set of waypoints allows the vehicle to generate a minimum time trajectory as in [START_REF] Rucco | Computing minimum lap-time trajectories for a single-track car with load transfer[END_REF]. This trajectory has a segment close to the route boundaries (tangent to the borders) which allows to navigate applying the maximum velocity. Δα and increase the number of branches n t then the waypoints will be on the reference trajectory. In Section 4.3.2, the waypoints are selected while considering only the points in the reference trajectory. It consists on analyzing the orientation variation of each points on the trajectory. In this case, the waypoints are selected in the environment to be close to the reference trajectory which allows to obtain less number of waypoints than the method used in Section 4.3.2 [4].

4.3.4.4/ DETERMINISTIC VERSUS PROBABILISTIC

This simulation shows the comparison between a deterministic and probabilistic expanding tree (i.e., where the values of ξ and α are probabilistically taken from an interval, instead of, to be fixed by the designer). Fig. 4.24 shows the minimum set of waypoints obtained using probabilistic expanding tree, where ξ ∈]0, 2.5] and α ∈ [-30

• , 30 • ]. The constant values are k 1 = 0.6, k 2 = 0.2, k 3 = 0.1, k 4 = 0.1 and k h = 0.1.
The processing time of the method with probabilistic expanding tree is less than the method with deterministic expanding tree. Nevertheless, the set of waypoints are not the optimal solution. The advantages of probabilistic selection of ξ and α is to reduce the convergence time and to obtain an online implementation [START_REF] Kuwata | Motion planning for urban driving using rrt[END_REF][START_REF] Vaz | Kinodynamic motion planning of a skidsteering mobile robot using rrts[END_REF]. In future works, the choice of the variation of ξ and α will be oriented to improve the efficiency of the algorithm. The proposed method based on expanding tree (Algorithm 3) was extended to multi-robot formation (MRF) where the formation is defined only according to the leader's configuration [START_REF] Consolini | Leader-follower formation control of nonholonomic mobile robots with input constraints[END_REF] (cf. [357] is used by the proposed OMWS-ET (Algorithm 3). This map allows implementing the navigation through successive waypoints in a real vehicle and situations. The proposed OMWS-ET computes the set of geo-referenced waypoints with optimal configuration. Certain areas are restricted to guide the Algorithm 3 through PAVIN platform which has intersections and roundabout (cf. Fig. 4.29). In our case, these restricted areas were selected by the user, nevertheless the selection can be made by considering the topological map of the environment.

4.4/ EXPERIMENTAL VALIDATIONS

4.4.1/ WAYPOINT PLANNING USING OMWS-ET

This experiment makes a comparison between two cases of the proposed OMWS-ET: the first, corresponds to give more priority for the safety criteria in eq. ( 4.10 on page 96) and the second gives more priority for the minimum angle steering rate. The analysis of the obtained solutions will be given in what follows. Moreover, the actual vehicle's trajectories are compared for these different set of waypoints. This experiment can be found online 1 . ). Therefore, the vehicle can perform different maneuvers between waypoints, in this case the obstacle avoidance without the use of any trajectory replanning method. The elliptical limit-cycle trajectories are applied for reactive obstacle avoidance using only position information and uncertain range data (cf. Section 3.2). Each hinder obstacle is surrounded by an ellipse which parameters are computed online using range data (cf. Section 3.4). The UGV has to reach successively each static waypoint while avoiding hinder obstacles. This experiment can be found online 2 .

2 http://maccs.univ-bpclermont.fr/uploads/Profiles/VilcaJM/NavigationWaypts.mp4 are the set of waypoints and the big red point is the current assigned waypoint (target).

It can be noted the online detection of the boxes using the LIDAR sensor (cf. Section 3.4) and the reactive obstacle avoidance (based on elliptical limit-cycle (cf. Section 3.2)) performed by the UGV (cf. Fig. 4.33(a) (b), (c) and (f)). Therefore, the vehicle can perform different maneuvers between waypoints, in this case the obstacle avoidance without the use of any trajectory replanning method.

Fig. 4. 33(b) shows that the UGV reaches accurately the successive static waypoints. Moreover, this figure shows the vehicle's trajectories when the obstacle avoidance is activated (green line). The UGV detects the hinder obstacle between the waypoints and it applies the reactive limit-cycle method [START_REF] Vilca | Robust on-line obstacle detection using range data for reactive navigation[END_REF][START_REF] Vilca | Reactive navigation of a mobile robot using elliptic trajectories and effective online obstacle detection[END_REF]. The UGV does never collide with any obstacle.

4.5/ CONCLUSION

This chapter presented an overall control architecture to cope with the problem of navigation in a structured environment. A novel safe and flexible navigation strategy based on successive target reaching for a UGV in structured environment was presented. It is based on target switching using appropriate reference frames, linked to the current selected waypoint and to the next one in the list. This target assignment strategy enables a smooth and flexible vehicle trajectory while satisfying an upper bound on distance and orientation errors (to not collide with the boundary of the environment). Furthermore, different target-reaching methods from the literature were presented and their performances were compared with the proposed navigation strategy. These comparisons showed the interesting features of the proposed control law in term of stability and flexibility for different tasks. Simulations shows the efficiency and the flexibility of the proposed navigation strategy for an urban vehicle in structured environment. Furthermore, this chapter has presented three planning methods to obtain the optimal waypoints configuration (Primary waypoint selection, Optimal Multi-criteria Waypoints Selection based on Expanding Tree (OMWS-ET) and Grid Map (OMWS-GM)) which guarantees safe, smooth and feasible vehicle navigation in a structured environment. The proposed OMWS-GM is based on the A * algorithm with an additional term to consider the orientation change between successive cells. OMWS-ET uses a multi-criteria function which takes into account the vehicle model and localization uncertainties to obtain the optimal set of waypoint configurations (position, orientation and velocity). Moreover, it has been shown that the proposed OMWS-ET is more accurate and flexible than OMWS-GM. Several simulations demonstrate the efficiency and reliability of the proposed OMWS-ET in different cases (trajectory specification, deterministic versus probabilistic, comparison with RRT*, multi-robot formation, local replanning according to the multi-criteria optimization).

MULTI-VEHICLE NAVIGATION IN FORMATION

Control and coordination of multiple robots is a challenging domain which has increased significantly over the last decades. Different tasks that may be performed by a single complex robot can be performed with more flexibility and efficiency by a group of elementary cooperative robots. Some examples of multi-robot tasks can be found in Section 2.1 such as: exploration [START_REF] Lozenguez | Map partitioning to approximate an exploration strategy in mobile robotics[END_REF], mapping of unknown locations [START_REF] Miner | Swarm robotics algorithms: A survey[END_REF], coverage of unknown area [START_REF] Dasgupta | Effects of multi-robot team formations on distributed area coverage[END_REF], transport of heavy objects [START_REF] Hichri | Cooperative lifting and transport by a group of mobile robots[END_REF] and platooning for public transportation [START_REF] Levinson | Robust vehicle localization in urban environments using probabilistic maps[END_REF]. This chapter addresses the navigation of a group of vehicles in formation with reconfiguration (cf. Section 2.3), i.e., when a group of mobile robots has to navigate while keeping (or changing) the desired relative positions to each other according to the context of the environment.

Different reconfiguration methods from the literature are presented in the next section. Section 5.2 presents the proposed navigation in formation based on Leader-follower and Behavior-based approaches according to the formation frame. An analysis of the formation constraints (physics, geometric and kinematics) is detailed in Section 5.3. Section 5.4 presents the proposed reconfiguration methods for the multi-robot dynamic formation. Several simulations and experiments are performed to demonstrate the stability, reliability, flexibility and advantages of the navigation in formation of a group of UGVs for each section. Finally, Section 5.5 provides a conclusion.

5.1/ RELATED WORKS

In Section 2.3, several approaches have been described to deal with formation control problem such as: behavior-based [START_REF] Tang | Hybrid behavior coordination mechanism for navigation of reconnaissance robot[END_REF], virtual structure [START_REF] Desai | Modeling and control of formations of nonholonomic mobile robots[END_REF] and Leader-follower [START_REF] Ghommam | Formation path following control of unicycle-type mobile robots[END_REF], etc. This chapter proposes a new control architecture based on Leader-follower and behavior-based approaches for the formation control problem.

The case of dynamic formation or reconfiguration, i.e., the formation shape changes to another (e.g. from square to triangle) was dealt in [START_REF] Das | A vision-based formation control framework[END_REF][START_REF] Chen | Smooth formation navigation of multiple mobile robots for avoiding moving obstacles[END_REF][START_REF] Shames | Safe formation control with obstacle avoidance[END_REF]. In [START_REF] Chen | Smooth formation navigation of multiple mobile robots for avoiding moving obstacles[END_REF], the leader generates a free-collision trajectory in a dynamic environment which is tracked using a formation control law based on neural network (NN), Lyapunov function and dynamic model of the robot. The stability of the dynamic formation and dynamic topology (adjacency matrix) are also demonstrated. In [START_REF] Das | A vision-based formation control framework[END_REF], 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 (omnidirectional 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 [START_REF] Shames | Safe formation control with obstacle avoidance[END_REF]. Obstacle avoidance for the formation is dealt while using potential fields.

In [START_REF] Lawton | A decentralized approach to formation maneuvers[END_REF], the authors added complex maneuvers that are decomposed into a sequence of elementary maneuvers between formation patterns (desired position of the robots). The behavior-based formation control relies on coupled dynamics (relative position and velocity) between neighbor robots. A non-rigid formation control that determines the most appropriate robot positions (formation shape) according to the directional visual perception was proposed in [START_REF] Lemay | Autonomous initialization of robot formations[END_REF]. The inter-robot communication allows to choose the leader of the group. The formation control with collision avoidance based on behavioral control was proposed in [START_REF] Antonelli | Experiments of formation control with collisions avoidance using the null-space-based behavioral control[END_REF]. The authors propose a Null-spaced-based behavioral control to coordinate the formation while performing different tasks. These approaches are very flexible and extensible to more complex tasks, nevertheless, it is difficult to guaranty the stability of the overall system. A static and dynamic virtual structure based on Lyapunov approach was proposed in [START_REF] Chang | A formation control framework based on lyapunov approach[END_REF]. A virtual structure with obstacle avoidance for a group of UAV was proposed in [START_REF] Chao | UAV formation flight based on nonlinear model predictive control[END_REF]. The approach uses a penalty function (distance to the virtual target, obstacle) and a priority strategy (according to the distance to an object (other UAV or an obstacle). The control law use the Model Predictive Control (MPC) based on time horizon and optimization of a cost function. Nevertheless, this method is generally time consuming due to predictive computation w.r.t. a time horizon.

In the already described works, interesting solutions for formation control problem are proposed. Nevertheless, they are based generally on predefined trajectories and they do not address issues related to constraints of the formation shape and to the UGV's kinematics (large UGVs require large space for navigation and obstacle avoidance). This chapter describes the navigation of a group of UGVs while keeping a specific formation and reconfiguration according to the environmental context. The dynamics of the followers' set-points are given by the specific dynamic of the leader (Leader-follower approach). This strategy allows a good flexibility properties for the formation shape [START_REF] Consolini | Leader-follower formation control of nonholonomic mobile robots with input constraints[END_REF] (cf. Section 5.2). The used behavior-based approach allows to use different elementary controllers to perform sequentially several sub-tasks (cf. Fig. 5.2). The dynamic of the formation according to vehicles' constraints is analyzed. A method to adapt the formation constraints, velocities and steering angle, according to the dynamic of the leader is proposed. The convergence of the formation is guaranteed when the adaptive constraints are satisfied (cf. Section 5.3). A new Strategy for Formation Reconfiguration (SFR) of the group of UGVs based on suitable smooth adaptation of the set-points (according, for instance, to the encountered obstacles or the new task to achieve) is also proposed in Section 5.4.

5.2/ PROPOSED STRATEGY FOR NAVIGATION IN FORMATION

The objective of the group of N UGVs is to reach and to keep their assigned configuration according to the desired formation shape and leader's configuration [START_REF] Das | A vision-based formation control framework[END_REF][START_REF] Ze-Su | Formation control and obstacle avoidance for multiple robots subject to wheel-slip[END_REF]. The proposed strategy consists in controlling each UGV (follower) to track its assigned virtual dynamic target [5] (cf. Subsection 5.2.1 and 5.2.2). This strategy, based on Leaderfollower and Behavior-based approaches, is defined by:

• A leader (UGV L in Fig. 5.3 and 5.4); its pose (x L , y L , θ L ) and its linear velocity v L determine the dynamic of the formation (cf. Fig. 5.3).

• 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 (T d i ). The formation is defined as The proposed control architecture for the navigation in formation (cf. Fig. 5.2 where N = 2) is based on the one proposed in Section 4.3 for the navigation through successive waypoints in structured environment. At this aim, two new blocks, Communications and Formation parameters have been added. The Communications block is related with the UGV's capability to send and to receive information from other UGVs. The Formation parameters block is designed to generate the virtual dynamic target T d i that each follower has to track using the proposed target-reaching control [4,[START_REF] Vilca | A novel safe and flexible control strategy based on target reaching for the navigation of urban vehicles[END_REF] (cf. Section 3.5).

F = {f i , i = 1 • • • N}, where f i are the coordinates (h i , l i ) T of
An important advantage of the proposed strategy for navigation in formation is that it takes, in addition to the target's positions (x T , y T ), the heading θ T of the virtual targets, which allows to have accurate formation navigation (cf. Section 5.2.3). Furthermore, the proposed approach is flexible in the sense that it does not generally depend on any predefined reference trajectory [START_REF] Chen | Smooth formation navigation of multiple mobile robots for avoiding moving obstacles[END_REF][START_REF] Shames | Safe formation control with obstacle avoidance[END_REF]. The formation is fully defined by the instantaneous dynamic of the leader.

One important consideration to achieve the proposed Multi-Robot Formation (MRF), is that the followers have to know, as accurately as possible, the leader state (pose and velocity). The leader sends its state by stable Wi-Fi communication (cf. Fig. 5.2). However, cameras and/or LIDAR sensors embedded in each follower, can be used to estimate the leader's state [START_REF] Das | A vision-based formation control framework[END_REF][START_REF] El-Zaher | Echelon platoon organisation: a distributed approach based on 2-spring virtual links[END_REF].

A detailed survey of two suitable frames for formation's definition, Cartesian and Frenet frames, are presented in next subsections. In the sequel, f i is given in Global Cartesian frame to homogenize the notation of the presented approaches (cf. Subsections 5.2.1 and 5.2.2).

5.2.1/ CARTESIAN FORMATION

In this subsection, a rigid definition of the formation is applied to maintain the formation shape (e.g. a triangle in Fig. 5.3). The position and orientation of each node (virtual target) are computed from the leader's configuration w.r.t. the Cartesian frame (local frame of the leader X m Y m ) [START_REF] Mariottini | Leaderfollower formations: Uncalibrated vision-based localization and control[END_REF][START_REF] Benzerrouk | Navigation of multirobot formation in unstructured environment using dynamical virtual structures[END_REF][START_REF] Shames | Safe formation control with obstacle avoidance[END_REF]. The leader's position determines the nodes' positions according to the formation shape. The instantaneous center of curvature Icc L of the formation is determined by the leader according to its movements (cf. Fig. 5.

3).

Icc L allows to compute the desired orientation of the nodes according to the formation shape (cf. Fig. 5.3). The leader turns around Icc L (positioned perpendicularly to its rear wheel), then the other target set-points T d i must also turn around Icc L to maintain the rigid formation. Thus, the target velocity v T i must be tangent to the circle which has Icc L as center and the distance between T d i and Icc L as radius r c T i .

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 coordinates (l i , ψ i ) can also be used by applying an elementary transformation [START_REF] Mariottini | Leaderfollower formations: Uncalibrated vision-based localization and control[END_REF][START_REF] Benzerrouk | Navigation of multirobot formation in unstructured environment using dynamical virtual structures[END_REF].

The pose of the virtual target T d i w.r.t the leader pose in the Global reference frame can be written as (cf. Fig. 5.3):

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ x c T i y c T i θ c T i ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ x L y L θ L ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ + ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(θ L ) -sin(θ L ) 0 sin(θ L ) cos(θ L ) 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ h c i l c i β i ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (5.1)
where (x L , y L , θ L ) is the current pose of the leader and β i is the T d i orientation w.r.t. the leader's pose. given by:

β i = arctan h c i r c L -l c i (5.2)
where r c L is the radius of curvature of the leader. Differentiating eq. (5.1), the velocities of each T d i are thus given by:

v c T i = (v L -l c i ω L ) 2 + (h c i ω L ) 2 (5.3
)

ω c T i =ω L + βi (5.4)
where v L and ω L are respectively the linear and angular velocities of the leader, βi is computed as:

βi = -h c i ṙc L (r c L -l c i ) 2 + (h c i ) 2 (5.5)
One can note from eq. (5.5) that when βi is equal to zero, the formation has a constant radius of curvature r c L and the angular velocities of the virtual targets are equal to the angular velocity of the leader (ω c T i = ω L ) eq. (5.4).

5.2.2/ FRENET FORMATION

This approach is applied when tracking the leader's movements is more important than keeping a rigid shape during the navigation in very structured environments (limited roads). The objective of this Frenet frame is to adapt the formation to the leader's trajectory. This trajectory is used to define the formation in longitudinal (curvilinear) and lateral (perpendicular to the trajectory) coordinates [START_REF] Avanzini | Urban vehicle platoon using monocular vision: Scale factor estimation[END_REF][START_REF] Klan Čar | A control strategy for platoons of differential drive wheeled mobile robot[END_REF] (cf. Fig. 5.4). In this section, the leader trajectory given in the Global reference frame can be written as:

f f i = (h f i , l f i ) T is given in Frenet coordinates where (h f i , l f i ) are respectively the curvilinear
⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ x f T i y f T i θ f T i ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ x L (h f i ) y L (h f i ) θ L (h f i ) ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ + ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos(θ L (h f i )) -sin(θ L (h f i )) 0 sin(θ L (h f i )) cos(θ L (h f i )) 0 0 0 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 0 l f i 0 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (5.6)
where

(x L (h f i ), y L (h f i ), θ L (h f i ))
is the pose of the leader at h i longitudinal distance from the current leader's pose along its trajectory ( cf. Fig. 5.4). If h f i < 0 then T d i is back to the current leader's pose.

The coordinates h f i has to be constant to keep the desired curvilinear distance then, the linear velocity at h f i must be equal to current linear velocity of the leader (v L (h

f i ) = v L ).
The velocities of each T d i are given by:

v f T i =v L -l f i ω L (h f i ) (5.7
)

ω f T i =ω L (h f i ) = v L r c L (h f i ) (5.8)
where r c L (h f i ) is the radius of curvature at h f i (longitudinal distance from the current pose of the leader along its trajectory).

5.2.3/ SIMULATION ANALYSIS OF THE NAVIGATION IN FORMATION

This section shows the evolution of a group of N = 3 UGVs that navigates while keeping the desired formation. Simulations using tricycle vehicles shows the efficiency and the flexibility of the proposed navigation in formation using only the leader's dynamic.

In these simulation, the used UGV's parameters, constraints, sensors and controller parameters were described in Section 3.6. In addition for multi-vehicle navigation, each UGV has a stable communication network between UGVs without latency. Firstly, to illustrate the application of each formation frame, a navigation for two presented formation definition with a constant linear velocity of the leader (v L = 1 m/s) was simulated. Fig. 5.5(a) shows the navigation for the two formation frames. It can be observed that the Frenet formation has parallel trajectories w.r.t. the leader's trajectory, however the geometric shape changes along the path (dashed lines). The Cartesian formation keeps the geometric shape along the path, but its trajectory has a variable deviation from the leader's trajectory. The velocities of the UGVs are shown in Fig. 5.5(b). It can be noted that the linear and angular velocities of the Cartesian Formation (continuous lines) are greater than the Frenet formation (dashed lines), mainly during curves, because the velocities of the virtual targets of the Cartesian formation are increased to keep the geometric shape along the leader's trajectory (5.3). Moreover, the angular velocities of the Frenet formation are equal to the leader with an offset. It happens when the leader velocity is constant, thus, the angular velocities of the virtual targets of the Frenet formation depends only on radius of curvature of the leader at h f i along the leader trajectory (5.8).

5.2.3.1/ CARTESIAN FORMATION ANALYSIS

To analyze the dynamic of the Cartesian formation in function of the leader dynamic, three different formation shapes F = F 1 , F 2 , F 3 are used for three different leader velocities v = 1.0, 1.5, 2.0 m/s, where virtual target velocities depends on the formation dimension (5.3), thus, the virtual target velocities of F 2 and F 3 are greater than F 1 . The followers of F 2 and F 3 apply greater velocities commands (less than v max ) to keep the desired formation. All followers of the formation converges to F for low velocities. Nevertheless, Fig. 5.7(c) (v L = 2.0 m/s) shows the case where followers' velocity of F 2 and F 3 are saturated (virtual target velocity greater than v max ), thus, followers can not reach the desired formation F 2 and F 3 . The values of P d and Dn max of F 2 and F 3 are greater than F 1 .

F 1 = {f 1 (-2, -1.5), f 2 (-2, 1.5)} m, F 2 = {f 1 (-4, -3), f 2 (-4, 3)} m
Table 5.1 shows the values of the L 2 (cf. Appendix F) norm applied to the evolution of P d and Dn max . This table quantifies the analysis described above of This table confirm that a steady formation shape depends on the formation dimension and the leader's dynamic.

L 2 (P d ) [m] L 2 (Dn max ) [m] v[m/s] 1.0 1.5 2.0 1.0 1.

5.2.3.2/ FRENET FORMATION ANALYSIS

This subsection analyzes the dynamic of the Frenet formation in function of the leader trajectory. At this aim, three different formation shapes F = F 1 , F 2 , F 3 are used for three different leader velocities v = 1.0, 1.5, 2.0 m/s, where

F 1 = {f 1 (-2, -1.5), f 2 (-2, 1.5)} m, F 2 = {f 1 (-2, -3), f 2 (-2, 3)} m and F 3 = {f 1 (-2, -4.5), f 2 (-2, 4.5)} m.
The group of UGVs (leader and followers) performs the same scenario as in Subsection 5.2.3.1 (cf. Fig. 5.8). The distance and orientation errors of each follower w.r.t. its target are used as criteria to quantify the performance of the formation F such as:

d rms = 1 N N i=1 d 2 i
(5.9)

e θ rms = 1 N N i=1 e 2 θ i (5. 10
)
where N is the number of UGVs. shows that the UGV followers (as in the case of Cartesian formation for F 2 and F 3 ) have saturated velocities since the virtual target's velocity is greater than v max . Then, the followers can not reach their assigned targets. The values of d rms and e θ rms of F 2 and F 3 are greater than F 1 . Table 5.2 quantify the analysis of Fig. 5.9(a), 5.9(b) and 5.9(c) using the L 2 (cf. Appendix F) norm applied to the evolution of d rms and e θ rms .

As expected from Cartesian formation analysis (cf. Subsection 5.2.

L 2 (d rms ) [m] L 2 (e θ rms ) [ • ] v[m/s] 1.0 1.

5.3/ FORMATION CONSTRAINTS

Different applications require that the group of UGVs have to accurately keep the formation shape during the navigation (e.g. transportation and reposition of heavy objects [START_REF] Chaimowicz | A mechanism for dynamic coordination of multiple robots[END_REF], exploration [START_REF] Lozenguez | Map partitioning to approximate an exploration strategy in mobile robotics[END_REF], platooning of military vehicle [START_REF] Murray | Recent research in cooperative control of multivehicle systems[END_REF], etc.). At this aim, the dynamic of leader must be constrained in order to allow each UGV to always converges to its assigned target. This section analyzes the dynamic of the formation according to UGVs' constraints.

In Section 2.3, several works presented interesting solutions for formation control problem. Nonetheless, they have not analyzed the constraints of the formation shape and the capabilities to be scaled to large vehicles (which need large space for navigation and obstacle avoidance). In [START_REF] Lewis | High precision formation control of mobile robots using virtual structures[END_REF], the authors present an algorithm to minimize the deformation of the rigid structure for rotational and translational displacement of the rigid structure outside its reachable area. In [START_REF] Gil | Decentralized strategy for car-like robot formations[END_REF], optimal trajectories were used to guide the followers according to the leader's trajectory. However, the proposed method requires an appropriate optimal trajectory planning which needs high computation for environments with large number of obstacles. In [START_REF] Consolini | Leader-follower formation control of nonholonomic mobile robots with input constraints[END_REF], the authors analyze the geometry of the formation to impose a bound on the curvature of the leader's trajectory. The formation control using input constraints is designed to adapt the formation geometry to the follower dynamic (the follower positions vary in suitable cones) to obtain lower control effort w.r.t. approaches based on rigid formation. However, in this work, the distortion of formation shape is not quantified. A criteria to evaluate the formation shape based on Procrustes distance was proposed in [START_REF] Ze-Su | Formation control and obstacle avoidance for multiple robots subject to wheel-slip[END_REF]. The authors use a virtual Leader-follower approach with artificial potential fields and the dynamic model of the robot. In [START_REF] Benzerrouk | Obstacle avoidance controller generating attainable set-points for the navigation of multi-robot system[END_REF], attainable set-points for obstacle avoidance behavior are generated according to the kinematic constraints of a unicycle robot and the radius of the limit-cycle (cf. Section 3.2). Nevertheless, the authors did not consider neither the dimension of the formation nor the steady convergence toward the desired formation geometric shape.

In this section is proposed an adaptive computation of the leader's constraints (its maximum linear velocity and steering angle (or radius of curvature)) to obtain the dynamic of each virtual target (thus the set-points for the followers) which satisfy the vehicles' constraints of the overall formation. The adaptive leader's constraints are related to its dynamic and the geometrical shape of the formation. These adaptive constraints allow to improve the convergence of the formation shape and to keep it during the multi-robot navigation (cf. Section 5.2.3).

5.3.1/ CONSTRAINTS IN CARTESIAN FORMATION

The local frame of the leader allows to keep a constant geometric structure during the navigation of the group of UGVs (cf. Section 5.2.3). The dynamic of this geometric structure is subordinated to the dynamic of the leader (cf. eq. ( 5.1), (5.3) and (5.4)). It is advantageous because the followers only require leader's configuration to determine its desired position in the formation [START_REF] Chen | Smooth formation navigation of multiple mobile robots for avoiding moving obstacles[END_REF]. Nevertheless, the dynamic of the leader has to be more constrained than the followers according to the formation shape to satisfy the followers' dynamic constraints [START_REF] Consolini | On the control of a leader-follower formation of nonholonomic mobile robots[END_REF][START_REF] Consolini | Leader-follower formation control of nonholonomic mobile robots with input constraints[END_REF], i.e., the dynamic of the virtual target can be greater than its maximum values (5.3) and (5.4) according to the dimension of the formation and the leader dynamics [START_REF] Vilca | Constrained leader-follower navigation of vehicles using adaptive dynamic of the leader[END_REF].

A homogeneous system is considered, i.e., all UGVs have the same physical constraints. The UGVs are modeled as a tricycle, then, the linear velocity and steering angle of the followers are constrained by v min , v max and γ max . Hence, they must satisfy: (5.11)

v min ≤ |v c T i | ≤ v max
|γ c T i | ≤ γ max (5.
12)

The dynamic of the target according to the leader dynamic, eq. ( 5.3) and (5.4), was given in subsection 5.2.1. Therefore, leader constraints such as velocity and steering angle can be defined as functions of the UGV constraints eq. ( 5.11) and (5.12) and the formation shape.

The steering angle is directly related to the curvature of the UGV c c = 1/r c = tan(γ)/l b . Therefore, the steering angle constraint eq. (5.12) can be written as curvature constraint c c max , moreover, ω L = v L c c L . This representation in function of c c L is useful for the following computation. To simplify the notation, let us introduce:

A(c c L ) = A c L = (1 -l c i c c L ) 2 + (h c i c c L ) 2
(5.13) By using eq. ( 5.3) and (5.13) in the velocity constraint (5.11), we obtain:

v min ≤ v L A 1/2 c L ≤ v max (5.14)
The steering angle's constraint can be written in function of curvature c c max . Using eq. ( 5.3) and (5.4) to compute the curvature of the followers c c T i = ω T i /v T i with eq. (5.5) and (5.13), we obtain: (5.15) where | • | is the absolute value of the expression.

c c L A 1/2 c L + h c i ċc L v L A 3/2 c L ≤ c c max
An important term for the constraints is ċc L . For a tricycle, it is related to the velocity of the steering angle of the leader and it is given by:

ċc L = γL sec 2 (γ L )/l b (5.16)
where γ L is the steering angle of the leader and sec is the secant function.

Analyzing eq. (5.14) and (5.15), it is observed that the limits of (5.13) allows to obtain the leader constraints. The first derivative of A(c c L ) was computed to obtain the minimum value of (5.13):

h 2 m / h 2 m + l 2 m = A c L min ≤ A(c c L ) ≤ A(c c L max ) (5.17)
where f m = (h m , l m ) T is the coordinates of the farthest node w.r.t. the instantaneous center of rotation determined by the leader Icc L . The limits of ċc L is given by: ċc L ≤ ċc L max = γLmax sec 2 (γ Lmax )/l b (5.18) where γ Lmax is the maximum steering angle of the leader.

Using eq. ( 5.17) and (5.18) and applying the triangle inequality in eq. (5.14) and (5.15), it is obtained:

v Lmax A 1/2 (c c L max ) < v max (5.19) c c L max A 1/2 c L min + h m ċc L max v Lmin A 3/2 c L min
< c c max (5.20) where v Lmin = v min /A 1/2 c L min . Finally, leader's constraints v Lmax and c c Lmax which respect to all the followers' physical constraints are obtained while solving numerically two inequalities eq. ( 5. [START_REF] Payton | An architecture for reflexive autonomous vehicle control[END_REF]) and (5.20).

Nevertheless, these fixed leader's constraints can reduce drastically the dynamic of the leader along its navigation in formation (velocity and steering angle close to their minimum values) and thus the dynamic of the formation. At this aim, the minimum and maximum velocity and maximum curvature (related to maximum steering angle) of the leader are adapted according to its instantaneous values of velocity and curvature. The proposed adaptive constraints (velocity and steering angle using the dynamic of the leader) permit to enhance the trajectory's smoothness and the convergence toward the desired formation and to keep it. The adaptive constraints of the Leader are given by: v Lmin =v L (5.21)

v Lmax =v max A -1/2 c L (5.22) c c L max =c c max A 1/2 c L min -A 1/2 c L min h m ċc L v Lmin A 3/2 c L (5.23)
where A(c c L ) and ċc L are the instantaneous values according to the current c c L of the leader. These adaptive constraints are obtained from eq. ( 5. [START_REF] Payton | An architecture for reflexive autonomous vehicle control[END_REF]) and (5.20) to obtain the maximum values (velocity and steering angle) according to the leader curvature (5.13).

The adaptation of v Lmin and the instantaneous values of ċc L allow us to increase the limits of γ Lmax while decreasing the second term of eq. (5.20). Furthermore, c c L contributes to increase the limits of v Lmax , e.g., when the leader is moving in straight line, the formation can navigate with v max .

5.3.2/ CONSTRAINTS IN FRENET FORMATION

The Frenet frame allows each UGV to navigate according to a reference trajectory (segment or complete) while keeping a curvilinear formation (cf. Subsection 5.2.2). This formation definition focuses on the tracking of a reference trajectory (or parallel to it) by UGVs to obtain safe navigation of the formation along the road. Nonetheless, the geometric formation shape changes according to the reference path (cf. Section 5.2.3). The leader should start before its movement to communicate its trajectory to other UGVs. Furthermore, the leader can also track a reference trajectory which is known beforehand by all UGVs. The main applications can be founded in urban transport systems [START_REF] Avanzini | Urban vehicle platoon using monocular vision: Scale factor estimation[END_REF], autonomous agricultural convoy [START_REF] Lenain | Adaptive and predictive path tracking control for offroad mobile robots[END_REF], exploration [START_REF] Klan Čar | A control strategy for platoons of differential drive wheeled mobile robot[END_REF]. Some drawback can be found:

• The formation shape is distorted according to the leader trajectory.

• The leader's trajectory must be known in advance by the followers.

The dynamic of the Frenet formation depends mainly on the reference trajectory (leader's trajectory) eq. (5.6), (5.7) and (5.8). This trajectory is represented in Frenet frame (curvilinear distance and radius of curvature), therefore, the followers' constraints are given by v max and r c min . The velocities and radius of curvature of the followers are constrained by

v min ≤ |v f T i | ≤ v max (5.24) |ω f T i | ≤ ω max (5.25) r c min ≤ v f T i ω f T i ≤ ∞ (5.26)
Eq. (5.24), (5.25) and (5.26) can be expressed in function of leader's velocities using eq.

(5.7) and (5.8) as follows:

v min ≤ |v L -l f i ω L (h f i )| ≤ v max (5.27) 0 ≤ |ω L (h i )| ≤ ω max (5.28) r c min ≤ | v L -l f i ω L (h i ) ω L (h i ) | ≤ ∞ (5.29)
The leader's linear velocity is obtained from eq. (5.27) and (5.28) as: (5.30) where f m = (h m , l m ) T is the coordinates of the farthest node and the minimum function (min) allows to saturate the leader's velocity with the minimum centripetal velocity (ω max r c min ). Using eq. (5.8) and (5.29), we obtain:

v min + |l m ω max | < |v L | < min v max -|l m ω max |, ω max r c min
r c min <|r c L (h m ) -l m | r c min + |l m | <|r c L (h m )| (5.31)
It can be noted that the linear velocity v L and the radius of curvature r c L of the leader are constrained mainly according to the lateral coordinate of the formation l m (eq. (5.30) and (5.31)). The linear velocity at h f i (curvilinear distance) and l f i = 0 is equal to the current linear velocity of the leader which allows to keep constant the curvilinear distance (cf. Fig.

5.5(b)).

Finally, solving eq. (5.30) and (5.31), the leader constraints of v Lmax and r c Lmin are obtained.

v Lmax =ω max r c min (5.32)

r c Lmin =r c min + |l m | (5.33)
These fixed leader's constraints can reduce drastically the dynamic of the leader along navigation in formation (velocity and radius of curvature close to their minimum values) and thus the dynamic of the formation. To address this issue, the adaptive constraints, velocity and radius of curvature using the dynamic of the leader, are proposed. They permit to improve the convergence toward the desired formation and to keep it. The proposed adaptive constraints of the Leader are given by:

r c Lmin = min |r c L + l f i | (5.34) r c Lmax = max |r c L + l f i | (5.35) v Lmax =v max min r c L /r c Lmax , 1 (5.36)
where r c L is the instantaneous value of radius of the curvature of the leader. These adaptive constraints are obtained from eq. ( 5.32) and (5.33) to obtain the maximum and minimum values (velocity and radius of curvature, respectively) according to the current radius of curvature of the leader. The minimum function in eq. ( 5.36) allows to saturate the leader's velocity to the maximum vehicle's velocity v max . In eq. ( 5.34), the minimum function selection the minimum radius of curvature among all UGVs. Furthermore, r c L contributes to decrease the limits of v Lmax , e.g., when the leader is moving in straight line and the followers are in a curve, the leader can navigate with v max , however, the followers can not to navigate with this maximum velocity. Therefore, the leader has to limit its velocity to permit the followers to navigate in a curve with suitable velocities.

5.3.3/ SIMULATION ANALYSIS OF FORMATION'S CONSTRAINTS

This subsection shows the application of the proposed dynamic constraints (cf. Section This simulation shows the proposed adaptive constraints applied to Cartesian frame. Fig. 5.10 shows the trajectories of the UGVs using the proposed adaptive leader's constraints Table 5.3 shows the values of the L 2 (cf. Appendix F) norm applied to the evolution of P d and Dn max . This table demonstrates that the use of the proposed adaptive constraints allows to keep formation shape during the navigation even when for fast leader's dynamics (e.g., Table 5.1 shows the navigation without using the adaptive constraints). The trajectories of the UGVs using the proposed adaptive leader's constraints (rely on kinematic vehicles' constraints v max and r c min ) are shown in Fig. 5.12. It can be noted that the followers perform smooth trajectories and track accurately their assigned targets. The velocity and steering angle of the UGVs using the proposed adaptive constraints for Frenet formation are shown in Fig. 5.13(a). It can be observed that the velocities and steering angle of the followers comply with the kinematic vehicles' constraints (v max and γ max ) while the leader satisfies the proposed adaptive constraints (v Lmax , r c Lmin ). This subsection presents simulations in COBAYE simulator (cf. Subsection G.2.1) and experiments with urban electric cars (VIPALABs) using the proposed control architecture for navigation in formation (cf. Fig. 5.3 and 5.4). One UGV of the group of VIPALABs is considered as the leader, i.e., the formation will be defined according to its dynamic. The leader of the group of VIPALABs has to reach successively static set of waypoints (cf. Chapter 4) and the followers has to keep the desired formation while avoiding the hinder obstacles. The Leader's configuration is sent by itself to each follower via WI-FI. At this aim, each follower tracks its assigned dynamic virtual target (cf. Fig. 5.17(a) and 5.17(b)) applying the proposed control law to the multi-robot system.

L 2 (P d ) [m] L 2 (Dn max ) [m] v[m/
In these simulations and experiments, the used UGVs' parameters, constraints, sensors and controller parameters were described in Section 3.6. Moreover, the communication system by Wi-FI between UGV is stable.

Firstly, this simulation focuses on the performance of the proposed control architecture for navigation in Cartesian formation in cluttered environment (cf. Fig. 5.14(a)-(a)). This simulation can be found online 1 The formation has a triangular shape F c = (-4, 2.5) T , (-4, -2.5) T , (-4, 0) T m. The initial positions of the vehicles have an offset (Δx, Δy) = (-1.0, 0.0) m from the initial position of their assigned virtual targets. In Fig. 5.14(a), the red line represent leader's trajectory and the big orange points are the set of waypoints. When the leader detects an obstacle with adequate range, then the whole formation will avoid it while keeping the desired shape using the limit-cycle method (limit-cycle is increased by R f = 2.5 m to allow a safe navigation (cf. Subsection 3.2)) (cf. The desired formation has a linear shape F f = (-5, 0) T , (-10, 0) T . The initial positions of the followers have an offset (Δx, Δy) = (-0.5, 0.5) m from the initial position of their assigned virtual targets.

Some screenshots of the developed Graphical Data Interface for VIPALAB (GDI-VIPA) of the followers are shown in Fig. 5.17(a) and 5.17 trajectory, the red point is the current leader's pose and the blue and green points are respectively the current virtual target to be tracked by follower 1 and follower 2. The Leader's analysis (navigation through waypoints and reactive obstacle avoidance) was described in Section 4.4.2.

Fig. 5.17(c) shows that the leader reaches accurately the successive static waypoints and the followers track accurately also their dynamic targets (w.r.t. leader). Moreover, the followers' trajectories using the proposed control law is close to the leader's trajectory (cf. Fig. 5.17(c)). The Leader detects the hinder obstacles between the waypoints and it applies the reactive limit-cycle method (cf. Subsection 4.4.2). The Followers avoid also the obstacle since it tracks accurately the leader's trajectory. It can be noted that the followers do not collide with the obstacles and they keep the desired Frenet formation. shows the distance and orientation errors of each follower w.r.t. its assigned virtual target. It can be observed that each follower tracks accurately its assigned virtual target. Nevertheless, a delay in the vehicle's communication at the end of the leader trajectory increases the error values. Therefore, the proposed navigation in Frenet formation allows safe, flexible and smooth movements for a group of UGVs in structured environment.

These simulations and experiments showed applications of two presented formation frames, Cartesian and Frenet. The Cartesian frame allows an accurate navigation of a group of UGVs while keeping a rigid shape in cluttered environments, nevertheless this rigid shape is very sensible to this dynamic of the leader and to the dimension of the formation. The Frenet formation frame is more suitable for structured environments since it allows safe movements based on the leader's trajectory, nonetheless, this dependency on the leader's trajectory can generate errors since the followers has to compute its virtual target based on the leader pose which can accumulate localization errors and communication delays.

These results demonstrate the feasibility of the proposed strategy for navigation in formation, in the sequel, the problem of dynamic reconfiguration of the fleet is dealt. A typical example of application of formation reconfiguration is when the formation detects a narrow tight corridor, therefore the formation has to adapt to the corridor dimension to continue the navigation. The challenge consists in what follows to guarantee the stability and the safety of the multi-vehicle system at the time of transitions between configurations (e.g., line towards square, triangle towards line, etc.) [5]. The inter-vehicles collisions are avoided during the SFR using a penalty function acting on the vehicle velocities (cf. Appendix B.3). Different algorithms optimizing target assignment can be easily integrated in the proposed control architecture (cf. Fig. 5.2) [START_REF] Benzerrouk | Altruistic distributed target allocation for stable navigation in formation of multi-robot system[END_REF][START_REF] Ze-Su | Formation control and obstacle avoidance for multiple robots subject to wheel-slip[END_REF].

5.4/ FORMATION RECONFIGURATION

In this thesis, the allocation of virtual targets to UGVs is achieved using elementary rules when a formation reconfiguration is required (cf. Section 5.4.3). These rules assign a label H i of the virtual target T d i to the UGV i at the beginning of the experiments. This label is kept by each UGV along the reconfiguration process (cf. Fig. 5.20).

3.2).

In this section, two strategies for formation reconfiguration are proposed. The first is a reactive strategy based on smooth switches between initial and final positions of targets, each UGV has to avoid the collision between UGVs. The second is a centralized extension of the first strategy while considering the distance between targets to change smoothly the targets from their initial to final poses.

5.4.1/ STRATEGY FOR FORMATION RECONFIGURATION BASED ON SMOOTH SWITCHES (SFR-S)

This reactive strategy for formation reconfiguration is based on suitable smooth switching of the virtual target configurations [5]. The new virtual targets defined for the new formation shape must be ahead to the UGVs to guarantee the stability of the overall system (the vehicle must not go back to reach the new virtual target). If this condition is not satisfied then the former formation will be adapted by increasing smoothly and contentiously the longitudinal coordinates h i (cf. Fig. 5.3 and 5.4) until that all UGVs will be positioned in the right configuration (5.38). The error between the coordinates of the initial and new formations e f i (e h i , e l i ) is defined as:

e i f i = f n i -f i i (5.37)
where

f i i (h i i , l i i ) and f n i (h n i , l n i )
are respectively the coordinates of the initial and new desired formations (cf. Fig. 5.3 and 5.20).

The reconfiguration process between the different formation shapes is given by:

f i = h i = h n i -e i h i e -k r (t-t r ) , l i = l n i ; if e h i < 0 h i = h n i , l i = l n i ; if e h i ≥ 0 (5.38)
where f i (h i , l i ) are the coordinates of the current virtual target T d i to be tracked by the follower UGV i . e h i is the longitudinal coordinate of e f i that allows to detect if the virtual target is ahead to its respective follower (e h i ≥ 0). The adaptation function when e h i < 0 (virtual target behind to follower i ) is set as proportional to the error between formation shapes, where k r is a real positive constant designed according to the dynamic of the leader and t r > 0 is the initial time for the reconfiguration process. The exponential function guarantee that the longitudinal coordinate converges to the new value h n i .

Figure 5.20: Formation reconfiguration between, for instance, triangular and linear formation shapes.

5.4.2/ STRATEGY FOR FORMATION RECONFIGURATION BASED ON INTER-TARGET DISTANCES (SFR-D)

The proposed strategy for formation reconfiguration is an extension of the SFR-S using a continuous functions instead of smooth switches. It is based on the Procrustes distance (cf. Appendix F) which uses the distance errors between the vertex of the initial and new formation (cf. Fig. 5.21). As described in the last subsection, the formation error of a vertex e f i is defined by eq. (5.37).

The proposed reconfiguration strategy analyzes the derivative of e f i to guarantee the convergence to the new formation shape and smooth trajectories of the virtual target during the reconfiguration process. Moreover, the proposed function has the following form:

ė f i = g e f 1 , . . . , e f 1 , . . . , e f N (5.39)
where ė f i takes into account formation errors of all N virtual targets of the formation. It allows to manage the minimum inter-target distance to avoid collision between the followers. The proposed function ė f of the whole formation is designed as a linear system:

ė f = Ae f (5.40)
where e f = [e f 1 , . . . , e f N ] T and A is a symmetric negative definite matrix, it has the following form: where a i with i = 1, . . . , N is related to the speed of convergence of the formation parameter e f i , and a i j with i j is related to the inter-target distances between T d i and T d j . The values of the matrix must be designed while taking into account the kinematic constraints of the UGVs to allows each follower to track accurately its assigned target. The stability of formation error system can be straightforwards proved using Lyapunov analysis (cf. Appendix E.2) with the following Lyapunov candidate function:

A = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ a 1 a 12 . . .
V = 1 2 e T f e f (5.42)
V is a positive-definite function. To guarantee the stability of the system, V must be negative-definite. By taking the derivative of eq. 5.42 and using (5.40), V can be written:

V =e T f ė f e T f Ae f (5.43)
Since A is negative-definite function, then V < 0 and the formation error system has an asymptotically convergence.

To avoid the collision between targets, the elements of matrix A must be designed according to the desired minimum distance between targets d T min . For simplicity, the case of two targets is analyzed. The inter-target error e f 12 is defined as:

e f 12 =f 1 -f 2 = -f n 1 + f 1 + f n 2 -f 2 + f n 1 -f n 2 = -e f 1 + e f 2 + e n f 12 (5.44)
Furthermore, the inter-target distance can be computed by:

d 2 T = e T f 12 e f 12 (5.45)
Taking the derivative of eq. (5.45) to obtain its minimum value:

d(d 2 T ) dt =0 d(e T f 12 e f 12 ) dt = 2e T f 12 ė f 12 =0 (5.46)
Eq. (5.46) can be expressed using the derivative of eq. (5.44) with eq. (5.40) as follows: Analyzing the left side of eq. (5.50), the value of m 12 has to be chosen to produce

e T f 12 ė f 12 =0 e T f 12 ė f 1 -ė f 2 = e T f 12 (a 1 + a 12 )e f 1 + (a 12 -a 2 )e f 2 =0 (
m 12 e f 1 + e n f 12
) ≥ d T min to satisfy the minimum inter-target distance. Furthermore, the values of a 1 and a 2 have to be chosen according to maximum UGV's velocity a i v max since ė f i is related to the variation f i = (h i , l i ) (cf. eq. (5.37)) which are used for the computation of the virtual target's velocity (cf. eq. ( 5.3) and (5.7)). The extension for more than two targets follows the same process while taking into account the errors between other targets, the value of m i j determines significantly the minimum distance between the targets T d i and T d j .

5.4.3/ VALIDATION OF THE DYNAMIC RECONFIGURATION OF FORMATION

This section shows the navigation of a group of UGVs in a cluttered environment using the proposed strategies for reconfiguration (SFR-S and SFR-D) in simulations and experiments.

In these simulations and experiments, the used UGVs' parameters, constraints, sensors, communication and controller parameters were described in Section 3.6 and 5.3.4. The initial positions of the vehicles have an offset (Δx, Δy) = (1, 0.5) m from the initial positions of their assigned virtual targets.

5.4.3.1/ SFR-S RESULTS

The simulation given in Fig. 5.22 to 5.23 focuses on the proposed reconfiguration method based on suitable switches between two Cartesian formation shapes (triangular and linear shapes) while navigating in a cluttered environment. This simulation can be found online 3 .

The initial formation coordinates are defined by

F i = (f i 1 , f i 2 ), with f i 1 = (-4, -2) T m and f i 2 = (-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 environment, the leader (and thus the formation) must go toward it while avoiding hinder obstacles. The new targeted formation is defined as straight line with the following coordinates

F n = (f n 1 , f n 2 ), with f n 1 = (-6, 0) T m and f n 2 = (-3, 0) T m.
The value of k r (cf. eq. 5.38) is set to 1 for both follower in the reconfiguration process (when a hinder obstacle is detected).

At the beginning of the simulation (cf. Fig. 5.22), the navigation of the group of UGVs is in triangular formation F i . When the leader detects an obstacle with adequate range to allow the formation reconfiguration, then the leader avoids the obstacle using the limit-cycle method (limit-cycle is increased by R f = 2 m to allow a safe navigation (cf. Subsection 3.2)) and sends the new desired formation F n to the other UGVs (followers) to modify the configuration of the formation. The formation returns to triangular shape F, when It can be noted in Fig. 5.22 that the vehicles trajectories are smooth along the navigation and there is not neither collisions with the obstacles nor inter-vehicle collisions. Fig. 5. 23(a) 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. Subsection 5.4.1). Moreover, some small peaks can be observed, they 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 is increased). Fig. 5. 23(b) shows the distance between each UGV of the formation. This last figure shows clearly the non-collision between the vehicles, i.e., d i j > R int 12 (cf. Appendix B.3). The figures show some peaks which are related to the adaptation and reconfiguration phase between formations. Fig. 5.23(c) shows the evolution of the formation coordinates (h i , l i ) (virtual target positions). It can be observed that adaptation phase of h i when the follower is always ahead of its new assigned virtual target (5.38). This attests on the efficiency of the strategy for formation reconfiguration.

The proposed control architecture allows thus to adapt the formation according to the environment context.

5.4.3.2/ SFR-D RESULTS

The objective of this experiment is to validate the proposed control architecture for navigation in formation and the proposed strategy for formation reconfiguration based on inter-target distance between two Frenet formation configurations (triangular and linear shapes) while navigating in a cluttered environment. This experiment has been done while using three VIPALABs (cf. Fig. 5.24). This experiment can be found online 4 .

The initial formation coordinates are defined by

F i = f i 1 , f i 2 , with f i 1 = (-5, - 3
) T m and f i 2 = (-5, 3) T m (triangular shape). Therefore, the group of UGVs must keep the Frenet formation while moving in a cluttered environment. A set of waypoints is defined in the environment, the leader (and thus the formation) must go toward them while avoiding hinder obstacles. The new targeted formation is defined as straight line with the following coordinates F n = f n 1 , f n 2 , with f n 1 = (-5, 0) T m and f n 2 = (-10, 0) T m. The value of the matrix

5.5/ CONCLUSION

This chapter presented a global control architecture to cope with the navigation in formation for a group of UGV in cluttered and structured environments. Two frame definitions (Cartesian and Frenet) for the formation, its advantages and drawbacks were analyzed and integrated to the proposed global control architecture. In the Cartesian formation, the leader's reference path is not taken into account such as in Frenet formation, only its current pose and dynamic has to be known by the followers. The Cartesian formation allows a stable geometric formation shape. The Frenet formation allows safe formation navigation since the formation shape is adapted according to the leader's trajectory which is safe in static environments. Furthermore, this chapter focused on the analysis to ensure the formation reliability and stability. At this aim, the adaptive constraints, taken by the leader (according to the desired formation shape and its current dynamic) have been proposed to improve the performance of the formation (fast and steady convergence).

To deal with the dynamic reconfiguration of the fleet of UGVs, a fully reactive strategies between the UGVs based on suitable smooth switches of the virtual target configurations and inter-target distances were proposed. This strategy avoids the use of predefined trajectories and it can be applied for different situations when the formation has to be modified according to the environment context (dynamic, cluttered, etc.). Furthermore, these strategies 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 simulations and experiments using UGVs have shown the reliability, efficiency and flexibility of the proposed strategies for multi-vehicle navigation and dynamic reconfiguration.

GENERAL CONCLUSION AND FUTURE WORKS GENERAL CONCLUSION

This presented thesis deals with the autonomous navigation of a single and a group of UGVs in formation. The considered navigation task is dedicated to urban transportation system where the UGV has to transport passengers through specific environments. At this aim, the proposed control architectures and methods along this manuscript were developed to guarantee safe, flexible and reliable navigation of a single UGV as well as a group of them with dynamic reconfiguration of the fleet in complex environments (e.g., unknown, cluttered and dynamic areas). Moreover, these proposals can be also extended to different applications of mobile robots from the literature such as exploration, coverage, agriculture, storage, etc.

The proposed control architectures are based on hybrid architecture (central module leading the whole system and elementary reactive behaviors) and Centralized/Decentralized MRS architecture (central entity allows to manage the different tasks of all entities and each entity perform its assigned task using its local sensors). The elements of the proposed control architectures are organized in three layers: Planning (selection of waypoints), Executive (behavior selection, target assignment and formation set-points) and Behaviors (target reaching and obstacle avoidance). The developed methods in each element/block were designed to obtain a generic, reliable and flexible control architecture and online and easy implementation for autonomous navigation of a single or a group of UGVs in unforeseen situations (obstacles) and/or well-know environments (planning).

Several simulations and experiments allow to validate the online application of the proposed control architectures for the navigation of a single and a group of VIPALABs. The main contributions of the thesis can be categorized according the following items:

1. Reliable control architectures for single and multi-vehicle systems.

2.

Stable control law for target reaching.

3.

Flexible navigation through successive waypoints.

4.

Efficient planning for optimal selection of waypoints.

5.

Suitable coordination of multi-vehicle navigation in formation.

1/ RELIABLE CONTROL ARCHITECTURES FOR SINGLE AND MULTI-VEHICLE SYS-

TEMS

The proposed multi-controller architectures have been designed to obtain a distributed and reactive control of each UGV. They break the complexity of the overall tasks (single and multi UGVs' navigations) into a group of reliable and feasible behaviors/controllers. The elementary controllers are Obstacle avoidance and Target reaching. The obstacle avoidance is based on elliptical limit-cycle approach since this approach allows a reactive behavior while using only local information (cf. Section 3.2). This online method uses the local information from range sensors (LIDAR) (Perception block (cf. Section 3.4)). Therefore, online, accurate and stable ellipses which surround the obstacle are obtained to apply the limit-cycle approach for obstacle avoidance. The Target reaching allows to guarantee a safe UGV's navigation towards one or more successive (static or dynamic) goals (cf. Section 4.2). The autonomous navigation in cluttered and structured environments is obtained combining these last behaviors using a suitable multi-controller coordination (selection of one controller according to presence of obstacles (cf. Section 3.3.5)).

2/ STABLE CONTROL LAW FOR TARGET REACHING

A single control law for both behaviors (Obstacle avoidance and Target reaching) in the control architecture has been proposed. These last behaviors have a homogeneous error definition (e x , e y , e θ , v T ) to be zeroed by the control law (cf. Section 3.3). This control law, in addition to position and orientation, introduces a new parameter related to the orientation of the line that connects the UGV and its target. This new parameter allows to perform static and dynamic target reaching. Therefore, the proposed control law has a satisfactory performance w.r.t. the other control laws (based on path following and trajectory tracking). The stability of this control law is proved using Lyapunov analysis. The feasible target reaching is guaranteed using the relation between the controller parameters K and the attainable maximal errors (E dis , E ∠ ) (cf. Section 3.5.2).

3/ FLEXIBLE NAVIGATION THROUGH SUCCESSIVE WAYPOINTS

A novel strategy to drive the vehicles through successive waypoints suitably placed in the environment have been presented. This strategy was inspired by human driver behavior where the driver reactively guides his vehicle through specific roads, round-about, etc. without the use of any predefined trajectory. This strategy is combined with the proposed control law which guarantees the smooth and feasible UGV's trajectories using notably successive waypoints. The strategy is flexible in the sense that the UGV can perform different behaviors between to waypoints such as obstacle avoidance.

4/ EFFICIENT PLANNING FOR OPTIMAL SELECTION OF WAYPOINTS

To perform the navigation based on successive waypoints (cf. Section 4.2) three methods for optimal selection of waypoints (number, poses, velocities, etc.) have been proposed. The first method is applied when a specific reference trajectory have been predefined. The proposed strategy based on Leader-follower consists in the definition of virtual targets and their configurations are linked to the leader's configuration (cf. Section 5.2). This strategy allows a distributed control since each follower tracks its assigned target while respecting the vehicle's constraints (cf. Section 5.3). The virtual targets can be defined according to current leader's pose (Cartesian frame (rigid geometric shape)) or leader's trajectory (Frenet frame (parallel trajectories to the leader)). Furthermore, the proposed strategies for formation reconfiguration based on smooth switches and movements of the virtual targets allow to each follower to converge towards its dynamic virtual target while maintaining safe inter-vehicle distances (cf. Section 5.4). The stability of the reconfiguration process has been proved using Lyapunov analysis.

FUTURE WORKS

This thesis allows to explore and to extend several research fields in autonomous navigation of a single UGV and a group of them. The most important future works are described below.

1/ RELIABLE CONTROL ARCHITECTURES FOR SINGLE AND MULTI-VEHICLE SYS-

TEMS

An important study will be the stability of the whole control architecture during the switching phase (between the Target-reaching and obstacle avoidance behaviors and between successive waypoints or obstacles). Moreover, the robustness of the MRS system will be improved while considering leadership change and fault detections. The Perception block for online obstacle detection will be improved using camera system or 3D LIDAR. These sensors can allow to discriminate the different obstacles (3D model) or even gives a priority in the selection and the way to avoid obstacles. Furthermore, the proposed control architectures will integrate other methods/blocks based on artificial intelligence (neural networks, fuzzy logic or Markovian process) to enhance the decision process.

2/ STABLE CONTROL LAW FOR TARGET REACHING

The control law block will be extended to considers uncertainties and dynamic modeling of the UGVs. It will allow to obtain a robust control for a safe navigation. Moreover, a dynamic adaptation of the controller parameters according to adequate performance criteria will be studied to improve the performance of the control law according to the assigned task.

3/ FLEXIBLE NAVIGATION THROUGH SUCCESSIVE WAYPOINTS

The Sequential target block for navigation through successive waypoints will take into account the different traffic signals (lights, pedestrian cross, velocity limits, etc.) for navigation in urban environments. Each waypoint will integrate semantic properties to allow the vehicle to stop or to navigate by adapting its velocity.

4/ EFFICIENT PLANNING FOR OPTIMAL SELECTION OF WAYPOINTS

The proposed algorithm for optimal selection of the set of waypoints OMWS-ET (Waypoint configuration selection) will be expanded to 3-dimensional space for UAV applications. Moreover, genetic algorithm will be studied to improve the performance of OMWS-ET, mainly according to the most appropriate K parameters (cf. Section 3.5).

5/ SUITABLE COORDINATION OF MULTI-VEHICLE SYSTEM

The methods of Formation parameters block will unify Cartesian and Frenet formation to allow an automatic selection of the suitable formation according to the application or to the environment. The strategy for formation reconfiguration will consider an optimal allocation of targets to contribute to the fast convergence towards the new desired formation shape.

6/ EXPERIMENTAL WORKS

The localization system will be robust using different sensors based on camera, LIDAR, odometry, etc. to improve vehicle's localization. Moreover, a security system for inaccurate localization will be developed. The methods combining different sensors (camera and LIDAR) to improve the obstacle detection will be implemented. A robust system to deal with delay or fault in communication will be integrated to the whole control architecture. The proposed control architecture with the theoretical perspectives will be analyzed and tested for higher UGV's velocities.

A MATHEMATICAL FORMULATION OF AN ELLIPSE

This appendix describes the definitions, representations and formulations of an ellipse. The terminology used in this appendix is based mainly on [START_REF] Thomas | Calculus and Analytic Geometry[END_REF][START_REF] Loan | Introduction to Computational Science and Mathematics[END_REF].

A.1/ STANDARD REPRESENTATION An ellipse is defined as a set of points in R 2 plane whose sum of the distances from two fixed point (Focis F 1 and F 2 (F 1 F 2 )) is a constant 2a ∈ R + (cf. In what follows, the overall methodology to achieve the used obstacle avoidance algorithm [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF] will be described. The algorithm is developed according to stimuli-response principle. To implement this kind of behavior it is important to have to:

• Detection of the hinder obstacle (cf. Section 3.1),

• Assignation of the direction of the avoidance (clockwise or counter-clockwise),

• Definition of an escape criterion to specify when the obstacle is completely avoided.

All these different steps must be followed and applied while guaranteeing that: the robot's trajectory is safe, smooth and it avoids undesirable situations as deadlocks or local minima; and that the stability of the applied control law is guaranteed (cf. Subsection 3.5). The obstacle observation is obtained in real time, the robot does not have any global information about the hinder obstacle nor the ellipse that encloses it. The robot discovers thus at each sample time the shape of the obstacle and increases progressively the knowledge of the enclosing ellipse to obtain a smooth elliptical trajectory (cf. section 3.4). The global information of the obstacle is not related to the used reactive navigation. The necessary steps to carry out the obstacle avoidance algorithm (5) are given below:

1. For each sample time, obtain the distance D RO i for each potentially disturbing obstacle "i" (cf. Fig. 3.1).

2.

Among the set of disturbing obstacles (which can constrain the robot movement to reach the target), choose the closest to the robot (the smallest D RO i (cf. Fig. 3.1)). This specific obstacle has the following features: (x obst , y obst ) center position, a and b as respectively major axis and minor semi-axes and Ω as orientation w.r.t. the global reference frame. The four specific areas surrounding the obstacle to avoid [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF].

the X O axis connects the center of the obstacle (x obst , y obst ) to the center of the target. This axis is oriented towards the target, the Y O axis is perpendicular to the X O axis and it is oriented while following trigonometric convention.

• apply the reference frame change of the position robot coordinate (x, y) G (given in absolute reference frame) towards the reference frame linked to the obstacle (x, y) O . The transformation is achieved while using the following homogeneous transformation: Once all necessary perceptions are obtained, one can apply the reactive obstacle avoidance strategy given by Algorithm 5. To obtain the robot set-points, it is necessary to obtain the value of a lc and b lc (cf. Section 3.1) of the orbital ellipse and the direction "clockwise or counter-clockwise" of the limit-cycle to follow. The position (x O , y O ) gives the configuration (x, y) of the robot according to obstacle reference frame. The definition of this specific reference frame provides an accurate means to the robot to know what it must do. In fact, the sign of x O gives the kind of behavior which must be taken by the robot (attraction or repulsion).

In repulsive phase, the limit-cycle takes an increase value of a lc and b lc values to guarantee the trajectory smoothness. The sign of y O gives the right direction to avoid the obstacle. In fact, if y O ≥ 0 then apply clockwise limit-cycle direction else apply counterclockwise direction. This choice permits to optimize the length of robot trajectory to avoid obstacles. Nevertheless, this direction is forced to the direction taken just before if the obstacle avoidance controller was already active at t -δT (δT is the sample time) and it allows to avoid local minimum and dead-end [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF].

In algorithm 5, some conflicting situations which are due to local minima or dead ends have to be managed to improve the performance. These specific local and reactive rules are detailed in [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF].

Algorithm 5 Obstacle avoidance algorithm [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF].

Require: All the features of the closest constrained obstacle. Ensure: Features of the limit-cycle trajectory to follow. with ξ a small constant value as ξ δ E which guarantees that the robot do not navigate very closely to the obstacle (cf. Section 3.1). Apply the same direction already used, equation (3.4) or (3.5) is thus applied. 12: {This will permit to avoid oscillations and several conflicting situations [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF]} 13: else 14:

The limit-cycle set-point is given by: This section briefly summarize the RRT* algorithm based on [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF][START_REF] Islam | Rrt*-smart: Rapid convergence implementation of rrt* towards optimal solution[END_REF]. RRT* is an incremental sampling based algorithm which finds an initial path very quickly. The path is later optimized as the execution takes place [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF].

The objective of RRT* is to find out an input u: [0 : T ] ∈ U that yields a feasible path x(t) ∈ C-space f ree that starts from initial position x(0) to x(T ) =goal while following the UGV's constraints. While finding this solution, RRT* maintains a tree T = (V, E) of vertices V sampled from the C-space f ree and edges E that connect these vertices together. The pseudocode of RRT* is explained in Algorithm 6.

At first in the Algorithm 6, a sample x rand is placed randomly in the C-space f ree . Then, the nearest node x nearest to x rand is checked for in the entire configuration space. A node x new is placed at a distance Δx from the nearest node x nearest in the line of direction of x rand . Then, the trajectory path x new is checked if it is free of obstacles. If the trajectory is obstacle free then a ball of radius β(log(n)/n) around x new is checked for near nodes x near . Among this set of nodes, the node that gives the least cost from the starting point to x new is selected as the parent of x new . Once the parent is selected, rewiring takes place. The costs of all the nodes inside this ball around x new is calculated through x new . If this cost is less than the previous cost for any node then that particular node is disconnected from its old parent and is connected to x new as its parent [START_REF] Karaman | Sampling-based algorithms for optimal motion planning[END_REF].

RRT* is a landmark sampling based algorithm to approach an optimal solution ensuring asymptotic optimality, apart from probabilistic completeness, as opposed to its predeces-

C.1/ ITERATIVE CLOSEST POINT

This section describes the Iterative Closest Point (ICP) algorithm. ICP is a powerful algorithm for calculating the displacement between scans [START_REF] Bosse | Map matching and data association for large-scale twodimensional laser scan-based slam[END_REF]. The objective of this algorithm is to find the translation T and rotation R that minimizes the sum of squared error between two corresponding point set X = {x 1 , . . . , x n } and Y = {y 1 , . . . , y n } as follows (cf. In order to address the issue of point correspondence, the ICP algorithm iteratively performs the following steps:

1. Matching: The nearest neighbor of every data point in the model point set is found.

Minimization:

The error E is minimized with translational T and rotational R matrix.

3.

Transformation: Data points are transformed using R and T matrix. The distance thresholds D th and D stop are used to focus on the meaningful distance information. They are given according to the current UGV's velocity v as follows:

D th =D max (1 -k th ) v v max + k th (C.3) D stop =D min (k stop -1) v v max + 1 (C.4)
where k th , k stop ∈ R + with D min /D max < k th < 1 and 1 < k stop < D max /D min . These relations guarantees D th > D stop . D max and D min are respectively the maximum detection distance of the sensor and the minimum allowed distance to an obstacle before to collide with it. v max and v are respectively the maximum and current UGV's velocity. β - th and β + th are limit angles for the opening angle to focus on the meaningful information.

The loop of ICP algorithm between current reading R and R tmp allows to eliminate the noise, outliers and possible false data. R tmp is updated at each comparison for n ICP times. n ICP ∈ Z + indicates the number of reading to be used for the comparison. The ICP algorithm between R tmp and R bu f f er allows to save and update the data from R tmp in R bu f f er . shows an example of an online application of the Filtering process using a UGV with a LIDAR sensor. The UGV's pose is given by a GPS-RTK and a IMU. It is observed that the Filtering block allows to obtain an accurate information while reducing the problems of inaccuracy and noise.

C.3/ SEGMENTATION METHODS

In computer vision, segmentation is the process of partitioning a digital image into multiple segments (sets of pixels, also known as superpixels). In this field, the goal of segmentation is to simplify and/or change the representation of an image into something which is more meaningful and easier to analyze. Image segmentation is typically used to locate objects and boundaries (lines, curves, etc.) into images.

In this thesis, segmentation is a fundamental issue in processing of point cloud data acquired by range sensors. The quality of segmentation determines largely the success of information retrieval. The objective of the segmentation of point clouds from range sensors is to spatially group points with similar properties into homogeneous regions or segments (cf. Fig. C.5). For instance, the segmentation is used to detect buildings in [START_REF] Wang | Segmentation of lidar point clouds for building extraction[END_REF].

The presented segmentation method is based on [START_REF] Jiménez | Improving the obstacle detection and identification algorithms of a laserscanner-based collision avoidance system[END_REF]. This method consists in grouping points detected by range sensors which correspond to the same obstacle. This phase is usually completed by considering two consecutive points as belonging to the same obstacle if they are not separated by a distance Δd greater than a distance threshold Δd th , otherwise, one points belongs to a new obstacle (cf. where s 0 ∈ R + is a constant value used for noise reduction, and s 1 can be chosen as:

s 1 ≥ 2 -2 cos(Δβ) (C.6)
where Δβ is the angular resolution of the range sensor.

The drawback of this method is the accurate categorization of points close to each other but belonging to different objects. This appendix describes the definitions of stability applied to a dynamic system. The system is represented by the following state equation:

ẋ = f (x) (E.1)
where x ∈ R n represents the state of the system. The equilibrium point is assumed at x = 0.

• Simple stability: Let us consider that the initial time of the system is at t 0 = 0. Therefore, the origin point is stable if it satisfies the following expression:

∀ > 0, ∃ δ > 0 : |x(0)| ≤ δ ⇒ |x(t)| ≤ ∀t ≥ 0 (E.2)
• Asymptotic stability: The system is asymptotically stable if it is stable and δ can be chosen such as:

|x(0)| ≤ δ ⇒ lim t→∞ x(t) → 0 (E.3)
If eq. (E.3) is satisfied for all δ, then the system is globally asymptotically stable.

• Exponential stability: The system is exponentially stable if it satisfies: The first method of Lyapunov is based on the analysis of linearization of the f (x) system around its equilibrium point. This method consists of analyzing the eigenvalues λ i (J) of the Jacobian matrix J at its equilibrium point:

By considering the rate of change w.r.t. time of eq. (E.6), it is obtained:

V = ∂V ∂x ẋ + ∂V ∂y ẏ = -2(2A lc x s + B lc y s )V 1/2 (C lc y s + B lc 2 x s ) + x s V 1/2
-2(2C lc y s + B lc x s )V Thus, it can be noted that V decreases on the trajectories given by eq. (3.4) and (3.5) except at the origin where it clearly has a relative maximum and on the ellipse A lc x 2 s -B lc x s y s -C lc y 2 s = 1 where it is identically zero. The term A lc x 2 s + B lc x s y s + C lc y 2 s is always positive since it can be represented in function of only quadratic terms (cf. standard form of an ellipse (cf. eq. A.2) in Appendix A) Outside the ellipse the function is decreasing towards the ellipse. Inside the ellipse the function is decreasing towards the ellipse again. Thus the trajectories of the system must move towards the ellipse from points outside or inside of it for increasing t.

It can be constructed a trapping region by taking any contour inside the ellipse as the inner boundary of an annular region together with any contour outside the ellipse as the outer boundary. Hence by the Poincaré-Bendixson theorem [START_REF] Khalil | Nonlinear Systems[END_REF] since there are no equilibrium points within the annular region there must be a stable limit cycle within the region. Note also that the contours can be taken as close as possible to the ellipse, therefore, the limit cycle must be the ellipse.

F PROCRUSTES DISTANCES

The Procrustes shape distance [START_REF] Kendall | A survey of the statistical theory of shape[END_REF] is used as criteria to quantify the distortion between the desired formation F and the real formation shape [START_REF] Ze-Su | Formation control and obstacle avoidance for multiple robots subject to wheel-slip[END_REF]. Basically, the Procrustes distance P d is a least-square type shape metric that require aligned shapes with one-toone point correspondence. The alignment part involves four steps:

1. Compute the centroid of each shape.

2.

Re-scale each shape to have equal size. It is given by:

P 2 d = n i=1 (x i 1 -x i 2 ) 2 + (y i 1 -y i 2 ) 2 (F.1)
where (x i 1 , y i 1 ) and (x i 2 , y i 2 ), are respectively the aligned coordinates of the vertex i of the first and second shapes. The maximum distance between vertex Dn max is defined as:

Dn 2 max = max{(x i 1 -x i 2 ) 2 + (y i 1 -y i 2 ) 2 , i = 1, . . . , n} (F.2)
Additionally, the L 2 norm can be applied to the evolution of P d and Dn max . The norm is given by:

L 2 (x) = 1 t f -t 0 t f t 0 x 2 2 dt (F.3)
where t 0 and t f are respectively the initial and final time of the variable x. • Rear wheels odometry: Each rear wheel is equipped with an encoder in the form of a ring gear providing 64 top per wheel revolution. The data provided by wheel encoders can be used to obtain displacement up to an accuracy of 2 cm and hence the velocity can be estimated.

• Steering angle: The steering angle is obtained by reading the analog value of a potentiometer. The saturated range of ±30 • of the steering angle are digitized using a 12-bit ADC, which gives a theoretical resolution of 0.015 degrees.

• Motor odometry: This proprioceptive sensor provides data from the motor encoder related to translational velocity with an accuracy of 0.1 m/s. Translational velocity and steering angle are combined using an Ackerman model in dead reckoning (40 cm in diameter of the wheels) to obtain the VIPALAB's trajectory. All proprioceptive measurements (odometry and steering angle) are accessible through a CAN gateway at a framerate of 50Hz. • RTK-GPS: ProFlex500 RTK GPS receiver (antenna) is mounted on the top of the VIPALAB. The data acquired with GPS receiver coupled with a Sagitta Magellan GPS base station are considered as the ground truth. This localization system provides an absolute localization measurement with an accuracy within 2 cm at a frequency of 10 Hz.

• Horizontal range sensor: SICK LMS151-10100 planar range sensor is mounted horizontally (parallel to the ground plane) on the frontal facade of the VIPALAB. This LIDAR allows to scan a horizontal plane at 40 cm from the ground plane. The information (output) given by the LIDAR are distance and angle (polar coordinates (r, θ)). The characteristic of the LIDAR are range in [0, 50] m, angle in [-45 • , 225 • ], resolution 0.5 • and frequency of 50 Hz.

• Wi-Fi communication: The wireless system uses a Laguna base system (2xITS 5.9 GHz 802.11p radios). It is universal for vehicle and infrastructure functionality.

  . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.19 Control output. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.20 Simulation results of the comparative study between control laws. . . . . . . 3.21 (a) UGV's trajectories, (b) their distance d and orientation errors e θ and (c) their evolution of Lyapunov functions for several initial UGV's orientations. . 3.22 (a) UGV's trajectories and (b) its distance d and orientation errors e θ for several initial UGV's orientations. . . . . . . . . . . . . . . . . . . . . . . . . 3.23 (a) Different terms of the Lyapunov function (cf. eq. (3.32)) and (b) Lyapunov function values for several initial orientations. . . . . . . . . . . . . . 3.24 Safe and reactive VIPALAB's navigation while avoiding hinder obstacles. . . 3.25 Validation of the safe navigation in cluttered environment. . . . . . . . . . . 3.26 Experimental results of the safe navigation in cluttered environment (AT: Attraction to the Target and OA: Obstacle Avoidance). . . . . . . . . . . . . 4.1 Autonomous navigation of a UGV in an urban environment (Clermont-Ferrand, France). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Nominal scenario with a road map and the task to achieve by the vehicle in its environment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Description of waypoints and target assignment. . . . . . . . . . . . . . . . 4.4 Proposed control architecture for navigation through waypoints. . . . . . . . 4.5 Flowchart of the Sequential target assignment. . . . . . . . . . . . . . . . .

xFigure 1 :

 1 Figure 1: General schema of the manuscript.

Figure 1 . 1 : 18 CHAPTER 1 .

 11181 Figure 1.1: Different robots applied to different domains.

Fig. 1 .

 1 3(a)), (a) Explorer robot (NASA ) named Curiosity rover. (b) Combat aerial robot (GA-ASI ) named Predator B. (c) Fish robot (MIT ) named RoboTuna. (d) Polar rover robot (NASA ) named Goddard Remote Operated VEhicle (GROVER).

Figure 1 . 2 :

 12 Figure 1.2: Different examples of mobile robots.

  Landshark UGV (Black-i Robotics) with complex mapping sensor system developed by SRI . (c) Battlefield Extraction-Assist Robot (BEAR) for search and rescue developed by Vecna Technologies . (d) Eight-legged walking robot (Scorpion) for space exploration developed by University of Bremen -Germany.

Figure 1 . 3 :

 13 Figure 1.3: Different applications of UGVs.

  Standford University develops the first robot 3D obstacle mapper called Cart. It Can find, track and avoid a few dozen object corners in the route. x 1980s: University Bundeswehr Munich (UniBW) leads by the Ernst

  x 2013: University of Parma presents an autonomous car BRAiVE to drive in Parma downtown, highways, narrow rural roads, etc. x 2013: Daimler and KIT present Mercedes Benz S-Class 500 intelligent drive to follow the Bertha Benz route (from Mannheim to Pforzheim), this vehicle uses vision and radar sensors in combination with accurate digital maps. x 2014: Google presents Google self-driving car. Other companies also present their driverless cars such as Mercedes, Audi, BMW, etc.

xFigure 1 . 4 :

 14 Figure 1.4: Brief overview of UGV's developments.

  (a) VaMP car developed by Prometheus project. (b) Google driverless car developed by Google.

Figure 1 . 5 :

 15 Figure 1.5: Examples of driverless cars.

Figure 1 . 6 :

 16 Figure 1.6: Summary of the levels of driving automation for on-Road vehicles [394].

Figure 1 . 8 :

 18 Figure 1.8: Example of Deliberative or SMPA architecture.

Figure 1 . 9 :

 19 Figure 1.9: Example of Reactive or Subsumption architecture.

Figure 1 . 10 :

 110 Figure 1.10: Example of Hybrid architecture.

  Cooperative quadrocopters catching a ball by ETHZ -Switzerland [336]. (c) Cooperative manipulators for spot welding by KUKA systems [279].

  (f) Heterogeneous small autonomous robots dynamically connected by Swarmanoid project[START_REF] Dorigo | a novel concept for the study of heterogeneous robotic swarms[END_REF].

Figure 2 . 1 :

 21 Figure 2.1: Different examples of multi-robot system.

x

  2000s: Several projects for Intelligent Transport System (ITS) are developed such as CarTALK2000 (2000), CyberCars (2001), MobiVIP (2004), PReVENT (2004), NOW (2004), CyberCars2 (2006), Sevecom (2006), CityMobil (2006). x 2001: German INVENT project is launched. It deals with Intelligent Traffic and driver assistance technology to make the future traffic safer and more efficient. x 2004: Centibots project by DARPA demonstrated the use of 100 robots for mapping, tracking, surveillance of a indoor area. x 2006: Swarmanoid project is launched. The goal is the development of distributed system based on heterogeneous, dynamically connected, small robots (eye-bots, hand-bots and foot-bots). x 2010s: Several projects for Intelligent Transport System (ITS) are implemented such as SARTRE (2012), HAVEit (2011), CityMobil2 (2012) SafePlatoon (2014), VIPAFLEET (2015). x 2010: Multi Autonomous Ground-robotic International Challenge (MAGIC) funded by U.S. Army (TARDEC) and Australian Department of Defence (ADOD) establish developing unmanned vehicle prototypes to autonomously coordinate, plan and execute a series of specific timed tasks in dynamic terrains. x 2010: Connect and Drive Project experiments a prototype Cooperative Adaptive

  x 1994: The PROMETHEUS project presents a convoy of two robot vehicles (VaMP and VITA-2) driving more than 1000 km on a Paris multi-lane highway in standard heavy traffic at maximum speed of 130 km/h. x 1995: The Praxitele project experiments a new kind of transportation in urban environments with a fleet of electric vehicles. The small electric vehicle, named Praxilab, was designed for low-speed driverless movement. x 1997: PATH project demonstrate multiple HONDA-PATH vehicles capable of platoon driving and safe maneuvers such as lane changes. bending joint CEBOT cells Three Termes robots building 3D structures Group of Khepera robot of EPFL Seven Toyata Prius in platooning Self-organizing swarm of a thousand Kilobots Self-organizing swarm of a thousand Kilobots Convoy of VITA and VaMP cars Cycab and RoBuCar driving in urban scenario Participants of the Grand Cooperative Drive Challenge

Figure 2 . 2 :

 22 Figure 2.2: Brief background of Multi-robot systems.

  (a) Convoy of a group of automated cars by PATH project. (b) Platoon of a group of automated trucks by PATH project.

Figure 2 . 3 :

 23 Figure 2.3: Platoon of a group of automated vehicles.

Figure 2 . 4 :

 24 Figure 2.4: Different vehicles used in Safeplatoon project.

Figure 2 . 5 :

 25 Figure 2.5: Example of centralized architecture: RoboSkeleton [209].

Figure 2 . 6 :

 26 Figure 2.6: Example of decentralized architecture: ALLIANCE.

Figure 2 . 7 :

 27 Figure 2.7: Example of Centralized/Decentralized architecture: DIRA or LMA.

  (a) l -ψ (UGV j ) and ll (UGV i ) controls.(b) ll (UGV i ) control for obstacle avoidance.

Figure 2 . 8 :

 28 Figure 2.8: Leader-follower formation.

Figure 2 . 10 :

 210 Figure 2.10: Virtual structure definition.

  (a) Competitive behavior-based. (b) Cooperative behavior-based.

Figure 2 .

 2 Figure 2.11: Behavior-based formation control.

Figure 3 . 1 :

 31 Figure 3.1: The obstacles and robot representation.

Figure 3 . 2 :

 32 Figure 3.2: Surrounding a wall using a circle and ellipse shapes.

Fig. 3 . 3 ) 1 Figure 3

 3313 Figure 3.3: a) Clockwise and b) counter-clockwise shapes for the used elliptical limitcycles.

  The UGV's navigation is operated by two elementary behaviors (Target reaching and Obstacle avoidance); at each sample time one of them is activated by Hierarchical action selection (executive layer (cf. Subsection 1.3.3) according to the perceived environment (Perception block). Each elementary controller (cf. Fig.3.4) provides as output an error state (e x , e y , e θ ) and a desired velocity v T to the Control law block. The proposed control architecture is designed to use a single control law for the overall elementary behaviors.The proposed control law, designed for a tricycle robot, 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. Section 3.5). The inputs of the control law (pose errors between the vehicle and target) are provided by the elementary behaviors (cf. subsection 3.3.3 and 3.3.4). The blocks of the proposed architecture are detailed below.

Figure 3 . 4 :

 34 Figure 3.4: Proposed control architecture embedded in the UGV.

Figure 3 . 5 :

 35 Figure 3.5: Vehicle's and target's configuration in Global (X G Y G ) and Local (X m Y m ) reference frames.

Fig. 3 .Figure 3 . 6 :

 336 Figure 3.6: Control variables of Target reaching block.

Fig. 3 .

 3 [START_REF] Dafflon | Adaptive autonomous navigation using reactive multi-agent system for control law merging[END_REF]. Let us first define d and θ RT as (cf. Fig.3.6):

  Fig.3.7). The Hierarchical action selection block activates the Obstacle avoidance block as soon as it detects at least one obstacle which can hinder the future vehicle movement toward its static target (i.e., it exists at least one intersection point between the line l f and the ellipse of influence E f i (cf. Fig.3.1))[START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF]. This block allows to anticipate the activation of obstacle avoidance behavior and to decrease the time to reach the assigned target (especially in very cluttered environments), instead of, activate the obstacle avoidance only when the robot is in the immediate vicinity of the obstacle (i.e. D RO ≤ R "a certain radius value")[START_REF] Brooks | A robust layered control system for a mobile robot[END_REF][START_REF] Adouane | Behavioral and distributed control architecture of control for minimalist mobile robots[END_REF].

Figure 3 . 7 :

 37 Figure 3.7: Flowchart of the Hierarchical action selection block.

  4.1). The observed data allows to identify online hinder obstacles (cf. Appendix C.2 and C.3) and surround them with the closest ellipse (cf. Subsection 3.4.2) to apply the obstacle avoidance approach based on elliptical limit-cycle [3]. Each process of the Perception block is shown in Fig. 3.8. The following sections focus on the used sensor model and the development of Enclosing data with an ellipse block. The used functions and methodologies of Filtering and Segmentation blocks are described in Appendix C.

Figure 3 . 8 :

 38 Figure 3.8: Internal block diagram of the Perception block.

Figure 3 . 9 :

 39 Figure 3.9: Range sensor model and its set of n points.

Figure 3 . 10 :

 310 Figure 3.10: Flowchart of the Enclosing data with an ellipse block.

Fig. 3 .Figure 3 . 12 :

 3312 Fig.3.12, (b) and (c) show the evolution of the identified ellipse for the Heuristic, Covariance and Least square approaches. We observe that the ellipse's shape of Least square and Covariance approaches change abruptly. Fig.3.13 shows the identified ellipse's pa-

Figure 3 . 13 :

 313 Figure 3.13: Parameters of the estimated ellipses.

Figure 3 . 14 :

 314 Figure 3.14: Example of the application of the proposed enclosing ellipse method.

Fig. 3 .

 3 Fig. 3.14 shows an example of the application of proposed Heuristic method using filtering data from a LIDAR sensor (cf. Fig. C.6).

34 )

 34 Substituting eq.(3.23) in (3.34) V = -e x v b + v T e y sin(e θ ) [K d + K l sin 2 (e RT )] + K o sin(e θ ) v T r c T v T cos(e θ )c cv b c c + K l sin(e RT ) cos(e RT ) d 2 v T r c T v T e x sin(e θ )e y v b = e y (K d + K l sin 2 (e RT ))e x K l sin(e RT ) cos(e RT ) v T sin(e θ ) + v T r c T d 2 K l sin(e RT ) cos(e RT ) + K o sin(e θ )v T K o sin(e θ ) cos(e θ )c c v b e x (K d + K l sin 2 (e RT ))v b e y K l sin(e RT ) cos(e RT ) + K o sin(e θ )c c (3.35)

  (3.40) are positive and they must be equal to zero (i.e., d, e θ , e RT = 0, and if d = 0 then e x , e y = 0). Hence, from eq.(3.40), V is equal to zero when v T = ξ and r c T → ∞, only if (e x , e y , e θ ) = (0, 0, 0).

Figure 3 . 15 :

 315 Figure 3.15: Limit vehicle configuration for tuning controller parameters.

Fig. 3 .

 3 Fig. 3.16 shows the global flowchart to obtain the minimal initial distance d i while having:

Fig. 3 .

 3 22). Using the described consideration of Fig. 3.15 and eq. ( 3.23 on page 62), ( 3.24 on page 62) and ( 3.25 on page 62) in ( 3.29 on page 63), ėθ can be written as:

Figure 3 . 16 :

 316 Figure 3.16: Flowchart of the analysis to obtain d i .

Fig. 3 .

 3 Fig. 3.18 and 3.19 show the trajectories and the control output of each vehicle (leader and followers). It can be noticed that the proposed control law has a similar performance to the controller based on a reference trajectory (Frenet control). Table 3.1 shows the convergence time to satisfy the error threshold in distance d T arget and orientation e θ T arget with regards to the target pose (cf. Fig. 3.6). The proposed control

Figure 3 . 18 :

 318 Figure 3.18: Trajectory of the leader and followers (Frenet control[START_REF] Samson | Control of chained systems. application to path following and timevarying point-stabilization of mobile robots[END_REF]) and (Chained system[START_REF] Laumond | La Robotique Mobile[END_REF]).

Figure 3 . 19 :

 319 Figure 3.19: Control output.

  (a) Errors defined according to Target's pose. (b) Errors defined according to Frenet reference frame.

Figure 3 . 20 :

 320 Figure 3.20: Simulation results of the comparative study between control laws.

  (3.46) and(3.51), the minimum initial euclidean distance to the target is d i = 10.4 m.In this first simulation, the desired final configuration is (x T , y T , θ T ) ≡ (10.4, 0, 0 • ) and v T = 0 m/s. The vehicle has an initial orientation θ i equal to 80 • and different initial distance equal to 7.4, 10.4 and 13.4 m. Fig. 3.21(a) shows the trajectory of the vehicle for these different initial distances. The distance and orientation errors are shown in Fig. 3.21(b). The Lyapunov function values are shown in Fig. 3.21(c).

Fig. 3 .

 3 Fig. 3.21(a) shows that the vehicle's trajectory towards its target depends on the initial distance. The upper bound of the distance and orientation errors are not satisfied when the initial distances (7.4 m) is less than d i = 10.4 m (dotted pink line). Fig. 3.21(b) shows that the system errors are bounded according to eq. (3.49) and (3.44) (black dashed line) and converge to zero (cf. Subsection 3.5.2).

Figure 3 .Figure 3 .

 33 Figure 3.21: (a) UGV's trajectories, (b) their distance d and orientation errors e θ and (c) their evolution of Lyapunov functions for several initial UGV's orientations.

Fig. 3 .

 3 Fig. 3.24 shows the cluttered environment with three static obstacles (two boxes and one static vehicle) and a VIPALAB. The red point is the assigned target. Some screenshots of the developed Graphical Data Interface for VIPALAB (GDI-VIPA) are shown in Fig. 3.25(a). The white line represent UGV's trajectory and the big red point is the assigned target. It can be noted the evolution of the online enclosing ellipse (orange circle) according to historic of the observed range data.

Fig. 3 .Figure 3 . 24 :

 3324 Fig. 3.25(b) shows the safe trajectory of the vehicle in the real cluttered environment where the parameters of the ellipse that enclose the obstacle are obtained online using the obstacle detection method [2, 3] (cf. Section 3.4.2.1). It can be observed that the 1 http://maccs.univ-bpclermont.fr/index.php/Profiles/VilcaJM/NavigationVipalab.mp4

Figure 3 . 25 :

 325 Figure 3.25: Validation of the safe navigation in cluttered environment.

  (a) Commands given by the control law. (b) Evolution of Lyapunov function during the vehicle navigation. (c) Position and orientation errors of the vehicle. (d) Distance from the vehicle to the ellipse of influence E f i .

Figure 3 . 26 :

 326 Figure 3.26: Experimental results of the safe navigation in cluttered environment (AT: Attraction to the Target and OA: Obstacle Avoidance).

Fig. 3 .

 3 Fig. 3.26(a) shows the output of the control law. It can be observed that the velocity decreases according to the distance to the obstacle (cf. Subsection 3.3.4) which allows to obtain a smooth and safe navigation. Fig. 3.26(b) shows the variation of the Lyapunov function V eq. (3.32) related to the control law (cf. Section 3.5) during the navigation task. This function decreases asymptotically which guarantees the stability of the system. Fig. 3.26(c) shows the controller parameters (position and orientation errors). It can be noted that the absolute value of errors converges towards zero after each behavioral switch. Therefore, the control law given by (3.23) and (3.24) remain always asymptotically stable.

Figure 4 . 1 :

 41 Figure 4.1: Autonomous navigation of a UGV in an urban environment (Clermont-Ferrand, France).

Figure 4 . 2 :

 42 Figure 4.2: Nominal scenario with a road map and the task to achieve by the vehicle in its environment.

Figure 4 . 3 :

 43 Figure 4.3: Description of waypoints and target assignment.

Figure 4 . 4 :

 44 Figure 4.4: Proposed control architecture for navigation through waypoints.

Figure 4 . 5 :

 45 Figure 4.5: Flowchart of the Sequential target assignment.

3 . 1 )

 31 , and the proposed control law (cf. Section 3.5) were considered.The values of the controller parameters (cf. eq. 3.25 on page 62 and 3.26 on page 62) are K = (1/d i , 1.8, 8, 0.15, 0.6, 0.01) (d i is the initial distance to the target). These parameters were chosen to obtain a safe and smooth trajectory (even during the target switching), fast response and velocity value within the limit of the vehicle, which are minimum movement velocity v min = 0.1 m/s, maximum velocity v max = 2.5 m/s, maximum steering angle γ max = ±23 • (or minimum radius of curvature r c min = 2.83 m) and maximum linear acceleration 1.0 m/s 2 . The sample time is T s = 0.01 s Firstly, the case of successive waypoints reaching through a set of waypoints is analyzed. Two set of waypoints selected from a reference trajectory are used, one set has a distance between waypoints equal to 2 m and the other equal to 4 m (cf. Fig. 4.7(a)).Figures 4.7(a) and 4.7(b) respectively, show the vehicle trajectories and lateral and angular errors w.r.t.the reference trajectory for two set of waypoints. It can be noted that the obtained vehicle trajectories are close to the reference trajectory; and as expected, the lateral and angular errors are smaller when the fixed distance between the waypoints decreases. Therefore, the proposed navigation strategy and control law permits also to the vehicle to perform accurate trajectory tracking behavior if the waypoints are close enough.

Figure 4 . 6 :

 46 Figure 4.6: Evolution of the SVC v used to ensure smooth control when the waypoint switching occurs.

Figure 4 . 7 :

 47 Figure 4.7: Simulation results for different distances between waypoints.

Fig. 4 .

 4 Fig.4.8 shows the trajectories of the UGV for different static waypoints (p i , i = 1, . . . , 6) and for a dynamic target p d (sinusoidal trajectory). The static waypoints are positioned at different initial distances d i and orientation angles between them (45 • until p 5 and 0 • for p 6 ). The velocity profile of the waypoints for each simulation are v p = 0.1, 0.5 and 1.0 m/s respectively. It can be noted that the vehicle converges to each waypoint (static and dynamic), located in different positions and with different set of velocities. The dynamic target starts its movement when the vehicle reaches the last static waypoint p 6 . Fig. 4.9(a) shows the values of errors d and e θ for all waypoints to reach. For static waypoints (p i , i = 1, . . . , 6), the obtained values of errors just before to switch from waypoint p i to p i+1 are shown. For dynamic target, the evolution of d(t) and e θ (t) during all tracking phase are shown. It is observed that the distance and orientation errors of static waypoints depend on initial UGV's configuration (pose and velocity). The errors increase when the static waypoints are closer. Moreover, for static waypoints where the profile velocity is the desired vehicle velocity at target position, the different profiles velocities have similar errors. It occurs in static waypoints because the proposed control law (angle steering) relies only on the dynamic of pose errors between the vehicle and the static waypoint (cf. eq. ( 3.24 on page 62) and ( 3.26 on page 62)) (the waypoint's velocity is a set-point for the vehicle when it reaches the waypoint's pose (cf. eq. ( 3.23 on page 62)) and the controller parameters K are tuning according to same initial distance d i to the waypoint. Obviously, for dynamic target, the small target profile velocity has a faster convergence to zero. The convergence of the Lyapunov function (3.32) is shown in Fig. 4.9(b) (cf. Subsection 3.5.1 on page 63) when the vehicle starts at the same initial position but with different initial orientations to the waypoint p 1 and while taking v p = 0.5 m/s. The use of Sigmoid function (cf. Subsection 4.2.2) is observed in the Lyapunov function and vehicle commands (velocity and steering angle) (cf. Fig. 4.9(b) and 4.9(c)) for the static waypoint with profile velocity of v p = 0.5 m/s. It can be noted that the Sigmoid function contributes to avoid peaks at the transition time and to obtain thus smooth vehicle commands while maintaining the stability of the control.

Figure 4 . 8 :

 48 Figure 4.8: Trajectories of the vehicle for several waypoints' velocities.

  (a) Distance and orientation errors for several waypoints' velocities. (b) Values of the Lyapunov function for v p = 0.5 m/s with and without the adaptive Sigmoid function. (c) Control commands with and without the adaptive Sigmoid for smooth transition (SVC v .

Figure 4 . 9 :

 49 Figure 4.9: Simulation results of the navigation through sequential waypoints

Figure 4 .

 4 Figure 4.10: (a) Road scheme and (b) its C-space with its Voronoï diagram.

Figure 4 . 11 :

 411 Figure 4.11: Planning block (green block) added to the control architecture for navigation through waypoints.

  (a) n p = 113 (b) n p = 34

Figure 4 . 12 :

 412 Figure 4.12: Example of waypoint selection based on a reference path and Algorithm 1.

(c) n p = 16

 16

Figure 4 . 12 :

 412 Figure 4.12: Example of waypoint selection based on a reference path and Algorithm 1 (continuation (cont.)).

Figure 4 .

 4 Figure 4.13: A group of cells of the global grid map, the current cell i j (red), its predecessor cell (blue) and its probable successive cell (green). State is the cell state and Pred is the predecessor of the cell.

Figure 4 . 14 :

 414 Figure 4.14: Representation in gray scale w.r.t.the distance to the closest Cspace obst (the whitest area represents the safest area).

Fig. 4 .

 4 [START_REF] Nilsson | Principles of Artificial Intelligence[END_REF]. Fig.4.15(a) shows the case where N h = 1 (which implies 8 neighbor cells). Fig.4.15(b) shows a larger neighbor cells when N h = 2 (24 neighbors).

  (a) N h = 1. (b) N h = 2.

Figure 4 . 15 :

 415 Figure 4.15: Different neighborhood values N h of the current cell (red). A gray cell represents a cell where the movements are forbidden.

7 ) 9 : 10 : 11 : 14 : 15 : 19 :

 791011141519 and α mn (4.8) These values are 0 when cell i j = p f Compute the total cost C(cell N ) (4.3) if S tate cell N == INIT then Add cell N to the queue Q and Pred cell N = cell i j 12: Set S tate cell N = OPEN 13: else if S tate cell N == OPEN then cell N is in Q with certain C(cell N ) and Pred cell N Update cell N in the queue Q with the lower total cost C(cell N ) Pred cell N is the cell which generates the lower C(cell N ) Sort the queue Q in ascending order of total cost C 20:

  (a) N h = 1. (b) N h = 3.

Figure 4 . 16 :

 416 Figure 4.16: Different sets of waypoints (red points) for different number of possible neighbor cells.

Figure 4 . 17 :

 417 Figure 4.17: Vehicle's trajectories which starts from extreme configurations (± l d max , ± t d max and ± θ max ) in the ellipse of localization uncertainties E loc .Δe l is the maximum lateral deviation of all vehicle's trajectories.

Figure 4 . 18 :

 418 Figure 4.18: Expanding tree method to obtain the appropriate set of waypoints.

  and 4.19(b). More examples of different tuned parameters will be shown in Section 4.3.4.

  (a) Safest path: k 1 = 1.0, k 2 = k 3 = k 4 = 0.0 and k h = 0.1 (b) Shortest path: k 2 = 1.0, k 1 = k 3 = k 4 = 0.0 and k h = 0.1

Figure 4 . 19 :

 419 Figure 4.19: Set of waypoints for different parameters values k i of the traveling cost.

  4.3.4/ SIMULATION VALIDATIONThis subsection shows optimal sets of waypoints, obtained according to the environment characteristics and/or the task to achieve. In what follows, it will be shown: a comparison between grid map and expanding tree algorithms (cf. subsection 4.3.4.1) and a comparison between the proposed OMWS-ET and a variation of RRT (cf. subsection 4.3.4.2). Several other simulations to demonstrate the efficiency of the OMWS-ET method for planning of UGV's navigation in a structured environment are also exhibited such as trajectory generation (cf. Subsection 4.3.4.3); a comparison between deterministic and probabilistic waypoints selection (cf. Subsection 4.3.4.4), extension to multi-robot formation (cf. Subsection 4.3.4.5) and finally, Subsection 4.3.4.6 shows the flexibility of our proposal for local replanning of the waypoints configurations when unexpected obstacles are detected.

Fig. 4 .

 4 20(a) andFig. 4.21(a) show the obtained path according to Algorithm 2 and 3 before to apply Algorithm 1. The minimum set of waypoints are given afterward in Fig. 4.20(b) and Fig. 4.21(b).

Figure 4 .Figure 4 .

 44 Figure 4.20: a) Obtained waypoints using Algorithm 2 based on grid map before applying Algorithm 1 and b) Minimum set of waypoints.

  (a) RRT* showing a few branches (b) OMWS-ET showing a few branches (c) Comparison with Voronoï

Figure 4 . 22 :

 422 Figure 4.22: Three obtained path according to Voronoï, RRT* and OMWS-ET.

Fig. 4 .

 4 Fig. 4.23(b) shows the use of the proposed OMWS-ET for the specific case where a

Figure 4 . 23 :

 423 Figure 4.23: Different scenarios for OMWS-ET.

Figure 4 . 24 :

 424 Figure 4.24: Set of waypoints using probabilistic expanding tree.

Fig. 4 .

 4 Figure 4.25: Multi-robot formation (straight line shape).Figure 4.26: Minimum set of waypoints for multi-robot formation obtained by Algorithm 3 based on expanding tree.

Figure 4 . 26 :

 426 Figure 4.25: Multi-robot formation (straight line shape).Figure 4.26: Minimum set of waypoints for multi-robot formation obtained by Algorithm 3 based on expanding tree.

Figure 4 . 27 :

 427 Figure 4.27: Flowchart of the Waypoint configuration selection block for local replanning.

Figure 4 .

 4 28(b) shows an example of the local replanning using the set of waypoints given in Subsection 4.3.4.1 (as initial set of waypoints) (cf. Fig. 4.21(b)). Finally, the vehicle moves through the new (a) Unexpected obstacle is detected. (b) Local replanning. (c) Safe vehicle trajectory.

Figure 4 . 28 :

 428 Figure 4.28: Local replanning for unexpected obstacle.

Fig. 4 .

 4 Fig. 4.29 and 4.30 show respectively the minimum obtained set of waypoints and the corresponding actual vehicle's trajectories using the kinematic model of the UGV (simulation) and the VIPALAB (experiment). Fig. 4.29(a) shows the set of waypoints of the first experiment where the constant values of the cost function eq. (4.10) are k 1 = 0.6, k 2 = 0.2, k 3 = 0.1, k 4 = 0.1 and k h = 0.4. The safety (k 1 ) has the highest priority in this experiment. Therefore, these waypoints guide the vehicle to be close to the middle of the road (cf. Fig. 4.30(a)). Fig. 4.29(b) shows the set of waypoints of the second experiment where the constant values are k 1 = 0.3, k 2 = 0.2, k 3 = 0.4, k 4 = 0.1 and k h = 0.4. The minimal steering angle rate k 3 has the highest priority in this experiment. The obtained result shows that the obtained waypoints are localized very close to the border of the road (cf. Fig. 4.30(b)). Fig. 4.30(a) and 4.30(b)show the actual vehicle's trajectories from the simulation and experiment. It can be observed that they are very close (maximal error between them is less than 0.15 m). We can conclude thus that the proposed optimal multi-criteria waypoint selection based on Expanding Tree (OMWS-ET, performed off-line (cf. Section 4.3.3.2)) permits to cope accurately with actual environment and experiments.

Fig. 4 .

 4 Fig. 4.29(c) and 4.30(c) show the comparison between the set of waypoints and the real trajectories of both experiments. The velocities and steering angle of the vehicle while

Figure 4 . 29 :

 429 Figure 4.29: Different set of obtained waypoints using OMWS-ET.

Figure 4 . 30 :

 430 Figure 4.30: Actual vehicle's trajectories for different obtained set of waypoints.

4. 4 . 2 /

 42 SAFE AND FLEXIBLE NAVIGATION THROUGH SUCCESSIVE WAYPOINTS This subsection focuses on the performance of the control architecture with the proposed target assignment strategy (cf. Section 4.2) using waypoint selection (OMWS) based on Algorithm 3 in the well-know environment (PAVIN) (cf. Fig. 4.32). The scenario was built to exhibit the flexibility of the proposed control architecture for obstacle avoidance situation (unexpected obstacle in the environment) (cf. Fig. 4.32 (c) and (f)). Two obstacle are placed between the waypoints (cf. Fig. 4.32 (b) and (e)). As mentioned in Section 4.2, the proposed control architecture integrates the obstacle avoidance behavior (cf. Fig. 4.4

Figure 4 . 32 :

 432 Figure 4.32: Safe and flexible navigation through successive waypoints .

Figure 4 . 33 :

 433 Figure 4.33: Validation of the navigation through successive waypoints.

Fig. 4 .

 4 Fig. 4.34(a) shows the velocity and steering angle of the vehicle. It can be observed that the UGV's velocity changes according to waypoint's velocity and decreases in the obstacle avoidance phase (since the velocity set-point in this phase takes into account the distance to the obstacle (cf. Section 3.2)) which allows to obtain a smooth and safe navigation. Fig. 4.34(b) shows the controller parameters (position and orientation errors). It is noted that the absolute value of errors converges towards zero for each waypoints. Therefore, the control law given by ( 3.23 on page 62) and ( 3.24 on page 62) drives with reliable and stable way the vehicle towards the assigned set of waypoints. Fig. 4.34(c) show the Lyapunov function values which highlight that the vehicle converges to each static waypoint.Therefore, the proposed navigation strategy allows safe, flexible and smooth trajectories between the waypoints and also to perform different behaviors, such as obstacle avoidance.

Figure 4 . 34 :

 434 Figure 4.34: Experimental results of the safe and flexible navigation through successive waypoints (SW: Sequential Waypoint and OA: Obstacle Avoidance).

Figure 5 . 1 :

 51 Figure 5.1: Autonomous navigation in formation of a group of UGVs in an urban environment (Clermont-Ferrand, France).

  the dynamic target T d i w.r.t. the leader's local reference frame (cf. Fig. 5.3 and 5.4).

Figure 5 . 2 :

 52 Figure 5.2: Proposed control architecture for navigation in formation of a group of UGVs.

Figure 5 . 3 :

 53 Figure 5.3: Formation definition in mobile Cartesian frame linked to the Leader.

Figure 5 . 4 :

 54 Figure 5.4: Formation definition in Frenet frame linked to the Leader's trajectory.

  (a) Vehicles' trajectories. (b) Vehicles' velocities.

Figure 5 . 5 :

 55 Figure 5.5: Example of navigation in formation using the Cartesian and Frenet frames.

Figure 5 . 6 :Figure 5 . 7 :

 5657 Figure 5.6: Schema of comparison of between Cartesian formations F 1 , F 2 and F 3 (v L = 1.5 m/s) (T d i -F3 is the i dynamic virtual target w.r.t. F 3 formation).

Fig. 5 .

 5 7(a), 5.7(b) and 5.7(c). The values of L 2 (P d ) and L 2 (Dn max ) for different shapes are closer for low velocities v L = 1.0 and 1.5 m/s. Nevertheless, the saturation occurs when v L = 2.0 m/s and the values of L 2 (P d ) and L 2 (Dn max ) of F 2 and F 3 are greater than F 1 .

Fig. 5 .

 5 8 shows the followers' trajectories to reach the formation F 3 . The evolutions of the distance d rms and orientation e θ rms errors for different leader velocities v L = 1.0, 1.5 and 2.0 m/s are shown in Fig. 5.9.

Fig. 5 .

 5 Fig.5.9(a) (v L = 1.0 m/s) and 5.9(b) (v L = 1.5 m/s) shows that the UGVs reach their assigned target for each different formation shape. Nevertheless, Fig.5.9(c) (v L = 2.0 m/s) shows that the UGV followers (as in the case of Cartesian formation for F 2 and F 3 ) have saturated velocities since the virtual target's velocity is greater than v max . Then, the followers can not reach their assigned targets. The values of d rms and e θ rms of F 2 and F 3 are greater than F 1 .

Figure 5 . 8 :

 58 Figure 5.8: Schema of comparison of between Frenet formations F 1 , F 2 and F 3 (v L = 1.5 m/s) (T d i -F3 is the i dynamic virtual target w.r.t. F 3 formation).

  3.1), the values of L 2 (d rms ) and L 2 (e θ rms ) for different shapes are closer for low velocities v L = 1.0 and 1.5 m/s. Nevertheless, when the velocity is high (v L = 2.0 m/s), the values of L 2 (d rms ) and L 2 (e θ rms )of F 2 and F 3 are greater than F 1 since the saturation occurs for great formation shape. This tables confirm that a steady formation w.r.t. leader's trajectory depends on the formation dimension and the leader's dynamic.

Figure 5 . 9 :

 59 Figure 5.9: Frenet formations F 1 , F 2 and F 3 with different leader's velocities.

5 . 3 )

 53 to obtain smooth (smooth values of the vehicle commands) navigation in formation (cf. Fig.5.10 and 5.12).5.3.3.1/ CARTESIAN FORMATIONThe scenario with a group of UGVs navigating in triangular formation F 3 with v L = 2 m/s presented in Subsection 5.2.3.1 is used. The leader tracks a reference sinusoidal trajectory and the followers have to track their assigned virtual targets.

Figure 5 . 10 :

 510 Figure 5.10: Navigation in Cartesian formation F 3 (v L max = 2 m/s) for a group of N = 3 UGVs using the proposed adaptive leader's constraints (T d i -F3 is the i dynamic virtual target w.r.t. F 3 formation).

  (a) Commands of the UGVs. (b) Formation shape F 3 (v L max = 2 m/s).

Figure 5 . 11 :

 511 Figure 5.11: Simulation results of the navigation with adaptive Leader Constraints (Cartesian formation).

Fig. 5 .

 5 13(b) shows the evolutions of the distance and orientation errors as in Subsection 5.2.3.2. It can be noted that the errors using the proposed adaptive leader constraints converges to the desired formation shape F 3 for v L = 2 m/s.

Figure 5 . 12 :

 512 Figure 5.12: Navigation in Frenet formation F 3 (v L max = 2 m/s) for a group of N = 3 UGVs using the proposed adaptive leader's constraints (T d i -F3 is the i dynamic virtual target w.r.t. F 3 formation).

  (a) Commands of the UGVs. (b) Distance and orientation errors F 3 (v L max = 2 m/s).

Figure 5 . 13 :

 513 Figure 5.13: Simulation results of the navigation with adaptive Leader Constraints (Frenet formation).

Fig. 5 .

 5 14(a) -(a) and (b)). The followers track their virtual targets to keep the desired formation F (cf. Fig. 5.14(a) -(c)).

Fig. 5 .

 5 Fig. 5.14(b) shows the trajectories of the UGVs during the navigation in formation. It can be seen that the leader reaches accurately the successive static waypoints. The followers track accurately their virtual targets even during obstacle avoidance phase. It can be noted that no UGV collides with any obstacle. Fig. 5.15(a) and 5.15(b) show respectively the values of Lyapunov function (cf. eq. (3.32)) for each UGV and the evolution of the Procrustes distance P d between the positions of the vertex. Some small peaks can be 1 http://maccs.univ-bpclermont.fr/uploads/Profiles/VilcaJM/SimNavCartFor.mp4

Figure 5 . 14 :

 514 Figure 5.14: Safe navigation in Cartesian formation in cluttered environment.

  (a) Lyapunov function of each UGV (AT: Attraction to the Target and OA: Obstacle Avoidance). (b) Procrustes distance of the Cartesian formation.

Figure 5 . 15 :

 515 Figure 5.15: Validation of the navigation in Cartesian formation.

Figure 5 . 16 :

 516 Figure 5.16: Safe navigation in Frenet formation in structured environment.

Figure 5 . 17 :

 517 Figure 5.17: Validation of the navigation in Frenet formation. (a) GDI-VIPA during the navigation of the follower 1 ("v UGV" and "v Leader" are respectively, the follower's and the leader's current velocities).

Figure 5 . 17 :

 517 Figure 5.17: Validation of the navigation in Frenet formation (cont.). (b) GDI-VIPA during the navigation of the follower 2 ("v UGV" and "v Leader" are respectively the follower's and the leader's current velocities) and (c) Vehicles' trajectories using the proposed control architecture for navigation in Frenet formation.

Figure 5 . 18 :Fig. 5 .

 5185 Figure 5.18: Experimental results of the navigation in Frenet formation.

Fig. 5 .

 5 Fig. 5.19 shows the Formation parameters block of the proposed control architecture (cf. Fig. 5.2) for navigation in formation of a group of UGVs with dynamic reconfiguration according to the environment context. The UGV L (Leader) determines the formation shape since its configuration is used to compute the virtual targets T d i of the followers (by Communication block in Fig. 5.2). The coordinates of the formation F are a priori defined by the designer. The UGVs use their range sensor to detect any unforeseen obstacle (cf. Fig. 5.19). The formation reconfiguration strategy is activated when any hinder obstacle is detected. This strategy takes into account the current environmental state and the current T d i pose to obtain a new formation shape (cf. Fig. 5.19). Moreover, in the case where the obstacle is close enough to any UGV, the proposed control architecture embedded in each UGV allows to have the capability of reactive obstacle avoidance (cf. Subsection

  -a 2N . . . a N ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (5.41)

Figure 5 . 21 :

 521 Figure 5.21: Formation reconfiguration between, for instance, triangular and linear formation shapes based on inter-target distances.

Figure 5 . 22 :

 522 Figure 5.22: Navigation with reconfiguration (SFR-S) in Cartesian formation for a group of N = 3 UGVs.

  (a) Distance and orientation errors of the UGVs w.r.t. their virtual targets. (b) Distance among the UGVs.

Figure 5 . 23 :

 523 Figure 5.23: Simulation results of the navigation with reconfiguration (SFR-S).

  (c) Progress of the set-point definition f i according to the proposed SFR-S.

Figure 5 . 23 :

 523 Figure 5.23: Simulation results of the navigation with reconfiguration (SFR-S) (cont.).

Figure 5 . 24 :Figure 5 .Figure 5 . 25 :Figure 5 . 25 :Figure 5 . 25 :

 5245525525525 Figure 5.24: Navigation with reconfiguration (SFR-D) in Frenet formation for a group of N = 3 UGVs.

Figure 5 .

 5 Figure 5.25(d) shows safe and smooth trajectories of the group of VIPALABs. It can be observed that the vehicles keep the desired formation while avoiding obstacles. Fig. 5.26(a)shows the velocity and steering angle of the vehicles. The reconfiguration strategy was designed to reduce the peaks of the control commands of each UGV when the transitions between the formation occur. Fig.5.26(b) shows the Lyapunov function evolution (cf. eq. 3.32) of each UGV which highlight that each vehicle is stable and converges to its assigned virtual dynamic target. At reconfiguration time, it can be observed some small peaks that are related to the switch between formation shapes which cause an abrupt jump of set-points. Fig.5.26(c) shows the distance between each UGV of the formation. This last figure shows clearly the non-collision between the vehicles in the formation. Fig.

Figure 5 . 26 :

 526 Figure 5.26: Experimental results of the navigation with reconfiguration (SFR-D).

Figure 5 . 26 :

 526 Figure 5.26: Experimental results of the navigation with reconfiguration (SFR-D) (cont.).

Figure A. 1 :

 1 Figure A.1: Ellipse's representation in Global (X G Y G ) and Local (X L Y L ) reference frames.

3 .( 3 )

 33 Figure B.1: The four specific areas surrounding the obstacle to avoid [293].

  ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ cos α -sin α 0 x obst sin α cos α 0 y obst 0

1 :

 1 I) Obtaining the values of a lc and b lc of the limit-cycle to follow 2: if x O ≤ 0 then a lc = a lc -ξ b lc = b lc -ξ (Attractive phase) 4:

5 : else 6 :

 56 {Escape criterion: go out of the obstacle ellipse of influence with smooth way} a lc = a lc + ξ b lc = b lc + ξ (Repulsive phase)8: end if 9: II) Obtaining the limit-cycle direction 10: if obstacle avoidance controller was active at t -δT then 11:

15 :ẋ

 15 = m( Clc y + 0.5 Blc x) + μx(1 -Ālc x 2 -Blc xy -Clc y 2 ) ẏ = -m( Ālc x + 0.5 Blc y) + μy(1 -Ālc x 2 -Blc xy -Clc y 2 ) 16: end if B.2/ RAPIDLY EXPLORING RANDOM TREE (RRT)

Figure B. 2 :

 2 Figure B.2: Representation of circles C int i and C ext of each UGV.

Figure B. 3 :

 3 Figure B.3: Integration of the penalty function in the proposed architecture.

Fig

Fig. C. 2

 2 Fig. C.2 shows the flowchart of the ICP algorithm. The initial error value E (cf. eq. C.1) is set to infinity to start the loop. E th is the threshold error value given by the user to indicate the convergence of ICP algorithm. The major problem is to determine the correct data associations (matching). Given the correct data associations, the transformation can be computed efficiently using SVD (Singular Value Decomposition)[START_REF] Corke | Robotics, Vision and Control: Fundamental Algorithms in MATLAB[END_REF]. The algorithm is terminated based on the number of iterations or the relative change in the error metric.

Fig. C. 3

 3 Fig. C.3 shows the flowchart of the Filtering block. The current UGV's pose is used to transform the range data (D r i , β i ), given in polar coordinates, to a Global reference frame X G Y G (cf. Fig. 3.9) using the following equation: r i = cos(θ) -sin(θ) sin(θ) cos(θ)

Figure C. 3 :

 3 Figure C.3: Flowchart of the Filtering block.

  Figure C.4: a) LIDAR data without filtering process and b) with filtering process.

Fig. C. 4

 4 Fig. C.4 shows an example of an online application of the Filtering process using a UGV with a LIDAR sensor. The UGV's pose is given by a GPS-RTK and a IMU. It is observed that the Filtering block allows to obtain an accurate information while reducing the problems of inaccuracy and noise.

  Figure C.5: Flowchart of the Segmentation block.

Figure C. 6 :

 6 Figure C.6: Example of the application of the segmentation method. Each segment is represented by different colors.

2 ) 6 )tan e θ 2 c 3 +

 2623 Fig. C.6 shows an example of the application of presented segmentation method using data from a LIDAR sensor which has a resolution equal to Δβ = 1 • . D ANALYTICAL SOLUTION OF CONTROL LAW EQUATIONS FOR SAFE TARGET REACHING This annex describes the solution of simplified differential equation given by eq. (3.41):ėθ = -K x (K d d + K o K θ ) K o cos 2 (e θ ) K d d + K o K θ sin 2 (e θ ) sin(e θ ) (D.1)The defined notations c 1 , c 2 and c 3 are given by eq. (3.42) as:These notations are replaced in eq. (D.1) to obtain:de θ dt = -K x (c 1 + c 2 ) K o cos 2 (e θ ) c 1 + c 2 sin 2 (e θ ) sin(e θ ) (D.3)while reorganizing eq. (D.3), it is obtained:cos 2 (e θ ) c 1 + c 2 sin 2 (e θ ) sin(e θ ) de θ = -K x c 2 (c 1 c -1 2 + 1)dt K o (D.4)Eq. (D.4) can be written as:cos 2 (e θ ) c 1 sin(e θ ) -c 2 sin(e θ ) cos 2 (e θ ) c 1 c 1 + c 2 sin 2 (e θ ) de θ = -K x (c 1 + c 2 )dt K o (D.5)Using trigonometrical properties in eq. D.5, it is obtained:cos 2 (e θ )de θ c 1 sin(e θ ) -sin(e θ ) cos 2 (e θ )de θ c 1 c 1 c -1 2 + 1 -cos 2 (e θ )Regrouping terms in eq. (D.6), it can be written as:de θ sin(e θ ) + c 3 d cos(e θ ) 2 [c 3 + cos(e θ )] + c 3 d cos(e θ ) 2 [c 3 -cos(e θ )] = -K x c 1 c 2 c 2 3 dt K o (D.7) APPENDIX D. ANALYTICAL SOLUTION OF CONTROL LAW EQUATIONS Finally, integrating eq. (D.7), it is obtained eq. (eq:diffequaResolTTx) ≡ (3.43): de θ sin(e θ ) + c 3 d cos(e θ ) 2 [c 3 + cos(e θ )] + c 3 d cos(e θ ) 2 [c 3 -cos(e θ )] = -K x c 1 c 2 c 2 3 dt K o ln (tan(e θ /2)) + c 3 /2 ln (c 3 + cos(e θ ))c 3 /2 ln (c 3 -cos(e θ )) = -K x c 1 c 2 c cos(e θ ) c 3 -cos(e θ ) DEFINITION OF STABILITY OF A SYSTEM

∃

  δ > 0, c > 0, λ > 0 : |x(0)| ≤ δ ⇒ |x(t)| ≤ c |x(0)| e -λt ∀t ≥ 0 (E.4) E.2/ STABILITY IN THE SENSE OF LYAPUNOV E.2.1/ FIRST METHOD: INDIRECT METHOD

3 .

 3 Align w.r.t. position the two shapes at their centroids.

4 .

 4 Align w.r.t. orientation by rotation. The P d is given by square root of the summed squared difference between the positions of the vertex in two optimally superimposed configurations at centroid size (cf. Fig. F.1).

G. 1

 1 .1/ USEFUL SENSORS Several sensors are mounted in the VIPALAB to obtain vehicle's interoceptive and exteroceptive information about the UGV or its environment [357] (cf. Fig. G.1). The main sensors used for experiments of the proposed control architectures are described below:

•

  Inertial Measurement Unit (IMU): Xsens Mti10 IMU sensor is embedded at the middle of the rear-axes of the VIPALAB. The IMU combines the information of accelerometers and gyroscopes to provide the angular and linear acceleration information along the three axes. It allows to obtain the yaw information (rotation along the axis perpendicular to the ground plane). The output frequency is up to 2 Khz with an accuracy of 0.2 • /s and latency is less than 2 ms.

Figure G. 1 :

 1 Figure G.1: VIPALAB with all sensors with their mounting locations and characteristics.

G. 1

 1 .2/ STRUCTURED ENVIRONMENT The test platform named Plate-forme d'Auvergne pour Véhicules INtelligents (PAVIN) is located at Campus Cézeaux of Blaise Pascal University in Clermont-Ferrand (cf. Fig. G.2). PAVIN lies between the faculties of ISIMA and Polytech. PAVIN is an artificial environment composed by two areas (urban and rural areas) which have a total ground surface of 5.000 m 2 which serves as a testbed for mobile robotic applications. The urban area has a trajectory of 317 m containing scaled street with several traffic junctions and roundabouts with traffic sign boards and lights wherever necessary. Moreover, building facades on both sides, vegetation and street furniture are set to bring to a whole scene. The rural area has a trajectory of 264 m with unpaved roads, grass and mud on the roadsides. In addition, the whole area is covered by a wireless network and a DGPS base station [357].Although PAVIN is a small scale environment, it stands as an ideal platform for evaluating algorithms related to autonomous driving such as navigation, road detection, traffic signal detection, etc. A 2D and a textured 3D model of PAVIN environment geo-referenced with high-precision GPS data are available in[357].

Figure G. 2 :

 2 Figure G.2: PAVIN experimental platform (Clermont-Ferrand, France).

Figure G. 3 :

 3 Figure G.3: Class diagram of the proposed control architecture implementation in each VIPALAB.

  3.5 Vehicle's and target's configuration in Global (X G Y G ) and Local (X m Y m ) reference frames. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6 Control variables of Target reaching block. . . . . . . . . . . . . . . . . . . . 3.7 Flowchart of the Hierarchical action selection block. . . . . . . . . . . . . . 3.8 Internal block diagram of the Perception block. . . . . . . . . . . . . . . . . 3.9 Range sensor model and its set of n points. . . . . . . . . . . . . . . . . . . 3.10 Flowchart of the Enclosing data with an ellipse block. . . . . . . . . . . . . . 3.11 Obtained ellipse using the proposed Heuristic approach. . . . . . . . . . . . 3.12 Evolution of the ellipse using Heuristic, Covariance and Least Square approaches. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3.13 Parameters of the estimated ellipses. . . . . . . . . . . . . . . . . . . . . . . 3.14 Example of the application of the proposed enclosing ellipse method. . . . 3.15 Limit vehicle configuration for tuning controller parameters. . . . . . . . . . 3.16 Flowchart of the analysis to obtain d i . . . . . . . . . . . . . . . . . . . . . . 3.17 Vehicle modeling in Frenet frame. . . . . . . . . . . . . . . . . . . . . . . . . 3.18 Trajectory of the leader and followers (Frenet control

Table 1 .

 1

	1: Comparison between architectures
	Deliberation	Reactive
	Slow response velocity Fast response velocity
	Cognition	Reaction
	World modeling	No modeling

Table 2 .

 2

	1: Comparison between MRS architectures [188].
	Perception Localization	Decision	Action
	Centralized

  .46) Using equation 3.44 and a fixed value of K, it is immediate to compute the time t f necessary to obtain e θ = E angle . If t > t f then e θ will be certainly ≤ E angle (cf. Fig.3.21). RT = 0 and e θ = 0 (straight line to the target). Obviously, a greater value of d(t = 0) > d i increases the certainty that the vehicle (in extreme configuration, as depicted in Fig.3.15), could reach the target with appropriate e θ ≤ E angle (cf. Fig.3.21).

Secondly, let us use the fixed time t f to determine d i which permit to guarantee always d(t f ) ≤ E dis and e θ (t f ) ≤ E angle at final time t f for any initial vehicle configuration respecting: e θ i , e RT i < π/2. For a fixed navigation time t = t f , the maximal possible initial distance d i permitting to reach the target, is given when the vehicle's initial configuration corresponds to: e

Table 3 .

 3

	Proposed control	12.75 s	3.71 s
	Frenet control	15.28 s	3.28 s
	Chained system	15.03 s	5.9 s

1: Comparison with dynamic target position (cf. Fig. 3.6) Time [s] to reach: d T arget < 0.15 m |e θ T arget | < 5 •

Table 3 .

 3 2 shows the convergence time to satisfy the error threshold in distance y i and orientation θ with regards to the target trajectory (Frenet reference frame X f Y f (cf. Fig.3.17)). The Proposed control law has the smallest convergence time to satisfy simultaneously both threshold errors. There is only a small difference (0.03 s) compared to Frenet control while the proposed control law uses only the current pose of the target (thus more flexible).

	Table 3.2: Comparison of the errors defined according to Frenet frame (cf. Fig. 3.17)
	Frenet control	2.93 s	3.61 s
	Chained system	11.34 s	10.15 s

Time [s] to reach: |y i | < 0.15 m | θ| < 5 • Proposed control 2.82 s 3.58 s

Table 4 .

 4 1) than the method OMWS-GM which does not consider the orientation neither the UGV's model. To avoid a large growing of the tree branch of OMWS-ET, a position and orientation comparison between nodes can be added at line 12 of the Algorithm 3. If two nodes from different branches have the same position and orientation then the node with lowest total cost value (cf. eq. (4.3)) is stored and the other node is removed.

		n p length [m] d border [m]
	OMWS-GM 27	77.22	52.1268
	OMWS-ET	19	77.50	56.7217

Table 4 .

 4

1: Comparison between the OMWS-GM and OMWS-ET.

  This section presents two experiments to demonstrate the efficiency of the proposed navigation strategy through successive waypoints (cf. Section 4.2) and the optimal selection of a set of waypoints based on expanding tree OMWS-ET (cf. Subsection 4.3.3.2) in structured environment. Subsection 4.4.1 focuses on the planning aspects using OMWS-ET and subsection 4.4.2 provides a complete experiment of the overall control architecture for vehicle's navigation in an urban environment containing hinder obstacles. These experiments were performed using a VIPALAB in PAVIN platform (Plate-forme d'Auvergne pour Véhicules INtelligents) (cf. Appendix G.1). The used UGV's parameters, constraints, sensors and controller parameters were described in Section 3.6. A metric map of PAVIN

Table 4 .

 4 3: Comparison among the set of waypoints possible from the road borders. It can be noticed by d border where its value is bigger in the first experiment than the second. Furthermore, the values of Δγ is less in the second experiment because the highest priority was for the steering angle rate. Therefore, the vehicle can navigate with higher velocity along the trajectory and the navigation time is smaller than the first experiment.

		n w T [s] l UGV [m] d border [m]	Δγ[ • ]
	1rst	Sim. 41	200	132.81	67.35	0.3123
	exp. Real 41	203	132.68	67.25	0.2945
	2nd	Sim. 39	199	133.00	66.54	0.3089
	exp. Real 39	198	132.79	66.64	0.2922

Table 5 .

 5

	5	2.0

1: Procrustes distances and maximum distance for different velocities and dimensions.

Table 5 .

 5 2: Distances and orientation errors for different velocities and dimensions.

		5	2.0	1.0	1.5	2.0
	F 1	0.2835 3.2239 0.3591 3.2879 0.4272 3.3738
	F 2	0.2845 3.2804 0.3604 3.3487 0.5259 3.6247
	F 3	0.2863 3.3916 0.3624 3.4251 0.8082 4.5671

Table 5 .

 5 3: Procrustes distances and maximum distance for different velocities and dimensions 5.3.3.2/ FRENET FORMATION This simulation shows the proposed adaptive constraints applied to Frenet formation. The used scenario is the same as in Subsection 5.2.3.2, a group of UGVs navigating in triangular formation F 3 w.r.t. leader's trajectory with v L = 2 m/s.

	s]	2.0	2.0
	F 3	0.3703	0.2903

Table 5 .

 5 4 shows the values of the L 2 (cf. Appendix F) norm applied to the evolution of d rms and e θ rms . As in the Cartesian formation case (cf. Subsection 5.3.3.1), this table demonstrates that the use of the proposed adaptive constraints designed for Frenet formation allows to keep formation w.r.t. leader's trajectory during the navigation even when for fast leader's dynamics (e.g., Table5.2 shows the navigation without using the adaptive constraints).

		L 2 (d rms ) [m] L 2 (e θ rms ) [ • ]
	v[m/s]	2.0	2.0
	F 3	0.4423	3.8867
	Table 5.4: Distances and orientation errors for different velocities and dimensions
	5.3.4/ SIMULATION AND EXPERIMENTAL VALIDATION OF THE WHOLE CONTROL
	ARCHITECTURE FOR NAVIGATION IN FORMATION

  This subsection proposes a new Strategy for Formation Reconfiguration (SFR) based on suitable smooth function between different virtual target configurations (cf. Subsection 5.4.1 and 5.4.2). These strategies allow to obtain a fully reactive architecture in the sense that followers track the instantaneous state (pose and velocity) of their virtual targets (thus, without any use of a reference trajectory or a trajectory planning process). Additionally to the reconfiguration process, one should manage potential collisions between UGVs and allocation of virtual targets to UGVs (cf. Appendix B.3).

  Using the minimum distance between targets d T min and eq. (5.45), then eq. (5.49) can be expressed as an inequality:

	e T f 12 m 12 e f 1 + e n f 12 ) =e T f 12 e f 12 ≥ e T f 12min e f 12min
	e T f 12 m 12 e f 1 + e n f 12 ) ≥d 2 T min	(5.50)
		(5.51)
		5.47)
	Using eq. (5.44) in eq. (5.47), it is obtained:
	e T f 12 (a 1 + a 12 )e f 1 + (a 12 -a 2 )e f 2 =0
	e T f 12 (a 1 + a 12 )e f 1 + (a 12 -a 2 )(e f 1 + e f 12 -e n f 12 ) =
	e T f 12 (a 1 -a 2 + 2a 12 )e f 1 + (a 12 -a 2 )e f 12 -(a 12 -a 2 )e n f 12 ) =0	(5.48)

Defining m 12 = (a 1a 2 + 2a 12 )/(a 2a 12 ) and replacing in eq. 5.48, it is obtained:

e T f

12 (a 1a 2 + 2a 12 )e f 1 + (a 12a 2 )e f 12 -(a 12a 2 )e n f 12 ) =0 e T f 12 m 12 e f 1 + e n f 12 ) -e T f 12 e f 12 =0 (5.49)

  The proposed method selects the suitable set of waypoints analyzing the orientation change at each point of the trajectory (cf. Subsection 4.3.2). The second and third methods are applied in a well-known environment (cf. Subsection 4.3.3). They are optimal multi-criteria waypoint selection (OMWS) based on Grid Map (GM) and Expanding Tree (ET). The multi-criteria optimization allows to take into account the safety, feasibility, velocity, steering angle and kinematic constraints of the UGV. OMWS-ET presents more advantages than OMWS-GM and others methods from literature such as less number of waypoints, smoother, safer and shorter trajectories (cf. Subsection 4.3.4).5/ SUITABLE COORDINATION OF MULTI-VEHICLE NAVIGATION IN FORMATIONThe proposed control architecture have been extended to cope with the navigation in formation of a group of UGVs where each UGV has to keep a desired distance and orientation w.r.t. the others with the property of dynamic reconfiguration of the formation according to environmental context (narrow tight corridor or a very cluttered environment).

  1/2 -(A lc x s + B lc 2 y s) + y s V 1/2 = -2V 1/2 (2A lc x s + B lc y s )(C lc y s + B lc 2 x s ) -(2C lc y s + B lc x s )(A lc x s + B lc 2 y s ) -2V x s (2A lc x s + B lc y s ) + y s (2C lc y s + B lc x s ) = -4V A lc x 2 s + C lc y 2 s + B lc x s y s = -4 1 -A lc x 2 s -B lc x s y s -C lc y 2 s 2A lc x 2 s + B lc x s y s + C lc y 2

	s

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

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

(b) (c)

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

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

ACKNOWLEDGEMENTS

The presented thesis was developed at Institut Pascal in the Modeling, Autonomy and Control in Complex system (MACCS) team of ISPR (ImageS, Perception systems and Robotics) group. I would like to express my sincere gratitude to this team for giving me the opportunity to particpate and collaborate in meaningful research trends. Without the LIST OF MY PUBLICATIONS

where (x , y ) are the coordinates of the point (x, y) w.r.t. Local (X L Y L ) reference frame. They are computed as:

x y = cos(Ω) sin(Ω) -sin(Ω) cos(Ω)

xh yk (A.

A.3/ CONIC REPRESENTATION

The ellipse can be represented by a general quadratic form (substituting eq. (A.3) in eq. (A.2) and expanding all terms). This general equation can describe different conic forms (circle, ellipse, hyperbole and parabola). It is given by:

where A, B, C, D, E, F ∈ R An ellipse is define if B 2 -4AC < 0. To avoid degenerated ellipse (imaginary or parallel lines), it is also required:

A. 4

/ CONVERSION BETWEEN REPRESENTATIONS

This section presents the methods to obtain the equivalence between the described representations.

A.4.1/ PARAMETRIC TO CONIC REPRESENTATION

The parameters of eq. (A.4) are used to compute the terms of eq. (A.5) as follows:

A = (a sin(Ω)) 2 + (b cos(Ω)) 2 (A. The terms of eq. (A.5) are used to obtain the parameters of eq. (A.4) as follows:

where the matrix M 0 and M are defined as:

A.5/ COMMON METHODS FOR ENCLOSING DATA WITH AN ELLIPSE

This section shows two common methods in the literature to enclose data with an ellipse.

A.5.1/ FITTING USING LEAST SQUARE METHOD

This approach uses the general conic equation which is given by:

According to the real constants A, B, C, D, E anf F, the analytic equation of the different kind of conics (parabola, ellipse and hyperbole) is obtained. An ellipse is defined if the conic parameters (A.20) satisfy the following condition B 2 -4AC < 0.

The problem is to fit a conic section (A.20) with a set of n points

As the data are noisy, it is unlikely to find a set of parameters (A, B, C, D, E, F) (except for the trivial solution

Instead, we will try to estimate them by minimizing some objective function J.

A common practice is to minimize the algebraic distance f (x i , y i ), the least-square fitting based on algebraic distance [START_REF] Zhang | Parameter estimation techniques: A tutorial with application to conic fitting[END_REF] is used to minimize the following function:

Note that there is no justification for using algebraic distance apart from easy implementation. To avoid the trivial solution, f (x i , y i ) should be normalized. the normalization A + C = 1 is used because it is one of the most popular methods proposed in the literature [START_REF] Zhang | Parameter estimation techniques: A tutorial with application to conic fitting[END_REF], and its implementation is easy.

Using the normalization in (A.20), the set of equations f (x i , y i ) can be written as:

The solution that minimize the functional J is given by

where

This method is known as the pseudo inverse technique.

A.5.2/ COVARIANCE METHOD

This approach is based on the analysis of the relationship between the n points. The covariance of the data and the Mahalanobis Distance (MD) [START_REF] De Maesschalck | The mahalanobis distance[END_REF] are used. In the field of multivariate calibration, the MD is used for different purposes, namely: for the detection of outliers, the selection of calibration samples from a large set of measurements and for investigating the representativity (matching) between two data sets. In the original variable space, the MD takes into account the correlation in the data, since it is calculated using the inverse of the covariance matrix of the data set of interest.

The principle of covariance approach is given below: First the covariance matrix C x is constructed:

where X c is the column-centered data matrix (X -X). X is the data matrix containing n objects in the rows measured for p variables and X is the data mean. Now, the covariance matrix C x is analyzed using the eigenvalues and eigenvectors to obtain the parameters of the ellipse f (x , y ) (cf. eq. (3.1)). Furthermore, the Mahalanobis distance is used to select the dimension of the semi-axes (percentage of the data inside the ellipse) [START_REF] De Maesschalck | The mahalanobis distance[END_REF]. The eigenvalues λ 1 , λ 2 (λ 1 > λ 2 ) are related to the semi-axes a and b as follows:

where MD max is the maximum Mahalanobis distance to ensure that all the set of data is enclosed by the ellipse. The eigenvectors v 1 , v 2 are related with the orientation Ω.

The center of the ellipse is the mean of the data X and the maximum semi-axes a is the direction where there are the most important amounts of data.

Choose x rand from C-space f ree 5:

Choose

Compute u new which drives the UGV from x nearest to x new x new is the closest state to x rand 7:

Obtain neighbor x near of x new 9:

Select the closest node x min (parent) among x nearest and x new to x near 10:

Insert vertice x new and the edge x newx min in T = (V, E)

Rewiring T = (V, E) if the cost through x new is less than older costs.

12:

end if 13: end for 14: Obtain the T = (V, E) from x(0) to x(T ) sor RRT (and its various other improved versions [START_REF] Abbadi | Rrts review and options[END_REF]).

B.3/ COLLISION BETWEEN UGVS AND OBSTACLES

Collisions between UGVs can occur during the reconfiguration phase of the group of UGVs. To address this collision risk, we use a penalty function acting on the linear velocity of the UGVs [START_REF] Benzerrouk | Architecture de Contrôle Hybride pour les Systèmes Multi-robots Mobiles[END_REF], [5]. Moreover, this reduced velocity of UGVs allows to obtain a smooth and less oscillating vehicles' movements. Each UGV is enclosed by two circle C int and C ext with respectively radius R int and R ext (R int < R ext ) (cf. Fig. B.2). The collision occurs when the distance d i j between UGV i and UGV j are less than R int . Hence, the penalty function ψ j i for the UGV i w.r.t. the UGV j is defined as:

The modified linear velocity of the UGV i is then given by:

Using the definition of R int i (where R int i R int j ), it is guaranteed that two UGVs do not stop simultaneously. Indeed, if the UGVs have the same R int i we can observe local minima in certain configurations, in fact, when d i j < R int i then ψ j i = ψ i j = 0 and the robots are stopped at the same time. R ext is designed according to communication constraints (latency) and localization errors (GPS). This penalty function can be straightforward integrated to our control architecture (cf. Fig.

The properties of the stability of systems are expressed as follows:

Theorem 2 . First method of Lyapunov:

1. If all eigenvalues of the J matrix have a strictly negative real part, then the system is exponentially stable.

2.

If the J matrix has at least one eigenvalue with a strictly positive real part, then the system is unstable.

If the system has at least one eigenvalue with a zero real part and any eigenvalue with a strictly positive real part, then any conclusion about the stability could be obtained. The system can be analyzed by the second method of Lyapunov.

E.2.2/ SECOND METHOD: DIRECT METHOD

This method consists of an mathematical interpretation of an elementary observation: If the total energy of a system decreases/dissipates along time continuously, then the system tends to an equilibrium point, i.e., the system is stable. The idea is thus to find a temporal positive define function which allows to have always a define negative derivative. This direct method is summarized in the next theorem Theorem 3 . Second method of Lyapunov: The equilibrium point is stable if exists a V function continuously differentiable and its derivative denoted by V satisfies:

) is replaced by V(x) < 0 then the system is asymptotically stable.

E.3/ ELLIPTICAL LIMIT-CYCLE

In the dynamical systems, there exists attractor and repellor points, i.e., stable or unstable nodes. Another type of attractor or repellor for nonlinear systems are the limit-cycles.

In what follows, it will be show that the differential equations (3.4) and (3.5) described in Section 3.1 represent a dynamical system which has a stable elliptical limit-cycle. Moreover, it is easily seen that the only equilibrium point of the system occurs at the origin.

The following Lyapunov function is considered:

G SIMULATION AND EXPERIMENTAL TESTBED

This appendix described the main elements for the implementation of the proposed control architecture for navigation in formation of a group of UGVs. The elements of the used platform and the structured environment are described in the next section. Section G.2 presents the used simulator software and the interface to perceive and command the UGVs. The structure of the implementation of the control architecture embedded in each UGV is explained in Section G.3.

G.1/ PLATFORM AND SCENARIOS

The Véhicule Individuel Public et Autonome (VIPALAB) from APOJEE company [357] is a platform dedicated to the development of autonomous vehicles. This urban electric vehicle VIPALAB is used to implement several proposed control architectures for autonomous navigation of one and a group of UGVs [4,[START_REF] Vilca | A novel safe and flexible control strategy based on target reaching for the navigation of urban vehicles[END_REF][START_REF] Vilca | Optimal multi-criteria waypoint selection for autonomous vehicle navigation in structured environment[END_REF]. Some specifications of VIPALAB are shown in Table G. 1 [357]. This vehicle carries different embedded proprioceptive and exteroceptive sensors such as cameras, odometers, IMUs, steering angle sensor, an RTK-GPS, a Wi-Fi communication system and a computer (more details are given in [357]). The VIPALAB can be controlled using the on-board computer (through CAN protocol) or the wired control panel attached to the vehicle.

G.2.2/ EFFIBOX MIDDLEWARE

Effibox is a middleware which access and record process from real time sensor data. This middleware was developed by Effidence [416]. The main characteristic are multithread, open C++ API (application programming interface), several certified commercial sensors (camera, GPS, LIDAR, Sonar, Bus CAN, etc.) and robotic platforms (Jaguar, VIPALAB, RobuFAST, Camaleon, etc.). Moreover, a recorded data allows a real-time replaying according to data acquisition from sensors. This recorded data can be exported to scientific softwares such as MATLAB .

G.3/ IMPLEMENTATION OF THE CONTROL ARCHITECTURE

The proposed control architecture and each functionality (perception, localization, control, etc.) for autonomous navigation has been implemented using C++. A class diagram summarizing the main programming functions and the interaction between them is given in These sensors provide enough accurate data w.r.t. the vehicle dynamic. Indeed, in these experiments, the vehicles move at maximum velocity of 2.5 m/s due to the short dimensions of the used urban platform (cf. Fig. G.2). The LIDAR is a very useful sensor to detect obstacles, nevertheless, according to the environment (e.g., PAVIN), the LIDAR can detect the floor and white lines on it. This detection of floor can generate an incorrect application of obstacle avoidance. To avoid this situation, the LIDAR data was filtered

Abstract:

Beyond the interest of robotics laboratories for the development of dedicated strategies for single vehicle navigation, several laboratories around the world are more and more involved in the general challenging field of cooperative multi-robot navigation. In this context, this work deals with the navigation in formation of a group of Unmanned Ground Vehicles (UGVs) dedicated to structured environments.

The complexity of this Multi-Robot System (MRS) does not permit the direct use of neither classical perception nor control techniques.

To overcome this problem, this work proposes to break up the overall control dedicated to the achievement of the complex task into a group of accurate and reliable elementary behaviors/controllers (e.g., obstacles avoidance, trajectory tracking, target reaching, navigation in formation, formation reconfiguration, etc.). These behaviors are linked to different information given by the sensors to the actions of vehicles. To guarantee the performances criteria (e.g., stability, convergence, state errors) aimed by the control architecture, the potentialities of hybrid controllers (which controlling continuous systems in the presence of discrete events) are considered. This control architecture is validated for a single vehicle to perform safe and flexible autonomous navigation using an appropriate strategy of navigation