Road Maps and Sensor Integration for the Enhancement of Lane-Keeping Assistants

Abstract

Current efforts of vehicle manufacturers and research groups in designing and developing safer Intelligent Transportation Systems have revolved around achieving higher levels of driving automation for on-road vehicles. However, current approaches remain unable to assure safe vehicle autonomy in all conditions. Leveraging the communication between the infrastructure, for instance, the road geometry from high-definition maps, and vehicles could be a key enabler of safer Intelligent Transportation Systems. This combination would increase the overall traffic awareness which could benefit current automation approaches. In this study, a new lane-keeping system integrating information from a road map, satellite receiver, and inertial sensors is presented. Tests driving in complex urban environments showed that the proposed system kept the vehicle centered in the lanes during long satellite outages. This result was accomplished with a novel integration between the inertial and road map where the inertial was calibrated by the Map. The position cross-track accuracy upper and lower bounds, at 95% confidence, were 3 and 1 cm from achieving the control limit level (0.1 m) for Intelligent Transportation Location Based Systems. With these results, this work provides a new contribution to increase the robustness of current lane-keeping assistant approaches.

1. Introduction

Road traffic deaths reach 1.19 million every year, being the leading cause of death for people aged between 5 and 29 years old (WHO) [1]. To reduce road accidents, several efforts in the development of vehicle safety systems, Advanced Driver Assistance Systems (ADAS), and Autonomous Vehicles (AV) technologies have been made.

A common approach to autonomous driving is to use machine learning techniques to learn the driving behavior from camera and other sensor data. This approach is restricted to the training data variability (light, weather, etc…) and location, thus being restricted in its Operational Design Domain (ODD) [2].

While higher levels of autonomy are still a few years ahead in the future, reliable Lane Departure Warning (LDW) and Lane-Keeping Assistant Systems (LKAS) could prevent collisions [3]. Current LKS have been mostly done using imagery sensors, such as cameras, to identify the lanes. Several road boundary detection and following have been developed assuming the road scene is identifiable [4]. However, combined with the road environment complexity, imagery systems performance for lane detection is affected by several factors: dependence of natural light, alternation of structured and unstructured roads; and appearance of roads in weather changes and scenes with shades and darkness [5, 6]. Two fatal autopilot incidents have demonstrated the limitation of cameras in poor light conditions and this alerted the industry and community that current systems still need major improvements [7, 8].

The accurate tracking of the vehicle’s position is an essential task to safely keep the vehicle in the correct lane of navigation. This task, also known as vehicle localization or positioning, is equipped with several sensors to estimate the vehicle position, velocity, and attitude (PVA) in space using a sensor fusion algorithm, e.g. the Kalman filter (KF). A commonly used approach in vehicle localization algorithms is the fusion of Global Navigation Satellite Systems (GNSS) with Inertial Navigation Systems (INS). GNSS errors are relatively noisy from the second to second period and have long-term accuracy, whereas INS exhibits relatively short-term low noise errors but tends to drift over time [9]. This combination has a complementary nature and the potential to provide accurate solutions anywhere on the Earth’s surface. However, during long GNSS outages, INS alone does not offer a reliable solution due to its inherent errors. Thus, additional information to provide a continuous and reliable solution needs to be explored along with different integration approaches.

Another piece of information that could provide extra robustness to vehicle navigation sensor fusion, which is neither affected by the surrounding environment nor by sensor noise, is the use of a High Definition (HD) map of the roads. For instance, applications such as curve speed warning, vision enhancement, path prediction, LKAS, and collision warning can all be enhanced with the use of an HD map of the roads and, consequently, provide large-scale autonomous navigation [10]. Some applications, integrating sensor fusion and maps along with a map-matching algorithm, have shown improvements in solution availability [11, 12]. Other contributions using a 3-D map of cities could predict non-line-of-sight (NLOS) satellite signals and improve 2-D positioning in dense urban areas [13, 14]. In the above-mentioned contributions, the map information and sensor fusion were used in separate filters, and the map-matching algorithm would connect them in a further step of the algorithm. It was also observed an improvement in reliability with low positioning accuracy. Moreover, the map-matching approaches in the literature were dependent on the correct road identification step, which can generate ambiguous matches. Thus, applications using road maps can be further improved; for instance, by inputting the map space directly into the navigation filter, navigation constraints, and calibration of sensors could be performed.

In summary, there are many challenges to providing a safe and reliable lane-keeping vehicle navigation system. Among these challenges, LKAS at a global scale must be proposed to enable AV systems without restrictions. To achieve this, the accurate track of the vehicle position on its current lane of navigation operating in any environment is the major task to be solved. This way, a road map could provide extra information to GNSS/INS sensor fusion and support its current limitations. With the intent of providing a generic approach for LKAS, we propose a novel approach combining map of the lanes information with current sensor fusion techniques using satellite and inertial information. This map of lanes is obtained before navigation with any mapping techniques, and it precisely represents the location of the center of lanes, which can be retrieved during navigation. This GNSS/INS with map fusion can track the vehicle position at all times and potentially provide a global and scalable solution. The objective of this system is to constantly track the vehicle’s position on the road at all times and in any environment. Another contribution of this system is to provide optional redundant information to enhance the safety of current LKAS.

2. Sensor navigation overview

Vehicle navigation can be represented by positions, velocities, and orientation states. Common sensors used to track either directly or indirectly are Inertial Measurement Units (IMUs), Global Navigation Satellite Systems (GNSS), wheel odometers, cameras, Light Detection And Ranging (LiDARs), RAdio Detection and Ranging (RADARs), and others. This work will use IMUs and GNSS as sensors; thus a brief explanation is given in Sections 2.1 and 2.2. To convert sensor measurements into parameters of interest, and track them over time, pre-processing and filtering are required. In 2.4, the pre-processing of IMU data and GNSS, and INS fusion techniques are described.

