Positioning drift detection method and device

Abstract

The invention provides a positioning drift detection method and device, and the method comprises the steps: carrying out the distortion removal processing of laser radar point cloud data, and obtaining the preprocessed laser radar point cloud data; performing point cloud registration degradation detection on the preprocessed laser radar point cloud data to obtain a laser radar pose increment and a degradation condition of point cloud registration; acquiring RTK pose data increments of any two moment intervals, and matching the laser radar odometer pose increment of the corresponding moment interval from the laser radar pose increments; when the point cloud registration is in a non-degradation state, calculating a horizontal direction error or a rotation direction error between the laser radar odometer pose increment and the RTK pose data; and if the horizontal direction error is greater than a preset horizontal direction error threshold value or the rotation direction error is greater than a preset rotation direction error threshold value, judging that RTK positioning drifts. Therefore, the robustness and the accuracy of RTK positioning drift detection are ensured.

Description

Technical Field

[0001] This invention relates to the field of unmanned vehicle technology, and in particular to a positioning drift detection method and device. Background Technology

[0002] In the operation of autonomous vehicles, accurate positioning information is a core element for ensuring safe driving. Vehicles need to acquire key data such as their position and attitude through multi-sensor fusion positioning. Among these, the Global Navigation Satellite System (GNSS) combined with Real-time Kinematic (RTK) technology can significantly improve positioning accuracy. However, inaccuracies in RTK positioning can lead to vehicle positioning drift, causing significant safety hazards. Therefore, accurately detecting positioning drift and promptly reporting it to the safety module has become a critical aspect that urgently needs to be addressed in autonomous driving technology, and is of great significance for ensuring vehicle operational safety.

[0003] Currently, RTK positioning drift detection mainly falls into three categories: The first category is multi-sampling detection, which uses time series analysis and the error between previous and subsequent positioning results to detect drift and then uses certain algorithms for correction. The key point of this technical solution is to detect the drift of the positioning system by comparing continuous positioning data. The second category is vehicle kinematic constraint fusion detection, which analyzes the vehicle's motion state and trajectory in real time to determine whether deviation has occurred, and thus determines whether RTK positioning drift exists. This method mainly relies on sensor data and preset rules to determine whether the vehicle is drifting. The third category is multi-sensor fusion detection, which detects positioning drift based on measurement information from multiple positioning sensors.

[0004] However, in practical applications, multiple sampling detection methods introduce time lags during sampling statistics, leading to untimely detection. Furthermore, introducing preset conditions or rules during statistical analysis significantly increases the probability of false detections. Detection methods incorporating vehicle kinematic constraints are susceptible to issues such as vehicle slippage. Most multi-sensor fusion detection methods neglect the detection of sensor degradation, especially for lidar sensors.

[0005] In summary, existing methods have limitations in terms of real-time performance, anti-interference capabilities, and sensor degradation compensation, making it difficult to meet the requirements of autonomous driving for high-precision and high-reliability positioning drift detection. Summary of the Invention

[0006] This invention provides a positioning drift detection method and apparatus. By calculating the information from the lidar odometry and RTK incremental odometry respectively, and considering the degradation problem of lidar point cloud registration, the robustness and accuracy of RTK positioning drift detection are guaranteed.

[0007] In a first aspect, the present invention provides a positioning drift detection method, comprising: Data preprocessing steps: Time synchronization is performed on the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data, and distortion correction is performed on the lidar point cloud data to obtain preprocessed lidar point cloud data. Point cloud registration degradation detection step: Perform point cloud registration degradation detection on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration; Positioning drift detection steps: Obtain RTK pose data increments at any two time intervals, and match the corresponding time interval of the LiDAR odometry pose increments from the LiDAR pose increments; when the point cloud registration is in a non-degenerate state, calculate the horizontal or rotational error between the LiDAR odometry pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted.

[0008] Optionally, the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data are time-synchronized, and the lidar point cloud data is subjected to distortion correction processing to obtain preprocessed lidar point cloud data, including: The timestamps of the lidar point cloud data, the GNSS positioning data, the IMU inertial measurement data, and the wheel speed meter odometer data are aligned using linear interpolation. A point cloud motion model is constructed based on the angular velocity and / or acceleration in the IMU inertial measurement data; Using the point cloud motion model, pose correction is performed on the lidar point cloud data collected at different times during lidar scanning to obtain the processed lidar point cloud data.

