Andrea Cherubini
email: acherubi@irisa.fr

François Chaumette
email: chaumette@irisa.fr

Giuseppe Oriolo
email: oriolo@dis.uniroma1.it

Visual servoing for path reaching with nonholonomic robots

We present two visual servoing controllers (pose-based and image-based ) enabling mobile robots with a fixed pinhole camera to reach and follow a continuous path drawn on the ground. The first contribution is the theoretical and experimental comparison between pose-based and image-based techniques for a nonholonomic robot task. Moreover, our controllers are appropriate not only for path following, but also for path reaching, a problem that has been rarely tackled in the past. Thirdly, in contrast with most works, which require the path geometric model, only two path features are necessary in our image-based scheme, and three in the pose-based scheme. For both controllers, a convergence analysis is carried out, and the performance is validated by simulations, and outdoor experiments on a car-like robot.

Introduction

In recent research, automatic vehicle guidance is often done by utilizing vision sensors [START_REF] Bonin-Font | Visual Navigation for Mobile Robots: A Survey[END_REF], which are very useful especially in urban environments, where numerous visual 'points of interest' exist. In a city, cameras can replace or integrate, GPS data [START_REF] Agrawal | Real-time Localization in Outdoor Environments using Stereo Vision and Inexpensive GPS[END_REF], since satellite signals can be masked by tall buildings. Various participants of the DARPA Urban Challenge 1 exploit vision [START_REF] Leonard | A perception-driven autonomous vehicle[END_REF]. Appearance-based navigation, consisting of replaying a topological path defined by a set of images, has been accomplished in [START_REF] Argyros | Robot homing by exploiting panoramic vision[END_REF][START_REF] Royer | Monocular vision for mobile robot localization and autonomous navigation[END_REF]. Apart from navigation, other mobile robot tasks, that exploit camera data include localization [START_REF] Scaramuzza | Appearance-Guided Monocular Omnidirectional Visual Odometry for Outdoor Ground Vehicles[END_REF][START_REF] Guerrero | Localization and Matching using the Planar Trifocal Tensor with Bearing-only Data[END_REF]. One of the prominent methods in vision-based navigation is visual servoing [START_REF] Chaumette | Visual servo control tutorial, part I and II[END_REF], which was originally developed for manipulators with a camera on their end-effector [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF], but has also been applied on nonholonomic mobile robots [START_REF] Masutani | Visual servoing for non-holonomic mobile robots[END_REF]. In some cases, the method relies on the geometry of the environment and on other metrical information. In this case, pose-based visual servoing is used to reduce the error, which is estimated in pose space. Other visual navigation systems use no explicit representation of the environment. In this case, image-based visual servoing techniques can be used to reduce the error, which is measured directly in the image. The image-based approach eliminates the need for image interpretation, and errors due to camera modeling and calibration. Applying such techniques on wheeled robots, involves well known problems related to the nonholonomic constraint: the linearization of these systems is uncontrollable, and smooth feedback laws stabilizing these systems do not exist.

In this paper, we focus on the path reaching task : the controller must zero some suitable error function, indicating the robot pose with respect to a path. The path is a curve drawn on the ground, and the features used for control are at the intersection of the curve with the image borders. As we will show in the non-exhaustive survey below, the path following problem [START_REF]Theory of Robot Control[END_REF][START_REF] Del Rio | A generalization of path following for mobile robots[END_REF] has been tackled in recent research. However, for path following, the initial error is assumed small (i.e., the robot is already on the path), whereas in the path reaching problem, the initial error can be arbitrarily large. The controller in [START_REF] Hsieh | A path following control algorithm for urban driving[END_REF] regulates the lateral displacement and orientation of the vehicle at a lookahead distance. Frezza and others [START_REF] Frezza | A Lagrangian formulation of nonholonomic path following[END_REF] approximate the path by feasible cubic B-splines, and apply feedback linearization on the derivatives of the splines at the wheel axle. In [START_REF] Das | Real-time vision-based control of a nonholonomic mobile robot[END_REF] and [START_REF] Skaff | Inertial navigation and visual line following for a dynamical hexapod robot[END_REF], straight line following is implemented, respectively for a car-like, and hexapod robot. All these controllers are pose-based, and require a complete geometric representation of the path. In other works, image-based techniques have been used to avoid complete knowledge of the path geometry. In [START_REF] Abdelkader | Omnidirectional Visual Servoing From Polar Lines[END_REF], a straight line follower for a mobile robot with para-catadioptric camera is presented. Coulaud and others [START_REF] Coulaud | Stability analysis of a visionbased control design for an autonomous mobile robot[END_REF] design a novel controller and discuss the stability of an equilibrium trajectory: for circular paths, asymptotic stability is guaranteed, whereas for continuously differentiable path curvature, the tracking error is bounded. In [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF], the path is approximated by a fuzzy function, which drives the steering velocity. Two-step techniques enable robot pose regulation, using a ceiling camera, in [START_REF] Wang | Visual servoing based regulation of nonholonomic mobile robots with uncalibrated monocular camera[END_REF].

We present two controllers (pose-based and image-based ), enabling nonholonomic robots with a fixed pinhole camera to reach and follow a continuous path on the ground. The development of the two schemes has been described in [START_REF] Cherubini | A position-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], and [START_REF] Cherubini | An image-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF]. The contribution of the present article with respect to those papers is the comparison of the two schemes. To our knowledge, a comparison between pose-based and image-based visual servoing for nonholonomic robot navigation has never been carried out. To achieve this, various theoretical details, and new experimental results and simulations, not present in [START_REF] Cherubini | A position-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], and [START_REF] Cherubini | An image-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], have been added here. From a theoretical viewpoint, a unique representation is given for all the closed-loop systems, leading to a unique formulation of the convergence analysis. Moreover, in contrast with [START_REF] Cherubini | A position-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], and [START_REF] Cherubini | An image-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], we numerically verify the closed-loop convergence of the two schemes, to get a deeper insight on their applicability and characteristics.

The contributions of our work are listed below.

1. An image-based and a pose-based path reaching approach, are compared. 2. Under certain conditions, convergence is guaranteed even when the initial error is large. For this reason, we claim that our controllers are appropriate not only for path following, but also for path reaching, a problem that has not been tackled in the cited works, which impose constraints on the initial configuration.

3. As opposed to most approaches, which require a geometric model of the path, in our image-based scheme, only two features (the position of a path point, and the path tangent orientation at that point) are necessary.

4.

The system is validated in an outdoor environment, with varying light, in contrast with most cited papers, where tests have been carried out only indoor, where controlled light facilitates image processing.

A convergence analysis is carried out.

The paper is organized as follows. In Sect. 2, the problem and variables are defined, along with the controllers, which are detailed in Sections 3 and 4. In Sect. 5, a convergence analysis is carried out. The experiments are presented in Sect. 6-8.

2 Problem statement

Definitions

We focus on the path reaching task for nonholonomic mobile robots equipped with a fixed pinhole camera. The ground is planar, and the path p to be followed is a curve which is drawn on the ground. For the pose-based control scheme, we assume that the curve is twice differentiable in IR 2 . For the image-based control scheme, the curve can be differentiable only once. A following direction is associated to the path (see Fig. 1). We name r the point on the robot sagittal plane that should track the path. We define the frames (see Fig. 1): world frame F W (W, x w , y w , z w ), robot frame F R (r, x, y, z) and image frame F I (I, X, Y ) (I is the image plane center). The image width and height are respectively 2X M and 2Y M . The robot configuration in F W is: q = [x w z w ψ] where x w and z w represent the Cartesian position of r in F W , and ψ ∈ (-π, +π] is the positive counterclockwise orientation of z with respect to z w . The camera optical axis has a constant tilt offset 0 < ρ < π 2 with respect to the z axis, and the optical center C is positioned in the robot sagittal plane at:

    
x = 0 y = t y z = t z with t y < 0, and t z ∈ IR. We also define the camera frame F C (C, x c , y c , z c ), (see Fig. 1(d)). We denote the control variables by: u = [v ω] . These are the driving and steering velocities (positive counterclockwise) of the robot. Point r is chosen as the center of rotation. Then, the state equation of the robot in the world frame is:

q =    -sin ψ cos ψ 0    v +    0 0 1    ω (1)
Although in this equation we have utilized a unicycle robot model, our approach can be extended to other vehicles (e.g., car-like).

Path reaching task

We hereby define the path reaching task, by recalling the characteristics of path following.

The difference is that in path reaching the initial error can be arbitrarily large. Recalling [START_REF] Del Rio | A generalization of path following for mobile robots[END_REF], the goal of path following is to drive the robot configuration q to the desired configuration (shown in Fig. 1(b)): q * = [x * w z * w ψ * ] , such that:

• point r * = [x * w 0 z * w ]
on the path p defines the desired robot position, • ψ * ∈ (-π, +π] defines the desired robot orientation, i.e., the orientation of the path tangent at r * in F R . Note that ψ * is always defined, since we have assumed that the path curve can be expressed by a differentiable function,

