Chapter 4

Fault-tolerant Data-fusion Method:
Application on Platoon Vehicle Localization ¹

,

 

 

 

4.1. Introduction

For several years, active security methods as well as intelligent technical devices have mostly been developed in public or private transport vehicles. Sophisticated methods from robotics, such as localization, obstacle detection and trajectory planning, are now available in intelligent transport systems. The implementation of these new functions strongly depends on the quality of data coming from different sensors embedded in vehicles as well as their merging, particularly when it comes to critical information that must be supplied for autonomous navigation systems.

The goal of our research within the framework of the FD2S project carried out by GIS 3SGS was to integrate the concept of tolerance to faults, failures or inaccuracy of sensors. The goal was to integrate the detection, diagnostics and processing of faults in the sensor data for data fusion, which requires the validation/invalidation of data collected, and the decision-making concerning the reconfiguration of the data fusion algorithm. These functionalities play an essential role in providing autonomy in order to give a vehicle (or a robot) the capability to adapt and react to unforeseen events, thus allowing it to fulfill the goals of its mission. This is a decisive element in improving the operational efficiency of intelligent transport systems and decreasing the cognitive load of their piloting.

Our research has enabled us to use Bayesian networks for multisensor data fusion and a decision algorithm [SMA 10] via the use of probabilistic graph theory. These Bayesian networks enable us to establish a probabilistic graph model from the joint probability distribution on a set of random variables. This formalism enables an intuitive representation of the knowledge on a given application area and facilitates the implementation of high-performance and clear models. The representation of knowledge is based on the graphical description of the relationships existing between the variables describing the system. Each variable is associated with a local probability distribution quantifying this relationship. The Bayesian network is shown as a formalism that is potentially useful because it can integrate data fusion, diagnostics, reconfiguration and learning in a same formalism. In this chapter, we also suggest a formulation for the problem of localizing a vehicle convoy using a Bayesian network. The approaches developed were tested on benchmarks of real data obtained by the “TRAVEL¹” platform used by LAGIS. It was extended using LORIA's CyCab vehicle. The results were used to suggest appropriate validation procedures.

This chapter is organized in the following way. After a review of the theoretical tools of multisensor data fusion, a description of Bayesian networks is suggested. Then, in section 4.4, we describe the method proposed for fault-tolerant data fusion for the localization of a single vehicle. In section 4.5, we present an approach for the modeling and localization of a vehicle convoy that is an extension of the approach suggested in section 4.4. Finally, we present the testing and results of approaches using real data. We end this chapter with a conclusion and perspectives.

4.2. Review

In filtering, we look for an algorithm that is able to carry out the calculation of the optimal filter that also fulfills real-time constraints. In concrete terms, the calculation time of an estimated variable must be smaller than the observation period. To fulfill this constraint and to spare the memory of the embedded computer, we preferably want a recursive algorithm. Thus, the calculation of the estimated variable at the current time must depend only on the current observation and on the previous estimated variable; the past observations are not reused. These constraints are paramount for the good efficiency of the algorithm.

In 1960, Kalman and Bucy [KAL 60] revolutionized estimation theory by providing the first recursive filtering algorithm. Kalman-Bucy's filter enables us to precisely and quickly calculate an optimal filter when the state and observation models only include linear functions and additive Gaussian noise. It has been shown, in fact, that in this case the probability distribution is assumed to be a Gaussian one. It is therefore sufficient to propagate its mean and its variance: the problem is of finite dimensions. Generally, however, the nonlinear filtering problem does not have a solution of finite dimensions. In practice, we often keep using versions derived from Kalman-Bucy's filter, such as the extended Kalman filter (EKF) that linearizes the model around the current estimate. It owes its success to:

– its very good robustness due to its “least square” nature;

– its numerical stability, which is well controlled because the implementations are often factorized; and

– its very good aptitude for real-time calculations (EKF being a particular version of a predictor/estimator observer).

Even though for the past 40 years this algorithm has been the most widely used nonlinear filter, it exhibits some weak points when the system is too nonlinear or when the filter is poorly initialized.

Another variant of EKF is the “unscented” Kalman filter (UKF) that was presented by Julier and Uhlmann [JUL 97] to address the following weaknesses of EKF:

– the difficulty of tuning an EKF (it must be done by trial and error);

– the difficulty of implementation (the filter must be rewritten in quadratic form); and

– the behavior of the system must be sufficiently linear between two registrations.

A UKF filter is a predictor/estimator that does not linearize the transition function f and observation function h in the way the EKF does. This method directly evaluates the first- and second-order moments of the images by the f and h of the variables of interest by using an unscented transformation. The numerical implementation of a UKF can be high performance if we use Cholesky's factorization.

The UKF filter has met with huge success since the end of the past century, particularly in robotics. It has replaced the EKF in several applications. It must be noticed that UKF has several variants, such as numerical derivative filters (first-order DD1 and second-order DD2) [NOR 00] and the central difference Kalman filter. These filters all use a strategy of deterministic sampling. They have been used by several researchers for localization [BON 05, DIS 01].

More recently, the combination between Kalman variants (EKF, UKF, etc.) and interacting multiple model (IMM) algorithms has achieved some success. The IMM algorithm is the most frequently used in the literature [LI 05, BLO 88] for the classification of the movement of a target according to several types of movement models. With this combination, the estimation of a state vector is based on the combination of a finite number of sub-models, to which one of the variants of a Kalman filter is simultaneously applied [BAR 82].

The particle filter has also gained some recognition since the beginning of this century. The method of particle resolution lies in the exploration of the state space by particles randomly drawn (Monte Carlo drawing). The principle resembles “grid” (or mesh) methods. It has the particularity of generating a progressive mesh as a function of the stochastic flow of the system studied and of its observations. The essential tools used in this method are Bayes' rule and a dynamic adaptation of the law of large numbers [ARU 02, GUS 02, BER 99].

The key idea behind the success of particle filtering, other than the evolution of numerical calculators, is the fact that the estimation and its quality depend little on the dimension of the state model. Under some conditions, we can hope to have the same estimation quality with the same number of particles for a system of a higher order [CRIS 00].

