An indoor dynamic SLAM method based on multi-source semantic perception

Abstract

The present invention belongs to the field of robot dynamic SLAM vision technology, and specifically relates to an indoor dynamic SLAM method under multi-source semantic perception. Aiming at the problems of low positioning and mapping accuracy in indoor dynamic environments, lack of unified semantic representation and calculation methods, the present invention intends to classify common indoor objects based on multiple sensors, identify and remove segmented moving objects and movable objects that interact with them, and obtain a clear and complete single-view three-dimensional point cloud; track inter-frame motion, calculate the local camera pose based on the weighted RANSAC method; calculate the global camera pose based on the multi-sensor perception factor graph and perform loop detection, and obtain the global three-dimensional point cloud map of the entire scene and the camera motion trajectory based on the differential manifold method.

Description

Technical Field

[0001] The invention belongs to the technical field of robot dynamic SLAM vision, and in particular relates to an indoor dynamic SLAM method under multi-source semantic perception. Background Art

[0002] With the development of science and technology, people have higher and higher expectations for smart homes. Service robots with service and safety inspection as the main needs have become a research hotspot. Service robots are a comprehensive system that integrates multiple functions such as environmental perception, dynamic decision-making and planning, behavior control and execution. Robots have been widely used in space exploration, factory automation, medical equipment, transportation, services and other fields, and have gradually entered people's lives, such as common sweeping robots and cleaning robots in life. The intelligence of autonomous mobile robots is mainly reflected in the ability to scan and identify various types of information in the surrounding environment through the hardware and software equipped by themselves, and then make correct decisions to complete the designated tasks.

[0003] Indoor service robots work in unstructured, complex and dynamic environments, which puts forward higher requirements on their autonomy, intelligence and reliability, and thus also leads to a large number of new problems that need to be solved urgently:

[0004] (1) The static assumption of SLAM is sometimes not valid. Moving objects that enter the camera's field of view can easily interfere with the camera's positioning, and the noise patches they form can affect the construction of the scene map.

[0005] (2) Visual SLAM methods perform well in texture-rich environments, but are sensitive to lighting changes, rapid motion, and initialization. LiDAR SLAM methods can capture details of distant environments, but are less effective in environments with fewer structural features.

[0006] (3) The accumulated drift caused by the camera moving too fast and the flight distance too long makes the cruise unable to close.

[0007] This project intends to use LiDAR and IMU (inertial measurement unit) to supplement the deficiencies of visual SLAM, predefine the semantic (moving, static, movable) information of indoor objects, combine semantic information and spatiotemporal geometry information to identify and remove moving objects and movable objects that interact with them, obtain a complete and clear single-view three-dimensional point cloud, construct and optimize the multi-sensor perception factor graph, obtain the global camera pose and complete loop detection at the same time, and obtain the global point cloud information of the scene and the camera motion trajectory based on the differential manifold method.

[0008] Indoor SLAM technology includes three aspects: map construction, camera pose estimation, and loop detection of scanning paths. In terms of indoor scene map construction, most SLAMs assume that the environment is static. However, there are dynamic objects such as people and pets in the indoor environment. These objects will pollute the map and affect the positioning accuracy of the robot. Based on deep learning, researchers have integrated semantic information with spatial geometric information to remove the influence of such objects so that it can be used in dynamic scenes. Camera pose estimation calculates the correspondence between two or more consecutive frames of data to obtain the transformation matrix of the camera relative to the initial position. Loop detection enables the robot to identify and align "places that have been visited" and is an important part of dynamic SLAM systems to reduce cumulative drift.

[0009] In summary, the visual SLAM method extracts feature points to identify the similarity of frames in the scene, which is suitable for static and texture-rich scenes, but is sensitive to lighting, viewing angle, moving objects, distance, etc.; loop detection is performed based on the bag-of-words model, but as the cruise route extends, the bag-of-words model will be infinite, limiting its scope of application. LiDAR has accurate ranging, a wide perception range, a viewing angle of 360°, a high refresh rate, and is robust to noise and lighting. IMU can provide the orientation and posture of the moving carrier and is not affected by dynamic objects. Fusion of visual RGB-D images, LiDAR 3D point clouds, and IMU data can repair depth images, correct distortion of point cloud data, and accurately estimate the position and heading angle of the robot to obtain high-level detail information of the scene. It has good robustness and accuracy for SLAM of indoor dynamic scenes with lack of features, less texture, and changing lighting. Summary of the invention

[0010] In order to solve the problems of low positioning and mapping accuracy in indoor dynamic environments and the lack of unified semantic representation and calculation methods, the present invention intends to classify common indoor objects based on multiple sensors, identify and remove moving objects and movable objects that interact with them, and obtain a clear and complete single-view three-dimensional point cloud; track the inter-frame motion, and calculate the local camera pose based on the weighted RANSAC method; based on the multi-sensor perception factor graph, calculate the global camera pose and perform loop detection, and obtain the global three-dimensional point cloud map of the entire scene and the camera motion trajectory based on the differential manifold method.

