Combining Dense Structure from Motion and Visual SLAM in a Behavior-Based Robot Control Architecture

1.1. Goal and Problem Statement

Modern intelligent robots are generally equipped with an abundance of sensors like for example GPS, laser, ultrasound sensors, etc to be able to navigate in an environment. However, this stands in contrast to the ultimate biological example for these robots: us humans. Indeed, humans seem perfectly capable to navigate in a complex, dynamic environment using only vision as a sensing modality. This constatation inspired us to develop a visually guided intelligent mobile robot.

As the goal of this research project is to develop an intelligent autonomous mobile robot, one should first pose the question: “What is an intelligent autonomous mobile robot or what does it do?” This is of course largely task-dependent, yet there are some capabilities which should necessarily be present.

First, to navigate autonomously in an unknown environment, the robot needs to dispose of a means to detect and avoid obstacles. Numerous sensors exist which can detect obstacles in the path of the robot. These include ultrasound sensors, laser range finders, infrared sensors, etc. When using visual input, obstacles can be detected through 3D reconstruction. In this paper, a technique to perform dense 3D reconstruction using input data from only one camera is presented.

Next, the robot needs some degree of self-consciousness, meaning that it needs to be able to infer its current status in relation to the outside world from its sensor readings. This problem is also referred to as the Simultaneous Localization and Mapping (SLAM) problem. Classical SLAM solving techniques use input data from laser range scanners or ultrasound sensors. In this paper, a technique to perform SLAM using only visual input data is presented. Third, the robot must dispose of some sort of “intelligence” to execute the tasks or objectives it has been given. These objectives can be multiple and may be in contradiction with one another. In this paper, a technique to solve the multi-objective decision making problem is presented.

We will now introduce a solution to solve these three issues and relate this to classical approaches.

1.2. The proposed approach towards 3D reconstruction and its relation to previous work

Recovering 3D-information has been in the focus of attention of the computer vision community for a few decades now, yet no all-satisfying method has been found so far. Most attention in this area has been on stereo-vision based methods, which use the displacement of objects in two (or more) images. The problem with these vision algorithms is that they require the matching of feature points, which is not easy for untextured surfaces. Where stereo vision must be seen as a spatial integration of multiple viewpoints to recover depth, it is also possible to perform a temporal integration. The problem arising in this situation is known as the “Structure from Motion” (SfM) problem and deals with extracting 3-dimensional information about the environment from the motion of its projection onto a two-dimensional surface (Chiuso, A.; Favaro, P.; Jin, H. & Soatto, S., 2002).

In general, there are two approaches to SfM. The first, feature based method is closely related to stereo vision. It uses corresponding features in multiple images of the same scene, taken from different viewpoints. The basis for feature-based approaches lies in the early work of Longuet-Higgins (Longuet-Higgins, H.C., 1981), describing how to use the epipolar geometry for the estimation of relative motion. These techniques have matured a lot over the past two decades, but a remaining problem is that they deliver only sparse 3D information. The second approach for SfM uses the optical flow field as an input instead of feature correspondences. The applicability of the optical flow field for SfM calculation originates from the epipolar constraint equation which relates the optical flow u (u,v) to the relative camera motion (translation t and rotation ω) and 3D structure, represented by the depth parameter d=1/Z, in a non-linear fashion, as indicated by equation 1:

u = Q ω ω + d Q t t

(1)

with the matrices Q_ω and Q _t , defined as:

Q ω = [ x y f − f − x 2 f y f + y 2 f − x y f − x ] ; Q t = [ − f 0 0 0 − f y ]

(2)

with f the focal length of the camera.

In (Hanna, K.J., 1991), Hanna proposed a method to solve the motion and structure reconstruction problem by parameterizing the optical flow and inserting it in the image brightness constancy equation. More popular methods try to eliminate the depth information first from the epipolar constraint and regard the problem as an egomotion estimation problem. Bruss and Horn already showed this technique in the early eighties using substitution of the depth equation (Bruss, A.R. & Horn, B.K.P., 1983), while Jepson and Heeger later used algebraic manipulation to come to a similar formulation (Heeger, D.J. & Jepson, A.D., 1992). Optical flow based SfM approaches are more suited to address dense reconstruction problem, as they can go out from the optical flow over the whole image field.

In order to bring together the advantages of both sparse and dense SfM theorems, we aim to fuse both methods into an integrated structure recovery algorithm. This leads to an approach as sketched by Fig. 1, showing two main input paths to the dense reconstruction: sparse epipolar reconstruction and dense optical flow estimation.

