Abdellah Khelloufi
email: akhelloufi@cdta.dz

Nouara Achour

Robin Passama

Andrea Cherubini

Sensor-based Navigation of Omnidirectional Wheeled Robots Dealing with both Collisions and Occlusions

Keywords: Sensor-based navigation, omnidirectional wheeled robots, obstacle avoidance, visibility constraints, occlusions

Introduction

Navigation strategies generally aim at endowing a robot with capacities of perception, decision, and action, that allow it to autonomously move in the environment. These strategies are traditionally divided in two main classes, depending on whether the problem is solved locally or globally. The global approach [START_REF] Lavalle | Planning Algorithms[END_REF]- [START_REF] Sprunk | An accurate and efficient navigation system for omnidirectional robots in industrial environments[END_REF], usually consists in motion planning, hence relies on accurate knowledge of the robot pose and on a global map of the environment. On the other hand, local or reactive strategies are based on instantaneous information from on-board sensors. These strategies include: potential fields [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF][START_REF] Cherubini | Visual Navigation with a time-independent varying reference[END_REF], vector field histogram [START_REF] Borenstein | The Vector Field Histogram -Fast obstacle avoidance for mobile robots[END_REF], dynamic window [START_REF] Fox | The Dynamic Window approach to obstacle avoidance[END_REF], obstacle-restriction method [START_REF] Minguez | The Obstacle-Restriction Method (ORM) for robot obstacle avoidance in difficult environments[END_REF], and closest gap [START_REF] Mujahad | Closest Gap based (CG) reactive obstacle avoidance navigation for highly cluttered environments[END_REF].

In contrast with static environments, where classical path planning approaches are suitable, dynamic environments are more challenging to deal with. Indeed, the decision about the robot motion must be related with the on-line perception of the workspace and the future behaviour of the moving obstacles. To address this issue, several motion planing algorithms have been developed [START_REF] Hoy | Algorithms for collision-free navigation of mobile robots in complex cluttered environments: a survey[END_REF]. Some of these works are based on the concept of velocity obstacle [START_REF] Fiorini | Motion planning in dynamic environments using velocity obstacles[END_REF]- [START_REF] Zhang | Dynamic Obstacle Avoidance for Unmanned Underwater Vehicles Based on an Improved Velocity Obstacle Method[END_REF]. Other approaches employ probabilistic representation to take into account the environment uncertainty [START_REF] Van Den Berg | Anytime path planning and replanning in dynamic environments[END_REF][START_REF] Du Toit | Robot motion planning in dynamic, uncertain environments[END_REF]. These approaches are however computationally expensive. The combination of global and local planers is generally used to improve the computational cost [START_REF] Fulgenzi | Probabilistic motion planning among moving obstacles following typical motion patterns[END_REF][START_REF] Foka | Probabilistic autonomous robot navigation in dynamic environments with human motion prediction[END_REF]. Learning-based methods are also applied by learning cost functions from human demonstration (teleoperation) [START_REF] Kim | Socially adaptive path planning in human environments using inverse reinforcement learning[END_REF][START_REF] Chen | Socially aware motion planning with deep reinforcement learning[END_REF]. On the other hand, reactive strategies have proved more appropriate to use in unknown environments. In particular, the potential field method was extended to take into account unpredictably moving obstacles [START_REF] Ge | Dynamic motion planning for mobile robots using potential field method[END_REF]- [START_REF] Zhang | Dynamic obstacle-avoiding path planning for robots based on modified potential field method[END_REF]. Biologically inspired algorithms are another class of paradigms that can be applied to avoid collisions [START_REF] Savkin | A simple biologically inspired algorithm for collision-free navigation of a unicycle-like robot in dynamic environments with moving obstacles[END_REF][START_REF] Montiel | Path planning for mobile robots using bacterial potential field for avoiding static and dynamic obstacles[END_REF].

One of the advantages of reactive strategies is that they can be well combined with other sensorbased approaches such as visual servoing [START_REF] Chaumette | Visual servo control, Part I: Basic approaches[END_REF][START_REF] Chaumette | Visual servo control, Part II : Advanced approaches[END_REF]. In visual servoing, the task is defined in the sensor space, instead of configuration space, and it does not require neither a global model of the environment nor robot localization. Works in this area include [START_REF] Folio | A redundancy-based scheme to perform safe vision-based tasks amidst obstacles[END_REF], where image-based navigation is combined with obstacle avoidance, for a differential-drive robot equipped with a pan actuated camera. Moving obstacles may be considered in the same approach, as shown in [START_REF] Futterlieb | A Navigational Framework Combining Visual Servoing and Spiral Obstacle Avoidance Techniques[END_REF]. Recently, another interesting framework for visual navigation with obstacle avoidance has been presented in [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF], for a car-like robot equipped with an actuated camera and a lidar. The framework is based on tentacles, i.e., drivable paths used to predict possible collisions in the near future. The task realized in [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF] consists in following a visual path represented by key images, without colliding with the ground obstacles. The authors show that obstacle avoidance does not affect visual navigation and that their approach outperforms potential fields [START_REF] Cherubini | A New Tentacles-based Technique for Avoiding Obstacles during Visual Navigation[END_REF]. The framework was later improved [START_REF] Cherubini | Avoiding moving obstacles during Visual Navigation[END_REF][START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF], by using a Kalman filter to deal with moving obstacles.

A frequent problem in vision-based task control is the loss of image features, due to occlusions or to the sensor's limited field of view. Various prediction and correction methods have been proposed to solve this issue [START_REF] Mezouar | Avoiding self-occlusions and preserving visibility by path planning in the image[END_REF]- [START_REF] Cazy | Visual Servoing when Visual Information is Missing: Experimental Comparison of Visual Feature Prediction Schemes[END_REF]. However, all these approaches are dedicated to manipulator arms. In the context of mobile robotics, the problem of keeping visual target or landmark visibility has been addressed from different viewpoints. The authors of [START_REF] Bhattacharya | Optimal paths for landmark-based navigation by differential-drive robots with field-of-view constraints[END_REF] and [START_REF] Salaris | Shortest paths for a robot with nonholonomic and field-of view constraints[END_REF] study the shortest paths in terms of distance in the image plane for a differential drive robot equipped with a limited field of view sensor. The objective is to obtain the globally optimal paths that allow the robot to maintain a landmark in sight in the absence of obstacles. Recently, these works have been extended to handle environments populated with static obstacles [START_REF] Hayet | A motion planner for maintaining landmark visibility with a differential drive robot[END_REF]- [START_REF] Panagou | Cooperative visibility maintenance for leader-follower formations in obstacle environments[END_REF]. However, all the methods proposed in these works are based on motion planning and require a priori knowledge of the environment. On the other hand, some researchers have addressed visibility constraints in reactive, purely sensor-based navigation strategies. For example, in [START_REF] Lopez-Nicolas | An optimal homography-based control scheme for mobile robots with nonholonomic and field-of-view constraints[END_REF], a homography-based visual controller drives a mobile robot to a goal pose by following some of the optimal paths proposed in [START_REF] Bhattacharya | Optimal paths for landmark-based navigation by differential-drive robots with field-of-view constraints[END_REF]. Another solution, proposed in [START_REF] Folio | A sensor-based controller able to treat total image loss and to guarantee noncollision during a vision-based navigation task[END_REF], consists in predicting the location of features, when they are occluded by static obstacles.

