On-Site Localization of Unmanned Vehicles in Large-Scale Outdoor Environments

Abstract

This paper proposes a method for the on-site localization of autonomous vehicles in large-scale outdoor environments, where a single sensor cannot achieve the high precision for localization. The method is based on an improved Kalman filter by fusion of odometry and LiDAR, and it is intended to address the challenge of localization in large-scale environments. Given the complex nature of such environments and the difficulty of identifying natural features at the worksite accurately, the paper uses artificial landmarks to model the working environment. The Iterative Closest Point (ICP) algorithm matches local features of landmarks that were scanned by LiDAR at the current time with local landmark features from the past to obtain the vehicle’s on-site pose. Within the extended Kalman filter (EKF) framework, odometry information is fused with the pose information obtained by the ICP algorithm to further enhance the accuracy of the vehicle’s localization. Simulation results demonstrate that the localization accuracy of unmanned vehicles optimized by the EKF algorithm improves by 9.21% and 53.91% compared to the ICP algorithm and odometry, respectively. This reduces the noise error of measurements, which improves the precise movement and on-site localization performance of unmanned vehicles in large-scale outdoor environments.

1. Introduction

Faced with the midst of the Artificial Intelligence (AI) and digitalization revolution, the construction industry is one of the key areas for industrial digitalization. Among these, autonomous vehicles can replace workers in performing repetitive, high-load, and high-risk mobile tasks, such as transporting construction materials, laying materials, and cleaning, thereby effectively shortening the construction lifecycle and driving the digitalization and intelligent upgrading of the construction industry [1]. Reliable localization in large-scale outdoor environments is the foundation for the stable operation of autonomous or semi-autonomous mobile unmanned vehicles. However, a single sensor typically cannot meet the on-site localization accuracy requirements of unmanned vehicles. If reliance is solely on measurement feedback from odometry, mobile unmanned vehicles will experience cumulative errors, leading to increasing movement deviations over time, and the limited environmental data obtained is insufficient for mobile operations. If only LiDAR is used to obtain pose information, sudden changes in environmental noise, LiDAR noise, and algorithm matching errors can lead to significant localization errors in the vehicle. Therefore, designing a multi-sensor fusion localization system to achieve on-site localization for unmanned vehicles is a comprehensive solution to explore.

The current mainstream localization technologies for autonomous vehicles primarily include: Simultaneous Localization and Mapping (SLAM) technology [2]; multi-sensor fusion localization technology [3]; GPS localization technology [4]; ultrasonic navigation localization technology [5], among others. Among these, feature-based matching methods in SLAM can accurately determine the spatial relationships between autonomous vehicles, such as the ICP algorithm and its variants. However, these methods must address issues such as noise interference in point cloud data, incomplete information, and uneven sampling density [6]. Filter-based techniques can rapidly and effectively estimate the state of unmanned vehicles, just as the Finite Impulse Response (FIR) structure state estimator and Generalized Minimum Variance FIR (GMVF) filter are developed [7,8]. Additionally, the Kalman filter is commonly applied in target tracking and localization, and it is regarded as the optimal linear state estimator. The nonlinearity of the system model limits the effective application of the Kalman filter, leading to the development of EKF and unscented Kalman filter (UKF) for nonlinear models [9,10,11]. The EKF introduces the concept of linearization based on the classical Kalman filter, making it more suitable for solving nonlinear models. The UKF provides accurate estimates for nonlinear systems under ideal conditions, but modeling errors may accumulate, which typically leads to filter divergence. State estimation methods based on Gaussian approximation have gained increasing attention due to their advantages of good estimation accuracy, moderate computational complexity, and ease of implementation [12]. Pak designed an extended MVF filter and combined it with Horizon Group Shift (HGS) to form the HGS-FIR filter [13]. Compared to the UKF, the HGS-FIR filter takes 60 times longer to compute the estimated values and has weaker resistance to external noise interference. To overcome the design flaws of the HGS-FIR filter, the TLNF filter and the fused TLNF/UK filter are further designed [14]. To enhance the real-time positioning performance of unmanned vehicles, the integrated Global Positioning System (GPS) and Inertial Measurement Unit (IMU) navigation scheme serves as the fundamental positioning solution. However, in GPS-denied environments such as urban canyons, this method suffers from significant cumulative errors. Meanwhile, LiDAR-based SLAM can achieve positioning and mapping without GPS, yet it involves high costs and demands substantial computational resources for processing feature-rich point clouds. Although visual SLAM offers a low-cost alternative for positioning, it is prone to failure in outdoor settings with poor lighting or insufficient texture details. Therefore, adopting a multi-sensor fusion positioning approach can improve rapid localization and robustness for unmanned vehicles in outdoor environments.

