A vehicle positioning method based on environment perception assisted particle filtering

Abstract

The present invention discloses a vehicle positioning method based on environmental perception-assisted particle filtering. First, laser point cloud data is acquired and preprocessed. Then, a rasterized map of the surrounding environment is constructed to show whether there are non-ground targets around the vehicle. Next, a positioning vehicle system state model and observation model are constructed. Finally, the rasterized map is used to assist in particle weight updates, reducing the weights of particles in non-ground areas to improve the particle filtering algorithm and iteratively update the vehicle's position. The method proposed in the present invention uses a laser radar to obtain real-world environmental information, introduces a point cloud rasterized two-dimensional map to optimize the particle weight formula in the traditional particle filtering algorithm, assists in particle weight updates, improves particle degradation problems, and improves vehicle positioning accuracy.

Description

Technical Field

[0001] The present invention relates to a vehicle positioning method, in particular to a vehicle positioning method based on environment perception assisted particle filtering, and belongs to the technical field of intelligent driving vehicle positioning. Background Art

[0002] Currently, the sensors used for positioning technology in smart vehicles primarily include the Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), LiDAR, wheel speedometers, and cameras. Each of these sensors has its own strengths, and a single sensor cannot adapt to complex real-world environments, such as changing weather and seasons, and obstructions from roads and surrounding buildings. Different sensors have inherent weaknesses that need to be compensated by others. Combining multiple sensors can complement each other's strengths, necessitating the use of fusion positioning technology and multi-sensor fusion algorithms. Common multi-sensor fusion algorithms include weighted averaging, DS evidence reasoning, filtering algorithms, and neural network-based algorithms. Filtering fusion algorithms are becoming increasingly popular.

[0003] With the increasing complexity of vehicle motion models and the increasing performance requirements for fusion algorithms, some traditional linear filtering methods such as the standard Kalman filter (KF) can no longer meet the requirements in some applications. Methods such as the extended Kalman filter (EKF) and the unscented Kalman filter (UKF) that approximate nonlinear systems through local linearization have higher accuracy than the standard Kalman filter (KF), but they still have limitations for strongly nonlinear and strongly non-Gaussian systems. In contrast, particle filters based on sequential Monte Carlo methods and recursive Bayesian estimation have excellent characteristics for more complex nonlinear models and non-Gaussian dynamic systems. However, due to the mismatch between their proposed distribution and the true distribution, as the number of filter iterations increases, the weights of most particles will become very small or even close to 0, and only a few particles have large weights, resulting in inevitable particle degradation. The present invention proposes a vehicle positioning method based on environment perception assisted particle filtering. By using laser radar to obtain real environment information, the generated point cloud rasterized two-dimensional map is introduced to optimize the particle weight formula, and the particle weight update is assisted to improve the particle degradation problem, which can improve the vehicle positioning accuracy to a certain extent. Summary of the Invention

[0004] The purpose of this paper is to propose a vehicle positioning method based on environmental perception-assisted particle filtering. First, laser point cloud data is acquired and preprocessed. Then, a point cloud raster map is constructed based on the preprocessed point cloud data. Second, a positioning vehicle system state model and observation model are constructed. Finally, an improved particle filtering algorithm is used to filter and estimate the motion state of the positioning vehicle. The specific steps include:

[0005] Step 1: Obtain laser point cloud data and preprocess it

[0006] A 16-line laser radar is used as the data source for the point cloud. It is installed on the roof and aligned with the center. After reading the point cloud data, it is pre-processed. The pre-processing process is as follows:

[0007] Sub-step 1: Crop the point cloud within the specified range

[0008] In the vehicle coordinate system, x represents the longitudinal direction of the vehicle, y represents the lateral direction of the vehicle, and the direction of the xy coordinate system pointing to the sky is defined as the z-axis direction, which conforms to the right-hand screw rule. To highlight the environment around the vehicle equipped with the lidar sensor, a region of interest (ROI) is set with the vehicle equipped with the lidar sensor as the center. The unit is meter, and the point cloud within the specified range is cropped. The range setting is: x∈[-20,20], y∈[-20,20];

[0009] Sub-step 2: Segmenting the ground plane and nearby obstacles

[0010] (1) Extract the ground plane and remove the ground plane points. Use the RANSAC (random sampling consensus) algorithm to detect and match the ground plane. The specific process is as follows:

[0011] a.1: Randomly select three points (x0, y0, z0) in the original point cloud data and calculate their plane equation Ax0+By0+Cz0+D=0;

