Obstacle detection method, device, robot and computer-readable storage medium

Abstract

The embodiment of the present application provides an obstacle detection method, device, robot and computer-readable storage medium, wherein the method includes: converting a depth image into first point cloud data; converting the first point cloud data into second point cloud data in the robot coordinate system according to the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system; extracting the point cloud data of interest from the second point cloud data according to the preset area of ​​interest; determining the point cloud data of the passable area according to the plane feature data of each plane in the point cloud data of interest; excluding the point cloud data of the passable area from the point cloud data of interest to obtain the remaining point cloud data; determining the obstacle information according to the remaining point cloud data. In this way, the obstacle information is calculated by using the three-dimensional point conversion relationship between the depth image taken by the depth camera, the depth camera coordinate system and the robot coordinate system, which can improve the accuracy of obstacle detection and improve the obstacle detection effect.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to an obstacle detection method, device, robot and computer-readable storage medium. Background Art

[0002] In the prior art, robot obstacle avoidance is a necessary condition for intelligent and safe operation. Obstacle avoidance means that when the robot senses static or dynamic obstacles on the planned route through sensors during walking, it updates the path in real time according to a certain algorithm, bypasses the obstacles, and finally reaches the target point. In order to achieve obstacle avoidance and navigation, the robot needs to perceive the environment. Specifically, in an unknown or partially unknown environment, the robot needs to obtain surrounding environment information through sensors for obstacle avoidance. The surrounding environment information includes information such as the size, shape and position of obstacles. Therefore, sensor technology plays a very important role in mobile robot obstacle avoidance. There are many kinds of sensors used in obstacle avoidance control, each with different principles and characteristics. Currently, the most common ones are ultrasonic sensors, laser sensors, infrared sensors, etc. At present, the mainstream environmental perception strategy of robots is that depth cameras and laser radars are used to detect obstacles in front, and anti-fall sensors are used for robots to perceive cliffs when they are close to cliffs. However, this strategy requires that part of the robot's body is already within the range of the cliff, and is only suitable for robots such as sweepers that do not have high safety requirements. Existing robots have the problem of poor obstacle detection effect. Summary of the invention

[0003] In order to solve the above technical problems, the embodiments of the present application provide an obstacle detection method, device, robot and computer-readable storage medium.

[0004] In a first aspect, an embodiment of the present application provides an obstacle detection method, the method comprising:

[0005] Converting the depth image captured by the depth camera into first point cloud data in the depth camera coordinate system according to the internal parameters of the depth camera;

[0006] Converting the first point cloud data into second point cloud data in the robot coordinate system according to a three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system;

[0007] Extracting point cloud data of interest from the second point cloud data according to a preset area of ​​interest;

[0008] Acquire at least one plane from the interest point cloud data, and determine the traversable area point cloud data from the interest point cloud data according to the plane feature data of each plane;

[0009] Excluding the traversable area point cloud data from the point cloud data of interest to obtain remaining point cloud data;

[0010] Obstacle information is determined according to the remaining point cloud data.

[0011] In a second aspect, an embodiment of the present application provides an obstacle detection device, the device comprising:

[0012] A first conversion module, configured to convert a depth image captured by the depth camera into first point cloud data in a depth camera coordinate system according to an internal parameter of the depth camera;

[0013] A second conversion module, configured to convert the first point cloud data into second point cloud data in the robot coordinate system according to a three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system;

[0014] An extraction module, used for extracting interested point cloud data from the second point cloud data according to a preset interested area;

[0015] An acquisition module, used to acquire at least one plane from the interest point cloud data, and determine the passable area point cloud data from the interest point cloud data according to the plane feature data of each plane;

[0016] An exclusion module, used to exclude the point cloud data of the passable area from the point cloud data of interest to obtain the remaining point cloud data;

[0017] A determination module is used to determine obstacle information based on the remaining point cloud data.

[0018] In a third aspect, an embodiment of the present application provides a mobile terminal, including a memory and a processor, wherein the memory is used to store a computer program, and when the computer program is run by the processor, the obstacle detection method provided in the first aspect is executed.

[0019] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium storing a computer program, which executes the obstacle detection method provided in the first aspect when running on a processor.

[0020] The obstacle detection method, device, robot and computer-readable storage medium provided by the present application above convert the depth image captured by the depth camera into the first point cloud data in the depth camera coordinate system according to the internal parameters of the depth camera; convert the first point cloud data into the second point cloud data in the robot coordinate system according to the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system; extract the point cloud data of interest from the second point cloud data according to the preset area of ​​interest; obtain at least one plane from the point cloud data of interest, and determine the point cloud data of the passable area from the point cloud data of interest according to the plane feature data of each plane; exclude the point cloud data of the passable area from the point cloud data of interest to obtain the remaining point cloud data; determine the obstacle information according to the remaining point cloud data. In this way, the obstacle information is calculated by using the depth image captured by the depth camera, the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system, which can improve the accuracy of obstacle detection and improve the obstacle detection effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the technical solution of the present application, the following is a brief introduction to the drawings required for use in the embodiments. It should be understood that the following drawings only illustrate certain embodiments of the present application and should not be regarded as limiting the scope of protection of the present application. In each of the drawings, similar components are numbered similarly.

