Unmanned equipment road obstacle detection method, device, unmanned equipment and storage medium

Abstract

The present invention discloses a method, device, unmanned equipment and storage medium for detecting obstacles on the road surface of unmanned equipment. After obtaining the point cloud collected by the laser radar during the travel of the unmanned equipment, the suspected obstacles on the travel path of the unmanned equipment are determined based on the point cloud, and the laser radar is controlled to scan the area where the suspected obstacles are located at at least two different angles to obtain at least two frames of target point clouds, and then the at least two frames of target point clouds are fused to obtain a dense point cloud, and the real obstacles on the travel path of the unmanned equipment are determined based on the dense point cloud. The area where the suspected obstacles are located is scanned at at least two different angles to increase the density of the light spots hitting the suspected obstacles, thereby improving the resolution of the laser radar and the coverage rate of the point cloud on the suspected obstacles, thereby improving the detection accuracy of the obstacles, avoiding the situation where missed detection affects the normal operation of the unmanned equipment, and improving the safety of the unmanned equipment.

Description

Technical Field

[0001] The embodiments of the present invention relate to unmanned equipment obstacle detection technology, and more particularly to an unmanned equipment road obstacle detection method, device, unmanned equipment and storage medium. Background Art

[0002] With the rapid development of artificial intelligence and robotics technology in recent years, unmanned equipment has begun to be applied to various industries.

[0003] Unmanned equipment needs to have a good perception of the surrounding environment during driving, including the perception of road structure, detection of obstacles and other unmanned equipment, etc. Reliable environmental perception plays a vital role in autonomous cruise control, collision warning and path planning.

[0004] The existing obstacle detection is mainly carried out by multi-line laser radar, which is not affected by light and can detect obstacles during the day and at night. However, the vertical resolution of multi-line laser radar is limited, and only part of the beam will hit the object, which will result in the situation that small obstacles are missed, affecting the normal operation and safety of unmanned equipment. Summary of the invention

[0005] The present invention provides an unmanned equipment road obstacle detection method, device, unmanned equipment and storage medium to improve the detection accuracy of obstacles, avoid missed detection affecting the normal operation of the unmanned equipment, and improve the safety of the unmanned equipment.

[0006] In a first aspect, an embodiment of the present invention provides a method for detecting road obstacles of an unmanned equipment, comprising:

[0007] Obtain the point cloud collected by the laser radar during the movement of the unmanned equipment;

[0008] Determining a suspected obstacle on a path of travel of the unmanned equipment based on the point cloud;

[0009] Controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds;

[0010] Fusing the at least two frames of target point clouds to obtain a dense point cloud;

[0011] The real obstacles on the travel path of the unmanned equipment are determined based on the dense point cloud.

[0012] Optionally, determining a suspected obstacle on the path of the unmanned device based on the point cloud includes:

[0013] Segmenting the point cloud into a first point cloud located above the ground and a second point cloud located below the ground;

[0014] Using a clustering algorithm to process the first point cloud and the second point cloud respectively, and taking points in the first point cloud and the second point cloud whose mutual distance is less than a preset distance threshold as a first cluster;

[0015] The size of the first cluster is compared with a preset size threshold, and the first cluster smaller than the size threshold is regarded as a suspected obstacle on the travel path of the unmanned equipment.

[0016] Optionally, controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds includes:

[0017] Controlling the unmanned equipment to stop moving;

[0018] After the unmanned equipment stops moving, the laser radar is controlled to swing around a rotation axis parallel to the ground and perpendicular to the moving direction, and the area where the suspected obstacle is located is scanned at at least two different angles to obtain at least two frames of target point clouds.

[0019] Optionally, fusing the at least two frames of target point clouds to obtain a dense point cloud includes:

[0020] Determine the pitch angle when the laser radar collects each frame of the target point cloud;

[0021] Converting the at least two frames of target point clouds to the same coordinate system based on the pitch angle;

[0022] The at least two frames of target point clouds located in the same coordinate system are merged to obtain a dense point cloud.

[0023] Optionally, the unmanned device is provided with a plurality of feature points for calibration, the target point cloud covers the feature points, and determining the pitch angle when the laser radar collects each frame of the target point cloud comprises:

[0024] Finding a point corresponding to the feature point from the target point cloud, and determining a first coordinate of the point corresponding to the feature point in the laser radar coordinate system;

[0025] Based on the conversion relationship between the first coordinate and the second coordinate of the feature point in the unmanned equipment coordinate system, construct a function of the second coordinate and the angle when the laser radar collects the target point cloud;

[0026] Constructing a distance function from the second coordinate to the feature surface where the feature point is located;

[0027] The distance function is minimized by taking the angle at which the laser radar collects the target point cloud as a variable, and the angle corresponding to the minimum value of the distance function is used as the pitch angle when the laser radar collects each frame of the target point cloud.

[0028] Optionally, the function of the second coordinate and the angle at which the laser radar collects the target point cloud is as follows:

[0029]

[0030] Among them, (x′ i ,y′ i , z′i) is the second coordinate of the i-th feature point in the unmanned equipment coordinate system, (x i ,y i ,z i ) is the first coordinate of the point corresponding to the i-th feature point in the target point cloud in the laser radar coordinate system, α,γ,t x ,t y ,t z are respectively the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, and θ is the angle at which the laser radar collects the target point cloud.

[0031] Optionally, the distance function is minimized with the angle at which the laser radar collects the target point cloud as a variable, as follows:

[0032]

[0033] Among them, θ * is the pitch angle when the laser radar collects each frame of the target point cloud, a pi , b pi , c pi , d pi is the plane equation parameter of the feature surface corresponding to the i-th feature point.

[0034] Optionally, converting the at least two frames of target point clouds into the same coordinate system based on the pitch angle includes:

[0035] Convert the at least two frames of target point clouds to the unmanned equipment coordinate system based on the following formula;

