A C Levasseur-Regourd

J Lasue

Eye-Sat, a triple Cubesat to monitor the zodiacal light intensity and polarization

Eye-Sat is a small telescope in a triple Cubesat, under development by students, mostly working at CNES. It should, in 2016-2017, provide a survey of the intensity and polarization of the light scattered by interplanetary dust cloud, leading to a better understanding of the properties and origin of its particles.

Introduction

CNES, the French Space Agency, has developed since 2012 the JANUS project, helping students to build their own nanosatellite. In this context, the triple Cubesat Eye-Sat is being developed by students of French engineering schools working at CNES as interns and students from the IUT (University Institute of Technology) of Cachan, France [START_REF] Elmaleh | A space telescope in a triple Cubesat[END_REF]. Eye-Sat purpose is not only to provide the opportunity for technological demonstrations, but also to study from Earth's orbit the intensity and polarization of the solar light scattered by interplanetary dust particles.

Scientific objectives 2.1 Context in planetary sciences

The interplanetary dust cloud extends from the Sun to at least the asteroid belt, with a symmetry plane slightly inclined on the ecliptic plane. Studying, from Earth's orbit, the intensity and linear polarization of the solar light scattered by interplanetary dust, the socalled zodiacal light, is of interest for two reasons. Firstly, the zodiacal light, as a faint glow that extends all over the sky, constitutes a foreground which needs to be corrected for in the observation of faint extended astronomical sources (e.g., circumstellar disks, galaxies), all the more since it varies with the revolution of the Earth around the Sun [START_REF] Leinert | The 1997 reference of diffuse night sky brightness[END_REF][START_REF] Levasseur-Regourd | Optical and thermal properties of interplanetary dust[END_REF]. Secondly, analysis of its polarization provides information o n t h e l o c a l p r o p e r t i e s o f t h e d u s t particles, such as their spatial density, morphology, composition and albedo. Previous observations, mostly from the Earth and with a resolution in the 5°-15° range, have been used to infer that the local polarization at 90° phase angle increases with increasing solar distance, at least up to 1.5 au in the ecliptic, while the local albedo decreases [START_REF] Leinert | The 1997 reference of diffuse night sky brightness[END_REF][START_REF] Levasseur-Regourd | Optical and thermal properties of interplanetary dust[END_REF]. Simulations of the polarimetric behavior of interplanetary dust particles have suggested that, in the inner solar system, they may consist of a mixture of absorbing and less absorbing materials, that radial changes could originate in a decrease of organic materials with decreasing solar distance (probably due to alteration processes), and that a significant fraction of the interplanetary dust is of cometary origin [START_REF] Lasue | Inferring the interplanetary dust properties from remote observations and simulations[END_REF][START_REF] Levasseur-Regourd | Physical properties of cometary and interplanetary dust[END_REF].

Purpose of Eye-Sat observations

Eye-Sat is dedicated to the study, during its nominal one-year mission, of the zodiacal light intensity and polarization from Earth orbit. Measurements should be obtained, for the first time with a high spatial resolution in the 0.5° to 2° range, over a wide portion of the sky and in four different wavelengths (visible and near-IR domains, roughly equivalent to B, V, R, I wide-band filters).

Design and mission

Instrument

The payload consists of a small telescope, with an optical system (13° x 13° field of view, 50 mm focal length), two rotating wheels (for the spectral filters and the shutter, and for the polarizing filters) and a detector (CMOS image sensor with a bayer pattern).

The total size (before solar panels deployment) is of 340 mm x 100 mm x 100 mm (Fig. 1), with a mass of about 4.8 kg. The maximum available power, provided by the four solar panels, is of 20 W. Telemetry will operate in X-band, to download up to 47 Mb/s. A de-orbiting system, allowing atmospheric re-entry within 25 years, will be included. New onboard technologies, developed by CNES R&D, include a 3-color CMOS camera and a dual-core microprocessor with programmable logic.

Observational constraints

Zodiacal light observations need to be free of straylight, originating not only from the Sun and the Earth, but also from the Earth's atmosphere, the Moon, the bright planets and stars, and our galaxy (typically for galactic latitudes below ± 20°). Constraints in the possibility of observing in a given direction are also related to the onboard startracker's field of view and to the crossing of the South A t l a n t i c A n o m a l y . Images of major scientific interest are to be obtained by 90° from the Sun near the ecliptic plane (providing, by direct inversion, information on the interplanetary dust properties near the Earth), towards the gegenschein region around 180° from the Sun in the ecliptic (providing information on the dust backscattering properties), and towards the ecliptic poles regions (to assess the geometry of the symmetry plane). Besides, images of the zodiacal cone (along the ecliptic, at a solar distance in the 40° to 90° range) and of the Milky Way are expected to be obtained, at least for outreach purposes.

Development and launch

Phase A was completed in 2013. Phase B is to be completed in September 2014. The structural model is to be tested for subsystems deployment during a parabolic flight campaign in October 2014. The Qualification Model will be tested in June 2015, and the Flight Model should be ready by January 2016. Eye-Sat is planned to be launched by a Soyuz rocket, in piggy-back with the Microscope micro-satellite in 2016. It will be injected in a Sun-Synchronous Orbit (720 km, 98.27°) for a one-year nominal mission.

Conclusions

For the first time, with a single payload in a Cubesat, a one-year high spatial resolution survey of the zodiacal light intensity and polarization, over a wide portion of the sky and in four different wavelengths (in the visible and near IR domains) will be obtained. This will be a new step in understanding the physical properties of the dust particles present in the interplanetary medium, and in estimating their evolution, together with their sources' relative contributions.

Figure 1 :

 1 Figure 1: Eye-Sat, with its telescope on the upper right and solar panels on the left

Acknowledgements

CNES teams support is warmly acknowledged.


Danilo Alves De Lima

Alessandro Corrêa

An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments

An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments

Danilo Alves de Lima and Alessandro Corrêa Victorino

Abstract-This paper presents a local navigation strategy for autonomous vehicles in urban environments with an Image Based Dynamic Window Approach (IDWA). Differently from the global navigation techniques, which requires the vehicle localization to perform its movement, the focus here was to solve the navigation problem in local navigation steps. For that, the environment features will be used, performing the road lane following e.g. The DWA performs a reactive obstacle avoidance while trying to reach a goal destination. In this case, reach the goal destination is based on the Image Based Visual Servoing equations for road lane following, which were incorporated into the DWA. The final solution takes into account the car kinematics/dynamics constraints to allow the vehicle to follow the road lane while avoiding obstacles. The results show the viability of the proposed methodology.

Index Terms-Dynamic Window Approach, Visual Servoing, Local Navigation, Obstacle Avoidance.

I. INTRODUCTION

Traditionally, a car-like robot navigates based on its perception of the environment and its localization related to the path previously planned. However, in urban environments, the localization systems based in GPS information must deal with signal losses and several noises caused by urban canyons, as reported by many DARPA's Challenges participants between 2004 and 2007 [START_REF] Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF]. Due to the GPS drift, this problem also increases locally when a vehicle tries to follow a road based on GPS points. One way to deal with it is using local navigation techniques, which do not use the vehicle global position to calculate the control action. These techniques are normally based on local features extracted from exteroceptive sensors (like vision systems), which in urban environments there are useful ones available [START_REF] Bonin-Font | Visual navigation for mobile robots: A survey[END_REF]. Thus, a global navigation task in urban environments can be divided in road following (branches), representing the local tasks, and road intersections maneuvers (nodes), connecting the next local task. To accomplish the global task, the global localization system can be limited only to the nodes, e.g. using techniques based on vehicle-to-infrastructure (V2I) communication [START_REF] Hassan | Vehicle localization system based on ir-uwb for v2i applications[END_REF].

Focusing on the local navigation, there are many approaches to perform vehicle control using visual data [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], [START_REF] Lee | Automatic lane following with a single camera[END_REF], [START_REF] Bonin-Font | Visual navigation for mobile robots: A survey[END_REF]. In addition to follow the desired features the vehicle must also consider reactive techniques for obstacle avoidance. A well-known reactive technique is the Dynamic Window Approach (DWA) [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF], which searches for an optimal input

The authors are with Heudiasyc UMR CNRS 7253 Université de Technologie de Compiègne. Danilo Alves Lima holds a Ph.D scholarship from Picardi region. Contact authors danilo.alves-de-lima@hds.utc.fr command between all possibles commands in a short time interval. Its optimization function takes into account the final goal position (heading), the obstacles distance (dist), and the maximum linear velocity (velocity) during the calculation. It also considers the kinematics and some dynamics constraints of the robot. Due to the nature of its optimization function, it can be adapted to several techniques [START_REF] Brock | High-speed navigation using the global dynamic window approach[END_REF], [START_REF] Ogren | A convergent dynamic window approach to obstacle avoidance[END_REF], [START_REF] Saranrittichai | Robust local obstacle avoidance for mobile robot based on dynamic window approach[END_REF], to different robot types, like car-like robots [START_REF] Rebai | Car-like robot navigation at high speed[END_REF], [START_REF] Rebai | Bi-steerable robot navigation using a modified dynamic window approach[END_REF], as well as to dynamic environments [START_REF] Seder | Dynamic window based approach to mobile robot motion control in the presence of moving obstacles[END_REF]. However, these works using DWA are conceived when the robot and goal positions are known in the world frame, recalling the localization problems previously mentioned.

