Longitudinal Velocity Estimation of Driverless Vehicle by Fusing LiDAR and Inertial Measurement Unit

Abstract

Aiming at the problem that, under certain extreme conditions, relying on tire force or tire angular velocity to represent the longitudinal velocity of the unmanned vehicle will fail, this paper proposes a longitudinal velocity estimation method that fuses LiDAR and inertial measurement unit (IMU). First, in order to improve the accuracy of LiDAR odometry, IMU information is introduced in the process of eliminating point cloud motion distortion. Then, the statistical characteristic of the system noise is tracked by an adaptive noise estimator, which reduces the model error and suppresses the filtering divergence, thereby improving the robustness and filtering accuracy of the algorithm. Next, in order to further improve the estimation accuracy of longitudinal velocity, time-series analysis is used to predict longitudinal acceleration, which improves the accuracy of the prediction step in the unscented Kalman filter (UKF). Finally, the feasibility of the estimation method is verified by simulation experiments and real-vehicle experiments. In the simulation experiments, medium- and high-velocity conditions are tested. In high-velocity conditions (0–30 m/s), the average error is 1.573 m/s; in the experiment, the average error is 0.113 m/s.

1. Introduction

Longitudinal velocity is the velocity component along the forward direction of the vehicle body during driving, and its estimation accuracy directly affects the safety control of the vehicle by active and passive safety systems such as the anti-lock braking system and acceleration slip regulation [1,2]. Low-precision longitudinal velocity estimation not only increases unnecessary energy consumption, but may even affect the safety of passengers. Therefore, in order to achieve high-precision control of driverless vehicles, reduce energy consumption, and ensure the safety of passengers, it is of great significance to carry out in-depth research on vehicle longitudinal velocity estimation methods in the field of driverless vehicles [3].

Research on vehicle longitudinal velocity estimation methods can be divided into two directions: dynamics-based methods and kinematics-based methods [4], as well as some intelligent estimation methods [5,6,7]. The dynamics-based method mainly relies on the dynamics model of the vehicle, and it estimates the longitudinal velocity of the vehicle by analyzing the force of the vehicle model. In [8], a nonlinear dynamic model including the longitudinal velocity, lateral velocity and yaw rate of the vehicle was established based on the linear tire model, and the real-time estimation of the vehicle velocity was realized. In [9], a nonlinear dynamic model including longitudinal velocity, lateral velocity, yaw angular velocity and four-wheel angular velocities was established based on the tire model of magic formula, and the vehicle velocity was estimated by using the unscented Kalman filter (UKF). In [10], a hybrid Kalman filter was proposed. When the system noise statistical characteristic error is small, the square root cubature Kalman filter is used to estimate the vehicle velocity; when the system noise statistical characteristic error is large, the square root cubature Kalman finite impulse filter is used for real-time estimation of the vehicle velocity. In [11], a state estimation method based on IMU is proposed, which uses two estimators based on the vehicle dynamics model to estimate longitudinal speed, pitch angle, lateral speed, and roll angle.

However, the dynamics-based method is less affected by external environmental factors, and it is limited by the dynamics model; the accuracy of the estimation algorithm cannot be guaranteed in some extreme conditions. Therefore, this paper mainly studies the kinematics-based estimation method. In [12], improvements have been made to the system noise of the UKF to prevent filtering divergence. In [13], the sideslip angle of the vehicle’s center of mass was described by using the vehicle yaw rate, longitudinal velocity, and lateral acceleration, and then the vehicle’s lateral velocity was deduced by using the definition formula of the vehicle’s center of mass sideslip angle. In [14], the vehicle yaw rate was taken as the scheduling parameter, a linear time-varying parameter vehicle kinematics model was established, and the real-time estimation of the vehicle velocity was realized by using the robust filter. In [15,16], a kinematics model was established including the longitudinal and lateral velocities of the vehicle using the tire longitudinal and lateral forces measured by the sensor, and real-time estimation of the longitudinal and lateral velocities of the vehicle was realized by using the Kalman filter. In [17], the tire lateral force measured by the sensor in a hybrid nominal model was established including the kinematics and dynamics of the vehicle, and the real-time estimation of the vehicle velocity was realized by using the extended Kalman filter. In [15,16,17], the tire force was considered measurable state information, but in view of the assembly position of the tire force sensor and the constraints of the use condition, it is difficult to meet the control requirements of driverless vehicles.

