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 x is a connected subset of R, defined by its lower bound x¯ and upper bound x¯. The following notations are used as shorthands to denote closed/open interval: x¯,x¯=x∈R∣x¯≤x≤x¯ and x¯,x¯=x∈R∣x¯≤x<x¯. When no confusion may arise, x is used to represent an interval whether it is closed or not. The width of a nonempty interval x is defined by wx=x¯−x¯.
Rules have been defined to perform every usual arithmetical operation on intervals [38, 39]. Considering two intervals x and y and a binary operator ⋄∈+,−,×,÷, the smallest interval which contains all feasible values for x⋄y is defined as
(1)x⋄y=x⋄y∣x∈x,y∈y.
The set-theoretic operations can be applied to intervals. The intersection of two intervals is defined by
(2)x∩y=z∈Rz∈x,z∈y,which is always an interval. This is not the case for their union(3)x∪y=z∈R∣z∈xorz∈y.
Consequently, an interval hull is defined as the smallest interval that contains a subset X of R. For example, the interval hull of 2,4∪5,8 is the interval 2,8. The interval hull of the union of x and y is denoted by x⊔y.
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 f, interval extensions f (also called “inclusion function”) that can be evaluated reasonably quickly and such that fx is not too large. For any function f defined with arithmetical operators and elementary functions,
(4)fx⊂fx,∀x∈R.
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,
(5)fx=4x4−7lnx−3,∀x∈Rhas the natural inclusion function
(6)fx=4x4−7lnx−3,∀x∈R.
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 c relating a set of variable x, an algorithm C is called a contractor (or filtering algorithm) if C returns a subdomain of the input domain x and the resulting subdomain Cx contains all the feasible points with respect to the constraint c:
(7)Cx⊆x,cx⇒x∈Cx,∀x∈x.
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
a set V of variables v1,v2,…,vn,
a set D of domains D1,D2,…,Dn, such as for each variable vi, a domain Di with the possible values for that variable is given and Di is an interval or union of intervals;
a set C of p constraints c1,c2,…,cp, 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 p 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 n intervals. The width of an interval vector wx=maxi=1,…,nwxi 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 c, with regard to an interval vector B, if and only if
(8)B=hull□B∩ρc,where hull□ is a function computing an approximation of the smallest interval vector containing its argument and ρc denotes the underlying relation of the given constraint c. If P is an ICSP involving an interval vector B, P is hull consistent if each constraint c of P is hull consistent. Consequently, B cannot be reduced anymore by applying c. Hull consistency is also called 2B consistency [50]. The interval vector B is the Cartesian product of n intervals: B=I1×⋯×In. 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 log. HC4 revise works in two phases:
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.
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 x×x+y2=2, with x,y∈−2,4×−1,1, hull consistency algorithms would not reduce the initial box, but would perfectly reduce it when the constraint is stated like x2+y2=2.
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 P be an ICSP, x a variable of P, and Dx=x¯,x¯ the domain of x. Dx is 3B consistent if and only if P′ and P″ are both nonempty where P′=ΦP∪x=x¯ and P″=ΦP∪x=x¯ with ΦP denoting the enclosure of P. P is 3B consistent if all the domains of variables are 3B consistent.
Example 1.
Let us consider that the constraints of P is given as follows:
(9)x+y=2,y≤x+1,y≥x,with Dx=0.5,1 and Dy=1,1.5. For each instantiation of x, 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 x is 3B consistent. The same method is used for the variable y and ultimately proves that the ICSP is 3B consistent.
For further information about 3B consistency methods, the reader might refer to [23].
Establishing 3B consistency.
Instantiation
Constraint
Solution
Empty
P′=ΦP∪x=0.5
x+y=2y≤x+1y≥xx=0.5
x=0.5y=1.5
Non
P″=ΦP∪x=1
x+y=2y≤x+1y≥xx=0.5
x=1y=1
Non
After presenting the mathematical tools required to understand our approach, let us now deal with the localization problem itself.
4. The Localization Problem4.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 xk, yk, and θk knowing the previous pose xk−1, yk−1, and θk−1. 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 (δsk and δθk) 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.
Vehicle localization problem.
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 δθgyro and its associated interval δθ:
(10)δθ=δθgyro−εgyro,δθgyro+εgyro,where εgyro 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 δp=δpodo−1,δpodo+1 where δpodo 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 εodo. Consequently, the displacement considering a sliding wheel is defined by
(11)δp=δpodo−1−εodo,δpodo+1+εodo.
The elementary displacement δsk of the vehicle is then obtained by
(12)δsk=πwlδpl+wrδprP,where wr and wl, respectively, stand for the radius of the right and left wheel and P 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 y=x,yT in a Cartesian local frame. Furthermore, the GPS receiver computes the measurement imprecision εgps on-line and sends it into the GST NMEA frame.
(13)y=xgps−εx,gps,xgps+εx,gpsygps−εy,gps,ygps+εy,gps.
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 k, a new ICSPk (Vk, Dk, Ck) is generated and defined as follows:
a set of variables, Vk,
(14)Vk=xk,yk,θk,xk−1,yk−1,θk−1,δsk,δθk,
a set of domains, Dk,
(15)Dk=xk,yk,θk,xk−1,yk−1,θk−1,δsk,δθk,
a set of constraints, Ck,
(16)Ck=Ck1,Ck2,Ck3T,(17)Ck1⇔xk=xk−1+δskcosθk−1+δθk2,Ck2⇔yk=yk−1+δsksinθk−1+δθk2,Ck3⇔θk=θk−1+δθk,
where Ck 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:
δsk and δθk are given by the odometer and gyro measurements thanks to (12) and (10).
xk and yk are initialized with GPS measurements (see (13)).
θk is initialized to −π,π.
xk−1, yk−1, and θk−1 are obtained from the solution of the previous CSP (at time k−1).
We consider all the state equations from time k−Ξ+1 to time k (where k 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 k, we add a new constraint set Ck to the existing constraint sets. If k>Ξ, we delete the oldest constraint set Ck−Ξ 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 (3×Ξ) but changes temporally. The localization DICSP is defined at time k by
(18)DICSPk=ICSPkICSPk−1ICSPk−2⋯ICSPk−Ξ+1.
ICP algorithms allow us to solve DICSPk. 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.
(19)θk−1=θk−1⋂arccosxk−xk−1δsk−δθk2,θk−1=θk−1⋂arcsinyk−yk−1δsk−δθk2.
Example 2.
Considering the case study with a straight displacement along the x-axis, the orientation variation is null −δθk/2=0. The correction on θ is realized by arcsinyk−yk−1/δsk and arccosxk−xk−1/δsk.
The width of yk−1 is around 10 m in most of our experiments: yk−1=−5,5. Thus, we can get
(20)yk−yk−1=−5+ϵy,5−ϵy−−5,5=−10+ϵy,10−ϵy,where ϵy is a small reduction of the imprecision due to a new GPS measurement. δsk is around 1 m:(21)δsk=1−ϵs,1+ϵs.
The value inside the arcsin function is far beyond −1,1, leading to a full ignorance on the orientation. The same reasoning leads to the same result for the arccos 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 x-axis and y-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 θk−Ξ into ν variables θk−Ξi,i∈1,2,…,ν where ν stands for the “splitting factor.”
Each domain θk−Ξi is then defined by
(23)θk−Ξi=θk−Ξ¯+i−1S¯,θk−Ξ¯+iS¯,where k≥Ξ, S=wθk−Ξ/v is the width of each θk−Ξi, and wθk−Ξ represents the width of the interval θk−Ξ. S¯ and S¯ are, respectively, the round down and the round up of the number S.
Example 3.
To illustrate, let us consider θk−Ξ=1,2. If ν=10 is the “splitting factor,” then, the following intervals will be generated: θk−Ξ1=1¯,1.1¯,θk−Ξ2=1.1¯,1.2¯,θk−Ξ3=1.2¯,1.3¯,…,θk−Ξ10=1.9¯,2¯.
In the first step of the algorithm, k<Ξ and θk−Ξ is undefined: the splitting is realized on θ0. θk−Ξ is the union of all the θk−Ξi:
(24)θk−Ξ=⋃i=1vθk−Ξi.
Other domains remain unchanged (unsplitted). The proposed splitting leads to a νDICSP:
(25)νDICSPk=DICSPk1,DICSPk2,…,DICSPkν,with
(26)DICSPki=ICSPki,ICSPk−1i,…,ICSPk−Ξ+1iT.
Algorithm 1: DICPS.
Ensure: DICSPk
(1) (θk−Ξ1,…,θk−Ξv) ← Split(θk−Ξ,v)
(2) fori ← 1 : v do
(3) DICSPki ← Build(DICSPk,θk−Ξ1)
(4) [Xki] ← Solve(DICSPki)
(5) end for
(6) [Xres] ← Hull(Xk1,…,Xkv)
(7) return [Xres]
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 νDICSPk. It defines the current position and orientation of the vehicle as an interval vector xk,yk,θk. 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 νDICSPk (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:
Line 1: the oldest variable θk−Ξ, representing the orientation in the initial DICSP, is split into ν intervals.
Line 3: for each θk−Ξi, a DICSP is generated.
Line 4: each DICSP is submitted to a solving process (by an ICP algorithm).
Line 6: the new domains, resulting from the process of the preview step, are grouped with an interval hull function.
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 x into boxes x1,x2,…,xm, so that
(27)x=⋃i=1mxi.
Then, the range of a function f· over x, possibly defined by (17), can be evaluated by
(28)fx=f⋃i=1mxi.
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
(29)fx⊆⋃i=1mfxi.
And thanks to the definition of interval hull (see Section 2.2), we obtain
(30)fx⊆⨆i=1mfxi.
Since ⊔i=1mfxi 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).
Experimental platform. (a) CARLLa platform. (b) Embedded hardware architecture. (c) The Satory track with timestamps.
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 (Ξ=10,20,30,40,50,100). 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.
Heading imprecision. (a) Average heading imprecision for different splitting factors. (b) Heading imprecision at each time step for different ICP algorithms.
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.
Mean duration of the algorithm for a localization step.
Algorithms parameters
DICPSΞ=20; ν=20
HC4Ξ=20
3BΞ=20
Duration (ms)
13.32
0.542
5257
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: Ξ=20. 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 (ν=20), 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 ν=20 and Ξ=40. 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 3σ 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 3σ error bound) has directly been used to compute the interval error. For the EKF, the imprecision (standard deviation) has been magnified 3 times.
Comparison between DICPS and EKF. (a) DICPS (3σ) and inconsistent EKF with 3σ error bounds. (b) DICPS (3σ) and consistent EKF with 17σ error bounds.
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 t=200 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 x value for the EKF to be consistent. We found 17 and drew the 17σ 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 x 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 Ξ=20 and ν=40 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.
GruyerD.LambertA.PerrollazM.GingrasD.Experimental comparison of Bayesian vehicle positioning methods based on multi-sensor data fusion20131212443ThrunS.BurgardW.FoxD.2005Cambridge, MA, USAThe MIT PressJaulinL.Robust set-membership state estimation; application to underwater robotics200945120220610.1016/j.automatica.2008.06.0132-s2.0-57949086651ColleE.GalerneS.A multihypothesis set approach for mobile robot localization using heterogeneous measurements provided by the Internet of Things20179610211310.1016/j.robot.2017.05.0162-s2.0-85032872469JaulinL.Localization of an underwater robot using interval constraint propagationInternational Conference on Principles and Practice of Constraint Programming2006Nantes, France24425510.1007/11889205_19GningA.BonnifaitP.Constraints propagation techniques on intervals for a guaranteed localization using redundant data20064271167117510.1016/j.automatica.2006.02.0242-s2.0-33646799133MackworthA. K.Consistency in networks of relations1977819911810.1016/0004-3702(77)90007-82-s2.0-49449120803GallaireH.Logic programming: further developmentsIEEE Symposium on Logic Programming1985Boston, MA, USA8899JaffarJ.LassezJ. L.Constraint logic programmingProceedings of the 14th ACM SIGACT-SIGPLAN Symposium on Principles of Programming Languages - POPL '871987Munich, Germany11111910.1145/41625.416352-s2.0-85032090044van HentenryckP.DevilleY.TengC. M.A generic arc-consistency algorithm and its specializations1992572-329132110.1016/0004-3702(92)90020-X2-s2.0-0026941401BessiereC.Arc-consistency and arc-consistency again199465117919010.1016/0004-3702(94)90041-82-s2.0-0028336331YuanlinZ.YapR.Arc consistency on n-ary monotonic and linear constraintsInternational Conference on Principles and Practice of Constraint Programming2000Singapore47048310.1007/3-540-45349-0_34DechterR.PearlJ.Tree clustering for constraint networks198938335336610.1016/0004-3702(89)90037-42-s2.0-0024640050RossiF.PetrieC.DharV.On the equivalence of constraint satisfaction problemsProceedings of the 9th European Conference on Artificial Intelligence (ECAI'90)1990Stockholm, Sweden550556MohrR.MasiniG.Good old discrete relaxationProceedings of the 8th European Conference on Artificial Intelligence (ECAI'88)1988Munich, Germany651656ClearyJ.Logical arithmetic19872125149DavisE.Constraint propagation with interval labels198732328133110.1016/0004-3702(87)90091-92-s2.0-0023386175HyvönenE.Constraint reasoning based on interval arithmetic2Proceedings of the 11th International Joint Conference on Artificial Intelligence (IJCAI'89)1989Detroit, MI, USA11931198BenhamouF.OlderW. J.Applying interval arithmetic to real, integer, and Boolean constraints199732112410.1016/S0743-1066(96)00142-22-s2.0-0031185563BenhamouF.GoualardF.GranvilliersL.PugetJ. F.Revising hull and box consistencyProceedings of the 1999 International Conference on Logic Programming1999Las Cruces, NM, USA230244BenhamouF.McAllesterD.HentenryckP. V.CLP(intervals) revisitedProceedings of the 1994 International Symposium on Logic Programming1994Melbourne, VIC, Australia124138GranvilliersL.Towards cooperative interval narrowingInternational Symposium on Frontiers of Combining Systems2000Nancy, France183110.1007/10720084_2LhommeO.Consistency techniques for numeric CSPs1Proceedings of the 13th International Joint Conference on Artifical Intelligence (IJCAI'93)1993Chambery, France232238GningA.BonnifaitP.Guaranteed dynamic localization using constraints propagation techniques on real intervalsIEEE International Conference on Robotics and Automation, 20042004New Orleans, LA, USA1499150410.1109/ROBOT.2004.1308036VinckeB.LambertA.GruyerD.ElouardiA.SeignezE.Static and dynamic fusion for outdoor vehicle localization2010 11th International Conference on Control Automation Robotics & Vision2010Singapore43744210.1109/ICARCV.2010.57078222-s2.0-79952419171VinckeB.LambertA.Enhanced constraints propagation for guaranteed localization predictive stepIEEE/RSJ IROS, Workshop on Planning, Perception and Navigation for Intelligent Vehicles2009St Louis, MO, USA3036WaltzD. L.1972Cambridge, MA, USAAI Lab, MITDrevelleV.BonnifaitP.Robust positioning using relaxed constraint-propagation2010 IEEE/RSJ International Conference on Intelligent Robots and Systems2010Taipei, Taiwan4843484810.1109/IROS.2010.56497942-s2.0-78651468783LassouedK.BonnifaitP.FantoniI.Cooperative localization with reliable confidence domains between vehicles sharing GNSS pseudoranges errors with no base station201791223410.1109/MITS.2016.26305862-s2.0-85010378529DesrochersB.JaulinL.A minimal contractor for the polar equation: application to robot localization201655839210.1016/j.engappai.2016.06.0052-s2.0-84976594273JaulinL.StancuA.DesrochersB.Inner and outer approximations of probabilistic setsProceedings 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 Analysis2014Liverpool, UK131610.1061/9780784413609.2252-s2.0-84933566245AubryC.DesmareR.JaulinL.Loop detection of mobile robots using interval analysis201349246347010.1016/j.automatica.2012.11.0092-s2.0-84872037140Le BarsF.SliwkaJ.JaulinL.ReynetO.Set-membership state estimation with fleeting data201248238138710.1016/j.automatica.2011.11.0042-s2.0-84856226693VinckeB.LambertA.Experimental comparison of bounded-error state estimation and constraints propagation2011 IEEE International Conference on Robotics and Automation2011Shanghai, China4724472910.1109/ICRA.2011.59803132-s2.0-84871681923KueviakoeI. K.LambertA.TarrouxP.Comparison of interval constraint propagation algorithms for vehicle localization2012051215716210.4236/jsea.2012.512B030KueviakoeI. K.LambertA.TarrouxP.Vehicle localization based on interval constraint propagation algorithms2013 IEEE International Conference on Cyber Technology in Automation, Control and Intelligent Systems2013Nanjing, China17918410.1109/CYBER.2013.67054422-s2.0-84893921016MooreR. E.1966Englewood Cliffs, NJ, USAPrentice-HallMooreR. E.BierbaumF.19792Philadelphia, PA, USASociety for Industrial MathematicsMooreR. E.KearfottR. B.CloudM. J.2009Philadelphia, PA, USASociety for Industrial MathematicsHyvönenE.Constraint reasoning based on interval arithmetic: the tolerance propagation approach1992581-37111210.1016/0004-3702(92)90005-I2-s2.0-44049111346HyvönenE.PascaleS. D.LehtolaA.Interval constraint satisfaction tool INC++Proceedings of 1993 IEEE Conference on Tools with Al (TAI-93)1993Boston, MA, USA29830510.1109/TAI.1993.6339712-s2.0-0027886788HentenryckP. V.1997Cambridge, MA, USAMIT PressGranvilliersL.BenhamouF.Algorithm 852: RealPaver: an interval solver using constraint satisfaction techniques200632113815610.1145/1132973.11329802-s2.0-33745226980MerletJ. P.ALIAS: an interval analysis based library for solving and analyzing system of equationsSéminaire Systèmes et Equations Algébriques2000INRIA Sophia Antipolis, Valbonne, France19641969LebbahY.ICOS: a branch and bound based solver for rigorous global optimization2009244-570972610.1080/105567809027534522-s2.0-68949187495DomesF.GLOPTLAB: a configurable framework for the rigorous global solution of quadratic constraint satisfaction problems2009244-572774710.1080/105567809029177012-s2.0-68949113580ChabertG.IBEX, an interval-based explorer2007, http://www.ibex-lib.org/ChabertG.JaulinL.Contractor programming2009173111079110010.1016/j.artint.2009.03.0022-s2.0-67349280632DechterR.DechterA.Belief maintenance in dynamic constraint networksProceedings of the Seventh AAAI National Conference on Artificial Intelligence (AAAI'88)1988Saint Paul, MN, USA3742CollavizzaH.DelobelF.RueherM.A note on partial consistencies over continuous domainsInternational Conference on Principles and Practice of Constraint Programming1998Pisa, Italy14716110.1007/3-540-49481-2_122-s2.0-84957708039EverettH. R.1995Natick, MA, USAAK PetersSeignezE.KiefferM.LambertA.WalterE.MaurinT.Experimental vehicle localization by bounded-error state estimation using interval analysisIEEE/RSJ International Conference on Intelligent Robots and Systems2005Edmonton, AB, Canada1084108910.1109/IROS.2005.15451552-s2.0-64549122186KueviakoeI. K.LambertA.TarrouxP.A real-time interval constraint propagation method for vehicle localization16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013)2013The Hague, Netherlands1707171210.1109/ITSC.2013.67284752-s2.0-84894318109JermannC.LebbahY.Sam-HaroudD.BenhamouF.JussienN.O’SullivanB. A.Interval analysis, constraint propagation and applications2007Hoboken, NJ, USAJohn Wiley & SonsChapter 4LambertA.GruyerD.VinckeB.SeignezE.Consistent outdoor vehicle localization by bounded-error state estimation2009 IEEE/RSJ International Conference on Intelligent Robots and Systems2009St. Louis, MO, USA1211121610.1109/IROS.2009.53546732-s2.0-76249083426