A road surface detection method, system and medium

Abstract

The present invention relates to a method, system and medium for detecting a moving road surface, and to the technical field of object detection based on laser radar. The method comprises: obtaining point cloud data of a target area in response to point cloud data obtained by scanning a wall by a single-line laser radar; fitting the wall baseline of the point cloud data of the target area; calculating the distance between any two adjacent points in the wall baseline as a first distance; in response to a trigger condition that the first distance is greater than a preset first distance threshold, calculating a second distance, the second distance including the distance between a moving object and a first point, the first point being the point closest to the moving object among any two adjacent points that meet the trigger condition; in response to a trigger condition that the second distance is less than a preset second distance threshold, setting the moving speed of the moving object so that the moving speed is less than a preset speed threshold. Based on the above scheme, the vehicle can decelerate in advance when approaching a section, and slowly pass through an area with a wall section to improve safety.

Description

Technical Field

[0001] The present invention relates to the technical field of laser radar-based object detection, and in particular to a method, system and medium for detecting a traveling road surface. Background Art

[0002] Road detection refers to the need to detect the walls on the road while the vehicle is moving. In autonomous driving and robot navigation, accurate road detection can improve the accuracy and safety of path planning. At present, commonly used detection methods include vision-based detection, ultrasonic detection, and infrared detection. Ultrasonic sensors are low in price but also have low accuracy, and are affected by air temperature, humidity, etc.; infrared sensors are low in cost but have the disadvantage of a 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] 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, outline and relative position of objects in the environment, and are characterized by high resolution and high precision. Compared with other sensors, LiDAR 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 safety of driving planning in current LiDAR-based road detection is still low. Summary of the invention

[0005] The technical problem to be solved by the present application is to provide a method, system and medium for detecting a traveling road surface, which have the characteristics of improving the safety of traveling planning through road surface detection.

[0006] In a first aspect, an embodiment provides a method for detecting a road surface, including:

[0007] In response to point cloud data obtained by scanning the wall by a single-line laser radar, point cloud data of the target area is acquired;

[0008] Perform wall baseline fitting on the point cloud data of the target area;

[0009] Calculate the distance between any two adjacent points in the wall baseline as the first distance;

[0010] In response to a trigger condition that the first distance is greater than a preset first distance threshold, calculating a second distance, the second distance comprising a distance between the moving object and a first point, the first point being a point closest to the moving object among any two adjacent points that satisfy the trigger condition;

[0011] In response to a trigger condition that the second distance is less than a preset second distance threshold, a travel speed of the moving object is set so that the travel speed is less than a preset speed threshold.

[0012] In one embodiment, the target area includes an area where a left wall is located and / or an area where a right wall is located, and accordingly, the target area point cloud data includes left target area point cloud data and / or right target area point cloud data.

[0013] In one embodiment, the step of calculating the distance between any two adjacent points in the wall baseline as the first distance includes:

[0014] The Euclidean distance between any two adjacent points is calculated as the first distance.

[0015] In one embodiment, in response to a trigger condition that the first distance is greater than a preset first distance threshold, calculating the second distance includes:

[0016] In response to a trigger condition that a first distance is greater than a preset first distance threshold, based on a constructed two-dimensional coordinate system, a coordinate point of the moving object in the two-dimensional coordinate system is obtained as a first coordinate point, a coordinate point of the first point in the two-dimensional coordinate system is obtained as a second coordinate point, and the distance between the first coordinate point and the second coordinate point is calculated as the second 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.

[0017] In one embodiment, in response to a trigger condition that the first distance is greater than a preset first distance threshold, calculating the second distance includes:

[0018] 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, and the distance between the first coordinate point and the third coordinate point is calculated as the second 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.

[0019] In one embodiment, the method further includes performing a centering detection on the road surface, including:

[0020] 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;

[0021] 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;

[0022] 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;

[0023] 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;

[0024] 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;

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

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

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

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

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

[0030] In one embodiment, the step of setting the first pose conversion matrix and the second pose conversion matrix includes:

[0031] 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;

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

[0033] In a second aspect, an embodiment provides a road surface detection system, including a single-line laser radar, a memory and a processor; the single-line laser radar is arranged on a moving object, and is used to obtain point cloud data of at least one wall on both sides of the moving object's route; the memory is used to store a program of the road surface detection method described in any of the above embodiments; the processor is used to load and execute the program stored in the memory.