To avoid this problem, the proposed work presents a local navigation strategy for autonomous vehicles in urban environments with an Image Based Dynamic Window Approach (IDWA). Differently from the global DWA techniques [START_REF] Brock | High-speed navigation using the global dynamic window approach[END_REF], [START_REF] Lima | Navigation of an autonomous car using vector fields and the dynamic window approach[END_REF], [START_REF] Saranrittichai | Robust local obstacle avoidance for mobile robot based on dynamic window approach[END_REF], which requires the vehicle localization to perform its movement, the focus here is to solve the navigation problem in local navigation steps using the environment features acquired from a camera, performing e.g. the road lane following. In this case, reaching the goal destination is a task guaranteed by the Image Based Visual Servoing equations [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF], [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF], incorporated into the DWA functions. This work also differs from the vision navigation approach proposed by [START_REF] Cherubini | A new tentacles-based technique for avoiding obstacles during visual navigation[END_REF], based on the tentacles technique of [START_REF] Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF], once the image based task and the obstacle avoidance are in the same controller on the robot velocity space. The objective in mind is to perform Image Based Visual Servoing control tasks while validating their velocity outputs in a obstacle avoidance methodology. In the near future, it will allow electric vehicles, like the one from the project VERVE1 , to perform local navigation in road lanes with a safe behavior.

In the block diagram of the Figure 1 it is shown the present methodology, structured in two general layers: workspace perception and robot control. Its concepts are presented in this article as follow: Section II presents the robot model used and the problem definition; Section III presents the workspace perception layer, describing the environment perception strategy to features extraction and obstacle detection; the navigation control layer, with the proposed Image Based Dynamic Window Approach, is presented in the Section IV; an experimental analysis and validation of the method, using a simulated car-like robot, is in Section V; and, finally, Section VI presents some conclusions and perspectives for future works.

II. GENERAL DEFINITIONS

The car-like robot used in this work is similar to the ones described in [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF], [START_REF] Lima | Navigation of an autonomous car using vector fields and the dynamic window approach[END_REF]. It is considered to move in a planar workspace with a fixed pinhole camera directed to the front to perceive and follow the road lane center, which defines a path once differentiable in IR 2 . The vehicle is also considerate to be over the road surface and able to always see a road lane. The kinematic model is based on a front wheel car, represented as [START_REF] Luca | Robot Motion Planning and Control[END_REF]:

    ẋr ẏr θ φ     =     cos θ cos φ sin θ cos φ sin φ/l 0     v 1 +     0 0 0 1     v 2 , (1)
where the vehicle configuration is given by q = [x r y r θ φ] T , with the position (x r , y r ) and orientation (θ) of the car's reference frame {R} in relation to a static world reference frame {O}, and φ is the average steering angle of each front wheel by the Ackerman approximation [START_REF] Luca | Robot Motion Planning and Control[END_REF]. The orientation and steering angles (θ and φ) are defined as θ ∈] -π, π] and φ ∈ [-φ max , φ max ], both positive counter-clockwise. The Figure 2 illustrates these variables. Note that the origin of {R} is located at the midpoint of the two rear wheels, which performs circular trajectories defined by the instantaneous center of curvature (ICC). The approximation for the steering angle φ is related to the x r axis, pointed to the front of the vehicle.

For the vehicle model (1), the control input is u = [v 1 v 2 ] T , where v 1 is the linear velocity of the front wheels and v 2 is the steering velocity. In this model, the robot linear velocity v is related to the front wheels velocity by v = v 1 cos(φ), and the angular velocity θ = v 1 cos(φ)/r 1 = ω is directed related to the steering angle (see the Figure 2), which allows to chose the robot control input as u r = [v ω] T . These inputs can be generalized for an unicycle robot, although the robot frame {R}, in the unicycle, be located in the projection of the wheel center on the ground. The main difference is regarding a constraint present in the car-like robot model, which limits the ICC (Figure 2) by φ, and that is not present in the unicycle robot.

Figure 2 also represents the camera frame {C} with optical center position in (x c , y c , z c ) = (t x , t y , t z ) in the robot frame and a constant tilt offset 0 < ρ < π 2 related to the x r axis, required for the image based approach. The camera final position is in the robot sagittal plane (t y = 0), which is not a limitation, but must be with a certain height from the floor (t z > 0). Finally, the camera's image frame {I} is illustrated in the Figure 3, with a defined size of (2X I , 2Y I ).

III. WORKSPACE PERCEPTION

The workspace perception is the first step for the local navigation task proposed (Figure 1), that provides the environment information (calculated by on-boarded camera and laser scan) required to perform the Image Based Dynamic Window Approach (IDWA). It was divided in 2D features extraction, obstacle detection and occupancy grid representation.

The current implementation of the IDWA uses a similar features set presented by Cherubini et al [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF], applied in a path reach and following strategy of a nonholonomic robot. It uses small path features set to navigate, defined as the projection in the image plane of a visible white line on the floor, with its features calculated in the image frame {I} in an Image Based Visual Servoing scheme [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF]. These features were adapted for the road lane following problem as described in Figure 3, where they are related to the tangent Γ of the path P (according to its direction) at the point D = (X, Y ), with an angular offset Θ ∈] -π, π] from Γ to the axis -Y (positive counterclockwise). P is the center of the road surface between the boundaries δ 1 and δ 2 , which are on the limit of the most right visible lane or, in case of non lane marks, are on the road limits.

An obstacle detection layer is also necessary in the IDWA to guarantee the right execution of obstacle avoidance maneuvers, and with an occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF] the obstacles can be stored during the robot movement. Once no entire environment information must be on the grid, the occupancy grid can be reduced to a local window around the robot, actualized with its movement (see Figure 1). For more details about the obstacle detection and occupancy grid layers, see [START_REF] Lima | Navigation of an autonomous car using vector fields and the dynamic window approach[END_REF]. This implementation considers only static environments for validation purposes, which does not restrain a future implementation with dynamic environments as presented in [START_REF] Seder | Dynamic window based approach to mobile robot motion control in the presence of moving obstacles[END_REF].

IV. NAVIGATION CONTROL

The present controller was based on the integration of the Image Based Visual Servoing (IBVS) [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF] equations with the Dynamic Window Approach (DWA) [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF] to perform the obstacle avoidance while performing the road lane following. This technique was called by Image Based Dynamic Window Approach (IDWA), used by the navigation control layer of Figure 1, and will be presented in this section.

A. The Dynamic Window Approach

DWA is a reactive obstacle avoidance technique proposed originally by [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF], adapted for car-like robots by [START_REF] Rebai | Car-like robot navigation at high speed[END_REF], which selects in the velocity space an optimum control input around the current robot state. It regards some kinematics/dynamics conditions of the robot to construct a control search space, classified by the weighted sum of three functions. They are based on the goal position (heading), the obstacle distance (dist) and the final linear velocity (velocity), compounding the objective function (3):

DW A(v, ω) =α • heading(v, ω) + β • dist(v, ω) + γ • velocity(v, ω), (3)
to be optimized.

1) The DWA Functions: In the original formulation of the DWA [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF], the function heading(v, ω) is responsible to guide the robot to a desired goal position, calculating high weights to the velocity inputs that lead the robot to a final orientation closer to the goal position in the world frame. It is frequently adapted when some specific navigation task is required [START_REF] Brock | High-speed navigation using the global dynamic window approach[END_REF], [START_REF] Lima | Navigation of an autonomous car using vector fields and the dynamic window approach[END_REF], [START_REF] Saranrittichai | Robust local obstacle avoidance for mobile robot based on dynamic window approach[END_REF]. Its improvements proposed for this work will be presented in the Subsection IV-B. The next function dist(v, ω) is the normalized distance to collision when performing circular movements, calculated for polygonal robots as proposed by [START_REF] Arras | Real-time obstacle avoidance for polygonal robots with a reduced dynamic window[END_REF]. It uses the obstacle information from the occupancy grid described in Section III.

To avoid unsafe conditions while performing the obstacle avoidance, a similar consideration from [START_REF] Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF] was applied to expand the robot neighborhood in the dist evaluation. The last function, velocity(v, ω) is calculated based on the desired robot linear velocity v d (which is constant regarding to the road speed limit), as follow:

velocity =          v (v d -v min ) if v ≤ v d , (v max -v) (v max -v d ) if v > v d . (4)
The importance of these previous functions in the objective function is adjusted by the constant gains α, β and γ.

2) The DWA Search Space: Initially, for the current vehicle velocity (v a , ω a ), the Dynamic Window V d is defined for all reachable velocities in a time interval △t as:

V d = {(v, ω)| v ∈ [v a -v△t, v a + v△t] , ω ∈ [ω a -ω△t, ω a + ω△t]} , (5)
with u r = [v ω] T the set of robot inputs (see section II), and v and ω are the robot accelerations.