• u * = [v * ω * ]
is the tracking control at the desired state q * . In practice, the goal of driving q to q * is equivalent to zeroing the error: e = q -q * ∈ IR 3 . Furthermore, in path following, this goal must be achieved under two conditions:

1. In opposition to the trajectory tracking problem [START_REF]Theory of Robot Control[END_REF], where the desired configuration is determined by a rigid law, (e.g., associated to time: q * = q * (t)), in path following we can arbitrarily choose the relationship that defines the evolution of q * . Such relationship, called path following constraint, eliminates 1 of the 3 state components, so that the task consists of zeroing a new two-dimensional error e = s -s * ∈ IR 2 , by using appropriate control inputs v and ω. The state dynamics are:

ṡ = J (s)u = J v (s) v + J ω (s) ω (2
) where J v , and J ω are the columns of the Jacobian J (s) that relates u to ṡ. 2. The robot should move at all times, while the control law ensures convergence to the path. This is the motion exigency condition defined in [START_REF] Del Rio | A generalization of path following for mobile robots[END_REF]:

(g) (e) (d) (c) (b) (a) X* = X M Y* = -Y M Θ* = -5π/8 X* = X M Y* = Y M Θ* = -2π/8 X* = 0 Y* = Y M Θ* = 0 (f)
|u| = 0 ∀s ∈ IR 2 (3)
In all the cited works, as well as here, motion is restricted to the forward direction (v > 0) for security reasons (sensing obstacles in front is easier on most robots).

Control design

Since in our work the camera is the only sensor available, we want to ensure path visibility at all times. Hence, we shall use a path following constraint that keeps r * in the camera field of view. The path following constraint that we chose will be detailed later in the paper.

Similarly to [START_REF] Skaff | Inertial navigation and visual line following for a dynamical hexapod robot[END_REF], [START_REF] Coulaud | Stability analysis of a visionbased control design for an autonomous mobile robot[END_REF], and [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF], since the only obstacle detecting sensor on our CyCab is a range scanner that points forward, we express the motion exigency as:

v = v * = const > 0 (4)
and we apply a nonlinear feedback on ω based on the features of a visible path point. Under the assumption that a portion of the path is initially visible, we utilize the features of the first (considering the path direction) visible path point r * , of coordinates

r * = [x 0 z] in F R , which is projected to R * = [X Y
] on the image plane (see Fig. 1(c)). We denote by: P the projection of the path on the image plane, Γ the oriented (according to the path direction) tangent of P at R * and Θ ∈ (-π, π] the angular offset from Γ to the -Y axis (positive counterclockwise). Note that Γ and Θ are always defined, since we have assumed that the path curve is differentiable in F W , and this property is preserved in F I .

In both control schemes that we propose (pose-based and image-based ), the task is defined by the path image features. As shown in Fig. 2(g), it consists of driving R * to the bottom pixel row of the image plane with vertical tangent:

X * = 0, Y * = Y M , Θ * = 0
Depending on the position of R * in the image, we use either of two primitive controllers: a row, and a column controller. In both primitive controllers, the task is to drive the path features to a desired configuration, while R * is constrained to a line in the image: a row of pixels (constant Y ) in the first case, and a column (constant X) in the second case. These conditions determine the path following constraint introduced in Sect. 2.2. By using both controllers, the path can be reached from general initial configurations. This is one of our main contributions. For example, the authors of [START_REF] Coulaud | Stability analysis of a visionbased control design for an autonomous mobile robot[END_REF], which use a similar approach, assume that in the initial configuration, the path already intersects the bottom pixel row, and this implies a bound on the initial position error. Instead, we simply assume that the path is visible.

Let us focus on the initial configuration where R * is on the top row of the image (Fig. 2(a)).

Initially, the row controller must be used to drive R * to a lateral column. The column selected depends on the initial value of Θ: for Θ ≥ 0 (respectively, Θ < 0) the left (right) column is chosen. For the case in Fig. 2, the right column is selected. The equilibrium configuration for the top row controller (TRC), is the top right corner, with desired tangent orientation Θ * = -5π/8 rad (Fig. 2(b)). Then, the right column controller (RCC) will be used to drive R * (Fig. This composition of row and column controllers will be used in both pose-based and imagebased control schemes, although the state variables will be different (i.e., defined in the pose, or in the image space). In the first case, the 3D path error dynamics, will depend on the curvature at the desired point, denoted c * , and related to the tracking control inputs by:

c * = ω * v * (5)
Instead, in the image-based scheme X, Y , and Θ will be used, without taking into account the curvature. In both cases, by imposing the motion exigency (4), system (2) becomes:

ṡ = J v (s)v * + J ω (s)ω (6)
and we apply the following feedback control:

ω = -J ω + (λe + J v v * ) (7)
with λ > 0, and J ω + the Moore-Penrose matrix pseudoinverse of J ω . Control [START_REF] Guerrero | Localization and Matching using the Planar Trifocal Tensor with Bearing-only Data[END_REF] allows ω to be minimal, under condition (3).

In practice, condition (3) is guaranteed by (4), and the term J v v * compensates the feature displacements due to the known driving velocity. In the next sections, we will instantiate this formulation for the two control schemes.

3 Pose-based path follower

Deriving the path 3D features

For the pose-based approach, the path 3D features in F R must be derived from the image features, by considering a pinhole camera model. The four camera parameters used for projecting are: the focal length in pixels f , and ρ, t y and t z (see Fig. 1(d)). For simplicity, let us consider a normalized perspective camera model :

X = x z Y = y z
The mapping between the F I and F C coordinates of a ground point gives:

x c = Xt y sin ρ + Y cos ρ y c = Y t y sin ρ + Y cos ρ z c = t y sin ρ + Y cos ρ
Note that these equations do not present singularities, since by construction the image projection of any ground point has Y > -tan ρ. Then, the robot frame coordinates of the ground point can then be easily derived, by using the homogeneous transformation from F C to F R . For the orientation of the tangent at r * , we obtain:

θ = ATAN2 (sin Θ (sin ρ + Y cos ρ) -X cos Θ cos ρ, cos Θ)
To derive c * , the path points 'near' R * are first projected to F R . Then, the equation of the path osculating circle in r * (thus, the value of c * ) is derived by least square interpolation.

Row controller

The goal of the pose-based row controller is to drive (x, z, θ) to a desired state (x * , z * , θ * ) while constraining R * to a row in the image: Y = const = Y * . This is equivalent to zeroing the error e = [x-x * θ-θ * ] , while constraining r * to the projection of the row on the ground (see Fig. 1(b)), which is equivalent to applying the path following constraint:

z = const = z * = t y sin ρ + Y * cos ρ
This equation can be projected in the path frame F P (see Fig. 1(b)), where the robot coordinates are: r = [x p 0 z p ] . Frame F P lies on the path, with origin at r * , y p parallel to y and z p coincident with the path tangent at r * in the following direction. For the nonholonomic model (1), the errors dynamics in F P can be derived, as we have shown in [START_REF] Cherubini | A position-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], to obtain:

     ẋp = ω * z p -v sin θ żp = -v * -ω * x p + v cos θ θ = ω -ω * (8) Then, plugging (8) into ż = d dt (x p sin θ -z p cos θ) = 0, leads to: v -v * cos θ + ωx = 0 (9)
This expression of the path following constraint relates the desired (v * ) and applied (v) driving robot velocities. Similarly, replacing (8) and ( 9) in the expression of ẋ, yields:

ẋ = (tan θ) v + (z * + x tan θ) ω
On the other hand, replacing ( 5) and ( 9) in the third equation of ( 8) leads to: Hence, the system state equations are:

θ = - c * cos θ v + 1 - c * x cos θ ω x z p r* x p z p z _ β θ _ x _ ROBOTICA θ z 0
ẋ θ = J v v + J ω ω with: J v = tan θ -c * cos θ , J ω = z * + x tan θ 1 -c * x cos θ (10)
under the constraint that |θ| = π 2 , which can be avoided by temporarily using the pose-based column controller while Γ is parallel to X.

By imposing the motion exigency (4), the system state equations (10) become:

ẋ θ = J v v * + J ω ω
This system can be controlled using the feedback law:

ω = -J ω + (λe + J v v * )

Column controller

The goal of the pose-based column controller is to drive (x, z, θ) to a desired state (x * , z * , θ * ) while constraining R * to a column in the image: X = const = X * . This is equivalent to constraining r * to the projection of the column on the ground (see Fig. 3), i.e. to the line:

z = z 0 + x tan β
where z 0 and β ∈ -π 2 , π 2 (shown in Fig. 3) are:

z 0 = t z -t y tan ρ β = ATAN2 (1, X cos ρ)
Let us redefine the variables in a new frame FR (r, x, ȳ, z), obtained by rotating F R by β around -y (Fig. 3). In FR , denoting by θ = θ + β the orientation error between z p and z, the task will consist of zeroing: e = x-x * θ-θ * , under the path following constraint: z = const = z * with z * = z 0 cos β. Hence, as before, but in FR , using [START_REF] Chaumette | Visual servo control tutorial, part I and II[END_REF], simple calculations yield:

v * cos θ -v cos β -ωx = 0 (11)
and using ( 8) and [START_REF]Theory of Robot Control[END_REF] gives:

ẋ = tan θ cos β -sin β v + z * + x tan θ ω
On the other hand, replacing ( 5) and [START_REF]Theory of Robot Control[END_REF] in the third equation of ( 8) leads to:

θ = -c * cos β cos θ v + 1 - c * x cos θ ω
Hence, the system state equations are:

ẋ θ = J v v + J ω ω with: J v = tan θ cos β -sin β -c * cos β cos θ , J ω = z * + x tan θ 1 -c * x cos θ (12)
under the constraint that θ = π 2 , which can be avoided by temporarily using the pose-based row controller while Γ is parallel to Y .

By imposing the motion exigency (4), the system state equations ( 12) become:

ẋ θ = J v v * + J ω ω
This system can be controlled using the feedback law:

ω = -J ω + (λe + J v v * )
4 Image-based path follower

Similarly to the pose-based path follower, the image-based path follower utilizes a row and a column primitive controllers. However, in this case, the controllers are based on the reference path point image features instead of its 3D features. Let us denote by: u c = [v c,x v c,y v c,z ω c,x ω c,y ω c,z ] the robot velocity expressed in F C . The interaction matrix L s , which relates the dynamics of visual features s = (X, Y, Θ) to u c , has been derived, for the normalized perspective camera model, in [START_REF] Espiau | A new approach to visual servoing in robotics[END_REF]:

L s =      -1 zc 0 X zc XY -1 -X 2 Y 0 -1 zc Y zc 1 + Y 2 -XY -X CρC 2 Θ ty CρCΘSΘ ty -CρCΘ(Y SΘ+XCΘ) ty -(Y SΘ + XCΘ) CΘ -(Y SΘ + XCΘ) SΘ -1      (13)
with CΘ, SΘ, Cρ and Sρ respectively denoting: cos Θ, sin Θ, cos ρ, and sin ρ. This expression is an approximation, since the observed 3D point corresponding to R * , is varying. At low velocities, this assumption is valid, and the visual feature dynamics can be expressed by:

ṡ = L s u c + ∂s ∂t (14)
The term ∂s ∂t corresponds to the feature motion. However, assuming low robot velocities, we can neglect this term in the control laws.

The robot velocity in F C can be expressed in function of u = [v ω] as:

u c = C T R u (15)
where C T R is the homogeneous transformation from F R to F C :

C T R =           0 -t z -sin ρ 0 cos ρ 0 0 0 0 -cos ρ 0 -sin ρ          
In the following, we will denote by T v and T ω respectively the first and second columns of C T R , and by L X , L Y and L Θ (top to bottom) the rows of L s . Replacing ( 15) in ( 14) yields:

ṡ = L s C T R u + ∂s ∂t (16)
This equation will be used to design the two image-based primitive controllers below.

Row controller

The task of the row controller is to drive (X, Θ) to a desired state (X * , Θ * ), while constraining R * to a row in the image. This is equivalent to zeroing: e = [X -X * Θ-Θ * ] , under the path following constraint:

Y = const = Y *
Since Ẏ = 0, the system state equations are:

Ẋ Θ = J v v + J ω ω + ∂X ∂t ∂Θ ∂t (17)
where the expressions of J v and J ω , can be derived from [START_REF] Skaff | Inertial navigation and visual line following for a dynamical hexapod robot[END_REF]:

J v = L X L Θ T v J ω = L X L Θ T ω
By imposing the motion exigency (4), the system state equations ( 17) become:

Ẋ Θ = J v v * + J ω ω + ∂X ∂t ∂Θ ∂t
This system can be controlled using the feedback law:

ω = -J ω + (λe + J v v * )
Note that, as aforementioned, this control law does not compensate the terms ∂X ∂t and ∂Θ ∂t .

Column controller

The task of the column controller is to drive (Y , Θ) to a desired state (Y * , Θ * ) while constraining R * to a column in the image. This is equivalent to zeroing: e = [Y -Y * Θ-Θ * ] , under the path following constraint:

X = const = X * control scheme pose-based image-based primitive controller row column row column s 1 x x X Y s 2 θ θ Θ Θ J v1 tan θ tan θ cos β -sin β L X T v L Y T v J v2 -c * / cos θ -c * cos β/ cos θ L Θ T v L Θ T v J ω1 z * + x tan θ z * + x tan θ L X T ω L Y T ω J ω2 1 -c * x/ cos θ 1 -c * x/ cos θ L Θ T ω L Θ T ω e 1 x -x * x -x * X -X * Y -Y * e 2 θ -θ * θ -θ * Θ -Θ * Θ -Θ *
Table 1: Components of: s, J v , J ω , and e for the four controllers Since Ẋ = 0, the system state equations are:

Ẏ Θ = J v v + J ω ω + ∂Y ∂t ∂Θ ∂t (18)
where the expressions of J v and J ω , can be derived from ( 16):

J v = L Y L Θ T v J ω = L Y L Θ T ω
By imposing the motion exigency (4), the system state equations ( 18) become:

Ẏ Θ = J v v * + J ω ω + ∂Y ∂t ∂Θ ∂t
This system can be controlled using the feedback law:

ω = -J ω + (λe + J v v * )
As aforementioned, this control law does not compensate the terms ∂Y ∂t and ∂Θ ∂t .

Convergence of the primitive controllers

The four dynamic systems that we have studied can all be expressed by ( 6), and the corresponding primitive controllers, by [START_REF] Guerrero | Localization and Matching using the Planar Trifocal Tensor with Bearing-only Data[END_REF], with the components of the two-dimensional vectors s, J v , J ω , and e recalled in Table 1. This general formulation will be exploited, in the following, to analyze the convergence of all four closed loop systems. A sufficient condition for convergence of control law [START_REF] Guerrero | Localization and Matching using the Planar Trifocal Tensor with Bearing-only Data[END_REF] is defined in the following theorem.

Theorem 1 Under assumption: e / ∈ ker J ω + (19) a sufficient condition for the convergence of the closed loop system at the desired state s * is:

λ > e J v -J ω J ω + J v e J ω J ω + e v * (20)
Proof: Let us consider the Lyapunov function V (e) = e e 2 , which is positive definite ∀e = 0. The time derivative of V along the closed-loop system, using ( 6) and ( 7) is:

V = e ṡ = e J v v * -J ω J ω + (λe + J v v * )
Since v * > 0, V is negative definite if and only if:

e J v -J ω J ω + J v - λ v * e J ω J ω + e < 0 (21)
Since we have assumed that J ω + e is non null, we also have J ω e = 0. As a consequence, e J ω J ω + e = J ω e 2 /J ω J ω > 0, and condition ( 21) can be rewritten as:

e J v -J ω J ω + J v e J ω J ω + e < λ v * (22)
which leads to condition (20), since J ω J ω + = I.

We have decided not to present the complete expressions of ( 19) and ( 20) for the four controllers, since these were very lengthy. However, they can be derived, by using the vector component values in Table 1. In all four cases, ( 19) is an inequality constraint which is linear in e and depends on both the features and the robot parameters, hence very difficult to verify analytically. Instead, we will verify it numerically in Sect. 6. Equation [START_REF] Wang | Visual servoing based regulation of nonholonomic mobile robots with uncalibrated monocular camera[END_REF] gives a sufficient condition for convergence of the closed loop system at e = 0, valid for all four primitive controllers. Note that this equation cannot be verified off line: since it is related to the state, the second part of (20) evolves during the experiment. However, Theorem 1 can be used to guarantee the convergence of the closed loop system at run time, by updating the gain λ at every iteration, to meet [START_REF] Wang | Visual servoing based regulation of nonholonomic mobile robots with uncalibrated monocular camera[END_REF]. Then, since, under assumption [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF], all primitive controllers converge to their equilibrium point, the complete control scheme, which is defined as a cascade of the primitive controllers, will also converge. An exception occurs if the posebased scheme singularities (θ = π 2 and θ = π 2 ) are reached. As we mentioned in Sect. 3, these are avoided by switching temporarily to the other primitive controller. In this paper, however, we do not analyze the convergence of the complete system in the particular case when the switching is done. The switching was never required in the numerous experiments that we carried out.

Experimental Setup

We hereby report the experiments obtained with the two control schemes. Videos are available at: www.irisa.fr/lagadic/demo/demo-cycab-path-following/cycab-path-following.