[0034] In one embodiment, a 3D laser radar is further included on the moving object, and the 3D laser radar is used to obtain target point cloud data including ground point cloud and wall point cloud.

[0035] In a third aspect, an embodiment provides a computer-readable storage medium, in which a program is stored. The program can be loaded by a processor and execute the road surface detection method described in any one of the above embodiments.

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

[0037] Based on the point cloud data obtained by scanning the wall with a single-line laser radar, the point cloud data of the target area is obtained and the wall baseline is fitted, so as to obtain the distance between any two adjacent points in the baseline, realize the detection of the section, and further control the speed of the moving object, so that it can slow down in advance when approaching the section and slowly pass through the area with the wall section to improve safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is a schematic diagram of the structure of a road surface detection system according to an embodiment of the present application;

[0039] Figure 2 is a schematic structural diagram of a road surface detection system according to another embodiment of the present application;

[0040] Figure 3 This is a schematic flow chart of a method for detecting a road surface according to an embodiment of the present application;

[0041] Figure 4 is a schematic diagram of obtaining a second distance based on a constructed two-dimensional coordinate system according to an embodiment of the present application;

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

[0043] Figure 6 This application Figure 5 A method flow diagram of an embodiment of step S300;

[0044] Figure 7It 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;

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

[0046] Fig. 9 This application Figure 5 A method flow chart of an embodiment of step S200 in FIG.

[0047] In the diagram: 01, single-line laser radar, 02, memory, 03, processor, 04, 3D laser radar, 05, target area, 06, wall. DETAILED DESCRIPTION

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

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

[0050] 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).

[0051] To facilitate the description of the inventive concept of the present application, the road surface detection technology is briefly described below.

[0052] The current road surface detection is more focused on the detection of a predetermined route, such as the centering detection of the moving object traveling along the central axis of the road surface, the position detection, etc. However, the applicant found in the research that during the movement of the vehicle, especially when there are tracks on the ground of the route, there will be low-lying areas on the road surface where the tracks meet, and the vehicle needs to slow down and pass slowly. The current technology directly identifies the connection position, or calculates the position according to the fixed length of the track. If the connection position is directly identified, the recognition algorithm is relatively complex, requires more computing resources, and has a high cost. If the position is calculated according to the fixed length of the track, there is an inaccuracy problem due to the existence of layer-by-layer errors, and other methods are needed to improve the accuracy.

[0053] The applicant discovered during the study that in a track environment, at the junction of two adjacent tracks, the side wall will have a corresponding section (for example, a section with a length of approximately 30 cm). This section is a specially designed structure. Therefore, the low-lying area can be obtained based on detecting this section.

[0054] In addition, the applicant also found in the study that where there is a cross-section in the wall, it is easy for objects to appear on the road surface from the cross-section. Therefore, the car is also required to slow down and pass slowly when passing through the cross-section to reduce the probability of accidents.

[0055] In view of this, a method for detecting a moving road surface is provided in one embodiment of the present application, which is based on point cloud data obtained by scanning the wall by a single-line laser radar to obtain point cloud data of the target area and perform wall baseline fitting, so as to obtain the distance between any two adjacent points in the baseline, thereby realizing section detection and further controlling the speed of the moving object, so that it can slow down in advance when approaching the section and slowly pass through the area with the wall section to improve safety.

[0056] To clearly explain the road surface detection method, the following describes the hardware that the method can rely on. Please refer to Figure 1 , a moving road surface detection system provided in one embodiment of the present application includes a single-line laser radar 01, a memory 02 and a processor 03. Among them, the single-line laser radar 01 is set on the moving object, and is used to obtain point cloud data of at least one wall on both sides of the moving object's moving route. The memory 02 is used to store a program for implementing a moving road surface detection method, and can be set on the moving object or not, and has wired or wireless communication with the processor 03. The processor 03 is used to load and execute the program of the moving road surface detection method stored in the memory 02, and can be set on the moving object or not, and has wired or wireless communication with the single-line laser radar 01.