[0036]

[0037] Among them, (x′1, y′1, z′1) is the coordinate of the point in the target point cloud converted to the unmanned equipment coordinate system, (x1, y1, z1) is the coordinate of the point in the target point cloud in the laser radar coordinate system, α, γ, t x ,t y ,t z are the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, θ * The pitch angle when the laser radar collects the target point cloud.

[0038] Optionally, determining a real obstacle on the path of the unmanned device based on the dense point cloud includes:

[0039] Processing the dense point cloud using a clustering algorithm, and taking points in the dense point cloud whose mutual distances are less than a preset distance threshold as a second cluster;

[0040] The size of the second cluster is compared with a preset size threshold, and the second cluster whose size is greater than or equal to the size threshold is regarded as a real obstacle on the travel path of the unmanned equipment.

[0041] In a second aspect, an embodiment of the present invention further provides a road obstacle detection device for unmanned equipment, comprising:

[0042] Point cloud acquisition module, used to obtain point clouds collected by the laser radar during the movement of unmanned equipment;

[0043] A suspected obstacle determination module, used to determine a suspected obstacle on the travel path of the unmanned equipment based on the point cloud;

[0044] A target point cloud acquisition module, used to control the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds;

[0045] A point cloud fusion module, used for fusing the at least two frames of target point clouds to obtain a dense point cloud;

[0046] A real obstacle determination module is used to determine the real obstacles on the travel path of the unmanned equipment based on the dense point cloud.

[0047] In a third aspect, an embodiment of the present invention further provides an unmanned device, including:

[0048] one or more processors;

[0049] A storage device for storing one or more programs;

[0050] When the one or more programs are executed by the one or more processors, the one or more processors implement the unmanned equipment road obstacle detection method provided in the first aspect of the present invention.

[0051] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the unmanned equipment road obstacle detection method provided in the first aspect of the present invention.

[0052] The unmanned equipment road obstacle detection method provided by the embodiment of the present invention determines the suspected obstacles on the unmanned equipment's path based on the point cloud after acquiring the point cloud collected by the laser radar during the unmanned equipment's travel, and controls the laser radar to scan the area where the suspected obstacles are located at at least two different angles to obtain at least two frames of target point cloud, and then fuses the at least two frames of target point cloud to obtain a dense point cloud, and determines the real obstacles on the unmanned equipment's path based on the dense point cloud. Scanning the area where the suspected obstacles are located at at least two different angles increases the density of the light spots hitting the suspected obstacles, improves the resolution of the laser radar and the coverage of the point cloud on the suspected obstacles, thereby improving the obstacle detection accuracy, avoiding the situation where missed detection affects the normal operation of the unmanned equipment, and improving the safety of the unmanned equipment. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 A schematic diagram of the structure of an unmanned device provided by an embodiment of the present invention;

[0054] Figure 2 A flow chart of a method for detecting road obstacles of unmanned equipment provided in Embodiment 1 of the present invention;

[0055] Figure 3A is a flow chart of a method for detecting road obstacles of unmanned equipment provided in Embodiment 2 of the present invention;

[0056] Figure 3B A side view of an unmanned device provided by an embodiment of the present invention;

[0057] Figure 3C A front view of an unmanned device provided by an embodiment of the present invention;

[0058] Figure 4 A schematic diagram of the structure of a road obstacle detection device for unmanned equipment provided in Embodiment 3 of the present invention;

[0059] Figure 5 A schematic diagram of the structure of an unmanned device provided in Embodiment 4 of the present invention. DETAILED DESCRIPTION

[0060] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0061] In the embodiment of the present invention, the unmanned equipment may be an unmanned vehicle, a transport robot, a cleaning robot or other wheeled unmanned equipment, which is not limited in the embodiment of the present invention. Figure 1 A schematic diagram of the structure of an unmanned device provided by an embodiment of the present invention is shown in FIG. Figure 1 As shown, the unmanned equipment may include a control device 101 , a bus 102 , an ECU (Electronic Control Unit) 103 , a sensor 104 and an actuator 105 .

[0062] Among them, the control device 101 is responsible for the overall intelligent control of the entire unmanned equipment. The control device 101 can be a separately set controller, such as a programmable logic controller (PLC), a single-chip microcomputer, an industrial control machine, etc.; it can also be a device composed of other electronic devices with input / output ports and operation control functions; it can also be a computer device installed with unmanned equipment control applications. The control device 101 can analyze and process the data sent by the ECU 103 and / or the data sent by the sensor 104 received from the bus 102, make corresponding decisions, and send the instructions corresponding to the decisions to the bus 102.

[0063] The bus 102 may be a bus for connecting the control device 101, the ECU 103, the sensor 104, and other unillustrated devices of the unmanned device. Since the high performance and reliability of the CAN (Controller Area Network) bus have been widely recognized, the CAN bus is currently the most commonly used bus in unmanned devices. Of course, it is understandable that the bus 102 may also be other types of buses.

[0064] The bus 102 can send the instructions sent by the control device 101 to the ECU 103, and the ECU 103 analyzes and processes the instructions and sends them to the corresponding execution device 105 for execution. The execution device may include but is not limited to brakes, steering gears, etc.

[0065] The above-mentioned sensors 104 include but are not limited to lidar, cameras, and the like.

[0066] It should be noted that the unmanned equipment road obstacle detection method provided in the embodiment of the present invention can be executed by the control device 101 , and accordingly, the unmanned equipment road obstacle detection device is generally arranged in the control device 101 .

[0067] It should be understood that Figure 1 The number of control devices 101, buses 102, ECUs 103, sensors 104, and actuators 105 is merely illustrative. Any number of control devices 101, buses 102, ECUs 103, sensors 104, and actuators 105 may be provided as required, and the present invention does not limit this.

[0068] Embodiment 1