[0022] Figure 1 A schematic diagram of an application environment of the robot provided by an embodiment of the present application is shown;

[0023] Figure 2 A schematic diagram of a process of an obstacle detection method provided in an embodiment of the present application is shown;

[0024] Figure 3 A schematic diagram of a depth image provided by an embodiment of the present application is shown;

[0025] Figure 4 A schematic diagram of point cloud data provided by an embodiment of the present application is shown;

[0026] Figure 5 Another schematic diagram of point cloud data provided by an embodiment of the present application is shown;

[0027] Figure 6 A schematic diagram showing a preset region of interest provided in an embodiment of the present application is shown;

[0028] Figure 7 A schematic diagram of a point cloud projection diagram provided by an embodiment of the present application is shown;

[0029] Figure 8 A schematic diagram of the structure of an obstacle detection device provided in an embodiment of the present application is shown. DETAILED DESCRIPTION

[0030] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments.

[0031] The components of the embodiments of the present application generally described and shown in the drawings herein may be arranged and designed in various configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present application.

[0032] Hereinafter, the terms "including", "having" and their cognates, which may be used in various embodiments of the present application, are intended only to indicate specific features, numbers, steps, operations, elements, components, or combinations of the foregoing items, and should not be understood as first excluding the existence of one or more other features, numbers, steps, operations, elements, components, or combinations of the foregoing items or adding the possibility of one or more features, numbers, steps, operations, elements, components, or combinations of the foregoing items.

[0033] Furthermore, the terms “first”, “second”, “third”, etc. are merely used for distinguishing descriptions and are not to be understood as indicating or implying relative importance.

[0034] Unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meanings as those generally understood by those skilled in the art to which the various embodiments of the present application belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meanings as the contextual meanings in the relevant technical field and will not be interpreted as having idealized meanings or overly formal meanings unless clearly defined in the various embodiments of the present application.

[0035] Common sensors used in existing obstacle avoidance technologies include ultrasonic sensors, laser sensors, infrared sensors, etc. Ultrasonic sensors use sound waves to measure distance, and sound generally propagates in a cone, so the distance obtained is the nearest object in the cone wave. In addition, ultrasonic waves cannot work when they absorb sound waves or are disturbed by strong winds.

[0036] Laser sensors are highly accurate, but two-dimensional laser sensors can only detect the radar plane and cannot detect areas below the ground or below the laser sensor. Three-dimensional laser sensors are expensive and are currently rarely used in low-speed radar driving.

[0037] Infrared sensors (depth cameras) use infrared emitters and CCD detectors. Infrared emitters emit infrared rays, which are reflected on objects. After the reflected light is received by the CCD detector, the distance of the object can be obtained. Infrared sensors have large data volume and relatively high accuracy. Compared with ultrasound, they have higher bandwidth. Infrared sensors are easily interfered by various heat sources and light sources.

[0038] In summary, existing robots have the problem of poor obstacle detection effect.

[0039] Example 1

[0040] The present disclosure provides an obstacle detection method, which is applied to a robot equipped with a depth camera. Figure 1 , Figure 1 The figure shows a schematic diagram of an application environment of the robot provided in this embodiment. The robot 101 is equipped with a depth camera 102. The robot 101 may encounter an obstacle 103 during movement. The depth camera 102 captures a depth image, which may include an image of the obstacle 103 and an image of the ground.

[0041] For details, see Figure 2 The obstacle detection method provided in this embodiment includes the following steps:

[0042] Step S201: converting a depth image captured by the depth camera into first point cloud data in a depth camera coordinate system according to an internal parameter of the depth camera.

[0043] See also Figure 3 , Figure 3 FIG. 1 is a schematic diagram of a depth image provided by this embodiment. Figure 3 The depth image 30 shown is Figure 1 The depth image 30 is captured by the depth camera 102, and includes an obstacle image area 301 and a robot local image area 302, as well as a ground image area 303 sandwiched between the obstacle image area 301 and the robot local image area 302.

[0044] In this embodiment, the internal parameters of the depth camera include the origin offset size and focal length of the depth camera. For example, Figure 1 As shown in the application environment diagram, the depth image 30 can be converted into the first point cloud data under the depth camera coordinates according to the origin offset size and focal length of the depth camera 102. Specifically, the depth image can be converted into the first point cloud data according to the following formula 1:

[0045] Formula 1: ;

[0046] Among them, j and i represent the horizontal coordinate and vertical coordinate of the pixel point in the depth image respectively, and d is the depth value of the pixel point. is the camera origin offset size, is the focal length of the camera, Indicates the point cloud coordinates corresponding to each pixel.

[0047] See also Figure 4 , Figure 4 Shown is the Figure 3 The depth image 30 shown is converted into the first point cloud data 400 under the depth camera coordinates. The first point cloud data 400 includes a first point cloud cluster 401, a second point cloud cluster 402 and a third point cloud cluster 403. The first point cloud cluster 401, the second point cloud cluster 402 and the third point cloud cluster 403 correspond to the obstacle image area 301, the robot local image area 302 and the ground image area 303 in the depth image 30, respectively.

