A vehicle positioning estimation method, system, storage medium and vehicle

Abstract

The application discloses a vehicle positioning estimation method, which comprises the following steps: obtaining an initial pose satisfying a switching condition based on a particle filter algorithm; fusing the initial pose of the vehicle satisfying the switching condition, laser point cloud data and wheel odometer data based on an unscented Kalman filter algorithm to estimate the pose of the vehicle and obtain vehicle pose estimation data; calculating the variance of the current pose state estimation to determine whether the current pose is reliable; if the current pose is reliable, updating the current pose as the current pose estimation data and estimating the pose state of the next state of the vehicle based on the unscented Kalman filter; and if the current pose is not reliable, estimating the pose state of the next state of the vehicle based on the particle filter algorithm. The application further discloses a corresponding system, a storage medium and a vehicle. When the positioning estimation is performed, the application can break the dependence on the third source input of the initial pose of the vehicle, and improve the positioning accuracy and stability of the vehicle in a complex nonlinear scene.

Description

Technical Field

[0001] This invention relates to the field of intelligent driving technology for vehicles, and in particular to a vehicle positioning estimation method, system, storage medium, and vehicle. Background Technology

[0002] As intelligent driving technology advances into parking functions across various complex and ever-changing scenarios, it places higher demands on the accuracy and robustness of vehicle positioning.

[0003] Accurate vehicle positioning can be achieved through various technical approaches depending on the sensors used. One such approach involves sensor fusion using wheeled odometers and lidar. Current technologies in this area typically employ Kalman filtering and its derivatives (unscented Kalman filtering, extended Kalman filtering) within a Bayesian filtering framework as the fusion processor, while using Iterative Closest Point (ICP) or Normal Distributions Transform (NDT) algorithms for localization. However, these algorithms suffer from limitations such as high reliance on initial position and low accuracy and robustness in complex scenarios. Specifically, the vehicle's initial pose (position and heading angle) requires input from a third source, preventing the sensor-based approach from achieving self-loop closure. The accuracy of existing solutions is highly dependent on the richness of sensor information. For instance, in solutions relying solely on wheeled odometers and lidar, accuracy is maintained only in scenarios with abundant point cloud features. In general environments, especially those lacking point cloud features such as smooth corridors or tunnels, the stability and accuracy of the positioning results are significantly affected.

[0004] Furthermore, in existing technologies, laser point cloud matching mostly utilizes a single feature as input to the ICP algorithm, with the feature's weight typically set to 1 or to be a simple linear function of distance. However, in practical applications, especially in highly nonlinear vehicle driving or parking environments, current technologies suffer from low accuracy and weak robustness. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a vehicle positioning estimation method, system, storage medium and vehicle, which can get rid of the dependence on the third source input of the vehicle initial pose when performing positioning estimation, thereby improving the positioning accuracy and stability of the vehicle in complex nonlinear scenarios.

[0006] To address the aforementioned technical problems, as one aspect of the present invention, a vehicle positioning estimation method is provided, which includes at least the following steps:

[0007] The initial pose that satisfies the switching conditions is obtained based on the particle filter algorithm;

[0008] The vehicle pose is estimated by fusing the initial vehicle pose, laser point cloud data and wheel odometer data that meet the switching conditions using the unscented Kalman filter algorithm, thus obtaining the vehicle pose estimation data for this current time.

[0009] Calculate the variance of the current pose state estimation to determine whether the current pose is reliable. If reliable, update the current pose to the current pose estimation data and estimate the pose state of the vehicle in the next state based on unscented Kalman filtering. If unreliable, estimate the pose state of the vehicle in the next state based on the particle filtering algorithm.

[0010] The initial pose that satisfies the switching conditions is obtained based on the particle filter algorithm, including:

[0011] The initial pose of the vehicle is continuously obtained based on the particle filter algorithm until the weight of a single particle is greater than a predetermined weight threshold, where the weight of a particle represents the degree of matching between the particle and the observation data.

[0012] The pose of a single particle whose weight is greater than a predetermined weight threshold is used as the initial pose of the vehicle that satisfies the switching conditions.

[0013] Specifically, the vehicle pose is estimated by fusing the initial vehicle pose, laser point cloud data, and wheel odometer data that meet the switching conditions using an unscented Kalman filter algorithm, resulting in the current vehicle pose estimation data, including:

[0014] Extract the laser point cloud density and point cloud orientation features from the current laser point cloud data;

[0015] Filter the number of point clouds based on the matching range;

[0016] Based on the direction, density, and nearest point of the point cloud, the detected points in the laser point cloud are matched with the points in the matching range of the point cloud map to form at least one matching pair. Each matching pair is assigned a weight using a weighting function based on M-Estimator. The matching range of the point cloud map is determined by wheel odometer data.

