Paolo Robuffo Giordano

Riccardo Spica
email: riccardo.spica@irisa.fr

Franc ¸ois Chaumette
email: francois.chaumette@irisa.fr

F Chaumette

An Active Strategy for Plane Detection and Estimation with a Monocular Camera

Plane detection and estimation from visual data is a classical problem in robotic vision. In this work we propose a novel active strategy in which a monocular camera tries to determine whether a set of observed point features belongs to a common plane, and, if so, what are the associated plane parameters. The active component of the strategy imposes an optimized camera motion (as a function of the observed scene) able to maximize the convergence in estimating the scene structure. Based on this strategy, two methods are then proposed to solve the plane estimation task: a classical solution exploiting the homography constraint (and, thus, almost completely based on image correspondances across distant frames), and an alternative method fully taking advantage of the scene structure estimated incrementally during the camera motion. The two methods are extensively compared in several case studies by discussing the various pros/cons.

I. INTRODUCTION

Plane detection and estimation from raw visual data is a classical problem in sensor-based robot control, especially in the context of mobile robotics. Indeed, planes are widespread in artificial (man-made) and natural environments, and therefore constitute the typical 3D structure one tries to segment in order to, e.g., plan safe paths among planar obstacles (e.g., vertical walls), or navigate by keeping a desired attitude or distance from special planes (e.g., ground plane for flying robots). The ability to classify planes in the perceived environment is therefore an important feature for several sensor-based applications.

When dealing with images taken by a (possibly moving) camera, a number of approaches has been developed for solving the problem of detecting and identifying planes from visual data. Several methods for instance exploit known correspondances across frames to identify point features (or directly pixels) as whether belonging to a common plane together with the associated plane parameters. These methods usually rely on special geometric constraints linking two views of a planar scene such as the well-known and widely used homography constraint [START_REF] Ma | An invitation to 3D vision[END_REF]. Other methods, instead, attempt to directly measure (using special sensors such as the RGB-D) or recover (exploiting structure from motion algorithms) a 'depth map' of the observed images, for then dealing with the issue of clustering and extracting planes from clouds of 3D points.

Examples from the first category can be found in [START_REF] Arróspide | Homographybased ground plane detection using a single on-board camera[END_REF]- [START_REF] Grabe | Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF] where the homography constraint in its various forms (discrete and/or continuous version) is employed. For instance, in [START_REF] Lin | Robust Ground Plane Detection for Obstacle Avoidance of Mobile Robots Using a Monocular Camera[END_REF] the homography constraint is exploited to classify features belonging to the (dominant) ground plane for a mobile robot equipped with a camera. The method however requires an initialization step to determine the location of the ground plane at the beginning of the motion. The authors in [START_REF] Arróspide | Homographybased ground plane detection using a single on-board camera[END_REF] address a similar problem but by including a Kalman filtering step that exploits the temporal correlation between consecutive images to improve the estimation of the homography matrix. Also, special constraints of the employed robot (a ground wheeled mobile robot) are used to simplify the problem. In [START_REF] Grabe | Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF] the continuous version of the homography constraint is instead used to segment the optical flow detected by a flying robot into clusters belonging to different planar patches. Finally, within the second category of methods dealing with 3D point clouds one can mention [START_REF] Vaskevicius | Fast plane detection and polygonalization in noisy 3D range images[END_REF]- [START_REF] Borrmann | The 3D Hough Transform for Plane Detection in Point Clouds: A Review and a new Accumulator Design[END_REF] and references therein. In these cases, the problem is rather on how to fit planes to sets of 3D points and on how to cluster them according to some reasonable 'planarity measure'.

With respect to this state-of-the-art, the problem addressed in this paper is the following: given a monocular camera observing a (possibly time-varying) set of N feature points in the scene, find an active strategy able to determine, in an optimized way, whether the N points belong to a common plane and what are the associated plane parameters (distance and normal vector). No special assumptions are made on the N points, nor special constraints are assumed for the camera motion. The sought strategy is termed active in the sense that, following the framework proposed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], it aims at controlling online the camera motion (as a function of the observed scene) in order to optimize the convergence rate of the plane estimation task. In this sense, our method differs from most of the previous literature which assumes a camera moving in an 'non-informed' way, i.e., without attempting to affect online its motion for facilitating the plane estimation.

The rest of the paper is then organized as follows: Section II briefly reviews the theoretical framework of [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], while Sect. III introduces and details the proposed strategy for plane detection and estimation. Here, two alternative methods are proposed: a first solution based on the classical homography decomposition, and a second solution fully exploiting the structure of the scene estimated exploiting the framework in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. The two methods are then extensively compared in Sect. V with several simulation case studies which point out the respective pros/cons. Interestingly, the (widely-used) homography decomposition results highly sensitive to non-idealities in the scene, with the other proposed method being instead much more robust to real-world conditions. Sect. VI then concludes the paper.

II. REVIEW OF ACTIVE STRUCTURE FROM MOTION

In this section, we briefly summarize the active estimation framework proposed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]. Let (s, χ) ∈ R m+p be the state of a dynamical system in the form

ṡ = f m (s, u) + Ω T (t)χ χ = f u (s, χ, u) (1)
where s and χ represent, respectively, a measurable and unmeasurable component of the state, and u ∈ R v is the system input vector. In formulation (1) vector χ is required to appear linearly in the dynamics of s (first equation), and matrix Ω(t) ∈ R p×m to be a known and sufficiently smooth time-varying quantity.

Structure from Motion (SfM) problems can be recast to formulation (1) by taking s as the set of visual features measured on the image plane, u = (v, ω) as the camera linear/angular velocity in camera frame, and χ as a suitable (and locally invertible) function of the unknown structure of the scene to be estimated 1 . Furthermore, for SfM problems one has Ω(t) = Ω(s, v(t)) with, in particular, Ω(s, 0) ≡ 0: as well-known, the camera linear velocity v plays a key role for the convergence of SfM algorithms.

For a system in form (1), a possible estimation scheme can be devised as follows: letting (ŝ , χ) ∈ R m+p be the estimated state, ξ = s-ŝ, z = χ-χ, consider the following observer ṡ = f m (s, u)

+ Ω T (t) χ + Hξ χ = f u (s, χ, u) + αΩ(t)ξ (2)
with H > 0 and α > 0. Note also that observer (2) does not require knowledge of ṡ (i.e., measurement of velocities on the image plane), but it only needs measurement of s (the 'visual features') and of (v, ω) (the camera linear/angular velocity in the camera frame). Following the derivations in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], one can show that, by a proper (state-dependent) choice of gain H, the dynamics of the estimation error z(t) = χ(t) -χ(t) (the error in estimating the structure of the scene χ) is equivalent to that of the following second-order linear and diagonal system

η + 2 √ αS η + αS 2 η = 0, (3)
where η ∈ R p , S = diag(σ i ) ∈ R p×p , and 0 ≤ σ 2 1 ≤ . . . ≤ σ 2 p are the p eigenvalues of the square matrix ΩΩ T . The convergence rate of system (3) is then dictated by the quantity ασ 2 1 , with σ 2 1 being the smallest eigenvalue of ΩΩ T . Since in the SfM case Ω = Ω(s, v), one can show that [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF]

(σ 2 1 ) = J v v + J s ṡ, (4)
1 For instance, in the point feature case [START_REF] De Luca | Feature depth observation for image-based visual servoing: Theory and experiments[END_REF], χ can be taken as the inverse of the feature depth Z, and, for image moments of planar scenes, χ can be taken as the normal vector of the observed plane scaled by its distance from the camera optical center [START_REF] Giordano | 3D structure identification from image moments[END_REF].

where the Jacobian matrices J v ∈ R 1×3 and J s ∈ R 1×m have a closed form expression function of (s, v) (known quantities). It is then possible to invert (4) w.r.t. vector v so as to act on σ 2 1 (t), e.g., for maximizing its value. We note that this step represents the active component of the strategy since, in the general case, inversion of (4) will yield a camera velocity v(t) function of the system measured state s(t).

We conclude by providing the explicit expressions of the above machinery for the point feature case (which is the case considered in the next developments). Assume a calibrated pin-hole camera, and let s = (x, y) = (X/Z, Y /Z) be the normalized perspective projection of a 3D point (X, Y, Z) onto the image plane. Formulation (1) can be applied by taking χ = 1/Z with, thus, m = 2 and p = 1, and

       f m (s, u) = xy -1 + x 2 y 1 + y 2 -xy -x ω Ω (s, v) = xvz -vx yvz -vy fu (s, χ, u) = vzχ 2 + (ywx -xwy) χ . (5)