Any user of particle filtering worries about the number of particles and wants to minimize their number to reduce the calculation load. The minimization amounts to selecting the particles that are representative of the probability density function. Since the quality of the estimation of the density depends on the number of particles, the decrease in the number of particles is tricky. In the literature there are several methods that can be used to decrease the number of particles. By introducing an additive noise to the particles, we can decrease their numbers. This method is known as jittering [FEA 98]. Another similar approach was introduced in [GOR 93] under the term roughening. In [GOR 93], the prior editing method is also discussed. In this method, a step is added to the filtering algorithm. In this step, a particle likelihood test is performed. The idea is to eliminate the particles that have an insufficient probability; they will not be selected during the following step.

Finally, our choice of theoretical tool for the f multi-sensor data fusion has been based on criteria that appear to be essential to solve the problem of the localization of a vehicle convoy:

– the aptitude to accept various information sources, both redundant and complementary, on top of the ease of integrating them into the method;

– the capability to provide a consistent estimation of the localization error;

– the tolerance to sensor failures such as faults, inaccuracies, inconsistencies, incoherences or abnormal data (the possibility of managing the multi-hypotheses aspect and the multimodality of measurements could be a way to be tolerant to sensor inaccuracies);

– the possibility to integrate a diagnostic layer for the sensors;

– the possibility to reconfigure the fusion method after the detection of a sensor failure in order to restrict the perturbating effect of a faulty sensor; and

– the aptitude for real time.

4.3. Bayesian network for data fusion

Among the probabilistic approaches, Bayesian networks (BN), also referred to as probabilistic graphical models, are sometimes defined as being a marriage between probability theory and graph theory (Figure 4.1). Probabilities allow these models to take into account the aspect of uncertainty when modeling a given event. The graphical part offers an intuitive tool in several areas of application where the users need to “understand” the model they are using [JEN 01, MUR 99, MUR 02, SMA 10]. The joint use of probabilities and graphs gives us a family of knowledge models that are very rich with dynamic Bayesian networks, hidden Markov models and their derivatives, Kalman filter, etc., for instance.

From a formal point of view, an BN is defined by a set of discrete and/or continuous random variables, by a structure expressed in the form of a directed acyclic graph that indicates the dependence between these variables and a set of conditional probabilities that links the value of a node to that of its parents. Figure 4.2 gives an example of a Bayesian network with several discrete and binary variables.

In this project, we were interested in dynamic Bayesian networks (also referred to as temporal Bayesian networks), which are directed graphical models of stochastic processes. They generalize hidden Markov models and the linear dynamic systems by representing the hidden (and observed) states as state variables with complex interdependences. The graphical structure provides an easy way by which to detail these conditional independences, and as a consequence also provides a reduced parametrization of the model.

The description of this theory is omitted in this chapter. More details can be found in [MUR 02, SAM 10].

4.3.1. Bayesian network and Kalman filter

As we saw previously, a Kalman filter is a tool to estimate the state of a dynamic system from noisy observations. More formally, to estimate state X_k at time k, we have information Y_0:k=(Y₀,…,Y_k) and of the previous state X_k−1 at our disposal, and the goal is to obtain the most information possible relating to X_k. In terms of probability, it involves calculating the conditional law of random vector X_k knowing that Y_0:k: P(X_k|Y₀,…,Yk).

Let us assume that we know the conditional law of random vector X_k−1 knowing Y_0:k−1. The Kalman filter proceeds in two phases:

— prediction: we calculate the conditional law: P(X_k|Y₀,…,Y_k−1), with the help of the evolution model; and

– correction: we use new observation Y_k to correct the state predicted in order to obtain a more accurate estimation.

A classical Kalman filter such as described previously can be represented by a Bayesian network with continuous variables. In [MUR 02], the author shows that a Kalman filter is a special case of a Bayesian network, which can be represented by the network shown in Figure 4.3.

A Kalman filter assumes that state x_t∈ ℜ^N_x observation function y_t∈ ℜ^N_y, filter input u_t∈ ℜ^N_u and the observation and transition functions are linearly Gaussian, i.e.:

[4.1]

and:

[4.2]

In other words:

[4.3]

where V_t ∼ N(μ_X ,Q) is a Gaussian noise. Similarly:

[4.4]

where W_t ~ N(μ_X, Q) is another Gaussian noise independent of V_t.

In the pieces of work carried out by [ELB 03, BON 05, QUD 06], Kalman filtering is used to match an estimate on a road segment. If we want to manage or manipulate several segments at the same time (multi-hypotheses), however, the Kalman filter as it has previously been described is not adapted, even though we could use N Kalman filters corresponding to the number of candidate segments.

4.3.1.1. The choice of use of Bayesian networks

In order to suggest a solution to the issue of dynamic localization of a vehicle, we use the Bayesian networks for the following grounds:

– The Bayesian network generalizes Kalman filters: the Kalman filter is only one particular case of a Bayesian network. Furthermore, this tool gives us the opportunity to handle the discrete and continuous variables in the same network, which is not allowed by a Kalman filter. The possibility to pair discrete and continuous variables in the same network is very interesting because it will allow us to generate Bayesian networks in the configuration of a switching Kalman filter, switching state-space model or jump-Markov model [MUR 02]. These are configurations that we have attempted to take advantage of and to manage the multi-hypotheses aspect (with more than one estimation) and the multi-modality aspect of our observations. These two aspects are necessary for our localization application on a 2D map in order to manage the inaccuracies in the data and, in our case, to manage the ambiguity by manipulating the multi-modality of the observation. This enables the management of several segments at the same time.

– To provide the possibility of integrating a diagnostic layer for the sensors in order to detect the possible failures.

– To provide the possibility to carry out a reconfiguration of the data fusion method after the detection of a sensor failure by using, for instance, the Bayesian network with a configuration of the switching state-space model.

– To provide the possibility of learning with the same theoretical tool that opens other research horizons.

4.4. Localization of a single vehicle: multisensor data fusion with a dynamic Bayesian network

The road network in a cartographic database is represented by polyline modeling of the central axis of the roads. Attributary information is attached to these polylines that we try to extract, for instance, to perform driving assistance functions.

There are two ways in which the localization of a vehicle on a map can be found:

– a first way involves the selection of the basis segment representing the road on which the vehicle is moving; and

– the second way consists in determing the curvilinear abscissa of the vehicle on the segment with respect to one of its extremities (this method is often used in navigation systems that are currently on the market).

In the rest of the chapter, the first definition will be considered.

In [ELB 03], there is a review covering the different methods of map localization existing in the literature. The reader can also refer to [QUD 06] for more details.

For an automated vehicle equipped with a GPS receiver and proprioceptive sensors such as odometry, the dynamic approach to the issue of map localization consists of searching for a recurrent method taking advantage of the map in order to register the drifts of dead reckoning.

For this, we can distinguish two approaches:

– a localization forced to remain attached to the road network: the solution is on the segment representing the road; and

– an absolute localization in the map referential, then a projection on the closest segment when we look for an attribute.

It is the second approach that we have considered in this project: it enables us to consider GPS and cartographic measurements equally. It therefore better lends itself to a formulation of the problem from the angle of data fusion.

4.4.1. Presentation of the approach developed

Let us focus on the multi-hypotheses approach for the rest of the section, managing multimodal observations that were studied in Cherif Smaili's thesis [SMA 10]. This approach belongs to the class of Bayesian methods [ZHA 97].

Figure 4.5 describes the approach based on a switching Kalman filter (SKF). Initially, in step A, a discrete variable is formed by using the segments supplied by Système d'Information Géographique (SIG) in order to build the multimodal observation. These observations are updated by using rules based on the topology of road networks. This discrete variable aims to generate as many states as there are modes in the multimodal observation. Afterwards, in step B, inferences are performed by using Bayesian networks to provide estimates that result from the merging of different observations and each state from the preceding time. These calculated inferences can be simulated using Kalman filters generated in parallel for each state. Besides the state estimation, the Bayesian inference gives a variance— covariance matrix associated with each state vector that represents an estimation of the error made by the inference. In step C, a policy of the creation and elimination of hypotheses is worked out to supply a ranking of hypotheses at a given time. This policy is based on heuristic rules such as the connectivity between segments and the movement history of the vehicle, as well as other criteria detailed later in this chapter. This is an essential step because it avoids the combinatorial explosion in the case where a large number of segments are selected and processed by the method.

The Bayesian network model developed for the issue of map-matching is shown in Figure 4.6. In this model, we use the discrete variable S_k, which represents the segments on which the vehicle can be situated: . This variable is initialized a priori at each step by using two criteria:

– the course of the segment that is the closest to the course of the vehicle; and

– the proximity of the GPS point with respect to the segments.

The segment whose GPS point and whose course is the closest is given a probability greater than the other candidates. Unlike the selection approaches of the most likely segment (using proximity criteria, course, etc.), we favor the segment that fulfills the criteria cited above. Under no circumstances, however, do we give this segment a probability equal to one. This strategy strengthens the uncertainty concept on which we are working.

The second variable used in the network in Figure 4.6 is continuous. Let us note that this variable is hidden: , representing the position and course of the vehicle for each candidate segment of variable S_k.

The graph given in Figure 4.6 allows us to properly represent the causal links between variables. Thus, continuous variable Xk is updated by the observations (obviousness or likelihood of measurements):

and:

if GPS signals are available. Otherwise (hidden GPS), we only use the cartographic observations when the odometric estimations are being updated. These two variables (Carto_k and GPS_k) are continuous.

The Bayesian network calculates joint probability P(S_k,X_k|GPS_k,Carto_k), which represents the probability that the vehicle lies in each candidate segment as well as the estimation of the placement of the vehicle (position, course): on each segment.

Let us note that the relationship between discrete variable S_k and continuous variable X_k plays the role of a generator containing N Kalman filters (where N represents the number of candidate segments).

In order to take into account evolution over time, we use a dynamic Bayesian network. This network is created by duplicating the static Bayesian network shown in Figure 4.6a or 4.6b and linking these networks from one step to next (every time we have a new observation).

Figure 4.7 represents the dynamic Bayesian network for the localization of a vehicle on a map in the presence of GPS signals. According to this figure, we have two variables that are linked from one step to the bext. The variables at stake are S and X.

Let us underline the resemblance between the model suggested above and what we refer to in the literature as the SKF represented by Figure 4.8. There are a number of different names for the SKF: switching linear dynamical system, switching state-space model, jump-Markov model, jump-linear system, conditional dynamic linear model, etc. This type of model is often used to approximate the nonlinear models (we assume that these models are piecewise linear) [MUR 02]. We can underline the fact that the methods known as IMM algorithms are particular cases of this representation.

4.4.2. Inference in switching Kalman filter

The fundamental problem with the SKF type of network is that the inference is almost impossible. The number of Gaussians in the distribution of the state increases exponentially with time. To illustrate this, let us assume that the initial distribution p(X₁) is a mixture of K Gaussians, one for each value of S₁. During the marginalization stage in S₁, each Gaussian must be propagated through K equations (one for each value of S₂) so that p(X₂) is a mixture of K² Gaussians. In general, at time t, the probability distribution p(X_t | y_{1 :t}) is a mixture of K^t Gaussians, one for each possible combination of S₁,…,S_t.

To cope with the number of Gaussians, which increases exponentially, particle filters and approximative inference algorithms have successfully been used in this type of model [URI 02].

Even though inference is almost impossible in the case of models of the SKF type, there are particular cases in which this inference is possible. The trivial case is when the discrete variable can be observed. In this case, the probability distribution of the continuous variable is unimodal (a single Gaussian). The second case where inference is possible is when the discrete variables are not linked from one step to the next. An illustrative example is given in [MUR 02].

Algorithms referred to as pruning algorithms decrease the number of Gaussians by eliminating some of them. These algorithms keep N Gaussians with strong probabilities, discard the others and normalize the probabilities so that the sum is equal to one [BAR 88, URI 02].

Another approach referred to as collapsing, or general pseudo-Bayesian (GPB) algorithms, consists in partitioning the Gaussians into N subsets. The GPB algorithm restricts the number of Gaussians of the state to N Gaussians that correspond to the number of subsets. For more details on these approaches, we refer the reader to [LAU 89] and [URI 02].

