Pose determination method and device based on laser radar point cloud, equipment and medium

Abstract

The invention provides a laser radar point cloud-based pose determination method and device, equipment and a medium. In the scheme, the reference point cloud and the real-time point cloud are screened to obtain a feature point set corresponding to the reference point cloud and a feature point set corresponding to the real-time point cloud, and the relative pose of the real-time point cloud is determined based on the feature point sets of the reference point cloud and the real-time point cloud. According to the method, the problem of pose calibration errors caused by pose changes in the working process of the laser radar is effectively solved, a laser radar pose determination scheme which is high in calibration efficiency, high in scale precision and low in calibration cost is provided, high-precision sensing of the laser radar can be achieved, and the overall performance of a vehicle-road cooperation system is improved.

Description

Technical Field

[0001] The present application relates to the field of radar technology, and in particular to a method, device, equipment and medium for determining a position and posture based on a laser radar point cloud. Background Art

[0002] Laser radar actively emits pulsed lasers to the surroundings, which are reflected when they touch objects. The system receiving unit receives the reflected signals to generate point clouds. It is an important sensor for the road side of vehicle-road collaboration. As the laser radar installation base sinks, tilts, or the installation crossbar deforms, as well as environmental factors such as strong winds or foundation vibrations, its calibration parameters change permanently or periodically. The distortion error of the laser radar calibration parameters seriously limits the accuracy advantage of the laser radar, thereby reducing the overall performance of the vehicle-road-cloud system.

[0003] The existing LiDAR parameter calibration is divided into two categories: manual calibration and automatic calibration. Most of the current automatic calibration uses the direct iterative registration (Iterative Closest Point, ICP) method. After initially aligning each point of the target point cloud with each point of the reference point cloud, the closest point pair between the point clouds is calculated, and the rigid transformation matrix is ​​calculated to adjust the target point cloud. Iterate until the registration error meets the standard or the maximum number of iterations is reached. This method relies on initial alignment, has complex calculations and slow convergence, is easily disturbed by noise and outliers, and is prone to local optimality, affecting the calibration efficiency and accuracy, which is particularly obvious in complex environments or when the data differences are large.

[0004] In summary, the existing lidar calibration technology cannot effectively solve the problem of posture calibration error caused by posture changes in lidar work. Providing a lidar point cloud posture determination solution with high calibration efficiency, high calibration accuracy and low calibration cost is a technical problem that needs to be solved urgently. Summary of the invention

[0005] The present application provides a method, device, equipment and medium for determining the posture based on a laser radar point cloud, so as to solve the technical problem that the existing laser radar calibration technology cannot effectively solve the posture calibration error caused by the posture change during the laser radar operation, and provide a technical solution for determining the posture of a laser radar point cloud with high calibration efficiency, high calibration accuracy and low calibration cost.

[0006] In a first aspect, the present application provides a method for determining a position and posture based on a laser radar point cloud, comprising:

[0007] Acquire a first feature point set corresponding to a reference point cloud, wherein the first feature point set includes: points selected from the reference point cloud whose relative distances to neighboring points are greater than a first preset distance, and points selected from the reference point cloud whose relative distances to neighboring points are less than a second preset distance, wherein the first preset distance is greater than the second preset distance;

[0008] Acquire a second feature point set corresponding to the real-time point cloud, the second feature point set including: points selected from the real-time point cloud whose relative distances to neighboring points are greater than the first preset distance, and points selected from the real-time point cloud whose relative distances to neighboring points are less than the second preset distance;

[0009] The relative position and posture of the real-time point cloud is determined according to the first feature point set and the second feature point set.

[0010] In a specific implementation, before obtaining the first feature point set corresponding to the reference point cloud, the method further includes:

[0011] filtering out dynamic targets in the reference point cloud;

[0012] Before obtaining the second feature point set corresponding to the real-time point cloud, the method further includes:

[0013] Dynamic objects in the real-time point cloud are filtered out.

[0014] In a specific implementation, for the reference point cloud or the real-time point cloud, obtaining a set of feature points corresponding to the point cloud includes:

[0015] For each point in the point cloud, calculate the roughness of the point according to a first distance between the point and a laser radar measurement center, a second distance between at least one point before the point and the laser radar measurement center, and a third distance between at least one point after the point and the laser radar center, wherein the roughness of each point is used to represent a relative distance between the point and adjacent points;

[0016] According to the roughness of each point in the point cloud, points whose roughness is greater than the first preset distance and points whose roughness is less than the second preset distance are screened out to obtain a set of feature points corresponding to the point cloud.

[0017] In a specific implementation, the calculating the roughness of the point according to a first distance between the point and a laser radar measurement center, a second distance between at least one point before the point and the laser radar measurement center, and a third distance between at least one point after the point and the laser radar center includes:

[0018] Calculating a difference between the first distance and each second distance to obtain at least one first distance difference;

[0019] Calculating a difference between the first distance and each third distance to obtain at least one second distance difference;

[0020] The roughness of the point is calculated based on the at least one first distance difference and the at least one second distance difference.

[0021] In a specific implementation, calculating the roughness of the point according to the at least one first distance difference and the at least one second distance difference includes:

[0022] An absolute value of an average value of the at least one first distance difference and the at least one second distance difference is determined as the roughness of the point.

[0023] In a specific implementation, determining the relative position and posture of the real-time point cloud according to the first feature point set and the second feature point set includes:

[0024] According to the first feature point set and the second feature point set, a Gauss-Newton nonlinear optimization algorithm is used to determine the relative position and posture of the real-time point cloud.

[0025] In a specific implementation, the determining the relative position and posture of the real-time point cloud using a Gauss-Newton nonlinear optimization algorithm according to the first feature point set and the second feature point set includes:

[0026] Performing rationality judgment on each point in the second feature point set, eliminating points that fail the rationality judgment, and obtaining a processed second feature point set;

[0027] Constructing a Jacobian matrix according to the processed second feature point set and the first feature point set;

[0028] According to the Jacobian matrix, the relative position and posture of the real-time point cloud is updated and iterated to obtain the relative position and posture of the real-time point cloud.

[0029] In a specific embodiment, the first feature point set includes a first convex feature point set and a first flat feature point set, the first convex feature point set includes points selected from the reference point cloud and whose relative distances to adjacent points are greater than the first preset distance; the first flat feature point set includes points selected from the reference point cloud and whose relative distances to adjacent points are less than the second preset distance;

[0030] The second feature point set includes a second convex feature point set and a second flat feature point set, the second convex feature point set includes points selected from the real-time point cloud and whose relative distances to adjacent points are greater than the first preset distance; the second flat feature point set includes points selected from the real-time point cloud and whose relative distances to adjacent points are less than the second preset distance;

[0031] Correspondingly, performing rationality judgment on each point in the second feature point set, removing points that fail the rationality judgment, and obtaining a processed second feature point set includes:

[0032] According to the first convex feature point set, performing rationality judgment on each point in the second convex feature point set;

[0033] According to the first flat feature point set, performing rationality judgment on each point in the second flat feature point set;

[0034] Points that fail to pass the rationality judgment in the second convex feature point set and points that fail to pass the rationality judgment in the second flat feature point set are eliminated to obtain the processed second feature point set.

[0035] In a specific implementation, judging the rationality of each point in the second convex feature point set according to the first convex feature point set includes:

[0036] For each point in the second convex feature point set, project the point to the coordinate system of the reference point cloud to obtain a projection point of the point;

[0037] Searching for at least three target points having the smallest distance from the projection point of the point from the first convex feature point set, and determining whether the at least three target points are on the same straight line;

[0038] If the at least three target points are on the same straight line, the points pass the rationality judgment;

[0039] If the at least three target points are not on the same straight line, the points fail the rationality determination.

[0040] In a specific implementation, judging the rationality of each point in the second flat feature point set according to the first flat feature point set includes:

[0041] For each point in the second flat feature point set, project the point to the coordinate system of the reference point cloud to obtain a projection point of the point;

[0042] Searching for at least four target points having the smallest distance from the projection point of the point from the first plane feature point set, and determining whether the at least four target points are on the same plane;

[0043] If the at least four target points are on the same plane, the points pass the rationality judgment;

[0044] If the at least four target points are not on the same plane, the points fail to pass the rationality determination.

[0045] In a specific implementation, constructing a Jacobian matrix according to the processed second feature point set and the first feature point set includes:

[0046] According to the first feature point set, the residual measurement of each point in the processed second feature point set is calculated, wherein the residual measurement of each point in the second convex feature point set in the processed second feature point set is the length of the perpendicular line segment between the point and the straight line where at least three target points are located, and the at least three target points are at least three points in the first convex feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system; the residual measurement of each point in the second flat feature point set in the processed second feature point set is the length of the foot of the perpendicular from the point to the plane where at least four target points are located, and the at least four target points are at least four points in the first flat feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system;

[0047] The Jacobian matrix is ​​constructed based on the residual measurement of each point in the processed second feature point set.

[0048] In a specific embodiment, the method further comprises:

[0049] According to the relative position and pose of the real-time point cloud, the absolute position and pose of the real-time point cloud in the world coordinate system is determined.

[0050] In a second aspect, the present application provides a posture determination device based on a laser radar point cloud, comprising:

[0051] A first processing module is used to obtain a first feature point set corresponding to a reference point cloud, wherein the first feature point set includes: points selected from the reference point cloud and having a relative distance from adjacent points greater than a first preset distance, and points selected from the reference point cloud and having a relative distance from adjacent points less than a second preset distance, wherein the first preset distance is greater than the second preset distance;

[0052] A second processing module is used to obtain a second feature point set corresponding to the real-time point cloud, wherein the second feature point set includes: points selected from the real-time point cloud whose relative distances to neighboring points are greater than the first preset distance, and points selected from the real-time point cloud whose relative distances to neighboring points are less than the second preset distance;

[0053] The third processing module is used to determine the relative position and posture of the real-time point cloud according to the first feature point set and the second feature point set.

[0054] In a third aspect, the present application provides an electronic device, including:

[0055] Processor, memory and communication interface;

[0056] The memory stores computer-executable instructions;