[0069] Figure 2 This is a flow chart of a method for detecting road obstacles of unmanned equipment provided in the first embodiment of the present invention. This embodiment is applicable to the detection of small obstacles on the road. The method can be performed by the unmanned equipment road obstacle detection device provided in the embodiment of the present invention. The device can be implemented in software and / or hardware and configured in the unmanned equipment provided in the embodiment of the present invention. Figure 2 As shown, the method specifically comprises the following steps:

[0070] S201. Obtaining a point cloud collected by a laser radar during the movement of the unmanned equipment.

[0071] During the movement of the unmanned device, the unmanned device drives the laser radar to continuously scan the environment around the unmanned device. The laser radar emits a laser beam including multiple (for example, 32 lines or 64 lines) lasers, and receives the reflected laser reflected from the surface of the object. The laser radar accurately measures the propagation time of the laser from emission to reflection. Since the speed of light is known, the propagation time can be converted into a measurement of distance. Combined with the height of the laser radar and the laser scanning angle, the three-dimensional coordinates of each light spot in the coordinate system where the laser radar is located can be accurately calculated, thereby obtaining a point cloud composed of multiple points.

[0072] S202: Determine suspected obstacles on the travel path of the unmanned equipment based on the point cloud.

[0073] In the embodiment of the present invention, for example, semantic segmentation can be performed on the collected point cloud, and each point in the point cloud can be classified to determine the object to which the point belongs, for example, a point belonging to the road surface, a point belonging to the edge of the road, a point belonging to a street lamp, a point belonging to an obstacle, a point belonging to other unmanned equipment, etc. Through the above semantic segmentation, obstacles on the path of the unmanned equipment can be determined, and the size of the obstacles can be evaluated, and obstacles greater than or equal to a preset size threshold can be marked as real obstacles, and obstacles less than the size threshold can be marked as suspected obstacles. Among them, the size threshold can be the maximum obstacle size that the unmanned equipment can pass, and the maximum obstacle size is less than the chassis height of the unmanned equipment.

[0074] As mentioned above, the vertical resolution of multi-line laser radar is limited, and only part of the beam will hit the object, which cannot cover the entire object. That is, the obstacle obtained through the above semantic segmentation is actually only a part of the real obstacle. This may cause the actual size of the obstacle smaller than the size threshold to be larger than the size threshold. Therefore, it is necessary to further analyze the suspected obstacles smaller than the size threshold to determine whether they are real obstacles.

[0075] It should be noted that, in the embodiment of the present invention, the specific algorithm for semantic segmentation of the point cloud is not limited, as long as the suspected obstacle can be determined from the point cloud.

[0076] S203, controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds.

[0077] In an embodiment of the present invention, the laser radar can rotate or swing. When a suspected obstacle appears in the point cloud, the unmanned equipment controls the laser radar to rotate or swing, scan the area where the suspected obstacle is located at at least two different angles, and obtain at least two frames of target point clouds.

[0078] S204: Fuse at least two frames of target point clouds to obtain a dense point cloud.

[0079] In an embodiment of the present invention, the at least two frames of target point clouds obtained above are fused to obtain a dense point cloud. In an embodiment of the present invention, by controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles, at least two frames of target point clouds are obtained, and the at least two frames of target point clouds are fused to increase the density of the light spot on the suspected obstacle, improve the resolution of the laser radar, and then accurately evaluate the size of the suspected obstacle, thereby improving the detection accuracy of the obstacle.

[0080] S205: Determine real obstacles on the path of the unmanned equipment based on the dense point cloud.

[0081] In the embodiment of the present invention, for example, semantic segmentation can be performed on the obtained dense point cloud to determine the obstacles on the path of the unmanned equipment, and the size of the obstacles can be evaluated, and obstacles greater than or equal to the preset size threshold can be marked as real obstacles, and obstacles less than the size threshold can be marked as non-obstacles. Since the semantic segmentation process processes the dense point cloud, the point cloud density on the obstacle is large, the resolution is high, and the coverage of the obstacle is high. Therefore, the obtained obstacle is close to the actual size of the real obstacle, which improves the detection accuracy of the obstacle.

[0082] The unmanned equipment road obstacle detection method provided by the embodiment of the present invention determines the suspected obstacles on the unmanned equipment's path based on the point cloud after acquiring the point cloud collected by the laser radar during the unmanned equipment's travel, and controls the laser radar to scan the area where the suspected obstacles are located at at least two different angles to obtain at least two frames of target point cloud, and then fuses the at least two frames of target point cloud to obtain a dense point cloud, and determines the real obstacles on the unmanned equipment's path based on the dense point cloud. Scanning the area where the suspected obstacles are located at at least two different angles increases the density of the light spots hitting the suspected obstacles, improves the resolution of the laser radar and the coverage of the point cloud on the suspected obstacles, thereby improving the obstacle detection accuracy, avoiding the situation where missed detection affects the normal operation of the unmanned equipment, and improving the safety of the unmanned equipment.

[0083] Embodiment 2

[0084] Figure 3Ais a flow chart of a method for detecting road obstacles of unmanned equipment provided in the second embodiment of the present invention. This embodiment is refined on the basis of the above-mentioned first embodiment, and describes in detail the specific implementation process of each step in the method for detecting road obstacles of unmanned equipment, such as Figure 3A As shown, the method includes:

[0085] S301, obtaining the point cloud collected by the laser radar during the movement of the unmanned equipment.

[0086] During the movement of the unmanned device, the unmanned device drives the laser radar to continuously scan the environment around the unmanned device to obtain a point cloud.