This problem can be solved by combining positioning technology and IMU to estimate the velocity of the vehicle. Usually, the combination of GPS and IMU can be used to obtain accurate estimation results [18]. In [19], the accuracy of estimated heading and position was improved by using carrier-phase differential GPS. In [20], the lateral velocity is estimated by using two low-cost GPS, and the problem of low frequency of traditional GPS signals is compensated by combining with IMU. In [21], an extended square-root cubic Kalman filter was proposed to combine GPS and IMU. In [22], motion and constraint models were combined with IMU data to overcome interference from gyroscope drift and disturbances in external acceleration. In [23], a novel Kalman filter is proposed to solve the problem of sensor jitter noise, which further improves the estimation accuracy. However, GPS may lose signal in some areas, resulting in serious estimation errors [24]. Meanwhile, with the development of autonomous driving technology, high-precision and highly robust autonomous vehicle positioning technology is one of the fundamental technologies in the field of autonomous driving. However, the above research results have not fully utilized the positioning information of driverless vehicles for velocity estimation.

Therefore, this paper proposes a method that integrates LiDAR and IMU information to achieve accurate estimation of longitudinal velocity, which addresses the issues of relying on tires to indicate the failure of vehicle longitudinal velocity under certain extreme operating conditions and the possibility of GPS losing signal in certain areas. At the same time, in order to reduce model error and suppress filtering divergence, a noise adaptive module (AUKF) is added to the UKF, and time-series analysis is introduced into the adaptive unscented Kalman filter (TSA-AUKF) to further improve estimation accuracy. The structure of the proposed method is shown in Figure 1. Based on this, the novelties of this paper are summarized as follows:

• To address the issue of point cloud distortion during the motion of LiDAR, IMU is used to predict the pose changes of LiDAR to reduce the impact of motion distortion on odometry accuracy.
• Time-series analysis is introduced into the motion equation to predict the trend of longitudinal acceleration changes through multiple sets of IMU historical data, thereby improving the estimation accuracy of the filtering algorithm.

The rest of this paper is structured as follows. In the second section, the LiDAR-IMU odometry is introduced. In the third section, the proposed longitudinal velocity estimation method is introduced. In the fourth section, simulation experiments and real vehicle experiments are conducted, and the results are analyzed. In the fifth section, the full text is summarized.

2. LiDAR-IMU Odometry

2.1. Correction of Point Cloud Motion Distortion

This paper uses a mechanical rotating LiDAR to output a point cloud collected within a cycle as one frame. When the system is in motion and the position of the LiDAR changes, the point cloud within a frame corresponds to different coordinate origins, resulting in motion distortion of the obtained point cloud data, as shown in Figure 2.

In order to correct the motion distortion of LiDAR, it is necessary to obtain the pose changes of LiDAR within one frame. However, due to the fact that LiDAR is often in variable velocity motion and the low update frequency of LiDAR odometry, the actual application effect is poor. The IMU sampling frequency can reach 100 Hz, which can accurately reflect the movement and high-precision local pose estimation. By integrating the data collected by IMU and using linear interpolation, the pose increment of the laser point at any time $t\in \left[t_{k-1},t_k\right]$ relative to time $t_{k-1}$ can be obtained. The calculation formula is

$$T_i=\frac{t_k-t_i}{t_k-t_{k-1}}{T}_{t_{k-1}}^{t_k}$$

(1)

The laser point coordinates at time $t_i$ are converted to the first laser point coordinate system through pose increment. The calculation formula is

$$P_i^{\prime }=T_i{P}_i$$

(2)

where $P_i$ is the position of the laser point at time $t_i$, and $P_i^{\prime }$ is the position of $P_i$ in the coordinate system of the first laser point. The use of IMU to eliminate motion distortion in LiDAR can effectively avoid problems such as map ghosting caused by distortion. The effect of motion distortion removal is shown in Figure 3.