[0057] The processor executes the computer-executable instructions stored in the memory, so that the processor executes the method for determining a posture based on a laser radar point cloud as described in the first aspect.

[0058] In a fourth aspect, the present application provides a computer-readable storage medium, comprising: the computer-readable storage medium stores computer execution instructions, and when the computer execution instructions are executed by a processor, they are used to implement a method for determining a posture based on a laser radar point cloud as described in the first aspect.

[0059] In a fifth aspect, the present application also provides a computer program product, including a computer program, which, when executed by a processor, implements a method for determining a posture based on a laser radar point cloud as described in the first aspect.

[0060] The present application provides a method, device, equipment and medium for determining the posture based on the laser radar point cloud. In this solution, by screening the reference laser radar point cloud and the real-time laser radar point cloud, a first feature point set corresponding to the reference point cloud and a second feature point set corresponding to the real-time point cloud are obtained, and the relative posture of the real-time point cloud is determined based on the feature point sets of the reference point cloud and the real-time point cloud. Through the above method, the problem of posture calibration error caused by posture changes in the laser radar work is effectively solved, and a laser radar posture determination solution with high calibration efficiency, high calibration accuracy and low calibration cost is provided. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0062] Figure 1 A schematic diagram of a flow chart of a first embodiment of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0063] Figure 2 A schematic flow chart of a second embodiment of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0064] Figure 3 A schematic diagram of a flow chart of a third embodiment of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0065] Figure 4 A schematic diagram of the principle of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0066] Figure 5 A flowchart of a specific implementation of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0067] Figure 6 Another flowchart diagram of a specific implementation method of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0068] Figure 7 Another flowchart of a specific implementation method of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0069] Figure 8 Another flowchart of a specific implementation of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application;

[0070] Fig. 9 A schematic diagram of the structure of an embodiment of a position and posture determination device based on a laser radar point cloud provided in an embodiment of the present application;

[0071] Fig.10 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application.

[0072] The above drawings have shown clear embodiments of the present application, which will be described in more detail later. These drawings and text descriptions are not intended to limit the scope of the present application in any way, but to illustrate the concept of the present application to those skilled in the art by referring to specific embodiments. DETAILED DESCRIPTION

[0073] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application. Instead, they are merely examples of devices and methods consistent with some aspects of the present application as detailed in the appended claims.

[0074] Before introducing the embodiments of the present application, the application background of the embodiments of the present application is first explained:

[0075] LiDAR measures the distance between an object and the sensor by reflecting a laser beam and receiving the reflected signal, generating high-precision three-dimensional point cloud data with depth information. This makes LiDAR play an important role as an important sensor in the field of vehicle-road collaboration. LiDAR works in conjunction with other sensors such as cameras and millimeter-wave radars to achieve real-time communication and collaboration between vehicles and road infrastructure through data fusion and information sharing.

[0076] With the long-term use of LiDAR, its mounting base is exposed to the natural environment for a long time, and the base material is corroded by environmental factors, resulting in a decrease in its structural strength, which causes the mounting base to sink; the mounting crossbar is subjected to external forces such as wind and equipment weight for a long time, and the drastic temperature changes cause the material to expand and contract, and external mechanical stresses such as strong winds and vehicle vibrations repeatedly act on the crossbar, causing the mounting crossbar to deform. The sinking of the mounting base and the deformation of the mounting crossbar cause the position of the LiDAR to change, which in turn causes permanent changes in the calibration parameters. At the same time, the strong wind caused by the violent movement of the air or the ground vibration caused by the surrounding vehicles passing by causes the position of the LiDAR to change, which in turn causes the calibration parameters to change periodically. The distortion error of the LiDAR calibration parameters seriously limits the accuracy advantage of the LiDAR, thereby reducing the overall performance of the vehicle-road-cloud system.

[0077] The existing LiDAR parameter calibration is divided into two categories: manual calibration and automatic calibration. Currently, most automatic calibrations use the direct iterative registration (ICP) method. The target point cloud is roughly aligned with the reference point cloud through initial alignment, and the closest point pair between each target point cloud point and the reference point cloud point is calculated. The rigid transformation matrix is ​​calculated based on the closest point pair, and the position and posture of the target point cloud are updated by applying the transformation matrix. The registration error is calculated. If the error is less than the preset threshold or the maximum number of iterations is reached, the iteration is stopped, otherwise it continues. This method has a high dependence on initial alignment, high computational complexity, and slow convergence speed. Especially when the amount of point cloud data is large or the initial alignment is poor, more iterations are required to reach the convergence condition. At the same time, it is easily affected by noise and outliers, causing the registration result to deviate from the true value. Since the ICP method is based on local optimization, it is easy to fall into the local optimal solution. Especially in complex environments or when there are large differences in point cloud data, it is difficult to find the global optimal solution, resulting in low calibration efficiency and low accuracy.

[0078] In summary, the existing lidar calibration technology cannot effectively solve the problem of posture calibration error caused by posture changes during lidar operation. Providing a lidar calibration solution with high calibration efficiency, high calibration accuracy and low calibration cost is a technical problem that needs to be solved urgently.

[0079] Based on the above technical problems, the inventors found in the process of studying laser radar calibration technology that the existing laser radar calibration technology has interference from dynamic targets when processing point cloud data in a dynamic environment, and does not fully consider the local set characteristics of the point cloud, resulting in low calibration efficiency, low calibration accuracy and high calibration cost. However, by preprocessing the reference laser radar point cloud and the real-time laser radar point cloud, and filtering out dynamic targets using the perception detection results, interference can be reduced; feature points are selected according to the roughness of each point and the relative position and posture of the real-time laser radar point cloud is iteratively updated using the Gauss-Newton nonlinear optimization algorithm, which improves the calibration accuracy and efficiency, and achieves the requirements of high calibration efficiency and high calibration accuracy through low calibration cost. Based on this, the present application provides a method, device, equipment and medium for determining the position and posture of a laser radar point cloud.

[0080] The technical solution of the embodiment of the present application is applicable to calibrating the laser radar to solve the problem of posture calibration error caused by posture change during the operation of the laser radar. The technical solution of the embodiment of the present application can be applied to servers, cloud servers, server networks or server clusters composed of servers, computer equipment and other devices with data processing capabilities such as data calculation, data storage and data access. The device is used to obtain the reference point cloud and the real-time point cloud, and by screening the reference point cloud and the real-time point cloud, obtain the feature point set corresponding to the reference point cloud and the feature point set corresponding to the real-time point cloud, and determine the relative posture of the real-time point cloud based on the feature point set of the reference point cloud and the real-time point cloud.

[0081] The physical devices involved in the above statements are all exemplary and do not represent the only ones. This application does not specifically limit the specific forms and types of the physical devices involved.

[0082] The technical solution of the present application and how the technical solution of the present application solves the above-mentioned technical problems are described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below in conjunction with the accompanying drawings.

[0083] Figure 1 A schematic diagram of a flow chart of a first embodiment of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application, such as Figure 1 As shown, the method for determining the position and posture based on the laser radar point cloud provided in this embodiment specifically includes the following steps:

[0084] S101: Obtain a first feature point set corresponding to a reference point cloud, the first feature point set including: points selected from the reference point cloud and having a relative distance from adjacent points greater than a first preset distance, and points selected from the reference point cloud and having a relative distance from adjacent points less than a second preset distance, the first preset distance being greater than the second preset distance.

[0085] In this step, the pose determination process of the LiDAR point cloud usually involves two groups of point clouds: the reference point cloud and the real-time point cloud. The two groups of point clouds can essentially be considered as being associated with two different coordinate systems corresponding to their respective pose parameters. The pose parameters of the reference point cloud are determined by the previous calibration process, and the pose parameters of the real-time point cloud are the core parameters to be determined by the LiDAR calibration.

[0086] In a specific implementation, before this step, the method for determining the position and posture based on the laser radar point cloud further includes: filtering out dynamic targets in the reference point cloud.

[0087] In order to reduce the computational complexity and enhance the features of point cloud data, the reference point cloud is first preprocessed, that is, dynamic targets are filtered out according to the reference lidar perception detection results. The lidar perception detection results are a group of traffic participants within the coverage of the point cloud at the current moment, including but not limited to dynamic targets such as motor vehicles, pedestrians, and animals. Since traffic participants are generally moving instantaneous environmental elements, they will not exist in both the reference point cloud and the real-time point cloud at the same time. Therefore, the traffic participant part in the reference point cloud and the real-time point cloud is equivalent to an interference noise signal for the registration of the reference point cloud and the real-time point cloud. In order to improve the calibration accuracy of subsequent lidars, dynamic targets in the point cloud need to be filtered out. It should be noted that dynamic targets in the reference point cloud are filtered out only at the start of the pose determination method based on the lidar point cloud.

[0088] Specifically, the result of LiDAR perception detection includes the bounding box of the current traffic participant, which is generally represented by a spatial cube, and the point cloud of dynamic targets in a specific area is filtered out in a targeted manner to obtain static environmental information including but not limited to buildings, plants or roadside signs. It should be noted that since the reference point cloud has a certain range of freedom of selection, the point cloud at a time when there are fewer traffic participants should be selected as much as possible to improve the registration rate between the reference point cloud and the real-time point cloud and improve the calibration accuracy.

[0089] In a specific implementation, based on filtering out the reference point cloud of dynamic targets, it is arranged into a two-dimensional matrix according to the reference laser radar scan lines.

[0090] In this step, the row dimension of the two-dimensional matrix is ​​the number of scan lines, and the column dimension is the number of points scanned and measured by each scan line in each frame. The row dimension and column dimension refer to the design parameters of the laser radar and are fixed values. It should be noted that only at the start of the posture determination method based on the laser radar point cloud, the reference point cloud that filters out dynamic targets is arranged into a two-dimensional matrix according to the reference laser radar scan line. Specifically, for each row of data of the two-dimensional matrix corresponding to the reference point cloud, it is arranged in the order of the reference laser radar measurement time, and each data includes but is not limited to three-dimensional coordinates.

