Suhyeon Gim

Lounis Adouane

Sukhan Lee

Jean-Pierre Derutin

Parametric Continuous Curvature Path for Smooth Steering with Car-like Vehicles

This paper proposes a solution to obtain parametric continuous curvature path for nonholonomic car-like vehicle and addresses also efficient algorithm to obtain an appropriate reference path for smooth path following. As a local planner using parametrically adjustable clothoid, the proposed solution, of constant sharpness, permits to obtain continuous curvature and thus smooth steering behavior of the vehicle which enhances the passenger comfort. The proposed parametric continuous curvature path is integrated in a global planner which includes an appropriate way of isolating the boundary conditions of each local planner. This last characteristic is useful to reconstruct a smooth reference path from raw data of actual vehicle trajectory. The solution is applied to a real road datasets and tested using Lyapunov based controller for validating the effectiveness on path following performance. The simulated results show reliable and enhanced performance for vehicle path following when the proposed algorithms are used.

Introduction

Autonomous navigated vehicle has attracted a lot of researchers during more than a decade. Its fundamental technologies on commercial usage are also growing with the innovative computational and systematic performance. The path generation or trajectory generation in the autonomous navigated vehicles is the core technology which is still under development and much attentions are paid to its practical implementation for assuring the passenger comfort with safe performance. The path generation for the nonholonomic car-like vehicle makes the path following be harder and difficult to reorient due to its nonholonomic characteristic with kinematic and dynamic constraints. There have been a lot of smooth path generation methods for the vehicle and the continuous curvature path has been focused for its close relationship with vehicle parameters and driving comfort [START_REF] Montes | Trajectory generation based on rational Bezier curves as clothoids[END_REF][START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and Clothoid based methods and its simulation[END_REF] The smooth path for the nonholonomic vehicle have been investigated by numerous groups. Dubins [START_REF] Dubins | On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents[END_REF], Reeds-Shepp [START_REF] Reeds | Optimal paths for a car that goes both forwards and backwards[END_REF] and Fraichard-Scheure [START_REF] Fraichard | From Reeds and Shepp's to continuous curvature paths[END_REF] are prominent pioneering works and its advanced model are carried out by other groups such as [START_REF] Girbes | On generating continuous-curvature paths for line following problem with curvature and sharpness constraints[END_REF][START_REF] Choi | Curvature-continuous trajectory generation with corridor constraint for autonomous ground vehicles[END_REF][START_REF] Villagra | Smooth path and speed planning for an automated public transport vehicle[END_REF][START_REF] Brezak | Real time approximation of clothoids with bounded error for path planning applications[END_REF]. The path is composed by clothoids, straight lines and circular arcs segments for its continuity of curvature under the parametrical constraints. [START_REF] Fraichard | From Reeds and Shepp's to continuous curvature paths[END_REF] solved the continuous curvature steering problem for car-like vehicle with constraints of upper bounded curvature and upper bounded curvature rate (or sharpness) which extends the Reeds and Shepp's path [START_REF] Reeds | Optimal paths for a car that goes both forwards and backwards[END_REF]. The authors in [START_REF] Parlangeli | Dubins inspired 2d smooth paths with bounded curvature and curvature derivative[END_REF] proposed an infinitely differentiable smooth paths which approximate Dubin paths with bounded curvature and sharpness. The work proposed in [START_REF] Montes | Trajectory generation based on rational Bezier curves as clothoids[END_REF] tries to solve the overtaking problem in the road based on clothoidal trajectories by approximating the rational Bézier curves, it used the homotetical factor, adjusted by scalable parameter and control points, however the solution needs a large amount of calculation for multi dimensional linear algebraic equation to find the Bézier control points. It is also difficult to acquire the curvature information from the resultant curve. [START_REF] Walton | A controlled clothoid spline[END_REF] addresses the clothoidal segments problems in order to satisfy the two already defined end configurations using rigorous mathematical analysis and while proving the solvability and solution uniqueness. However, the proposed solution and its algorithm lack on the curvature and sharpness parameters analysis and thus, it is hard to get the curvature diagram from the solution. Moreover, a paired clothoids are managed by control points which should be pointed manually by human designer. One of the most noticeable works to be referenced is [START_REF] Wilde | Computing clothoid segments for trajectory generation[END_REF]. It presented a simple and fast trajectory generation method of continuous curvature with minimum sharpness as human natural and safe driving. However, the solution was limited only for the lane change maneuver example. For the path following issues, [START_REF] Solea | Trajectory planning with velocity planner for fullyautomated passenger vehicle[END_REF] produces a smooth trajectory in the respect to the standard human comfort while following the trajectory using sliding mode control. The trajectory comes from the predefined quintic polynomial but, the presented results have different steering angle discontinuous. It could be expected that the system may suffer from other errors in the real vehicle experiments linked to these discontinuouty in the steering set-points. Authors in [START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and Clothoid based methods and its simulation[END_REF] propose smooth trajectory using cubic and trigonometric splines. The obtained control results were compared to a clothoidal planning while considering human comfort criteria. It concluded with the remark that the use of the clothoid curves is better for the vehicle performances but it is much more complex and harder to obtain these clothoids (mainly when the modification of the trajectory occurs). In [START_REF] Avanzini | Manual convoying of automated urban vehicles relying on monocular vision[END_REF], automatic multi-vehicle navigation for urban transportation is proposed. Vehicles shares a common reference trajectory with the compensation of the localization by outward sensors. The used trajectories was locally optimized by B-spline interpolation method. Artificial Potential Field methods [START_REF] Khatib | The potential field approach and operational space formulation in robot control[END_REF] are among the most interesting and really broadcasted work in the literature for path planning. Authors in [START_REF] Mouad | Mobile robot navigation and obstacles avoidance based on planning and re-planning algorithm[END_REF] used this technic for planning and re-planning one mission for a group of mobile robots which evolve in structured environment to attain different way-points. Even if the obtained results gave an interesting tool for making a decision (which trajectory to take by each robot) in complex context as the multi-robot and multi-target mission, the obtained trajectories did not take into account the different robots' structural constraints and thus the planned missions can not be reached, in certain configurations. This paper proposes a solution of Parametric Continuous Curvature Path (pCCP) using algorithm which adjusts the clothoid's parameters. The proposed methodology is applied to the well-defined local boundary conditions [START_REF] Fraichard | From Reeds and Shepp's to continuous curvature paths[END_REF][START_REF] Meek | An arc spline approximation to a clothoid[END_REF] and then implemented for an effective path following problem. The proposed solution aims at reconstructing one reference path, as close as possible from the original path, permitting thus the smoothing and/or slightly replanning of the already predefined path with one which be close and with continuous curvature.

Therefore, the proposed pCCP algorithm could be useful for making a reference path or reconstructing one from an already obtained raw data in the global frame work [START_REF] Villagra | Smooth path and speed planning for an automated public transport vehicle[END_REF][START_REF] Solea | Trajectory planning with velocity planner for fullyautomated passenger vehicle[END_REF]. To validate the reliability of the obtained paths, these path was followed by a robot while using Lyapunov based controller [START_REF] Canudas | Theory of Robot Control[END_REF][START_REF] Deluca | Feedback control of a nonholonoic carlike robot(Robot Motion Planning and Control)[END_REF]. The simulated results show reliable and better path following behavior using obtained smooth path. This paper is organized as following. In the next section, the path following model for the nonholonomic vehicle and the problem is defined. In section 3, the proposed clothoids solutions for the path generation are given with algorithmic description. In section 4, the smoothing procedure for the application to a practical implementation is addressed. It shows some simulated results. Section 5 gives finally some conclusions and prospects.

2 Path following model for nonholonomic car-like vehicle

Kinematics modeling for path following

Nonholonomic car-like vehicle is driven by acceleration/brake mechanism at rear wheels while it is steered by steering actuator at front wheels. The vehicle has a nonholonomic kinematics model where it is assumed that there exists no rolling contact-slip on the ground surface The steering angle corresponds to the curvature of the robot (cf. Fig. 1).When the vehicle is modeled as a point of the motion center, the nonholonomic vehicle kinematics are represented by following differential form.

ẋ (t) = v (t) • cos θ (t) (1)
ẏ (t) = v (t) • sin θ (t) (2)
θ (t) = v (t) L • tan γ (t) (3)
where, v (t) and θ (t) are the linear driving velocity and orientation angle at time t respectively and x (t), y (t) middle position of rear wheels (where x and y ∈ IR 2 ) represent the position of the vehicle. γ (t) is the steering angle of the vehicle at time t and it is related to the curvature κ (t) where the radius of curvature ρ (t) is defined by the two axis interconnection at the Instantaneous Center of Rotation (ICR). L is the distance between frontal and rear axis. For the description of the whole path, above equations requires the whole information on the velocity and steering angle through the time. Different to the trajectory tracking, the path following requires for the vehicle to converge and follow the path without time specification. The major control is performed thus by its steering wheels (cf. Fig. 1) with generally constant forward velocity. For smoother convergence to the desired path, virtual target projected of the actual vehicle on the path is tackled to bypass some problem of the feedback controller [START_REF] Soetanto | Adaptive, non-singular pathfollowing control of dynamic wheeled robots[END_REF] where the target is defined on Frenet-Serret frame F . Fig. 1 depicts the nonholonomic car-like vehicle based on virtual target for the path following on the global coordinate X -Y. In Fig. 1, the local frame of the vehicle is located on its motion center, P v with orientation θ v and its virtual target frame places on P t with θ t . At P t on the path, the pose is described by curvilinear abscissa s t and it has curvature κ t and radius of curvature ρ t . From above definition, following equations presents the kinematic relations on the Frenet frame F .

ܮ ¡ ߛ ¡ ¢ ߠ £ ߠ ¤ ¥ ߢ ¥ ¥ ¥ ¡ ¦
     ṡe = v cos θe 1-yeκ(se) , ẏe = v sin θ e , θe = v( tan γ L -κ(se) cos θe 1-yeκ(se) ). (4)
In ( 4), F is represented as (s t , y t ) at P t and θ e = θ v -θ t where θ v and θ t is measured counterclockwise from X-axis on the global coordinate. The control strategy for path following must permit to have y e , θ e to converge to 0 while keeping 1 -y e κ(s e ) = 0 . For the strategy of path following based on virtual target, Lyapunov based controller are augmented to extend kinematic control to a dynamic consideration as well as its robustness to the parametrical modelling uncertainties [START_REF] Samson | Control of chained systems: application to path following and time-varying point stabilization of mobile robots[END_REF].

Clothoids representation with basic properties

There is a useful mathematical representation for the nonholonomic vehicle path according to its length (curvilinear abscissa) where it uses clothoid. A clothoid is represented by curvature variation with the length. Fully defined clothoidal form for a path is efficient for car-like vehicle in that it gives information on the curvature along the length and it could also give maneuvering information to the vehicle driver of the vehicle located on its local or moving coordinate frame (cf. Fig. 1). By the definition of clothoid (or Euler spiral) [START_REF] Walton | A controlled clothoid spline[END_REF], the coordinates are consecutively defined along the length s. A clothoid is defined by parametrical forms given by,

κ (s) = κ 0 + αs (5) θ (s) = s 0 κ (u) du (6)
x (s) =

s 0 cos θ (u) du (7)
y (s) = s 0 sin θ (u) du (8)
where, α is the sharpness (or rate of curvature) for curvature κ (s). Equation [START_REF] Fraichard | From Reeds and Shepp's to continuous curvature paths[END_REF] shows that the curvature increase or decrease by constant sharpness α and the orientation in ( 6) changes with integration of curvature in [START_REF] Fraichard | From Reeds and Shepp's to continuous curvature paths[END_REF]. It is to be noted that this mathematical relation is same as the physical relation between the steering angle and the vehicle orientation where the orientation varies in integrated amount of steering changes. It is also noted that the position in coordinate is determined only after the orientation at a length is calculated as in ( 7), [START_REF] Villagra | Smooth path and speed planning for an automated public transport vehicle[END_REF]. This means that to find the sharpness for a clothoid, to meet the desired position and orientation at a certain curvilinear distance, requires to have at least an analytic model of the inverse kinematic solution. This model is unfortunately not available yet in the literature. In fact, the problem is very complex to resolve.

A clothoid has the property to have continuous curvature which is either increasing or decreasing through the length. There are some kinds of formulations for the rate of the curvature such as polynomials, exponential or trigonemetric functions with composition. However, the 1 st order form 5, relying on the constant sharpness, is well-known not only for its computational simplicity but also for phenomenal similarity to the real vehicle actuation system. To be more specific, the curvature of a point on the path corresponds to the steering angle of the vehicle which follows the path at the point and the sharpness signifies the rate of the steering change at that point. Thus, the sharpness is related to the steering actuation which exerted by human handling motion or steering motor system.

To specify the convergence criteria as well as determining initial values for the algorithm, some basic properties or pattern according to the parameter variation are useful to understand. When a clothoid is defined, the relations among the parameters are presented as follows,

κ = √ 2δα, δ = κ 2 2α , s = 2δ α . ( 9
)
where all parameters are the values at the end point of the clothoid and δ means the amount of orientation change through the whole length which is called deflection [START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and Clothoid based methods and its simulation[END_REF]. Note that α, κ, δ and s are closely related to each other and furthermore, if two of them are defined, then the others are also determined. This relation is important to make a clothoid meet a desired pose by adjusting independent two parameters, e.g. (α, κ) or (α, δ). One can find some important properties for the clothoidal segment that the curvature, sharpness, length and deflection are closely related to each other. In the following analysis, some regular and scalable patterns for the parameters conditions could be found. In Fig. 2,

(a) α ↑, δ * (b) α ↑, κ * (c) δ ↑, κ * Fig. 2.
Basic observed properties for parametrical variation on a clothoid one parameter varies whereas the other stay constant which are denoted as ↑ and * respectively. The long (red) arrow in Fig. 2 indicates the varying direction of the clothoid according to its parametrical change. From the observed results, one can find that the shape of the clothoid shrinks or expands as the sharpness α varies. In addition, the position at the clothoidal end moves on shrinking or expanding direction. It is also noticeable that while its shape and position moves, the other parameter imposes the end pose of the segment, i.e. the orientation or curvature. Such a pattern is used as an important property in the proposed algorithm (cf. section 3) to find the solution of the clothoid parameters.

3 Continuous Curvature Path generation

Problem definition and its solution

This subsection deals with the problem of generating a smooth path in local planner for a nonholonomic car-like vehicle. The local planner acts as generating a shorter trajectory or path for the minimum allowable distance in the configuration defined by boundary condition. The configuration P is represented by four variables x, y, θ and κ that determines the vehicle motion along the path, which correspond respectively to x and y vehicle position, orientation and the curvature at that position.

For pCCP generation, clothoid is the major component on the trajectory and each clothoid is composed with other segments satisfying pose configuration with attributed constraints. Well-defined path of continuous curvature guides the vehicle to follows the path with smoothing steering pattern. For pCCP as a local planner, it requirs at least two clothoids to satisfy the both end configurations where one clothoid increases from initially zero curvature and then the other decreases to final zero curvature. The first clothoid is by C 1 and the second by C 2 and their composition is defined by C 1 ⊕ C 2 . This composition must satisfy the orientation continuity G 1 as well as curvature continuity G 2 where G n is n-th order of Geometric Continuity [START_REF] Piazzi | Quintic g2-splines for trajectory planning of autonomous vehicles[END_REF]. In this work, we make a focus on the path generation for a car-like vehicle in a road environment.

Problem: From P i (x i , y i , θ i , κ i ) to P f (x f , y f , θ f , κ f ) passing through a point P c at its maximal curvature κ max , with κ i = 0 and κ f = 0, find the minimum number of clothoids which satisfy the both configurations with curvature continuity along the path. Proposition 1. If the both end poses could not be accessed by clothoidal parametric variation, straight line or circular arc segments are added with the conformity on the orientation G 1 and curvature continuity constraint Fig. 3(a) depicts the defined problem. The initial pose P i and the final pose P f are denoted by red arrows and the extension lines are displayed from each pose as ℓ i and ℓ f respectively. While fulfilling the pose configuration at both ends, clothoid segment C 1 and C 2 (∀s ∈ [s 0 , s l ]) are generated from the point p c with same orientation and curvature to meet ℓ i and ℓ f at P i and P f respectively, where the inner-tangential circle r c with a radius ρ c and the center c c . For above case of π 2 < |θ i -θ f | < π, the minimal number of clothoid is two as proven in [START_REF] Walton | A controlled clothoid spline[END_REF]. Note that p c is added to satisfy the solution. With the initial pose and final pose configuration defined in the problem, there are numerous solution to converge to ℓ i or ℓ f . However, to make the reconstructed path closer to the original path (cf. Fig. 5(a)), additional constraint is required. In our problem, the objective is fulfilled by adding the maximal curvatured point p c (κ max ). The position p c in the path gives an information on the vehicular steering behavior such that until this position, the steering rotate up to its maximal turn. Then it turns back to its middle center where former part is represented by the first clothoid and later part is expressed by the second clothoid. Another importance in fixing p c corresponds to when a vehicle path includes some region of constant angle of steering which is drawn as an circular arc segment. As usual, this steering behavior is shown at the region of highly curvatured road geometry section such as road intersection. In this case, the resultant solution only only two clothoids withot any circular arc segment could contain large differences with regard to the original path.

G 2 . § ℓ § ߩ ¨© ℓ © © ߠ ߩ ! " ߠ # $% $& (
Fig. 3(b) shows the case where an circular arc segment is included between two clothoids. This solution is imposed in the cases where the both end positions are too close or/and the maximal curvature constraint is reached. Note that with the same configurations, the maximum curvature as given in Fig. 3(a) is greater then the case in (b) while it explicitly true if ρ c < ρ ′ c . This case is mainly useful for reconstructing the origianl reference path having regions of constant curvature as dealt with in the demonstrative example given in 3.

The proposed methodology to resolve the presented problem is firstly to determine p c with ℓ i and ℓ f from P i and P f . The determination of p c is described in the procedure given in Algorithm 2. By adjusting α 1 and δ 1 , C 1 is generated until its end point C 1 (s 0 ) to meet ℓ i for the distance to converge within a small constant threshold value defined by designer as D e 1 < ε ( ε is a very small positive contant). After the convergence of C 1 to ℓ i , C 2 is generated with its resultant κ 2 and δ 2 on G 1 and G 2 to check C 2 (s 0 ) to be close to ℓ f for D e 2 in ε. In the non-converging case by clothoid parameter variation, arc segment is added as depicted in Fig. 3(b). In order for C 1 and C 2 to converge to each configuration, the circular arc segment p c p c1 , p c p c2 are included to make C r by controlling its circular arc angle θ c1 , θ c2 where C r = ρ ′ c (θ c1 + θ c2 ) and θ c2 is determined as the same way as θ c1 . Until the convergence of both segments to end configurations, the iterative variation on the parameters are performed for α 1 , δ 1 , θ c1 and θ c2 .

The detailed procedure for finding clothoids parameters, taking into account the two cases given in Fig. 3, is described in Algorithm 1.

In Algorithm 1, initial sharpness α 1i and deflection δ 1i for C 1 are assumed given by designer before entering the loop.

It is noted that δ 1i is set in the proposed work to

(θi-θ f ) 2
from the fact of δ 1 +δ 2 +θ c = θ i -θ f (G 1 continuity at p c ) with initially θ c = 0, δ 1 = δ 2 . The α 1i is set for C 1 (s 0 ) to be located between ℓ i and p c , i.e. if C 1 (s 0 ) resulting from δ 1i and κ c = 1 ρc is not located in ℓ i and p c , α 1i is doubled until the convergence conditions (line 2, 4 in Algorithm 1) is satisfied. In line 2 to 3, the parameters are varied according to the property found as in Fig. 2 with [START_REF] Brezak | Real time approximation of clothoids with bounded error for path planning applications[END_REF]. The convergence toward the solution is defined in line 4 where D e , the distance between the composed curve C 1,2 and ℓ i,f reach the threshold ε.The solution is always convergent as far as authors experience observed (there is not any encountered counter example

Algorithm 1 two clothoids algorithm

Require: α 1i , δ 1i , ε = 10 -3 , N th = 500 for N = 1

⊲ Positive constants values for C 1 1: procedure Clothoid2(P i , P f , pc) 2:

while D e 2 ≥ ε do ⊲ Distance C 2 (s 0 ) to ℓ f 3: for δ 1 ← (δ 1 + dδ) do 4: while D e 1 ≥ ε do ⊲ Distance C 1 (s 0 ) to ℓ i 5:
for α ← (α + dα) do 6:

κ 1 ← f 1 (α 1 , δ 1 )
⊲ C 1 generation with (9) 7:

D e 1 ← ℓ i , C 1 (s 0 )
8:

N = N + 1 9: if (N > N th ) then ⊲ N th :
κ 2 = κ 1 ⊲ G 2 continuity 15: δ 2 ← δ 1 , θ i , θ f ⊲ G 1 continuity 16: α 2 ← f 2 (κ 2 , δ 2 )
⊲ C 2 generation with (9) 17:

D e 2 ← ℓ f , C 2 (s 0 )
18:

end for

19:

end while 20:

C ← C 1 ⊕ C 2 ⊲ Composition: ⊕ 21: return α sol , C(α sol )
⊲ Solution attained 22: end procedure which do not lead to the desired configuration). Nevertheless, the convergence time depend obviously on the values of the set values of dα, dδ and dθ. This work is focusing its purpose on reconstructing a pre-obtained path and thus is not optimized for real-time planning. Its current convergence, while setting dα = 10 -2 , dδ = 10 -1 and dθ = 10 -1 requires at least 20 to 50 iterations with more than a few seconds depending on the configurations. The efficiency linked to the rate of convergence remain an issue which will be addressed in near future.

Global smooth path reconstruction

To obtain autonomous navigation of vehicles, it is important to have smooth path, in the sens that frequent changes (or jerking) of the vehicle steering could increase undesirable noises and vibrations of the vehicle. These drawbacks, could cause the passenger discomfort but also some danger in high speed or at high curved road. In the proposed work, the smooth trajectory generated by continuous curvatures is applied to a test-bed of actual road environment. One of the performance to be evaluated is to smooth the real data acquired from the vehicle and then let it follow by a standard controller with comparative purpose.

To evaluate the proposed global smooth path reconstruction(cf. Algorithm 2), we will use in this set-up a raw date obtained by actual vehicle navigating in real environement. The points localization of the vehicle are recorded along its path. Algorithm 2 is used afterward for smoothing the path. The global path is divided to several local paths defined by two end configurations to be solved for each case using Algorithm 1. In this respect, the data is acquired from a DGPS which has positional accuracy less than 2 cm on the robotic vehicle of 1.3 m in width, 1.96 m in length and 1.5 m of the axis length in PAVIN test beds [START_REF] Avanzini | Manual convoying of automated urban vehicles relying on monocular vision[END_REF]. Each point data is sampled in 10Hz and the vehicle tries to keep the velocity constant at 1m/s.

Focusing on these raw data, the smoothed path generation is carried out by proposed solution where it should have continuous curvature while reconstructing as close as possible to the original experimental conditions. From the raw data acquired, it needs to set the boundary conditions for solving the clothoid segments problem as a local planner. For the general and automatic procedure that could be applied in the global framework, some pre-procedures are required to find the geometric boundary points in order to obtain the more appropriate clothoids for the corresponding conditions and constraints. In the pre-procedures, the geometric data is mapped to the image plane and then the Hough Transform (HT) which is especially useful to extract the regional information for line (Hough Line Transform: HLT) or arc (Hough Circle Transform: HCT) [START_REF] Duda | Use of the hough transformation to detect lines and curves in pictures[END_REF] is performed on that image. The procedure is given in Algorithm 2.

In Algorithm 2, the overlapped lines are integrated into one segment and the circle are chosen to make boundary conditions to determine maximal curvature point p c . For circular redundancy, firstly, the minimal radius is taken from the vehicle kinematic constraints i.e. κ ∈ [κ min , κ max ] and then the circles made from the straight regions (detected from HLT) are extracted out. Finally, the circular objects outside the turning curve are deleted.

The application of HT [START_REF] Duda | Use of the hough transformation to detect lines and curves in pictures[END_REF] for line and arc segments are performed as shown in Fig. 4(a). There are unnecessary information and noises in the arc results,

Algorithm 2 pCCP global planner

Require: Raw traj. or path data, P (x, ȳ) ⊲ Raw data set 1: procedure pCCP Global planner(x, ȳ) 2:

Pre-processing ⊲ Data smoothing by noise filtering 3:

procedure Map analysis 4:

P (x, ȳ) -→ I(ū, v)
⊲ Mapping on Image map 5:

HLT : S i , (i = 1, . . . , m) ⊲ n Line boundary points 6:

HCT: C j , (j = 1, . . . , n) ⊲ m Arc boundary points 7:

Elimination of redundant points 8:

Boundary points determination 9:

I(ū, v) -→ P (x, ȳ)
⊲ Inverse mapping on Cartesian map 10:

end procedure

11:

procedure Local Planner pCCP((P i , P f , pc)) 12:

Configurations definition ⊲ θ i,f from S i and pc from C j 13:

Local planning using Algorithm 1 ⊲ Two clothoids and/or circular arc 14:

end procedure

15:

Post processing ⊲ Connecting the acquired solutions 16: end procedure thus clustering by k-means nearest neighbor method are proceeded [START_REF] Duda | Pattern Classification[END_REF] and the cluster centers are sorted by minimal value of the distance summation as defined by,

min μ (i=p,j=q) i =j σ i -σ j , (10)
where σ i is the i th circle center in a cluster μ and L 2 (Euclidean) distance between the path and the circle center of minimal radius is selected as p c as shown in Fig. 3. The integrated result in this procedure is shown in 4(b) where the line and clothoid/circular arc segments are noted by S i and C j for i = 1, 2, ..., m, j = 1, 2, ..., n where m, n are the number of the isolated segments respectively. From the results, isolation of straight and curved sections gives each problem for local planner pCCP, thus global continuous curvature trajectory is generated by connecting each solution result. Each solution has its own sharpness which is found from the geometric boundary configuration. Connected clothoids solution (cf. Algorithm 1 and 2) makes the continuous curvature diagram along the whole travel length. The diagram gives fully defined global reference trajectory in steering control with velocity information. In every segment of the diagram, the constant sharpness is found to meet the configuration where the curvature grows up to the maximal authorized curvature with the given boundary poses (initial and final configurations). The obtained curvature diagram (c.f. Fig. 5(b)) is continuous and smoothly drivable (inside each elementary local planning) over all the travel distance and it can be used thus efficiently to reconstruct a smooth reference path for vehicles.

Simulation and Control Performance

The obtained continuous curvature path was applied for autonomous vehicle navigation while using an appropriate Lyapunov based controller in the global framework described in Section 1. If the initial pose of the vehicle is exactly aligned to the beginning pose of the path, the vehicle follows the reference path without errors. However, any uncertainty in the actuation system, slippage on the road or time delays in controller could cause pose discrepancy to the expected values. Otherwise, additional sensors information to calibrate the pose are required and it also have to be stabilized by the used controller. The proposed algorithmic solution is tested using the Lyapunov based controller [START_REF] Canudas | Theory of Robot Control[END_REF] for comparing its performance with the control using raw data (DGPS). The car-like vehicle starts from its initial pose with errors, d e = 10 -1 m and θ e = 10 • with constant velocity of 1 m/s. Fig. 6 depicts the comparison results of two trajectories on the steering setpath and actual steering where the steering angle of the vehicle is controlled to follow the set-point. Fig. 6(b) shows explicitly the reduction of the fluctuation and smoother steering behaviors. Fig. 7 displays two comparison results on the lateral pose and orientation error.With the above results including sharpness comparison, performance of the proposed path is compared to the result of the raw data. Performance comparisons are summarized in Table 1. In Table 1, the performance are evaluated by quantitative as well as qualitative measures such that the peak values (max) evaluate the quality for each path and integrating or average (avg) values give the quantity level to analyze the performance in synthetical manner. The maximum and average lateral pose error are denoted by p e max and p e avg in [m]. It is also noted for orientation error θ e max , θ e avg in [ • ] and absolute sharpness by α max and α avg and for the maximal absolute steering κ max in [ 1 m ] respectively. Note that average errors of position, orientation for pCCP are much less than the errors for raw data such as 54.4 % and 42.6 % respectively in its performance ratio. For the peak value on the errors of position and steering control, the performances of pCCP get worse than the raw data by -2.6 % and -14.3 % respectively. Analyzing the data results, the peak occurs around 35 sec. as given in Fig. 7(a) and marked as A in Fig. 5(a) (the region of B also makes the vehicle hard to follow in the same way). In that region, the vehicle of the original trajectory is assumed to make rapid turning of steering to follow the road profile where the curvature is also changed from positive to negative with high sharpness as shown in Fig. 5. Another possible reason is from the convergence threshold D e used in Algorithm 1 where the infenitesimal discontinuity occurs at the junction of the solution. This part of the problem will be subject to near future work. The sharpness terms α max and α avg get the advantage of 30.5 % and 46.5 % to the raw data and these validate that the steering behavior obtained using global pCCP is much smoother than the case using raw data.

The remarkable difference is also shown at the total steering work through the path |κ| ds. The steering work is reduced by 41.7 % compared to the driving based on raw data. It shows that pCCP algorithm generates energysaving trajectory for the vehicle by smooth and continuous steering. Results given in Figs. 6 and7, demonstrate that the smoothed path by clothoids segments gives an enhanced performance for the path following of the nonholonomic carlike vehicle. The reduced steering work and sharpness decrease permit to enhance passenger comfort by lessening the lateral acceleration and vehicle's jerk [START_REF] Labakhua | Smooth trajectory planning for fully automated passengers vehicles: Spline and Clothoid based methods and its simulation[END_REF].

Conclusion

This paper proposed mainly an effective algorithm to generate a continuous curvature path using appropriate clothoid's parametric adjustment. The proposed algorithm, called Parametric Continuous Curvature Path (pCCP) could thus generates vehicles paths with smooth steering with lower lateral acceleration in energy-saving way. The effectiveness of using pCCP for a practical implementation was shown by accurate simulated results. The pCCP is afterward integrated in one proposed global planner which permits, among other, to transform recorded raw data corresponding to actual vehicle path, to one very close path w.r.t. the original one, but with smooth and continuous (in the sens of curvature) path. This kind of obtained smooth path is really desirable in order to enhance, for instance, the passenger comfort while using autonomous car-like vehicle. Indeed, in the showed simulation using standard controller, the vehicle could follow much better, the obtained path using global pCCP. In the future work, the performance of the proposed path generation algorithm will be used with an actual autonomous vehicle while considering uncertain errors, such as its motor actuation dynamics and/or the nonlinear tire contact with the road surface.

Fig. 1 .

 1 Fig. 1. Path following model for the car-like vehicle

Fig. 3 .

 3 Fig. 3. Basic pCCP problem

Fig. 4 .

 4 Fig. 4. Procedure for extracting the boundary conditions

Fig. 5 (Fig. 5 .

 55 Fig. 5. Comparison of the path and the curvature diagram using global pCCP

Fig. 6 .Fig. 7 .

 67 Fig. 6. Comparison of steering control with/without pCCP

Table 1 .

 1 Performance Comparison

	Performance p e max	p e avg	θ e max	θ e avg	|κmax|	|κ| ds |α| max |α| avg
	Raw Data (a) 0.0272 0.0068 0.0869 0.0202 15.1842 974.6058 23.4613 4.6522
	pCCP (b) 0.0279 0.0031 0.0680 0.0116 17.3489 574.8520 16.3169 2.4906
	Ratio a-b a [%] -2.6 54.4 21.7 42.6 -14.3	41.0	30.5	46.5


Amani Azzabi
email: ameni.azzabi@gmail.com

Marwa Regaieg
email: marwaregaiegrm@gmail.com

Lounis Adouane
email: lounis.adouane@univ-bpclermont.fr

Othman Nasri
email: nasri.othman@gmail.com

Hybrid and Multi-controller Architecture for Autonomous System: Application to the navigation of a mobile robot

Keywords: Control architecture, Mobile robotics, Stability and Reachability analysis, Hybrid dynamic system, Hybrid automata, Interval analysis

This paper deals with the problem of unicycle mobile robot navigation in cluttered environments. It presents in particular an approach which permits to verify the stability of the control architecture of mobile robot using the reachability analysis. To perform this analysis, we consider the robot as a hybrid dynamic system. The latter is modeled by an hybrid automata in order to verify the reachability property by using the interval analysis. The simulation results validate the proposed control architecture.

INTRODUCTION

The navigation control of a mobile robot in cluttered environment is a determining problem and is identified as among the priority field of research in the robotics community. The main issues in this field is how to obtain accurate, flexible and reliable navigation? In the proposed set-up, the mobile robot has for mission to reach its target while avoiding any annoying obstacles. Thus, its main behavior is the attraction toward the target and the Obstacle avoidance.

In the literature, a part of the community supposes that the mobile robots use methods of path planning. This means that the environment where it navigates is totally or partially known. Thus, the robot leans on a model of the world which is generally a map supporting a planning. Among these methods, Voronoï diagrams, visibility graphs or artificial potential functions include all the information about the task to be achieved and the environment features [START_REF] Garrido | Path planning for mobile robot navigation using voronoi diagram and fast marching[END_REF]. The other part of the community admit that robot's navigation is based only on the capacity of the robot to answer to the perceived stimuli using appropriate control law (called reactive) which takes into account the robot's constraints as well as the local state of the environment [START_REF] Luciano | Robot navigation based on electrostatic field computation[END_REF], [START_REF] Egerstedt | A hybrid control approach to action coordination for mobile robots[END_REF].

Several control architectures based reactive modes are proposed in the literature. The conception of the latter, called also behavioral architectures, is based on several elementary controllers/behaviors to be coordinated: Selection of the actions (competitive architectures) and the fusion of the actions (cooperative architectures), [START_REF] Adouane | Behavioral and distributed control architecture of control for minimalist mobile robots[END_REF], [START_REF] Brooks | A robust layered control system for a mobile robot[END_REF]. The work proposed in this paper studies the stability of reactive control architecture, proposed in [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF], for a unicycle robot.

Using the considered architecture, it is necessary to guarantee the robot capacity to accomplish its mission while avoiding the obstacles. One of the solutions consists in considering it as as hybrid dynamical system whose behavior is modeled by a hybrid automaton. As shown in [START_REF] Luciano | Robot navigation based on electrostatic field computation[END_REF], this approach of hybrid control allows the coordination of the action of several mobile robots and checks the reachability property. This analysis consists in determining if a position or a configuration can be reached by the system. Thus it can check that no unwanted behavior of the system will occur. To verify this property for dynamic systems, several approaches are proposed in the literature (J. [START_REF] Toibero | Switching control of mobile robots for autonomous navigation in unknown environments[END_REF]. Among these approaches, we adopt the reachability analysis using the interval analysis [START_REF] Ramdani | A Hybrid Bounding Method for Computing an Over-Approximation for the Reachable Set of Uncertain Nonlinear Systems[END_REF]. This approach, based on a hybrid automatons, allows finding all the minimal and maximal trajectories of the system, governed by Ordinary Differential Equations (ODE), and to check according to these later if the unwanted system configuration could occurs.

The rest of the paper is organized as follows: we present briefly, in the section 2, the control architecture for the navigation of unicycle mobile robot in a cluttered environment. After having modeled the robot behavior by a hybrid automaton, we shall study in section 3 the control architecture while considering the reachability analysis using the interval analysis. We present in section 4 the simulation results. Section 5 concludes this paper with some prospects.

CONTROL ARCHITECTURE FOR REACTIVE NAVIGATION IN A CLUTTERED ENVIRONMENT

We present in this section, the control/command architecture of mobile robot developed in [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF].

Navigation in a cluttered environment

The mobile robot has for mission to reach its target while avoiding the obstacles met during its navigation. Therefore, its behavior is mainly the attraction toward the target and obstacle avoidance. Furthermore, the navigation of the robot has to be safe, smooth and fast.

The obstacles met during the robot navigation can have different forms. In order that the robot can avoid them, these obstacles are generally encompassed by simple forms of circular or elliptic type [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]. After that, the obstacle avoidance behavior based on limit-cycles can be achieved [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], [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]. In this work, the limit-cycles will be considered as circular shape. In order to avoid collision, the robot and the target will also be characterized or encompassed by circles. Several perceptions are necessary for the navigation of the robot (cf. Fig. 1):

• D ROi distance between the robot and the obstacle "i",

• D PROi perpendicular distance between the line (l) and the obstacle "i",

• D T Oi is the distance between the obstacle i and the target.

• R R , R T and R Oi are respectively the radius of the robot, the target and the Obstacle i ,

• for each detected obstacle i we define a circle of influence with a radius of

R Ii = R R + R Oi + margin
The margin corresponds to a safety tolerance which includes: the uncertainty of the perception, control reliability and accuracy, etc.

Control architecture

The proposed control architecture is represented by the figure 2. This control architecture uses a procedure for selecting a hierarchical action to manage the switching between the controllers according to the environment perception and ensure the stability of the overall control. Its objective is also to ensure safe, smooth and fast robot navigation [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF]. The mechanism of selection activates the "obstacle avoidance" controller if at least one obstacle is detected. In order to understand the implemented hybrid architecture of control to check the unicycle mobile robot navigation, it is important to know its kinematic model.

Model of the used unicycle robot

The well known kinetic model of a unicycle robot, in a Cartesian reference frame (cf. Fig. 3), is given below:

  ẋ ẏ θ   =   cos θ -l 2 cos θ -l 1 sin θ sin θ -l 2 cos θ + l 1 sin θ 0 1   × v ω
(1) with :

• x, y, θ: configuration state of the unicycle robot at the point P t of abscissa and ordinate (l 1 , l 2 ) according to the mobile reference frame (O m , X m , Y m ) associated to the robot center.

• v: the robot's linear velocity at the point "P t ".

• ω : the robot's angular velocity at the point "P t ". Knowing the model of the robot and the task to achieve, we present the both controller "Attraction to the target" and "Obstacle avoidance ". The set of these controllers will be synthesized using the Lyapunov theorem [START_REF] Benzerrouk | Lyapunov global stability for a reactive mobile robot navigation in presence of obstacles[END_REF].

• (O A , X A ,Y A ) is the absolute reference. m O 1 l 2 l x y θ A X A Y A O m X m Y P t

Attraction to the Target Controller

This controller guides the robot toward the target position represented by a circle of center (x T , y T ) and of R T radius. As detailed in [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF], the used control law is a control of position at the point P t of coordinates (l 1 , 0). It is based on the configuration of the robot position relative to the target, represented by the errors e x and e y .

To guarantee that the center of the robot axis reaches the target with asymptotic convergence, the distance d = e 2

x + e 2 y must be smaller than R T . The errors of position are given by the following equations:

e x = x -x T e y = y -y T (2) Thus: ėx = ẋ ėy = ẏ (3)
For the stabilization of the error towards zero, a proportional controller was used [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF]:

v w = -K cos θ -l 1 sin θ sin θ l 1 sin θ -1 × e x e y (4)
with K > 0.

To study the asymptotic stability of the proposed controller, let us consider the following Lyapunov function:

V 1 = 1 2 d 2 .
Therefore, to guarantee this stability in the sense of Lyapunov, it is necessary that: V1 < 0, so d ḋ < 0, what is easily proven as long as d = 0.

Obstacle Avoidance Controller

During the activation of this controller, the robot follows limit-cycle vector fields given by two differential equations:

• For the clockwise trajectory motion:

(cf. Fig 4(a)) ẋs = y s + x s (R 2 c -x 2 s -y 2 s ) ẏs = -x s + y s (R 2 c -x 2 s -y 2 s ) • For the counter-clockwise trajectory motion:(cf. Fig 4(b)) ẋs = -y s + x s (R 2 c -x 2 s -y 2 s ) ẏs = x s + y s (R 2 c -x 2 s -y 2 s )
where (x s , y s ) corresponds to the robot position according to the center of the convergence circle (characterized by an R c radius). These equations show the direction of trajectories according to axes (x s , y s ). The control law associated with this controller allows the robot to follow the trajectories of the limitcycles. The robot will be controlled according to its center of coordinates (l 1 , l 2 ) = (0, 0). The desired robot orientation θ d is:

θ d = arctan( ẏs ẋs ) (5)
The error of orientation θ e is given by:

θ e = θ d -θ (6)
The desired orientation is reached using the following control law:

w = θd + K p θ e (7)
with K p > 0.

The orientation error is given by the following differential equation: θe = -K p θ e . The asymptotic stability of the controller is verified through the following Lyapunov function: V 2 = 1 2 θ 2 e . V2 is equal then to θ e θe = -K p θ 2 e which is always strictly negative (thus, asymptotically stable).

Hierarchical Action Selection Block

The block corresponding the selection of action manages the switching between the controllers. It takes a decision thanks to environment's information, collected by the sensors. The selection of action to be made ("Attraction to the target" or "Obstacle avoidance ") depends on the distance between the perpendicular distance to the line (l) and the distance between the obstacle and the circle of influence (cf. Fig 1).

• If (D PROi ≤ R Ii ) then the controller "Obstacle avoidance " is activated.

• Else the controller "Attraction to the target " remain always active.

STABILITY OF THE CONTROL ARCHITECTURE USING REACHABILITY BY INTERVAL ANALYSIS

In this section, we study the stability of the considered reactive control architecture by checking the reachability property. That's why, it is necessary to model the robot behavior by a hybrid automaton.

Hybrid Automaton for the Robot's Control

An hybrid automaton handles a set of continuous differential equations, models a dynamic system and describes its behavior. As mentioned in [START_REF] Ramdani | A Hybrid Bounding Method for Computing an Over-Approximation for the Reachable Set of Uncertain Nonlinear Systems[END_REF], a hybrid automaton used to find the envelope which frames, in a guaranteed way, the real robot's path, is generally considered as being one 6-uplet H = (Q, X, P, F, T, RI) defined by: ⋄ Q is a discrete set of modes or situations;

⋄ X represents the continuous state space (of dimension n), the state vector is noted x and the initial state is noted x 0 ;

⋄ P = [p] = [p, p] represents the parameters' admissible bounded domain;

⋄ F = {( f q , fq ), q ∈ Q}, f is the collection of vector fields that surround all the possible dynamics;

⋄ T = {t e , e ∈ E} is the collection of the transitions' moments where E ⊆ Q × Q is the set of transitions;

⋄ RI = R e , e ∈ E is the collection of the functions of update.

According to the case of our studied model, we have:

⋄ Q = {q 1 , q 2 }={
Attraction to the target , Obstacle avoidance} : the set of discrete states;

⋄ X = R 3 , z = (x, y, θ) ∈ X is the state vector;

⋄ P = {p q 1 , p q 2 } is the admissible bounded domain of parameters;

⋄ T = {t q 1 q 2 ,t q 2 q 1 } where

t q 1 q 2 = (q 1 , D PROi ≤ R Ii , Id, q 2 )
and t q 2 q 1 = (q 2 , D PROi > R Ii , Id, q 1 ) ⋄ F = ( f q , fq ), q ∈ Q where f is the collection of vector fields that surround all the possible dynamics.

The unicycle robot behavior in a cluttered environment can be then modeled by the hybrid automaton of the figure 5. The continuous dynamics of the robot in the discrete state q 1 at the point P t of coordinates (l 1 , 0) is defined by:

ż = f q 1 (z, u, p q 1 ) =      ẋ = v cos θ -wl 1 sin θ ẏ = v sin θ -wl 1 cosθ θ = w (8)
with z = (x, y, θ) ; u = (v, w) and p q 1 = (l 1 , K).

The continuous dynamics of the robot in the discrete state q 2 which is controlled by its center of coordinates (l 1 , l 2 ) = (0, 0) is defined by:

ż = f q 2 (z, u, p q 2 ) =      ẋ = v cos θ ẏ = v sin θ θ = w (9)
with z = (x, y, θ) , u = (v, w) and p q 2 = (K p ).

The invariant regions associated with both discrete states q 1 and q 2 are respectively (D PROi > R Ii ) and (D PROi ≤ R Ii ). The transition from a state to another requires that the position and the orientation of the robot verify the conditions of guard ((D PROi > R Ii ) and (D PROi ≤ R Ii )). The update functions RI are consider as the identity functions because the robot dynamics doesn't present discontinuity during the crossing of the transitions between the discrete states.

After modeling the robot behavior by an hybrid automaton, we will verify the stability of the control architecture considered using the reachability analysis.

Principle of the Bounding Method

As indicated in [START_REF] Ramdani | A Hybrid Bounding Method for Computing an Over-Approximation for the Reachable Set of Uncertain Nonlinear Systems[END_REF], The method of hybrid bounding is an approach of reachability analysis. It allows thus to find the envelope enclosing all possible trajectories of the robot in order to study its stability. For that purpose, we will detail the algorithm used to calculate an over-approximation of the reachable space.

The algorithm 1 of the hybrid bounding method looks first for the initial discrete mode q through the function Initialization "Initialization". This function identifies the initial discrete state according to the signs of all the partial derivatives of the vector fields relative to the uncertain parameters. In our case, these parameters are θ, x and y. Then, as long as the time t doesn't reach the final value t nT , a guarantee integration of the current mode is given by the algorithm 2 "Integer-one-step" which makes a step of integration noted h. Next, according to the signs of partial derivatives, the function "switching" verifies if there is a transition towards another mode on the interval [x j , x j+1 ] and calculates the new mode q ′ . If it is the case, then the boolean variable jump, that indicates the existence of a transition, sets the boolean value to 1 and if not it takes the value 0. The switching from a mode to another depends on the current mode. If it is equal to 0 then it is enough to turn to the new mode q ′ = 0 and continues the integration. Else (q = 0), it is necessary to repeat the integration on the mode q = 0 to save the guarantee of the frame for the transition crossing. Besides, the algorithm 2 "Integer-onestep" detailed below calculates on each step of integration, a protected frame from solutions of uncertain differential equation. In fact, for the case of the mode q = 0 the digital integration of the uncertain differential equation is given through the method of Taylor Intervalle (MTI)in the line 5. This method contains essentially two steps: • a step of prediction which verify the existence and the uniqueness of the solution. • a step of correction which calculate the solution [x j+1 ] at the moment t j+1 = t j + h j .

q := Initialization ( f , [x 0 ], [p]) ; 6 while( j < nT ) do 7 (h j , [x j+1 ], [ x j ]):= Integer-one-step(q, f , F,t j , [x j ], [p]); 8 ( jump, q 0 ) := switching (q, f , [ x j ]) ; 9 if ( jump) then 10 if (q = 0) then 11 q := q 0 ;
However, for the modes q = 0, firstly we select the ODE bounding in the line 7 and the initial conditions at the moment t j (ω(t j ), Ω(t j )) are fixed to lines 8 and 9. Then, the digital integration of this EDO bounding is executed in the line 10. Finally, to guarantee numerically the obtained results, we use as in the line 5 the integration methods based on the models of Taylor intervals to solve the ODE.

SIMULATION RESULTS

The stability of the proposed architecture is verified using the hybrid bounding method. For that purpose, The simulation results show the guarantee frame of all the temporal trajectories of the robot. Indeed, there is no intersection with hazardous areas and all solutions do not leave the envelope during the period of simulation. Therefore, these simulations prove the stability of the robot and demonstrate the efficiency of the reactive control architecture presented in section 2.

Algorithm 2 Integer-one-step 1 ⋄ Inputs: q, F, t j , [x j ], [p] 2 ⋄ Outputs:h j , [x j+1 ], [ x j ] 3 Begin 4 if q := 0 then 5 (h j ,[x j+1 ],[ x j ]) := MTI( f ,[x j ],[p],t j ); 6 else 7 ( f q , f q ) := select frame(q, F) ; 8 [ω j ] := [x j ] ; 9 [Ω j ] := [x j ] ; 10 (h j ,[ω j+1 ],[Ω j+1 ],[ Ω j ],[ ω j ]):= MTI( f q , f q , [ω j ], [Ω j ], [p], [p], t j ) ; 11 [ x j ] := [ ω j , Ω j ] ; 12 [x j+1 ] := [ω j+1 , Ω j+1 ] ;

CONCLUSION

In this work, we were interested in the problem of mobile robot navigation in a cluttered environment. In particular, the stability of the dynamics of the robot using reactive control architecture was studied. At first, we modeled the set(continuous dynamics of the robot and the discrete change of the used control laws) by a hybrid automaton. Thereafter, we used an approach of reachability analysis based on the interval method in order to verify the stability of the dynamic system even in presence of uncertainty. This approach is based on the calculation of an over-approximation of the reachable space set of the robot (which correspond to all the points of the robot's trajectory). The idea was to find a guarantee frame of all the trajectories possible taking into account the uncertainties on the data received by the sensors. Thus, the reachability analysis is handled by calling on to the interval analysis. Finally, we validated the proposed solution by simulations. The obtained result show that the considered control architecture allows the robot to have a smooth behavior while avoiding the obstacles met throughout its navigation toward the target. Future work will aims to test in real experimentation the obtained results.

Figure 1 :

 1 Figure 1: The used perceptions for mobile robot navigation.

Figure 2 :

 2 Figure 2: Control architecture for mobile robot navigation.

Figure 3 :

 3 Figure 3: Robot configuration in a Cartesian reference frame.

Figure 4 :

 4 Figure 4: Shape possibilities for the used limit-cycles.

Figure 5 :

 5 Figure 5: Hybrid automaton of the navigation task to be performed by the robot.

Algorithm 1 ⋄⋄

 1 Main algorithm 1 Inputs:t 0 , t nT , F, [x 0 ], [p] 2 Outputs:[ x0 ], [ x1 ],..., [ xnT ], [x 1 ],..., [x nT ]

  5

  MATLAB toolbox INTLAB and we suppose that the data received from the robot sensors are uncertain. Thus, the state vector components are represented by intervals translating this uncertainty on the position (x et y) and on the orientation θ. The execution of the hybrid bounding algorithm over the period of simulation [0, 5]s gave the results of figures 6, 7 and 8.

Figure 6 :

 6 Figure 6: Frame of all the possible solutions of x(t) due to the hybrid bounding method.

Figure 7 :

 7 Figure 7: Frame of all the possible solutions of y(t) due to the hybrid bounding method.

Figure 8 :

 8 Figure 8: Frame of all the possible solutions of θ(t) due to the hybrid bounding method.

ACKNOWLEDGEMENTS

This work was supported by LABEX IMobS3 Innovative Mobility: Smart and Sustainable Solutions.


Fatima Debbat
email: debbatfatima@gmail.com

Lounis Adouane
email: lounis.adouane@univ-bpclermont.fr

FORMATION CONTROL AND ROLE ASSIGNMENT OF AUTONOMOUS MOBILE ROBOTS IN UNSTRUCTURED ENVIRONMENT

Keywords: Mobile robot, formation control, leader-follower approach, role switching, obstacle avoidance, limit-cycle method

. The overall control architecture is validated through a multitude of simulation.

Introduction

Currently, navigation in formation of a group of mobile robots receives a very great interest by multi-robot community. Various advantages have been shown in several application compared with a single mobile robot [START_REF] Lozenguez | Map partitioning to approximate an exploration strategy in mobile robotics[END_REF], [START_REF] Khan | Multi-robot cooperation framework based on artificial immune system[END_REF], [START_REF] Geng | Cooperative task planning for multiple unmanned aerial vehicles using a genetic algorithm[END_REF], [START_REF] Benzerrouk | Toward an hybrid control architecture for a mobile multirobot systems[END_REF], [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]. In this paper, we focus on the problem of how to control a group of mobile robots moving in formation in an unknown environment and while switching from a specific formation to another. We propose a resolution strategy of initialization, maintaining formation, and switching in other shape when perceptual event occurs. An improvement of an obstacle avoidance approach using limit-cycle [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF], [START_REF] Jie | Real time obstacle avoidance for mobile robot using limit-cycle and vector field method[END_REF], [START_REF] Vilca | Reactive navigation of mobile robot using elliptic trajectories and effective online obstacle detection[END_REF] was proposed. This approach is characterized by its simplicity, no need for global knowledge and allows to choose the direction of avoidance according to the target position. For this purpose, we design an adaptive controller to drive a group of mobile robots in a leader-follower configuration. This control allows the robots group: to explore the environment, to initialize a formation shape, to change the formation structure according to the navigation context, and to avoid obstacles. Petri nets formalism is used to modelling the overall control architecture embedded in each robot.

Navigation Control Strategy of the Group of Robot

This study proposes a Petri Net [START_REF] Murata | Petri nets: properties, analysis and applications[END_REF] model in order to managing the different states allowed for the formation control. The total functionality of the group is decomposed into functional behaviours. The proposed net (cf. Fig. 1) comprises 6 places and 12 transitions.

Formation Initialization

The mobile robots must arrange themselves into a specific geometric configuration without centralized control. Only information from local sensors are used which are inevitably incomplete. Initially (cf. Fig. 1), the robots are randomly positioned in the environment. The robots begin to explore and observe the environment in order to find other robots while avoiding the detected static obstacles (P1: Exploration). Each robot will be in P2 (Move state) if it detects another robot. It tries to join the other robot until certain distance (to do not collide with it) while avoiding detected obstacle in its way. When the robot is close enough to the other robot, it will be in P3 (Position Assignment state).

Obstacle Avoidance

The mobile robots considered in this study are wheeled mobile robots of unicycle type. The obstacle avoidance is enabled if the distance between robot and obstacle is smaller than a specific distance and the obstacle is located 1 on the robot way. A real-time obstacle avoidance method named limit-cycle method [START_REF] Adouane | Orbital obstacle avoidance algorithm for reliable and on-line mobile robot navigation[END_REF] has been implemented. One supposes in the setup that obstacles and the robot could surround by bounding cylindrical boxes with respectively R O and R R radii. The target to reach is also characterized by a circle of R T radius. Several perceptions are also necessary for the robot navigation: D ROi distance between the robot and the obstacle "i," D P ROi perpendicular distance between the line (l) and the obstacle "i," and D T Oi distance between the target and the obstacle "i" (Fig. 2).

For each detected obstacle we define a circle of infiuence with a radius of Rc = R R + R Oi + Margin. Margin corresponds to a safety tolerance.

The limit-cycle method in its original version assumes that the obstacles have a known surface and a geometrical Figure 3. Avoidance of the obstacle using limit-cycle method: (a) known obstacle shape and (b) unknown obstacle shape.

shape, but in an unstructured environment this knowledge is not always available to the robots. In this context, a solution for obstacles whose shape is unknown is proposed. We follow the same limit-cycle algorithm but instead of surrounding the obstacle by the circle of influence, this circle surround only the first detected point of this obstacle (cf. Fig. 3).

The circle radius represents the distance between the robot and the obstacle and equals R R + Margin. We repeat this process each T equals to RR vR with v R is the robot velocity.

Navigation in Formation

In this section, we will develop and describe the role assignment between robots and how robots choose the appropriate formation. The proposed strategy is composed by different states (cf. Fig. 1). A leader-follower approach is used for the initialization of mobile formations [START_REF] Consolini | Leader follower formation control of nonholonomic mobile robots with input constraints[END_REF]. Initially, each robot is considered as leader. When it detects another robot, it remains leader according to the priority given by its identification number (ID). In P3 position assignment (cf. Fig. 1) an information exchange on the ID value between the robots is necessary for the recruitment of the leader. Each robot has its identification number, called Robot ID (RID). The RID determines the priority of a robot in formation. In the proposed strategy, the RID of the leader robot is the smallest. In this work, we adopted two types of formation shape: triangular formation and line formation. The switching mechanism between them is given by following the proposed process:

1. If the path is free (T7), the robots organize themselves according to the triangular formation. This shape is realized by using (L -L) control [START_REF] Consolini | Leader follower formation control of nonholonomic mobile robots with input constraints[END_REF].

2. If an obstacle is detected and the robots are in Triangle formation (P4), the robots organize themselves according to the line formation in order to ensure a safe avoidance (T10). This shape is realized by using (ψ -L) control [START_REF] Consolini | Leader follower formation control of nonholonomic mobile robots with input constraints[END_REF].

3. If one of the robots does not see any of other robots, it returns to P1 (Exploration state).

4. If an obstacle is detected during assignment position process, the robots organize themselves according to the line formation (T8).

Simulation Results

Examples of simulation results are given in Figs. Figure 4 shows the trajectory of three robots in the second simulation case. The robots navigate in formation (triangular shape) in order to reach the target. When an obstacle is detected, the robots activate the avoidance obstacle behaviour while changing the formation shape to line. Other ability of the proposed control architecture is observed in Fig. 5. In this case, the three robots try to join a target. When the robots are close to the obstacle, robot R2 relinquishes the leadership to the robot R3 (Formation switching behaviour).

Conclusion

In this paper, it is presented a new decentralized formation control for small-scale mobile robot team using the leaderfollower strategy. The proposed control resolves the problems of obstacle avoidance and formations changes. In the proposed approach, the autonomous mobile robots must arrange themselves into a specific geometric configuration without centralized control. The approach has been tested through a multitude of simulations to demonstrate validity and effectiveness of the proposed control architecture. The future research work will focus on extending the results to more general applications by employing more than three robots in formation.

Figure 1 .

 1 Figure 1. Petri Net for the proposed formation control (embedded in each robot).

Figure 2 .

 2 Figure 2. The used perceptions for mobile robot navigation [1].

Figure 4 .

 4 Figure 4. Navigation with keeping and switching of the formation shape.

Figure 5 .

 5 Figure 5. Navigating in formation while avoiding obstacles.

4 and 5 .

 5 The plots correspond to the simulation results of the robot team navigating in cluttered environment. The initial positions of three robots are randomly generated and we want to keep the relative distance and angle between two consecutive robots as constant fixed value: d L = 2 m, and ψ L = 0 • in Line formation and Triangle formation. (d L : distance between leader and follower, ψ L : angle between leader and follower). The distance between followers is d F = 2 √ 2 m in Triangle formation and d F = 2 m in Line formation.


José Miguel Vilca

Lounis Adouane

Ahmed Benzerrouk

Youcef Mezouar

Cooperative On-line Object Detection using Multi-Robot Formation

Keywords: component-Multi-robot system, Formation control, Mobile robot navigation, Obstacle detection, Telemetry, Parameter identification

 which use global reference frame) and leader-follower approach. Simulations will permit to show the efficiency of the proposed reactive and distributed cooperative navigation.

I. INTRODUCTION

In recent years, research in control and coordination of multiple mobile robots has increased significantly. Tasks that may be performed by a single complex robot can be performed more efficiently and cheaply by a group of elementary cooperating robots. Some examples of multi-robot applications can be seen in exploration [START_REF] Kruppa | A probabilistic approach to collaborative multirobot localization[END_REF] [START_REF] Lozenguez | Calcul distribue de politiques d'exploration pour une flotte de robots mobiles[END_REF], management and platooning of autonomous vehicles [START_REF] Alami | Multi-robot cooperation in the martha project[END_REF] [START_REF] Bom | Nonlinear control for urban vehicles platooning, relying upon a unique kinematic gps[END_REF], mapping of unknown locations [START_REF] Howard | Multi-robot simultaneous localization and mapping using particle filters[END_REF] [START_REF] Miner | Swarm robotics algorithms: A survey[END_REF], and transport or reposition of heavy objects [START_REF] Adouane | Hybrid behavioral control architecture for the cooperation of minimalist mobile robots[END_REF] [START_REF] Chaimowicz | A mechanism for dynamic coordination of multiple robots[END_REF], etc. However, the coordination of multi-robot navigation is among the most challenging tasks.

A particular problem of multi-robot coordination is formation control, i.e., when a group of mobile robots has to navigate and keep a desired formation. In the literature, there are different approaches to deal with this problem such as: behavior-based [START_REF] Tang | Hybrid behavior coordination mechanism for navigation of reconnaissance robot[END_REF], leader-follower [START_REF] Desai | Modeling and control of formations of nonholonomic mobile robots[END_REF] and virtual structure approach [START_REF] Ghommam | Formation path following control of unicycle-type mobile robots[END_REF]. In this work we address the formation control problem using the combination between the three approaches: behavior based, dynamical virtual structure (already presented in [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]) and the leader-follower approach. Keeping a desired shape by the robots can then be achieved by considering the formation as a single virtual rigid body. The dynamic of this latter is given by one specific position of the rigid body. In the proposed work, this position corresponds to the leader of the formation. The control law of each follower robot is derived thus according to dynamic of the leader and the desired formation shape [START_REF] Do | Formation tracking control of unicycle-type mobile robots[END_REF] [START_REF] Li | Backstepping based multiple mobile robots formation control[END_REF].

In [START_REF] Beard | A coordination architecture for spacecraft formation control[END_REF], virtual structures have been achieved by having all members of the formation tracking assigned nodes which moves into desired configuration. In the literature, some of the topics of cooperative detection is localization, mapping and tracking [START_REF] Rekleitis | Multi-robot cooperative localization: A study of trade-offs between effciency and accuracy[END_REF] [17] and [START_REF] Wang | Distributed optimal cooperative tracking control of multiple autonomous robots[END_REF]. In [START_REF] Santos | Multi-robot cooperative object localization: decentralized bayesian approach[END_REF], the author presents a multi-robot/sensor cooperative object detection and a tracking method based on a decentralized Bayesian approach which uses particle filters. In the proposed paper, we use the range data from the group of robots to on-line cooperative object detection and localization. According to this range data, the parameters of the ellipse that enclose this range data will be obtained. The ellipse center will correspond to the position of the leader to follow.

Different approaches have been proposed in the literature to enclose the 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]. In [START_REF] Welzl | Smallest enclosing disks (balls and ellipsoids)[END_REF], the author proposed a technique to obtain the smallest enclosing ellipse by a set of data using primitive operation 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 review of the methods to fit a set of data with an ellipse. The proposed method is based on a simple and efficient heuristic approach based on Euclidean distance estimation.

This paper presents the main concepts of current work, it is organized as follows: in the next section, the strategy of navigation in formation, its properties and representations are described. In section III, the details of the control architecture are introduced and the control law of the controllers is given. Section IV presents the methods for enclosing the range data with an ellipse. Simulation results are given in section V. Finally, conclusion and some future works are given in section VI.

II. NAVIGATION IN FORMATION

We consider a group of N robots with the objective of reaching and maintaining its positions and orientations in a desired formation while using only local perceptions.

A. The cooperative control strategy

The proposed strategy consists to control each robot i to track a secondary dynamical target (node) of a virtual geometrical structure (cf. Fig. 1). Each secondary target is defined according to the position and orientation of a main target (which corresponds to the leader robot in the proposed work).

Reaching or tracking a moving target has been widely explored in the literature [START_REF] Tatematsu | Tracking motion of mobile robot for moving target using nurbs curve[END_REF], [START_REF] Chen | Coordination and control of a group of small mobile robots[END_REF]. In [START_REF] Belkhouche | Line of sight robot navigation toward a moving goal[END_REF], a specific setpoint is designed for a mobile robot to reach a dynamical target. However, this work assumes that both the robot and the target are evolving with constant linear velocities (it is assumed that the robot goes faster than its target). Therefore, it is only proved that the robot meets the target but is not able to track it. The proposed virtual dynamical structure that must be followed by the group of robots is defined as follow:

• Define one point which gives the dynamics (v M T and w M T linear and angular velocities) of the applied structure. This point is called the main dynamical target (cf. Fig. 1) and here corresponds to the dynamic of the leader robot.

• Define the virtual structure to follow by defining as much nodes as necessary to obtain the desired geometry. Each node i is called a secondary target T i and is defined according to a specific distance D i and angle Φ i with respect to the main target. • The position of each secondary target w.r.t the Local Reference Frame (LRF) of the assigned robot i is given by (cf. Fig. 1)

x Si = x M Ti + D i cos(θ M Ti + Φ i ) y Si = y M Ti + D i sin(θ M Ti + Φ i ) (1)
where (x M Ti , y M Ti , θ M Ti ) are the position and orientation of the main target w.r.t. the LRF of the assigned robot i (cf. Fig. 1).

Note that the secondary targets are defined with respect to the main target and this later is localized according to the LRF of the assigned robot. The objective of this approach is to eliminate the dependency between each robot to a global reference frame [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]. This will permit to each robot to use only its LRF to reach its target. An example to get a triangular formation is given in Fig. 1.

MT v j v j ) i ) k ) j D i D k D j Robot i Robot k Robot k v i v

