An obstacle positioning method based on RGBD camera fusion single-line laser radar

Abstract

The application relates to an obstacle positioning method based on RGBD camera fusion single-line laser radar, which comprises the following steps: a control device periodically receives a color image collected by a color camera, a depth image of a depth camera and first data of a single-line laser radar; according to an obstacle detection strategy, two or more of the color image, the depth image and the first data are fused and the fusion result is screened to obtain a final obstacle positioning result. The above method realizes multi-data fusion to obtain a positioning result through the scene and obstacle type division mode, and the positioning precision is improved.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to an obstacle localization method based on RGBD camera fusion with single-line LiDAR. Background Technology

[0002] Currently, autonomous driving and mobile robots are collectively referred to as robots. In the field of robotics, robots need to acquire information about their surrounding environment and abstract this raw information in a timely, effective, and accurate manner to obtain concise environmental information, which lays the foundation for subsequent decision-making and planning. Therefore, a good perception module plays a crucial role in robots.

[0003] Small, low-speed mobile robots, considering their specific needs and cost, often do not use expensive sensors such as multi-line LiDAR. Instead, they are typically equipped with single-line LiDAR for mapping and localization. RGBD cameras can acquire richer environmental information at a lower cost, and with the improvement of robot computing power, they can abstract more advanced semantic information.

[0004] Common sensors for robot environmental perception include LiDAR, sonar, monocular cameras, binocular cameras, and RGBD cameras. Each sensor has its own characteristics. For example, LiDAR is further divided into single-line LiDAR, multi-line LiDAR, and solid-state LiDAR. LiDAR features long range and high accuracy. Indoor robots often use single-line LiDAR mainly because of its lower cost and stronger resistance to sunlight interference compared to RGBD cameras. However, its perception range is limited to a single plane, making it difficult to distinguish between different types of obstacles. Sonar has low positioning accuracy and a small perception range. Monocular cameras lack scale information, making it difficult to perceive the true spatial location of obstacles. Binocular cameras perform poorly in detecting textureless obstacles, consume high computational resources, and their measurement accuracy decreases significantly with distance. RGBD cameras are small in size, low in power consumption, and have a wide field of view (FOV), but their effective ranging range is limited, and they are weakly resistant to sunlight interference, making them difficult to use outdoors.

[0005] Therefore, how to achieve obstacle localization based on RGBD camera fusion with single-line LiDAR has become an urgent technical problem to be solved. Summary of the Invention

[0006] (a) Technical problems to be solved

[0007] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides an obstacle localization method based on RGBD camera fusion single-line lidar.

[0008] (II) Technical Solution

[0009] To achieve the above objectives, the main technical solutions adopted by the present invention include:

[0010] In a first aspect, embodiments of the present invention provide an obstacle localization method based on RGBD camera fusion with single-line LiDAR. The mobile robot includes: a control device inside the robot, an RGBD camera and a single-line LiDAR mounted on the front side of the robot; the RGBD camera and the single-line LiDAR are electrically connected to the control device; the RGBD camera includes: a color camera and a depth camera with a relative positional relationship; the method includes:

[0011] The control device periodically receives color images captured by the color camera, depth images from the depth camera, and first data from the single-line lidar.

[0012] The control device selects to fuse two or more of the color image, depth image and first data according to the obstacle detection strategy and filters the fusion results to obtain the final obstacle localization result.

[0013] Obstacle detection strategies include one or more of the following combinations:

[0014] Determine the result of fusing the color image and depth map in the scene;

[0015] Determine the result of fusing the color image with the first data in the given scene;

[0016] Determine the result of fusing the color map and depth map within a preset distance in the scene;

[0017] Determine the result of fusing the color image outside the preset distance in the scene with the first data;

[0018] The result of fusing the color image with the first data in the obstacle scene;

[0019] The result of fusing the color image with the first data and depth map in an obstacle scene.

[0020] Optionally, for indoor close-range scenarios, the aforementioned control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including:

[0021] Obtain the obstacle detection bounding boxes in the color image;

[0022] The detection box is mapped onto the depth map, and the depth data of each pixel in the detection box and the clustering result of the depth data from the origin of the camera coordinate system are obtained to obtain the depth information of the obstacles in the detection box.

[0023] Based on the depth information of the obstacle, obtain the position information of the obstacle relative to the robot.

[0024] Optionally, based on the depth information of the obstacle, the position information of the obstacle relative to the robot is obtained, including:

[0025] Based on the depth information of the obstacle and the internal parameters of the depth camera, obtain the position of the obstacle relative to the RGBD camera; according to the external parameters of the system of the RGBD camera on the robot, obtain the position information of the obstacle relative to the robot.

[0026] Before obstacle positioning, with the help of multiple reflective columns arranged outside the robot, by adaptively adjusting the distance between the reflective columns and the robot, form an external parameter equation of the system to obtain the external parameters of the system.

[0027] Optionally, for indoor long-distance scenarios and / or outdoor scenarios, the control device selects two or more of the color map, depth map, and first data for fusion and filters the fusion results according to the obstacle detection strategy, including:

[0028] Convert the point cloud data of the single-line lidar, that is, the first data PnL: {id, (xL, yL, zL)}, to the color camera coordinate system, obtain the data Pnc: {id, (xc, yc, zc)} in the color camera coordinate system, and then project it onto the pixel coordinate system of the color camera to obtain the data Pnp: {id, (xc, yc, zc), (u, v)} in the pixel coordinate system;