2.1 IMU navigation

IMUs sense rotations and accelerations in three orthogonal axes. The combination of these two sensors is capable of producing a navigation solution independent of any other sensor. This solution, also known as the Inertial Navigation System (INS), can be obtained by integrating the angular rates and specific force over time. The solution is composed of position, ${\mathit{r}}_{\mathit{pb}}$, velocity, ${\mathit{v}}_{\mathit{ib}}^b$, and attitude, ${\mathit{\psi }}_{\mathit{ib}}^b$, vectors, also referred to as a PVA solution. The IMU measurements are affected by errors and must be corrected before computing a PVA solution. Depending on the quality of the IMU, these errors greatly impact the computation of PVA, in some cases, making IMU navigation by itself impracticable. Details on IMU quality, error sources, and how to mitigate them see [15].

2.2 GNSS

GNSS solution is composed of accurate 3-D position, velocity, and timing by transmitting radio signals from satellite constellations and the user’s receiver antenna. This solution is obtained by computing ranges from measured time or phase difference between received signals and receiver-generated signals. The transmitted messages have ranging code signals, navigation data stream with satellite orbits, and clock corrections to GNSS system time. GNSS observables can be computed using the transmitted message contents.

The basic observables are pseudoranges, carrier-phase, and Doppler. Examples of fully operational constellations including the United States Global Positioning System (GPS) and Russian Global’naya Navigatsionnaya Sputnikova Sistema (GLONASS).

There are several GNSS positioning modes that vary in terms of processing, modeling, physical structure, and precision and accuracy. The most common algorithm used in every GNSS positioning filter is the single point positioning (SPP). The most accurate navigation solutions are achieved using the relative mode. Another well-established processing mode, due to its simplicity in physical structure but complexity in modeling, is precise point positioning (PPP). PPP uses a single receiver to obtain centimeter-to-decimeter accurate positions. PPP started mainly as a post-processing technique; however, advances in real-time corrections are allowing PPP in real-time. An in-depth discussion of GNSS processing modes can be found in Refs. [16, 17, 18]. In this work, a PPP processing mode is utilized.

2.3 Road maps

Since the advances of mapping techniques in the 90s, there has been an increasing investment of companies in producing maps for ADAS [19, 20]. The main reason is the wide range of applications and impacts on safety systems maps can provide, for instance: curve speed warning, vision enhancement, path prediction, lane-keeping assistance, collision warning, and especially the development of autonomous navigation systems [10, 21]. Examples of mapping products are Mobileye’s Road Experience Management) and TomTom’s RoadDNA. RoadDNA is generated with the combination of mobile mapping using LiDARS and other sensors. It also contains lane information such as width, height, markings, and speed limits. By providing road-way profile, curvature, and terrain information, these maps can extend the user’s view for better vehicle prediction. HD maps are key information to enable fully autonomous driving. An example of the RoadDNA is shown in Figure 1.

Figure 1.

HD RoadDNA with 10 cm relative accuracy, from [22].

2.4 Filters

2.4.1 Kalman filter

The task of tracking the state changes over time is accomplished by the Kalman filter (KF) using the sensor data. KF is composed of two main steps, the system propagation and measurement update. Before system propagation, state and error covariance are represented as: ${\widehat{\mathit{x}}}_k^{-}$ and ${\mathit{P}}_k^{-}$, respectively. After the measurement update, the notation adopted to represent the state and error covariance are: ${\widehat{\mathit{x}}}_k^{+}$ and ${\mathit{P}}_k^{+}$, respectively. Where $k$ represents the current epoch of update.

The system model describes how the Kalman filter states and covariance matrix vary with time, in other words, it represents the state dynamics, $\dot{\mathit{x}}$, as shown in the dynamic model, in Eq. (1).

$$\dot{\mathit{x}}\left(t\right)=\mathit{F}\left(t\right)\mathit{x}\left(t\right)+\mathit{G}\left(t\right){\mathit{v}}_s\left(t\right),$$E1

where $\mathit{F}\left(t\right)$ is a continuous function of the dynamic behavior of the states, called the system matrix, and $\mathit{G}\left(t\right)$ is the continuous system noise distribution matrix. The continuous system noise vector, ${\mathit{v}}_s\left(t\right)$, is assumed to be zero-mean Gaussian distribution with a system noise covariance:

$$\mathit{Q}=\mathbf{E}\left({\mathit{v}}_s{\mathit{v}}_s^T\right).$$E2

The measurement vector, $\mathit{z}$, corresponds to observations of the system, described by the state-vector, made from information coming from sensors, such as position, orientation, distance ranging, and rate-rotation measurements. The measurement model is composed of the measurement matrix, $\mathit{H}\left(t\right)$, which represents the relationship between states, $\mathit{x}\left(t\right)$, and measurements, $\mathit{z}\left(t\right)$, and a measurement white noise vector, ${\mathit{w}}_m\left(t\right)$:

$$\mathit{z}\left(t\right)=\mathit{H}\left(t\right)\mathit{x}\left(t\right)+{\mathit{w}}_m\left(t\right).$$E3

An important quantity during the measurement update is the measurement innovation, ${\mathit{\delta z}}^{-}$, which represents the difference between the measurements and the modeled measurements using the propagated state vector:

$${\mathit{\delta z}}^{-}=\mathit{z}-\mathbf{h}\left({\widehat{\mathit{x}}}^{-}\right).$$E4

The quality of the measurements is given by the measurement covariance matrix, $\mathit{R}$. Defined as the expectation of the square of the measurement noise, KF assumes to be Gaussian and uncorrelated in time:

$$\mathit{R}=\mathbf{E}\left({\mathit{w}}_m{\mathit{w}}_m^T\right).$$E5