[0011] In order to achieve the above object, the present invention adopts the following technical solutions:

[0012] (1) Based on the semantic characteristics of unstructured and complex indoor environments, this paper studies the robot's perception and understanding of dynamic environments, and explores methods for removing moving objects and movable objects that interact with them from the scene.

[0013] (2) Using RGB-D point cloud and IMU information as data sources, and under the constraint of the spatiotemporal geometric consistency principle, we study the precise matching method between the existing frame and the current frame to estimate the local camera pose.

[0014] (3) By integrating visual, lidar, and inertial sensor information, a multi-sensor perception factor graph is constructed. Based on continuous inter-frame motion and three types of sensor odometry constraints, the global camera pose is estimated and loop closure detection is completed to obtain the camera motion trajectory.

[0015] An indoor dynamic SLAM method under multi-source semantic perception includes the following steps:

[0016] Step 1, the RGB image and depth image of the object are collected by an RGB-D camera;

[0017] Step 2: Use the distortion-corrected LiDAR 3D point cloud information to complete and repair the depth image;

[0018] Step 3: Input the collected RGB image and the completed and repaired depth image into the visual SLAM system:

[0019] Step 3.1, the RGB image is input into the lightweight semantic segmentation network BlitzNet in the visual SLAM system to obtain the initial mask and semantic bounding box of the object. The semantic bounding box is based on the epipolar constraint and uses weighted RANSAC to select static points with high matching confidence to obtain the local camera pose between consecutive frames;

[0020] Step 3.2, using the depth information of the area covered by the initial mask to correct the depth image, and obtain the depth mask after the object is repaired;

[0021] Step 3.3, remove the noise spots in the depth mask to obtain a single-view 3D point cloud image;

[0022] Step 4: Extract feature points from the LiDAR 3D point cloud information and input them into the LiDAR inertial system:

[0023] Step 4.1, store the visual odometry information as a feature map by minimizing the residual value of the visual reprojection and the IMU measurement value;

[0024] Step 4.2, the extracted feature points are matched with the feature map using a sliding window mode based on edge and plane features and visual odometer information to obtain the lidar odometer information;

[0025] Step 4.3, jointly optimize the multi-sensor perception factor graph by visual odometry constraints, lidar odometry constraints, IMU pre-integration constraints, and loop closure constraints, and initialize the lidar-assisted visual inertial odometry;

[0026] Step 4.4: Initialize the candidate matching frame of the current frame using the BRIEF descriptor based on the DBoW2 algorithm, input the candidate frame timestamp into the lidar inertial system for verification, use the corrected IMU offset term to correct the IMU measurement value, optimize the discontinuous frame pose, combine all poses to obtain the global camera pose, and complete the loop detection;

[0027] Step 5: The visual SLAM system and the lidar inertial system work independently and complement each other. When the visual SLAM system lacks information or has serious noise, the global camera pose is called to match the local camera pose between consecutive frames with the single-view three-dimensional point cloud map to obtain the global point cloud map and camera motion trajectory based on the differential manifold.

[0028] Furthermore, in step 2, the depth image is completed and repaired using the distortion-corrected laser radar three-dimensional point cloud information, and the specific steps are as follows:

[0029] Use a sliding window of size 2×2 to traverse the depth image and record the depth value within the sliding window, as shown in formula (1):

[0030] D block =d(u:u+1,v:v+1)(1)

[0031] Edge point extraction is done by formula (2):

[0032]

[0033] Among them, (u, v) represents the image coordinates corresponding to the upper left corner pixel of the sliding window, τ 1 is the threshold value.

[0034] Furthermore, the semantic bounding box in step 3.1 selects static points with higher matching confidence based on epipolar constraints and weighted RANSAC to obtain the local camera pose between consecutive frames. The specific steps are as follows:

[0035] (1) Based on the semantic bounding box of the moving object, the image is quickly divided into static areas and potential dynamic areas;

[0036] (2) Based on the epipolar constraint and using the weighted RANSAC method, the potential dynamic area is classified into static and dynamic matching points, higher confidence is assigned to static object points with higher reliability, and the remaining static matching points are used to calculate the camera pose.

[0037] Furthermore, in step (2), the remaining static matching points are used to calculate the camera pose, and the specific steps are as follows:

[0038] (2.1) Let the previous frame I p and the current frame I c The two sets of static matching points are Pp = {P p1 ,P p2 ,...,P pm} and P c = {P c1 ,P c2 ,...,P cm}, I p with I c The camera pose transformation between is obtained by solving equation (9) using the least squares method:

[0039]

[0040] (2.2) From I p and I c Matching points are extracted from the static area of ​​​​the , and the basic matrix F between the two is calculated by the weighted RANSAC algorithm.

[0041] Furthermore, a local optimization algorithm is added to the weighted RANSAC algorithm.

[0042] Furthermore, in step 3.2, the depth image is corrected using the depth information of the area covered by the initial mask to obtain the depth mask after the object is repaired. The specific steps are:

[0043] (1) Mark the object in the image based on the initial mask of the object to obtain the object label {Obj(1),...,Obj(k)};

[0044] (2) In the initial mask Based on the depth value, remove the 0 value and outliers to get the depth value set And find the pixels in the depth image that have the same depth range as the object Obj(i), as shown in formula (3):

[0045]

[0046] Among them, U d and L d Respectively The maximum and minimum values ​​of d(u,v) represent the depth value of the (u,v) coordinate. represents the area with the same depth value as Obj(i), τ 2 is the threshold value;

[0047] (3) Construct semantic constraints and save the image blocks belonging to the depth mask, as shown in formula (4):

[0048]

[0049] in, It is M iThe number of pixels with semantic information of Obj(i) in It is M i The total number of pixels in , τ 3 is the threshold value;

[0050] (4) Repaired mask The initial mask and depth mask The union of is shown in formula (5);

[0051]

[0052] Furthermore, in step 3.3, the noise spots in the depth mask are removed to obtain a single-view 3D point cloud image. The specific steps are as follows:

[0053] (1) Remove the area where the moving object is located:

[0054] If Obj(i) is a moving object, it needs to be removed If Obj(i) is a static object, it is necessary to construct the semantic point cloud mapping. Regional mapping;

[0055] (2) Remove movable objects that interact with moving objects:

[0056] (2.1) Determine whether the two interact

[0057] After the mask is repaired, if the mask of the moving object intersects with the mask of the movable object, it is considered that there is an interaction between the two, as shown in formula (6):

[0058]

[0059] Where i = 1, ..., n, n is the total number of moving objects in the image, j = 1, ..., m, m is the total number of movable objects in the image, D Obj and S Obj are the collections of moving objects and static objects respectively;

[0060] (2.2) Remove residual noise from moving objects:

[0061] First, the movable objects that interact with the moving objects are identified as moving objects, and the area;

[0062] Secondly, the previous frame I P The feature points extracted from the environment area and the current frame I C To match, the homography matrix H between the two frames is calculated by the LM algorithm. Let p P =[u P ,vP ,1] T For I P The coordinates of a point on the C The coordinates of the point on C =[u C ,v C ,1] T From formula (7), we can get:

[0063] p C =Hp P (7)

[0064] The boundary noise is removed by taking advantage of the large difference in depth values ​​between the residual boundaries of two adjacent frames, as shown in formula (8):

[0065]

[0066] Among them, d P and d C isI P and I C The corresponding depth image, τ 4 is the threshold value;

[0067] Finally, isolated image blocks in the depth image are removed by morphological methods to ensure that the area where the moving object is located is completely removed.

[0068] Compared with the prior art, the present invention has the following advantages:

[0069] 1) From the perspective of semantic perception, the mask of the object is repaired based on the depth information to obtain the area where the moving object is located. By judging the relationship between the depth values, the area where the movable object interacting with the moving object is located is obtained. The noise patches formed by these two types of areas are removed to obtain a clear single-view point cloud image.

[0070] 2) In the point cloud matching calculation, the weighted RANSAC method is used to assign different confidence levels to the matching points to solve the problem of inaccurate local pose estimation of the camera due to the threshold sensitivity between inliers and outliers.

[0071] 3) The multi-sensor perception factor graph completes loop detection and alleviates the cumulative drift problem by nonlinearly iteratively optimizing the objective function containing the global position of the camera under the four types of constraints of multi-sensor information. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 It is the step of multi-source semantic perception indoor dynamic SLAM;

[0073] Figure 2 It is an overall solution for dynamic SLAM system with multi-source semantic perception;

[0074] Figure 3 It is the semantic classification of indoor objects;

[0075] Figure 4 It is a flowchart of indoor dynamic SLAM technology;

[0076] Figure 5 It is the weighted RANSAC method process;

[0077] Figure 6 It is the RANSAC local optimization algorithm process;

[0078] Figure 7 It is the static matching point algorithm process;

[0079] Figure 8 It is a lidar inertial system. DETAILED DESCRIPTION

[0080] Example 1