Once defined the reachable velocities, they must be classified in admissible or not due to the obstacle distance (function dist(v, ω) defined previously and proposed by [START_REF] Arras | Real-time obstacle avoidance for polygonal robots with a reduced dynamic window[END_REF]) and the robot maximum breaking accelerations ( vb , ωb ). The resulting set is defined as:

V a = {(v, ω)| v ≤ 2 • dist(v, ω) • vb , ω ≤ 2 • dist(v, ω) • ωb } . (6)
Finally, the Dynamic Window search space is computed as:

V DW = V d ∩ V a ∩ V s , (7)
where V s is the set of points that satisfy the maximum acceleration constraints vmax and ωmax . It considers the current speed of the vehicle, its accelerations/physicals limits, and the obstacles in the workspace. By discretization of the search space V DW , a velocity must be selected following the criteria presented by the objective function (3).

B. The Image Based Dynamic Window Approach

Considering the objective function (3) and the search space [START_REF] Brock | High-speed navigation using the global dynamic window approach[END_REF], the main changes for the Image Based Dynamic Window Approach (IDWA) concern to the function heading(v, ω). As previously mentioned, it is responsible to guide the robot to a desired goal in the world frame. For the present formulation, the goal is to lead the features set s = [X Y Θ] T , defined in Section III by the tangent Γ in the image frame {I} (see Figure 3), to the final configuration X * = Θ * = 0 and Y * = Y I , which means the vehicle in the center of the road.

Based on the Image Based Visual Servoing (IBVS) equations proposed by Cherubini et al [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF], the heading function must estimate the features error:

e =   X t+△t -X * Y t+△t -Y * Θ t+△t -Θ *   , L s =      -sin ρ-Y cos ρ tz 0 X(sin ρ+Y cos ρ) tz XY -1-X 2 Y 0 -sin ρ-Y cos ρ tz Y (sin ρ+Y cos ρ) tz
[ Ẋ Ẏ Θ] T = L s (X, Y, Θ)u c . ( 8
)
Note that each line of the matrix L s are related to its respective image feature (L X , L Y and L Θ ). The robot velocity u r can be expressed in the camera frame {C} by ( 9) using the homogeneous transformation [START_REF] Rebai | Car-like robot navigation at high speed[END_REF]:

u c = C T R u r , (9)
C T R =         0 -t x -sin ρ 0 cos ρ 0 0 0 0 -cos ρ 0 -sin ρ         . ( 10
)
The final features configuration can be acquired using the equations ( 8) and ( 9) to estimate the features velocity ṡ and integrating them over the time interval △t. Cherubini et al has defined a row and column controller, depending of the D = (X, Y ) point location in the image frame {I} (see Figure 3). The row controller is applied when Y = const = Y * or the column one otherwise. Under this constraint, the function heading(v, ω) was divided in: XY error (v, ω), responsible for the row/column error (X or Y ); and Θ error (v, ω) with the Θ error. The final values are calculated as:

XY error =      1 -|eX | eXmax , if row controller, 1 -|eY | eY max , otherwise. (11)
Θ error = 1 - |e Θ | π . ( 12
)
where e X , e Y , and e Θ are the features error in the image frame I t+△t , and e Xmax and e Y max are the maximum measurable error in X and Y . The final value is defined as:

heading(v, ω) = α 1 XY error (v, ω) + α 2 Θ error (v, ω). (13)

V. EXPERIMENTAL RESULTS

To validate the navigation methodology proposed by the block diagram of Figure 1, a simulation environment using Matlab was created for some road configurations (see Figures 5a and 7a). The vehicle moves based on the kinematic model of the equation 1, respecting its kinematics constraints and some actuators dynamics. It simulates a monocular camera with a focal length 1.8mm and large field of view (≃ 140 • ). The camera tilt offset ρ = 10 • and (t x , t y , t z ) = (2.0, 0.0, 2.0)m. The obstacles are detected by a simulated laser sensor with 180 • of coverage in front of the vehicle and the information was stored in an occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF], locally constructed around the robot. Each laser reading is represented in the occupancy grid by a bidimensional Gaussian model. The relative movement of the robot frame updates the grid information, using its proprioceptive data, like odometry (in real environments the visual odometry can be applied), velocity and steering angle, which is enough for the simulation purposes and low speed experiments.

The Image based Dynamic Window Approach (IDWA) must considers the road limits, obstacles and linear velocity variations when adjusting the gains α, β, and γ from equation 3. When no obstacle obstructs the robot path, the movement is similar to the one in Figure 5b. To visualize their influence in the final navigation, the Figure 6(a-c) combines them one by one. In the Figure 6a only the function velocity was applied, resulting in a movement with no direction that stops at the first visible obstacle. Enabling the function dist, it gives the Figure 6b with a movement over regions free of obstacles but without any goal, better observed in the curves. Adding the heading functions vs1 and vs2 to the previous ones, it results the Figure 6c. Here the robot avoid the obstacles and follow when possible the It is important to observe that even using only one function in the objective function, the final movement will be safe from collisions, once we are using only the velocities from the DW search space in 7. An extended experiment is presented in Figure 7.

The influence of changing the road lane setpoint when overtaking an obstacle was verified in Figure 8. Comparing the Figures 8a, with the setpoint ways on the right lane, and 8b, where the setpoint changes due to the lane obstruction, the second one presents a smoothest trajectory and farther from the obstacles than the first one. This is better for real world applications, providing more safety for the vehicle movement. Finally, the Figure 9 shows the resulting commands and the image features error evolution during the first 21 seconds of simulation using the IDWA controller. Note that, even if the IDWA is a discrete technique, the output commands are smooth for real cars actuators. This can be better observed in the steering angle (Figure 9c), rarely reaching values higher than ±10 • in the complete circuit, which results in more comfort for a final user. The image features error converge smoothly to zero when there are no obstacles preventing the vehicle movement, as seen in Figure 9d.

VI. CONCLUSIONS AND FUTURE WORKS

This work presented an image based local navigation approach applicable to car-like robots in urban environments among obstacles. For that, it was integrated the Image-Based Visual Servoing (IBVS) equations, originally conceived to follow lines on the floor with a small features set, in the Dynamic Window Approach (DWA), resulting in a local navigation system independent of the vehicle global localization and the final destination. The Image Based Dynamic Window Approach (IDWA), as it was called here, allowed the robot to perform the path reaching, to follow the road lane center avoiding obstacles. The methodology was tested in a simulation environment over Matlab considering the car kinematics and some dynamics constraints, and sensors limitations, which provided a solid validation for the proposed solution in a static environment.

The optimization function of the DWA taken in to account the path reaching and following problem, the obstacles avoidance, and the linear velocities variations, in order to find the control inputs that best attend the gains setup. However, find the best adjustment for these gains depends of which element must be considered preferentially, if is to follow the road lane, to keep the distance from the surrounding obstacles, or moves with higher velocities. For the current configuration, the vehicle was able to complete its navigation task with smooth control inputs. The present approach also left us switch the setpoint between the lanes during the overpass maneuver. It is important to mention that other IBVS equations could be integrated in the present solution to allow different tasks.

Thanks to the nature of the elements analyzed, the application of this solution in a real car-like robot could be done with low cost sensors. The experiments are being prepared to the autonomous car Iris of the projects VERVE and ROBOTEX, from the Heudiasyc laboratory. Considering the real environments, more robust techniques must be applied to detect the image features, like the one proposed in [START_REF] Vitor | A 2d/3d vision based approach applied to road detection in urban environments[END_REF], as well as the moving obstacles like in [START_REF] Seder | Dynamic window based approach to mobile robot motion control in the presence of moving obstacles[END_REF]. Other applications in mind are related to the human machine interface, improving the interaction between the vehicle and its conductor.

Fig. 1 .

 1 Fig. 1. Methodology block diagram.

Fig. 2 .

 2 Fig. 2. Kinematic model diagram for a front wheel car-like robot. In this model the vehicle reference frame R performs circular trajectories related to the instantaneous center of curvature (ICC). The pinhole camera frame is also represented in C.

Fig. 3 .

 3 Fig. 3. Image frame {I} with the road lane center projection P (in red) related to the boundaries δ 1 and δ 2 (in yellow), its tangent Γ (in blue) at the point D and the angle offset Θ of Γ to the axis -Y .

Fig. 4 .

 4 Fig. 4. Estimation of the features set Γ i (blue line) in the frame I t+△t applying the control inputs (v 1 , ω 1 ), (v 2 , ω 2 ) and (v 3 , ω 3 ). The reference position is also represented in red, which means the vehicle in the center of the road lane.

Fig. 5 .

 5 Fig. 5. Environment for car-like robot simulation (a) and its navigation in this condition (b). The car initial pose is represented in yellow, and in red are the car instantaneous positions for a clockwise movement.