III. CONTROL ARCHITECTURE

The proposed control structure (cf. Fig. 2) aims to manage the interactions between different elementary controllers while guaranteeing the stability of the overall control as proposed in [START_REF] Adouane | Hybrid and safe control architecture for mobile robot navigation[END_REF]. Its objective is also to insure safe, smooth and fast robot navigation. The specific blocks composing the global controller are detailed below. The proposed control architecture uses a hierarchical action selection mechanism to manage the switches between the controllers, according to environment perception. In this paper, only the Attraction to dynamical target controller is considered and developed. In future works, the proposed multi-robot navigation will be applied in cluttered environment, specifically while using the obstacle avoidance controller proposed in [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF].

Before describing each elementary controller, let us briefly recall the kinematic model of an unicycle robot (cf. Fig. 3)

  ẋi ẏi θi   =   cos(θ i ) 0 sin(θ i ) 0 0 1   v i ω i (2)
where x i , y i , θ i are configuration state of the unicycle with respect to the global reference frame X -Y , v i and ω i are respectively the linear and angular velocity of the robot at the point O m (cf. Fig. 3). The strategy is to use the global reference frame to obtain the controller parameters w.r.t the local reference frame (setting x i = 0, y i = 0 θ i = 0), see Fig. 4.

A. Attraction to dynamical target controller