OPEN IN VIEWER

Fig. 1.

The general approach of the proposed dense 3D reconstruction algorithm: merging sparse information (epipolar geometry represented by the fundamental matrix F) with dense information (the optical flow u).

In case of navigation in an unknown environment starting from an unknown location with no a priori knowledge, a SLAM system (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007) simultaneously computes an estimate of the robot location and the landmark locations. While continuing its motion, the robot builds a complete map of landmarks and uses these to provide continuous estimates of the vehicle location.

Techniques for solving the SLAM problem have focused in using probabilistic methods, taking account the uncertainty in the measurement. Two main groups of techniques have been considered depending on the way of representing such uncertainty: a) Gaussian filters and b) non-parametric filters, which are discussed in the following paragraphs.

The most well-known Gaussian filter for treating the SLAM problem is the Extended Kalman Filter (EKF), where the belief is represented by a Gaussian distribution. The EKF estimates recursively the state of a dynamic system using data from sensors. It uses only information from previous steps and the actual measurements in order to estimate the current state and update the system. Whenever a landmark is observed by the on-board sensors of the robot, the system determines whether it has been already registered and updates the filter. In addition, when a part of the scene is revisited, all the gathered information from past observations is used by the system to reduce uncertainty in the whole mapping. This strategy is known as closing-the-loop.

In EKF-based SLAM approaches (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007), the environment is represented by a stochastic map M = (○, P), where ○ is the estimated state vector, containing the location of the vehicle y _R and the features of the environment x _{F 1} … x _{F n} , and P is the estimated error covariance matrix, where all the correlations between the elements of the state vector are defined. All data is represented in the same reference system. The map M is built incrementally, using the set of measurements z _k obtained by sensors such as cameras or lasers. For each new acquisition, a data association process is carried out, searching correspondences between the new acquired features and the previously perceived ones.

Simultaneous Localization and Mapping (SLAM) problem has also been tackled by using non-parametric filters such as the histogram filter or the particle filter (PF) (Stachniss, C.; Grisetti, G. & Burgard, W., 2005). The main difference compared to Gaussian filters is the possibility of dealing with multimodal data distribution, using multiple values (particles) to represent the belief. That is, each estate y ^t of the environment can be represented for multiple particles, one for each hypothesis.

In comparison with EKF-based filters, a PF presents more robustness to periods of considerably uncertainty and sensor noise, due to its multi-modal data distribution. However, Gaussian filters usually have a polynomic computational cost, whereas the computational cost of a non-parametric filter may be exponential. During last years, several interesting approaches based on particle filters have been presented as an alternative to EKF-based techniques, with the aim of solving the SLAM problem. Stachniss et al. propose in (Stachniss, C.; Grisetti, G. & Burgard, W., 2005) the use of a Rao-Blackwellized particle filter for local map representation, combined with some techniques for particle reduction and a “Closing the loop” strategy. The strongest point of this approach is the possibility of dealing with periods of great uncertainty, due to its ability to recover already vanished hypotheses. This represents a considerable improvement with respect to EKF-based approaches, which do not allow to recover hypotheses that have been already vanished in the past. Alternatively, Montemerlo et al. propose in (Montemerlo, M.; Thrun, S.; Koller, D. & Wegbreit, B. 2002) a new PF-based approach named FastSlam, which combines the use of particles with Kalman filters for map representation. That is, each particle y ^t _[m] (composed by all the hypothesis of the robot pose estimation at time state t) has, at the same time, K Kalman filters representing each landmark pose estimation with respect to the vehicle pose. This hybrid method has provided reliable solutions to several problems of EKF-based approaches such as the high computational cost that requires updating filters containing a considerable amount of data. That is, since the problem is divided into multiple small Kalman Filters containing only Gaussians of two dimensions (for 2D feature location), the computational cost can be reduced to O(MlogK), where M is the number of particles and K the number of landmarks. However, if the complexity of the environment requires the use of 3D data, then the computational cost increases considerably, forcing the reduction of the number of features at each step, which has a direct effect on the quality of the results.

The main open problem of the current state of the art SLAM approaches and particularly vision based approaches is mapping large-scale areas (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007). Relevant shortcomings of this problem are, on the one hand, the computational burden, which limits the applicability of the EKF-based SLAM in large-scale real time applications and, on the other hand, the use of linearized solutions which compromises the consistency of the estimation process. Added to this, vision poses an extra challenge over lasers for the SLAM problem, due to the very high input data rate, the inherent 3D quality of visual data, the lack of direct depth measurement and the difficulty in extracting long-term features to map (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007). Due to those factors, there have been relatively few successful vision-only SLAM systems which are able to construct persistent and consistent maps while closing loops.

