Robust Positioning in Safety Applications for the CVIS Project

Transcription

1 2010 IEEE Intelligent Vehicles Symposium University of California, San Diego, CA, USA June 21-24, 2010 TuE1.3 Robust Positioning in Safety Applications for the CVIS Project Dominique GRUYER, Steve PECHBERTI, Denis GINGRAS and Francis DUPIN Abstract This paper describes hybrid fusion module used in a strong localization context (POMA) for embedded vehicle applications. This work has been developed in order to give an answer to the POMA (Positioning, Maps and local referencing) sub project objectives. These objectives are to provide, for a set of high level applications, a positioning service included a service quality, a metric accuracy (lane) and a robust result. This work is involved in CVIS European project. The use of IMM approach in the Hybrid Fusion module will be justified in comparison to the different current probabilistic methods. The IMM, contrary to the non modular methods, is based on the discretization of the vehicle evolution space into simple maneuvers, represented each by a simple dynamic model such as constant velocity or constant turning etc. This allows the method to be optimized for highly dynamic vehicles. The application of this positioning service will be presented in a real time embedded architecture. The presented results are based on real measurements collected from representatives scenarios (test track, peri-urban road, highway). These results show a real interest in using the new IMM method in order to reach the POMA s objectives. I. INTRODUCTION Until now vehicle positioning has been mainly used in navigation systems for information delivery applications In recent years however, various positioning-based safety systems have been developed For these applications, position estimation must be much more robust and reliable. To achieve this goal, a new positioning module called POMA, will be presented and described in details. For this kind of applications, positioning is usually based on hybrid fusion algorithms. In this paper, we will review briefly an improved IMM (Interacting Multiple Model) which will be further described in details. The IMM algorithm is a well known modular approach, which is based on the discretization of the vehicle evolution space into simple methods to be optimized for highly dynamic systems such as vehicles. Unfortunately classical IMM shows some drawbacks concerning some real time multi sensors applications. In this work, we focus on Dominique GRUYER is a Researcher at the Laboratoire des Interactions Vehicules Infrastructure Conducteur (LIVIC - LCPC/INRETS), 14, Route de la minière, VERSAILLES, dominique.gruyer@inrets.fr Steve PECHBERTI is a Research Engineer at the Laboratoire des Interactions Vehicules Infrastructure Conducteur (LIVIC - LCPC/INRETS), 14, Route de la minière, VERSAILLES, steve.pechberti@inrets.fr Denis GINGRAS is Professor Dr Eng. at the Universit de Sherbrooke, Sherbrooke, Qc, Canada, denis.gingras@usherbrooke.ca Francis DUPIN is a Research Engineer at the Laboratoire des Interactions Vehicules Infrastructure Conducteur (LIVIC - LCPC/INRETS), 14, Route de la minière, VERSAILLES, francis.dupin@inrets.fr vehicle positioning with asynchronous sensors in order to report these drawbacks and then propose a new solution. The algorithm developed in this paper is embedded in the POMA platform in order to provide a positioning data. The developed architecture (Hardware and software) have been made in order to give an answer to the constraint of lane accuracy and robustness of the CVIS European project. In the following section, a state of art is presented in order to justify the use of IMM approach in POMA project. Then IMM will by briefly presented. Moreover, the adaptation and improvement of this method for POMA project will be presented. These improvements are mainly done in order to get a more adapted set of used models and to allow an efficient likelihood updating. Section III will present both real hardware and software architecture developed specially for POMA project and real time use. Section IV will show the results, the performances and the limits of this hybrid fusion on several situations. The tests will be done in the Satory s test track, in the Minire s peri-urban area and the A86 highway in south of Paris. II. DATA FUSION FOR ROBUST POSITIONNING SYSTEM A. The state of the art In this section, we describe the different robust positioning methods usually applied to driver assistance systems[5]. 1) Kalman Filter: The most popular approach for vehicle robust positioning is based on the Kalman filter (KF) [11]. In this case, sensors data considered imperfect and vague can be modeled as a random noise distribution known a priori. The data fusion is reduced to solving a system of state equations. For the standard Kalman Filter, the system is composed only of linear equations, its resolution being optimal in a statistical sense (minimum variance of the estimation error). 2) Extended Kalman Filter: In a real situation however, the behavior of a vehicle is not linear. To solve the corresponding nonlinear problem, an extension of the Kalman filter is often applied, known as the Extended Kalman filter (EKF). It is based on a linearization of the equation system and assumes that all variations are small enough to insure the validity of the linear hypothesis and estimation. Depending on the acquisition frequency, however, the linearization can generate distorted estimation, and in this case, provide results which are corrupted or unreliable information. It has been found that in practice, without exteroceptive data, the prediction is correct for short periods of time, depending on the complexity of the vehicle s movement. [4] /10/$ IEEE 262

