Journal of Sensors

Volume 2018, Article ID 3769058, 12 pages

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

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

^{1}LIMSI, CNRS, Univ. Paris-Sud, Université Paris-Saclay, 91403 Orsay, France^{2}LRI, CNRS, Univ. Paris-Sud, Université Paris-Saclay, 91403 Orsay, France

Correspondence should be addressed to Alain Lambert; rf.dusp-u@trebmal.niala

Received 13 September 2017; Revised 23 January 2018; Accepted 8 February 2018; Published 10 April 2018

Academic Editor: Oleg Lupan

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.

#### 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].