[0091] In the specific implementation of obtaining the first feature point set corresponding to the reference point cloud based on the reference point cloud with dynamic targets removed, feature points are selected according to the roughness of each point in the reference point cloud to obtain the first feature point set corresponding to the reference point cloud, and the first feature point set includes: points selected from the reference point cloud whose relative distance to adjacent points is greater than a first preset distance, and points selected from the reference point cloud whose relative distance to adjacent points is less than a second preset distance. It should be noted that the first feature point set corresponding to the reference point cloud is obtained based on the reference point cloud with dynamic targets removed only at the start of the posture determination method based on the laser radar point cloud.

[0092] In a specific embodiment, the first feature point set selected according to the roughness of each point in the reference point cloud includes convex feature points and flat feature points, and the first feature point set includes a first convex feature point set consisting of all convex feature points, and a first flat feature point set consisting of all flat feature points. The roughness of each point is used to represent the relative distance between the point and the adjacent points. The first convex feature point set includes points in the reference point cloud whose relative distance to the adjacent points is greater than a first preset distance, that is, convex feature points, and the first preset distance is a convex feature point screening threshold. The first flat feature point set includes points in the reference point cloud whose relative distance to the adjacent points is less than a second preset distance, that is, flat feature points, and the second preset distance is a flat feature point screening threshold.

[0093] It should be noted that the neighboring points in this solution are at least one point in front of and at least one point behind each point in the reference point cloud on each laser radar scan line. The specific number of at least one point can be selected according to the actual situation. For example, the neighboring points can be the 5 points in front of and 5 points behind the point on the laser radar scan line. It should be noted that the first preset distance is greater than the second preset distance, that is, the convex feature point screening threshold is greater than the flat feature point screening threshold. Therefore, for each point in the reference point cloud, it is impossible to belong to both the convex feature point set and the flat feature point set. The screening thresholds for convex feature points and flat feature points can be determined based on empirical rules, statistical analysis of the point cloud, calculation of the distribution of the roughness of the points, or automatic learning through training data sets using machine learning algorithms. Reasonable threshold values ​​are determined. The specific settings need to be considered in combination with application requirements, data characteristics and other factors.

[0094] S102: Obtain a second feature point set corresponding to the real-time point cloud, the second feature point set including: points selected from the real-time point cloud whose relative distance to adjacent points is greater than a first preset distance, and points selected from the real-time point cloud whose relative distance to adjacent points is less than a second preset distance.

[0095] In a specific implementation, before the steps are implemented, the method for determining the position and posture based on the laser radar point cloud further includes: filtering out dynamic targets in the real-time point cloud. The implementation principle and technical effect of filtering out dynamic targets in the real-time point cloud are similar to the implementation principle and technical effect of filtering out dynamic targets in the reference point cloud, and will not be repeated here.

[0096] In a specific implementation, based on filtering out the real-time point cloud of dynamic targets, it is arranged into a two-dimensional matrix according to the real-time laser radar scan line. The setting principle and technical effect of the two-dimensional matrix are similar to the setting principle and technical effect of the two-dimensional matrix of the reference point cloud, and will not be repeated here.

[0097] In the specific implementation of obtaining a second feature point set corresponding to the real-time point cloud based on filtering out dynamic targets, feature points are selected according to the roughness of each point in the real-time point cloud to obtain the second feature point set corresponding to the real-time point cloud. The second feature point set includes: points selected from the real-time point cloud whose relative distance to adjacent points is greater than a first preset distance, and points selected from the real-time point cloud whose relative distance to adjacent points is less than a second preset distance.

[0098] In a specific embodiment, the second feature point set selected according to the roughness of each point in the real-time point cloud includes convex feature points and flat feature points, and the second feature point set includes a second convex feature point set consisting of all convex feature points, and a second flat feature point set consisting of all flat feature points. The roughness of each point is used to represent the relative distance between the point and the adjacent points. The second convex feature point set includes points in the real-time point cloud whose relative distance to the adjacent points is greater than a first preset distance, that is, convex feature points, and the first preset distance is a convex feature point screening threshold. The second flat feature point set includes points in the real-time point cloud whose relative distance to the adjacent points is less than a second preset distance, that is, flat feature points, and the second preset distance is a flat feature point screening threshold.

[0099] The implementation principle and technical effect of the selection of neighboring points and the feature point screening threshold in the real-time point cloud are consistent with the implementation principle and technical effect of the selection of neighboring points and the feature point screening threshold in the reference point cloud, and will not be repeated here.

[0100] S103: Determine the relative position and posture of the real-time point cloud according to the first feature point set and the second feature point set.

[0101] In this step, after obtaining the first feature point set of the reference point cloud and the second feature point set of the real-time point cloud, the relative pose of the real-time point cloud is obtained through accurate feature point registration. This method can reduce the computational complexity and effectively reduce the influence of noise and error, thereby obtaining a more reliable pose estimation.

[0102] The method for determining the pose based on the laser radar point cloud provided in the embodiment of the present application screens the reference point cloud and the real-time point cloud, filters out the dynamic targets in the reference point cloud and the real-time point cloud, obtains the first feature point set corresponding to the reference point cloud and the second feature point set corresponding to the real-time point cloud, and determines the relative pose of the real-time point cloud based on the feature point set of the reference point cloud and the real-time point cloud. Through the above method, accurate registration of the feature points of the reference point cloud and the real-time point cloud can be achieved, reducing the computational complexity while effectively reducing the influence of noise and errors, thereby obtaining a more reliable pose estimation and improving the calibration accuracy and efficiency.

[0103] Figure 2 A schematic diagram of a second embodiment of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application is shown in FIG. Figure 2 As shown, on the basis of the above embodiment, obtaining the first feature point set corresponding to the reference point cloud and obtaining the second feature point set corresponding to the real-time point cloud in step S101 and step S102, that is, obtaining the corresponding feature point set for the reference point cloud or the real-time point cloud, includes the following steps:

[0104] S201: For each point in the point cloud, the roughness of the point is calculated based on a first distance between the point and the laser radar measurement center, a second distance between at least one point before the point and the laser radar measurement center, and a third distance between at least one point after the point and the laser radar center. The roughness of each point is used to represent the relative distance between the point and adjacent points.

[0105] In this solution, the method for determining the feature point set corresponding to the real-time point cloud or the reference point cloud is the same. In the specific solution description, point cloud refers to either the real-time point cloud or the reference point cloud.

[0106] In this step, roughness is used to indicate the degree of height variation of a local area of ​​the point cloud surface, reflecting the smoothness or irregularity of the point surface. Specifically, the roughness can be quantified by calculating the distance difference between the point and its neighboring points. If the difference is large, it indicates that the surface of the point is relatively rough; if the difference is small, it indicates that the surface of the point is relatively smooth. The lidar measurement center is the reference point for each point in the point cloud. By comparing and analyzing the distance difference between the point and the lidar measurement center and the distance difference between at least one point in front of the point and at least one point behind the point on the lidar scan line and the lidar measurement center, the height difference and position change of the point relative to its neighboring points are determined, thereby evaluating the roughness of the point.

[0107] Specifically, for each point in the reference point cloud or the real-time point cloud, the distance between the point and the laser radar measurement center is calculated, which is expressed as the first distance.

[0108] The distance between at least one point before the point on the laser radar scan line and the laser radar measurement center is calculated and expressed as the second distance. For example, 5 points before the point can be selected, and the distance between the 5 points before the point on the laser radar scan line and the laser radar measurement center is calculated as the second distance.

[0109] The distance between at least one point behind the point on the laser radar scan line and the laser radar measurement center is calculated, which is expressed as the third distance. For example, 5 points after the point can be selected, and the distance between the 5 points behind the point on the laser radar scan line and the laser radar measurement center is calculated as the third distance. It should be noted that the 5 points before the point and the 5 points after the point are the aforementioned neighboring points.

[0110] S202: Calculate the difference between the first distance and each second distance to obtain at least one first distance difference; calculate the difference between the first distance and each third distance to obtain at least one second distance difference; calculate the roughness of the point based on at least one first distance difference and at least one second distance difference.

[0111] In this step, the height difference and position change of each point in the point cloud relative to its neighboring points can be determined by comparing and analyzing the distance from the lidar measurement center, and then the roughness of the point can be determined.

[0112] Specifically, the difference between the first distance and each second distance is calculated to obtain at least one first distance difference. For example, the difference between the first distance and the five second distances is calculated respectively to obtain five first distance differences.

[0113] The difference between the first distance and each third distance is calculated to obtain at least one second distance difference. For example, the difference between the first distance and the five third distances is calculated respectively to obtain five second distance differences.

[0114] Finally, the roughness of the point is calculated based on the at least one first distance difference and the at least one second distance difference.

[0115] S203: Determine the absolute value of the average value of at least one first distance difference and at least one second distance difference as the roughness of the point.

[0116] Specifically, the absolute value of the average of at least one first distance difference and at least one second distance difference is calculated as the roughness of the point. For example, the absolute value of the average of 5 first distance differences and 5 second distance differences is calculated and determined as the roughness of the point.

[0117] S204: According to the roughness of each point in the point cloud, points with a roughness greater than a first preset distance and points with a roughness less than a second preset distance are screened out to obtain a feature point set corresponding to the point cloud.

[0118] Specifically, according to the roughness of each point in the point cloud, the points with a roughness greater than the first preset distance are screened out, namely, convex feature points. The distance difference between this point and its neighboring points from the laser radar measurement center is large, and the surface is relatively rough. All convex feature points form a convex feature point set. The points with a roughness less than the second preset distance are screened out, namely, flat feature points. The distance difference between this point and its neighboring points from the laser radar measurement center is small, and the surface is relatively smooth. All flat feature points form a flat feature point set.

[0119] In a specific embodiment, for the reference point cloud, the first feature point set includes a first convex feature point set and a first flat feature point set, the first convex feature point set includes points selected from the reference point cloud and whose relative distance to adjacent points is greater than a first preset distance; the first flat feature point set includes points selected from the reference point cloud and whose relative distance to adjacent points is less than a second preset distance.

[0120] For the real-time point cloud, the second feature point set includes a second convex feature point set and a second flat feature point set. The second convex feature point set includes points selected from the real-time point cloud and whose relative distance to adjacent points is greater than a first preset distance; the second flat feature point set includes points selected from the real-time point cloud and whose relative distance to adjacent points is less than a second preset distance.