Aside the need for environment knowledge, another limitation of the cited works is that none addresses omnidirectional autonomous ground vehicles (AGVs). These systems, however, are getting increasingly popular in industrial applications, since they can perform complex three-dimensional trajectories, that are indispensable in limited footprint environments. It is therefore surprising that no attention has been paid to incorporating collision and occlusion constraints in the navigation of such platforms.

In this paper, we address the problem of dealing with both collisions and occlusions caused by static and moving obstacles, during visual navigation of an omnidirectional AGV equipped with fixed -limited field of viewon-board sensors. Specifically, the navigation task consists in making the robot safely and autonomously navigate towards a static or moving target, in an unknown environment. To solve this problem, we propose a generalization of the framework designed in [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF] for car-like robots, by introducing omnidirectional tentacles. These tentacles are characterized not only by curvature, but also by course angle, since the robot linear velocity is not necessarily aligned with its heading, as in traditional nonholonomic systems. We also consider the visibility constraints that are induced by the limitations of the fixed sensors field of view. Furthermore, if the target is lost or occluded by an obstacle, a fast and efficient strategy is designed to estimate its current pose, by using its previous pose along with the control inputs. Let us summarize the constraints that are satisfied by our approach: 1) safety is ensured, by avoiding collisions with static and moving obstacles, 2) the target is maintained as much as possible within the sensors field of view even in presence of obstacles, 3) the loss or occlusion of the target is handled by continuous online estimation of its pose.

To our knowledge, this is the first work addressing and validating experimentally safe sensorbased navigation of omnidirectional robots under these three constraints. A preliminary version of our framework was presented in [START_REF] Khelloufi | Tentacle-based moving obstacle avoidance for omnidirectional robots with visibility constraints[END_REF] without considering the target occlusion problem nor real vision (the target pose was estimated via odometry alone). Our main contributions with regards to that work are indicated hereby. show the robot frame F R (R, X R ,Y R ), the occupancy grid (yellow), occupied cells (gray), some omnidirectional tentacles (red and orange), and the robot linear and angular velocities (purple). In this view, we assume the robot is following the red tentacle: this tentacle is tangent to v, with radius v /ω. We also represent the visual sensors frame F C (C, X C ,Y C , Z C ) (centered at C and blue) and the sensors' field of view of amplitude β (cyan).

r The development of an algorithm for detecting and tracking the target pose, using one or two cameras along with the lidar measures. r The inclusion of target loss management to deal simultaneously with collision and occlusion. r Analysis of the controller stability.

The remainder of the paper is organized as follows. In Sect. 2, the problem and relevant variables are defined. The omnidirectional generalization of tentacles is presented in Sect. 3, and we detail our controller in Sect. 4. In Sect. 5, we present the strategy for determining the best path (tentacle) to be tracked by the controller. Section 6 describes the algorithm used to detect and track the visual target. Experiments are presented in Sect. 7, and we finally conclude in Sect. 8.

General Definitions

Omnidirectional platform

We consider an omnidirectional wheeled robot, which can move in any direction on the ground plane. The reader is referred to Fig. 1. We define the robot frame F R (R, X R ,Y R ), which is rigidly constrained to the robot. It has center in R, the robot center of rotation, and axes X R pointing forward and Y R pointing left. The robot control inputs are:

u = [v X v Y ω] , (1)
where v X and v Y are the robot linear velocities in the direction of the axes X R and Y R respectively, and ω is the robot angular velocity (positive counterclockwise). The resultant linear velocity is noted

v = [v X v Y ]
, and its norm is:

v = v 2 X + v 2 Y .
The robot is equipped with a set of forward looking visual sensors (e.g., one or more cameras) with a combined field of view β , centered at C. The visual sensors are fixed, so the robot heading also determines its viewing direction. We define the visual sensors frame F C (C, X C ,Y C , Z C ). This frame is also rigidly linked to the robot chassis, with origin in C, Z C pointing in the forward robot direction, X C pointing right and Y C downward1 . The robot also has fixed on board distance sensors for building a local map of the obstacles surrounding it. This is the occupancy grid, shown in yellow in Fig. 1.

Visual navigation

The navigation task consists in driving towards a visible and possibly moving target, while avoiding the environment obstacles. When the environment is safe, the robot should progress forward, while visually tracking the target. In case avoidable obstacles are present, the robot should circumnavigate them, while maintaining the target in the field of view. If the target is lost or occluded by an obstacle, its current pose must be estimated by using its previous location along with the current control inputs. Finally, if collision is inevitable or if the target is not visible for a long time, the robot must stop.

The specifications of the navigation task are:

1. orient the robot so that it points the visual sensors towards the target, 2. make the robot go towards the target, 3. avoid collisions with the obstacles, while realizing 1 and 2. 4. estimate the current target pose at all times, including during sensor occlusions, in order to predict its pose in case of reappearance.

More formally, the navigation task (i.e., with the four specifications above) is defined in the robot frame F R , instead of in a fixed world coordinate system. In particular, to fulfill specifications 1 and 2, the robot should move so that the visual target T (that can be static or moving

) is displaced in F R , from the current pose R p T = R X T R Y T R θ T to a desired (final) constant pose, R p * T = R X * T R Y * T R θ * T
. This final pose R p * T is defined by the user depending on the application. For example, consider a mobile manipulator having to realize a manipulation on a table: the visual target T will be the table and R p * T will be some table pose in the robot frame that makes the manipulator easily access the table. In summary, R p * T is the desired target pose in the robot frame to be reached, to consider the task achieved.

We define (see Fig. 2 for a complete illustration of these variables):

1. the distance between current and desired target positions:

ρ * = ( R X T -R X * T ) 2 + ( R Y T -R Y * T ) 2 , (2)
2. the orientation offset between current and desired target positions, defined only when ρ * = 0:

α * = atan2 R Y T -R Y * T , R X T -R X * T , (3)
3. the distance between robot and target position:

ρ T = R X T 2 + R Y T 2 , (4)
4. the heading to the target:

α T = atan2 R Y T , R X T if R X T , R Y T = (0, 0) , 0 otherwise. ( 5
) Y R R 𝜃 𝑇 R R 𝜃 𝑇 * X R R 𝑋 𝑇 R 𝑋 𝑇 * R 𝑌 𝑇

Obstacle occupation times

For obstacle modeling, we use the occupancy grid shown in Fig. 1: it is linked to F R , with cell sides parallel to X R and Y R . Its longitudinal and lateral extensions are limited (X m ≤ X R ≤ X M and Y m ≤ Y R ≤ Y M ), to ignore obstacles that are too far to be dangerous. Any grid cell c centered at (X,Y ) is considered occupied if an obstacle has been sensed in c.

The set of occupied cells with their estimated velocities, is denoted by state vector O:

O = {c 1 , . . . , c n , ċ1 , . . . , ċn } . (6)
This is used to derive possible future collisions. Indeed, the estimations of the obstacles positions and velocities are updated at every iteration, by the Kalman observer designed in [START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF]. Then, for each c i that may be occupied by an obstacle within time horizon T h , we can predict initial t i0 (O) ∈ [0, T h ] and final t i f (O) ∈ [t i0 , T h ] obstacle occupation times, in function of the set of occupied cell states O.

Omnidirectional Tentacles

We hereby present a generalization of the tentacles-based approach proposed in [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF], to omnidirectional robots.

Definitions

We use a set of drivable paths (tentacles) both for perception and motion execution. Each tentacle j is a semicircle that starts in R and is tangent to the robot linear velocity v. In contrast with the tentacles originally proposed in [START_REF] Cherubini | Visual navigation of a mobile robot with laser-based collision avoidance[END_REF][START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF], our omnidirectional tentacles are characterized not only by their curvature (i.e., inverse radius)

κ j = ω/ v if v = 0, 0 otherwise, (7)
but also by their course angle

α j = atan2 (v Y , v X ) if v = 0, 0 otherwise. (8)
Instead, on traditional nonholonomic systems, since the linear velocity is aligned with the heading v Y = 0, thus α j = 0 for all control inputs (i.e., all tentacles are tangent to the heading). Curvature κ j belongs to K , a uniformly sampled set2 :

κ j ∈ K = {-κ M , . . . , 0, . . . , κ M } (9)
and α j belongs to A , another uniformly sampled set:

α j ∈ A = {α Min , . . . , 0, . . . , α Max } ⊆ [-π, π[ . (10)
The set of tentacles is T = K × A . The total number of tentacles in T is the product of the number of candidate curvatures by the number of candidate course angles. In Fig. 1, we show an example with 12 tentacles (5 linear and 7 circular tentacles) that could be followed by the robot. At each time instant, only one tentacle (drawn in red) is selected. As indicated in the figure, the robot linear velocity is tangent to this tentacle. Since tentacles are used both for perception and motion, a compromise between computational cost and control accuracy is needed to set the sizes of K and A .

In Figure 3, a straight (α j = 0 and κ j = 0), an oblique (α j = 0 and κ j = 0) and a circular (κ j = 0) tentacles are dashed. Each tentacle j is characterized by two classification areas (collision and dangerous), which are obtained by rigidly displacing, along the tentacle, two rectangular boxes, with increasing size. The boxes are overestimated with respect to the robot dimensions. For each tentacle j, the sets of cells belonging to the two classification areas (shown in Fig. 3) are noted C j and D j3 . The sets O, C j and D j are used to calculate the variables required in our control law, as will be explained just below. In particular, the largest classification area D j is used to select the safest tentacle and to assess its danger, while the thinnest one C j determines the -eventually needed -deceleration.

Robot occupation times

For each dangerous collision cell in tentacle j (i.e., for each cell c i ∈ D j ), we compute the robot occupation time t i j . This is an estimate of the time at which the box will enter the cell, assuming the robot follows the tentacle at the current velocity. To calculate t i j (c i , v, α j , κ j ), we assume that the robot motion is uniform, and displace the box at its current velocities:

   v X = v cos α j v Y = v sin α j ω j = κ j v , (11)
until the instant t i j at which the box intersects cell c i .

Dangerous instants and collision instants

Once the obstacle and robot occupation times have been calculated for each cell, we can derive the earliest time instant at which a collision between obstacle and robot may occur on each tentacle j. By either checking all cells in D j , or focusing just on C j ⊂ D j , and using variables t i0 and t i f introduced in Sect. 2.3, we discern between dangerous instants and collision instants. These are defined as:

t j = inf c i ∈D j {t i j : t i0 ≤ t i j ≤ t i f } (12)
t c j = inf c i ∈C j {t i j : t i0 ≤ t i j ≤ t i f } (13)
respectively. In both cases, we seek the earliest time at which a cell is simultaneously occupied by the obstacle and by the robot. Computation of t j and t c j is illustrated in the example of Fig. 3. These metrics give a good approximation of the time that the robot can travel along the tentacle without colliding. Further details are given in [START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF].

Control Scheme

Tentacle risk function

To assess the danger of each tentacle j, we design a tentacle risk function, by using t j and tuned thresholds t d > 0 and t s > t d (d stands for dangerous, and s for safe):

H j =      0 if t j ≥ t s 1 2 1 + tanh 1 t j -t d + 1 t j -t s if t d < t j < t s 1 if t j ≤ t d . (14)
Note that H j smoothly varies from 0, when possible collisions are in the far future, to 1, when they are forthcoming. If H j = 0, tentacle j is tagged as clear.

To determine the best behaviour (among visual target tracking and obstacle avoidance) to adopt, we assess the danger of the environment via the risk function H = H v of the visual task tentacle, (κ v , α v ). This is the tentacle that best approximates the visual path that the robot would follow to reach the target in absence of obstacles. Depending on the value of H (noted situation risk function), we distinguish the contexts (safe or unsafe) explained below. The derivation of the visual task tentacle is also explained hereby.

Safe context

In the safe context (H = 0), no dangerous obstacle is detected on the robot path. In this case, it is desirable that the robot realizes the task of driving R p T to R p * T , while keeping as much as possible T within its field of view.

Since the angular velocity ω determines both the convergence of R θ T to R θ * T (for pose regulation) and that of α T to 0 (for target visibility), a compromise must be reached. We weigh the two objectives respectively with a gain λ ω ∈ [0, 1] and with its complementary 1λ ω . Priority is given to target visibility when the desired position is farther than ρ α , and to pose regulation when it is nearer than ρ θ 4 . In between, we use a smoothing function, as in ( 14):

λ ω (ρ * ) =      1 if ρ * ≥ ρ α 1 2 1 + tanh 1 ρ θ -ρ * + 1 ρ α -ρ * if ρ θ < ρ * < ρ α 0 if ρ * ≤ ρ θ . ( 15
)
The norm of the translation velocity v must be reduced, as the target is approached. We specify this through variable:

v s (ρ * ) = V if ρ * > ρ v , ρ * ρ v V otherwise, (16)
with V > 0 the maximum desired value for v s and ρ v > 0 the distance at which the robot should slow down. Both V and ρ v are easily tunable variables. Finally, the translation velocity must be aligned with the heading towards the desired position α * , when the target is far (λ ω = 1). As the target gets near (λ ω < 1), we also use the angular velocity ω to regulate R θ T to R θ * T . In summary, the robot control inputs in the safe context are:

   v X = v s cos α * + (1 -λ ω ) ω R Y T v Y = v s sin α * -(1 -λ ω ) ω R X T ω = λ ω α T + (1 -λ ω ) R θ T -R θ * T . (17)
The above safe context control law ( 17) is also used to define the visual path curvature κ s and course angle α s :

κ s = λ ω α T + (1 -λ ω ) R θ T -R θ * T / v α s = atan2 (v Y , v X ) . ( 18
)
Note that in general κ s and α s will not correspond to a tentacle belonging to set T , but they will be used to find the nearest tentacle in T , i.e., the visual task tentacle (κ v , α v ) that characterizes the risk function H, as explained in Sect. 5.3.

Unsafe context

In the unsafe context (H = 1), dangerous obstacles are detected. The robot should circumnavigate them by following the best tentacle. The norm of the translation velocity must be reduced for safety reasons (i.e., to avoid collisions). We specify this by using a function v u ∈ [0, v s ]:

v = v u =    v s if t c b ≥ t c s v s t c b -t c d /t c s -t c d if t c d < t c b < t c s 0 if t c b ≤ t c d (19)
(with t c d > 0 and t c s > t c d two thresholds corresponding to dangerous and safe collision times) to guarantee that the robot decelerates (and eventually stops) as the collision instant on the best tentacle t c b decreases. Then, the control inputs in the unsafe context are:

   v X = v u cos α b v Y = v u sin α b ω = κ b v u , (20)
where α b and κ b are respectively the course angle and the curvature of the best tentacle. In Sect. 5, we will explain how we determine α b and κ b .

General control law

In intermediate contexts (0 < H < 1), the robot should navigate between the visual path, and the best tentacle. The transition between these two extremes will be driven by H. Using all the variables defined above, we can write our controller for visual navigation with obstacle avoidance:

   v X = (1 -H) v s cos α * + (1 -λ ω ) ω R Y T + H v u cos α b v Y = (1 -H) v s sin α * -(1 -λ ω ) ω R X T + H v u sin α b ω = (1 -H) λ ω α T + (1 -λ ω ) R θ T -R θ * T + H v u κ b . (21)
When H = 0 this coincides with [START_REF] Foka | Probabilistic autonomous robot navigation in dynamic environments with human motion prediction[END_REF], and for H = 1, with (20).

Stability Analysis

Assuming that the target is not moving in the environment, we can write:

   R ẊT = -v X + ω R Y T R ẎT = -v Y -ω R X T R θT = -ω. ( 22
)
Based on this hypothesis, we can derive three important stability properties of control law [START_REF] Ren | Modified Newtons method applied to potential field-based navigation for nonholonomic robots in dynamic environments[END_REF]. Although this hypothesis is needed for the proofs, in the experiments we have also validated our framework in cases where the target is moving. Property 1. In the safe context, if ρ * ≥ ρ α (and therefore λ ω = 1) controller ( 21) guarantees asymptotic convergence of task vector s = [ρ * α T ] to s * = [ρ α 0] . Therefore, the robot will orient its heading towards the target while simultaneously approaching it until ρ * = ρ α .

Proof. First, let us derive α T with respect to time, using (5) (for ρ T = 0), along with [START_REF] Xin | A new potential field method for mobile robot path planning in the dynamic environment[END_REF]:

αT = v X R Y T -v Y R X T ρ T 2 -ω. ( 23
)
Injecting the safe context controller [START_REF] Foka | Probabilistic autonomous robot navigation in dynamic environments with human motion prediction[END_REF] in [START_REF] Zhang | Dynamic obstacle-avoiding path planning for robots based on modified potential field method[END_REF] when λ ω = 1:

αT = v s ρ T 2 R Y T cos α * -R X T sin α * -α T . (24)
Since R Y T /ρ T = sin α T and R X T /ρ T = cos α T :

αT = v s ρ T sin (α T -α * ) -α T . (25)
Note that when ρ * ≥ ρ α , α T ≈ α * , since the Euclidean distance between points R and T * becomes neglectable with respect to ρ * (see Fig. 2). Hence, replacing this in [START_REF] Montiel | Path planning for mobile robots using bacterial potential field for avoiding static and dynamic obstacles[END_REF], we can consider the following approximation:

αT = -α T . (26)
Now, let us derive ρ * with respect to time, using (4):

ρ * = R X T -R X * T R ẊT + R Y T -R Y * T R ẎT ρ * . ( 27
)
From (3) if ρ * = 0 (as is the case here):

R X T -R X * T = ρ * cos α * R Y T -R Y * T = ρ * sin α * . (28)
Injecting this in [START_REF] Chaumette | Visual servo control, Part II : Advanced approaches[END_REF] yields:

ρ * = -(v X cos α * + v Y sin α * ) + ω ρ T sin (α T -α * ) , (29)
and with the safe context controller (17) since α T ≈ α * , the previous equation can be approximated as:

ρ * = -v S . (30)
Finally, let us consider the Lyapounov function V = e e/2 where e = ss * . Its time derivative is:

V = ė e = [ ρ * αT ] ρ * -ρ α α T . (31)
Injecting ( 26) and ( 30) in ( 31) yields:

V = -v s (ρ * -ρ α ) -α 2 T , (32)
which is clearly negative definite for e = 0 when ρ * ≥ ρ α .

An alternative control strategy would have consisted in applying the linear velocity that cancels the effect of ω in [START_REF] Xin | A new potential field method for mobile robot path planning in the dynamic environment[END_REF]:

v X = v s cos α * + ω R Y T v Y = v s sin α * -ω R X T . (33)
Although this controller guarantees asymptotic convergence of both ω R X T and ω R Y T to 0, it makes the value of α T constant throughout navigation. Indeed, plugging [START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF] in [START_REF] Zhang | Dynamic obstacle-avoiding path planning for robots based on modified potential field method[END_REF] yields:

αT = v s ρ sin (α T -α * ) , (34)
which is neglectable, since α T ≈ α * . In summary, controller [START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF] does not center the target in the field of view, as one would wish (particularly for far target).

Property 2. In the safe context, if ρ * ≤ ρ θ , controller [START_REF] Ren | Modified Newtons method applied to potential field-based navigation for nonholonomic robots in dynamic environments[END_REF] guarantees asymptotic convergence of task vector s = ρ * R θ T to s * = 0 R θ * T . Hence, the robot will asymptotically converge to the desired pose with respect to the target.

Proof.

When λ ω = 0 (since ρ * ≤ ρ θ ), injecting the safe context controller [START_REF] Foka | Probabilistic autonomous robot navigation in dynamic environments with human motion prediction[END_REF] in 29 yields:

ρ * = -v s , (35)
and from the last equation of ( 22):

R θT = R θ * T -R θ T . (36)
Consider the same Lyapounov function as above. Since R p * T is constant, the Lyapounov function derivative is:

V = ė e = ρ * R θT ρ * R θ T -R θ * T . ( 37
)
If ρ * = 0, injecting [START_REF] Chesi | Keeping features in the field of view in eye-in-hand visual servoing: a switching approach[END_REF] and ( 36) in (37) yields:

V = -v s ρ * -R θ T -R θ * T 2 , (38)
which is clearly negative definite for e = 0.

If

ρ * = 0, R X T = R X * T and R Y T = R Y * T . Hence, V = -R θ T -R θ * T 2
, which is also negative definite for e = 0.

Property 3. In the unsafe context, if moving ( v = 0), the robot will precisely follow the best tentacle (κ b , α b ).

Proof.

When H = 1, ( 21) becomes [START_REF] Ge | Dynamic motion planning for mobile robots using potential field method[END_REF], and it is trivial to see, by applying ( 7) and ( 8) that whenever v = 0, the curvature and course angle will respectively coincide with κ b and α b .

These three properties outline some nice features of our framework. For instance, consider the safe context. Property 1 shows that from far, the robot will orient its heading towards the target (to keep it within its field of view), while approaching it. When the robot is near enough to the target, Property 2 is triggered. Then, the robot will point its heading towards the desired relative orientation, while converging to the desired position with respect to the target.

It is noteworthy to mention that the properties are valid in given operating situations (H and λ ω either null or unitary). It is very difficult (and out of scope here) to study analytically the behaviour of the system in the intermediate situations, i.e., when H and λ ω are different from 0 or 1. However, the smooth design of these two functions, as well the extensive experimental validation of (21) (see Sect. 7) make us very confident about the global performance of our framework.

In the next section, we explain the strategy that is adopted to select the best tentacle to be followed in the unsafe context.

Tentacles Selection Strategy

Here, we describe our strategy for selecting the best tentacle (κ b , α b ), that allows the robot to avoid the obstacles while simultaneously performing the visual task. To this end, we first define a metric for sorting the tentacles. This is explained in Sect. 5.1. The visibility constraints to be satisfied by the best tentacle are given in Sect. 5.2. These criteria are finally used to select the best tentacle, as explained in Sect. 5.3.

Sorting tentacles

To apply any search algorithm to set T , in order to find the best tentacle (κ b , α b ), the set must be somehow sorted. To this end, we must find a metric characterizing similarity (or difference) among tentacles. This is a non-trivial task, since T is defined in a discrete non-Euclidean space:

T = K × A ⊂ R × SO (2) , (39)
where a distance metric cannot be defined, without weighing -arbitrarily -the relative importance of a scalar (κ j ) and an angle (α j ). To solve this, we rely on the following property.

Property 4. Let an omnidirectional robot be modeled as a point. Its Cartesian position, after having followed tentacle (κ j , α j ) at velocity v for an iteration ∆t << 1, depends only on the scalar function:

φ R = α j + v ∆t 2 κ j .
Proof. Since we model the robot as a point, only changes in the robot position (not orientation) are considered. We start by calculating the position change after ∆t, in function of (κ j , α j ). To this end, we define a fixed frame F O (O, X O ,Y O ) that corresponds to the robot frame at the beginning of the iteration (see Fig. 4). For each tentacle j, we can calculate the robot position

O p R = O X R O Y R
in F O after it has followed the tentacle for time ∆t. We can relate the derivative of the robot pose in F O and the control inputs:

   Ȯ X R = v X cos O θ R -v Y sin O θ R Ȯ Y R = v X sin O θ R + v Y cos O θ R Ȯ θ R = ω. (40)
Equation ( 40) can be derived from the kinematic model of omnidirectional mobile robots [START_REF] Canudas De Wit | Theory of Robot Control[END_REF]. This model is generally used to calculate the robot pose with respect to a fixed reference frame by using odometry. In this paper, we employ this model to predict the future robot position, after one sampling time, with respect to the robot pose at the beginning of the sample time. Using [START_REF] Fiorini | Motion planning in dynamic environments using velocity obstacles[END_REF], we can rewrite (40) in function of the followed tentacle, i.e., in function of κ j and α j :

   Ȯ X R = v (cos α j cos O θ R -sin α j sin O θ R ) Ȯ Y R = v (cos α j sin O θ R + sin α j cos O θ R ) Ȯ θ R = v κ j . (41)
By integrating this expression, we can obtain the robot position at ∆t. We distinguish two cases according to κ j : r if κ j = 0 (i.e. the tentacle is a straight line), the robot position will be:

O X R = v ∆t cos α j O Y R = v ∆t sin α j ; (42)
r if κ j = 0, the robot position will be:

O X R = 2/κ j sin (κ j v ∆t/2) cos (α j + κ j v ∆t/2) O Y R = 2/κ j sin (κ j v ∆t/2) sin (α j + κ j v ∆t/2). (43)
Converting the position to polar coordinates

(r R , φ R ), with r R = O X 2 R + O Y 2 R and φ R = atan2( O Y R , O X R ) (see Fig. 4) yields r R (α j , κ j ) = 2 κ j sin κ j 2 v ∆t , ( 44
) φ R (α j , κ j ) = α j + v ∆t 2 κ j . (45)
Since the robot movement is small during an iteration (as ∆t << 1), sin

κ j 2 v ∆t ≈ κ j
2 v ∆t, and therefore r R will not vary much from a tentacle to the other (r R ≈ v ∆t). Hence, the robot position after having followed the tentacle at velocity v for ∆t will depend only on φ R .

As a consequence of Property 4, we have decided to sort the tentacles according to their values of φ R , calculated using [START_REF] Lopez-Nicolas | An optimal homography-based control scheme for mobile robots with nonholonomic and field-of-view constraints[END_REF], and noted φ R, j for each tentacle j.

Tentacles guaranteeing visibility

In this section, we introduce the visibility constraints in our obstacles avoidance strategy. To ensure that the target is not lost during navigation, the best tentacle should keep the target in sight. However, this is not possible for all tentacles, since the robot is constrained to look in its heading direction. Here, we consider as tentacles that guarantee visibility, those that will keep the target in the field of view after an iteration ∆t.

For a given tentacle j, we define (again, refer to Fig. 4):

1. O X T , O Y T , O θ T : the target pose (at t = 0) in the initial robot frame

F O (R O , X O ,Y O ) 2. R X T , R Y T , R θ T : the target pose (at t = ∆t) in the robot frame F R (R R , X R ,Y R ).
The relation between these two poses is given by :

   R X T R Y T R θ T 1    =    cos (ω∆t) sin (ω∆t) 0 R X O -sin (ω∆t) cos (ω∆t) 0 R Y O 0 0 1 -ω∆t 0 0 0 1        O X T O Y T O θ T 1     (46)
where R X O , R Y O are the coordinates of the origin of F O in F R . After ∆t these can be expressed in polar coordinates as:

R X O = -r R cos (φ R -ω ∆t) R Y O = -r R sin (φ R -ω ∆t). ( 47
)
Injecting ( 47) in [START_REF] Folio | A sensor-based controller able to treat total image loss and to guarantee noncollision during a vision-based navigation task[END_REF] and using [START_REF] Lopez-Nicolas | An optimal homography-based control scheme for mobile robots with nonholonomic and field-of-view constraints[END_REF], we derive the target position R X T , R Y T in F R after ∆t:

     R X T = cos (κ j v ∆t) O X T + sin (κ j v ∆t) O Y T -2/κ j sin (κ j v ∆t/2) cos (α j -κ j v ∆t/2) R Y T = -sin (κ j v ∆t) O X T + cos (κ j v ∆t) O Y T -2/κ j sin (κ j v ∆t/2) sin (α j -κ j v ∆t/2), (48)
For the target to be visible, its position in the robot frame must satisfy the following constraints:

       R X T > R X C R Y T > R Y C -( R X T -R X C ) tan β 2 R Y T < R Y C + ( R X T -R X C ) tan β 2 , (49)
with R X C , R Y C the sensors center coordinates in F R and β their field of view (see Fig. 1). In summary, to determine whether tentacle j guarantees target visibility, it is sufficient to inject its (κ j , α j ) into ( 48) and then check if the resulting R X T and R Y T verify constraints (49).

Selecting the best tentacle

Algorithm

T ← RemoveOtherTentacles(T ); 4: end if 5: φ R,s ← CalculateSortingAngle(α s , κ s ); 6: φ R,v ← FindNearestSortingAngle(T , φ R,s ); 7: (α v , κ v ) ← CalculateTentacleParameters(φ R,v ); 8: H ← CalculateRiskFunction(α v , κ v ); 9: (α b , κ b ) ← null; 10: if H = 0 then 11: φ R,p ← CalculateSortingAngle(α p , κ p ); 12: if φ R,v < φ R,p then 13: (φ i , φ f ) ← (φ R,v , φ R,p );
(φ i , φ f ) ← (φ R,p , φ R,v ); 16: end if 17: (α b , κ b ) ← FindNearestClearTentacle(T , [φ i , φ f ]); 18: if (α b , κ b ) = null then 19: (α b , κ b ) ← FindNearestClearTentacle(T , 20: ]φ min , φ i [ ∪ ]φ f , φ max [); 21: end if 22: end if 23: return (H, (α b , κ b ));
Let us describe the best tentacle selection strategy. At initialization, all tentacles in set T = K × A are sorted according to their φ R, j , calculated with [START_REF] Lopez-Nicolas | An optimal homography-based control scheme for mobile robots with nonholonomic and field-of-view constraints[END_REF]. Then, at each iteration ∆t we proceed as follows (see Algorithm 1).

1. The subset of all tentacles that guarantee the visibility constraints is derived as explained in Sect. 5.2. If the number of tentacles in this subset is sufficient (at least 5 in this work), all other tentacles are removed from T . Otherwise, the entire set T is kept, to privilege obstacle avoidance over occlusion avoidance. This choice is motivated on one hand by obvious safety reasons, and on the other by the possibility -offered by the method that will be explained in the next Section -of estimating the target pose even when it is not visible. 2. For the path that the robot would perform if there were no obstacles, i.e., if the safe context controller [START_REF] Foka | Probabilistic autonomous robot navigation in dynamic environments with human motion prediction[END_REF] was applied, we compute: the course angle α s , curvature κ s using [START_REF] Kim | Socially adaptive path planning in human environments using inverse reinforcement learning[END_REF], and polar angular coordinate φ R,s . 3. The tentacle that best approximates the visual path in T (i.e., the nearest in terms of φ R ) is computed; we denote it as the visual task tentacle (κ v , α v ) and calculate its situation risk function as H v using ( 14). This is also the value that will be used in control law ( 21): H = H v . 4. If H = 0, the visual task tentacle is clear and the safe context controller (17) can be applied. 5. Instead, if H = 0, we seek a clear tentacle (H j = 0). a) First, we search among the tentacles with φ R, j between the one of the visual task tentacle and that of the best tentacle at the previous iteration. b) If there are many clear tentacles, the nearest to the visual task tentacle is chosen. c) If none is clear, we search among the others (those that are not between the visual task and the previous best tentacles). Again, the best tentacle will be the clear one closest to the visual task tentacle.