[0017] Using a pre-established error function, the gradient descent method is used to iteratively solve for the minimum value of the error function and obtain the transformation matrix corresponding to the minimum value; wherein, the error function contains the deviations of three features of the point cloud: direction, density, and nearest point distance;

[0018] The convergence degree of the transformation matrix is ​​detected, and the iterative processing ends when all values ​​of the transformation matrix have converged to a predetermined convergence threshold.

[0019] The transformation matrix is ​​multiplied by the vector of the vehicle's initial pose at the current moment to obtain the latest estimated pose data for this pose state estimation, and then output.

[0020] The weighting function is as follows: ;

[0021] The error function is: ;

[0022] Where e is the deviation, w is the weight, and k is an adjustable engineering parameter; m is the number of matching points, and n is the number of features; when n is 3, the features represent the nearest point distance, point cloud density, and point cloud direction, respectively; corresponding to the three features, e represents the deviation of the orientation angle, the deviation of the density, and the deviation of the nearest point distance, respectively.

[0023] The step of calculating the variance of the current pose state estimation and determining whether the current pose is reliable includes:

[0024] Calculate the variance of the pose estimation data for this experiment;

[0025] The variance is compared with a variance threshold.

[0026] If the variance is less than the variance threshold, the pose estimation is considered reliable; otherwise, the pose estimation is considered unreliable.

[0027] As another aspect of the present invention, a vehicle positioning estimation system is also provided, comprising at least:

[0028] The particle filter processing unit is used to obtain the initial pose that meets the switching conditions based on the particle filter algorithm.

[0029] The unscented Kalman filter processing unit is used to estimate the vehicle pose by fusing the initial vehicle pose, laser point cloud data and wheel odometer data that meet the switching conditions based on the unscented Kalman filter algorithm, and obtain the vehicle pose estimation data for this time.

[0030] The filtering and switching processing unit is used to calculate the variance of the current pose state estimation and determine whether the current pose is reliable. If reliable, the current pose is updated with the current pose estimation data, and the pose state of the vehicle in the next state is estimated based on the unscented Kalman filter. If unreliable, the pose state of the vehicle in the next state is estimated based on the particle filter algorithm.

[0031] The particle filtering processing unit includes:

[0032] The continuous processing unit is used to continuously obtain the initial pose of the vehicle based on the particle filter algorithm until the weight of a single particle is greater than a predetermined weight threshold, where the weight of the particle represents the degree of matching between the particle and the observation data.

[0033] A specific initial pose determination unit is used to determine the pose of a single particle whose weight is greater than a predetermined weight threshold as the initial pose of a vehicle that meets the switching conditions.

[0034] The unscented Kalman filter processing unit includes:

[0035] A multi-dimensional feature extraction unit is used to extract the laser point cloud density and point cloud orientation features from the current laser point cloud data.

[0036] A filtering unit is used to filter the number of point clouds according to the matching range;

[0037] The matching and weighting processing unit is used to match the points detected in the laser point cloud with the points in the matching range of the point cloud map according to the direction, density and nearest point of the point cloud, to form at least one matching pair, and to assign weights to each matching pair using a weighting function based on M-Estimator. The matching range of the point cloud map is determined by wheel odometer data.

[0038] The iterative solution unit is used to iteratively solve for the minimum value of the pre-established error function using the gradient descent method, and obtain the transformation matrix corresponding to the minimum value; wherein, the error function includes the deviations of three features of the point cloud: direction, density, and nearest point distance;

[0039] The convergence termination detection unit is used to detect the degree of convergence of the transformation matrix. When the values ​​of each item of the transformation matrix have converged to a predetermined convergence threshold, the iterative processing process ends.

[0040] The estimation unit is used to multiply the transformation matrix by the vector of the vehicle's initial pose at the current time to obtain the latest estimated pose data for the current pose state estimation, and then output it.

[0041] The weighting function is as follows: ;

[0042] The error function is: ;

[0043] Where e is the deviation, w is the weight, and k is an adjustable engineering parameter; m is the number of matching points, and n is the number of features; when n is 3, the features represent the nearest point distance, point cloud density, and point cloud direction, respectively; corresponding to the three features, e represents the deviation of the orientation angle, the deviation of the density, and the deviation of the nearest point distance, respectively.

[0044] The filtering switching processing unit includes:

[0045] The credibility judgment unit is used to calculate the variance of the current pose estimation data, compare the variance with a variance threshold, and if the comparison result is that the variance is less than the variance threshold, the current pose estimation is considered credible; otherwise, the current pose estimation is considered unreliable.