[0087] In the embodiments of the present invention, the unmanned equipment is cleaning equipment such as sweeping robots and unmanned sweepers. Common application environments of such equipment include urban roads, parks, sidewalks, etc., and the road conditions in the above places often have small obstacles such as potholes, bricks, gravel, and water pipes. As mentioned above, the vertical resolution of the multi-line laser radar is limited, and the point cloud cannot cover the entire object, and these small obstacles may be ignored (i.e., missed obstacles). In order to facilitate ground cleaning, the chassis of such equipment is usually low, and its passability is often poor, which makes it difficult for unmanned equipment to cross these small obstacles, and even small objects may be sucked into the unmanned equipment, causing damage to the unmanned equipment.

[0088] S302: Segment the point cloud into a first point cloud located above the ground and a second point cloud located below the ground.

[0089] In an embodiment of the present invention, based on the three-dimensional coordinates of each point in the point cloud in the coordinate system where the laser radar is located, such as the vertical coordinate values, the points in the point cloud are divided into three parts, namely points on the ground, points below the ground and points above the ground, and the point cloud composed of points above the ground (i.e., above the ground) is called the first point cloud, and the point cloud composed of points below the ground (i.e., below the ground) is called the second point cloud.

[0090] S303: Use a clustering algorithm to process the first point cloud and the second point cloud respectively, and take points in the first point cloud and the second point cloud whose mutual distance is less than a preset distance threshold as a first cluster.

[0091] In the embodiment of the present invention, a clustering algorithm is used to perform semantic segmentation on the first point cloud and the second point cloud, respectively. Specifically, the clustering algorithm is used to perform clustering processing on the first point cloud and the second point cloud, respectively, and the points in the first point cloud and the second point cloud whose mutual distance is less than a preset distance threshold are taken as the first cluster. For the first cluster in the first point cloud above the ground, we consider it to be an object located on the ground, and for the first cluster in the second point cloud below the ground, we consider it to be a pothole on the ground.

[0092] Exemplarily, in an embodiment of the present invention, a Euclidean clustering algorithm is used to process the first point cloud and the second point cloud respectively, and the points in the first point cloud and the second point cloud whose mutual distance is less than a preset distance threshold are taken as the first cluster. Euclidean clustering is a clustering algorithm based on the Euclidean distance metric. Specifically, taking the processing of the first point cloud as an example, for a certain point p in the first point cloud, the KD-Tree nearest neighbor search algorithm is used to find the k points closest to point p, and the points whose distance is less than the preset distance threshold are clustered into the first cluster (i.e., set) Q. If the number of elements in Q no longer increases, the entire clustering process ends; otherwise, points other than point p must be selected in the first cluster Q, and the above process is repeated until the number of elements in Q no longer increases. KD-Tree is the abbreviation of K-dimension Tree, which is a data structure for dividing data points in k-dimensional space. A kd tree is a binary tree in which each node is a k-dimensional numerical point. Each node on it represents a hyperplane that is perpendicular to the coordinate axis of the current partition dimension and divides the space into two parts on this dimension, one in its left subtree and the other in its right subtree. That is, if the partition dimension of the current node is d, the coordinate values ​​of all points on its left subtree in the d-dimensional dimension are less than the current value, and the coordinate values ​​of all points on the right subtree in the d-dimensional dimension are greater than or equal to the current value, and this definition holds for any of its child nodes.

[0093] S304: Compare the size of the first cluster with a preset size threshold, and regard the first cluster smaller than the size threshold as a suspected obstacle on the path of the unmanned equipment.

[0094] In the embodiment of the present invention, the size (including length, width, and height) of each first cluster is compared with a preset size threshold, and the first cluster smaller than the size threshold is regarded as a suspected obstacle on the path of the unmanned device, and the obstacle greater than or equal to the preset size threshold is marked as a real obstacle. In the embodiment of the present invention, the unmanned device is a cleaning device such as a sweeping robot or an unmanned cleaning vehicle, so the size threshold can be the size of the largest object that can be cleaned by the unmanned device.

[0095] S305. Control the unmanned equipment to stop moving.

[0096] When a suspected obstacle is detected, the unmanned equipment is controlled to stop moving.

[0097] S306, controlling the laser radar to swing around a rotation axis parallel to the ground and perpendicular to the traveling direction, scanning the area where the suspected obstacle is located at at least two different angles, and obtaining at least two frames of target point clouds.

[0098] Figure 3B A side view of an unmanned device provided by an embodiment of the present invention, Figure 3C A front view of an unmanned device provided by an embodiment of the present invention, such as Figure 3B , Figure 3C As shown, in an embodiment of the present invention, a laser radar and a driving mechanism are installed on the top of the unmanned equipment, the laser radar is connected to the driving mechanism, and the driving mechanism is fixedly connected to the unmanned equipment through a mounting bracket. The driving mechanism includes a rotating shaft parallel to the ground and perpendicular to the direction of travel, and the laser radar is fixed on the rotating shaft. The driving mechanism drives the rotating shaft to rotate, thereby driving the laser radar to swing, so that the laser radar can obtain point clouds at different angles. After the unmanned equipment stops moving, the driving mechanism is controlled to drive the rotating shaft to rotate, thereby driving the laser radar to swing around the rotating shaft parallel to the ground and perpendicular to the direction of travel, scanning the area where the suspected obstacle is located at at least two different angles, and obtaining at least two frames of target point clouds.

[0099] S307, determining the pitch angle when the laser radar collects each frame of target point cloud.

[0100] In some embodiments of the present invention, the driving mechanism may be a servo driving mechanism, such as a servo motor, and the pitch angle when the laser radar collects each frame of the target point cloud can be determined by the rotation angle of the servo driving mechanism. Since the cost of the servo driving mechanism is relatively high, in the embodiment of the present invention, the geometric features of the shape of the unmanned equipment and the point cloud projected on the unmanned equipment are used to determine the pitch angle when the laser radar collects each frame of the target point cloud. The specific process is as follows:

[0101] 1. Find the point corresponding to the feature point from the target point cloud and determine the first coordinate of the point corresponding to the feature point in the lidar coordinate system.