Experiments took place outdoor using a CyCab, in varying light conditions. Our CyCab is a 4 wheel steered intelligent vehicle equipped with a 70 • field of view, forward looking, B&W Marlin F-131B camera with image resolution 320 × 240 pixels. It is used in car-like mode (only the front wheels are used for steering). This induces a bound on the curvature, which, for our CyCab, is: |c| < 0.35m -1 . The path features are tracked with the ViSP software [START_REF] Marchand | ViSP for visual servoing: a generic software platform with a wide class of robot control skills[END_REF], which must be initialized at the beginning of the experiment by clicking on five path points indicating the desired path following direction. This is the only human intervention: at run time, the tracker detects the position of R * , and selects the corresponding primitive controller. The tracker proved always effective, and the path was never lost. In the real experiments, we set v * = 0.2 ms -1 . This velocity was limited for security reasons, and because of the low camera frequency (10 Hz). The system was coarsely calibrated, to obtain: f = 240 pixels, ρ = 0.545 rad, t y = -0.55 m, and t z = 1.63 m. Ideally, the vehicle trajectory should be assessed using a sensor independent from the camera. This was possible in the simulations, by using an ideal GPS, but since such sensor is not available on our CyCab, the camera was used to qualitatively assess the performance during the real experiments.

For each of the two control schemes, experiments with 3 different initial conditions have been carried out. The 3 experiments are enumerated below.

1. CyCab is initially positioned on the path. Hence, R * is on the bottom image row (Fig. 4, left). The top row controller (TRC) is used to drive R * to X * = Θ * = 0. 2. CyCab is initially near the path, with R * on the right column (Fig. 4, center). Initially, the right column controller (RCC) drives R * to the bottom right corner. Then, the BRC drives R * along the bottom row to X * = Θ * = 0. 3. CyCab is initially far from the path, with R * on the top row of the image (Fig. 4, right). Initially, the top row controller (TRC) drives R * to the right pixel column. Then, the RCC drives R * to the bottom right corner. Finally, the BRC drives R * along the bottom row to X * = Θ * = 0.

Moreover, we have numerically verified condition [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF], as the state evolves. For the pose-based approach, with the CyCab parameters and state ranges, condition e / ∈ ker J ω + is always met, except, of course, at e = 0. This is true both for straight (c * = 0), and curved (c * = 0) path portions. This implies that for any initial condition, and appropriate gain tuning to guarantee [START_REF] Wang | Visual servoing based regulation of nonholonomic mobile robots with uncalibrated monocular camera[END_REF], the pose-based primitive controllers will converge to their equilibrium point, leading to convergence of the cascaded scheme. Instead, for the imagebased controller, [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF] is not always met. In Fig. 5, we have represented in gray the state loci where [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF] is not verified in the image-based scheme, and with a black cross the desired states for each primitive controller. In practice, as long as the state variables do not enter the gray loci, and the gain guarantees [START_REF] Wang | Visual servoing based regulation of nonholonomic mobile robots with uncalibrated monocular camera[END_REF], the image-based primitive controllers will converge to their equilibrium point, leading to convergence of the cascaded scheme. If the state variables enter the gray loci, convergence cannot be guaranteed anymore (see Theorem 1). However, for both schemes, the states that do not meet [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF] can be numerically determined off line, according to the robot parameters and state ranges, and this information can be used to select the appropriate scheme to avoid that the states enter the loci. In the following sections, the loci in Fig. 5 will be used to verify [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF] during the experiments.

Simulations

For simulations, we have adopted Webots 2 , an environment for modeling and controlling mobile robots, where we have designed a robot with the same characteristics as CyCab. A circular path of radius 12.5 m (i.e., c * = const = 0.08 m -1 ) has been drawn, and the two control schemes (pose-based and image-based) have been simulated starting with the path intersecting the right column (see Fig. 4, center). The RRC and BRC are used. All gains are tuned off line to avoid abrupt changes in the steering velocity at the changing point.

With both control schemes, the robot is able to reach and follow the path, and condition ( 19) is always met. For the image-based case, the robot positions and processed images during the simulation are shown in Fig. 6. The relevant variables (state errors and applied curvature c = ω/v * ) for the two schemes, are plotted in Fig. 7. For the image-based error, instead of e 1 = X -X * and e 1 = Y -Y * , we have plotted the scaled values e 1,n = X-X * 2X M for the row controller, and e 1,n = Y -Y * 2Y M for the column controller. Note that the pose-based controller initially saturates the curvature to its maximum 0.35 m -1 , to enable path reaching. Since in Webots we can add a GPS to the robot, the controllers have been assessed by measuring the distance from the path. For the pose-based controller, the average distance is 4.1 cm, whereas for the image-based controller, it is 4.3 cm. Both results are excellent, since they are below 0.5% of the path radius. In both cases, at the end of the first phase (after the RRC has been applied) the orientation error e 2 has not reached 0, because the BRC is activated only by e 1 . Nevertheless, when the BRC is applied, the tracking errors converge, and the

Experiments

After the simulations, the two control schemes have been tested outdoor on the real CyCab. The path is composed of two straight lines of length 6 m joined by a 60 • arc of circle of radius 10 m (i.e., c * = ±0.1 m -1 , with the sign of c * depending on the path direction to be followed by the robot). The primitive controller gains are the same as in the simulations. To verify the robustness of the controllers, the experiments have been repeated with a random calibration error of either +10% or -10% on each of the four camera parameters.

Pose-based experiments

In the first pose-based experiment (see Fig. 8), R * is on the bottom pixel row of the image plane. The row controller is used. The evolution of the relevant variables during the experiment is shown in Fig. 9. The robot successfully follows the path, and the tracking errors (solid and dotted black curves) are low throughout the experiment. At the end of the experiment, both errors are below 0.1. Both errors increase when the robot reaches the discontinuity in the path curvature (frame 270). Correspondingly, ω and, therefore, c = ω/v * , increases to compensate the error and enable CyCab to follow the curve.

In the second experiment, CyCab is initially near the path, but with R * on the right column. The relevant variables (state errors, as well as path and applied curvature) are plotted in Fig. 10. The robot successfully reaches and follows the path. Again, when the robot reaches the path curve (frame 285), the error increases. However, ω compensates the error, and enables the robot to follow the curve, and to zero both state errors.

In the third experiment (Fig. 11), CyCab is initially far from the path, with R * on the top row. Once again, the robot is able to successfully follow the path. The curvature is initially saturated to 0.35 m -1 to enable the robot to reach the path. At the end of the experiment, both errors are below 0.1.

The three pose-based experiments have been repeated with camera calibration error. The variables in the coarse calibration experiments are also shown in Fig. 9, 10, and 11 (gray curves), for comparison with the calibrated case (black). Although CyCab follows the path in all three cases, the convergence is slower than in the calibrated experiments. In particular, in the second experiment, the performance is slightly worsened (see Fig. 10, center and bottom): the RCC convergence is slower than in the calibrated case, the final error is higher (0.35 m instead of 0.05 m), and the applied curvature oscillates more.

Image-based experiments

In the first image-based experiment, R * is on the bottom row. The relevant variables are shown in Fig. 12. CyCab follows the path, and the tracking errors are low throughout the experiment (at the end, both are below 0.03). As in the pose-based experiment, both errors increase when the robot reaches the discontinuity in the path curvature, and correspondingly, c increases in order to compensate for the error. Using Fig. 5, we verify that throughout the experiment, the state variables verify condition [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF].

In the second experiment, CyCab is initially near the path, but with R * on the right column.

The variables are plotted in Fig. 13. CyCab successfully reaches and follows the path, and at the end, both state errors are zeroed. The initial trend of c is completely different from the pose-based experiment (Fig. 10, bottom). Condition ( 19) is always verified.

In the third experiment, CyCab is initially far from the path, with R * on the top pixel row. The experiment fails while using the BRC, as the path exits the field of view. Tests with other values of λ are also unsuccessful. The reason is the failure of ( 19) during control with the BRC, at the iterations highlighted in Fig. 14. To clarify this, we have plotted in black, in Fig. 5, the state evolution during this experiment, and the corresponding images.

As the curves show, with TRC and RCC, the state is consistent with [START_REF] Rezoug | Visual based lane following for non-holonomic mobile robot[END_REF]. Instead, during control with the BRC, the state error enters the kernel of J ω + , and (6) cannot be controlled using [START_REF] Guerrero | Localization and Matching using the Planar Trifocal Tensor with Bearing-only Data[END_REF].

As we mentioned, a flaw of the image-based scheme is that it does not consider the curvature c * , nor the feature motion ∂s ∂t (see ( 14)). This is relevant here, and causes the failure. In fact (see the third snapshot on the left of Fig. 14), the BRC is activated with a large error on e 2 , in a critically curve path portion. In contrast with the pose-based scheme, the error cannot be regulated since the curvature is not in the feedback law. However, the initial state of the BRC controller cannot be changed, since it is determined by the fact, that the gain λ used with TRC and RRC must be tuned to saturate the c to its maximum 0. with the calibrated experiments (black). The robot successfully follows the path in both experiments, and again, ( 19) is always verified. The convergence is slightly slower than in the calibrated experiments. However, in particular for the second experiment, the image-based approach outperforms the pose-based approach when the camera is coarsely calibrated.

Comparison between the two control schemes