[0121] The method for determining the position and posture based on the laser radar point cloud provided in the embodiment of the present application calculates and analyzes the roughness of each point in the reference point cloud and the real-time point cloud, selects points with relatively rough surfaces and relatively smooth surfaces, and constructs a combination of feature points corresponding to the reference point cloud and the real-time point cloud. Through the above method, not only the processing accuracy of the point cloud is improved, but also the recognition ability of the surface features of the point cloud is enhanced, which provides a reliable foundation for the subsequent point cloud data analysis and application such as the registration of the reference point cloud and the real-time point cloud.

[0122] Figure 3 A schematic diagram of a process for determining a position and posture based on a laser radar point cloud according to an embodiment of the present application is provided. Figure 3 As shown, based on the above embodiment, step S204 includes determining the relative position and posture of the real-time point cloud using the Gauss-Newton nonlinear optimization algorithm according to the first feature point set and the second feature point set. Specifically, it includes:

[0123] S301: Performing rationality judgment on each point in the second feature point set, removing points that fail the rationality judgment, and obtaining a processed second feature point set.

[0124] The registration of the feature point set of the reference point cloud and the feature point set of the real-time point cloud is an iterative process. The relative pose of the real-time lidar coordinate system relative to the reference lidar coordinate system is updated once in each iteration until the relative pose update solution value is less than the threshold or the number of iterations reaches the threshold. Therefore, determining the relative pose of the real-time point cloud is an iterative process. The relative pose of the real-time point cloud is iteratively updated using the Gauss-Newton nonlinear optimization algorithm.

[0125] The Gauss-Newton nonlinear optimization algorithm is an iterative method for solving nonlinear least squares problems. The algorithm is applicable to the case where the objective function is the residual sum of squares, that is, the objective function can be expressed as the sum of squares of multiple nonlinear functions. The Gauss-Newton algorithm minimizes the objective function by linearizing the nonlinear function and gradually approaching the optimal solution. The algorithm is divided into three steps: rationality judgment, construction of the Jacobian matrix, and optimization update.

[0126] In this step, by rationality judgment, it is verified whether each point in the second feature point set meets the applicable conditions of the Gauss-Newton nonlinear optimization algorithm, which can avoid the waste of computing resources and the risk of optimization failure caused by the algorithm being inapplicable, ensure the applicability and effectiveness of the algorithm, and lay the foundation for subsequent matrix construction and optimization iteration. The rationality judgment is different for each point in the second convex feature point set and the second flat feature point set in the second feature point set.

[0127] In a specific implementation, the rationality of each point in the second convex feature point set is judged based on the first convex feature point set.

[0128] First, for each point in the second convex feature point set, the point is projected to the coordinate system of the reference point cloud to obtain the projection point of the point.

[0129] Specifically, for each point in the second convex feature point set, the projection matrix is ​​constructed using the initial relative pose of the real-time point cloud, and the point is projected into the coordinate system of the reference point cloud to obtain the projection point of the point. Among them, at the start-up time of the pose determination method based on the laser radar point cloud, that is, when the relative pose of the real-time point cloud is updated for the first time, the initial relative pose is 0; after the pose determination method based on the laser radar point cloud is started, when the relative pose of the real-time point cloud is not updated for the first time, the initial relative pose is the relative pose of the previous real-time point cloud.

[0130] Secondly, at least three target points having the smallest distance from the projection point of the point are searched from the first convex feature point set, and it is determined whether the at least three target points are on the same straight line.

[0131] In this step, in order to determine the collinearity of the target points, at least three target points need to be determined. By calculating the Euclidean distance between the point and its projection point, at least three points with the smallest Euclidean distance can be selected as target points in the first convex feature point set, that is, the target point closest to the projection point is found, ensuring that these target points have a strong correlation in space. Determine whether the at least three target points are on the same straight line. If the at least three target points are on the same straight line, the point passes the rationality judgment; if the at least three target points are not on the same straight line, the point fails the rationality judgment.

[0132] Specifically, whether the target points are on a straight line can be determined by analyzing the eigenvalues ​​and eigenvectors of the matrix formed by these target points. The eigenvectors represent the main directions of the distribution of the target points in the point cloud data, and the eigenvalues ​​quantify the variance of the target points in the point cloud data in the direction of the corresponding eigenvectors. A larger eigenvalue indicates that the target points in the point cloud data have a larger distribution in this direction.

[0133] In a specific implementation, in order to determine the collinearity of the target points, at least three target points need to be determined. For example, the five points with the smallest Euclidean distance from the projection point of the point are searched from the first convex feature point set as the target points, and the eigenvectors and eigenvalues ​​of the symmetric matrix constructed by the five target points are analyzed to determine whether the target points are on the same straight line. If the absolute value of the largest eigenvalue of the symmetric matrix is ​​much larger than the absolute value of the second largest eigenvalue, it means that the target points are mainly distributed along the direction of the eigenvector corresponding to the largest eigenvalue, that is, the target points tend to be on the same straight line, satisfying the straightness condition, and the point passes the rationality judgment for subsequent real-time point cloud relative pose optimization update. If the absolute value of the largest eigenvalue of the symmetric matrix is ​​not much larger than the absolute value of the second largest eigenvalue, the target points are not on the same straight line, do not meet the straightness condition, and the point fails the rationality judgment, and the point is excluded from the subsequent real-time point cloud relative pose optimization update.

[0134] In a specific implementation, the rationality of each point in the second flat feature point set is judged based on the first flat feature point set.

[0135] First, for each point in the second flat feature point set, the point is projected to the coordinate system of the reference point cloud to obtain the projection point of the point.

[0136] Specifically, for each point in the second flat feature point set, it is projected to the coordinate system of the reference point cloud to obtain the projection point of the point. The implementation principle and technical effect are similar to those of projecting each point in the second convex feature point set to the coordinate system of the reference point cloud to obtain the projection point of the point, and will not be repeated here.

[0137] Secondly, at least four target points having the smallest distance from the projection point of the point are searched from the first plane feature point set, and it is determined whether the at least four target points are on the same plane.

[0138] In this step, in order to determine the coplanarity of the target point, at least four target points need to be determined. By calculating the Euclidean distance between the point and its projection point, at least four points with the smallest Euclidean distance can be selected as target points in the first plane feature point set, that is, the target point closest to the projection point is found, ensuring that these target points have a strong correlation in space. Determine whether the at least four target points are on the same plane. If the at least four target points are on the same plane, the point passes the rationality judgment; if the at least four target points are not on the same plane, the point fails the rationality judgment.

[0139] Specifically, whether the target points are on the same plane can be realized by analyzing the plane equations constructed by these target points.

[0140] In a specific implementation, in order to determine the coplanarity of the target point, at least four target points need to be determined. For example, the five points with the smallest Euclidean distance from the projection point of the point are searched from the first plane feature point set as the target points, and the residual from the target point to the plane is calculated by analyzing the plane equation constructed by the five target points to determine whether the target points are on the same plane. If the residual satisfies the residual threshold condition, the target point satisfies the flatness condition, and the point passes the rationality judgment and is used for subsequent real-time point cloud relative pose optimization update. If the residual does not meet the residual threshold condition, the target point does not meet the flatness condition, and the point fails to pass the rationality judgment, and the point is excluded from the subsequent real-time point cloud relative pose optimization update. The determination of the residual threshold for judging whether the target point meets the flatness condition can be based on experience, statistical analysis, error analysis or cross-validation.

[0141] In a specific implementation, points that fail to pass the rationality judgment in the second convex feature point set and points that fail to pass the rationality judgment in the second flat feature point set are eliminated to obtain a processed second feature point set.

[0142] S302: constructing a Jacobian matrix according to the processed second feature point set and the first feature point set.

[0143] In this step, Gauss-Newton nonlinear optimization needs to use the first-order multivariate partial differential of the multivariate vector function. The Jacobian matrix is ​​the first-order partial derivative matrix of the vector-valued function, which is used to describe the linear approximation of the multivariate function near a certain point. The elements in the Jacobian matrix are the partial derivatives of the residuals of the feature points in the second feature point set after processing of the real-time point cloud with respect to the relative posture of the real-time point cloud. The dimension of the matrix is ​​the product of the number of feature points judged by rationality and the dimension of the relative posture, that is, the product of the number of points in the second feature point set after processing and the dimension of the relative posture. According to the first feature point set, the residual measurement of each point in the second feature point set after processing is calculated, and the residual measurement is calculated differently for each point in the second convex feature point set and the second flat feature point set in the second feature point set.

[0144] Specifically, the residual measurement of each point in the second convex feature point set in the processed second feature point set is the length of the perpendicular line segment between the point and the straight line where at least three target points are located, and the at least three target points are at least three points in the first convex feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system. The residual measurement of each point in the second flat feature point set in the processed second feature point set is the length of the foot of the perpendicular from the point to the plane where at least four target points are located, and the at least four target points are at least four points in the first flat feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system.

[0145] In a specific implementation, for each point in the second convex feature point set in the processed second feature point set, rationality has been judged, so at least three target points of each point must be on the same straight line. The residual measurement of the point is the vertical distance from the projection point of the point in the reference point cloud coordinate system to the straight line, that is, the length of the perpendicular line segment between the point and the straight line where the at least three target points are located.

[0146] In a specific implementation, for each point in the second flat feature point set in the processed second feature point set, the rationality judgment has been passed, so at least four target points of each point must be on the same plane. The residual measurement of the point is the vertical distance from the projection point of the point in the reference point cloud coordinate system to the plane where it is located, that is, the length of the foot of the perpendicular from the point to the plane where the at least four target points are located.

[0147] In a specific implementation, a Jacobian matrix is ​​constructed based on the residual measurement of each point in the processed second feature point set.

[0148] Specifically, the total residual measurement is determined according to the residual measurement of each point in the second convex feature point set in the processed second feature point set and the residual measurement of each point in the second flat feature point set in the processed second feature point set. The partial derivative of the total residual measurement relative to the initial relative pose of the real-time point cloud is calculated to obtain the Jacobian matrix.