The computational complexity of the EKF stems from the fact that covariance matrix P ^t represents every pairwise correlation between the state variables. Incorporating an observation of a single landmark will necessarily have an affect on every other state variable. This makes the EKF computationally infeasible for SLAM in large environments. Methods like Network Coupled Feature Maps (Bailey, I. 2002), Sequential Map Joining (Estrada, C.; Neira, J. & Tardos, J. D., 2005), and the Constrained Local Submap Filter (CRSF) (Williams, S. B., 2001) have been proposed to solve the problem of SLAM in large spaces by breaking the global map into submaps. This leads to a more sparse description of the correlations between map elements. When the robot moves out of one submap, it either creates a new submap or relocates itself in a previously defined submap. By limiting the size of the local map, this operation requires a constant time per step. Local maps are joined periodically into a global absolute map in a O(N²) step. However, these computational gains come at the cost of slowing down the overall rate of convergence. The Constrained Relative Submap Filter (Williams, S. B., 2001) proposes to maintain the local map structure. Each map contains links to other neighboring maps, forming a tree. The method converges by revisiting the local maps and updating the links through correlations. This method allows to reduce the computation time and memory requirements and to obtain accurate maps of large environments in real time.

In our study, we are interested in the visual navigation of a mobile robot in large spaces. We proposed a procedure to build a global representation of the environment based on several size limited local maps built using a modified version of the vision based SLAM approach called monoSLAM algorithm of Davdison et al. (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007). The monoSLAM algorithm is a real-time SLAM approach for indoors in room-size domains, which uses an Extended Kalman Filer (EKF) to recover the 3D trajectory of a monocular camera, moving rapidly through an unknown scene. The role of the map, in this work, is primarily to permit real-time localization rather than to serve as a complete scene description.

1.3. The proposed approach towards behavior-based navigation and its relation to previous work

In the behavior-based spirit, a complex control problem is divided into a set of simpler control problems that collectively solve the original complex control problem. To do this, it is thus necessary to address the problem of coordination of the activities of the behaviors so to satisfy the initial complex system's control objectives.

The behavior fusion problem can be formulated as a multiple objective decision making (MODM) problem (Pirjanian, P., 1998). The formalism of MODM can be cast to encompass ideas from behavior-based system synthesis, where each behavior is cast as an objective function estimator. This means that each behavior calculates an objective function over a set of permissible actions. The action that maximizes the objective function corresponds to the action which best satisfies that objective. Multiple behaviors are blended into a single more complex behavior that seeks to select the action that satisfies simultaneously all the objectives as well as possible. In this approach action selection is comprised of generating and then selecting a (feasible) set of satisfying solutions among the set of solutions that are optimal.

An important characteristic of MODM problems is that the multiple objectives can be competitive and conflicting, i.e., the improvement in one can be associated with a deterioration in another. Thus ranking of the alternatives according to a single measure of attainability (criterion) becomes more difficult if not impractical. Additionally, optimal solutions might not exist and thus the concept of optimality should be modified to endow similar and useful concepts in MODM problems (Keen, P.G.W., 1977). Mathematically, a multi-objective decision problem can be represented in the following way:

arg ⁡ max x ⁡ [ o 1 ( x ) , … , o n ( x ) ] .

(3)

Where o₁(x),…, o _n (x) are a set of system objectives, tasks or criteria and where x = (x₁,…,x _n ) ∈ R ⁿ is a n-dimensional decision variable vector. The degree of attainment of a particular alternative x, with respect to the k^th objective is given by o _k (x). X ⊆ R ⁿ defines the set of feasible alternatives.

A common method for solving a MODM problem is the weighting method. This method is based on scalar vector optimization and is formulated in the following way:

x ∗ = arg x ∈ X ⁡ max ∑ i = 1 n w i o i ( x ) .

(4)

where w _i are weights normalized so that ∑ i = 1 n w i = 1 .

This method appears simple, but has some serious disadvantages:

It is not possible for a human decision maker to input subjective knowledge or preferences into the decision making process.

To address these issues, we present a MODM solving technique that:

Incorporates a human decision maker's preferences by allowing goal programming.

The robot control architecture brings together the different capabilities described above in a consistent framework. This architecture is sketched in Fig. 2.