Furthermore, one has

σ 2 1 = ΩΩ T = (xv z -v x ) 2 + (yv z -v y ) 2 (6)
as the single eigenvalue of ΩΩ T , and in (4)

Jv = 2 vx -xvz vy -yvz (xvz -vx) x + (yvz -vy) y Js = 2 (xvz -vx)vz (yvz -vy)vz . (7)

III. DETECTION AND ESTIMATION OF A PLANE

FROM A SET OF POINT FEATURES Let P i = (X i , Y i , Z i ) be N 3D points expressed in the camera frame, and let p i = (x i , y i , 1) = (X i /Z i , Y i /Z i , 1) be the corresponding normalized feature positions on the (assumed calibrated) camera image plane. The problem addressed in this paper is how to optimally determine whether the N points P i belong to a common plane, and what are the associated plane parameters (normal and distance). We now detail two possible strategies to achieve this goal.

A. Plane reconstruction from estimated 3D points

Assume that an estimation Ẑi of the unknown depth Z i of each point is available. Then, from each measured point feature p i one can recover an estimation P i = Ẑi p i of the corresponding 3D point P i in the current camera frame. Let P : n T E + d = 0 be the equation of the sought plane, with n ∈ S 2 and d ∈ R representing the unit normal vector and distance in camera frame. For the estimated points P i to belong to P, it must hold

n T P i + d = 0, i = 1 . . . N. (8)
Equation ( 8) can be rearranged in matrix form as

   P T 1 1 . . . . . . P T N 1    n d = A n d = 0 (9)
with A ∈ R N ×4 . Assuming N ≥ 4, the linear system (9) has a unique solution (up to a scalar factor) iff rank(A) = 3.

Let U A S A V T A = A be the singular value decomposition of matrix A, with σ 2 1,A ≤ • • • ≤ σ 2 4
,A being the associated singular values. The inverse of the condition number σ A = σ 2 1,A /σ 2 4,A can be taken as a normalized measure of the planarity of the N points P i (rank(A) = 3 ⇐⇒ σ A = 0). Furthermore, as well-known, a (least-square) solution of the homogeneous system ( 9) is given by v

= (v 1 , . . . v4 ), the column of V A associated to σ 2 1,A . From v one can then recover n d = ± v v2 1 + v2 2 + v2 3 , (10)
i.e., by imposing n = 1. The final sign ambiguity can be resolved by fixing the sign of d according to the adopted convention.

Summarizing, given a collection of N (estimated) 3D points P i , one can obtain a measure of their planarity by computing σ A (σ A = 0 if they belong to the same plane, σ A > 0 otherwise), and then from (10) obtain a unique solution for the plane parameters (n, d) which best fits the N points. Clearly, one still faces the issue of obtaining the N estimations P i from the measured feature points p i . Section IV explains how to optimally solve this problem exploiting the active estimation framework of the previous Sect. II.

B. Plane reconstruction from the homography decomposition

As a (well-known and widely-used) alternative method, one can also exploit the homography constraint for recovering the plane normal n from a moving camera observing N feature points, see [START_REF] Ma | An invitation to 3D vision[END_REF]. In short, let 0 F C be the camera frame of reference at the beginning of the motion, F C the camera frame at the current time (i.e., after some displacement has taken place), and (R, T ), R ∈ SO(3), T ∈ R 3 , be the rotation matrix from 0 F C to F C and the position of 0 F C w.r.t. F C and expressed in F C , respectively. Let also 0 p i and p i represent the measured locations of the i-th feature point in frames 0 F C and F C .

Assuming again a planar scene P : n T E + d = 0, it is well-known that the following relationship holds for all the N image pairs ( 0 p i , p i )

[p i ] × H 0 p i = 0, i = 1 . . . N, (11)
where

[x] × y = x × y and H = R + T d n T ∈ R 3×3
is the so-called homography matrix, which encodes the structure of the scene (plane parameters) and the displacement among the two frames. Equation (11) can be rearranged as b A (least-square) solution H s of (12) can again be found by exploiting the singular value decomposition of B (by taking the column of V B associated to σ 2 1,B ). Using standard algorithms [START_REF] Ma | An invitation to 3D vision[END_REF], it is finally possible to decompose the associated recovered homography H into two physically possible solutions (R 1 , T 1 /d, n 1 ) and (R 2 , T 2 /d, n 2 ). However, the ambiguity among these two solutions can only be resolved by exploiting prior knowledge of the scene (e.g., approximated known direction of n in one of the two frames, or comparison against the homography estimated from a third frame).

T i H s = 0 where b i = 0 p i ⊗ [p i ] × ∈ R 9×3
We note that the above machinery only involves observed image pairs ( 0 p i , p i ), and thus allows to compute σ B and to recover the plane normal n without requiring knowledge of the (unknown) depths Z i as in the previous case. Knowledge of the scene 3D structure is nevertheless still needed for obtaining the plane distance d: from the recovered n and the estimated P i , a simple possibility from ( 8) is indeed

d = - N i=1 n T P i N . (13)

C. Final considerations

Let us denote with method A (based on the estimated P i ) and method B (based on the classical homography decomposition) the two techniques discussed in the previous Sect. III-A and Sect. III-B. It is interesting to draw the following comparison:

1) Assumptions: while method A only relies on a frameby-frame tracking of the point features, method B requires the correspondences of several points across distant frames (the initial and the current ones). Furthermore, method A can straightforwardly cope with the loss/gain of feature points (e.g., because of visibility constraints as shown in Sect. V-C), while method B needs to either always keep (a subset of) the initial features within visibility, or to periodically reinitialize the initial frame at the current one (thus, temporarily suffering from a small baseline);

2) Complexity: the planarity measure σ A is obtained from the svd decomposition of the N × 4 matrix A in (9), while evaluation of σ B requires the decomposition of the 3N × 9 matrix B in (12). For large N one can then prefer σ A (method A) in terms of reduced algebraic computational load;

3) Convergence rate: method A relies, in all its steps, on the estimated 3D points P i . Therefore, during the transient phase of the depth estimation error (i.e., when Z i -Ẑi is still large), no reliable results can be expected by method A, while method B can in principle be successfully employed for recovering n and computing σ B as soon as the camera has undergone a sufficient displacement w.r.t. its initial pose; 4) Accuracy: Method A provides a unique solution for n (eq. ( 10)), while method B results in two physically possible n 1 and n 2 which must then be disambiguated. Finally, and as it will be shown extensively in Sect. V, the homography decomposition of method B results highly sensitive to nonidealities of the observed scene (e.g., when the observed points P i are approximately, but not exactly, planar), with method A being instead much more robust to these issues.

In conclusion method A results superior to method B in all aspects apart from the potentially slower convergence rate, which is anyway traded for a much higher robustness w.r.t. non-idealities as those found in real-world conditions.

IV. OPTIMAL DEPTH ESTIMATION FOR A SET OF POINT FEATURES

We now address the issue of optimally recovering the unknown depths Z i of the N observed point features p i by optimizing the camera motion. We stress, again, that this 'optimized depth estimation' only relies on the measured p i and on the (assumed known) camera velocity (v, ω). We also note that the N points P i are not required to be planar for applying the following strategy, but they can be arranged in any spatial configuration (as long as they can be tracked).

In case of N point features, one can directly apply observer (2) by defining s = (p 1 , . . . p N ) ∈ R m and χ = (1/Z 1 , . . . , 1/Z N ) ∈ R p , with m = 2N and p = N . Since the dynamics of each s i = p i is only affected by its associated unknown χ i = 1/Z i , this is equivalent to implementing in parallel N instances of (2), one for each tracked feature (i.e., N instances with m = 2 and p = 1). Indeed, we can also note that, for N points, matrix Ω takes the expression

Ω =        x 1 vz -vx 0 0 . . . 0 y 1 vz -vy 0 0 . . . 0 0 x 2 vz -vx 0 . . . 0
       T , (14)
with, therefore,

ΩΩ T =       σ 2 1,1 0 . . . 0 0 σ 2 1,2 . . . 0 . . . . . . . . . . . . 0 0 . . . σ 2 1,N       = diag(σ 2 1,i ), where σ 2 1,i = (x i v z -v x ) 2 + (y i v z -v y ) 2 (15)
is the eigenvalue determining the convergence speed of the estimation error