[0081] The steps of the indoor dynamic SLAM system under multi-source semantic perception are as follows Figure 1 As shown: Acquire and fuse multi-source data, and optimize the single-view 3D point cloud data by removing moving objects and movable objects that interact with them. Obtain the camera pose (local pose) between continuous frames through continuous frame data and matching. Obtain the camera pose between discontinuous frames through discontinuous frame data and matching, and then obtain the global camera pose, optimize the multi-sensor perception factor graph, perform loop detection and feed the results back to the initial position. Optimize the single-view data and motion model. When the trajectory forms a loop, fuse all single-view point cloud data based on the differential manifold method to obtain the global point cloud of the scene and the camera motion trajectory.

[0082] Based on the Songling mobile robot, LiDAR, RGB-D camera, industrial computer and its built-in IMU inertial measurement unit purchased by the laboratory, the dynamic SLAM method of multi-source semantic perception includes a visual SLAM system and a LiDAR inertial system. The specific solution is as follows Figure 2 shown.

[0083] 1) Visual SLAM system

[0084] According to the object characteristics, the semantic information of common indoor objects is predefined, such as Figure 3 As shown, the movable object is ultimately determined as a static object or a moving object based on whether it interacts with a moving object.

[0085] The RGB image is input into the lightweight semantic segmentation network BlitzNet to obtain the initial mask and semantic bounding box of the object. Since the initial mask cannot completely cover the object or contain other objects, it needs to be repaired.

[0086] Since the depth image has missing information and stratification, the depth image of the object is repaired based on the lidar 3D point cloud information, and the corrected mask is obtained based on the initial mask and the depth mask.

[0087] Noise patches caused by moving objects and movable objects interacting with them are removed to obtain a clear single-view 3D point cloud. Based on epipolar constraints and weighted RANSAC, static points with high matching confidence are selected to obtain the local camera pose between consecutive frames, and the scene is expanded through point cloud registration.

[0088] 2) LiDAR Inertial System

[0089] (1) In order to reduce the accumulated drift in SLAM calculation, the current frame needs to be matched with all stored frames. The visual odometry information is stored as a feature map by minimizing the residual value of visual reprojection and IMU measurement.

[0090] (2) Extract feature points from the lidar point cloud, match them with the feature map using a sliding window mode based on edge and plane features and visual odometry information, and obtain lidar odometry information.

[0091] (3) The multi-sensor perception factor graph is optimized by jointly using visual odometry constraints, lidar odometry constraints, IMU pre-integration constraints, and loop closure constraints to reduce the number of data exchanges, improve system efficiency, and initialize the lidar-assisted visual-inertial odometry.

[0092] (4) Based on the DBoW2 algorithm, the candidate matching frames of the current frame are initialized using the BRIEF descriptor, and the timestamp of the candidate frame is input into the lidar inertial system for verification. The IMU measurement value is corrected using the corrected IMU offset term, the pose between non-continuous frames is optimized, and the global pose is obtained by combining all poses to complete the loop detection.

[0093] 3) Interaction between the two systems

[0094] The two systems can work independently and complement each other, especially when the other system is missing or noisy. The depth map is repaired based on the point cloud information in the LiDAR inertial system, and the non-continuous frame camera pose provided by the LiDAR inertial system is used to optimize and verify the local pose.

[0095] Specifically,

[0096] An indoor dynamic SLAM method based on multi-source semantic perception, its technical route is as follows Figure 4 As shown, the following steps are included:

[0097] Step 1, the RGB image and depth image of the object are collected by an RGB-D camera;

[0098] Step 2: For points in the depth image where depth information is missing, the depth image is completed and repaired using the distortion-corrected LiDAR 3D point cloud information. The specific steps are as follows:

[0099] To solve the problem of discontinuous edges of objects in the image, a sliding window of size 2×2 is used to traverse the depth image and record the depth value within the sliding window, as shown in formula (1):

[0100] D block =d(u:u+1,v:v+1)(1)

[0101] Edge point extraction is done by formula (2):

[0102]

[0103] Among them, (u, v) represents the image coordinates corresponding to the upper left corner pixel of the sliding window, τ 1 is the threshold value.

[0104] Step 3: Input the collected RGB image and the completed and repaired depth image into the visual SLAM system:

[0105] Step 3.1: Input the RGB image into the lightweight semantic segmentation network BlitzNet in the visual SLAM system to obtain the initial mask and semantic bounding box of the object. The semantic bounding box is based on the epipolar constraint and uses weighted RANSAC to select static points with high matching confidence to obtain the local camera pose between consecutive frames. The specific steps are as follows:

[0106] (1) During the camera positioning process, the image is quickly divided into static areas and potential dynamic areas based on the semantic bounding boxes of moving objects;

[0107] (2) Based on the epipolar constraint and using the weighted RANSAC method, the potential dynamic area is classified into static and dynamic matching points, and higher confidence is given to static object points with higher reliability to effectively eliminate the adverse effects from moving objects and movable objects. The remaining static matching points are used to calculate the camera pose:

[0108] (2.1) Let the previous frame I p and the current frame I c The two sets of static matching points are P p = {P p1 ,P p2 ,...,P pm} and P c = {P c1 ,P c2 ,...,P cm}, I p with I cThe camera pose transformation between is obtained by solving equation (9) using the least squares method:

[0109]

[0110] (2.2) From I p and I c The matching points are extracted from the static area of ​​​​the image, and the basic matrix F between the two is calculated by the weighted RANSAC algorithm. The weighted RANSAC method integrates a weighted evaluation function and an additional local optimization step. The weighted RANSAC solution process is as follows Figure 5 As shown in , where I is the set of matching points corresponding to the potential dynamic region. The inlier search function uses the input model M to evaluate the sample and returns a subset of inliers whose error is less than a threshold θ.

[0111] In order to ensure the robustness of the camera pose, local optimization is introduced into the traditional RANSAC method. The samples only include ε s (The model interior point found by the minimum solver when the value is large), and nonlinear optimization is used as a PNP solver to introduce more information. Local optimization algorithms such as Figure 6 As shown. Different from the traditional RANSAC method, weighted RANSAC uses the sum of the Lagrange distances between the estimated value and the true value as the scoring criterion, and the model score ε M The calculation process is shown in formula (10):

[0112]

[0113] where p i is the actual position of the point, Indicates the use of model M for p i The position after reprojection, Thr error is the threshold used to limit the influence of a single data point.

[0114] in I p and I c Extract matching point set P from the potential dynamic region DP =[u DP ,v DP ,1],P DC =[u DC ,v DC ,1]. Calculate the distance from the potential dynamic matching point to the corresponding epipolar line, as shown in formula (11), where l x and l x It can be obtained by formula (12).

[0115]

[0116]

[0117] The method for calculating the state of the i-th matching point in the potential dynamic region is shown in formula (13), where D and S represent the dynamic and static matching point sets respectively, τ 5 is the threshold, the static matching point positioning algorithm is as follows Figure 7 shown.

[0118]

[0119] Step 3.2, using the area M covered by the initial mask 0 The depth information of the object is used to correct the depth image and obtain the depth mask M after the object is repaired. d , the specific steps are:

[0120] (1) Mark the object in the image based on the initial mask of the object to obtain the object label {Obj(1),...,Obj(k)};

[0121] (2) In the initial mask Based on the depth value, remove the 0 value and outliers to get the depth value set And find the pixels in the depth image that have the same depth range as the object Obj(i), as shown in formula (3):

[0122]

[0123] Among them, U d and L d Respectively The maximum and minimum values ​​of d(u,v) represent the depth value of the (u,v) coordinate. represents the area with the same depth value as Obj(i), τ 2 is the threshold value;

[0124] (3) Construct semantic constraints and save the image blocks belonging to the depth mask, as shown in formula (4):

[0125]

[0126] in, It is M i The number of pixels with semantic information of Obj(i) in It is M i The total number of pixels in , τ 3 is the threshold value;

[0127] (4) Repaired mask The initial mask and depth mask The union of is shown in formula (5);

[0128]

[0129] Step 3.3: remove the noise spots in the depth mask to obtain a single-view 3D point cloud image. The specific steps are as follows:

[0130] (1) Remove the area where the moving object is located:

[0131] Based on the above operations, if Obj(i) is a moving object, it needs to be removed If Obj(i) is a static object, it is necessary to construct the semantic point cloud mapping. Regional mapping;

[0132] (2) Remove movable objects that interact with moving objects:

[0133] (2.1) Determine whether the two interact

[0134] After the mask is repaired, if the mask of the moving object intersects with the mask of the movable object, it is considered that there is an interaction between the two, as shown in formula (6):

[0135]

[0136] Where i = 1, ..., n, n is the total number of moving objects in the image, j = 1, ..., m, m is the total number of movable objects in the image, D Obj and S Obj are the collections of moving objects and static objects respectively;

[0137] (2.2) Remove residual noise from moving objects:

[0138] First, the movable objects that interact with the moving objects are identified as moving objects, and M is removed. P r (i) area;

[0139] Secondly, although the area where the moving object is located has been removed, some boundary information of the moving object still remains, forming long and narrow boundary noise, which affects the mapping result. To remove this kind of noise, the previous frame I P The feature points extracted from the environment area and the current frame I C To match, the homography matrix H between the two frames is calculated by the LM algorithm. Let p P =[u P ,v P ,1] T For I P The coordinates of a point on the C The coordinates of the point on C =[u C ,v C ,1] TFrom formula (7), we can get:

[0140] p C =Hp P (7)