[0009] Optionally, point cloud registration degradation detection is performed on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration, including: The covariance matrix of the preprocessed lidar point cloud data is calculated by PCA and eigenvalue decomposition is performed to determine the three-dimensional principal direction. The constructed rotation information matrix and translation information matrix are projected onto the three-dimensional principal direction, and information pairs are filtered to obtain the projected information matrix. Information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration. Based on the reliable information pair and the strong information pair, the constraints are constructed and substituted into the optimization objective function of point cloud registration to obtain the lidar pose increment.

[0010] Optionally, information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration, including: The reliable information pairs are obtained by retaining the pairs of elements in the rotation information matrix and the translation information matrix whose element values ​​are greater than a preset element threshold, and the first constraint degree vector is obtained by summing the columns. Among the reliable information pairs, the information pairs whose angle with the main direction is less than a preset degree are retained to obtain the strong information pairs, and the second constraint degree vector is obtained by summing the columns. Based on the first constraint degree vector and the second constraint degree vector, the degradation status of the point cloud registration is determined.

[0011] Optionally, based on the first constraint degree vector and the second constraint degree vector, the degradation status of the point cloud registration is determined, including: If the first constraint degree vector is greater than the first preset threshold in the corresponding dimension, or the second constraint degree vector is greater than the second preset threshold in the corresponding dimension, then it is determined that the point cloud registration has no degradation in that dimension, and no additional constraints are applied. If the above conditions are not met, then when the first constraint degree vector is greater than the second preset threshold, or the second constraint degree vector is greater than the third preset threshold in the corresponding dimension, it is determined that the point cloud registration has some degradation in that dimension, and the previously discarded corresponding information pairs are recovered. If neither the first constraint degree vector nor the second constraint degree vector satisfies the above conditions, then the point cloud registration is determined to be severely degraded in this dimension.

[0012] Optionally, the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data are time-synchronized, and the lidar point cloud data is distorted to obtain preprocessed lidar point cloud data. The process further includes: The preprocessed lidar point cloud data is divided into ground points and non-ground points.

[0013] In a second aspect, the present invention provides a positioning drift detection device, comprising: The data preprocessing module is used to synchronize the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data and wheel speedometer odometer data, and to perform distortion correction on the lidar point cloud data to obtain preprocessed lidar point cloud data. The point cloud registration degradation detection module is used to perform point cloud registration degradation detection on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration. The positioning drift detection module is used to acquire RTK pose data increments at any two time intervals and match the corresponding time interval of the LiDAR odometry pose increments from the LiDAR pose increments; when the point cloud registration is in a non-degenerate state, it calculates the horizontal or rotational error between the LiDAR odometry pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted.

[0014] Thirdly, the present invention provides an electronic device including a processor and a memory, the memory storing computer-readable instructions that, when executed by the processor, perform the steps of the method provided in the first aspect above.

[0015] Fourthly, the present invention provides a storage medium having a computer program stored thereon, which, when executed by a processor, performs the steps of the method provided in the first aspect above.

[0016] Fifthly, the present invention provides a computer program product comprising a computer program that, when executed by a processor, performs the steps of the method provided in the first aspect above.

[0017] As can be seen from the above technical solutions, the present invention has the following advantages: This invention provides a positioning drift detection method and apparatus. The method includes: a data preprocessing step: time synchronization of synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data, and distortion correction processing of the lidar point cloud data to obtain preprocessed lidar point cloud data; a point cloud registration degradation detection step: point cloud registration degradation detection of the preprocessed lidar point cloud data to obtain lidar pose increments and point cloud registration degradation status; a positioning drift detection step: acquiring RTK pose data increments at any two time intervals, and matching lidar odometry pose increments at corresponding time intervals from the lidar pose increments; when point cloud registration is in a non-degraded state, calculating the horizontal or rotational error between the lidar odometry pose increment and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then RTK positioning is determined to have drifted. The odometry information of LiDAR and RTK incremental odometry are calculated separately to verify the consistency of odometry increments. The degradation problem of LiDAR point cloud registration itself is fully considered to ensure the robustness and accuracy of RTK positioning drift detection. Attached Figure Description