[0057] In one embodiment, please refer to Figure 2The road surface detection system also includes a 3D laser radar 04, which is arranged on the moving object and is used to obtain target point cloud data including ground point cloud and wall point cloud.

[0058] In one embodiment of the present application, a method for detecting a road surface is provided, which can be implemented based on the road surface detection system in any of the above embodiments. Figure 3 , which may include:

[0059] Step S10, 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.

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

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

[0062] In some embodiments, a single-line laser radar may include only one. If only the left wall needs to be cross-sectionally detected, the interval from 90 to 180 degrees is scanned to obtain the 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 the point cloud data of the left target area. If only the right wall needs to be cross-sectionally detected, the interval from 0 to 90 degrees is scanned to obtain the 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 the point cloud data of the right target area. If the left wall and the right wall need to be cross-sectionally detected separately, 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.

[0063] In some embodiments, the single-line laser radar 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.

[0064] Those skilled in the art can understand that since it is difficult 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 is established and the acquired point cloud data is 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.

[0065] Step S20: performing wall baseline fitting on the target area point cloud data.

[0066] 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 , where j represents the index of the laser beam, represents the angle of the jth laser beam, Represents the distance of the jth laser beam. The polar coordinates of any point can be converted into Cartesian coordinates and expressed as .

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

[0068] Step S30, calculating the distance between any two adjacent points in the wall baseline as the first distance.

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

[0070] 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 :

[0071] ;

[0072] in, and Represent the coordinates of point u and point v respectively, Represents the Euclidean distance between point u and v.

[0073] Step S40, in response to the trigger condition that the first distance is greater than a preset first distance threshold, calculate the second distance, the second 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.

[0074] Based on the first distance obtained in step S30, it is determined whether the first distance is greater than the preset first distance threshold. If so, it means 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 first distance is greater than the preset first distance threshold, please refer to Figure 4 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 06) 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 second 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.

[0075] Please refer to Figure 4 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 second 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.

[0076] Please refer to Figure 4 , the distance between point U and point V can be calculated as the second 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 second distance.

[0077] Those skilled in the art can understand that the moving object can be an AGV, or a device and / or equipment that needs to travel automatically, such as a robot. The moving object here does not specifically refer to a moving object, but a device and / or equipment object that is provided with a moving structural component (such as a roller, a stepping structure, etc.).

[0078] Step S50, in response to a trigger condition that the second distance is less than a preset second distance threshold, setting a travel speed of the moving object so that the travel speed is less than a preset speed threshold.

[0079] Please refer to Figure 4 When the distance between point U and point V is used as the second distance, the preset second distance threshold is used as the second distance threshold 1, and when the distance between point U and point W is used as the second distance, the preset second distance threshold is used as the second distance threshold 2. The second distance threshold 1 and the second distance threshold 2 can be the same or different, and the specific settings can be set according to the needs. For example, when the distance between the wall 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 second distance threshold can be set.

[0080] Based on the second distance obtained in step S40, determine whether the second distance is less than the second 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 second distance is less than the preset second 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.

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

[0082] In order to further improve the safety of moving objects on the road surface, it is often necessary to drive in the center of the road surface, so it is necessary to achieve centering detection. In the current technology, in the object detection technology based on 3D laser radar, 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 is parallel to the corresponding base plane, and the vertical plane is also parallel to the corresponding base plane. Therefore, the corresponding target plane can be obtained by simply translating the established base plane.

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

[0084] In view of this, a new method for centering detection of a traveling road surface is provided in an embodiment of the present application. In this method, since the posture matrix for plane conversion 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 a plane conversion, 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, it is possible to not only move the base plane up, down, left, and right to obtain a 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 conversion matrix, only one base plane needs to be set, and the corresponding plane can be obtained according to the corresponding posture conversion matrix, reducing the complexity of setting and calculation.

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

[0086] Again, 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 amount of optimization can be reduced and the operating efficiency can be improved.

[0087] Please refer to Figure 5 In one embodiment of the present application, a centering detection method based on a 3D laser radar may include:

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

[0089] 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:

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

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

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

[0093] ;

[0094] Among them, A, B, C, and D are the four parameters of the base plane equation.

[0095] Then we can get the parameter matrix of the basis plane equation .

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

