Manuel Yguel

Dizan Vasquez

Olivier Aycard

Roland Siegwart

Christian Laugier

Laugier Yguel

Siegwart Vasquez

Error-Driven Refinement of Multi-scale Gaussian Maps Application to 3-D Multi-scale map building, compression and merging

The accuracy of Grid-based maps can be enhanced by putting a Gaussian in every cell of the map. However, this solution works poorly for coarse discretizations in multi-scale maps. This paper proposes a method to overcome the problem by allowing several Gaussians per cell at coarse scales. We introduce a multi-scale approach to compute an error measure for each scale with respect to the finer one. This measure constitutes the basis of an incremental refinement algorithm where the error is used to select the cells in which the number of Gaussians should be increased. As a result, the accuracy of the map can be selectively enhanced by making efficient use of computational resources. Moreover, the error measure can also be applied to compress a map by deleting the finer scale clusters when the error in the coarse ones is low. The approach is based on a recent clustering algorithm that models input data as Gaussians rather than points, as is the case for conventional algorithms. In addition to mapping, this clustering paradigm makes it possible to perform map merging and to represent feature hierarchies under a sound theoretical framework. Our approach has been validated with both real and simulated 3-D data.

INTRODUCTION

The idea of producing multi-scale grids has been present since the very first works on grid-based representations, [START_REF] Elfes | Occupancy grids: a probabilistic framework for robot perception and navigation[END_REF]. Coarse maps are used in path planning [2,3] or localization [4] algorithms in order to obtain a rough trajectory or position estimate at a low computational cost. Then, at a second stage, this estimate is used to initialize the fine scale search algorithms, thus accelerating convergence. For localization, this procedure also enlarges -in most cases -the convergence region. Examples of algorithms that benefit from such an approach include sampling-based, gradientbased and line-to-point or plane-to-point ICP-based algorithms.

However, as the resolution becomes coarser, the aliasing effect of the geometry of the cells becomes more evident and it can no longer be neglected. When considering a cell as a full block, all the information concerning the shape of the cell contents is lost. A way to alleviate this problem is to attach some sort of statistical shape description to every occupied cell. Two seminal works in this direction are tensor maps [START_REF] Medioni | A Computational Framework for Segmentation and Grouping[END_REF] and the Normal Distribution Approach (NDT) [START_REF] Biber | The normal distributions transform: A new approach to laser scan matching[END_REF], these approaches significantly improve accuracy by approximating the shape of the cell contents with ellipsoids that are encoded as symmetric semi-definite positive (SSDP) matrices. The accuracy of these approaches, together with their relative simplicity has contributed to making them very popular in the map building community [START_REF] Grisetti | Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling[END_REF]4].

That being said, a single ellipsoid is still a poor representation when there are several objects with different orientations in the cell, which is the case -for instanceof a pole standing on the ground (see fig. 1). In this paper, we present a method to overcome this problem by allowing a coarse resolution cell to contain multiple ellip-soids -more specifically, Gaussian clusters. The idea is to start with a single cluster per cell, and then to refine it by inserting additional clusters in order to achieve a balance between representational complexity and accuracy. In particular, from now on, we will assume that there is a given budget of Gaussians per coarse scale that needs to be allocated in an optimal way through a refinement process.

This paper is structured as follows: In the following section, we review related works in robotics and computer graphics. Section 3 provides an overview of our mapping framework. In section 4, we explain how to update a Gaussian map from a range image and how occupancy may be used as a learning rate. Section 5 presents our error-driven refinement algorithm for coarse scales. Section 6 discusses mapping results on simulated and real data sets. Finally, we present our conclusions and outline possible directions for future work.

RELATED WORKS

Related mapping approaches in robotics

An interesting approach to grid refinement are multi-level surface maps (MLS) [8], which can be considered as a refinement of an elevation map. An MLS map is a 2-D grid where, for each cell, several planes are stored at different heights, together with the associated thickness. Their structure makes them particularly well suited to represent traversability information, as shown by their impressive results on real data sets. However, they share the aliasing related problems of 2-D grids particularly in the horizontal plane. Moreover, due to their lack of merging mechanisms they often fail to represent tall vertical structures as a single element if those structures were partially occluded during early observations.

A different approach to cope with cell aliasing is to use a multi-scale grid map which is refined where features are denser. Tree-based representations, such as quadtrees and octrees [9, 10] are the most popular data structures of this kind for two and three dimensional input spaces, respectively. Nevertheless, these structures also suffer from the aliasing problem because of their cubic cell shape, which makes them inappropriate to represent curves or surfaces.

Tensor voting and NDT aim at improving geometric accuracy by representing all the points that fall into a given cell by an ellipsoid, whose orientation and shape are such that the representation error is minimized. In both cases, the ellipsoid is encoded as an SSDP matrix (Fig. 2). In both tensor voting and NDT, having one cluster per cell produces large ellipsoids (called junctions in the tensor voting framework) as soon as the resolution is too coarse and the cell encompasses several distinct objects or the object geometry is not linear. This problem has been addressed in the original NDT paper [START_REF] Biber | The normal distributions transform: A new approach to laser scan matching[END_REF] by storing four overlapping maps shifted by half a cell. However this approach is expensive in terms of the number of Gaussians added and the advantages are unclear when compared to a map with twice the resolution.

Related approaches in computer graphics

In the computer graphics community the problem of 3-D model simplification has received a lot of attention. The objective in this case is to obtain simpler models to streamline manipulation and speed up rendering when high accuracy is not required e.g. when objects are far from the virtual camera or moving rapidly. This is deeply related to refinement as it is, essentially, the inverse problem.

The seminal work in this field is the paper of Garland et al. [11] where edge contraction is performed on the mesh edges that introduce the smallest amount of quadric error in the model. As indicated by its name, this metric is defined by calculating a special type of quadric on the vertices, described by SSPD matrices. The simplification algorithm uses a priority queue. At every iteration, the edge having the lowest error is contracted, the error of all the affected edges is recomputed and they are reinserted in the queue. The process continues until the target budget of edges is reached.

A second class of effective simplification approaches is based on clustering. They can operate either on meshes or on point clouds. Probably the most relevant example of mesh clustering is [12] where clustering is performed on the triangles of the mesh to be simplified. An essential component of this approach is a shape metric that makes it possible to assign each triangle to its closest cluster and to compute the parameters of the cluster. Cohen and Steiner [12] consider two metrics: Garland's quadric error metric [11] and the Euclidean distance between the normals of the triangles and their cluster normals.

In [13], Pauly et al. have studied the simplification of point clouds of the same kind than those obtained with a laser range finder. They describe several agglomerative and partitional approaches, applying techniques proposed in [11].

Our approach is largely inspired by the work of Cohen and Steiner [12] and by the partitional algorithm of Pauly [13]. The main difference lies in the fact that we do not restrict our main representation to surfaces, because at coarse scales many important features such as poles, trees trunks and towers may appear as one-dimensional curves rather than surfaces.

APPROACH OVERVIEW

Our approach processes every scale in parallel, adjusting the field of view of finer scales in order to have a similar number of cells to update for each scale. As shown in Fig. 3, the approach is composed of three main components:

. Fig. 3 Framework components. Light gray boxes are processed less than once per range image.

1. Occupancy computation: the occupancy is a measure of how often a particular cluster has been observed. In our framework is has two uses. First, it determines the plasticity of a cluster, allowing it to adapt faster in regions that have not been observed often. Second, it is used as a criterion to delete clusters, making it possible to remove dynamic objects from the map. 2. Map updating: it adapts existing clusters in order to minimize the representation error. It is similar to the standard update of conventional Gaussian maps, except that it takes into account the fact that a cell may contain several Gaussians. 3. Map refinement: this step is our main contribution, at every refinement step new Gaussian clusters are added to the cells where the representation error is maximum. It is worth noting that no refinement is performed on the finest scale.