The localization performance of outdoor autonomous vehicles is closely related to the measurement values of sensors, which are susceptible to various external interferences, faults, and attacks, such as false measurements, missing measurements, delayed measurements, and non-Gaussian measurement noise. Developed for autonomous vehicle steering systems, a fault-tolerant path-tracking controller utilizing static output-feedback and linear parameter varying techniques is presented [15]. Its operational framework employs dual-mode coordination between steering and torque-vectoring control to preserve vehicle stability in the presence of steering actuator gain faults. Among them, Intentional Electromagnetic Interference (IEMI) comprises the conducted interference and radiated interference [16]. Conducted interference infiltrates vehicular electronic systems through physical conductors such as power lines and communication cables, while radiated interference involves attackers projecting high-frequency electromagnetic waves toward vehicles from a distance via directional antennas or high-power electromagnetic pulse devices. These emissions can couple into critical sensor systems and Controller Area Network (CAN) bus lines of unmanned vehicles. The specific impacts manifest as follows: IEMI can induce sensor failure across multiple modalities. In LiDAR systems, strong electromagnetic noise generates substantial artifacts in point cloud data, potentially causing complete distortion; for imaging sensors, electromagnetic saturation may produce entirely white or streaked output frames; regarding high-precision GPS/IMU integrated navigation systems, electromagnetic interference directly disrupts high-frequency circuitry and signal reception, leading to significant positioning deviations or complete navigation failure. Concurrently, IEMI induces communication interruptions and protocol violations in CAN. Furthermore, when IEMI power exceeds electronic components’ tolerance thresholds, it can cause thermal breakdown or electrical overstress damage to sensor chips, main processors, and power components. The Minimum Mean Square Error (MMSE) criterion is employed in state estimation using nonlinear Kalman filters, and a Huber–Kalman filter (HKF) that suppresses outliers in both the system and observation data is proposed [17]. The robust Student’s t-based Kalman filter (RSTKF) utilizes the Student’s t-distribution to model heavy-tailed non-Gaussian noise, thereby generating robust posterior estimates [18]. Additionally, Information Theory Language (ITL) [19] has been applied to counteract non-Gaussian noise, incorporating metrics such as the Maximum Correntropy Criterion (MCC) from ITL into adaptive filtering theory to capture higher error statistics. Replacing the MMSE criterion in the Kalman filter with the MCC, the maximum correntropy Kalman filter (MCKF) [20] and the maximum correlation entropy unscented Kalman filter (MCUKF) [21,22,23] are also developed for combating non-Gaussian noise.

For the uncertainty noise data generated by multiple sensors of autonomous vehicles, a line feature SLAM algorithm is combined with EKF, and the EKF is used to estimate the robot’s position state [24]. The UKF is combined with Pedestrian Dead Reckoning (PDR) to fuse the results of Wi-Fi localization, which achieved localization results superior to those obtained by the single sensor of Wi-Fi or PDR [25]. A Kalman filter algorithm based on the maximum entropy correlation criterion is proposed and applied to address the issue of non-Gaussian distribution data in UWB indoor localization under complex environmental conditions [26]. The BIM-based indoor navigation using end-to-end visual localization and ARCore is proposed, which enhances robot localization through BIM’s map [27].