[0048] Step S202: converting the first point cloud data into second point cloud data in the robot coordinate system according to the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system.

[0049] It should be noted that when the robot installs the depth camera, it will perform preliminary installation according to the standard installation parameters of the depth camera. Due to the errors between the actual installation parameters and the standard installation parameters during the actual installation process, for example, the standard installation parameters are that the depth camera is 70cm from the ground and tilted 45° downward, while the actual installation parameters are 75cm from the ground and tilted 50° downward. These installation errors will cause the point cloud cluster corresponding to the ground image area in the first point cloud data converted from the depth image to not coincide with the ground plane. For details, please refer to Figure 4 , the third point cloud cluster 403 cannot coincide with the ground plane 404 .

[0050] In order to reduce the impact of installation errors on the obstacle detection process, it is necessary to adopt a coordinate conversion method to convert the point cloud data in the depth camera coordinate system into the point cloud data in the robot coordinate system.

[0051] In one embodiment, the step of acquiring the three-dimensional point conversion relationship includes the following steps:

[0052] Extracting an auxiliary plane from the first point cloud data;

[0053] Determining a normal vector of the auxiliary plane in the depth camera coordinate system;

[0054] Determine a projection vector of the first direction axis vector projected onto the auxiliary plane according to the distance between the first direction axis vector and the auxiliary plane in the robot coordinate system and the first direction axis vector;

[0055] Determine a second direction axis vector in the depth camera coordinates according to the projection vector and the normal vector;

[0056] Determining the intercept of the auxiliary plane in the robot coordinate system;

[0057] The three-dimensional point conversion relationship is determined according to the projection vector, the normal vector, the second direction axis vector and the intercept.

[0058] The following is an explanation of the process of obtaining the three-dimensional point conversion relationship. Extract the auxiliary plane from the first point cloud data according to the following formula 2:

[0059] Formula 2: , where a, b, c, and d are known constants.

[0060] Thus auxiliary plane The plane normal vector is , assuming that the y-axis unit vector in robot coordinates is The projection vector onto the auxiliary plane is .

[0061] Calculate the y-axis unit vector in the robot coordinate system according to the following formula 3 With the auxiliary plane The distance between

[0062] Formula 3: ;

[0063] Among them, t represents the distance between the y-axis unit vector in the robot coordinate system and the auxiliary plane, and a, b, c, and d represent the plane feature data of the auxiliary plane.

[0064] because is the y-axis unit vector The projection vector on the auxiliary plane, so the projection vector and the y-axis unit vector are on a straight line, and the straight line is perpendicular to the auxiliary plane, is the y-axis unit vector Collinear, we can get the following formula 4:

[0065] Formula 4: ;

[0066] Among them, e, f, g represent the projection vector Components of, a, b, c represent the normal vector , t represents the distance between the y-axis unit vector in the robot coordinate system and the auxiliary plane.

[0067] Suppose the X-axis vector in the robot coordinate system is , we can derive the following formula 5:

[0068] Formula 5: ;

[0069] in represents the aforementioned projection vector, represents the aforementioned normal vector;

[0070] X-axis vector , projection vector , normal vector After normalization, we get the following normalized vector , , ; Get auxiliary plane The intercept in the robot coordinate system ;

[0071] According to the nature of the rotation change, the following formula 6 is obtained to determine the three-dimensional point transformation relationship:

[0072] Formula 6: ; Where R represents the transformation matrix;

[0073] In one embodiment, the first point cloud data in the depth camera coordinate system may be converted into the second point cloud data in the robot coordinate system according to the following formula 7:

[0074] Formula 7: ;

[0075] Among them, R represents the transformation matrix in the previous text, P represents the first point cloud data before the transformation, , Represents the second point cloud data after transformation, .

[0076] In another embodiment, the first point cloud data in the depth camera coordinate system may be converted into the second point cloud data according to the following formula 8;

[0077] Formula 8: ;

[0078] Among them, R represents the transformation matrix in the previous text, Represents the homogeneous equation of the first point cloud data before transformation, , Represents the homogeneous equation of the second point cloud data after transformation, .

[0079] In this embodiment, after the first point cloud data is converted into the second point cloud data through the three-dimensional point conversion relationship, the point cloud data in the depth camera coordinate system can be converted into the point cloud data in the robot coordinate system, which meets the actual visual requirements of the robot.

[0080] For example, see Figure 5 , Figure 5 The figure shows the point cloud data after the coordinate system is transformed through the three-dimensional point transformation relationship. Figure 5 Shown is the Figure 3 The point cloud data shown is converted into the second point cloud data 500 in the robot coordinate system. The second point cloud data 500 includes a fourth point cloud cluster 501, a fifth point cloud cluster 502 and a sixth point cloud cluster 503. The fourth point cloud cluster 501, the fifth point cloud cluster 502 and the sixth point cloud cluster 503 correspond to the obstacle image area 301, the robot local image area 302 and the ground image area 303 in the depth image 30 respectively. The sixth point cloud cluster 503 coincides with the ground plane, which meets the actual visual requirements of the robot.

[0081] Step S203: extracting point cloud data of interest from the second point cloud data according to a preset area of ​​interest.