Fig. 6 .

 6 Fig. 6. Evaluation of the IDWA functions in the navigation task for: velocity (a), velocity + dist (b), and the complete objective function with the heading functions vs1 and vs2 (c). The gains where set to α 1 = α 2 = 0.01, β = 0.2, and γ = 0.3. The car initial pose is represented in yellow, the obstacles are in blue, and in red are the car instantaneous positions for a clockwise movement.

Fig. 7 .

 7 Fig. 7. Environment for car-like robot simulation (a) and its navigation in this condition (b), with α 1 = α 2 = 0.01, β = 0.2, and γ = 0.3. In the cases where do not have the road markers, the vehicle follows the road center. The car initial pose is represented in yellow and starts the movement to the left. The obstacles are in blue, and in red are the car instantaneous positions.

Fig. 8 .

 8 Fig. 8. Vehicle movement with the setpoint defined only on the right lane (a) and switching due to the lane obstruction (b), illustrated by the red line in the camera image from the car point of view. The car initial pose is represented in yellow, the obstacles are in blue, and in red are the car instantaneous positions for a clockwise movement.

Fig. 9 .

 9 Fig. 9. IDWA controller outputs and image error evolution for the trajectory represented in (a). In (b) is the linear velocity and (c) the steering angle commands. The image features errors are in (d).

The project VERVE stands for Novel Vehicle Dynamics Control Technique for Enhancing Active Safety and Range Extension of Intelligent Electric Vehicles.


Danilo Alves De Lima

Alessandro Corrêa

A visual servoing approach for road lane following with obstacle avoidance

Keywords: Visual Servoing, Dynamic Window Approach, Local Navigation, Obstacle Avoidance

This paper presents a local navigation strategy with obstacle avoidance applied to autonomous robotic automobiles in urban environments, based on the validation of a Visual Servoing controller in a Dynamic Window Approach. Typically, Visual Servoing applications do not consider velocity changes to stop the robot in danger situations or avoid obstacles, while performing the navigation task. However, in several urban conditions, these are elements that must be deal with to guarantee the safe movement of the car. As a solution for this problem, in this work a line following Visual Servoing controller will be used to perform road lane following tasks and its control outputs will be validated in an Image-Based Dynamic Window Approach. The final solution is a validation scheme for the Visual Servoing velocities which allows the obstacle avoidance, taking into account the car kinematics and some dynamics constraints. Experiments in simulation and with a full-sized car show the viability of the proposed methodology.

I. INTRODUCTION

Traditionally, a car-like robot performs its movement based on a trajectory planned from its perception and localization sensor's data. The localization is common related to GPS data which must deal with noise and signal losses, some described by many participants of the DARPA Grand Challenges competitions held by the American's Defense Advanced Research Projects Agency (DARPA) between 2004 and 2007 [START_REF] Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF]. For global navigation tasks, a localization system is important to the task accomplishment, similarly when the human driver uses the GPS information or his previous knowledge to navigate in a city. However, in a local navigation task, following GPS points results in many path following problems that must be avoided. In recent works, the use of exteroceptive sensors (like LIDAR and vision systems) for automatic vehicle guidance has considerably raised, specially in urban environments, once there are useful features available [START_REF] Bonin-Font | Visual navigation for mobile robots: A survey[END_REF].

Visual Servoing is one of the many ways to deal with this guidance problem using visual features in a sensorbased navigation. It can be divided in two main approaches: the Position-Based Visual Servoing (PBVS) and the Image-Based Visual Servoing (IBVS) [START_REF] Chaumette | Visual servo control. i. basic approaches[END_REF]. They are related to where the control objective is expressed, which means in the robot's Cartesian Space or in the image space directly. Based on this principle, several control laws can be defined to allow a vehicle to converge and follow different features

The authors are with Heudiasyc UMR CNRS 7253 Université de Technologie de Compiègne. Danilo Alves Lima holds a Ph.D scholarship from Picardi region. Contact authors danilo.alves-de-lima@hds.utc.fr and primitives, like points, lines, and ellipses [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF]. It is important to mention that there are many others Visual Servoing approaches for nonholonomic robots, combining the both IBVS and PBVS information, as in [START_REF] Malis | 2 1/2 d visual servoing[END_REF], or without prior knowledge of the scene [START_REF] Rives | Visual servoing based on epipolar geometry[END_REF], [START_REF] Silveira | Visual servoing over unknown, unstructured, large-scale scenes[END_REF].

Although these tasks can guide the vehicle, they do not directly perform velocities changes to avoid obstacles, essential in urban environments navigation. Considering the obstacle avoidance problem, some approaches define control laws which combine the Visual Servoing task with some obstacle avoidance methodology (e.g., potential field and tentacles) [START_REF] Cadenat | A controller to perform a visually guided tracking task in a cluttered environment[END_REF], [START_REF] Folio | A redundancy-based scheme to perform safe vision-based tasks amidst obstacles[END_REF], [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF]. Normally, these tasks are merged in the control level by some switching methodology that changes the task weight and, consequently, the control law.

This work address the car-like robot local navigation problem using an IBVS as deliberative control for road lane following with a reactive obstacle avoidance, compounding a hybrid control strategy. Once that the IBVS is independent of the vehicle localization and does not consider the presence of obstacles to perform its task, an obstacle avoidance technique must be incorporated to safely perform the local navigation. Using a reduced feature set, based on the one presented in [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF], the Visual Servoing equations were integrated to the Dynamic Window Approach (DWA) [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF], compounding a reactive obstacle avoidance called by Image-Based Dynamic Window Approach (IDWA) [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF]. With the IDWA, the original IBVS velocities are validated, taking in to account the obstacles and the vehicle dynamic/kinematic constraints in the control loop framework. The methodology used diverges from the one presented in [START_REF] Lima | Navigation of an autonomous car using vector fields and the dynamic window approach[END_REF], where a Velocity Vector Field was integrated to the DWA to perform the navigation of a car-like robot, once it required a path planning and global localization of the vehicle, becoming unsuited for visual local navigation. This work also diverges from the previous ones [START_REF] Cadenat | A controller to perform a visually guided tracking task in a cluttered environment[END_REF], [START_REF] Folio | A redundancy-based scheme to perform safe vision-based tasks amidst obstacles[END_REF], [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF] based on Visual Servoing, once the obstacle avoidance proposed with the DWA incorporates the path following and the velocity control in its calculations. Moreover, differently from [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF] that uses the reactive control of the IDWA to guide the car, the present work combines the advantages of both deliberative/reactive controllers in a hybrid control. The objective in mind is to allow electric vehicles, like the one from the project VERVE1 , to perform local navigation in road lanes with a safe behavior.

The proposed methodology was structured in two layers: Workspace Perception and Navigation Control, as show in the block diagram of the Figure 1. To present its concepts, this article is structured as follow: Section II presents the robot model used and some definitions; Section III presents the workspace perception layer, describing the strategy used to features extraction and obstacle detection; the navigation control layer, that proposes a validation scheme for the Visual Servoing controller in the Image-Based Dynamic Window Approach, is presented in the Section IV; an experimental analysis and validation of the method, using a simulated and real autonomous vehicle, is in Section V; and Section VI presents some conclusions and perspectives for future works.

II. GENERAL DEFINITIONS

The robot is considered to move in a planar workspace, similar to the one described in [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF], executing an Image-Based Visual Servoing task (IBVS) with a fixed pinhole camera. It follows the road lane center which defines a path once differentiable in IR 2 . The vehicle is considerate to be over the road surface, and able to always see the road lane. For validation purposes at low speed, a kinematic model of a front wheel drive car was considered, represented as [START_REF] Luca | Robot Motion Planning and Control[END_REF]:

    ẋr ẏr θ φ     =     cos θ cos φ sin θ cos φ sin φ/l 0     v 1 +     0 0 0 1     v 2 , (1)
where the vehicle configuration is given by q = [x r y r θ φ] T , with the position (x r , y r ) and orientation (θ) of the car's reference frame {R} in relation to a static world reference frame {O}, and φ is the average steering angle of each front wheel by the Ackerman approximation [START_REF] Luca | Robot Motion Planning and Control[END_REF]. The orientation and steering angles (θ and φ) are positive counter-clockwise, with θ ∈] -π, π] and φ ∈ [-φ max , φ max ]. The Figure 2 illustrates these variables. Note that the origin of {R} is located at the midpoint of the two rear wheels, which performs circular trajectories defined by the instantaneous center of curvature (ICC), and the approximation for the steering angle φ is related to the x r axis, pointed to the front of the vehicle. The control input for the vehicle of the model ( 1)

is u = [v 1 v 2 ] T
, where v 1 is the linear velocity of the front wheels and v 2 is the steering velocity. In this model, the robot linear velocity v is related to the front wheels velocity by v = v 1 cos(φ), and the angular velocity θ = v 1 cos(φ)/r 1 = ω is directed related to the steering angle (see the Figure 2), which allows to chose the robot control input as u r = [v ω] T . The Figure 2 also represents the camera frame {C} with optical center position in (x c , y c , z c ) = (t x , t y , t z ) in the robot frame and a constant tilt offset 0 < ρ < π 2 related to the x r axis. In this work the camera was positioned in the robot sagittal plane (t y = 0), which is not a limitation, and must be with a certain height from the floor (t z > 0). Finally, the camera's image frame {I} is represented in the Figure 3a, with defined size as (2X I , 2Y I ).