[0141] The boundary noise is removed by taking advantage of the large difference in depth values ​​between the residual boundaries of two adjacent frames, as shown in formula (8):

[0142]

[0143] Among them, d P and d C isI P and I C The corresponding depth image, τ 4 is the threshold value;

[0144] Finally, isolated image blocks in the depth image are removed by morphological methods to ensure that the area where the moving object is located is completely removed.

[0145] Step 4: Extract feature points from the LiDAR 3D point cloud information and input them into the LiDAR inertial system:

[0146] Step 4.1, store the visual odometry information as a feature map by minimizing the residual value of the visual reprojection and the IMU measurement value;

[0147] Step 4.2, the extracted feature points are matched with the feature map using a sliding window mode based on edge and plane features and visual odometer information to obtain the lidar odometer information;

[0148] Step 4.3, jointly optimize the multi-sensor perception factor graph by visual odometry constraints, lidar odometry constraints, IMU pre-integration constraints, and loop closure constraints, and initialize the lidar-assisted visual inertial odometry;

[0149] Step 4.4: Initialize the candidate matching frame of the current frame using the BRIEF descriptor based on the DBoW2 algorithm, input the candidate frame timestamp into the lidar inertial system for verification, use the corrected IMU offset term to correct the IMU measurement value, optimize the discontinuous frame pose, combine all poses to obtain the global camera pose, and complete the loop detection;

[0150] Construct a multi-sensor perception factor graph, in which visual odometry constraints, lidar odometry constraints, IMU pre-integration constraints, and loop closure constraints are jointly optimized. The IMU measurements are corrected using the corrected IMU offset term for global pose optimization and loop closure detection, such as Figure 8 shown.

[0151] By minimizing the residual value of visual reprojection and IMU measurement, the visual odometry is used as the initial value of LiDAR scan matching. The LiDAR point cloud is distorted using IMU measurement data, and the edge and plane features of the point cloud are extracted. The extracted features are matched with the feature map maintained in the sliding window to obtain the LiDAR odometry data. The camera pose estimation problem is reduced to the maximum a posteriori probability problem (MAP) and solved by a joint optimization method. The pose information can also be sent to the LiDAR-assisted visual inertial odometry to facilitate its initialization of visual odometry information.

[0152] The visual SLAM system provides candidates for loop constraints, which are further optimized through scan matching. The feature map maintains a sliding window of lidar keyframes to simplify the computational complexity. When the posture change exceeds the threshold, a new lidar keyframe is selected and the remaining frames between the keyframes are discarded. After obtaining the new lidar keyframe, the robot state node x is added to the factor graph. Relying on keyframes can strike a balance between memory usage and map density, improving the real-time performance of the system.

[0153] (1) Odometer initialization

[0154] When the camera pose changes dramatically, a reasonable initialization method plays a crucial role in matching between frames. Before initialization, it is assumed that the robot starts moving from a static position with zero velocity, and the bias and noise of the original IMU measurements are zero. After the lidar inertial system is initialized, the IMU bias, camera pose, and velocity information estimated in the factor graph are sent to the lidar-assisted visual inertial odometry to assist its initialization.

[0155] (2) Failure Detection

[0156] Although LiDAR can capture details of the environment at a long distance, matching will fail when the environmental features are lacking. The nonlinear optimization problem in scan matching can be expressed as an iterative solution of a linear problem, as shown in Equation (14), where A and b are obtained by linearizing T. In the first optimization iteration, when A T A failure is detected when the minimum eigenvalue of A is less than a threshold, and the lidar odometry constraint is discarded in the factor graph.

[0157]

[0158] (3) Transformation Matrix

[0159] The distances from the point cloud features in the scene to the edges and planes where they are located are calculated based on equations (15) and (16), where k, u, v, and w are feature indices in the corresponding sets. The edge features in and yes Corresponding points on the edge line.

[0160]

[0161]

[0162] for Plane features in and yes The Gauss-Newton method is used to minimize the optimal transformation, as shown in equation (17). Then, x can be obtained. i With x i+1 The transformation matrix ΔT between i,i+1 , the relationship between the postures of the two nodes is shown in formula (18).

[0163]

[0164] ΔT i,i+1 =T i T T i+1 (18)

[0165] (4) Loop detection

[0166] The visual SLAM system makes a preliminary judgment on the candidate matches, uses the DBoW2 algorithm to extract the BRIEF descriptor from the candidate image keyframe, and matches it with the feature point descriptor of the stored frame. The timestamp of the loop candidate frame returned by the DBoW2 algorithm is sent to the lidar inertial system for further verification. Due to the use of factor graphs based on multi-sensor optimization, loop detection can be naturally integrated into the lidar inertial system. When a new state x is updated in the factor graph i+1 When , we first search the graph to find the Euclidean space corresponding to x i+1 The closest existing state. i+1 With keyframe {F 3-m ,...,F 3 ,...,F 3+m}Convert to the world coordinate system and use the frame-by-frame scanning method to match and obtain the transformation matrix ΔT 3,i+1 , and add it to the factor graph as a loop constraint.