To compare the two control schemes, in Table 2, we show the error norms averaged over each of the real experiments. When the initial error is small (experiments 1 and 2), the imagebased approach is better (smaller |e|, as seen in the table, and smoother ω, as seen in the curves), and more robust to calibration errors. Instead, experiment 3 fails with the imagebased approach, and succeeds with the pose-based approach. As explained above, this is due to the failure of ( 19) for large error. In a series of simulations with path intersecting the top row with various values of the initial error, we have verified that when the initial |e| is larger than 0.22, the robot is not able to follow the path with the image-based control scheme. In all cases, the path is lost during BRC control, like in experiment 3. The pose-based simulations, instead, are all successful. In summary, image-based control is less effective when the initial error is large, since it does not rely on the curvature, which, acting as a second derivative, fosters the prediction of the error dynamics. On the other hand, since it uses the curvature, which is typically more biased than path position and tangent orientation measurements, the pose-based controller is less smooth and less robust to calibration errors.

Conclusions

In this paper, we presented two visual servoing control schemes (pose-based and imagebased) enabling nonholonomic mobile robots to reach and follow a path on the ground. The controllers require only a small set of path features: the position of a path point, and the path tangent orientation at that point. For the pose-based controller, the path curvature at the point is also necessary. Although the two schemes had been introduced in [START_REF] Cherubini | A position-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], and [START_REF] Cherubini | An image-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], this article presents a unique representation for all the closed-loop systems used, which eases comparison and leads to a unique formulation of the convergence analysis. New experimental results, not present in [START_REF] Cherubini | A position-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], and [START_REF] Cherubini | An image-based visual servoing scheme for following paths with nonholonomic mobile robots[END_REF], have also been added here, along with the numerical analysis of the closed-loop convergence, which gives a deeper insight on the controllers characteristics. The main results of our comparison are that the pose-based controller can be used in general initial conditions, and is appropriate for path reaching, whereas the image-based controller, is more precise and robust, and should be preferred for path following (i.e., when the error is small). In future work, we aim at integrating the two schemes, to design a general framework which is able to exploit the advantages of both. We also plan to vary the driving velocity v, in order to tackle more sophisticated scenarios.

Figure 1 :

 1 Figure 1: Relevant variables, and reference frames F W , F R , F C , F P and F I . The task for the robot (represented in orange), equipped with a fixed pinhole camera (blue) is to follow the red path p. The camera field of view and its projection on the ground are represented in cyan. (a) Perspective view: (b) Top view: robot configuration, desired configuration, applied (v, ω) and tracking (v * , ω * ) control variables. (c) Image plane view. (d) Side view.

Figure 2 :

 2 Figure 2: Outline of the control scheme, with 7 possible configurations of P in the image, including the 3 equilibrium configurations (yellow).

  2(c) and 2(d)) to the bottom right corner: Θ * = -2π/8 rad (Fig.2(e)). Finally, the bottom row controller (BRC) will drive R * along the bottom row (Fig.2(f)) to the center, with vertical tangent Θ * = 0 (Fig.2(g)). If the left column is initially selected, the equilibrium configurations are symmetric to the ones in the figure, and they are considered reached by thresholding the point position error, i.e., the coordinates of R * .

Figure 3 :

 3 Figure 3: Relevant variables utilized for the pose-based column controller: frames F R , F P , and FR , robot current and desired configuration, z 0 and β.

Figure 4 :Figure 5 :

 45 Figure 4: Initial conditions used in the outdoor experiments. The point R * and tangent Γ derived by image processing are indicated respectively by the black circle and arrow.

2

  www.cyberbotics.com

Figure 6 :

 6 Figure 6: Image-based simulation with robot positions and corresponding images.

Figure 7 :

 7 Figure 7: Evolution of relevant variables during the pose-based (top) and image-base (bottom) simulations. Pose-based errors e 1 (solid, in m) and e 2 (dotted, in rad), and image-based errors e 1,n (solid, dimensionless) and e 2 (dotted, in rad) for column (left) and row (center) controllers. Applied (solid) and desired (dotted) curvatures in m -1 (right). mean of the curvature at steady state is as expected: c * = 0.08 m -1 .

Figure 8 :

 8 Figure 8: First pose-based experiment (CyCab is initially positioned on the path with small error), with robot positions and corresponding processed images at various iterations.

Figure 9 :Figure 10 :

 910 Figure 9: Evolution of relevant variables during the first pose-based experiment, with correct (black) and coarse (gray) camera calibration. Left: errors e 1 (solid, in m) and e 2 (dotted, in rad). Right: applied (thick) and desired (thin) curvatures in m -1 .

Figure 11 :

 11 Figure 11: Snapshots and variables for the third pose-based experiment, with correct (black) and coarse (gray) calibration. Left: errors e 1 (solid, in m) and e 2 (dotted, in rad), with TRC, RCC, and BRC (left to right). Right: applied (thick) and desired (thin) curvatures (m -1 ).

Figure 12 :

 12 Figure 12: Evolution of relevant variables during the first image-based experiment, with correct (black) and coarse (gray) calibration. Left: errors e 1,n (solid, dimensionless), and e 2 (dotted, in rad). Right: applied (thick) and desired (thin) curvatures in m -1 .

1 Figure 13 :Figure 14 :

 11314 Figure 13: Variables for the second image-based experiment, with correct (black) and coarse (gray) calibration. Errors e 1,n (solid, dimensionless), and e 2 (dotted, in rad), with RCC (left) and BRC (center). Applied (thick) and desired (thin) curvatures (right, in m -1 .)

Table 2 :

 2 Error norm |e| averaged over the experiments.

	control scheme	pose-based	image-based
	calibration correct coarse correct coarse
	experiment 1	0.22	0.30	0.09	0.09
	experiment 2	0.21	0.32	0.16	0.17
	experiment 3	0.38	0.42	failed skipped

Proof for review only


Andrea Cherubini

Franc ¸ois Chaumette

Visual Navigation With Obstacle Avoidance

Keywords: Visual Navigation, Visual Servoing, Collision Avoidance

We present and validate a framework for visual navigation with obstacle avoidance. The approach was originally designed in [1], but major improvements and real outdoor experiments are added here. Visual navigation consists of following a path, represented as an ordered set of key images, that have been acquired in a preliminary teaching phase. While following such path, the robot is able to avoid new obstacles which were not present during teaching, and which are sensed by a range scanner. We guarantee that collision avoidance and navigation are achieved simultaneously by actuating the camera pan angle, in the presence of obstacles, to maintain scene visibility as the robot circumnavigates the obstacle. The circumnavigation verse and the collision risk are estimated using a potential vector field derived from an occupancy grid. The framework can also deal with unavoidable obstacles, which make the robot decelerate and eventually stop.

I. INTRODUCTION

A great amount of robotics research focuses on vehicle guidance, with the goal of automatically reproducing the tasks usually performed by humans [START_REF] Buehler | Special Issue on the 2007 DARPA Urban Challenge, Part I-III[END_REF], [START_REF] Broggi | Sensing requirements for a 13000 km intercontinental autonomous drive[END_REF]. Among others, an important task is obstacle avoidance, i.e., computing a control such that the trajectory generated is collision-free, and drives the robot to the goal [START_REF] Minguez | Motion planning and obstacle avoidance[END_REF]. A common obstacle avoidance technique is the potential field method [START_REF] Khatib | Real-time obstacle avoidance for manipulators and mobile robots[END_REF], which is often associated to a global path planner. Instead of using a global model, we propose a framework for obstacle avoidance with simultaneous execution of a visual servoing task [START_REF] Chaumette | Visual servo control tutorial, part I and II[END_REF]. Visual servoing is a well known method that uses vision directly in the control loop, and that has been applied on mobile robots in [7 -9]. In [START_REF] Mariottini | Image-Based Visual Servoing for Nonholonomic Mobile Robots Using Epipolar Geometry[END_REF] and [START_REF] Becerra | A Sliding-Mode-Control Law for Mobile Robots Based on Epipolar Visual Servoing From Three Views[END_REF], the epipoles are exploited to drive a nonholonomic robot to a desired configuration. Trajectory tracking is tackled in [START_REF] Allibert | Real-time visual predictive controller for image-based trajectory tracking of a mobile robot[END_REF] by merging differential flatness and predictive control.

We focus on appearance-based navigation, which in contrast with model-based navigation, operates in the sensor space, without any geometrical knowledge of the environment. In the framework that we developed in the past [START_REF] Šegvić | A mapping and localization framework for scalable appearance-based navigation[END_REF][START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF], the path is represented by a database of ordered key images 1 . Navigation is divided into subtasks, each consisting of driving the robot towards the next key image in the database. To our knowledge, this scheme, which is popular in robotics [12 -15], has never been extended to take into account obstacles. Navigation schemes with obstacle avoidance have been presented in [16 -19]. However, all these approaches require a 3D model of the environment (e.g., of walls and doors). An exception is [START_REF] Folio | A redundancy-based scheme to perform safe vision-based tasks amidst obstacles[END_REF], which exploits redundancy to avoid obstacles during a visual positioning task. Here, we focus on this problem: a wheeled robot, equipped with an actuated pinhole camera and with A. Cherubini and F. Chaumette are with INRIA Rennes -Bretagne Atlantique and IRISA, Campus de Beaulieu, Rennes, France. E-mail: {Andrea.Cherubini, Francois.Chaumette}@inria.fr 1 See: www.irisa.fr/lagadic/demo/demo-cycab-vis-navigation/vis-navigation. a range scanner, must follow a visual path represented by key images, without colliding with the ground obstacles.