[0082] In one embodiment, the preset region of interest can be set by the user. The preset region of interest is an area that needs to be focused on to see if there are obstacles. For example, the preset region of interest can be a preset area directly in front of the robot. Figure 6 , move the robot 600 The range area is set to the preset area of ​​interest 601, and the area in front of the robot can be extracted from the second point cloud data. The point cloud data within is taken as the point cloud data of interest.

[0083] Step S204, obtaining at least one plane from the point cloud data of interest, and determining the passable area point cloud data from the point cloud data of interest according to the plane feature data of each plane.

[0084] In one embodiment, it is possible to preliminarily determine whether the region of interest is blocked based on the number of three-dimensional points in the point cloud data of interest. Specifically, the obstacle detection method of this embodiment further includes the following steps:

[0085] Determining whether the number of three-dimensional points of the point cloud data of interest is greater than a first preset three-dimensional point threshold;

[0086] If the number of three-dimensional points of the interest point cloud data is greater than the first preset three-dimensional point threshold, executing the step of acquiring at least one plane from the interest point cloud data;

[0087] If the number of three-dimensional points in the point cloud data of interest is less than or equal to the first preset three-dimensional point threshold, it is determined that no obstacle is detected in the preset area of ​​interest.

[0088] The first preset 3D point threshold is related to the depth camera resolution, the robot operation site, and the depth camera installation location, and can be determined by the user based on multiple experience values. If the number of 3D points in the point cloud data of interest is greater than the first preset 3D point threshold, it indicates that there may be obstacles blocking the view, and it is necessary to further obtain obstacle information from the point cloud data of interest. If the number of 3D points in the point cloud data of interest is less than or equal to the first preset 3D point threshold, it indicates that there are no obstacles blocking the view, and it can be determined that there are no obstacles in the preset area of ​​interest, and the robot can drive safely.

[0089] It should be noted that there are often multiple planes in the point cloud data of interest, such as walls, ground, etc. At least one plane can be extracted from the point cloud data of interest according to the following formula 9:

[0090] Formula 9: ;

[0091] Among them, A, B, C, and D are parameters.

[0092] In this embodiment, the plane feature data of each plane includes the plane inclination and the plane height, and the preset obstacle recognition threshold may include an obstacle inclination threshold and an obstacle height threshold. Whether each plane is a passable plane can be determined based on whether the plane inclination and the plane height of each plane are less than the obstacle inclination threshold and the obstacle height threshold.

[0093] Specifically, the step of determining the traversable area point cloud data from the interest point cloud data according to the plane feature data of each plane in step S204 includes the following steps:

[0094] Determine whether the plane feature data of each plane is less than a preset obstacle recognition threshold;

[0095] If there is a passable plane in each plane whose plane feature data is less than the preset obstacle recognition threshold, the point cloud data corresponding to the passable plane is used as the point cloud data of the passable area.

[0096] It should be noted that the inclination and height of each plane can be determined according to the plane equation corresponding to each plane. Specifically, according to Formula 9, the inclination of each plane is the normal vector The angle with the z-axis, the height of each plane is D;

[0097] The inclination angle of each plane can be calculated according to the following formula 10:

[0098] Formula 10: ;

[0099] Among them, θ represents the inclination angle of the plane, and A, B, and C represent the parameters of the plane equation.

[0100] Specifically, when the plane inclination angle of a plane is less than the obstacle inclination angle threshold, and the plane height of the plane is less than the obstacle height threshold, the plane is determined to be a passable plane. When the plane inclination angle of a plane is greater than or equal to the obstacle inclination angle threshold, or the plane height of the plane is greater than or equal to the obstacle height threshold, the plane is determined to be an obstacle plane.

[0101] In this way, the traversable area point cloud data can be determined from the point cloud data of interest based on the plane feature data of each plane, which facilitates the rapid exclusion of traversable point cloud data such as ground point cloud data from the point cloud data of interest, reduces the data processing volume, and improves the data processing speed.

[0102] Step S205 , excluding the traversable area point cloud data from the point cloud data of interest to obtain remaining point cloud data.

[0103] It should be noted that, since the amount of obstacle point cloud data is generally large during the actual application of the robot, it is possible to determine whether there are obstacles in the remaining point cloud data based on the number of three-dimensional points in the remaining point cloud data.

[0104] Specifically, the obstacle detection method of this embodiment further includes the following steps:

[0105] Determining whether the number of three-dimensional points of the remaining point cloud data is greater than a second preset three-dimensional point threshold;

[0106] If the number of three-dimensional points of the remaining point cloud data is greater than the second preset three-dimensional point threshold, executing the step of determining obstacle information according to the remaining point cloud data;

[0107] If the number of three-dimensional points of the remaining point cloud data is less than or equal to the second preset three-dimensional point threshold, it is determined that no obstacle is detected in the preset area of ​​interest.

[0108] In this embodiment, if the number of three-dimensional points in the remaining point cloud data is greater than the second preset three-dimensional point threshold, it means that there may be obstacles in the remaining point cloud data, and it is necessary to further obtain obstacle information from the remaining point cloud data. If the number of three-dimensional points in the remaining point cloud data is less than or equal to the second preset three-dimensional point threshold, it means that there are no obstacles in the remaining point cloud data. It can be determined that there are no obstacles in the preset area of ​​interest and the robot can drive safely.