MAP UPDATING

This section presents the procedure to update an existing map from sensor data. At this point, we assume that the number k of Gaussian clusters per cell is known. The actual estimation of k is handled by the refinement algorithm that we will discuss in § 5.

Our goal here is to update the Gaussians' mean value µ and covariance Σ in order to minimize the representation error with respect to the input data. Every point in the range image is used to incrementally update the different scales independently. As we will explain in § 4.3, the basic idea is to find the cell where the input point falls and then updating the cluster in that cell that is "closest" to the input point.

As in most incremental approaches, an important question is how much to adapt the clustersi.e. finding the 'right' learning rate. In the following subsection we describe the use of the cluster's occupancy to control the adaptation. It can be intiuitively explained as follows: the more a cluster has been observed, the more is known about it and the less reasonable it is to modify it. As we will see in § 5, occupancy is also used as a criterion to filter out dynamic objects from the map.

Computing cluster occupancy

Occupancy can be seen as a counter associated to every object. Its value gets increased when the object is visible in the range image, and decreased when the object is supposed to be visible but is in fact not. Fig. 4 illustrates the idea: if point C, is visiblei.e. the dashed red line is free from obstacles between I and C -then the value of the range image at cell I will correspond to the distance r C . If, on the other hand, the value of cell I is greater than r C , this can be considered as evidence that C is not there anymore and its occupancy should be decreased.

For visible cells, we compute occupancy in a per point basis. The occupancy of a point, C, in the map is given by comparing the range measured in the pixel of its projection, I, in the range image with its actual range, δ . For a Gaussian, the occupancy is obtained by averaging the occupancy of n points sampled from the Gaussian distribution (rejecting those that fall outside the cell).

We only need to guarantee that there are enough samples to provide a good estimate. To define n, we compute an upper bound of the number of points in the range image that can be contained in the cell. This is done using the projected bounding sphere of the cell. Let δ min and δ be the distance to the image plane in the camera coordinate system and the distance to the center of c (fig. 4), respectively. Then the projection of the bounding sphere of c occupies an area of 3π 4 Ä δ min δ a ä 2 (orange disc in Fig. 4), where a is the length of the side of c. Knowing the area of one pixel of the range image p, an upper bound for the number of pixels that may be projected back into the original cluster is:

B ⌈ 3π 4p Å δ min δ a ã 2 ⌉ (1)
which is the number of samples we are looking for. So, making n = B gives us a good chance to cover every range image cell that effectively contains an observation from the cluster.

Hierarchical culling

In order to minimize computation, we perform hierarchical visibility culling. Consider the cell c centered in C in (fig. 4). If the projection of the bounding sphere of c is outside the range image (camera field of view, blue lines in fig. 4), the finer children cells of c are not further explored. The search is also finished if all the ranges observed in the disc of the projected bounding sphere (orange disc in the image plane) are smaller than the smallest possible range for the cell, meaning that all the cell content is occluded by closer objects.

Updating Gaussian clusters from data

For every input point, a single cluster per scale will be updated. The cluster is selected by finding the cell that contains the input point and then finding the cluster having the minimum distance to that point.

Once the cluster has been selected, its parameters are updated by means of a stochastic gradient descent algorithm. The principle is to update the reference vector by a fraction of the negative gradient with respect to each input datum. As more and more samples are processed, the magnitude of the adaptation should decrease (typically faster than 1/n) to ensure convergence. A good example is the on-line computation of the sample mean:

µ n = µ n-1 + 1 n (z n -µ n-1 )
where n represents the name of samples processed so far, and z nµ n-1 can be understood as the negative gradient, and 1 n the fraction of the gradient to be taken into account. This decreasing weight is called the learning rate and is noted ε. In our approach, the value of ε depends on the occupancy, as described in § 4.4.

In the case of points, a distance metric between a point and a Gaussian should be used. We have chosen to use the probability measure given by (2):

d(p, w) 1 2 î (p -µ w ) T Σ w -1 (p -µ w ) + log (det(Σ w )) ó , (2)
This distance is the addition of the Mahalanobis distance and a volume term. Compared to the pure Mahalanobis distance, the volume term aims at compensating the fact that the Mahalanobis distance of a big cluster tends to make every point very close. This measure has the advantage of yielding simple map update rules, since its derivative is:

∂ d(p, w) ∂ µ w = -Σ w -1 (p -µ w ) (3)
and

∂ d(p, w) ∂ Σ w ∝ -(p -µ w )(p -µ w ) T -Σ w , (4)
giving the following gradient descent algorithm for point-based update:

Learning rate

Our idea is to define the learning rate from the occupancy: the higher the occupancy of a cluster, the better the accuracy of its position and shape is supposed to be; thus, a small value of ε should be used. If, on the other hand, the occupancy is low, the current estimated state of the reference vector can be assumed to be based on insufficient statistics and the learning rate should be high to permit the reference vector to adapt itself.

In log-ratio the occupancy typically is bounded in [-o max , o max ] and the learning rate varies within [ε min , ε max ]. For our approach we have chosen a linear mapping between both values:

ε(o) = ε min -ε max 2o max (o + o max ) + ε max (5)
In our experiments, we have set o max = 10.0, ε max = 5 • 10 -2 and ε min = 5 • 10 -4 .

ERROR-DRIVEN REFINEMENT OF COARSE SCALES

The refinement process is driven by a measure of the representation error. The map is refined by inserting a new cluster in the cell that has the maximum error. After every insertion, the shape of the other clusters in the same cell should be modified accordingly; this is done by running a clustering algorithm using the cells of the finer scale as input.

We periodically refine the map by adding a fixed number p of Gaussian clusters at a time. In order to choose the p vectors that have the maximum error without sorting the whole set of reference vectors, we use a priority queue of size p as was done in [11]. The following subsections provide the details of the refinement algorithm: the error metric used to build the queue is introduced in § 5.1 and § 5.2 presents the clustering algorithm.

During the mapping process it is often necessary to delete clusters that correspond to moving obstacles; this process is described in § 5.3. Finally, the application of our approach to map merging and simplification is discussed in § 5.4.

Error computation

We aim at adding clusters only in those regions where the Gaussian shapes have already converged to their final shapes, which can be deduced from its occupancy. Accordingly, we choose to refine a cell c only if the average occupancy probability of the finer cells in φ (c) is above 0.5. Furthermore, we only refine those parts of the map that are visible for the sensor.

To find the cell to refine, we compute an error value per cell. This value is basically the sum of the Mahalanobis distance between the center of the coarse cluster and the Gaussian cluster of the finer scale.

For the cells c s of the coarse scale, s, having reference vectors (i.e. mean values) {w 1 , . . . , w k } and finer data at s -1: {z 1 , . . . , z N } ∈ G(φ (c s )), we compute the average distance of each datum to its closest reference vector: where δ (w i , z j ) is one if w i is the closest reference vector to z j using the Mahalanobis distance defined by z j and zero otherwise. The occupancy is used through the learning rate to assign higher error weights to occupied clusters, disregarding those whose occupancy is low and, in consequence, whose accuracy may still improve without the need of adding extra clusters.

E (c s ) = 1 N k i=1 N j=1 (1 -ε z j )δ (w i , z j ) (µ w i -µ z j ) T Σ -1 z j (µ w i -µ z j ) , (6)

Clustering for map refinement

Algorithm 2 describes our clustering approach for map refinement. This method solves a hard clustering problem: we are looking for a partition C * = {C * 1 , . . . ,C * k } of the G(φ (c s )) into k classes represented by k reference vectors that minimizes the clustering distortion:

E (C * ,{w * 1 ,...,w * k }) = arg min {C 1 ,...,C k },{w 1 ,...,w k } k i=1 E C i (w i ) (7)
This is done by using the well known k-means clustering algorithm [14].The optimal clusters are computed iteratively from the set of reference vectors : each datum is associated to its closest reference vector; then, the minimizer of each cluster energy is computed. In the basic Lloyd algorithm, both input data and reference vectors are simply points in feature space (3-D space in our case) Z = F = R 3 and the distance function, d Z ×F (z, w) = w iz 2 is the square Euclidean distance.

Algorithm 2 Map refinement using hard clustering Z = {z j | j = 1, . . . , N} ← the N Gaussians of the fine scale s 2: A = {α j | j = 1, . . . , N} ← the non negative weights of the fine Gaussians

W = {w 0 1 , . . . , w 0 k }[k -1] ← the k -1 Gaussians of the coarse scale s + 1 4: d Z ×F (•, •) ← the distance function W 0 ← {w 0 1 , . . . , w 0 k }[k -1] {z 0 max } // Init. with the data of maximum distortion 6: {(C 0 i , w 0 i )|i = 1, . . . , k}, E W 0 ← kMeans(Z, A,W 0 , d Z ×F ) //
W t+1 ← (W t \ {w c min }) {z max(cmax) } else 20: c ←∼ U ([[1; k]] \ {c max })// Draw a random candidate W t+1 ← (W t \ {w c }) {z max(cmax) } 22: end if {(C t+1 i , w t+1 i )|i = 1, . . . , k}, E W t+1 ← kMeans(Z, A,W t+1 , d Z ×F ) 24: t ← t + 1 until E W t > E W t-1 // Accept the swap if the clustering distortion decreases 26: return {(C t-1 i , w t-1 i )|i = 1, . . . , k}, E W t-1
An important drawback of k-means is that is highly dependent on initialization. Moreover, even if the algorithm is guaranteed to converge, it often gets stuck in local minima of E W * . To get out of the local minima a so called "swap" procedure is used (line 7 to 25, alg. 2). One cluster is chosen, either randomly or because of its short distance to a different cluster with more elements. Then, a simulation is done by reallocating that reference vector to the region of space with maximum error. If the resulting partition has a lower clustering distortion, the result is accepted and a new simulation is done. Otherwise, the result is rejected and the procedure stops. A reference vector metric is used to evaluate the similarity between clusters:

d F : (F × F) → R + . If Z = F it is possible to use d F = d Z ×F ,
to compute both the distance between clusters and the distortion between a datum and its corresponding cluster (line 16, alg. 2).

It is worth noting that this clustering framework naturally defines a hierarchy: a cluster is the parent of all the clusters of the finer scale that are closer to it than to any other cluster (see fig. 6).

K-Means extension for Gaussian inputs

In order for the covariance matrices of the clusters at the coarse scale to be as accurate as possible, we need to use the information provided by the covariance matrices at the finer scale. Therefore, we need a clustering algorithm that is able to properly handle Gaussian input data Z = F = G 3 {(µ, Σ)|µ ∈ R 3 and Σ is SDP} 1 . Davis [START_REF] Davis | Differential entropic clustering of multivariate gaussians[END_REF] has proposed such an algorithm, proving that it converges. The algorithm uses the Kullback-Leibler divergence (Eq. 8) as a distance function d Z ×F for Gaussians:

D KL (z w) = 1 2 ï (µ z -µ w ) T Σ -1 w (µ z -µ w ) + log Å det Σ w det Σ z ã + Tr Ä Σ z Σ -1 w ä -d ò (8)
where d is the dimension. The metric is composed of three terms corresponding to the Mahalanobis distance, the volume and the orientation, respectively. The use of this metric in clustering means that the decision of grouping fine scale clusters together does not only depend on their positions, but also on their sizes and orientations. This property is particularly important for mapping, since it will tend to preserve sharp features such as corners and edges because the distance between the linear components of such features will increase with the angle difference between them.

As explained in [START_REF] Davis | Differential entropic clustering of multivariate gaussians[END_REF] the computation of the optimal reference vectors from a set of Gaussians {z j = (µ z j , Σ z j )| j = 1, . . .} weighted by positive reals (α z j ), is done in two steps:

First the optimal mean is computed using (9):

µ * = 1 j α z j j α z j µ z j , (9)
then the covariance matrix is given by (10):

Σ * =   1 j α z j j α z j (Σ z j + µ z j T µ z j )   -(µ * ) T µ * . ( 10
)
It is interesting to remark that, if the weights are defined as the number of samples used to compute the Gaussians at the fine scales, then the optimal Gaussian is given by the sample mean and covariance of the fine samples that compose the cluster.

Cluster deletion

In order to account for dynamic objects that have been detected once and that are not there anymore, we delete those clusters whose occupation has fallen below a given threshold. As for cluster insertion, the remaining Gaussians are adjusted by running the clustering algorithm. It should be stressed that, for the sake of consistency, cluster deletion at a given scale should only happen when no corresponding clusters exist at the finer scales.

Map merging and simplification

Now, we explain how to merge two maps whose cells contain multiple Gaussian clusters. This is a form of map simplification since the goal is to delete redundant cluster centers after the union of maps.

Merging is performed through straightforward application of the clustering algorithm to every cell that appears as occupied in both maps. In order to fix the number of clusters for a given cell, we select the maximum number in the two maps. This, of course, can be later refined as described above. Thus, the main problem is the initialization of the clustering algorithm.

The idea is to take all the clusters of both maps, then to compute the inter-cluster divergences as in line 14 of Alg. 2. From there, the algorithm proceeds by replacing by a single cluster, the pair of clusters that are separated by the smallest distance, and then starting over until the target number of clusters is reached. This is done using equations 9 and 10. The procedure is very efficient because no access to the finer scales is required. After finishing the merging step, a run of the clustering algorithm is executed to smooth out the result.

It is worth noting that the same process can be used to simplify the map when a reduction in the number of clusters is required.

RESULTS

We use the Hausdorff distance to evaluate the results. This metric may be seen as the worst-case distance. For two sets of points, it is obtained by computing, for every point in one set, the distance to closest point in the other set, and then taking the maximum of these distances. In our case, we compute this distance by sampling Gaussians and rejecting points that fall outside the cells. The samples are directly measured against point clouds in the case of real data. In the case of simulated data, the ground truth is available in the form of the original mesh; the corresponding points are obtained by representing each triangle with a number of samples. In all cases, the number of samples is proportional to the size of the object being considered: the volume of the ellipsoids in the case of Gaussians and the area in the case of triangles. For each data set we use 3 scales; at each scale the cell side is ten times larger than the finer one. For the 2-D data set (fig. 7) the finest side is 5 cm and for the 3-D data set the finest side is 10 cm. Regarding the algorithm approach modules (Fig. 3) we set the occupancy computation to take place at acquisition rate for two dimensional data and every three acquisitions for the three dimensional case. As for refinement, the algorithm has been configured to insert 4 clusters of the remaining Gaussian budget, every 10 acquisitions.

The results we have obtained on real and simulated data sets exhibit similar advantages and drawbacks:

• Advantages:

-Accuracy vs map size: the method is able to significantly improve accuracy with a relatively small increase in the size of the model. In our experiments increasing the number of clusters by four leads to a factor of three reduction of the Hausdorff distance. -Multi-scale error reduction: the huge size reduction ratio (10 4 to 1 in 2-D and 10 8 to 1 in 3-D) between the finest and the coarsest scales is kept by the refinement process, while considerably improving accuracy. For instance, when refining the coarsest map in the 3-D data set, we increase the number of clusters from 53 to 181 and reduce the error by 3. Note that large flat clusters remain (in particular on the ground and on the walls ) while a lot of detail clusters are added at poles and corners (fig. 1). This could not have been done by simply adding an intermediate scale.