Target Detection and Tracking

In this section, we describe our approach for detecting and tracking over time the target pose in the robot frame, R p T . This is done by the robot visual sensor/s with an image processing algorithm described in Sect. 6.1. If the target is not visible, because it exits the field of view or is occluded, an alternative method is applied, as explained in Sect. 6.2.

Vision-based target detection

Our target detection algorithm relies on the following assumptions: XR R YR Fig. 6. Detecting the cart face segment using lidar measurements.

r the target is characterized by a predominant color, r the visible part of the target is planar, r the target height is known, r as position of the target ( R X T , R Y T ), we take the Cartesian coordinates of the visible plane in F R , r as target orientation R θ T , we take the angular offset from X R to the normal vector entering the visible plane.

The described target detection and tracking algorithm has been developed for the sensor suite available on our self-made robotic platform BAZAR [START_REF] Cherubini | Research on cobotics at the LIRMM IDH group[END_REF]. This is composed of a forward looking fixed pair of 55.8 • field of view cameras operating at 30 Hz, and two 270 • Hokuyo lidars placed at opposite corners of the base, and operating at 40 Hz. The algorithm relies on both cameras, and on the forward looking lidar. The lidar however, is used only when the target is near, to obtain a precise estimation of R p T . The reason is that, although lidar measurements are more precise on a short range, they are biased by inaccuracy and false positives on the long range. On the other hand, from far, when only vision is used, orientation R θ T cannot be properly estimated. Note that controller [START_REF] Ren | Modified Newtons method applied to potential field-based navigation for nonholonomic robots in dynamic environments[END_REF] does not require R θ T as long as ρ * ≥ ρ α . Hence, the most intuitive design choice is to start using the lidar only when ρ * < ρ α . While ρ * ≥ ρ α , only the target position ( R X T , R X T ) is calculated from vision without using lidar data. Even if this estimation is imprecise, it will make the robot approach the target, while orienting its heading towards it until, for ρ * < ρ α , the lidar will be activated and R θ T will also be estimated, to drive the robot to the desired pose with respect to the target.

