A zero-speed correction method and system for the height direction of a laser odometer

Abstract

The present invention discloses a method and system for zero-speed correction in the height direction of a laser odometer. The method comprises the following steps: collecting data using a laser radar and a wheel speedometer, and preprocessing the data; extracting point cloud features from the preprocessed data; subscribing to the point cloud information and calculating the relative position between frames; inferring the vehicle's inter-frame acceleration and angular velocity from the relative position between frames output by the front end and the frequency of the laser radar, and inputting these into a zero-speed detection module. The zero-speed detection module determines the vehicle's height direction state based on the acceleration and angular velocity data; after identifying the zero-speed state, performing zero-speed correction to assist in the optimization of SLAM in the z-axis direction. The present invention adopts an adaptive zero-speed detection strategy, quantifies the zero-speed state, integrates zero-speed correction with SLAM, increases the constraint of SLAM in the height direction under the zero-speed state, and effectively optimizes SLAM positioning in the z-axis direction.

Description

Technical Field

[0001] The present invention belongs to the technical field of laser odometers, and in particular relates to a zero-speed correction method and system for a laser odometer in a height direction. Background Art

[0002] Existing laser SLAM systems obtain three-dimensional point cloud information of the environment through lidar. First, the point cloud data is read for preprocessing, the front-end pose estimation is completed, and the input is sent to the back-end for nonlinear optimization. The loop detection part then performs global optimization and outputs the final optimized pose to complete real-time positioning and mapping.

[0003] When a vehicle is operating on a flat road, its displacement in the z-axis (the direction of gravity) should be zero. However, existing SLAM systems have few constraints in this direction. As a result, the optimized pose drifts to a certain extent in this direction, causing the vehicle's positioning to deviate from this direction. As positioning time accumulates, the error increases, affecting the z-axis positioning effect. This drift in the z-axis will affect the determination of vehicle height, significantly interfering with certain operating conditions, such as when a vehicle needs to cross floors indoors (determine the floor it is on) or when the vehicle is traveling up or down a slope.

[0004] Point cloud matching is performed only through the point cloud information of the lidar. The constraint on the height direction only comes from the point cloud data of the lidar. The lidar can independently complete the tasks of positioning and mapping.

[0005] The existing SLAM algorithm has insufficient constraints in the z-axis direction, and a certain degree of drift will occur in the z-axis direction. As the positioning time increases, the drift in the z-axis direction will affect the positioning effect.

[0006] Existing zero-speed detection and correction technologies read acceleration and angular velocity data within a sliding window. Fixed thresholds are set for acceleration, angular velocity, and acceleration variance, and these are combined for determination, requiring them to be met simultaneously to screen for zero-speed conditions. After zero-speed conditions are identified, a displacement of 0 along the z-axis is used as the observation value and input into a Kalman filter to optimize z-axis drift. Using a fixed threshold for zero-speed detection does not scale well across all operating conditions. Furthermore, after determining the zero-speed condition, the degree of zero speed is not quantified, which compromises the effectiveness of zero-speed correction. Summary of the Invention

[0007] In order to solve the above technical problems existing in the prior art, the present invention provides a zero-speed correction method and system for the height direction of a laser odometer, which is used to optimize the drift problem of SLAM in the height direction.

[0008] The technical solution of the present invention to solve the above technical problems is: a method for zero-speed correction in the height direction of a laser odometer, comprising the following steps:

[0009] Step S1, using laser radar and wheel speed meter to collect data, and preprocessing the laser point cloud and wheel speed meter data;

[0010] Step S2, performing point cloud feature extraction on the preprocessed data;

[0011] Step S3: Subscribe to point cloud information, synchronize messages, perform point cloud feature matching between frames, construct residuals, and perform nonlinear optimization to calculate the relative pose between frames.