2.2. LiDAR Point Cloud Clustering

During the information collection process of LiDAR, small objects may not repeatedly appear in adjacent frames, causing errors in inter-frame matching. Therefore, this paper calculates the angle between adjacent laser beam scanning points and point cloud clusters based on the size of the angle. If the clustering results are less than 30 points, they are removed as noise points to improve feature extraction accuracy and inter-frame matching efficiency. As shown in Figure 4, traverse from the red point as the center to adjacent points (green points) around, calculate the angle between the center point and adjacent points $\alpha$, until the clustering conditions are not met.

The calculation process is

$$h=d_2·\sin \alpha$$

(3)

$$l_2=d_2·\cos \alpha$$

(4)

$$l_1=d_1-l_2=d_1-d_2·\cos \alpha$$

(5)

Flatness $Fangle$ is

$$Fangle=ata{n}^2\left(d_2·\sin \alpha ,d_1-d_2·\cos \alpha \right)$$

(6)

where $d_1$ and $d_2$ are the lengths of blue and orange laser beams, and $alpha$ and $alph{a}^{\prime }$ are the angular resolutions in the horizontal and vertical directions, respectively. When adjacent laser beam scanning points are on the same horizontal line, $\alpha$ is equal to the horizontal angular resolution $alpha$. When adjacent laser beam scanning points are on the same vertical line, $\alpha$ is equal to the vertical angular resolution $alph{a}^{\prime }$. During the iteration process, points with flatness greater than the threshold are placed in set $\mathcal{N}$. When the number of points in set $\mathcal{N}$ is less than 30 at the end of the iteration, points in set $\mathcal{N}$ are removed as noise points.

2.3. Feature Extraction of Point Cloud

After clustering the original point cloud of the LiDAR, the distance image is horizontally divided into several sub-images, and each sub-image is traversed and processed sequentially. Any point $p_i$ is selected in the point cloud, multiple continuous points in the vertical direction of point $p_i$ are selected to construct a point set $S$, and the smoothness $c$ of each point in the point set is calculated as

$$c=\frac{1}{\left|S\right|\cdot ||r_i||}{\displaystyle ||{\displaystyle \sum }_{j\in S,j\ne i}\left(r_j-r_i\right)||}$$

(7)

By setting threshold $c_{th}$ to distinguish the features of points, those with smoothness greater than the threshold are edge points, while those with smoothness greater than the threshold are planar points. To evenly extract features from all directions, the LiDAR scanning data are divided into two subsets horizontally. The smoothness of the points in the subset is sorted, and the edge feature point ${𝒫}_i^e$ with the highest roughness is selected from each row of the subset. Then, the planar feature point ${𝒫}_i^f$ with the lowest roughness is selected in the same way.

2.4. Feature Matching of Point Cloud

By extracting the feature points of the LiDAR keyframe and matching the feature points of adjacent keyframes, the pose transformation relationship of the LiDAR can be solved. The point clouds scanned by the LiDAR at time $t_k$ and time $t_{k+1}$ are set as ${ℱ}_k$ and ${ℱ}_{k+1}$, the edge feature point sets extracted from the feature extraction as ${ℱ}_k^e$ and ${ℱ}_{k+1}^e$, and the planar feature point sets as ${ℱ}_k^p$ and ${ℱ}_{k+1}^p$. The purpose of inter-frame matching is to obtain the transformation relationship between ${ℱ}_k$ and ${ℱ}_{k+1}$, as well as the transformation relationship between ${ℱ}_{k+1}^e$ and ${ℱ}_k^e$ and the transformation relationship between ${ℱ}_k^p$ and ${ℱ}_{k+1}^p$. Due to the distortion generated during the LiDAR motion process, the edge feature point set ${ℱ}_{k+1}^e$ and planar feature point set ${ℱ}_{k+1}^p$ extracted at time $t_{k+1}$ are projected to time $t_k$, denoted as ${ℱ}_{k+1}^e{}^{\prime }$ and ${ℱ}_{k+1}^p{}^{\prime }$. Edge points are points in sharp positions in a 3D environment, and edge feature points use a point-to-line matching method. Planar points are points located in a smooth region in a 3D environment, and planar feature points use point-to-face matching.