For image processing, the same steps (developed with OpenCV 5 ) are applied to the left and right cameras. These steps are detailed below and illustrated in Fig. 5.

1. The images are binarized with a logic conjunction (and operator) of predefined ranges in the Hue, Saturation and Value color space. This way, pixels of a certain color (blue in Figures 5c and5d) can be isolated. 2. A dilation and an erosion are applied to the binary image to form blobs of pixels of the target's color. 3. The blob with largest contour is extracted (Fig. 5e,5f). 4. Knowing the real target height and the camera intrinsic and extrinsic parameters, the position of the front face centroid is projected in F R to obtain ( R X T , R Y T ).

5 http://opencv.org/ 5. If the blob is detected by both cameras, the average of the two positions is used.

As the robot approaches the target and ρ * < ρ α , the front lidar is also utilized, to improve the position, and add the orientation estimates. We proceed as follows (see Fig. 6):

1. A high resolution occupancy grid (cell size 3 × 3 cm) is built from lidar measurements. 2. The two side borders of the blob are projected in the grid for both the left (blue cone in Fig. 6) and right (green) cameras. 3. The Hough transform is applied to detect all line segments in the two cones. 4. The extreme (nearer and farther) corners among all these line segments are used to define a region of interest, where linear regression on the raw lidar data is used to find the visible target projection (red segment in Fig. 6). 5. From this segment, the target pose R p T is derived. 6. A Kalman Filter (with state vector composed of R p T and R ṗT ) is finally used to ensure continuity and robustness of the target pose estimation.