[0012] Step S4: The vehicle's inter-frame acceleration and angular velocity are calculated from the inter-frame relative pose output by the front end and the frequency of the laser radar, and are input into the zero-speed detection module. The zero-speed detection module determines the vehicle's height direction state based on the acceleration and angular velocity data.

[0013] Step S5: After identifying the zero-speed state, zero-speed correction is performed to assist SLAM optimization in the z-axis direction.

[0014] Furthermore, the specific steps of step S2 are: point cloud extraction adopts classification of all point clouds based on curvature size, first calculates the curvature of all points, then filters and classifies the point clouds according to the curvature size, extracts typical point and line features of the point clouds, and publishes the processed point cloud information to the SLAM front end.

[0015] Furthermore, the residual constructed in step S3 is:

[0016]

[0017] In the above formula, q is the coordinate of the point cloud of the previous frame, pi is the coordinate of the point cloud of the current frame, R is the rotation matrix, t is the translation, and n is the number of feature points involved in the optimization calculation.

[0018] Furthermore, the four constraint judgment formulas for zero speed detection in step S4 are as follows:

[0019] (5) Acceleration threshold detection

[0020]

[0021] (6) Angular velocity threshold detection

[0022]

[0023] (7) Acceleration variance detection

[0024]

[0025] (8) Combined state detection

[0026]

[0027] In the above formula, a k =[a xk ,a yk ,a zk ] T is the acceleration data at time k, w k =[w xk ,w yk ,w zk ] T is the angular velocity data at time k, threshold a , threshold w , threshold σ , threshold m They are acceleration threshold, angular velocity threshold, acceleration variance threshold and combined observation threshold respectively, ||a|| represents the size of vector a, represents the mean value of acceleration within the sliding window, and are the random noise variances of acceleration and angular velocity respectively, N is the size of the sliding window, and g is the local gravitational acceleration.

[0028] Furthermore, the evaluation function of the optimization coefficient in step S5 is as follows:

[0029]

[0030] In the above formula, A4 is the calculation result of the combined state formula, a1 is the zero-speed state threshold, a2 is the lower limit of the optimization coefficient, and k is the quadratic term coefficient of the evaluation function.

[0031] A zero-speed correction system for height-direction auxiliary correction of a laser odometer is characterized by comprising a laser radar, a wheel speedometer, a data preprocessing module, a laser odometer module, and a zero-speed detection module. The laser radar and the wheel speedometer are both connected to the data preprocessing module. The data preprocessing module reads data from the two sensors, obtains point cloud information of the environment through the laser radar, provides good environmental point cloud information for vehicle positioning, integrates information from the wheel speed odometer, and preprocesses the point cloud data and the wheel speedometer data; outputs a front-end inter-frame relative pose, calculates the vehicle angular velocity and acceleration information based on the frequency of the laser radar, and inputs the information into the zero-speed detection module to perform zero-speed detection in the z-axis height direction. The detected state is input into the laser odometer module, and in the zero-speed state, adds a constraint that the z-axis displacement is 0 to the residual of the laser odometer, performs nonlinear optimization, and corrects the drift in the z-axis direction.

[0032] The present invention has the following beneficial effects: It adopts an adaptive zero-speed detection strategy, quantifies the zero-speed state, and integrates zero-speed correction technology with SLAM, thereby increasing the height constraint of SLAM under the zero-speed state and optimizing SLAM positioning in the z-axis direction. Furthermore, the technical solution of the present invention has a simple hardware structure, strong environmental adaptability, and can adapt to various working conditions in indoor and outdoor environments, with good stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 This is a system framework diagram of the present invention.

[0034] Figure 2 This is a flow chart of the zero-speed correction method of the present invention.

[0035] Figure 3 Flowchart of the present invention. DETAILED DESCRIPTION