[0012] a.2: Calculate all points (x i ,y i ,z i ), the algebraic distance d to the plane obtained in process a.1 i =|Ax i +By i +Cz i +D|, where i=1,2,…,N, N is the length of the point cloud data, and the threshold d is selected. max = 0.2 m, if d i ≤d max , are considered as inliers, that is, all inliers are considered to be within 0.2 meters of the ground plane. Otherwise, they are considered as outliers, and the normal direction of the ground plane is set to be upward along the z-axis, that is, the normal vector is [0,0,1]. Considering that the actual road may have a small slope, the maximum angular distance is set to 5 degrees, and the number of inliers that meet the conditions is recorded;

[0013] a.3: Repeat the above steps and select the best fitting parameters, that is, the model corresponding to the plane with the largest number of inliers;

[0014] a.4: Iteration ends and the ground plane model coefficients [A, B, C, D] are output;

[0015] (2) Retrieve points within a certain radius and mark them as obstacles

[0016] First, place the lidar sensor at the center of the point cloud map coordinate system, set a radius of 20 meters based on the cropping range, and search for adjacent points within this radius;

[0017] Secondly, mark the obstacle points, use the Euclidean clustering algorithm to segment the obstacles and use color labels to mark the obstacle points;

[0018] Finally, all the point clouds that have been marked and removed from the ground plane are projected onto the ground plane matched using the RANSAC algorithm to obtain all the point clouds (x i ,y i ,z i )Point cloud data after projection on the ground plane (x p ,y p ,z p ),in:

[0019]

[0020] Step 2: Build a point cloud raster map based on the preprocessed point cloud data

[0021] Based on the projected point cloud data obtained in step 1, rasterization processing is performed. The specific steps are as follows:

[0022] Sub-step 1: Determine extreme values and set grid size

[0023] For a given point cloud data P i (i=1,2,…,N), calculate the maximum and minimum values x of its data points in the coordinate axis direction max , x min ,y max ,y min , and set the grid size to 0.5*0.5, that is, the grid side length along the x-axis is size_x=0.5, and the side length along the y-axis is size_y=0.5;

[0024] Sub-step 2: Divide the number of grids

[0025] Divide the grid into m grids according to the given grid side length, where:

[0026]

[0027] Sub-step 3: Storing point cloud data into a raster array

[0028] (1) Storing the divided grid information in a two-dimensional array;

[0029] (2) Hierarchical search for point cloud data storage point cloud data, the specific process is as follows:

[0030] b.1: Search the point cloud along the x-axis to determine the row position of the grid where the point cloud data is located;

[0031] b.2: Search the point cloud along the y-axis for the point cloud in the same row of the grid to determine the grid column position of the point cloud data;

[0032] b.3: Repeat the above steps until all point cloud data are searched;

[0033] b.4: Store all point cloud data into the grid array corresponding to the corresponding grid according to the search results;

[0034] Sub-step 4: Build a point cloud grid map based on the stored result information of sub-step 3

[0035] Step 3: Build the positioning vehicle system state model and observation model

[0036] The positioning vehicle is equipped with a high-precision satellite navigation system receiver to achieve three-dimensional positioning of the vehicle, and the state equation and observation equation of the positioning vehicle system are established based on the motion characteristics of the vehicle during driving;

[0037] Sub-step 1: Establishing a positioning vehicle state estimation model

[0038] After Δt from time k-1 to time k, the position motion state of the vehicle is expressed as

[0039]

[0040] In the formula λ,h represent the latitude, longitude and altitude of the positioning vehicle respectively, v E ,v N ,v U Respectively represent the eastward speed, northward speed, and vertical speed of the positioning vehicle, Represents the differential symbol, R M and R N They represent the radius of the earth's meridian and the radius of curvature of the zodiac circle respectively;

[0041] Sub-step 2: Establish the system state equation

[0042] First, based on formula (3), the vehicle state estimation model is established and the state vector is selected Where, the superscript T represents the matrix transpose;

[0043] Secondly, establish the positioning vehicle state equation model:

[0044] X=f(X,U,W,γ)(4)

[0045] Where f(·) is the nonlinear state transfer equation, X is the system state vector, U is the system external input vector and U=[v E ,v N ,v U ] T , where v E ,v N ,v U Both can be obtained from the satellite navigation system, W is the system noise matrix, γ is the input noise matrix;

[0046] Assuming the sampling period is T, the system state equation at time k is obtained by discretizing formula (4):

[0047] X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)}(5)

[0048] Where, the system process noise is expressed as W = [w1,w2,w3] T , the Gaussian white noise covariance matrix corresponding to W(k-1) is Input noise And the noise component is hidden in the three external inputs of the system, and the state transfer equation f(·) is expressed as:

[0049]

[0050] in,

[0051]

[0052] f3{X(k-1),U(k-1),W(k-1),γ(k-1)}=h(k-1)+Tv U (k)

[0053] Sub-step 3: Establishing the system observation equation