• Drawbacks: the main problem we have detected is that, sometimes, the Hausdorff distance does not have a significant decrease when a cluster is added. We believe that there are two reasons for this: first, an aliasing phenomenon that arises from the fact that the underlying cells force the algorithm to artificially cut a big object in pieces, some of which can be very small with respect to other Gaussians in the same cell, leading to big error contributions because of the small size of the covariance. The second reason is that, when the 'right' number of clusters is not yet reached, the resulting Gaussian may represent two separate clusters, yielding a smaller but still important value for the Hausdorff distance.

Conclusions and Future Work

In this paper we have proposed a comprehensive framework to build two and threedimensional maps from range data. The proposed representation enhances the accuracy of previous approaches by enabling the presence of several Gaussians per cell. These Gaussians are added by means of a refinement algorithm which inserts them where the representation error is maximum. The algorithm makes use of a recent Gaussian clustering approach that uses the Kullback-Leibler divergence as a distance function, thanks to this, our algorithm is able to preserve important features of the environment (e.g. corners) that are usually smoothed out by other approaches.

The framework provides a theoretically sound foundation for map merging. In order to deal with moving objects and noise, our approach makes use of occupancy to determine when to delete parts of the map that have become empty, as well as to adjust the plasticity of the map. Experiments with real and simulated data show that, for coarse scales, significant accuracy gains may be obtaining by a small augmentation in the number of clusters. Moreover, when compared with existing approaches, the additional computational cost that is required to insert these clusters is marginal. Further work includes working towards real time mapping of huge streams of 3-D points by exploiting parallelization and hierarchical multi-scale update. Middle term research will be directed to exploring the application of our approach to higher dimensional spaces that include point properties such as color.

Fig. 1

 1 Fig. 1 Top left: simulated scene. Top right: fine scale of the map. Bottom left: coarse scale of the map, one Gaussian per cell. Bottom right: coarse scale of the refined map

Fig. 2

 2 Fig.2Decomposition of an SSDP matrix ellipsoid into elementary shapes called tensors in[START_REF] Medioni | A Computational Framework for Segmentation and Grouping[END_REF].

Fig. 4

 4 Fig. 4 Computation of occupancy in the range image of the bounding sphere of a cell.

Algorithm 1

 1 Map update with points: pointUpdate {w 1 , . . . , w k } ← the k Gaussian reference vectors of the cell 2: {ε j |i = 1, . . . , k} ← the associated learning rates z = p ← the observed point 4: n ← arg min i=1,...,k d(z, w i ) µ wn ← µ wn + ε n (p -µ wn ) 6: Σ wn ← Σ wn + ε n (p -µ wn ) T (p -µ wn ) -Σ wn

Fig. 5

 5 Fig. 5 Up: fine scale is colored with the magnitude of the contribution to the error at the coarser scale. Down: coarse scale, mean error. Error palettes are on the right.

Fig. 6

 6 Fig. 6 Cluster hierarchy. Up: coarse clusters are translated for better display.

Fig. 7

 7 Fig.7Real 2-D dataset from[START_REF] Stachniss | Corrected robotic log-files[END_REF]. The coarsest (blue ellipsoids) and finest (red ellipsoids) maps are displayed. Black squares represent the coarse cells. All ellipsoids correspond to the covariance matrices plotted with three times the square of the eigenvalues.

SDP matrices are a subset of SSDP matrices, meaning that analysis tools such as those proposed for NDT[START_REF] Biber | The normal distributions transform: A new approach to laser scan matching[END_REF], tensor voting[START_REF] Medioni | A Computational Framework for Segmentation and Grouping[END_REF] and quadric error [11] approaches, may be applied to them.


Arturo Escobedo

Anne Spalanzani

Christian Laugier

Experimental Setup for Human Aware Navigation

Keywords: robotic wheelchair, human centered robotics, human aware navigation, assistance robotics, shared control, Bayesian inference

Validation of algorithms developed by assistance robotics research on real platforms is essential. Producing robotic architectures that promote scientific advances while regarding usability for the final user is a challenging issue where an appropriate trade-off between both requirements must be found. This paper proposes a new framework for the development of mobility assistance techniques to improve the quality of life of elder population using a robotic wheelchair. An example of improvement of the wheelchair navigation is presented. The control of the wheelchair is done using a Bayesian semi-autonomous approach that estimates the user intended destination from input commands given through a Kinect sensor. Safe and comfortable autonomous navigation is performed using a probabilistic navigation method that takes into account the dynamics of the environment and human social conventions.

I. INTRODUCTION

The aging of world's population is bringing the need to provide robotic platforms capable to assist elder people to move. These robots can be vehicles or wheelchairs and is necessary that such transportation is reliable, safe (at least as much as a human) and comfortable.

Patients and medical staff have a strong desire for the services that a smart wheelchair can offer [START_REF] Fehr | Adequacy of power wheelchair control interfaces for persons with severe disabilities: a clinical survey[END_REF]. Some users cannot use a normal power wheelchair because they lack the required motor skills, strength, or visual acuteness.

When using a robotic wheelchair, the occupant must feel that this mode of travel is tailored to its needs. The vehicle or wheelchair must meet specific needs: those of people with disabilities or reduced mobility or just those of people without disabilities but who want a service of comfort .

No matter what the mobility assistance device is (car, wheelchair, walking aid...), navigation in human environments is a central problem. If one aims to develop a robotic device, it must combine many technologies proposed in the robotic domain: perception, prediction, fusion, navigation, control, but also must integrate social conventions knowledge and a way to share the control with the user in order to guarantee a safe navigation while avoiding frustration due to the disregarding of the user desires by the autonomous navigation system. † {jesus.escobedo-cabello, anne.spalanzani, christian.laugier}@inria.fr This work has been supported by INRIA large scale initiative action PAL (Personal Assisted Living) http: \\pal.inria.fr.

In this article, an architecture combining all these technologies and some experiments on a robotic wheelchair are presented.

Section II presents related work on automated wheelchairs. Part III describes the system architecture with a focus on the human intention estimation process and human aware navigation method. Sections IV and V present the experimental platform and section VI details the results. Conclusions are discussed in section VII.

II. RELATED WORK

The growing interest in producing an autonomous wheelchair to assist elder people mobility has led to the development of many different wheelchair platforms [START_REF] Simpson | Smart wheelchairs: A literature review[END_REF]. Most of the presented wheelchairs operate in a manner very similar to autonomous robots; the user gives the system a final destination and supervises as the smart wheelchair plans and executes a path to the target location (e.g.,NavChair [START_REF] Levine | The NavChair Assistive Wheelchair Navigation System[END_REF], MIT Media Lab wheelchair [START_REF] Kollar | Grounding Verbs of Motion in Natural Language Commands to Robots[END_REF]).

Other smart wheelchairs confine their assistance to collision avoidance and leave the majority of planning and navigation duties to the user. These systems do not normally require prior knowledge of an area or any specific alterations to the environment. They do, however, require more planning and continuous effort on the part of the user and are only appropriate for users who can effectively to plan and to execute a path to the destination. A final group of smart wheelchairs offers both autonomous and semi-autonomous navigation (e.g., VAHM [START_REF] Pruski | VAHM: a user adapted intelligent wheelchair[END_REF], Sharioto [START_REF] Vanhooydonck | Shared control for intelligent wheelchairs: an implicit estimation of the user intention[END_REF], SmartChair [START_REF] Parikh | Usability study of a control framework for an intelligent wheelchair[END_REF]).