Based on the above, although existing research has made progress in improving the accuracy and robustness of outdoor positioning for unmanned vehicles, further exploration is still required to achieve a balance between high performance and low cost while maintaining reliable positioning performance in construction scenarios characterized by GPS signal obstruction and lack of natural features. In addition, the large-scale outdoor environments typically feature uneven terrain, which can cause autonomous vehicles to slip, resulting in significant discrepancies between the odometry-measured speed and the actual speed. Therefore, it is essential to realize the fusion estimation by LiDAR and odometry. In our paper, the term “large-scale” primarily refers to the fact that the working area for outdoor material handling tasks is substantially larger than the structural dimensions of the unmanned vehicle itself, and the vehicle must move to complete the transportation tasks. The term “large-scale” herein does not carry a strict quantitative value. Additionally, the Kalman filter, as a mature probabilistic localization technique, significantly reduces the interference of various uncertainty errors while ensuring computational efficiency and minimal memory usage, thereby enhancing localization accuracy and real-time performance. Therefore, in conjunction with outdoor material handling tasks, this paper proposes LiDAR sensors and odometry to design a localization optimization system for an unmanned vehicle based on an extended Kalman filter in an outdoor construction environment.

The main innovations of this paper are as follows:

(1)

Due to the mixed features of the large-scale construction environment that are difficult to identify and extract effectively, this paper proposes an artificial landmark to build the construction map, which realizes on-site localization of the unmanned vehicle based on the landmark map.

(2)

Considering the rapid and robust localization performance of on-site, the paper provides an extended Kalman filter framework to combine the current pose of the unmanned vehicle by odometry information, and the ICP algorithm is adopted to match the current landmark feature with that scanned at the previous moment.

(3)

Simulation experiments were conducted to validate the fused localization solution of an unmanned vehicle based on the operational path that considers the flatness of the on-site environment, and the results from odometry, ICP matching, and EKF-fused methods are obtained and compared.

The remainder of this paper is organized as follows: Section 2 provides model descriptions of the environment map and the unmanned vehicle. Section 3 describes the on-site localization methods of unmanned vehicles based on an improved framework of the EKF algorithm. Section 4 presents the simulation experiments and discusses the results. Finally, the conclusion and future work are presented in Section 5.

2. Model Descriptions

For the outdoor handling tasks, unlike indoor service robots, unmanned vehicles rely heavily on good localization performance, which is crucial for them to perform tasks efficiently and accurately in outdoor environments. Considering that the operating areas in outdoor scenarios have obvious undulations and complex time-varying characteristics, it is difficult for unmanned vehicles to extract effective environmental features for localization.

This makes the method based on landmark arrangement a simple and reliable solution. Specifically, on-site localization of the unmanned vehicle is achieved by arranging artificial landmarks on-site and extracting data through LiDAR scanning, and the specific operating environment is shown in Figure 1.

Figure 1. Two-dimensional plan of the large-scale outdoor environment.

In this paper, the tracked unmanned vehicle is simplified into the motion model of a two-wheeled differential drive robot, where $V_L$ and $V_R$ represent the linear velocities of the left and right wheels, respectively; d_LR denotes the wheel spacing, and r_C and V_C represent the rotation radius of point O and total velocity of vehicle, which are shown in Figure 2.

Figure 2. Motion model for an equivalent two-wheeled differential-drive unmanned vehicle.

Taking O-O_L as the horizontal axis and the X-axis as the vertical axis, the positions of the equivalent virtual left and right wheels are at points L and R, respectively. By calculating the velocity of center O through the velocities of the left and right driving wheels, the forward kinematics model of the unmanned vehicle can be obtained as follows:

$$\left[\begin{array}{c}{V}_c\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{V_r+V_l}{2}}\\ {\displaystyle \frac{V_r-V_l}{d_{LR}}}\end{array}\right]=\left[\begin{array}{cc}{\displaystyle \frac{1}{2}}& {\displaystyle \frac{1}{2}}\\ {\displaystyle \frac{1}{d_{LR}}}& -{\displaystyle \frac{1}{d_{LR}}}\end{array}\right]\left[\begin{array}{c}{V}_r\\ V_l\end{array}\right]$$

(1)

By decomposing the velocity of the centroid O, the velocities of the left and right driving wheels are derived, and the inverse kinematics model is simplified as follows:

$$\left[\begin{array}{c}{V}_r\\ V_l\end{array}\right]=\left[\begin{array}{c}{V}_c+{\displaystyle \frac{d_{LR}}{2}}\dot{\theta }\\ V_c-{\displaystyle \frac{d_{LR}}{2}}\dot{\theta }\end{array}\right]=\left[\begin{array}{cc}1& {\displaystyle \frac{d_{LR}}{2}}\\ 1& -{\displaystyle \frac{d_{LR}}{2}}\end{array}\right]\left[\begin{array}{c}{V}_c\\ \dot{\theta }\end{array}\right]$$

(2)

Since the distance between the equivalent left and right wheels (denoted as $d_{LR}$) of the unmanned vehicle is not necessarily equal to the actual distance between the two tracks, and $d_{LR}$ is dynamically variable, a dimensionless parameter $\gamma$ is introduced. Through derivation, the virtual wheel spacing $d_{LR}$ can be expressed as:

$$d_{LR}=\gamma d_{wb}$$

(3)

where $d_{wb}$ represents the wheel spacing of the unmanned vehicle; $\gamma$ is the coefficient of the wheel spacing, which is related to the relative friction coefficient between the unmanned vehicle’s tracks and the ground, total load, turning radius, and centroid position.

Thus, the forward kinematics model of the unmanned vehicle can be obtained as:

$$\left[\begin{array}{c}{V}_c\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}{\displaystyle \frac{1}{2}}& {\displaystyle \frac{1}{2}}\\ {\displaystyle \frac{1}{\gamma d_{wb}}}& -{\displaystyle \frac{1}{\gamma d_{wb}}}\end{array}\right]\left[\begin{array}{c}{V}_r\\ V_l\end{array}\right]$$

(4)

The inverse kinematics model is as follows:

$$\left[\begin{array}{c}{V}_r\\ V_l\end{array}\right]=\left[\begin{array}{cc}1& {\displaystyle \frac{\gamma d_{wb}}{2}}\\ 1& -{\displaystyle \frac{\gamma d_{wb}}{2}}\end{array}\right]\left[\begin{array}{c}{V}_c\\ \dot{\theta }\end{array}\right]$$

(5)

Finally, the system state equation of the unmanned vehicle can be derived from the linear velocity and angular velocity as follows:

$$X_{[k+1]}=\left[\begin{array}{c}{x}_{[k+1]}\\ y_{[k+1]}\\ r_{[k+1]}\\ v_{[k+1]}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_{[k]}\\ y_{[k]}\\ r_{[k]}\\ v_{[k]}\end{array}\right]+\left[\begin{array}{cc}\cos (r)dt& 0\\ \sin (r)dt& 0\\ 0& dt\\ 1& 0\end{array}\right]\left[\begin{array}{c}{v}_k\\ \dot{\theta }\end{array}\right]+w_{[k]}$$

(6)

where parameter $X_{[k]}$ is a state vector containing the displacement in the x-axis direction, the displacement in the y-axis direction, the angle r, and the linear velocity v; the parameter dt is the time interval for sensor data acquisition, and the parameter $w_{[k]}$ represents the process noise vector of the system.

3. Localization Framework of Unmanned Vehicle in Large-Scale Outdoor Environment

The research object is an unmanned vehicle equipped with a 2D LiDAR and odometry, and the global landmark map and initial pose information of the unmanned vehicle are also known. Based on the global point cloud map, the unmanned vehicle can scan the current local map in real time and match it with the global map to obtain the current pose of the robot. It is assumed that the initial pose of the unmanned vehicle satisfies the rough matching condition of the ICP algorithm, which avoids the matching from falling into a local optimal solution, and the transformation matrix for each subsequent frame of ICP is derived from the previous frame.