[0054] The system observation equation is expressed as:

[0055] Z=h(X)+V(7)

[0056] In the formula, the system observation vector in λ g ,h G They represent the latitude, longitude and altitude information obtained by the satellite navigation system, H is the system observation matrix, V is the system observation noise and is uncorrelated with W;

[0057] Discretize formula (7) to obtain the system observation equation at time k:

[0058] Z(k)=h{X(k),V(k)}=h[X(k)]+V(k)(8)

[0059] Where, The measurement noise covariance matrix corresponding to the system observation noise V in and They are all determined by the statistical characteristics of the position measurement noise of the satellite navigation system;

[0060] Step 4: Improve the particle filter algorithm to filter and estimate the motion state variables of the positioning vehicle

[0061] Sub-step 1: Initialize particles

[0062] At time k, the particle set {(X j (k-1),η j (k-1))|j=1,…,N}, where η j (k-1)=1 / N,η j (k-1) represents the particle weight, N represents the number of particles, the covariance matrix is Φ, and the average value is set to the state estimate at the previous moment, that is:

[0063]

[0064] Where, represents the estimated value of the state at time k-1;

[0065] Sub-step 2: Prediction

[0066] When k>0, the nonlinear state transfer equation of formula (9) is used to update the particle swarm {(X j (k-1),η j (k-1))|j=1,…,N} to obtain a new particle swarm

[0067] Sub-step 3: Update particle weights and normalize based on the point cloud raster map

[0068] (1) The weight of each particle is expressed as

[0069]

[0070] Where, Represents the observation likelihood function. Since the particle sample conforms to the Gaussian distribution, It can be expressed as:

[0071]

[0072] Where m represents the dimension of the observation vector, |R| represents the determinant of the measurement noise covariance matrix;

[0073] (2) Introduce the point cloud grid map to assist in updating the weights of some particles. The specific process is as follows:

[0074] c.1: Calculate the coordinate value of each latitude and longitude particle prediction value in the vehicle body coordinate system at the current moment

[0075] c.2: Combined with rasterization process calculation The index value (d, l) that falls on the divided grid;

[0076] c.3: Determine whether the grid array value A(b,l) corresponding to the current index is 0. If the corresponding array value is not 0, it means that the grid corresponding to the current index is occupied by objects around the vehicle, that is, the corresponding particle falls on an object outside the actual position of the vehicle. Assign a value of 0;

[0077] c.4: Repeat the above steps until all particles are judged;

[0078] (3) Based on the current observation value, combine formula (10) and process c.1 to c.4 to update the weight values of all particles and perform normalization to obtain new weights. Right now:

[0079] Select the prior probability distribution as the importance density function, that is After introducing the point cloud grid map, the weight formula is simplified to

[0080]

[0081] Where d = 0, 1, ..., m-1, l = 0, 1, ..., m-1, m is the number of grid cells, and A(b, l) represents the grid array value in the bth row and lth column.

[0082] (4) Perform weight normalization processing, that is,

[0083] Sub-step 4: State Estimation

[0084] Using the obtained weighted particles To approximate the posterior probability density p(X(k)|Z(k)), and then obtain the state estimate at time k, that is,

[0085]

[0086] Sub-step 5: Resampling

[0087] According to the size of the particle weight value, the particle set Resample to obtain a new particle swarm {(X j (k),η j (k))|i=1,...,N}, where, η j (k) = 1 / N, and return to repeat (1) for the next iteration;

[0088] After the recursive calculation based on the environment perception assisted particle filter composed of the above formulas (10), (11) and (12), the output state estimation To locate the vehicle's latitude, longitude, and altitude status information;

[0089] In summary, when combining LiDAR to obtain real-world environmental information and introducing the generated point cloud rasterized 2D map to optimize the particle weight formula, the particle filter algorithm is used to assist in vehicle positioning. First, the laser point cloud data is acquired and preprocessed. Then, a point cloud rasterized map is constructed based on the preprocessed point cloud data from "Step 1." Second, a system state model and observation model for the positioning vehicle are constructed. Finally, an improved particle filter algorithm is used to filter and estimate the position and motion state of the positioning vehicle.

[0090] Advantages and significant effects of the present invention:

[0091] The vehicle positioning method based on environment perception assisted particle filtering proposed in this invention obtains the vehicle's surrounding environment information through laser radar, introduces point cloud raster map, and further optimizes the particle weights in combination with the real environment around the vehicle to assist in particle weight update, thereby improving the filtering accuracy of the particle filtering algorithm and improving the vehicle positioning accuracy to a certain extent. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] Figure 1 It is a schematic diagram of the technical route;

[0093] Figure 2 It is the RANSAC algorithm that detects and matches the ground flow chart;

