Tag: IMU

A lane-keeping system uses a sensor-fusion engine integrating GPS and an IMU with a two-stage map-matching algorithm. The system does not require explicit lane-level geo-referencing, saving massive storage required for lane-level spatial reference information, and reduces the computational complexity of the map-matching algorithm.

By Mohamed M. Atia, Carleton University and Allaa Hilal, Intelligent Mechatronics Systems

Lane determination is an important feature of advanced automotive navigation and guidance systems. It can be used in advanced driving assistance systems (ADAS), lane-departure warnings, and self-driving cars to perform lane-level, turn-by-turn guidance and control. It is also valuable information for telematics applications such as usage-based insurance. Lane-estimation systems have been dominated by vision and infrared sensors. Light detection and ranging (lidar) has also been used as a lane-determination technique. Those systems depend on visually recognizable features and landmarks that may not be available in some areas due to weather conditions or unstructured environments.

In addition, visual data processing may need specialized accelerators and parallel computing platforms to satisfy real-time constraints. To explore other alternatives, several research projects have started to investigate the feasibility of using low-cost global positioning and navigation technologies such as GPS, micro-electromechanical systems (MEMS) inertial measurement units (IMU) and geographical information systems (GIS) as an alternate lane-determination technology. However, most current systems have two main drawbacks: they use high-end RTK GPS, which suffers from coverage issues, and they use explicit lane geo-referencing, which leads to increased storage and processing.

Here we investigate the feasibility of using standard GPS fused with low-cost MEMS-IMU and a road network that includes lane information but not explicitly storing geo-referenced lane-level links.

The accuracy of Standard Positioning Service (SPS) GPS is within 3.351 meters (m) with a 95 percent confidence level. Figure 1 shows the results of standard single-point positioning test for a stationary receiver.

The standard lane width in North America is approximately 3.6 m, requiring an unbiased precise positioning solution of much less than 1.8 m. If a safety margin of 50% is considered, unbiased precise positioning of less than 0.9 m is needed. Therefore, a standard SPS GPS technology may not be precise enough to accurately determine the vehicle’s lane. Advanced precise positioning technology like differential GPS (DGPS) can be used with high-resolution lane-level maps to achieve the lane determination.

However, these techniques may require additional cost/infrastructures and extra processing. To target a lower cost lane-determination system, this work suggests the fusion of measurements from a standard GPS, MEMS IMU and road-level network.

The work includes a sensor-fusion engine that is developed to integrate GPS and IMU using a loosely coupled extended Kalman filter (EKF). Then, a two-stage map-matching algorithm using a Hidden-Markov-Model (HMM) and a least-squares (LS) regression is developed.

The system does not require explicit lane-level geo-referencing; consequently, it saves massive storage required to save explicit lane-level spatial reference information, and it reduces the computational complexity of the HMM algorithm by reducing the number of road segments the HMM needs to decode. The overall system is illustrated in Figure 2.

PROBLEM DEFINITION

A geometric illustration of the problem is shown in Figure 3. The road-network map is represented as a set of connected segments. Each road segment is defined by a straight line segment with a start position and end position. Curved roads are approximated by a sufficiently large number of straight line segments. Based on this notation and geometric illustration, the estimation problem that this article is addressing is the determination of the lane on which the vehicle is moving.

Map-Matching with Hidden-Markov Model. The simplest map-matching method, point-to-curve-matching, is performed by searching for the nearest road segments within a threshold from the current vehicle’s position. The distance is calculated between the vehicle’s position and its projection on the map segment. However, this approach is sensitive to state estimation errors, and it fails at intersections, joins, branches or dense parallel roads. For example, Figure 4 shows a situation where biased GNSS position measurements exist, and the wrong map segment is selected because of the pure dependence on the distance metric only (for instance, D1 is less than D2).

To avoid these errors and to improve map-matching accuracy, the matching criteria must include several constraints such as map topology (connectivity), vehicle dynamics, road geometry and legal direction of motions. In this work, to consider these constraints, we keep a recent portion of the vehicle motion history and use it in the matching criteria. This strategy is known as curve-to-curve matching.

To process a noisy stream of data, the HMM algorithm is used. A Markov model is a stochastic model that describes a sequence of states. The transition from one state to another can be modeled by a conditional transition probability.

If the states are not directly observable (hidden) but can be indirectly observed through a sequence of outputs, the process is called a Hidden Markov Process. The HMM in this case is characterized by the transition probability and an emission probability that represents the probability that a given state generates a certain observable.