z i (t) = χ i (t) -χi (t) = 1/Z i (t) -1/ Ẑi (t)
for the i-th feature point, see [START_REF] Vaskevicius | Fast plane detection and polygonalization in noisy 3D range images[END_REF][START_REF] Suttasupa | Plane Detection for Kinect Image Sequences[END_REF].

In order to optimize the error convergence of the whole error vector z(t), one can simply aim at maximizing the mean σ 2 = N i=1 σ 2 1,i /N . Let J v,i and J s,i be the Jacobian matrixes associated to each σ 2 1,i as given in [START_REF] Borrmann | The 3D Hough Transform for Plane Detection in Point Clouds: A Review and a new Accumulator Design[END_REF]. One then has

(σ 2 ) = 1 N N i=1 J v,i v + N i=1 J s,i ṡi .
Maximization of σ 2 can then be obtained by choosing v as

v = vσ = k σ N N i=1 J T v,i - N i=1 J v,i † N i=1 J s,i ŝi , k σ > 0, (16)
where † denotes the pseudo-inverse operator. Note that in (16) the (not directly measured) ṡi is replaced by an estimation ŝi obtained by evaluating (1) on χi . This, indeed, avoids the need of obtaining the (possibly noisy) image velocity ṡi by, e.g., numerical differentiation.

V. RESULTS

In this section we report five simulated case studies meant to illustrate the proposed machinery for plane detection and estimation. In all simulations, we considered a free-flying camera delivering images at 30 Hz (the update rate for obtaining vector s) and controlled at 100 Hz (the update rate for imposing the camera velocity commands (v, ω)). The vector of visual features s was corrupted, componentwise, with a uniformly distributed random noise of 2 pixels.

In all simulations we considered presence of a plane P with n = (0, 0, 1) and d = -1 [m] in 0 F C , and tested methods A and B in five different combinations: (i) cases I-II involve a perfectly planar scene made of N = 10 points P i lying exactly on P and randomly generated from a uniform distribution of radius 0.2 [m] (therefore, all points start with Z i (t 0 ) = 1 [m]). The camera is also assumed to have an unlimited field of view (fov). Case I implements the optimization action on σ 2 , while case II does not implement any optimization action; (ii) cases III and IV involve a (more realistic) approximately planar scene made again of N = 10 points generated as in cases I-II, but by then corrupting their position with an additional uniformly distributed noise of amplitude 0.05 [m] along the direction of n (thus, simulating presence of an uncertainty of 5% in the planarity assumption of points P i w.r.t. the initial camera pose). The camera is again assumed to have an unlimited fov. Case III implements the optimization of σ 2 while case IV does not implement it; (iii) the last case V involves again an approximately planar scene as in the previous cases III-IV, but considers a limited camera fov. The possibility of losing/gaining features over time when exiting/entering the camera fov is then taken into account. This case implements the optimization action on σ 2 .

As for the optimization of σ 2 , we note that each σ 2 1,i in (15) depends on the norm of the camera linear velocity v (the larger the camera speed ∼ v , the faster the estimation error convergence for a given set of gains). In order to obtain a fair comparison among all cases, we thus considered the maximization of σ 2 under the constraint v = const. This was obtained as follows:

letting κ = 1 2 v 2 , κ des = 1 2 v(t 0 ) 2 , we replaced (16) with v = v v 2 k 1 (κ des -κ) + k 2 I - vv T v 2 vσ (17)
with k 1 > 0 and k 2 ≥ 0. Cases I-III-V were then implemented with k 1 > 0 and k 2 > 0 (maximization of σ 2 ), while cases II-IV with k 1 > 0 and k 2 = 0 (no action on σ 2 ). Finally, the camera angular velocity ω was exploited in all cases for keeping the centroid of the observed point features p i at the center of the image plane (as σ 2 = σ 2 (v, s), one can freely chose ω to fulfil additional goals).

A. Camera with unlimited field of view and perfect planar scene (cases I-II)

For illustrating the results of cases I-II, let us denote with ( nA , dA ) and ( nB , dB ) the estimation of the plane parameters (n, d) obtained with method A and method B, respectively, and then define e d A = d -dA , e d B = d -dB , e n A = arccos(n T nA ), e n B = arccos(n T nB ) as the corresponding estimation errors 2 . The following values were used in the simulations: k 1 = 10 and k 2 = 50 (for case II) in ( 17), α = 200 in (2), and v(t 0 ) = (-0.05, 0.05, 0.1) with, thus, v(t 0 ) 2 = 0.015. The initial values for the estimations Ẑi (t 0 ) were taken as the real Z i (t 0 ) plus a uniformly distributed random noise of amplitude 0. 5 [m].

The results of the four cases are reported in Figs. 1(a-l) from which we can then draw the following considerations: first of all, note how the convergence speed of the depth estimation error z(t) is much slower in case I w.r.t. case II (Fig. 1(c) vs. Fig. 1(d)). Thanks to the active maximization of σ 2 in the latter case, convergence of the depth estimation errors is approximately reached in about 4 [s]. We recall that in both cases the camera was traveling with the same linear velocity norm v(t) = v(t 0 ) : the faster convergence of z(t) was then only due to the 'active optimization' of the direction of v as dictated by (17). One can find a similar pattern in the behavior of the N eigenvalues σ 2 1,i in Figs. 1(ef): note how in case I (Fig. 1(e)) all eigenvalues stay below 0.001, with some of them decreasing over time, while in case II (Fig. 1(f)) the eigenvalues are actively regulated towards the common value v(t 0 ) 2 = 0.015 (as explained in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], when p i ≃ (0, 0) one has max σ 2 1,i = v 2 ). Coming to the planarity test and the estimation of the plane parameters (n, d), from Figs. 1(g-h) one can see that σ B (t) ≃ 0, ∀t (red solid line), while σ A (t) converges to zero only when vector z(t) has reached convergence (compare Figs. 1(g-h) with Figs. 1(c-d)). Both criteria then successfully recognize the N points as belonging to a common plane but, clearly, σ B outperforms σ A . This is not surprising since, as explained in Sect. III-C, the quantity σ B is obtained in terms of sole image measurements ( 0 p i , p i ), while σ A requires a good enough knowledge of the estimated 3D points P i (and, thus, provides a reliable answer only when z(t) is close to convergence).

A similar behavior can be found in Figs. 1(i-j) for the errors e n A (solid blue line) and e n B (dashed red line). The estimated normal nB of method B (homography decomposition) coincides almost immediately with n. The initial 'noisy' behavior of e n B (t) is due to the lack of a large enough baseline w.r.t. the simulated image noise added to the measurement s which negatively affects the homography decomposition. Finally, having obtained a good normal estimation nB allows to symmetrically obtain a good estimation dB from (13). This is shown in Figs. 1(k-l) where the errors (e d A , e d B ) are reported. We can again note how e d B (t) (dashed red line) quickly converges to 0 when compared to e d A (t) (solid blue line). 2 Here, for simplicity we always assumed ability to disambiguate among the two physically possible solutions of method B. Summarizing, these results clearly show two facts: on the one hand, they illustrate the benefits of the optimization action on σ 2 for improving the convergence of z(t). On the other hand, they also indicate method B (based on the classical homography decomposition) as apparently superior for what concerns both the planarity test and the estimation of the plane parameters (n, d). This latter conclusion is, however, completely different when considering the (more realistic) situation of an approximately planar scene of the next cases III-IV.

σ 2 1,i (e)

B. Camera with unlimited field of view and approximately planar scene (cases III-IV)

The results are reported in Figs. 2(a-l). Here, Figs. 2(g-h) show again the planarity criteria σ A and σ B : being the scene not exactly planar, both measures correctly reach a constant non-zero value which represents the confidence level in considering the points P i as belonging to the same plane. Note, however, that now σ B (solid red line) has a transient behavior qualitatively equivalent to σ A (solid blue line). Thus, both quantities provide a similar level of information before reaching their respective 'steady-state'. Furthermore, when considering the errors e n A and e n B in Figs. 2(i-j) we can note another interesting result: while e n B (t) (dashed red line) has a highly erratic behavior (indicating a quite unreliable estimation of nB ), e n A (t) (solid blue line) converges towards 0 (here, the error is computed w.r.t. the normal n best fitting the real N points P i ). Analogous considerations also hold for the errors e d A (solid blue line) and e d B (dashed red line) in Figs. 2(k-l): e d A (t) correctly converges to 0 while e d B (t) does not converge at all. These results then allow to conclude that, in the more realistic condition of an approximately planar scene (with an error of ≈ 5% w.r.t. the camera initial distance to the plane), method B is not able to provide a reliable estimation of the plane parameters (n, d) as opposite to method A which, instead, shows an unsatisfactory performance. This is most likely because the homography decomposition of method B only relies on image correspondences and is, thus, highly sensitive to non-idealities such as image noise or non perfect planarity of the observed points, while method A exploits the estimation of the point depths Ẑi for then drawing conclusions from an (estimated) cloud of 3D points in the current camera frame.