[0018] 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, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 This is a flowchart illustrating the steps of a positioning drift detection method according to a first embodiment of the present invention. Figure 2 This is a flowchart illustrating the steps of a second embodiment of the positioning drift detection method of the present invention. Figure 3 This is a structural block diagram of an embodiment of the positioning drift detection device of the present invention. Detailed Implementation

[0020] This invention provides a positioning drift detection method and apparatus. By calculating the information from the lidar odometry and RTK incremental odometry respectively, and considering the degradation problem of lidar point cloud registration, the robustness and accuracy of RTK positioning drift detection are guaranteed.

[0021] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described below are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0022] Example 1 Please see Figure 1 , Figure 1 This is a flowchart illustrating the steps of a positioning drift detection method according to a first embodiment of the present invention. The method includes: Step S101, data preprocessing step: synchronize the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data and wheel speedometer odometer data, and perform distortion correction on the lidar point cloud data to obtain preprocessed lidar point cloud data. In this embodiment, all sensors begin data acquisition simultaneously to reduce initial timing errors and ensure that lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data are acquired synchronously. Then, using the angular velocity and acceleration information from the IMU inertial measurement data, a point cloud motion model is constructed to perform pose correction on lidar point cloud data acquired at different times.

[0023] In practice, since there may be slight differences in the data acquisition frequency and timestamps of different sensors, an interpolation algorithm is used to align the timestamps of the acquired data.

[0024] Step S102, Point cloud registration degradation detection step: Perform point cloud registration degradation detection on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration. In this application embodiment, point cloud registration degradation detection is used to improve positioning accuracy, enhance environmental perception capabilities, and optimize industrial quality inspection processes. In particular, it can significantly improve system performance in complex or degraded environments (such as scenarios with missing features or changes in illumination).

[0025] In an optional embodiment, point cloud registration degradation detection is performed on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration, including: The covariance matrix of the preprocessed lidar point cloud data is calculated by PCA and eigenvalue decomposition is performed to determine the three-dimensional principal direction. The constructed rotation information matrix and translation information matrix are projected onto the three-dimensional principal direction, and information pairs are filtered to obtain the projected information matrix. Information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration. Based on the reliable information pair and the strong information pair, the constraints are constructed and substituted into the optimization objective function of point cloud registration to obtain the lidar pose increment.

[0026] In this embodiment of the application, the covariance matrix of the preprocessed lidar point cloud data is calculated using principal component analysis and eigenvalue decomposition is performed to obtain its eigenvalues ​​and eigenvectors, thereby determining the three-dimensional principal direction of the point cloud data.

[0027] Subsequently, based on the characteristics of the point cloud data, a rotation information matrix and a translation information matrix are constructed. These matrices are then projected onto the previously determined 3D principal direction to obtain the projected information matrix. Based on this projected information matrix, a threshold is set, and information pairs with element values ​​greater than this threshold are retained; these pairs are considered reliable information pairs. A first constraint degree vector is obtained by summing the columns of the reliable information pairs. Based on these reliable information pairs, strong information pairs with angles less than a preset degree to the principal direction are further selected. A second constraint degree vector is obtained by summing the columns of the strong information pairs. The values ​​of the first and second constraint degree vectors are compared with the preset threshold to determine the degradation of the point cloud registration.

[0028] Finally, constraints are constructed based on reliable information pairs and strong information pairs. These constraints are then substituted into the objective function of point cloud registration. The objective function is solved using an optimization algorithm to obtain the lidar pose increment.

[0029] Step S103, Positioning Drift Detection Step: Obtain RTK pose data increments at any two time intervals, and match the corresponding time interval of the LiDAR odometry pose increments from the LiDAR pose increments; when the point cloud registration is in a non-degenerate state, calculate the horizontal or rotational error between the LiDAR odometry pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted.