2.4.1. Edge Point Matching

A point $F_{k+1}^{e,a}{}^{\prime }$ in the set of edge feature points ${ℱ}_{k+1}^e{}^{\prime }$ is selected at time $t_{k+1}$, and the closest point $F_k^{e,b}$ to $F_{k+1}^{e,a}{}^{\prime }$ and the closest point $F_k^{e,c}$ in the scan line adjacent to point $F_k^{e,b}$ at time $t_k$ are selected, with coordinates $X_{k+1}^{e,a}{}^{\prime }$, $X_k^{e,b}$, and $X_k^{e,c}$, as shown in Figure 5. The distance $d_E$ from point $F_{k+1}^{e,a}{}^{\prime }$ to line $\left\{F_k^{e,b},F_k^{e,c}\right\}$ is

$$d_E=\frac{\left|\left(X_{k+1}^{e,a}{}^{\prime }-X_k^{e,b}\right)\times \left(X_{k+1}^{e,a}{}^{\prime }-X_k^{e,c}\right)\right|}{\left|X_k^{e,b}-X_k^{e,c}\right|}$$

(8)

2.4.2. Planar Point Matching

A point $F_{k+1}^{p,a}{}^{\prime }$ in the set of planar feature points ${ℱ}_{k+1}^p{}^{\prime }$ at time $t_{k+1}$ is selected, the closest point $F_k^{p,b}$ to $F_{k+1}^{p,a}{}^{\prime }$ at time $t_k$ is selected, and the closest point $F_k^{p,c}$ to $F_k^{p,b}$ in the same vertical resolution point set is found, as well as the closest point $F_k^{e,d}$ in the adjacent scanning laser line with point $F_k^{p,b}$, with coordinates $X_{k+1}^{p,a}{}^{\prime }$, $X_k^{p,b}$, $X_k^{p,c}$, and $X_k^{e,d}$, as shown in Figure 6.

The distance $d_P$ from point $F_{k+1}^{p,a}{}^{\prime }$ to plane $\left\{F_k^{p,b},F_k^{p,c},F_k^{e,d}\right\}$ is

$$d_P=\frac{\left|\left(X_{k+1}^{p,a}{}^{\prime }-X_k^{p,b}\right)·\left(\left(X_k^{p,b}-X_k^{p,c}\right)\times \left(X_k^{p,b}-X_k^{e,d}\right)\right)\right|}{\left(X_k^{p,b}-X_k^{p,c}\right)\times \left(X_k^{p,b}-X_k^{e,d}\right)}$$

(9)

2.5. Pose Estimation

After matching the features of edge points and planar points, the point-to-line distance $d_E$ and point-to-planar distance $d_P$ are obtained. To obtain the optimal pose, the following cost function can be established by combining (8) and (9):

$$f\left(T_{k+1}^{\prime }\right)=min\left(d_E+d_P\right)$$

(10)

The error function is established by using the Lewinberg-Marquardt (L-M) method to optimize the solution:

$$min\frac{1}{2}||f\left(T_{k+1}^L\right)+\mathcal{J}{\left(T_{k+1}^L\right)}^T\Delta T_{k+1}^L||,s.t||D\Delta x_k||\le \mu$$

(11)

where $\mu$ is the trust radius, and $D$ is the coefficient matrix. Constructing Lagrangian function:

$$L\left(\Delta T_{k+1}^L,\lambda \right)=\frac{1}{2}||f\left(T_{k+1}^L\right)+\mathcal{J}{\left(T_{k+1}^L\right)}^T\Delta T_{k+1}^L||{}^2+\frac{\lambda }{2}(||D\Delta T_{k+1}^L||{}^2-\mu )$$

(12)

where $\lambda$ Is the coefficient. Simplify and derive (12) to obtain