The measurement covariance matrix values come from the sensors’ measurement properties.

In Appendix A, the discrete KF, describing a sequence of steps to run the KF estimation, is presented.

2.4.2 GNSS/INS integration

An important aspect of GNSS/INS integration is the complementary nature of these systems. GNSS has global coverage and is accurate over long periods, whereas INS is a self-contained sensor with high accuracy over short periods. This way, GNSS can calibrate the INS drift, and INS can bridge GNSS solution during outages.

Several integration architectures combine GNSS and INS measurements [9]. If the integrated PVA solution is acquired by combining GNSS and INS raw measurements, a Tightly Coupled (TC) integration strategy is adopted. When GNSS and INS PVA solutions are combined, the integration strategy is called Loosely Coupled (LC). These integration strategies are described in 2.4.4 and 2.4.3, respectively.

In the GNSS/INS error-state integration, the final solution is the corrected INS navigation, called closed-loop correction. Thus, the final PVA solution is obtained with the following equations:

$${\widehat{\mathit{C}}}_b^e\left(+\right)=\delta {\left({\widehat{\mathit{C}}}_b^e\right)}^T{\widehat{\mathit{C}}}_b^e\left(-\right)\approx \left({\mathit{I}}_3-\left[\delta {\widehat{\mathtt{\Psi }}}_{\mathit{ib}}^{\mathit{b}}\wedge \right]\right){\widehat{\mathit{C}}}_b^e\left(-\right),$$E6

$${\widehat{\mathit{v}}}_{\mathit{eb}}^e\left(+\right)={\widehat{\mathit{v}}}_{\mathit{eb}}^e\left(-\right)-\delta {\widehat{\mathit{v}}}_{\mathit{eb}}^e,$$E7

$${\widehat{\mathit{r}}}_{\mathit{eb}}^e\left(+\right)={\widehat{\mathit{r}}}_{\mathit{eb}}^e\left(-\right)-\delta {\widehat{\mathit{r}}}_{\mathit{eb}}^e,$$E8

where $\left[\wedge \right]$ denotes the skew-symmetric matrix operator of the Euler angles ($\left[\delta {\widehat{\mathtt{\Psi }}}_{\mathit{ib}}^{\mathit{b}}\wedge \right]$). ${\widehat{\mathit{C}}}_b^e$ represents a rotation matrix from body ($b$) to Earth frames ($e$).

The error-state INS/GNSS integration architecture normally estimates three attitude, three velocity, and three-position errors, and six IMU accelerometer and gyroscope biases. These are the parameters for an LC approach. For a TC approach, GNSS receiver clock, $\delta {\mathit{dt}}_r$, and drift, $\delta \dot{d}{t}_r$, are also estimated.

The INS propagation and LC error-state vector, with 15 parameters, is given by:

$${\mathit{x}}_{\mathit{INS}}^e=\left(\begin{array}{c}\mathit{\delta }{\mathtt{\Psi }}^e\\ \mathit{\delta }{\mathit{v}}^e\\ \mathit{\delta }{\mathit{r}}^e\\ {\mathit{b}}_a^e\\ {\mathit{b}}_g^e\end{array}\right).$$E9

For a GNSS/INS TC approach, the error-state vector is composed of the following 17 parameters:

$${\mathit{x}}_{\mathit{\text{GNSS}}}^e=\left(\begin{array}{c}{\mathit{x}}_{\mathit{INS}}^e\\ \delta {\mathit{dt}}_r\\ \delta \dot{d}{t}_r\end{array}\right).$$E10

The INS and GNSS systems models do not interact during the system propagation step:

$$\mathit{F}=\left(\begin{array}{cc}{\mathit{F}}_{\mathit{INS}}& 0\\ 0& {\mathit{F}}_{\mathit{\text{GNSS}}}\end{array}\right),\mathbf{\Phi }=\left(\begin{array}{cc}{\mathit{F}}_{\mathit{INS}}& 0\\ 0& {\mathit{F}}_{\mathit{\text{GNSS}}}\end{array}\right),\mathit{Q}=\left(\begin{array}{cc}{\mathit{Q}}_{\mathit{INS}}& 0\\ 0& {\mathit{Q}}_{\mathit{\text{GNSS}}}\end{array}\right),$$E11

with state vector:

$$\mathit{x}=\left(\begin{array}{c}{\mathit{x}}_{\mathit{INS}}\\ {\mathit{x}}_{\mathit{\text{GNSS}}}\end{array}\right).$$E12

To obtain the system propagation equations the system model, based on the parameters of the state vector 9, a time derivative of each state must be calculated. The system propagation equations derivative is presented in [15].

2.4.3 LC GNSS/INS data fusion

An LC GNSS/INS architecture integrates GNSS position and velocity and INS PVA solutions in the filter to estimate a full PVA solution. The accelerometer and gyroscopes biases are fed back to correct INS navigation. A loosely coupled architecture is shown in the flowchart of Figure 2. The dashed lines represent closed-loop computations.

Figure 2.

Loosely coupled GNSS/INS integration.

Also known as position-domain filter, the main benefits of this architecture are simplicity and the number of parameters are fixed. The main drawbacks are the necessity of having two filters, the GNSS and INS/GNSS, thus increasing processing time, and a minimum of four satellites must be observed to obtain a solution, otherwise the data tracked of three or less satellites are not utilized.

In the LC architecture, the measurements are composed by the GNSS and INS propagated position and velocity solutions. Thus, the innovation vector is:

$${\mathit{\delta z}}_k^e={\left(\begin{array}{c}{\widehat{\mathit{r}}}_{\mathit{eG}}^e-{\widehat{\mathit{r}}}_{\mathit{eb}}^e\\ {\widehat{\mathit{v}}}_{\mathit{eG}}^e-{\widehat{\mathit{v}}}_{\mathit{eb}}^e\end{array}\right)}_k,$$E13

