Multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping

Abstract

The invention discloses a multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping, and belongs to the field of environment dynamic target recognition. A laser radar and a camera are fused to establish a multi-sensor system, and the multi-sensor system is applied to an inspection robot or a logistics robot and the like and used in an outdoor unstructured environment. The method comprises the following steps: inputting image information collected by a camera into a semantic segmentation network to obtain a segmentation map marked with scene semantics and each barrier segmentation confidence coefficient, dividing dynamic possibilities of instances in a scene according to a training result, and dividing barrier categories according to the dynamic confidence coefficients; meanwhile, point cloud data of the radar is subjected to viewpoint visibility detection, and the point cloud data is divided into a dynamic point set and a static point set; and finally, combining a confidence division result and a viewpoint visibility detection result for further judgment, and reserving a static point cloud set as a final map. According to the method, the precision of dynamic point filtering is improved, the detection accuracy is enhanced, the environmental adaptability is high, and the requirement for the environment is low.

Description

Technical Field

[0001] The present invention relates to the field of environmental dynamic target recognition, and particularly to a dynamic point filtering method, specifically to an environmental dynamic point filtering method for outdoor non-structured environments, especially environments with a large number of dynamic objects, which utilizes the fusion of laser radar viewpoint visibility and depth camera semantic segmentation, and is mainly used for robot positioning, autonomous mapping and robot navigation in non-structured and dynamic environments. Background Art

[0002] With the popularization of automation and intelligence, smart products such as drones, driverless cars, and smart robots are gradually entering human life, and more and more industries are increasingly demanding autonomous navigation systems. Most traditional robot autonomous localization and mapping (SLAM) methods are based on the assumption of a static environment. Distinguishing obstacles from traversable sections usually relies on identifying feature points in the scene. The movement of dynamic objects may cause blurring or occlusion of feature points, making feature extraction unstable, or unable to distinguish between static backgrounds and dynamic obstacles, resulting in positioning errors. Therefore, improving the robustness and accuracy of positioning and mapping in outdoor dynamic scenes is one of the challenges currently faced by robot autonomous localization and mapping technology. Most of the dynamic point filtering solutions proposed in recent years are based on a single sensor.

[0003] The method based on LiDAR sensor usually constructs a grid map, (CN117031491A) projects each frame of point cloud into the grid map according to the pose, and judges whether the laser obstruction is dynamic based on the probability of the grid being penetrated and hit by the laser light path. This method has a high probability of mistakenly deleting static obstacles, and the accuracy of dynamic point filtering is low. (CN117289297A) Assuming that all dynamic objects are located on the ground, a clustering method based on concentric circle segmentation is proposed to eliminate dynamic points above 0.5m from the ground. This method is less effective for low-dynamic objects on the ground in outdoor unstructured environments.

[0004] Dynamic object removal based on visual sensors is mainly divided into geometry-based methods (CN116563340A) and optical flow-based methods (CN111950561A, CN116129110A, CN116664631A, CN117523190A). Geometry-based methods generally have the problem of large amount of calculation, and optical flow-based methods generally have the problem of high requirements for environmental texture and lighting stability. The visual method combining feature points and sparse optical flow (CN118261943A) overcomes the requirement of optical flow method for unchanged texture and ambient light, but the effect is poor in dark outdoor environments.

[0005] In addition, there are also a few dynamic point removal methods that combine multiple sensors. Some methods combine IMU with camera (CN114199259A), obtain feature points through semantic segmentation, and then combine IMU pre-integration to detect dynamic points in feature points that are different from the robot's posture change and remove them. This method mainly collects environmental data based on cameras, and also has the problem of single camera being sensitive to light. Summary of the invention

[0006] The purpose of the present invention is to overcome the problems of low precision of existing laser dynamic point filtering methods and high environmental requirements of visual dynamic point filtering methods, and it is difficult to operate stably under outdoor conditions. A multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping is provided, which fuses the information of laser radar and depth camera and is used in outdoor unstructured environments, especially in environments with a large number of dynamic objects. The multimodal scene dynamic point filtering method solves the problem that environmental dynamic points are difficult to filter out in outdoor unstructured environments.

[0007] A multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping is divided into the following steps:

[0008] Step 1: Establish a multi-sensor system based on the robot, place it in an outdoor dynamic environment, and collect environmental information;

[0009] The multi-sensor system includes a visual sensor and a lidar sensor equipped on the robot, wherein the visual sensor is a camera.