[0102] The unmanned device is provided with a plurality of feature points for calibration, and the target point cloud covers the feature points. In the embodiment of the present invention, in order to ensure that the target point cloud can cover all feature points, the laser radar is invertedly installed on the top of the unmanned device, the upper end is fixed on the rotating shaft of the driving mechanism, and the lower end can swing around the rotating shaft to increase the coverage rate of the laser radar on the unmanned device.

[0103] In an embodiment of the present invention, for each frame of target point cloud, a point corresponding to the feature point is found from the target point cloud, and a first coordinate of the point corresponding to the feature point in the laser radar coordinate system is determined.

[0104] 2. Based on the conversion relationship between the first coordinate and the second coordinate of the feature point in the unmanned equipment coordinate system, a function of the second coordinate and the angle when the laser radar collects the target point cloud is constructed.

[0105] Specifically, the function of the second coordinate and the angle when the laser radar collects the target point cloud is as follows:

[0106]

[0107] Among them, (x′ i ,y′i ,z′ i ) is the second coordinate of the i-th feature point in the unmanned equipment coordinate system, (x i ,y i ,z i ) is the first coordinate of the point corresponding to the i-th feature point in the target point cloud in the laser radar coordinate system, α,γ,t x ,t y ,t z are the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, and θ is the angle when the laser radar collects the target point cloud. Among them, the roll angle, yaw angle and position parameters can be obtained by pre-calibration.

[0108] 3. Construct a distance function from the second coordinate to the feature surface where the feature point is located.

[0109] In the embodiment of the present invention, the distance function from the second coordinate to the feature surface where the feature point is located is as follows:

[0110]

[0111] Among them, D i is the distance from the second coordinate to the feature surface where the feature point is located, a pi , b pi , c pi , d pi is the plane equation parameter of the feature surface corresponding to the i-th feature point.

[0112] 4. Minimize the distance function with the angle when the laser radar collects the target point cloud as the variable, and use the angle corresponding to the minimum value of the distance function as the pitch angle when the laser radar collects each frame of the target point cloud.

[0113] Specifically, the distance function is minimized with the angle at which the laser radar collects the target point cloud as a variable, as follows:

[0114]

[0115] Among them, θ * is the pitch angle when the laser radar collects each frame of the target point cloud. pi , b pi , c pi , d pi The plane equation parameters of the feature surface corresponding to the i-th feature point are known constants, so the above formula can be simplified to:

[0116]

[0117] Exemplarily, the above equation may be solved based on the Gauss-Newton method to obtain the angle that minimizes the distance function from the second coordinate to the feature surface where the feature point is located as the pitch angle.

[0118] The embodiment of the present invention uses the geometric features of the unmanned equipment shape and the point cloud projected on the unmanned equipment to determine the pitch angle when the laser radar collects each frame of the target point cloud, which is not affected by dynamic objects in the external environment (such as vehicles, pedestrians, trees blowing in the wind, etc.), so that the obtained pitch angle has higher accuracy and reliability. In addition, using the geometric features of the unmanned equipment shape and the laser point cloud projected on the unmanned equipment to determine the pitch angle can reduce the complexity and cost of the system.

[0119] It should be noted that the above method for determining the pitch angle of the laser radar is an exemplary description of an embodiment of the present invention. In other embodiments of the present invention, other methods may also be used to determine the pitch angle of the laser radar, and the embodiment of the present invention is not limited here.

[0120] S308: Convert at least two frames of target point clouds to the same coordinate system based on the pitch angle.

[0121] The point cloud fusion process is to find a translation transformation matrix (rigid transform oreuclidean transform rigid transformation or Euclidean transformation) between two point clouds, and transform the source point cloud to the same coordinate system as the target point cloud.

[0122] In an embodiment of the present invention, converting at least two frames of target point clouds to the same coordinate system based on the pitch angle includes:

[0123] Convert at least two frames of target point clouds to the unmanned equipment coordinate system based on the following formula.

[0124]

[0125] Among them, (x′1, y′1, z′1) is the coordinate of the point in the target point cloud converted to the unmanned equipment coordinate system, (x1, y1, z1) is the coordinate of the point in the target point cloud in the laser radar coordinate system, α, γ, t x ,t y ,t z are the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, θ * The elevation angle when collecting target point cloud for the LiDAR.

[0126] S309: Fusing at least two frames of target point clouds in the same coordinate system to obtain a dense point cloud.

[0127] At least two frames of target point clouds in the same coordinate system are fused to obtain a dense point cloud with a higher point density, which increases the density of the light spot on the suspected obstacle and improves the resolution of the lidar. The size of the suspected obstacle can be accurately assessed, thereby improving the detection accuracy of the obstacle.

[0128] S310: Process the dense point cloud using a clustering algorithm, and take points in the dense point cloud whose mutual distances are less than a preset distance threshold as a second cluster.

[0129] Exemplarily, in an embodiment of the present invention, a clustering algorithm is used to perform clustering processing on a dense point cloud, and points in the dense point cloud whose mutual distance is less than a preset distance threshold are taken as a second cluster. Exemplarily, in an embodiment of the present invention, a Euclidean clustering algorithm is used to process a dense point cloud, and points in the dense point cloud whose mutual distance is less than a preset distance threshold are taken as a second cluster. Specifically, the Euclidean clustering algorithm has been described in detail in the aforementioned embodiment of the present invention, and the embodiment of the present invention will not be repeated here.

[0130] S311. Compare the size of the second cluster with a preset size threshold, and regard the second cluster whose size is greater than or equal to the size threshold as a real obstacle on the path of the unmanned equipment.