[0094] Figure 3 It is the flow chart of the improved particle filter algorithm;

[0095] Figure 4 It is a flowchart for updating some particle weights by introducing point cloud grid maps. DETAILED DESCRIPTION

[0096] In intelligent driving technology, positioning enables intelligent vehicles to determine their precise location. In autonomous driving, positioning plays a crucial role in vehicle positioning. Currently, the sensors used in intelligent vehicles for positioning technology primarily include the Global Navigation Satellite System (GNSS), Inertial Measurement Unit (IMU), LiDAR (Light Detection and Ranging), wheel speedometers, and cameras. These sensors can meet specific needs, but due to the complexity of the real world, such as weather and seasonal variations, road conditions, and surrounding building conditions, a single sensor that meets specific requirements cannot meet practical needs. Furthermore, different sensors have inherent weaknesses that need to be addressed by other sensors. Consequently, fusion positioning technologies and multi-sensor fusion algorithms have emerged. Common multi-sensor fusion algorithms include weighted averaging, DS evidence reasoning, filtering algorithms, and neural network-based algorithms. Filtering fusion algorithms are becoming increasingly popular.

[0097] With the increasing complexity of vehicle motion models and the continuous improvement of filtering accuracy, some traditional nonlinear filtering methods such as standard Kalman filtering (KF) can no longer meet the requirements in some applications. Methods such as extended Kalman filtering (EKF) and unscented Kalman filtering (UKF) that approximate nonlinear systems through local linearization have higher accuracy than standard Kalman filtering (KF), but they still have limitations for strongly nonlinear and strongly non-Gaussian systems. In contrast, particle filtering based on sequential Monte Carlo method and recursive Bayesian estimation has excellent characteristics for more complex nonlinear models and non-Gaussian dynamic systems. However, due to the mismatch between its proposed distribution and the true distribution, as the number of filtering iterations increases, the weights of most particles will become very small or even close to 0, and only a few particles have large weights, resulting in inevitable particle degradation. The present invention proposes a vehicle positioning method based on environment perception assisted particle filtering. By using laser radar to obtain real environment information, the generated point cloud rasterized two-dimensional map is introduced to optimize the particle weight formula, assist in particle weight update, and improve the particle degradation problem, which can improve vehicle positioning accuracy to a certain extent.

[0098] The purpose of this invention is to propose a method for estimating vehicle motion parameters based on an improved particle filter using rasterized maps. First, laser point cloud data is acquired and preprocessed; then, a point cloud rasterized map is constructed based on the preprocessed point cloud data; secondly, a positioning vehicle system state model and an observation model are constructed; finally, an improved particle filter algorithm is used to filter and estimate the motion state of the positioning vehicle. Figure 1 As shown, the specific steps include:

[0099] Step 1: Obtain laser point cloud data and preprocess it

[0100] A 16-line laser radar is used as the data source for the point cloud. It is installed on the roof and aligned with the center. After reading the point cloud data, it is pre-processed. The pre-processing process is as follows:

[0101] Sub-step 1: Crop the point cloud within the specified range

[0102] In the vehicle coordinate system, x represents the longitudinal direction of the vehicle, y represents the lateral direction of the vehicle, and the direction of the xy coordinate system pointing to the sky is defined as the z-axis direction, which conforms to the right-hand screw rule. To highlight the environment around the vehicle equipped with the lidar sensor, a region of interest (ROI) is set with the vehicle equipped with the lidar sensor as the center. The unit is meter, and the point cloud within the specified range is cropped. The range setting is: x∈[-20,20], y∈[-20,20];

[0103] Sub-step 2: Segmenting the ground plane and nearby obstacles

[0104] (1) Extract the ground plane and remove the ground plane points, and use the RANSAC (random sampling consensus) algorithm to detect and match the ground plane. The specific process is as follows: Figure 2 As shown, the steps are as follows:

[0105] a.1: Randomly select three points (x0, y0, z0) in the original point cloud data and calculate their plane equation Ax0+By0+Cz0+D=0;

[0106] a.2: Calculate all points (x i ,y i ,z i ), the algebraic distance d to the plane obtained in process a.1 i =|Ax i +By i +Cz i +D|, where i=1,2,…,N, N is the length of the point cloud data, and the threshold d is selected. max = 0.2 m, if d i ≤d max , are considered as inliers, that is, all inliers are considered to be within 0.2 meters of the ground plane. Otherwise, they are considered as outliers, and the normal direction of the ground plane is set to be upward along the z-axis, that is, the normal vector is [0,0,1]. Considering that the actual road may have a small slope, the maximum angular distance is set to 5 degrees, and the number of inliers that meet the conditions is recorded;