2 3) Unscented Kalman Filter: Recently, Julier and Uhlmann have introduced an approach called the Unscented Kalman Filter (UKF) [9] [8] [10]. It is based on the Scaled Unscented Transformation for the computation of random data. This approach has been used to manage non linear cases which cannot be linearized by the EKF. Due to the many hypotheses implied however, it is not well adapted to vehicles positioning having a quick dynamical behavior [16] [17]. 4) DD1 and DD2 algorithms: Nørgaard and al [20] proposed two closely related algorithms, namely Divided Differences of order 1 (DD1) and Divided Differences of order 2 (DD2). These algorithms introduce a set of estimators based on a polynomial approximation of the non linear function obtained by an extension of the Stirling interpolation formula. Conceptually, the principle of these algorithms is the same as the EKF one, but the implementation is different. Contrary to the Taylor approximation, no derivative is requested for the interpolation formula. The implementation is much easier and can provide an estimator even on a singularity where no derivatives are defined. It should be pointed out that the DD2 has the same spatial latency problems as the UKF. Only the DD1 algorithm provides results similar to the EKF with better precision on the position estimate. 5) Monte Carlo localization and particle filters: In order to better take into account the systems non-linearities, another method based on points clouds has been proposed by J.E. Handschin [7], called the Monte Carlo Localization. It uses a random set of sample points on the underlying probability density function. It doesn t attempt to describe the density function exactly, but rather provides a coarse representation of it by keeping some random sample points (called particles) which evolve thanks to different algorithms [12] [13] [6]. This approach is powerful because it can be used to achieve global positioning, solve multiple positioning problems or model sensor noise. The more we have particles, the better the results are. The main issue by using this approach is to obtain a proper trade-off between quality of the results and computation time which is proportional to the number of particules used. 6) The Interacting Multiple Model: The approach described here makes the hypothesis that according to a finite set of prototypical behaviors of a given complex systems, its modeling can be achieved by decomposing it into several sub-systems, which are less complex and easier to implement. So each of these sub system will emulate the behavior of the global system for a specific situation. The complementarities and redundancies of the sub systems can, after a fusion step, cover the entire functional space of the global system. This approach called Interacting Multiple Model (IMM), has been developed by Blom and Bar Shalom [1] [2]. It is widely used for estimation, control, system modeling, object tracking with highly nonlinear dynamic, and recently for vehicle positioning. In this case, the method is robust and provides a good position estimate for the various typical vehicle movement [18]. In its original form, however, the IMM filter is sensitive to the use of corrupted or noisy data, which can cause it to fail. That s why the IMM used in this work is a modified version with new strategies presented in the following section. B. Hybrid Fusion for robust positioning In order achieve the objective to assess an accurate and robust positioning, sensors are needed. Theses sensors are shared in 2 groups. The first group involves the exteroceptive and reference sensors (GPS). The second group corresponds to the prorioceptive sensors (INS, Odometer ). Figure 1 presents this hardware sensors architecture of the POMA project. Figure 2 shows the functional architecture included the hybrid fusion module. Figures 1 and 2 presents the hardware and functional architectures of the POMA module for vehicle positioning. Fig. 1. Fig. 2. POMA Main architecture POMA Functional architecture 1) IMM Principles: The proposed IMM formulation used here is based on the hypothesis that the vehicular system of equations describes a Markov process [1]. In that case, the state of the chain represents, for a given time range, a vehicle behavioral mode (or model) of the vehicular system (chosen among a given set of available models or behavioral modes). The markovian process is characterized by its probability 263