[0109] It is further explained that, since there are often multiple planes in the point cloud data of interest, such as walls, ground, etc., only one plane can be extracted in sequence to determine whether the extracted plane is a passable plane. Specifically, the obstacle detection method provided in this embodiment can also include the following steps:

[0110] Extracting a plane from the cloud data of points of interest as a first extracted plane, and obtaining first plane feature data of the first extracted plane;

[0111] Determining whether the first plane feature data is less than the preset obstacle recognition threshold;

[0112] If the first plane feature data is less than the preset obstacle recognition threshold, the first extracted plane is determined to be a passable plane, and the point cloud data corresponding to the first extracted plane is excluded from the point cloud data of interest to obtain first local point cloud data;

[0113] Determine whether the first local point cloud data is greater than a third preset 3D point threshold, wherein the third preset 3D point threshold is determined according to the number of 3D points of the point cloud data of interest and a preset ratio;

[0114] If the first local point cloud data is greater than the third preset three-dimensional point threshold, extracting a plane from the first local point cloud data as a second extracted plane, obtaining second plane feature data of the second extracted plane, and determining whether the second plane feature data is less than the preset obstacle recognition threshold;

[0115] If the second plane feature data is smaller than the preset obstacle recognition threshold, the point cloud data corresponding to the second plane is excluded from the first local point cloud data to obtain the second local point cloud data, and the second local point cloud data is subjected to the exclusion of the point cloud data corresponding to the passable plane to obtain the processed point cloud data, until the number of three-dimensional points in the processed point cloud data is smaller than the third preset three-dimensional point threshold, and the last processed point cloud data is used as the remaining point cloud data.

[0116] It should be noted that the above-mentioned step of determining whether the first plane feature data is less than the preset obstacle recognition threshold, and the step of determining whether the second plane feature data is less than the preset obstacle recognition threshold, are similar to the aforementioned specific process of determining whether the plane feature data of each plane is less than the preset obstacle recognition threshold. Please refer to the relevant content for understanding. To avoid repetition, they will not be described here.

[0117] In one implementation, the third preset three-dimensional point threshold is determined according to the point cloud data of interest and a preset ratio. Specifically, it can be determined according to the following formula 11:

[0118] Formula 11: ;

[0119] Wherein, N represents the number of three-dimensional points of the point cloud data of interest, and ratio represents the preset ratio.

[0120] The preset ratio refers to the remaining point cloud ratio after removing the point cloud of the ground and other drivable areas from the area of ​​interest. The purpose of setting the preset ratio is to prevent infinite plane extraction from the remaining point cloud data. The preset ratio ratio can be 5%~10%.

[0121] The obstacle detection method provided in this embodiment may further include the following steps:

[0122] If the first plane feature data or the second plane feature data is greater than or equal to the preset obstacle recognition threshold, it is determined that the first extracted plane or the second plane feature data is an obstacle plane.

[0123] It is additionally noted that, if an extracted plane is determined to be an obstacle plane, it is not necessary to exclude the point cloud data corresponding to the obstacle plane from the point cloud data of interest or the first partial point cloud data.

[0124] Step S206: Determine obstacle information based on the remaining point cloud data.

[0125] In this embodiment, since there are a large number of three-dimensional points in the remaining point cloud data, the three-dimensional points can be traversed according to the distance from the robot to obtain obstacle information from the remaining point cloud data.

[0126] Specifically, step S206 may include the following steps:

[0127] Projecting each three-dimensional point of the remaining point cloud data into a preset grid area to obtain a point cloud projection map;

[0128] Traversing a plurality of projection points in the point cloud projection image, and determining a preset number of projection points closest to a current traversal point from the plurality of projection points;

[0129] Determine whether the distance between each projection point of the preset number of projection points and the current traversal point is less than a preset distance threshold;

[0130] If the distance between each projection point of the preset number of projection points and the current traversal point is less than the preset distance threshold, determining that the current traversal point and the preset number of projection points are obstacle point clouds;

[0131] If there is a projection point among the preset number of projection points whose distance to the current traversal point is greater than or equal to the preset distance threshold, continue to traverse other projection points until the traversal of other projection points is completed;

[0132] Obstacle information is determined according to the obstacle point cloud.

[0133] In one embodiment, the step of projecting each three-dimensional point of the remaining point cloud data into a preset grid area to obtain a point cloud projection map includes:

[0134] Sorting the three-dimensional points of the remaining point cloud data according to the distance between each three-dimensional point of the remaining point cloud data and the robot to obtain sorted point cloud data;

[0135] The sorted point cloud data is projected onto the preset grid area to obtain the point cloud projection image.

[0136] See also Figure 7 , Figure 7 The figure shows a schematic diagram of the point cloud projection. Figure 7 The point cloud projection diagram shown includes multiple grids 701 and multiple projection points 702 located on the multiple grids 701. The multiple projection points 702 are obtained by projecting each three-dimensional point in the remaining point cloud data to a preset grid area according to the distance between the robot and the robot. The size of the preset grid area can be determined according to the size of the region of interest. The preset grid area can be designed by the user to have a custom grid size, generally 5 cm, and one grid can contain multiple three-dimensional points. The preset grid area can be designed according to the aforementioned preset region of interest, and the three-dimensional points in the remaining point cloud data can be projected into the corresponding grid.