C. Camera with limited field of view (case V)

In this last simulation we considered a camera with a limited fov for introducing the possibility of losing/gaining point features during the camera motion because of the visibility constraint. This was meant to test the robustness of the proposed estimation strategy when relaxing the (often unrealistic) assumption of being able to track the same set of point features over time. The simulation involved a total of N = 15 points arranged in an approximately planar scene, and the active optimization of σ 2 . Only method A was employed for recovering the plane parameters ( nA , dA ) since, as explained, method B requires keeping a sufficient number feature points always within the camera fov.

Whenever during motion a new point feature p i entered visibility, its estimated depth Ẑi was initialized so as to force P i = Ẑi p i to belong to the current estimation of the plane given by ( nA , dA ), i.e., by choosing

Ẑi = -dA /( nT A p i ). (18)
The identity of each feature exiting visibility was not retained approximately reach and keep the value v(t 0 ) 2 thanks to the active optimization of σ 2 . The vertical jumps in the plots represent features which have left/entered the image plane, with their estimated depths being either discarded ( Ẑi = 0) or reset as in (18). Figure 3(d) depicts the behavior of the planarity measure σ A (t), and Figs. 3(e-f) the estimation errors e d A (t) and e n A (t). We can again note how method A is able to successfully determine the (approximate) planarity of the observed points and the associated plane parameters despite the additional visibility constraint which forces features to randomly enter/exit the camera fov (note, again, the several jumps in the plots indicating loss/gain of some features). Finally, Fig. 3(g) shows the total number of tracked features n p over time (which keeps varying as expected).

VI. CONCLUSIONS

In this paper we have presented and critically compared two methods for determining whether a set of point features belongs to a common plane and the associated plane parameters. The first method exploits an estimation of the point depths for retrieving the 'best plane' fitting a set of 3D points, while the second method is based on the classical homography decomposition and, thus, strongly depends on correspodances across distant image frames (initial and current ones) or on a reinitialization of the initial frame. Both methods also rely (to different extents) on a newly developed active SfM strategy which allows to optimize online the camera trajectory in order to maximize the SfM convergence rate. An extensive set of simulation results in realistic conditions was then presented for assessing the pros/cons of both methods: the results showed the poorer performance of the classical homography-based approach w.r.t. the other approach.

We are currently aiming for an experimental validation of this approach by also investigating the use of different kinds of visual features (e.g., dense/discrete image moments), as well as more sophisticated strategies able to cope with outliers and/or presence of multiple planes in the scene.

  (with ⊗ indicating the Kronecker product), and H s = (H 11 , H 21 , . . . , H 33 ) ∈ R 9 . By now letting B = (b 1 , . . . , b N ) ∈ R 3N ×9 be the collection of all the N b i , one can compactly rewrite equation (11) for all measured pairs as BH s = 0. (12) Similarly to before, equation (12) has a unique solution, up to a scalar factor, iff rank(B) = 8. Letting then σ 2 1,B ≤ . . . ≤ σ 2 9,B be the singular values of B, one can take again the inverse of the condition number σ B = σ 2 1,B /σ 2 9,B as an alternative measure of planarity besides the previously introduced σ A .

0 y 2

 2 vzvy 0 . . . . x N vzvx 0 0 0 . . . y N vzvy

Fig. 1 :

 1 Fig. 1: Simulations with unlimited camera fov and perfect planar scene (case I on the left, case II on the right). (a-b) camera 3D trajectory; (c-d) behavior of the depth estimation error z(t); (e-f) behavior of the N eigenvalues σ 2 1,i ; (g-h) behavior of the planarity measures σA (solid blue line) and σB (solid red line); (i-j) behavior of en A (solid blue line) and en B (dashed red line); (k-l) behavior of e d A (solid blue line) and e d B (dashed red line)

Fig. 2 :

 2 Fig.2: Results of the simulations with unlimited camera fov and approximately planar scene (case III on the left, case IV on the right). Case III is shown on the left side and case IV on the right side. The pattern of the plots is the same as in Figs.1(a-l).

Fig. 3 :

 3 Fig. 3: Results of the simulations with limited camera fov (case V). (a) camera 3D trajectory; (b) behavior of the depth estimation error z(t); (c) behavior of the N eigenvalues σ 2 1,i ; (d) behavior of the planarity measures σA; (e) behavior of en A ; (f) behavior of e d A ; (g) behavior of np, the total number of features in visibility. The vertical jumps in the various plots indicate loss/gain of point feature which exit/enter the image planeand, when re-entering in the image plane, it was treated as a newly acquired point. The simulation was run with the same gains of the previous cases and by taking v(t 0 ) = (-0.1, 0.1, 0) with, thus, v(t 0 ) 2 = 0.02.Figures3(a-g) show the results of the simulation: Fig.3(a) depicts the camera trajectory in space with the arrow indicating its optical axis. Figure 3(b) shows the convergence of the estimation error z(t) and Fig. 3(c) the behavior of the N = 15 eigenvalues σ 2 1,i over time. Note, again, how all σ 2

Figure 3 (

 3 b) shows the convergence of the estimation error z(t) and Fig. 3(c) the behavior of the N = 15 eigenvalues σ 2 1,i over time. Note, again, how all σ 2 1,i


Carlo Masone
email: carlo.masone@tuebingen.mpg.de.

Paolo Robuffo Giordano

Heinrich H Bülthoff

Antonio Franchi
email: antonio.franchi@tuebingen.mpg.de.

Semi-autonomous Trajectory Generation for Mobile Robots with Integral Haptic Shared Control

A new framework for semi-autonomous path planning for mobile robots that extends the classical paradigm of bilateral shared control is presented. The path is represented as a B-spline and the human operator can modify its shape by controlling the motion of a finite number of control points. An autonomous algorithm corrects in real time the human directives in order to facilitate path tracking for the mobile robot and ensures i) collision avoidance, ii) path regularity, and iii) attraction to nearby points of interest. A haptic feedback algorithm processes both human's and autonomous control terms, and their integrals, to provide an information of the mismatch between the path specified by the operator and the one corrected by the autonomous algorithm. The framework is validated with extensive experiments using a quadrotor UAV and a human in the loop with two haptic interfaces.

I. INTRODUCTION

Mobile robots can be deployed to remote or dangerous locations that are not accessible by human workers, thus making them valuable resources for many tasks such as exploration, monitoring and damage assessment. For example, in [START_REF] Murphy | Use of remotely operated marine vehicles at minamisanriku and rikuzentakata japan for disaster recovery[END_REF] underwater robots were teleoperated to inspect critical infrastructures by human pilots who could rely on a force feedback to perceive the environment.

The previous example is indicative of the fact that in real world applications robots are usually operated by human pilots, because they lack the sufficient cognitive capabilities to take complex decisions or to cope with unstructured environments. The example shows also that the human operator must be provided with cues that are informative of the state of the robots and of the remote environment [START_REF] Hokayem | Bilateral teleoperation: An historical survey[END_REF]. In this regard, the adoption of a 'bilateral' interaction with haptic cues appears very promising, because: i) it was proven to increase the situation awareness and performance of the operator, see e.g., [START_REF] Lam | Artificial force field for haptic feedback in UAV teleoperation[END_REF], [START_REF] Abbink | Measuring neuromuscular control dynamics during car following with continuous haptic feedback[END_REF]; ii) it requires little bandwidth in comparison to video streaming (see [START_REF] Riedel | Experiments on intercontinental haptic control of multiple UAVs[END_REF]), thus making it a good solution when there is limited bandwidth available, e.g., for intercontinental control of mobile robots over the internet [START_REF] Riedel | Experiments on intercontinental haptic control of multiple UAVs[END_REF] or remote control of underwater vehicles [START_REF] Murphy | Use of remotely operated marine vehicles at minamisanriku and rikuzentakata japan for disaster recovery[END_REF].