[0046] The switching unit is used to continue using unscented Kalman filtering to estimate the pose state of the vehicle in the next state when the judgment result is reliable; when the judgment result is unreliable, it switches to the particle filtering algorithm and updates the initial pose of the vehicle.

[0047] Accordingly, in another aspect, the present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method as described above.

[0048] Accordingly, in another aspect, the present invention also provides a chip including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above.

[0049] Accordingly, another aspect of the present invention provides a vehicle having the system as described above deployed thereon.

[0050] Accordingly, in another aspect, the present invention also provides a computer program product, including computer instructions that instruct a computer device to perform operations corresponding to the methods described above.

[0051] Implementing the embodiments of the present invention has the following beneficial effects:

[0052] This invention provides a vehicle positioning estimation method, system, storage medium, and vehicle. By utilizing two filtering methods, particle filter (PF) and unscented Kalman filter (UKF), in the fusion positioning scheme of wheel sensors and lidar, the dependence on a third source input for vehicle initial pose is eliminated, thereby reducing the need for sensor diversity and complexity and improving the positioning accuracy and stability of the vehicle in complex nonlinear scenarios.

[0053] In an embodiment of the present invention, when the vehicle encounters a special state during the localization estimation process using unscented Kalman filtering, such as being moved by an uncontrollable external force from a third party, having extremely simple environmental point cloud features, or experiencing a sudden sensor failure for a period of time, it can quickly switch back to the particle filtering process and quickly restore its own localization.

[0054] When performing unscented Kalman filtering, multi-feature (nearest point distance, point cloud density, point cloud orientation) matching is used, and each matching pair is assigned a weight value based on robust regression analysis M-Estimator. This can greatly suppress the influence of outliers and noise points on pose estimation, thereby eliminating the need for rich sensor information sources, improving the accuracy of laser point cloud and map matching, reducing the impact of environmental noise on positioning, and thus enabling vehicle self-positioning to be completed using only wheel sensors and LiDAR, with high accuracy and strong robustness. Attached Figure Description

[0055] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, obtaining other drawings based on these drawings without creative effort still falls within the scope of the present invention.

[0056] Figure 1 This is a schematic diagram of the main flow of an embodiment of a vehicle positioning estimation method provided by the present invention;

[0057] Figure 2 yes Figure 1 A more detailed flowchart of step S10;

[0058] Figure 3 yes Figure 1 A more detailed flowchart of step S11;

[0059] Figure 4 This is a schematic diagram of a weighting function used in this invention;

[0060] Figure 5 This is a schematic diagram of an embodiment of a vehicle positioning estimation system provided by the present invention;

[0061] Figure 6 yes Figure 5 A schematic diagram of the structure of the particle filter processing unit;

[0062] Figure 7 for Figure 5 A schematic diagram of the structure of the unscented Kalman filter processing unit;

[0063] Figure 8 for Figure 5 A schematic diagram of the filter switching processing unit. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings.

[0065] like Figure 1 The diagram shown illustrates the main flowchart of an embodiment of a vehicle positioning estimation method provided by the present invention. (In conjunction with...) Figures 2 to 4 As shown, in this embodiment, the vehicle positioning estimation method includes at least the following steps:

[0066] Step S10: Based on the current point cloud map of the vehicle, obtain the initial pose that meets the switching conditions using the particle filter algorithm (PF).

[0067] Step S11: Based on the unscented Kalman filter (UKF) algorithm, the vehicle's initial pose, laser point cloud data and wheel odometer data that meet the switching conditions are fused to estimate the vehicle's pose and obtain the vehicle's current pose estimation data.

[0068] Step S12: Calculate the variance of the current pose state estimation to determine whether the current pose is reliable. If reliable, update the current pose to the current pose estimation data and estimate the pose state of the vehicle in the next state based on unscented Kalman filtering. If unreliable, estimate the pose state of the vehicle in the next state based on the particle filtering algorithm.

[0069] Understandably, the UKF algorithm has a shorter execution time than the PF algorithm. Under normal conditions where environmental nonlinearity is not extreme, this filtering algorithm can fuse data from laser odometry (vehicle pose estimated by matching laser point cloud with map) and wheel odometry to estimate the vehicle's own pose.

[0070] More specifically, such as Figure 2 As shown, in a specific example, step S10, obtaining the initial pose that satisfies the switching conditions based on the particle filter algorithm, includes:

[0071] The initial pose of the vehicle is continuously obtained based on the particle filter algorithm until the weight of a single particle is greater than a predetermined weight threshold, where the weight of a particle represents the degree of matching between the particle and the observation data.

[0072] The pose of a single particle whose weight is greater than a predetermined weight threshold is used as the initial pose of the vehicle that satisfies the switching conditions.

[0073] In a specific example, the following steps are included:

[0074] Step S100: A predetermined number of particles are randomly and discretely deployed into the point cloud map. Each particle represents a possible position and attitude of the vehicle. Initially, the weight of all particles is set to 1. Position refers to the spatial coordinates of the particle in the coordinate system, while attitude describes the particle's orientation or direction. In vehicle localization, pose estimation provides specific position information of the vehicle, such as x and y coordinates and heading angle, as well as attitude information such as roll angle, pitch angle, and yaw angle. This data can comprehensively describe the vehicle's accurate position and orientation in the world coordinate system. In the embodiments of this invention, the particle represents the possible pose of the vehicle.

[0075] Step S101: Match the environmental information acquired by the laser sensor with the information in the map, select a certain range of map point clouds, and calculate the average distance between each particle and the map point cloud.

[0076] It's understandable that when selecting a certain area on the map, the vehicle's position can be considered, but it doesn't necessarily need to be completely related to the vehicle's position. The selection of this area is primarily to reflect information related to the vehicle's pose on the map; therefore, it only needs to cover map information relevant to vehicle pose estimation. In some cases, this area can be selected based on a feature point (such as a road sign or traffic light) in the vehicle's current laser point cloud data.

[0077] More specifically, the choice can be determined based on the specific scenario and requirements. For example, if the map point cloud in the scene is densely distributed, a relatively small range can be selected to ensure that the map point cloud within that range contains sufficient vehicle pose information. If the map point cloud is sparsely distributed, a relatively large range can be selected to ensure that the PF algorithm can obtain relatively accurate vehicle pose estimation results even with insufficient map information.

[0078] Step S102: Calculate and update the weight of each particle based on the average value, where the closer the particle is, the higher its weight. The weight represents the probability that the vehicle is located at that position. If a particle (i.e., the vehicle's location) is closer to these features, its weight increases, meaning that this location is more likely to be the vehicle's actual location. Conversely, if a particle is farther away from these features, its weight decreases.

[0079] In practical applications, there are many ways to calculate and update the weight of each particle based on the average value. For example, in one case, a Gaussian function can be used to calculate the distance between the particle and the map point cloud, and the weight calculation formula could be as follows:

[0080] Weight = exp(-(distance^2) / (2) σ^2))

[0081] Here, σ is the width parameter of the Gaussian function, which determines the rate of weight decay. If σ is large, the weight decays slowly, and the particle's weight will be large; if σ is small, the weight decays quickly, and the particle's weight will be small.

[0082] Step S103: Resample according to the weight of the particles, retain the particles with higher weights and discard the particles with lower weights.

[0083] Step S104: Perform recursive iteration according to the aforementioned steps. When the weight of a single particle is greater than a predetermined weight threshold, use the pose of that single particle as the initial pose of the vehicle that satisfies the switching conditions. The weight threshold is obtained in advance through calibration.

[0084] Specifically, steps S101 to S104 are recursively iterated to converge the number of particles, continuously increasing the probability weight of a single particle. When the weight of a single particle exceeds a predetermined weight threshold (…), the probability weight is increased. If the initial position condition is considered satisfactory, the particle filter algorithm is terminated, and the task of estimating the vehicle pose state is handed over to the unscented Kalman filter (UKF) algorithm.

[0085] More specifically, such as Figure 3 As shown, in a specific example, step S11, based on the unscented Kalman filter algorithm, fuses the vehicle's initial pose, laser point cloud data, and wheel odometer data that meet the switching conditions to estimate the vehicle's pose, obtaining the vehicle's current pose estimation data, including:

[0086] Step S110: Extract the laser point cloud density and point cloud orientation features from the current laser point cloud data;

[0087] Step S111: Filter the number of point clouds based on the matching range, where the matching range is determined in conjunction with wheel odometer data. It is understood that, in order to reduce the amount of computation and improve efficiency, this step filters the number of point clouds based on a certain matching range, trying to retain point clouds related to the vehicle's position. For example, in some cases, if the current point is determined based on wheel odometer data and the matching range is set to within 10 meters of the current point, then only point clouds within this range will be retained, while other point clouds far away from the vehicle will be filtered out.

[0088] Step S112: Based on the direction, density and nearest point of the point cloud, match the detected points in the laser point cloud with the points on the point cloud map to form at least one matching pair, and assign weights to each matching pair using a weighting function based on M-Estimator.

[0089] The weighting function is: ;

[0090] Where e is the deviation, w is the weight, and k is an adjustable engineering parameter;

[0091] Understandably, this step matches points in the point cloud with points on the map based on the point cloud's orientation, density, and nearest-neighbor matching. These matches help determine the vehicle's location on the map. For example, if a point in the point cloud is very close to a building on the map and their orientations are the same, then these two points can be considered a matching pair.

