Obstacle detection system and method based on robot depth camera

Abstract

The application discloses a robot depth camera-based obstacle detection system and method, and belongs to the technical field of robots, and aims to solve the technical problem of how to realize detection and obstacle avoidance of each visual angle of a robot through a depth camera without the cooperation of other sensors. The system comprises a depth camera, a tracking control module and a detection conversion and processing module. The depth camera comprises two first and second depth cameras. The first depth camera is arranged above the robot in a fixed position with a forward direction of the robot and a predetermined downward pitch angle, and the visual field of the first depth camera is not blocked. The second camera is arranged above the first camera through a rotating mechanism, and the visual field of the second camera is not blocked. The second camera has the same (x, y) position coordinate position and the same (rx, ry) angle position as the first camera. The first depth camera performs obstacle identification, and the second depth camera cooperates with the line to realize real-time tracking and position prediction, so that all angle obstacles can be accurately and timely updated.

Description

Technical Field

[0001] This invention relates to the field of robotics, specifically to an obstacle detection system and method based on a robot depth camera. Background Technology

[0002] Traditional robots use depth cameras for obstacle detection and avoidance, typically employing a single camera or multiple cameras working in conjunction with other sensors such as LiDAR and ultrasonic sensors to detect and avoid obstacles from various perspectives. When an obstacle is no longer visible to the robot due to the robot having already passed it or other reasons, it may retain it (considering it a fixed obstacle) or remove it (considering it a temporary obstacle) depending on functional requirements and the scene. Alternatively, it may make some probability-assisted estimations based on its own movement direction or use other sensors to supplement the blind spots. However, without the cooperation of other sensors, the retention or removal is essentially based on subjective judgment, which leads to inaccurate judgments about the addition or disappearance of obstacles outside the field of view, posing a safety hazard.

[0003] How to achieve robot detection and obstacle avoidance from various perspectives using a depth camera without the assistance of other sensors is a technical problem that needs to be solved. Summary of the Invention

[0004] The technical objective of this invention is to address the above-mentioned shortcomings by providing an obstacle detection system and method based on a robot depth camera, thereby solving the technical problem of how to achieve obstacle detection and avoidance from various perspectives of a robot using a depth camera without the assistance of other sensors.

[0005] In a first aspect, the present invention provides an obstacle detection system based on a robot depth camera, comprising a depth camera, a tracking control module, and a detection conversion and processing module. There are two depth cameras, namely a first depth camera and a second depth camera. The first depth camera is fixedly positioned above the robot with a predetermined downward pitch angle and an unobstructed field of view. The second depth camera is positioned above the first camera through a rotation mechanism and has an unobstructed field of view. The second camera and the first camera have the same (x, y) position coordinates and the same (rx, ry) angle pose.

[0006] Both the first and second depth cameras interact with the detection, conversion, and processing module to send the collected point cloud data to the detection, conversion, and processing module.

[0007] The detection, conversion, and processing module is used to perform the following:

[0008] For the point cloud data collected by the first depth camera, obstacle recognition and downward height difference recognition are performed based on the point cloud data.

[0009] For detected obstacles, feature identification and numbering are performed based on the range of the obstacle's movement angle, and the obstacles are classified into moving obstacles and currently stationary obstacles based on whether the obstacle's position changes continuously.

[0010] The obstacle number, obstacle category, and obstacle location are recorded as obstacle information in the obstacle list, and the obstacle locations are updated on the map in real time;

[0011] Based on the distance and angular displacement of obstacles between adjacent frames, the position of obstacles in the next frame is predicted by the same angular coordinate displacement.

[0012] When the first depth camera cannot detect an obstacle within its field of view, the obstacle will be removed from the obstacle list and map in real time.

[0013] When an obstacle disappears from the edge of the field of view of the first depth camera or when the predicted position of the obstacle in the next frame is outside the field of view of the first depth camera, the tracking control module is used to control the second depth camera to perform rotation scanning at a predetermined rotation angular velocity, and during rotation scanning, moving obstacles are given higher priority than currently stationary obstacles.

[0014] Correspondingly, the detection, conversion, and processing module is used to perform the following:

[0015] For the point cloud data acquired by the second depth camera, obstacle recognition and downward height difference recognition are performed based on the point cloud data;

[0016] For detected obstacles, feature identification and numbering are performed based on the range of the obstacle's movement angle, and the obstacles are classified into moving obstacles and currently stationary obstacles based on whether the obstacle's position changes continuously.

[0017] The obstacle number, obstacle category, and obstacle location are used as obstacle information. It is determined whether the obstacle exists in the obstacle list. If it exists, the obstacle location is updated on the map in real time. If it does not exist, the obstacle information is added to the obstacle list and the obstacle location is updated on the map in real time.

[0018] Based on the distance and angular displacement of obstacles between adjacent frames, the position of obstacles in the next frame is predicted by the same angular coordinate displacement.

[0019] Preferably, the detection, conversion, and processing module is used to perform the following obstacle recognition and downward height difference recognition based on point cloud data:

[0020] The point cloud data is traversed. When a point with an absolute z value less than a predetermined distance L1 is detected, and there are only points with an absolute z value less than the predetermined distance L1 in its circular neighborhood with a radius of a predetermined number M1 points, a plane is fitted with the detected point. The height difference between other point clouds and the plane is determined, and the height difference between the point cloud and the plane is used as the z coordinate of the point cloud. When a point with a height difference from the plane z axis greater than the predetermined distance L1 is found, its circular neighborhood with a radius of a predetermined number M2 points is determined, and the absolute z value of this point is recorded as z0. When the number of points with an absolute z value greater than the predetermined distance L2 is more than the predetermined number M3, and there are more than the predetermined number M4 points with an absolute z value greater than the predetermined distance L2 and less than z0, then all points with z greater than the predetermined distance L2 in this neighborhood are marked as non-ground points, and points with z less than the predetermined distance L2 are marked as ground points.