[0030] In this embodiment of the application, the RTK pose data increments at any two time intervals are first obtained from the RTK device, and the corresponding time intervals of the LiDAR odometry pose increments are matched from the previously calculated LiDAR pose increments.

[0031] Subsequently, while the point cloud registration is in a non-degenerate state, the errors between the LiDAR odometry pose increment and the RTK pose data in the horizontal and rotational directions are calculated. The calculated horizontal and rotational errors are then compared to preset horizontal and rotational error thresholds, respectively. If the horizontal error exceeds the preset horizontal error threshold, or the rotational error exceeds the preset rotational error threshold, RTK positioning is determined to have drifted.

[0032] The positioning drift detection method provided in this embodiment of the invention includes: a data preprocessing step: time synchronization of synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data, and distortion correction processing of the lidar point cloud data to obtain preprocessed lidar point cloud data; a point cloud registration degradation detection step: point cloud registration degradation detection of the preprocessed lidar point cloud data to obtain lidar pose increments and point cloud registration degradation status; a positioning drift detection step: acquiring RTK pose data increments at any two time intervals, and matching lidar odometer pose increments at corresponding time intervals from the lidar pose increments; when point cloud registration is in a non-degraded state, calculating the horizontal or rotational error between the lidar odometer pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted. The odometry information of LiDAR and RTK incremental odometry are calculated separately to verify the consistency of odometry increments. The degradation problem of LiDAR point cloud registration itself is fully considered to ensure the robustness and accuracy of RTK positioning drift detection.

[0033] Example 2 Please see Figure 2 , Figure 2 This is a flowchart illustrating a second embodiment of the positioning drift detection method of the present invention. The steps include: Step S201: Use linear interpolation to align the timestamps of the lidar point cloud data with the GNSS positioning data, the IMU inertial measurement data, and the wheel speed meter odometer data; In this embodiment, a hardware trigger signal ensures that the lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data begin acquiring data at the same time, with the error controlled within the sub-millisecond level. Simultaneously, a linear interpolation method is used to align the timestamps of the lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data to the same time axis.

[0034] Step S202: Construct a point cloud motion model based on the angular velocity and / or acceleration in the IMU inertial measurement data; Point cloud data acquired by lidar is distorted by the high-speed movement of vehicles, requiring correction to improve the accuracy of point cloud registration. In this embodiment, a point cloud motion model describing the motion state of the lidar during a single frame scan is constructed based on angular velocity and acceleration information measured by an IMU.

[0035] Step S203: Using the point cloud motion model, pose correction is performed on the lidar point cloud data collected at different times during the lidar scanning process to obtain the processed lidar point cloud data. In this embodiment of the application, a point cloud motion model is used to perform reverse motion compensation on each point collected at different times in a frame of lidar point cloud, and correct it to the coordinate system of the scanning start time (or end time).

[0036] In one optional implementation, the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data are time-synchronized, and the lidar point cloud data is then subjected to distortion correction to obtain preprocessed lidar point cloud data. The process further includes: The preprocessed lidar point cloud data is divided into ground points and non-ground points.

[0037] In this embodiment, after distortion correction of the point cloud, a ground segmentation algorithm is used to segment the preprocessed LiDAR point cloud data into ground points and non-ground points. Dividing the point cloud into ground points and non-ground points allows for step-by-step optimization in subsequent registration, accelerating computation and improving the accuracy of registration across different dimensions.

[0038] Step S204: Calculate the point cloud covariance matrix corresponding to the preprocessed lidar point cloud data using PCA and perform eigenvalue decomposition to determine the three-dimensional principal direction. First, calculate the covariance matrix of the point cloud: ; in, Let covariance matrix be the variance matrix. The number of points in the point cloud. For the first A point in the cloud, For the first The coordinates of the points The mean is the center point of the point cloud.

[0039] Perform eigenvalue decomposition on the covariance matrix, i.e.: , in, The eigenvectors represent the directions of the principal components. The eigenvalues ​​represent the variance of the principal components.

[0040] The eigenvectors define the three-dimensional principal directions of the point cloud distribution, including the principal translation direction. and rotation of the main direction .in, , , These are the feature vectors in the x, y, and z directions of the point cloud, respectively. , , These are the feature vectors in the roll, pitch, and yaw directions of the point cloud, respectively.