[0137] Combine the following Figure 7 The process of obtaining obstacle information is described. According to the principle of from front to back, a projection point is extracted from the point cloud projection map by traversal, and it is determined whether the current traversal point is the last projection point in the point cloud projection map. If it is not the last projection point, the KD-Tree algorithm is used to search for the N0 projection points closest to the current traversal point. For the above N0 projection points, it is determined whether the distance between each projection point in the N0 projection points and the current traversal point is less than the preset distance threshold L0. If the distance between each projection point in the N0 projection points and the current traversal point is less than the preset distance threshold L0, it is determined that the current traversal point and the N0 projection points closest to the current traversal point are obstacle point cloud clusters, and the corresponding obstacle three-dimensional point cloud data is obtained according to the obstacle point cloud cluster. The obstacle information is determined according to the obstacle three-dimensional point cloud data, wherein the obstacle information includes obstacle height, obstacle width, obstacle length, obstacle inclination and other information. If the obstacle height is greater than 0, it means that the obstacle is an obstacle above the ground. If the obstacle height is less than 0, it means that the obstacle is an obstacle such as a step below the ground.

[0138] It should be noted that according to the principle of from front to back, a projection point is extracted from the point cloud projection map, and the obstacle information obtained is determined to be the obstacle information closest to the robot. After detecting the nearest obstacle information, since the robot needs to perform obstacle avoidance design, which may cause the driving path to change, it is not necessary to traverse the subsequent projection points, which can reduce the amount of calculation.

[0139] It should be noted that the KD-Tree algorithm is an algorithm that quickly searches for the point closest to the current point in space. Generally, a KD-tree is constructed first and then searched, which can improve computing efficiency.

[0140] In one embodiment, if there is a projection point among the N0 projection points whose distance from the current traversal point is greater than or equal to a preset distance threshold L0, it is determined that the current traversal point and the N0 projection points closest to the current traversal point are not obstacle point cloud clusters, and the next projection point is traversed in sequence until the current traversal point is the last projection point in the point cloud projection map.

[0141] The obstacle detection method provided in this embodiment converts the depth image captured by the depth camera into the first point cloud data in the depth camera coordinate system according to the internal parameters of the depth camera; converts the first point cloud data into the second point cloud data in the robot coordinate system according to the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system; extracts the point cloud data of interest from the second point cloud data according to the preset area of ​​interest; obtains at least one plane from the point cloud data of interest, and determines the point cloud data of the passable area from the point cloud data of interest according to the plane feature data of each plane; excludes the point cloud data of the passable area from the point cloud data of interest to obtain the remaining point cloud data; and determines the obstacle information according to the remaining point cloud data. In this way, the obstacle information is calculated by using the three-dimensional point conversion relationship between the depth image captured by the depth camera, the depth camera coordinate system and the robot coordinate system, which can improve the accuracy of obstacle detection and improve the obstacle detection effect.

[0142] Example 2

[0143] In addition, an embodiment of the present disclosure provides an obstacle detection device.

[0144] Specifically, Figure 8 As shown, the obstacle detection device 800 includes:

[0145] A first conversion module 801, configured to convert a depth image taken by the depth camera into first point cloud data in a depth camera coordinate system according to an internal parameter of the depth camera;

[0146] A second conversion module 802, configured to convert the first point cloud data into second point cloud data in the robot coordinate system according to a three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system;

[0147] An extraction module 803 is used to extract point cloud data of interest from the second point cloud data according to a preset area of ​​interest;

[0148] An acquisition module 804 is used to acquire at least one plane from the interest point cloud data, and determine the passable area point cloud data from the interest point cloud data according to the plane feature data of each plane;

[0149] An exclusion module 805 is used to exclude the traversable area point cloud data from the point cloud data of interest to obtain the remaining point cloud data;

[0150] The determination module 806 is used to determine obstacle information according to the remaining point cloud data.

[0151] In one embodiment, the obstacle detection device 800 further includes:

[0152] A first processing module, used for determining whether the number of three-dimensional points of the point cloud data of interest is greater than a first preset three-dimensional point threshold;

[0153] If the number of three-dimensional points of the interest point cloud data is greater than the first preset three-dimensional point threshold, executing the step of acquiring at least one plane from the interest point cloud data;

[0154] If the number of three-dimensional points of the point cloud data of interest is less than or equal to the first preset three-dimensional point threshold, it is determined that no obstacle is detected in the preset area of ​​interest.

[0155] In one embodiment, the obstacle detection device 800 further includes:

[0156] The acquisition module 804 is also used to determine whether the plane feature data of each plane is less than a preset obstacle recognition threshold;

[0157] If there is a passable plane in each plane whose plane feature data is less than the preset obstacle recognition threshold, the point cloud data corresponding to the passable plane is used as the point cloud data of the passable area.

[0158] In one embodiment, the obstacle detection device 800 further includes:

[0159] A second processing module, used for determining whether the number of three-dimensional points of the remaining point cloud data is greater than a second preset three-dimensional point threshold;