[0097] Step S200, setting a first pose conversion matrix and a second pose conversion matrix.

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

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

[0100] In view of this, any posture transformation matrix in the embodiments of the present application includes, in order, 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.

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

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

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

[0104] ;

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

[0106] ;

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

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

[0109] Step S300, obtaining 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.

[0110] In one embodiment, please refer to Figure 6 , step S30 comprises:

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

[0112] Please refer to Figure 7 The LIDAR in the figure represents a 3D laser radar. In one embodiment, the point cloud of the target area 05 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 uses a straight-through filter to select a rectangular area of ​​the current frame point cloud. The selected target area is larger than the area where the target point cloud is located, and includes the ground and wall point clouds.

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

[0114] Step S3002, based on the base plane equation and the set first pose transformation matrix and second pose transformation matrix, obtain the transformed ground plane equation and vertical plane equation.

[0115] 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).

[0116] 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:

[0117] ;

[0118] in, , , , are the four parameters of the ground plane equation.

[0119] Then we can get the parameter matrix of the ground plane equation .

[0120] 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:

[0121] ;

[0122] in, , , , are the four parameters of the ground plane equation.

[0123] Then we can get the parameter matrix of the ground plane equation .

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

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

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

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

[0128] Step S3003, calculating the distance between all points in the preprocessed point cloud and the ground plane as a fourth distance, and taking the points whose fourth distance is less than a preset fourth distance threshold as ground points.

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

[0130] ;

[0131] in, Indicates the fourth distance.

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

[0133] Step S3004, search for points above the ground plane among all non-ground points in the preprocessed point cloud, and calculate the distance between all points above the ground plane and the vertical plane as the fifth distance, and take the points whose fifth distance is less than the preset fifth distance threshold as wall points.

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

[0135] 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 k can be calculated. If the direction value k is greater than 0, then any non-ground point is above the ground plane, that is, a potential wall point. If the direction value k is less than 0, then any non-ground point is below the ground plane, that is, a non-wall point. In one embodiment, the expression for calculating the direction value k can be expressed as:

[0136] .

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

[0138] ;

[0139] in, Indicates the fifth distance.

[0140] The potential wall point whose fifth distance is less than a preset fifth distance threshold is taken as the wall point.

[0141] Step S400, 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.

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

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

[0144] Step S1000, calculating the distance from each point in the corresponding point cloud type to the plane obtained after optimization as the third distance.

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

[0146] (1)

[0147] 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

[0148] (2)

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

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

[0151] (3)

[0152] 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:

[0153] (4)

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

[0155] 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 angles of rotation of the base plane around the x-axis, the y-axis, and the z-axis. Then:

[0156] ;

[0157] ;

[0158] Among them, R represents the 3*3 rotation matrix in the upper left corner of the homogeneous transformation matrix, and t represents the 3*1 translation vector in the upper right corner of the homogeneous transformation matrix. The rotation angle is The rotation matrix around the x-axis is , The rotation angle is The rotation matrix around the y axis is , The rotation angle is The rotation matrix around the z-axis is , T represents transpose; , and They represent the optimized unit length of the base plane translation along the x-axis, the unit length of the base plane translation along the y-axis, and the unit length of the base plane translation along the z-axis.

[0159] Using the homogeneous transformation matrix to represent the optimized posture transformation matrix F, we have:

[0160] ;

[0161] F is a 4*4 homogeneous transformation matrix.

[0162] According to R and t, we can calculate for:

[0163] ;

[0164] Among them, g1, g2 and g3 are all intermediate variables. , , .

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

[0166] ;

[0167] 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 third distance corresponding to any point, can be expressed as:

[0168] (5)

[0169] in, Represents any point in the corresponding point cloud type The corresponding third 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.

[0170] Therefore, when the point cloud type is a ground point cloud, the third distance obtained is 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, the third distance obtained is from any point in the wall point cloud to the vertical plane obtained after optimization.

[0171] Step S2000, calculating the cost optimization function of the third distance corresponding to all points in the corresponding point cloud type.

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

[0173] (6)

[0174] Where f represents the cost optimization function.

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

[0176] In one embodiment, step S3000 includes:

[0177] (7)

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

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

[0180] .