To better understand the principles of this approach, let us give an example that illustrates it. Let us assume, at time t = 1, that we have M Gaussians to represent the state (M is the number of combinations of the discrete variable that are possible). After the propagation phase, we obtain M₂ Gaussians. These M₂ Gaussians are collapsed into M Gaussians. Figure 4.9 illustrates this approach for a number of Gaussians equal to three (M = 3).

In the case of the Bayesian network represented in Figure 4.7, the number of Gaussians in the distribution of state X_i does not increase (exponentially) with time. A single Gaussian per candidate segment is considered at each time, t. The distribution along a segment at time t is assumed to depend only on the distribution along this same segment at time t − 1. This can be translated into the introduction of a conditional probability P(S_t | S_t−1) that is equal to the identity matrix. The intuition behind this conditional relationship is that the segments are “disconnected” from each another. The hypotheses are propagated along each segment but not between segments. Let us note that this approach resembles the reduction approach given by pruning algorithms.

4.4.3. Detailed synopsis of the method based on Bayesian networks

We use the fusion of GPS measurements, the measurements of proprioceptive sensors and data on the map in order to generate an estimation of the posture of the vehicle. We are particularly interested in what follows in the search for the path (road segment) on which the vehicle drives, with a quantification of the degree of certainty as a function of the quality of the information handled and the geometric configuration of the road network around the vehicle. The synopsis of this approach based on Bayesian networks is given in Figure 4.10.

Initially, let us assume that we have a GPS or odometric estimate (when the GPS is hidden) of the position of the vehicle. This estimation is given to SIG in order to select a set of segments around this estimate. The result of this step is a set of segments that are numbered as follows: represents the selection time.

In order to build cartographic observations (evidence), we project the estimation onto each candidate segment. If there is no orthogonal projection, then we take the point that is closest to the segment of interest. The result of this step is the set:

The inclusion of Carto_k and GPS_k observations in the Bayesian network enables us to update the probability distributions of hidden variables, X_k. After inference, we obtain the probabilities of each candidate segment in which the vehicle is likely to be (represented by variable S_k) as well as the position and course of the vehicle in each segment (represented by variable X_k).

4.4.4. Example of management of multi-hypotheses by a Bayesian network

In order to clarify ideas, let us give an example that illustrates how our method works. In Figure 4.11, the vehicle moves from road segment number 1 towards road segment number 8. Let us assume at time t = 1 that an estimation is given by the GPS (or odometer). From this position we select all of the possible segments (this step is given by SIG). Here there is only one single candidate segment. We project the GPS estimation onto this candidate segment (segment number 1) to build a cartographic observation:

Observations Carto₁ and GPS₁ are used to update the probability distributions of hidden variable X₁. In this trivial case, we are certain that the vehicle is driving on road segment number 1 (P(S₁=seg₁|Carto₁, GPS₁)=1). The position of the vehicle on segment seg1 is given by the hidden variable

where μ₁ = (x₁, y₁, θ₁) and ∑₁ represents the covariance matrix (the uncertainty of this estimation).

Table 4.1. Details of the inference process given by SKF for each road segment

At time t = 2, the uncertainty of the sensors (odometer, map, GPS) means that segments 2 and 3 are two candidates. These segments are used to generate two cartographic observations:

The update of the Bayesian network is done using new observations, Carto₂ and GPS₂, and we obtain two new estimations corresponding to the two segments P(S₂=seg₂|Carto₂,GPS₂)=0.75 and P(S₂=seg₃|Carto₂, GPS₂)=0.25. In this case, we have a 75% chance that the vehicle is driving on road segment number 2 and a 25% chance that the vehicle is on road segment number 3. The position of the vehicle on each segment is given by the joint probability: P(X₂, S₂|Carto₂, GPS₂). Table 4.1 summarizes the inference process for the example where 1 ≤ t ≤ 4.

4.4.5. Illustration of the map localization method using SKF

The trajectory chosen to test the validity of our approach is presented in Figure 4.12a. This trajectory is chosen because it presents two parallel roads and two crossings, which are the sources of ambiguity. This journey was carried out in Compiègne with the experimental vehicle Strada from the Heudiasyc laboratory at UTC.

We drew the estimations given by the odometric model on Figure 4.12b. The major drawback with odometry is its drift over time due to slipping of the wheels and the imperfection of the roads (holes, bumps, etc.). This drift is well represented in this figure.

In the first experiment (see Figure 4.12a), we used the GPS data at the beginning of the test; then a hidden GPS was simulated for the rest of the experiment. Figure 4.13 showed the speed of the vehicle throughout the test. In the absence of the information given by the GPS, the localization algorithm becomes an algorithm using only odometry and cartography.

4.4.5.1. First ambiguous situation: the case of parallel roads

In the situation of parallel roads, we want to show how the Bayesian network deals with this type of ambiguous situation, even in the case of GPS hiding.

In the case shown in Figure 4.14, there are two parallel roads. This is a typical case of ambiguity. With the suggested approach, the Bayesian network manages several segments during several time steps until the presence of more accurate GPS data enable it to minimize the uncertainty of the estimate. Let us note that we have not taken the direction of the traffic into account in any of our experiments.

Figure 4.15 provides more details about the ambiguity zone in Figure 4.14. The most likely segment in each step is represented by a dotted line. The probability distribution of each segment is given in Table 4.2.

Table 4.2. Details of the inference process given by SKF for each road segment

i	Segments	P(Segi)
1	Seg1,Seg2	P(Segl) = 0.20, P(Seg2) = 0.79
2	Seg1,Seg2	P(Segl) = 0.20, P(Seg2) = 0.79
3	Seg1,Seg2,Seg3	P(Segl) = 0.10, P(Seg2) = 0.71, P(Seg3) = 0.18
4	Seg1,Seg2,Seg3,Seg4	P(Seg1) = 0.06, P(Seg2) = 0.64, P(Seg3) = 0.15, P(Seg4) = 0.14

4.4.5.2. Second ambiguous situation: the case of a road junction

With this part of the trajectory, we can see how SKF deals with ambiguous situations in the case of a road junction, which is often found at the entrance or the exit to a crossing.