Whenever two or more entities aim to work together they must be able to communicate their intentions to each other. When a human is driving an automated wheelchair, the explicit communication of users plans is not always possible which leads to the development of techniques to estimate the user intention implicitly. The implicit estimation of the user intention provides an easier control of the robotic system (wheelchair) for user's who cannot give accurate or fast commands due to its handicap. Some methods are presented in [START_REF] Demeester | Assessing the user's intent using bayes' rule: application to wheelchair control[END_REF], [START_REF] Demeester | Bayesian Estimation of Wheelchair Driver Intents: Modeling Intents as Geometric Paths Tracked by the Driver[END_REF] [START_REF] Taha | POMDP-based longterm user intention prediction for wheelchair navigation[END_REF].

Recent works have focused on the understanding of social interactions between humans to provide socially accepted movement to increase the comfort for the user [START_REF] Kirby | Companion: A constraintoptimizing method for person acceptable navigation[END_REF], [START_REF] Rios-Martinez | Probabilistic autonomous navigation using rist-rrt approach and models of human interaction[END_REF].

III. SYSTEM ARCHITECTURE

When performing robotics research, often the scope of the investigation is limited to a well-defined area of the system, such as a software module which performs some type of planning, reasoning, perception, or control. However, to get a robotic system up and running for experiments, a much larger software ecosystem must exist. Fig. 1 presents an overview of our systems architecture. It is divided into several subsystems, some of them are being developed by our team while others were taken from external sources to perform necessary tasks that are not crucial for our research domain.

• User Intentions Estimation: The user intention subsystem estimates the desired goal within the map of the environment among a list of possible predefined goals. Those locations can be previously selected by an expert caregiver, the user, or learned by the system using machine learning techniques. The user intention estimation computes the probability for each typical goal given the current position of the wheelchair and the user command and then selects the goal with the highest probability. The computation of probabilities is done using a Bayesian network approach.

• Tracking: The off-board tracker provides global information about moving obstacles which is the learning input for our motion prediction module. It is built as a conventional detect-then-track system. The tracking subsystem is also necessary to identify the interactions between people (e.g. two persons involved in a conversation).

• Prediction: Processes data from the trackers and transforms it into probabilistic predictions about the configuration of the free space in the future environment.

The motion prediction subsystem takes tracking data (i.e. position, orientation and velocity) and outputs grids, representing the posterior probability of the space being occupied at a given time step in the future. Prediction itself is accomplished with a Growing Hidden Markov Model (GHMM) [START_REF] Vasquez | Growing hidden markov models: a tool for incremental learning and prediction of motion[END_REF] and an Extended Kalman Filter. • Social Filter Detects social interactions and creates virtual obstacles corresponding to those interaction zones. In order to produce socially acceptable motion, it has been proposed the 'Social Filter', which integrates constraints inspired by social conventions in order to evaluate the risk of disturbance and take it into account when making the autonomous navigation planning. We focus on detecting and predicting conversations in the environment surrounding the wheelchair [START_REF] Rios-Martinez | Probabilistic autonomous navigation using rist-rrt approach and models of human interaction[END_REF]. • Motion Planning: The navigation subsystem includes a laser-based localization module and a motion-planner which integrates predictions to compute safe trajectories that are fed to the execution module. The motion planner is based on Risk-RRT [START_REF] Fulgenzi | Probabilistic motion planning among moving obstacles following typical motion patterns[END_REF], a partial motion planner which integrates motion predictions to provide safe trajectories. This algorithm was thought to operate in dynamic, uncertain environments, it supposes that the moving pedestrians detected in the environment follow typical motion patterns that are represented by Growing Hidden Markov Model (GHMM). This motion planner generates human friendly paths respecting people's per-

IV. EXPERIMENTAL PLATFORM

The proposed experimental setting is shown in figure 2. The main entity is the robotic wheelchair with all the onboard sensors and computer. The scenario proposed for the experiments is a human populated environment where people can be moving and interacting. Those persons can be tracked using the camera mounted on the top of the scenario. A remote computer is in charge to send the tracking information to the wheelchair.

A. Visual Tracking System

A camera mounted over the scenario is used to track the present people. A marker based visual tracking system [START_REF] Lamb | Artoolkit website[END_REF] is used to accurately track the position and orientation of special marked cards. In order to track the people in the experimental scene they wear markers on their heads as seen in figure 3.

B. Wheelchair

The equipment used is the robotic wheelchair shown in figure 4 that consists of a mobile base equipped with the seat, all the on-board electronics and different attached devices. Sensors on-board the wheelchair consist of a LIDAR (Light Detection and Ranging) model SICK LMS-200, wheel based quadrature encoders for odometry measurements and The mobile base has a rectangular footprint with dimensions 0.56 m long by 0.67 m wide, however, the dimensions including the attached seat and protections is of 1.48 m height, 1.08 m long by 0.67 m wide. The autonomous wheelchair can carry a maximum payload of 150 kg with a maximum nominal linear velocity of 1.39 m/s and a maximum rated angular velocity of 1.5 rad/s. Its maximum acceleration is rated at 1.35 m/s 2 .

V. SOFTWARE ARCHITECTURE

The system architecture has been developed using the Robot Operating System (ROS) [START_REF] Quigley | ROS: an open-source robot operating system[END_REF]. ROS is used to get some functionalities like hardware abstraction, low-level device control, message-passing between processes and package management.

The fundamental concepts of the ROS implementation are nodes, messages, topics, and services. Nodes communicate with each other by passing messages. A message is an strictly typed data structure. Messages can be composed of other messages, and arrays of other messages, nested arbritrarily deep. A node sends a message by publishing it to a given topic. The nodes that are interested in a certain kind of data will subscribe to the appropriate topic. There may be multiple concurrent publishers and subscribers for a single topic, and a single node may publish and/or subscribe to multiple topics. In general, publishers and subscribers are not aware of each others existence.

The Diagram of the processes running in the remote and on-board computers are displayed in figure 5 and6 where each process/node is shown as an ellipsis while the topics are rectangles. Nodes that are developed by our team are presented in grey.

All our implementation uses a publisher/subscriber communication paradigm where each nodes works at its own frequency. Due to that, it is necessary to validate input data timing in some nodes in order to avoid receiving too old information. This validation is done by posting a time stamp on each message so that the subscriber can always know when the received data was published and decide whether it is good or not.

ROS currently supports TCP/IP-based and UDP-based message transport. The TCP/IP-based transport is known as TCPROS and streams message data over persistent TCP/IP connections. TCPROS is the default transport used in ROS and is the only one used in this work.

In figure 6, it can be observed the transformation system, which is the service in charge of the tracking of spatial relationships as for example those between the mobile robot This system is called tf. The tf system constructs a dynamic transformation tree which relates all frames of reference in the system. As information streams in from the various subsystems of the robot (joint encoders, localization algorithms, etc.), the tf system can produce streams of transformations between nodes on the tree by constructing a path between the desired nodes and performing the necessary calculations.

The relationship between coordinate frames are maintained in a tree structure buffered in time, so that it is possible to transform points, vectors, etc between any two coordinate frames at any desired point in time.

Nodes that need to add some reference frame to the tree use a broadcaster which is represented as the doted lines going from any node to the transformation server in figure 6. Listeners are services that get the values of a given transformation between two reference frames (doted arrows from tf server to any of the other nodes).

VI. EXPERIMENTAL RESULTS

The system was evaluated at INRIA's hall shown in figure 3. Two possible scenarios were considered. In the first one the wheelchair moves in an static environment with no humans just to test the performance of the user intentions estimation algorithm. The second one considers the presence of humans moving and interacting around the wheelchair.

At the beginning the user is asked to do the calibration of the Kinect used as input device. This allows the Kinect Fig. 7. A user pointing to his desired destination. The idea is to be able to understand which are the most probable goal given the direction of the gesture Fig. 8. Typical user destinations in this scenario are marked with an small arrow tracker to retrieve the information relating the links of the user skeleton (head, neck, torso, shoulders, elbows and hands).

