Method and system for correcting speed of low-speed automatic driving vehicle based on point cloud iteration

Abstract

The invention discloses a method and system for correcting the speed of a low-speed automatic driving vehicle based on point cloud iteration, and relates to the technical field of low-speed automatic driving, and the method comprises the steps: setting t1 as a starting point and t2 as an ending point; calculating motion displacement; the displacement is used as an estimated position, the Pt1 and the Pt2 are matched, and relative displacement is obtained; calculating a speed error; calculating a correction coefficient; judging whether the relative displacement is smaller than a displacement threshold, if not, returning to the step of calculating the motion displacement; if yes, whether the number of iterations is smaller than an iteration number threshold value or not is judged, if yes, the two time points meeting the time difference threshold value are selected again to serve as the starting point t1 and the ending point t2, and the step of collecting the vehicle speed at the moment t1 in real time is executed again; and if not, taking the current correction coefficient k as a final correction coefficient. Under the condition of low speed, tire pressure and other structure changes, the vehicle output speed is corrected, and the influence caused by a low-speed dead zone and tire pressure changes can be reduced.

Description

Technical Field

[0001] This invention relates to the field of low-speed autonomous driving technology, and more specifically, to a method and system for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud. Background Technology

[0002] With the development of artificial intelligence and computer technology, autonomous driving technology is becoming increasingly mature. Autonomous vehicles can efficiently utilize transportation resources, alleviate traffic congestion, and reduce carbon emissions. Autonomous driving technology has developed rapidly in recent years and has become a hot topic. As a result, various industries are gradually introducing unmanned intelligent equipment.

[0003] Low-speed autonomous vehicles are increasingly being used in the transportation of various products. Unlike public road scenarios, transportation operations at low speeds require extremely high speed accuracy. Current technologies for speed feedback in low-speed autonomous vehicles primarily involve converting motor speed into vehicle speed or obtaining speed through the addition of wheel or axle encoders. Converting speed from motor speed introduces errors due to the transmission mechanism, particularly in the low-speed range where dead zones exist, leading to inaccurate speed output and impacting autonomous driving algorithms and mileage statistics. Adding wheel or axle encoders requires additional hardware and is susceptible to significant errors due to tire pressure variations, resulting in varying degrees of false or dead zones, further complicating speed output at low speeds and affecting autonomous driving algorithms and mileage statistics. Summary of the Invention

[0004] This invention provides a method and system for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud, in order to overcome at least one technical problem existing in the prior art.

[0005] On one hand, embodiments of the present invention provide a method for iteratively correcting the speed of low-speed autonomous vehicles based on point clouds, comprising:

[0006] Set time point t1 as the starting point and time point t2 as the ending point, and the difference between t2 and t1 is less than the time difference threshold;

[0007] Set the displacement threshold D, the initial iteration count n = 0, and the iteration count threshold N;

[0008] Set the initial correction factor k = 1;

[0009] The vehicle speed v at time t1 is collected in real time, and the lidar point cloud P at times t1 and t2 are collected respectively. t1 and P t2 ;

[0010] Calculate the displacement d = k * v * (t2 - t1) of the vehicle traveling at speed v at time t1 from t1 to t2.

[0011] Using d as the estimated position, for P t1 and P t2 By performing matching, the relative displacement Δd is obtained;

[0012] Based on the relative displacement Δd, the velocity error Δv = Δd / (t2-t1) is calculated.

[0013] Based on the speed error, the correction coefficient k = (v + Δv) / v is calculated.

[0014] Set the number of iterations n = n + 1;

[0015] Determine if the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, return to the step of calculating the motion displacement from t1 to t2. If it is less than the displacement threshold, determine if the iteration number n is less than the iteration number threshold. If it is less than the iteration number threshold, reselect two time points that meet the time difference threshold as the starting point t1 and the ending point t2, and return to the step of real-time acquisition of the vehicle speed v at time t1. If it is not less than the iteration number threshold, use the current correction coefficient k as the final correction coefficient.

[0016] Optionally, d is used as the estimated position for P. t1 and P t2 By performing matching, the relative displacement Δd is obtained, specifically:

[0017] P is determined by the estimated position d. t2 Transform to P t1 coordinate system, to obtain P t2 In P t1 Coordinates P in coordinate system t2 ';

[0018] P is calculated using a local neighborhood-based method. t1 and P t2 'curvature;

[0019] Feature points are extracted based on curvature, and these feature points include edge points and planar points;

[0020] Construct an objective function and solve the objective function based on feature points to obtain the optimal pose transformation;

[0021] Based on the optimal pose transformation, the relative displacement Δd is obtained.