[0029] where id is the index number of the laser point cloud data, (xL, yL, zL) is the coordinate of the laser point cloud data in the single-line lidar coordinate system, (xc, yc, zc) is the coordinate of the laser point cloud data in the color camera coordinate system, and (u, v) is the coordinate of the laser point cloud data in the pixel coordinate system of the color camera;

[0030] Obtain the obstacle detection frame in the color map, and record the vertical coordinates vl and vr on the left and right sides of the detection frame. Based on the data Pnp: {id, (xc, yc, zc), (u, v)} in the pixel coordinate system, when the coordinate v satisfies vl < v < vr, obtain the maximum value idmax and the minimum value idmin of the radar data index coordinate v corresponding to the left and right sides of the detection frame;

[0031] And according to the maximum value idmax and the minimum value idmin, map the pixel coordinates of the image to the index range of the corresponding radar data in the single-line lidar coordinate system (solved according to the pixel coordinates of the image, the internal parameters of the RGBD camera, and the relative external parameters between the RGBD camera and the single-line lidar), that is, the coordinates of all laser point cloud data where id satisfies idmin < id < idmax;

[0032] Perform point cloud cluster analysis on the obtained laser point cloud data that satisfies idmin < id < idmax to obtain the analysis result;

[0033] If the analysis result is a clustering result, use the center coordinate of this point cloud cluster as the coordinate of the obstacle in the single-line lidar coordinate system.

[0034] If the analysis results are multiple clustering results, sort the center coordinates of multiple point cloud clusters and the distance values ​​of the single-line lidar, retain the center coordinates of the point cloud cluster with the smallest distance value, and use the retained center coordinates as the coordinates of the obstacle in the single-line lidar coordinate system;

[0035] The coordinates of the obstacle in the single-line lidar coordinate system are transformed to the robot coordinate system to obtain the position information of the obstacle relative to the robot.

[0036] Optionally, the control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including:

[0037] When the color image and depth image are fused, the position information of the obstacle relative to the robot is within a preset distance. When the color image and the first data are fused, the position information of the obstacle relative to the robot is within a preset distance. The position information of the obstacle relative to the robot when the color image and depth image are fused is used as the position information output by the control device.

[0038] When the color image and depth image are fused, if the position information of the obstacle relative to the robot is within a preset distance, and when the color image and the first data are fused, if the position information of the obstacle relative to the robot is outside the preset distance, and the category of the detection box in the color image is a suspended or low obstacle, then the position information of the obstacle relative to the robot when the color image and depth image are fused will be used as the position information output by the control device.

[0039] When the color image and depth image are fused, the position information of the obstacle relative to the robot cannot be determined. When the color image and the first data are fused, the position information of the obstacle relative to the robot is within a preset distance, and the category of the detection box in the color image is the black obstacle category or outdoor scene. The position information of the obstacle relative to the robot when the color image and the first data are fused is used as the position information output by the control device.

[0040] Optionally, the control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including:

[0041] When fusion of color and depth maps, the position information of obstacles relative to the robot cannot be determined; or, when fusion of color and depth maps, the position information of obstacles relative to the robot is outside a preset distance.

[0042] When the color image and the first data are fused, if the position information of the obstacle relative to the robot is outside a preset distance, the position information of the obstacle relative to the robot when the color image and the first data are fused is used as the position information output by the control device.

[0043] Optionally, the control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including:

[0044] When the detection box in the color image belongs to the table category;

[0045] The second data in the single-line LiDAR coordinate system corresponding to the detection box is obtained, and point cloud clustering is performed to obtain the table leg coordinates PL(xL,yL,zL) in the single-line LiDAR coordinate system. The table leg coordinates are transformed to the color camera coordinate system to obtain Pc(xc,yc,zc), and then transformed to the pixel coordinate system Pp(up,vp). The width estimate of the tabletop is the Euclidean distance of the maximum lateral distance of multiple table legs in the single-line LiDAR coordinate system corresponding to the detection box.

[0046] Get the horizontal coordinates ut and ub of the top and bottom edges of the detection box of the color image. The pixel coordinates of the table leg directly above and below the detection box are (ut, vp) and (ub, vp).

[0047]

[0048] Where x, y, z are the coordinates in the camera coordinate system, fx, fy, cx, cy are the intrinsic parameters of the RGBD camera, and u, v are the pixel coordinates; substitute u=ut, v=vp into the formula to calculate y, denoted as yt, and similarly substitute u=ub, v=vp into the formula to calculate yb; obtain the estimated height of the table h=|yt-yb|.

[0049] Determine whether the table is an obstacle for the current robot by comparing the height and width of the empty area under the table with the robot's dimensions.

[0050] Optionally, before the control device periodically receives the color image acquired by the color camera, the depth image from the depth camera, and the first data from the single-line lidar, it further includes:

[0051] An obstacle detection model based on color images is trained using a pre-established dataset, where each image in the dataset is pre-labeled with a category and an obstacle detection box.

[0052] The trained obstacle detection model can identify obstacles in a color image and output the bounding box to which the obstacle belongs and the category of the obstacle.

[0053] Secondly, embodiments of the present invention also provide a control device, which includes a memory and a processor. The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory and perform the steps of the obstacle localization method based on RGBD camera fusion single-line lidar as described in any of the first aspects above.