$$\left(\mathcal{J}{\mathcal{J}}^T+\lambda D^{T}D\right)\Delta T=-\mathcal{J}f$$

(13)

Substituting (13) into equation (10) yields

$$\Delta T=-{\left(\mathcal{J}{\mathcal{J}}^T+\lambda D^{T}D\right)}^{-1}\left(\mathcal{J}f\right)$$

(14)

where the coefficient matrix $D$ can be approximated as $\mathcal{J}{\mathcal{J}}^T$, so under the premise that the initial Jacobian matrix $\mathcal{J}$ is known, the residual $\Delta T$ can be solved. Substitute the gradient descent formula as

$$\begin{array}{c}{T}_{k+1}^L\leftarrow T_{k+1}^L+\Delta T_{k+1}^L\\ T_{k+1}^L\leftarrow T_{k+1}^L-{\left(\mathcal{J}{\mathcal{J}}^T+\lambda D^{T}D\right)}^{-1}\left(\mathcal{J}f\right)\end{array}$$

(15)

Using two L-M optimization iterations to solve until convergence, obtain the pose transformation matrix $T_{k+1}^L$. Finally, take the differential value of $T_{k+1}^L$ as a rough estimation of longitudinal velocity $Z_k$.

3. Fusion Method of LiDAR and IMU

3.1. AUKF

The nonlinear system of vehicle motion is