The wheelchair is located in an initial position, and the user starts the movement by pointing towards his intended destination as shown in figure 7. The commanded direction is read as the angle formed by the torso and the right hand of the user. A set of typical destinations were dened into the map as shown by the small arrows in figure 8. When the command is read the user estimation module computes the goal with the highest posterior probability and send it to the navigation module.

The navigation module receives the map of the environment, the currently computed goal, the list of people present in the scene and plan its trajectory. While moving it maintains a path with the highest probability of success to reach the goal and computes the velocity commands (linear velocity, angular velocity) that is sent to the wheelchair. Figure 9 shows the path followed by the wheelchair when there are no dynamic obstacles or humans in the scene. The rectangle is the footprint of the wheelchair.

In the second scenario two persons are talking and they are positioned in the middle of the path between the wheelchair and the current estimated goal. The user points to the goal that is located just in front of him to select the desired position and he/she dont have to worry about the necessary planning and commands to avoid interrupting the conversation because it is the autonomous navigation system the one that takes this responsibility. As it can be seen in figure 10 the space between the two persons is big enough to let the robot to pass by if the social interaction cost is not considered.

Standard navigation algorithms would have disturbed the conversation, producing an uncomfortable situation for both the user and the people around him.

VII. CONCLUSION AND FUTURE WORK

The proposed experimental platform is an ongoing effort and the results we have obtained should be considered preliminary. The most important results up to this point are:

• Tracking: We are still developing and testing different tracking methods. Meanwhile, we have performed several tests using augmented reality markers (wore as hats). This has allowed us to validate the overall architecture, even if it is not a viable solution in the long run. • Prediction: The proposed prediction algorithms have been extensively validated and compared about other state of the art techniques [START_REF] Vasquez | Growing hidden markov models: a tool for incremental learning and prediction of motion[END_REF]. Our approach consistently yields comparable predictions with much smaller models and is able to update its knowledge as new motion patterns are observed. • Planning: The RiskRRT algorithm has been extensively tested in simulation. It is now implemented in our real platform, where it will be tested against similar approaches to assess the actual impact that integrating risk estimation and trajectory prediction has in terms of safety.

• Socially acceptable behavior is very important. Even in our scripted tests, both interacting people and the wheelchair's user reported that they felt very uncomfortable when the robot passed right through the middle of a talking group. • Predictive behavior and socially acceptable behavior are often similar. The use of the social lter and prediction module as part of the navigation algorithm increases the user comfort by avoiding embarrassing situations as disturbing people around him and even more it avoids dangerous situations as crashing due to its capability to avoid even dynamic obstacles. For example, when pedestrians were passing through the robot's path, it often happened that it stopped (knowing that the path was going to be free) to let the person pass. This seems to indicate that in many cases, knowing how people will move, the most reasonable thing to do is to be polite. It also suggests game theory as a possible way to analyze these interactions. • User intentions estimation. The user intention algorithm has proven to be useful to translate the input commands taken from typical input devices into high level orders (goals). Even in the cases when the system cant decide accurately which is the intended goal, as this is a probabilistic approach we still can get useful information from the system in order to assess the amount of uncertainty in the estimation. This information can be used to evaluate the need to ask to the user for some extra information in order to solve the ambiguity. In order to work in a non-supervised environment a sophisticated/accurate user intention algorithm combining machine learning techniques is desired in order to add the capability to adapt autonomously to the user disability.

• Kinect as input device. Using the Kinect as input device can be good for the elderly because it provides a more natural way of interaction when giving directions (pointing to the desired direction is more natural than trying to control the wheelchair using a joystick) so they can be more confident when using the wheelchair. Validation of the method with other user-machine interfaces would be useful to take into account people who does not have the necessary strength or motor skills to move their arms.

Fig. 1 .Fig. 2 .

 12 Fig. 1. System architecture overview.

Fig. 3 .Fig. 4 .

 34 Fig. 3. Three tracked persons interacting at INRIA's hall

Fig. 5 .

 5 Fig. 5. Diagram showing the communication between the different nodes running in the remote computer (connected to the camera) The rectangles are topics where information is exchanged between two or more nodes (ellipses). Grey elipsis are the nodes being developed by our team.

Fig. 6 .

 6 Fig. 6. Diagram showing the communication between the different nodes running in the on-board computer. The ROS tf system is in charge to compute the transformations between different reference frames

Fig. 9 .Fig. 10 .

 910 Fig. 9. Results obtained with the wheelchair moving without any obstacle in the path


Arturo Escobedo

Jorge Rios-Martinez

Anne Spalanzani

Christian Laugier

Context-Based Face Control of a Robotic Wheelchair

Keywords: Shared control, Bayesian network, semiautonomous navigation, wheelchair control, face pose

In this article a method to perform semiautonomous navigation on a wheelchair is presented, contextual information from the environment as user's habits and points of interest are employed to infere the user's desired destination in a global map. Illogical steering signals comming from the usermachine interface input are filtered out to improve the overall performance of the system. Examples using a face tracking system and voice recognition are presented. The estimation of the user's desired goal is performed employing a Bayesian network. An autonomous navigation system is used to control the wheelchair's low level navigation while the user is just concerned at pointing to the desired destination.

I. INTRODUCTION

The aging of world's population is bringing the need to provide robotic platforms capable to assist elder people to move [START_REF] Simpson | How many people would benefit from a smart wheelchair? the journal of rehabilitation research and development[END_REF], [START_REF] Lachmann | Assessment of need for special seating and/or electronic control systems for wheelchairs among people with severe physical disabilities[END_REF]. It is necessary that such transportation is reliable, safe (at least as much as a human) and comfortable. Patients and medical staff have a strong desire for the services that a smart wheelchair can offer [START_REF] Fehr | Adequacy of power wheelchair control interfaces for persons with severe disabilities: a clinical survey[END_REF]. Some users cannot use a normal power wheelchair because they lack the required motor skills, strength, or visual acuteness for some others the main reason to use such robotic systems would be the increased ease of use that they can provide. When using a robotic wheelchair, the occupant must feel that this mode of travel is tailored to its needs. The vehicle or wheelchair must meet specific needs: those of people with disabilities or reduced mobility or just those of people without disabilities but who want a service of comfort . No matter what the mobility assistance device is (car, wheelchair, walking aid...), navigation in human environments is a central problem. If one aims to develop a robotic device, many different technologies must be combine: perception, prediction, fusion, navigation, control. Even more the system must integrate a way to share the control with the user to guarantee safe navigation while avoiding frustration due to the disregarding of the user's requests by the autonomous navigation system. The operation of the platform discussed in this paper has been designed around the following requirements:

• Usability: People with motor disabilities or aging people often have problems using joysticks and other standard control devices. The system should account for this, for example by favoring the most reasonable actions when presented with an ambiguous command. † {jesus.escobedo-cabello, anne.spalanzani, christian.laugier}@inria.fr This work has been founded by the INRIA large scale initiative project PAL, Personally Assisted Living

• Safety: The system should avoid collisions with both static and dynamic entities. • Respect of social conventions: When moving, a robot may considerably disturb people around it, especially when its behavior is perceived as unsocial. Even worse, the wheelchairs passenger may be held responsible for that behavior. It is thus important to produce socially acceptable motion.

This article is structured as follows: Section II offers an overview of related works. A general description of the system architecture is presented in III while section IV focuses in our technique for estimating the user intended destination and control of the wheelchair. In section V examples of execution on our real platform are exhibited. Section VI presents conclusions about the work and perspectives.

II. RELATED WORK