[0054] Thirdly, embodiments of the present invention also provide a mobile robot, which includes the control device described in the second aspect above, an RGBD camera and a single-line lidar mounted on the front side of the robot; the RGBD camera and the single-line lidar are electrically connected to the control device.

[0055] (III) Beneficial Effects

[0056] The method of this invention combines the advantages and disadvantages of RGBD cameras and single-line LiDAR, and proposes a method for fusion perception of the two sensors in different environments / scenes. For example, obstacle detection and localization can be achieved by fusing RGBD cameras with single-line LiDAR, so as to achieve accurate positioning of obstacles. At the same time, it can also achieve spatial positioning of obstacles in special scenarios. Attached Figure Description

[0057] Figure 1 This is a flowchart illustrating an obstacle localization method based on RGBD camera fusion with single-line LiDAR, according to an embodiment of the present invention.

[0058] Figure 2 This is a flowchart illustrating an obstacle localization method based on RGBD camera fusion with single-line LiDAR, provided as another embodiment of the present invention. Detailed Implementation

[0059] To better explain and facilitate understanding of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0060] Currently, both single-line LiDAR and depth cameras have their own limitations: single-line LiDAR has a detection range of up to 40 meters, but the detection height is fixed, and many low and suspended obstacles cannot be detected. Depth cameras can detect obstacles in a wider range, including low and suspended obstacles, but their detection range is shorter, with accuracy dropping significantly beyond 2 meters. Furthermore, sunlight greatly affects depth cameras, resulting in poor positioning performance outdoors.

[0061] In view of this, this embodiment of the invention combines the advantages and disadvantages of RGBD cameras and single-line LiDAR, and proposes a method for fusion perception of the two sensors under different environments / scenes. Specifically, it fuses obstacle detection and localization of RGBD cameras with single-line LiDAR. Specifically, it detects and identifies obstacles (the color camera of the RGBD camera uses deep learning to obtain detection boxes and categories on the image); furthermore, it can perform spatial localization of obstacles in special scenes (by mapping the detection boxes onto the depth map or the LiDAR point cloud), and achieve precise localization of obstacles.

[0062] It should be noted that the RGBD camera mentioned in the embodiments of the present invention includes a regular color camera and a depth camera. Before leaving the factory, the camera manufacturer aligns the data of the color camera and the data of the depth camera, and provides the point cloud data to which the RGBD camera belongs, and the relative positional relationship between the point cloud data and the color camera and the depth camera.

[0063] Depth cameras typically measure distance by actively emitting and receiving infrared light. However, for dark, low-reflectivity objects, the depth camera receives too little infrared light to pinpoint the obstacle's location. Furthermore, some obstacles are hollow, such as shelves or tables, and neither single-line LiDAR nor depth cameras can independently detect the complete obstacle. In this invention, the outline of the obstacle is estimated using a detection frame, category, and data from single-line LiDAR and depth cameras. This prevents the robot from colliding with the side of a table when passing underneath it, effectively improving the precise location of obstacles.

[0064] The mobile robot in this embodiment of the invention includes: a control device inside the robot, an RGBD camera and a single-line lidar mounted on the front side of the robot; the RGBD camera and the single-line lidar are electrically connected to the control device; the RGBD camera includes: a color camera and a depth camera with a relative positional relationship.

[0065] Example 1

[0066] like Figure 1 As shown, this embodiment of the invention provides an obstacle localization method based on RGBD camera fusion with single-line LiDAR. The execution subject of this method can be any robot control device / mobile robot, and the specific implementation method includes the following steps:

[0067] S10. The control device periodically receives color images captured by the color camera, depth images from the depth camera, and first data from the single-line lidar.

[0068] S20. The control device selects two or more of the color image, depth image and first data to fuse according to the obstacle detection strategy and filters the fusion results to obtain the final obstacle localization result.

[0069] Obstacle detection strategies include one or more of the following combinations:

[0070] Determine the result of fusing the color image and depth map in the scene;

[0071] Determine the result of fusing the color image with the first data in the given scene;

[0072] Determine the result of fusing the color map and depth map within a preset distance in the scene;

[0073] Determine the result of fusing the color image outside the preset distance in the scene with the first data;

[0074] The result of fusing the color image with the first data in the obstacle scene;

[0075] The result of fusing the color image with the first data and depth map in an obstacle scene.

[0076] For example, in one implementation scenario, such as a close-range indoor detection scenario, the aforementioned S20 can perform the fusion of color images and depth maps, specifically including:

[0077] S21. Obtain the obstacle detection box in the color image;

[0078] In practical applications, a pre-established dataset can be used to train an obstacle detection model for color images. Each image in the dataset is pre-labeled with a category and an obstacle detection box. The trained obstacle detection model can identify obstacles in the color images and output the detection box to which the obstacle belongs and the category of the obstacle.

[0079] The obstacle detection model in this embodiment can be a deep learning model, but it is not limited to a deep learning model and can be selected according to actual needs.

[0080] S22. Map the detection box onto the depth map, and obtain the depth data of each pixel in the detection box and the clustering result of the depth data from the origin of the camera coordinate system, so as to obtain the depth information of the obstacle in the detection box.

[0081] S23. Based on the depth information of the obstacle, obtain the position information of the obstacle relative to the robot.