[0092] Understandably, the M-Estimator is a robust regression analysis model that modifies the classical least squares regression objective function, which is highly sensitive to outliers, thereby reducing the impact of outliers and strong deviations. By assigning weights to each matching pair using the M-Estimator, the influence of noise and singularities can be reduced. The weighting function described above is merely an example; other forms are also possible and can be applied to the method of this invention.

[0093] Step S113: Using a pre-established error function, the minimum value of the error function is iteratively solved using the gradient descent method, and the transformation matrix corresponding to the minimum value is obtained; wherein, the error function includes the deviations of three major features of the point cloud: direction, density, and nearest point distance;

[0094] The error function is: ;

[0095] Where e is the deviation, w is the weight, and k is an adjustable engineering parameter; m is the number of matching points, and n is the number of features; when n is 3, the features represent the nearest point distance, point cloud density, and point cloud direction, respectively; corresponding to the three features, e represents the deviation of the orientation angle, the deviation of the density, and the deviation of the nearest point distance, respectively.

[0096] Understandably, the error function incorporates three key features of the point cloud: orientation, density, and nearest-neighbor distance, thereby increasing the matching factors and accuracy. The specific behavior of the error function can be found in [reference needed]. Figure 4 As shown, when the deviation When the value is large, w(e) decreases in a function-like manner, thereby greatly suppressing the influence of noise points on point cloud matching.

[0097] In this step, gradient descent is used to optimize the error function to obtain the minimum error value. The transformation matrix corresponding to this minimum value can be used to represent the vehicle's position on the map.

[0098] Step S114: Detect the convergence degree of the transformation matrix. When each value of the transformation matrix has converged to a predetermined convergence threshold, the iterative processing process ends; wherein, the convergence threshold is obtained by pre-setting.

[0099] Step S115: Multiply the transformation matrix by the vehicle pose vector at the current moment to obtain the latest estimated pose data for this pose state estimation, and output it. This step is used to check whether the algorithm has converged to a certain accuracy. If the values ​​of each term in the transformation matrix have converged to a certain threshold, then the algorithm can be considered to have reached the accuracy requirement and does not need to continue iterating.

[0100] In one example, if the current vehicle pose vector is [x, y, θ] and the transformation matrix is ​​[0.8, 0.2; 0.2, 0.8], then the new estimated pose vector is [0.8x + 0.2y, 0.2x + 0.8y, θ].

[0101] Understandably, in step S1, by combining laser point cloud matching and the unscented Kalman filter (UKF) algorithm, an efficient and accurate vehicle pose estimation method is provided. By extracting and utilizing features such as the density and orientation of the laser point cloud, and by optimizing the error function using the gradient descent method, this scheme can improve the accuracy and efficiency of vehicle pose estimation.

[0102] More specifically, in a concrete example, step S12, calculating the variance of the current pose state estimate and determining whether the current pose is reliable, includes:

[0103] Step S120: Calculate the variance of the pose estimation data for this time. It can be understood that the variance is calculated by the covariance matrix of the prediction stage and the update stage, and the variance represents the uncertainty between the predicted position and the actual position.

[0104] Step S121: Compare the variance with a variance threshold (e.g., 0.1 meters);

[0105] Step S122: If the comparison result shows that the variance is less than the variance threshold, the pose estimation is considered reliable; otherwise, if the comparison result shows that the variance is greater than or equal to the variance threshold, it indicates that the nonlinear factors of the current environment may be quite extreme, and the pose estimation is considered unreliable. The variance threshold is obtained in advance through calibration.

[0106] If the result is reliable, it indicates that the prediction of the Unscented Kalman Filter (UKF) algorithm is relatively reliable. In this case, the current pose is taken as the initial pose, and the Unscented Kalman Filter (UKF) algorithm is used for the next round of pose estimation.

[0107] If the results are unreliable, the predictions of the Unscented Kalman Filter (UKF) algorithm are considered unreliable. The algorithm then switches to the Particle Filter (PF) algorithm, randomly and discretely dropping weighted particles around the current estimated position, and restarting the Particle Filter (PF) prediction and update process. After obtaining the initial pose, the algorithm switches back to the Unscented Kalman Filter (UKF) algorithm.

[0108] like Figure 5 The diagram shown illustrates a structural schematic of an embodiment of a vehicle positioning estimation system provided by the present invention. (In conjunction with...) Figures 6 to 8 As shown, in this embodiment, the vehicle positioning estimation system includes at least:

[0109] The particle filter processing unit 10 is used to obtain the initial pose that meets the switching conditions based on the particle filter algorithm.

[0110] The unscented Kalman filter processing unit 11 is used to estimate the vehicle pose by fusing the vehicle's initial pose, laser point cloud data and wheel odometer data that meet the switching conditions based on the unscented Kalman filter algorithm, and to obtain the vehicle's current pose estimation data.