This controller guides the robot i toward its secondary dynamical target T i (x Si , y Si ) (cf. sub-section II-A). The kinematic model of this secondary target according to the global reference frame X -Y is given by:

ẋSi = v Si cos(θ Si + θ i ) ẏSi = v Si sin(θ Si + θ i ) (3)
The linear and angular velocities of the secondary target (v Si and w Si ) are computed from [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]. Moreover, we consider that each robot knows the velocities of the main target at each sample time. And the secondary target T i satisfy the constraints v Si ≤ v i . This controller is based on the position control of the robot to the target, represented by d Si and Γ i in Fig. 3, more details are given in [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]. The position errors are:

e xi = x Si -x i = d Si cos(Γ i ) e yi = y Si -y i = d Si sin(Γ i ) (4)
where d Si is the distance of the robot to the target T i and Γ i is the orientation of the line passing through the robot and the target w.r.t global reference frame, it is equal to:

Γ i = γ i + θ i (5)
with γ i is the orientation of the line passing through the robot and the target w.r.t the orientation of the robot. The distance d Si and the orientation Γ i can be defined as

d Si = e 2 xi + e 2 yi ( 6
)
Γ i = arctan ey i ex i (7)
The derivate of the distance d Si can be expressed as

ḋSi = e xi ėxi + e yi ėyi d Si ( 8
)
By using ( 2) and (3), ėxi and ėyi will be given by