Both transition probability and emission probability constitute the Bayesian network of HMM. A fundamental problem of HMM is that, given a sequence of outputs, what is the best sequence of states that explains the observed outputs? This problem is solved by selecting the sequence of states that maximize the HMM probability.

This estimation process, called decoding, is solved using the Viterbi algorithm. In the proposed system, the hidden states represent map links, and the observable outputs are the vehicle poses. To develop a robust map-matching framework, the vehicle pose history, roads geometry, and map topology constraints must be considered. Therefore, the emission and transition probabilities of an HMM are formulated such that they reflect all of these constraints. The Bayesian network of the HMM for our system is shown in Figure 5. The vehicle states (poses) is obtained from the INS/GNSS filter described shortly.

In the proposed work, the length of the processed buffer of the vehicle’s state is determined based on the traveled distance. The aim is to accumulate a reasonable geometric knowledge about the trajectory segment that enables the HMM to accumulate enough geometric and topological constraints to be able to select the correct sequence of road segments in difficult intersections, joins and exit/entry roads.

EKF GNSS/INS SYSTEM

The navigation problem can be modeled as a dynamic system of states vector x(t) as follows:

 (1)
 (2)

(Figures: Mohamed M. Atia and Allaa Hilal)

where f(.) is a nonlinear dynamic model, w(t) is a stochastic system noise vector, u(t) is a control signal vector that triggers the transition from current state to a future state, y(t) is external measurements vector (observables), h(.) is a nonlinear measurement model and v(t) is a stochastic measurement noise vector. Using first-order Taylor series approximation, (1) and (2) can be linearized as follows:

 (3)
 (4)

 (5)

 (6)

(Figures: Mohamed M. Atia and Allaa Hilal)

A Kalman filter calculates an optimal estimation of provided that w(t) and v(t) are zero-mean Gaussian noise vectors with covariance matrices defined by:

 (7)

 (8)

and δx is the error vector with zero-mean and a covariance matrix P defined by:

 (9)

Using zero-hold discretization where derivative is approximated by:

 (10)

where T is the sampling time, equations involving HMM probability can be written in discrete form as follows:

(11)

(12)

The optimal estimation of the error vector, δx_k, given measurements, y_k, is calculated using two steps: prediction,

 (13)

  (14)

and update,

(15)

(16)

(17)

(Figures: Mohamed M. Atia and Allaa Hilal)

In INS/GNSS systems, the dynamic system state transition (x(t)) is triggered by IMU sensors (accelerometer and gyroscopes) while GNSS measurements are used as observables (y(t)). The observables update in our case is GNSS position and velocity. Therefore, the measurement error model is defined as follows:

(18)

where H is defined as follows:

(19)

Lane Estimation. When the road segments have been accurately selected based on the filtered vehicle’s pose, the projection of the vehicle’s positions on segment lanes can be easily calculated knowing the lane widths and number of lanes. The sum of squared errors for each lane is then calculated by:

(20)

where N is number of epochs, and p_v is the projection of vehicle’s position on lane. The lane associated with the minimum error is selected as the designated lane.

(Figures: Mohamed M. Atia and Allaa Hilal)

Lane-Change Detection. If a lane change occurred within the processed buffer of data, the least-squares regression will not converge to the correct lane. Therefore, the buffer needs to be partitioned at the lane-switch locations. Thus, a lane-change detection module is developed. In this work, a lane-change detection method is designed based on capturing the patterns of the vehicle’s orientation and raw gyroscope measurements. The heading and raw gyroscope measurements during lane changes are shown in Figure 6 and Figure 7.

The general pattern that the lane-change module detects is a peak or a valley in azimuth accompanied by a peak/valley or valley/peak sequence in the gyroscope measurements. To detect peaks and valleys, the standard deviation of a moving window of data is calculated and compared to a peak/valley threshold. If both gyro and azimuth peak/valley sequence are consistent and matched with the pattern described above, a lane change is declared.

Two algorithm phases of processing are then applied:

Acquisition Phase. GNSS and IMU measurements are fused in the main EKF, and HMM map-matching is performed and a lane is estimated. The innovation sequence of the main EKF, which is the difference between the predicted state and GNSS updates, is calculated over a buffer of data. If the innovation sequence is within a small threshold and no lane change has been detected, the acquisition phase is concluded and the tracking phase begins.