[0021] Iterate through the point cloud until all ground points and non-ground points are separated. When the number of consecutive non-ground points is greater than a predetermined number M3, it is considered that an obstacle or a downward height difference has been detected. When z is greater than 0, it is considered an obstacle; when z is less than 0, it is considered a downward drop.

[0022] Preferably, for detected obstacles, the detection conversion and processing module performs the following operations:

[0023] An angular coordinate system is established with the first depth camera as the origin and the line directly in front as 0°. The angle to the right of the camera is negative and the angle to the left is positive. Non-ground points are classified and numbered with a predetermined radian N1 as the minimum resolution. The detected non-ground points are numbered according to the different angle intervals they are in. At the same time, the distance between the projection of the detected obstacle or downward drop point on the (x,y) coordinate plane and the origin is calculated.

[0024] Calculate the obstacle point angle based on the number and angular resolution, and record the calculated obstacle angle and distance;

[0025] For each detected obstacle, feature recognition is performed. The angle difference between the rightmost and leftmost points of the obstacle is detected, and then the z-coordinate of its highest point is detected. If there are multiple points with the same height, the z-coordinate of the median of the left and right angles is taken. If there is an even number of points, the z-coordinate of the median of the left side is taken. Then, the center points of the left and right angles of the obstacle are detected, and K points are selected on the left and right sides at intervals of K points. The z-coordinates of 2K+1 points are extracted. Each obstacle is numbered according to different features and its features are recorded separately.

[0026] The system continuously traverses and detects three frames of depth camera images, comparing the highest z-coordinate of obstacles in two adjacent frames. If the obstacle numbered in the first frame does not change its angular position in the second and third frames, i.e., its angular coordinate remains unchanged, then the obstacle is recorded as a stationary obstacle at the current moment. If there is a change, it is recorded as a moving obstacle.

[0027] The position of obstacles on the robot's map is updated in real time based on angular coordinates;

[0028] The system continuously traverses and detects five frames of point cloud images captured by the depth camera. It compares the highest point of the obstacle's z-coordinate in two adjacent frames. If the angle position of an obstacle moves by approximately the same angle for five consecutive frames, and the error is within a predetermined range, and the distance error is within a predetermined range, then the current angle and distance are added to the fifth frame to predict the angle, distance, and position of the next frame.

[0029] Preferably, for a currently stationary obstacle, the tracking control module and the detection, conversion, and processing module work together to perform the following operations:

[0030] Based on the robot's odometry distance and the obstacle's angular distance, the robot updates the obstacle's estimated position relative to the robot in real time. When there are no obstacles within a predetermined angle range to the left and right of the estimated position more than three times in a row, the obstacle is removed from the obstacle list, and obstacles within the corresponding angle range are also removed from the map.

[0031] When the second depth camera detects a new obstacle more than three times, the obstacle within the corresponding angle range is added to the obstacle list and map.

[0032] Preferably, for moving obstacles, the tracking control module and the detection, conversion, and processing module work together to perform the following operations:

[0033] The obstacle's position is predicted based on its inter-frame motion trajectory within the field of view of the first depth camera. The obstacle distance is updated in real time based on the real-time scan results of the second depth camera in each cycle, including updating the obstacle list and the obstacle distance in the map. At the same time, the obstacle's position is predicted based on the obstacle's angle and distance displacement during each scan cycle until the obstacle disappears. If there are no obstacles within a predetermined angle range to the left and right of the predicted position for more than three times, the obstacle is removed from the obstacle list, and obstacles within the corresponding angle range are removed from the map. The second depth camera performs two cycles of scanning.

[0034] When the second depth camera detects a new obstacle more than three times, it adds the obstacle within the corresponding angle range to the obstacle list and map, thereby completing obstacle detection and clearance.

[0035] In a second aspect, the present invention provides an obstacle detection method based on a robot depth camera, which performs obstacle detection using an obstacle detection system based on a robot depth camera as described in any one of the first aspects, the method comprising the following steps:

[0036] For the point cloud data collected by the first depth camera, obstacle recognition and downward height difference recognition are performed based on the point cloud data.

[0037] For detected obstacles, feature identification and numbering are performed based on the range of the obstacle's movement angle, and the obstacles are classified into moving obstacles and currently stationary obstacles based on whether the obstacle's position changes continuously.

[0038] The obstacle number, obstacle category, and obstacle location are recorded as obstacle information in the obstacle list, and the obstacle locations are updated on the map in real time;

[0039] Based on the distance and angular displacement of obstacles between adjacent frames, the position of obstacles in the next frame is predicted by the same angular coordinate displacement.

[0040] When the first depth camera cannot detect an obstacle within its field of view, the obstacle will be removed from the obstacle list and map in real time.

[0041] When an obstacle disappears from the edge of the field of view of the first depth camera or when the predicted position of the obstacle in the next frame is outside the field of view of the first depth camera, the second depth camera is controlled to perform a rotational scan at a predetermined rotational angular velocity. During the rotational scan, moving obstacles are given higher priority than currently stationary obstacles.

[0042] For the point cloud data acquired by the second depth camera, obstacle recognition and downward height difference recognition are performed based on the point cloud data;

[0043] For detected obstacles, feature identification and numbering are performed based on the range of the obstacle's movement angle, and the obstacles are classified into moving obstacles and currently stationary obstacles based on whether the obstacle's position changes continuously.

[0044] The obstacle number, obstacle category, and obstacle location are used as obstacle information. It is determined whether the obstacle exists in the obstacle list. If it exists, the obstacle location is updated on the map in real time. If it does not exist, the obstacle information is added to the obstacle list and the obstacle location is updated on the map in real time.

[0045] Based on the distance and angular displacement of obstacles between adjacent frames, the position of obstacles in the next frame is predicted by the same angular coordinate displacement.

