A centering detection method, system and medium based on 3D laser radar

Abstract

This invention relates to a centering detection method, system, and medium based on 3D LiDAR, belonging to the field of object detection technology. It establishes a base plane equation based on a constructed three-dimensional global coordinate system and obtains the parameter matrix of the base plane; sets a first pose transformation matrix and a second pose transformation matrix, where each pose transformation matrix includes a unit length of translation along the x-axis, y-axis, and z-axis in sequence, and rotation angles around the x-axis, y-axis, and z-axis; acquires a target point cloud based on 3D LiDAR; optimizes the first pose transformation matrix based on the ground point cloud, and optimizes the second pose transformation matrix based on the wall point cloud; obtains the ground plane parameter matrix based on the optimized first pose transformation matrix, thus obtaining the ground plane equation; and obtains the vertical plane parameter matrix based on the optimized second pose transformation matrix, thus obtaining the vertical plane equation. This application's solution reduces the complexity of setup and calculation, and improves the accuracy of road surface prediction.

Description

Technical Field

[0001] The present invention relates to the technical field of object detection based on 3D laser radar, and in particular to a centering detection method, system and medium based on 3D laser radar. Background Art

[0002] The detection of the centering of the road surface on the route refers to the process of detecting and adjusting the position of the target object or itself in a dynamic or static environment so that it is located at a certain reference center. In autonomous driving and robot navigation, accurate centering detection can improve the accuracy and safety of path planning. At present, the commonly used centering detection methods include vision-based detection, ultrasonic detection, and infrared detection. Ultrasonic sensors are low in price but also have low accuracy, which will be affected by air temperature, humidity, etc.; infrared sensors are low in cost but have the disadvantage of short measurement distance; for depth cameras, light is an unfavorable condition that is difficult to overcome. It can be seen that these methods are often affected by some external factors in complex environments, and it is difficult to provide stable and high-precision detection results.

[0003] 3D laser radar (LiDAR) measures the distance between an object and the sensor by emitting a laser beam and receiving its reflected signal, thereby obtaining high-precision three-dimensional point cloud data. These data contain the shape and relative position of objects in the environment, and are characterized by high resolution and high precision. Compared with other sensors, 3D laser radar is basically unaffected by lighting and weather conditions, and can provide stable perception capabilities in various complex environments. This sensor has been widely used in fields such as autonomous driving, drones, and robot navigation. Its high precision and real-time performance make it a key tool for environmental perception.

[0004] However, the current 3D LiDAR-based centering detection scheme has low accuracy in road surface prediction. Summary of the invention

[0005] The technical problem to be solved by the present application is to provide a centering detection method, system and medium based on 3D laser radar, which has the characteristic of improving the accuracy of road surface prediction.

[0006] In a first aspect, an embodiment provides a centering detection method based on a 3D laser radar, including:

[0007] Based on the constructed three-dimensional space global coordinate system, a base plane equation is established, and a parameter matrix of the base plane is obtained;

[0008] A first pose conversion matrix and a second pose conversion matrix are set, wherein the first pose conversion matrix is ​​used to convert the base plane parameter matrix into a ground parameter matrix, and the second pose conversion matrix is ​​used to convert the base plane parameter matrix into a vertical plane parameter matrix; wherein any pose conversion matrix includes, in order, a unit length of translation along the x-axis, a unit length of translation along the y-axis, a unit length of translation along the z-axis, an angle of rotation around the x-axis, an angle of rotation around the y-axis, and an angle of rotation around the z-axis;

[0009] Acquire a target point cloud based on a 3D laser radar, wherein the target point cloud includes a ground point cloud and a wall point cloud;

[0010] The first pose conversion matrix is ​​optimized based on the ground point cloud, and the second pose conversion matrix is ​​optimized based on the wall point cloud;

[0011] Based on the optimized first posture conversion matrix, the ground plane parameter matrix is ​​obtained, and then the ground plane equation is obtained; based on the optimized second posture conversion matrix, the vertical plane parameter matrix is ​​obtained, and then the vertical plane equation is obtained;

[0012] The ground centering detection is realized based on the ground plane equation and the vertical plane equation;

[0013] Among them, the method for optimizing any posture transformation matrix includes:

[0014] Calculate the distance from each point in the corresponding point cloud type to the plane obtained after optimization as the first distance;

[0015] Calculate the cost optimization function of the first distance corresponding to all points in the corresponding point cloud type;

[0016] Based on the cost optimization function, a least squares iterative solution is performed to obtain an optimized posture transformation matrix that meets the solution requirements.

[0017] In the second aspect, an embodiment provides a centering detection system based on a 3D laser radar, including a 3D laser radar, a first memory and a first processor arranged on a vehicle body; wherein the 3D laser radar and the first memory are respectively connected to the first processor, the 3D laser radar is used to obtain a target point cloud, the target point cloud includes a ground point cloud and a wall point cloud, a program is stored in the first memory, and the first processor is used to load the program stored in the first memory and execute the centering detection method based on the 3D laser radar described in any one of the above embodiments.

[0018] In a third aspect, an embodiment provides a computer-readable storage medium, in which a program is stored, and the program can be loaded by a processor to execute the centering detection method based on 3D laser radar described in any one of the above embodiments.

[0019] The beneficial effects of the present invention are:

[0020] Since the pose matrix used for plane transformation includes the unit length of translation along the x-axis, the unit length of translation along the y-axis, the unit length of translation along the z-axis, the angle of rotation around the x-axis, the angle of rotation around the y-axis, and the angle of rotation around the z-axis in order, when performing plane transformation, since the matrix includes the translation amount of the plane, the base plane can be moved up, down, left, and right. Since the matrix includes the rotation angle of the plane, not only can the base plane be moved up, down, left, and right to obtain a second plane, but it can also be rotated at any angle to obtain a more accurate second plane. At the same time, based on this form of pose transformation matrix, only one base plane needs to be set, and the corresponding plane can be obtained according to the corresponding pose transformation matrix, which reduces the complexity of setting and calculation.

[0021] In addition, it includes a method for optimizing the pose transformation matrix, by first calculating the distance from each point in the corresponding point cloud type to the optimized plane as the first distance, then calculating the cost optimization function of the first distance corresponding to all points in the corresponding point cloud type, and then performing the least squares iterative solution based on the cost optimization function to obtain the optimized pose transformation matrix that meets the solution requirements, so that a more accurate pose transformation matrix can be obtained, thereby obtaining a more accurate plane equation. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 is a schematic diagram of a centering detection system based on a 3D laser radar according to an embodiment of the present application;

[0023] Figure 2 This is a schematic diagram of a centering detection method based on a 3D laser radar according to an embodiment of the present application;

[0024] Figure 3 This application Figure 2 A method flow diagram of an embodiment of step S30;

[0025] Figure 4 It is a top view schematic diagram of obtaining a point cloud of a target area including the ground and the wall based on a 3D laser radar in one embodiment of the present application;

[0026] Figure 5 It is a schematic diagram of a method flow for optimizing any posture transformation matrix according to an embodiment of the present application;

[0027] Figure 6 This application Figure 2 A method flow diagram of an embodiment of step S20;

[0028] Figure 7This is a schematic diagram of a wall section detection method flow chart of an embodiment of the present application;

[0029] Figure 8 is a schematic diagram of a centering detection system based on a 3D laser radar according to another embodiment of the present application;

[0030] Fig. 9 It is a schematic diagram of obtaining the fifth distance based on a constructed two-dimensional coordinate system according to an embodiment of the present application.

[0031] In the diagram, 01, 3D laser radar, 02, first memory, 03, first processor, 04, target area, 05, single-line laser radar, 06, second memory, 07, second processor, 08, wall. DETAILED DESCRIPTION

[0032] The present invention is further described in detail below by specific embodiments in conjunction with the accompanying drawings. Wherein similar elements in different embodiments adopt associated similar element numbers. In the following embodiments, many detailed descriptions are for making the present application better understood. However, those skilled in the art can easily recognize that some features can be omitted in different situations, or can be replaced by other elements, materials, methods. In some cases, some operations related to the present application are not shown or described in the specification, this is to avoid the core part of the present application being overwhelmed by too much description, and for those skilled in the art, it is not necessary to describe these related operations in detail, and they can fully understand the related operations according to the description in the specification and the general technical knowledge in the art.

[0033] In addition, the features, operations or characteristics described in the specification can be combined in any appropriate manner to form various implementations. At the same time, the steps or actions in the method description can also be interchanged or adjusted in a manner that is obvious to those skilled in the art. Therefore, the various sequences in the specification and the drawings are only for the purpose of clearly describing a certain embodiment and are not meant to be a required sequence, unless otherwise specified that a certain sequence must be followed.

[0034] The serial numbers assigned to the components herein, such as "first", "second", etc., are only used to distinguish the objects described and do not have any order or technical meaning. The "connection" and "coupling" mentioned in this application, unless otherwise specified, include direct and indirect connections (couplings).

[0035] To facilitate the description of the inventive concept of the present application, the object detection technology based on 3D lidar is briefly described below.

[0036] In current technology, when obtaining the equation of the ground plane and the equation of the vertical plane where the wall is located, the usual method is to establish the base plane of the ground plane and the base plane of the vertical plane, and ideally assume that the ground plane and its corresponding base plane are parallel, and the vertical plane and its corresponding base plane are also parallel. Therefore, the corresponding target plane can be obtained by simply translating the established base plane.

[0037] However, the applicant has found in research that the established base plane and its corresponding target plane are often not strictly parallel, so if only translation is performed, the accurate target plane cannot be obtained.

[0038] In view of this, in the centering detection method based on 3D laser radar provided in the embodiment of the present application, since the pose matrix for plane transformation includes x The unit length of the axis translation, along y The unit length of the axis translation, along z The unit length of the axis translation, around x Angle of rotation around the axis y The angle of rotation around the axis, and z The angle of axis rotation, so that when performing plane transformation, since the matrix includes the plane translation, the base plane can be moved up, down, left, and right. Since the matrix includes the plane rotation angle, it is not only possible to move the base plane up, down, left, and right to obtain the second plane, but also to rotate it at any angle to obtain a more accurate second plane. At the same time, based on this form of posture transformation matrix, only one base plane needs to be set, and the corresponding plane can be obtained according to the corresponding posture transformation matrix, which reduces the complexity of setting and calculation.

[0039] In addition, in order to obtain a more accurate target plane, the present application also provides a method for optimizing the pose transformation matrix, by first calculating the distance from each point in the corresponding point cloud type to the optimized plane as the first distance, and then calculating the cost optimization function of the first distance corresponding to all points in the corresponding point cloud type, and then performing the least squares iterative solution based on the cost optimization function to obtain the optimized pose transformation matrix that meets the solution requirements, so that a more accurate pose transformation matrix can be obtained, thereby obtaining a more accurate plane equation.