[0082] For example, based on the depth information of the obstacle and the intrinsic parameters of the depth camera, the position of the obstacle relative to the RGBD camera is obtained; based on the extrinsic parameters of the RGBD camera on the robot, the position information of the obstacle relative to the robot is obtained.

[0083] Before obstacle localization, multiple reflective pillars are placed outside the robot. By adaptively adjusting the distance between the reflective pillars and the robot, the system extrinsic parameter equations are formed, and the system extrinsic parameters are obtained.

[0084] For example, in the second implementation scenario, such as indoor long-distance detection or outdoor scene detection, the above S20 may include:

[0085] S21a. Convert the point cloud data of the single-line lidar, i.e., the first data PnL: {id, (xL, yL, zL)}, to the color camera coordinate system, obtaining the data Pnc: {id, (xc, yc, zc)} in the color camera coordinate system, and then project it onto the pixel coordinate system of the color camera to obtain the data Pnp: {id, (xc, yc, zc), (u, v)} in the pixel coordinate system;

[0086] Among them, id is the index number of the laser point cloud data, (xL, yL, zL) is the coordinate of the laser point cloud data in the single-line lidar coordinate system, (xc, yc, zc) is the coordinate of the laser point cloud data in the color camera coordinate system, and (u, v) is the coordinate of the laser point cloud data in the pixel coordinate system of the color camera;

[0087] S22a. Obtain the obstacle detection box in the color image, and record the vertical coordinates vl and vr on the left and right sides of the detection box. Based on the data Pnp: {id, (xc, yc, zc), (u, v)} in the pixel coordinate system, when the coordinate v satisfies vl < v < vr, obtain the maximum value idmax and the minimum value idmin of the coordinate v in the radar data indexes corresponding to the left and right sides of the detection box;

[0088] S23a. And according to the maximum value idmax and the minimum value idmin, map the pixel coordinates of the image to the index range of the corresponding radar data in the single-line lidar coordinate system (solved according to the pixel coordinates of the image, the internal parameters of the camera, and the relative external parameters between the camera and the single-line lidar), that is, the coordinates of all laser point cloud data where id satisfies idmin < id < idmax;

[0089] S24a. Perform point cloud cluster analysis on the obtained laser point cloud data where idmin < id < idmax, and obtain the analysis result;

[0090] If the analysis result is the result of one clustering, then use the center coordinate of this point cloud cluster as the coordinate of the obstacle in the single-line lidar coordinate system;

[0091] If the analysis result is the result of multiple clusterings, then sort the center coordinates of the multiple point cloud clusters and the distance values from the single-line lidar, retain the center coordinate of the point cloud cluster with the smallest distance value, and use the retained center coordinate as the coordinate of the obstacle in the single-line lidar coordinate system;

[0092] S25a. Convert the coordinate of the obstacle in the single-line lidar coordinate system to the robot coordinate system to obtain the position information of the obstacle relative to the robot.

[0093] Based on the above two scenarios, in practical applications, the final output result can be selected based on the following four screening methods:

[0094] The first filtering method is as follows: when the color image and depth image are fused, the position information of the obstacle relative to the robot is within a preset distance; when the color image and the first data are fused, the position information of the obstacle relative to the robot is within a preset distance; the position information of the obstacle relative to the robot when the color image and depth image are fused is used as the position information output by the control device.

[0095] The second filtering method is as follows: when the color image and depth image are fused, the position information of the obstacle relative to the robot is within a preset distance; when the color image and the first data are fused, the position information of the obstacle relative to the robot is outside a preset distance; and the category of the detection box in the color image is a suspended or low obstacle, then the position information of the obstacle relative to the robot when the color image and depth image are fused is used as the position information output by the control device.

[0096] The third filtering method is as follows: When the color image and depth image are fused, the position information of the obstacle relative to the robot cannot be determined. When the color image and the first data are fused, the position information of the obstacle relative to the robot is within a preset distance, and the category of the detection box in the color image is the black obstacle category or outdoor scene. The position information of the obstacle relative to the robot when the color image and the first data are fused is used as the position information output by the control device.

[0097] The fourth filtering method: When the color image and depth map are fused, the position information of the obstacle relative to the robot cannot be determined; or, when the color image and depth map are fused, the position information of the obstacle relative to the robot is outside a preset distance.

[0098] When the color image and the first data are fused, if the position information of the obstacle relative to the robot is outside a preset distance, the position information of the obstacle relative to the robot when the color image and the first data are fused is used as the position information output by the control device.

[0099] Example 2

[0100] Based on the description of Embodiment 1 above, it can be understood that the above method mainly includes the following three parts:

[0101] First: Use a calibration object (such as a reflector) to calibrate the color camera and single-line LiDAR in the RGBD camera to obtain the system's external parameters.

[0102] It should be noted that current relative extrinsic parameter calibration for color cameras and LiDAR typically involves calibrating the color camera and a multi-line (or solid-state) LiDAR (point cloud data is richer than that of a single-line LiDAR). In this embodiment, the system extrinsic parameters are calibrated using a single-line LiDAR and a color camera.

[0103] Second, based on the color camera, depth camera and single-line LiDAR in the RGBD camera, adaptive sensing and localization of obstacles in different indoor and outdoor scenarios are achieved.

[0104] This section uses deep learning to detect obstacles in images from a color camera, obtaining bounding boxes. Then, based on the distance to the obstacle within the bounding box and the environment (with a set threshold), it selects between a single-line LiDAR and a depth camera for localization. This selection method makes the obstacle localization results more robust.