[0167] Step 5: The visual SLAM system and the lidar inertial system work independently and complement each other. When the visual SLAM system lacks information or has serious noise, the global camera pose is called to match the local camera pose between consecutive frames with the single-view three-dimensional point cloud map to obtain the global point cloud map and camera motion trajectory based on the differential manifold.

[0168] Example 2

[0169] The ATE and RPE of the four systems, namely, the present invention, ORB-SLAM2, Dyna-SLAM and DS-SLAM, are quantitatively analyzed.

[0170] The root mean square error (RMSE) and standard deviation (SD) values ​​of ATE and RPE of the four SLAM systems are shown in Tables 1 to 3. RMSE represents the deviation between the measured observation and the true value, which reflects the robustness of the system. SD measures the degree of deviation of a group as a whole, which reflects the stability of the system.

[0171] The improvement values ​​in the table are calculated as follows:

[0172]

[0173] Wherein, κ represents the improved value, α is the value of ORB-SLAM2, and β represents the value of the present invention.

[0174] Table 1 Results of absolute trajectory error (ATE)

[0175]

[0176] Table 2 Results of translation relative pose error (RPE)

[0177]

[0178] Table 3 Results of rotational relative posture error (RPE)

[0179]

[0180] Tables 1 and 2 show the results of ATE and translation RPE, respectively. The present invention achieves the best results in fr3 / w / half and fr3 / w / xyz. In fr3 / w / rpy, the results of the present invention are second only to Dyna-SLAM.

[0181] Table 3 shows the results of rotational RPE, and the present invention achieves the best value in SD index with fr3 / w / half as the unit. In fr3 / w / rpy, the results of the present invention are second only to Dyna-SLAM.

[0182] According to the results obtained in Tables 1-3, in a high dynamic environment, the camera's pose estimation will be greatly improved after eliminating moving objects. Dyna-SLAM, DS-SLAM and the present invention achieved good pose estimation results in the above four groups of sequences. For the present invention, the average RMSE improvement values ​​of ATE, translation RPE and rotation RPE in the sequence are 95.46%, 92.45% and 90.88%, respectively. The average SD improvement values ​​of ATE, translation RPE and rotation RPE in the walking sequence are 94.88%, 94.76% and 92.80%, respectively. This shows that the proposed dynamic target removal front end can effectively improve the performance of ORB-SLAM2 in a high dynamic environment.

Claims

1. An indoor dynamic SLAM method under multi-source semantic perception, characterized in that: The following steps are involved: Step 1, the RGB image and depth image of the object are collected by an RGB-D camera; Step 2: Use the distortion-corrected LiDAR 3D point cloud information to complete and repair the depth image; Step 3: Input the collected RGB image and the completed and repaired depth image into the visual SLAM system: Step 3.1, the RGB image is input into the lightweight semantic segmentation network BlitzNet in the visual SLAM system to obtain the initial mask and semantic bounding box of the object. The semantic bounding box is based on the epipolar constraint and uses weighted RANSAC to select static points with high matching confidence to obtain the local camera pose between consecutive frames; Step 3.2, using the depth information of the area covered by the initial mask to correct the depth image, and obtain the depth mask after the object is repaired; Step 3.3, remove the noise spots in the depth mask to obtain a single-view 3D point cloud image; Step 4: Extract feature points from the LiDAR 3D point cloud information and input them into the LiDAR inertial system: Step 4.1, store the visual odometry information as a feature map by minimizing the residual value of the visual reprojection and the IMU measurement value; Step 4.2, the extracted feature points are matched with the feature map using a sliding window mode based on edge and plane features and visual odometer information to obtain the lidar odometer information; Step 4.3, jointly optimize the multi-sensor perception factor graph by visual odometry constraints, lidar odometry constraints, IMU pre-integration constraints, and loop closure constraints, and initialize the lidar-assisted visual inertial odometry; Step 4.4: Initialize the candidate matching frame of the current frame using the BRIEF descriptor based on the DBoW2 algorithm, input the candidate frame timestamp into the lidar inertial system for verification, use the corrected IMU offset term to correct the IMU measurement value, optimize the discontinuous frame pose, combine all poses to obtain the global camera pose, and complete the loop detection; Step 5: The visual SLAM system and the lidar inertial system work independently and complement each other. When the visual SLAM system lacks information or has serious noise, the global camera pose is called to match the local camera pose between consecutive frames with the single-view three-dimensional point cloud map to obtain the global point cloud map and camera motion trajectory based on the differential manifold.