III. WORKSPACE PERCEPTION

As presented in the Figure 1, the workspace perception is the first step for the navigation task proposed, responsible to provide all environment information (calculated by onboarded camera and laser scan) required to perform the visual servoing task and obstacle avoidance. It was divided in the 2D features extraction, the obstacle detection and representation in the occupancy grid.

For the visual servoing task, Cherubini et al. [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF] implemented a path reach and following strategy using a small set of path features to navigate a nonholonomic robot. The path was defined by the projection in the image plane of a visible white line on the floor, with its features calculated in the image frame {I} for an IBVS scheme. These features, as shown in the Figure 3a, are related to the tangent Γ of the path P (according to its direction) at D, with an angular offset Θ ∈] -π, π] from Γ to the axis -Y (positive counterclockwise). In this work, the path P was defined as the center of the road surface between the boundaries δ 1 and δ 2 , which are on the limit of the most right visible lane or, in case of non lane marks, are on the road limits. With this defined path, the Γ features were defined similarly to the ones presented by Cherubini et al. [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF]. These features can be better visualized in the Figure 3a.

L s =      -sin ρ-Y cos ρ tz 0 X(sin ρ+Y cos ρ) tz XY -1-X 2 Y 0 -sin ρ-Y cos ρ tz Y (sin ρ+Y cos ρ) tz 1+Y 2 -XY -X cos ρ cos 2 Θ tz cos ρ cos Θ sin Θ tz - cos ρ cos Θ(Y sin Θ+X cos Θ) tz -(Y sin Θ+X cos Θ) cos Θ -(Y sin Θ+X cos Θ) sin Θ -1      (2)
Since the simulation environment can be easily structured and environment perception was not the focus, the features detection was readily accomplished. However, the simulated camera uses the same intrinsic parameters of the real camera of the final experiments. For the real scenario (Figure 3b), the features detection was performed in a road segmented image, based on [START_REF] Neto | Real-time estimation of drivable image area based on monocular vision[END_REF], which the final result is in the Figure 3c. The final image features were calculated by detecting the road boundaries in the previous segmentation and then fitting a line by the RANSAC approach, giving the line of the Figure 3d.

To guarantee the robot surrounding perception and perform the obstacle avoidance maneuvers, the occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF] is the tool for this task. It is a probabilistic technique which maps the workspace using sensors data acquired during the robot movement. Considering that no entire environment information must be on the grid, the occupancy grid can be reduced to a local window around the robot, actualized with its movement (see Figure 1). The occupancy grid was filled with road surface and obstacle information from a laser sensor (for simulation experiments) and a stereo camera (for real experiments), as presented in the Figures 4a and4b. The obstacle detection for the real experiments were based on a combination of the U/V disparity maps [START_REF] Labayrade | Real time obstacle detection in stereovision on non flat road geometry through "V-disparity" representation[END_REF], with the results represented in the occupancy grid as described in [START_REF] Nguyen | Stereo-camera-based urban environment perception using occupancy grid and object tracking[END_REF].

IV. NAVIGATION CONTROL

The present controller was divided in two base schemes, one to perform the Image-Based Visual Servoing (IBVS) [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF] to follow the road path on the floor, as a deliberative controller, and other to perform the obstacle avoidance maneuvers, as a reactive controller, with the validation of the Visual Servoing velocities in the Image-Based Dynamic Window Approach (IDWA) [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF]. This section will describe this controller, represented by the block VS+IDWA of Figure 1.

A. Visual Servoing Controller

This subsection describes the formulation used in Cherubini et al. [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF] for the IBVS approach to follow a line path projected in the image frame. Let us consider the image plane presented in the Figure 3a

[ Ẏ Θ] T = L s (X, Y, Θ)u c . (3)
Note that each line of the matrix L s are related to its respective image feature (L X , L Y and L Θ ). The robot velocities u r can be expressed in the camera frame {C} by (4) using the homogeneous transformation (5):

u c = C T R u r , (4)
C T R =         0 -t x -sin ρ 0 cos ρ 0 0 0 0 -cos ρ 0 -sin ρ         (5)
Each column of the transformation C T R are related to the robot velocity, named by T v and T ω . The row controller must drive (X,Θ) to a desired set point (X * ,Θ * ), regulating the error e = [X -X * Θ -Θ * ] T to zero. This task is performed under the constraint Y = const = Y * . Under this constraint the system state equations are:

[ Ẋ Θ] T = A r v + B r ω, (6)
with

A r = L X L Θ T v and B r = L X L Θ T ω .
When B r = 0, the control law is:

ω = -B + r (λe + A r v), (7)
where λ = [λ X λ Θ ] T are positive gains. The column controller can be analogously defined changing the row controller definitions from X to Y . For more details of the implementation and stability analysis see [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF].

B. VS+IDWA

The Visual Servoing (VS) methodology presented in the previous subsection do not guarantee safeness to the car movement, once the obstacle information and the vehicle constraints and dimensions are not considered. Due to this, the VS velocities must be validated before being applied in the robot. The Dynamic Window Approach (DWA) is a reactive obstacle avoidance technique proposed originally by [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF], with a modification for car-like robots presented in [START_REF] Rebai | Car-like robot navigation at high speed[END_REF], which selects in the velocity space the best control input to the robot regarding some conditions. It takes into account the weighted sum of three functions based on the goal position (heading), the obstacle collision distance (dist) and the final linear velocity (velocity), compounding an objective function ( 8) to be optimized.

DW A(v, ω) =α • heading(v, ω) + β • dist(v, ω) + γ • velocity(v). (8)
When the VS velocities are invalid, the DWA objective function is performed to return a new valid velocity input.

Although the resulted velocities are valid, it is necessary to guarantee the same VS goal in the DWA to continue with the road lane following in focus while avoiding the obstacle. For that, in our previous work [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF] a new approach for the DWA was presented, which considers 2D image features to guide the robot and 3D obstacles information to avoid them. This reactive controller was named as Image-Based Dynamic Window Approach (IDWA) and the main change in the objective function ( 8) is concerned to the function heading, as explained bellow.

1) The IDWA Functions:

The first function heading(v, ω), in the original formulation of the DWA [START_REF] Fox | The dynamic window approach to collision avoidance[END_REF], is responsible to guide the vehicle to a desired goal. It calculates high weights to the velocity inputs which lead the vehicle to a final orientation closer to the goal position. Based on the robot localization, the goal is precisely known in the world. However, in the present VS application the robot localization is not required, and the goal is leading the robot to the condition were the image features error is closer to zero (see Sub-Section IV-A).

Recalling the VS features (the tangent Γ in the Figure 3) for the row (X, Θ) and column (Y , Θ) controllers, to allow the IDWA to reduce the features errors, the function heading(v, ω) was divided in: vs1(v, ω), responsible for the row/column error (X or Y ); and vs2(v, ω) with the Θ error. The main point for both functions is to estimate the position of the tangent Γ, formed in the image frame I t+△t , if applied the control input (v, ω). This can be acquired using the equations ( 3) and ( 4) to estimate the features velocity ṡ and integrating the computed values over the time (see [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF] for further details). The final values are calculated as:

vs1 =      1 -|eX | eXmax , if row controller, 1 -|eY | eY max , otherwise. (9)
vs2 = 1 - |e Θ | π . (10)
where e X , e Y , and e Θ are the features errors in the image frame I t+△t and e Xmax and e Y max are the maximum measurable errors in X and Y . The final value is defined as:

heading(v, ω) = α 1 vs1(v, ω) + α 2 vs2(v, ω). (11)
The next function dist(v, ω) is the normalized distance to collision, calculated for polygonal robots as proposed by [START_REF] Arras | Real-time obstacle avoidance for polygonal robots with a reduced dynamic window[END_REF]. The last function, velocity(v) is calculated based on the desired robot linear velocity v d from the VS approach of the Subsection IV-A (which is constant regarding to the road speed limit), as follow:

velocity =          v (v d -v min ) if v ≤ v d , (v max -v) (v max -v d ) if v > v d . (12)
2) The IDWA Search Space and VS validation: Initially, for the robot current velocity (v a , ω a ), the Dynamic Window V d is defined for all reachable velocities in a time interval △t as:

V d = {(v, ω)| v ∈ [v a -v△t, v a + v△t] , ω ∈ [ω a -ω△t, ω a + ω△t]} , (13)
with the robot input set u r = [v ω] T section II) and the robot accelerations ( v, ω). Following, each reachable velocity must be classified in admissible or not due to the obstacle collision distance (function dist(v, ω) defined previously and proposed by [START_REF] Arras | Real-time obstacle avoidance for polygonal robots with a reduced dynamic window[END_REF]) and the robot maximum breaking accelerations ( vb , ωb ). If the distance to the obstacle in a circular trajectory is bigger than the distance required to stop safely the vehicle, then the velocity is admissible. The resulting set is defined as:

V a = {(v, ω)| v ≤ 2 • dist(v, ω) • vb , ω ≤ 2 • dist(v, ω) • ωb } . (14)
Finally, the Dynamic Window search space considering the current speed of the vehicle, its accelerations/physical limits, and the obstacles in the workspace is computed as:

V DW = V d ∩ V a ∩ V s , (15)
where V s is the set of points that satisfy the maximum acceleration constraints vmax and ωmax . By discretization of the search space V DW , a velocity must be selected following the criteria presented by the objective function [START_REF] Silveira | Visual servoing over unknown, unstructured, large-scale scenes[END_REF]. The resulted search space V DW is the one used to validate the control input calculated from the VS approach. For the case where this input are not allowed, a new control input is calculated by the objective function defined previously.

V. EXPERIMENTAL RESULTS

To validate the navigation methodology proposed, both simulation and real environment experiments were performed. In these situations, the vehicle moves based on the kinematic model of the equation 1, respecting its kinematics constraints and some actuators dynamics. They were defined to be equal to those from our experimental electrical carlike robot from the project ROBOTEX in Figure 5. For the workspace perception, the vehicle uses a monocular camera with a focal length of 1.8mm and large field of view (≃ 140 • ) to detect the road lane center, as described in Section III. The camera is in a rigid structure with tilt offset ρ ≃ 7 • and (t x , t y , t z ) = (2.0, 0.0, 2.0)m. It also detect the obstacles from a laser sensor with 180 • of coverage (simulation experiments) or a stereo vision camera (real experiments). The detected obstacles data are represented in occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF], locally constructed around the robot. This occupancy grid is updated by a bidimensional Gaussian model to each sensor measurement and the relative movement of the robot frame, using its proprioceptive information, like odometry, velocity and steering angle, which is enough for low speed experiments.

The simulation environment was codified using Matlab with the road surface approximated as the Figure 6a. It implements the methodology presented in Figure 1 to adjust the Visual Servoing (VS) controller parameters and the Image-Based Dynamic Window Approach (IDWA) validation during the robot navigation. Using the right road lane center as reference for the Image-Based Visual Servoing (IBVS), similar to the Figure 3a, the vehicle trajectory without considering the obstacles is performed as the Figure 6b. In this figure, the result presented in [START_REF] Cherubini | Visual servoing for path reaching with nonholonomic robots[END_REF] is confirmed showing that the IBVS is not robust for path reaching which results in large overshoots on the final path. However, for the path following, when the lateral error is small, the robot can track the road lane with better precision than a position-based controller, not presented here.

To avoid this path reaching problem, the VS+IDWA controller was applied considering the road limits, obstacles and linear velocity variations. The gains α, β, and γ from equation 8, were adjusted similarly to the result presented in [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF]. The complete adjustment of the VS+IDWA can be seen at Figures 6c, 6d, and6f, where the gains were set to λ = 0.7, α 1 = α 2 = 0.01, β = 0.2, and γ = 0.3 to guarantee a movement that follows the VS features and avoid smoothly the obstacles. To compare the reactive controller IDWA alone and the hybrid controller VS+IDWA, the Figures 6e and6f were created. The vehicle was able to avoid the obstacles in both cases, but when there are no frontal obstacles, the hybrid solution presents a better performance to guide the robot fast to the reference condition, which means the robot in the road lane center.

To verify the VS and VS+IDWA outputs command, the Figure 7 shows a 20 seconds of simulation during an obstacle avoidance maneuver. As expected in Figure 7(b-c), when the VS outputs are admissible to be applied on the robot (see Section IV), its values are kept by the IDWA. However, after 3 seconds the IDWA starts to modify the VS output to guarantee the obstacle avoidance.

The real size car-like robot experiments were performed in a non-structured circuit, as the one of Figure 8a, which reproduces the road center following experiment. In this case, the robot must move respecting the road boundaries limits and the desired linear velocity of 1m/s. As expected, the robot was able to correct the detected features errors (Figures 8b-c) and converge to the road center, even in the presence of considerable variations in the features detection. The validation of the VS velocities in the IDWA can be observed in the Figures 8d-e, where the linear velocity and steering angle were limited by the current approach.

VI. CONCLUSIONS AND FUTURE WORKS

This work presented a local navigation approach for carlike robots in urban environments, combining an Image-Based Visual Servoing with an Image-Based Dynamic Window Approach in a hybrid controller called VS+IDWA. Using only the IBVS controller, the accomplishment of the road lane following task was not guaranteed, due to some limitations like: the path reaching problems, the constant linear velocity, and no obstacle avoidance. The hybrid VS+IDWA controller was validated in simulation, performing the road lane following with obstacles avoidance in different scenarios. A full-sized car-like robot experiment also showed the viability of the proposed methodology. In the future, more experiments using the full-sized car-like robot must be performed, considering a dynamic model to increase the vehicle speed and some improvements in the workspace perception layer to reduce the features and obstacles detection variations.

Fig. 1 .

 1 Fig. 1. Methodology block diagram.

Fig. 2 .

 2 Fig. 2. Kinematic model diagram of a front wheel drive car-like robot centered in the reference frame R. The pinhole camera frame is represented in C.

Fig. 3 .

 3 Fig. 3. Acquired image frame {I} from the simulation environment (a) with the road lane center projection P (in red) related to the boundaries δ 1 and δ 2 (in yellow), its tangent Γ (in blue) at the point D and the angle offset Θ of Γ to the axis -Y . (b) is a sample image of the real environment, (c) is the road segmentation approach based in [16], and (d) the fitted road center line by the RANSAC approach.

Fig. 4 .

 4 Fig. 4. Obstacle detection for the simulation (a) and real environments (b).

  , and the features set s = [X Y Θ] T defined by the tangent Γ, which is the same features base used by Cherubini et al. [5], as explained in the Section III. The goal is to compute a control input to drive these features to the final configuration X = Θ = 0 and Y = Y I , which means the vehicle in the center of the road. It starts defining a constant linear velocity v = v d > 0 regarding the road speed limits, and applying a nonlinear feedback control law in the angular velocity ω. Cherubini et al. [5] describes two primitive controllers (row and column), which have the same principle. The controller must relate the image features velocities ṡ = [ Ẋ Ẏ Θ] T to the robot velocities u r = [v ω] T . First of all, the image features velocities must be written in therms of the camera frame velocities u c = [v c,x v c,y v c,z ω c,x ω c,y ω c,z ] T . Using the interaction matrix L s (X, Y, Θ) (2), expressed for a normalized perspective camera model, thus:

Fig. 5 .

 5 Fig. 5. Car-like robot from the project ROBOTEX.

Fig. 6 .

 6 Fig. 6. Simulation environment (a), with the Image-Based Visual Servoing (IBVS) controller performing the lane following with λ = 0.7 (b). The VS+IDWA for the same task is in (c) and with some static obstacles is in (d), where λ = 0.7, α 1 = α 2 = 0.01, β = 0.2, and γ = 0.3. For comparison, the car trajectory using only the reactive controller IDWA is in (e) and the complete solution VS+IDWA integrating the deliberative and reactive controllers are in (f). The car initial pose is represented in yellow, and in red are the car instantaneous positions for a clockwise movement.

Fig. 7 .

 7 Fig. 7. VS and the VS+IDWA output for the simulated path represented in (a). In (b) is the linear velocity and (c) is the steering angle φ.

Fig. 8 .

 8 Fig. 8. Road center following applying the VS+IDWA, where (a) presents some detected image features sequence during the experiment, with the evolution of the X/Y errors in (b) and the θ error in (c). The final output calculated by VS and the VS+IDWA are in (d) and (e).

The project VERVE stands for Novel Vehicle Dynamics Control Technique for Enhancing Active Safety and Range Extension of Intelligent Electric Vehicles.

ACKNOWLEDGEMENT

This work was carried out and funded in the framework of the Equipex ROBOTEX (Reference ANR-10-EQPX-44-01). It was equally supported by the French Picardie project VERVE, French Government, through the program "Investments for the future" managed by the National Agency for Research, and the the European Fund of Regional Development FEDER. The authors wish to thank the helpful assistance of Mr. Pierre Hudelaine during the experiments.


Elisa C M Pereira

Danilo A Lima

Alessandro C Victorino

Autonomous vehicle global navigation approach associating sensor based control and digital maps

Keywords: Digital maps, Global Navigation, Sensor Based Navigation

This paper proposes a global navigation strategy for autonomous vehicle combining sensor based control and digital maps information. In order to avoid localization problems in urban environments, this approach intends to solve the global navigation focusing on two local navigation tasks: road lane following and road intersection maneuvers. For that, it is important to use digital maps, once they provide a rich database containing information about the environment structure, like speed limit, number of lanes, traffic directions, etc. Associating the data extracted from the digital map with local navigation approaches, the vehicle is able to perform its global navigation task. The experiments take into account real data and a simulated scenario, which show the viability of this approach.

I. INTRODUCTION