[0105] Third, for the parts of the image that are missed in different indoor and outdoor scenarios, the depth camera in the RGBD camera is used to supplement the image, so as to ensure accurate positioning and determination of the spatial shape of obstacles.

[0106] The following is in conjunction with the appendix Figure 2 The above three parts will be explained in detail.

[0107] First, the system external parameter calibration process.

[0108] a. An RGBD camera and a single-line LiDAR are mounted on the front of the robot. Prepare multiple reflective pillars of uniform height (e.g., eight 0.5m high), and place them vertically within a 10m x 10m area in front of the robot. With the robot stationary, manually ensure that all these reflective pillars can be simultaneously observed by the color camera and LiDAR without obstruction, i.e., all reflective pillars are simultaneously viewed by the color camera and the single-line LiDAR.

[0109] b. Obtain the number of reflective pillars in the LiDAR and color camera images.

[0110] The LiDAR adaptively acquires the number of reflective pillars. The method for acquiring the number of reflective pillars using a single-line LiDAR is as follows: a point cloud is identified as a reflective pillar when both of the following conditions are met: i. The point cloud intensity value within a 10m*10m area in front of the robot is greater than the detection threshold for reflective pillars; ii. Point cloud clusters with high intensity values ​​are clustered, and arcs are fitted using the least squares method. If the difference between the radius of curvature of the arc and the actual radius of the reflective pillar is less than the threshold, then the point cloud cluster can be identified as a reflective pillar.

[0111] Method for obtaining the number of reflective pillars using a color camera: Count the reflective pillars using the detection boxes detected by deep learning.

[0112] If the number of reflective columns detected by the single-line lidar or color camera is less than 8, the position of the reflective columns needs to be manually readjusted until the number of reflective columns detected by the single-line lidar or color camera is equal to 8.

[0113] c. A color camera and a single-line lidar respectively record the coordinates of the center point at the top of the reflective column (e.g., ...). Figure 2 As shown in the figure, specifically, the 3D coordinates are recorded in the single-line lidar coordinate system; and the 2D pixel coordinates are recorded in the color camera pixel coordinate system.

[0114] Method for obtaining the 3D coordinates of the center point at the top of the reflector column in the single-line lidar coordinate system: In reality, a single-line lidar can only obtain the 2D coordinates (x, y) of the reflector column in the lidar coordinate system. The 2D coordinates are the midpoint of the fitted circular arc of the reflector column point cloud. Here, the height value of the 3D coordinate is the reflector column height minus the installation height of the lidar above the ground. For example, if the lidar installation height is 0.1m and the reflector column height is 0.5m, the height difference is 0.4m, then the 3D coordinates are (x, y, 0.4). Mark 1 to 8 reflector columns from left to right and record the 3D coordinates of the center point at the top of each reflector column.

[0115] Method for obtaining the pixel coordinates of the center point at the top of the reflector column in the color camera pixel coordinate system: Number the reflectors in the color camera image from left to right as 1 to 8. Trigger the center point at the top of each reflector column so that the calibration program records the pixel coordinates (u, v) of that center point. Further, the program uses a labeling method to help determine if the clicked location is indeed the center point at the top of the reflector column. When the calibration program displays a window asking whether to record the coordinates of that point, the user can then confirm the final recorded coordinates of the center point at the top of the reflector column for that number.

[0116] d. Manually move all reflector positions, repeating sub-steps a, b, and c at least 3 times to obtain 3 sets of a total of 24 3D coordinates:

[0117] (((x11,y11,0.04),(x12,y12,0.04)...(x18,y18,0.04)),((x21,y21,0.04),(x22,y22,0.04)...(x28,y28,0.04)),((x31,y31,0.04),(x32,y32,0.04)...(x38,y38,0.04))) and 2D pixel coordinates (((u11,v11),(u12,v12)...(u18,v18)),((u21,v21),(u22,v22)...(u28,v28)),((u31,v31),(u32,v32)...(u38,v38))).

[0118]

[0119] Zc represents the Z-component of the pose of the camera coordinate system relative to the single-line lidar coordinate system (OLXLYLZL). Zc can be eliminated after formula expansion. K is the camera intrinsic parameter, (u,v) is the image coordinate system, and T is the relative pose relationship in the single-line lidar coordinate system.

[0120] Substituting the 24 3D and 2D coordinates into the above equation for SVD decomposition, we can obtain T. Inverse T yields the relative pose of the single-line lidar in the color camera coordinate system.

[0121] Second, the process of detecting and locating obstacles.

[0122] 1.1 Deep Learning Model Training Phase:

[0123] a. Manually collect image data of specific types of obstacles, such as people and robots (e.g., 10,000 images), and manually label the detection boxes and categories of the obstacles.

[0124] b. Select an appropriate object detection model based on the GPU performance of the industrial control computer to which the robot belongs. For example, if the industrial control GPU is an Intel integrated graphics card, a smaller deep learning network model such as YOLOv2-tiny can be selected. Deep learning frameworks (such as Darknet) can also be used to train deep network models.

[0125] c. Deploy the trained network model to the industrial control computer of the robot, start the target detection program, and obtain the pixel coordinates of the detection box of the obstacle on the color camera image.

[0126] 1.2 Target detection phase after the robot starts the target detection program:

[0127] This step is a data-level fusion positioning process, which consists of two parts: fusion of color image and depth image, and fusion of color image and single-line LiDAR.

