Localization of a Vehicle: A Dynamic Interval Constraint Satisfaction Problem-Based Approach

Kueviakoe, Kangni; Wang, Zhan; Lambert, Alain; Frenoux, Emmanuelle; Tarroux, Philippe

doi:https://doi.org/10.1155/2018/3769058

Journal of Sensors

On this page

Abstract Introduction Results Conclusion Conflicts of Interest Authors’ Contributions References Copyright Related Articles

Research Article | Open Access

Volume 2018 | Article ID 3769058 | https://doi.org/10.1155/2018/3769058

Localization of a Vehicle: A Dynamic Interval Constraint Satisfaction Problem-Based Approach

Kangni Kueviakoe,¹Zhan Wang,²Alain Lambert,²Emmanuelle Frenoux,¹and Philippe Tarroux¹

Academic Editor: Oleg Lupan

Received13 Sept 2017

Revised23 Jan 2018

Accepted08 Feb 2018

Published10 Apr 2018

Abstract

This paper introduces a new interval constraint propagation (ICP) approach dealing with the real-time vehicle localization problem. Bayesian methods like extended Kalman filter (EKF) are classically used to achieve vehicle localization. ICP is an alternative which provides guaranteed localization results rather than probabilities. Our approach assumes that all models and measurement errors are bounded within known limits without any other hypotheses on the probability distribution. The proposed algorithm uses a low-level consistency algorithm and has been validated with an outdoor vehicle equipped with a GPS receiver, a gyro, and odometers. Results have been compared to EKF and other ICP methods such as hull consistency (HC4) and 3-bound (3B) algorithms. Both consistencies of EKF and our algorithm have been experimentally studied.

1. Introduction

This paper deals with outdoor vehicle ego-localization. Localization is part of many automotive applications where safety is of crucial importance. This estimation problem, which involves fusion of data from dead reckoning and absolute sensors, is generally solved by Bayesian approaches [1, 2]. Interval analysis-based methods are alternative and less known methods. These methods use either set inversion [3, 4] or constraint propagation techniques [5, 6] to deal with the localization problem and provide a set of intervals containing the vehicle pose (position and orientation) rather than probabilities.

Most of the early published papers on constraint programming can be dated back to the 1970s. They have mainly dealt with discrete domains [7]. In the 1980s, Gallaire [8] and Jaffar and Lassez [9] noted that logic programming can be seen as a particular type of constraint programming. Both logic programming and constraint programming imply that the user states what has to be solved instead of how to solve it. Many combinatorial problems such as scheduling and timetabling were then formulated in the form of constraint satisfaction problems (CSP).

Constraint propagation is the most used technique for solving CSPs. It combines consistent techniques and systematic search methods. Arc consistency could be achieved by the AC-3 (“AC” stands for “arc consistency”) algorithm [7] or by many other algorithms (like AC-5 [10], AC-6 [11], etc.) using only binary CSPs (each constraint involves at most two variables). However, many real-life problems can be naturally modeled as nonbinary CSPs: the constraint involves more than two variables [12]. Two approaches were developed to deal with nonbinary CSPs. The first one translates a nonbinary CSP into several binary CSPs [13, 14]. The second kind of approach directly deals with nonbinary constraints. For example, GAC-4 [15] is a generalization of AC-4 to nonbinary constraints.

In 1987, Cleary [16] and Davis [17] pioneered the use of constraint propagation for interval analysis. In 1989, a generalized constraint propagation scheme based on interval arithmetic was designed by Hyvönen [18]. Cleary [16] proposed a narrowing algorithm which was improved by Benhamou and Older [19]. Later, that algorithm was renamed as HC3 [20] because it is very close to the classical AC-3 algorithm. Next, Benhamou et al. proposed that HC4 [20] does not need constraints decomposition but it encounters the well-known dependency problem. To cope with this problem, BC3 [21] was developed. It overcomes the dependency problem at the cost of a huge computing time. BC4 [20] merges both BC3 and HC4 and reduces the computation time. BC5 [22], by using the interval Gauss-Seidel method, further reduces the computing time. All the preceding methods (HC4, BC3, BC4, and BC5) work with one constraint at a time. They compute the variables of a constraint locally without taking into account the whole constraint system. They are then named “local consistency algorithms.” By then, they do not reach the optimal contraction for all the domains of variables of the problem. This leads to a problem called “locality problem.” To take into account all the constraints of a given problem at a time, strong consistency algorithms such as the 3B algorithm [23] have been proposed. Strong consistency algorithms seek the strength of the domain contractions rather than the shortness of computation time.

Interval constraint propagation (ICP) was used to solve the robotic localization problems by Gning and Bonnifait [6, 24] more than ten years ago. It was mainly used for outdoor vehicle localization [25, 26] and underwater robot localization [3, 5]. Those works use a forward-backward propagation technique based on primitive constraints (following the principle of the Waltz algorithm [27]). Drevelle and Bonnifait [28] use a relaxed constraint propagation approach to deal with erroneous GPS measurements. Lassoued and Bonnifait [29] combine constraint propagation and set inversion techniques and present a cooperative localization method with significant enhancement in terms of accuracy and confidence domains. Some other researchers propose specific contractors [30] or separators [31] to solve the localization problem. Tubes are used in scenarios where the system is described by a continuous time state equation. This tool allows to both localize the robot and correct its clock. The readers might refer to [32, 33] to see more details about the application of tubes in the robotic area. The main advantage of ICP over Bayesian algorithms is that it guarantees that the position of the robot is contained in a box. Bayesian algorithms [1] can only associate a probability to such a box. Consequently, safety cannot be guaranteed by Bayesian algorithms.