[0041] Step S205: Project the constructed rotation information matrix and translation information matrix onto the three-dimensional main direction, perform information pair filtering, and obtain the projected information matrix; In the implementation of this application, the definition information pairs ,in For points in a point cloud, The normal vector corresponding to this point is the information pair that expresses the contribution of this point to the point cloud registration.

[0042] The rotation and translation information matrices for point cloud registration are defined as follows: ; ; in, For rotating information matrices, To translate the information matrix, For the first point cloud The coordinates of the points for The plane normal vector corresponding to the point.

[0043] The constructed rotation information matrix and translation information matrix are projected onto the principal directions obtained by PCA, respectively, to obtain the projected information matrix, i.e.: ; ; in, Let Fr be the projection of the rotation information matrix Fr onto the principal rotation direction. Let Ft be the projection of the translation information matrix Ft onto the principal translation direction. The operator represents the absolute value of the element direction vector, that is, taking the absolute value of each element of the projected information matrix, avoiding the problem of positive and negative signs when projecting the main direction.

[0044] Step S206: Retain the information pairs in the rotation information matrix and the translation information matrix whose element values ​​are greater than a preset element threshold to obtain the reliable information pairs, and sum them column by column to obtain the first constraint degree vector; In this embodiment, due to measurement noise in lidar measurements, if a point is perpendicular to the main direction, it is considered that this point may not effectively constrain the main direction due to measurement errors. Therefore, only information pairs with threshold values ​​greater than a preset element threshold are retained, and these information pairs are called reliable information pairs. The preset element threshold in this implementation is 0.5. The formula is expressed as: ; in, For the preset element threshold, , , , For information pairs, The screening results are based on reliable information.

[0045] The reliable information pairs are then summed column-wise to obtain the first constraint degree vector. Each component of the first constraint degree vector represents the total constraint strength of all reliable information pairs on a certain degree of freedom (x, y, z, roll, pitch, yaw). The expression for calculating the first constraint degree vector is as follows: ; in, This is the first constraint degree vector.

[0046] Step S207: Retain the information pairs in the reliable information pairs whose angle with the main direction is less than a preset degree to obtain the strong information pairs, and sum them by column to obtain the second constraint degree vector; In this embodiment, if a point forms an angle of less than 45° with a principal direction, it indicates that the point can effectively constrain that direction, and the corresponding information pair formed by the point and its corresponding feature normal vector is considered a strong information pair. That is: ; in, To strengthen the information filtering results.

[0047] The corresponding second constraint degree vector is: ; in, This is the second constraint degree vector.

[0048] Step S208: Determine the degradation status of the point cloud registration based on the first constraint degree vector and the second constraint degree vector; In this embodiment of the application, if the first constraint degree vector is greater than the first preset threshold in the corresponding dimension, or the second constraint degree vector is greater than the second preset threshold in the corresponding dimension, it is determined that the point cloud registration has no degradation in that dimension, and no additional constraints are applied. If the above conditions are not met, then when the first constraint degree vector is greater than the second preset threshold, or the second constraint degree vector is greater than the third preset threshold in the corresponding dimension, it is determined that the point cloud registration has some degradation in that dimension, and the previously discarded corresponding information pairs are recovered. If neither the first constraint degree vector nor the second constraint degree vector satisfies the above conditions, then the point cloud registration is determined to be severely degraded in this dimension.