[0149] S303: According to the Jacobian matrix, the relative position and posture of the real-time point cloud is updated and iterated to obtain the relative position and posture of the real-time point cloud.

[0150] In this step, the Jacobian matrix is ​​used to construct a set of overdetermined linear equations. By solving these linear equations, the updated solution value of the relative posture is obtained, that is, the increment used to update the current solution in each iteration, and the current solution plus the updated solution value is used to obtain a new solution. This method can ensure that the updated solution value solved under the first-order approximation minimizes the Euclidean L2 norm of the residual vector, thereby gradually approaching the optimal solution.

[0151] Specifically, the relative pose is partially accumulated and iterated using the updated solution value until the iterative loop condition is met to obtain the final relative pose of the real-time point cloud. The iterative loop condition is the number of iterations or the updated solution value of the relative pose, and either of them can be satisfied. Through the iterative process and partial cumulative update, the optimal solution can be gradually approached and the stability of the algorithm can be ensured.

[0152] S304: Determine the absolute position and posture of the real-time point cloud in the world coordinate system according to the relative position and posture of the real-time point cloud.

[0153] In this step, after obtaining the relative pose of the real-time point cloud, it is synthesized with the absolute pose of the reference point cloud to obtain the absolute pose of the real-time point cloud in the world coordinate system.

[0154] The method for determining the pose based on the laser radar point cloud provided in the embodiment of the present application adopts the Gauss-Newton nonlinear optimization algorithm to realize the accurate estimation of the relative pose of the real-time point cloud. First, the rationality judgment is performed on each feature point in the second feature point set, and the points that fail to pass the rationality judgment are eliminated to obtain the processed second feature point set. Then, the residual measurement of each feature point in the processed second feature point set is calculated to construct the Jacobian matrix. The Jacobian matrix is ​​used to construct an overdetermined linear equation, solve and update the solution value, and update the relative pose by partial accumulation iteration until the iterative loop condition is met to obtain the final relative pose. Finally, the absolute pose of the real-time point cloud in the world coordinate system is determined in combination with the absolute pose of the reference point cloud. Through the above method, the embodiment of the present application can effectively improve the accuracy and stability of the relative pose estimation of the real-time point cloud, and ensure that the absolute pose of the real-time point cloud in the world coordinate system is accurate and reliable.

[0155] Figure 4 A schematic diagram of the principle of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application is shown in FIG. Figure 4 As shown, the overall logic of this solution is:

[0156] Based on the input reference point cloud and real-time point cloud, according to the laser radar perception detection results, the dynamic targets in the reference point cloud and the real-time point cloud are filtered out. It should be noted that the dynamic targets in the reference point cloud are filtered out only at the start time of the method for determining the position and posture based on the laser radar point cloud. Then, based on the reference point cloud and the real-time point cloud with the dynamic targets filtered out, they are arranged into a two-dimensional matrix according to the laser radar scan line. It should be noted that the reference point cloud with the dynamic targets filtered out is arranged into a two-dimensional matrix according to the reference laser radar scan line only at the start time of the method for determining the position and posture based on the laser radar point cloud. Then, based on the roughness of each point in the reference point cloud and the real-time point cloud with the dynamic targets filtered out, the feature point set corresponding to the reference point cloud and the real-time point cloud is obtained. It should be noted that the feature point set corresponding to the reference point cloud is obtained only at the start time of the method for determining the position and posture based on the laser radar point cloud. Then, based on the feature point set corresponding to the reference point cloud and the real-time point cloud, the Gauss-Newton nonlinear optimization algorithm is used to align each feature point, and iteratively update to determine the relative position and posture of the real-time point cloud. Finally, the absolute pose of the reference point cloud and the relative pose of the real-time point cloud are synthesized to obtain the absolute pose of the real-time point cloud in the world coordinate system.

[0157] This method improves the accuracy and stability of relative pose estimation of real-time point clouds, ensuring that the absolute pose of real-time point clouds in the world coordinate system is accurate and reliable.

[0158] On the basis of the above embodiments, the present solution is further clearly and thoroughly described below in combination with a specific example. The example is only a typical representative of the present application and is illustrative, and does not represent the only one. The present application does not specifically limit the specific form and type involved in the example.

[0159] The input data for this example includes the reference point cloud Cloud ref , Pose parameters of the reference point cloud ref , the perception detection result of the benchmark point cloud Object ref , Real-time Point Cloud Ti , and the perception detection results of real-time point cloud Object Ti .

[0160] Among them, the pose parameter Pose of the reference point cloud ref Is a 6-dimensional vector, the first 3 dimensions represent the point cloud in x, y, z The translation in the axis direction, the last three dimensions represent the point cloud around x, y, z The rotation angle of the axis, which is composed of 3D position and 3D posture, Pose ref =[x ref y ref z ref p ref r ref h ref ]; the perception detection result is a set of cubes, Object = {o1 ... o i …o n}, where each element corresponds to a cube, representing a detected object, represented by the coordinates of the relative vertices of the lower left corner and the upper right corner of the detected object cube: in are the coordinates of the lower left corner of the detected object cube, are the coordinates of the upper right corner of the detected object cube; the point cloud matrix is ​​expressed as:

[0161]

[0162] In the formula, the dimension m of the point cloud matrix represents the number of scan lines of the laser radar, n represents the number of measurement points of each scan line, and each element in the matrix represents a measurement point of the laser radar. In the coordinate system of the body at the moment of laser radar measurement,

[0163] First, preprocessing is performed based on the reference point cloud and the real-time point cloud. Corresponding to the above embodiment: based on the reference point cloud and the real-time point cloud, dynamic targets in the point cloud are filtered out.

[0164] Filter out dynamic targets in real-time point cloud. Ti Each dot in ji , to determine whether the point does not belong to any detected object cube, that is, to determine the real-time point cloud Cloud Ti Each dot in ji Relative to Object Ti Every point o in i , whether the following three conditions are met at the same time:

[0165]

[0166] If the real-time point cloud Ti Each dot in ji Relative to Object Ti If any of the detected object elements in satisfies the above three conditions, then dot ji It is not a real-time point cloud perception detection object, it is a static point, real-time point cloud Ti All the static points in the real-time static point cloud set Cloud Static_Ti .

[0167] The implementation principle and technical effect of filtering out dynamic targets in the reference point cloud are the same as those of filtering out dynamic targets in the real-time point cloud, so we will not repeat them here. ref The baseline static point cloud set Cloud composed of all static points in Static_ref , dynamic targets in the reference point cloud are filtered out only at the start of the pose determination method based on the lidar point cloud.

[0168] Secondly, based on the benchmark static point cloud set Cloud Static_ref and real-time static point cloud collection Cloud Static_Ti , arranged into a two-dimensional matrix according to the laser radar scan line. Corresponding to the above embodiment: based on filtering out the reference point cloud and the real-time point cloud of the dynamic target, they are arranged into a two-dimensional matrix according to the reference laser radar scan line.

[0169] Specifically, the row dimension of the two-dimensional matrix is ​​the number of scan lines, and the column dimension is the number of points scanned and measured by each scan line in each frame. The row dimension and column dimension refer to the design parameters of the laser radar and are fixed values. It should be noted that only at the start of the laser radar point cloud-based pose determination method, the reference point cloud that filters out dynamic targets is arranged into a two-dimensional matrix according to the reference laser radar scan line. For each row of data in the point cloud matrix, it is arranged in the order of the reference laser radar measurement time, and each data includes but is not limited to three-dimensional coordinates.

[0170] Then, according to the benchmark static point cloud set Cloud Static_ref and real-time static point cloud collection Cloud Static_Ti The feature point is selected based on the roughness of each point in the image. This is equivalent to the above embodiment: based on the reference point cloud with dynamic targets filtered out, a first feature point set corresponding to the reference point cloud is obtained; based on the real-time point cloud with dynamic targets filtered out, a second feature point set corresponding to the real-time point cloud is obtained.

[0171] Figure 5 A schematic diagram of a specific implementation of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application, according to a real-time static point cloud set Cloud Static_Ti The schematic diagram of the process of selecting feature points for the roughness of each point is as follows Figure 5 As shown, specifically including:

[0172] Calculate real-time static point cloud collection Cloud Static_Ti The roughness of each point in is:

[0173] Step 1: Calculate the real-time static point cloud collection Cloud Static_Ti The distances from each point in the point cloud and the points before and after the point on the laser radar scanning line to the laser radar measurement center are calculated. This is equivalent to the above embodiment: for each point in the point cloud, the roughness of the point is calculated according to the first distance between the point and the laser radar measurement center, the second distance between at least one point before the point and the laser radar measurement center, and the third distance between at least one point after the point and the laser radar center. The roughness of each point is used to represent the relative distance between the point and the adjacent points.

[0174] For real-time static point cloud collection Cloud Static_Ti For each point in , calculate its distance to the laser radar measurement center, that is, the distance from this point to the origin of the laser radar body coordinate system at the current moment.

[0175]

[0176] Step 2: Calculate the real-time static point cloud set Cloud Static_Ti The difference between the distances from each point in the laser radar scan line to the laser radar measurement center and the points before and after the point. This is equivalent to the above embodiment: calculating the difference between the first distance and each second distance to obtain at least one first distance difference; calculating the difference between the first distance and each third distance to obtain at least one second distance difference; and calculating the roughness of the point based on at least one first distance difference and at least one second distance difference.

[0177] ΔDis ji-1 =Dis ji -Dis ji-1

[0178] …

[0179] ΔDis ji-5 =Dis ji -Dis ji-5

[0180] ΔDis ji+1 =Dis ji -Dis ji+1

[0181] …

[0182] ΔDis ji+5 =Dis ji -Dis ji+5