[0040] Third, in order to obtain more accurate ground plane equations at each position point to achieve more accurate road centering detection, in an embodiment of the present application, for the first pose conversion matrix and the second pose conversion matrix at any moment other than the first moment, the optimized first pose conversion matrix and the second pose conversion matrix obtained at the previous moment are used as the first pose conversion matrix and the second pose conversion matrix at any moment. Not only can more accurate ground plane equations and vertical plane equations be obtained in real time, but also because the ground plane and / or vertical plane at adjacent moments are unchanged or the amount of change is small, the optimization amount can be reduced and the operating efficiency can be improved.

[0041] For ease of understanding, a centering detection system based on 3D laser radar provided in this application is first introduced.

[0042] Please refer to Figure 1 In one embodiment, the system includes a 3D laser radar 01, a first memory 02 and a first processor 03 arranged on the vehicle body. The 3D laser radar 01 and the first memory 02 are respectively connected to the first processor 03, the 3D laser radar 01 is used to obtain a target point cloud, the target point cloud includes a ground point cloud and a wall point cloud, the first memory 02 stores a program, and the first processor 03 is used to load the program stored in the first memory 02 and execute instructions in any one embodiment of the centering detection based on the 3D laser radar.

[0043] Based on the above system, a centering detection method based on 3D laser radar is provided in one embodiment of the present application, please refer to Figure 2 ,include:

[0044] Step S10, based on the constructed three-dimensional space global coordinate system, establish the base plane equation and obtain the parameter matrix of the base plane.

[0045] In order to reduce the amount of calculation, it is first necessary to select the target point cloud. Therefore, in one embodiment, a method for constructing a three-dimensional space global coordinate system includes:

[0046] The 3D laser radar is used as the origin of the coordinate system, with the x-axis and y-axis parallel to the horizontal plane and the z-axis perpendicular to the horizontal plane and upward, to construct a global coordinate system in three-dimensional space.

[0047] In order to further obtain the ground plane where the road surface is located and the vertical plane where the wall is located, which are required for ground centering detection, the base plane equation is first established.

[0048] In one embodiment, the established base plane equation can be expressed as:

[0049] ;

[0050] in, A , B ,C , D are the four parameters of the base plane equation. Then we can get the parameter matrix of the base plane equation .

[0051] In a specific embodiment, the plane where z=0 is located can be used as the base plane, and the parameter matrix of the base plane equation is .

[0052] Step S20, setting a first pose conversion matrix and a second pose conversion matrix.

[0053] Among them, the first pose conversion matrix is ​​used to convert the base plane parameter matrix into the ground parameter matrix, and the second pose conversion matrix is ​​used to convert the base plane parameter matrix into the vertical plane parameter matrix.

[0054] In the current scheme, the posture transformation matrix often only includes displacements along the three coordinate axes. Therefore, the second plane obtained after the transformation can only be parallel to the set base plane. Therefore, if you want to get the ground plane, you need to set a base plane parallel to the ground plane. If you want to get a vertical plane, you need to set a base plane parallel to the vertical plane. The base plane parallel to the ground plane is often parallel to the horizontal plane, and the ground plane may be tilted relative to the horizontal plane. It is difficult to get an accurate ground plane based on the base plane parallel to the horizontal plane, and the same is true for the vertical plane. In addition, the setting of the base plane is also more complicated.

[0055] In view of this, any one of the posture transformation matrices in the embodiment of the present application includes x The unit length of the axis translation, along y The unit length of the axis translation, along z The unit length of the axis translation, around x Angle of rotation around the axis y The angle of rotation around the axis, and z The angle of axis rotation.

[0056] Based on this form of posture conversion matrix, since the matrix includes the translation of the plane, the base plane can be moved up, down, left, and right. Since the matrix includes the rotation angle of the plane, it is not only possible to move the base plane up, down, left, and right to obtain the second plane, but also to rotate it at any angle to obtain a more accurate second plane. Therefore, only one base plane needs to be set, and the corresponding plane can be obtained according to the corresponding posture conversion matrix, which reduces the complexity of setting and calculation, and can be rotated to obtain a more accurate plane that is not parallel to the base plane.

[0057] In order to obtain the ground plane where the road surface is located and the vertical plane where the wall is located based on the base plane, it is necessary to first obtain the plane transformation matrix, which includes the first posture transformation matrix and the second posture transformation matrix.

[0058] In one embodiment, the first pose transformation matrix can be expressed as:

[0059] ;

[0060] The second pose transformation matrix can be expressed as:

[0061] ;

[0062] in, represents the first pose transformation matrix, represents the second pose transformation matrix; , , , , and They respectively represent that under the first pose transformation matrix, the base plane is along the x The unit length of the axis translation, along y The unit length of the axis translation, along z The unit length of the axis translation, around x Angle of rotation around the axis y The angle of rotation around the axis and z Angle of axis rotation; , , , , and They respectively represent that under the second posture transformation matrix, the base plane is along the x The unit length of the axis translation, along y The unit length of the axis translation, along z The unit length of the axis translation, around x Angle of rotation around the axis y The angle of rotation around the axis and z The angle of axis rotation.

[0063] In one embodiment, the first pose conversion matrix and the second pose conversion matrix are preset pose conversion matrices.

[0064] Step S30, acquiring a target point cloud based on a 3D laser radar, wherein the target point cloud includes a ground point cloud and a wall point cloud.

[0065] In one embodiment, please refer to Figure 3 , step S30 comprises:

[0066] Step S301, acquiring a point cloud of a target area based on a 3D laser radar, and performing preprocessing to obtain a preprocessed point cloud.

[0067] Please refer to Figure 4 , the slashed area represents the effective area scanned by the 3D laser radar. In one embodiment, the point cloud of the target area 04 is selected according to the coordinate system of the location where the 3D laser radar is installed. The 3D laser radar scans in the corridor area, and a straight-through filter is used to select a rectangular area of ​​the current frame point cloud. The selected target area 04 is larger than the area where the target point cloud is located, and includes the ground and wall point clouds.

[0068] It should be noted that since it is a 3D LiDAR, the z-axis needs to be limited. Depending on the number and density of the filtered point cloud, voxel filtering can be performed again.

[0069] Step S302, based on the base plane equation and the set first pose conversion matrix and second pose conversion matrix, obtain the converted ground plane equation and vertical plane equation.

[0070] After preprocessing the point cloud, it needs to be classified according to the characteristics of the point cloud into ground points and vertical ground points (that is, wall points).

[0071] Based on the base plane equation and the set first pose conversion matrix, the converted ground plane equation is obtained. In one embodiment, the converted ground plane equation can be expressed as:

[0072] ;

[0073] in, , , , are the four parameters of the ground plane equation. Then we can get the parameter matrix of the ground plane equation .

[0074] Based on the base plane equation and the set second posture transformation matrix, the transformed vertical plane equation is obtained. In one embodiment, the transformed vertical plane equation can be expressed as:

[0075] ;

[0076] in, , , , are the four parameters of the ground plane equation. Then we can get the parameter matrix of the ground plane equation .

[0077] The above vertical plane equation may be the equation of the vertical plane where the wall at the left position is located, or may be the equation of the vertical plane where the wall at the right position is located.

[0078] Those skilled in the art can understand that, in one embodiment, when the distance between two walls is known, then among the equations of the vertical planes where the left and right walls are located, only the equation of the vertical plane where one of the walls is located needs to be known, so that the centering detection of the road surface can be achieved.

[0079] If the distance between the two walls is unknown, in one embodiment, the centering detection of the road surface can also be achieved by obtaining the equation of the vertical plane where the left and right walls are located.

[0080] In one embodiment, if there is only one wall between the left and right walls, the distance from the wall can be set after obtaining the equation of the vertical plane where the wall is located to achieve centering detection of the road surface.

[0081] Step S303: Calculate the distance between all points in the preprocessed point cloud and the ground plane as the second distance, and take the points whose second distance is less than a preset second distance threshold as ground points.

[0082] In one embodiment, it is assumed that the coordinates of any point after preprocessing are , then the second distance corresponding to any point can be expressed as:

[0083] ;

[0084] in, Indicates the second distance.

[0085] The point whose second distance is less than a preset second distance threshold is taken as a ground point.

[0086] Step S304, searching for points above the ground plane among all non-ground points in the preprocessed point cloud, and calculating the distance between all points above the ground plane and the vertical plane as the third distance, and taking the points whose third distance is less than the preset third distance threshold as wall points.

[0087] For the preprocessed point cloud, the point cloud identified as ground points is first excluded, and the points in the remaining point cloud are non-ground points.

[0088] First find the points above the ground plane among the non-ground points. Assume that the coordinates of any non-ground point are , a direction value can be calculated k , if the direction value k If the direction value is greater than 0, then any non-ground point is above the ground plane, that is, it is a potential wall point. kIf the direction value is less than 0, then the non-ground point is below the ground plane, that is, it is a non-wall point. k The expression can be expressed as:

[0089] .

[0090] For the potential wall points, assume that the coordinates of any potential wall point are , then the third distance corresponding to any potential wall point can be expressed as:

[0091] ;

[0092] in, Indicates the third distance.

[0093] The potential wall point whose third distance is less than a preset third distance threshold is taken as a wall point.

[0094] Step S40, optimizing the first pose conversion matrix based on the ground point cloud, and optimizing the second pose conversion matrix based on the wall point cloud.

[0095] Since the ground plane obtained based on the set first pose transformation matrix is ​​an estimated approximate ground plane, similarly, the vertical plane where the wall is located based on the set second pose transformation matrix is ​​also an approximate vertical plane. Therefore, it is necessary to optimize the first pose transformation matrix to obtain a more accurate ground plane based on the optimized first pose transformation matrix, and it is also necessary to optimize the second pose transformation matrix to obtain a more accurate vertical plane where the wall is located based on the optimized second pose transformation matrix.

[0096] In one embodiment, please refer to Figure 5 , the methods for optimizing any pose transformation matrix may include:

[0097] Step S100, calculating the distance from each point in the corresponding point cloud type to the plane obtained after optimization as the first distance.

[0098] In one embodiment, the basis plane equation can be written in a matrix form that is easy to calculate, which can be expressed as:

[0099] (1)

[0100] Assume that any point in the base plane corresponding to the base plane , after transformation based on the optimized posture transformation matrix, the corresponding point in the target plane is , then

[0101] (2)

[0102] Among them, F represents the optimized posture transformation matrix, represents the coordinates of point P, Indicate point The coordinates of represents the inverse matrix of F.

[0103] Based on the above formula (1) and formula (2), we have:

[0104] (3)

[0105] Therefore, the target plane, that is, the base plane, can be transformed based on the optimized posture transformation matrix, and the coefficient equation of the obtained plane can be expressed as:

[0106] (4)

[0107] in, , , and Four parameters representing the equation of the target plane (i.e., the plane to be acquired).