[0111] The filtering and switching processing unit 12 is used to calculate the variance of the current pose state estimation and determine whether the current pose is reliable. If it is reliable, the current pose is updated to the current pose estimation data, and the pose state of the vehicle in the next state is estimated based on the unscented Kalman filter. If it is not reliable, the pose state of the vehicle in the next state is estimated based on the particle filter algorithm.

[0112] More specifically, such as Figure 6 As shown, the particle filtering processing unit 10 includes:

[0113] The continuous processing unit 100 is used to continuously obtain the initial pose of the vehicle based on the particle filtering algorithm until the weight of a single particle is greater than a predetermined weight threshold, wherein the weight of the particle represents the degree of matching between the particle and the observation data.

[0114] Specifically, the following tasks need to be handled in the continuous processing unit 100:

[0115] A predetermined number of particles are randomly and discretely deployed in the point cloud map. Each particle represents the possible position and orientation of the vehicle, and the weight of all particles is initialized to 1.

[0116] The environmental information acquired by the laser sensor is matched with the information in the map, a certain range of map point cloud is selected, and the average distance between each particle and the map point cloud is calculated.

[0117] The weight of each particle is calculated and updated based on the average value, where the closer the particle is, the higher its weight.

[0118] Resampling is performed based on the particle weights, retaining particles with higher weights and discarding particles with lower weights.

[0119] Perform recursive iterative processing based on the aforementioned steps.

[0120] The specific initial pose determination unit 101 is used to take the pose of a single particle whose weight is greater than a predetermined weight threshold as the initial pose of the vehicle that meets the switching conditions.

[0121] More specifically, such as Figure 7 As shown, the unscented Kalman filter processing unit 11 includes:

[0122] The multidimensional feature extraction unit 110 is used to extract the laser point cloud density and point cloud orientation features from the current laser point cloud data.

[0123] Filtering unit 111 is used to filter the number of point clouds according to the matching range, wherein the matching range of the point cloud map is determined by wheel odometer data.

[0124] The matching and weighting processing unit 112 is used to match the detected points in the laser point cloud with the points in the point cloud map according to the direction, density and nearest point of the point cloud, to form at least one matching pair, and to assign weights to each matching pair using a weighting function based on M-Estimator.

[0125] The iterative solution unit 113 is used to iteratively solve for the minimum value of the error function by using a pre-established error function and the gradient descent method, and obtain the transformation matrix corresponding to the minimum value; wherein, the error function includes the deviation of three major features of the point cloud: direction, density, and nearest point distance;

[0126] The convergence end detection unit 114 is used to detect the convergence degree of the transformation matrix. When the values ​​of each item of the transformation matrix have converged to a predetermined convergence threshold, the iterative processing process ends.

[0127] The estimation unit 115 is used to multiply the transformation matrix by the pose vector of the vehicle pose at the current time to obtain the latest estimated pose data for the current pose state estimation, and output it.

[0128] The weighting function is as follows: ;

[0129] The error function is: ;

[0130] Where e is the deviation, w is the weight, and k is an adjustable engineering parameter; m is the number of matching points, and n is the number of features; when n is 3, the features represent the nearest point distance, point cloud density, and point cloud direction, respectively; corresponding to the three features, e represents the deviation of the orientation angle, the deviation of the density, and the deviation of the nearest point distance, respectively.

[0131] More specifically, such as Figure 8 As shown, the filter switching processing unit 12 includes:

[0132] The credibility judgment unit 120 is used to calculate the variance of the current pose estimation data, compare the variance with a variance threshold, and if the comparison result is that the variance is less than the variance threshold, the current pose estimation is considered credible; otherwise, the current pose estimation is considered unreliable.

[0133] The switching unit 121 is used to continue using unscented Kalman filtering to estimate the pose state of the vehicle in the next state when the judgment result is reliable; when the judgment result is unreliable, it switches to the particle filtering algorithm and updates the initial pose of the vehicle.

[0134] For more details, please refer to and combine with the above. Figures 1 to 4 The description of that will not be repeated here.

[0135] Accordingly, in another aspect, the present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the aforementioned... Figures 1 to 4 The steps of the described method. For more details, please refer to and combine with the foregoing descriptions. Figures 1 to 4 The description of that will not be repeated here.

[0136] Accordingly, in another aspect, the present invention also provides a chip including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the aforementioned... Figures 1 to 4 The steps of the described method. For more details, please refer to and combine with the foregoing descriptions. Figures 1 to 4 The description of that will not be repeated here.