Currently, the research on autonomous vehicle has focused on low cost systems, adapted to different urban environments, since these factors influence the viability of a future production of the vehicle in question. Navigation is a key aspect when developing an autonomous vehicle. To navigates, a car-like robot must be able to perceive its surroundings and localize itself. However, as the surrounding becomes more complex, so does the localization problem. Even with differential corrections, the GPS information fails when the urban environment has structures like tall buildings, overpasses and tunnels [START_REF] Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF]. One way to solve this problem is to use a navigation system that do not depend on accurate localization information.

The structure of an urban environment can be divided into corridors and critical points. The corridors represent the streets which are connected by critical points, as road intersections and traffic lights. When the vehicle is in a corridor, the navigation task does not change given that there is only one possible road to follow. At critical points, on the other side, the navigation system must take a decision, like, for example, choosing the correct road to follow from the the current available options in a road intersection or stopping in a traffic light. In this scenario, the car-like robot must localize itself only when it is near a critical point, avoiding the localization problems explained above.

This work is funded in part by the Picardie Region VERVE project, in part by the ANR JCJC RECIF and carried out in the framework of the Labex MS2T (Reference ANR-11-IDEX-0004-02). The authors are working at Heudiasyc Laboratory, UMR CNRS 7253, Université de Technologie de Compiègne, BP 20529, 60205 Compiègne, France. danilo.lima-de-alves@hds.utc.fr, alessandro.victorino@hds.utc.fr, elisacmpereira@gmail.com

In order to solve the navigation task, it is fundamental to use a database containing information about the environment structure. For this, digital maps provide rich information about streets, tracks, railways, waterways, points of interest detailing number of lanes and speed limits. There are many approaches using digital maps to improve the global navigation problem, like enhancing the vehicle localization as it was done in [START_REF] Mattern | High-accurate vehicle localization using digital maps and coherency images[END_REF], [START_REF] Irie | Localization and road boundary recognition in urban environments using digital street maps[END_REF], [START_REF] Schindler | Vehicle self-localization with high-precision digital maps[END_REF]. In this article, the focus on extracting information from digital maps is not to solve localization problems, but the difficulties of navigation as a whole [START_REF] Hentschel | Autonomous robot navigation based on openstreetmap geodata[END_REF].

Dealing with corridor navigation, there are approaches based on robot proprioceptive information which are able to control the vehicle without accurate position information [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], [START_REF] Lee | Automatic lane following with a single camera[END_REF], [START_REF] Broggi | The argo autonomous vehicle's vision and control systems[END_REF]. These techniques use local environment information acquired from cameras and others sensors to accomplish the local task of road lane following. In addition to use the proprioceptive information, the approach presented in [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF] reacts to the environment, thus interacting with obstacles during the robot movement. Even avoiding the localization problems during a local task, these approaches were not developed to deal with road intersections. In this case, road lane features can not be well detected, resulting in wrong movements for the car and failing to complete a global navigation task.

To perform global navigation task, including at road intersections, DARPA's Challenges participants [START_REF] Thrun | The robot that won the darpa grand challenge[END_REF] used a detailed predefined map of the path and the route network. With the same objective, [START_REF] Regele | Using ontology-based traffic models for more efficient decision making of autonomous vehicles[END_REF] proposes a hierarchical world model and distinguishes between a low-level model for the trajectory planning. Differently from these techniques, which requires the vehicle localization to perform its movement, the focus here is to solve the global navigation in local tasks.

These local tasks are divided in corridor following and critical points. The technique presented in [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF], which controls the vehicle by combining the road lane following with an obstacle avoidance, is used to solve the local navigation task at corridor. To improve this approach, a routing table will provide information about number of lanes and speed limit, enriching the route knowledge. Considering critical points as road intersections, our approach intends to artificially change the environment information, guiding the robot to the next local navigation task. The goal is to induce the techniques like [START_REF] Victorino | Safe navigation for indoor mobile robots. part I: a sensor-based navigation framework[END_REF] and [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF] to provide the correct destination when there is more than one possibility. For that, it is fundamental to provide information about the critical points, as its structure and approximated geographic position.

The block diagram of the Figure 1 shows the present methodology, structured in two main layers: Global planning and Global navigation. This article is organized as follow: Section II presents the robot model and the environment perception; Section III presents the global planning, describing the network of roads; the global navigation is presented in Section IV; Section V presents the experimental results and Section VI the conclusions.

II. GENERAL DEFINITIONS

The workspace perception (composed by on-boarded camera and laser scan) is responsible for provide the local features necessary to the navigation task. The construction of the workspace perception in 2D follows the order: features extraction, obstacle detection and occupancy grid representation. After extracting the environment information, an obstacle detection is applied with the purpose of characterizing the navigation area. The last step correspond to map the obstacles in a local occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF]. Actualized with the robot movement, the occupancy grid contains only the local obstacles around the robot. For more details about the obstacle detection and occupancy grid layers, see [START_REF] Lima | Navigation of an autonomous car using vector fields and the dynamic window approach[END_REF]. This implementation considers only static environments for validation purposes, which does not restrain a future implementation with dynamic environments as presented in [START_REF] Seder | Dynamic window based approach to mobile robot motion control in the presence of moving obstacles[END_REF].

A low-cost GPS provides the vehicle position (longitude, latitude and altitude). Without any correction, its expected error can reach 6 meters. Even with this rude error, the technique proposed is able to perform the navigation.

To validate the navigation approach proposed in this work, a simulated car-like robot will be used. It moves in a planar workspace with a fixed pinhole camera and a laser scan, both directed to the front to perceive the environment. The kinematic model is based on a front wheel car [START_REF] Luca | Robot Motion Planning and Control[END_REF] as:

    ẋr ẏr θ φ     =     cos θ cos φ sin θ cos φ sin φ/l 0     v 1 +     0 0 0 1     v 2 , (1)
where the vehicle configuration is given by q = [x r y r θ φ] T , with the position (x r , y r ) and orientation (θ) of the car's reference frame {R} in relation to a static world reference frame {O}, and φ is the average steering angle of each front wheel by the Ackerman approximation [START_REF] Luca | Robot Motion Planning and Control[END_REF]. The steering and orientation angles (θ and φ) are positive counter-clockwise, with θ ∈] -π, π] and φ ∈ [-φ max , φ max ]. The Figure 2 illustrates these variables, for more information see [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF].

III. GLOBAL ROUTE PLANNING

The global route planning is the first step for the global navigation task, this step is calculated before the robot movements starts and can be actualized if any unexpected change happens on the way. The global route planning is based on OpenStreetMap [START_REF] Haklay | Openstreetmap: User-generated street maps[END_REF] information.

The OpenStreetMap data include three basic geometric elements: nodes, ways and relations. The nodes are geometrical points interconnected by ways used to represent the geometry of the way, in case of roads, they also represent the critical points, e.g. the road intersections. To fit the geometry, the ways represent a set of nodes characterizing a specified object like roads, buildings, railways, etc. The element relations constructs a relationship of geoobjects, the relation may contain nodes and ways. To classify the different types of nodes, the term key tag is applied according the origin of the node (building, railways, highways). In our case, it is necessary to filter the data considering only the ways with the key tag "highways", once they correspond to the navigable area.

For path planning, the optimal path from the current q init position to q goal position is calculated using the A* search algorithm [START_REF] Hart | A formal basis for the heuristic determination of minimum cost paths[END_REF]. This search algorithm minimizes the cost function, computing the path with the lowest cost. The Euclidean distance between two nodes correspond to the cost of the path, in this way, the shortest path is calculated choosing the least-cost paths to reach the q goal position. The result is a list of nodes in which the robot must pass (see Figure 3).

From this list, the local information is extracted and organized as a routing table, as shown in the Table I. Four classifications are considered according to the traffic directions at the critical point. If all the ways arrive at the node, the critical point is classified as arrival point (P a ); similarly, the critical point is classified as a departure point (P d ) if all connected ways exit. It is also important to take into account the case in which both are at the same node, the classification is called arrival and departure (P a d).

The roundabouts must be considered as a special critical point, once the perception requirements increase as well as the risks of accident. Therefore, there is a classification called roundabout point (P r ) which represents this case. The Figure 3 shows traffic directions at critical points using arrows with two different colors: yellow is representing the right way and blue is representing the wrong way. In this way, critical points with only yellow arrows may be considered as departure points, as well as points with only blue arrows may be considered arrival points.

The last two columns of the table focus on the geometry. It is desirable to guide the local navigation task at critical points, thus preventing the car to take wrong ways. The connected ways have an angle and may have three directions: path to follow(4), wrong way(3) and right way(2) (see columns angles and directions in Table I). Only the way represented as path to follow(4) must be considered as free space.

IV. GLOBAL NAVIGATION

Global navigation takes into account the problem of guide the robot from an initial configuration q init to a final configuration q goal . Focusing on local navigation approaches to avoid localization problems [START_REF] Hundelshausen | Driving with tentacles: Integral structures for sensing and motion[END_REF], the global navigation task will be divided in two local tasks: road lane following and road intersection maneuvers. At this section, both navigation will be presented using a routing table, as the one of the Section III, to solve and improve the navigation problems.