[0108] From formula (4), we can see that to calculate the coefficients of the plane equation obtained after optimizing the pose transformation matrix, we need to calculate For convenience, we let , , , , , ;in, , , , , ,and All are intermediate variables; , and Respectively represent the optimized, base plane around x Angle of rotation around the axis y The angle of rotation around the axis and z The angle of rotation of the axis. Then:

[0109] ,

[0110] ;

[0111] in, R Represents the 3*3 rotation matrix of the upper left corner in the homogeneous transformation matrix, t Represents the 3*1 translation vector of the upper right corner in the homogeneous transformation matrix, The rotation angle is Time around x The rotation matrix for axis rotations, , The rotation angle is Time around y The rotation matrix for axis rotations, , The rotation angle is Time around z The rotation matrix for axis rotations, , T represents transpose; , and Respectively represent the optimized, base plane along x The unit length of the axis translation, along y The unit length of the axis translation and the z Unit length of axis translation.

[0112] Use the homogeneous transformation matrix to represent the optimized posture transformation matrix F , then:

[0113] ,

[0114] F It is a 4*4 homogeneous transformation matrix.

[0115] according to R and t It can be calculated for:

[0116] ;

[0117] in, g 1. g 2 and g 3 are all intermediate variables. , , .

[0118] Combining formula (4), we can get

[0119] ;

[0120] Then you can get any point in the corresponding point cloud type The distance to the plane obtained after optimizing the pose matrix, that is, the first distance corresponding to any point, can be expressed as:

[0121] (5)

[0122] in, Represents any point in the corresponding point cloud type The corresponding first distance, Represents any point The coordinates of i Indicates the index of the point, 1≤ i ≤ n , n Indicates the total number of points in the corresponding point cloud type.

[0123] Therefore, when the point cloud type is a ground point cloud, what is obtained is the first distance from any point in the ground point cloud to the ground plane obtained after optimization. When the point cloud type is a wall point cloud, what is obtained is the first distance from any point in the wall point cloud to the vertical plane obtained after optimization.

[0124] Step S200, calculating a cost optimization function of a first distance corresponding to all points in a corresponding point cloud type.

[0125] According to formula (5), the cost optimization function in step S200 can be expressed as:

[0126] (6)

[0127] in, f represents the cost optimization function.

[0128] Step S300, performing a least squares iterative solution based on the cost optimization function to obtain an optimized posture transformation matrix that meets the solution requirements.

[0129] In one embodiment, step S300 includes:

[0130] (7)

[0131] in, represents the optimized pose transformation matrix, Indicates the value corresponding to the minimum value of the function .

[0132] In one embodiment, the ceres library is used to By iteratively solving the least squares method, we can obtain:

[0133] .

[0134] Based on the method from step S100 to step S300, the optimized first pose conversion matrix can be obtained. And the optimized second pose transformation matrix .

[0135] Then we have:

[0136] ;

[0137] ;

[0138] in, , , , , and They respectively represent that under the optimized first pose transformation matrix, the base plane is along the order x The unit length of the axis translation, along y The unit length of the axis translation, along z The unit length of the axis translation, around x Angle of rotation around the axis y The angle of rotation around the axis and z Angle of axis rotation; , , , , and They respectively represent that under the optimized second posture transformation matrix, the base plane is along the order x The unit length of the axis translation, along y The unit length of the axis translation, along z The unit length of the axis translation, around x Angle of rotation around the axis y The angle of rotation around the axis and z The angle of axis rotation.

[0139] The applicant found in the research that as the road surface extends, the road surface is often uneven and the walls are also curved. Therefore, in order to obtain a more accurate ground plane equation for each location point and to achieve more accurate road centering detection, it is necessary to update the ground plane equation and the vertical plane equation in real time.

[0140] In view of this, in one embodiment, please refer to Figure 6 In step S20, the first pose conversion matrix and the second pose conversion matrix are set, including:

[0141] Step S201, for a first pose conversion matrix and a second pose conversion matrix at a first moment, preset the first pose conversion matrix and the second pose conversion matrix.

[0142] For the initial moments of the obtained ground plane equation and the vertical plane equation, the first pose conversion matrix and the second pose conversion matrix are first preset as the set first pose conversion matrix and the second pose conversion matrix.

[0143] Step S202: For the first pose conversion matrix and the second pose conversion matrix at any moment other than the first moment, the optimized first pose conversion matrix and the second pose conversion matrix obtained at the previous moment are used as the first pose conversion matrix and the second pose conversion matrix at the any moment.

[0144] At the moment when the ground plane equation and the vertical plane equation need to be obtained after the initial moment, the optimized first pose conversion matrix and the second pose conversion matrix obtained at the previous moment are used as the first pose conversion matrix and the second pose conversion matrix at any moment. In this way, not only can more accurate ground plane equations and vertical plane equations be obtained in real time, but also because the ground plane and / or vertical plane at adjacent moments are unchanged or the transformation amount is small, the optimization amount can be reduced to improve the operating efficiency.

[0145] Step S50, obtaining a ground plane parameter matrix based on the optimized first posture conversion matrix, and then obtaining a ground plane equation; obtaining a vertical plane parameter matrix based on the optimized second posture conversion matrix, and then obtaining a vertical plane equation.

[0146] It can be seen from the content of step S100 that in one embodiment, based on the optimization of the posture transformation matrix, the corresponding homogeneous matrix can be obtained, thereby obtaining the inverse matrix of the homogeneous matrix, and then based on formula (4), the four parameters of the target plane are obtained, thereby obtaining the target plane equation, that is, the ground plane equation and the vertical plane equation.