[0022] Alternatively, assume P t2 Coordinates are (x t2 y t2 , z t2 The estimated position d is (dx d y d z If P, then t2 In P t1 Coordinates P in coordinate system t2 'for (x) t2 +d x y t2 +d y , z t2 +d z ).

[0023] Alternatively, the curvature calculation formula is: Indicates the neighborhood centroid;

[0024] The objective function is Among them, T * Represents the optimal pose transformation, e k Let d represent an edge point, ε represent the set of edge points, and d represent the edge point set. edge Indicates the translation of edge point constraints, s l Let S represent a plane point, and d represent the set of plane points. plane This represents a plane point constrained rotation, and T represents the rigid body transformation matrix.

[0025] Optionally, the displacement threshold D = 0.1m.

[0026] Optionally, the time difference threshold is 3 seconds.

[0027] Optionally, the iteration threshold N = 10.

[0028] On the other hand, the present invention also provides a system for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud, comprising:

[0029] The first setting module is used to set time point t1 as the starting point and time point t2 as the ending point, and the difference between t2 and t1 is less than the time difference threshold.

[0030] The second setting module is used to set the displacement threshold D, the initial iteration count n = 0, and the iteration count threshold N;

[0031] The setting module is used to set the initial correction coefficient k = 1;

[0032] The data acquisition module is used to acquire the vehicle speed v at time t1 in real time, and to acquire the lidar point cloud P at times t1 and t2 respectively. t1 and P t2 ;

[0033] The first calculation module is used to calculate the displacement d = k*v*(t2-t1) of the vehicle from t1 to t2 when the vehicle is traveling at a speed v at time t1.

[0034] The matching module is used to match P with d as the estimated position. t1 and P t2 By performing matching, the relative displacement Δd is obtained;

[0035] The second calculation module is used to calculate the velocity error Δv = Δd / (t2-t1) based on the relative displacement Δd.

[0036] The third calculation module is used to calculate the correction coefficient k = (v + Δv) / v based on the speed error;

[0037] The third setting module is used to set the number of iterations n = n + 1;

[0038] The judgment module is used to determine whether the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, it returns to the step of calculating the motion displacement from t1 to t2. If it is less than the displacement threshold, it determines whether the iteration number n is less than the iteration number threshold. If it is less than the iteration number threshold, it reselects two time points that meet the time difference threshold as the starting point t1 and the ending point t2, and returns to the step of real-time acquisition of the vehicle speed v at time t1. If it is not less than the iteration number threshold, it uses the current correction coefficient k as the final correction coefficient.

[0039] Optionally, the matching module is specifically used for:

[0040] P is determined by the estimated position d. t2 Transform to P t1 coordinate system, to obtain P t2 In P t1 Coordinates P in coordinate system t2 ';

[0041] P is calculated using a local neighborhood-based method. t1 and P t2 'curvature;

[0042] Feature points are extracted based on curvature, and these feature points include edge points and planar points;

[0043] Construct an objective function and solve the objective function based on feature points to obtain the optimal pose transformation;

[0044] Based on the optimal pose transformation, the relative displacement Δd is obtained.

[0045] Alternatively, the curvature calculation formula is: Indicates the neighborhood centroid;

[0046] The objective function is Among them, T * Represents the optimal pose transformation, e k Let d represent an edge point, ε represent the set of edge points, and d represent the edge point set. edgeIndicates the edge point constraint translation, s l Let S represent a plane point, and d represent the set of plane points. plane This represents a plane point constrained rotation, and T represents the rigid body transformation matrix.

[0047] The innovative aspects of this invention include:

[0048] (1) In this embodiment, the vehicle output speed is corrected under the condition of low speed or structural changes such as tire pressure, which can reduce the impact of low-speed dead zone and tire pressure changes. This is one of the innovative points of this embodiment.

[0049] (2) In this embodiment, speed calibration is performed by combining the displacement predicted by vehicle speed with radar matching. No additional hardware is required, which not only helps to save costs, but also avoids the conversion error caused by the transmission device when using motor speed conversion. This is one of the innovative points of this embodiment.

[0050] (3) In this embodiment, in order to avoid the problem of false convergence due to partial feature matching malfunction or insufficient local features without change, in addition to relative displacement, the present invention also sets an iteration number threshold to iteratively correct the correction coefficient. Only when both conditions are met will the corresponding correction coefficient k be used as the final correction coefficient. This is beneficial to improving the correction success rate and accuracy, which is one of the innovative points of the present invention. Attached Figure Description

[0051] 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.

[0052] Figure 1 A flowchart of an iterative method for correcting the speed of a low-speed autonomous vehicle provided in an embodiment of the present invention;

[0053] Figure 2 A flowchart of point cloud matching provided in an embodiment of the present invention;

[0054] Figure 3 This is a schematic diagram of an iterative correction system for the speed of a low-speed autonomous vehicle provided in an embodiment of the present invention. Detailed Implementation

[0055] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this invention are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0057] This invention discloses a method and system for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud data. These will be described in detail below.

[0058] Figure 1 A flowchart of an iterative method for correcting the speed of a low-speed autonomous vehicle provided in an embodiment of the present invention is shown below. Figure 1 The method for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud provided in this embodiment of the invention includes:

[0059] Step 1: Set time point t1 as the starting point and time point t2 as the ending point, and the difference between t2 and t1 is less than the time difference threshold;

[0060] Step 2: Set the displacement threshold D, the initial iteration count n = 0, and the iteration count threshold N;

[0061] Step 3: Set the initial correction coefficient k = 1;

[0062] Step 4: Collect the vehicle speed v at time t1 in real time, and collect the LiDAR point cloud P at times t1 and t2 respectively. t1 and P t2 ;

[0063] Step 5: Calculate the displacement d = k * v * (t2 - t1) of the vehicle from t1 to t2 when it travels at speed v at time t1.

[0064] Step 6: Using d as the estimated position, for P t1 and P t2 By performing matching, the relative displacement Δd is obtained;

[0065] Step 7: Calculate the velocity error Δv = Δd / (t2-t1) based on the relative displacement Δd;

[0066] Step 8: Calculate the correction coefficient k = (v + Δv) / v based on the speed error;

[0067] Step 9: Set the number of iterations n = n + 1;

[0068] Step 10: Determine if the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, return to the step of calculating the motion displacement from t1 to t2. If it is less than the displacement threshold, determine if the number of iterations n is less than the number of iterations threshold. If it is less than the number of iterations threshold, reselect two time points that meet the time difference threshold as the starting point t1 and the ending point t2, and return to the step of real-time acquisition of the vehicle speed v at time t1. If it is not less than the number of iterations threshold, use the current correction coefficient k as the final correction coefficient.

[0069] For details, please refer to Figure 1 The method for correcting the speed of low-speed autonomous vehicles based on point cloud iterative correction provided in this invention requires correction of the speed of vehicles traveling at low speeds in straight lines. Since this invention corrects speed through point cloud matching, if the interval between two point cloud frames is too large, there may be a matching problem. Therefore, a relatively small time interval is used as the basis for speed correction. It should be noted that, generally, low speed refers to a driving speed of less than 20 km / h. Therefore, this invention is implemented in scenarios where the vehicle's operating speed is less than 20 km / h.

[0070] Based on this, the present invention first selects two time points as the calibration start and end points in step 1, for example, using time point t1 as the start point and time point t2 as the end point, where the difference between t2 and t1 is less than a time difference threshold. The time threshold can be set according to actual needs; for example, it can be set to 3 seconds. Thus, during point cloud matching, because the time interval is very small, the result of inter-frame motion can be approximated as only translational motion, ignoring rotational motion, which helps reduce the computational load.

[0071] This invention determines whether the velocity error meets the requirements by measuring the relative displacement between two point cloud frames. Therefore, in step 2, a displacement threshold D is set. By comparing the relative displacement with the displacement threshold D, it can be determined whether the velocity error meets the requirements. It should be noted that the displacement threshold can be set according to the specific application scenario. For example, in this embodiment, the displacement threshold D = 0.1m is set. However, setting it to 0.1m is only one application in this embodiment and is not intended to limit the invention. In other embodiments, other values ​​can be used, such as 0.05m, 0.2m, 0.3m, etc.

[0072] Furthermore, during the matching process, there may be a problem of stopping the iteration due to local optima. Therefore, in addition to relative displacement, this invention also sets an iteration threshold N and an initial iteration count n = 0 in step 2. During the iteration process, iteration stops only when both of the above conditions are met simultaneously, thereby avoiding the problem of false termination due to partial feature matching convergence or falling into false convergence due to insufficient local features and no change. Similarly, the iteration threshold can be set according to the specific application scenario, for example, it can be set to N = 10 or N = 15, etc., and this invention does not make a specific limitation on it.

[0073] This invention iteratively corrects vehicle speed by calculating a correction coefficient. Therefore, step 3 first sets the initial correction coefficient k = 1. Then, step 4 involves real-time acquisition of the vehicle speed v at time t1, and the lidar point cloud P at times t1 and t2. t1 and P t2 So that it can be used to match P later. t1 and P t2 To achieve correction of vehicle speed v.

[0074] This invention performs pose estimation by combining the displacement predicted by vehicle speed with radar matching. Therefore, after calculating the displacement predicted by vehicle speed in step 5, assuming the vehicle is traveling at a speed v at time t1, the displacement of the vehicle from time t1 to time t2 can be calculated as d = k*v*(t2-t1).

[0075] After obtaining the displacement predicted by the vehicle speed, in step 6, the displacement d predicted by the vehicle speed is used as the initial estimated position for P. t1 and P t2 By performing matching, the relative pose can be obtained. Since the time interval between two point clouds is very small, the result of inter-frame motion can be approximated as only translational motion, while ignoring rotational motion. That is, the relative pose can be directly approximated as the relative displacement Δd.

[0076] Figure 2 A flowchart illustrating point cloud matching provided in an embodiment of the present invention is provided below. Figure 2 In this embodiment, when dealing with P t1 and P t2 During matching, first in step 61, P is matched using the estimated position d. t2 Transform to P t1 coordinate system, to obtain P t2 In P t1 Coordinates P in coordinate system t2 Assume P t2 Coordinates are (x t2 y t2 , z t2 The estimated coordinates of position d are (d x d yd z After coordinate transformation, we obtain P. t2 In P t1 Coordinates P in coordinate system t2 'for (x) t2 +d x y t2 +d y , z t2 +d z ).

[0077] Get P t2 In P t1 Coordinates P in coordinate system t2 After that, P t1 and P t2 Alignment reduces the initial deviation between two point clouds, improving subsequent matching efficiency. In step 62, based on the local neighborhood N... i Calculate P respectively t1 and P t2 The curvature of ' can be calculated using the curvature calculation formula. Perform calculations, where It represents the neighborhood centroid.

[0078] Then, in step 63, feature points are extracted based on curvature. These feature points include edge points and planar points, where high curvature points, such as c, are extracted. i If the value is greater than the edge point threshold, the corresponding point is considered an edge point, thus obtaining the edge point set. Low curvature points, such as c... i If the value is less than the plane point threshold, the corresponding point is treated as a plane point, thus obtaining a set of plane points. The edge point threshold and the plane point threshold can be set according to actual needs. For example, the edge point threshold can be set to take the top 10% of the curvature distribution, and the plane point threshold can be set to take the bottom 50% of the curvature distribution.

[0079] After obtaining the feature points, for P t1 and P t2 Perform feature association, such as for P t1 For each edge point in P, based on the principle that two points form a straight line, it can be found that... t2 Find the nearest edge line in '; for P t1 Each plane point in the equation, according to the principle that three points form a plane, can be represented at point P. t2 The nearest planar block is found in the data. In this way, feature association between two point clouds can be achieved.

[0080] After feature association is completed, the corresponding edge point constraint translation and planar point constraint rotation can be calculated based on the associated features. Therefore, in step 64, the objective function can be constructed based on minimizing the distance from the feature point to the corresponding geometric structure in the next frame. Among them, T ε Represents the optimal pose transformation, e k Let d represent an edge point, ε represent the set of edge points, and d represent the edge point set. edge Indicates the translation of edge point constraints, s l Let S represent a plane point, and d represent the set of plane points. plane Let T represent the planar point constraint rotation, and let T represent the rigid body transformation matrix. The optimal pose transformation can be obtained by substituting each feature point into the objective function.

[0081] In step 65, the relative displacement Δd can be obtained based on the optimal pose transformation. Since the time interval between two frames of point clouds is very small, the result of the inter-frame motion can be approximated as only translational motion, while ignoring rotational motion. That is, the relative pose can be directly approximated as the relative displacement Δd.

[0082] The displacement difference between the two point clouds is caused by the velocity error in the interval between t1 and t2. Given the relative displacement Δd, in step 7, the velocity error Δv = Δd / (t2-t1) can be calculated based on the relative displacement Δd.

[0083] In step 8, the correction coefficient k = (v + Δv) / v can be calculated based on the speed error and the collected initial speed v. At this point, one speed correction is completed. To facilitate determining whether the number of iterations meets the requirements, the number of iterations n = n + 1 is set in step 9 to reflect the current actual number of iterations in real time.

[0084] In step 10, it is first determined whether the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, it means that the correction of velocity does not meet the requirements in the current time period and needs to be corrected. Therefore, return to step 5, calculate the displacement with the current correction coefficient, perform point cloud matching again, recalculate the relative displacement, calculate the correction coefficient and make a judgment. Repeat the above steps until the relative displacement is less than the displacement threshold, which means that the correction of velocity meets the requirements in the current time period.

[0085] When the relative displacement meets the displacement threshold requirement, it only means that the speed in the current time period meets the requirement, but it does not mean that the requirement is met for all time periods. In order to avoid the problem of false termination due to partial feature matching convergence or the problem of falling into false convergence due to too few local features and no change, in addition to the relative displacement, this invention also sets the number of iterations as the condition for stopping iteration. Only when both are met at the same time does it mean that the correction coefficient k can meet the speed correction in each time period.

[0086] Therefore, after the relative displacement meets the displacement threshold requirement, it is necessary to determine whether the iteration number n is less than the iteration number threshold. If it is less than the iteration number threshold, it is necessary to verify whether the current correction coefficient k can make the relative displacement meet the displacement threshold requirement in other time periods. Therefore, two new time points are selected as the starting point t1 and the ending point t2. The two newly selected time points also need to be less than the time difference threshold. After selecting the time points, return to step 4, collect the vehicle speed v at the newly selected time t1, and the lidar point cloud P at times t1 and t2. t1 and P t2 The algorithm calculates whether the relative displacement in the current time period is less than the displacement threshold, thereby determining whether the correction coefficient k obtained in the previous iteration is applicable to the current time period.

[0087] By repeating the above steps until the number of iterations reaches the threshold, that is, if the number of iterations is not less than the threshold, it means that the current correction coefficient meets the requirements in multiple time periods. Therefore, the current correction coefficient k is taken as the final correction coefficient.

[0088] The method for correcting the speed of low-speed autonomous vehicles based on point cloud iterative correction provided by this invention can reduce the impact of low-speed dead zones and tire pressure changes by correcting the vehicle's output speed under conditions of low speed or structural changes such as tire pressure. Speed ​​calibration is performed by combining displacement predicted by vehicle speed with radar matching, eliminating the need for additional hardware. This not only saves costs but also avoids conversion errors caused by the transmission device when using motor speed conversion.

[0089] Furthermore, to avoid false convergence due to partial feature matching malfunctions or insufficient local features causing no change, this invention, in addition to relative displacement, also sets an iteration number threshold to iteratively correct the correction coefficient. Only when both conditions are met is the corresponding correction coefficient k used as the final correction coefficient. This helps to improve the correction success rate and accuracy.

[0090] Based on the same inventive concept, this invention also provides a point cloud-based iterative correction system for the speed of low-speed autonomous vehicles. Figure 3 A schematic diagram of a system for iteratively correcting the speed of a low-speed autonomous vehicle provided in an embodiment of the present invention is shown below. Figure 3 The present invention provides a system 100 for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud, comprising:

[0091] The first setting module is used to set time point t1 as the starting point and time point t2 as the ending point, and the difference between t2 and t1 is less than the time difference threshold.

[0092] The second setting module is used to set the displacement threshold D, the initial iteration count n = 0, and the iteration count threshold N;

[0093] The setting module is used to set the initial correction coefficient k = 1;

[0094] The data acquisition module is used to acquire the vehicle speed v at time t1 in real time, and to acquire the lidar point cloud P at times t1 and t2 respectively. t1 and P t2 ;

[0095] The first calculation module is used to calculate the displacement d = k*v*(t2-t1) of the vehicle from t1 to t2 when the vehicle is traveling at a speed v at time t1.

[0096] The matching module is used to match P with d as the estimated position. t1 and P t2 By performing matching, the relative displacement Δd is obtained;

[0097] The second calculation module is used to calculate the velocity error Δv = Δd / (t2-t1) based on the relative displacement Δd.

[0098] The third calculation module is used to calculate the correction coefficient k = (v + Δv) / v based on the speed error;

[0099] The third setting module is used to set the number of iterations n = n + 1;

[0100] The judgment module is used to determine whether the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, it returns to the step of calculating the motion displacement from t1 to t2. If it is less than the displacement threshold, it determines whether the iteration number n is less than the iteration number threshold. If it is less than the iteration number threshold, it reselects two time points that meet the time difference threshold as the starting point t1 and the ending point t2, and returns to the step of real-time acquisition of the vehicle speed v at time t1. If it is not less than the iteration number threshold, it uses the current correction coefficient k as the final correction coefficient.

[0101] For details, please refer to Figure 3 The system 100 for low-speed autonomous driving vehicle speed correction based on point cloud iterative method provided in this embodiment of the invention needs to correct the speed of vehicles traveling in a straight line at low speed. Since this invention corrects speed through point cloud matching, if the interval between two point cloud frames is too large, there may be a matching problem. Therefore, when correcting speed, a small time interval is used as the basis. It should be noted that, generally speaking, low speed refers to a driving speed of less than 20 km / h. Therefore, this invention is implemented in scenarios where the vehicle's operating speed is less than 20 km / h.

[0102] Based on this, the present invention first selects two time points as the calibration start and end points through the first setting module. For example, time point t1 is used as the start point and time point t2 as the end point, where the difference between t2 and t1 is less than a time difference threshold. The time threshold can be set according to actual needs, for example, it can be set to 3 seconds. In this way, when performing point cloud matching, since the time interval is very small, the result of inter-frame motion can be approximated as only translational motion, while ignoring rotational motion, which helps to reduce the amount of computation.

[0103] This invention determines whether the velocity error meets the requirements by measuring the relative displacement between two point cloud frames. Therefore, a displacement threshold D is set through the second setting module. By comparing the relative displacement with the displacement threshold D, it can be determined whether the velocity error meets the requirements. It should be noted that the displacement threshold can be set according to the specific application scenario. For example, in this embodiment, the displacement threshold D is set to 0.1m. However, setting it to 0.1m is only one application in this embodiment and is not intended to limit the invention. In other embodiments, it can be set to other values, such as 0.05m, 0.2m, 0.3m, etc.

[0104] Furthermore, during the matching process, there may be a problem of stopping the iteration due to local optima. Therefore, in addition to relative displacement, this invention also sets an iteration count threshold N and an initial iteration count n = 0 through a second setting module. During the iteration process, iteration stops only when both of the above conditions are met simultaneously, thereby avoiding the problem of false termination due to partial feature matching convergence or falling into false convergence due to insufficient local features and no change. Similarly, the iteration count threshold can be set according to the specific application scenario, for example, it can be set to N = 10 or N = 15, etc., and this invention does not make a specific limitation in this regard.

[0105] This invention iteratively corrects vehicle speed by calculating a correction coefficient. Therefore, the initial correction coefficient k = 1 is first set through the setting module. Then, the vehicle speed v at time t1, and the lidar point cloud P at times t1 and t2 are collected in real time through the acquisition module. t1 and P t2 So that it can be used to match P later. t1 and P t2 To achieve correction of vehicle speed v.

[0106] This invention performs pose estimation by combining the displacement predicted by vehicle speed with radar matching. Therefore, the first calculation module calculates the displacement predicted by vehicle speed. Assuming the vehicle is traveling at a speed v at time t1, the displacement of the vehicle from time t1 to time t2 can be calculated as d = k*v*(t2-t1).

[0107] After obtaining the displacement predicted by the vehicle speed, the matching module uses the displacement d predicted by the vehicle speed as the initial estimated position for P. t1 and Pt2 By performing matching, the relative pose can be obtained. Since the time interval between two point clouds is very small, the result of inter-frame motion can be approximated as only translational motion, while ignoring rotational motion. That is, the relative pose can be directly approximated as the relative displacement Δd.

[0108] In this embodiment, when dealing with P t1 and P t2 During matching, P is first matched by estimating its position d. t2 Transform to P t1 coordinate system, to obtain P t2 In P t1 Coordinates P in coordinate system t2 Assume P t2 Coordinates are (x t2 y t2 , z t2 The estimated coordinates of position d are (d x d y d z After coordinate transformation, we obtain P. t2 In P t1 Coordinates P in coordinate system t2 'for (x) t2 +d x y t2 +d y , z t2 +d z ).

[0109] Get P t2 In P t1 Coordinates P in coordinate system t2 After that, P t1 and P t2 Alignment reduces the initial deviation between two point clouds, improving subsequent matching efficiency. Based on the local neighborhood N i Calculate P respectively t1 and P t2 The curvature of ' can be calculated using the curvature calculation formula. Perform calculations, where It represents the neighborhood centroid.

[0110] Then, feature points are extracted based on curvature. These feature points include edge points and planar points, where high curvature points, such as c, are extracted. i If the value is greater than the edge point threshold, the corresponding point is considered an edge point, thus obtaining the edge point set. Low curvature points, such as c... iIf the value is less than the plane point threshold, the corresponding point is treated as a plane point, thus obtaining a set of plane points. The edge point threshold and the plane point threshold can be set according to actual needs. For example, the edge point threshold can be set to take the top 10% of the curvature distribution, and the plane point threshold can be set to take the bottom 50% of the curvature distribution.

[0111] After obtaining the feature points, for P t1 and P t2 Perform feature association, such as for P t1 For each edge point in P, based on the principle that two points form a straight line, it can be found that... t2 Find the nearest edge line in '; for P t1 Each plane point in the equation, according to the principle that three points form a plane, can be represented at point P. t2 The nearest planar block is found in the data. In this way, feature association between two point clouds can be achieved.

[0112] After feature association is completed, the corresponding edge point constraint translation and planar point constraint rotation can be calculated based on the associated features. The objective function can be constructed by minimizing the distance from the feature point to the corresponding geometry in the next frame. Among them, T * Represents the optimal pose transformation, e k Let d represent an edge point, ε represent the set of edge points, and d represent the edge point set. edge Indicates the translation of edge point constraints, s l Let S represent a plane point, and d represent the set of plane points. plane Let T represent the planar point constraint rotation, and let T represent the rigid body transformation matrix. The optimal pose transformation can be obtained by substituting each feature point into the objective function.

[0113] The relative displacement Δd can be obtained from the optimal pose transformation. Since the time interval between two frames of point clouds is very small, the result of the inter-frame motion can be approximated as only translational motion, while ignoring rotational motion. That is, the relative pose can be directly approximated as the relative displacement Δd.

[0114] The displacement difference between two point clouds is caused by the velocity error in the interval between t1 and t2. Given the relative displacement Δd, the second calculation module can calculate the velocity error Δv = Δd / (t2-t1) based on the relative displacement Δd.

[0115] The third calculation module calculates the correction coefficient k = (v + Δv) / v based on the speed error and the collected initial speed v. At this point, one speed correction is completed. To facilitate determining whether the number of iterations meets the requirements, the third setting module sets the number of iterations n = n + 1, reflecting the current actual number of iterations in real time.

[0116] The judgment module first determines whether the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, it means that the velocity correction does not meet the requirements in the current time period and needs to be corrected. Therefore, it returns to the displacement calculation step, calculates the displacement with the current correction coefficient, performs point cloud matching again, recalculates the relative displacement, calculates the correction coefficient, and makes a judgment. The above steps are repeated until the relative displacement is less than the displacement threshold, which means that the velocity correction meets the requirements in the current time period.

[0117] When the relative displacement meets the displacement threshold requirement, it only means that the speed in the current time period meets the requirement, but it does not mean that the requirement is met for all time periods. In order to avoid the problem of false termination due to partial feature matching convergence or the problem of falling into false convergence due to too few local features and no change, in addition to the relative displacement, this invention also sets the number of iterations as the condition for stopping iteration. Only when both are met at the same time does it mean that the correction coefficient k can meet the speed correction in each time period.

[0118] Therefore, after the relative displacement meets the displacement threshold requirement, it is necessary to determine whether the iteration number n is less than the iteration number threshold. If it is less than the iteration number threshold, it is necessary to verify whether the current correction coefficient k can make the relative displacement meet the displacement threshold requirement in other time periods. Therefore, two new time points are selected as the starting point t1 and the ending point t2. The two newly selected time points also need to be less than the time difference threshold. After selecting the time points, return to the vehicle speed acquisition step, and acquire the vehicle speed v at the newly selected time t1, as well as the LiDAR point cloud P at times t1 and t2. t1 and P t2 The algorithm calculates whether the relative displacement in the current time period is less than the displacement threshold, thereby determining whether the correction coefficient k obtained in the previous iteration is applicable to the current time period.

[0119] By repeating the above steps until the number of iterations reaches the threshold, that is, if the number of iterations is not less than the threshold, it means that the current correction coefficient meets the requirements in multiple time periods. Therefore, the current correction coefficient k is taken as the final correction coefficient.

[0120] The system for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud data provided by this invention corrects the vehicle's output speed under conditions of low speed or structural changes such as tire pressure, thereby reducing the impact of low-speed dead zones and tire pressure variations. By combining displacement predicted by vehicle speed with radar matching for speed calibration, no additional hardware is required, which not only saves costs but also avoids conversion errors caused by the transmission device when using motor speed conversion.

[0121] Furthermore, to avoid false convergence due to partial feature matching malfunctions or insufficient local features causing no change, this invention, in addition to relative displacement, also sets an iteration number threshold to iteratively correct the correction coefficient. Only when both conditions are met is the corresponding correction coefficient k used as the final correction coefficient. This helps to improve the correction success rate and accuracy.

[0122] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing the present invention.

[0123] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0124] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; 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; and these 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 method for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud, characterized in that, include: Set time point t1 as the starting point and time point t2 as the ending point, and the difference between t2 and t1 is less than the time difference threshold; Set the displacement threshold D, the initial iteration count n = 0, and the iteration count threshold N; Set the initial correction factor k = 1; The vehicle speed v at time t1 is collected in real time, and the lidar point cloud P at times t1 and t2 are collected respectively. t1 and P t2 ; Calculate the displacement d = k * v * (t2 - t1) of the vehicle traveling at speed v at time t1 from t1 to t2. Using d as the estimated position, for P t1 and P t2 By performing matching, the relative displacement Δd is obtained; Based on the relative displacement Δd, the velocity error Δv = Δd / (t2-t1) is calculated. Based on the speed error, the correction coefficient k = (v + Δv) / v is calculated. Set the number of iterations n = n + 1; Determine if the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, return to the step of calculating the motion displacement from t1 to t2. If the displacement is less than the threshold, then determine whether the number of iterations n is less than the threshold of the number of iterations. If it is less than the threshold of the number of iterations, then reselect two time points that satisfy the time difference threshold as the starting point t1 and the ending point t2, and return to the step of real-time acquisition of vehicle speed v at time t1. If it is not less than the threshold of the number of iterations, then use the current correction coefficient k as the final correction coefficient.

2. The method for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud as described in claim 1, characterized in that, Using d as the predicted position for P t1 and P t2 By performing matching, the relative displacement Δd is obtained, specifically: P is determined by the estimated position d. t2 Transform to P t1 coordinate system, to obtain P t2 In P t1 Coordinates P in coordinate system t2 '; P is calculated using a local neighborhood-based method. t1 and P t2 'curvature; Feature points are extracted based on curvature, and these feature points include edge points and planar points; Construct an objective function and solve the objective function based on feature points to obtain the optimal pose transformation; Based on the optimal pose transformation, the relative displacement Δd is obtained.

3. The method for iteratively correcting the speed of low-speed autonomous vehicles based on point clouds according to claim 2, characterized in that, Assume P t2 Coordinates are (x t2 y t2 , z t2 The estimated position d is (d x d y d z If P, then t2 In P t1 Coordinates P in coordinate system t2 'for (x) t2 +d x y t2 +d y , z t2 +d z ).

4. The method for iteratively correcting the speed of a low-speed autonomous vehicle based on point cloud as described in claim 2, characterized in that, The formula for calculating curvature is: Indicates the neighborhood centroid; The objective function is Among them, T * Represents the optimal pose transformation, e k Let d represent an edge point, ε represent the set of edge points, and d represent the edge point set. edge Indicates the edge point constraint translation, s l Let S represent a plane point, and d represent the set of plane points. plane This represents a plane point constrained rotation, and T represents the rigid body transformation matrix.

5. The method for iteratively correcting the speed of a low-speed autonomous vehicle based on point cloud as described in claim 1, characterized in that, Displacement threshold D = 0.1m.

6. The method for iteratively correcting the speed of a low-speed autonomous vehicle based on point cloud as described in claim 1, characterized in that, The time difference threshold is 3 seconds.

7. The method for iteratively correcting the speed of a low-speed autonomous vehicle based on point cloud as described in claim 1, characterized in that, The threshold for the number of iterations is N = 10.

8. A system for iteratively correcting the speed of low-speed autonomous vehicles based on point cloud, characterized in that, include: The first setting module is used to set time point t1 as the starting point and time point t2 as the ending point, and the difference between t2 and t1 is less than the time difference threshold. The second setting module is used to set the displacement threshold D, the initial iteration count n = 0, and the iteration count threshold N; The setting module is used to set the initial correction coefficient k = 1; The data acquisition module is used to acquire the vehicle speed v at time t1 in real time, and to acquire the lidar point cloud P at times t1 and t2 respectively. t1 and P t2 ; The first calculation module is used to calculate the displacement d = k*v*(t2-t1) of the vehicle from t1 to t2 when the vehicle is traveling at a speed v at time t1. The matching module is used to match P with d as the estimated position. t1 and P t2 By performing matching, the relative displacement Δd is obtained; The second calculation module is used to calculate the velocity error Δv = Δd / (t2-t1) based on the relative displacement Δd. The third calculation module is used to calculate the correction coefficient k = (v + Δv) / v based on the speed error; The third setting module is used to set the number of iterations n = n + 1; The judgment module is used to determine whether the relative displacement Δd is less than the displacement threshold. If it is not less than the displacement threshold, it returns to the step of calculating the motion displacement from t1 to t2. If the displacement is less than the threshold, then determine whether the number of iterations n is less than the threshold of the number of iterations. If it is less than the threshold of the number of iterations, then reselect two time points that satisfy the time difference threshold as the starting point t1 and the ending point t2, and return to the step of real-time acquisition of vehicle speed v at time t1. If it is not less than the threshold of the number of iterations, then use the current correction coefficient k as the final correction coefficient.

9. The method for iteratively correcting the speed of a low-speed autonomous vehicle based on point cloud as described in claim 8, characterized in that, The matching module is specifically used for: P is determined by the estimated position d. t2 Transform to P t1 coordinate system, to obtain P t2 In P t1 Coordinates P in coordinate system t2 '; P is calculated using a local neighborhood-based method. t1 and P t2 'curvature; Feature points are extracted based on curvature, and these feature points include edge points and planar points; Construct an objective function and solve the objective function based on feature points to obtain the optimal pose transformation; Based on the optimal pose transformation, the relative displacement Δd is obtained.

10. The method for iteratively correcting the speed of a low-speed autonomous vehicle based on point cloud as described in claim 9, characterized in that, The formula for calculating curvature is: Indicates the neighborhood centroid; The objective function is Among them, T * Represents the optimal pose transformation, e k Let d represent an edge point, ε represent the set of edge points, and d represent the edge point set. edge Indicates the edge point constraint translation, s l Let S represent a plane point, and d represent the set of plane points. plane This represents a plane point constrained rotation, and T represents the rigid body transformation matrix.

Images