[0046] As a preferred method, obstacle recognition and downward height difference recognition based on point cloud data include the following steps:

[0047] The point cloud data is traversed. When a point with an absolute z value less than a predetermined distance L1 is detected, and there are only points with an absolute z value less than the predetermined distance L1 in its circular neighborhood with a radius of a predetermined number M1 points, a plane is fitted with the detected point. The height difference between other point clouds and the plane is determined, and the height difference between the point cloud and the plane is used as the z coordinate of the point cloud. When a point with a height difference from the plane z axis greater than the predetermined distance L1 is found, its circular neighborhood with a radius of a predetermined number M2 points is determined, and the absolute z value of this point is recorded as z0. When the number of points with an absolute z value greater than the predetermined distance L2 is more than the predetermined number M3, and there are more than the predetermined number M4 points with an absolute z value greater than the predetermined distance L2 and less than z0, then all points with z greater than the predetermined distance L2 in this neighborhood are marked as non-ground points, and points with z less than the predetermined distance L2 are marked as ground points.

[0048] Iterate through the point cloud until all ground points and non-ground points are separated. When the number of consecutive non-ground points is greater than a predetermined number M3, it is considered that an obstacle or a downward height difference has been detected. When z is greater than 0, it is considered an obstacle; when z is less than 0, it is considered a downward drop.

[0049] Preferably, the following operation is performed on detected obstacles:

[0050] An angular coordinate system is established with the first depth camera as the origin and the line directly in front as 0°. The angle to the right of the camera is negative and the angle to the left is positive. Non-ground points are classified and numbered with a predetermined radian N1 as the minimum resolution. The detected non-ground points are numbered according to the different angle intervals they are in. At the same time, the distance between the projection of the detected obstacle or downward drop point on the (x,y) coordinate plane and the origin is calculated.

[0051] Calculate the obstacle point angle based on the number and angular resolution, and record the calculated obstacle angle and distance;

[0052] For each detected obstacle, feature recognition is performed. The angle difference between the rightmost and leftmost points of the obstacle is detected, and then the z-coordinate of its highest point is detected. If there are multiple points with the same height, the z-coordinate of the median of the left and right angles is taken. If there is an even number of points, the z-coordinate of the median of the left side is taken. Then, the center points of the left and right angles of the obstacle are detected, and K points are selected on the left and right sides at intervals of K points. The z-coordinates of 2K+1 points are extracted. Each obstacle is numbered according to different features and its features are recorded separately.

[0053] The system continuously traverses and detects three frames of depth camera images, comparing the highest z-coordinate of obstacles in two adjacent frames. If the obstacle numbered in the first frame does not change its angular position in the second and third frames, i.e., its angular coordinate remains unchanged, then the obstacle is recorded as a stationary obstacle at the current moment. If there is a change, it is recorded as a moving obstacle.

[0054] The position of obstacles on the robot's map is updated in real time based on angular coordinates;

[0055] The system continuously traverses and detects five frames of point cloud images captured by the depth camera. It compares the highest point of the obstacle's z-coordinate in two adjacent frames. If the angle position of an obstacle moves by approximately the same angle for five consecutive frames, and the error is within a predetermined range, and the distance error is within a predetermined range, then the current angle and distance are added to the fifth frame to predict the angle, distance, and position of the next frame.

[0056] As a preferred option, for a currently stationary obstacle, the following operation should be performed:

[0057] Based on the robot's odometry distance and the obstacle's angular distance, the robot updates the obstacle's estimated position relative to the robot in real time. When there are no obstacles within a predetermined angle range to the left and right of the estimated position more than three times in a row, the obstacle is removed from the obstacle list, and obstacles within the corresponding angle range are also removed from the map.

[0058] When the second depth camera detects a new obstacle more than three times, the obstacle within the corresponding angle range is added to the obstacle list and map.

[0059] As a preferred option, for moving obstacles, the following operation should be performed:

[0060] The obstacle's position is predicted based on its inter-frame motion trajectory within the field of view of the first depth camera. The obstacle distance is updated in real time based on the real-time scan results of the second depth camera in each cycle, including updating the obstacle list and the obstacle distance in the map. At the same time, the obstacle's position is predicted based on the obstacle's angle and distance displacement during each scan cycle until the obstacle disappears. If there are no obstacles within a predetermined angle range to the left and right of the predicted position for more than three times, the obstacle is removed from the obstacle list, and obstacles within the corresponding angle range are removed from the map. The second depth camera performs two cycles of scanning.

[0061] When the second depth camera detects a new obstacle more than three times, it adds the obstacle within the corresponding angle range to the obstacle list and map, thereby completing obstacle detection and clearance.

[0062] The obstacle detection system and method based on a robot depth camera of the present invention has the following advantages: the first depth camera performs obstacle identification, and the second depth camera cooperates with real-time tracking and position prediction to ensure that obstacles from all angles can be accurately and in real time updated, thereby ensuring the robot's accurate and efficient obstacle detection and removal, and avoiding the safety hazards caused by the accidental removal or retention of obstacles outside the field of view. Attached Figure Description

[0063] To more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0064] The invention will be further described below with reference to the accompanying drawings.

[0065] Figure 1 This is a scene diagram of an obstacle detection system based on a robot depth camera, as shown in Example 1.

[0066] Figure 2 This is a schematic diagram of the structure of an obstacle detection system based on a robot depth camera, as shown in Example 1.

[0067] Figure 3 This is a top-view schematic diagram of the field of view and scanning area of ​​two depth cameras in an obstacle detection system based on a robot depth camera, as described in Example 1.

[0068] In the diagram, 1 is the robot, 2 is the first depth camera, 3 is the second depth camera, and 4 is the rotation mechanism. Detailed Implementation

[0069] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments are not intended to limit the present invention. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0070] This invention provides an obstacle detection system and method based on a robot depth camera, which solves the technical problem of how to achieve obstacle detection and avoidance from various perspectives of a robot using a depth camera without the cooperation of other sensors.

