Localization System Enhanced with CDLPE: A Low-Cost, Resilient Map-Matching Algorithm

Abstract

Accurate and robust localization is a critical component in intelligent vehicles, playing a significant role in route planning and efficient navigation. There is a rising trend towards affordable positioning solutions that use common vehicular sensors like GPS, IMU, and cameras to improve navigation accuracy. This paper presents a comprehensive, low-cost localization framework with a lightweight map. The framework introduces two key novelties. Firstly, we propose a method known as the Cross-Dimensional Lane and Pose Estimator (CDLPE), designed to effectively resist scenarios with poor satellite signals. Additionally, our system delivers a reliable localization service by effectively integrating matching results and capitalizing on the benefits of the sensors used, coupled with the understanding of the environment. We have verified the robustness of our method under different driving scenarios. Compared to the classical Iterative Closest Point (ICP) algorithm, the lane identification accuracy has improved by 4.42% and 9.23% during normal and weak satellite signal conditions, respectively. Videos in: https://youtu.be/DsYXSeWQhWc.

Appendices

Appendices

1.1 Road-Level Localization

Problem Definition.

Given a trajectory $X$, the goal of road level matching is to find the correspondence between each trajectory point in $X$ to a road segment in $G$, also known as $Link$.

Definition 1: A road network $G$ consists of a set of road segment $r$, in which the road segment $r_i = [p_1, p_2, p_3, ..., p_m]$ point polyline representing a road segment curve.

Definition 2: A trajectory $X$, is a sequence of the history pose, denoted by $X = [x_1, x_2, x_3, ..., x_n]$:

$$\begin{aligned} x_i = [x, y, \theta , d, b]^T, i = 1, 2, ..., n \end{aligned}$$

(16)

Each $x_i$ is specified by its longitudinal and lateral coordinates of the position $x, y$, angle $\theta$ between the position point and the road segment, the distance $d$ from the position point to the road segment. Note that if the position point is on the road, then $d$ is 0. $b$ represents the confidence of the position point, which is estimated by sensor fusion.

Fig. 9.

A, B, C, D represent road segments, and there are different road network observations at point 1, 2, and 3.

Modeling. This work employs a standard forward algorithm in Hidden Markov Modeling (HMM) due to its ability to handle uncertainty in GPS data, consider the sequence of observations and maintain multiple hypotheses. As shown in Fig. 9, the algorithm calculates the posterior probability of each observed link at each point 1, 2, and 3. To ensure the real-time performance of the algorithm, this algorithm only calculate the forward probability of the chains in the sliding window, and maintain a sliding window with a certain length (Fig. 10).

It should be noted that in order to utilize the historical information in a better way, this program defines three states:

• 1. INIT-SELECT: In initial stage, the algorithm ignore historical information and keep all the topology to maintain multiple hypotheses.

• 2. STABLE-SELECT: In stable derivation stage, the cumulative probability of the link at the time of discard will be used as the prior probability in subsequent calculations. Meanwhile, the past topology branches with a low probability will be reduced.

• 3. INTERRUPT: In intermediate unstable stage, the historical road probability will be ignored.

Fig. 10.

Three states in road-level matching algorithm

Emission Probability is determined by three observations:

$$\begin{aligned} P_e = \lambda _1P_d(X, r_j) + \lambda _2P_\theta (X, r_j) + \lambda _3P_{vision}(r_j) \end{aligned}$$

(17)

In which assumed that there are $m$ road segments, noted as $r_j, j = 1, 2, ..., m$, where the value of $\lambda _{i=1,2,3}$ was pre-trained offline.

$P_d(X, r_j)$ represents the measurement probability between the current trajectory sequence and the road segment in distance $d_i(x_i, r_j)$, where $b_i$ denotes the position confidence.

$$\begin{aligned} P_d(X, r_j) = \frac{1}{n} \sum _{i = 1, 2, ..., n} e^{-kd_i(x_i, r_j)/b_i} \end{aligned}$$

(18)

$P_\theta (X, r_j)$ is the probability between the current trajectory sequence and the road segment in angle $\theta _i(r_j)$.

$$\begin{aligned} P_\theta (X, r_j) = \frac{1}{n} \sum _{i = 1, 2, ..., n} cos(\theta _i(r_j)) \end{aligned}$$

(19)

$P_{vision}(r_j)$ is the measurement probability of the matching degree of perception result and map knowledge.

$$\begin{aligned} P_{vision}(r_j) = \frac{1}{n} \sum _{i = 1, 2, ..., n} p_i \end{aligned}$$

(20)

Transition Probability between road segments are governed by road topology, thus the setting of the transition probability depends on the connectivity of road networks, i.e. if there is a direct topological connection between two road segments, the value of transition probability is 1, otherwise it is 0. In our algorithm, the topological relationship between all the candidate segments in the sliding window will be merged into an adjacency matrix $T_f$ for chain probability calculation, as shown in Fig. 11.

Fig. 11.

$T_{ij}$ represents the transition probability from $link_i$ to $link_j$

In this road-level part, the longitude and latitude of the center point of the recommended lane $pos_{recommend}$on the planned route of the located road is used as the output of the vehicle position, which serves as a reference position for fusion in extremely harsh scenarios where GNSS signals are severely drifting and perception fails. Meanwhile, the subsequent lane-level positioning will be based on the link id output from this step.