[0107] a.3: Repeat the above steps and select the best fitting parameters, that is, the model corresponding to the plane with the largest number of inliers;

[0108] a.4: Iteration ends and the ground plane model coefficients [A, B, C, D] are output;

[0109] (2) Retrieve points within a certain radius and mark them as obstacles

[0110] First, place the lidar sensor at the center of the point cloud map coordinate system, set a radius of 20 meters based on the cropping range, and search for adjacent points within this radius;

[0111] Secondly, mark the obstacle points, use the Euclidean clustering algorithm to segment the obstacles and use color labels to mark the obstacle points;

[0112] Finally, all the point clouds that have been marked and removed from the ground plane are projected onto the ground plane matched using the RANSAC algorithm to obtain all the point clouds (x i ,y i ,z i )Point cloud data after projection on the ground plane (x p ,y p ,z p ),in:

[0113]

[0114] Step 2: Build a point cloud raster map based on the preprocessed point cloud data

[0115] Based on the projected point cloud data obtained in step 1, rasterization processing is performed. The specific steps are as follows:

[0116] Sub-step 1: Determine extreme values and set grid size

[0117] For a given point cloud data P i (i=1,2,…,N), calculate the maximum and minimum values x of its data points in the coordinate axis direction max , x min ,y max ,y min , and set the grid size to 0.5*0.5, that is, the grid side length along the x-axis is size_x=0.5, and the side length along the y-axis is size_y=0.5;

[0118] Sub-step 2: Divide the number of grids

[0119] Divide the grid into m grids according to the given grid side length, where:

[0120]

[0121] Sub-step 3: Storing point cloud data into a raster array

[0122] (1) Storing the divided grid information in a two-dimensional array;

[0123] (2) Hierarchical search for point cloud data storage point cloud data, the specific process is as follows:

[0124] b.1: Search the point cloud along the x-axis to determine the row position of the grid where the point cloud data is located;

[0125] b.2: Search the point cloud along the y-axis for the point cloud in the same row of the grid to determine the grid column position of the point cloud data;

[0126] b.3: Repeat the above steps until all point cloud data are searched;

[0127] b.4: Store all point cloud data into the grid array corresponding to the corresponding grid according to the search results;

[0128] Sub-step 4: Build a point cloud grid map based on the stored result information of sub-step 3

[0129] Step 3: Build the positioning vehicle system state model and observation model

[0130] The positioning vehicle is equipped with a high-precision satellite navigation system receiver to achieve three-dimensional positioning of the vehicle, and the state equation and observation equation of the positioning vehicle system are established based on the motion characteristics of the vehicle during driving;

[0131] Sub-step 1: Establishing a positioning vehicle state estimation model

[0132] After Δt from time k-1 to time k, the position motion state of the vehicle is expressed as

[0133]

[0134] In the formula λ,h represent the latitude, longitude and altitude of the positioning vehicle respectively, v E ,v N ,v U Respectively represent the eastward speed, northward speed, and vertical speed of the positioning vehicle, Represents the differential symbol, R M and R N They represent the radius of the earth's meridian and the radius of curvature of the zodiac circle respectively;

[0135] Sub-step 2: Establish the system state equation

[0136] First, based on formula (3), the vehicle state estimation model is established and the state vector is selected Where, the superscript T represents the matrix transpose;

[0137] Secondly, establish the positioning vehicle state equation model:

[0138] X=f(X,U,W,γ)(4)

[0139] Where f(·) is the nonlinear state transfer equation, X is the system state vector, U is the system external input vector and U=[v E ,v N ,v U ] T , where v E ,v N ,v U Both can be obtained from the satellite navigation system, W is the system noise matrix, γ is the input noise matrix;

[0140] Assuming the sampling period is T, the system state equation at time k is obtained by discretizing formula (4):

[0141] X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)}(5)

[0142] Where, the system process noise is expressed as W = [w1,w2,w3] T , the Gaussian white noise covariance matrix corresponding to W(k-1) is Input noise And the noise component is hidden in the three external inputs of the system, and the state transfer equation f(·) is expressed as:

[0143]

[0144] in,

[0145]

[0146] f3{X(k-1),U(k-1),W(k-1),γ(k-1)}=h(k-1)+Tv U (k)

[0147] Sub-step 3: Establishing the system observation equation

[0148] The system observation equation is expressed as:

[0149] Z=h(X)+V(7)

[0150] In the formula, the system observation vector in λ g ,h G They represent the latitude, longitude and altitude information obtained by the satellite navigation system, H is the system observation matrix, V is the system observation noise and is uncorrelated with W;

[0151] Discretize formula (7) to obtain the system observation equation at time k:

[0152] Z(k)=h{X(k),V(k)}=h[X(k)]+V(k)(8)