A previous work [34] demonstrated that classical forward-backward ICP do not reduce the heading imprecision for a vehicle equipped with a GPS receiver, a gyro, and odometers. Another study [35] concluded that various ICP algorithms ([16, 20, 21, 23]) have the same drawback. Only the 3B consistency algorithm [23] (based on a strong consistency technique) can reduce the heading imprecision of the vehicle during its displacement. The price to pay is an increase in the computing time.

We improved previous works by proposing a new ICP method for vehicle localization with heading correction and real-time capabilities. The proposed algorithm employs HC4 as a low-level algorithm. It is easier to be used than previous ones as it does not need to decompose all the constraints into elementary/binary constraints. Our algorithm has been validated with an outdoor vehicle equipped with a GPS receiver, a gyro, and odometers. The results obtained by our method are compared with three algorithms by using a very accurate positioning system as ground truth. This paper includes and extends results previously presented in [36].

Section 2 introduces constraint propagation and interval analysis. Two ICP algorithms (HC4 and 3B) are discussed in Section 3. A new localization problem formalization and its solving are presented in Section 4 while Section 5 shows our experimental results.

2. Overview of Interval Analysis and Constraint Propagation

Interval analysis and constraint propagation are the two main mathematical concepts on which our work is based. A brief overview of these methods is given in this section.

2.1. Interval Analysis

Interval analysis was introduced in the 1960s by Moore [37–39] in order to deal with approximation problems encountered during calculation. The main idea is to represent numbers by intervals which include their values. An interval is a connected subset of , defined by its lower bound and upper bound . The following notations are used as shorthands to denote closed/open interval: and . When no confusion may arise, is used to represent an interval whether it is closed or not. The width of a nonempty interval is defined by .

Rules have been defined to perform every usual arithmetical operation on intervals [38, 39]. Considering two intervals and and a binary operator , the smallest interval which contains all feasible values for is defined as

The set-theoretic operations can be applied to intervals. The intersection of two intervals is defined by which is always an interval. This is not the case for their union

Consequently, an interval hull is defined as the smallest interval that contains a subset of . For example, the interval hull of is the interval . The interval hull of the union of and is denoted by .

2.2. Interval Extension

We often need to compute the image of a number by a function. One purpose of interval analysis is to provide, for a large variety of functions , interval extensions (also called “inclusion function”) that can be evaluated reasonably quickly and such that is not too large. For any function defined with arithmetical operators and elementary functions,

To create an inclusion function, it is possible to replace all the variables by their intervals and then all the included operators by their interval equivalents. The resulting interval function is called a “natural inclusion function.” For instance, has the natural inclusion function

An interval extension can be used to represent an equation based on interval variables. Such equations, also called constraints, are the core of an interval-based constraint satisfaction problem which is covered by the next subsection.

2.3. Contractor

The concept of the contractor is directly inspired by the ubiquitous concept of filtering algorithms in constraint programming. Given a constraint relating a set of variable , an algorithm is called a contractor (or filtering algorithm) if returns a subdomain of the input domain and the resulting subdomain contains all the feasible points with respect to the constraint :

2.4. Interval Constraint Satisfaction Problem (ICSP)

The concept of interval constraint satisfaction problem (ICSP) was introduced by Hyvönen [40] in 1992. An interval constraint satisfaction problem (ICSP) or interval CSP is a mathematical problem solved by finding states satisfying the constraints. An ICSP is defined by (i)a set of variables ,(ii)a set of domains , such as for each variable , a domain with the possible values for that variable is given and is an interval or union of intervals;(iii)a set of constraints , defining the relations between a subset of variables, restricting the possible solutions.