[0071] Example 1:

[0072] This invention discloses an obstacle detection system based on a robot depth camera, comprising a depth camera, a tracking control module, and a detection conversion and processing module. There are two depth cameras, namely a first depth camera 2 and a second depth camera. The first depth camera 2 is fixedly positioned above the robot 1 with a predetermined downward pitch angle and an unobstructed field of view, facing the robot 1 in the forward direction. The second camera is positioned above the first camera through a rotation mechanism 4 and has an unobstructed field of view. The second camera and the first camera have the same (x, y) position coordinates and the same (rx, ry) angle pose.

[0073] Both the first depth camera 2 and the second depth camera 3 interact with the detection, conversion and processing module to send the collected point cloud data to the detection, conversion and processing module.

[0074] The detection, conversion, and processing module is used to perform the following:

[0075] (1) For the point cloud data collected by the first depth camera 2, obstacle recognition and downward height difference recognition are performed based on the point cloud data;

[0076] (2) For detected obstacles, feature identification and numbering of obstacles are performed based on the range of their movement angles, and obstacles are classified into moving obstacles and currently stationary obstacles based on whether their positions change continuously.

[0077] (3) Record the obstacle number, obstacle category and obstacle location as obstacle information in the obstacle list, and update the obstacle location on the map in real time;

[0078] (4) Based on the distance and angular displacement of obstacles between adjacent frames, predict the position of obstacles in the next frame by using the same angular coordinate displacement.

[0079] When the first depth camera 2 cannot detect an obstacle within its field of view, the obstacle is removed from the obstacle list and map in real time.

[0080] When an obstacle disappears from the edge of the field of view of the first depth camera 2 or when the predicted position of the obstacle in the next frame is outside the field of view of the first depth camera 2, the tracking control module is used to control the second depth camera 3 to perform rotation scanning at a predetermined rotation angular velocity, and during rotation scanning, moving obstacles are given higher priority than currently stationary obstacles.

[0081] Correspondingly, the detection, conversion, and processing module is used to perform the following:

[0082] (1) For the point cloud data collected by the second depth camera 3, obstacle recognition and downward height difference recognition are performed based on the point cloud data;

[0083] (2) For detected obstacles, feature identification and numbering of obstacles are performed based on the range of their movement angles, and obstacles are classified into moving obstacles and currently stationary obstacles based on whether their positions change continuously.

[0084] (3) Use obstacle number, obstacle category and obstacle location as obstacle information, determine whether the obstacle exists in the obstacle list, if it exists, update the obstacle location on the map in real time, if it does not exist, add the obstacle information to the obstacle list, and update the obstacle location on the map in real time.

[0085] (4) Based on the distance and angular displacement of obstacles between adjacent frames, predict the position of obstacles in the next frame by using the same angular coordinate displacement.

[0086] In this embodiment, the detection, conversion, and processing module is used to perform the following obstacle recognition and downward height difference recognition based on point cloud data: When a point with an absolute z value < 0.05m is detected, and there are only points with an absolute z value < 0.1m in its circular neighborhood with a radius of 10 points, a plane is fitted with these detected points, the height difference between other point clouds and the plane is determined, and the height difference between the point cloud and the plane is used as the z coordinate of the point cloud. When a point with a height difference greater than 0.1m from the z axis of the plane appears, its circular neighborhood with a radius of 8 points is determined, and the absolute z value of this point is recorded as z0. When the number of points with an absolute z value greater than 0.03m is more than 20, and there are more than 6 points with an absolute z value greater than 0.03 and less than z0, then all points with z greater than 0.03 in this neighborhood are marked as non-ground points, and points with z less than 0.03 are ground points. The point cloud is iterated repeatedly until all ground and non-ground points are separated. When the number of consecutive non-ground points is greater than 20, an obstacle or downward height difference is considered detected, where z>0 is considered an obstacle and z<0 is considered a downward drop. Through the above operation, obstacles or downward height differences (such as steps) can be detected, while ground noise is filtered out.

[0087] For detected obstacles, the detection conversion and processing module performs the following operations:

[0088] (1) Establish an angular coordinate system with the first depth camera 2 as the origin and the line directly in front as 0°. The angle on the right side of the camera is negative and the angle on the left side is positive. The non-ground points are classified and numbered with an angle of 0.01 radians as the minimum resolution. The detected non-ground points are numbered 0, 1, 2... according to the different angle intervals they are in. At the same time, the distance between the projection of the detected obstacle or the point of downward drop on the (x,y) coordinate plane and the origin is calculated.

[0089] (2) Calculate the obstacle point angle based on the number and angular resolution, and record the calculated obstacle angle and distance;

[0090] (3) Feature recognition is performed on each detected obstacle. The specific method is as follows: detect the angle difference between the rightmost point and the leftmost point of the obstacle, and then detect the z coordinate of the highest point of its z coordinate (if there are multiple points with the same height, take the median of the left and right angles; if there is an even number of points, take the median of the left side. For example, if there are 4 highest points with values ​​of 20°, 16°, 11° and 6°, then take the 16° point). Then detect the center point of the left and right angles of the obstacle and select 3 points on each side at intervals of 3 points to extract the z coordinates of these 7 points. Number each obstacle according to different features and record its features respectively.

[0091] (4) Continuously traverse and detect three frames of depth camera images and compare the highest point of the z coordinate of the obstacle in two adjacent frames (the method of determination is the same as above). If the obstacle numbered in the first frame does not change its angle position in the second and third frames (i.e. its angular coordinate remains unchanged), then the obstacle is recorded as a stationary obstacle at the current time. If there is a change, it is recorded as a moving obstacle.

[0092] (5) Update the position of obstacles on the robot map in real time based on angular coordinates;