ėxi = ẋSi -ẋi = v Si cos(θ Si + θ i ) -v i cos(θ i ) ėyi = ẏSi -ẏi = v Si sin(θ Si + θ i ) -v i sin(θ i ) (9)
While using (4), ( 5) and ( 9) in ( 8), ḋSi is obtained as:

ḋSi = v Si cos(θ Si -γ i ) -v i cos(γ i ) (10)
Equation [START_REF] Tang | Hybrid behavior coordination mechanism for navigation of reconnaissance robot[END_REF] shows that the variation of the distance is regardless of the parameters of the global reference frame. Similarly, the derivate of th angle Γ i is calculated as

Γi = ṙyx 1 + r 2 yx ( 11
)
where r yx = e yi /e xi . By using (4), ( 5) and ( 9) in [START_REF] Desai | Modeling and control of formations of nonholonomic mobile robots[END_REF], it becomes then

Γi = v Si sin(θ Si -γ i ) d Si - v i sin(θ i -Γ i ) d Si (12)
To obtain the set-point robot angle θ SP in order to reach its dynamical target, as described in [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF], the idea is to keep Γ i constant. In other words, we would like to have Γi = 0. Under this constraint, we show that the defined set-point angle leads the robot to its target, more details are given in [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]. Equation [START_REF] Ghommam | Formation path following control of unicycle-type mobile robots[END_REF] allows thus to write:

v Si sin(θ Si -γ i ) d Si - v i sin(θ i -Γ i ) d Si = 0 (13)
The set-point angle that the robot must follow to reach its dynamical target is then given by

θ SPi = arcsin v Si v i sin(θ Si -γ i ) + Γ i (14)
By using ( 5), ( 14) is obtained in function of θ i :

θ SPi = arcsin v Si v i sin(θ Si -γ i ) + γ i + θ i (15)
Equation ( 15) represents the desired orientation of the robot w.r.t the global reference frame.

As already cited, a close result was given in [START_REF] Belkhouche | Line of sight robot navigation toward a moving goal[END_REF]. However, the two results are differently developed. In fact, in [START_REF] Belkhouche | Line of sight robot navigation toward a moving goal[END_REF], the line of sight of an observer was used to build this set-point and the position of this observer affects the set-point. This work presents the formulation of the controller parameters according to the global reference frame [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF] to obtain this parameters w.r.t. local reference frame. At this aim, Eq. ( 15) is evaluated in θ i = 0 (local reference frame) and the results will depend only on the local information of the robot and its target (cf. Fig. 4). The proposed control law (cf. Subsection III-B) regulates the robot by accelerating or decelerating according to the robot distance d Si and γ i . The target is then tracked whereas reached as in [START_REF] Belkhouche | Line of sight robot navigation toward a moving goal[END_REF] where the goal is just to prove that the robot and its target meet each other. In [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF], the proof that the robot reaches its target for two possible cases (escaping and approaching target) is demonstrated.