A. Global navigation management

Given the routing table from the q init position to q goal position, the role of navigation management is to determine which local navigation must be performed. For that, it is necessary to know the critical points around the vehicle during its movement. If the robot is between two critical points, the local navigation "Road lane Following" is performed. The information about number of lanes and speed limit from the last node is used to improve this local task (as explained in the Section IV-B bellow). When the robot is near to a critical point, the information about angles and directions are extracted from the table and used to solve the local navigation task as "Road intersection maneuver" (as explained in the Section IV-C bellow).

B. Local navigation: road lane following

There are several approaches based on road lane following [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], [START_REF] Lee | Automatic lane following with a single camera[END_REF], [START_REF] Broggi | The argo autonomous vehicle's vision and control systems[END_REF], which are able to control the vehicle without accurate position information. These approaches require a linear velocity setpoint for the control law, as well as a good perception system to detect the road lane marks. With the previous knowledge of the road speed limits and the number of lane, the speed setpoint can be adjusted and the lane detection algorithms optimized for each road. In addition, with the number of lanes it is possible to reduce the number of false positives in the final result. This work will apply the road lane following algorithm proposed by [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF], called Image Based Dynamic Window Approach (IDWA), to accomplish the current local navigation task. The IDWA was based on the integration of a line following Visual Servoing equations, adapted to the road lane following problem, in a Dynamic Window Approach for reactive obstacle avoidance. Details of its implementation and validation are in [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF]. Beyond the advantages above, this method can use the number of lanes to restrict the robot movement during an obstacle avoidance.

C. Local navigation: road intersection manoeuvres

The approaches based on "Road lane Following" were not developed to deal with road intersection maneuvers, thus requiring different methods to accomplish this task. Our approach intends to complement the vehicle control proposed by [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF] with an algorithm that will artificially change the environment information contained at the local occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF]. This algorithm is based on a routing table which contains the geometry of intersections making possible to previously know the directions that the robot should take and the approximated angle between the current road and the next candidate road.

Considering the previous "Road Lane Following" approach, it is possible to force the vehicle to take right directions at intersections limiting the navigable area. The Figure 4 illustrates the technique proposed, the map shows the path to be crossed by the robot considering the points where exist more than one path to follow. As the vehicle gets closer to these points, it uses the geometry information (given by the routing table) to determine the pink line in the middle of the free space. This line is considered as an obstacle by the perception limiting the next decisions. The arrows represent all the possible decisions but only the green ones are corrects.

To adapt the occupancy grid with virtual obstacles, different types of critical points must be considered: right turn, left turn and roundabout. Each type requires a different algorithm to calculate the pink line (considered as obstacle) given that the obstacle geometry around the robot changes according to the critical point. Once calculated, the pink line is actualized with robot movement.

1) Right turn:

When the vehicle gets closer to a critical point where it must turn right, the pink line is calculated following three steps, as the first critical point in the Figure 4 shows. The first step corresponds to searching the points of the occupancy grid above the vehicle, the blue dotted box represents the area where the points are sought and the red line shows the set of points found. From this set of points, it is possible to calculate the window inclination around the robot. The second step searches the point which corresponds to the street corner, the dotted box represents the search area and the red circle the point found. To find the pink line slope, the inclination of the occupancy grid (calculated at the first step) and the intersection angle (extracted from the routing table) are combined. If it is a two-way street, the street width must be considered.

2) Left turn: The steps to adapt the occupancy in a left turn are similar to the ones mentioned above, see the second critical point at Figure 4. The set of points below the vehicle is sought using a search area represented by a blue dotted box at the first step. This set is used for two reasons: estimate the window inclination and extend the red line giving rise to the pink line (represented at third step). The step two corresponds to finding the street corner which is used to localize the pink line represented in the step four.

3) Roundabout: The set of points representing the obstacles at the occupancy grid is geometrically different when the vehicle arrives and leaves the roundabout. In this way, there are two algorithms used to guide the robot in a roundabout. The first represents the arrival case, where the steps are the same as described in the section Section IV-C.1, only changing the way to perform the second step given that at this case there is no street corner. To leave the roundabout, the steps used to close the free space are equals to the ones described in Section IV-C.1.

V. EXPERIMENTAL RESULTS

To demonstrate the application of digital maps for global navigation on autonomous vehicle, two experiments have been performed. The first experiment intends to create a global route planning at a real environment. The second experiment focuses on simulating the global navigation technique proposed using an environment created by Matlab.

A. Global route planning

The experiment was conducted at the city of Compiègne, in France. The urban area was around the campus of Université de Technologie de Compiègne and included all the types of intersection: arrival point (P a departure point (P d ), arrival and departure point (P ad ) and roundabout point (P r ), Fig. 5. Predefined path from a initial position q init to final position q goal . The red dash line is the differential GPS data and the yellow line the data from the OpenStreetMap. The average error is around 6 to 10 meters.

as seen in Section III. Based on a predefined path, from the initial position q init and to the final position q goal , the algorithm based on OpenStreetMap (OSM) data was applied to extract high level information of this path (see Table II). The next step was driving the vehicle thought this path to extract the localization data generated by a differential GPS.

The Figure 5 compares the OSM data (red) with GPS data (yellow) during the predefined path. It shows the consistency of the data from the OSM database, comparing with the GPS data. The error was around 6 to 10 meters, due to the nature of the OSM data, provided by final users with lowcost GPS. For a global navigation application based on it, an error over than 10 meters must be considered to deal with critical points.

To generate the routing table presented at Table II only a part of the path was used, as presented at Figure 6. The critical points are ordered from 1 to 7, where the traffic directions are presented by arrows with two different colors: yellow for the right way and blue is to the wrong way.

B. Global navigation

In the second experiment, the global navigation technique based on a routing table was simulated with Matlab. The vehicle moves based on the kinematic model of the equation 1, respecting its kinematics constraints and some actuators dynamics. It simulates a monocular camera and a laser sensor as described in [START_REF] Lima | An image based dynamic window approach for local navigation of an autonomous vehicle in urban environments[END_REF], where more details about the simulation environment can be found. The detected obstacles were stored in an occupancy grid [START_REF] Elfes | Using occupancy grids for mobile robot perception and navigation[END_REF] by a bidimensional Gaussian model, locally constructed around the robot. The relative movement of the robot frame updates the grid information, using its proprioceptive data, like odometry, velocities and steering angle, which is enough for the simulation purposes and low speed experiments.

The Figure 7 presents the simulation environment considering the two cases of local navigation: road lane following and road intersection maneuver. At each local navigation task, the information extracted from the routing table are shown according to the nodes around. To visualize the changes at local occupancy grid according to the distance to the critical points, the Figure 8 presents some steps during the road intersection maneuvers. The distance considered to change the local navigation tasks was 15 meters, due to the average error calculated in the Subsection V-A. The localization system simulates a low-cost GPS with a Gaussian error of 6 meters. Finally, the Figure 9 shows the global navigation for different paths.

VI. CONCLUSIONS AND FUTURE WORKS

This paper proposes a global navigation approach independent of an accurate localization system and based on digital map data. The information extracted from this digital map had two main objectives: to solve the navigation task at a critical point and to complement the techniques of road following. The difference between the OpenStreetMap and the real data from differential GPS was close enough to guarantee the viability of the current approach. According to the results, the car-like robot was able to solve the global navigation task with a low-cost localization system, given that it was validated in several circuits conditions with different critical points. Future works will consider real environments for complete validation of the navigation task. In this case, more robust techniques must be applied to change artificially the environment in the occupancy grid and detect the lane to follow, with a low-cost localization system. Also, to improve the technique proposed, the deterministic analysis used in this work must be replaced by a probabilistic analysis taking different event sequences into account.

Fig. 1 .

 1 Fig. 1. Methodology block diagram.

Fig. 2 .

 2 Fig. 2. Kinematic model diagram for a front wheel car-like robot. In this model the vehicle reference frame R performs circular trajectories related to the instantaneous center of curvature (ICC). The pinhole camera frame is also represented in C.

Fig. 3 .

 3 Fig.3. Optimal path planning from q init position to q goal position with the list of nodes representing critical points. The arrows represent the traffic directions at each node, exiting in yellow and arriving in blue.

Fig. 4 .

 4 Fig. 4. Application of local navigation task: road intersection maneuver. The intersections shown correspond to the critical points 1,3,4 and 5 of the Figure 3.

Fig. 6 .

 6 Fig. 6. Part of the predefined path of Figure 5 with the list of critical points ordered from 1 to 7. The arrows represent the traffic directions of exiting (yellow) and arriving (blue) for each node.

Fig. 7 .

 7 Fig. 7. Simulation of a global navigation emphasizing the two local tasks and their respective information extracted from the TableII.

Fig. 8 .

 8 Fig. 8. Steps during the road intersection maneuvers considering limit distance of 15 meters.

Fig. 9 .

 9 Fig. 9. Global navigation simulation for different paths.

TABLE I NAVIGATION

 I INFORMATION TO THE PATH PRESENTED IN FIGURE3