[0049] In this embodiment of the application, it is assumed that when the lidar moves in a certain main direction, it passes through point ( , Then the following constraints apply: ; ; in , This represents the current pose of the lidar.

[0050] For each pose dimension, the following judgments are made sequentially: If the first constraint level is greater than the first preset threshold (e.g., 300) in the corresponding dimension, or the second constraint level is greater than the second preset threshold (e.g., 200) in the corresponding dimension, it indicates that the constraint in that direction is sufficient, there is no degradation, and no additional constraint is applied.

[0051] If the above conditions are not met, and the first constraint level is greater than the second preset threshold (e.g., 200) in the corresponding dimension, or the second constraint level is greater than the third preset threshold (e.g., 30) in the corresponding dimension, it indicates that the constraint in that direction is weak and there is some degradation. In this case, the previously discarded weaker information pairs are recycled to strengthen the constraint.

[0052] If none of the above conditions are met, it indicates that there are almost no effective constraints in that direction, suggesting severe degradation. In this case, the initial pose constraints corresponding to that dimension should be reset to zero to avoid registration failure due to incorrect constraints.

[0053] Step S209: Construct constraints based on the reliable information pair and the strong information pair, substitute them into the optimization objective function of point cloud registration, and solve to obtain the lidar pose increment; In this embodiment, the constraints determined in S208 (unconstrained, weak information pairs recovered, or zero constraints) are constructed into constraint matrices and vectors, and then substituted into a unified constrained point cloud registration objective function for solving. The final output is a highly reliable lidar pose increment after degradation detection and processing.

[0054] In the specific implementation, it is assumed that there is rotation There is degradation and translation. A degradation mechanism is used to unify the point cloud registration constraints with a constrained objective function: ; in, , , , .

[0055] Where q is the nearest point pair corresponding to point p in the source point cloud in the target point cloud, and the other variables are mentioned above.

[0056] This achieves a degenerate solution in a single point cloud registration iteration. After completing the optimized solution, the pose of the registered point cloud is output.

[0057] Step S210: Obtain the RTK pose data increment at any two time intervals, and match the LiDAR odometry pose increment at the corresponding time interval from the LiDAR pose increment; when the point cloud registration is in a non-degenerate state, calculate the horizontal or rotational error between the LiDAR odometry pose increment and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted.

[0058] In this embodiment of the application, the following are first read respectively , Given RTK pose data at time points, calculate the RTK pose increment between two time points: ; in, This represents the RTK pose increment between two time points.

[0059] Then read , The lidar point cloud at each time step is used, with the RTK pose increment as a prior for lidar point cloud registration. The registration results of the lidar point clouds at two time steps are solved to obtain the lidar odometry pose increment. When point cloud registration degradation is detected, the localization drift detection is invalid. The difference between the RTK pose increment and the lidar pose increment is calculated as follows: ; in, This represents the difference between the RTK pose increment and the LiDAR pose increment. This is the pose increment for the lidar odometry.

[0060] If the difference between the RTK pose increment and the LiDAR pose increment is greater than 10 cm in the horizontal direction or greater than 0.1 degrees in the rotation direction, the RTK data is considered to have drifted; otherwise, the RTK data is normal.

[0061] The positioning drift detection method provided in this embodiment of the invention includes: a data preprocessing step: time synchronization of synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data, and distortion correction processing of the lidar point cloud data to obtain preprocessed lidar point cloud data; a point cloud registration degradation detection step: point cloud registration degradation detection of the preprocessed lidar point cloud data to obtain lidar pose increments and point cloud registration degradation status; a positioning drift detection step: acquiring RTK pose data increments at any two time intervals, and matching lidar odometer pose increments at corresponding time intervals from the lidar pose increments; when point cloud registration is in a non-degraded state, calculating the horizontal or rotational error between the lidar odometer pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted. The odometry information of LiDAR and RTK incremental odometry is calculated separately, the consistency of odometry increment is verified, and the degradation problem of LiDAR point cloud registration itself is fully considered. At the same time, the constraint conditions are determined based on the degradation problem, thereby ensuring the robustness and accuracy of RTK positioning drift detection.

[0062] Example 3 Please see Figure 3 , Figure 3 This is a structural block diagram of an embodiment of a positioning drift detection device according to the present invention. The device includes: The data preprocessing module 301 is used to synchronize the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data and wheel speedometer odometer data, and to perform distortion correction on the lidar point cloud data to obtain preprocessed lidar point cloud data. The point cloud registration degradation detection module 302 is used to perform point cloud registration degradation detection on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration. The positioning drift detection module 303 is used to acquire the RTK pose data increment between any two time intervals, and match the LiDAR odometry pose increment between the LiDAR pose increments with the corresponding time intervals; when the point cloud registration is in a non-degenerate state, it calculates the horizontal or rotational error between the LiDAR odometry pose increment and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, it is determined that RTK positioning has drifted.

[0063] In one optional embodiment, the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data are time-synchronized, and the lidar point cloud data is subjected to distortion correction processing to obtain preprocessed lidar point cloud data, including: The timestamps of the lidar point cloud data, the GNSS positioning data, the IMU inertial measurement data, and the wheel speed meter odometer data are aligned using linear interpolation. A point cloud motion model is constructed based on the angular velocity and / or acceleration in the IMU inertial measurement data; Using the point cloud motion model, pose correction is performed on the lidar point cloud data collected at different times during lidar scanning to obtain the processed lidar point cloud data.

[0064] In an optional embodiment, point cloud registration degradation detection is performed on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration, including: The covariance matrix of the preprocessed lidar point cloud data is calculated by PCA and eigenvalue decomposition is performed to determine the three-dimensional principal direction. The constructed rotation information matrix and translation information matrix are projected onto the three-dimensional principal direction, and information pairs are filtered to obtain the projected information matrix. Information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration. Based on the reliable information pair and the strong information pair, the constraints are constructed and substituted into the optimization objective function of point cloud registration to obtain the lidar pose increment.

[0065] In an optional embodiment, information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration, including: The reliable information pairs are obtained by retaining the pairs of elements in the rotation information matrix and the translation information matrix whose element values ​​are greater than a preset element threshold, and the first constraint degree vector is obtained by summing the columns. Among the reliable information pairs, the information pairs whose angle with the main direction is less than a preset degree are retained to obtain the strong information pairs, and the second constraint degree vector is obtained by summing the columns. Based on the first constraint degree vector and the second constraint degree vector, the degradation status of the point cloud registration is determined.

[0066] In an optional embodiment, determining the degradation status of the point cloud registration based on the first constraint degree vector and the second constraint degree vector includes: If the first constraint degree vector is greater than the first preset threshold in the corresponding dimension, or the second constraint degree vector is greater than the second preset threshold in the corresponding dimension, then it is determined that the point cloud registration has no degradation in that dimension, and no additional constraints are applied. If the above conditions are not met, then when the first constraint degree vector is greater than the second preset threshold, or the second constraint degree vector is greater than the third preset threshold in the corresponding dimension, it is determined that the point cloud registration has some degradation in that dimension, and the previously discarded corresponding information pairs are recovered. If neither the first constraint degree vector nor the second constraint degree vector satisfies the above conditions, then the point cloud registration is determined to be severely degraded in this dimension.

[0067] In one optional embodiment, the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data are time-synchronized, and the lidar point cloud data is subjected to distortion correction processing to obtain preprocessed lidar point cloud data. The process further includes: The preprocessed lidar point cloud data is divided into ground points and non-ground points.

[0068] Example 4 This invention also provides an electronic device, including a memory and a processor. The memory stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of a positioning drift detection method according to any embodiment.

[0069] Example 5 This invention also provides a computer storage medium storing a computer program thereon, wherein the computer program, when executed by the processor, implements the steps of a positioning drift detection method according to any embodiment.

[0070] Example 6 This invention also provides a computer program product having a computer program stored thereon, wherein the computer program, when executed by the processor, implements the steps of a positioning drift detection method according to any embodiment.

[0071] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0072] In the several embodiments provided in this application, it should be understood that the methods, apparatuses, electronic devices, and storage media disclosed in this invention can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual couplings, direct couplings, or communication connections may be through some interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