B. Control law

The used control law allows to robot i to converge to its set-point and it is expressed as follows [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]:

v i = v max -(v max -v Si ) e -d 2 S i /σ 2 (16) ω i = ω Si + K p θi (17)
where v max is the maximum linear velocity of the robot, K p and σ are the constant such that K p and σ > 0 and, d Si is the distance between the robot and the target. θi is the angular error between set-point angle θ Si and the orientation of the robot θ i . Like we use the local information of the robot, then θ i = 0. θi and ω Si are given by θi = θ Si (18) ω Si = θSi [START_REF] Welzl | Smallest enclosing disks (balls and ellipsoids)[END_REF] where θ Si is the set-point angle of the attraction to the target controller θ SPi and was already computed by [START_REF] Li | Backstepping based multiple mobile robots formation control[END_REF].

More details about the convergence of the controller using a Lyapunov function are shown in [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF].

IV. ENCLOSING DATA RANGE WITH AN ELLIPSE

During the navigation of the robots in the environment, it is important to detect and localize some specific objects to avoid or to follow it. In the proposed work, this object will correspond to the leader of the formation (cf. subsection II-A). The detection of the object could be performed more accurately if the data from all the robots are used. It is what we will show in section V.

First, let us assume that the object O can be surrounded by an elliptical box [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]. The elliptical shape is represented by its Cartesian form:

(x -h) 2 a 2 + (y -k) 2 b 2 = 1 (20)
where (h, k) ∈ 2 are the ellipse center coordinates and a, b ∈ + are the semi-axes with a ≥ b. At this aim, the observed range data are used to surround the object with the closest ellipse. In this work, we consider that the robots exchange the information (data points) and, this data points are computed from the data range and a general frame such as GPS system. The observed range data is noiseless and without the presence of outliers. Furthermore, we consider that this data corresponds only to one object that will be enclosed by an ellipse. The segmentation method of the set of points will be used in future works to the detection of more than one object at a time.

The choice of ellipse box rather than circle is to have one more generic and flexible mean to surround and fit accurately different kind of objects shapes (longitudinal shapes) [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF].

Let us consider a set of n points in 2 with coordinates P i (x i , y i ) (cf. Fig. 5). This set of points is computed from data range from all the robots, therefore only one ellipse will computed to enclose all this points. In future work, we will compare the performance to computed one big ellipse or many ellipse to enclose the object.

In the following sub-section, it will be presented the efficient method to compute the ellipse parameters from range data [START_REF] Vilca | On-line obstacle detection using data range for reactive obstacle avoidance[END_REF].

A. Heuristic approach

The proposed heuristic approach uses the distance between the points to obtain one of the axes.