[0036] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0037] like Figure 1 As shown, Figure 1 This is a structural diagram of the zero-speed correction system for the height direction of a laser odometry according to the present invention. The system includes a laser radar, a wheel speedometer, a data preprocessing module, a laser odometry module, and a zero-speed detection module. The data preprocessing module includes the data processing components of both the laser radar and the wheel speedometer sensors. The laser odometry module receives data processed by the preprocessing module, performs inter-frame matching, and calculates the inter-frame pose. The zero-speed detection module receives information from both modules, performs zero-speed detection in the height direction of the z-axis, and outputs the zero-speed status. The main functions of the laser radar module include setting the laser radar scanning mode and sampling frequency, parsing the laser radar point cloud information, and outputting the processed laser point cloud to the processor. The main functions of the wheel speedometer module include setting the sampling frequency and inputting wheel speed information to the processor. The laser radar and wheel speedometer are fixed in their installation positions, thereby maintaining a fixed relative position between the wheel speedometer and the laser radar. Calibration can be used to convert the wheel speed odometry pose to that of the laser odometry, thereby removing motion distortion from the laser point cloud. This platform only requires lidar and wheel speed meters as sensors. The system is simple and stable, can adapt to complex and changing environments indoors and outdoors, and has good stability.

[0038] Figure 2This is a flow chart of the zero-speed correction method of the present invention. The zero-speed detection module receives the angular velocity and acceleration information output by the data preprocessing module and the laser odometer module, and simultaneously performs speed threshold detection, angular velocity threshold detection, acceleration variance detection and combined state detection in the z-axis direction. Only when the four detection conditions are met at the same time is it determined to be a zero-speed state in the z-axis height direction. Considering that the vehicle operating conditions are closely related to the vehicle speed, the detection threshold is set to a dynamic adaptive threshold associated with the speed to improve the generalization ability and accuracy of zero-speed detection. In the zero-speed state, an evaluation function is constructed, coupled with the output value of the combined state detection, and an optimization coefficient is calculated. The constraint of a displacement of 0 is added to the z-axis direction, and then added to the residual calculation of the laser odometer for subsequent nonlinear optimization to correct the posture.

[0039] Figure 3 The present invention is a flow chart of the present invention. A method for zero-speed correction in the height direction of a laser odometer of the present invention comprises the following steps:

[0040] Step 1: Data preprocessing.

[0041] LiDAR and wheel speedometers are used for real-time environmental perception, and the laser point cloud and wheel speedometer data are preprocessed. First, the wheel speedometer and LiDAR data are read. Since the wheel speedometer frequency is generally higher than the LiDAR frequency, sensor data time synchronization is required. Furthermore, the laser point cloud data is not acquired instantaneously; rather, each frame of data is acquired by the LiDAR's horizontal sweep. Since the LiDAR itself moves, each frame of the point cloud is not collected at the same location. Therefore, motion distortion in the laser point cloud data needs to be corrected. The wheel speedometer can assist the LiDAR in removing motion distortion based on a uniform motion assumption model.

[0042] Step 2: Feature extraction.

[0043] After data preprocessing is completed, point cloud feature extraction can be performed. The point cloud extraction algorithm classifies all point clouds based on the size of curvature. First, the curvature of all points is calculated, and then the point clouds are screened and classified according to the size of the curvature. The typical point and line features of the point cloud are extracted, and the processed point cloud information is released to the SLAM front end.

[0044] Step 3, front end.

[0045] Subscribe to point cloud information, synchronize messages, perform point cloud feature matching between frames, construct residuals, and perform nonlinear optimization to calculate the relative pose between frames. The front-end laser odometry part is completed.

[0046]

[0047] The residual is shown above, q is the coordinate of the point cloud of the previous frame, pi is the coordinate of the point cloud of the current frame, R is the rotation matrix, t is the translation, and n is the number of feature points involved in the optimization calculation.

[0048] Step 4: Zero speed detection.

[0049] The vehicle's inter-frame acceleration and angular velocity can be calculated from the inter-frame relative posture output by the front end and the frequency of the lidar, and then input into the zero-speed detection module. The zero-speed detection module determines the vehicle's height direction state based on the acceleration and angular velocity data.