Therefore, this paper proposes an improved Kalman localization optimization scheme based on the fusion of LiDAR and odometry. The specific global localization optimization framework flow of the unmanned vehicle is shown in Figure 3. First, the current XOY coordinate position and angle r of the robot are calculated based on the odometry. Considering the existence of cumulative errors in long-term odometry measurements, the reliability of the pose information fed back by the odometry will decrease, which is corrected and compensated for by the LiDAR measurement results. Meanwhile, with time frames as the unit, each time frame is designed to have an interval of 0.1 s, and each frame includes odometry information collection, point cloud information from LiDAR, point cloud matching based on ICP, and localization results from the improved Kalman filter, to obtain the optimized pose of the robot in each time frame in real time. In the improved Kalman filter framework, a prediction model is established to enable the prediction and estimation of the pose from the previous frame to the current frame. Then, the pose information (X, Y) from odometry and the fused pose results of the x and y axes matched by the ICP algorithm are used as the optimized estimation results of the current frame, which are applied to the prediction and estimation of the next frame.

Figure 3. Flowchart of the global localization framework for the unmanned vehicle.

3.1. Localization of Unmanned Vehicle Based on Feature Matching of Artificial Landmarks

The rough localization of the unmanned vehicle is realized based on the ICP matching algorithm, that is, to solve the transformation matrix T that minimizes the distance between the target point cloud $Q=\left\{q_i,i=1,2,\dots ,N_q\right\}$ and the source point cloud $P=\left\{p_i,i=1,2,\dots ,N_p\right\}$. The source point cloud is the local point cloud information acquired by the laser radar, and the target point cloud is the global point cloud map, so $N_q>N_p$, and the relationship between target point cloud Q and source point cloud P before matching is $q_k\in Q$, thus obtaining $\min ({\displaystyle \sum _{i=1}^{N_p}‖q_i-p_i‖})$. Then, the corresponding homogeneous transformation matrix T (Rotation matrix R, Translation matrix t) can be solved based on the parameters $q_i$ and $p_i$.

Also, the objective function of the point cloud matching error can be obtained as:

$$E(R,t)={\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n{{\displaystyle ‖q_i-(R{p}_i+t)‖}}^2}$$

(7)

where the parameter n represents the number of point clouds acquired by the laser radar, and it belongs to [1, N_p].

The basic matching steps of the specific ICP algorithm are as follows:

(1)

Take the source point cloud P, denote its point cloud set as $p_i$, and detect potential landmark arc feature points;

(2)

Generate a descriptor for each feature point, and obtain the subset $q_k$ in the target point cloud Q that satisfies $\min ‖q_k-p_i‖$;

(3)

Take $q_k$ and $p_i$ as input quantities, and obtain the rotation matrix R and translation matrix t that minimize E(R,t) through the similarity of feature vector matching;

(4)

Obtain a new point cloud set $p_i^{\prime }$ based on the rotation and translation matrices R and t;

$$p_i^{\prime }=R{p}_i+t,p_i\in P$$

(8)

(5)

Calculate the average distance $d$ from $p_i^{\prime }$ to $q_k$ again;

(6)

When the average distance $d$ is less than the set threshold or the number of iterations exceeds the maximum set number of iterations, stop the iteration, jump out of the loop, and output the current transformation matrix;

(7)

When the condition in step 6 is not met, return to step 2 and continue the iteration.

3.2. Optimization of Localization for Unmanned Vehicle by an Improved Kalman Filter

The extended Kalman filter algorithm, as an improved filtering method for nonlinear systems, has state-space equations that can be expressed as:

$$\begin{array}{l}{X}_{[k+1]}=f(X_{[k]},u_{[k]},w_{[k]})\\ z_{[k+1]}=h(X_{[k]},v_{[k]})\end{array}$$

(9)

where $f(·)$ and $h(·)$ are the nonlinear function equations, respectively; $w\sim \mathrm{{\rm N}}(0,Q_c)$ and $v\sim \mathrm{{\rm N}}(0,R_c)$ represent normal distributions with an expected value of 0; Q_c and R_c are the a priori process noise and a priori measurement noise of the covariance matrix, respectively; $z_{[k+1]}$ represents the measurement of point cloud features of artificial landmarks acquired by LiDAR; and the nonlinear function $h(X_{[k]},v_{[k]})$ computes the theoretical coordinates of the landmarks observed from the vehicle’s pose $X_{[k]}$ and $v_{[k]}$ which represent the additive measurement noise of the LiDAR system.