[0010] Environmental information includes raw point cloud data generated by the lidar sensor and image information recorded by the camera.

[0011] The multi-sensor system is jointly calibrated to establish the corresponding model and coordinate system, specifically:

[0012] (1) The robot moves on a two-dimensional ground, so the robot posture has 3 degrees of freedom. The robot homogeneous transformation matrix is:

[0013]

[0014] Among them, (x 1 ,y 1 ) is the two-dimensional coordinate of the robot, α is the rotation angle of the robot;

[0015] (2) The camera modeling process is:

[0016] The three-dimensional point in the camera coordinate system c a is projected onto the image plane to form a two-dimensional pixel u t =[u,v] T :

[0017]

[0018] Among them, π(·) is the camera projection model, f x 、f y is the normalized focal length, c x 、c y is the principal point coordinate, (x 2 ,y 2 ,z 2 ) is a three-dimensional point c a is the coordinate of the camera coordinate system. Perform inverse transformation of the projection model to project the two-dimensional pixel coordinates to the three-dimensional space coordinates:

[0019]

[0020] (3) The process of converting the target point coordinate system of the laser radar to the three-dimensional space coordinate system is as follows:

[0021] P'=R x (γ)R y (θ)R z (φ)×P+P 0

[0022] Among them, P 0 represents the translation transformation of the target coordinate system relative to the three-dimensional space coordinate system; P represents the coordinate of the target point in the target coordinate system; P' represents the coordinate of the target point in the three-dimensional space coordinate system; φ, θ, γ represent the yaw angle, pitch angle, and roll angle of the target relative to the origin of the radar coordinate system, respectively; R x , R y , R z Represents the rotation coordinate transformation matrix of yaw angle, pitch angle, and roll angle respectively; the rotation coordinate transformation matrix R of the target tg The calculation formula is:

[0023]

[0024] Step 2: Input the image information collected by the camera into the semantic segmentation network to obtain the segmentation map of the environment scene semantics and the segmentation confidence of each obstacle, and divide the dynamic possibility of the instances in the scene according to the training results.

[0025] Select key frame images from the image information collected by the camera, and perform object detection and semantic segmentation on the key frame images simultaneously in a parallel architecture:

[0026] (1) The specific process of target detection is as follows: input the key frame image information into the neural network, obtain the hyperparameter label corresponding to each image, perform a mIoU (mean intersection over union) test on the obtained anchor frame results, and perform a non-maximum test, retain the anchor frame and its image information with the highest test probability, and complete the target detection of the key frame image.

[0027] The hyperparameter label of the image is: Split the key frame image into m rows and m columns, and get m 2 squares, each with 2 anchor boxes, then specify the hyperparameter label l for the image:

[0028]

[0029] Among them, p c The true or false value indicates whether there is an object in the anchor frame; b x 、b y represents the coordinates of the center point of the anchor box; b h 、b w Indicates the height and width of the anchor box; c 1 c...c n The value of represents the confidence that the object in the anchor box is the specified object.

[0030] The intersection-over-union ratio is the ratio of the intersection of the predicted image set and the true annotation image set to the union of these two sets. In image semantic segmentation, it indicates the degree of overlap between the segmentation image in the anchor box and the true value of the original image. The calculation method of the intersection-over-union ratio is:

[0031]

[0032] The average intersection-and-union ratio is the average value of the intersection-and-union ratio of each class of image pixels:

[0033]

[0034] Among them, i represents the true value, j represents the predicted value; p ij It means that i is predicted as j, which is a false negative; p ji It means that j is predicted as i, which is a false positive; p ii It means that i is predicted as i, which is true; k represents the total number of segmentation categories in the anchor box.

[0035] (2) The specific process of semantic segmentation is: perform feature detection on the key frame image information and output n instance prototype image segmentation results, which correspond to the target detection results respectively.

[0036] Combine the results of the parallel processes, use the instance prototype and its specified object confidence to perform a linear combination, and pass the sigmoid function σ to generate the final segmentation result. The calculation formula is:

[0037] M=σ(DC T )

[0038] Where D is the h×ω×g instance prototype parameter matrix, h, ω, and g represent the length, width, and height of the parameter matrix, respectively, and C is the n×k confidence parameter matrix of n instance prototypes that pass the intersection-over-union test and non-maximum suppression.

[0039] Use the predicted bounding box to crop the final instance, color different instances separately, and output object detection and semantic segmentation results simultaneously.