OPEN IN VIEWER

Fig. 2.

The proposed control architecture for a visually guided robot. A camera data stream is analyzed by a Dense Structure from Motion and a Visual SLAM algorithm. This information serves as input for obstacle avoidance and goal seeking behaviors which are fused to form one consistent and globally optimal robot command.

As vision is the only sensing capability regarded, it all starts with a camera stream which serves as input for a structure from motion and visual SLAM algorithm. The structure from motion algorithm calculates a depth map corresponding to the current input image. This depth map serves as input for an obstacle avoidance behavior which aims to steer the robot away from obstacles. On the other hand, the Visual SLAM algorithm calculates and updates a map of the environment and localizes the robot on this map using tracked visual features. A global path planner decomposes the trajectory to a set goal position into a number of intermediate waypoints. These waypoints serve as input for a goal seeking behavior which aims to drive the robot into the direction of the current waypoint. To conclude the actions proposed by the different behaviors are fused together to form one consistent command to be sent to the robot.

The remains of this paper is organized as follows: in section 2 the 3D reconstruction technique is discussed more in detail, whereas section 3 focusses on the Visual SLAM and section 4 describes the development and fusion of the different behaviors.

3.1. Dynamic Model and Observation Model

In our application, a camera is fixed on the top of the mobile car-like robot. The vehicle travels through the environment using the camera to observe features around it. A world coordinate frame W is defined such that its X and Z axes lie in the ground plane, and its Y axis point vertically upwards. The system state vector y _R = (y₁, y₂, γ) ^T in this case is defined as the 2D position vector (y₁, y₂) of the head center in the world frame coordinates and the robot's orientation γ relative to the Z axis. The system dynamic model in this case, considering the control u as identity, is given by:

y R t = [ y 1 t y 2 t γ t ] = f ( y R t − 1 ) = [ y 1 t − 1 + ( υ t − 1 + V ) cos ⁡ ( γ t − 1 ) Δ t y 2 t − 1 + ( υ t − 1 + V ) sin ⁡ ( γ t − 1 ) Δ t γ t − 1 + ( ω t − 1 + Ω ) Δ t ] ,

(9)

where γ and ω are the linear and the angular velocities, respectively.

A feature L _i is represented in the state vector by its location in the world coordinate system W:

x i = ( x 1 , i , x 2 , i , x 3 , i ) T

(10)

Making a measurement of a feature i consists of determining its position in the camera image. Using a perspective projection, the observation model in the robot coordinate system is obtained as follows:

z i t = [ z 1 , i t z 2 , i t ] = h ( x i ) = [ x 0 + f R x 1 , i R x 3 , i y 0 + f R x 2 , i R x 3 , i ]

(11)

where x₀ and y₀ are the image center coordinates and f is the focal length of the camera. ^R x _i =( ^R x_1,i, ^R x_2,i, ^R x_3,i) ^T are the coordinates of the feature i in the robot coordinate frame R. They are related to x _i by:

R x i t = [ R x 1 , i R x 2 , i R x 3 , i ] = [ cos ⁡ ( γ t ) 0 − sin ⁡ ( γ t ) 0 1 0 sin ⁡ ( γ t ) 0 cos ⁡ ( γ t ) ] [ x 1 , i t − y 1 t x 2 , i t − h x 3 , i t − y 2 t ]

(12)

where h is the height of the camera. The measurement model is then:

z i t = h ( x i t ) = [ x 0 + f ( x 1 , i t − y 1 t ) c o s ( γ t ) − ( x 3 , i t − y 2 t ) s i n ( γ t ) ( x 1 , i t − y 1 t ) s i n ( γ t ) + ( x 3 , i t − y 2 t ) c o s ( γ t ) y 0 + f x 2 , i t − h ( x 1 , i t − y 1 t ) s i n ( γ t ) + ( x 3 , i t − y 2 t ) c o s ( γ t ) ]

(13)

Working with visual features in an outdoor environment requires great care in order not to run into problems due to changing illumination conditions and changing scales and viewpoints. Usually the features used in vision-based SLAM algorithms are salient and distinctive objects detected from images. Typical features might include regions, edges, object contours, corners etc. In our work, the map features are obtained using the SIFT feature detector (Lowe, D.G., 2004), which maps an image data into scale-invariant coordinates relative to local features. These features were contemplated to be highly distinctive and invariant to image scale and rotation. The work of Mikolajczyk and Schmid (Mikolajczyk, K. & Schmid, C., 2003) proved that SIFT features remain stable to affine distortions, change of viewpoint, noise and change in illumination, which is crucial when working in outdoor environments. The same procedure for feature depth estimation as in (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007) is used in our implementation.