However, the calculation of the nonlinear model is complex and time-consuming. To improve the on-site performance of the unmanned vehicle, this paper solves the nonlinear model through a linearization method. It is assumed that when linearizing at $X_{[k+1]}$, the higher-order terms in the series expansion are ignored, and the error $W_{[k]}$ near the point ${\widehat{x}}_{[k]}$ is set to 0. Thus, at time k, the nonlinear functions f() and h() are expanded in a first-order Taylor series, and the posterior state estimate ${\widehat{x}}_{[k]}$ after ignoring the higher-order terms is used to obtain an approximate linearized model as follows:

$$X_{[k+1]}=f(X_{[k]},u_{[k]},w_{[k]})\approx f({\widehat{x}}_{[k]},u_{[k]},0)+A_{[k+1]}(X_{[k]}-{\widehat{x}}_{[k]})+W_{[k+1]}(w_{[k]}-0)$$

(10)

where $A_{[k+1]}$ and $W_{[k+1]}$ are the Jacobian matrices obtained by differentiating the function f with respect to the parameters x and w at time k + 1, respectively.

Finally, the nonlinear EKF algorithm is transformed into the classic Kalman filtering algorithm for calculation, and the state estimation value X_[k+1] at the next moment k + 1 is predicted through the prediction and update steps. The specific implementation process is as follows:

Initialize the initial values of parameters, mainly including the initial state estimation value ${\widehat{x}}_{[0]}$ and the covariance matrix ${\mathrm{P}}_{[0]}$ of the initial state estimation error.

Calculate the a priori state estimation:

$${\widehat{x}}_{[k+1]}^{-}=f({\widehat{x}}_{[k]},u_{[k]},0)$$

(11)

Then, calculate the corresponding Jacobian matrices $A_{[k+1]}$ and $W_{[k+1]}$, and the specific equations are:

$$\begin{array}{l}{A}_{[k+1]}={\displaystyle \frac{\partial f}{\partial x}}({{\widehat{x}}_{[k]}}^{-},u_{[k]},0)\\ W_{[k+1]}={\displaystyle \frac{\partial f}{\partial w}}({{\widehat{x}}_{[k]}}^{-},u_{[k]},0)\end{array}$$

(12)

Based on the covariance matrix $P_{[k+1]}$ of the previous state estimation error and the Jacobian matrix calculated from the process noise matrix $Q$, the covariance matrix ${P_{[k+1]}}^{-}$ of the a priori state estimation error can be obtained:

$${P_{[k+1]}}^{-}=A_{[k+1]}{P}_{[k+1]}{A_{[k]}}^T+W_{[k+1]}{Q}_c{W_{[k+1]}}^T$$

(13)

At the same time, its Jacobian matrix is calculated as:

$$\begin{array}{l}{V}_{[k+1]}={\displaystyle \frac{\partial \mathrm{h}}{\partial v}}({{\widehat{x}}_{[k+1]}}^{-},0)\\ H_{m_{[k+1]}}={\displaystyle \frac{\partial \mathrm{h}}{\partial X}}({{\widehat{x}}_{[k+1]}}^{-},0)\end{array}$$

(14)

The calculation of the Kalman gain can be obtained as:

$$K_{[k+1]}={\displaystyle \frac{{P_{[k+1]}}^{-}{H_{m_{[k+1]}}}^T}{H_{m_{[k+1]}}{P_{[k+1]}}^{-}{H_{m_{[k+1]}}}^T+V_{[k+1]}{R}_{[c]}{V_{[k+1]}}^T}}$$

(15)

The calculation of its posterior state estimation is:

$${\widehat{x}}_{[k+1]}={{\widehat{x}}_{[k+1]}}^{-}+K_{[k+1]}({\mathrm{Z}}_{[k+1]}-h({{\widehat{x}}_{[k+1]}}^{-},0))$$

(16)

Update the covariance of the state estimation error, and the specific equation is depicted as:

$$P_{[k+1]}=(I-K_{[k+1]}{H}_{m_{[k+1]}}){P_{[k+1]}}^{-}$$