$$\{\begin{array}{c}{X}_k=F{X}_{k-1}+Q_k\\ Z_k=H{X}_k+R_k\end{array}$$

(16)

where $X_k$ and $X_{k-1}$ represent the motion state variables at time $k$ and $k-1$, respectively, $F$ represents the motion model transfer matrix, $Q_k$ represents process noise, $Z_k$ represents the observation at time $k$, $H$ represents the observation model transfer matrix, and $R_k$ represents the observation noise.

Set the initial estimation value ${\widehat{X}}_0$ and error covariance $P_0$ for the motion state:

$${\widehat{X}}_0=\mathrm{E}\left[X_0\right]$$

(17)

$$P_0=\mathrm{E}\left[\left(X_0-{\widehat{X}}_0\right){\left(X_0-{\widehat{X}}_0\right)}^{\mathrm{T}}\right]$$

(18)

At the same time, calculate the Sigma sampling points for the motion state:

$$X_{{\sigma }_{k-1}^i}=\{\begin{array}{c}{\widehat{X}}_{k-1}^0,i=0\\ {\widehat{X}}_{k-1}^i+{\left(\sqrt{\left(n+\lambda \right)L_{k-1}}\right)}_i,i=1,2,\cdots ,n\\ {\widehat{X}}_{k-1}^i-{\left(\sqrt{\left(n+\lambda \right)L_{k-1}}\right)}_{i-n},i=1,2,\cdots ,n\end{array}$$

(19)

where $n$ represents the dimension of $X$, and $\lambda$ represents the scaling factor, taken as $\lambda =1,\sqrt{L_{k-1}}\sqrt{L_{k-1}}=P_{k-1}$.

Sigma sampling points are mapped through the motion model transfer matrix $F$:

$$X_{{\sigma }_{k|k-1}^i}=F{X}_{{\sigma }_{k-1}^i}$$

(20)

Then, perform weighted calculations to obtain the prediction and error covariance matrix of the motion state variables at time $k$:

$${\widehat{X}}_k^{-}={\displaystyle \sum }_{i=0}^{2n}\left(w_i{X}_{{\sigma }_{k|k-1}^i}\right)$$

(21)

$$P_k^{-}={\displaystyle \sum }_{i=0}^{2n}\left[w_i\left(X_{{\sigma }_{k|k-1}^i}-{\widehat{X}}_k^{-}\right){\left(X_{{\sigma }_{k|k-1}^i}-{\widehat{X}}_k^{-}\right)}^{\mathrm{T}}\right]+Q_k$$

(22)

where $w_i$ represents the weight

$$w_i=\{\begin{array}{c}\frac{\lambda }{\left(\lambda +n\right)},i=0\\ \frac{\lambda }{2\left(\lambda +n\right)},i=1,2,\cdots ,2n\end{array}$$

(23)

Calculate the Sigma sampling points for the observed value as

$$Y_{{\sigma }_{k-1}^i}=\{\begin{array}{c}{\widehat{X}}_{k-1}^{0-},i=0\\ {\widehat{X}}_k^{i-}+{\left(\sqrt{\left(n+\lambda \right)L_{k-1}}\right)}_i,i=1,2,\cdots ,n\\ -{\left(\sqrt{\left(n+\lambda \right)L_{k-1}}\right)}_{i-n},i=1,2,\cdots ,n\end{array}$$

(24)

where $\sqrt{L_{k-1}}\sqrt{L_{k-1}}=P_t^{-}$. Sigma sampling points are mapped through the motion model transfer matrix $H$:

$$Y_{{\sigma }_{k|k-1}^i}=H{Y}_{{\sigma }_{k-1}^i}$$

(25)

Then, perform weighted calculations to obtain the prediction and error covariance matrix of the observed value at time $k$, as well as the cross-covariance matrix of the observation prediction error:

$$\widehat{Y}={\displaystyle \sum }_{i=0}^{2n}\left(w_i{Y}_{{\sigma }_{k|k-1}^i}\right)$$

(26)

$$P_Y={\displaystyle \sum }_{i=0}^{2n}\left[w_i\left(Y_{{\sigma }_{k|k-1}^i}-\widehat{Y}\right){\left(Y_{{\sigma }_{k|k-1}^i}-\widehat{Y}\right)}^{\mathrm{T}}\right]+R_k$$

(27)

$$P_{XY}={\displaystyle \sum }_{i=0}^{2n}\left[w_i\left(X_{{\sigma }_{k|k-1}^i}-X_k^{-}\right){\left(X_{{\sigma }_{k|k-1}^i}-{\widehat{X}}_k^{-}\right)}^{\mathrm{T}}\right]$$

(28)

The Kalman gain can be calculated as

$$K=P_{XY}{P}_Y^{-1}$$

(29)

Finally, the motion state at time $k$ can be obtained as

$${\widehat{X}}_k={\widehat{X}}_k^{-}+K\left(Z_k-\widehat{Y}\right)$$

(30)

$$P_k=P_k^{-}-K{P}_Y{K}^{\mathrm{T}}$$

(31)

Modeling error is an important component of process noise. By adaptively adjusting the covariance matrix of process noise based on the difference between observed and predicted values, estimation error can be reduced and filtering divergence can be suppressed. So, the difference between the observed and predicted values is defined as

$$d=Z_k-\widehat{Y}$$

(32)

where $d$ is affected by modeling errors and initial conditions, and the noise covariance matrix can be estimated based on $d$. So, the theoretical covariance matrix of $d$ is

$$C_k=E\left[v_k{v}_k^T\right]=H{P}_k{H}^T+R_k$$

(33)

Due to the influence of modeling errors and observation noise, the actual value of the covariance matrix of $d$ often deviates from the theoretical value. So, the actual covariance matrix calculation method for $d$ is

$${\widehat{C}}_k^{}=\frac{1}{M}{\displaystyle \sum }_{i=1}^M{v}_{i-1}{v}_{i-1}^T$$

(34)

where $M$ is the length of the sliding window.

$R_k$ is adjusted by comparing the actual covariance matrix $C_k$ of $d$ with the theoretical covariance matrix ${\widehat{C}}_k^{}$. $R_k$ is reduced when $C_k>{\widehat{C}}_k^{}$; when $C_k<{\widehat{C}}_k^{}$ is used, theoretically $R_k$ should be added, but to avoid filter divergence, $R_k$ can be kept constant. Define the $R_k$ adjustment factor as

$${\alpha }_k=\underset{}{\max }\left(1,\frac{trace\left({\widehat{C}}_k^{}\right)}{trace\left(C_k\right)}\right)$$

(35)

In order to improve the estimation accuracy of the Kalman filter, the observation noise covariance matrix and the process noise covariance matrix are generally adjusted in the opposite direction [25]. Therefore, the adaptive adjustment method for the covariance matrix of state estimation error is

$$R_k={\alpha }_k{R}_k$$

(36)

$$Q_k=\frac{Q_k}{{\alpha }_k}$$

(37)

3.2. Longitudinal Acceleration Prediction

Due to the existence of (20), it is necessary to predict acceleration, but the acceleration of the vehicle is related to driver habits and current road conditions, making it difficult to describe it in mathematical language. Therefore, the ARIMA model in time-series analysis was used to predict the longitudinal acceleration of vehicles.

The essence of the ARIMA model is to achieve short-term prediction through historical observation data with $p$ sampling intervals and historical random interference with $q$ sampling intervals. Its mathematical expression is

$$x_k={\varphi }_0+{\varphi }_1{x}_{k-1}+{\varphi }_2{x}_{k-2}+\cdots +{\varphi }_p{x}_{k-p}+{\epsilon }_k-{\theta }_1{\epsilon }_{k-1}-\cdots -{\theta }_q{\epsilon }_{k-q}$$

(38)

where $p$ represents the order of the AR model; $q$ represents the order of the MA model; $x_k$ represents predicted data; $x_{k-1}$, $x_{k-2}$, and $x_{k-p}$ represent historical observation data; ${\varphi }_0$, ${\varphi }_1$, ${\varphi }_2$, and ${\varphi }_p$ represent the AR model coefficients; ${\epsilon }_k$, ${\epsilon }_{k-1}$, and ${\epsilon }_{k-q}$ represent historical noise interference; $\epsilon \sim N\left(0,{\sigma }_{\epsilon }^2\right)$; ${\theta }_1$, ${\theta }_2$, and ${\theta }_q$ represent the coefficients of the MA model.

We used ten longitudinal acceleration datasets as a set of historical data and predicted the acceleration value at the next moment using ARIMA. The predicted and true values of longitudinal acceleration are shown in Figure 7, indicating that ARIMA has a certain predictive ability for longitudinal acceleration.

From (38), the predicted longitudinal acceleration at time $k$ can be obtained. However, observing (20), it is found that a mapping relationship from time $k-1$ to time k is required. Therefore, we used a simple processing method

$$F_a=a_k-a_{k-1}$$

(39)

where $F_a$ represents the transfer matrix. So, (20) can be changed to

$$X_{{\sigma }_{k|k-1}^i}=\left[\begin{array}{cc}F& F_a\end{array}\right]\left[\begin{array}{c}{X}_{{\sigma }_{k-1}^i}\\ 1\end{array}\right]$$

(40)

4. Simulation and Experimental Results

To verify the effectiveness of the proposed longitudinal velocity estimation method, we conducted medium- to high-velocity tests (0–30 m/s) in Carla simulation software and low-velocity tests (0–5 m/s) on campus.

4.1. Carla Simulation Experiment

The experimental equipment used is shown in

Table 1. Information about experimental equipment.

Equipment	Model
CPU	I7–7700HQ
GPU	GTX 1050Ti

. Under the conditions of this device, Carla simulation software data may occasionally drift, as we will explain in the experimental results.

The driving trajectory in simulation experiment 1 is shown in Figure 8, and the vehicle velocity varies between 0–15 m/s. The longitudinal velocities obtained through LiDAR-IMU odometry, AUKF, and TSA-AUKF are shown in Figure 9, and the errors with respect to the longitudinal velocity reference value in Carla are shown in Figure 10. It can be seen that TSA-AUKF has higher estimation accuracy. During 19–22 s, due to the drift phenomenon in IMU data, only the LiDAR-IMU odometry could obtain a reasonable longitudinal velocity estimation, resulting in complete failure of AUKF. TSA-AUKF, based on the prediction of IMU historical data, could undergo significant correction.

The driving trajectory in simulation experiment 2 is shown in Figure 11, and the vehicle velocity varies between 0–30 m/s. Figure 12 shows the longitudinal velocity of the vehicle obtained by different methods, and the error with respect to the longitudinal velocity reference value in Carla is shown in Figure 13. It can be clearly seen that TSA-AUKF still has better accuracy. A tunnel was passed between 28–37 s, resulting in a decrease in the performance of the LiDAR-IMU odometry. Predictions based on time-series analysis can be used to correct longitudinal velocity estimates. However, due to the lag of time-series analysis, there was a certain deviation in TSA-AUKF between 31–37 s.

The root mean square errors (RMSEs) of LiDAR-IMU odometry, AUKF, and TSA-AUKF estimation results are shown in

Table 2. RMSE of estimation error.

Scenario (Velocity)	LiDAR-IMU Odometry	AUKF	TSA-AUKF
0–15 m/s	1.828	2.597	1.149
0–30 m/s	4.708	3.201	1.573

. In simulation experiment 1, compared with laser odometer and AUKF, TSA-AUKF can achieve approximately 37% and 55% performance improvements; in simulation experiment 2, compared with LiDAR-IMU odometry and AUKF, TSA-AUKF can achieve approximately 62% and 51% performance improvements.

4.2. Real-Vehicle Experiment

Figure 14 shows the experimental platform. The platform’s sensors provide longitudinal reference velocity information of 10 Hz, LiDAR provides point cloud information of 10 Hz, IMU provides acceleration and angular velocity information of 100 Hz, and GPS provides positioning information of 5 Hz. Lidar, IMU, and GPS are installed in the red box position in Figure 14. Velodyne VLP-16 is used as a LiDAR sensor. The specifications of IMU are shown in

Table 3. Datasheet of LPMS-IG1.

Sensor	Quantity	Performance
Gyroscopes	Bias stability	4 $\mathrm{deg}/\mathrm{hr}$
	Angular random walk	0.12 $\mathrm{deg}/\surd \mathrm{hr}$
Accelerometers	Bias stability	25 $\mathrm{ug}$
	Velocity random walk	0.045 $\mathrm{m}/\mathrm{s}/\surd \mathrm{hr}$

hr = hour, s = second; g = 9.8 m/s², deg = degree, u = $1.0\times {10}^{-6}$, m = meter.

.

The driving trajectory is shown in Figure 15, and the vehicle’s velocity varies between 0–5 m/s. In this experiment, we compared TSA-AUKA with the traditional GPS-IMU method. Figure 16 shows the longitudinal velocity of vehicles obtained using different methods, and the error with respect to the reference longitudinal velocity is shown in Figure 17. It can be clearly seen that TSA-AUKA can provide stable longitudinal velocity estimates.

The RMSEs of LiDAR-IMU odometry, GPS-IMU, and TSA-AUKF estimation results are shown in

Table 4. RMSE of estimation error.

LiDAR-IMU Odometry	GPS-IMU	TSA-AUKF
0.279	0.158	0.113

. Compared with LiDAR-IMU and GPS-IMU, TSA-AUKF can achieve approximately 59% and 28% performance improvements.

5. Conclusions and Future Work

This paper proposes a longitudinal velocity estimation method for driverless vehicles. By integrating LiDAR and IMU information, the goal of not relying on tires to represent longitudinal velocity is achieved, and the problem of GPS losing signal in certain areas can also be avoided. By using an adaptive noise estimator to track the statistical characteristics of system noise, model errors are reduced and filtering divergence is suppressed, thereby improving the robustness and filtering accuracy of the algorithm; introducing time-series analysis into the motion equation improves the estimation accuracy. The feasibility of the estimation method under medium- to high-velocity conditions was verified in simulation experiments. Under a velocity variation of 0–15 m/s, the RMSE of TSA-AUKF is 1.149 m/s; at a velocity variation of 0–30 m/s, the RMSE of TSA-AUKF is 1.573 m/s. In addition, the data drift generated by the simulation software can also verify the high robustness of the proposed algorithm. In real-vehicle experiments, TSA-AUKF achieved a 28% performance improvement compared to existing GPS-IMU longitudinal velocity estimation methods. The results show that the method proposed in this paper can provide stable longitudinal velocity estimates for driverless vehicles. In future work, more realistic vehicles will be used for experiments on standard roads to improve the performance of the algorithm.