3 transition matrix which is assumed known a priori. An IMM estimator is used to predict the current behavioral mode of the vehicular system. For a given IMM estimator, a set of equations is used to describe each of the different models defined [2]. The Markov model probabilities are updated at each measurement, and the computed weighting factors are used to determine the next model. So the observed system evolves by selecting one model out of r defined models with a switch between models managed by a Markov chain given by the matrix. We consider that r models describe all behaviors of the system. The model j is defined by the state equations : { x(k) = Fj (k)x(k 1) + G j (k)u(k 1) + r j (k 1) y(k) = H j (k)x(k) + q j (k) (1) where x(k) is the state vector of the vehicular system at the instant t, F j (k) is the evolution matrix of the system state during the event noted M j (k) = {the model j is active at the time k}. u(k) is the vector of command at the time k, G j (k) is the command matrix, H j (k) is the measurement matrix, r j (k) and q j (k) are respectively the state noise and the measurement noise. These noises are Gaussian, white, mutually independent, with zero mean and with covariance matrix R j (k) and Q j (k). y(k) is the measurement vector of the system at time k. i, j 1,..., r are indices of the N models. The switch between models are governed by a Markov chain with a limited number of states and where the probability to switch from M i (k 1) to M j (k) p ij = P r { M j (k) M i (k 1) } (2) The r 2 probabilities p ij are supposed known. If it is not the case, we can set them to 1 r at the initial state [2]. 2 The IMM estimator is a recursive process which can be splitted in multiple steps at each iteration, the aim is to obtain the state of the vehicular system and its associated covariance matrix. ˆx(k k) = E(x(k) y k 1 ) P (k k) = E( x(k) x(k) T y k 1 ) where x(k) = x(k) x(k k) and y k = y(1),..., y(k). For that, we use different filters, which produces different estimations, one for each local model. Then we use these estimations to compute the residual errors. These residuals are used to compute the probability of activation for each model by using a likelihood function. The global estimation of the system state is then computed as a weighting of the estimations coming from the different models filters. The figure below shows the structure of the IMM algorithm for two models. At each iteration, the IMM algorithm executes four steps to provide a global estimation of the vehicular system state. The four steps at each iteration are : (3) Fig. 3. IMM structure for 2 models The interaction and estimation mixing From the estimation ˆx i (k 1 k 1) given for each model and the probability u i (k 1) computed for each model at the previous stage, the mixed estimation ˆ x i (k 1 k 1) is obtain. The mixed covariance matrix P i (k 1 k 1) is provided too. The filtering and the likelihood computation The Kalman filter is used in order to update both the system state and its dedicated covariance matrix ˆx i (k k), P i (k k). Activation probabilities update The likelihood function is used in order to update the activation probability µ i (k). global estimation This stage allows to merge the estimations provided by each model ˆx i (k k). This stage has for main goal to evaluate each filter and to merge their outputs in order to compute a new global estimation ˆx(k k) and a new covariance matrix P (k k). The current fusion function consists to make the sum of every estimations weighted by their respective probability. More details on this filter implementation can be found in [18], [14] and [15]. 2) Application to robust vehicle positioning: In order to represent the various system maneuvers, we used three simple models to describe the vehicle evolution: the constant velocity CV, the constant acceleration CA, the constant turning CT. Details for the first three models (CV, CA, and CT) are given in [18] and [14]. 3) IMM functionalities improvement for POMA service: In order to adapt the classical IMM approach to the positioning problem, it is necessary to take into account the different vehicle manoeuvres. For this reason, 2 new models have been added to the existing ones. The final set of models used in the POMA s Hybrid Fusion module are the CV, the CA, the CT, the CS (Constant Stop) and the CR (Constant Rear) for backwards driving situation. Moreover, the data provided by GPS and proprioceptive sensors are asynchronous. So, now, the stage of likelihood updating is not adapted to the constraint of POMA service accuracy. In order to solve 264