[0183] Without loss of generality, for example, we can select 5 points before and after the laser radar scanning line where the point is located as the basis for calculation, Dis ji Indicates the first distance. Dis ji-1 、…、Dis ji-5 They represent the distances from the first five points on the laser radar scanning line where the point is located to the laser radar measurement center, that is, the second distance. ji+1 、…、Dis ji+5 They represent the distances from the next five points on the laser radar scanning line where the point is located to the laser radar measurement center, that is, the third distance. ji-1 ,…,ΔDis ji-5 They represent the distance difference between the point and the five previous points on the laser radar scanning line to the laser radar measurement center, that is, the first distance difference. ΔDis ji+1 ,…,ΔDis ji+5 They represent the distance difference between the point and the five points behind it on the laser radar scanning line to the laser radar measurement center, that is, the second distance difference. It should be noted that for the first five points and the last five points on each laser radar scanning line, no roughness calculation is performed.

[0184] Step 3: Calculate the absolute value of the average of all distance differences as the real-time static point cloud set Cloud Static_Ti This is equivalent to the above embodiment: determining the absolute value of the average value of at least one first distance difference and at least one second distance difference as the roughness of the point.

[0185]

[0186] According to the real-time static point cloud collection Cloud Static_Ti The roughness of each point in the feature point selection:

[0187] The first case: When the real-time static point cloud collection CloudStatic_Ti When the roughness of a certain point in the real-time point cloud is greater than the convex feature point screening threshold, the point is incorporated into the convex feature point set. This is equivalent to the above embodiment: the second convex feature point set corresponding to the real-time point cloud includes points selected from the real-time point cloud whose relative distance to adjacent points is greater than the first preset distance.

[0188] Specifically, if Diff ji >Threshhold c , then click dot ji is a convex feature point. All convex feature points constitute the convex feature point set Cloud corresponding to the real-time point cloud. Convex_Ti , which is the second convex feature point set.

[0189] The second case: When the real-time static point cloud collection Cloud Static_Ti When the roughness of a certain point in the real-time point cloud is less than the flat feature point screening threshold, the point is incorporated into the flat feature point set. This is equivalent to the above embodiment: the second flat feature point set corresponding to the real-time point cloud includes points selected from the real-time point cloud whose relative distance to adjacent points is less than the second preset distance.

[0190] Specifically, if Diff ji <Threshhold s , then click dot ji is a flat feature point. All flat feature points constitute the flat feature point set Cloud corresponding to the real-time point cloud. Smooth_Ti , which is the second flat feature point set.

[0191] Similarly, for the benchmark static point cloud set Cloud Static_ref Each point in the convex feature point set Cloud corresponding to the reference point cloud is also screened according to the above process. The specific steps are not repeated here. Convex_ref And the flat feature point set Cloud corresponding to the reference point cloud Smooth_ref , that is, the first convex feature point set and the first flat feature point set.

[0192] It should be noted that the convex feature point screening threshold is greater than the flat feature point screening threshold, that is, the first preset distance is greater than the second preset distance, so for the real-time static point cloud collection Cloud Static_Ti And the benchmark static point cloud set Cloud Static_ref Each point in cannot belong to both the convex feature point set and the flat feature point set. And only at the start time of the pose determination method based on the laser radar point cloud, based on the reference point cloud that filters out dynamic targets, the first feature point set corresponding to the reference point cloud is obtained.

[0193] Next, based on the feature point set of the real-time point cloud, it is aligned with the feature point set of the reference point cloud to obtain the real-time relative pose. The alignment of the real-time point cloud and the reference point cloud is an iterative process. The relative pose of the real-time lidar point cloud relative to the reference lidar point cloud is updated once each iteration until the loop condition is met. The iterative update of the relative pose adopts the Gauss-Newton nonlinear optimization algorithm. This is equivalent to the above embodiment: according to the first feature point set and the second feature point set, the Gauss-Newton nonlinear optimization algorithm is used to determine the relative pose of the real-time point cloud.

[0194] The Gauss-Newton nonlinear optimization algorithm is divided into three steps: rationality judgment, Jacobian matrix construction, and optimization update.

[0195] In the rationality judgment, each point in the second convex feature point set and the second flat feature point set in the second feature point set is processed differently.

[0196] The first case: for each point in the second convex feature point set, the rationality of the point is judged based on the first convex feature point set.

[0197] Figure 6 Another flowchart of a specific implementation method of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application is shown in the flowchart of determining the rationality of each point in the second convex feature point set according to the first convex feature point set. Figure 6 As shown, specifically including:

[0198] Step 1: For each point in the second convex feature point set, a projection matrix is ​​constructed using the initial relative pose of the real-time point cloud, and the point is projected to the coordinate system of the reference point cloud to obtain the projection point of the point.

[0199] At the start-up time of the LiDAR point cloud-based pose determination method, that is, when the real-time point cloud relative pose is first updated, the initial relative pose is 0: Pose Rel =[000000]; After the posture determination method based on the laser radar point cloud is started, that is, when the relative posture of the real-time point cloud is not updated for the first time, the initial relative posture is the relative posture of the previous real-time point cloud:

[0200] Pose Rel =[x Rel y Rel z Rel p Rel r Rel h Rel ]

[0201] Construct the projection matrix using the initial relative pose of the real-time point cloud:

[0202]

[0203] Use the projection matrix to transform the convex feature point set Cloud corresponding to the real-time point cloud Convex_Ti And the flat feature point set Cloud corresponding to the real-time point cloud Smooth_Ti Each point in is projected into the reference point cloud coordinate system according to the following formula to obtain the projection point of the point:

[0204]

[0205] Step 2: Search from the first convex feature point set for at least three target points whose distance to the projection point of this point is the smallest.

[0206] The convex feature point set Cloud corresponding to the reference point cloud Convex_Ti In the search, search for at least three target points with the smallest distance from the projection point of the point. For example, search for the projection point dot To_Ref_ji The 5 points with the smallest Euclidean distance are denoted as: dot Ref_ji_1 、dot Ref_ji_2 、dot Ref_ji_3 、dot Ref_ji_4 、dot Ref_ji_5 .

[0207] Step 3: Construct a symmetric matrix based on at least three target points, and determine whether at least three target points are on the same straight line based on the properties of the symmetric matrix principal component significance.

[0208] (1) Construct a symmetric matrix based on at least three target points.

[0209] First calculate the coordinates of the center points of at least three target points:

[0210]

[0211] By dot Ref_ji_1 、dot Ref_ji_2 、dot Ref_ji_3 、dot Ref_ji_4 、dot Ref_ji_5 Calculate V according to the following formula Ref_ji_1 、V Ref_ji_2 、V Ref_ji_3 、V Ref_ji_4 、V Ref_ji_5 :

[0212]

[0213] Then the symmetric matrix is:

[0214]

[0215] (2) Calculate the real eigenvalues ​​of the symmetric matrix and determine whether at least three target points are on the same straight line.

[0216] You can use open source library functions such as OpenCV to calculate the eigenvalues ​​of a symmetric matrix. The unit vector with the largest absolute value is V. feature , V feature The corresponding eigenvalue is λ max .

[0217] If max The absolute value of max The eigenvalue λ second The absolute value of |λ max |>10|λ second |, then it is considered that at least three target points meet the straightness condition, and these five target points are on the same straight line. Then the point passes the rationality judgment and is used for the subsequent real-time point cloud relative pose optimization update. max The absolute value of max The eigenvalue λ second If the absolute value of is less than , it is considered that at least three target points do not meet the straightness condition. If these five target points are not on the same straight line, the point fails to pass the rationality judgment and is excluded from the subsequent real-time point cloud relative pose optimization update.

[0218] The second case: for each point in the second flat feature point set, the rationality of the point is judged according to the first flat feature point set.

[0219] Figure 7 Another flowchart of a specific implementation method of a method for determining a position and posture based on a laser radar point cloud provided in an embodiment of the present application, wherein a flowchart of determining the rationality of each point in the second flat feature point set according to the first flat feature point set is shown in FIG. Figure 7 As shown, specifically including:

[0220] Step 1: For each point in the second flat feature point set, use the initial relative pose of the real-time point cloud to construct a projection matrix, project the point to the coordinate system of the reference point cloud, and obtain the projection point of the point.

[0221] The implementation principle and technical effect of obtaining the projection point of each point in the flat feature point set corresponding to the real-time point cloud are consistent with the implementation principle and technical effect of obtaining the projection point of each point in the convex feature point set corresponding to the real-time point cloud, and will not be repeated here.

[0222] Step 2: Search for at least four target points with the shortest distance to the projection point of the point from the first flat feature point set.

[0223] The flat feature point set Cloud corresponding to the reference point cloud Smooth_TiIn the search, search for at least four target points with the smallest distance from the projection point of the point. For example, search for the projection point dot To_Ref_ji The 5 points with the smallest Euclidean distance are denoted as: dot Ref_ji_1 、dot Ref_ji_2 、dot Ref_ji_3 、dot Ref_ji_4 、dot Ref_ji_5 .

[0224] Step 3: Construct a plane equation based on at least four target points and calculate the residual from the target point to the plane to determine whether the target points are on the same plane.

[0225] (1) Construct a plane equation based on at least four target points.

[0226] First, construct the surface point matrix corresponding to the target point:

[0227]

[0228] Construct the equation of a plane:

[0229]

[0230] And solve the normal vector V of the plane equation by calling the open source library OpenCV vertical .

[0231] (2) Calculate the residual from the target point to the plane and determine whether the target points are on the same plane.

[0232]

[0233] If Q < 2, it is considered that at least four target points meet the flatness condition, that is, the five target points are on the same plane, and the point passes the rationality judgment and is used for the subsequent real-time point cloud relative pose optimization update; if Q ≥ 2, it is considered that at least four target points do not meet the flatness condition, that is, the five target points are not on the same plane, and the point fails to pass the rationality judgment and is excluded from the subsequent real-time point cloud relative pose optimization update.

[0234] Eliminate the convex feature point set Cloud corresponding to the real-time point cloud Convex_Ti And the flat feature point set Cloud corresponding to the real-time point cloud Smooth_Ti The points that have not passed the rationality judgment in the second convex feature point set and the points that have not passed the rationality judgment in the second flat feature point set are eliminated to obtain the processed second feature point set.

[0235] In constructing the Jacobian matrix, the elements in the Jacobian matrix are the partial derivatives of the residuals of the feature points in the second feature point set after processing of the real-time point cloud with respect to the relative pose of the real-time point cloud. The residual measurement is calculated differently for each point in the second convex feature point set and the second flat feature point set in the second feature point set.