Dealing with total loss of the target

If none of the tentacles that guarantee robot safety can satisfy the visibility constraints, the target will exit the field of view and the above approach cannot be applied. The same applies if obstacles on the robot path provoke a partial or total target occlusion. To overcome both problems, we estimate the target pose R X T , R Y T , R θ T in the robot frame by using its previous pose O X T , O Y T , O θ T and control inputs (v X , v Y , ω). This is done by integrating the target pose over time interval ∆t, using ( 46) and ( 47): if ω = 0:

   R X T = cos (ω∆t) O X T + sin (ω∆t) O Y T -2 v /ω sin (ω∆t/2) cos (α -ω∆t/2) R Y T = -sin (ω∆t) O X T + cos (ω∆t) O Y T -2 v /ω sin (ω∆t/2) sin (α -ω∆t/2) R θ T = O θ T -ω ∆t, (50)
with α defined as in [START_REF] Minguez | The Obstacle-Restriction Method (ORM) for robot obstacle avoidance in difficult environments[END_REF], but in the general case (not just α ∈ A ). Injecting these equations in the same Kalman Filter as above (with state vector composed of R p T and R ṗT ), it is possible to deal with the target total loss, by predicting its location without affecting the robot behaviour. In practice, we use (50) in control law [START_REF] Ren | Modified Newtons method applied to potential field-based navigation for nonholonomic robots in dynamic environments[END_REF].

Experimental Validation

Here, we report the simulated and real experiments that we performed to validate our approach. These are also shown in the video attached to this paper 6 . In all tests, we use the same setup and parameters, the only difference being the wheeled platform: KUKA youBot in simulation and Neobotix MPO700 (the base of our BAZAR platform) in real experiments. The target is a blue wheeled cart, similar to the one used for kitting in automotive manufacturing by our partner PSA Peugeot Citroën. It is automatically moved in simulations, and manually pushed in the real experiments. For simplicity, we position the target in the sensors field of view at the beginning of each simulation/experiment. A "target searching" navigation procedure, out of scope here, could be devised as future work.

Both the simulated KUKA youBot and real Neobotix MPO700 are equipped with the BAZAR sensor suite described in Sect. 6.1. In the visibility constraint equation [START_REF] Cherubini | Research on cobotics at the LIRMM IDH group[END_REF], we use as C the midpoint of the 2 optical centers and as β (conservatively) the field of view of the cameras (55.8 • ). The algorithm is implemented on a computer with an Intel Xeon E5 2620 V3 2.4 Ghz Hexacore processor with integrated graphics card (without GPU acceleration) and 32 Gb of RAM. We set to ∆t = 200 ms the sampling time, that includes visual processing (Sect. 6), tentacle processing (Sections 3 and 5) and control (Sect. 4) in a unique thread. This choice of ∆t is motivated by the limitations of the graphics card, since the average computational times of the algorithms are: 100 ms for visual processing, 60 ms for tentacle processing and less than 1 ms for control. One way of reducing ∆t would consist in optimizing the computing time of the vision and tentacle processing pipelines by using a NVidia GeForce GTX 1080 graphics card along with the Opencv GPU API (instead of CPU API). Another improvement would consist in computing the omnidirectional tentacles (from laser scanners) and

Simulation

Let us recall that the main goal is to perform vision-based omnidirectional navigation in the presence of static or moving -and possibly occluding -obstacles. To this end, preliminary simulations were carried out in V-REP 7 , with the KUKA youBot, an omnidirectional robot with four omni wheels. In V-REP, we have designed three scenarios A, B, and C shown respectively in Fig. 7, Fig. 10, and Fig. 11, along with the images acquired by the robot cameras during the simulations. In these figures, the robot, target and obstacles are represented respectively by green, blue and brown boxes. In scenario A, the visual target is static, whereas in scenarios B and C, it is moving. In all scenarios, the robot is able to perform the visual task and to reach the desired relative pose R p * T , while avoiding collisions and managing occlusions provoked by the obstacles. We hereby describe the robot behaviour for each scenario.

7.1.1. Scenario A. The environment is cluttered, with four obstacles that may partially or totally occlude the target. The closest obstacle is static, while the three others move. We have plotted, in Fig. 8, the robot control inputs during this navigation task. The plot shows that after approximately 2 s, the robot is deviated and oriented so that the camera is pointing at the target. When it approaches the first obstacle (at t = 3 s), it starts avoiding it while keeping the target in the camera field of view. Then, the robot progresses toward the desired pose before avoiding the second obstacle which induces a total occlusion of the target. Nevertheless, the robot performs its task thanks to the target pose estimation module described in Sect. 6.2. Afterwards, the robot succeeds in avoiding the last two obstacles and reaches the target despite the occlusions they cause (see images 7 and 8 in Fig. 7). Then (at t = 33 s), the environment is free again, and the visual task can be performed for the rest of the experiment. At the end, the robot decelerates and stops when the desired pose relative to the target has been reached. The target position error

R X T -R X T * , R Y T -R Y T * and orientation error R θ T -R θ T * are
plotted in Fig. 9. Notice that the target orientation R θ T is not estimated until t ≈ 27 s since before that, ρ * ≥ ρ α , as explained in Sect. 6.1. As we can see in these plots, all the errors converge to zero while the robot is driving to the target. This result is coherent, as the visual task consists in moving the target from the initial to the desired pose in F R .

7.1.2. Scenario B. This scenario (shown in Fig. 10) is designed to test lateral collision avoidance by exploiting the omnidirectional characteristic of the robot. The target is moving in the lateral direction with regards to the robot (roughly in the direction parallel to the Y R axis) and an obstacle approaches the robot in the same direction, but opposite verse. After reaching the desired pose relative to the moving target, the controller deviates the robot to maintain its relative heading (hence to guarantee target visibility) while avoiding the moving obstacle. When the environment is clear again, the robot returns to the desired relative pose.

7.1.3. Scenario C. Here, we consider navigation in a corridor (light purple in Fig. 11), common in indoor environments. The target is moving in the corridor, and the robot must follow it, while avoiding the walls as well as two brown boxes. The first one moves perpendicularly to the path and induces an occlusion, while the second one is static and presents a risk of collision. Despite the limited mobility within this environment, the robot is capable of dealing with the occlusion and avoiding both obstacles, while successfully following the target.

Real experiments

After the simulations, we have validated our approach in extensive real experiments, carried out on our Neobotix MPO700 platform. This is an omnidirectional robot with 4 steerable wheels, hence with a reduced mobility as compared to the omni wheels platform used in V-REP. The MPO700 wheels are driven by a low level controller that runs on the embedded PC at a rate of 40 Hz. Instead, our controller and sensor processing algorithms run on an on-board PC that sends the velocity commands (v X , v Y , ω) to the MPO700 PC, via ethernet. We have designed three scenarios:

7.2.1. Scenario A. The MPO700 navigates towards the -static -blue cart, with two obstacles (brown panels) present in the environment. The purpose of the experiment is to validate our framework when the robot has to handle simultaneously collisions and occlusions. We show in Fig. 12 the scenario as well as the images acquired by the left camera during navigation. The control inputs are plotted in Fig. 13. At the beginning (snapshots 1-3), since there is no risk of collision or occlusion, the robot is deviated (v Y ) and oriented (ω) so that the cameras points at the blue chariot. The first obstacle is then circumnavigated without provoking occlusions nor target loss. After that, we manually move the second obstacle to totally occlude the target (snapshots 4 and 5). As can be seen, the robot successfully predicts the target location, while avoiding the obstacle. Finally, when there is no more risk of collisions or occlusions, the MPO700 converges to the blue cart (snapshots 6-10).

The evolution of the target position and orientation errors, during the navigation task, are plotted in Fig. 14. It can be seen that all three errors converge to zero as desired. As shown in Fig. 13 , the control inputs generated by our controller are not smooth. This is due to the nature of our approach, that is based on sampling a set of drivable paths. Since the sample time of our controller is higher than that of the steering wheels controller, it could be possible to filter this signal at low level. This will be done as future work.

7.2.2. Scenario B. In the second experiment, the navigation task consists in exploiting the omnidirectional characteristic of the MPO700, to follow and keep in sight a moving target, while avoiding a lateral moving obstacle (a pedestrian). As shown in Fig. 15, the robot successfully performs the visual task in spite of the walking person. 7.2.3. Scenario C. A final very challenging experiment was carried out to assess the performance in a complex environment, including first a corridor and then a cluttered hall (see Fig. 16). The MPO700 starts by following the blue chariot that is moving along the corridor (snapshots 1-4). A first moving obstacle (human) crosses the robot path and occludes the target without affecting its behaviour (snapshot 5). The robot also succeeds in avoiding a second obstacle, while following and keeping the target in its field of view until reaching the hall (snapshot 6 and 7). Then, the MPO700 is in a difficult situation where an obstacle crosses its way, while the target has slightly changed direction (snapshot 8). This leads to a complete loss of target visibility. Once again, the robot manages to avoid the obstacle and to recover the chariot, thanks to the target pose estimation module. Finally, it reaches the desired pose with respect to the target (snapshots 9 and 10), after having avoided all obstacles while either keeping the target visibility or predicting its location.

Conclusions

We have presented a framework that guarantees obstacle avoidance during visual navigation of an omnidirectional wheeled robot that has to deal with visibility constraints. Additionally, the proposed framework can deal with partial and total visual occlusions provoked by the obstacles. For both perception and motion execution, we design omnidirectional tentacles that exploit the kinematics of the platform. Simulated and real experiments show that the robot is able to perform the task, with safety and smoothness, in spite of occlusions. Future work will investigate how to extend the proposed framework to Multi-Target Tracking (i.e., looking at multiple targets) in the presence of static and moving obstacles.

Fig. 1 .

 1 Fig. 1. Top view of the robot (green box with black wheels) with all the variables used in this work. We

Fig. 2 .

 2 Fig. 2. Definition of the variables related to the visual target T : R X T R Y T R θ T and R X * T R Y * T R θ * T are the current (variable) and desired (constant) poses of the target, (ρ * , α * ) are the distance and orientation between the current and desired target positions, (ρ T , α T ) are the polar coordinates of the current target position, (v X , v Y , ω) are the robot control inputs. All the variables are defined in the robot frame F R (R, X R ,Y R ).

Fig. 3 .

 3 Fig. 3. Tentacles (dashed red), with classification areas, corresponding boxes and delimiting arcs of circle, and cells c i ∈ D j displayed in light blue. The robot linear velocity v, the dangerous instant t j and the collision instant t c j are also illustrated for three tentacles: (a) straight tentacle with α j = 0 and κ j = 0, (b) oblique tentacle with α j = 0 and κ j = 0, (c) circular tentacle with κ j = 0.

Fig. 4 .

 4 Fig. 4. Variables needed for tentacle selection. F O and the control inputs:

Fig. 5 .

 5 Fig. 5. Target detection and tracking in the left and right images.(a-b) Raw images. (c-d) Blob detection. (e-f) Cart contour extraction.

Fig. 7 .

 7 Fig. 7. V-REP Scenario A: the robot (green box), equipped with two cameras (blue cones) and lidar scanners (red lines) navigates towards a static target (blue) while avoiding collisions and dealing with occlusions caused by one static and three moving obstacles (all brown).

Fig. 8 .

 8 Fig. 8. V-REP Scenario A: robot control inputs (linear velocities v X and v Y in m/s and angular velocity ω in rad/s) during the simulation.

Fig. 9 .

 9 Fig. 9. V-REP Scenario A: evolution of position (in meters) and orientation (in degrees) errors of the target in the robot frame.

Fig. 10 .

 10 Fig. 10. V-REP Scenario B: the robot performs lateral avoidance of a moving obstacle while maintaining visibility of a moving target.

Fig. 11 .

 11 Fig. 11. V-REP Scenario C: the robot follows a moving target in a cluttered corridor with two obstacles (one moving, one static).

Fig. 12 .

 12 Fig. 12. Real Scenario A: the MPO700 platform navigates safely towards a static blue chariot. Top: images from the left camera, bottom: the MPO700 (grey) avoids two obstacles while dealing with total target occlusion.

Fig. 13 .

 13 Fig. 13. Real Scenario A: MPO700 Control inputs (linear velocities v X and v Y in m/s and angular velocity ω in rad/s).

Fig. 14 .

 14 Fig. 14. Real Scenario A: evolution of position (in meters) and orientation (in degrees) errors of the target in the robot frame.

Fig. 15 .

 15 Fig. 15. Real Scenario B: lateral obstacle avoidance while maintaining the blue chariot in sight.

Fig. 16 .

 16 Fig. 16. Real Scenario C: the MPO700 follows a moving target in a cluttered environment, composed first of a corridor and then of a crowded hall.

1

  Selecting the best tentacle input: Sorted set of tentacles T , visual path (α s , κ s ), previous best tentacle (α p , κ p ), and target pose O p T . output: Risk function H and best tentacle (α b , κ b ) (if H = 0).

1: N ← FindVisibilityGuaranteeingTentacles(T , O p T ); 2: if N > 5 then 3:

Note that if the robot is equipped only with one forward looking camera, this definition follows the pinhole camera frame convention.

For algorithmic reasons, we bound the tentacle curvature to an arbitrarily large value κ M , although omnidirectional robots can follow tentacles of infinite curvature (i.e., pivot in place) whenever v = 0.

For further details on the derivation of C j and D j , refer to[START_REF] Cherubini | Autonomous Visual navigation and Laser-based moving obstacle avoidance[END_REF].

Both ρ α and ρ θ are pre-tuned, and 0 < ρ θ < ρ α .

Also visible online at https://youtu.be/K9yoNlqkqSI

http://www.coppeliarobotics.com

Acknowledgements

This work was funded by the EU Horizon 2020 research and innovation program under grant 731330 (Versatile Project), by the Algerian Ministry of Higher Education and Scientific Research, and by PSA Peugeot Citroën.