[0128] (1) Fusion of color images from a color camera and depth images from a depth camera:

[0129] a. The detection bounding box results of the color image are mapped to the depth image.

[0130] It should be noted that the pose relationship between the color camera and the depth camera in an RGBD camera is known.

[0131] b. Obtain the distance to the depth data within the detection box, retain the clustering result that is smallest to the origin of the camera coordinate system, which can filter out the depth information of non-target obstacles in the detection box and retain the depth information of obstacles.

[0132] c. Calculate the position of the obstacle relative to the camera based on the depth information and the intrinsic parameters of the depth camera (provided by the manufacturer or calibrated using a checkerboard pattern). Calculate the position of the obstacle relative to the robot based on the relative extrinsic parameters between the various sensors preset by the robot.

[0133] (2) Fusion of color images from color cameras and data from single-line lidar:

[0134] a. Convert the data of each point of the single-line lidar PnL: {id, (xL, yL, zL)} to the color camera coordinate system Pnc: {id, (xc, yc, zc)}, and then project it onto the pixel coordinate system of the color camera Pnp: {id, (xc, yc, zc), (u, v)}. Here, id is the index number of the lidar point cloud data, (xL, yL, zL) is the coordinate of the lidar point cloud data in the single-line lidar coordinate system (zL is a fixed value, which is the installation height of the lidar), (xc, yc, zc) is the coordinate of the lidar point cloud data in the color camera coordinate system, and (u, v) is the coordinate of the lidar point cloud data in the pixel coordinate system of the color camera.

[0135] b. Record the vertical coordinates vl and vr of the left and right sides of the obstacle detection frame in the color image, and find all the point clouds of the lidar data index coordinates v in the pixel coordinate system that satisfy vl < v < vr. Find idmax and idmin corresponding to the maximum and minimum values of v.

[0136] c. According to the maximum value idmax and the minimum value idmin, map the index coordinate v in the pixel coordinate system of the image to the index range of the corresponding lidar data in the single-line lidar coordinate system. According to the mapped index range idmax and idmin, find the coordinates of all the lidar point cloud data whose id satisfies idmin < id < idmax in the single-line lidar coordinate system.

[0137] d. Analyze the point cloud clusters within the range of idmin and idmax, and perform clustering on the point clouds within the range. If there is only one clustering result, use the center coordinate of this point cloud cluster as the coordinate of the obstacle in the lidar coordinate system. If there are multiple clustering results, sort the center coordinates of the multiple point cloud clusters and the distance values from the lidar, and finally only retain the center coordinate of the point cloud cluster with the smallest distance value. (The point clouds within the range of idmin and idmax include the lidar point clouds of the obstacle, and may also include other lidar point clouds behind the side of the obstacle within this range.)

[0138] e. According to b, find the coordinates of all the obstacles within the detection frame in the lidar coordinate system, and then convert them to the robot coordinate system to complete the obstacle detection and positioning.

[0139] (3) Obstacle positioning for decision-level fusion in different environments:

[0140] Since the depth data obtained by the RGBD camera is unreliable in outdoor strong light, when encountering black low-reflection substances, and at long distances, this embodiment will set a threshold according to the camera characteristics and only retain the depth data within the threshold.

[0141] 3.1) Obstacle detection at close range (such as within 3m):

[0142] a) If the obstacle localization results of the combination of color image and depth image and the localization results of color image and LiDAR are both within 3m and have a high degree of overlap, then the localization result of color image and depth image shall prevail.

[0143] b. Under the same obstacle detection frame in the color image, if the obstacle localization result of the combination of color camera and depth camera is within 3m, and the localization result of color camera and LiDAR is outside 3m, then the obstacle is a nearby suspended or low obstacle (all individual obstacles are located above or below the radar sensing plane). The localization result of color image and depth image shall prevail.

[0144] c. If the obstacle localization results of the combination of color camera and depth camera are not available, and the localization results of color camera and LiDAR are within 3m, the obstacle may be a black obstacle or outdoors. If the depth camera perception results are poor (the depth map corresponding to the detection box has no depth value, or the number of effective pixels with depth values ​​is less than 1 / 5 of the total number of pixels in the detection box), the localization results of color camera and LiDAR shall prevail.

[0145] 3.2) Long-distance obstacle detection: If the obstacle localization result of the combination of color camera and depth camera is not found or the detection distance is more than 3m away, and the localization result of color camera and LiDAR is more than 3m away, it can be determined that it is a long-distance obstacle. In this case, the localization result of color camera and LiDAR shall prevail.

[0146] 3.3) Estimation of the profile height of special obstacles:

[0147] For obstacles with legs on both sides and an open space in the middle, such as doors, tables, and tall shelves (taking tables as an example), RGBD camera depth data may be ineffective or unable to perceive the complete depth of the obstacle. In this case, the LiDAR can only detect the table legs, and the robot's path planning may lead it to pass under the table. If the robot is taller than the table, a collision will occur. Therefore, it is necessary to estimate the contour and height of specific obstacles.

[0148] The specific steps are as follows:

[0149] a) Deep learning detects specific obstacles resembling tables. Similar to the color image and single-line LiDAR fusion process described above, it identifies point clouds with matching IDs in the single-line LiDAR coordinate system corresponding to the color image detection boxes. It then clusters these point clouds to find the table leg coordinates PL(xL, yL, zL) in the single-line LiDAR coordinate system and transforms them to Pc(xc, yc, zc) in the color camera coordinate system, where zc represents the depth. Finally, it transforms to the pixel coordinate system Pp(up, vp). The table width is estimated as the Euclidean distance of the maximum lateral distance between the table legs in the single-line LiDAR coordinate system corresponding to the detection boxes.

[0150] b. Find the horizontal coordinates ut and ub of the top and bottom edges of the detection box in the color image. Then the pixel coordinates of the table leg directly above and below the detection box are (ut, vp) and (ub, vp).

[0151]

[0152] Where x, y, z are the coordinates in the camera coordinate system, fx, fy, cx, cy are the camera intrinsic parameters, and u, v are the pixel coordinates. Substituting u=ut, v=vp into the formula, we can calculate y, denoted as yt. Similarly, substituting u=ub, v=vp into the formula, we can calculate yb. The estimated height of the table, h, is |yt-yb|.

[0153] c. Determine whether to use the table as an obstacle based on the height and width of the empty area under the table and the size of the robot.

[0154] This embodiment combines the advantages and disadvantages of RGBD cameras and single-line LiDAR, and proposes a method for fusion perception of the two sensors under different environments / scenes, which achieves accurate positioning of obstacles.

[0155] Example 3

[0156] This embodiment also provides a control device for a mobile robot, including: a memory and a processor; the processor is used to execute a computer program stored in the memory to implement the steps of the obstacle localization method based on RGBD camera fusion LiDAR described in either Embodiment 1 or Embodiment 2.

[0157] On the other hand, embodiments of the present invention also provide a computer-readable storage medium for storing a computer program, which, when executed by a processor, implements the steps of the obstacle localization method based on RGBD camera fusion lidar of any of the above embodiments.

[0158] It should be noted that any reference numerals placed between parentheses in the claims should not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In claims that enumerate several means, several of these means may be embodied by the same hardware. The use of the terms first, second, third, etc., is merely for convenience of expression and does not indicate any order. These terms can be understood as part of the component names.

[0159] Furthermore, it should be noted that in the description of this specification, the terms "one embodiment," "some embodiments," "embodiment," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0160] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the claims should be interpreted to include both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0161] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, then this invention should also include these modifications and variations.

Claims

1. An obstacle localization method based on RGBD camera fusion with single-line lidar, characterized in that, The mobile robot includes: a control unit inside the robot, an RGBD camera and a single-line LiDAR mounted on the front of the robot; the RGBD camera and the single-line LiDAR are electrically connected to the control unit; the RGBD camera includes: a color camera and a depth camera with relative positional relationship; the method includes: The control device periodically receives color images captured by the color camera, depth images from the depth camera, and first data from the single-line lidar. The control device selects to fuse two or more of the color image, depth image and first data according to the obstacle detection strategy and filters the fusion results to obtain the final obstacle localization result. The control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including: When the detection box in the color image belongs to the table category; The second data in the single-line LiDAR coordinate system corresponding to the detection box is obtained, and point cloud clustering is performed to obtain the table leg coordinates PL(xL,yL,zL) in the single-line LiDAR coordinate system. The table leg coordinates are transformed to the color camera coordinate system to obtain Pc(xc,yc,zc), and then transformed to the pixel coordinate system Pp(up,vp). The width estimate of the tabletop is the Euclidean distance of the maximum lateral distance of multiple table legs in the single-line LiDAR coordinate system corresponding to the detection box. Get the horizontal coordinates ut and ub of the top and bottom edges of the detection box of the color image. The pixel coordinates of the table leg directly above and below the detection box are (ut, vp) and (ub, vp). ； Where x, y, z are the coordinates in the camera coordinate system, fx, fy, cx, cy are the intrinsic parameters of the RGBD camera, and u, v are the pixel coordinates; substitute u=ut, v=vp into the formula to calculate y, denoted as yt, and similarly substitute u=ub, v=vp into the formula to calculate yb; obtain the estimated height of the table h=|yt-yb|. Determine whether the table is an obstacle for the current robot by comparing the height and width of the empty area under the table with the robot's size; The obstacle detection strategy includes: the result of fusing the color image with the first data and the depth map in the obstacle scene; Prior to obstacle localization, the method further includes: calibrating the extrinsic parameters of the color camera and the single-line lidar, specifically including: Multiple reflective pillars were placed in front of the robot; The single-line lidar identifies point cloud clusters as reflective pillars and counts them if the difference between the radius of curvature of the arc and the actual radius of the reflective pillar is less than the threshold. The color camera uses a deep learning model to detect and count the bounding boxes of reflective pillars; When the lidar and color camera detect the same number of reflective pillars at the same time, record the 3D coordinates of the center point of the top of the reflective pillar in the single-line lidar coordinate system and the 2D pixel coordinates in the color camera pixel coordinate system. Based on multiple sets of recorded 3D coordinates and 2D pixel coordinates, the system extrinsic parameter equations are constructed, and the relative extrinsic parameters between the color camera and the single-line lidar are solved.

2. The obstacle localization method according to claim 1, characterized in that, The control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including: Obtain the obstacle detection box in the color image; Map the detection box to the depth image, and obtain the depth data of each pixel point in the detection box and the clustering result of the distance of the depth data from the origin of the camera coordinate system, and obtain the depth information of the obstacle in the detection box; Obtain the position information of the obstacle relative to the robot according to the depth information of the obstacle.