[0050] First, it is necessary to identify the zero-speed state, that is, to perform zero-speed detection. The accuracy of the zero-speed state detection directly affects the constraint effect of the subsequent SLAM in the z-axis direction. If the vehicle's z-axis is in motion and is determined to be zero speed, then the constraint of the vehicle's z-axis position being 0 will cause the calculation of the posture to be performed in the wrong direction, resulting in a large error; if the vehicle's z-axis is not in motion and is not determined to be in a zero-speed state, the constraint effect of adding a SLAM residual z-axis displacement of 0 in the zero-speed state proposed by the present invention will be affected. Therefore, the accuracy of zero-speed detection is an important part of the present invention.

[0051] Zero-speed detection uses acceleration and angular velocity data to calculate the past N frames of data within a sliding window (window size N). When certain conditions are met, it is determined to be in a zero-speed state. When the vehicle is stationary in the z-axis direction, its acceleration and angular velocity data should be very small, and the variance of the acceleration is also small. Therefore, four constraint judgment formulas are proposed as follows:

[0052] (9) Acceleration threshold detection

[0053]

[0054] (10) Angular velocity threshold detection

[0055]

[0056] (11) Acceleration variance detection

[0057]

[0058] (12) Combined state detection

[0059]

[0060] In the above formula, a k =[a xk ,a yk ,a zk ] T is the acceleration data at time k, w k =[wxk ,w yk ,w zk ] T is the angular velocity data at time k, threshold a , threshold w , threshold σ , threshold m They are acceleration threshold, angular velocity threshold, acceleration variance threshold and combined observation threshold respectively, ||a|| represents the size of vector a, represents the mean value of acceleration within the sliding window, and are the random noise variances of acceleration and angular velocity respectively, N is the size of the sliding window, and g is the local gravitational acceleration.

[0061] If and only if the above four detection state formulas are satisfied simultaneously within a sliding window, it is determined to be a zero-speed state, otherwise no zero-speed correction is performed.

[0062] The vehicle's motion conditions are variable. The vehicle's motion conditions at high and low speeds vary greatly. At low speeds, the vehicle runs relatively smoothly, so the threshold should be set smaller. At high speeds, the vehicle's motion impact is greater, so the threshold should be set larger.

[0063]

[0064] In the above formula, the threshold is set as a dynamic adaptive threshold associated with speed. At low speed (speed lower than the v_min constant), a threshold c1 is set, c2 is the upper threshold limit, and a quadratic relationship is constructed between c1 and c2 and the maximum speed of the vehicle in the sliding window, and a is the quadratic coefficient.

[0065] Step 5: Zero-speed correction assisted SLAM.

[0066] After identifying the zero-speed state, zero-speed correction is performed to assist SLAM in optimizing the z-axis. In this zero-speed state, the z-axis displacement constraint of 0 is added to the SLAM front-end matching calculation. By calculating the residual and continuously iterating during the nonlinear SLAM optimization process, the z-axis displacement is corrected, thereby optimizing the vehicle's height drift.

[0067]

[0068] The residual is shown in the above formula. The z-axis constraint is added to the residual calculated by inter-frame matching, t s =[t x ,t y ,0] T .

[0069] After the zero-speed state is identified, considering that the zero-speed effects are different, different zero-speed states have different degrees of optimization for correction with SLAM fusion. The present invention sets an evaluation function for the zero-speed state, obtains an optimization coefficient α through the evaluation function, and then inputs it into the residual for iterative optimization.

[0070]

[0071] The evaluation function for establishing the optimization coefficient is as follows:

[0072]

[0073] In the above formula, A4 is the calculation result of the combined state formula, a1 is the zero-speed state threshold, a2 is the lower limit of the optimization coefficient, and k is the quadratic term coefficient of the evaluation function.

Claims