To maintain scene visibility while avoiding the obstacles, we have decided to use an actuated camera. Although an alternative design would consist of using an omnidirectional camera, we have prefered a more conventional and widespread platform setup. The camera detects the features for navigating, while the scanner senses the obstacles in front of the robot. As in [START_REF] Folio | A redundancy-based scheme to perform safe vision-based tasks amidst obstacles[END_REF], we guarantee that obstacle avoidance has no effect on the visual task. In contrast with that work, however, our controller is compact, while in [START_REF] Folio | A redundancy-based scheme to perform safe vision-based tasks amidst obstacles[END_REF] three controllers are needed and the transitions between them are quite complex. Let us summarize the main contributions of our work. For the first time, obstacle avoidance and visual navigation are merged at the control level (without the need for planning), and validated in real outdoor experiments. Besides, our approach is merely appearance-based, and no model of the environment is necessary. Moreover, the desired states are globally asymptotically stable for our closed-loop system in the safe context and when the obstacles are unavoidable, guaranteeing that the robot never collides. Our framework is inspired from the work in [START_REF] Cherubini | A redundancy-based approach for obstacle avoidance in mobile robot navigation[END_REF]. However, many modifications have been applied. Firstly, the current framework does not require redundancy, as in [START_REF] Cherubini | A redundancy-based approach for obstacle avoidance in mobile robot navigation[END_REF]. Besides, the linear velocity, which was constant in [START_REF] Cherubini | A redundancy-based approach for obstacle avoidance in mobile robot navigation[END_REF], is varied here, to improve visual tracking and eventually stop the robot in the presence of unavoidable obstacles. Thirdly, the design of the activation function has been modified, to reduce undesired accelerations, which jeopardize convergence. Finally, the present article reports real experiments, carried out on our outdoor mobile robot.

II. MODELING AND CONTROL

A. General Definitions

The reader is referred to Fig. 1 for the definitions below. We define the robot frame F R (R, X , Y , Z ) (R is the robot center of rotation), image frame F I (O, x, y) (O is the image center), and camera frame F C (C, X, Y, Z) (C is the optical center). The robot control inputs are: These are, respectively, the linear and angular velocities of the vehicle, and the camera pan angular velocity. We use the normalized perspective camera model:

u = (v, ω, φ)
x = X Z , y = Y Z
We assume that the camera pan angle is bounded: |ϕ| ≤ π 2 , that C belongs to the pan axis, and that the path can be tracked with continuous v (t) > 0. This ensures safety, since only obstacles in front of the robot can be detected by our scanner. The distance between R and C is denoted by δ ≥ 0.

The path that the robot must follow is represented as a database of ordered key images, such that each neighboring pair contains some common static visual features (points). First, the vehicle is manually driven along a taught path, with the camera pointing forward (ϕ = 0), and all the images are saved. Afterwards, a subset (database) of N key images I 1 , . . . , I N representing the path (Fig. 2) is selected. Then, during autonomous navigation, the current image, noted I, is compared with the next key image in the database, I d ∈ {I 1 , . . . , I N }, and a pose estimation between I and the key images is used to check when the robot passes the pose where I d was acquired. For key image selection, as well as visual point detection and tracking, we use the algorithm presented in [START_REF] Royer | Monocular vision for mobile robot localization and autonomous navigation[END_REF], which has proved successful in the presence of environmental changes (e.g., luminosity variation) and nonstatic features. The output of this algorithm, which is used by our navigation controller, is the set of points which are visible in both I and I d . Then, navigation consists of driving the robot forward, while I is driven towards the next key image in the database. The task of maximizing similarity between I and I d can be achieved with only one feature: the abscissa x of the centroid of the points matched on I and I d [START_REF] Šegvić | A mapping and localization framework for scalable appearance-based navigation[END_REF]. When I d has been passed, a topological transition is made: the next image in the set becomes the desired one, and so on, until I N is reached.

Along with the visual path following problem, which we have tackled in [START_REF] Šegvić | A mapping and localization framework for scalable appearance-based navigation[END_REF] and [START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF], here we consider obstacles which are on the path, but not in the database, and sensed by the range scanner. To model these obstacles, we use an occupancy grid, yellow in Fig. 1(c). In Sect. II-C, we will explain how it is utilized in our controller.

In summary, the desired specifications, in this work, are: 1) orienting the camera to drive the feature centroid abscissa x to its value at the next key image x d , 2) making the vehicle progress forward along the path, 3) avoiding collision with the obstacles, while remaining near the 3D taught path. These specifications will be detailed just below.

B. Task Specifications and Control Design

Let us recall the Jacobian paradigm which relates the control inputs and the desired task. We name s ∈ IR m the task vector, and u ∈ IR m the inputs. These are related by: ṡ = Ju where J is the task jacobian of size m × m. In this work, m = 3. The required task evolution can be written:

ṡ * = ṡd -Λ s -s d
with s d and ṡd indicating the desired values of the task, and of its first derivative, and Λ = diag (λ 1 . . . λ m ) a positive definite diagonal gain matrix.

Since we assume that the visual features are static, the first specification on camera orientation can be expressed by:

ẋ * = -λ x x -x d (1)
This guarantees that the abscissa of the centroid of the points converges exponentially to its value at the next key image x d , with null velocity there ( ẋd = 0). The dynamics of this task can be related to the robot control inputs by:

ẋ = J x u = [ j v j ω j φ ] u (2)
The components of J x will be given in Sect. II-C. The two other specifications (vehicle progression with collision avoidance) are related to the danger represented by the obstacles. To assess such danger, we discern two contexts (safe and unsafe), and we design the following obstacle activation function to smoothen the transition in between:

H : C → [0, 1]
The value of H indicates the danger in a given context C, and it will be defined in Sect. II-C.

• In the safe context (H = 0), since no obstacles are present, it is not necessary to deform the taught path. We use ω for the visual task [START_REF] Cherubini | A redundancy-based approach for obstacle avoidance in mobile robot navigation[END_REF]. Thus, as it was during teaching, the camera pan should point forward (i.e, to ϕ = 0, φ = 0). Moreover, the linear velocity v must be reduced in the presence of sharp turns, to ease the visual tracking of quickly moving features; we specify this using a function v s , which will be given in Sect. II-C. In summary, the specifications in the safe context are:

   ẋ = -λ x x -x d v = v s φ = -λ ϕ ϕ (3)
Thus, the current and desired task dynamics are:

ṡs = ẋ v φ ṡ * s =   -λ x x -x d v s -λ ϕ ϕ  
With (2) we can derive the Jacobian relating ṡs and u:

ṡs = J s u = j v j ω j φ 1 0 0 0 0 1 u (4)
Matrix J s is invertible if j ω = 0, a condition that we can easily guarantee, as will be shown in Sect. II-C. • In the unsafe context (H > 0), obstacles are present.

If they are too near to be circumnavigated (H = 1), the vehicle should stop (v = 0). Instead, if they can be circumnavigated (0 < H < 1), the robot should avoid collision by orienting its heading to a desired value α (i.e., by setting ω = λ α α). The value of α is related to the obstacles position (see Fig. 1(c)), and will be defined in Sect. II-C. Since this heading variation drives the robot away from the 3D taught path, the camera pan angle must be actuated to maintain visibility of the features, i.e., to guarantee (1). The linear velocity must be reduced for safety reasons; we specify this using a function v u , which will be given in Sect. II-C. In summary, the specifications in the unsafe context are:

   ẋ = -λ x x -x d v = v u ω = λ α α (5)
Thus, the current and desired task dynamics are:

ṡu = ẋ v ω ṡ * u =   -λ x x -x d v u λ α α   (6)
With ( 2) we can derive the Jacobian relating ṡu and u:

ṡu = J u u = j v j ω j φ 1 0 0 0 1 0 u (7)
Matrix J u is invertible if j ϕ = 0, a condition that we will prove to be always true in Sect. II-C. To fulfill the desired tasks in the safe and unsafe contexts, we propose the following control law:

u = HJ -1 u ṡ * u + (1 -H) J -1 s ṡ * s (8)
Replacing ( 8) in ( 4) and [START_REF] Mariottini | Image-Based Visual Servoing for Nonholonomic Mobile Robots Using Epipolar Geometry[END_REF] it is trivial to see that this controller leads to convergence to the desired tasks:

ṡs = ṡ * s if H = 0 ṡu = ṡ * u if H = 1
and that, in these cases, the desired states are globally asymptotically stable for the closed loop system. Note also that, since the first rows of J -1 u and J -1 s are both equal to [0 1 0], the linear velocity derived using ( 8) is:

v = Hv u + (1 -H) v s (9)
This equation will be used in Sect. II-C to derive v u and v s .

C. System Characteristics