[0093] (6) Continuously traverse and detect five frames of depth camera images, compare the highest point of the obstacle's z coordinate in two adjacent frames (the method of determination is the same as above). If the angle position of a certain obstacle moves by approximately the same angle (within ±2°) and distance (within ±3cm) for five consecutive frames, then add this angle and distance to the fifth frame as the prediction of the angle and distance position of the next frame.

[0094] In this embodiment, when an obstacle disappears from the edge of the field of view of the first depth camera 2 (the obstacle cannot be detected), or when the position of the obstacle in the next frame is predicted to be outside the field of view, the second depth camera 3 begins to rotate back and forth at a rotational angular velocity of 45° / s. Taking the camera laser angle coordinate system as positive (0° forward, positive left, negative right) as an example, the camera rotates left and right by -180° to the right field of view limit of the first depth camera 2 and the left field of view of the first depth camera 2 to the range of 180° (the second depth camera 3 quickly rotates through the field of view of the first depth camera 2).

[0095] In this embodiment, for stationary obstacles, the tracking control module and the detection, conversion, and processing module work together to perform the following operations: First, the robot's odometry distance is combined with the obstacle's angular distance to update the obstacle's estimated position relative to the robot in real time. When no obstacles are found within a 10° range to the left and right of the estimated position more than three times consecutively, the obstacle is removed from the obstacle list, and obstacles within the corresponding angular range are also removed from the map. When the second depth camera 3 detects a new obstacle more than three times, obstacles within the corresponding angular range are added to both the obstacle list and the map.

[0096] For moving obstacles, the tracking control module and the detection, conversion, and processing module work together to perform the following operations: Predict the obstacle's position based on its inter-frame motion trajectory when it is within the field of view of the first depth camera 2; update the obstacle distance (including the obstacle list and map) in real time based on the real-time scan results of the second depth camera 3 for each cycle; and predict the obstacle's position based on its angle and distance displacement during each scan cycle (two cycles in one scan). Continue this process until the obstacle disappears. If there are no obstacles within a 10° radius to the left or right of the predicted position more than three times, the obstacle is removed from the obstacle list, and obstacles within the corresponding angle range are removed from the map. If the second depth camera 3 detects a new obstacle more than three times, it adds obstacles within the corresponding angle range to the obstacle list and map, thus completing obstacle detection and removal.

[0097] Example 2:

[0098] This invention discloses an obstacle detection method based on a robot depth camera. The method, using the system disclosed in Example 1, includes the following steps:

[0099] (1) For the point cloud data collected by the first depth camera 2, obstacle recognition and downward height difference recognition are performed based on the point cloud data;

[0100] (2) For detected obstacles, feature identification and numbering of obstacles are performed based on the range of their movement angles, and obstacles are classified into moving obstacles and currently stationary obstacles based on whether their positions change continuously.

[0101] (3) Record the obstacle number, obstacle category and obstacle location as obstacle information in the obstacle list, and update the obstacle location on the map in real time;

[0102] (4) Based on the distance and angular displacement of obstacles between adjacent frames, predict the position of obstacles in the next frame by using the same angular coordinate displacement;

[0103] (5) When the first depth camera 2 cannot detect an obstacle within its field of view, the obstacle will be removed from the obstacle list and map in real time;

[0104] (6) When the obstacle disappears from the edge of the field of view of the first depth camera 2 or the predicted position of the obstacle in the next frame is outside the field of view of the first depth camera 2, the second depth camera 3 is adjusted to perform a rotational scan at a predetermined rotational angular velocity, and during the rotational scan, the moving obstacle has a higher priority than the currently stationary obstacle.

[0105] (7) For the point cloud data collected by the second depth camera 3, obstacle recognition and downward height difference recognition are performed based on the point cloud data;

[0106] (8) For detected obstacles, feature identification and numbering of obstacles are performed based on the range of their movement angles, and obstacles are classified into moving obstacles and currently stationary obstacles based on whether their positions change continuously.

[0107] (9) Use obstacle number, obstacle category and obstacle location as obstacle information, determine whether the obstacle exists in the obstacle list, if it exists, update the obstacle location on the map in real time, if it does not exist, add the obstacle information to the obstacle list, and update the obstacle location on the map in real time.

[0108] (10) Based on the distance and angular displacement of obstacles between adjacent frames, predict the position of obstacles in the next frame by using the same angular coordinate displacement.

[0109] In this embodiment, obstacle recognition and downward height difference recognition are performed based on point cloud data: When a point with an absolute z value < 0.05m is detected, and there are only points with an absolute z value < 0.1m in its circular neighborhood with a radius of 10 points, a plane is fitted with these detected points, and the height difference between other point clouds and the plane is determined. The height difference between the point cloud and the plane is used as the z coordinate of the point cloud. When a point with a height difference greater than 0.1m from the z axis of the plane appears, its circular neighborhood with a radius of 8 points is determined, and the absolute z value of this point is recorded as z0. When the number of points with an absolute z value greater than 0.03m is more than 20, and there are more than 6 points with an absolute z value greater than 0.03 and less than z0, then all points with z greater than 0.03 in this neighborhood are marked as non-ground points, and points with z less than 0.03 are ground points. The point cloud is iterated repeatedly until all ground and non-ground points are separated. When the number of consecutive non-ground points is greater than 20, an obstacle or downward height difference is considered detected, where z>0 is considered an obstacle and z<0 is considered a downward drop. Through the above operation, obstacles or downward height differences (such as steps) can be detected, while ground noise is filtered out.

[0110] For detected obstacles, perform the following operations for obstacle tracking:

[0111] (1) Establish an angular coordinate system with the first depth camera 2 as the origin and the line directly in front as 0°. The angle on the right side of the camera is negative and the angle on the left side is positive. The non-ground points are classified and numbered with an angle of 0.01 radians as the minimum resolution. The detected non-ground points are numbered 0, 1, 2... according to the different angle intervals they are in. At the same time, the distance between the projection of the detected obstacle or the point of downward drop on the (x,y) coordinate plane and the origin is calculated.