In accordance with these considerations, the bilateral haptic shared control framework [START_REF] Niemeyer | Telerobotics[END_REF] has been successfully applied to mobile robots, see e.g., [START_REF] Giordano | A passivity-based decentralized strategy for generalized connectivity maintenance[END_REF], [START_REF] Franchi | Shared control: Balancing autonomy and human assistance with a group of quadrotor UAVs[END_REF], [START_REF] Franchi | Modeling and control of UAV bearingformations with bilateral high-level steering[END_REF], [START_REF] Lee | Semi-autonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles[END_REF]. The classical bilateral teleoperation paradigm adopted in these works operates with a circular structure: the human operator directly commands the current 1 desired state of the robot(s) (e.g., position, velocity and acceleration); the robots try to execute the command and exploit their limited autonomy to achieve or preserve some desired objective (e.g., desired shape of the formation, obstacle avoidance); the force feedback closes the loop by giving back to the operator an information of the mismatch between commanded and actual motion. The drawback of this paradigm is that navigation of the robot(s), e.g., for a monitoring task, requires the human operator to always give motion directives, thus being very demanding.

In order to reduce the human's commitment, we introduced in [START_REF] Masone | Interactive planning of persistent trajectories for human-assisted navigation of mobile robots[END_REF] a novel shared control framework for persistent tasks in which a person modifies the 'closed' path that is autonomously followed by a single mobile robot, rather than piloting the robot itself. The human is assisted by an autonomous algorithm based on reactive deformations which ensures that the path is collision free or generates an alternative path. Reactive deformations allow for real-time implementation while the presence of the human alleviates the problems (e.g., local minima) implicit of purely reactive methods such as [START_REF] Quinlan | Elastic bands: Connecting path planning and control[END_REF], [START_REF] Brock | Elastic strips: A framework for motion generation in human environments[END_REF], [START_REF] Lamiraux | Reactive path deformation for nonholonomic mobile robots[END_REF]. In this paper we extend [START_REF] Masone | Interactive planning of persistent trajectories for human-assisted navigation of mobile robots[END_REF] with several contributions:

1) The autonomous algorithm is extended to ensure path regularity and to prevent the presence of cusps.

2) The haptic feedback is better detailed.

3) The algorithm that generates alternative paths includes a new step (Expansion). 4) The framework is validated with experiments using a quadrotor and with both closed and open paths (in [START_REF] Masone | Interactive planning of persistent trajectories for human-assisted navigation of mobile robots[END_REF] the framework was only tested in simulation with closed paths).

Note that in this paper we focus on path corrections and in practice we generate the trajectory by using a timinglaw that modulates the traveling speed with the curvature of the path. A description of the timing-law algorithm is omitted because it is not the focus of the paper, however the problem of timing-law generation is widely treated in literature, see, e.g., [START_REF] Faulwasser | Optimal exact pathfollowing for constrained differentially flat systems[END_REF]. The rest of the paper is organized as follows. Section II introduces the model of the path and of the environment. The outline of the framework is described in Sec. III and its components are detailed in Secs. III-A to III-C. The algorithm that autonomously plans new paths in presence of obstacles is detailed in Sec. IV. The experimental setup and results are presented in Sec. V.

II. PRELIMINARIES

We consider a single mobile robot with a characteristic point that is capable of traveling a sufficiently smooth and regular planar 2 path by always keeping a non-zero speed. This assumption relates to the concept of differential flatness [START_REF] Faulwasser | Optimal exact pathfollowing for constrained differentially flat systems[END_REF] and applies to the large variety of mobile robots that are differentially flat with the characteristic point as (a part of) its flat output [START_REF] Fliess | Flatness and defect of nonlinear systems: Introductory theory and examples[END_REF] or feedback linearizable with the characteristic point taken as linearizing output [START_REF] Isidori | Nonlinear Control Systems[END_REF].

The robot is tasked to follow a path that is described by a B-spline [START_REF] Biagiotti | Trajectory Planning for Automatic Machines and Robots[END_REF], a linear combination of control points 2n and basis functions B λ j : S → R for j = 1, . . . , n given as

x = x T 1 • • • x T n T ∈ R
γ(x, •) : S → R 2 s → n j=1 x j B λ j (s, s) = B s (s)x (1)
where S is a compact subset of R. The degree λ > 0 and knots s = (s 1 , . . . , s l ) are constant parameters chosen such that γ(x, •) is sufficiently smooth w.r.t. s. Further details on these parameters and on the computation of the basis functions can be found in [START_REF] Biagiotti | Trajectory Planning for Automatic Machines and Robots[END_REF], and in [START_REF] Masone | Interactive planning of persistent trajectories for human-assisted navigation of mobile robots[END_REF] for closed Bsplines. The path corresponding to the B-spline curve γ(x,

•) is γ S (x) = {γ(x, s) ∈ R 2 | s ∈ S}, (2)
i.e., the set of points γ(x, s) obtained by varying the coordinate s within S. Note that the shape of the path is determined by x. Regarding the regularity of γ S (x), it is defined as follows.

Definition 1. A point γ(x, s) with x ∈ R 2n , s ∈ S and such that ∂ γ ∂s (x,s) = (0 0) T ∈ R 2 is called a singularity of γ S (x). A path γ S (x) without singularities is called regular.
Geometrically, a singularity is a point where γ S (x) could have a cusp, i.e., where the direction of motion (the unit tangent vector) vanishes. The following definition will be instrumental to ensure path regularity.

Definition 2. Consider a regular path γ

S (x) with S ⊂ R and x = (x T 1 x T 2 . . . x T n ) T ∈ R 2n , and indi- cate with x ⋆ i (x, s) ∈ R 2 , where s ∈ S i = {s ∈ [s i-λ , s i ) : d B λ i (s,s) d s = 0} 3 , a point such that γ (x T 1 . . . x T i-1 x ⋆ i (x, s) T x T i+1 . . . x T n ) T , s is a singular- ity. The 'singular curve' of the control point x i ∈ R 2 is the collection of points Ω i (x) = {x ⋆ i (x, s) | s ∈ S i }. From (1), it is straightforward to see that x ⋆ i (x, s) = - n j=1,j =i x j d B λ j (s,s) d s d B λ i (s,s) d s . (3)
The singular curve Ω i describes how the control point x i of a regular γ S (x) can be modified without creating singularities. These concepts are illustrated by the example of Fig. 1. 2 The framework generalizes to R 3 , but we formulate our machinery in R 2 to simplify the exposition and because the planar case is relevant per se in many real-world scenarios, e.g., an aircraft monitoring the earth surface while flying at a constant altitude. 3 If iλ ≤ 0, use s 1 instead of s i-λ . The framework also accounts for the presence in the environment of points of interest (PoIs) and static obstacles. The points of interests are locations that are meaningful for the task, e.g., meeting points, fixed stations for data transfer, or victims to be rescued. These locations are modeled as a finite set of points R ∈ R 2×n R . The obstacles are modeled as balls centered in a finite set of static points O ∈ R 2×n O and such that the robot is not in collision if the characteristic point lies outside all the obstacle balls. To simplify the notation, hereinafter we consider balls having all radius R O .

III. OVERVIEW OF THE PROPOSED FRAMEWORK

We assume that an initial regular path γ S (x 0 ) is given by a preliminary planning algorithm tailored for the task at hand 4 . Path modifications are achieved by introducing a time dependency in x so that γ S (x(t)) becomes a time-varying path. The signal x(t) is generated online according to the following dynamical system

ẋ = N u h + u a , x(0) = x 0 , N ∈ R 2n×2n (4)
where u h ∈ R 2n is a term influenced by the human operator (described in Sec. III-A), and u a ∈ R 2n and N ∈ R 2n×2n are two control terms generated by the autonomous algorithm (described in Sec. III-B). The proposed framework that computes the control terms in ( 4) is organized with a circular structure (see Fig. 2): a) Human guidance: it steers γ S (x) towards a desired path γ S (x h ) that is modified by an human operator using an actuated multi-DoFs tool as input device b) Autonomous corrector: an autonomous algorithm corrects, if necessary, the human commands so that the actual path γ S (x) remains regular, collision free and is attracted by nearby points of interest.

c) Haptic feedback: another algorithm closes the interaction-loop between human operator and autonomous correction algorithm by controlling the force exerted by the actuated input device. The force feedback physically informs the operator about the changes brought by the autonomous correction to his/her suggested modifications of the current path.

The three parts of the framework are thoroughly illustrated in Sections III-A, III-B and III-C respectively.

A. Human Guidance