In this section, we will define the variables introduced above. We will show how to derive the centroid abscissa Jacobian J x , the linear velocity in the safe and unsafe context (v s and v u ), and the obstacle characteristics (heading for avoidance α, and activation function H).

1) Jacobian of the Centroid Abscissa: We will hereby derive the components of J x introduced in (2). Let us define: v = (v c , ω c ) the camera velocity, expressed in F C . Since we have assumed that the features are static, the dynamics of x can be related to v by:

ẋ = L x v
where L x is the interaction matrix of x [START_REF] Chaumette | Visual servo control tutorial, part I and II[END_REF]. In the case of a point of depth Z, it is given by [START_REF] Chaumette | Visual servo control tutorial, part I and II[END_REF]:

L x = -1 Z 0 x Z xy -1 -x 2 y ( 10
)
We consider the centroid as a point, which is known to be a sufficiently accurate approximation. We thus use [START_REF] Šegvić | A mapping and localization framework for scalable appearance-based navigation[END_REF] instead of the exact but more complex form given in [START_REF] Tahri | Point-based and region-based image moments for visual servoing of planar objects[END_REF]. To avoid depth estimation, which can be unreliable and timeconsuming, we set the depth to a fixed value. Although this approximation requires Z to be tuned by the user, depending on the workspace characteristics, it has proved successful for most visual servoing applications [START_REF] Chaumette | Visual servo control tutorial, part I and II[END_REF], including nonholonomic navigation [START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF]. In that work, we have also shown that the uniform depth approximation proved successful even in the presence of depth tuning errors.

For our robot model, the camera velocity v can be expressed in function of u by using the homogeneous transformation:

v = C T R u
with:

C T R =        sin ϕ -δ cos ϕ 0 0 0 0 cos ϕ δ sin ϕ 0 0 0 0 0 -1 -1 0 0 0       
Then, multiplying L x by C T R , it is trivial to obtain:

j v = -sin ϕ+x cos ϕ Z j ω = δ(cos ϕ+x sin ϕ) Z + 1 + x 2 j φ = 1 + x 2 . (11)
From [START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF] it is clear that j φ ≥ 1 ∀x ∈ IR; hence, J u is never singular (see [START_REF] Mariottini | Image-Based Visual Servoing for Nonholonomic Mobile Robots Using Epipolar Geometry[END_REF]). On the other hand, it is possible to ensure that j ω = 0, so that J s is also invertible (see ( 4)), by setting Z > δ 2 . Indeed, condition j ω = 0 is equivalent to:

δ (cos ϕ + x sin ϕ) Z + 1 + x 2 = 0 (12)
Since |ϕ| ≤ π 2 : cos ϕ + x sin ϕ ≥ -x, ∀x ∈ IR. Hence, a sufficient condition for (12) is:

x 2 - δ Z x + 1 > 0 which occurs ∀x ∈ IR when δ Z < 2.
In practice, δ Z < 2 is always guaranteed, since on most robots δ < 1 m, which is much less than the scene depth in outdoor environments. Moreover, Z is tuned by the user, and in [START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF], we have shown that overestimating it with respect to its real value is more effective than underestimating it, for navigation performance.

2) Linear Velocity In The Safe and Unsafe Context: Let us now define the velocities v s and v u , used in [START_REF] Allibert | Real-time visual predictive controller for image-based trajectory tracking of a mobile robot[END_REF].

As we mentioned, when the features motion in the image is fast, the visual tracker is less effective, and the linear velocity should be reduced. To assess the features motion, we compare their position in the current image I and in the next I d and second next I sd key images 2 . Since the robot moves on a plane, we relate v s to the image abscissa error between the centroids of the points matched in I and I d :

e d = x -x d
and to the abscissa error between the centroids of the points matched in I and I sd :

e sd = x -x sd
Then, defining the weighted average between these errors as: e = 2e d + e sd 3 we design the linear velocity in the safe context as:

v s = V 2 1 + tanh π - |e| γ (13)
Function ( 13) has an upper bound V > 0, and smoothly decreases to 0, as |e| grows. Its inflection point is determined by empirically tuned parameter γ > 0.

Instead, in the presence of obstacles, the robot should reduce its linear velocity to v u , and stop for unitary H. Imposing v = 0 when H = 1 in (9) obviously yields:

v u = 0
as our design choice for the velocity in the unsafe context.

3) Modeling The Obstacles: For obstacle modeling, we use an occupancy grid linked to F R , with cell sides parallel to the X and Z axes (see Fig. 1(c)). Its forward and lateral extensions are smaller than the scanner radial range, to ignore obstacles that are too far to jeopardize the robot. Given an arbitrary integer K, the grid is built from the latest 2K + 1 scans. For each cell centered at c = (X , Z ), we define the 2K + 1 occupancies r at the j-th oldest iteration as:

r j (c) = {0, 1}, j = 0, . . . , 2K + 1
We set r j = 1 if an obstacle has been sensed in c at the j-th iteration prior to the current one, and 0 otherwise. Then, we associate to each cell a coefficient µ (c), obtained by linear combination of the occupancies, weighted with a normalized Gaussian filter that smoothens the cell effect over time:

µ (c) = 2K+1 j=0 e -(j-K) 2 /K √ Kπ r j (c)
The filter maximum weight is set at the K-th latest scan, to avoid control input overshoot at a new obstacle detection (an issue that existed in [START_REF] Cherubini | A redundancy-based approach for obstacle avoidance in mobile robot navigation[END_REF]). If the robot velocity is negligible with respect to the scanner acquisition frequency, and K is reasonably small, the effect of motion on the occupancies can be ignored, and this model is consistent with the current robot position. We will show that the above assumptions are appropriate in our experimental setup. Obstacle avoidance is derived by using vortex potential fields [START_REF] De Luca | Local Incremental Planning for Nonholonomic Mobile Robots[END_REF]. For each cell c, we define the potential:

U c = µ (c) c
where µ (c) has been defined above, and c is the distance from R to the cell 3 . In practice, for two cells with equal µ (c) = 0, the nearest one will yield the highest potential. We define the vortex field for each cell as the rotor of U c :

f c = f c,X f c,Z =    ± ∂U c ∂Z ∓ ∂U c ∂X    = µ (c)      ∓ Z c 3 ± X c 3     
3 Designing the grid without the cell at R = (0, 0) (where obstacles are not detectable by the range scanner), guarantees that Uc is non-singular.

The signs of f c,X and f c,Z depend on the cell abscissa: X positive (strictly negative) will induce a clockwise (counterclockwise) vortex, so that the field always points forward. The fields f c,i generated by all the n cells c i are then superimposed to obtain the total field:

f = n i=1 f c,i
The orientation α ∈ -π 2 , π 2 of this field is (see Fig. 1):

α = 0 if f = 0 -ATAN2 (f X , f Z ) otherwise
As in [START_REF] De Luca | Local Incremental Planning for Nonholonomic Mobile Robots[END_REF], this is the desired heading for circumnavigation in unsafe situations (see Sect. II-B). However, α alone is not indicative of the danger. Thus, we also use the field norm:

|f | = f 2 X + f 2 Z
which is a good metric for evaluating the obstacle distance, and reducing v accordingly, when the obstacles are near. Hence, to assess the context danger, we consider both α and |f |, and using two empirically tuned thresholds ρ and such that 0 < ρ < , we design the activation function as:

H =      κ |α| if |f | <= ρ 1 if |f | >= 1+κ|α| 2 + 1-κ|α| 2 tanh 1 -|f | + 1 ρ-|f | otherwise
Note that H = 0 if no obstacle is detected (since both |f | and α are null in that case), and it is bounded by 1. For small |f |, H is determined only by α: the obstacles are 'far enough' to be circumnavigated, and parameter κ ∈ 0, 2 π weighs the danger provoked by the field orientation 4 . On the other hand, for large |f | ≥ , the obstacles are 'too near': H = 1 and the robot must stop. A hyperbolic function guarantees that H is C ∞ for all (X , Z ) ∈ IR * 2 . Parameters ρ and are tuned to determine the three intervals. For example, small values of make the approach more conservative, since the robot will stop even in the presence of distant obstacles.

D. Control Analysis

In this section, we will instantiate and comment our controller for visual navigation with obstacle avoidance. Plugging all the variables defined above in [START_REF] Becerra | A Sliding-Mode-Control Law for Mobile Robots Based on Epipolar Visual Servoing From Three Views[END_REF] we obtain:

       v = (1-H) v s ω = (1-H) λx(x d -x)-jvvs+λϕj φ ϕ jω + Hλ α α φ = H λx(x d -x)-λαjωα j φ -(1-H) λ ϕ ϕ (14)
This control law has the following interesting properties. 1) In the safe context (H = 0), (14) becomes:

     v = v s ω = λx(x d -x)-jvvs+λϕj φϕ jω φ = -λ ϕ ϕ (15)