Hyvönen et al. proposed, in 1993, “INC++” [41], a basic interval processing tool which allows evaluating interval functions and aims at solving ICSP. Many other interval solvers such as Numerica [42], RealPaver [43], ALIAS [44], ICOS [45], GloptLab [46], and IBEX (IBEX is available at http://www.ibex-lib.org/) [47, 48] have been designed so far.

Interval constraint propagation (ICP) consists in iterating domain reductions, by using the set of constraints, until no domain can be contracted. Many kinds of CSP have been introduced in order to express specific situations. The concept of “dynamic CSP” (DCSP) was introduced by Dechter [49]. It represents a CSP that is evolving or changing with the time. This means that at any time, new constraints can be added to the constraint network and other constraints can be removed from the CSP. The addition (resp. deletion) operation is also known as “restriction” (resp. “relaxation”). An interval CSP which is dynamic is consequently a “dynamic ICSP” or DICSP for short.

2.5. Interval Vector

Interval vector is a generalization of the interval’s concept: an interval vector, also called “a box,” has intervals as components. It is a Cartesian product of intervals. The width of an interval vector can be computed in order to estimate its size.

A vehicle pose is represented by 3 parameters: x-axis and y-axis values and the heading information . Consequently, for vehicle localization, the solution is a three-dimensional interval vector. Interval constraint propagation (ICP) algorithms process interval vectors as the variables of an interval constraint satisfaction problem. We use these ICP algorithms to solve the vehicle localization problem which we stated as an ICSP. The next section will present two ICP algorithms.

3. ICP Algorithms

In this section, we introduce two interval constraint propagation (ICP) algorithms. Previous studies [35] have shown that HC4 is the fastest one and 3B is the one that gives the best contraction of domains. We do not introduce BC5 which is slower than HC4 and unnecessary because of no dependency inside our constraints.

3.1. The HC4 Algorithm

HC4 [20] enforces hull consistency over complex constraints without decomposing them into primitive ones. Hull consistency [20] is ensured, for a constraint , with regard to an interval vector , if and only if where is a function computing an approximation of the smallest interval vector containing its argument and denotes the underlying relation of the given constraint . If is an ICSP involving an interval vector , is hull consistent if each constraint of is hull consistent. Consequently, cannot be reduced anymore by applying . Hull consistency is also called 2B consistency [50]. The interval vector is the Cartesian product of intervals: . HC4 follows a loop propagation and processes constraints individually using the HC4 revise function. HC4 revise reduces the domains by removing inconsistent values and returns the new narrowed domains to HC4.

HC4 revise uses a binary tree representation of constraints, where leaves are constants or variables and nodes represent elementary operation symbols such as , , and . HC4 revise works in two phases: (i)The “forward evaluation phase” goes through the tree, from the leaves to the root. It evaluates recursively the interval of subexpression represented by the current node using the natural extensions of the underlying functions.(ii)The “backward propagation phase” traverses the tree from the root to the leaves applying on each node a contraction operator (a projection). The contraction operator narrows the interval of the current node by removing inconsistent values.

The main limitation of the hull consistency algorithms (HC4 and HC3) is their sensitivity to multiple occurrences of variables [21]. This is referred as the dependency problem. When considering, for instance, the constraint , with , hull consistency algorithms would not reduce the initial box, but would perfectly reduce it when the constraint is stated like .

The 3-bound consistency algorithm overcomes the dependency problem and the locality problem as described in Section 1.

3.2. The 3B Algorithm

3B algorithm [23] also referred as the “strong consistency algorithm” computes the projection of the sets of constraints over the variables: it combines constraints in order to improve the precision of domain narrowing. The principle of 3B is that a variable is instantiated to one of its interval bounds and the remaining problem is solved in a hull or box consistency way: 3B uses a low level algorithm (for instance BC4) in order to contract the intervals. Let be an ICSP, a variable of , and the domain of . is 3B consistent if and only if and are both nonempty where and with denoting the enclosure of . is 3B consistent if all the domains of variables are 3B consistent.

Example 1. Let us consider that the constraints of is given as follows: with and . For each instantiation of , we have a 2B-consistent subproblem to solve (see Table 1). For each line of Table 1, the solution domain is not empty, which indicates that the domain of variable is 3B consistent. The same method is used for the variable and ultimately proves that the ICSP is 3B consistent.
For further information about 3B consistency methods, the reader might refer to [23].

After presenting the mathematical tools required to understand our approach, let us now deal with the localization problem itself.

4. The Localization Problem

4.1. Problem Statement

The localization problem consists of estimating in a continuous way the position and heading information of an autonomous mobile system. Figure 1 depicts a localization situation. The goal is to determine the current pose , , and knowing the previous pose , , and . It is a state estimation of a nonlinear dynamic system. Our vehicle is equipped with two proprioceptive sensors (gyro and odometers) and an exteroceptive sensor (GPS receiver). The elementary displacement and rotation ( and ) are given by a static fusion process which involves (see [24, 26] for more details) (10), (11), and (12). Then, they can be directly used in a dynamic fusion process (Algorithm 1) to estimate the vehicle location when a GPS measurement is available.

4.1.1. Gyro

A gyro is a heading sensor which measures the rotational speed in an inertial reference system. It uses the Coriolis force to output a yaw rate from which we deduce the elementary rotation and its associated interval : where is the error on .

4.1.2. Odometers

Odometers are set on both rear wheels of our vehicle. They give the distance traveled by each wheel independently. The accuracy of an odometer relies on its number of steps and its maximum error. In our case, the maximum error is known to be one step. Thus the value of the displacement of a nonsliding wheel can be bounded by where stands for the number of measured steps. Then, the movement of a sliding wheel can be deduced from a nonsliding one by adding a sliding noise . Consequently, the displacement considering a sliding wheel is defined by

The elementary displacement of the vehicle is then obtained by where and , respectively, stand for the radius of the right and left wheel and represents the odometer resolution.

4.1.3. GPS Receiver

The Global Positioning System (GPS) is a global navigation satellite system that provides geolocation and time information to a GPS receiver anywhere on or near the Earth. When a GPS receiver reads the transmission of three or more satellites, it calculates the arrival time differences and its relative distance to each satellite. Our GPS receiver performs the necessary calculation and returns latitude and longitude coordinates which are converted as a position in a Cartesian local frame. Furthermore, the GPS receiver computes the measurement imprecision on-line and sends it into the GST NMEA frame.

For further information about sensors, the reader might refer to [51].

Once we have constituted the intervals from sensor data, we can define a nonlinear dynamic-bounded error model to build constraints of a DICSP, as presented in the next section.

4.2. A Dynamic Interval Constraint Satisfaction Problem (DICSP)

We propose to define the localization problem as a dynamic interval constraint satisfaction problem: at time , a new ICSP (, , ) is generated and defined as follows: (i)a set of variables, , (ii)a set of domains, , (iii)a set of constraints, , where represents the set of constraints deduced from the vehicle displacement model proposed by Seignez et al. [52]. The domains of variables are defined as follows: (i) and are given by the odometer and gyro measurements thanks to (12) and (10).(ii) and are initialized with GPS measurements (see (13)).(iii) is initialized to .(iv), , and are obtained from the solution of the previous CSP (at time ).

We consider all the state equations from time to time (where is the current time and is the window size): the contractions are performed in a sliding window of length . In other words, we use sets of equations (current and past information) in our interval constraint satisfaction problem in order to localize the vehicle. At each time , we add a new constraint set to the existing constraint sets. If , we delete the oldest constraint set from it. The addition (resp. deletion) operation is also known as “restriction” (resp. “relaxation”).

The localization DICSP (dynamic ICSP) keeps the same number of constraints () but changes temporally. The localization DICSP is defined at time by

ICP algorithms allow us to solve . For our experiment, some local consistency algorithms including HC4 and the strong consistency 3B algorithm have been used.

We compared many ICP algorithms for solving our DICSPs. The best results are obtained by the 3B algorithm [35]. Unfortunately, 3B is too slow to work in a real-time environment. And consequently, it cannot be used to solve our real-time problem. We noticed that apart from 3B, no algorithms can contract the parameter [36], as explained detailedly in the next section.

4.3. Uncorrected Yaw Problem

Vincke and Lambert [34] had shown that classical interval propagation algorithms can not reduce the heading uncertainty of a vehicle equipped with a GPS receiver, a gyro, and odometers. Let us consider a simple forward-backward propagation algorithm, the contraction of the heading uncertainty is performed by (19), which is deduced from (17) in a backward way.

Example 2. Considering the case study with a straight displacement along the -axis, the orientation variation is null . The correction on is realized by and .
The width of is around m in most of our experiments: . Thus, we can get where is a small reduction of the imprecision due to a new GPS measurement. is around m:Finally, The value inside the function is far beyond , leading to a full ignorance on the orientation. The same reasoning leads to the same result for the function. Thus, (19) is unable to correct the orientation.

During the vehicle’s displacement, the interval representing the heading uncertainty will grow continuously although other parameters of the pose are contracted. It is a big issue because the heading information is important for a good estimation of the vehicle’s position on the -axis and -axis. Reference [34] explained the problem but gives no solution. Let us notice that their approach needed to decompose all the constraints of the problem into elementary/binary constraints.

The uncorrected yaw problem is observed only with local consistency algorithms which solve constraints one by one. They are subject to the “locality problem”: the solution is always local with regard to one constraint at a time. Only a strong consistency algorithm (3B for instance) can overcome this problem and correct the yaw. Unfortunately, strong consistency algorithms have a huge computing time and are not suited for real-time issue [53]. We propose to solve this problem by introducing a new algorithm which splits the parameter and creates multiple DICSP (called DICSP). The next section details our method.

4.4. A Multiple Dynamic Interval Constraint Satisfaction Problem

In order to correct the yaw, we propose to split the variable into variables where stands for the “splitting factor.”

Each domain is then defined by where , is the width of each , and represents the width of the interval . and are, respectively, the round down and the round up of the number .

Example 3. To illustrate, let us consider . If is the “splitting factor,” then, the following intervals will be generated: .

In the first step of the algorithm, and is undefined: the splitting is realized on . is the union of all the :

Other domains remain unchanged (unsplitted). The proposed splitting leads to a DICSP: with

Ensure: DICSP_k
(1) () ← Split(,v)
(2) fori ← 1 : v do
(3) ← Build(DICSP_k,)
(4) [] ← Solve()
(5) end for
(6) [X_res] ← Hull()
(7) return [X_res]

The DICSPs (columns of the DICSP) are separately solved by a local consistency algorithm. The solution of the localization problem is given by applying the interval hull function on the results obtained from the first row of the . It defines the current position and orientation of the vehicle as an interval vector . When new measurements are available, the next localization process starts by applying the interval hull function on the results of the last row of the (first step before splitting).

Sometimes, one (or more) DICSP does not get a solution: one of the domains is empty. This is an expected behavior that enables fast correction by eliminating a whole set of erroneous pose.

The building and the solving of the multiple dynamic interval constraint satisfaction problem are realized by our proposed dynamic interval constraint propagation with the splitting algorithm (DICSP, see Algorithm 1), explained hereafter: (i)Line 1: the oldest variable , representing the orientation in the initial DICSP, is split into intervals.(ii)Line 3: for each , a DICSP is generated.(iii)Line 4: each DICSP is submitted to a solving process (by an ICP algorithm).(iv)Line 6: the new domains, resulting from the process of the preview step, are grouped with an interval hull function.(v)Line 7: the interval hull constitutes an interval vector which is returned as the vehicle position.

This algorithm has been implemented and tested on a vehicle running on a test track as explained in the next section.

4.5. About Completeness

“The two most appealing features of interval analysis and numerical constraint propagation, when used to solve numerical problems, are completeness and rigor. Completeness means the ability to find all solutions, whereas rigor is the ability to control the rounding errors due to floating-point computation” [54]. The rigor property is kept by our algorithm as we only used interval operators. The completeness is discussed because our algorithm splits the variables before applying classical constraint propagation techniques. We propose a new ICP algorithm with splitting: it splits an interval vector into boxes , so that

Then, the range of a function over , possibly defined by (17), can be evaluated by

The bisection usually leads to an improved estimate. Our algorithm takes advantage of this property and combine it with DICSP to solve the uncorrected yaw problem by splitting the parameter . According to the interval extension definition given in Section 2.2, we have

And thanks to the definition of interval hull (see Section 2.2), we obtain

Since is a genuine superset of the left-hand side, all correct answers are included in it. Furthermore, each term of the interval hull is solved by a local consistency algorithm which ensures that none of the solutions are removed (thus achieving the completeness of our method).

5. Results

This section introduces the tools and equipment (vehicle prototype with its sensors) that we used to validate our method as well as the test track where data have been collected. Our algorithm has been compared with 2 interval algorithms and a Bayesian one.

5.1. Experimental Setup

The proposed algorithm has been implemented in C language on a computer equipped with an Intel Core i7 CPU 960 @ 3.20 GHz running under Ubuntu 14.04 LTS. Solving has been realized with a set of timestamped data that has been collected on the Satory test track (Figure 2(c)) with LIVIC’s prototype (Figures 2(a) and 2(b)) running at an average speed of 50 km/h. A solid-state vertical gyro VG400CC provides the yaw rate data. The global localization is performed by an AgGPS132 (datasheet: http://dragadoshidraulicos.com/batimetria/GPS-Trimble-AGP132.pdf). GPS measurements and gyro/odometer data acquisitions are realized at a 5 Hz frequency. The centimeter reference used for the evaluation of the positioning method is provided by a RTK GPS (datasheet: http://www.ctsystems.eu/Resources/Sagitta.pdf).

(a)

(b)

(c)

5.2. Comparison between DICPS, HC4, and 3B Algorithms

A full lap run last 400 seconds while 2000 localization DICSPs have been solved under different window sizes (). Figure 3(a) shows how the value of the splitting factor influences the correction of heading imprecision in DICPS. The efficiency of the splitting decreases as a hyperbolic function, indicating that the value of the splitting factor can be bounded around 20. Nevertheless, the higher both variables are ( and ), the lower is the localization error.

(a)

(b)

Computing time has been measured for HC4, 3B, and our DICPS algorithm. Table 2 shows that HC4 is the fastest, but Figure 3(b) indicates its inability to correct the heading imprecision: the heading imprecision increases linearly as the time goes by.

This is a common result obtained by local consistency algorithms such as HC3 [19], BC3 [21], BC4 [20], and BC5 [22]. DICPS results are observed with three window sizes (20, 40, and 100) depicted in Figure 3(b). The heading imprecision is stabilized between 10 and 35 degrees depending on the parameters. It shows that the window size is an important parameter that improves the correction of heading imprecision.

3B consistency considers the bounds of each variable’s domain. It tends to calculate the solution of the localization DICSP for each bound (see Section 3.2). If no solution is found, the bound is eliminated from the considered domain and so on. We use 3B with the same window size than DICPS: . The observed results are the same while 3B computing time is much larger than the DICPS one: 5257 ms for 3B versus 13.32 ms for DICPS (see Table 2). Different window sizes give the same localization results but with a 3B prohibitive computing time.

If the window size is increased to 40 while keeping the same splitting (), then the computing time of DICPS is 172 ms. Thus, it is still real-time while ensuring a 16° (±8°) imprecision in average. This imprecision is below the value provided by other interval approaches (including CP and BESE [55]) in the same environment.

5.3. Comparison between DICPS and EKF

The previous section shows that DICPS outdoes the other methods based on interval analysis. This section aims to compare DICPS with a wide-spread Bayesian filter: the extended Kalman filter (EKF).

5.3.1. Theoretical Comparison

DICPS considers bounded errors (with no assumption on the error inside the bound) while EKF deals with Gaussian errors. DICPS returns interval vectors which contain the vehicle’s pose (it searches all solutions that are consistent with the model and data of the given problem). EKF gives a vector (that has the highest probability to be the solution) and its associated Gaussian uncertainty, represented by a matrix.

5.3.2. Experimental Comparison

We set up DICPS with parameters and . In order to ensure fairness, EKF and DICPS use the same data set.

The EKF is fed with the standard deviation of the sensors whereas DICPS uses a error bound which corresponds to a 0.99 confidence level. Consequently, where we should consider finding a data with a 0.99 probability, the DICPS assumes to find it with a 1 probability.

Figure 4(a) shows the interval error of the DICPS and the EKF. The interval error is defined by the upper and lower bounds of the estimated state minus the corresponding bound reference state. A filter exhibits good results if its interval error is thin and always includes the reference value zero. The imprecision area of DICPS (using a error bound) has directly been used to compute the interval error. For the EKF, the imprecision (standard deviation) has been magnified 3 times.

(a)

(b)

The EKF is more confident (has a thinner corridor) than DICPS, but the EKF is sometimes inconsistent (does not include the zero value) whereas DICPS is always consistent. The large peaks (around t = 100, 150, 200, 340, and 370) are caused by GPS measurements with large imprecision. The top peaks define steps when the imprecision of the robot state has increased before being reduced thanks to GPS measurement. The great inconsistency of the EKF around s is due to a large number of repeated biased GPS measurements. The EKF greatly underestimates its covariance matrices when dealing with repeated biased measurements. DICPS does not suffer from those biased measurements because it has no assumption on the distribution of the noise (contrarily to the EKF).

We could get a consistent EKF by tuning carefully (enlarging) the noise value for each simulation (we can do it a posteriori by using the reference). However, such an EKF tuning does not seem feasible for the localization problem in the real world because we do not have such a reference (if we have such a reference then computing the localization will be useless). Nevertheless, as part of this paper, we have such a reference and we have computed the value for the EKF to be consistent. We found 17 and drew the interval error in Figure 4(b). Thus, we can compare both consistent filters. The resulting EKF corridors are larger than those of the consistent DICPS. Consequently, even if it was possible to compute the value during the localization, the EKF results would not be better than the results of DICPS.

6. Conclusion

This paper deals with the vehicle localization problem using interval analysis. Different algorithms have been compared on a vehicle equipped with a GPS receiver, a gyro, and odometers. Every algorithm can correct the position. But under real-time constraints, no ICP algorithms were able to correct the vehicle’s orientation. Only 3B was capable of orientation correction but at the cost of a huge computing time.

Our algorithm (DICPS) is able to correct the orientation and is real-time executable. As the computation time increases with the splitting factor, we recommend to conduct DICPS with parameters and in order to keep a reasonable calculation time (172 ms for each localization step). DICPS has been shown to reduce the orientation imprecision around ±8. This method is robust and provides consistent results.

DICPS has been compared to the well-known extended Kalman filter. We point out that the EKF can locally converge towards a wrong solution due to bias measurements which lead to a huge local inconsistency. We claim that consistency is the biggest advantage of DICPS over Bayesian approaches.

Future works will deal with improvements to further reduce the imprecision and minimizing the CPU load. It can be achieved by dynamically defining the involved parameters (window size and splitting factor). In addition, no change or partial change on variables will allow us to simplify the localization problem by removing constraints. Finally, small changes could be stacked up, waiting for a significant amount of change to start the constraint propagation process.

Conflicts of Interest

The authors declare that they have no conflicts of interest.

Authors’ Contributions

Kangni Kueviakoe and Zhan Wang contributed equally to this work.

References

D. Gruyer, A. Lambert, M. Perrollaz, and D. Gingras, “Experimental comparison of Bayesian vehicle positioning methods based on multi-sensor data fusion,” International Journal of Vehicle Autonomous Systems, vol. 12, no. 1, pp. 24–43, 2013.
View at: Google Scholar
S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics (Intelligent Robotics and Autonomous Agents Series), The MIT Press, Cambridge, MA, USA, 2005.
L. Jaulin, “Robust set-membership state estimation; application to underwater robotics,” Automatica, vol. 45, no. 1, pp. 202–206, 2009.
View at: Publisher Site | Google Scholar
E. Colle and S. Galerne, “A multihypothesis set approach for mobile robot localization using heterogeneous measurements provided by the Internet of Things,” Robotics and Autonomous Systems, vol. 96, pp. 102–113, 2017.
View at: Publisher Site | Google Scholar
L. Jaulin, “Localization of an underwater robot using interval constraint propagation,” in International Conference on Principles and Practice of Constraint Programming, pp. 244–255, Nantes, France, 2006.
View at: Publisher Site | Google Scholar
A. Gning and P. Bonnifait, “Constraints propagation techniques on intervals for a guaranteed localization using redundant data,” Automatica, vol. 42, no. 7, pp. 1167–1175, 2006.
View at: Publisher Site | Google Scholar
A. K. Mackworth, “Consistency in networks of relations,” Artificial Intelligence, vol. 8, no. 1, pp. 99–118, 1977.
View at: Publisher Site | Google Scholar
H. Gallaire, “Logic programming: further developments,” in IEEE Symposium on Logic Programming, pp. 88–99, Boston, MA, USA, 1985.
View at: Google Scholar
J. Jaffar and J. L. Lassez, “Constraint logic programming,” in Proceedings of the 14th ACM SIGACT-SIGPLAN Symposium on Principles of Programming Languages - POPL '87, pp. 111–119, Munich, Germany, 1987.
View at: Publisher Site | Google Scholar
P. van Hentenryck, Y. Deville, and C. M. Teng, “A generic arc-consistency algorithm and its specializations,” Artificial Intelligence, vol. 57, no. 2-3, pp. 291–321, 1992.
View at: Publisher Site | Google Scholar
C. Bessiere, “Arc-consistency and arc-consistency again,” Artificial Intelligence, vol. 65, no. 1, pp. 179–190, 1994.
View at: Publisher Site | Google Scholar
Z. Yuanlin and R. Yap, “Arc consistency on n-ary monotonic and linear constraints,” in International Conference on Principles and Practice of Constraint Programming, pp. 470–483, Singapore, 2000.
View at: Publisher Site | Google Scholar
R. Dechter and J. Pearl, “Tree clustering for constraint networks,” Artificial Intelligence, vol. 38, no. 3, pp. 353–366, 1989.
View at: Publisher Site | Google Scholar
F. Rossi, C. Petrie, and V. Dhar, “On the equivalence of constraint satisfaction problems,” in Proceedings of the 9th European Conference on Artificial Intelligence (ECAI'90), pp. 550–556, Stockholm, Sweden, 1990.
View at: Google Scholar
R. Mohr and G. Masini, “Good old discrete relaxation,” in Proceedings of the 8th European Conference on Artificial Intelligence (ECAI'88), pp. 651–656, Munich, Germany, 1988.
View at: Google Scholar
J. Cleary, “Logical arithmetic,” Future Computing Systems, vol. 2, pp. 125–149, 1987.
View at: Google Scholar
E. Davis, “Constraint propagation with interval labels,” Artificial Intelligence, vol. 32, no. 3, pp. 281–331, 1987.
View at: Publisher Site | Google Scholar
E. Hyvönen, “Constraint reasoning based on interval arithmetic,” in Proceedings of the 11th International Joint Conference on Artificial Intelligence (IJCAI'89), vol. 2, pp. 1193–1198, Detroit, MI, USA, 1989.
View at: Google Scholar
F. Benhamou and W. J. Older, “Applying interval arithmetic to real, integer, and Boolean constraints,” The Journal of Logic Programming, vol. 32, no. 1, pp. 1–24, 1997.
View at: Publisher Site | Google Scholar
F. Benhamou, F. Goualard, L. Granvilliers, and J. F. Puget, “Revising hull and box consistency,” in Proceedings of the 1999 International Conference on Logic Programming, pp. 230–244, Las Cruces, NM, USA, 1999.
View at: Google Scholar
F. Benhamou, D. McAllester, and P. V. Hentenryck, “CLP(intervals) revisited,” in Proceedings of the 1994 International Symposium on Logic Programming, pp. 124–138, Melbourne, VIC, Australia, 1994.
View at: Google Scholar
L. Granvilliers, “Towards cooperative interval narrowing,” in International Symposium on Frontiers of Combining Systems, pp. 18–31, Nancy, France, 2000.
View at: Publisher Site | Google Scholar
O. Lhomme, “Consistency techniques for numeric CSPs,” in Proceedings of the 13th International Joint Conference on Artifical Intelligence (IJCAI'93), vol. 1, pp. 232–238, Chambery, France, 1993.
View at: Google Scholar
A. Gning and P. Bonnifait, “Guaranteed dynamic localization using constraints propagation techniques on real intervals,” in IEEE International Conference on Robotics and Automation, 2004, pp. 1499–1504, New Orleans, LA, USA, 2004.
View at: Publisher Site | Google Scholar
B. Vincke, A. Lambert, D. Gruyer, A. Elouardi, and E. Seignez, “Static and dynamic fusion for outdoor vehicle localization,” in 2010 11th International Conference on Control Automation Robotics & Vision, pp. 437–442, Singapore, 2010.
View at: Publisher Site | Google Scholar
B. Vincke and A. Lambert, “Enhanced constraints propagation for guaranteed localization predictive step,” in IEEE/RSJ IROS, Workshop on Planning, Perception and Navigation for Intelligent Vehicles, pp. 30–36, St Louis, MO, USA, 2009.
View at: Google Scholar
D. L. Waltz, Generating Semantic Descriptions from Drawings of Scenes with Shadows, [Ph.D. thesis], AI Lab, MIT, Cambridge, MA, USA, 1972.
V. Drevelle and P. Bonnifait, “Robust positioning using relaxed constraint-propagation,” in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 4843–4848, Taipei, Taiwan, 2010.
View at: Publisher Site | Google Scholar
K. Lassoued, P. Bonnifait, and I. Fantoni, “Cooperative localization with reliable confidence domains between vehicles sharing GNSS pseudoranges errors with no base station,” IEEE Intelligent Transportation Systems Magazine, vol. 9, no. 1, pp. 22–34, 2017.
View at: Publisher Site | Google Scholar
B. Desrochers and L. Jaulin, “A minimal contractor for the polar equation: application to robot localization,” Engineering Applications of Artificial Intelligence, vol. 55, pp. 83–92, 2016.
View at: Publisher Site | Google Scholar
L. Jaulin, A. Stancu, and B. Desrochers, “Inner and outer approximations of probabilistic sets,” in Proceedings of the American Society of Civil Engineers (ASCE) Second International Conference on Vulnerability and Risk Analysis and Management and Sixth International Symposium on Uncertainty Modelling and Analysis, pp. 13–16, Liverpool, UK, 2014.
View at: Publisher Site | Google Scholar
C. Aubry, R. Desmare, and L. Jaulin, “Loop detection of mobile robots using interval analysis,” Automatica, vol. 49, no. 2, pp. 463–470, 2013.
View at: Publisher Site | Google Scholar
F. Le Bars, J. Sliwka, L. Jaulin, and O. Reynet, “Set-membership state estimation with fleeting data,” Automatica, vol. 48, no. 2, pp. 381–387, 2012.
View at: Publisher Site | Google Scholar
B. Vincke and A. Lambert, “Experimental comparison of bounded-error state estimation and constraints propagation,” in 2011 IEEE International Conference on Robotics and Automation, pp. 4724–4729, Shanghai, China, 2011.
View at: Publisher Site | Google Scholar
I. K. Kueviakoe, A. Lambert, and P. Tarroux, “Comparison of interval constraint propagation algorithms for vehicle localization,” Journal of Software Engineering and Applications, vol. 05, no. 12, pp. 157–162, 2012.
View at: Publisher Site | Google Scholar
I. K. Kueviakoe, A. Lambert, and P. Tarroux, “Vehicle localization based on interval constraint propagation algorithms,” in 2013 IEEE International Conference on Cyber Technology in Automation, Control and Intelligent Systems, pp. 179–184, Nanjing, China, 2013.
View at: Publisher Site | Google Scholar
R. E. Moore, Interval Analysis, Prentice-Hall, Englewood Cliffs, NJ, USA, 1966.
R. E. Moore and F. Bierbaum, Methods and Applications of Interval Analysis, vol. 2, Society for Industrial Mathematics, Philadelphia, PA, USA, 1979.
R. E. Moore, R. B. Kearfott, and M. J. Cloud, Introduction to Interval Analysis, Society for Industrial Mathematics, Philadelphia, PA, USA, 2009.
E. Hyvönen, “Constraint reasoning based on interval arithmetic: the tolerance propagation approach,” Artificial Intelligence, vol. 58, no. 1-3, pp. 71–112, 1992.
View at: Publisher Site | Google Scholar
E. Hyvönen, S. D. Pascale, and A. Lehtola, “Interval constraint satisfaction tool INC++,” in Proceedings of 1993 IEEE Conference on Tools with Al (TAI-93), pp. 298–305, Boston, MA, USA, 1993.
View at: Publisher Site | Google Scholar
P. V. Hentenryck, Numerica: A Modeling Language for Global Optimization, MIT Press, Cambridge, MA, USA, 1997.
L. Granvilliers and F. Benhamou, “Algorithm 852: RealPaver: an interval solver using constraint satisfaction techniques,” ACM Transactions on Mathematical Software, vol. 32, no. 1, pp. 138–156, 2006.
View at: Publisher Site | Google Scholar
J. P. Merlet, “ALIAS: an interval analysis based library for solving and analyzing system of equations,” in Séminaire Systèmes et Equations Algébriques, pp. 1964–1969, INRIA Sophia Antipolis, Valbonne, France, 2000.
View at: Google Scholar
Y. Lebbah, “ICOS: a branch and bound based solver for rigorous global optimization,” Optimization Methods and Software, vol. 24, no. 4-5, pp. 709–726, 2009.
View at: Publisher Site | Google Scholar
F. Domes, “GLOPTLAB: a configurable framework for the rigorous global solution of quadratic constraint satisfaction problems,” Optimization Methods and Software, vol. 24, no. 4-5, pp. 727–747, 2009.
View at: Publisher Site | Google Scholar
G. Chabert, “IBEX, an interval-based explorer,” 2007, http://www.ibex-lib.org/.
View at: Google Scholar
G. Chabert and L. Jaulin, “Contractor programming,” Artificial Intelligence, vol. 173, no. 11, pp. 1079–1100, 2009.
View at: Publisher Site | Google Scholar
R. Dechter and A. Dechter, “Belief maintenance in dynamic constraint networks,” in Proceedings of the Seventh AAAI National Conference on Artificial Intelligence (AAAI'88), pp. 37–42, Saint Paul, MN, USA, 1988.
View at: Google Scholar
H. Collavizza, F. Delobel, and M. Rueher, “A note on partial consistencies over continuous domains,” in International Conference on Principles and Practice of Constraint Programming, pp. 147–161, Pisa, Italy, 1998.
View at: Publisher Site | Google Scholar
H. R. Everett, Sensors for Mobile Robots: Theory and Application, AK Peters, Natick, MA, USA, 1995.
E. Seignez, M. Kieffer, A. Lambert, E. Walter, and T. Maurin, “Experimental vehicle localization by bounded-error state estimation using interval analysis,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1084–1089, Edmonton, AB, Canada, 2005.
View at: Publisher Site | Google Scholar
I. K. Kueviakoe, A. Lambert, and P. Tarroux, “A real-time interval constraint propagation method for vehicle localization,” in 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013), pp. 1707–1712, The Hague, Netherlands, 2013.
View at: Publisher Site | Google Scholar
C. Jermann, Y. Lebbah, and D. Sam-Haroud, “Interval analysis, constraint propagation and applications,” in Trends in Constraint Programming, F. Benhamou, N. Jussien, and B. A. O’Sullivan, Eds., John Wiley & Sons, Hoboken, NJ, USA, 2007, Chapter 4.
View at: Google Scholar
A. Lambert, D. Gruyer, B. Vincke, and E. Seignez, “Consistent outdoor vehicle localization by bounded-error state estimation,” in 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 1211–1216, St. Louis, MO, USA, 2009.
View at: Publisher Site | Google Scholar

Copyright

Copyright © 2018 Kangni Kueviakoe et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

PDF Download Citation

Download other formats

Order printed copies

Views

2501

Downloads

1016

Citations