Many efforts have been made to develop robotic wheelchairs that operate similarly to an autonomous robot so that the user gives a final destination and supervises as the smart wheelchair moves to the goal [START_REF] Levine | The NavChair Assistive Wheelchair Navigation System[END_REF], [START_REF] Kollar | Grounding Verbs of Motion in Natural Language Commands to Robots[END_REF], [START_REF] Hemachandra Madhawa | title = Narrated Guided Tour Following and Interpretation by an Autonomous Wheelchair[END_REF]. Other smart wheelchairs limit their assistance level to collision avoidance where the user is in charge of most of the navigation task. These systems do not normally require prior knowledge of an area or any specific alterations to the environment. They require instead more planning and continuous effort on the part of the user [START_REF] Vanacker | Context-based filtering for assisted brain-actuated wheelchair driving[END_REF], [START_REF] Demeester | Bayesian Estimation of Wheelchair Driver Intents: Modeling Intents as Geometric Paths Tracked by the Driver[END_REF], [START_REF] Demeester | Bayesian plan recognition for Brain-Computer Interfaces[END_REF] .

Shared control is presented in situations in which the assisting device combines the control input coming from the robot and the user in order to cooperate in the task [START_REF] Demeester | Assessing the user's intent using bayes' rule: application to wheelchair control[END_REF], [START_REF] Demeester | Bayesian plan recognition for Brain-Computer Interfaces[END_REF] The estimation of the user's plan is a key point in many shared control tasks because it allows to the automatic controller/robot to adequate its actions to the desire of its user. Inferring the user plan is necessary whenever the interface with the user doesn't allow him to explicitly dictate this to the robot as with many popular electric wheelchair interfaces (Brain control interface, Face tracking, gaze tracking, sip and puff, joystick, etc). A robotic wheelchair can assist by taking over low-level control, requiring the user to use the input method only to give high-level directional commands like right, left, back, forward, etc. Some methods to perform an implicit estimation of the user intention from simple inputs have been proposed in [START_REF] Demeester | Assessing the user's intent using bayes' rule: application to wheelchair control[END_REF], [START_REF] Demeester | Bayesian Estimation of Wheelchair Driver Intents: Modeling Intents as Geometric Paths Tracked by the Driver[END_REF], [START_REF] Demeester | Bayesian plan recognition for Brain-Computer Interfaces[END_REF]. They model the user intent as possible trajectories to follow, then a probability distribution is maintained over the set of trajectories and finally the selection of the most probable one is done using the input from the user within a Bayesian framework.

In [START_REF] Taha | POMDP-based longterm user intention prediction for wheelchair navigation[END_REF] a learned Partially Observable Markov Decision Process (POMDP) is used to estimate the intended destination into a predefined map of the environment in a high level topological manner. This means that the user focuses on driving the wheelchair from one spatial location to another without having to worry about all the low level control. The spatial representation used is based on a topological graph representation of the environment, where vertices are locations and edges represent a viable path connecting two locations as a result of performing an action. Places of interest are selected as spatial locations in the environment where the user spends significantly most of his time.

The method presented in this article to infer the user intended goal aims to build a model combining a Bayesian network approach with a topological goal based representation of the environment. In [START_REF] Taha | POMDP-based longterm user intention prediction for wheelchair navigation[END_REF], [START_REF] Demeester | Bayesian Estimation of Wheelchair Driver Intents: Modeling Intents as Geometric Paths Tracked by the Driver[END_REF] they both used a joystick as input device while in this work a more natural human-machine interface based on a face tracking system and voice recognition was used to command the wheelchair (V-C) while the navigation is performed using a human aware planning algorithm that avoids uncomfortable situations when the wheelchair is navigating among humans [START_REF] Vasquez | Human aware navigation for assistive robotics[END_REF], [START_REF] Rios-Martinez | Understanding human interaction for probabilistic autonomous navigation using Risk-RRT approach[END_REF].

III. SYSTEM ARCHITECTURE

When performing robotics research, often the scope of the investigation is limited to a well-defined area of the system, such as a software module which performs some type of planning, reasoning, perception, or control. However, to get a robotic system up and running for experiments, a much larger software ecosystem must exist.

Fig. 1 presents an overview of our systems architecture. It is divided into several subsystems, some of them are being developed by our team while others were taken from external sources to perform necessary tasks that are not crucial for our research domain.

• User Intentions Estimation: The user intention subsystem estimates the desired goal within the map of the environment among a list of possible predefined goals. Those locations can be previously selected by an expert caregiver, the user, or learned by the system using machine learning techniques. The user intention estimation computes the probability for each typical goal given the current position of the wheelchair and the user command and then selects the goal with the highest probability. The computation of probabilities is done using a Bayesian network approach. • Tracking: The off-board tracker provides global information about moving obstacles which is the learning input for our motion prediction module. It is built as a conventional detect-then-track system. The tracking subsystem is also necessary to identify the interactions between people (e.g. two persons involved in a conversation). • Prediction: Processes data from the trackers and transforms it into probabilistic predictions about the configuration of the free space in the future environment. The motion prediction subsystem takes tracking data (i.e. position, orientation and velocity) and outputs grids, representing the posterior probability of the space being occupied at a given time step in the future. Prediction itself is accomplished with a Growing Hidden Markov Model (GHMM) [START_REF] Vasquez | Growing Hidden Markov Models: a Tool for Incremental Learning and Prediction of Motion[END_REF] and an Extended Kalman Filter. • Social Filter Detects social interactions and creates virtual obstacles corresponding to those interaction zones. In order to produce socially acceptable motion, it has been proposed the Social Filter, which integrates constraints inspired by social conventions in order to evaluate the risk of disturbance and take it into account when making the autonomous navigation planning. We focus on detecting and predicting conversations in the environment surrounding the wheelchair [START_REF] Rios-Martinez | Understanding human interaction for probabilistic autonomous navigation using Risk-RRT approach[END_REF].

• Motion Planning: The navigation subsystem includes a laser-based localization module and a motion-planner which integrates predictions to compute safe trajectories that are fed to the execution module. The motion planner is based on RiskRRT [START_REF] Fulgenzi | Risk based motion planning and navigation in uncertain dynamic environment[END_REF], a partial motion planner which integrates motion predictions to provide safe trajectories. This algorithm was thought to operate in dynamic, uncertain environments, it supposes that the moving pedestrians detected in the environment follow typical motion patterns that are represented by Growing Hidden Markov Model (GHMM). This motion planner generates human friendly paths respecting people's personal and interaction spaces, as provided by the social filter.

IV. USER INTENTIONS ESTIMATION SYSTEM

The user intentions are modelled as topological poses into a predefined map. Those poses are defined by the user habits (places where the user spends most of his time during the day) and interesting points taken from the map of the environment as doors, desks and other facilities.

The reasoning method used is based on a Bayesian Network Fig. 2. The variable command C is dependent on the variables goal G and position X (because the user normally will point towards the desired goal). To estimate the status of the goal variable the command direction coming from the usermachine interface and the current user's position are applied as evidence. It is represented as P (G i t |C t X t ) which can be computed as a Bayesian filter:

P (G i t |C t X t ) = P (C t |X t , G i t ) j [P (G i t |G j t-1 ) * P (G j t-1 |C t-1 X t-1 )] (1)
The notation G i t is used to express G t = g i where g i is one of the predefined goals in the environment as appear in Fig. 3.

P (C t |X t , G i t )
represents the probability of giving a command C t when the user is located at position X t and her goal is at position G i t at current time t. Under the assumption that commands are directed straight to the goal position rather than anywhere else, as shown in Fig. 3, the non normalized probability P ′ (C t |X t , G i t ) can be computed as follows:

P ′ (C t |X t , G i t ) = 1 -|a i | π ( 2
)
This probability is the normalized:

P (C t |X t , G i t ) = P ′ (C t |X t , G i t ) i P ′ (C t |X t , G i t ) (3)
The a i term is the angle between the command's direction and the goal's direction.

P (G i t |G j t-1 ) expresses the probability that the current estimated goal is different to the last one. This works as an smoothness term which avoids abrupt changes in the estimated goal from one command to another. Caution should be taken when choosing the value of this term because large values lead to a slow response whenever the user changes of intended of goal. After practical experimentation it was

(G i t |G j t-1
) that is 10 times bigger if the last estimated goal was the same.

P (G i t |G j t-1 ) = 10/(n + 9) if G i t = G j t-1 1/(n + 9) else ( 4
)
Where n is the number of possible goals in the environment.

V. EXPERIMENTAL SETUP AND RESULTS

The proposed experimental setting is shown in Fig. 4. The main entity is the robotic wheelchair with all the on-board sensors and computer Fig. 5. The scenario proposed for the experiments is a human populated environment where people can be moving and interacting. Those persons can be tracked using the camera mounted on the top of the scenario. A remote computer is in charge to send the tracking information to the wheelchair.

A map of the environment is built using the LIDAR mounted on the wheelchair and some important goals are set up by hand within this map. A Kinect mounted in front of the user's face is used as input device to control the direction of the wheelchair while a microphone supplies the sound data for the voice recognition system.

A. Visual Tracking Subsystem

A camera mounted over the scenario is used to track the present people. A marker based visual tracking system is used to accurately track the position and orientation of Fig. 5. Robotic wheelchair used for the described experiments.The mobile base includes all the electronic components and the computer in charge of the low level control of the wheelchair special marked cards. In order to track the people in the experimental scene they wear markers as explained in [START_REF] Vasquez | Human aware navigation for assistive robotics[END_REF].

B. Wheelchair

The equipment used is the robotic wheelchair shown in Fig. 5 that consists of a mobile base equipped with a seat, all the on-board electronics and different attached devices. Wheelchair's sensors consist of a LIDAR (Light Detection and Ranging) model SICK LMS-200, quadrature encoders for odometry measurements, emergency bumpers sensors (contact switches) and 1 Kinect sensor used as command input for the user-intentions subsystem presented in this work.

C. Face Control Subsystem

The user can control the robotic wheelchair by using the movements of his face, with or without using the user intention estimation system. This is accomplished by means of a face tracking system that estimates the direction of the sight of the user using data from a Microsoft's Kinect [START_REF]Pi face tracker webpage[END_REF]. The images taken by the 2D RGB camera are used to set up a region of interest over the depth data coming from the infrared sensor. The face of the user is located using a Haar detector and a set of SWIFT features are selected over it, those features are used to perform the 2D tracking using the Lucas-Kanade method. The identification of the face pose is done by a random forest classifier which takes as input the 3D data from the Kinect sensor and gives the estimated position of the face [START_REF] Fanelli | Real time head pose estimation with random regression forests[END_REF], the results of the face tracking are shown in figure 6.

The wheelcair can be controlled in semi-autonomous mode employing the user intention prediction module or in manual Fig. 6. In the presented approach the user drives the wheelchair by using his face. The face is tracked by processing the RGB Kinect's image (left) to set a region of interest used to estimate the face position from range data (right). mode in which the user is in charge of driving by her self.

In manual mode the user controls the wheelchair's angular speed moving her head while the linear speed is controlled with voice commands as 'faster, full-speed slower, stop' explained in sec. V-D.

In semi-autonomous mode the user shows the direction to her desired destination facing towards it. Whenever a new command is read from the face pose estimation system. The user's intention module computes the goal with the highest posterior probability, depicted in Fig. 7 (b) as the biggest sphere.The navigation module receives the map of the environment, the list of humans present in the scene and the currently estimated goal to compute the necessary trajectory to the goal as shown in Fig. 7 (b). In the example the user is looking to the wall located to the left however the wheelchair instead of moving towards this wall goes to the goal that is in the other side of the hall which is the one with the highest probability in that direction. When moving the user does not have to worry about the necessary planning to avoid obstacles because the autonomous navigation system is in charge of that. In the example Fig. 7, the user is seeing to the left therefore it is more probable that she is aiming to go to any of the goals in that direction.

D. Voice Control Subsystem

In the frame of the presented approach it was used the speaker-independent, continuous speech, recognition system Pocketsphinx [START_REF]Cmu sphinx webpage[END_REF] from Carnegie Mellon University.

This system was trained on a large speech corpus developed for acoustic phonetic research, such that appropriate acoustic models can be precomputed. Pocketsphinx allow the designer to specify a set of syntactic rules (or grammar) which specifies constraints on the ordering of words within a sentence. This grammar can be useful to enhance speech recognition quality by constraining the hypothesis space. In general a small vocabulary makes speech recognition more accurate, therefore a dictionary that focuses in a very small fixed set of tasks was considered (Go, Start, Move, Stop, Break, cancel, faster,. . . ).

The voice interface is used to fulfill some lack in functionality from the head control system. The main function is to switch between manual and semi-autonomous mode by saying 'semi-autonomous' or 'manual' commands. In semiautonomous mode the voice interface is used to 'stop' the wheelchair whenever it is required. To improve the reliability of the system some other synonyms were considered to perform the same action ('break', 'cancel', and a panic exclamation 'ah!'). When manual mode is selected, voice control can be used to 'stop' or 'start' the movement of the wheelchair if the user says the 'stop' command once, the wheelchair stops moving linearly but keeps turning in the direction of user's face. If a second 'stop' command is then used the wheelchair stops completely until it receives a move, start, forward, go or backward command.

VI. CONCLUSIONS AND FUTURE WORK

The proposed experimental platform is an ongoing effort and the results we have obtained should be considered preliminary. The most important results up to this point are:

The proposed autonomous navigation system (RiskRRT with Social Filter and GHMM predictor) has been extensively validated [START_REF] Vasquez | Human aware navigation for assistive robotics[END_REF]. This approach consistently yields to more socially acceptable and safer navigation in dynamic environments.

The user's intention algorithm has proved to be useful to translate simple input commands (direction of the desired movement) into high level orders (the desired destination) that can be used to feed the autonomous navigation system developed by emotion team at INRIA Rhône-Alpes.

It is necessary to take into account cases where there exist ambiguity in the possible desired goals. As it can be seen in Fig. 7 even if g 1 was chosen over g 2 both of them have similar probability values (size of the sphere) In those cases when the system can not decide accurately which is the intended goal it should be requested some extra information from the user to make a better choice. Another open question is how to distinguish and omit normal head movements that does not aim to control the wheelchair.

In order to work in a non-supervised environment, the user intention algorithm must be extended combining machine learning techniques in order to add the capability to adapt autonomously to the user's specific disability.

Despite all the well known limitations of the Kinect (useful only indoors, limited field of view, etc.) using it as input device can be advantageous to assist the elderly because it provides a more natural way of interaction when giving directions (we usually look to where we want to go) so they can be more confident when using the wheelchair.

14 Fig. 1 .

 141 Fig. 1. System Architecture Overview

Fig. 2 .

 2 Fig. 2. The Bayesian network used to estimate the user's intended goal G from the current position X, the user command C and the prior knowledge of the environment

Fig. 3 .Fig. 4 .

 34 Fig. 3. The probability distribution for a given command Ct (big arrow) is proportional to the angle a i formed with respect to each goal g i in the environment.

Fig. 7 .

 7 Fig. 7. Experimental scenario. a) INRIA's hall. Possible goals are marked with small arrows and the current given command (direction of the head) is depicted with a big one. b)Results of the system in a real scenario. The size of the spheres in the environment represent the value of the computed posterior probability for each destination. The computed trajectory is also shown.