To simulate the hiding of the GPS data, we stop using these data just before the road junction (see Figure 4.16). In the absence of accurate information, the localization algorithm only uses odometry and cartography. The uncertainty surrounding the estimate increases, which forces the SKF to deal with several segments over several time steps until the presence of GPS data that minimize the uncertainty around the estimate, and as a consequence the number of candidate segments decreases.

Figure 4.17 gives more details on the ambiguous zone in Figure 4.16. According to Table 4.3, we can see from among the different hypotheses (candidate segments) that SKF gives a greater probability to the right-hand segment. These results were published in [SMA 08a, SMA 08b].

Table 4.3. Details of the inference process given by SKF for each road segment

i	Segments	P(Segi)
1	Seg1,Seg2,Seg3,Seg6	P(Seg1) = 0.08, P(Seg2) = 0,13, P(Seg3) = 0.14, P(Seg6) = 0.65
2	Seg2,Seg4,Seg6	P(Seg2) = 0.07, P(Seg3) = 0.18, P(Seg6) = 0.74
3	Seg3,Seg4,Seg6	P(Seg3) = 0.13, P(Seg4) = 0.15, P(Seg6) = 0.72
4	Seg4,Seg5,Seg6	P(Seg4) = 0.2, P(Seg5) = 0.11, P(Seg6) = 0.69
5	Seg5,Seg6	P(Seg5) = 0.28, P(Seg6) = 0.71

4.5. Multi-vehicle localization

Over the past decade a large number of vehicles referred to as “smart” cars have appeared. These cars provide the user with a mode of short-range transport adapted to the city, decreasing travel expenses, reducing stress generated by driving, reducing sound and atmospheric pollution, etc. Among these smart cars, there are instances when cars move in a convoy.

4.5.1. The problem studied

The task of navigating a convoy involves moving a set of vehicles in a queue from one point to another (following a reference path or the head car) by keeping a predefined gap between the vehicles (see Figure 4.18). In order to carry out this task, each vehicle in the convoy must be able to place itself with respect to the reference path and to the preceding vehicles.

In this context, most studies and experiments are based on control. The authors of these pieces of work assume that each vehicle is able to locate itself with respect to the reference path via a very accurate GPS [BOM 06, THU 02]. The use of the GPS for localization is not always possible, however, because of the irregularity of the signals given by the satellites: the absence of GPS in a tunnel, an erroneous estimation if the number of satellites is insufficient, etc.

We are interested in the issue of the localization of a convoy (a chain of vehicles). The merging of data will allow us to overcome the aforementioned GPS problems. Let us note for the sake of simplicity that during the simulation we will use a simple command (a proportional command).

4.5.2. Communication within the convoy

For a set of vehicles in a queue to follow a reference path while keeping a predefined gap between the vehicles, we assume that they can communicate with each other. The question that is then raised is:

– what do we want to transmit?

– and to whom?

In fact, if we assume that the vehicles follow the reference trajectory given by the lead vehicle, this trajectory must be transmitted to at least the first vehicle or to all of the vehicles in the convoy. In order to avoid the issue of cumulative errors, we transmit the trajectory of the lead vehicle to all of the cars making up the convoy.

The second thing to be taken into account when we want to control a set of vehicles is the maintainance of a safe distance between each pair of vehicles. Thus, each vehicle must know the speed (or position) of the preceding vehicle.

From a practical point of view, we only need these two pieces of information (the trajectory of the lead vehicle and the speed of the preceding vehicle) for the management of the convoy.

4.5.3. Sensors used on each vehicle in the convoy

The flexibility of the use of Bayesian networks allows us to add or remove sensors quite easily. Each sensor is modeled and represented in the Bayesian network by a variable referred to as an observation that is used to update the estimation of the position and course (x, y, θ).

The trajectory of the lead vehicle (known by all of the vehicles in the convoy) must be highly accurate. Thus, the leader vehicle is equipped with a centimetric localization system such as GPS-LRK. This GPS provides measurements with a sampling frequency of 10 Hz, with a precision of 2 cm [BOM 06]. In order to provide the lead vehicle with a good estimation of its course, we provide this vehicle with a second “gyroscopic” sensor.

In several pieces of work, the follower vehicles are equipped with a GPS-RTK centimetric positioning system [BOM 06, MAR 05] and a telemeter.

In order to emphasize the concept of merging using Bayesian networks, the follower vehicles are assumed to be equipped with a GPS of metric accuracy. A telemeter is placed at the front of each car to give an estimation of the distance between a vehicle and the one preceding it and the angle with respect to the axis of the abscissa.

We believe that merging the distance given by the telemeter and the centimetric position given by the GPS-LRK allows us to decrease the uncertainty surrounding the estimation given by the GPS of the first follower vehicle. Thus, each vehicle can correct its position with respect to its predecessor.

4.5.4. Bayesian network for the localization of a chain of vehicles

To ensure that a chain of vehicles follows a reference trajectory while keeping a predefined gap between the vehicles, we consider the following hypotheses:

– the lead vehicle is equiped with a centimetric localization system (GPS RTK) and a gyroscope to measure the angle of the bend;

– the lead vehicle is driven by a human being;

– the reference path is transmitted to all of the vehicles in the convoy;

– the follower vehicles are equiped with a GPS receiver and a telemeter to give the distance between the vehicles;

– each vehicle in the convoy must maintain a safe distance (DistS = longitudinal distance) from the preceding vehicle and control the direction actuator to decrease the lateral gap (DistL = lateral distance) with the reference path (see Figure 4.18); and

– the evolution model of each vehicle is an odometric model [ELB 03], with integration of the elementary displacement and rotation. The elementary displacement is used to maintain a safe distance (DistS) between a vehicle and its predecessor. The elementary rotation is used to actuate the direction of the wheels to decrease the lateral gap (DistL) with the reference path.

4.5.5. Extension of the approach: modeling and localization of a chain of vehicles