[0131] In the embodiment of the present invention, the size (including length, width, and height) of each second cluster is compared with a preset size threshold, and the second cluster greater than or equal to the size threshold is regarded as a real obstacle on the path of the unmanned device, and the obstacle less than the size threshold is marked as a non-obstacle. Since the second cluster is obtained by clustering the dense point cloud, the point cloud density on the obstacle is large, the resolution is high, and the coverage of the obstacle is high. Therefore, the obtained obstacle is close to the actual size of the real obstacle, which improves the detection accuracy of the obstacle.

[0132] The unmanned equipment road obstacle detection method provided by the embodiment of the present invention, when determining the suspected obstacle on the path of the unmanned equipment, scans the area where the suspected obstacle is located at at least two different angles, increases the density of the light spot on the suspected obstacle, improves the resolution of the laser radar and the coverage of the point cloud on the suspected obstacle, thereby improving the detection accuracy of the obstacle, avoiding the situation where missed detection affects the normal operation of the unmanned equipment, and improving the safety of the unmanned equipment. In addition, the geometric features of the shape of the unmanned equipment and the point cloud on the unmanned equipment are used to determine the pitch angle when the laser radar collects each frame of the target point cloud, thereby improving the accuracy of the pitch angle and reducing the cost of the unmanned equipment.

[0133] Embodiment 3

[0134] Figure 4 A schematic diagram of the structure of a road obstacle detection device for unmanned equipment provided in Embodiment 3 of the present invention is shown in FIG. Figure 4 As shown, the device comprises:

[0135] The point cloud acquisition module 401 is used to acquire the point cloud collected by the laser radar during the movement of the unmanned equipment;

[0136] A suspected obstacle determination module 402, configured to determine a suspected obstacle on the travel path of the unmanned equipment based on the point cloud;

[0137] A target point cloud acquisition module 403 is used to control the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds;

[0138] A point cloud fusion module 404 is used to fuse the at least two frames of target point clouds to obtain a dense point cloud;

[0139] The real obstacle determination module 405 is used to determine the real obstacles on the travel path of the unmanned equipment based on the dense point cloud.

[0140] In some embodiments of the present invention, the suspected obstacle determination module 402 includes:

[0141] a point cloud segmentation submodule, used for segmenting the point cloud into a first point cloud located above the ground and a second point cloud located below the ground;

[0142] A first clustering submodule, configured to process the first point cloud and the second point cloud respectively using a clustering algorithm, and take points in the first point cloud and the second point cloud whose mutual distance is less than a preset distance threshold as a first cluster;

[0143] The first size comparison submodule is used to compare the size of the first cluster with a preset size threshold, and regard the first cluster smaller than the size threshold as a suspected obstacle on the travel path of the unmanned equipment.

[0144] In some embodiments of the present invention, the target point cloud acquisition module 403 includes:

[0145] A stop submodule, used for controlling the unmanned equipment to stop moving;

[0146] The target point cloud acquisition submodule is used to control the laser radar to swing around a rotation axis parallel to the ground and perpendicular to the direction of travel after the unmanned equipment stops moving, scan the area where the suspected obstacle is located at at least two different angles, and obtain at least two frames of target point clouds.

[0147] In some embodiments of the present invention, the point cloud fusion module 404 includes:

[0148] A pitch angle determination submodule, used to determine the pitch angle when the laser radar collects each frame of the target point cloud;

[0149] A coordinate conversion submodule, used for converting the at least two frames of target point clouds into the same coordinate system based on the pitch angle;

[0150] The fusion submodule is used to fuse the at least two frames of target point clouds located in the same coordinate system to obtain a dense point cloud.

[0151] In some embodiments of the present invention, the unmanned equipment is provided with a plurality of feature points for calibration, the target point cloud covers the feature points, and the pitch angle determination submodule includes:

[0152] A feature point coordinate determination unit, used to find a point corresponding to the feature point from the target point cloud, and determine a first coordinate of the point corresponding to the feature point in the laser radar coordinate system;

[0153] A first function construction unit is used to construct a function of the second coordinate and the angle when the laser radar collects the target point cloud based on the conversion relationship between the first coordinate and the second coordinate of the feature point in the unmanned equipment coordinate system;

[0154] A second function construction unit, used to construct a distance function from the second coordinate to the feature surface where the feature point is located;

[0155] The pitch angle determination unit is used to minimize the distance function with the angle at which the laser radar collects the target point cloud as a variable, and use the angle corresponding to the minimum value of the distance function as the pitch angle when the laser radar collects each frame of the target point cloud.

[0156] In some embodiments of the present invention, the function of the second coordinate and the angle at which the laser radar collects the target point cloud is as follows:

[0157]

[0158] Among them, (x′ i ,y′ i ,z′ i ) is the second coordinate of the i-th feature point in the unmanned equipment coordinate system, (x i ,y i ,z i ) is the first coordinate of the point corresponding to the i-th feature point in the target point cloud in the laser radar coordinate system, α,γ,t x ,t y ,t z are respectively the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, and θ is the angle at which the laser radar collects the target point cloud.

[0159] In some embodiments of the present invention, the distance function is minimized by taking the angle at which the laser radar collects the target point cloud as a variable, as follows:

[0160]

[0161] Among them, θ * is the pitch angle when the laser radar collects each frame of the target point cloud, a pi , b pi , c pi , d pi is the plane equation parameter of the feature surface corresponding to the i-th feature point.

[0162] In some embodiments of the present invention, the coordinate conversion submodule converts the at least two frames of target point clouds into the unmanned equipment coordinate system based on the following formula:

[0163]

[0164] Among them, (x′1, y′1, z′1) is the coordinate of the point in the target point cloud converted to the unmanned equipment coordinate system, (x1, y1, z1) is the coordinate of the point in the target point cloud in the laser radar coordinate system, α, γ, t x ,t y ,t z are the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, θ * The pitch angle when the laser radar collects the target point cloud.

[0165] In some embodiments of the present invention, the real obstacle determination module 405 includes:

[0166] A second clustering submodule, configured to process the dense point cloud using a clustering algorithm, and to take points in the dense point cloud whose mutual distances are less than a preset distance threshold as a second cluster;

[0167] The second size comparison submodule is used to compare the size of the second cluster with a preset size threshold, and regard the second cluster whose size is greater than or equal to the size threshold as a real obstacle on the travel path of the unmanned equipment.

[0168] The above-mentioned unmanned equipment road obstacle detection device can execute the unmanned equipment road obstacle detection method provided by any embodiment of the present invention, and has corresponding functional modules and beneficial effects for executing the unmanned equipment road obstacle detection method.

[0169] Embodiment 4

[0170] Embodiment 4 of the present invention provides an unmanned device. Exemplarily, the unmanned device is a cleaning device such as a sweeping robot and an unmanned sweeper. Figure 5 A schematic diagram of the structure of an unmanned device provided in the fourth embodiment of the present invention is shown in FIG. Figure 5 As shown, the unmanned device includes a processor 501, a memory 502, a communication module 503, an input device 504 and an output device 505; the number of processors 501 in the unmanned device can be one or more. Figure 5 A processor 501 is taken as an example; the processor 501, the memory 502, the communication module 503, the input device 504 and the output device 505 in the unmanned device can be connected by a bus or other means. Figure 5 The above processor 501, memory 502, communication module 503, input device 504 and output device 505 can be integrated on a control mainboard of an unmanned device.

[0171] The memory 502 is a computer-readable storage medium that can be used to store software programs, computer executable programs and modules, such as the module corresponding to the unmanned equipment road obstacle detection method in this embodiment. The processor 501 executes various functional applications and data processing of the unmanned equipment by running the software programs, instructions and modules stored in the memory 502, that is, the unmanned equipment road obstacle detection method provided in the above embodiment is implemented.

[0172] The memory 502 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system and at least one application required for a function; the data storage area may store data created according to the use of the unmanned device, etc. In addition, the memory 502 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other non-volatile solid-state storage device. In some instances, the memory 502 may further include a memory remotely arranged relative to the processor 501, and these remote memories may be connected to the unmanned device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0173] The communication module 503 is used to establish a connection with an external device (such as a smart terminal) and realize data interaction with the external device. The input device 504 can be used to receive input digital or character information and generate key signal input related to user settings and function control of the unmanned device.

[0174] The unmanned equipment provided in this embodiment can execute the unmanned equipment road obstacle detection method provided in any of the above embodiments of the present invention, and has specific corresponding functions and beneficial effects.

[0175] Embodiment 5

[0176] Embodiment 5 of the present invention provides a storage medium containing computer executable instructions, on which a computer program is stored. When the program is executed by a processor, the method for detecting road obstacles of an unmanned equipment provided in any of the above embodiments of the present invention is implemented. The method includes:

[0177] Obtain the point cloud collected by the laser radar during the movement of the unmanned equipment;

[0178] Determining a suspected obstacle on a path of travel of the unmanned equipment based on the point cloud;

[0179] Controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds;

[0180] Fusing the at least two frames of target point clouds to obtain a dense point cloud;

[0181] The real obstacles on the travel path of the unmanned equipment are determined based on the dense point cloud.

[0182] Of course, the storage medium containing computer executable instructions provided in the embodiment of the present invention is not limited to the method operations described above, and can also execute related operations in the unmanned equipment road obstacle detection method provided in the embodiment of the present invention.

[0183] It should be noted that, for the device, equipment and storage medium embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiments.

[0184] Through the above description of the implementation methods, technicians in the relevant field can clearly understand that the present invention can be implemented with the help of software and necessary general hardware, and of course it can also be implemented by hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as a computer floppy disk, read-only memory (ROM), random access memory (RAM), flash memory (FLASH), hard disk or optical disk, etc., including a number of instructions for a computer device (which can be a robot, a personal computer, a server, or a network device, etc.) to execute the unmanned equipment road obstacle detection method described in any embodiment of the present invention.

[0185] It is worth noting that in the above-mentioned device, the various modules, sub-modules and units included are only divided according to functional logic, but are not limited to the above-mentioned division, as long as the corresponding functions can be achieved; in addition, the specific names of the functional units are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention.

[0186] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, a plurality of steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution device. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0187] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0188] Note that the above are only preferred embodiments of the present invention and the technical principles used. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and that various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in more detail through the above embodiments, the present invention is not limited to the above embodiments, and may include more other equivalent embodiments without departing from the concept of the present invention, and the scope of the present invention is determined by the scope of the appended claims.

Claims

1. A method for detecting road obstacles of unmanned equipment, characterized in that: include: Obtain the point cloud collected by the laser radar during the movement of the unmanned equipment; Determining a suspected obstacle on a path of travel of the unmanned equipment based on the point cloud; Controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds; Fusing the at least two frames of target point clouds to obtain a dense point cloud; Determine real obstacles on the path of the unmanned equipment based on the dense point cloud; The step of fusing the at least two frames of target point clouds to obtain a dense point cloud includes: Determine the pitch angle when the laser radar collects each frame of the target point cloud; Converting the at least two frames of target point clouds to the same coordinate system based on the pitch angle; Fusing the at least two frames of target point clouds in the same coordinate system to obtain a dense point cloud; The unmanned equipment is provided with a plurality of feature points for calibration, the target point cloud covers the feature points, and the pitch angle when the laser radar collects each frame of the target point cloud is determined, including: Finding a point corresponding to the feature point from the target point cloud, and determining a first coordinate of the point corresponding to the feature point in the laser radar coordinate system; Based on the conversion relationship between the first coordinate and the second coordinate of the feature point in the unmanned equipment coordinate system, construct a function of the second coordinate and the angle when the laser radar collects the target point cloud; Constructing a distance function from the second coordinate to the feature surface where the feature point is located; The distance function is minimized by taking the angle at which the laser radar collects the target point cloud as a variable, and the angle corresponding to the minimum value of the distance function is used as the pitch angle when the laser radar collects each frame of the target point cloud; The distance function is minimized by taking the angle at which the laser radar collects the target point cloud as a variable, as follows: Among them, θ * is the pitch angle when the laser radar collects each frame of the target point cloud, a pi , b pi , c pi , d pi is the plane equation parameter of the feature surface corresponding to the i-th feature point, (x i ′,y i ′,z i ′) is the second coordinate of the i-th feature point in the unmanned equipment coordinate system, and θ is the angle at which the laser radar collects the target point cloud.