[0153] Where, The measurement noise covariance matrix corresponding to the system observation noise V in and They are all determined by the statistical characteristics of the position measurement noise of the satellite navigation system;

[0154] Step 4: Improve the particle filter algorithm to filter and estimate the motion state variables of the positioning vehicle

[0155] The improved particle filter algorithm process is as follows: Figure 3 As shown, the specific steps include:

[0156] Sub-step 1: Initialize particles

[0157] At time k, the particle set {(X j (k-1),η j (k-1))|j=1,…,N}, where η j (k-1)=1 / N,η j (k-1) represents the particle weight, N represents the number of particles, the covariance matrix is Φ, and the average value is set to the state estimate at the previous moment, that is:

[0158]

[0159] Where, represents the estimated value of the state at time k-1;

[0160] Sub-step 2: Prediction

[0161] When k>0, the nonlinear state transfer equation of formula (9) is used to update the particle swarm {(X j (k-1),η j (k-1))|j=1,…,N} to obtain a new particle swarm

[0162] Sub-step 3: Update particle weights and normalize based on the point cloud raster map

[0163] (1) The weight of each particle is expressed as

[0164]

[0165] Where, Represents the observation likelihood function. Since the particle sample conforms to the Gaussian distribution, It can be expressed as:

[0166]

[0167] Where m represents the dimension of the observation vector, |R| represents the determinant of the measurement noise covariance matrix;

[0168] (2) Introduce point cloud grid map to assist in updating some particle weights. The specific process is as follows Figure 4 As shown, the steps are as follows:

[0169] c.1: Calculate the coordinate value of each latitude and longitude particle prediction value in the vehicle body coordinate system at the current moment

[0170] c.2: Combined with rasterization process calculation The index value (d, l) that falls on the divided grid;

[0171] c.3: Determine whether the grid array value A(b,l) corresponding to the current index is 0. If the corresponding array value is not 0, it means that the grid corresponding to the current index is occupied by objects around the vehicle, that is, the corresponding particle falls on an object outside the actual position of the vehicle. Assign a value of 0;

[0172] c.4: Repeat the above steps until all particles are judged;

[0173] (3) Based on the current observation value, combine formula (10) and process c.1 to c.4 to update the weight values of all particles and perform normalization to obtain new weights. Right now:

[0174] Select the prior probability distribution as the importance density function, that is After introducing the point cloud grid map, the weight formula is simplified to

[0175]

[0176] Where d = 0, 1, ..., m-1, l = 0, 1, ..., m-1, m is the number of grid cells, and A(b, l) represents the grid array value in the bth row and lth column.

[0177] (4) Perform weight normalization processing, that is,

[0178] Sub-step 4: State Estimation

[0179] Using the obtained weighted particles To approximate the posterior probability density p(X(k)|Z(k)), and then obtain the state estimate at time k, that is,

[0180]

[0181] Sub-step 5: Resampling

[0182] According to the size of the particle weight value, the particle set Resample to obtain a new particle swarm {(X j (k),η j (k))|i=1,...,N}, where, η j (k) = 1 / N, and return to repeat (1) for the next iteration;

[0183] After the recursive calculation based on the environment perception assisted particle filter composed of the above formulas (10), (11) and (12), the output state estimation To locate the vehicle's latitude, longitude, and altitude status information;

[0184] In summary, when combining LiDAR to obtain real-world environmental information and introducing the generated point cloud rasterized 2D map to optimize the particle weight formula, the particle filter algorithm is used to assist in vehicle positioning. First, the laser point cloud data is acquired and preprocessed. Then, a point cloud rasterized map is constructed based on the preprocessed point cloud data from "Step 1." Second, a system state model and observation model for the positioning vehicle are constructed. Finally, an improved particle filter algorithm is used to filter and estimate the position and motion state of the positioning vehicle.

[0185] The vehicle positioning method based on environment perception assisted particle filtering proposed in the present invention introduces a point cloud raster map showing the real environment around the vehicle through lidar point cloud data, further optimizes the particle weights in combination with the real environment around the vehicle to assist in particle weight update, thereby improving the filtering accuracy of the particle filtering algorithm and, to a certain extent, improving the vehicle positioning accuracy, with significant advantages.

Claims