2. The indoor dynamic SLAM method under multi-source semantic perception according to claim 1 is characterized in that: In step 2, the depth image is completed and repaired using the distortion-corrected laser radar three-dimensional point cloud information. The specific steps are as follows: Use a sliding window of size 2×2 to traverse the depth image and record the depth value within the sliding window, as shown in formula (1): D block =d(u:u+1,v:v+1) (1) Edge point extraction is done by formula (2): Among them, (u, v) represents the image coordinates corresponding to the upper left corner pixel of the sliding window, and τ1 is the threshold.

3. The indoor dynamic SLAM method under multi-source semantic perception according to claim 1 is characterized in that: In step 3.1, the semantic bounding box selects static points with high matching confidence based on epipolar constraints and weighted RANSAC to obtain the local camera pose between consecutive frames. The specific steps are as follows: (1) Based on the semantic bounding box of the moving object, the image is quickly divided into static areas and potential dynamic areas; (2) Based on the epipolar constraint and using the weighted RANSAC method, the potential dynamic area is classified into static and dynamic matching points, higher confidence is assigned to static object points with higher reliability, and the remaining static matching points are used to calculate the camera pose.

4. The indoor dynamic SLAM method under multi-source semantic perception according to claim 3 is characterized in that: In step (2), the remaining static matching points are used to calculate the camera pose, and the specific steps are as follows: (2.1) Let the previous frame I p and the current frame I c The two sets of static matching points are P p = {P p1 ,P p2 ,...,P pm } and P c = {P c1 ,P c2 ,...,P cm }, I p with I c The camera pose transformation between is obtained by solving equation (9) using the least squares method: (2.2) From I p and I c Matching points are extracted from the static area of ​​​​the , and the basic matrix F between the two is calculated by the weighted RANSAC algorithm.

5. The indoor dynamic SLAM method under multi-source semantic perception according to claim 4 is characterized in that: A local optimization algorithm is added to the weighted RANSAC algorithm.

6. The indoor dynamic SLAM method under multi-source semantic perception according to claim 1 is characterized in that: In step 3.2, the depth image is corrected using the depth information of the area covered by the initial mask to obtain the depth mask after the object is repaired. The specific steps are: (1) Mark the object in the image based on the initial mask of the object to obtain the object label {Obj(1),...,Obj(k)}; (2) In the initial mask Based on the depth value, remove the 0 value and outliers to get the depth value set And find the pixels in the depth image that have the same depth range as the object Obj(i), as shown in formula (3): Among them, U d and L d Respectively The maximum and minimum values ​​of d(u,v) represent the depth value of the (u,v) coordinate. represents the area with the same depth value as Obj(i), τ2 is the threshold; (3) Construct semantic constraints and save the image blocks belonging to the depth mask, as shown in formula (4): in, It is M i The number of pixels with semantic information of Obj(i) in It is M i The total number of pixels in, τ3 is the threshold; (4) Repaired mask The initial mask and depth mask The union of is shown in formula (5); 7. The indoor dynamic SLAM method under multi-source semantic perception according to claim 6 is characterized in that: In step 3.3, the noise spots in the depth mask are removed to obtain a single-view 3D point cloud image. The specific steps are as follows: (1) Remove the area where the moving object is located: If Obj(i) is a moving object, it needs to be removed If Obj(i) is a static object, it is necessary to construct the semantic point cloud mapping. Regional mapping; (2) Remove movable objects that interact with moving objects: (2.1) Determine whether the two interact After the mask is repaired, if the mask of the moving object intersects with the mask of the movable object, it is considered that there is an interaction between the two, as shown in formula (6): Where i = 1, ..., n, n is the total number of moving objects in the image, j = 1, ..., m, m is the total number of movable objects in the image, D Obj and S Obj are the collections of moving objects and static objects respectively. and They represent the mask of the moving object and the mask of the movable object respectively; (2.2) Remove residual noise from moving objects: First, the movable objects that interact with the moving objects are identified as moving objects, and the area; Secondly, the previous frame I P The feature points extracted from the environment area and the current frame I C To match, the homography matrix H between the two frames is calculated by the LM algorithm. Let p P =[u P ,v P ,1] T For I P The coordinates of a point on the C The coordinates of the point on C =[u C ,v C ,1] T From formula (7), we can get: p C =Hp P (7) The boundary noise is removed by taking advantage of the large difference in depth values ​​between the residual boundaries of two adjacent frames, as shown in formula (8): Among them, d P and d C isI P and I C Corresponding depth image, τ4 is the threshold; Finally, isolated image blocks in the depth image are removed by morphological methods to ensure that the area where the moving object is located is completely removed.

Images