Heuristic map management criteria are used to decide when to initialize new features: essentially, the requirement is to keep a predefined number of features visible from all camera locations. A typical number used is 10; whenever fewer than 10 features are visible new ones are detected and initialized. Features are not deleted from the map when they leave the field of view, but remain in the map and can be re-observed when the camera moves back and they become visible again. In some cases it is necessary to delete features which are not being reliably matched on a regular basis: some features detected will be frequently occluded or may contain parts of objects at very different depths. These features will lead to failed correlation attempts and can be removed from the map automatically.

A major problem for SLAM methods are changes in the environment over time. Some changes may be relatively slow, such as the change of appearance of a tree across different seasons, or the structural changes that most office buildings are subjected to over time. Others are faster, such as the change of door status or the location of furniture items, such as chairs. Even faster may be the change of location of other agents in the environment, such as cars or people. To see, imagine a robot facing a closed door that previously was modelled as open. Such an observation may be explained by two hypotheses, namely that the door status changed, or that the robot is not where it believes to be. Unfortunately, there are almost no mapping algorithms that can learn meaningful maps of dynamic environments.

To deal with the problem of SLAM in dynamic scenes with moving object we use our algorithm for motion segmentation (Berrabah, S.A.; De Cubber, G.; Enescu, V. & Sahli, H., 2006) to remove the outliers features which are associated with moving objects. In other words, the detected features which correspond to the moving parts in the scene are not considered in the built map. For more security we use a bounding box around the moving objects (as in Fig. 5). Another security measure sees to it that newly detected features are not added directly to the map but they should be detected and matched in at least n consecutive frames (in our application, n = 5).

OPEN IN VIEWER

Fig. 5.

Features detected in a scene with moving objects

Some stationary objects in the scene can suddenly start moving. In this case, their corresponding features should be deleted from the built map. Deleting a feature L _i from the map consists in extracting its parameters from the system state vector and the covariance matrix.

When a feature is first detected, the measurement from a single camera position provides good information on its direction relative to the camera, but its depth is initially unknown. Since depth information is not provided, EKF can not be directly initialized, leading to a new challenge known as Bearing-Only SLAM. An early approach was proposed by Deans (Deans, M. & Hebert, M., 2000), who combined Kalman filter and bundle adjustment in filter initialization, obtaining accurate results at the expense of increasing filter complexity. In (Davison, A. J.; Reid, I. D.; Molton, N. D. & Stasse, O., 2007), Davison uses for initialization an A4 piece of paper as a landmark to recover metric information of the scene. Then, whenever a scene feature is observed a set of depth hypotheses are made along its direction.

In subsequent steps, the same feature is seen from different positions reducing the number of hypotheses and leading to an accurate landmark pose estimation. Besides, Ortega et al. proposed in (Ortega, J. S.; Lemaire, T.; Devy, M; Lacroix, S. & Monin, A., 2005) a 3D Bearing-Only SLAM algorithm based on EKF filters, in which each feature is represented by a sum of Gaussians. In our application, to estimate the 3D position of the detected features, we use an approach based on epipolar geometry. This geometry represents the geometric relationship between multiple viewpoints of a rigid body and it depends on the internal parameters and relative positions of the camera. The two-view geometry has been used in our case.

3.4. Feature Matching

At step t, an onboard sensor obtains a set of measurements z ^t _i (i = 1,…,m) of m environment features. Feature matching corresponds to data association, also known as the correspondence problem, which consists in determining the origin of each measurement, in terms of the map features L _j , j = 1,…,n.

The result is a hypothesis H t = [ h 1 t , … , h m t ] associating each measurement z ^t _i with its corresponding map feature. h i t = 0 indicates that z ^t _i does not come from any feature in the map. For data association a measure of the discrepancy between a predicted measurement that each feature would generate and an actual sensor measurement is measured by the innovation ɛ.

In order to establish the consistency of a hypothesis H t , measurements can be jointly predicted using the function h H t ( x t | t − 1 ) = [ | h h 1 t ( x t | t − 1 ) … h h m t ( x t | t − 1 ) ] T , which can also be linearized around the current estimate to yield:

h H t ( x t | t − 1 ) ≃ h H t ( x ^ t | t − 1 ) + H H t ( x t | t − 1 − x ^ t | t − 1 ) ,