The model of a Bayesian network for the localization of a convoy of vehicles is given in Figure 4.20. The lead vehicle X_T is equipped with a centimetric localization system modeled by variable GPS = (x_GPS, y_GPS). A second sensor (gyroscope) is used by the lead vehicle in order to determine the bend angle (represented by variable Gyro). The position and course of the lead vehicle X_T=(x_T,y_T,θ_T) are updated by the observations made by the GPS and Gyro.

Since the distance given by the telemeter requires two positions — (x₁,y₁) and (x₂,y₂) — a relationship linking the two points is written in the following form:

[4.5]

The telemeter variable is thus represented in the graph (Figure 4.20) between X_si and X_si + 1 for the distance between the position of follower i and follower i + 1.

The idea is that, knowing the centimetric position of the lead vehicle and the distance separating it from the first follower (which is given by the telemeter), we can decrease the uncertainty around the estimation of the first follower. Given that the mean and covariance of the first follower have changed (there is more accuracy of the position), this new position and the distance linking the two vehicles give a new estimation for the second vehicle (see Figure 4.21).

Each follower vehicle is modeled in Figure 4.20 by its state X_Si. Each vehicle is equipped with a GPS localization system (differential or natural) and a telemeter. Merging these two sensors provides a new estimation of the position of each vehicle. The trajectory of the lead vehicle must be transmitted to all of the follower vehicles and as a consequence links (X_T,U_S1) and (X_T,U_S2) will be used to control the course of the vehicle and decrease the lateral gap (DistL) with the reference path (these links only represent the transmission of the trajectory of the leader vehicle to the follower vehicles and not the probability P(U_S1|X_T), P(U_S2|X_T)). Finally, to maintain a safe distance (DistS), each vehicle must know the speed (or position) of the vehicle in front, which is represented in Figure 4.20 by the links (X_T,U_S1) and (X_S1,U_S2). (These links represent only the transmission of the speed or the position of the follower vehicle and not the probability P(U_S1|X_T), P(U_S2|X_S1).)

Let us note that the lead vehicle (driven by a human) does not possess control variable U, contrary to the follower vehicles (U_S1, U_S2). Let us also note that we have removed the cartographic observation. The flexibility of Bayesian networks allows us to add other observations at any time.

Estimated position X_Si is the result of the fusion of observations provided by the GPS, the distance given by the telemeter and input U_Si. Let us remind ourselves that input U_Si = (d_k, w_k) represents the command given to the vehicle to follow the trajectory of the lead vehicle and keep a predefined gap with the preceding vehicle.

Let us note that the new observation equation (GPS, Gyro) for the lead vehicle is given by equation [4.6]. The observation equation (GPS) for the follower vehicles is the same as for the case of a single vehicle:

[4.6]

where a_k∼N(μ,Q). Covariance matrix Q is given by:

[4.7]

where σ²_x , σ²_y and σ²_θ respectively represent the deviations of the measurements in longitude, latitude and course. σ_xy represents the covariance between x and y.

4.5.6. The issue with this model

The observation equations handled so far (GPS, cartography and gyroscope) have been linear equations. The only observation that cannot be written in a linear form, however, is the distance given by the telemeter (see equation [4.5]). In order to keep the observation equations linear, a transformation is carried out on the Bayesian network indicated in Figure 4.20.

4.5.7. New model for the localization of a chain of vehicles

The variable representing the telemeter in the network in Figure 4.20 is a nonlinear function of Gaussian variables (X_T, X_S1, X_S2).

The issues we faced were the following:

– we needed to rewrite the distance given by the telemeter in a linear form; and

– take advantage of the accuracy of the estimation given by the GPS of the lead vehicle and the telemeter in order to give a more accurate estimation of each follower vehicle.

In order to take into account these remarks, we have suggested the following idea. As long as each follower vehicle has a telemeter to estimate the distance between it and the preceding vehicle and the trajectory of the lead vehicle, we proceed as follows (see Figure 4.22).

The first follower vehicle S₁ estimates its location from the position and the distance separating it from the lead vehicle. Knowing distance D₁(given by its telemeter) and the position of the leader vehicle (x_T, y_T), which is transmitted by the latter, we build a new observation as follows:

[4.8]

Similarly, the second follower vehicle estimates its position from that of the preceding vehicle (S₁) and from the distance D₂ given by its telemeter. Thus, each follower vehicle S_i where 2 ≤ i ≤ n estimates its position is as follows:

[4.9]

The new observation built from the distance given by the telemeter and the position of the predecessor is therefore written:

[4.10]

where α_k is the Gaussian noise generated by the telemeter. x_k and y_k are the position of the vehicle.

The new model for the localization of a convoy of vehicles is given in Figure 4.23. This model is equivalent to that given in Figure 4.20. The position estimated for each follower vehicle (X_Si) is the result of merging the newly-generated observation and GPS measurements.

4.5.8. Proportional commands

In order for a set of vehicles aligned in a queue to follow a reference trajectory given by the lead vehicle while keeping a predefined gap between each other, each vehicle in the convoy must know its position and the position (or the speed) of the vehicle that precedes it. Let us remind ourselves that in this piece of work, we were interested in localization and not command. To validate our localization experiments, we used simple commands referred to as proportional commands.

By referring to Figure 4.24, each vehicle S_i in the convoy can adjust its speed V_Si knowing the speed (or position) of the preceding vehicle. Thus, the longitudinal command of each vehicle is given by:

[4.11]

where DR_i represents the true distance given by the telemeter, and k₁ is a proportionality factor enabling us to specify the rate of convergence towards a safe distance, DS.

By still referring to Figure 4.24, each vehicle in the convoy must adjust its course so that it matches the trajectory of the lead vehicle. Thus, knowing the course of the lead vehicle θ at time k (the tangent to the curve at the perpendicular point of the position of each vehicle) and its own course θ^Si , we calculate the difference:

[4.12]

The lateral command of each vehicle is given:

[4.13]

where DL_i represents the lateral distance of each vehicle with respect to the reference trajectory and k₂ is a proportionality factor enabling us to converge towards the reference trajectory indicated by the lead vehicle.

Let us note that equation [4.13] does not take into account the lock angle of the wheels. In this case, the vehicles are considered (represented) to be two-wheel robots.