[0137] Accordingly, in another aspect, the present invention also provides a vehicle having a configuration as described above. Figures 5 to 8 The system described. For more details, please refer to and combine with the foregoing descriptions. Figures 5 to 8 The description of that will not be repeated here.

[0138] Accordingly, in another aspect, the present invention also provides a computer program product, including computer instructions that instruct a computer device to perform the aforementioned actions. Figures 1 to 4 The operation corresponding to the described method.

[0139] Implementing the embodiments of the present invention has the following beneficial effects:

[0140] This invention provides a vehicle positioning estimation method, system, storage medium, and vehicle. By utilizing two filtering methods, particle filter (PF) and unscented Kalman filter (UKF), in the fusion positioning scheme of wheel sensors and lidar, the dependence on a third source input for vehicle initial pose is eliminated, thereby reducing the need for sensor diversity and complexity and improving the positioning accuracy and stability of the vehicle in complex nonlinear scenarios.

[0141] In an embodiment of the present invention, when the vehicle encounters a special state during the localization estimation process using unscented Kalman filtering, such as being moved by an uncontrollable external force from a third party, having extremely simple environmental point cloud features, or experiencing a sudden sensor failure for a period of time, it can quickly switch back to the particle filtering process and quickly restore its own localization.

[0142] When performing unscented Kalman filtering, multi-feature (nearest point distance, point cloud density, point cloud orientation) matching is used, and each matching pair is assigned a weight value based on robust regression analysis M-Estimator. This can greatly suppress the influence of outliers and noise points on pose estimation, thereby eliminating the need for rich sensor information sources, improving the accuracy of laser point cloud and map matching, reducing the impact of environmental noise on positioning, and thus enabling vehicle self-positioning to be completed using only wheel sensors and LiDAR, with high accuracy and strong robustness.

[0143] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, apparatus, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0144] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1A device that provides the functions specified in one or more boxes.

[0145] The above description is merely a preferred embodiment of the present invention and should not be construed as limiting the scope of the invention. Therefore, any equivalent variations made in accordance with the claims of the present invention are still within the scope of the present invention.

Claims

1. A vehicle positioning estimation method, characterized in that, It should include at least the following steps: The initial pose that satisfies the switching conditions is obtained based on the particle filter algorithm; The vehicle pose is estimated by fusing the initial vehicle pose, laser point cloud data and wheel odometer data that meet the switching conditions using the unscented Kalman filter algorithm, thus obtaining the vehicle pose estimation data for this current time. Calculate the variance of the current pose state estimation to determine whether the current pose is reliable; If reliable, the current pose is updated with the current pose estimation data, and the pose state of the vehicle in the next state is estimated based on the unscented Kalman filter. If unreliable, the pose state of the vehicle in the next state is estimated based on the particle filter algorithm described above. The estimation of vehicle pose includes: matching and optimizing based on current laser point cloud data and point cloud map. The matching and optimization process utilizes three features in the point cloud data: nearest point distance, point cloud density, and point cloud direction. It also uses an M-Estimator-based weighting function to assign weights to the matching pairs and uses an error function containing the deviations of the three features for iterative optimization. The weighting function is: ; The error function is: ; Where e is the deviation, w is the weight, and k is an adjustable engineering parameter; m is the number of matching points, and n is the number of features; when n is 3, the features represent the nearest point distance, point cloud density, and point cloud direction, respectively; corresponding to the three features, e represents the deviation of the orientation angle, the deviation of the density, and the deviation of the nearest point distance, respectively.

2. The method as described in claim 1, characterized in that, The initial pose that satisfies the switching conditions is obtained based on the particle filter algorithm, including: The initial pose of the vehicle is continuously obtained based on the particle filter algorithm until the weight of a single particle is greater than a predetermined weight threshold, where the weight of a particle represents the degree of matching between the particle and the observation data. The pose of a single particle whose weight is greater than a predetermined weight threshold is used as the initial pose of the vehicle that satisfies the switching conditions.

3. The method as described in claim 2, characterized in that, Based on the unscented Kalman filter algorithm, the vehicle's initial pose, laser point cloud data, and wheel odometer data that meet the switching conditions are fused to estimate the vehicle's pose, obtaining the vehicle's current pose estimation data, including: Extract the laser point cloud density and point cloud orientation features from the current laser point cloud data; The number of point clouds is filtered according to the matching range, which is determined in combination with wheel odometer data; Based on the direction, density, and nearest point of the point cloud, the detected points in the laser point cloud are matched with the points on the point cloud map to form at least one matching pair, and a weight is assigned to each matching pair using a weighting function based on M-Estimator. Using a pre-established error function, the gradient descent method is used to iteratively solve for the minimum value of the error function and obtain the transformation matrix corresponding to the minimum value; wherein, the error function contains the deviations of three features of the point cloud: direction, density, and nearest point distance; The convergence degree of the transformation matrix is ​​detected, and the iterative processing ends when all values ​​of the transformation matrix have converged to a predetermined convergence threshold. The transformation matrix is ​​multiplied by the vector of the vehicle pose at the current moment to obtain the latest estimated pose data for this pose state estimation, and then output.