2. The method for detecting road obstacles of unmanned equipment according to claim 1, characterized in that: Determining a suspected obstacle on the path of the unmanned equipment based on the point cloud includes: Segmenting the point cloud into a first point cloud located above the ground and a second point cloud located below the ground; Using a clustering algorithm to process the first point cloud and the second point cloud respectively, and taking points in the first point cloud and the second point cloud whose mutual distance is less than a preset distance threshold as a first cluster; The size of the first cluster is compared with a preset size threshold, and the first cluster smaller than the size threshold is regarded as a suspected obstacle on the travel path of the unmanned equipment.

3. The method for detecting road obstacles of unmanned equipment according to claim 1, characterized in that: Controlling the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds, including: Controlling the unmanned equipment to stop moving; After the unmanned equipment stops moving, the laser radar is controlled to swing around a rotation axis parallel to the ground and perpendicular to the moving direction, and the area where the suspected obstacle is located is scanned at at least two different angles to obtain at least two frames of target point clouds.

4. The method for detecting road obstacles of unmanned equipment according to claim 1, characterized in that: The function of the second coordinate and the angle when the laser radar collects the target point cloud is as follows: Among them, (x i ′,y i ′,z i ′) is the second coordinate of the i-th feature point in the unmanned equipment coordinate system, (x i ,y i ,z i ) is the first coordinate of the point corresponding to the i-th feature point in the target point cloud in the laser radar coordinate system, α,γ,t x ,t y ,t z are respectively the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, and θ is the angle at which the laser radar collects the target point cloud.

5. The method for detecting road obstacles of unmanned equipment according to claim 1, characterized in that: The method converts the at least two frames of target point clouds into the same coordinate system based on the pitch angle, comprising: Convert the at least two frames of target point clouds to the unmanned equipment coordinate system based on the following formula; Among them, (x1′, y1′, z1′) is the coordinate of the point in the target point cloud converted to the unmanned equipment coordinate system, (x1, y1, z1) is the coordinate of the point in the target point cloud in the laser radar coordinate system, α, γ, t x ,t y ,t z are the roll angle, yaw angle and position parameters of the laser radar relative to the unmanned equipment, θ * The pitch angle when the laser radar collects the target point cloud.

6. The method for detecting road obstacles of unmanned equipment according to any one of claims 1 to 5, characterized in that: Determining a real obstacle on the path of the unmanned equipment based on the dense point cloud includes: Processing the dense point cloud using a clustering algorithm, and taking points in the dense point cloud whose mutual distances are less than a preset distance threshold as a second cluster; The size of the second cluster is compared with a preset size threshold, and the second cluster whose size is greater than or equal to the size threshold is regarded as a real obstacle on the travel path of the unmanned equipment.

7. An unmanned equipment road obstacle detection device, characterized in that: include: Point cloud acquisition module, used to obtain point clouds collected by the laser radar during the movement of unmanned equipment; A suspected obstacle determination module, used to determine a suspected obstacle on the travel path of the unmanned equipment based on the point cloud; A target point cloud acquisition module, used to control the laser radar to scan the area where the suspected obstacle is located at at least two different angles to obtain at least two frames of target point clouds; A point cloud fusion module, used for fusing the at least two frames of target point clouds to obtain a dense point cloud; A real obstacle determination module, used to determine the real obstacles on the travel path of the unmanned equipment based on the dense point cloud; Wherein, the point cloud fusion module includes: A pitch angle determination submodule, used to determine the pitch angle when the laser radar collects each frame of the target point cloud; A coordinate conversion submodule, used for converting the at least two frames of target point clouds into the same coordinate system based on the pitch angle; A fusion submodule, used for fusing the at least two frames of target point clouds in the same coordinate system to obtain a dense point cloud; The unmanned equipment is provided with a plurality of feature points for calibration, the target point cloud covers the feature points, and the pitch angle determination submodule includes: A feature point coordinate determination unit, used to find a point corresponding to the feature point from the target point cloud, and determine a first coordinate of the point corresponding to the feature point in the laser radar coordinate system; A first function construction unit is used to construct a function of the second coordinate and the angle when the laser radar collects the target point cloud based on the conversion relationship between the first coordinate and the second coordinate of the feature point in the unmanned equipment coordinate system; A second function construction unit, used to construct a distance function from the second coordinate to the feature surface where the feature point is located; A pitch angle determination unit, used to minimize the distance function with the angle at which the laser radar collects the target point cloud as a variable, and use the angle corresponding to the minimum value of the distance function as the pitch angle when the laser radar collects each frame of the target point cloud; The distance function is minimized by taking the angle at which the laser radar collects the target point cloud as a variable, as follows: Among them, θ * is the pitch angle when the laser radar collects each frame of the target point cloud, a pi , b pi , c pi , d pi is the plane equation parameter of the feature surface corresponding to the i-th feature point, (x i ′,y i ′,z i ′) is the second coordinate of the i-th feature point in the unmanned equipment coordinate system, and θ is the angle at which the laser radar collects the target point cloud.

8. An unmanned device, characterized in that: include: one or more processors; A storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the unmanned equipment road obstacle detection method as described in any one of claims 1-6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the unmanned equipment road obstacle detection method as described in any one of claims 1-6 is implemented.

Images