where the subscript $G$ denotes GNSS.

The measurement matrix is given by taking the first derivative of the measurement model 3 with respect to the state vector in 9:

$${\mathit{H}}_k^e={\left.\frac{\partial \mathbf{h}\left(\mathit{x},t_k\right)}{\partial \mathit{x}}\right|}_{\mathit{x}={\widehat{\mathit{x}}}_k^{-}}={\left.\frac{\partial \mathit{z}\left(\mathit{x},t_k\right)}{\partial \mathit{x}}\right|}_{\begin{array}{c}\mathit{x}={\widehat{\mathit{x}}}_k^{-}\end{array}}.$$E14

Thus, by neglecting lever arm effects, the LC measurement matrix can be approximated to

$${\mathit{H}}_k^e\approx {\left(\begin{array}{ccccc}{\mathbf{0}}_3& {\mathbf{0}}_3& -{\mathit{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& -{\mathit{I}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right)}_k.$$E15

2.4.4 TC GNSS/INS data fusion

A TC integration is a measurement-domain filter. It uses the GNSS raw measurements as pseudorange, carrier-phase, and Doppler shift and INS PVA solutions to the filter instead of pre-processed position and velocity solutions, as in the loosely coupled architecture. Similar to the loosely coupled, the tightly coupled feedbacks corrections to compensate accelerometer and gyroscopes measurements. In Figure 3, a TC GNSS/INS integration architecture is presented.

Figure 3.

Tightly coupled GNSS/INS integration.

The first obvious advantage of the tightly coupled is the use of a single filter. Other advantages include: the INS errors are better estimated due to the GNSS covariance variation with satellite geometry, and even when less than four satellites are tracked, the satellite data can be used to aid the INS in computing an integrated solution [15]. The main limitation over the loosely coupled integration is the increased size of the number of parameters, which increases the computational efforts [23].

In TC architecture, the measurements are the GNSS pseudorange and carrier-phase observables. The TC measurement innovation is written by the difference between GNSS pseudorange, $\tilde{P}$, and carrier-phase, $\tilde{L}$, measurements and the computed pseudorange, $\widehat{P}$, and carrier-phase, $\widehat{L}$, values are derived from the current INS navigation PVA solution, per $m$-satellites available. Thus:

$$\begin{array}{cc}{\mathit{\delta z}}_k^e={\left(\begin{array}{c}{\mathit{z}}_P^e\\ {\mathit{z}}_L^e\end{array}\right)}_k,& \begin{array}{c}{\mathit{\delta z}}_P^e={\left({\tilde{P}}_G^1-{\widehat{P}}_{\mathit{INS}}^1,{\tilde{P}}_G^2-{\widehat{P}}_{\mathit{INS}}^2,\cdots ,{\tilde{P}}_G^m-{\widehat{P}}_{\mathit{INS}}^m\right)}_k\\ {\mathit{\delta z}}_L^e={\left({\tilde{L}}_G^1-{\widehat{L}}_{\mathit{INS}}^1,{\tilde{L}}_G^2-{\widehat{L}}_{\mathit{INS}}^2,\cdots ,{\tilde{L}}_G^m-{\widehat{L}}_{\mathit{INS}}^m\right)}_k\end{array}\end{array},$$E16

where the subscript $G$ denotes GNSS, $\widehat{P}$ and $\widehat{L}$ are described in 2.4.4 and 2.4.3 respectively.

The TC measurement matrix is given by 14 with respect to the state vector parameters in 10, thus:

$${\mathit{H}}_k^e={\left(\begin{array}{ccccccc}\frac{\partial \mathbf{\delta }{\mathit{z}}_P^e}{\partial \mathit{\delta }{\mathtt{\Psi }}^e}& {\mathbf{0}}_{m,3}& \frac{\partial \mathbf{\delta }{\mathit{z}}_P^e}{\partial \mathit{\delta }{\mathbf{r}}^e}& {\mathbf{0}}_{m,3}& {\mathbf{0}}_{m,3}& \frac{\partial \mathbf{\delta }{\mathit{z}}_P^e}{\partial \delta {\mathit{dt}}_r}& {\mathbf{0}}_{m,1}\\ \frac{\partial \mathbf{\delta }{\mathit{z}}_L^e}{\partial \mathit{\delta }{\mathtt{\Psi }}^e}& \frac{\partial \mathbf{\delta }{\mathit{z}}_L^e}{\partial \mathit{\delta }{\mathit{v}}^e}& \frac{\partial \mathbf{\delta }{\mathit{z}}_L^e}{\partial \mathit{\delta }{\mathit{r}}^e}& {\mathbf{0}}_{m,3}& \frac{\partial \mathbf{\delta }{\mathit{z}}_L^e}{\partial {\mathit{b}}_g^e}& {\mathbf{0}}_{m,1}& \frac{\partial \mathbf{\delta }{\mathit{z}}_L^e}{\partial \delta \dot{d}{t}_r}\end{array}\right)}_k.$$E17

2.5 Map-matching

Road map data are interfaced with vehicle navigation applications through the use of map-matching techniques. Map-matching is often described and used in the literature as a tool to correctly identify the road segment that a vehicle is traveling on and its position on that segment [24]. The input data required to perform a map-matching are simply the position of the vehicle and a map of the roads. The process of map-matching is normally divided in two tasks: first, road segment identification, and second, vehicle positioning with respect to the selected road segment.

Several techniques are used to solve the map-matching problem, from simple geometric, topological, and probabilistic techniques to advanced Kalman filtering, fuzzy logic, and belief theory. According to Ref. [25], the identification task is the main limitation of map-matching performances in the following situations: road junctions, intersections, and complex urban areas. Another major source of possible degradation in the quality of map-matching is in the input data: the position and the map database.

Many applications use map-matching to identify the correct road of navigation and determine the vehicle location on the same segment. Another common application is to visualize the trajectory onto the road.

3. Lane-keeping system architecture proposal

The LKAS system proposed has as inputs satellite and IMU measurements with a map of lane information. The architecture of the system, referred to as (TC + LC/MAP), is composed of two data fusion approaches: a tightly coupled GNSS/INS with loosely-coupled INS/MAP. The flowchart in Figure 4 gives a high-level view of the system architecture.

Figure 4.

Full-system architecture.

The blue boxes in Figure 4 show the type of available data, and the red boxes represent the type of data fusion used. The “INS Navigation” and “SPP” boxes are described in 2.1 and 2.2, respectively. At the center of the system flowchart, a condition checks for satellite observation availability; if the condition is met, a TC GNSS/INS is performed; otherwise, an LC INS/MAP solution is computed to bridge the GNSS outage. The “INS correction” box in Figure 4 corresponds to the corrected INS solution.

In the next sections, an improved map-matching (the “MM” box) process is described in 3.1, along with an explanation of the candidate search space and best candidate selection in 3.1.1 and 3.1.2, respectively. In 3.2, a description of the LC INS/MAP filter; and how the full-system solution transition is detailed, respectively. Finally, in 3.3, a position accuracy criteria based on the vehicle and road relationship is proposed.

3.1 Map-matching

Standard map-matching applications start the identification process by searching for the road segment the vehicle is within a road network. If the GNSS position is degraded, ambiguous match-matching can happen. However, in this work, a fundamental assumption will be made to eliminate the problem of road ambiguity matching.

From the driver’s perspective, the driving task can be described into the following: the travel plan, composed of the desired final location and vehicle starting position. With these two locations, a unique route passing through known streets can be defined. Assuming that the trajectory is fixed and no detours or overtaking other vehicles is necessary, driving becomes a constrained problem, and road identification can be eliminated with this assumption.

Then, the only task left in this map-matching is to estimate the vehicle position in the lane map trajectory. Instead of only snapping the GNSS position onto the trajectory, a standard process of map-matching approaches, in this case, our interest is to select a probable region where the true position of the vehicle may be located. This process is described in the next section.

3.1.1 Candidates search space definition

For the LC INS/MAP, the candidate search space area is selected by using the latest INS navigated solution available after a TC GNSS/INS solution. By finding the closest point to the map, a candidate buffer area is obtained by selecting a pre-defined area in both directions of the closest point. This area selection is depicted in Figure 5.

Figure 5.

LC INS/MAP searching space schema—top view.

This search space should match where the GNSS receiver is mounted on the vehicle. By accounting for this height difference we have a search space that will closely contain the true GNSS location, Figure 6 illustrates this situation.

Figure 6.

Map-matching—side view.

Once the search space, containing the most probable candidates, is defined, the best candidate selection can be performed. The next section explains this selection.

3.1.2 Candidate selection

The selection of the candidate is based on the Kalman filter residuals. The residuals, frequently called innovation term, tell us how well the model predicts the measurements, thus being a good measure of accuracy. Therefore, the smaller the residuals, the more accurate the estimate.

The measurement residuals ($r_k$) can be extracted from the second term multiplying the Kalman gain (${\mathit{K}}_k$) on the right-hand side of Eq. (27):

$${\mathit{r}}_k={\mathit{z}}_k-{\widehat{\mathit{z}}}_k^{-},$$E18

where ${\widehat{\mathit{z}}}_k^{-}=\mathbf{h}\left({\mathit{x}}^0\right)-{\mathit{H}}_k\mathbf{\delta }{\widehat{\mathit{x}}}_k^{-}$ is the predictive estimate of the measurement. ${\mathit{x}}^0$ represents an initial information about the true state.

In the LC INS/MAP constraint, the measurement vector ${\mathit{z}}_k$ is composed of position and velocity observations, as shown in Eq. (13).

As the search space, shown in Figure 5, gives a set of position candidates, they can be used as an approximation to the true states to generate the computed measurement estimate $\widehat{\mathit{z}}$. Then, after the Kalman update step, each candidate generates a normalized (${\chi }_c^2$), as the following Equation:

$${\chi }_c^2={\left[\frac{{\mathit{r}}_i^c}{{\sigma }_i^c}\right]}^2.$$E19

where $i=1,2,\cdots ,c$, with $c$ being the number of candidates in the search space.

The best candidate is the one with the smallest residuals. To visualize this selection, an example showing six candidates in the search space is depicted in Figure 7. If there are no systematic errors in the measurements, candidate three would provide the smallest residual.

Figure 7.

Candidate space selection.

3.2 LKAS filter

3.2.1 Loosely coupled INS/MAP constraints

The LC INS/MAP filter is activated during GNSS outages, as shown in the full-system architecture 4. In this architecture, the MAP candidates are used to substitute the measurements in the LC Kalman measurement update.

Each candidate position, ${\mathit{r}}^c$, from the search space, defined in Figure 5, substitute the LC measurement innovation, in Eq. (13), thus:

$$\delta {\mathit{z}}_i={\left(\begin{array}{c}{\mathit{r}}_i^c-{\widehat{\mathit{r}}}^{\mathit{ins}}\\ {\mathit{v}}_i^c-{\widehat{\mathit{v}}}^{\mathit{ins}}\end{array}\right)}_k.$$E20

where $i=1,2,\cdots ,c$, with $c$ as the number of candidates in the search space.

For each LC measurement innovation generated in Eq. (20), a chi-square residual is computed for the best candidate selection with Eq. (19). After selected, the best candidate position is again substituted in Eq. (20), for a final LC measurement update.

An important piece of information is the velocity, ${\mathit{v}}_i^c$, which is not actually available from the map. Since GNSS is not available, velocity information needs to be measured from another system. The INS could provide velocity, however, because of the accelerometer drift, after a few seconds, the INS derived velocities are not useful anymore [26]. A sensor that would provide such information would be wheel odometers.

In this work, only a GNSS receiver and an IMU were used as instruments. Therefore, velocity information provided when GNSS was not available was obtained from the GNSS baseline solution, of the same data set, and is used as reference.

3.2.2 System solution transitions

The changes of the types of solutions will determine how the states are weighted. In the filter equations, key matrices to obtain smooth transition solutions are the state and measurement covariance, ${\mathit{P}}_k$ and ${\mathit{R}}_k$, respectively, and system covariance noise, ${\mathit{Q}}_k$. The system noise covariance matrix reflects the sensor’s noise, in this case, the IMU’s accelerometers and gyroscopes. The IMU noise information can be seen in Appendix B. In Eq. (21), the covariance matrix is built with the IMU used in this experiment [26]:

$$\begin{array}{lll}{\mathit{Q}}_{\mathit{INS}}^e& =& \left(\begin{array}{cccccccc}{\mathbf{S}}_{\mathit{ra}}{\mathbf{I}}_3& 0_3& 0_3& 0_3& 0_3& 0& 0& 0\\ 0_3& {\mathbf{S}}_{\mathit{rg}}{\mathbf{I}}_3& 0_3& 0_3& 0_3& 0& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 0& 0\\ 0_3& 0_3& 0_3& {\mathbf{S}}_{\mathit{bad}}{\mathbf{I}}_3& 0_3& 0& 0& 0\\ 0_3& 0_3& 0_3& 0_3& {\mathbf{S}}_{\mathit{bgd}}{\mathbf{I}}_3& 0& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& S_{\mathit{c\varphi }}& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& S_{\mathit{cf}}& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 0& q_T\end{array}\right)\\ & =& \left(\begin{array}{cccccccc}2.0409& 0_3& 0_3& 0_3& 0_3& 0& 0& 0\\ 0_3& {5.3884}^{-7}& 0_3& 0_3& 0_3& 0& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 0& 0\\ 0_3& 0_3& 0_3& {1.2769}^{-12}& 0_3& 0& 0& 0\\ 0_3& 0_3& 0_3& 0_3& {2.3814}^{-11}& 0& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 1& 0& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 1& 0\\ 0_3& 0_3& 0_3& 0_3& 0_3& 0& 0& 0.3\end{array}\right).\end{array}$$E21

where ${\mathbf{S}}_{\mathit{ra}}$ is in micro-$g^2/h$, ${\mathbf{S}}_{\mathit{rg}}$ in ${deg}^2/h$, ${\mathbf{S}}_{\mathit{bad}}$ in $m^2{s}^{-5}$, and ${\mathbf{S}}_{\mathit{bgd}}$ in ${\mathit{rad}}^2{s}^{-3}$. The receiver clock frequency-drift and phase-drift are in $m^2/s^3$ and $m^2/s$, respectively. Troposphere standard deviation, $q_T$, is in meters.

Whenever a TC GNSS/INS solution is obtained, the measurement matrix values represent the GNSS carrier-phase and pseudorange observable errors in meters:

$${\mathit{R}}_k^{\mathit{TC}}=\left(\begin{array}{cc}{\sigma }_P& 0\\ 0& {\sigma }_L\end{array}\right)=\left(\begin{array}{cc}0.001& 0\\ 0& 1\end{array}\right).$$E22

When an LC INS/MAP is computed, a few modifications are done to accommodate the MAP observations. As the map tests its candidate space in the measurement update, in this step, we input the standard deviation of the map and and velocity in meters:

$${\mathit{R}}_k^{\mathit{LC}}=\left(\begin{array}{cc}{\sigma }_{r_{\mathit{MAP}}}& 0\\ 0& {\sigma }_{v_{\mathit{MAP}}}\end{array}\right)=\left(\begin{array}{cc}10.0& 0\\ 0& 0.1\end{array}\right).$$E23

These values were derived after a manual tuning of the filter.

3.3 Lane-keeping accuracy limit

In this work, a lane accuracy limit is proposed based on the vehicle ($v_w$) and lane ($l_w$) dimensions. The relationship between the vehicle and lane widths is depicted in the Figure 8.

Figure 8.

Vehicle navigation threshold.

The accuracy criteria is derived from the average lane sizes where vehicle navigates and the vehicle lateral widths into the following equation:

$${\sigma }_{\mathit{lim}}=\left(l_{w/2}\right)-\left(v_{w/2}\right).$$E24

For the real-scenario experiments, few lane width measurement samples were taken in Google Earth. In downtown, the average lane size was 3.75 m, and outside downtown, the average lane width was 4.10 m. Thus, the average lane size for the real-scenario experiment is $l_w=3.93$ m. The vehicle used had a width of $v_w=1.84$ m, thus the accuracy limit is ${\sigma }_{\mathit{lim}}=\pm 1.04$ m.

4. Real-world experiment

In this section, we present a real-world experiment conducted to test the proposed system shown in Section 3. The data required for the tests are presented in Section 4.1. In the next Section, 4.2, we show the results obtained along with the discussions.

4.1 Experiment setup

A GNSS and inertial dataset was collected onboard a vehicle driving in the streets of Fredericton, New Brunswick, Canada. The instruments used were a Septentrio Altus APS3G GNSS receiver, multi-constellation, and multi-frequency (L1, L2, and L5); and a tactical-grade KVH TG-6000 IMU. The GNSS receiver and IMU data rates were 1 and 100 Hz, respectively. The sensors were mounted aligned with the longitudinal axis of the vehicle, with the appropriate offsets accounted for during the processing.

The road map, composed with the location of the center of the streets, was created from the dataset with centimeter-accurate post-processing GNSS relative processing. A section of the road map is shown in Figure 9.

Figure 9.

Road map of lanes obtained from GNSS relative processing.

To test the proposed system, the driving test areas were selected where GNSS signal tracking was momentarily lost, with instances of complete signal interruption for approximately 20 seconds. For example, in Figures 10–12, the following areas were tested: walking bridge, road overpass, and streets with tall buildings, respectively.

Figure 10.

Walking bridge.

Figure 11.

Road overpass.

Figure 12.

Street with tall buildings on both sides.

4.2 Results and discussions

This section presents an assessment of the proposed architecture, the TC GNSS/INS + LC/MAP, compared to a standard PPP (without convergence and re-convergences solutions) and TC GNSS/INS. The two GNSS/INS solutions are compared in terms of horizontal ground-track, in Section 4.2.1, accuracy analysis, adding PPP in analysis, in terms of cross and along-tracks, and up off-track differences in Section 4.2.2. In Section 4.2.3, a cross-track assessment based on the position accuracy criteria developed in Section 3.3, and finally, some solution challenges are described in Section 4.3.

The full-system solution will be referred to as TC + LC/MAP in the text and tables. For the figures, a separation of TC GNSS/INS and LC INS/MAP is made for a better visualization of the these solutions.

Knowing that PPP convergence and re-convergences greatly distort the accuracy of PPP solution, these periods were removed in order to have a fair comparison with the proposed architecture. Thus, the standard PPP solution in the accuracy analysis does not represent the real PPP accuracy for this trajectory.

4.2.1 Horizontal ground-track displacement

The horizontal comparisons in this section presents the behavior of the filters in places where the GNSS lost signal tracking. These places are the walking bridge, Forest Hill highway underpass, and downtown area with tall buildings.

The standard TC GNSS/INS is presented in green, with squares representing GNSS/INS integration and lines representing the INS solution. The proposed TC + LC/MAP solution is represented in gold and red. The integrated TC GNSS/INS is represented with gold circles, and integrated LC INS/MAP is shown with a red star, while gold lines represent the INS solution.

The first obstruction happens on the walking bridge, as shown in Figure 10. The walking bridge is a short overpass that obstructs the GNSS signal for a brief moment. From the solution horizontal plot in 13, the TC GNSS/INS solution goes off the driving path as GNSS is lost. This behavior happens because the IMU does not have a reference to correct its drift. For the TC+LC/MAP, a position solution is available from the LC INS/MAP filter, thus bridging the GNSS signal outage during the obstruction Figure 13.

Figure 13.

Walking bridge obstructions.

At the Forest Hill highway underpass, as depicted in Figure 11, the longest complete GNSS signal interruption occurs in the experiment. Similar to the walking bridge, the TC GNSS/INS solution drifts off the trajectory, whereas the TC + LC/MAP has a continuous solution with the LC INS/MAP, as shown in Figure 14.

Figure 14.

Forest Hill underpass.

In the downtown area, a sequence of tall buildings on both sides of the street caused difficulties in GNSS signal tracking for about 20 seconds. This area is presented in Figure 12. During these blocks, the GNSS receiver suffered consecutive obstructions as seen in Figure 15, causing the TC GNSS/INS solution to drift due to having only INS solution available, as can be seen by a long green line disappearing out of plot. Once more, the proposed architecture provides a LC INS/MAP solution throughout the obstructions, effectively bridging the GNSS outages.

Figure 15.

Downtown area—king street tall buildings.

4.2.2 Off-track accuracy analysis

The cross and along-tracks differences between the ground-truth to the solutions are compared in histograms plots in Figures 16 and 17.

Figure 16.

Cross-track histograms.

Figure 17.

Along-track histograms.

The TC GNSS/INS histograms presented the larger tails with a low frequency of samples localized around zero. This shows that a large amount the differences were off from the lane center, meaning that it was neither accurate nor precise. This behavior, presented in plots from Section 4.2.1, happens during long periods of GNSS obstructions, where the INS drifts several meters off the trajectory.

TC + LC/MAP presented better performances than PPP in cross-track, with a higher frequency of samples around zero and smaller spread, in other words, more accurate and precise, as can be seen in Figure 16. For the along-track, the contrary is true, PPP achieved a better accuracy and precision, 17.

Table 1 summarizes the cross-track and along-track-off-track mean and standard deviations. In terms of cross-track performance, the proposed system outperformed the PPP by 1 cm in average and by 3 cm in standard deviation. However, in along-track measurements, the full-system did worse than PPP in terms of average by 28 cm and outperformed in standard deviation by 30 cm. As mentioned previously, TC GNSS/INS had the worse performances in cross and along-tracks due to INS solution drifts.

	Cross-track		Along-track
	avg (m)	std (m)	avg (m)	std (m)
PPP	0.02	0.09	−0.17	4.49
TC GNSS/INS	−0.05	1.40	−0.63	64.86
TC + LC/MAP	−0.01	0.06	−0.45	4.19

Table 1.

Cross and along-tracks off-track statistics.

4.2.3 Cross-track accuracy criteria evaluation

As the positioning accuracy criteria developed in Section 3.3 consider lateral lane offsets, only cross-track time series is analyzed in this section.

Figure 18 shows the cross-track for the three solutions. As seen in Section 3.3, the lane accuracy limit for analyzing cross-track is ±1.04 m, represented by the dashed red lines. Table 2 shows the statistics about the cross-tracking series of Figure 18.

Figure 18.

Cross-track PPP, TC GNSS/INS, and TC + LC/MAP.

	avg (m)	std (m)	95% C.I.
			Lower bound	Upper bound
PPP	0.02	0.09	−0.15	0.20
TC GNSS/INS	−0.05	1.40	−2.79	2.69
TC + LC/MAP	−0.01	0.06	−0.13	0.11

Table 2.

Cross-track statistics.

In summary, the TC + LC/MAP solutions were more accurate and precise than the others, and, with 95% of confidence level, only TC GNSS/INS did not achieve the lane accuracy limit established.

PPP and TC + LC/MAP solutions had respective biases of 2 and 1 cm, and standard deviations of 13 and 11 cm. The TC GNSS/INS solution had a bias of 5 with 140 cm standard deviation.

Based on a more strict position accuracy limit for lane-level, presented in Section 3.3, the TC + LC/MAP was 3 and 1 cm away from achieving the active control level (0.1 m) when looking at the 95% lower and upper bounds of the cross-track confidence interval series: −0.13 and 0.11, respectively.

4.3 Challenges

This section will explore the reasons the proposed system along-track did worse than the PPP solution, as presented in Table 1.

This region is located in an area covered by trees on both sides of the street. The solutions analyzed are the first and third runs in downtown, shown in Figures 19 and 20, respectively.

Figure 19.

Downtown area first run—Queen/York and York/King street corners.

Figure 20.

Downtown area third run—Queen/York and York/King street corners.

A few gaps are observed after LC + INS solutions from the proposed system. The reason of this behavior is the lack of reliable velocity information.

Integration spikes are also observed in the TC GNSS/INS solutions, represented by the green boxes in Figures 19 and 20. According to the filter design, in GNSS quality control, integration would only happen when the GNSS solution is reliable. Therefore, in these cases, the probable source of such behavior is signal multipath, which is still a major challenge in GNSS positioning research.

5. Conclusions

This work proposed a satellite and INS with map constraints integration filter architecture. The major contribution of this architecture was to demonstrate the possibility of lane-level vehicle navigation using a simple set of sensors: a GNSS receiver, an IMU, and a map of the lanes. A TC GNSS/INS with LC INS/MAP architecture was developed.

In the real-world experiments, the system was exposed to complex urban situations. The system was able to correctly maintain solutions centered during long GNSS outages of 20 seconds with the LC INS/MAP filter. The elimination of INS drift, a common behavior when there is no information to calibrate the IMU. A position accuracy was achieved in cross-track, with 95% confidence, the full-system cross-track lower and upper bounds were 3 and 1 cm away, respectively, from achieving the active control limit level (0.1 m) for ITLBS.

Along with current lane-keeping assistant systems, this new technique could be a strong addition to vision approaches due to its independence of lighting conditions. This system has potential for global operation since the proposed system is not restricted in terms of the area of operation, as current approaches are based on machine learning algorithms, which normally depend on limited training data of a specific region.

One of the challenges for such a system to operate worldwide is the mapping coverage. In recent years, private companies have increased their interest in road mapping. However, for a long-term and larger-scale solution, an interesting approach to solve the problem would be the involvement of the public sector to provide such maps and be responsible for updating and maintaining them, as this is a matter of public road safety.

Future improvements could include the addition of other sensors, for instance, wheel odometers that could provide velocity information for longer GNSS obstructions. Such an addition could also enable indoor navigation.

Acknowledgments

This work was funded by the Brazilian National Council for Scientific Technological Development (CNPq) through the Sciences Without Borders (SwB) program.

Appendix A: discrete kalman filter

From the system and measurement continuous models respectively presented in Tables 1 and 3, the discrete Kalman filter system and measurement equations are represented in the following form, respectively:

$$\begin{array}{lll}{\mathit{x}}_k^{-}& =& {\mathit{F}}_k{\mathit{x}}_{k-1}^{+}+{\mathit{v}}_{k-1}\\ {\mathit{z}}_k& =& {\mathit{H}}_k{\mathit{x}}_k^{-}+{\mathit{w}}_k.\end{array}$$E25

The Kalman filter is a recursive process with the system prediction and measurement update steps. In the system prediction step, the state and error covariance are estimated from previous time-step:

$$\begin{array}{lll}{\widehat{\mathit{x}}}_k^{-}& =& {\mathbf{\Phi }}_{k-1}{\widehat{\mathit{x}}}_{k+1}^{+}\\ {\mathit{P}}_k^{-}& =& {\mathbf{\Phi }}_{k-1}{\mathit{P}}_{k-1}^{+}{\mathbf{\Phi }}_{k-1}^T+{\mathit{Q}}_{k-1},\end{array}$$E26

where ${\mathit{P}}_k$ is the state error covariance, and the transition matrix ${\mathit{\text{bmUpphi}}}_{k-1}$ is computed as a power-series expansion of the system matrix ${\mathit{F}}_{k-1}$.

As new measurements are available, the predicted state and covariances are corrected in the measurement update step:

$$\begin{array}{lll}{\widehat{\mathit{x}}}_k^{+}& =& {\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k\left({\mathit{z}}_k-{\mathit{H}}_k{\widehat{\mathit{x}}}_k^{-}\right)\\ {\mathit{P}}_k^{+}& =& \left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_k^{-},\end{array}$$E27

where ${\mathit{K}}_k={\mathit{P}}_k^{-}{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{P}}_k^{-}{\mathit{H}}_k^T+{\mathit{R}}_k\right)}^{-1}$ is the Kalman gain. The Kalman gain weights the state estimation according to the system prediction and measurements update quality.

Appendix B: KVH TG-600 tactical INS noise

See Tables 3 and 4.

	m/s^2$\sqrt{\mathit{Hz}}$
PSD	0.000014
Bias	0.000001

Table 3.

KVH accelerometers noise.

	rad/s^2$\sqrt{\mathit{Hz}}$
PSD	0.000769
Bias	0.000005

Table 4.

KVH gyroscopes noise.