4. The method as described in claim 3, characterized in that, The calculation of the variance of the current pose state estimation to determine whether the current pose is reliable includes: Calculate the variance of the pose estimation data for this experiment; The variance is compared with a variance threshold. If the variance is less than the variance threshold, the pose estimation is considered reliable; otherwise, the pose estimation is considered unreliable.

5. A vehicle positioning estimation system, characterized in that, At least including: The particle filter processing unit is used to obtain the initial pose that meets the switching conditions based on the particle filter algorithm. The unscented Kalman filter processing unit is used to estimate the vehicle pose by fusing the initial vehicle pose, laser point cloud data and wheel odometer data that meet the switching conditions based on the unscented Kalman filter algorithm, and obtain the vehicle pose estimation data for this time. The filtering and switching processing unit is used to calculate the variance of the current pose state estimation and determine whether the current pose is reliable. If reliable, the current pose is updated with the current pose estimation data, and the pose state of the vehicle in the next state is estimated based on the unscented Kalman filter. If unreliable, the pose state of the vehicle in the next state is estimated based on the particle filter algorithm described above. In the unscented Kalman filter processing unit, the vehicle pose is estimated using the following method: The matching and optimization process is based on the current laser point cloud data and point cloud map. The matching and optimization process utilizes three features in the point cloud data: the nearest point distance, the point cloud density, and the point cloud direction. It also uses an M-Estimator-based weighting function to assign weights to the matching pairs and uses an error function that includes the deviations of the three features for iterative optimization. The weighting function is: ; The error function is: ; Where e is the deviation, w is the weight, and k is an adjustable engineering parameter; m is the number of matching points, and n is the number of features; when n is 3, the features represent the nearest point distance, point cloud density, and point cloud direction, respectively; corresponding to the three features, e represents the deviation of the orientation angle, the deviation of the density, and the deviation of the nearest point distance, respectively.

6. The system as described in claim 5, characterized in that, The particle filtering processing unit includes: The continuous processing unit is used to continuously obtain the initial pose of the vehicle based on the particle filter algorithm until the weight of a single particle is greater than a predetermined weight threshold, where the weight of the particle represents the degree of matching between the particle and the observation data. A specific initial pose determination unit is used to determine the pose of a single particle whose weight is greater than a predetermined weight threshold as the initial pose of a vehicle that meets the switching conditions.

7. The system as described in claim 6, characterized in that, The unscented Kalman filter processing unit includes: A multi-dimensional feature extraction unit is used to extract the laser point cloud density and point cloud orientation features from the current laser point cloud data. A filtering processing unit is used to filter the number of point clouds according to the matching range, wherein the matching range of the point cloud map is determined by wheel odometer data. The matching and weighting processing unit is used to match the detected points in the laser point cloud with the points in the point cloud map according to the direction, density and nearest point of the point cloud, to form at least one matching pair, and to assign weights to each matching pair using a weighting function based on M-Estimator. The iterative solution unit is used to iteratively solve for the minimum value of the pre-established error function using the gradient descent method, and obtain the transformation matrix corresponding to the minimum value; wherein, the error function includes the deviations of three features of the point cloud: direction, density, and nearest point distance; The convergence termination detection unit is used to detect the degree of convergence of the transformation matrix. When the values ​​of each item of the transformation matrix have converged to a predetermined convergence threshold, the iterative processing process ends. The estimation unit is used to multiply the transformation matrix by the vector of the vehicle pose at the current time to obtain the latest estimated pose data for the current pose state estimation, and output it.

8. The system as described in claim 7, characterized in that, The filtering switching processing unit includes: The credibility judgment unit is used to calculate the variance of the current pose estimation data, compare the variance with a variance threshold, and if the comparison result is that the variance is less than the variance threshold, the current pose estimation is considered credible; otherwise, the current pose estimation is considered unreliable. The switching unit is used to continue using unscented Kalman filtering to estimate the pose state of the vehicle in the next state when the judgment result is reliable; when the judgment result is unreliable, it switches to the particle filtering algorithm and updates the initial pose of the vehicle.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method as described in any one of claims 1 to 4.

10. A chip comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method as described in any one of claims 1 to 4.

11. A vehicle, characterized in that, It is equipped with the system as described in any one of claims 5 to 8.

12. A computer program product comprising computer instructions that instruct a computer device to perform an operation corresponding to the method of any one of claims 1 to 4.

Images