[0040] According to the content of different training scenes, the dynamic probability of using supervised methods to segment the results is β 1 , β 2 ,......,β n For example, the possibility of static objects such as trees, houses, and the ground being dynamic is zero, that is, let β 静物 = 0; pedestrians and other creatures must be dynamic during the mapping process, that is, let β 生物 = 1. Objects with dynamic possibilities such as vehicles are divided into dynamic possibilities according to the scene training results. The dynamic possibility of the instance is assigned.

[0041] In step three, the dynamic confidence of the instance in the scene is obtained according to its dynamic possibility, and the obstacle category is divided according to the dynamic confidence. If it is a dynamic or static obstacle, the point cloud data is directly removed or retained; otherwise, step four is executed to perform viewpoint visibility detection on the instance in the scene.

[0042] Define the dynamic confidence δ as:

[0043] δ=f(C,β)

[0044] Among them, C is the confidence parameter matrix obtained by the semantic segmentation process, and β is the dynamic possibility given by the supervised method.

[0045] When the dynamic confidence is lower than the lower threshold, that is, δ≤δ min , the instance is classified as an obstacle without dynamic possibility, and its point cloud is retained as a static point cloud; the dynamic confidence is higher than the upper threshold δ≥δ max When , the instance is classified as a high dynamic possibility obstacle, and the obstacle point cloud is removed as a whole. Dynamic confidence threshold δ min ≤δ≤δ max , the instance is classified as a medium dynamic possibility obstacle and enters the viewpoint visibility detection.

[0046] Step 4: perform viewpoint visibility detection on the point cloud data collected by the lidar sensor, and divide the point cloud data into a dynamic point set and a static point set;

[0047] The specific process is as follows:

[0048] Step 401: The inspection robot scans the environment N times continuously through the laser radar sensor to obtain N groups of original point cloud scan frames, from which the e-th scan frame is selected. And several adjacent frame point clouds, several adjacent frame point clouds are spliced ​​into a local point cloud map, denoted as P e M ;

[0049] Step 402, comparison With P e M , will soon P e M Converted into depth images respectively and recorded as I e Q ,I e M ; Depth Image I e M The pixel values ​​and point cloud maps are:

[0050]

[0051] Similarly, I e Q The pixel value of .

[0052] Among them, r(p) represents the three-dimensional space coordinates of the point cloud in the key frame; Represents the pixel value with index (I,J) in the depth image; P M A subset of , representing the set of all points in the spherical coordinate system that fall within the corresponding (I,J) index.

[0053] Will I e Q ,I e M Subtract the pixel values:

[0054]

[0055] If I e M The depth of a pixel in is less than I e Q The depth of the corresponding pixel in the local point cloud map indicates that the corresponding point in the scan frame occludes the point in the scan frame, that is, I e M This point in is dynamic. The final dynamic point cloud map is defined as:

[0056]

[0057] Among them, τ DIt is the threshold value of the algorithm’s sensitivity coefficient to point distance.

[0058] The static point cloud map is defined as the difference between the local point cloud map and the dynamic point cloud map, that is,

[0059]

[0060] Step 403, traverse the scanned frames of N scans, perform dynamic map point detection based on the range image for each scanned frame, and count the total number of map points marked as SM (static) or DM (dynamic), which are recorded as n respectively. SM With n DM .

[0061] Step 404, dynamic map P DM ∈P M Use n SM With n DM The weighted function of , that is, the static degree scoring function s(·), is defined, and the final result of the static map is the complement of the dynamic map. The dynamic map and the static map are:

[0062] P DM ={p M |s(p M )<τ s}

[0063] P SM =P M -P DM

[0064] Among them, τ s is the threshold of the static score function of the dynamic map, and the static score of a single 3D point is s(·)=α SM n SM (p M )+α DM n DM (p M ), where α SM and α DM is the positive and negative weight coefficient.

[0065] Step 405, compare the multi-resolution depth map to restore the misidentified ground points, that is, reduce I Q ,I M The resolution is re-compared to restore the misidentified points to the static point set. Repeat until all the set resolution maps are completed.

[0066] Step 5: Combining the dynamic confidence obtained by semantic segmentation with the dynamic nature of the point cloud set obtained by viewpoint visibility detection, further determine the dynamic point set cooperation and retain the static point cloud set as the final map.

[0067] For the pixels in the dynamic point set, calculate their overall static score:

[0068]

[0069] Multiply the overall static score by the dynamic possibility parameter β obtained from the semantic segmentation result to obtain the object dynamic estimate:

[0070] ε(·)=β×S(·)

[0071] If the dynamic estimation value exceeds the threshold ε(max), the object blocking the light path in the map is considered to be a dynamic obstacle and is removed; otherwise, the point is retained and added to the static point set.

[0072] After all judgments are completed, the final static point cloud collection is a static map and serves as the final map.

[0073] The advantages and positive effects of the present invention are:

[0074] 1. This dynamic point filtering method combines lidar and camera information. For outdoor scenes with poor light quality and cameras that cannot work properly, most scenes will be directly input into the lidar viewpoint visibility processing system to continue working, and dynamic point filtering can also be performed. It has strong environmental adaptability and low environmental requirements.

[0075] 2. There are a large number of objects outside the data set in outdoor unstructured scenes. This dynamic point filtering method introduces a parameter called dynamic confidence. To solve the problem that the semantic segmentation of pure visual methods cannot identify objects outside the data set, the present invention inputs the objects that cannot be identified or have a low recognition success rate into the viewpoint visibility detection module to directly determine their dynamic confidence. This confidence-based judgment method can flexibly handle different types of dynamic objects and adjust the dynamic object filtering strategy according to the actual scene conditions, thereby improving the accuracy of dynamic point filtering.

[0076] 3. For the problem of easy misjudgment when using the viewpoint visibility method in the pure laser dynamic point filtering solution, the present invention first uses semantic segmentation to identify purely static obstacles and purely dynamic obstacles, reducing the probability of static obstacle point clouds being misjudged. Secondly, multi-resolution images are used to implement multiple viewpoint visibility dynamic detections, first deleting and then restoring, to further enhance the detection accuracy.

[0077] 4. The present invention does not require a priori scene maps and is suitable for outdoor unstructured scene positioning, mapping and navigation.

[0078] 5. The present invention has no special requirements on the robot posture and is versatile and universal. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 Schematic diagram of the overall process of the dynamic point filtering solution. DETAILED DESCRIPTION

[0080] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0081] This paper proposes a multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping, which is particularly suitable for accurate filtering of dynamic objects in outdoor unstructured environments. This method combines lidar and visual camera information, and through the collaborative work of multiple sensors, accurately identifies and processes dynamic obstacles, thereby improving the accuracy and robustness of environmental mapping.

[0082] A multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping, such as Figure 1 As shown, the specific steps are as follows:

[0083] Step 1: Model building and information collection. Build a multi-sensor system based on the robot, place it in an outdoor dynamic environment, and collect environmental information;

[0084] The multi-sensor system includes a visual sensor and a laser radar sensor equipped on the robot, wherein the visual sensor is a camera. The camera and the laser radar are jointly calibrated to establish their models and coordinate systems. For distant objects, the laser radar is used to generate the original point cloud. For close objects, the camera is used to record its key frame image information.

[0085] The robot is modeled and its multi-sensor system is jointly calibrated to establish the corresponding model and coordinate system. Specifically:

[0086] (1) The robot moves on a two-dimensional ground, so the robot posture has 3 degrees of freedom. The robot homogeneous transformation matrix is:

[0087]

[0088] Among them, (x 1 ,y 1 ) is the two-dimensional coordinate of the robot, α is the rotation angle of the robot;

[0089] (2) The camera modeling process is:

[0090] The three-dimensional point in the camera coordinate system c a is projected onto the image plane to form a two-dimensional pixel u t =[u,v] T :

[0091]

[0092] Among them, π(·) is the camera projection model, f x 、f yis the normalized focal length, c x 、c y is the principal point coordinate, (x 2 ,y 2 ,z 2 ) is a three-dimensional point c a is the coordinate of the camera coordinate system. Perform inverse transformation of the projection model to project the two-dimensional pixel coordinates to the three-dimensional space coordinates:

[0093]

[0094] (3) The process of converting the target point coordinate system of the laser radar to the three-dimensional space coordinate system is as follows:

[0095] P'=R x (γ)R y (θ)R z (φ)×P+P 0

[0096] Among them, P 0 represents the translation transformation of the target coordinate system relative to the three-dimensional space coordinate system; P represents the coordinate of the target point in the target coordinate system; P' represents the coordinate of the target point in the three-dimensional space coordinate system; φ, θ, γ represent the yaw angle, pitch angle, and roll angle of the target relative to the origin of the radar coordinate system, respectively; R x , R y , R z Represents the rotation coordinate transformation matrix of yaw angle, pitch angle, and roll angle respectively; the rotation coordinate transformation matrix R of the target tg The calculation formula is:

[0097]

[0098] Step 2: Semantic segmentation. The image information collected by the camera is input into the semantic segmentation network to obtain a segmentation map with the semantics of the scene and the segmentation confidence of each obstacle, and the dynamic possibility of the instance in the scene is divided according to the training results.

[0099] First, select the key frame and preprocess the key frame capture image. Divide the key frame image into m rows and m columns of squares, each with 2 anchor boxes. Specify the hyperparameter training label l for the image:

[0100]

[0101] Among them, p c The true or false value indicates whether there is an object in the anchor frame; b x 、b y represents the coordinates of the center point of the anchor box; b h 、b w Indicates the height and width of the anchor box; c 1 ,…,cn The value of represents the confidence that the object in the anchor box is the specified object.

[0102] Simultaneously perform object detection and semantic segmentation on keyframe images in a parallel architecture.

[0103] The specific process of target detection is as follows: input the key frame image information into the neural network to obtain the hyperparameter label corresponding to each image. Perform mIoU (mean intersection over union) test on the obtained anchor frame results.

[0104] The intersection-over-union ratio is the ratio of the intersection of the predicted image set and the true annotated image set to the union of these two sets. In image semantic segmentation, it often indicates the degree of overlap between the segmentation image in the anchor box and the true value of the original image. The calculation method of the intersection-over-union ratio is:

[0105]

[0106] The average intersection-and-union ratio is the average value of the intersection-and-union ratio of each class of image pixels:

[0107]

[0108] Among them, i represents the true value, j represents the predicted value; p ij It means that i is predicted as j, which is a false negative; p ji It means that j is predicted as i, which is a false positive; p ii It means that i is predicted as i, which is true; k represents the total number of segmentation categories in the anchor box.

[0109] Calculate the IoU of the anchor frames respectively, and calculate the average IoU of the image. Perform a non-maximum test, retain the anchor frame with the highest detection probability and its information, and complete the target detection of the key frame image.

[0110] The specific process of semantic segmentation is: perform feature detection on the key frame image, and output n instance prototype image segmentation results, which correspond to the target detection results respectively.

[0111] The results of the parallel processes are combined, using the linear combination of the instance prototype and its specified object confidence as the coefficient. The calculation formula is:

[0112] M=σ(QC T )

[0113] Among them, Q is the instance prototype parameter matrix of h×ω×g, h, ω, and g represent the length, width, and height of the parameter matrix respectively; C is the n×k confidence parameter matrix of n instances that pass the intersection-over-union test and non-maximum suppression.

[0114] Use the predicted bounding box to crop the final instance, color different instances separately, and output object detection and semantic segmentation results simultaneously.

[0115] According to the content of different training scenes, the dynamic probability of using supervised methods to segment the results is β 1 , β 2 ,......,β n For example, the possibility of static objects such as trees, houses, and the ground being dynamic is zero, that is, let β 静物 = 0; pedestrians and other creatures must be dynamic during the mapping process, that is, let β 生物 = 1. Objects with dynamic possibilities such as vehicles are divided into dynamic possibilities according to the scene training results. The dynamic possibility of the instance is assigned.

[0116] Step 3: Dynamic confidence determination: According to the dynamic confidence of the object, decide whether to perform viewpoint visibility detection on it or directly retain it as a static or dynamic point cloud.

[0117] Let the scene dynamic confidence δ = f(C, β), where C is the confidence parameter matrix obtained by the semantic segmentation process, and β is the dynamic possibility given by the supervised method.

[0118] When the dynamic confidence is lower than the lower threshold, that is, δ≤δ min , the instance is classified as an obstacle without dynamic possibility, and its point cloud is retained as a static point cloud; the dynamic confidence is higher than the upper threshold δ≥δ max When , the instance is classified as a high dynamic possibility obstacle, and the obstacle point cloud is removed as a whole. Dynamic confidence threshold δ min ≤δ≤δ max , the instance is classified as a medium dynamic possibility obstacle and enters the viewpoint visibility detection.

[0119] Step 4: Viewpoint visibility detection: Perform viewpoint visibility detection on the point cloud data collected by the lidar sensor and divide the point cloud data into a dynamic point set and a static point set.

[0120] The static score of a pixel is calculated by calculating the difference between the range image of the query local map and the range image of the overall map. If the difference is greater than a certain threshold, the corresponding point is considered dynamic. The specific process is:

[0121] According to the principle of viewpoint visibility, a set of original point cloud scan frames are first collected by the LiDAR sensor, and several adjacent frame point clouds are selected to be stitched into a local point cloud map without removing dynamic objects. Let its nearby submap be P e M .contrast With P e M , will soon P eM Converted into depth images respectively and recorded as I e Q ,I e M ; Depth Image I e M The pixel values ​​and point cloud maps are:

[0122]

[0123] Similarly, I e Q The pixel value of .

[0124] Among them, r(p) represents the three-dimensional space coordinates of the point cloud in the key frame; Represents the pixel value with index (I,J) in the depth image; P M A subset of , representing the set of all points in the spherical coordinate system that fall within the corresponding (I,J) index.

[0125] Will I e Q ,I e M Subtract the pixel values:

[0126]

[0127] If I e M The depth of a pixel in is less than I e Q The depth of the corresponding pixel in the local point cloud map indicates that the corresponding point in the scan frame occludes the point in the scan frame, that is, I e M This point in is dynamic. The final dynamic point cloud map is defined as:

[0128]

[0129] Among them, τ D It is the threshold value of the algorithm’s sensitivity coefficient to point distance.

[0130] The static point cloud map is defined as the difference between the overall point cloud map and the dynamic point cloud map, that is,

[0131]

[0132] Assume that a set of N consecutive scans along the robot motion is given in advance. For each scan Where e = 1, ..., N, perform dynamic map point detection based on range image, and count the total number of each map point marked as SM (static) or DM (dynamic), denoted by n SMWith n DM . Dynamic Map P DM ∈P M Use n SM With n DM The weighted function of , that is, the static degree scoring function s(·), is defined, and the final result of the static map is the complement of the dynamic map. That is:

[0133] P DM ={p M |s(p M )<τ s}

[0134] P SM =P M -P DM

[0135] Among them, τ s is the threshold of the static degree scoring function of the dynamic map;

[0136] The static score of a single 3D point is:

[0137] s(·)=α SM n SM (p M )+α DM n DM (p M )

[0138] Among them, α SM and α DM is the positive and negative weight coefficient.

[0139] At the same time, multi-resolution depth maps are compared to restore misidentified ground points, that is, to reduce I Q ,I M The resolution is re-compared, and the points that were mistakenly killed are restored to the static point set. Repeat until all the set resolution maps are executed.

[0140] Step 5: Dynamic point cloud filtering. Combining the dynamic confidence obtained by semantic segmentation with the object dynamics obtained by viewpoint visibility detection, the dynamic object point cloud is judged and removed, and the static point cloud set is retained as the final map.

[0141] The specific process of step 5 is:

[0142] For the pixels within the dynamic obstacle, calculate its overall static score:

[0143]

[0144] Multiply the overall static score by the dynamic possibility parameter β obtained from the semantic segmentation result to obtain the object dynamic estimate:

[0145] ε(·)=β×S(·)

[0146] If the dynamic estimation value exceeds the threshold ε(max), the object blocking the light path in the map is considered to be a dynamic obstacle and is removed. After all judgments are completed, the static point cloud is assembled into a static map and used as the final map.

[0147] If the above functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium so that they can be carried by robots that move in scenes with many dynamic obstacles or in outdoor scenes, such as inspection robots and logistics robots. If there are dynamic objects in the static map established by the robot's environmental perception system, it will have a significant impact on the robot's navigation and other actions. After the robot is equipped with this functional unit, it can autonomously filter dynamic obstacles in the scene and establish a high-precision static map of the scene, which is convenient for the robot to complete subsequent navigation, interaction and other actions.

[0148] The multi-sensor fusion technology proposed in the present invention can effectively integrate data from different sensors, utilize the complementary advantages of each sensor, and overcome the limitations of a single sensor. For example, in rainy or nighttime environments, the camera may not be able to identify obstacles, while the lidar may be affected by reflection errors. When the running time is too long or the environment is unstable, the single sensor solution has a higher risk, especially when the sensor fails or the performance degrades, the entire system may not work properly. In these cases, this solution uses redundant design, so that even if a sensor fails or works inefficiently, other sensors can still maintain perception of the environment, thereby improving the fault tolerance and stability of the system. For other multi-sensor combined with autonomous positioning and mapping solutions, the present invention uses a lightweight neural network, and sets the dynamic confidence of obstacles in advance through the supervision system, eliminating the dynamic inspection of obstacles with low dynamic probability, and improving the inspection speed of the system; at the same time, retaining the dynamic confidence is conducive to subsequent navigation work.