1. A zero-speed correction method for the height direction of a laser odometer, characterized in that: The following steps are involved: Step S1, using laser radar and wheel speed meter to collect data, and preprocessing the laser point cloud and wheel speed meter data; Step S2, performing point cloud feature extraction on the preprocessed data; Step S3: Subscribe to point cloud information, synchronize messages, perform point cloud feature matching between frames, construct residuals, and perform nonlinear optimization to calculate the relative pose between frames. Step S4: The vehicle's inter-frame acceleration and angular velocity are calculated from the inter-frame relative pose output by the front end and the frequency of the laser radar, and are input into the zero-speed detection module. The zero-speed detection module determines the vehicle's height direction state based on the acceleration and angular velocity data. In step S5, after identifying the zero-speed state, zero-speed correction is performed to assist SLAM optimization in the z-axis direction. In the zero-speed state, the constraint of the z-axis displacement being 0 is added to the calculation of the SLAM front-end matching. By calculating the residual, the displacement in the z-axis direction is corrected through continuous iteration in the nonlinear optimization process of SLAM, thereby optimizing the vehicle's drift in the height direction.

2. A zero-speed correction method for the height direction of a laser odometer according to claim 1, characterized in that: The specific steps of step S2 are as follows: point cloud extraction classifies all point clouds based on curvature size, first calculates the curvature of all points, then filters and classifies the point clouds according to the curvature size, extracts typical point and line features of the point clouds, and publishes the processed point cloud information to the SLAM front end.

3. The zero-speed correction method for the height direction of a laser odometer according to claim 1, characterized in that: The residual constructed in step S3 is: ； In the above formula, q i is the coordinate of the point cloud of the previous frame, p i is the coordinate of the point cloud in the current frame, R is the rotation matrix, t is the translation, and n is the number of feature points involved in the optimization calculation.

4. The zero-speed correction method for the height direction of a laser odometer according to claim 1, characterized in that: The four constraint judgment formulas for zero speed detection in step S4 are as follows: (1) Acceleration threshold detection ； (2) Angular velocity threshold detection ； (3) Acceleration variance detection ； (4) Combined state detection ； In the above formula, is the acceleration data at time k, is the angular velocity data at time k, , , , They are acceleration threshold, angular velocity threshold, acceleration variance threshold and combined observation threshold, represents the magnitude of vector a, represents the mean value of acceleration within the sliding window, and are the random noise variances of acceleration and angular velocity respectively, N is the size of the sliding window, and g is the local gravitational acceleration.

5. The zero-speed correction method for the height direction of a laser odometer according to claim 1, characterized in that: The evaluation function of the optimization coefficient in step S5 is as follows: ； In the above formula, A4 is the calculation result of the combined state formula, a1 is the zero-speed state threshold, a2 is the lower limit of the optimization coefficient, and k is the quadratic term coefficient of the evaluation function.

6. A zero-speed correction system for the height direction of a laser odometer, characterized in that: It includes a laser radar, a wheel speed meter, a data preprocessing module, a laser odometer module, and a zero-speed detection module. The laser radar and the wheel speed meter are both connected to the data preprocessing module. The data preprocessing module reads the data of the two sensors, obtains the point cloud information of the environment through the laser radar, provides good environmental point cloud information for vehicle positioning, integrates the information of the wheel speed odometer, and preprocesses the point cloud data and the wheel speed meter data; Output a relative position between frames of the front end, then calculate the vehicle angular velocity and acceleration information according to the frequency of the lidar and input it into the zero-speed detection module to perform zero-speed detection in the z-axis height direction. The detected status is input into the laser odometer module. In the zero-speed state, the constraint of the z-axis displacement being 0 is added to the calculation of the SLAM front-end matching. Through the calculation of the residual, the displacement in the z-axis direction is continuously iterated in the nonlinear optimization process of SLAM, and the drift of the vehicle in the height direction is optimized.

Images