[0112] (2) Calculate the obstacle point angle based on the number and angular resolution, and record the calculated obstacle angle and distance;

[0113] (3) Feature recognition is performed on each detected obstacle. The specific method is as follows: detect the angle difference between the rightmost point and the leftmost point of the obstacle, and then detect the z coordinate of the highest point of its z coordinate (if there are multiple points with the same height, take the median of the left and right angles; if there is an even number of points, take the median of the left side. For example, if there are 4 highest points with values ​​of 20°, 16°, 11° and 6°, then take the 16° point). Then detect the center point of the left and right angles of the obstacle and select 3 points on each side at intervals of 3 points to extract the z coordinates of these 7 points. Number each obstacle according to different features and record its features respectively.

[0114] (4) Continuously traverse and detect three frames of depth camera images and compare the highest point of the z coordinate of the obstacle in two adjacent frames (the method of determination is the same as above). If the obstacle numbered in the first frame does not change its angle position in the second and third frames (i.e. its angular coordinate remains unchanged), then the obstacle is recorded as a stationary obstacle at the current time. If there is a change, it is recorded as a moving obstacle.

[0115] (5) Update the position of obstacles on the robot map in real time based on angular coordinates;

[0116] (6) Continuously traverse and detect five frames of depth camera images, compare the highest point of the obstacle's z coordinate in two adjacent frames (the method of determination is the same as above). If the angle position of a certain obstacle moves by approximately the same angle (within ±2°) and distance (within ±3cm) for five consecutive frames, then add this angle and distance to the fifth frame as the prediction of the angle and distance position of the next frame.

[0117] In this embodiment, when an obstacle disappears from the edge of the field of view of the first depth camera 2 (the obstacle cannot be detected), or when the position of the obstacle in the next frame is predicted to be outside the field of view, the second depth camera 3 begins to rotate back and forth at a rotational angular velocity of 45° / s. Taking the camera laser angle coordinate system as positive (0° forward, positive left, negative right) as an example, the camera rotates left and right by -180° to the right field of view limit of the first depth camera 2 and the left field of view of the first depth camera 2 to the range of 180° (the second depth camera 3 quickly rotates through the field of view of the first depth camera 2).

[0118] In this embodiment, for stationary obstacles, the following operations are performed for tracking: First, the robot's odometry distance is combined with the obstacle's angular distance to update the obstacle's estimated position relative to the robot in real time. When no obstacles are found within a 10° range to the left and right of the estimated position more than three times consecutively, the obstacle is removed from the obstacle list, and obstacles within the corresponding angular range are also removed from the map. When the second depth camera 3 detects a new obstacle more than three times, obstacles within the corresponding angular range are added to both the obstacle list and the map.

[0119] For moving obstacles, the following operations are performed for tracking: The obstacle's position is predicted based on its inter-frame motion trajectory when it is within the field of view of the first depth camera 2. The obstacle distance (including the obstacle list and map) is updated in real-time based on the real-time scan results of the second depth camera 3 for each cycle. Simultaneously, the obstacle's position is predicted based on its angle and distance displacement during each scan cycle (two cycles in one scan). This process continues until the obstacle disappears. If no obstacles are found within 10° to the left or right of the predicted position more than three times, the obstacle is removed from the obstacle list, and obstacles within the corresponding angle range are removed from the map. If the second depth camera 3 detects a new obstacle more than three times, obstacles within the corresponding angle range are added to both the obstacle list and the map, thus completing obstacle detection and removal.

[0120] The present invention has been shown and described in detail above with reference to the accompanying drawings and preferred embodiments. However, the present invention is not limited to these disclosed embodiments. Based on the above multiple embodiments, those skilled in the art will know that more embodiments of the present invention can be obtained by combining the code review methods in the different embodiments above. These embodiments are also within the protection scope of the present invention.

Claims

1. A robot depth camera based obstacle detection system, characterized in that, The system comprises a depth camera, a tracking control module and a detection conversion and processing module. The depth camera comprises a first depth camera and a second depth camera. The first depth camera is arranged above the robot in a fixed position with a predetermined downward pitch angle and has an unobstructed field of view. The second depth camera is arranged above the first depth camera by a rotating mechanism and has an unobstructed field of view. The second depth camera has the same (x, y) position coordinates and the same (rx, ry) angle position as the first depth camera. The first depth camera and the second depth camera are both connected to the detection conversion and processing module to send the collected point cloud data to the detection conversion and processing module. The detection conversion and processing module is used to perform the following operations: For the point cloud data collected by the first depth camera, obstacle identification and downward height drop identification are performed based on the point cloud data. For the detected obstacles, feature identification and numbering are performed according to the activity angle range of the obstacles, and the obstacles are classified as moving obstacles and current static obstacles according to whether the obstacle position changes continuously. The obstacle number, obstacle classification and obstacle position are recorded as obstacle information in an obstacle list, and the obstacle position is updated in real time on a map. The next frame obstacle position is predicted by the same angular coordinate displacement according to the distance and angular displacement of the obstacles between adjacent frames. When the first depth camera cannot detect obstacles in the field of view, the obstacles are removed from the obstacle list and the map in real time. When the obstacle disappears from the edge of the field of view of the first depth camera or the predicted next frame obstacle position is located outside the field of view of the first depth camera, the tracking control module is used to control the second depth camera to rotate and scan at a predetermined angular velocity, and the priority of the moving obstacle is higher than that of the current static obstacle during the rotation and scanning. Correspondingly, the detection conversion and processing module is used to perform the following operations: For the point cloud data collected by the second depth camera, obstacle identification and downward height drop identification are performed based on the point cloud data. For the detected obstacles, feature identification and numbering are performed according to the activity angle range of the obstacles, and the obstacles are classified as moving obstacles and current static obstacles according to whether the obstacle position changes continuously. The obstacle number, obstacle classification and obstacle position are recorded as obstacle information, and the obstacle position is updated in real time on a map. The next frame obstacle position is predicted by the same angular coordinate displacement according to the distance and angular displacement of the obstacles between adjacent frames.