Figures 4.25 and 4.26 show the experimental vehicles used within the framework of the FD2S and FDADI projects. In the test presented, the lead vehicle is manually driven on the journey given in Figure 4.27. The light gray curve represents the trajectory acquired by GPS receiver Sagitta 02 in LRK centimetric mode. In this experiment, we assume that the vehicles are initially on the reference trajectory and do not necessarily have the same course as the lead vehicle. Each follower vehicle has the trajectory of the lead vehicle and the speed of the preceding vehicle at its disposal via Wifi and in real time. The follower vehicles determine their position by using a LASER telemeter and the position of the preceding vehicle. In Figure 4.27, we can see that the follower vehicles perfectly follow the trajectory of the lead vehicle. Figure 4.28 gives more details on zone which is circled in Figure 4.27.

Another point to take into account in the formation of a vehicle convoy is the speed control of the different vehicles in a convoy to continue to keep the specified gap between vehicles. This command can be set via interdistance or relative time between the vehicles. In our experiments, the interdistance threshold between two vehicles was set to 1.5 meters.

Figure 4.29 gives the interdistance between the vehicles as a function of their evolution time. We can see that the distance threshold was adhered to. The true speed of the lead vehicle shows several oscillations that influenced the distance curves at the beginning of the test, the longitudinal command (simply proportional) being so elementary. This aspect and the impact of the chosen model were assessed in order to be dealt with in the functional analysis carried out within the FD2S project framework. It is presented in the following section.

4.5.9. Functional analysis of models of the convoy

Within the framework of the FD2S project, a functional study of the chosen convoy models was carried out [GLA 08]. The dynamic functional analysis of the models consists of evaluating a simulation of the functional properties and robustness of the algorithms at stake. By ignoring the material constraints, the study aims to focus on the longitudinal control (1D simulation) and the lateral control (2D simulation) of the convoy. By designing a dedicated simulator, the goal is to explore the following properties:

– the non-collision between vehicles;

– the global stability of the convoy (does it converge towards a stable state or not, and under which hypotheses?); and

– the preservation of the global coherence of the convoy faced with particular perturbating events (the sudden stopping of a vehicle, large interdistance between vehicles, radius of the curvature, etc.).

As well as testing the robustness of the system using specific scenarios presenting the extreme cases (worst cases), the presence of noise (simulated error) in sensor measurements, and a systematic study of the parameters in order to establish the ranges of application and the optimal values of operation, this procedure lies on several levels of simulation according to an incremental “granularity”:

– a simplistic 1D model of the vehicles (point model), ideal sensors and actuators;

– a 2D tricycle model [BOM 06] of vehicles, the integration of noise on the sensors and actuators; and

– a 2D model connected to a physical motor, with the integration of constraints such as features of the ground (adherence), slope, etc.

The third level of simulation is carried out by running fine models of existing vehicles. Within this framework we aim to use a simulator such as Callas that enables us to link the results with the supervision module.

4.6. Conclusions and perspectives

The pieces of work presented in this chapter have shown the relevance of using Bayesian networks as a tool to merge multisensor data and a decision algorithm through the use of probabilistic graph theory. The Bayesian network proves to be a formalism that is potentially very useful because it can integrate the data fusion, diagnostics, learning and control in the same formalism. The other very interesting criterion that led us to choose this formalism is the reconfiguration aspect of an approach based on Bayesian networks by using specific configurations such as switching state-space model, jump-Markov model or SKF [MUR 02]. In the previous pieces of rearch, it is SKF that was put in place.

Within the framework of the GIS 3SGS ongoing FDADI project, we suggest integrating a diagnostic layer and sensor management before merging the data. We suggest adopting a complementary approach with the use of information theory, and in particular Shannon's entropy [DES 98]. Whereas Bayes' criterion relies on the minimization of the error probability, the entropy enables better results in terms of ROC (receiver operating characteristic) curves (where the probability of detection as a function of the probability of false alarms), mainly when the a priori probabilities of the hypotheses implemented are unknown or weak. Finally, the method of model construction in the form of trees is generally carried out by using a top-down approach which, in the end does not give an optimal model, contrary to the bottom-up approach that we are implementing.

The issue in which we are currently interested within the framework of the FDADI project is the absolute localization of each vehicle in a convoy with an open GPS (which provides raw satellite data). The goal is to tackle the problem upstream with “tight coupling” [BON 05] instead of “loose coupling”, as was the case within the framework of the FD2S project.

The localization approach under development comes within the context of studying the contribution of the infrastructure-vehicle interaction. More particularly, it aims to study the contribution of local information drawn from the evolution of the environment on monitoring of the integrity and loss of GPS measurements made in an urban environment.

The goal of our projects is to develop a method of data fusion with an integrated diagnostic of errors that can affect pseudo-distances (satellite-receiver distance) such as the multipath phenomenon of waves. We will develop a tightly coupled approach by looking to fuse the pseudo-distances of a Global navigation satellite system with measurements built with a LASER telemeter and a 3D cartographic database of the environment. The design of such an approach aims to use the redundancy of information in order to diagnose errors in satellite pseudo-distances for the elimination of failing satellite measurements from the procedure of position calculation.

4.7. Bibliography

[ARU 02] ARULAMPALAM S., MASKELL S., GORDON N., “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking”, IEEE Transactions on Signal Processing, vol. 50, pp. 174–188, 2002.

[BAR 82] BAR-SHALOM Y., CHANG K., BLOM H., “Tracking a manoeuvring target using input estimation versus the Interacting Multiple Model Algorithm”, IEEE Transactions on Aerospace and Electronic Systems, vol. AES-25, no. 2, pp. 296–300, 1982.

[BAR 88] BAR-SHALOM Y., FORTMANN T., Tracking and Data Association, Academic Press, New York, 1988.

[BER 99] BERGMAN N., Recursive Bayesian estimation: Navigation and tracking applications, PhD Thesis, Linkoping University, Sweden, 1999.

[BLO 88] BLOM H., BAR-SHALOM Y., “The interacting multiple model algorithm for systems with Markovian switching coefficients”, IEEE Transactions on Auto. Contr., vol. AC-33, no. 8, pp. 780–783, 1988.