[0160] If the number of three-dimensional points of the remaining point cloud data is greater than the second preset three-dimensional point threshold, executing the step of determining obstacle information according to the remaining point cloud data;

[0161] If the number of three-dimensional points of the remaining point cloud data is less than or equal to the second preset three-dimensional point threshold, it is determined that no obstacle is detected in the preset area of ​​interest.

[0162] In one embodiment, the determination module 806 is further configured to project each three-dimensional point of the remaining point cloud data into a preset grid area to obtain a point cloud projection map;

[0163] Traversing a plurality of projection points in the point cloud projection image, and determining a preset number of projection points closest to a current traversal point from the plurality of projection points;

[0164] Determine whether the distance between each projection point of the preset number of projection points and the current traversal point is less than a preset distance threshold;

[0165] If the distance between each projection point of the preset number of projection points and the current traversal point is less than the preset distance threshold, determining that the current traversal point and the preset number of projection points are obstacle point clouds;

[0166] If there is a projection point among the preset number of projection points whose distance to the current traversal point is greater than or equal to the preset distance threshold, continue to traverse other projection points until the traversal of other projection points is completed;

[0167] Obstacle information is determined according to the obstacle point cloud.

[0168] In one embodiment, the determination module 806 is further configured to sort the three-dimensional points of the remaining point cloud data according to the distance between each three-dimensional point of the remaining point cloud data and the robot to obtain sorted point cloud data;

[0169] The sorted point cloud data is projected onto the preset grid area to obtain the point cloud projection image.

[0170] In one embodiment, the obstacle detection device 800 further includes:

[0171] A third processing module, used for extracting an auxiliary plane from the first point cloud data;

[0172] Determining a normal vector of the auxiliary plane in the depth camera coordinate system;

[0173] Determine a projection vector of the first direction axis vector projected onto the auxiliary plane according to the distance between the first direction axis vector and the auxiliary plane in the robot coordinate system and the first direction axis vector;

[0174] Determine a second direction axis vector in the depth camera coordinates according to the projection vector and the normal vector;

[0175] Determining the intercept of the auxiliary plane in the robot coordinate system;

[0176] The three-dimensional point conversion relationship is determined according to the projection vector, the normal vector, the second direction axis vector and the intercept.

[0177] The obstacle detection device provided in this embodiment converts the depth image captured by the depth camera into the first point cloud data in the depth camera coordinate system according to the internal parameters of the depth camera; converts the first point cloud data into the second point cloud data in the robot coordinate system according to the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system; extracts the point cloud data of interest from the second point cloud data according to the preset area of ​​interest; obtains at least one plane from the point cloud data of interest, and determines the point cloud data of the passable area from the point cloud data of interest according to the plane feature data of each plane; excludes the point cloud data of the passable area from the point cloud data of interest to obtain the remaining point cloud data; and determines the obstacle information according to the remaining point cloud data. In this way, the obstacle information is calculated by using the depth image captured by the depth camera, the three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system, which can improve the accuracy of obstacle detection and improve the obstacle detection effect.

[0178] Example 3

[0179] In addition, an embodiment of the present disclosure provides a robot, including a memory and a processor, wherein the memory stores a computer program, and when the computer program runs on the processor, the obstacle detection method provided in the above method embodiment 1 is executed.

[0180] The robot provided in this embodiment may be equipped with a depth camera, and may implement the obstacle detection method provided in Embodiment 1, which will not be described here in detail to avoid repetition.

[0181] Example 4

[0182] The present application also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the obstacle detection method provided in Example 1 is implemented.

[0183] In this embodiment, the computer-readable storage medium may be a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0184] The computer-readable storage medium provided in this embodiment can implement the obstacle detection method provided in Embodiment 1, which will not be described again here to avoid repetition.

[0185] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or terminal including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or terminal. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or terminal including the element.