[0147] Step S60, realizing the center detection of the ground based on the ground plane equation and the vertical plane equation.

[0148] Those skilled in the art can understand that based on the ground plane equation and the vertical plane equation, the relationship between the ground plane and the vertical plane belonging to the wall in three-dimensional space can be known, thereby realizing the center detection of the ground.

[0149] However, the applicant also found in the study that when the car is moving, there will be places where the wall has a section, and for places where the wall has a section, there may be low-lying areas or other situations that require the car to pass slowly. Therefore, a section detection method is also provided in the specific embodiment of the present application. Based on the section detection method, the section existing in the wall is identified, so that the car can slow down in advance when approaching the section and slowly pass through the area with the wall section to improve safety.

[0150] In view of this, in one embodiment, the centering detection method based on 3D laser radar also includes a wall section detection method, please refer to Figure 7 ,include:

[0151] Step S1000, in response to the point cloud data obtained by scanning the wall by a single-line laser radar, obtaining the point cloud data of the target area.

[0152] In one embodiment, considering the problem of the radar itself, invalid points may exist in the point cloud data obtained by scanning the wall. Therefore, after acquiring the point cloud data, the invalid points are removed first.

[0153] In one embodiment, if only the left wall needs to be cross-sectionally inspected, only the area where the left wall is located needs to be obtained as the target area, and the point cloud data of the target area needs to be obtained as the target area point cloud data. If only the right wall needs to be cross-sectionally inspected, only the area where the right wall is located needs to be obtained as the target area, and the point cloud data of the target area needs to be obtained as the target area point cloud data. If both the left wall and the right wall need to be cross-sectionally inspected, the area where the left wall is located needs to be obtained as the left target area, and the area where the right wall is located needs to be obtained as the right target area, and the point cloud data of the left target area is used as the left target area point cloud data, and the point cloud data of the right target area is used as the right target area point cloud data.

[0154] In one embodiment, please refer to Figure 8 The system further includes a single-line laser radar 05, a second memory 06 and a second processor 07. The second processor 07 is connected to the single-line laser radar 05 and the second memory 06 respectively. The single-line laser radar 05 is used to scan the wall to obtain point cloud data. As the single-line laser radar 05 scans, point cloud data on the trajectory line obtained along the scanning trajectory can be obtained. The second memory 06 stores a program, and the second processor 07 is used to load the program stored in the second memory 06 and execute the instructions of the wall detection method.

[0155] In one embodiment, the first memory 02 and the second memory 06 may be the same memory.

[0156] In one embodiment, the first processor 03 and the second processor 07 may be the same processor.

[0157] In one embodiment, the single-line laser radar 05 includes only one. If only cross-sectional detection of the left wall is required, the interval from 90 to 180 degrees is scanned to obtain point cloud data of the left target area, or the interval from 0 to 180 degrees is scanned, and the point cloud data of the interval from 0 to 90 degrees is filtered out to obtain point cloud data of the left target area.

[0158] In one embodiment, the single-line laser radar 05 includes only one. If only cross-sectional detection of the right wall is required, the interval from 0 to 90 degrees is scanned to obtain point cloud data of the right target area, or the interval from 0 to 180 degrees is scanned, and the point cloud data of the interval from 90 to 180 degrees is filtered out to obtain point cloud data of the right target area.

[0159] In one embodiment, the single-line laser radar 05 includes only one. If cross-sectional detection is required for the left wall and the right wall respectively, the interval from 0 to 180 degrees is scanned, and the point cloud data of the left target area (i.e., the point cloud data of the interval from 90 to 180 degrees) and the point cloud data of the right target area (i.e., the point cloud data of the interval from 0 to 90 degrees) are obtained respectively.

[0160] In some embodiments, the single-line laser radar 05 may include two, one of which scans the interval from 0 to 90 degrees to obtain point cloud data of the right target area, and the other single-line laser radar scans the interval from 90 to 180 degrees to obtain point cloud data of the left target area.

[0161] Those skilled in the art will appreciate that, since it is impossible to enumerate all the embodiments, in order to obtain the point cloud data of the target area, based on the principle that the actual coordinate system established and the acquired point cloud data are completely covered, under the inventive conception of solving the technical problems in the above embodiments of this application, the number of single-line laser radars, the selection of the scanning area interval and the selection of the acquired point cloud area interval can be set by those skilled in the art as needed, which is a replacement of conventional technical means and is within the scope of protection of this application.

[0162] Step S2000: performing wall baseline fitting on the point cloud data of the target area.

[0163] In one embodiment, since the single-line laser radar is a 2D laser radar, the point cloud data obtained is in polar coordinate form, so the polar coordinates need to be converted into Cartesian coordinates in three-dimensional space for subsequent calculations, where the polar coordinates of any point in the point cloud data can be expressed as ,in, j represents the index of the laser beam, Indicates j The angle of the laser beam, Indicates j The distance of the laser beam. The polar coordinates of any point can be converted into Cartesian coordinates and expressed as .

[0164] In one embodiment, when performing wall baseline fitting, it is determined whether the number of inner points in the target point cloud data belonging to the fitted baseline equation reaches a preset threshold value. If so, the fitted baseline equation can be used as the wall baseline fitting equation.

[0165] Step S3000, calculating the distance between any two adjacent points in the wall baseline as a fourth distance.

[0166] In one embodiment, the Euclidean distance between any two adjacent points is calculated as the fourth distance.