[BOM 06] BOM J., Etude et mise en œuvre d'un convoi de véhicules urbains avec accrochage immatériel, PhD Thesis, University Blaise Pascal — Clermont II, July 2006.

[BON 05] BONNIFAIT P., Contribution à la localisation dynamique d'automobiles. Application à l'aide à la conduite, HDR Thesis, University of Technology of Compiègne, December 2005.

[CRI 00] CRISAN D., DOUCET A., Convergence of Seqeuntial Monte Carlo Methods, Technical Report CUED/F-INFENG/TR381, Signal Processing Group, Departement of Engineering, University of Cambridge, 2000.

[DES 98] DESROUSSEAUX C., Utilisation d'un critère entropique dans les systèmes de détection, Thesis, Lille University I, number 2381, December 1998.

[DIS 01] DISSANAYAKE M.-G., NEWMAN P., CLARK S., DURRANT-WHYTE H., “A solution to the simultaneous localization and map building (SLAM) problem”, IEEE Transactions on Robotics and Automation, vol. 17, no. 3, pp. 229–241, 2001.

[ELB 03] EL BADAOUI EL NAJJAR M., Localisation dynamique d'un véhicule sur une carte routière numérique pour l'assistance à la conduite, PhD thesis, University of Technology of Compiègne, France, 2003.

[FEA 98] FEARNHEAD P., Sequential Monte Carlo methods in filter theory, PhD thesis, University of Oxford, 1998.

[GLA 08] GLAD A., SIMONIN O., BUFFET O., CHARPILLET F., “Theoretical study of ant-based algorithms for multi-agent patrolling”, ECAI'08 18th European Conference on Artificial Intelligence in Proceedings, pp. 626–630, 2008.

[GOR 93] GORDON N.-J., SALMOND D.-J., SMITH A.-F.-M., “A novel approach to nonlinear/non-Gaussian Bayesian state estimation”, IEE Proceedings on Radar and Signal Processing, vol. 140, pp. 107–113, 1993.

[GUS 02] GUSTAFSSON F., GUNNARSSON F., BERGMAN N., FORSSELL U., JANSSON J., KARLSSON R., NORDLUND P., “Particle filters for positioning, navigation, and tracking”, IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 425–437, 2002.

[JAB 07] JABBOUR M., Localisation de véhicules en milieu urbain à l'aide d'un lidar et d'une base de données navigables, PhD thesis, University of Technology of Compiègne, France, November 2007.

[JEN 90] JENSEN F., LAURITZEN S., OLSEN K., “Bayesian updating in recursive graphical models by local computations”, Computational Statisticals Quarterly, vol. 4, pp. 269–282, 1990.

[JUL 97] JULIER S., UHLMANN J., “A new extension of the Kalman filter to nonlinear systems”, International Symposium on Aerospace/Defense Sensing, Simulation and Controls, pp. 182–193, Orlando, Florida, USA, 1997.

[KAL 60] KALMAN R.-E., “A new approach to linear filtering and prediction problems”, Transaction of the AMSE — Journal of Basic Engineering, vol. 82, pp. 34–45, 1960.

[LAU 89] LAURITZEN S.-L., WERMUTH N., “Graphical models for associations between variables, some of which are qualitative and some quantitative”, Annals of Statistics, vol. 17, pp. 31–57, 1989.

[LI 05] LI X., JILKOV V., “Survey of maneuvering target tracking part V: Multiple-model methods”, IEEE Trans. Aero. Elect. Syst., vol. 41, no. 4, pp. 112–120, October 2005.

[MAR 05] MARTINET P., THUILOT B., BOM J., “From autonomous navigation to platooning in urban context”, Proceedings of the IARP-Workshop on Adaptive and Intelligent Robots: Present and Future, vol. 1, no.1, pp. 1–9, 2005.

[MUR 99] MURPHY P., A variational approximation for Bayesian networks with discrete and continuous latent variables, Thesis, UAI, 1999.

[MUR 02] MURPHY P., Dynamic Bayesian networks: representation, inference and learning, PhD Thesis, University of California, Berkeley, 2002.

[NOR 00] NORGAARD M., POULSEN N.-K., RAVN O., Advances in Derivative-free State Estimation for Nonlinear Systems, Technical Report IMM-REP-1998–15, Technical University of Denmark, 2000.

[QUD 06] QUDDUS M.A., NOLAND R.B., OCHIENG W.Y., “A high accuracy fuzzy logic based map matching algorithm for road transport”, Journal of Intelligent Transportation Systems: Technology, Planning, and Operations, vol. 10, no. 3, pp.103–115, 2006.

[SMA 08a] SMAILI C., EL NAJJAR M.-E., CHARPILLET F., “A road matching method for precise vehicle localization using hybrid Bayesian network”, Special Issue of the International Journal of Intelligent Transportation Systems: Technology, Planning, and Operations, vol. 12, no. 4, pp. 176–188, 2008.

[SMA 08b] SMAILI C., EL NAJJAR M.-E., CHARPILLET F., “Multi-sensor Fusion Method Using Bayesian Network for Precise Multi-vehicle Localization”, The 11th International IEEE Conference on Intelligent Transportation Systems, Beijing, China, October 12–15, 2008.

[SMA 10] SMAILI C., Fusion des données multi-capteurs à l'aide des réseaux Bayesiens dynamiques pour la navigation d'un mobile en monde extérieur, PhD thesis, University of Nancy 2, 2010.

[THU 02] THUILOT B., CARIOU C., MARTINET P., BERDUCAT M., “Automatic guidance of a farm tractor relying on a single cp-dgps”, Autonomous Robots, vol. 13, no. 1, pp. 53–71, 2002.

[URI 02] URI L., Hybrid Bayesian networks for reasoning about complex systems, PhD thesis, Stanford University, Department of Computer Science, October 2002.

[ZHA 97] ZHAO Y., Intelligent Transportation Systems: Vehicle Location and Navigation Systems, Artech House, Boston, 1997.

 

 

1 Chapter written by Maan EL BADAOUI EL NAJJAR, Cherif SMAILI, François CHARPILLET, Denis POMORSKI and MIREILLE BAYART.

1 The TRAVEL platform is a set of three electric overequiped RobuCar type vehicles supplied by Robosoft.