Claims

1. A multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping, characterized in that: It is divided into the following steps: Step 1: Establish a multi-sensor system based on the robot, place it in an outdoor dynamic environment, and collect environmental information; The multi-sensor system includes a visual sensor and a laser radar sensor equipped on the robot, and the environmental information includes raw point cloud data generated by the laser radar sensor and image information recorded by the visual sensor; Step 2: Input the image information collected by the camera into the semantic segmentation network to obtain the segmentation map of the environment scene semantics and the segmentation confidence of each obstacle, and divide the dynamic possibility of the instance in the scene according to the training results; Select key frame images from the image information collected by the camera, and perform object detection and semantic segmentation on the key frame images simultaneously in a parallel architecture: (1) The specific process of target detection is as follows: input the key frame image information into the neural network, obtain the hyperparameter label corresponding to each image, perform mIoU test on the obtained anchor box results, and perform non-maximum test, retain the anchor box with the highest test probability and its image information, and complete the target detection of the key frame image; (2) The specific process of semantic segmentation is: perform feature detection on the key frame image information and output n instance prototype image segmentation results, which correspond to the target detection results respectively; Combine the results of the parallel processes, use the instance prototype and its specified object confidence to perform a linear combination, and pass the sigmoid function σ to produce the final segmentation result; its calculation formula is: M=σ(DC T ) Where D is the h×ω×g instance prototype parameter matrix, h, ω, g represent the length, width, and height of the parameter matrix respectively, C is the n×k confidence parameter matrix of n instance prototypes through intersection-over-union test and non-maximum suppression, k represents the total number of segmentation categories in the anchor box; Use the predicted bounding box to crop the final instance, color different instances separately, and output object detection and semantic segmentation results simultaneously; According to the content of different training scenes, the dynamic probability of segmentation results using supervised methods is β1, β2, ..., β n , complete the dynamic possibility assignment to the instance; Step 3: According to the dynamic possibility of the instance in the scene, the dynamic confidence is obtained, and the obstacle category is divided according to the dynamic confidence. If it is a dynamic or static obstacle, the point cloud data is directly removed or retained; otherwise, step 4 is executed to perform viewpoint visibility detection on the instance in the scene; Define the dynamic confidence δ as: δ=f(C,β) Among them, C is the confidence parameter matrix obtained by the semantic segmentation process, and β is the dynamic possibility given by the supervised method; Step 4: perform viewpoint visibility detection on the point cloud data collected by the lidar sensor, and divide the point cloud data into a dynamic point set and a static point set; The specific process is as follows: Step 401: The inspection robot scans the environment N times continuously through the laser radar sensor to obtain N groups of original point cloud scan frames, and selects the e-th scan frame P from them. e Q , and several adjacent frame point clouds, stitching several adjacent frame point clouds into a local point cloud map, denoted as P e M ; Step 402, compare P e Q With P e M , which will be P e Q , P e M Converted into depth images respectively and recorded as I e Q ,I e M ; Depth Image I e M The pixel values ​​and point cloud maps are: Similarly, I e Q The pixel value of Among them, r(p) represents the three-dimensional space coordinates of the point cloud in the key frame; Represents the pixel value with index (I,J) in the depth image; P M A subset of represents the set of all points in the spherical coordinate system that fall within the corresponding (I,J) index; Will I e Q ,I e M Subtract the pixel values: If I e M The depth of a pixel in is less than I e Q The depth of the corresponding pixel in the local point cloud map indicates that the corresponding point in the scan frame occludes the point in the scan frame, that is, I e M This point in is dynamic; the final dynamic point cloud map is defined as: Among them, τ D It is the threshold value of the algorithm’s sensitivity coefficient to point distance; The static point cloud map is defined as the difference between the local point cloud map and the dynamic point cloud map, that is, Step 403, traverse the scanned frames of N scans, perform dynamic map point detection based on the range image for each scanned frame, and count the total number of map points marked as SM (static) or DM (dynamic), which are recorded as n respectively. SM With n DM ; Step 404, dynamic map P DM ∈P M Use n SM With n DM The weighted function of , that is, the static degree scoring function s(·), is defined. The final result of the static map is the complement of the dynamic map; the dynamic map and the static map are: P DM ={p M |s(p M )<τ s } P SM =P M -P DM Among them, τ s is the threshold of the static score function of the dynamic map, and the static score of a single 3D point is s(·)=α SM n SM (p M )+α DM n DM (p M ), where α SM and α DM is the positive and negative weight coefficient; Step 405, compare the multi-resolution depth map to restore the misidentified ground points, that is, reduce I Q ,I M Re-compare the resolution and restore the misidentified points to the static point set; repeat until all the set resolution maps are completed; Step 5: Combine the dynamic confidence obtained by semantic segmentation with the dynamic nature of the point cloud set obtained by viewpoint visibility detection, further determine the dynamic point set cooperation, and retain the static point cloud set as the final map; For the pixels in the dynamic point set, calculate their overall static score: Multiply the overall static score by the dynamic possibility parameter β obtained from the semantic segmentation result to obtain the object dynamic estimate: ε(·)=β×S(·) If the dynamic estimation value exceeds the threshold ε(max), the object blocking the light path in the map is considered to be a dynamic obstacle and is removed; otherwise, the point is retained and added to the static point set; After all judgments are completed, the final static point cloud collection is a static map and serves as the final map.