[0167] Based on the obtained wall baseline fitting equation, we can obtain all the internal points in the point cloud data, including the wall baseline. In one embodiment, the difference between two adjacent points can be calculated by differential method. and The distance difference between :

[0168] ;

[0169] in, and Respectively u Dot and v The coordinates of the point, express u Dot and v The Euclidean distance.

[0170] Step S4000, in response to the trigger condition that the fourth distance is greater than the preset fourth distance threshold, calculate the fifth distance, the fifth distance includes the distance between the moving object and the first point, and the first point refers to the point closest to the moving object among any two adjacent points that meet the trigger condition.

[0171] Based on the fourth distance obtained in step S3000, it is determined whether the fourth distance is greater than the preset fourth distance threshold. If so, it indicates that there is a section on the current target wall. To ensure safety, it may be necessary to move forward slowly. In response to the trigger condition that the fourth distance is greater than the preset fourth distance threshold, please refer to Fig. 9 In one embodiment, based on the constructed two-dimensional coordinate system, the coordinate point of the moving object in the two-dimensional coordinate system is obtained as the first coordinate point (U point), the coordinate point of the first point (the first point is on the wall 08) in the two-dimensional coordinate system is obtained as the second coordinate point (V point), and the distance between the first coordinate point and the second coordinate point is calculated as the fifth distance. The coordinate system plane of the two-dimensional coordinate system is parallel to the ground plane, and the moving direction of the moving object is the y-axis direction.

[0172] Please refer to Fig. 9 In another embodiment, based on the constructed two-dimensional coordinate system, the coordinate point of the moving object in the two-dimensional coordinate system is obtained as the first coordinate point, the coordinate point of the first point in the two-dimensional coordinate system is obtained as the second coordinate point, a perpendicular line from the first coordinate point to the straight line where the second coordinate point is located is constructed, the intersection of the perpendicular line and the straight line where the second coordinate point is located is obtained as the third coordinate point (point W), and the distance between the first coordinate point and the third coordinate point is calculated as the fifth distance; the coordinate system plane of the two-dimensional coordinate system is parallel to the ground plane, wherein the moving direction of the moving object is the y-axis direction, and the straight line where the second coordinate point is located is parallel to the x-axis.

[0173] Please refer to Fig. 9, the distance between point U and point V can be calculated as the fifth distance, but since the moving object moves along the UW direction, in order to obtain a more accurate position distance, the distance between point U and point W can be calculated as the fifth distance.

[0174] Those skilled in the art will appreciate that the moving object may be an AGV vehicle, or may be a device and / or equipment such as a robot that requires automatic movement.

[0175] Step S5000: In response to a trigger condition that the fifth distance is less than a preset fifth distance threshold, setting a travel speed of the moving object so that the travel speed is less than a preset speed threshold.

[0176] Please refer to Fig. 9 When the distance between point U and point V is used as the fifth distance, the preset fifth distance threshold is used as the fifth distance threshold 1, and when the distance between point U and point W is used as the fifth distance, the preset fifth distance threshold is used as the fifth distance threshold 2. The fifth distance threshold 1 and the fifth distance threshold 2 can be the same or different, and the specific setting can be set according to the needs. For example, when the distance between the wall 08 and the moving center line of the moving object is relatively small, and / or it is necessary to slow down relatively early so that the difference between the distance between point U and point V and the distance between point U and point W can be ignored, the same fifth distance threshold can be set.

[0177] Based on the fifth distance obtained in step S4000, determine whether the fifth distance is less than the fifth distance threshold. If so, it means that the moving object has approached or is about to enter the range of the road section area where the detected wall section is located, and needs to slow down. At this time, in response to the trigger condition that the fifth distance is less than the preset fifth distance threshold, the moving speed of the moving object is set so that the moving speed is less than the preset speed threshold, so that the moving object can safely pass through the range of the road section area where the wall section is located.

[0178] Based on the above-mentioned wall section detection method, the sections existing in the wall can be identified, so that the vehicle can slow down in advance when approaching the section and slowly pass through the area with the wall section to improve safety.

[0179] In one embodiment of the present application, a computer-readable storage medium is provided, on which a program is stored. The stored program includes a method that can be loaded by a processor and process any of the above embodiments.

[0180] Those skilled in the art will appreciate that all or part of the functions of the various methods in the above-mentioned embodiments can be implemented by hardware or by computer programs. When all or part of the functions in the above-mentioned embodiments are implemented by computer programs, the program can be stored in a computer-readable storage medium, and the storage medium can include: read-only memory, random access memory, disk, optical disk, hard disk, etc., and the program is executed by a computer to implement the above-mentioned functions. For example, the program is stored in the memory of the device, and when the program in the memory is executed by the processor, all or part of the above-mentioned functions can be implemented. In addition, when all or part of the functions in the above-mentioned embodiments are implemented by computer programs, the program can also be stored in a storage medium such as a server, another computer, disk, optical disk, flash disk or mobile hard disk, and can be downloaded or copied and saved in the memory of the local device, or the system of the local device is updated, and when the program in the memory is executed by the processor, all or part of the functions in the above-mentioned embodiments can be implemented.

[0181] The above specific examples are used to illustrate the present invention, which is only used to help understand the present invention and is not intended to limit the present invention. For those skilled in the art, according to the concept of the present invention, some simple deductions, modifications or substitutions can be made.

Claims