2. The robotically-enabled depth camera-based obstacle detection system of claim 1, wherein, The detection conversion and processing module is used to perform the following operations to realize obstacle identification and downward height drop identification based on point cloud data: The point cloud data is traversed, when a point with an absolute value of z less than a predetermined distance L1 is detected and only points with an absolute value of z less than the predetermined distance L1 are present in a circular neighborhood with a radius of a predetermined number M1 of points, a plane is fitted using the detected points, the height difference of other points from the plane is determined, the height difference of the point cloud from the plane is taken as the z coordinate of the point cloud, when a point with a height difference from the plane greater than the predetermined distance L1 is present, a circular neighborhood with a radius of a predetermined number M2 of points is determined, and the absolute value of z is recorded as z0, when the number of points with an absolute value of z greater than the predetermined distance L2 is greater than a predetermined number M3, and there are more than a predetermined number M4 points with an absolute value of z greater than the predetermined distance L2 and less than z0, all points with z greater than the predetermined distance L2 in the neighborhood are marked as non-ground points, and points with z less than the predetermined distance L2 are marked as ground points; The point cloud is traversed until all ground points and non-ground points are segmented, when a continuous piece of non-ground points is greater than a predetermined number M3, an obstacle or a downward height difference is detected, wherein z greater than 0 is considered as an obstacle, and z less than 0 is considered as a downward height difference.

3. The robotically-enabled depth camera-based obstacle detection system of claim 2, wherein, For the detected obstacle, the detection, conversion and processing module is configured to perform the following operations: An angle coordinate system is established with the first depth camera as the origin and the front as the 0° line, wherein the angle on the right side of the camera is negative, and the angle on the left side of the camera is positive, the non-ground points are classified and numbered by angle with a predetermined arc N1 as the minimum resolution, the detected non-ground points are numbered according to different angle intervals, and the projection of the detected obstacle or downward height difference point on the (x, y) coordinate plane and the distance from the origin are calculated; The angle of the obstacle point is calculated according to the number and angle resolution, and the calculated obstacle angle and distance are recorded; For each detected obstacle, feature recognition is performed, the angle difference of the obstacle from the rightmost point to the leftmost point is detected, then the z coordinate of the highest point of the obstacle is detected, if there are multiple points with the same height, the z coordinate of the median point between the left and right angles is taken, if the number of points is even, the z coordinate of the left median point is taken, then the left and right angle center points of the obstacle are detected, and K points are selected on the left and right sides at intervals of K points, the z coordinates of 2K+1 points are extracted, each obstacle is numbered according to different features, and the features are recorded respectively; Three frames of depth camera images are continuously detected and compared, and the z coordinate of the highest point of the obstacle in the adjacent two frames is compared, if the angle position of the obstacle numbered in the first frame does not change in the second frame and the third frame, that is, the angle coordinate remains unchanged, the obstacle is recorded as a static obstacle at the current time, if there is a change, it is recorded as a moving obstacle; The position of the obstacle on the robot map is updated in real time according to the angle coordinate; Five frames of point cloud images collected by the depth camera are continuously detected and compared, and the z coordinate of the highest point of the obstacle in the adjacent two frames is compared, if the angle position of a certain obstacle moves approximately the same angle for five consecutive frames, and the error is within a predetermined range, and the distance error is within a predetermined range, the fifth frame is added to the current angle and distance as the prediction of the angle and distance position of the next frame.

4. The robotically-enabled depth camera-based obstacle detection system of claim 3, wherein, For the current static obstacle, the tracking control module and the detection, conversion and processing module cooperate to perform the following operations: The robot odometer moving distance is combined with the obstacle angle distance to update the estimated position of the obstacle relative to the robot in real time. When the obstacle is not detected in the left and right angle range of the estimated position for more than three times, the obstacle is removed from the obstacle list and the corresponding angle range in the map. When the second depth camera detects a new obstacle for more than three times, the obstacle in the corresponding angle range is added to the obstacle list and the map.

5. The robotic depth camera based obstacle detection system of claim 3, wherein, For the moving obstacle, the tracking control module and the detection conversion and processing module are used to perform the following operations: The position of the obstacle is predicted according to the inter-frame motion trajectory of the obstacle in the field of view of the first depth camera, and the distance of the obstacle is updated in real time according to the real-time scanning result of the second depth camera in each period, including updating the distance of the obstacle in the obstacle list and the map. The position of the obstacle is predicted according to the angle and distance displacement of the obstacle in each scanning period, until the obstacle disappears. When the obstacle is not detected in the left and right angle range of the estimated position for more than three times, the obstacle is removed from the obstacle list and the corresponding angle range in the map. The second depth camera scans back and forth for two periods at a time. When the second depth camera detects a new obstacle for more than three times, the obstacle in the corresponding angle range is added to the obstacle list and the map, thereby completing the obstacle detection and obstacle removal.