1. A vehicle positioning method based on environment perception assisted particle filtering, characterized by: First, the laser point cloud data is acquired and preprocessed; then, a point cloud raster map is constructed based on the preprocessed point cloud data; Secondly, the positioning vehicle system state model and observation model are constructed; finally, the improved particle filter algorithm is used to filter and estimate the motion state of the positioning vehicle; the specific steps include: Step 1: Obtain laser point cloud data and preprocess it A 16-line laser radar is used as the data source for the point cloud. It is installed on the roof and aligned with the center. After reading the point cloud data, it is pre-processed. The pre-processing process is as follows: Sub-step 1: Crop the point cloud within the specified range In the vehicle coordinate system, x represents the longitudinal direction of the vehicle, y represents the lateral direction of the vehicle, and the direction of the xy coordinate system pointing to the sky is defined as the z-axis direction, which conforms to the right-hand screw rule. To highlight the environment around the vehicle equipped with the lidar sensor, a region of interest (ROI) is set with the vehicle equipped with the lidar sensor as the center. The unit is meter, and the point cloud within the specified range is cropped. The range setting is: x∈[-20,20], y∈[-20,20]; Sub-step 2: Segmenting the ground plane and nearby obstacles (1) Extract the ground plane and remove the ground plane points. Use the RANSAC algorithm to detect and match the ground plane. The specific process is as follows: a.1: Randomly select three points (x0, y0, z0) in the original point cloud data and calculate their plane equation Ax0+By0+Cz0+D=0; a.2: Calculate all points (x i ,y i ,z i ), the algebraic distance d to the plane obtained in process a.1 i =|Ax i +By i +Cz i +D|, where i=1,2,…,N, N is the length of the point cloud data, and the threshold d is selected. max = 0.2 m, if d i ≤d max , it is considered to be a sample point inside the model, that is, an inlier, and all inliers are considered to be within 0.2 meters of the ground plane. Otherwise, it is considered to be a sample point outside the model, that is, an outlier, and the normal direction of the ground plane is set to be upward along the z-axis, that is, the normal vector is [0,0,1], and the maximum angular distance is set to 5 degrees. The number of inliers that meet the conditions is recorded; a.3: Repeat the above steps and select the best fitting parameters, that is, the model corresponding to the plane with the largest number of inliers; a.4: Iteration ends and the ground plane model coefficients [A, B, C, D] are output; (2) Retrieve points within a certain radius and mark them as obstacles First, place the lidar sensor at the center of the point cloud map coordinate system, set a radius of 20 meters based on the cropping range, and search for adjacent points within this radius; Secondly, mark the obstacle points, use the Euclidean clustering algorithm to segment the obstacles and use color labels to mark the obstacle points; Finally, all the point clouds that have been marked and removed from the ground plane are projected onto the ground plane matched using the RANSAC algorithm to obtain all the point clouds (x i ,y i ,z i )Point cloud data after projection on the ground plane (x p ,y p ,z p ),in: Step 2: Build a point cloud raster map based on the preprocessed point cloud data Based on the projected point cloud data obtained in step 1, rasterization processing is performed. The specific steps are as follows: Sub-step 1: Determine extreme values and set grid size For a given point cloud data P i (i=1,2,…,N), calculate the maximum and minimum values x of its data points in the coordinate axis direction max , x min ,y max ,y min , and set the grid size to 0.5*0.5, that is, the grid side length along the x-axis is size_x=0.5, and the side length along the y-axis is size_y=0.5; Sub-step 2: Divide the number of grids Divide the grid into m grids according to the given grid side length, where: Sub-step 3: Storing point cloud data into a raster array (1) Storing the divided grid information in a two-dimensional array; (2) Hierarchical search for point cloud data storage point cloud data, the specific process is as follows: b.1: Search the point cloud along the x-axis to determine the row position of the grid where the point cloud data is located; b.2: Search the point cloud along the y-axis for the point cloud in the same row of the grid to determine the grid column position of the point cloud data; b.3: Repeat the above steps until all point cloud data are searched; b.4: Store all point cloud data into the grid array corresponding to the corresponding grid according to the search results; Sub-step 4: Build a point cloud grid map based on the stored result information of sub-step 3 Step 3: Construct the positioning vehicle system state model and observation model; Step 4: Improve the particle filter algorithm to filter and estimate the motion state variables of the positioning vehicle.