4 Figure 4 shows a bad behaviour of the filter when, during a loss of GPS data, the navigation path involving several different manoeuvres (i.e. straight and turn manoeuvres). Of course, with asynchronous data and without likelihood updating during the prediction stage, the filter diverges. The probability curves shows a CV manoeuvre selection (time steps 100 to 170) even when the vehicle make a turn manoeuvre. Figure 5 presents the result with the new IMM algorithm and a comparison between the reference track, the recorded GPS positions and the outputs of the Hybrid fusion. We could notice that even an important GPS loss appears, the IMM computation is still robust regarding the reference track. During the GPS loses, models probabilities are not update and the IMM algorithm is only feed with proprioceptive sensors data. The details of this new implementation of the IMM filter and explanation of the improvements are given in [19]. Fig. 4. positioning and model probabilities with standard IMM approach this problem, an innovative approach has been implemented in order to update likelihood both in correction stage and prediction stage. With this mechanism, the identification of the current manoeuvre is easier. Effectively, the probability applied to each model is now available every time. In order to shows the efficiency of these improvements, a simple scenario is presented. Two sets of results are given (Figure 4 and 5). These results present the positioning and the probabilities for the different models (maneuver). The used models are Constant Velocity model (CV) for the free motion mode, Constant Acceleration model (CA) that describe the longitudinal dynamics, Constant Turn model (CT) for lateral dynamics, Constant Stop model (CS) for no motion and Constant Rear model (CR) for backwards driving situation. The higher the probability is, the more confident the system is in the predicted active mode. III. POSITIONNING MODULE ARCHITECTURE (POMA) A. Embedded Hardware implementation In order to test and to evaluate the POMA architecture and the positioning service, a demonstrator has been designed. The embedded hardware was composed by a PC running the POMA system equipped with a QFree CVIS PCI card. On this card, the following sensors are available: a GPS receiver, an accelerometer and a WAVE module. Figure 6 illustrates the material used to validate the demonstrator Fig. 6. hardware embedded architecture Fig. 5. positioning and model probabilities with new IMM approach 265 1) Software implementation: The POMA s software architecture was build using RTMaps platform[3]. RTMaps allows to record data flow, to replay the data and to prototype applications. The Hybrid Fusion package consists of three levels of components (Figure 7). The first level (interfaces) allows processing the data coming from the sensors. These interface modules give a set of functionalities for data denoising, bias estimation, data formatting. The second level provides multiplexors for each sensor type. The last level concerns the hybrid fusion algorithms. Two fusion algorithms are provided: an extended Kalman filter and the new IMM filter. This way to implement application is really efficient because at every time, it is possible to tune and to change every parameter of the RTMaps components. In fact, each component (interface, multiplexor, hybrid fusion, result display) has an interface of properties.