(17)

4. Simulation Experiment and Analysis

4.1. Experimental Preparation

According to the transportation task in a large-scale outdoor environment, artificial landmarks are randomly arranged to construct a prior map to avoid its symmetry. The specific scene model is shown in Figure 4. A large-scale transportation working scene is designed according to the operation task, and the path for the mobile unmanned vehicle to carry out the transportation operation is also designed.

Figure 4. Specific scene model with placement of artificial landmarks.

Among them, the design of the prior map needs to consider in advance factors such as building tunnels, equipment obstacles, and fences in the working area, and the information about positions with obvious depressions, protrusions, and so on in the construction area is known, which effectively prevents the unmanned vehicle from slipping and feature mismatching caused by unevenness. Under the above condition, a total of nine cylindrical landmarks are arranged, and it is assumed that the unmanned vehicle can achieve self-localization at any location on its movement route.

To confirm the measurement characteristics of actual sensors in this experiment, the LiDAR sensor noise that made by SLAMTEC of china is modeled using a Gaussian distribution, with the measurement noise parameters calibrated based on robotic localization data from another study [28] to simulate real-world localization of an unmanned vehicle. As a result, Gaussian noise with a mean of 0 is added to construct the point cloud data of LiDAR. For odometry, cumulative translation noise with a signal-to-noise ratio of 35 and cumulative rotation noise with a signal-to-noise ratio of 30 are added to prevent the unmanned vehicle from drifting. In addition, Gaussian noise is added to the input quantities of the velocity and angular velocity that control the robot to simulate process noises such as crosswind and frictional sliding.

4.2. Effect of On-Site Localization

To effectively verify the optimization effect of the proposed algorithm in this paper on the on-site localization of the unmanned vehicle, under the simulated landmark map, this paper compares the actual travel route of the unmanned vehicle, the feedback route based on the odometry, the estimated route based on ICP, and the estimated route after fusing the extended Kalman filter. The specific routes are shown in Figure 5.

Figure 5. Global view of the autonomous planning route for the unmanned vehicle.

As can be seen in Figure 5, specific routes are represented by different parameter symbols, mainly including the preset path of the unmanned vehicle (black “◇” dotted line), the odometry route (blue dotted line), the ICP-based estimated route (pink “×” line), the EKF-based estimated route (red dot-dash line), and the actual movement route of the unmanned vehicle (green dotted line). From the partially enlarged view of Figure 6, the unmanned vehicle’s route optimized by EKF is closer to the actual route compared with the estimated route by ICP and the feedback route of odometry. When the ICP has a large map-matching error, the movement route fused with the EKF algorithm can significantly reduce the impact caused by the unmanned vehicle’s matching error.

Figure 6. Partially enlarged view of the autonomous planning route for the unmanned vehicle.

To effectively verify the localization effect of an unmanned vehicle optimized based on EKF, this paper conducts 10 simulations on the same route. Through random noise generation, different movement routes of the unmanned vehicle are obtained, and the other simulated routes of nine numbers are shown in Figure 7. As a result, the preset path, estimated route based on EKF, pose estimation by ICP, the actual route and route by odometry are represented as the diamond-shaped dashed lines, red dotted solid lines, cross marks with pink, green dots and blue dotted dashed lines, respectively.

Figure 7. Schematic diagram of repeated operation routes performed nine times by an unmanned vehicle.

4.3. Results and Discussion

From the above tests, it can be concluded that even under the influence of cumulative odometry errors, the unmanned vehicle route optimized based on EKF can effectively correct the deviation from the actual route.

As shown in Figure 8, when the process noise is consistent with the measurement noise in the simulation experiment, the covariance matrices for them are represented as Q and R, respectively, and the estimation results based on EKF can offset the excessively varying measurement noise. In this paper, assuming the initial values of Q and R are diagonal matrices and approximately equivalent. After the parameters for them are obtained by numerous simulation experiments and verification, the values of parameters Q and R are set as follows:

$$\begin{array}{l}Q=\left[\begin{array}{cccc}0.2& 0& 0& 0\\ 0& 0.2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0.01\end{array}\right]\\ R=\left[\begin{array}{ccccc}0.2& 0& 0& 0& 0\\ 0& 0.2& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0.2& 0\\ 0& 0& 0& 0& 0.2\end{array}\right]\end{array}$$

(18)

Figure 8. Enlarged comparison diagram of local routes for an unmanned vehicle.

Even when there is a large error between the source point cloud and target point cloud, the route optimized based on EKF can effectively correct the movement trajectory of the unmanned vehicle, thereby reducing the impact caused by a sudden increase in matching errors.

To further verify the superiority of each method, this paper quantitatively analyzes the localization effect of the unmanned vehicle by calculating the Root Mean Squared Error (RMSE) in the x and y axis directions. The specific formula for the RMSE is:

$$\mathrm{RMSE}=\sqrt{{\displaystyle \frac{1}{\mathrm{n}}}{\displaystyle {\sum }_{i=1}^n{(y_i-{\widehat{y}}_i)}^2}}$$

(19)

where n is the number of samples, $y_i$ is the true value, and ${\widehat{y}}_i$ is the predicted value.

Then, the total localization error of the unmanned vehicle is obtained as:

$${\mathrm{RMSE}}_{Total}=\sqrt{{\displaystyle \frac{1}{\mathrm{n}}}{\displaystyle {\sum }_{i=1}^n({(x_i-{\widehat{x}}_i)}^2+{(y_i-{\widehat{y}}_i)}^2)}}$$

(20)

where n is the number of samples, $x_i,y_i$ are the true values, and ${\widehat{x}}_i,{\widehat{y}}_i$ are the predicted values, respectively.

As it is depicted in Table 1, the feedback localization error based on odometry is the largest, and it is significantly higher than the pose errors based on ICP and the extended Kalman filter. After 10 simulation tests, the RMSE results of localization for the unmanned vehicle are obtained, and they are shown in Table 1.

Table 1. Results of the simulation experiment based on three methods.

Methods	X-Direction Error (cm)	Y-Direction Error (cm)	Total Localization Error (cm)
ICP	12.21	9.30	15.35
Odometry	21.24	21.52	30.24
Ours	10.71	8.92	13.94

Due to the high-noise characteristic of odometry, when designing the noise measurement matrix, the covariance corresponding to odometry is significantly larger than the localization covariance based on ICP, which makes the final motion route estimated based on EKF closer to the estimated route based on ICP. The comparative results of localization errors for an unmanned vehicle based on three methods are shown in Figure 9.

Figure 9. Comparative results of localization errors for an unmanned vehicle based on three methods.

5. Conclusions

This paper fully considers the measurement errors of sensors in real environments, adds appropriate process noise and measurement noise, and quantitatively evaluates the optimization effects of various localization algorithms through Root Mean Squared Error under multiple simulation experiment comparisons. The experimental results show that the localization effect based on the EKF framework is significantly better than that of single-sensor measurement. Compared with the matching effect based on ICP, it effectively reduces the localization Root Mean Squared Error by 1 cm, and when there is an obvious sudden error in the ICP matching results, this algorithm can also effectively reduce the impact of sudden errors on the localization estimation of the unmanned vehicle. At the same time, the localization accuracy of the unmanned vehicle based on the extended Kalman filter is improved by 9.21% and 53.91%, respectively, compared with the localization results based on the ICP algorithm and the feedback pose based on the odometry. This effectively reduces the noise error of LiDAR scanning and improves the mobile operation effect of the unmanned vehicle in large-scale outdoor environments. In future work, we will explore the optimization algorithms to systematically tune the Q and R parameters of the Kalman filter and adopt a deep learning algorithm to improve the matching efficiency and localization accuracy of the unmanned vehicle. Also, we will establish a systematic fault injection experimental platform to quantitatively analyze the fault tolerance boundaries for concurrent multi-sensor failures under electromagnetic interferences [29]. Furthermore, we will design and construct a physical unmanned vehicle platform integrating LiDAR and odometry and validate the proposed algorithm in a controlled outdoor condition.