1. A centering detection method based on 3D laser radar, characterized in that: include: Based on the constructed three-dimensional space global coordinate system, a base plane equation is established, and a parameter matrix of the base plane is obtained; A first pose conversion matrix and a second pose conversion matrix are set, wherein the first pose conversion matrix is ​​used to convert the base plane parameter matrix into a ground parameter matrix, and the second pose conversion matrix is ​​used to convert the base plane parameter matrix into a vertical plane parameter matrix; wherein any pose conversion matrix includes, in order, a unit length of translation along the x-axis, a unit length of translation along the y-axis, a unit length of translation along the z-axis, an angle of rotation around the x-axis, an angle of rotation around the y-axis, and an angle of rotation around the z-axis; Acquire a target point cloud based on a 3D laser radar, wherein the target point cloud includes a ground point cloud and a wall point cloud; The first pose conversion matrix is ​​optimized based on the ground point cloud, and the second pose conversion matrix is ​​optimized based on the wall point cloud; Based on the optimized first posture conversion matrix, the ground plane parameter matrix is ​​obtained, and then the ground plane equation is obtained; based on the optimized second posture conversion matrix, the vertical plane parameter matrix is ​​obtained, and then the vertical plane equation is obtained; The ground centering detection is realized based on the ground plane equation and the vertical plane equation; Among them, the method for optimizing any posture transformation matrix includes: Calculate the distance from each point in the corresponding point cloud type to the plane obtained after optimization as the first distance; Calculate the cost optimization function of the first distance corresponding to all points in the corresponding point cloud type; Performing a least squares iterative solution based on the cost optimization function to obtain an optimized posture conversion matrix that meets the solution requirements; The calculating of the distance from each point in the corresponding point cloud type to the plane obtained after optimization as the first distance includes: （5） in, Represents any point in the corresponding point cloud type The corresponding first distance, Represents any point The coordinates of the point, i represents the index of the point, 1≤i≤n, and n represents the total number of points in the corresponding point cloud type; , , and Four parameters representing the plane equation of the plane to be obtained; The cost optimization function for calculating the first distance corresponding to all points in the corresponding point cloud type includes: （6） Where f represents the cost optimization function.

2. The centering detection method based on 3D laser radar according to claim 1, characterized in that: The method for constructing a three-dimensional space global coordinate system includes: The 3D laser radar is used as the origin of the coordinate system, with the x-axis and y-axis parallel to the horizontal plane and the z-axis perpendicular to the horizontal plane and upward, to construct a global coordinate system in three-dimensional space.

3. The centering detection method based on 3D laser radar according to claim 1, characterized in that: The setting of the first pose conversion matrix and the second pose conversion matrix includes: For the first pose conversion matrix and the second pose conversion matrix at the first moment, the first pose conversion matrix and the second pose conversion matrix are preset; For the first pose conversion matrix and the second pose conversion matrix at any moment except the first moment, the optimized first pose conversion matrix and the second pose conversion matrix obtained at the previous moment are used as the first pose conversion matrix and the second pose conversion matrix at the any moment.

4. The centering detection method based on 3D laser radar according to claim 1, characterized in that: The first posture conversion matrix includes: ， The second posture transformation matrix includes: ， in, represents the first pose transformation matrix, represents the second pose transformation matrix; , , , , and They represent the unit length of the base plane translated along the x-axis, the unit length of the base plane translated along the y-axis, the unit length of the base plane translated along the z-axis, the angle of rotation around the x-axis, the angle of rotation around the y-axis, and the angle of rotation around the z-axis in order under the first pose transformation matrix; , , , , and They respectively represent the unit length of the base plane translated along the x-axis, the unit length of the y-axis, the unit length of the z-axis, the angle of rotation around the x-axis, the angle of rotation around the y-axis, and the angle of rotation around the z-axis in sequence under the second pose transformation matrix.

5. The centering detection method based on 3D laser radar according to claim 1, characterized in that: The target point cloud is obtained based on 3D laser radar, and the target point cloud includes ground point cloud and wall point cloud, including: Acquire the point cloud of the target area based on the 3D laser radar, and perform preprocessing to obtain the preprocessed point cloud; Based on the base plane equation and the set first pose conversion matrix and second pose conversion matrix, the converted ground plane equation and vertical plane equation are obtained; Calculate the distance between all points in the preprocessed point cloud and the ground plane as the second distance, and take the points whose second distance is less than a preset second distance threshold as ground points; Find the points above the ground plane among all non-ground points in the preprocessed point cloud, calculate the distance between all the points above the ground plane and the vertical plane as the third distance, and take the points whose third distance is less than the preset third distance threshold as wall points.

6. The centering detection method based on 3D laser radar according to claim 1, characterized in that: The least squares iterative solution is performed based on the cost optimization function to obtain an optimized posture transformation matrix that meets the solution requirements, including: （7） in, represents the optimized pose transformation matrix, Indicates the value corresponding to the minimum value of the function .

7. A centering detection system based on 3D laser radar, characterized in that: It includes a 3D laser radar, a first memory and a first processor arranged on a vehicle body; wherein the 3D laser radar and the first memory are respectively connected to the first processor, the 3D laser radar is used to obtain a target point cloud, the target point cloud includes a ground point cloud and a wall point cloud, a program is stored in the first memory, and the first processor is used to load the program stored in the first memory and execute the centering detection method based on the 3D laser radar as described in any one of claims 1 to 6.

8. A computer-readable storage medium, characterized in that: The medium stores a program, which can be loaded by a processor and execute the centering detection method based on 3D laser radar as described in any one of claims 1 to 6.

Images