[0073] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0074] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0075] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a readable storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned readable storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0076] The above-described embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning drift detection method, characterized in that, include: Data preprocessing steps: Time synchronization is performed on the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data, and distortion correction is performed on the lidar point cloud data to obtain preprocessed lidar point cloud data. Point cloud registration degradation detection step: Perform point cloud registration degradation detection on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration; Positioning drift detection steps: Obtain RTK pose data increments at any two time intervals, and match the corresponding time interval of the LiDAR odometry pose increments from the LiDAR pose increments; when the point cloud registration is in a non-degenerate state, calculate the horizontal or rotational error between the LiDAR odometry pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted.

2. The positioning drift detection method according to claim 1, characterized in that, Time synchronization was performed on the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data. Distortion correction was then applied to the lidar point cloud data to obtain preprocessed lidar point cloud data, including: The timestamps of the lidar point cloud data, the GNSS positioning data, the IMU inertial measurement data, and the wheel speed meter odometer data are aligned using linear interpolation. A point cloud motion model is constructed based on the angular velocity and / or acceleration in the IMU inertial measurement data; Using the point cloud motion model, pose correction is performed on the lidar point cloud data collected at different times during lidar scanning to obtain the processed lidar point cloud data.

3. The positioning drift detection method according to claim 1, characterized in that, The preprocessed lidar point cloud data is subjected to point cloud registration degradation detection to obtain the lidar pose increment and the point cloud registration degradation status, including: The covariance matrix of the preprocessed lidar point cloud data is calculated by PCA and eigenvalue decomposition is performed to determine the three-dimensional principal direction. The constructed rotation information matrix and translation information matrix are projected onto the three-dimensional principal direction, and information pairs are filtered to obtain the projected information matrix. Information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration. Based on the reliable information pair and the strong information pair, the constraints are constructed and substituted into the optimization objective function of point cloud registration to obtain the lidar pose increment.