The human operator interacts with the path by means of one or more input devices with 1 ≤ m ≤ 2n fully-actuated DoFs 5 . The devices are modeled as generic mechanical systems M (q)q + C(q, q) q = τ + τ h [START_REF] Riedel | Experiments on intercontinental haptic control of multiple UAVs[END_REF] where q ∈ R m is the configuration vector of the device, M (q) ∈ R m×m is the positive-definite and symmetric inertia matrix, C(q, q) q ∈ R m are the Coriolis and centrifugal terms, and τ , τ h ∈ R m are the control and human forces, respectively 6 . The computation of τ is done by the haptic feedback algorithm described in Sec. III-C. The operator controls the desired shape γ S (x h ) of the path, according to the following dynamical system

ẋh = Q (x h ) Kq , x h (0) = x 0 , (6)
where K ∈ R m×m is a diagonal matrix of positive gains and Q : R 2n → R 2n×m is a nonlinear mapping. The term u h in (4) steers the actual x towards the desired x h by implementing a feedforward/proportional-like action

u h = ẋh + k h (x h -x), (7)
with k h > 0.

Matrix Q in (6) defines the path modifications that the operator can command. In order to provide an intuitive interface for the operator we design Q as the juxtaposition of l elementary matrices

Q i (x h ) ∈ R 2n×νi with i = 1 . . . l that implement 'canonical' transformations Q(x h ) = Q 1 (x h ) | . . . | Q l (x h ) , (8)
where with q i ∈ R νi for i = 1 . . . l, so that each q i is mapped by a a different elementary matrix Q i . Among the several choices allowed by this general formulation, we provide in the following three significative examples of elementary matrices Q i , corresponding to three canonical motions: Translation: The configuration q i ∈ R 2 is mapped to a translation of γ S (x h ) (see Fig. 3a) by the following elementary matrix

1 ≤ l ≤ m, 1 ≤ ν i ≤ m , and l i=1 ν i = m. Partition (8) induces a corresponding partition of q q = q T 1 | . . . | q T l T (
Q i (x h ) = I 2n (9)
where I 2n is the 2n × 2n identity matrix. Notice that matrix Q i does not depend on x h , since the velocity applied to each control point is a fixed scaled version of q i . Scaling: The single DoF q i ∈ R commands a scaling of the path w.r.t. a given fixed point pi ∈ R 2 by means of the following elementary matrix

Q i (x h ) = x h -1 n ⊗ pi (10)
where 1 n is an n-dimensional column vector of ones and ⊗ denotes the Kronecker product. Rotation: The single DoF q i ∈ R commands a rotation of the control points around a given fixed point pi ∈ R 2 by means of the following elementary matrix

Q i (x h ) = diag( Ī2 , . . . , Ī2 n times ) (x h -1 n ⊗ pi ) (11)
where Ī2 = 0 -1 1 0 .

B. Autonomous Corrector

The autonomous corrector must ensure that the actual path γ S (x) satisfies the following requirements:

Objective 1. The distance between any obstacle point o ∈ O and γ S (x) is always greater than R O .

Objective 2. γ S (x) is regular. In order to satisfy these Objectives u a in ( 4) is designed as the sum of three terms

0 1 2 3 4 5 0 2 4 6 8 ϕ O , ϕ R , ϕ I , ϕ E ϕ O ϕ R ϕ I ϕ E
u a = u a,O (x, O) + u a,I (x) + u a,R (x, R). ( 12
)
Obstacle avoidance: u a,O is chosen as

u a,O = - o∈O S ∂γ(x(t), s) ∂x(t) † ∂ϕ O ( γ(x(t), s) -o ) ∂γ(x(t), s) ṗo (s) ds (13) where ϕ O : R ≥R O → R ≥0 is a smooth distance-based artificial potential function such that ϕ O = 0 if γ(x, s) -o ≥ RO ϕ O → ∞ if γ(x, s) -o → R + O with RO > R O . Furthermore, ϕ O is strictly monotonic in [R O ,
RO ] (see Fig. 4 for an example).

For every obstacle o ∈ O, u a,O applies to every point γ(x(t), s) ∈ γ S (x) a repulsive velocity ṗo (s) that goes to infinity as γ(x(t), s) goes towards the boundary R O around o. The repulsive velocity is then mapped to a velocity in the R 2n space of control points by inverting the relation ṗo (s) = ∂γ(x,s) ∂x ẋ with the pseudo-inverse ∂γ(x,s) ∂x †

. Finally, the line integral in [START_REF] Brock | Elastic strips: A framework for motion generation in human environments[END_REF] evaluates the effect of the artificial potential over all the points of the path.

Path regularity: u a,I is designed as

u a,I = - n i=1 n j=1 Si ∂ϕ I ( x i (t) -x ⋆ i (x(t), s) ) ∂x j (t) ṗx j ds (14)
where and ϕ I : R ≥0 → R ≥0 is a smooth distance-based artificial potential function such that 4).

ϕ I = 0 if x i -x ⋆ i ≥ R I ϕ I → ∞ if x i -x ⋆ i → 0 + Furthermore, ϕ I is strictly monotonic in [0, R I ] (see Fig.
The action of ( 14) is twofold. If j = i, then it applies to x i a velocity ṗxi that is directed away from x ⋆ i (x, s) ∈ Ω i (x). If j = i, then it applies to x j a velocity ṗxj such that it moves x ⋆ i (x, s) ∈ Ω i (x) away from x i . In both cases, the intensity of the velocity goes to infinity as

x i -x ⋆ i → 0. PoIs: u a,R is chosen as u a,R = - r∈R ∂γ(x(t), s) ∂x(t) † ∂ϕ R ( γ(x(t), s) -r ) ∂γ(x(t), s) ṗr (s) sr ( 15
)
where sr indicates the closest point of γ S (x) to r and ϕ R : R ≥0 → R ≥0 is a smooth distance-based artificial potential function such that

ϕ R = 0 if γ(x, s) -r = 0 ϕ R = U R > 0 if γ(x, s) -r ≥ R R
Furthermore, ϕ R is strictly monotonic in [0, R R ] (see Fig. 4 for an example). Unlike ϕ O and ϕ I , the action of ϕ R is bounded because reaching the PoIs has a lower priority w.r.t. to the other objectives. However, the force feedback produced by the attractive action helps the operator to locate a PoI even if it is not reached. The structure of ( 15) is similar to that of u a,O (cf. ( 13)): the attractive velocity ṗr (s) is applied to the point γ(x, sr ) ∈ γ S (x) and then projected on the R 2n space of the control points using the pseudo-inverse ∂γ(x,s) ∂x †

. Despite the action of u a , the dependency of u h from the exogenous human directive ẋh might produce path variations that are difficult to cope with for the robot. Moreover, since the time derivatives of ẋh are not available, then also the time derivatives of ẋ cannot be computed and therefore the signals

d k γ(x(t),s(t)) dt k
with k ≥ 2 cannot be used to control the robot 7 .

To solve these problems we design N in (4) as

N = I 2n -J † J (16)
where I 2n ∈ R 2n×2n is the identity matrix and

J ∈ R 2k×2n with k < n is J (x, s) = ∂γ ∂x T ∂ ∂x ∂γ ∂s T . . . ∂ ∂x ∂ k γ ∂s k T T
.

(17) Jacobian J relates variations of x to changes of local geometric properties of the path in s(t), such as the position of the point γ(x(t), s(t)), the tangent vector ∂ ∂ s γ(x(t), s(t)), and so on. Matrix N defined in [START_REF] Fliess | Flatness and defect of nonlinear systems: Introductory theory and examples[END_REF] is the well known orthogonal projection matrix [START_REF] Chiaverini | Kinematically redundant manipulators[END_REF] in the null-space of J , i.e., it imposes that J N (u h + u a ) = 0 2n . The intuitive interpretation to this design is that it imposes the invariance of the local geometric properties of the path at the current location of the robot regardless of the global changes brought by u h and u a .

Formally, using the projection property of N , we can see that:

d γ(x(t),s(t)) d t = ∂ γ(x,s) ∂ x ẋ =02n + ∂ γ(x,s) ∂ s ṡ d 2 γ(x(t),s(t)) d t 2 = ∂ ∂ x ∂ γ(x,s) ∂ s ṡ ẋ =02n + 2 j=1 ∂ ∂s (j-1) ∂ γ(x,s) ∂ s ṡ s (j)
(18) It is straightforward to verify, by iterating with the chain rule, that also the higher order derivatives are independent from ẋk with k ≥ 1. We omit writing these derivatives for the lack of space. In conclusion, N eliminates the effect of abrupt human commands and allows the computation of the signals d k γ(x(t),s(t))

d t k with k ≥ 2. x Q(x) Q † (x) Q † (x h ) Kq ẋ ẋh Q † (x) ẋ R 2n (Control Points Space) R m (Input Device Space) ẋproj e ẋ Q(x) Q(x h ) x h Q(x h ) (a) x Q(x) Q † (x h ) R 2n (Control Points Space) R m (Input Device Space) Q(x h ) x h Q(x h ) k(x -x h ) proj k(x -x h ) e x (b)

C. Haptic Feedback

We design τ as a function of two haptic cues, e ẋ and e x .

First haptic cue: e ẋ is used to provide an information on how well the actual path γ S (x) is following the directives given by the operator. For example, if the human commands an expansion to γ S (x h ) then e ẋ should inform if the expansion rate of γ S (x) is different from the desired one. To compute e ẋ, it is useful to start by making some observations with the help of Fig. 5a: 1) ẋh is obtained by mapping the input device configuration Kq through the matrix Q(x h ). Note that the map Q(•) depends on the application point and in general Q(x h ) = Q(x) (see dashed lines in Fig. 5a). 2) ẋ is generated by reactive actions and in general it does not correspond to any of the path modifications defined by the map Q(•), i.e., ẋ / ∈ ℑ(Q(x)) where ℑ(•) indicates the range space of a matrix. 3) Vector ẋhẋ does not correspond in general to a path modification achievable through the map Q(•), i.e., ( ẋhẋ) / ∈ ℑ(Q(x h )) and ( ẋhẋ) / ∈ ℑ(Q(x)). In view of these considerations, a feedback proportional to ẋhẋ would not give a good information on how well the operator's commands are executed. The approach that we adopt is to map ẋh and ẋ back onto the space of input device configurations R m and only then compute the mismatch between the corresponding configurations. The haptic cue e ẋ is thus