5 Fig. 7. modular architecture for hybrid fusion Fig. 9. trajectory reference on the Satory s test track Fig. 8. POMA s service in CVIS platform Fig. 10. time period of the EGNOS sensor Figure 8 present the final CVIS s platform with the POMA s platform. In this figure, in same time POMA s component provide vehicle positioning and mapmatching services. IV. INTERPRETATION AND PERFORMANCE ANALYSIS A. Test scenarios and results This section will present the Hybrid Fusion results with the POMA platform. Three types of road have been tested. The two first scenario deal with the Satory s test track and the Miniere s peri-urban road. The third scenario corresponding to highway situation. This last scenario is very interesting because of the several tunnels and area disturbing the GPS data reception. B. Results on the Satory s test track The first site where the POMA module has been tested is the Satory test track. A vehicle equipped with the POMA module drove around the test track. Figure 9 shows the comparative performance of different hybrid fusion algorithms compared to the reference test track and the GPS position estimate. For the figures 9, 12 and 16, the same color code is used: that is the GPS position in blue, the EKF position estimate in red, the result of the IMM in green and the reference test track position in cyan. Figure 10 presents the time period of the EGNOS received signal, we can observe that EGNOS signal is not as stable as we could expect. A lot of data losses are visible. Figure 11 presents the error between the Hybrid Fusion output and the reference. We can notice that the maximum error observed is less than 5 meters with an average value of 2 meters. These results are obtained with a low cost configuration (low cost sensors). It is an interesting result because this average value corresponds to the POMA accuracy constraint. This accuracy is lower than the width of a lane (3.5 m). Fig. 11. positioning error (in meter) 1) Results on the Minière s peri-urban road: The next test is performed on the Miniere s road. For this test session, a number of areas had no signal from the sensor EGNOS. This was mainly due to the forest zone. This session is very interesting in order to evaluate the divergence of the filter when no GPS data are available to feed the correction stage of the hybrid fusion. Figure 12 presents the different hybrid fusion outputs compared with GPS impacts and the test track reference. We can see the 2 critical forest areas where no GPS data are available (Figure 15). The second test has been performed on the Minière road. For this test, we have simulated the loss of the EGNOS signal 266

6 in order to determine the drift of the system when this one is only fed with a low-cost GPS. Figure 12 shows the different computation outputs compared to the GPS position and the test track reference position. Fig. 15. loss of GPS data in forest area Fig. 12. reference trajectory on the Minire s peri-urban road. Figure 13 shows three important losses of the EGNOS data for a long time, more than 30 seconds. Fig. 16. reference trajectory for highway (A86) Fig. 13. time period for EGNOS sensor About the positioning error (Figure 14), we can observe that the maximum error for this peri-urban scenario is less than 12 meters after a 30 second lose of EGNOS signal, even if this error is higher than in optimal condition, this error is lower than the GPS inaccuracy. In this case, EGNOS sensor provides a bad positioning (20 meters of inaccuracy) due to a forest area. During the EGNOS unavailability, the divergences are due to the inertial sensor bias and noise. So, in future works, an effort will be done on this bias and noise assessment in order to improve the quality of proprioceptive data. Moreover the mechanism used for the likelihood updating in the prediction stage is very useful for the proprioceptive failure identification and hybrid fusion auto diagnosis stage. 2) Results on the A86 highway: The last test session takes place on a section of the A86 highway. This section is interesting because many sections of this highway have tunnels. Figure 16 presents the bird view of route (half a hour) on the A86 highway. Figure 17 shows the different area producing disturbances in the EGNOS data availability (loss of data, low integrity, and bad accuracy). These critical areas correspond to the tunnels locations. These figures also show the error between the reference, the GPS impacts and hybrid fusion outputs. Different interesting situations are displayed on these graphics. The first interesting point to notice is on the graph 1. We can observe a discrepancy between the hybrid fusion outputs and the reference, but if you look at Figure 18, at the same time we can observe that the EGNOS confidence is very good whereas the results differ, it is due to the fact that although confidence in the system EGNOS at this moment is correct, the values provided by EGNOS are corrupted. For the graph 2 and 3 of the figure 17, even if we get an EGNOS loss, the positioning is still robust with an error always lower than 15 meters which is better than some results obtained with a low cost GPS in good condition. Here we must keep in mind that these results are obtained in tunnel situation. Fig. 14. positioning results for Minire s peri-urban road Fig. 17. loss of GPS or bad behavior of GPS data for highway (A86) 267