2. The vehicle positioning method based on environment perception assisted particle filtering according to claim 1, characterized in that: The step three specifically includes: equipping the positioning vehicle with a high-precision satellite navigation system receiver to achieve three-dimensional positioning of the vehicle, and establishing a positioning vehicle system state equation and observation equation based on the vehicle's motion characteristics during driving; Sub-step 1: Establishing a positioning vehicle state estimation model After Δt from time k-1 to time k, the position motion state of the vehicle is expressed as In the formula λ,h represent the latitude, longitude and altitude of the positioning vehicle respectively, v E ,v N ,v U Respectively represent the eastward speed, northward speed, and vertical speed of the positioning vehicle, Represents the differential symbol, R M and R N They represent the radius of the earth's meridian and the radius of curvature of the zodiac circle respectively; Sub-step 2: Establish the system state equation First, based on formula (3), the vehicle state estimation model is established and the state vector is selected Where, the superscript T represents the matrix transpose; Secondly, establish the positioning vehicle state equation model: X=f(X,U,W,γ) (4) Where f(·) is the nonlinear state transfer equation, X is the system state vector, U is the system external input vector and U=[v E ,v N ,v U ] T , where v E ,v N ,v U Both can be obtained from the satellite navigation system, W is the system noise vector, γ is the input noise vector; Assuming the sampling period is T, the system state equation at time k is obtained by discretizing formula (4): X(k)=f{X(k-1),U(k-1),W(k-1),γ(k-1)}(5) Where, the system process noise vector is expressed as w = [w1, w2, w3] T , the Gaussian white noise covariance matrix corresponding to w(k-1) is Input noise vector γ=[γ v E ,γ v N ,γ v U ,] T , and the noise component is hidden in the three external inputs of the system, the state transfer equation f(·) is expressed as: in, Sub-step 3: Establishing the system observation equation The system observation equation is expressed as: Z=h(X)+V(7) In the formula, the system observation vector in λ g ,h G They represent the latitude, longitude and altitude information obtained by the satellite navigation system, h is the system observation matrix, V is the system observation noise and is uncorrelated with W; Discretize formula (7) to obtain the system observation equation at time k: Z(k)=h{X(k),V(k)}=h[X(k)]+V(k)(8) The measurement noise covariance matrix corresponding to the system observation noise V in and They are all determined by the statistical characteristics of the position measurement noise of the satellite navigation system.

3. The vehicle positioning method based on environment perception assisted particle filtering according to claim 2, characterized in that: The step 4 is specifically as follows: Sub-step 1: Initialize particles At time k, the particle set {(X j (k-1),η j (k-1))|j=1,…,N}, where η j (k-1)=1 / N,η j (k-1) represents the particle weight, N represents the number of particles, the covariance matrix is Φ, and the average value is set to the state estimate at the previous moment, that is: Where, represents the estimated value of the state at time k-1; Sub-step 2: Prediction When k>0, the nonlinear state transfer equation of formula (9) is used to update the particle swarm {(X j (k-1),η j (k-1))|j=1,…,N} to obtain a new particle swarm Sub-step 3: Update particle weights and normalize based on the point cloud raster map (1) The weight of each particle is expressed as Where, Represents the observation likelihood function. Since the particle sample conforms to the Gaussian distribution, It can be expressed as: Where m represents the dimension of the observation vector, |R| represents the determinant of the measurement noise covariance matrix; (2) Introduce the point cloud grid map to assist in updating the weights of some particles. The specific process is as follows: c.1: Calculate the coordinate value of each latitude and longitude particle prediction value in the vehicle body coordinate system at the current moment c.2: Combined with rasterization process calculation The index value (d, l) that falls on the divided grid; c.3: Determine whether the grid array value A(d,l) corresponding to the current index is 0. If the corresponding array value is not 0, it means that the grid corresponding to the current index is occupied by objects around the vehicle, that is, the corresponding particle falls on an object outside the actual position of the vehicle. Assign a value of 0; c.4: Repeat the above steps until all particles are judged; (3) Based on the current observation value, combine formula (10) and process c.1 to c.4 to update the weight values of all particles and perform normalization to obtain new weights. Right now: Select the prior probability distribution as the importance density function, that is After introducing the point cloud grid map, the weight formula is simplified to Where d = 0, 1, ..., m-1, l = 0, 1, ..., m-1, m is the number of grids, and A(d, l) represents the grid array value in the dth row and lth column. (4) Perform weight normalization processing, that is, Sub-step 4: State Estimation Using the obtained weighted particles To approximate the posterior probability density p(X(k)|Z(k)), and then obtain the state estimate at time k, that is, Sub-step 5: Resampling According to the size of the particle weight value, the particle set Resample to obtain a new particle swarm {(X j (k),η j (k))|j=1,…,N}, where η j (k) = 1 / N, and return to repeat sub-step 1 of step 4 for the next iteration; After the recursive calculation based on the environment perception assisted particle filter composed of the above formulas (10), (11) and (12), the output state estimation To locate the vehicle's latitude, longitude, and altitude status information; In summary, when combining laser radar to obtain real-world environmental information and introducing the generated point cloud rasterized 2D map to optimize the particle weight formula to assist the particle filter algorithm in vehicle positioning, the following steps are performed: first, laser point cloud data is acquired and preprocessed; then, a point cloud rasterized map is constructed based on the preprocessed point cloud data from "step 1"; second, a system state model and observation model for the positioning vehicle are constructed; finally, an improved particle filter algorithm is used to filter and estimate the position and motion state of the positioning vehicle.

Images