This method computes the distance between all the points d ij = p i -p j with i, j = 1, . . . , n; and select the maximum distance d max . We have thus, d ij ≤ d max . With this manner, this maximum distance is not decreasing. The line connecting the corresponding two points (the points with the maximum distance) is one of the ellipses axes, where the ellipse center C O is the middle point between the points with maximum distances and the first semi-axes a 1 = d max /2 (cf. Fig 6). Now, we work in the new coordinates system X Y to obtain the second ellipse semi-axes a 2 . We transform the n points to new coordinates system using [START_REF] Tatematsu | Tracking motion of mobile robot for moving target using nurbs curve[END_REF].

P i = cos(Ω) sin(Ω) -sin(Ω) cos(Ω) (P i -C O ) (21)
Where Ω is the orientation of the line between the two points that have the maximum distance. P i (x i , y i ) is the coordinate in the new system, P i (x i , y i ) is the coordinate in the initial system and C O is the coordinate of the ellipse center in the initial system. If the coordinate y i of the points is greater than a threshold > 0, the distance of P i to the origin O is computed, i.e., |y i | > ⇒ d i = P i . The threshold is used to eliminate the points that are collinear with the two points that have the maximum distance. We know that all points inside the ellipse have the distance d i < max {a 1 , a 2 }. As we do not know the spatial distribution of the set of points and to ensure that all the points are inside the ellipse, we choose that a 2 = max {b i }, where b i is the computed semi-axes using P i in [START_REF] Zhang | Parameter estimation techniques: A tutorial with application to conic fitting[END_REF]. In other words, b i ≤ a 2 ≤ max {a 1 , a 2 } ⇒ P i ∈ Ellipse. Therefore, the ellipse will enclose all points without regard of the object shape.

Finally, we obtain the semi-axes of the ellipse (20), cf. Fig. 6, such as:

a = max {a 1 , a 2 } b = min {a 1 , a 2 } ( 22
)
The heuristic approach is an efficient method to enclose data with an ellipse, however, this method does not consider the uncertainties in the data which characterize the real experiments.

V. SIMULATIONS RESULTS

To demonstrate the efficiency of the proposed multi-robot navigation using the cooperative object detection, a survey was made. In this survey, we consider a dynamic object as the main target (leader) i.e., the formation will be defined according to the leader configuration. The leader has a square shape with a side of l = 0.13 m. We consider a group of N = 2 mobile robots with a radius of R R = 0.065 m and each one has six infrared range sensors with the maximum detected range equal to D L max = 0.30 m (cf. Fig. 5). The virtual structure is defined by D i = 5R R and Φ 1 = -120 • and Φ 2 = 120 • (cf. Fig. 1). The maximum velocity of the robots is 0.4 m/s and the sample time is 0.01 s. For each simulation the robots start at the same configuration and try to reach the same final configuration. We do not start to use any object detection method until we have enough data range (n data ≥ 3).

This survey is used to make a focus around navigation of the multi-robot formation. Furthermore, the performance of the navigation of MRF using range data of each robot separately or of all group of robots to detect the leader is compared. Fig. 7 shows the trajectory of the multi-robot formation using accurate information of the position an velocity of the object (leader). We observe that the robots converge toward the desired formation (secondary target ST i ) while tracking a smooth trajectories. Fig. 8 shows the distance and orientation error of the mobile robots of the formation. The orientation error shows an overshoot that it was generated when the robots were tracking the curve. The heuristic method gives satisfactory results enclosing the leader by an appropriate circle (cf. Fig 9 and 11).

Fig. 9 shows the trajectory of the multi-robot formation using the estimated position of the object computed from the range data of each robot separately. Also, one of the sample of the evolution of the obtained ellipse is shown. We observe that the robots do not converge toward the desired formation (secondary targets). Fig. 10 shows the distance and orientation error of the mobile robots of the formation. Fig. 11 shows the trajectory of the multi-robot formation using the estimated position of the object computed from the range data of all group of robots. Also, one of the sample of the evolution of the obtained ellipse is shown. We observe that the robots converge to the desired formation (secondary targets) tracking a smooth trajectory. Fig. 12 shows the distance and orientation error of the mobile robots of the formation. These figures show the moment when the evolution of the obtained ellipse has an abrupt change (t c ≈ 10 s). After t c , the error decrease rapidly because the center of the obtained ellipse is close to the center of the effective leader position.

VI. CONCLUSION

This paper proposes to address multi-robot formation control problem using the combination between: behavior based, dynamical virtual structure (already presented in [START_REF] Benzerrouk | Navigation of multi-robot formation in unstructured environment using dynamical virtual structures[END_REF]) and the leader-follower approach. In addition, to obtain reactive and distributed control architecture, only robot's local frame were used. Moreover, an on-line object detection, performed by the multi-robot system is presented. The objective is to localize the position of the leader in the formation. This leader detection is obtained while using a heuristic method that exploits the range data from the group of cooperative robots to obtain the parameters of an ellipse [START_REF] Adouane | Mobile robot navigation in cluttered environment using reactive elliptic trajectories[END_REF]. This ellipse surround completely the leader to follow. Collision with objects was not addressed in this work, it will be subject of further works.

In future works, the problem of outlier detection and uncertainty of the range data will be considered. Also, the segmentation method will be used to categorize between the presence of big and many small objects.

Fig. 1 .

 1 Fig. 1. Keeping a triangular formation by defining a virtual geometrical structure.

Fig. 2 .

 2 Fig. 2. Control architecture for mobile robot navigation.

Fig. 3 .

 3 Fig. 3. Robot configuration in a global reference frame.

Fig. 4 .

 4 Fig. 4. Robot configuration in a local reference frame.

Fig. 5 .

 5 Fig. 5. Range sensor model and data set of n points.

Fig. 6 .

 6 Fig.6. Obtained ellipse using heuristic approach.

Fig. 7 .

 7 Fig. 7. Navigation of the multi-robot formation.

Fig. 8 .

 8 Fig. 8. Distance and orientation error of the multi-robot formation.

Fig. 9 .

 9 Fig. 9. Navigation of the multi-robot formation using object detection of each robot separately.

Fig. 10 .

 10 Fig. 10. Distance and orientation error of the multi-robot formation.

Fig. 11 .

 11 Fig. 11. Navigation of the multi-robot formation using object detection of all group of robot.

Fig. 12 .

 12 Fig. 12. Distance and orientation error of the multi-robot formation.

ACKNOWLEDGMENT

This work is supported by the National Research Agency of France (ANR).