e ẋ = Q(x h ) † ẋh -Q(x) † ẋ = Kq -Q(x) † ẋ (19)
where [START_REF] Meyer | Matrix Analysis and Applied Linear Algebra[END_REF]).

Q(•) † = (Q(•) T Q(•)) -1 Q(•) T and Q(x) † ẋ is the mapping onto R m of the orthogonal projection of ẋ on ℑ(Q(x)) (cf.
Second haptic cue: e x is designed to provide a feedback that indicates how much γ S (x) differs in shape from γ S (x h ) and guides the operator in steering γ S (x h ) towards γ S (x).

To achieve this result, we consider a velocity vector k(xx h ) ∈ R 2n , with k > 0, whose effect is to drive x h towards x (see Fig. 5b). This fictitious velocity vector in general does not correspond to a human input, i.e., k(xx h ) / ∈ ℑ(Q(x h )), therefore we project it onto ℑ(Q(x h )) and then map it to a configuration of the input device. Hence, it is

e x = kQ(x h ) † (x -x h ). ( 20
)
Force feedback: τ is computed from e ẋ and e x as

τ = -B q -K M q -K * (e ẋ -e x ) ( 21
)
where B is a positive definite damping matrix used to stabilize the device, K M is a diagonal non-negative matrix used to provide a perception of the distance from the zerocommanded velocity 8 , and K * a diagonal positive definite matrix of gains. In order to guarantee stability despite the possible presence of non-modeled dynamics, communication delays and packet losses, we make use of the passive setposition modulation (PSPM) approach, a very general and flexible framework for guaranteeing stability (passivity) of the master side and of the closed-loop teleoperation system [START_REF] Lee | Passive-set-position-modulation framework for interactive robotic systems[END_REF]. Let z[k] be the PSPM version of the following signal

z = Q(x) † ẋ -kQ(x h ) † (x -x h ), (22)
that is sampled and sent from the mobile robot to the haptic interface through the (possibly non-ideal) communication channel. Exploiting the PSPM action, the final passive implementation of τ in ( 21) then becomes

τ = -B q -K M q -K * (Kq -z[k]) . (23)
This is sufficient for guaranteeing stability (passivity) of the bilateral system assuming that the human operator behaves as a passive system (see [START_REF] Lee | Passive-set-position-modulation framework for interactive robotic systems[END_REF] for more details).

IV. ALTERNATIVE PATHS IN PRESENCE OF OBSTACLES

The reactive approach used in u a,O achieves collision avoidance but prevents the path from 'passing over an obstacle'. This well known limitation of reactive methods (see e.g. [START_REF] Brock | Elastic strips: A framework for motion generation in human environments[END_REF]) can degrade the operator's capability of steering the path. We tackle this problem by implementing a strategy that generates alternative paths in presence of obstacles and that is based on reactive deformations to allow real time implementation. The underlying idea is that, given an obstacle o and a collision free path γ S (x) between two points 9 , we can find another collision free path γ S (x o ) with the same endpoints and nonhomotopic to γ S (x) (i.e., it cannot be continuously morphed into γ S (x) without intersecting o). For each obstacle o ∈ O, the computation of x o is done in three steps: Crossing: The algorithm starts when the reaction applied to γ S (x) by o is greater than a predefined threshold F > 0, i.e., (Cond. C1) When condition C1 becomes true, let this be at time t 1 , x o is initialized and updated according to the following dynamic system

∂ϕ O ( γ(x,s)-o ) ∂γ(x,s) ≥ F s.t. s = argmin s∈S γ(x, s) -o (24) γ(x o , ŝ) γ(x, s) o R O R O G (a) Crossing γ(x o , ŝ) γ(x, s) o R O R O (b) C2 = true o R O R O (c) Expansion o R O R O (d) Activation o R O R O (e) Switch (C4) o R O R O F (f) C5 = true
ẋo = ∂γ(x, s) ∂x † (xo,ŝ) G o -γ(x, s) o -γ(x, s) x o (t 1 ) = x(t 1 ) (25)
where G > 0 is a parameter and γ(x o , ŝ) is the intersection between γ S (x o ) and the segment oγ(x, s). System (25) 'pulls' the point γ(x o , ŝ) ∈ γ S (x o ) towards and beyond o, as illustrated in Fig. 6a.

Expansion: System (25) is active until γ S (x o ) be- comes non-homotopic to γ S (x) w.r.t. o, i.e., (Cond. C2) (o -γ(x, s)) T (γ(x o , ŝ) -γ(x, s)) o -γ(x, s) 2 ≥ 1 + F c (26)
where F c > 0 is a user defined threshold. Condition (26) is illustrated in Fig. 6b. When (26) becomes true, since γ S (x o ) can be too close to the obstacle o, its evolution switches to

ẋo = - S ∂γ(x,s) ∂x † ∂ϕ E ( γ(x,s)-o ) ∂γ(x,s) xo(t) ds (27)
where ϕ E : R ≥0 → R ≥0 is a smooth distance-based artificial potential function, that is strictly monotonic in [0, RO ] and such that

ϕ E = 0 if γ(x o , s) -o ≥ RO ϕ E → U if γ(x o , s) -o → 0 +
where U > 0 is a fixed parameter. An example for the design of ϕ 0 O is shown in Fig. 4. System (27) 'pushes' γ S (x o ) outside the ball of radius R O and centered in o, as shown in Fig. 6c.

Activation: Once γ S (x o ) is collision free, i.e., (Cond. C3) min s∈S γ(x o , s) -o > R O (28)
the evolution of x o switches to (4) 10 (see Fig. 6d). The alternative path γ S (x o ) becomes active (i.e., x and x o are switched) only when closer to the desired γ S (x h ), i.e.,

(Cond. C4)                x o -x h < x -x h γ(x, s(t)) -γ(x o , s(t)) ≃ 0 . . . d k γ(x, s(t)) d t k - d k γ(x o , s(t)) d t k ≃ 0 (29)
10 Using xo instead of x for the computation of N , u h and ua.

The conditions on the derivatives in C4 are meant to prevent that the change from x to x o causes discontinuities in the trajectory tracked by the robot, so that the initial requirement of a sufficiently smooth trajectory is still satisfied. The switch to the new path is depicted in Fig. 6e. Finally, γ S (x o ) is deleted if the reaction applied to γ S (x) by o becomes smaller than a posistive threshold F < F (see Fig. 6f), i.e.,

(Cond. C5) max s∈S ∂ϕ O ( γ(x, s) -o ) ∂γ(x, s) ≤ F (30)
The algorithm presented here for a single obstacle has been used in practice with multiple obstacles. In particular we allowed the generation of a single alternative paths for each obstacle. Although not complete, this solution was very effective due to the rapidity of the algorithm.

V. EXPERIMENTS

The experimental setup consists, on the slave side, of a single quadrotor which internally implements the attitude and position control laws described in [START_REF] Lee | Semi-autonomous haptic teleoperation control architecture of multiple unmanned aerial vehicles[END_REF] and, on the master side, two force feedback devices (see Fig. 7). In particular, we used one device to command path translations (2 DoF) and the other to command path scalings (1 DoF) and rotations (1 DoF). The proposed framework is delegated to a PC running Matlab and which communicates via wireless ethernet to the low level flight control onboard the quadrotor. Finally, a motion capture system is used to measure the pose of the robot. Since the focus is on testing the action of the framework, the position of obstacles and PoIs is predefined. All the experiments are featured in the attached video 11 .

Experiment 1: In this experiment we assess the contribution of the control term N in (4) by letting the operator command lateral translations (yellow arrow in Fig. 7b) to a straight reference path that is traveled by the quadrotor trying to keep a constant speed. The experiments is executed twice, first without using the control term N and recording the inputs provided by the operator, and then repeating it with the recorded inputs and activating N . Snapshots of the experiment in the two different conditions are presented in Figs. 8a and8b. A simple visual inspection of the snapshots shows that:

1) without N , γ S (x) is identical to γ S (x h ) but the robot cannot track γ(x(t), s(t)) due to the human commands; 11 The video is also available online at http://antoniofranchi.com/videos/trajgen_inthap.html 2) with N , γ S (x) differs from γ S (x h ) and the robot can track γ(x(t), s(t)) quite precisely. This analysis is confirmed by the plot of the tracking error of the trajectory γ(x(t), s(t)) in Fig. 8c. The experiment also shows that, when the projector N is used, the quadrotor has to tilt (roll) less (see Fig. 8d). This reduction in the tracking effort is due to the cancellation of the time derivatives of x that is produced by N , as described in Sec. III-B.