4. The positioning drift detection method according to claim 3, characterized in that, Information pairs are filtered based on the projected information matrix to obtain reliable information pairs, strong information pairs, and the degradation status of the point cloud registration, including: Retain the information pairs in the rotation information matrix and the translation information matrix whose element values ​​are greater than a preset element threshold to obtain the reliable information pairs, and sum them column by column to obtain the first constraint degree vector; Among the reliable information pairs, the information pairs whose angle with the main direction is less than a preset degree are retained to obtain the strong information pairs, and the second constraint degree vector is obtained by summing the columns. Based on the first constraint degree vector and the second constraint degree vector, the degradation status of the point cloud registration is determined.

5. The positioning drift detection method according to claim 4, characterized in that, Based on the first constraint degree vector and the second constraint degree vector, the degradation status of the point cloud registration is determined, including: If the first constraint degree vector is greater than the first preset threshold in the corresponding dimension, or the second constraint degree vector is greater than the second preset threshold in the corresponding dimension, then it is determined that the point cloud registration has no degradation in that dimension, and no additional constraints are applied. If the above conditions are not met, then when the first constraint degree vector is greater than the second preset threshold, or the second constraint degree vector is greater than the third preset threshold in the corresponding dimension, it is determined that the point cloud registration has some degradation in that dimension, and the previously discarded corresponding information pairs are recovered. If neither the first constraint degree vector nor the second constraint degree vector satisfies the above conditions, then the point cloud registration is determined to be severely degraded in this dimension.

6. The positioning drift detection method according to claim 1, characterized in that, The process involves time synchronization of synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data, and wheel speedometer odometer data, followed by distortion correction of the lidar point cloud data to obtain preprocessed lidar point cloud data. This process also includes: The preprocessed lidar point cloud data is divided into ground points and non-ground points.

7. A positioning drift detection device, characterized in that, include: The data preprocessing module is used to synchronize the synchronously acquired lidar point cloud data, GNSS positioning data, IMU inertial measurement data and wheel speedometer odometer data, and to perform distortion correction on the lidar point cloud data to obtain preprocessed lidar point cloud data. The point cloud registration degradation detection module is used to perform point cloud registration degradation detection on the preprocessed lidar point cloud data to obtain the lidar pose increment and the degradation status of point cloud registration. The positioning drift detection module is used to acquire RTK pose data increments at any two time intervals and match the corresponding time interval of the LiDAR odometry pose increments from the LiDAR pose increments; when the point cloud registration is in a non-degenerate state, it calculates the horizontal or rotational error between the LiDAR odometry pose increments and the RTK pose data; if the horizontal error is greater than a preset horizontal error threshold, or the rotational error is greater than a preset rotational error threshold, then it is determined that RTK positioning has drifted.

8. An electronic device, characterized in that, It includes a processor and a memory, the memory storing computer-readable instructions that, when executed by the processor, perform the method as described in any one of claims 1-6.

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

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method of any one of claims 1-6.

Images