2. The multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping according to claim 1 is characterized in that: The multi-sensor system is jointly calibrated to establish the corresponding model and coordinate system, specifically: (1) The robot moves on a two-dimensional ground, so the robot posture has three degrees of freedom, and the robot homogeneous transformation matrix is: Among them, (x1, y1) is the two-dimensional coordinate of the robot, and α is the rotation angle of the robot; (2) The camera modeling process is: The three-dimensional point in the camera coordinate system c a is projected onto the image plane to form a two-dimensional pixel u t =[u,v] T : Among them, π(·) is the camera projection model, f x 、f y is the normalized focal length, c x 、c y are the coordinates of the principal point, (x2, y2, z2) are the three-dimensional points c a is the coordinate of the camera coordinate system; perform inverse transformation of the projection model to project the two-dimensional pixel coordinates to the three-dimensional space coordinates: (3) The process of converting the target point coordinate system of the laser radar to the three-dimensional space coordinate system is as follows: P'=R x (c)R y (i)R z (φ)×P+P0 Among them, P0 represents the translation transformation of the target coordinate system relative to the three-dimensional space coordinate system; P represents the coordinates of the target point in the target coordinate system; P' represents the coordinates of the target point in the three-dimensional space coordinate system; φ, θ, γ represent the yaw angle, pitch angle, and roll angle of the target relative to the origin of the radar coordinate system, respectively; R x , R y , R z Respectively represent the rotation coordinate transformation matrix of yaw angle, pitch angle, and roll angle; the rotation coordinate transformation matrix R of the target tg The calculation formula is:

3. The multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping according to claim 1 is characterized in that: The hyperparameter label of the image is: divide the key frame image into m rows and m columns, and get m 2 squares, each with 2 anchor boxes, then specify the hyperparameter label l for the image: Among them, p c The true or false value indicates whether there is an object in the anchor frame; b x 、b y represents the coordinates of the center point of the anchor box; b h 、b w Indicates the height and width of the anchor box; c1 c...c n The value of represents the confidence that the object in the anchor box is the specified object.

4. The multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping according to claim 1 is characterized in that: The calculation process of average intersection over union (mIoU) is as follows: The intersection-over-union ratio is the ratio of the intersection of the predicted graph set and the true labeled graph set to the union of these two sets. In image semantic segmentation, it represents the degree of overlap between the segmentation graph in the anchor box and the true value of the original image. The calculation method of the intersection-over-union ratio is: The average intersection-and-union ratio is the average value of the intersection-and-union ratio of each class of image pixels: Among them, i represents the true value, j represents the predicted value; p ij It means that i is predicted as j, which is a false negative; p ji It means that j is predicted as i, which is a false positive; p ii It means that i is predicted as i, which is true; k represents the total number of segmentation categories in the anchor box.

5. The multi-sensor fusion dynamic point filtering method for outdoor dynamic environment mapping according to claim 1 is characterized in that: The standard for classifying obstacles based on dynamic confidence is: when the dynamic confidence is lower than the lower threshold, that is, δ≤δ min , the instance is classified as an obstacle without dynamic possibility, and its point cloud is retained as a static point cloud; the dynamic confidence is higher than the upper threshold δ≥δ max , the instance is classified as a high dynamic possibility obstacle, and the obstacle point cloud is removed as a whole; the dynamic confidence threshold δ min ≤δ≤δ max , the instance is classified as a medium dynamic possibility obstacle and enters the viewpoint visibility detection.

Images