Experiment 2: This experiment showcases the action of the autonomous corrector when the path is steered by the operator in an environment populated by obstacles and a single PoI (a target placed on the floor). A downfacing camera mounted on the quadrotor confirms whether the PoI is actually reached. Snapshot 9a illustrates the initial configuration of the path and Snapshot 9a shows how the path has been teleoperated between the obstacles and the attractive action exerted by the PoI on the upper part of γ S (x). The commands given by the operator and the forces rendered on the input devices are depicted in Figs. 9c and9d respectively. Once the PoI is within the range of action of ϕ R (indicated by a black dashed line), the force feedback guides the operator in steering the path towards the target. Thanks to the automatic attractive force and to the operator's reaction to the force feedback the distance of γ S (x) from the PoI rapidly decreases once it is within the range R R (see Fig. 9e). The video from the onboard camera confirms that the robot actually flies above the target (see Fig. 9f). At same (h) Distance from singularities. Fig. 9: Experiment 2. In snapshots a) and b): overlay of the desired path γ S (x h ) in blue and of the actual path γ S (x) in red, with their corresponding control points x h and x (blue and red squares, respectively) and their velocities ẋh and ẋ (blue and red thin lines, respectively). In c) the limit M on the y axis is equal to 0.8 m/s for translations, 0.1 for the scaling rate and 7 deg/s for the rotation rate.

time, the distance of the path from the obstacles alway stays above the threshold R O , as illustrated in Fig. 9g. Similarly, the minimum distance between the control points and the singular curves decreases when the path is forced between the obstacles but it never reaches zero (see Fig. 9h).

Experiment 3: In this last experiment the algorithm that generates alternative paths is activated to assist the operator who can only command translations. The environment is also populated by three obstacles and the path was chosen so that it is too big to pass between the obstacles. Snapshots 10a to 10d illustrate the various steps in the generation of an alternative paths. Snapshot 10e shows that the algorithm could generate simultaneously multiple alternative paths, one for each obstacle. The commands given by the operator and the forces rendered on the input devices are depicted in Figs. 10f and 10g respectively. Observe that path switches (black dashed lines) cause a jump in the forces due to the switch from x to x o . The effect perceived is similar to stretching an elastic until it breaks. Similarly to Experiment 2, the autonomous corrector ensures that the distance between γ S (x) and the obstacles stays greater than R O , even when multiple obstacles present at the same time (see Fig. 10h.) However, the use of the alternative paths reduces the mismatch xx h , as shown by Fig. 10i. Notice that the error xx h decreases after every path switch.

VI. CONCLUSIONS

This paper introduces a novel framework for semiautonomous path corrections, which allows a human operator to modify the shape of the path traveled by a robot while an autonomous algorithm ensures obstacle avoidance, path regularity and assists the human in reaching meaningful locations. The algorithm is based on continuous reactive deformations and requires limited computational power so that is suitable for real time implementation. The framework also includes a haptic feedback algorithm in which the force is not explicitly given by the motion of the robot but rather depends on the teleoperated path. Experiments with a quadrotor UAV validated the framework. As a future extension of this work, we plan to formally study the stability of the closed-loop system. Furthermore, we plan to include the additional path constraints (e.g., minimum curvature) that arise for specific robots (e.g., fixed wing UAV). Lastly, we are working to use onboard sensing (e.g., cameras) to locate the obstacles.

Fig. 1 :

 1 Fig.1: Example of a B-spline (black line) of degree λ = 3, with 4 control points (colored points). By moving one control point, the B-spline is made non-regular. Top-Left: initial regular B-spline and singular curves (colored lines) of the control points (with the same color pattern). The dashed lines are the singular curves of the fixed control points. Other boxes: the B-spline becomes non-regular when one control point (red one) is moved onto its singular curve.

Fig. 2 :

 2 Fig. 2: Schematics of the framework.

Fig. 3 :

 3 Fig. 3: Examples of canonical path transformations applied to three different kind of paths. Green arrows represent the DoFs. Depending on the transformation the device motion is artificially restricted (continuous green arrows): 2 DoFs for translation, 1 DoFs for scaling, 1 DoFs for rotation. Blue arrows represent the commands and corresponding motion of the control points.

Fig. 4 :

 4 Fig. 4: Example of the artificial potentials ϕ O , ϕ R and ϕ I that are used to compute ua , and of potential ϕ E used in Sec. IV Objective 3. γ S (x) is attracted by nearby PoIs.

Fig. 5 :

 5 Fig. 5: Computation of the haptic feedback. a) First haptic cue, e ẋ. b) Second haptic cue, ex.

Fig. 6 :

 6 Fig.6: Generation of an alternative path: γ S (xo) (green line), γ S (x) (red line), γ S (x h ) (blue line), obstacle o (gray disc). From a) to f), γ S (x h ) is moving from left to right, passing over the obstacle.

Fig. 7 :

 7 Fig. 7: Experimental setup. a): the quadrotor is equipped with reflective markers that are used for the motion capture system. b): the device on the left (Omega.6) commands changes of scale (magenta direction) and rotations (cyan direction), the device on the right (PhantomOmni) commands 2D translations.

  tracking error [m] x N off y N off x N on y N on (c) Tracking error.

Fig. 8 :

 8 Fig. 8: Experiment 1: Projection term N . In snapshots a) and b): overlay of the desired path γ S (x h ) in blue and of the actual path γ S (x) in red, with their corresponding control points x h and x (blue and red squares) and their velocities ẋh and ẋ (blue and red thin lines).

  (a) Snapshot: t ≃ 1s. (b) Snapshot: t ≃ 22s.

  s) -r [m] time [s] (e) Distance from PoI.(f) PoI (onboard camera).

  minS γ(x, s) -o [m] RO RO o1 o2(g) Distance from obstacles.

mini

  Ωi(x) -xi [m] time [s]

  Mismatch xx h .

Fig. 10 :

 10 Fig.10: Experiment 3. In snapshots a)-e): overlay of the desired path γ S (x h ) in blue and of the actual path γ S (x) in red, with their corresponding control points x h and x (blue and red squares) and their velocities ẋh and ẋ (blue and red thin lines). The alternative paths γ S (xo), control points xo and their speed ẋo are drawn in green.

For example, it can be an exploration algorithm planning the next move based on the current partial map.

In practice, the number n of control points is much larger than the number of fully-actuated DoFs (m) because of mechanical limitations while the number n of control points that specifies a path easily reaches the hundreds even in simple cases.

In the case of multiple input devices q, τ and τ h are obtained by stacking in columns the corresponding vectors of each device while M and C are block diagonal matrices.

Under the assumption that a timing law s(t) is provided with its time derivatives.

If this effect is not desired, one can alway disable it by taking K M = 0.

It can also be a portion of a path.