7 Fig. 18. Fig. 19. time period for EGNOS sensor positioning results for highway area V. CONCLUSION AND FUTURE WORKS This paper has presented a embedded positioning system developed for safety vehicular application in the European CVIS project. Different hybrid fusion approaches have been compared for the positioning issue. One method has been selected for its robustness, the Interacting Multiple Model solution. To this solution, two models have been added in order to take into account the complete vehicular dynamic system in urban condition. Moreover, a novel approach for IMM mode likelihoods updating by using only proprioceptive sensors data has been implemented and tested on several scenario with degrading GPS signals, particularly on the motorway A86 where tunnels are located. All obtained results are encouraging. They introduce complementary works on this positioning system, namely, a mechanism of denoising the received information; and a self calibration to autocorrect for the system in case of information data loss, such as EGNOS loss. [3] Ph. Bonnifait, J. Laneurit, C. Fouque, and G. Dherbomez. Multihypothesis map-matching using particle filtering. 16th World Congress of ITS Systems and Services, Stockholm, Sweden, [4] C. K. Chui & G. Chen. Kalman filtering with real-time applications. Springer-Verlag, second edition, 0:0, [5] D. Gruyer, A. Lambert, and B. Mourllion. Etat de lart des stratgies de fusion de donnes pour la localisation. Rapport du Thme 3, Projet ARCOS, T3.2. [6] F. Gustafsson, N. Bergman, and all. Particles filters for positioning, navigation and tracking. IEEE Transaction on Signal Processing. Special issue on Monte Carlo methods for statistical signal processing, [7] J.E. Handschin. Monte carlos technics for prediction and filtering of nonlinear stochastique precesses. Automatica, 6:55 563, [8] S.J. Julier. The scaled unscented transformation. Automatica, [9] S.J. Julier and J.K. Uhlmann. A general method for approximating nonlinear transformation of probability distribution. Technical report, University of Oxford, November [10] S.J. Julier and J.K. Uhlmann. A new extension of the kalman filter and nonlinear systems. In Proceedings of the SPIE Aerosense International Symposium on Aerospace/Defense Sensing, Simulation and Controls, Orlando, Florida, September [11] R. E. KALMAN. A new approach to linear filtering and prediction system. In Transactions of the ASMEJournal of Basic Engineering, 82 (Series D): 35-45, [12] Francois LeGland. Introduction au filtrage en temps discret. filtre de kalman, filtre particulaire, modles de markov cachs. Master s thesis, Universit de Rennes 1, [13] Franois LeGland. Filtrage particulaire. In Actes du 19me Colloque GRETSI sur le Traitement du Signal et des Images, volume II, pages 1 8, September [14] X. R. LI and V. P. JILKOV. Survey of maneuvering target tracking, part v: Multiple model methods. In IEEE Transactions onaerospace and Electronic Systems, vol. 41, no. 4, [15] R. T. MOREO, A. Z. IZQUIERDO, B. U. MINARRO, and A. F. G. SKARMETA. High-integrity imm-ekf-based road vehicle navigation with low-cost gps/sbas/ins. In IEEE Tans. on Intelligent Transportation Systems, vol. 8, no. 3, pp , [16] B. Mourllion, D. Gruyer, A. Lambert, and S. Glaser. Kalman filters comparison for vehicle localization data alignment. In ICAR 2005 Seattle, Washington, USA, July 18th-20th [17] B. Mourllion, D. Gruyer, A. Lambert, and S. Glaser. Kalman filters predictive steps comparison for vehicle localization. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS05), Alberta, Canada,, August 2-6, [18] A. Ndjeng Ndjeng, S. Glaser, and D. Gruyer. A multiple model localization system for outdoor vehicles. In Proc. IEEE International Symposium, Istanbul, [19] A. Ndjeng Ndjeng, D. Gruyer, and S. Glaser. New likelihood updating for the imm approach application to outdoor vehicles localization. In IROS09, Saint Louis, USA, Octobre [20] M. Norgaard, N. K. Poulsen, and O. Ravn. Advances in derivativefree state estimation for nonlinear systems. In Revised edition of the technical report IMM-REP , Technical University of Denmark, VI. ACKNOWLEDGMENT This work has been completed within the POSITIONING, MAPS and LOCAL REFERENCING sub-project (POMA) in the European project CVIS. REFERENCES [1] H. A. P. Blom. An efficient filter for abruptly changing systems. In 23rd IEEE Conference on Decision and Control, pages , Las Vegas, NV, December [2] H. A. P. Blom and Y. Bar-Shalom. The interacting multiple model algorithm for systems with markovian switching coefficients. In IEEE Transaction on Automatic Control, August