(14)

with H H t = [ H h 1 t … H h m t ] T , the joint innovation ε H t t = z t − h H t ( x t | t − 1 ^ ) and its covariance S H t = G H t P t H H t T + R H t .

Measurements z ^t can be considered compatible with their corresponding features according to H t if the Mahalanobis distance satisfies D H t 2 = ε H t T S H t − 1 ε H t < t h .

In our application, as we are using SIFT features, the matching between feature is checked using a product of the Mahalanobis distance between measurements and their predictions and the Euclidean distance between the descriptor vectors of the features D² _desc =|| desc₁ - desc₂||.

This will allow using the advantage of looking for feature matching based on the prediction of their position based on the system model and the advantage of the space-scale invariance parameters.

In order to enhance the performance of the feature matching process in the outdoor case, an additional constraint was set up. Corresponding features should also satisfy the epipolar constraint, hence an image point x ^t _i that corresponds to x^t−1 _i is located on or near the epipolar line that is induced by x^t−1 _i . The distance of the image point x ^t _i from that epipolar line is computed as follows:

D e p i 2 = ( x i i T F x i t − 1 ) 2 ( F x i t − 1 | 1 ) 2 + ( F x i t − 1 | 1 ) 2

(15)

where F x i t − 1 F x i t − 1 | j is the j component of the vector F x i t − 1 . F is the fundamental matrix which is computed based on the estimations from the Extended Kalman Filter. Knowing the camera position at instants t, the estimated camera position at instant t−1, and the camera calibration matrix K, the fundamental matrix F is computed as follows:

F = K − T R [ t ] × K − 1

(16)

where R is the rotation matrix and [t]_x is the skew matrix corresponding to the translation vector t. The notation K^−T denotes the transpose of the inverse K.

As a result, the cost function for feature matching is defined as D² and D² _epi

D m a t c h 2 = D H t 2 + D d e c t 2 + D e p i 2

(17)

Measurements for which correspondences in the map cannot be found by data association can be directly added to the current stochastic state vector as new features.

In our study, we are interested in robot navigation in large spaces. For that, we propose a procedure to build a global representation of the environment based on several size limited local maps built using the previously described approach. Two methods for local map joining are proposed. The first method consists in transforming each local map into a global frame before to start building a new local map. In the second method, the global map consists only of a set of robot positions where new local maps started (i.e. the base references of the local maps). In both methods, the base frame for the global map is the robot position at instant t₀.

Each local map is built as follows: at a given instant t k , a new map is initialized using the current vehicle location, y t k R , as base reference B k = y t k R , k = 1,2,… being the local map order. Then, the vehicle performs a limited motion acquiring sensor information about the L i neighboring environment features. An EKF-based technique is used to model the local maps. The kth local map is defined by M k = ( X k , P k ) , where X k is the state vector in the base reference B k of the L k detected features and P k is their covariance matrix.

The decision to start building a new local map at an instant t _k is based on two criteria: the number of features in the current local map and the scene cut detection result. The instant t _k is called a key-instant. In our application we defined two thresholds for the number of features in the local maps: a lower Th⁻ and a higher Th⁺ threshold. A key-instant is selected if the number of features n _{k l} in the current local map k is bigger then the lower threshold and a scene cut has been detected or the number of features has reached the higher threshold. This allows kipping reasonable dimensions of the local maps and avoids building too small maps. At the key-instants t _k some frames are selected to be used for features initialization in the new local map. These frames are called key-frames. The motion between two frames must be sufficiently large to accurately compute the 3D positions of matched points. For that we select frames relatively far from each other but that have enough common points.

In the first method for global map building, the first local map is used as global map. Each finalized local map is transferred to the global map before starting a new one, by computing the state vectors and the covariance matrix. The goal of map joining is to obtain one full stochastic map M = ( X ( 0 ⊕ 1 ⊕ 2 ⊕ … ) 0 , P ( 0 ⊕ 1 ⊕ 2 ⊕ … ) 0 ) , where X ( 0 ⊕ 1 ⊕ 2 ⊕ … ) 0 is a concatenation in the frame B0 of all sets of features from local maps M 0 , M 1 , M 2 , … As each camera position, y t k R corresponding to the frame reference B k is given in the previous frame reference Bk-1, the transformation of feature i coordinates vector from frame B k to B0 is obtained by successive transformations:

( x ̄ i 0 1 ) = T k → ( k − 1 ) T ( k − 1 ) → ( k − 2 ) … T 1 → 2 ⋅ ( x i k 1 )

(18)

T k → ( k − 1 ) = ( R ∣ t ) is the space transformation matrix corresponding to rotation R and t from frame B _k to B_k−1 and is given by:

T k → ( k − 1 ) = ( cos ⁡ ( γ t k ) 0 − sin ⁡ ( γ t k ) y 1 t k 0 1 0 0 sin ⁡ ( γ t k ) 0 cos ⁡ ( γ t k ) y 2 t k 0 0 0 1 )

(19)

The covariance P⁰_{(0⊕1⊕2⊕…)} of the joint map is obtained from the linearization of the state transition function f. As the local maps are independents, the Jacobian (from linearization) is then applied separately to the local map covariance:

P ( 0 ⊕ 1 ⊕ 2 ⊕ … ) 0 = ∂ f ∂ y R | 0 P 0 ∂ f ∂ y R | 0 T + ∂ f ∂ y R | 1 P 1 ∂ f ∂ y R | 1 T + … + ∂ f ∂ y R | i P i ∂ f ∂ y R | i T + …

(20)

where ∂ f ∂ y R | k is the Jacobian of the state transition function f with respect to y _R in the reference frame k.

In the second method for global map building, the global map is limited to the set of the coordinates of the local maps frame origins M G B = ( y ̄ R 0 , y ̄ R 1 , y ̄ R 2 , … ) , where y ̄ R k are the robot coordinates in B₀, where it decides to build the local map M k at instant t _k . These robot coordinates are updated via

[ y ̄ R k 1 ] = T k → ( k − 1 ) ⋅ [ y R t k 1 ] ,

(21)

with t₀ = 0 and y ̄ R 0 = y R t 0 = ( 0 , 0 , 0 ) and the transformation matrix T k → 0 is obtained by successive transformations:

T k → 0 = T 1 → 0 ⋅ T 2 → 1 … T ( k − 1 ) → ( k − 2 ) ,

(22)

where T k → k − 1 = ( R ∣ t ) is the transformation matrix given by equation 19.

In this case, for feature matching at instant t, the robot uses the local map with the closest base frame to its current location, by evaluating the function:

arg ⁡ min i ( ∥ y ̄ R k − y ̄ R t ∥ ) ,

(23)

where y ̄ R t is the robot position at instant t in B₀.

4.1. Development of the Obstacle Avoidance behavior

The obstacle avoidance has two main responsibilities:

It points the robot away from obstacles. Thus, it must assign low values to actions that cause the robot to face obstacles and high values to actions that do the opposite.

The objective function o _OA (v,θ _robot ) can now be composed by making the product of a factor o^θ _OA (θ _robot ) taking into account the first, heading constraint and a factor o ^v _OA (v) taking into account the second constraint.

In order to calculate these objective functions, the dense depth map as estimated by the structure from motion algorithm is analyzed. This analysis process is illustrated in Fig. 6.

OPEN IN VIEWER

Fig. 6.

The different steps of the depth map analysis: Upper Left: A dense depth map is calculated through structure from motion; Upper Right: This depth map is thresholded in order to not take into account far away objects; Lower Left: The ground plane is removed from the depth map; Lower Right: The remaining part of the depth map is downprojected onto the ground plane, such that a one-dimensional function of relative depth over the image plane is obtained.

This process, involves the following steps:

A dense depth map is estimated by the structure from motion algorithm, as described in section 2.

The relative depth function f is a direct indicator of the presence of obstacles and is a function in the interval [0,1]. It can thus be directly related to the objective function controlling the robot heading: o^θ _OA (θ _robot ) = f.

The velocity of the robot should in general be as high as possible. Only when obstacles are near, the velocity should be decreased. This can be expressed as:

o O A v ( v ) = { ( v v max ) 2 if d obstacle > d threshold 1 1 + ( v v max ) 2 if d obstacle < d threshold

(24)

with d_obstacle = min(f), d_threshold a set threshold distance and v _max the maximum robot velocity.

The goal seeking behavior is assigned two tasks as well:

It points the robot to the goal position and must thus assign high values to actions that cause the robot to face the target and low values to actions that do the opposite.

Again, this means the development of the objective function can be split up as o _GS (v,θ _robot ) = o^θ _GS (θ _robot ).o ^v _GS (v).

To calculate these objective functions, the (Euclidian) distance to the goal d _goal and heading to this goal θ _goal are calculated from the current robot position given by the Visual SLAM algorithm, described in section 3, and the current waypoint given by the global path planner.

It is now obvious that the goal seeking behavior needs to minimize the difference between the robot heading θ _robot and the goal heading θ _goal , which we can formulate as:

o G S θ ( θ r o b o t ) = 1 1 + ( θ robot − θ goal β ) 2 .

(25)

with β the window size which is considered.

For o ^v _GS (v), a similar reasoning as been done before for o ^v _OA (v) can be performed, as the velocity should always be high, with the exception of near the goal positions. This can be expressed as:

o G S v ( v ) = { ( v v max ) 2 if d goal > d threshold 1 1 + ( v v max ) 2 if d goal < d threshold .

(26)

This methodology allows to compose the objective functions o _GS (v,θ _robot ) and o _OA (v,θ _robot ). The remaining problem is how to fuse these objectives to form one globally optimal command, leading to intelligent robot behavior.

As mentioned above in the introduction, the classical weight optimization methods for behavior fusion have some serious disadvantages. The first one is that they do not allow introducing some a priori knowledge from a human decision maker. This issue is addressed by so-called goal programming methods. Goal programming methods define a class of techniques for generating satisfying solutions also known as compromise solutions in this context. The decision maker gives his/her preferences in terms of weights, priorities, goals, and ideals. The concept of best alternative is then defined in terms of how much the actual achievement of each objective deviates from the desired goals or ideals. Further, the concept of best compromise alternative is defined to have the minimum combined deviation from the desired goals or ideals. Goal programming methods thus choose an alternative having the minimum combined deviation from the decision maker's ideal goals o*₁,…,o* _n , given the weights or priorities of the objective functions. This can be formulated as (Pirjanian, P., 1998):

arg ⁡ min x ∈ X ⁡ ∑ i = 1 n w i | o i ( x ) − o i ∗ | p .

(27)

where 1 ⩽ p ⩽ ∞, o* is the ideal goal, w _i is the weight or priority given to the i ^th objective.

x ^p _w is a solution to this equation for a given p and weight vector w = (w₁,…,w _n ) and represents an action to be performed.

A second disadvantage of the weight optimization which is still not solved by the goal programming method is that it does not take into account errors on the sensor measurements which will also make the output of a behavior which use this data less reliable. In (Doroftei, D.; Colon, E. & De Cubber, G., 2007), we proposed a method to choose the weights based upon a reliability measure associated to each behavior. The principle behind the calculation of the activity levels is that the output of a behavior should be stable over time in order to trust it. Therefore, the degree of relevance or activity is calculated by observing the history of the output of each behavior. This history-analysis is performed by comparing the current output ϖ j , k b i to a running average of previous outputs, which leads to a standard deviation, which is then normalized. For a behavior b _i with outputs j these standard deviations σ j b i are:

σ j b i = c j ∑ k = i − h i ( ϖ j , k b i − ∑ l = 1 N ϖ j , l b i N ) 2 ,

(28)

with c _j some normalization constants.

The bigger this standard deviation, the more unstable the output values of the behavior are, so the less they can be trusted. The same approach is followed all behaviors. This leads to an estimate for the activity levels or weights for each behavior:

w b i = ∑ j = 1 n u m b e r o f o u t p u t s ( 1 − σ j b i )

(29)

As such it is now possible to take into account the unreliability of the behavior output. On the other hand, this architecture again does not offer a human decision maker the ability to interact with the decision process.

One could thus argue that while reliability analysis-based approaches are too robot centric, these second set of approaches is too human-centric. In the following, we present an approach to integrate the advantages of both theorems. This can be achieved by minimizing the goal programming and reliability analysis constraints in an integrated way, following:

arg x ∈ X ⁡ min w i ∈ w [ λ ( ∑ i = 1 n w i | o i ( x ) − o i ∗ | p ) + ( 1 − λ ) ∑ i = 1 n | w i − ∑ j = 1 numberofoutputs ( 1 − σ j b i ) | p ] ,

(30)

with Λ a parameter describing the relative influence of both constraints. This parameter indirectly influences the control behavior of the robot. Large values of Λ will lead to a human-centered control strategy, whereas lower values will lead to a robot-centered control strategy. The value of Λ would therefore depend on the expertise or the availability of human experts interacting with the robot.

It is obvious that this method increases the numerical complexity of finding a solution to MODM, but this does not necessarily leads to increased processing time, as the search interval can be further reduced by incorporating constraints from both data sources.