[0186] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present application, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, a magnetic disk, or an optical disk), and includes a number of instructions for a terminal (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present application.

[0187] The embodiments of the present application are described above in conjunction with the accompanying drawings, but the present application is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present application, ordinary technicians in this field can also make many forms without departing from the purpose of the present application and the scope of protection of the claims, all of which are within the protection of the present application.

Claims

1. An obstacle detection method, characterized in that: The method comprises: Converting the depth image captured by the depth camera into first point cloud data in the depth camera coordinate system according to the internal parameters of the depth camera; Converting the first point cloud data into second point cloud data in the robot coordinate system according to a three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system; Extracting point cloud data of interest from the second point cloud data according to a preset area of ​​interest, wherein the preset area of ​​interest is a preset area directly in front of the robot; Determining whether the number of three-dimensional points of the point cloud data of interest is greater than a first preset three-dimensional point threshold; If the number of three-dimensional points of the point cloud data of interest is greater than the first preset three-dimensional point threshold, obtaining at least one plane from the point cloud data of interest, and determining the passable area point cloud data from the point cloud data of interest according to the plane feature data of each plane; Excluding the traversable area point cloud data from the point cloud data of interest to obtain remaining point cloud data; Determining whether the number of three-dimensional points of the remaining point cloud data is greater than a second preset three-dimensional point threshold; If the number of three-dimensional points of the remaining point cloud data is greater than the second preset three-dimensional point threshold, projecting each three-dimensional point of the remaining point cloud data into a preset grid area to obtain a point cloud projection map; Traversing a plurality of projection points in the point cloud projection image, and determining a preset number of projection points closest to a current traversal point from the plurality of projection points; Determine whether the distance between each projection point of the preset number of projection points and the current traversal point is less than a preset distance threshold; If the distance between each projection point of the preset number of projection points and the current traversal point is less than the preset distance threshold, determining that the current traversal point and the preset number of projection points are obstacle point clouds; If there is a projection point among the preset number of projection points whose distance to the current traversal point is greater than or equal to the preset distance threshold, continue to traverse other projection points until the traversal of other projection points is completed; Obstacle information is determined according to the obstacle point cloud, where the obstacle information includes obstacle height. When the obstacle height is less than 0, the obstacle is an obstacle below the ground.

2. The method according to claim 1, characterized in that The method further comprises: If the number of three-dimensional points of the point cloud data of interest is less than or equal to the first preset three-dimensional point threshold, it is determined that no obstacle is detected in the preset area of ​​interest.

3. The method according to claim 1, characterized in that The step of determining the traversable area point cloud data from the interest point cloud data according to the plane feature data of each plane comprises: Determine whether the plane feature data of each plane is less than a preset obstacle recognition threshold; If there is a passable plane in each plane whose plane feature data is less than the preset obstacle recognition threshold, the point cloud data corresponding to the passable plane is used as the point cloud data of the passable area.

4. The method according to claim 1, characterized in that The method further comprises: If the number of three-dimensional points of the remaining point cloud data is less than or equal to the second preset three-dimensional point threshold, it is determined that no obstacle is detected in the preset area of ​​interest.

5. The method according to claim 1, characterized in that The step of projecting each three-dimensional point of the remaining point cloud data into a preset grid area to obtain a point cloud projection map comprises: Sorting the three-dimensional points of the remaining point cloud data according to the distance between each three-dimensional point of the remaining point cloud data and the robot to obtain sorted point cloud data; The sorted point cloud data is projected onto the preset grid area to obtain the point cloud projection image.

6. The method according to claim 1, characterized in that The step of acquiring the three-dimensional point conversion relationship includes: Extracting an auxiliary plane from the first point cloud data; Determining a normal vector of the auxiliary plane in the depth camera coordinate system; Determine a projection vector of the first direction axis vector projected onto the auxiliary plane according to the distance between the first direction axis vector and the auxiliary plane in the robot coordinate system and the first direction axis vector; Determine a second direction axis vector in the depth camera coordinates according to the projection vector and the normal vector; Determining the intercept of the auxiliary plane in the robot coordinate system; The three-dimensional point conversion relationship is determined according to the projection vector, the normal vector, the second direction axis vector and the intercept.

7. An obstacle detection device, characterized in that: The device comprises: A first conversion module, configured to convert a depth image captured by the depth camera into first point cloud data in a depth camera coordinate system according to an internal parameter of the depth camera; A second conversion module, configured to convert the first point cloud data into second point cloud data in the robot coordinate system according to a three-dimensional point conversion relationship between the depth camera coordinate system and the robot coordinate system; An extraction module, used to extract interested point cloud data from the second point cloud data according to a preset interested area, wherein the preset interested area is a preset area directly in front of the robot; A first processing module, used for determining whether the number of three-dimensional points of the point cloud data of interest is greater than a first preset three-dimensional point threshold; an acquisition module, configured to acquire at least one plane from the point cloud data of interest if the number of three-dimensional points of the point cloud data of interest is greater than the first preset three-dimensional point threshold, and determine the traversable area point cloud data from the point cloud data of interest according to the plane feature data of each plane; An exclusion module, used to exclude the point cloud data of the passable area from the point cloud data of interest to obtain the remaining point cloud data; A second processing module, used for determining whether the number of three-dimensional points of the remaining point cloud data is greater than a second preset three-dimensional point threshold; a determination module, configured to project each 3D point of the remaining point cloud data into a preset grid area to obtain a point cloud projection diagram if the number of 3D points of the remaining point cloud data is greater than the second preset 3D point threshold; Traversing a plurality of projection points in the point cloud projection image, and determining a preset number of projection points closest to a current traversal point from the plurality of projection points; Determine whether the distance between each projection point of the preset number of projection points and the current traversal point is less than a preset distance threshold; If the distance between each projection point of the preset number of projection points and the current traversal point is less than the preset distance threshold, determining that the current traversal point and the preset number of projection points are obstacle point clouds; If there is a projection point among the preset number of projection points whose distance to the current traversal point is greater than or equal to the preset distance threshold, continue to traverse other projection points until the traversal of other projection points is completed; Obstacle information is determined according to the obstacle point cloud, where the obstacle information includes obstacle height. When the obstacle height is less than 0, the obstacle is an obstacle below the ground.

8. A robot, characterized in that: The invention comprises a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the obstacle detection method according to any one of claims 1 to 6 is executed.

9. A computer-readable storage medium, characterized in that: The computer program stores a computer program, which executes the obstacle detection method according to any one of claims 1 to 6 when running on a processor.

Images