[0236] Figure 8 Another flowchart of a specific implementation method of a method for determining a pose based on a laser radar point cloud provided in an embodiment of the present application, constructing a Jacobian matrix, updating and iterating the relative pose of the real-time point cloud, and obtaining a flowchart of the relative pose of the real-time point cloud is as follows: Figure 8 As shown, specifically including:

[0237] The first case: the residual measurement of each point in the second convex feature point set in the processed second feature point set is the length of the perpendicular segment between the point and the straight line on which at least three target points are located, and the at least three target points are at least three points in the first convex feature point set with the smallest distance from the projection point of the point in the reference point cloud coordinate system.

[0238] Step 1: Calculate the upper and lower vertical point vectors of at least three target points.

[0239]

[0240]

[0241] Step 2: Calculate the connection vector from the projection point to the upper and lower vertical points.

[0242] V To_up_ji =dot To_Ref_ji -V feature_up

[0243] V To_down_ji =dot To_Ref_ji -V feature_down

[0244] Step 3: Calculate the residual measurement of each point in the second convex feature point set in the processed second feature point set.

[0245]

[0246] Among them, × represents the cross product of the vector, and | | represents the modulus of the second norm of the orientation vector.

[0247] The second case: the residual measurement of each point in the second flat feature point set in the processed second feature point set is the length of the foot of the perpendicular from the point to the plane where at least four target points are located, and the at least four target points are at least four points in the first flat feature point set with the smallest distance from the projection point of the point in the reference point cloud coordinate system.

[0248] The residual measurement of each point in the second flat feature point set in the processed second feature point set is:

[0249]

[0250] Here, · represents the dot product of vectors.

[0251] The total residual measurement is determined according to the residual measurement of each point in the second convex feature point set in the processed second feature point set and the residual measurement of each point in the second flat feature point set in the processed second feature point set. The partial derivative of the total residual measurement relative to the initial relative pose of the real-time point cloud is calculated to obtain the Jacobian matrix.

[0252] The total residual is measured as:

[0253]

[0254] Construct the Jacobian matrix to measure the total residual error relative to the initial relative pose Pose of the real-time point cloud Rel =[x Rel y Rel z Rel p Rel r Rel h Rel ]Calculate partial derivatives:

[0255]

[0256] Take the partial derivative vector h of each residual vector ji Is a row vector, compressed to get the Jacobian matrix:

[0257]

[0258] In the optimization update, the relative pose of the real-time point cloud is updated and iterated according to the Jacobian matrix to obtain the relative pose of the real-time point cloud.

[0259] Step 1: Use the Jacobian matrix to construct a set of overdetermined linear equations to solve the updated solution of the relative posture.

[0260] ΔPose Rel =(H T H) -1 H T Z

[0261] Step 2: Use the updated solution value to perform partial cumulative iteration on the relative pose until the iterative loop conditions are met to obtain the final relative pose of the real-time point cloud.

[0262] Pose Rel =Pose Rel +λ·ΔPose Rel

[0263] If |ΔPose Rel_f |<Threshold_f and |ΔPose Rel_b |<Threshold_b or the number of iteration updates is greater than Count update , then exit the iterative update of the relative position of the real-time point cloud at the current moment. Rel_f | is ΔPose Rel_f The modulus of the first three elements, |ΔPose Rel_b | is ΔPose Rel_b The modulus of the last three elements, threshold Threshold_f = 0.02, Threshold_b = 10 -4 , iterative update threshold times Count update = 20. The update step size λ = 0.1.

[0264] Finally, the relative pose of the real-time point cloud is obtained and synthesized with the pose of the reference point cloud in the previous step to obtain the absolute pose of the real-time point cloud relative to the world coordinate system. This is equivalent to the above embodiment: according to the relative pose of the real-time point cloud, the absolute pose of the real-time point cloud in the world coordinate system is determined.

[0265] After the real-time point cloud relative pose is iteratively updated, the reference point cloud pose parameter Pose is used ref Relative pose to real-time point cloud Rel Synthesize and obtain the absolute pose of the real-time point cloud now .

[0266] This example screens the input reference point cloud and real-time point cloud to obtain the feature point set corresponding to the reference point cloud and the real-time point cloud, uses the Gauss-Newton nonlinear optimization algorithm to align each feature point in the feature point set corresponding to the reference point cloud and the real-time point cloud, iteratively updates and determines the relative pose of the real-time point cloud, and then obtains the absolute pose of the real-time point cloud in the world coordinate system. Through the above method, the accuracy and stability of the relative pose estimation of the real-time point cloud are improved, ensuring that the absolute pose of the real-time point cloud in the world coordinate system is accurate and reliable.

[0267] Fig. 9 A schematic diagram of a structure of an embodiment of a position determination device based on a laser radar point cloud provided in an embodiment of the present application, such as Fig. 9 As shown, the position and posture determination device 900 based on the laser radar point cloud includes:

[0268] The first processing module 901 is used to obtain a first feature point set corresponding to the reference point cloud, the first feature point set including: points selected from the reference point cloud and having a relative distance from adjacent points greater than a first preset distance, and points selected from the reference point cloud and having a relative distance from adjacent points less than a second preset distance, the first preset distance being greater than the second preset distance.

[0269] The second processing module 902 is used to obtain a second feature point set corresponding to the real-time point cloud, and the second feature point set includes: points selected from the real-time point cloud and whose relative distance to adjacent points is greater than a first preset distance, and points selected from the real-time point cloud and whose relative distance to adjacent points is less than a second preset distance.

[0270] The third processing module 903 is used to determine the relative position and posture of the real-time point cloud according to the first feature point set and the second feature point set.

[0271] The present embodiment provides a laser radar point cloud-based posture determination device for executing the technical solution in the aforementioned method embodiment, and its implementation principle and technical effect are similar and will not be repeated here.

[0272] On the basis of the above embodiment, the apparatus 900 for determining a position and posture based on a laser radar point cloud further includes a fourth processing module 904, which is used to:

[0273] Filter out dynamic objects in the fiducial point cloud and filter out dynamic objects in the live point cloud.

[0274] In a specific implementation, for the reference point cloud or the real-time point cloud, a feature point set corresponding to the point cloud is obtained, that is, the first processing module 901 and the second processing module 902 are specifically used to:

[0275] For each point in the point cloud, the roughness of the point is calculated according to a first distance between the point and a laser radar measurement center, a second distance between at least one point before the point and the laser radar measurement center, and a third distance between at least one point after the point and the laser radar center, wherein the roughness of each point is used to represent a relative distance between the point and adjacent points;

[0276] Calculate the difference between the first distance and each second distance to obtain at least one first distance difference; calculate the difference between the first distance and each third distance to obtain at least one second distance difference; calculate the roughness of the point according to at least one first distance difference and at least one second distance difference;

[0277] Determine the absolute value of the average of at least one first distance difference and at least one second distance difference as the roughness of the point;

[0278] According to the roughness of each point in the point cloud, points with a roughness greater than a first preset distance and points with a roughness less than a second preset distance are screened out to obtain a set of feature points corresponding to the point cloud.

[0279] In a specific implementation, the third processing module 903 is specifically used to:

[0280] According to the first feature point set and the second feature point set, a Gauss-Newton nonlinear optimization algorithm is used to determine the relative position and pose of the real-time point cloud.

[0281] In a specific implementation, the third processing module 903 is specifically used to:

[0282] Performing rationality judgment on each point in the second feature point set, eliminating points that fail the rationality judgment, and obtaining a processed second feature point set;

[0283] Constructing a Jacobian matrix according to the processed second feature point set and the first feature point set;

[0284] According to the Jacobian matrix, the relative pose of the real-time point cloud is updated and iterated to obtain the relative pose of the real-time point cloud.

[0285] In a specific implementation, the third processing module 903 is specifically used to:

[0286] The first feature point set includes a first convex feature point set and a first flat feature point set, the first convex feature point set includes points selected from the reference point cloud and whose relative distances to adjacent points are greater than a first preset distance; the first flat feature point set includes points selected from the reference point cloud and whose relative distances to adjacent points are less than a second preset distance;

[0287] The second feature point set includes a second convex feature point set and a second flat feature point set, the second convex feature point set includes points selected from the real-time point cloud and having a relative distance from adjacent points greater than a first preset distance; the second flat feature point set includes points selected from the real-time point cloud and having a relative distance from adjacent points less than a second preset distance;

[0288] According to the first convex feature point set, the rationality of each point in the second convex feature point set is judged;

[0289] According to the first flat feature point set, the rationality of each point in the second flat feature point set is judged;

[0290] Points that fail to pass the rationality judgment in the second convex feature point set and points that fail to pass the rationality judgment in the second flat feature point set are eliminated to obtain a processed second feature point set.

[0291] In a specific implementation, the third processing module 903 is specifically used to:

[0292] For each point in the second convex feature point set, project the point to the coordinate system of the reference point cloud to obtain the projection point of the point;

[0293] Searching for at least three target points with the smallest distance from the projection point of the point from the first convex feature point set, and determining whether the at least three target points are on the same straight line;

[0294] If at least three target points are on the same straight line, the point passes the rationality judgment; if at least three target points are not on the same straight line, the point fails the rationality judgment.

[0295] In a specific implementation, the third processing module 903 is specifically used to:

[0296] For each point in the second flat feature point set, project the point to the coordinate system of the reference point cloud to obtain the projection point of the point;

[0297] Searching for at least four target points whose distances from the projection point of the point are the smallest from the first plane feature point set, and determining whether the at least four target points are on the same plane;

[0298] If at least four target points are on the same plane, the point passes the rationality judgment; if at least four target points are not on the same plane, the point fails the rationality judgment.

[0299] In a specific implementation, the third processing module 903 is specifically used to:

[0300] According to the first feature point set, the residual measurement of each point in the processed second feature point set is calculated, wherein the residual measurement of each point in the second convex feature point set in the processed second feature point set is the length of the perpendicular segment between the point and the straight line where at least three target points are located, and the at least three target points are at least three points in the first convex feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system. The residual measurement of each point in the second flat feature point set in the processed second feature point set is the length of the foot of the perpendicular from the point to the plane where at least four target points are located, and the at least four target points are at least four points in the first flat feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system;