6. A method of robot depth camera based obstacle detection, the method comprising: The obstacle detection is performed by the robot depth camera-based obstacle detection system according to any one of claims 1-5, and the method comprises the following steps: For the point cloud data collected by the first depth camera, obstacle recognition and downward height drop recognition are performed based on the point cloud data. For the detected obstacle, the obstacle is identified and numbered according to the active angle range of the obstacle, and the obstacle is classified as a moving obstacle and a current static obstacle according to whether the position of the obstacle changes continuously. The obstacle number, obstacle classification, and obstacle position are recorded as obstacle information in the obstacle list, and the obstacle position is updated in real time on the map. The position of the obstacle in the next frame is predicted by the same angular coordinate displacement according to the distance and angle displacement of the obstacle between adjacent frames. When the first depth camera cannot detect the obstacle in the field of view, the obstacle is removed from the obstacle list and the map in real time. When the obstacle disappears from the edge of the field of view of the first depth camera or the predicted position of the obstacle in the next frame is outside the field of view of the first depth camera, the second depth camera is controlled to rotate and scan at a predetermined angular velocity, and the priority of the moving obstacle is higher than that of the current static obstacle during the rotation and scanning. For the point cloud data collected by the second depth camera, obstacle recognition and downward height drop recognition are performed based on the point cloud data. For the detected obstacle, the obstacle is identified and numbered according to the active angle range of the obstacle, and the obstacle is classified as a moving obstacle and a current static obstacle according to whether the position of the obstacle changes continuously. The obstacle number, obstacle classification and obstacle position are taken as obstacle information, it is judged whether the obstacle exists in the obstacle list, if it exists, the obstacle position is updated in real time on the map, if it does not exist, the obstacle information is added to the obstacle list, and the obstacle position is updated in real time on the map; According to the distance and angular displacement of the obstacle between adjacent frames, the next frame obstacle position is predicted through the same angular coordinate displacement.

7. The robotically-enabled depth camera-based obstacle detection method of claim 6, wherein, The obstacle recognition and downward height difference recognition are performed based on the point cloud data, including the following steps: The point cloud data is traversed, when a point with an absolute value of z less than a predetermined distance L1 is detected and only points with an absolute value of z less than the predetermined distance L1 are found in the circular neighborhood with a radius of a predetermined number M1 points, a plane is fitted with the detected point, the height difference between the other point cloud and the plane is judged, the point cloud z coordinate is taken as the height difference between the point cloud and the plane, when a point with a z axis height difference greater than the predetermined distance L1 is found, the circular neighborhood with a radius of a predetermined number M2 points is judged, the z absolute value is recorded as z0, when the number of points with a z absolute value greater than a predetermined distance L2 is more than a predetermined number M3, and there are more than a predetermined number M4 points with a z absolute value greater than the predetermined distance L2 and less than z0, all points with z greater than the predetermined distance L2 in the neighborhood are marked as non-ground points, and points with z less than the predetermined distance L2 are ground points; The point cloud is traversed until all ground points and non-ground points are segmented, when the number of a continuous non-ground point is greater than a predetermined number M3, it is considered that an obstacle or a downward height difference is detected, wherein z greater than 0 is considered to be an obstacle, and z less than 0 is considered to be a downward height difference.

8. The robotically-enabled depth camera-based obstacle detection method of claim 6, wherein, For the detected obstacle, the following operations are performed: An angular coordinate system is established with the first depth camera as the origin and the front as the 0° line, wherein the right side of the camera is negative and the left side is positive, the non-ground points are classified and numbered by angle with a predetermined radian N1 as the minimum resolution, the detected non-ground points are numbered according to different angle intervals, and the projection of the detected obstacle or downward height difference point on the (x, y) coordinate plane and the distance from the origin are calculated; The obstacle point angle is calculated according to the number and angular resolution, and the calculated obstacle angle and distance are recorded; For each detected obstacle, feature recognition is performed, the angle difference of the obstacle from the rightmost point to the leftmost point is detected, then the z coordinate size of the highest point of the obstacle is detected, if there are multiple points with the same height, the z coordinate size of the median point of the left and right angles is taken, if the number is even, the z coordinate size of the left median point is taken, then the left and right angle center points of the obstacle are detected and K points are selected on the left and right at intervals of K points, the z coordinates of 2K+1 points are extracted, each obstacle is numbered according to different features and the features are recorded respectively; Three frames of depth camera images are continuously traversed and compared, the z coordinate highest point of the obstacle of adjacent two frames is detected, if the obstacle numbered in the first frame does not change in the angle position in the second frame image and the third frame image, that is, the angular coordinate remains unchanged, the obstacle is recorded as a current static obstacle, if there is a change, it is recorded as a moving obstacle; The position of the obstacle on the robot map is updated in real time according to the angular coordinate. Continuously traverse the point cloud graphics collected by five frames of depth cameras, compare the highest points of the z coordinates of adjacent two frames of obstacles, if the angle position of a certain obstacle moves approximately the same angle for five consecutive frames, and the error is within a predetermined range, and the distance error is within a predetermined range, then the fifth frame is added to the current angle and distance as the prediction of the angle and distance position of the next frame.

9. The robotically-enabled depth camera-based obstacle detection method of claim 8, wherein, For the current static obstacle, the following operations are performed: Based on the robot odometer moving distance combined with the obstacle angle distance, the estimated position of the obstacle relative to the robot at this time is updated in real time, and when there are no obstacles in the left and right predetermined angle range of the estimated position for more than three times in continuous scanning, the obstacle is removed from the obstacle list, and the obstacle in the corresponding angle range in the map is also removed. When the second depth camera detects more than three times of new obstacles, the obstacle in the corresponding angle range in the obstacle list and the map is added.

10. The robotically-enabled depth camera-based obstacle detection method of claim 8, wherein, For the moving obstacle, the following operations are performed: According to the inter-frame motion trajectory of the obstacle when it is in the field of view of the first depth camera, the position of the obstacle at this time is predicted, and the distance of the obstacle is updated in real time according to the real-time scanning result of the second depth camera every cycle, including updating the distance of the obstacle in the obstacle list and the map, and simultaneously predicting the position of the obstacle according to the angle and distance displacement of the obstacle between each scanning cycle, until the obstacle disappears, and simultaneously predict that there are no obstacles in the left and right predetermined angle range for more than three times, remove the obstacle from the obstacle list, and remove the obstacle in the corresponding angle range in the map; wherein the second depth camera scans back and forth for two cycles at a time; When the second depth camera detects more than three times of new obstacles, the obstacle in the corresponding angle range in the obstacle list and the map is added, thereby completing the obstacle detection and obstacle removal.

Images