Tracking Phase. Two EKF filters are initiated. One EKF accepts position updates from the projection of the vehicle’s position on the selected lane, and the other EKF accepts GNSS position updates only. A discrepancy measure is evaluated between the two EKF instances for a short window of time. If this discrepancy measure is higher than a threshold, a temporary GNSS deviation is assumed and the system keeps reporting the current lane as the designated lane. If GNSS measurements started to be centered again on the new lane, a lane change is confirmed and the output of the first EKF instance will be the correct state. Otherwise, this lane change is declared as false and the second EKF output is the correct output. The overall block diagram of the proposed system is shown in Figure 8.

TESTS AND RESULTS

The proposed system has been tested on a computer connected to a GNSS receiver and an automotive MEMS-grade IMU, and road-network map data. A GPS-enabled camera was installed to capture video of the experiment, to be used as a ground truth to verify the results of our algorithms. Sensor specifications are given in Table 1 and Table 2. The effect of level arm (distance between IMU and GNSS antenna) was not considered in this implementation.

Three testing trajectories were collected during July 2015 through Highway 400 from Wilson Avenue in the south to Davis Drive in the north. Approximately 65 kilometers of trip data was collected. The data included some urban areas but was mostly open sky. It also included challenging road intersections and road joining/branching points. The experimental setup was designed such that the system automatically started when the vehicle’s engine was turned on. A Linux OS was installed on the gigabyte computer box, and a data acquisition firmware was configured to automatically begin when the computer starts. Measurements from the GNSS receiver at 1 Hz and the IMU at 50 Hz were synchronized on the computer. The main algorithm including GNSS/INS fusion and map-matching was developed in native ANSI C language for efficient processing. Original raw IMU data was set to 50 Hz down-sampled to 5 Hz. Within this interval, the real-time system could fetch map information from a cached database file, perform basic prediction steps and implement the forward calculation of a Viterbi algorithm (including calculation of emission and transition probabilities) that is needed for the HMM map-matching step.

Lane-Determination Results. The lane estimation results were logged and time-tagged. Using the video recording, the ground truth lane-level solution was visually inspected and manually recorded in a file. Since both the video camera and the proposed INS/GNSS/maps systems log data tagged by GPS time, synchronization between ground truth and the estimated lane were possible. The estimated lanes were visually inspected record by record and results were saved in an Excel sheet. The results were written into a time-tagged file where each row can be easily visually inspected by looking at the portion of images corresponding to the same time-tag. The time-tag used was the UTC-time contained in the NMEA GNSS raw measurements. The overall accuracy of the proposed system in lane determination is shown in Table 3.

Figure 9 and Figure 10 show example snapshots of the visual inspection software tool developed to evaluate the accuracy of the system. As can be seen in the figures, an image of the road that indicates the correct lane is displayed in the upper graph, while the estimated lane information is displayed along with road information including lane errors in the lower graph. Figure 10 shows that the system can identify the correct lane when the number of lanes is increased.

CONCLUSION

This work described a low-cost lane-level positioning system using a conventional GNSS receiver, MEMS IMUand commercially available road-level network without the need for explicit spatial storage of lanes. The research used a conventional GNSS receiver and MEMS IMU with a computationally efficient two-stage HMM-based map-matching algorithm that avoids the explicit use of lanes as hidden states, which significantly reduces the size of the HMM network and consequently enhance its real-time performance. The proposed system provides an alternative lane determination method without the need for computationally expensive vision/lidar methods that may fail in dark, foggy or dynamically changing environments. The work showed extensive experiments under different road sections, showing an average lane-determination accuracy of 97.14%.

ACKNOWLEDGMENTS

This work was first presented at ION International Technical Meeting, January 2018.

MANUFACTURERS

The system comprises an Intel Celeron N2807 1.58-GHz Mini PC connected to a u-blox EVK-7P kit GNSS receiver and an automotive MEMS-grade IMU 3D space sensor IMU from YOST Labs, and road-network map data from HERE. A GPS-enabled HP f310 car camcorder captured video.

---

MOHAMED M. ATIA received a Ph.D. in electrical and computer engineering from Queen’s University at Kingston. He is assistant professor and founder/director of the Embedded Multi-sensor Systems research laboratory in Carleton University, Ontario, Canada.

ALLAA HILAL received a Ph.D. degree in electrical and computer Engineering from the University of Waterloo. She is director of the innovation and emerging technology department at Intelligent Mechatronic Systems, a connected-car company based in Waterloo, Canada.