[0301] The Jacobian matrix is ​​constructed based on the residual measurement of each point in the processed second feature point set.

[0302] In a specific implementation, the third processing module 903 is further specifically configured to:

[0303] According to the relative pose of the real-time point cloud, the absolute pose of the real-time point cloud in the world coordinate system is determined.

[0304] The laser radar point cloud-based posture determination device provided in any of the above embodiments is used to execute the technical solution in the aforementioned method embodiment, and its implementation principle and technical effect are similar and will not be repeated here.

[0305] Fig.10 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application, such as Fig.10 As shown, the embodiment of the present application also provides a computer device, including:

[0306] Processor, memory and communication interface;

[0307] Memory stores computer-executable instructions;

[0308] The processor executes the computer execution instructions stored in the memory, so that the processor executes the technical solution provided in the aforementioned method embodiment. The implementation principle and technical effect are similar and will not be repeated here.

[0309] An embodiment of the present application provides a computer-readable storage medium, in which computer execution instructions are stored. When the computer execution instructions are executed by a processor, they are used to implement the technical solution in the aforementioned method embodiment. The implementation principle and technical effect are similar and will not be repeated here.

[0310] The embodiment of the present application also provides a computer program product, including a computer program. When the computer program is executed by a processor, the technical solution in the aforementioned method embodiment is implemented. The implementation principle and technical effect are similar and will not be repeated here.

[0311] The computer-readable storage medium mentioned above can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory, electrically erasable programmable read-only memory, erasable programmable read-only memory, programmable read-only memory, read-only memory, magnetic memory, flash memory, magnetic disk or optical disk. The readable storage medium can be any available medium that can be accessed by a general or special purpose computer.

[0312] Optionally, a readable storage medium is coupled to a processor so that the processor can read information from the readable storage medium and write information to the readable storage medium. Of course, the readable storage medium can also be a component of the processor. The processor and the readable storage medium can be located in an application specific integrated circuit (ASIC). Of course, the processor and the readable storage medium can also exist in the device as discrete components.

[0313] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the invention disclosed herein. The present application is intended to cover any modification, use or adaptation of the present application, which follows the general principles of the present application and includes common knowledge or customary techniques in the art that are not disclosed in the present application. The specification and examples are intended to be exemplary only, and the true scope and spirit of the present application are indicated by the following claims.

[0314] It should be understood that the present application is not limited to the precise structures that have been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present application is limited only by the appended claims.

Claims

1. A method for determining a position and posture based on a laser radar point cloud, characterized in that: include: Acquire a first feature point set corresponding to a reference point cloud, wherein the first feature point set includes: points selected from the reference point cloud whose relative distances to neighboring points are greater than a first preset distance, and points selected from the reference point cloud whose relative distances to neighboring points are less than a second preset distance, wherein the first preset distance is greater than the second preset distance; Acquire a second feature point set corresponding to the real-time point cloud, the second feature point set including: points selected from the real-time point cloud whose relative distances to neighboring points are greater than the first preset distance, and points selected from the real-time point cloud whose relative distances to neighboring points are less than the second preset distance; The relative position and posture of the real-time point cloud is determined according to the first feature point set and the second feature point set.

2. The method according to claim 1, characterized in that Before obtaining the first feature point set corresponding to the reference point cloud, the method further includes: filtering out dynamic targets in the reference point cloud; Before obtaining the second feature point set corresponding to the real-time point cloud, the method further includes: Dynamic objects in the real-time point cloud are filtered out.

3. The method according to claim 1 or 2, characterized in that: For the reference point cloud or the real-time point cloud, obtaining a set of feature points corresponding to the point cloud includes: For each point in the point cloud, calculate the roughness of the point according to a first distance between the point and a laser radar measurement center, a second distance between at least one point before the point and the laser radar measurement center, and a third distance between at least one point after the point and the laser radar center, wherein the roughness of each point is used to represent a relative distance between the point and adjacent points; According to the roughness of each point in the point cloud, points whose roughness is greater than the first preset distance and points whose roughness is less than the second preset distance are screened out to obtain a set of feature points corresponding to the point cloud.

4. The method according to claim 3, characterized in that The calculating the roughness of the point according to a first distance between the point and a laser radar measurement center, a second distance between at least one point before the point and the laser radar measurement center, and a third distance between at least one point after the point and the laser radar center includes: Calculating a difference between the first distance and each second distance to obtain at least one first distance difference; Calculating a difference between the first distance and each third distance to obtain at least one second distance difference; The roughness of the point is calculated based on the at least one first distance difference and the at least one second distance difference.

5. The method according to claim 4, characterized in that The calculating the roughness of the point according to the at least one first distance difference and the at least one second distance difference comprises: An absolute value of an average value of the at least one first distance difference and the at least one second distance difference is determined as the roughness of the point.

6. The method according to claim 1 or 2, characterized in that: The determining the relative position and posture of the real-time point cloud according to the first feature point set and the second feature point set includes: According to the first feature point set and the second feature point set, a Gauss-Newton nonlinear optimization algorithm is used to determine the relative position and posture of the real-time point cloud.

7. The method according to claim 6, characterized in that The determining the relative position and posture of the real-time point cloud by using a Gauss-Newton nonlinear optimization algorithm according to the first feature point set and the second feature point set includes: Performing rationality judgment on each point in the second feature point set, eliminating points that fail the rationality judgment, and obtaining a processed second feature point set; Constructing a Jacobian matrix according to the processed second feature point set and the first feature point set; According to the Jacobian matrix, the relative position and posture of the real-time point cloud is updated and iterated to obtain the relative position and posture of the real-time point cloud.

8. The method according to claim 7, characterized in that The first feature point set includes a first convex feature point set and a first flat feature point set, the first convex feature point set includes points selected from the reference point cloud and whose relative distances to adjacent points are greater than the first preset distance; the first flat feature point set includes points selected from the reference point cloud and whose relative distances to adjacent points are less than the second preset distance; The second feature point set includes a second convex feature point set and a second flat feature point set, the second convex feature point set includes points selected from the real-time point cloud and whose relative distances to adjacent points are greater than the first preset distance; the second flat feature point set includes points selected from the real-time point cloud and whose relative distances to adjacent points are less than the second preset distance; Correspondingly, performing rationality judgment on each point in the second feature point set, removing points that fail the rationality judgment, and obtaining a processed second feature point set includes: According to the first convex feature point set, performing rationality judgment on each point in the second convex feature point set; According to the first flat feature point set, performing rationality judgment on each point in the second flat feature point set; Points that fail to pass the rationality judgment in the second convex feature point set and points that fail to pass the rationality judgment in the second flat feature point set are eliminated to obtain the processed second feature point set.

9. The method according to claim 8, characterized in that The step of judging the rationality of each point in the second set of convex feature points according to the first set of convex feature points includes: For each point in the second convex feature point set, project the point to the coordinate system of the reference point cloud to obtain a projection point of the point; Searching for at least three target points having the smallest distance from the projection point of the point from the first convex feature point set, and determining whether the at least three target points are on the same straight line; If the at least three target points are on the same straight line, the points pass the rationality judgment; If the at least three target points are not on the same straight line, the points fail the rationality determination.

10. The method according to claim 8, characterized in that The step of judging the rationality of each point in the second flat feature point set according to the first flat feature point set includes: For each point in the second flat feature point set, project the point to the coordinate system of the reference point cloud to obtain a projection point of the point; Searching for at least four target points having the smallest distance from the projection point of the point from the first plane feature point set, and determining whether the at least four target points are on the same plane; If the at least four target points are on the same plane, the points pass the rationality judgment; If the at least four target points are not on the same plane, the points fail to pass the rationality determination.

11. The method according to any one of claims 8 to 10, characterized in that: The constructing a Jacobian matrix according to the processed second feature point set and the first feature point set includes: According to the first feature point set, the residual measurement of each point in the processed second feature point set is calculated, wherein the residual measurement of each point in the second convex feature point set in the processed second feature point set is the length of the perpendicular segment between the point and the straight line where at least three target points are located, and the at least three target points are at least three points in the first convex feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system; the residual measurement of each point in the second flat feature point set in the processed second feature point set is the length of the foot of the perpendicular from the point to the plane where at least four target points are located, and the at least four target points are at least four points in the first flat feature point set that have the smallest distance from the projection point of the point in the reference point cloud coordinate system; The Jacobian matrix is ​​constructed based on the residual measurement of each point in the processed second feature point set.

12. The method according to claim 1 or 2, characterized in that: The method further comprises: According to the relative position and pose of the real-time point cloud, the absolute position and pose of the real-time point cloud in the world coordinate system is determined.

13. A device for determining a position and posture based on a laser radar point cloud, characterized in that: include: A first processing module is used to obtain a first feature point set corresponding to a reference point cloud, wherein the first feature point set includes: points selected from the reference point cloud and having a relative distance from adjacent points greater than a first preset distance, and points selected from the reference point cloud and having a relative distance from adjacent points less than a second preset distance, wherein the first preset distance is greater than the second preset distance; A second processing module is used to obtain a second feature point set corresponding to the real-time point cloud, wherein the second feature point set includes: points selected from the real-time point cloud whose relative distances to neighboring points are greater than the first preset distance, and points selected from the real-time point cloud whose relative distances to neighboring points are less than the second preset distance; The third processing module is used to determine the relative position and posture of the real-time point cloud according to the first feature point set and the second feature point set.

14. An electronic device, characterized in that: include: Processor, memory and communication interface; The memory stores computer-executable instructions; The processor executes the computer-executable instructions stored in the memory, so that the processor executes a method for determining a posture based on a laser radar point cloud as described in any one of claims 1 to 12.

15. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement a method for determining a posture based on a laser radar point cloud as described in any one of claims 1 to 12.

16. A computer program product, characterized in that It comprises a computer program, which, when executed by a processor, implements a method for determining a posture based on a laser radar point cloud as described in any one of claims 1 to 12.

Images