[0181] Based on the method from step S1000 to step S3000, it can be obtained that the first pose transformation matrix after optimization is And the optimized second pose transformation matrix .

[0182] Then we have:

[0183] ;

[0184] ;

[0185] in, , , , , 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 optimized 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 optimized second pose transformation matrix.

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

[0187] In view of this, in one embodiment, please refer to Fig. 9 In step S200, the first pose conversion matrix and the second pose conversion matrix are set, including:

[0188] Step S2001, 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.

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

[0190] Step S2002: 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.

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

[0192] Step S500, based on the optimized first posture conversion matrix, obtain the ground plane parameter matrix, and then obtain the ground plane equation; based on the optimized second posture conversion matrix, obtain the vertical plane parameter matrix, and then obtain the vertical plane equation.

[0193] It can be seen from the content of step S1000 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.

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

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

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

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

[0198] 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 method for detecting a road surface, characterized in that: include: In response to point cloud data obtained by scanning the wall by a single-line laser radar, point cloud data of the target area is acquired; Perform wall baseline fitting on the point cloud data of the target area; Calculate the distance between any two adjacent points in the wall baseline as the first distance; In response to a trigger condition that the first distance is greater than a preset first distance threshold, calculating a second distance, where the second distance includes a distance between the moving object and a first point, where the first point refers to a point closest to the moving object among any two adjacent points that meet the trigger condition; In response to a trigger condition that the second distance is less than a preset second distance threshold, setting a travel speed of the moving object so that the travel speed is less than a preset speed threshold; The target area includes an area where the left wall is located and / or an area where the right wall is located, and correspondingly, the target area point cloud data includes point cloud data of the left target area and / or point cloud data of the right target area; The wall baseline fitting of the target area point cloud data includes: determining whether the number of inner points in the target point cloud data belonging to the fitted baseline equation reaches a preset threshold value, and if so, using the fitted baseline equation as the wall baseline fitting equation.

2. The method for detecting a road surface according to claim 1, wherein: The step of calculating the distance between any two adjacent points in the wall baseline as the first distance includes: The Euclidean distance between any two adjacent points is calculated as the first distance.

3. The method for detecting a road surface according to claim 1, wherein: The calculating of the second distance in response to a trigger condition that the first distance is greater than a preset first distance threshold comprises: In response to a trigger condition that the first distance is greater than a preset first distance threshold, based on the constructed two-dimensional coordinate system, a coordinate point of the moving object in the two-dimensional coordinate system is obtained as a first coordinate point, a coordinate point of the first point in the two-dimensional coordinate system is obtained as a second coordinate point, and a distance between the first coordinate point and the second coordinate point is calculated as the second 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 y Axis direction.

4. The method for detecting a road surface according to claim 1, wherein: The calculating of the second distance in response to a trigger condition that the first distance is greater than a preset first distance threshold comprises: 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, and the distance between the first coordinate point and the third coordinate point is calculated as the second 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 y Axis direction, the straight line where the second coordinate point is located and x Axis parallel.

5. The method for detecting a road surface according to claim 1, wherein: It also includes centering detection of the road surface, including: 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; Set a first pose conversion matrix and a second pose conversion matrix, 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 one of the pose conversion matrices includes the following steps in 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; 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 initial posture transformation matrix includes: Calculate the distance from each point in the corresponding point cloud type to the plane obtained after optimization as the second distance; Calculate the cost optimization function of the second distance corresponding to all points in the corresponding point cloud type; 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.

6. The method for detecting a road surface according to claim 5, wherein: 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.

7. A road surface detection system, characterized in that: It comprises a single-line laser radar, a memory and a processor; the single-line laser radar is arranged on a moving object and is used to obtain point cloud data of at least one wall on both sides of the moving object's moving route; the memory is used to store a program of a moving road surface detection method as described in any one of claims 1 to 6; the processor is used to load and execute the program stored in the memory.

8. The road surface detection system according to claim 7, characterized in that: It also includes a 3D laser radar arranged on the moving object, and the 3D laser radar is used to obtain target point cloud data including ground point cloud and wall point cloud.

9. A computer-readable storage medium, characterized in that: The medium stores a program, which can be loaded by a processor and execute the road surface detection method according to any one of claims 1 to 6.

Images