3. The obstacle localization method according to claim 2, characterized in that, Obtain the position information of the obstacle relative to the robot according to the depth information of the obstacle, including: Based on the depth information of the obstacle and the internal parameters of the depth camera, obtain the position of the obstacle relative to the RGBD camera; according to the external parameters of the RGBD camera system on the robot, obtain the position information of the obstacle relative to the robot; Before obstacle positioning, with the help of multiple reflective columns arranged outside the robot, by adaptively adjusting the distance between the reflective columns and the robot, form an external parameter equation of the system, and obtain the external parameters of the system.

4. The obstacle localization method according to claim 2, characterized in that, The control device selects and fuses two or more of the color image, depth image and first data according to the obstacle detection strategy and filters the fusion result, including: Convert the point cloud data of the single-line lidar, that is, the first data PnL:{id,(xL,yL,zL)} to the color camera coordinate system, obtain the data Pnc:{id,(xc,yc,zc)} in the color camera coordinate system, and then project it to the pixel coordinate system of the color camera to obtain the data Pnp:{id,(xc,yc,zc),(u,v)} in the pixel coordinate system; Where, id is the index number of the laser point cloud data, (xL,yL,zL) is the coordinate of the laser point cloud data in the single-line lidar coordinate system, (xc,yc,zc) is the coordinate of the laser point cloud data in the color camera coordinate system, and (u,v) is the coordinate of the laser point cloud data in the pixel coordinate system of the color camera; Obtain the obstacle detection box in the color image, and record the vertical coordinates vl and vr on the left and right sides of the detection box. Based on the data Pnp:{id,(xc,yc,zc),(u,v)} in the pixel coordinate system, when the radar data index coordinates v corresponding to the left and right sides of the detection box satisfy vl < v < vr, obtain the maximum value idmax and the minimum value idmin of the radar data index coordinates v corresponding to the left and right sides of the detection box; And according to the maximum value idmax and the minimum value idmin, map the pixel coordinates of the image to the index range of the corresponding radar data in the single-line lidar coordinate system, that is, the coordinates of all laser point cloud data where id satisfies idmin < id < idmax in the single-line lidar coordinate system; Perform point cloud cluster analysis on the obtained laser point cloud data that satisfies idmin < id < idmax, and obtain the analysis result; If the analysis result is a clustering result, use the center coordinates of this point cloud cluster as the coordinates of the obstacle in the single-line lidar coordinate system; If the analysis result is multiple clustering results, sort the center coordinates of the multiple point cloud clusters and the distance values of the single-line lidar, retain the center coordinates of the point cloud cluster with the smallest distance value, and use the retained center coordinates as the coordinates of the obstacle in the single-line lidar coordinate system; The coordinates of the obstacle in the single-line lidar coordinate system are transformed to the robot coordinate system to obtain the position information of the obstacle relative to the robot.

5. The obstacle localization method according to claim 4, characterized in that, The control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including: When the color image and depth image are fused, the position information of the obstacle relative to the robot is within a preset distance. When the color image and the first data are fused, the position information of the obstacle relative to the robot is within a preset distance. The position information of the obstacle relative to the robot when the color image and depth image are fused is used as the position information output by the control device. When the color image and depth image are fused, if the position information of the obstacle relative to the robot is within a preset distance, and when the color image and the first data are fused, if the position information of the obstacle relative to the robot is outside the preset distance, and the category of the detection box in the color image is a suspended or low obstacle, then the position information of the obstacle relative to the robot when the color image and depth image are fused will be used as the position information output by the control device. When the color image and depth image are fused, the position information of the obstacle relative to the robot cannot be determined. When the color image and the first data are fused, the position information of the obstacle relative to the robot is within a preset distance, and the category of the detection box in the color image is the black obstacle category or outdoor scene. The position information of the obstacle relative to the robot when the color image and the first data are fused is used as the position information output by the control device.

6. The obstacle localization method according to claim 1, characterized in that, The control device selects to fuse two or more of the color image, depth image, and first data according to the obstacle detection strategy and filters the fusion results, including: When fusion of color and depth maps, the position information of obstacles relative to the robot cannot be determined; or, when fusion of color and depth maps, the position information of obstacles relative to the robot is outside a preset distance. When the color image and the first data are fused, if the position information of the obstacle relative to the robot is outside a preset distance, the position information of the obstacle relative to the robot when the color image and the first data are fused is used as the position information output by the control device.

7. The obstacle localization method according to claim 1, characterized in that, Before the control device periodically receives the color image acquired by the color camera, the depth image from the depth camera, and the first data from the single-line lidar, it also includes: An obstacle detection model based on color images is trained using a pre-established dataset, where each image in the dataset is pre-labeled with a category and an obstacle detection box. The trained obstacle detection model can identify obstacles in a color image and output the bounding box to which the obstacle belongs and the category of the obstacle.

8. A control device, characterized in that, It includes a memory and a processor, the memory being used to store a computer program, and the processor being used to execute the computer program stored in the memory and perform the steps of the obstacle localization method based on RGBD camera fusion single-line lidar as described in any one of claims 1 to 7.

9. A mobile robot, characterized in that, It includes the control device as described in claim 8, an RGBD camera and a single-line lidar mounted on the front side of the robot; the RGBD camera and the single-line lidar are electrically connected to the control device.

Images