A method and system for fast point cloud removal of dynamic obstacles based on YOLO
Through YOLOv5 detection and adaptive pass filtering algorithm, the trajectory accuracy reduction and inaccurate positioning caused by dynamic obstacles in laser SLAM systems are solved, and more efficient point cloud removal and system accuracy improvement are achieved.
Patent Information
- Application Number
- CN202410085741.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-01-22
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-01-22
AI Technical Summary
In complex environments, dynamic obstacles cause the trajectory accuracy of the laser SLAM system to decrease, inaccurate positioning or failure of path planning, especially when there are many dynamic objects in narrow environments and close to lidar.
Using the fast point cloud removal method of dynamic obstacles based on YOLO, the external parameter matrix is obtained through lidar and camera calibration, and using YOLOv5 to detect dynamic objects in the image, project them into the point cloud, and identify and eliminate dynamics through adaptive direct-pass filtering algorithm. Object point cloud.
It improves the positioning accuracy and environmental mapping quality of SLAM algorithm in dynamic scenarios, reduces the amount of mismatch and calculation, enhances map generation efficiency, and ensures the reliability of positioning navigation and path planning.
Smart Images

Figure CN117911271B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of dynamic obstacle point cloud removal, and in particular to a method and system for fast point cloud removal of dynamic obstacles based on YOLO. Background Art
[0002] Laser SLAM (Simultaneous Localization and Mapping) is a real-time 3D environment mapping and positioning technology that is widely used in fields such as unmanned vehicles, robot navigation, and augmented reality. Laser SLAM builds maps and self-positions based on point cloud data collected by laser radar. However, in complex actual scenes, point cloud data may contain a large number of dynamic obstacles, such as pedestrians, vehicles, etc. The existence of these dynamic obstacles poses a challenge to SLAM algorithms. In point cloud-based SLAM perception systems, point cloud data is usually used to represent the three-dimensional information of the scene. Traditional laser SLAM algorithms usually assume that the environment is static when processing point cloud data, so they are insensitive to dynamic obstacles. This causes dynamic obstacles to be mistakenly regarded as static points in the map, resulting in map inaccuracies and positioning offsets. To this end, dynamic point cloud removal is required.
[0003] Dynamic point cloud culling refers to identifying and removing moving objects in the environment in laser SLAM to improve the accuracy and stability of positioning and mapping. LiDAR scanning is a real-time capture of the surrounding scene. When the environment is limited, especially in a narrow indoor environment, the presence of more dynamic objects will bring greater challenges to the SLAM system. When there are many objects indoors and they are close to the LiDAR, the laser beam will stop flying immediately after it is emitted to the surface of the dynamic object and reflect back to the laser receiver. Therefore, the more dynamic objects there are and the closer they are to the LiDAR, the higher the proportion of dynamic points in a frame of point cloud, and the greater the challenge to the assumption of static environment. At this time, when performing point cloud registration, dynamic objects will interfere with the registration between LiDAR scanning and sub-maps, resulting in decreased trajectory accuracy or flight. The presence of dynamic objects will also form "ghosts" in the point cloud map, affecting the integrity and availability of the map, thereby affecting map-based positioning and navigation, resulting in inaccurate positioning or path planning failure. Summary of the invention
[0004] The technical problems to be solved by the present invention are:
[0005] In order to solve the problem that when existing dynamic point clouds are removed, the more dynamic objects there are in a narrow environment and the closer they are to the lidar, the lower the trajectory accuracy, resulting in inaccurate positioning or path planning failure.
[0006] The present invention adopts the following technical solutions to solve the above technical problems:
[0007] The present invention provides a method for fast point cloud removal of dynamic obstacles based on YOLO, comprising the following steps:
[0008] Step 1: Calibrate the laser radar and camera. Use the laser radar and camera to observe the same calibration plate at the same time. By extracting the feature points in the image and the corresponding points in the point cloud, at least three 2D-3D point pairs are obtained. Then, the rotation matrix R and translation vector t from the laser radar coordinate system to the camera coordinate system are solved, and the external parameter matrix between the camera and the laser radar is obtained.
[0009] Step 2: Obtain camera images and laser point clouds, using cameras and laser sensors to obtain synchronized image and point cloud data;
[0010] Step 3: Detect dynamic objects in the RGB image captured by the camera in step 2, use YOLOv5 to detect dynamic objects in the image, and obtain the coordinate information of their bounding boxes;
[0011] Step 4: Project the dynamic object determined in step 3 to the point cloud, and use the internal and external parameters of the camera and laser sensor to project the position information of the dynamic object bounding box in the RGB image to the point cloud to obtain the corresponding point cloud;
[0012] Step 5: Use the adaptive through-filter algorithm to identify and remove the dynamic object point cloud obtained in step 4.
[0013] Furthermore, in step one, the coordinate value of the center point of the Aruco code in the camera coordinate system is first obtained, and then its three-dimensional coordinates in the laser radar coordinate system are obtained; after obtaining the coordinates of the center of the calibration plate in the respective coordinate systems of the camera and the laser radar, the point coordinates are converted to the laser radar coordinate system through coordinate transformation.
[0014] Furthermore, in step one, the following steps are included:
[0015] S11, place the calibration plate in the field of view of the system to be calibrated, and collect camera images and lidar point cloud data at the same time; use the OpenCV library to obtain the center point A of the Aruco code in the camera coordinate system O C X C Y C Z C The three-dimensional coordinates A C (x C ,y C ,z C );
[0016] S12, extract the four corner points of the calibration plate in the laser radar coordinate system O L X L Y L Z LThe three-dimensional coordinates of the center of the calibration plate in the laser radar coordinate system are obtained by weighted averaging. L (x L ,y L ,z L );
[0017] S13. By solving the following formula to obtain the rotation matrix R and translation vector t from the laser radar coordinate system to the camera coordinate system, the initial external parameter estimation of the camera and laser radar can be obtained:
[0018]
[0019] Expand the rotation and translation matrix in equation (1) to obtain a 3×4 matrix,
[0020]
[0021] After expanding equation (2), we can get three quaternary linear equations:
[0022]
[0023] Collect at least three sets of 3D-2D point pairs of the laser radar and camera recognition center point coordinates, and take the weighted average of the external parameter results as the external parameters of the laser radar and camera;
[0024] S14. Perform nonlinear optimization on the external parameters of the lidar and camera. Take the initially estimated external parameters as the initial guess. Under the nonlinear optimization framework, minimize the projection error of the feature points in the pixel coordinate system and the lidar coordinate system. Use the Levenberg-Marquardt iterative method for optimization.
[0025] Furthermore, it also includes verifying the calibration results after nonlinear optimization, assigning the color of the lidar point cloud in the common view area to the color of the corresponding pixels of the image, projecting it onto the camera image, and checking whether there is obvious deviation or inconsistency; if so, the parameters can be adjusted or the above steps can be repeated until there is no obvious deviation or complete consistency after the lidar point cloud is projected onto the camera image.
[0026] Furthermore, in step three, the following steps are included:
[0027] S31, network model training, input the detection training data set into the YOLO network model for training, extract features through the convolutional neural network, and when the category label is a category that may move, mark the detection box whose output confidence is greater than the threshold;
[0028] S32, image input, input the RGB image collected in step 2 into the YOLO network model;
[0029] S33, object detection. In the YOLO network model, the RGB image input in step S32 is judged to see whether there is a dynamic object. If so, the center and length and width of the bounding box of the dynamic object are output.
[0030] Furthermore, in step 4, the following steps are included:
[0031] S41, according to the center and length and width of the bounding box of the dynamic object output in step S33, and the image coordinates of the center of the dynamic object in the RGB image, searching the depth map for the depth value corresponding to the coordinate point;
[0032] S42, using the camera's intrinsic parameter matrix, calculate the coordinates P of the center point of the bounding box in the camera coordinate system C (x c ,y c ,z c ), the calculation formula is as follows,
[0033]
[0034] Among them, (x c ,y c ,z c ) is the coordinate in the camera coordinate system, (x p ,y p ) is the pixel coordinate, (f x ,f y ) is the focal length of the camera, (c x ,c y ) is the center point of the bounding box, z c is the depth value;
[0035] S43, the external parameters of the laser radar and camera obtained in step 1 are transformed into coordinates to obtain the center coordinates P of the dynamic object in the laser radar coordinate system L for,
[0036]
[0037] Furthermore, in step five, the following steps are included:
[0038] S51, according to the semantic labels on the point cloud in step 3, the point cloud belonging to the dynamic object is identified from the original point cloud by using an adaptive through-filtering algorithm;
[0039] S52, in step 4, obtain the coordinates P of the obstacle center in the laser radar coordinate system L (x L ,y L ,z L ), it is necessary to remove the point cloud of the cuboid direct-pass filter near the point;
[0040] S53, filtering out the clustered dynamic object point cloud from the original point cloud to obtain a static point cloud after the dynamic objects are removed, and the static point cloud is used for registration and mapping of the laser radar SLAM.
[0041] Furthermore, in the adaptive through-filtering algorithm, the boundary of the cuboid is set as follows:
[0042]
[0043] Where a, b, and c are the offsets of the boundary of the cuboid to be removed from the center in the x, y, and z directions, respectively, and k 1 , k 2 , k 3 It is the bounding box coefficient that is adaptively adjusted according to the size and distance of the box.
[0044] A dynamic obstacle fast point cloud removal system based on YOLO, including:
[0045] The laser radar and camera calibration module is used to use the laser radar and camera to simultaneously observe the unified calibration plate and solve the rotation matrix R and translation vector t through coordinate transformation;
[0046] Camera image and laser point cloud acquisition module, used to acquire synchronized image and point cloud data;
[0047] The dynamic object judgment module is used to judge the dynamic object through the YOLO network model and obtain the center, length and width of the dynamic object's bounding box;
[0048] A dynamic object point cloud acquisition module is used to project the dynamic object acquired by the dynamic object judgment module into the point cloud to obtain a dynamic object point cloud;
[0049] The dynamic object point cloud recognition and elimination module is used to identify the dynamic object point cloud from the original point cloud through adaptive straight-through filtering, and then eliminate the dynamic object point cloud.
[0050] A computer-readable storage medium stores a computer program, wherein the computer program is configured to implement the steps of a YOLO-based dynamic obstacle fast point cloud removal method when called by a processor.
[0051] Compared with the prior art, the present invention has the following beneficial effects:
[0052] The present invention discloses a method and system for fast point cloud removal of dynamic obstacles based on YOLO. The method obtains an external parameter matrix between a camera and a laser radar by calibrating a laser radar and a camera; obtains synchronized image and point cloud data by using a camera and a laser sensor; selects YOLOv5 to detect dynamic objects in an image and obtains coordinate information of their bounding boxes; projects the position information of the bounding boxes of the dynamic objects in the image into a point cloud to obtain a corresponding point cloud area; identifies the point cloud belonging to the dynamic object from the original point cloud according to the semantic label on the point cloud; and filters the clustered point cloud of the dynamic object from the original point cloud;
[0053] The present invention discloses a method and system for fast point cloud removal of dynamic obstacles based on YOLO, wherein YOLOv5 is selected to detect dynamic objects in an image, and interference of dynamic objects during laser radar scanning and sub-image registration is eliminated; the present invention proposes a calibration method for laser radar and camera, obtains an external parameter matrix between the camera and the laser radar, realizes good registration between the two sensors, and improves the accuracy of the entire system; in the process of removing the point cloud of dynamic obstacles, by projecting the bounding box of the dynamic object into the point cloud, considering the different sizes of pedestrians and the change of camera depth estimation accuracy with distance, the present invention proposes an adaptive straight-through filtering algorithm to remove the point cloud of pedestrians within the camera field of view, and effectively improves the positioning accuracy and environmental mapping quality of the SLAM algorithm in dynamic scenes. This method not only significantly reduces mismatching, improves map generation efficiency, but also reduces the amount of calculation and time, which is beneficial to subsequent positioning navigation and path planning. BRIEF DESCRIPTION OF THE DRAWINGS
[0054] Figure 1 Flow chart of a method for fast point cloud removal of dynamic obstacles based on YOLO in an embodiment of the present invention;
[0055] Figure 2 A schematic diagram of laser radar and camera calibration in an embodiment of the present invention;
[0056] Figure 3 This is a rendering of the corner point extraction of the laser radar calibration plate in an embodiment of the present invention;
[0057] Figure 4 It is an image-point cloud projection diagram in an embodiment of the present invention;
[0058] Figure 5 This is a diagram showing the effect of pedestrian point cloud removal in an embodiment of the present invention. DETAILED DESCRIPTION
[0059] In the description of the present invention, it should be noted that the terminology nouns in the various embodiments, such as "up", "down", "front", "back", "left", "right", etc., which indicate directions, are only for simplifying the description of the positional relationship based on the drawings in the specification, and do not mean that the referred elements and devices must be operated according to the specific directions and defined operations and methods and structures in the specification. Such directional nouns do not constitute a limitation on the present invention.
[0060] In the description of the present invention, it should be noted that the terms "first", "second", and "third" mentioned in the embodiments of the present invention are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first", "second", and "third" may explicitly or implicitly include one or more of the features.
[0061] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.
[0062] Specific implementation plan 1: Combine Figures 1 to 5 As shown, the present invention provides a method for fast point cloud removal of dynamic obstacles based on YOLO, comprising the following steps:
[0063] Step 1: Calibration of LiDAR and camera, such as Figure 2 As shown, the laser radar and camera are used to observe the same calibration plate at the same time, and at least three 2D-3D point pairs are obtained by extracting the feature points in the image and the corresponding points in the point cloud. Then, the rotation matrix R and the translation vector t from the laser radar coordinate system to the camera coordinate system are solved.
[0064] First, the coordinate value of the center point of the Aruco code in the camera coordinate system is obtained, and then its three-dimensional coordinates in the laser radar coordinate system are obtained; after obtaining the coordinates of the center of the calibration plate in the respective coordinate systems of the camera and laser radar, the point coordinates are converted to the laser radar coordinate system through coordinate transformation;
[0065] The specific steps are as follows:
[0066] S11. Place the calibration plate in the field of view of the system to be calibrated, and collect camera images and lidar point cloud data at the same time; using the OpenCV library, the camera can quickly and accurately recognize the Aruco code and obtain the center point A of the Aruco code in the camera coordinate system O. C X C Y C Z C The three-dimensional coordinates A C (x C ,y C ,z C );
[0067] S12. Use CloudCompare software to extract the four corner points of the calibration plate in the laser radar coordinate system O L X L Y L Z L The three-dimensional coordinates of the center of the calibration plate in the laser radar coordinate system are obtained by weighted averaging. L (x L ,y L ,z L ), the corner point extraction effect of the laser radar calibration plate is shown in the figure Figure 3 As shown;
[0068] S13. By solving the following formula to obtain the rotation matrix R and translation vector t from the laser radar coordinate system to the camera coordinate system, the initial external parameter estimation of the camera and laser radar can be obtained:
[0069]
[0070] Expand the rotation and translation matrix in equation (1) to obtain a 3×4 matrix,
[0071]
[0072] After expanding equation (2), we can get three quaternary linear equations:
[0073]
[0074] Collect at least three sets of 3D-2D point pairs of the laser radar and camera recognition center point coordinates, and take the weighted average of the external parameter results as the external parameters of the laser radar and camera;
[0075] S14, nonlinearly optimizing the external parameters of the laser radar and the camera, taking the initially estimated external parameters as the initial guess, minimizing the projection error of the feature points in the pixel coordinate system and the laser radar coordinate system under the nonlinear optimization framework, and optimizing by using the Levenberg-Marquardt iterative method; the Levenberg-Marquardt iterative method is an existing iterative method and will not be described in detail here;
[0076] S15. Verify the calibration result after nonlinear optimization, assign the color of the laser radar point cloud in the common view area to the corresponding pixel of the image, and project it onto the image of the camera, such as Figure 4 Check if there is any obvious deviation or inconsistency. If there is, adjust the parameters or repeat the above steps until there is no obvious deviation or the laser radar point cloud is completely consistent after being projected onto the camera image.
[0077] Step 2: Obtain camera images and laser point clouds, using cameras and laser sensors to obtain synchronized image and point cloud data;
[0078] The image acquisition tool is Microsoft Azure Kinect DK, whose depth camera has a 1-megapixel ToF, and the RGB camera has a 12-megapixel rolling shutter CMOS sensor. The point cloud acquisition tool is DJI Livox Mid-360 hybrid solid-state laser radar, which has a horizontal field of view of 360° and has both repeated scanning and non-repeated scanning technology.
[0079] Step 3: Detect dynamic objects in the RGB image captured by the camera in step 2, use YOLOv5 to detect dynamic objects in the image, and obtain the coordinate information of their bounding boxes;
[0080] The specific steps are as follows:
[0081] S31, network model training, input the detection training data set into the YOLO network model for training, extract features through the convolutional neural network, and when the category label is a possible moving category, such as vehicles, pedestrians, bicycles, etc., mark the detection box whose output confidence is greater than the threshold;
[0082] S32, image input, input the RGB image collected in step 2 into the YOLO network model;
[0083] S33, object detection, in the YOLO network model, the RGB image input in step S32 is judged to see whether there is a dynamic object, and if so, the center and length and width of the bounding box of the object are output;
[0084] Step 4: Project the dynamic object determined in step 3 to the point cloud, and use the internal and external parameters of the camera and laser sensor to project the dynamic object bounding box position information in the RGB image to the point cloud. The bounding box position information is the center and length and width of the bounding box in step S33, and obtain the corresponding point cloud;
[0085] The specific steps are as follows:
[0086] S41, searching the depth map for a depth value corresponding to the coordinate point according to the center and length and width of the bounding box of the object output in step S33, and the image coordinates of the center of the bounding box in the RGB image;
[0087] S42, using the camera's intrinsic parameter matrix, calculate the coordinates P of the center point of the bounding box in the camera coordinate system C (x c ,y c ,z c ), the calculation formula is as follows,
[0088]
[0089] Among them, (x c ,y c ,z c ) is the coordinate in the camera coordinate system, (x p ,y p ) is the pixel coordinate, (f x ,f y ) is the focal length of the camera, (c x ,c y ) is the center point of the bounding box, z c is the depth value;
[0090] S43, the external parameters of the laser radar and camera obtained in step 1 are transformed into coordinates to obtain the center coordinates P of the dynamic object in the laser radar coordinate system L for,
[0091]
[0092] Step 5: Identify and remove the dynamic object point cloud obtained in step 4;
[0093] The specific steps include:
[0094] S51, according to the semantic label on the point cloud in step 3, the point cloud belonging to the dynamic object is identified from the original point cloud; this process is achieved by adaptive straight-through filtering;
[0095] S52, in step 4, obtain the coordinates P of the obstacle center in the laser radar coordinate system L (x L ,y L ,z L ), it is necessary to remove the point cloud within a certain range near the point; Figure 5 As shown in the figure, the point cloud with a larger radius is the point cloud on the obstacle in the current frame. Since most of the indoor obstacles are pedestrians, the point cloud is generally in the shape of an upright column. Therefore, when searching the point cloud in the neighborhood based on the center of the human body, the static points in the environment are eliminated as little as possible. The rectangular straight-through filter fits the human body better than the radius spherical filter, which can minimize the number of eliminated point clouds.
[0096] Considering the different sizes of human bodies and the variation of camera depth estimation accuracy with distance, the present invention eliminates an adaptive straight-through filtering algorithm to eliminate the point cloud of the human body within the camera field of view.
[0097] The boundaries of the cuboid are set as follows:
[0098]
[0099] Where a, b, and c are the offsets of the boundary of the cuboid to be removed from the center in the x, y, and z directions, respectively, and k 1 , k 2 , k 3 is the bounding box coefficient that is adaptively adjusted according to the size and distance of the box;
[0100] S53, filtering out the clustered dynamic object point cloud from the original point cloud to obtain a static point cloud after removing the dynamic objects, and using the static point cloud for laser radar SLAM registration and mapping.
[0101] Specific implementation plan 2: Combined Figures 1 to 5 As shown, the present invention provides a dynamic obstacle fast point cloud removal system based on YOLO, comprising:
[0102] The laser radar and camera calibration module is used to use the laser radar and camera to simultaneously observe the unified calibration plate and solve the rotation matrix R and translation vector t through coordinate transformation;
[0103] Camera image and laser point cloud acquisition module, used to acquire synchronized image and point cloud data;
[0104] The dynamic object judgment module is used to judge the dynamic object through the YOLO network model and obtain the center, length and width of the dynamic object's bounding box;
[0105] A dynamic object point cloud acquisition module is used to project the dynamic object acquired by the dynamic object judgment module into the point cloud to obtain a dynamic object point cloud;
[0106] The dynamic object point cloud recognition and elimination module is used to identify the dynamic object point cloud from the original point cloud through adaptive straight-through filtering, and then eliminate the dynamic object point cloud.
[0107] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.
[0108] Specific implementation scheme three: The present invention provides a computer-readable storage medium, which stores a computer program, and the computer program is configured to implement the steps of a YOLO-based dynamic obstacle fast point cloud removal method when called by a processor.
[0109] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.
[0110] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.
Claims
1. A fast point cloud removal method for dynamic obstacles based on YOLO, It is characterized in that The following steps are involved: Step 1: Calibrate the laser radar and camera. Use the laser radar and camera to observe the same calibration plate at the same time. By extracting the feature points in the image and the corresponding points in the point cloud, at least three 2D-3D point pairs are obtained. Then, the rotation matrix R and translation vector t from the laser radar coordinate system to the camera coordinate system are solved, and the external parameter matrix between the camera and the laser radar is obtained. Step 2: Obtain camera images and laser point clouds, using cameras and laser sensors to obtain synchronized image and point cloud data; Step 3: Detect dynamic objects in the RGB image captured by the camera in step 2, use YOLOv5 to detect dynamic objects in the image, and obtain the coordinate information of their bounding boxes; Step 4: Project the dynamic object determined in step 3 to the point cloud, and use the internal and external parameters of the camera and laser sensor to project the position information of the dynamic object bounding box in the RGB image to the point cloud to obtain the corresponding point cloud; Step 5: Use the adaptive through-filter algorithm to identify and remove the dynamic object point cloud obtained in step 4. include, S51, according to the semantic labels on the point cloud in step 3, the point cloud belonging to the dynamic object is identified from the original point cloud by using an adaptive straight-through filtering algorithm; S52, in step 4, obtain the coordinates P of the obstacle center in the laser radar coordinate system L (x L ,y L ,z L ), it is necessary to remove the coordinate P L (x L ,y L ,z L ) Point cloud of the cuboid direct filter near ; S53, filtering out the clustered dynamic object point cloud from the original point cloud to obtain a static point cloud after the dynamic objects are removed, and the static point cloud is used for registration and mapping of the laser radar SLAM.
2. According to claim 1, a method for fast point cloud removal of dynamic obstacles based on YOLO, Features: In step one, first obtain the coordinate value of the center point of the Aruco code in the camera coordinate system, and then obtain its three-dimensional coordinates in the laser radar coordinate system; after obtaining the coordinates of the center of the calibration plate in the respective coordinate systems of the camera and laser radar, convert the point coordinates to the laser radar coordinate system through coordinate transformation.
3. According to claim 2, a method for fast point cloud removal of dynamic obstacles based on YOLO, It is characterized in that In step one, the following steps are included: S11, place the calibration plate in the field of view of the system to be calibrated, and collect camera images and lidar point cloud data at the same time; use the OpenCV library to obtain the center point A of the Aruco code in the camera coordinate system O C X C Y C Z C The three-dimensional coordinates A C (x C ,y C ,z C ); S12, extract the four corner points of the calibration plate in the laser radar coordinate system O L X L Y L Z L The three-dimensional coordinates of the center of the calibration plate in the laser radar coordinate system are obtained by weighted averaging. L (x L ,y L ,z L ); S13. By solving the following formula to obtain the rotation matrix R and translation vector t from the laser radar coordinate system to the camera coordinate system, the initial external parameter estimation of the camera and laser radar can be obtained: Expand the rotation and translation matrix in equation (1) to obtain a 3×4 matrix, After expanding equation (2), we can get three quaternary linear equations: Collect at least three sets of 2D-3D point pairs of the laser radar and camera recognition center coordinates, and take the weighted average of the external parameter results as the external parameters of the laser radar and camera; S14. Perform nonlinear optimization on the external parameters of the lidar and camera. Take the initially estimated external parameters as the initial guess. Under the nonlinear optimization framework, minimize the projection error of the feature points in the pixel coordinate system and the lidar coordinate system. Use the Levenberg-Marquardt iterative method for optimization.
4. According to claim 3, a method for fast point cloud removal of dynamic obstacles based on YOLO, Features: It also includes verifying the calibration results after nonlinear optimization, assigning the color of the lidar point cloud in the common view area to the color of the corresponding pixels of the image, projecting it onto the camera image, and checking whether there is obvious deviation or inconsistency; if so, adjust the parameters or repeat the above steps until there is no obvious deviation or complete consistency after the lidar point cloud is projected onto the camera image.
5. According to the method for fast point cloud removal of dynamic obstacles based on YOLO in claim 4, It is characterized in that In step three, the following steps are included: S31, network model training, input the detection training data set into the YOLO network model for training, extract features through the convolutional neural network, and when the category label is a category that may move, mark the detection box whose output confidence is greater than the threshold; S32, image input, input the RGB image collected in step 2 into the YOLO network model; S33, object detection. In the YOLO network model, the RGB image input in step S32 is judged to see whether there is a dynamic object. If so, the center and length and width of the bounding box of the dynamic object are output.
6. According to claim 5, a method for fast point cloud removal of dynamic obstacles based on YOLO, It is characterized in that In step 4, the following steps are included: S41, according to the center and length and width of the bounding box of the dynamic object output in step S33, and the image coordinates of the center of the dynamic object in the RGB image, searching the depth map for the depth value corresponding to the image coordinates; S42, using the camera's intrinsic parameter matrix, calculate the coordinates P of the center point of the bounding box in the camera coordinate system C (x c ,y c ,z c ), the calculation formula is as follows, Among them, (x c ,y c ,z c ) is the coordinate in the camera coordinate system, (x p ,y p ) is the pixel coordinate, (f x ,f y ) is the focal length of the camera, (c x ,c y ) is the center point of the bounding box, z c is the depth value; S43, the external parameters of the laser radar and camera obtained in step 1 are transformed into coordinates to obtain the center coordinates P of the dynamic object in the laser radar coordinate system L for, 7. According to claim 6, a method for fast point cloud removal of dynamic obstacles based on YOLO, Features: In the adaptive through-filtering algorithm, the boundary of the cuboid is set as follows: Where a, b, and c are the offsets of the boundary of the cuboid to be removed from the center in the x, y, and z directions, respectively, and k 1 , k 2 , k 3 It is the bounding box coefficient that is adaptively adjusted according to the size and distance of the box.
8. A dynamic obstacle fast point cloud removal system based on YOLO, Features: include, The laser radar and camera calibration module is used to use the laser radar and camera to observe the same calibration plate at the same time, solve the rotation matrix R and translation vector t through coordinate transformation, and then obtain the external parameter matrix between the camera and the laser radar; Camera image and laser point cloud acquisition module, used to acquire synchronized image and point cloud data; The dynamic object judgment module is used to judge the dynamic object through the YOLO network model and obtain the center, length and width of the dynamic object's bounding box; A dynamic object point cloud acquisition module is used to project the dynamic object acquired by the dynamic object judgment module into the point cloud to obtain a dynamic object point cloud; The dynamic object point cloud recognition and elimination module is used to identify the dynamic object point cloud from the original point cloud through adaptive straight-through filtering, and then eliminate the dynamic object point cloud. include, S51, according to the semantic labels on the point cloud acquired by the dynamic object point cloud acquisition module, the point cloud belonging to the dynamic object is identified from the original point cloud by using an adaptive straight-through filtering algorithm; S52, the coordinate P of the obstacle center in the laser radar coordinate system L (x L ,y L ,z L ), it is necessary to remove the coordinate P L (x L ,y L ,z L ) Point cloud of the cuboid direct filter near ; S53, filtering out the clustered dynamic object point cloud from the original point cloud to obtain a static point cloud after the dynamic objects are removed, and the static point cloud is used for registration and mapping of the laser radar SLAM.
9. A computer-readable storage medium, Features: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of the YOLO-based dynamic obstacle fast point cloud removal method described in any one of claims 1 to 7 when called by a processor.
Citation Information
Patent Citations
Three-dimensional obstacle detection system and method applied to low-speed autonomous driving robot
CN113743391A
Mapping method and device of high-precision semantic map and unmanned vehicle
CN115372990A
Dynamic environment-oriented camera and solid-state laser radar fusion repositioning method
CN115718303A
Environmental map generating device, environmental map generating method, and program
JP7254222B1
Multi-target detection and tracking method, system, storage medium and application
US20220309835A1
Cited By
Laser SLAM mapping positioning method and system in dynamic environment
CN120101768A