As in [START_REF] Šegvić | A mapping and localization framework for scalable appearance-based navigation[END_REF] and [START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF], where obstacles were not considered, the visual task ( 1) is realized by ω, which also compensates the centroid displacements due to v and to φ through the image jacobian components [START_REF] Cherubini | Comparing appearance-based controllers for nonholonomic navigation from a visual memory[END_REF]. The two other specifications in (3) are achieved by v and φ: v is regulated to improve tracking according to [START_REF] Zhang | Robust Appearance Based Visual Route Following for Navigation in Large-scale Outdoor Environments[END_REF], while the camera is driven forward, to ϕ = 0. 2) In the unsafe context when H ≈ 1, a good approximation of ( 14) is:

     v = (1-H) v s ω = λ α α φ = λx(x d -x)-λαjωα j φ (16)
In this case, the visual task ( 1) is executed by φ, which also compensates the robot rotation, to keep the features in view. The two other specifications are ensured by v and ω: the linear velocity is reduced (and even zeroed to v = v u = 0, for H = 1), while the angular velocity aligns the robot with f . 3) Control law [START_REF] Fontanelli | Visual Servoing in the Large[END_REF] can also be derived using a redundancy-based framework, as has been done in [START_REF] Cherubini | A redundancy-based approach for obstacle avoidance in mobile robot navigation[END_REF].

In that work, we proved that for a similar problem, obstacle avoidance had no effect on the visual task, which could be achieved for any H ∈ [0, 1]. Note that also here, plugging the expressions of v, ω, and φ from ( 14) into the visual task equation:

ẋ = j v v + j ω ω + j φ φ yields (1)
. Thus, desired state x d is globally asymptotically stable for the closed loop system, ∀H ∈ [0, 1].

III. EXPERIMENTAL RESULTS

Here, we report the experiments obtained with controller [START_REF] Fontanelli | Visual Servoing in the Large[END_REF] and shown in the video attached to this paper. All experiments have been carried out on a CyCab robot equipped with a 70 • field of view, B&W camera mounted on a TRACLabs Biclops Pan/Tilt head (the tilt angle is null, to keep the optical axis parallel to the ground), and with a 4layer, 110 • scanning angle, laser SICK LD-MRS. The offset between R and C is δ = 0.7 m, and we set Z = 15 m that meets condition Z > δ 2 . The grid is limited to 1.5 m on each side, and to 5 m in front, and it is built by projecting the laser readings from the 4 layers on the ground. Since camera (10 Hz) and laser (40 Hz) processing are not synchronized, they run on two different threads, and control input u is sent to the robot when the visual information is available (10 Hz). For the linear velocity design, we use V = 0.4 ms -1 and γ = 75. The velocity is limited for safety reasons, since the framework runs on a real robot, operating on our campus, with image processing at 10 Hz. With V = 0.4 ms -1 , scanner acquisition at 40 Hz, and K = 20, neglecting the effect of motion on the occupancies is reasonable. In all experiments, we set: λ x = 0.5, λ ϕ = λ α = 0.3, κ = 0.25, ρ = 3 and = 4.

Controller [START_REF] Fontanelli | Visual Servoing in the Large[END_REF] has been validated in the three experiments in Fig. 3. In each case, after manually driving CyCab to acquire the image database, we test two setups (safe and unsafe). First, no obstacle is present: since H = 0, (15) is used, with fixed forward-looking camera (ϕ = 0). Then, some obstacles (including pedestrians) are present, near and on the taught path, and the robot must deal with them; in addition, the obstacles may occlude the features. The replayed paths, estimated from odometry and snapshots, are outlined in Fig. 3 (left), in the safe (yellow) and unsafe (red) setup. Since the focus here is on obstacle avoidance, the taught path is not drawn. It is hard to find a general interpretation of the results, since these depend on many factors (e.g., position of obstacles and visual features). Nevertheless, in all the experiments, the robot follows the visual path without colliding, in spite of occlusions (although a minimal number of visual features is obviously required). Although some portions of the replayed 3D path are far from the taught one, these motions are indispensable to avoid collisions. In the following, we detail the experiments.

The path used in experiment 1 is 60 m long, and composed of N = 34 key images. Three obstacles are present, and the robot overtakes them all (the first and third on the left, the second on the right), to reach the last key image. For the unsafe setup, snapshots are shown in Fig. 3 (top), and v, ω, φ and ϕ are plotted in Fig. 4 (left). The iterations with H > 0 are highlighted in yellow. The smooth trend of v at the beginning and end of all three experiments is due to the acceleration saturation carried out at the CyCab low-level control. CyCab is initially near a wall parallel to the Z axis, which is too far to be considered in the grid. This shows the utility of ignoring lateral data, which would have driven the robot away from the path. The first obstacle is detected at iteration 250: vector field f generates a positive rotation (green in the figure), compensated by the camera pan, and v is reduced. The obstacle is overtaken while maintaining feature visibility. Then, since the path is free, the pan angle (blue) is zeroed, until the second obstacle, which triggers negative ω, and positive φ. Afterwards, H is activated by the third obstacle, which is overtaken on the left (iteration 1200), while decelerating. Again, H is canceled at iteration 1400, and the robot is driven by [START_REF] Courbon | Autonomous Navigation of Vehicles from a Visual Memory Using a Generic Camera Model[END_REF]. The average velocity v is slightly reduced, from 0.39 ms -1 in the safe setup, to 0.35 in the presence of obstacles. The image error with respect to the database, averaged over the experiment, increases from 11 to 17 pixels. This slight increment in the unsafe setup is simply due to the visual occlusions provoked by the obstacles (as shown in the video).

Experiments 2 and 3 are carried out on a straight path of 60 m and N = 30 key images.

In experiment 2, a grey car, which was not present during teaching, is on the path during navigation, and a barrier blocks the road halfway through. Snapshots of the experiment are shown in the center of Fig. 3, and the velocities are plotted in Fig. 4 (center). The grey car is overtaken on the left at iterations 240-380, while the robot slows down (black curve). Then, as soon as the context is safe again, the robot accelerates and returns on the path, while the camera is reset forward. The barrier is more difficult to deal with. In fact, when detected, it is centered on the Z axis and orthogonal to it (see the grids on Fig. 3); this induces α ≈ 0 and drives the robot towards the barrier. However, as the CyCab approaches the barrier, the norm of f increases, and eventually becomes greater than , to make H = 1 and stop the robot. Note that when there is no obstacle, v ≈ V , because, in contrast with experiment 1, the taught path here is straight, leading to v s ≈ V , from [START_REF] Zhang | Robust Appearance Based Visual Route Following for Navigation in Large-scale Outdoor Environments[END_REF]. The value of v, averaged over the path diminishes, from 0.39 in the safe setup, to 0.34 in the unsafe one. This time, the image error, averaged up to the barrier, does not vary in the two setups (13 pixels in both cases). In experiment 3, although the grey car and barrier are not present anymore, three persons are standing on the path. On Fig. 3 (bottom), we show some snapshots of the experiment, while the velocities are plotted in Fig. 4 (right). From iteration 100 to 320, the activation function is triggered by the detection of the pedestrians. This generates a counterclockwise angular velocity (the persons are overtaken on the left), and a slight deceleration. Afterwards, since the street is free again, the robot accelerates and returns on the path, which this time is completed up to the last key image, since the barrier has been removed. Just like in experiment 2, when there is no obstacle, v ≈ V , since the path is straight. The average linear velocity (v = 0.38 ms -1 ) is almost the same as in the safe setup (0.39), and so is the average image error (12 pixels).

IV. CONCLUSIONS

For the first time, a framework with simultaneous obstacle avoidance and outdoor visual navigation is presented. It merges techniques from potential fields and visual servoing, and guarantees path following, obstacle bypassing, and collision avoidance by deceleration. The method has been validated by outdoor experiments with real obstacles (parked cars and pedestrians). In the future, we plan to take into account moving obstacles, as well as visual occlusions provoked by the obstacles. Finally, it may be interesting to record scanner data during the teaching step too, in order to improve obstacle avoidance.

Fig. 1 .

 1 Fig. 1. Top view of the robot (orange), equipped with an actuated camera (blue). (a) Reference frames. (b) Obstacle-induced vortex fields, and control variables (v, ω, φ). (c) Occupancy grid and potential field f construction.

Fig. 2 .

 2 Fig.2. The current and desired images contain some common visual features, that are used for navigation.

Fig. 3 .

 3 Fig. 3. Top to bottom: experiments 1 to 3. Left: reconstruction of the replayed path with (red) and without (yellow) obstacles. Right: snapshots (above) and grids (below) in the presence of obstacles. The red vectors show the orientation of f .

Fig. 4 .

 4 Fig. 4. Relevant variables in experiments 1 to 3 (left to right): v (black, in ms -1), ω (green, in rad s -1 ), ϕ (blue, in rad) and φ (red, in rad s -1 ). The iterations with non-null H are highlighted in yellow.

When the next image is the final one, we use I sd = I d , since the second next key image is undefined.

To guarantee H < 1, parameter κ must be chosen smaller than 2 π .

ACKNOWLEDGMENTS

This work has been supported by ANR (French National Agency) CityVIP project under grant ANR-07 TSFA-013-01.