Scene-adaptive synchronous positioning and mapping method based on dynamic object

CN119068053APending Publication Date: 2024-12-03JILIN UNIVERSITY
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411102163.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-12
Publication Date
2024-12-03

Smart Images

  • Figure CN119068053A_ABST
    Figure CN119068053A_ABST
Patent Text Reader

Abstract

The invention relates to a scene self-adaptive synchronous positioning and mapping method based on a dynamic object, which realizes synchronous positioning and mapping of a dynamic complex scene by fusing a laser radar, a camera and an inertial measurement instrument. The method comprises the following steps: S1, acquiring data of a plurality of sensors, and preprocessing the data; s2, constructing a visual odometer; s3, constructing a laser radar odometer; s4, constructing an inertial navigator factor; s5, constructing a loopback detection factor; s6, pre-integration factors and closed-loop detection factors of the radar, the vision and the inertia measuring instrument are jointly optimized through the factor graph, and therefore the optimal pose transformation of the unmanned vehicle is obtained; and S7, constructing an environment map. By implementing the method, the visual odometer applicable to the method filters out dynamic objects, the mapping precision is improved, different sensors are adaptively selected according to the environmental complexity, and all-weather all-terrain autonomous navigation and driving in complex scenes such as structured roads, fluctuating roads and mountainous areas are realized.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving, and in particular to a method for adaptive synchronous positioning and mapping based on dynamic object scenes. Background Art

[0002] Simultaneous localization and mapping plays a key role in autonomous driving, robot navigation, and rescue by creating an accurate and consistent map of the environment to achieve accurate positioning. This technology estimates the position of the vehicle over a certain period of time by integrating cameras, lidar, inertial navigation, and GPS sensors, thereby achieving map construction and accurate positioning of the vehicle.

[0003] Mapping accuracy is crucial for unmanned vehicle navigation. At present, the mainstream mapping methods include vision-based and laser sensor-based simultaneous positioning and mapping methods. The vision-based simultaneous positioning and mapping method has the characteristics of simple structure, low cost, and rich information, but its mapping accuracy is low and it is more sensitive to light. In environments with multiple dynamic objects such as intersections or highways, it is easy to cause feature point matching failures, thereby affecting the mapping effect. Similarly, when the laser sensor-based simultaneous positioning and mapping method encounters an environment with dense dynamic objects (such as a large number of pedestrians or moving vehicles), the point cloud data of the lidar may be partially blocked, affecting the accuracy of mapping and positioning.

[0004] In general, tightly coupled methods have higher accuracy and robustness, but under extreme conditions, due to the use of only a single pose estimator, tightly coupled methods are more susceptible to failures of other sensors. For example, in a dark but geometrically feature-rich environment, due to the degradation of the visual sensor, more noise will be introduced during multi-sensor fusion, resulting in lower positioning accuracy than a single-modality SLAM method using only lidar.

[0005] In contrast, loosely coupled methods are more robust under challenging perceptual conditions. Loosely coupled methods usually use the lidar odometry as the main pose estimator and regard the estimation results of other odometry as the initial values ​​for inter-frame matching. However, the large initial value error caused by environmental degradation can lead to reduced algorithm accuracy or slower convergence. At the same time, pose estimation is easily disturbed by various dynamic objects such as pedestrians and vehicles, resulting in reduced mapping accuracy. To address these challenges, we designed a simultaneous localization and mapping method based on dynamic object scene adaptation. This method combines visual sensors, lidar sensors, inertial measurement units, and global positioning systems. By setting specific thresholds, appropriate sensors are selected according to different scenarios to improve mapping accuracy, thereby achieving all-weather, all-terrain autonomous navigation and driving in complex scenarios such as structured roads, undulating roads, and mountainous areas. Summary of the invention

[0006] The present invention aims to solve the problems existing in the above-mentioned background technologies and proposes a method for synchronous positioning and mapping based on dynamic object scene adaptation. This method aims to achieve all-weather and all-terrain autonomous navigation and driving in complex scenes such as urban roads and rural markets, where environmental factors seriously affect mapping and positioning.

[0007] In order to solve the above technical problems, an embodiment of the present invention designs a method for synchronous positioning and mapping based on dynamic object scene adaptation, comprising the following steps:

[0008] S1. Obtain multiple sensor data and perform preprocessing;

[0009] S2. Build a visual odometry;

[0010] S3. Build a laser radar odometer;

[0011] S4. Construct inertial navigation system factors;

[0012] S5. Construct loop detection factor;

[0013] S6. Use the factor graph to jointly optimize the radar, vision, inertial measurement instrument pre-integration factors and closed-loop detection factors to obtain the optimal posture transformation of the unmanned vehicle;

[0014] S7. Build an environment map.

[0015] In step S1, the data preprocessing includes time synchronization of multi-sensor data, taking the time of the inertial measurement unit as a reference, synchronizing the time of the monocular camera, the lidar, and the global positioning system to the time system of the current inertial measurement unit.

[0016] Furthermore, in step S1, the present invention designs a method for adaptively selecting an odometer, and adaptively selects different sensors according to different environmental conditions. The specific steps are as follows:

[0017] (1) First, calculate the Gram matrix mat of the original point cloud matrix point. The purpose of this is to reduce the dimension, thereby reducing the computational complexity and reducing the interference of noise eigenvalues.

[0018]

[0019] mat=point T ·point

[0020] Where n represents the number of point clouds in one frame.

[0021] (2) Perform singular value decomposition on the Gram matrix mat,

[0022]

[0023] Where U is the left singular value matrix, V is the right singular value matrix, and s1, s2, and s3 are the singular values ​​obtained in order from large to small.

[0024] (3) Define the adaptive environmental degradation coefficient:

[0025]

[0026] like It shows that the information in the environment is very rich, and the lidar cannot describe the scene in detail, so the visual odometry is used to characterize the scene. At this time, the use of laser radar can better bring out the performance of laser radar and depict the scene in detail. At this time, an inertial navigation system is used to estimate the relative position based on the acceleration and deflection angle of the unmanned vehicle, saving computing resources.

[0027] In step S2, the present invention designs a method for filtering out dynamic objects in visual odometer, and in particular, relates to a method and system for detecting dynamic objects using an RGB-D camera combined with RGB images and point cloud data. The specific process is as follows:

[0028] (1) Obtain RGB-D data:

[0029] Use an RGB-D camera to obtain RGB images and depth images, and convert the RGB images into grayscale images for edge detection. Preprocess the input images:

[0030] I1=resize(I,H1,W1,C)

[0031] Where I is the input image, H1 and W1 are the height and width of the adjusted image respectively. C is the number of channels, usually 3, representing the three channels of R, G, and B. I1 is the output image.

[0032] (2) Normalize the pixels of the image:

[0033]

[0034] Where I2 is the normalized image.

[0035] (3) Convert RGB image to grayscale image:

[0036]

[0037] in, is the normalized image under the corresponding RGB channel, α, β, γ are the specific parameters required to convert the normalized image into a grayscale image.grey is the converted grayscale image. Perform data enhancement on the grayscale image:

[0038] I aug =augment(I grey )

[0039] (4) Detecting dynamic objects in RGB images:

[0040] The bounding box is used to locate the area of ​​the object in the point cloud and roughly filter the point cloud data. The segmentation mask can accurately filter out the points belonging to the object to avoid mistaking background points for object points. After preprocessing the RGB image, the target detection algorithm Mask R-CNN is used to detect dynamic objects in the RGB image to obtain the bounding box or segmentation mask of the dynamic object.

[0041] B MaskRCNN ,P MaskRCNN ,M MaskRCNN =Mask R-CNN(I aug θ MaskRCNN ) Among them, Mask R-CNN is the target detection model we trained, which can recognize and detect dynamic objects, and the network parameter is θ MaskRCNN , the output is the bounding box B MaskRCNN , category probability P MaskRCNN and segmentation mask P MaskRCNN Through the bounding box and segmentation mask, we locate the specific pixel coordinates (u, v) of the dynamic object.

[0042] (5) Camera calibration:

[0043] Perform camera calibration to obtain the intrinsic and extrinsic parameters of the RGB camera and depth camera, and determine the coordinate mapping relationship between the RGB image and the point cloud data through calibration. Camera intrinsic parameter matrix K:

[0044]

[0045] Among them, f x and f y Represents the focal length of the camera on the horizontal and vertical axes respectively. x and c y Represent the coordinates of the principal point of the image on the horizontal and vertical axes respectively. Distortion coefficient vector D:

[0046] D=[k1,k2,p1,p2,k3]

[0047] Among them, k1, k2, k3 are used to describe the radial distortion of the image, and p1, p2 are used to describe the tangential distortion of the image. The rotation vector R and the translation vector T are calculated through a calibration process. This process usually involves taking a set of calibration plate images of known geometric shapes (such as a checkerboard or dot grid), and then using these images to estimate the camera's intrinsic and extrinsic parameters.

[0048] R=[r1,r2,r3]

[0049] t=[t1,t2,t3]

[0050] (6) Project the detection results of the RGB image onto the point cloud:

[0051] Convert the bounding box or segmentation mask of the dynamic object detected in the RGB image to the corresponding area in the point cloud, and use the camera internal and external parameters to convert the pixel coordinates of the bounding box to the three-dimensional coordinates in the point cloud. Pixel coordinates (u, v) are converted to normalized plane coordinates:

[0052]

[0053] Normalized plane coordinates are converted into three-dimensional coordinates:

[0054] X=x'·D

[0055] Y=y'·D

[0056] Z=D

[0057] (7) Locating dynamic objects in point clouds:

[0058] According to the converted 3D coordinates, the points belonging to dynamic objects are screened out in the point cloud data and marked or eliminated.

[0059] (8) Optimization and post-processing:

[0060] Optimize the selected point cloud, such as removing isolated points and smoothing, verify the detection effect of dynamic objects, and adjust parameters or algorithms to improve detection accuracy and stability.

[0061] Furthermore, in step S3, a multi-strategy pose optimization method is designed for the laser radar odometer. First, the original point cloud is clustered, and then the clustered point cloud is subjected to point-to-point, point-to-line, and point-to-surface pose optimization methods to obtain the optimal pose. The specific steps are as follows:

[0062] (1) Clustering the original point cloud data.

[0063] Assume that the original point cloud data is:

[0064] P = {p i |pi ∈R,i=1,…,N}

[0065] Among them, each point p i =(x i ,y i ,z i ). Use a clustering algorithm DBSCAN (Density-Based Spatial Clustering of Applications with Noise) to cluster the point cloud data. For each point, calculate the ω neighborhood centered on it:

[0066] N ω (p i )={p j ||p i -p j |≤ω}

[0067] |N ω (p i )|≥MinPts

[0068] Where ω is the neighborhood radius and MinPts is the minimum number of points in a cluster. Each clustered point cloud is optimized from point to point, from point to line, and from point to surface. Let the initial pose T0 = (R0, t0), where R0 is the rotation matrix and t0 is the translation vector.

[0069] (2) Perform point-to-point optimization on the clustered point cloud.

[0070] The goal of point-to-point optimization is to minimize the Euclidean distance between two frames of point clouds. Objective function:

[0071]

[0072] Among them, q i is a point in the target point cloud, p i is a point in the current point cloud. Minimize the objective function and find the optimal rotation matrix and translation vector.

[0073] (R opt ,t opt ) = argminE ptp (R,t)

[0074] (3) Perform point-to-line optimization on the clustered point cloud.

[0075] The optimization of point to line is to minimize the distance from point to line. Objective function:

[0076]

[0077] Among them, l iis a point to a line segment [q i1 ,q i2 ]’s nearest point.

[0078] (R opt ,t opt ) = argminE ptl (R,t)

[0079] (4) Optimize the clustered point cloud from point to surface.

[0080] The goal of point-to-face optimization is to minimize the distance between points and faces. Suppose each face in the target point cloud is represented by q i1 ,q i2 ,q i3 Definition. The normal vector of a face:

[0081]

[0082] The distance from a point to a surface is as follows:

[0083] d i =(R·p i +tq i1 )·n i

[0084] The objective function is as follows:

[0085]

[0086] Minimize the objective function and solve the optimal rotation matrix and translation vector:

[0087] (R opt ,t opt ) = argminE pte (R,t)

[0088] (5) Perform point-to-point, point-to-line, and point-to-surface joint optimization on two adjacent frame point clouds to obtain the optimal pose:

[0089] E(R,t)=λ ptp ·E ptp (R,t)+λ ptl ·E ptl (R,t)+λ pte ·E pte (R,t)

[0090] In the S4 module, the present invention uses an improved IMU pre-integration method to optimize the posture. The IMU provides acceleration a t and angular velocity w t , we use the pre-integration technique to integrate these data between two key frames and perform pose optimization through the covariance matrix. The specific process is as follows:

[0091] (1) De-noising and calibration of IMU data:

[0092] (2) Calculate IMU pre-integral:

[0093] Calculates IMU pre-integrated values ​​between keyframes, including displacement, rotation, and velocity changes.

[0094]

[0095] Among them, R represents the rotation matrix, v represents the velocity, p represents the position, g represents the gravitational acceleration, Δt ij Indicates the time difference.

[0096] (3) Sliding window optimization:

[0097] Sliding window optimization is used to optimize the current key frame and historical key frames together. All frames in the window are optimized together to improve estimation accuracy and robustness. The optimization goal is to minimize the following loss function:

[0098]

[0099] Among them, T i Indicates t i The position at the moment, T j Indicates t j The position at the moment, f i Indicates t i All frame point clouds within the sliding window at time instant.

[0100] (4) Covariance Propagation:

[0101] Calculate the covariance of posture and velocity during the optimization process and define the state vector x, including posture (position p and rotation q), velocity v and IMU bias (acceleration bias b a and angular velocity bias b g )

[0102]

[0103] The state covariance matrix P is defined as:

[0104]

[0105] Assume that the state transition equation of the system is:

[0106] x k+1 =f(x k ,u k )+w k

[0107] Among them, u kis the control input, w k is the process noise, whose covariance is Q k , the covariance propagation formula is:

[0108]

[0109] Among them, F k , G k are the state transfer functions of the state and noise respectively.

[0110]

[0111] (5) Real-time update:

[0112] Real-time updating includes adding new keyframes to the sliding window and optimizing them. Set the sliding window size to N. When a new keyframe x k+1 When entering the sliding window, all states and covariances in the sliding window are updated. If the number of key frames in the window exceeds N, the earliest key frame is removed to keep the window size fixed.

[0113] (6) Relocation:

[0114] When the system fails to track, it uses IMU data and existing maps for relocalization.

[0115] Compute the relative pose between the current frame and the matching historical keyframe.

[0116]

[0117] Use the relative pose to update the current frame pose:

[0118] T new =T history T rel

[0119] Recalculate the covariance matrix using the relocated pose. Let the state after relocation be: X new , the state before relocation is: X old , the status is updated to:

[0120] X new =X old +ΔX

[0121] The covariance is updated as:

[0122] P new =P old +K(R-HP old H T )K T

[0123] Where K is the Kalman gain, R is the measurement noise covariance, and H is the measurement matrix. Through covariance propagation, real-time update, and relocalization, the pose estimation accuracy and robustness of the SLAM system can be significantly improved. These steps and formulas ensure the stability and reliability of the system in a dynamic environment.

[0124] In the S5 module, a loop detection factor is designed to achieve pose optimization.

[0125] (1) Feature extraction:

[0126] First, perform point cloud segmentation and extract the probability density and information entropy features of the local point cloud after segmentation. The probability density score is calculated by kernel density estimation (KDE). Assuming the kernel function is K and the bandwidth is h, the probability density score is:

[0127]

[0128] Among them, P i For point p i Neighborhood, N i is the number of points in the neighborhood. Information entropy can be calculated by the probability distribution of the local area in the point cloud. Suppose the neighborhood probability distribution of a point in the point cloud is P i :

[0129]

[0130] The above eigenvalues ​​are weighted to obtain the comprehensive eigenvalue F i :

[0131] F i =w f f i +w h H i

[0132] Among them, w f and w h is the weight of each feature.

[0133] (2) Similarity calculation:

[0134] The present invention designs a new weighted similarity calculation method, which is to calculate the cosine distance and the correlation coefficient by weighting:

[0135]

[0136] Where α represents the weight of the cosine distance in the two similarity scores, and β represents the weight of the correlation coefficient in the two similarity scores. i Represents the weighted value of cosine distance and correlation coefficient.

[0137] (3) Determine whether there is a loop based on the calculated similarity.

[0138] Setting Thresholds like If the value is 0, then it is considered that there is a loop. Otherwise, it is considered that there is no loop.

[0139] (4) Optimize the loop error factor by optimizing the loop error term:

[0140]

[0141] Among them, Δp ij and ΔR ij The relative displacement and rotation are obtained by loop detection. The loop detection factor is added to the sliding window.

[0142] In the S6 module, a keyframe sliding window based on linear interpolation is used to optimize the factor graph. The specific steps are as follows:

[0143] (1) Determine the key frame insertion position: Assume the time interval between key frames: Δt k , for each new key frame P i , calculate its difference with the previous key frame P k-1 The time difference t i -t k-1 , t i -t k-1 ≥Δt k , a new keyframe is inserted.

[0144] (2) Calculate the interpolation factor α:

[0145]

[0146] (3) Calculate the linear interpolation pose T i :

[0147] T i =(1-α)T k-1 +αT k+1

[0148] Where T k-1 and T k+1 are the poses of the previous key frame and the next key frame respectively. Designing the sliding window according to this method reduces the computational complexity.

[0149] Further, in step S6, the method for adaptive simultaneous localization and mapping of scenes based on dynamic objects, the factors include visual odometer factors LiDAR Odometry Factor Inertial Measurement Unit Pre-Integration Factor Loopback Factor This method automatically selects the appropriate sensor under the current environmental conditions according to the designed adaptive environmental degradation coefficient. The construction of the above four factors is as follows:

[0150] (1) The visual odometry factor can be expressed as:

[0151]

[0152] Among them, u i is the ith observation point. wi is the i-th 3D point in the world coordinate system. cw and t cw Refers to the rotation matrix and translation vector between two frames. K is the camera intrinsic parameter matrix. Through nonlinear least squares optimization methods (such as Levenberg-Marquardt), the reprojection error of all observation points is minimized to obtain the optimized pose estimate T cw (Including R cw and t cw ).

[0153] (2) The laser radar odometer factor can be expressed as:

[0154]

[0155] Among them, X t Indicates the current key frame, X t+1 The frame collected by the LiDAR at time t+1, p i Indicates key frame X t The i-th point in the image, R is the rotation matrix, t is the translation vector, q i Indicates key frame X t+1 The i-th point in the i Indicates key frame X t+1 The distance from the corresponding point to the corresponding line. i Indicates key frame X t+1 Any three points form the normal vector of the surface. i Is keyframe X t+1 A little inside.

[0156] (3) The construction process of the inertial measurement unit pre-integration factor is as follows:

[0157] 1) According to the inertial measurement unit pre-integration method, the relative motion Δv of the body in adjacent time stamps is obtained ij , Δp ij and ΔR ij :

[0158]

[0159] Among them, w k and ak are the measurements from the gyroscope and accelerometer, respectively, and are used to minimize the error between the pre-integrated value and the current estimate.

[0160] 2) Construct an error function of the inertial measurement unit pre-integration factor to minimize the error between the pre-integration value and the current estimate:

[0161]

[0162] Where Δt is the time interval between two key frames, and the Log function represents the logarithmic mapping of the rotation matrix, which converts the rotation error into a vector form in the Lie algebraic space. i represents the rotation matrix of the i-th frame, p i represents the position of the i-th frame, v i Indicates the speed of the i-th frame. b a represents the bias of the accelerometer, b g Represents the bias of the gyroscope. (4) The construction of the loopback factor is as follows:

[0163] The loop closure factor often uses loop detection as a factor in the factor graph to optimize mapping and positioning. It is usually divided into vision-based loop detection and lidar-based loop detection, which aims to eliminate the time-accumulated pose drift. Compared with cameras, lidar is more robust and can work in different environments, including weak lighting, uneven lighting, multiple light sources, etc. At the same time, lidar can provide more accurate three-dimensional information, so it is more effective in actual three-dimensional scenes. In the present invention, lidar data is used for loop detection instead of image data. When a loop is detected, it is necessary to add a loop detection factor to the factor graph, and add the constraint between the current frame and the loop keyframe to the factor graph. The error function of the loop detection factor can be expressed as:

[0164]

[0165] The Log function represents the logarithmic mapping in the Lie algebraic space, which converts the pose transformation matrix into a vector error representation. ij Represents the relative pose transformation matrix from frame i to frame j, including the rotation R ij and pan t ij . T i represents the pose transformation matrix of frame i, T j represents the pose transformation matrix of frame j. Represents the relative measured pose obtained through loop closure detection. Through this formula and steps, the loop closure factor can be constructed and optimized, thereby significantly reducing the cumulative error in the SLAM system and improving the accuracy of global pose estimation.

[0166] In S6, after the factor graph is constructed, a dynamic adaptive Levenberg-Marquardt method is used to perform pose optimization, which adaptively adjusts the damping coefficient to make the algorithm have better convergence performance at different stages. The specific steps are as follows:

[0167] (1) Initialization: set the initial estimate c0, initialize the damping coefficient λ and the adjustment parameter v>1.

[0168] (2) Calculate the error and Jacobian matrix: At the current estimate x k At this point, calculate the error vector r(x k )

[0169]

[0170] (3) Calculate the Jacobian matrix J(x k ).

[0171]

[0172] (4) Construct the linear equation of damping:

[0173] (J T J+λI)δx=-J T e

[0174] (5) Update the estimated value:

[0175] x k+1 =x k +δx

[0176] (6) Adjust the damping coefficient:

[0177]

[0178] Through the above steps and formulas, the dynamic adaptive Levenberg-Marquardt method can effectively perform factor graph optimization, combining the measurement data of visual odometry, lidar odometry, IMU and loop closure detection to improve the global pose estimation accuracy and robustness of the SLAM system.

[0179] Compared with the prior art, the multi-sensor fusion synchronous positioning and mapping method for ground mobile platforms described in the present invention has the following advantages:

[0180] 1. Better adaptability and computational efficiency: Adaptively select appropriate sensors according to different environments, improve the system mapping accuracy, and make this method more universal for positioning and mapping in different environments. Only fewer sensors are used, which improves the system computing efficiency and reduces time complexity.

[0181] 2. Wider application areas: The system filters out dynamic objects. When faced with complex dynamic scenes, it calculates the pose of the scene after filtering out dynamic objects, which improves the accuracy of pose estimation and has better adaptability. At the same time, due to the dynamic selection of different sensors in the factor graph optimization process, this method can be applied to various environments and occasions. BRIEF DESCRIPTION OF THE DRAWINGS

[0182] Figure 1 A flowchart of a scene-adaptive synchronous positioning and mapping method based on dynamic objects provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0183] The present invention is described in detail below with reference to the accompanying drawings and examples. The present invention proposes a scene-adaptive synchronous positioning and mapping method based on dynamic objects, and the specific implementation steps are as follows:

[0184] Step 1: Acquire multiple sensor data and preprocess the data of each sensor, including data calibration and synchronization, as well as estimation and correction of noise and drift.

[0185] Furthermore, in step 1, the present invention designs a method for adaptively selecting an odometer, and we adaptively select different sensors according to different environmental conditions. The specific steps are as follows:

[0186] (1) First, calculate the Gram matrix mat of the original point cloud matrix point.

[0187] The purpose is to reduce the computational complexity by reducing the dimension, while reducing the interference of noise eigenvalues.

[0188]

[0189] mat=point T ·point

[0190] Where n represents the number of point clouds in one frame.

[0191] (2) Perform singular value decomposition on the Gram matrix mat.

[0192]

[0193] Where U is the left singular value matrix, V is the right singular value matrix, and s1, s2, and s3 are the singular values ​​obtained in order from large to small.

[0194] (3) Define the adaptive environmental degradation coefficient:

[0195]

[0196] like It shows that the information in the environment is very rich, and the lidar cannot describe the scene in detail, so the visual odometry is used to characterize the scene. At this time, the use of laser radar can better bring out the performance of laser radar and depict the scene in detail. At this time, an inertial navigation system is used to estimate the relative position based on the acceleration and deflection angle of the unmanned vehicle, saving computing resources.

[0197] In step 2, the present invention designs a method for filtering out dynamic objects in visual odometer, and in particular, relates to a method and system for detecting dynamic objects using an RGB-D camera combined with RGB images and point cloud data. The specific process is as follows:

[0198] (1) Obtain RGB-D data:

[0199] Use an RGB-D camera to obtain RGB images and depth images, and convert the RGB images into grayscale images for edge detection. Preprocess the input images:

[0200] I1=resize(I,H1,W1,C)

[0201] Where I is the input image, H1 and W1 are the height and width of the adjusted image respectively. C is the number of channels, usually 3, representing the three channels of R, G, and B. I1 is the output image.

[0202] (2) Normalize the pixels of the image:

[0203]

[0204] Where I2 is the normalized image.

[0205] (3) Convert RGB image to grayscale image:

[0206]

[0207] in, is the normalized image under the corresponding RGB channel, α, β, γ are the specific parameters required to convert the normalized image into a grayscale image. grey is the converted grayscale image.

[0208] Perform data augmentation on the grayscale image:

[0209] I aug =augment(I grey )

[0210] (4) Detecting dynamic objects in RGB images:

[0211] The bounding box is used to locate the area of ​​the object in the point cloud and roughly filter the point cloud data. The segmentation mask can accurately filter out the points belonging to the object to avoid mistaking background points for object points. After preprocessing the RGB image, the target detection algorithm Mask R-CNN is used to detect dynamic objects in the RGB image to obtain the bounding box or segmentation mask of the dynamic object.

[0212] B MaskRCNN ,P MaskRCNN ,B MaskRCNN =Mask R-CNN(I aug θ MaskRCNN ) Among them, Mask R-CNN is the target detection model we trained, which can recognize and detect dynamic objects, and the network parameter is θ MaskRCNN , the output is the bounding box B MaskRCNN , category probability P MaskRCNN and segmentation mask M MaskRCNN Through the bounding box and segmentation mask, we locate the specific pixel coordinates (u, v) of the dynamic object.

[0213] (5) Camera calibration:

[0214] Perform camera calibration to obtain the intrinsic and extrinsic parameters of the RGB camera and depth camera, and determine the coordinate mapping relationship between the RGB image and the point cloud data through calibration. Camera intrinsic parameter matrix K:

[0215]

[0216] Distortion coefficient vector D:

[0217] D=[k1,k2,p1,p2,k3]

[0218] The rotation vector R and translation vector T are calculated through a calibration process. This process usually involves taking a set of images of a calibration plate with a known geometry (such as a checkerboard or dot grid), and then using these images to estimate the camera's intrinsic and extrinsic parameters.

[0219] R=[r1,r2,r3]

[0220] T=[t1,t2,t3]

[0221] (6) Project the detection results of the RGB image onto the point cloud:

[0222] The bounding box or segmentation mask of the dynamic object detected in the RGB image is converted to the corresponding area in the point cloud, and the pixel coordinates of the bounding box are converted to 3D coordinates in the point cloud using the camera intrinsic and extrinsic parameters.

[0223] Pixel coordinates (u, v) are converted to normalized plane coordinates:

[0224]

[0225] Normalized plane coordinates are converted into three-dimensional coordinates:

[0226] X=x'·D

[0227] Y=y'·D

[0228] Z=D

[0229] (7) Locating dynamic objects in point clouds:

[0230] According to the converted 3D coordinates, the points belonging to dynamic objects are screened out in the point cloud data and marked or eliminated.

[0231] (8) Optimization and post-processing:

[0232] Optimize the selected point cloud, such as removing isolated points and smoothing, verify the detection effect of dynamic objects, and adjust parameters or algorithms to improve detection accuracy and stability.

[0233] Furthermore, in step 3, a multi-strategy pose optimization method is designed for the laser radar odometer. First, the original point cloud is clustered, and then the clustered point cloud is subjected to point-to-point, point-to-line, and point-to-surface pose optimization methods to obtain the optimal pose. The specific steps are as follows:

[0234] (1) First, cluster the original point cloud data. Assume that the original point cloud data is

[0235] P = {p i |p i ∈R,i=1,…,N}

[0236] Among them, each point p i =(x i ,y i ,z i ). Use a clustering algorithm DBSCAN (Density-Based Spatial Clustering of Applications with Noise) to cluster the point cloud data. For each point, calculate the ω neighborhood centered on it:

[0237] N ω (p i )={p j ||p i -p j |≤ω}

[0238] |N ω (p i )|≥MinPts

[0239] Where ω is the neighborhood radius and MinPts is the minimum number of points in a cluster. Each clustered point cloud is optimized point-to-point, point-to-line, and point-to-plane. Assume that the initial pose T0 = (R0, t0), where R0 is the rotation matrix and t0 is the translation vector. The clustered point cloud is optimized point-to-point. The goal of point-to-point optimization is to minimize the Euclidean distance between two frames of point clouds. Objective function:

[0240]

[0241] Among them, q i is a point in the target point cloud, p i is a point in the current point cloud. Minimize the objective function and find the optimal rotation matrix and translation vector.

[0242] (R opt ,t opt ) = argminE ptp (R,t)

[0243] (2) Perform point-to-line optimization on the clustered point cloud. The point-to-line optimization is to minimize the distance between points and lines.

[0244] Objective function:

[0245]

[0246] Among them, l i is a point to a line segment [q i1 ,q i2 ]’s nearest point.

[0247] (R opt ,t opt ) = argminE ptl (R,t)

[0248] (3) Perform point-to-face optimization on the clustered point cloud. The goal of point-to-face optimization is to minimize the distance between points and faces. Suppose each face in the target point cloud is represented by q i1 ,q i2 ,q i3 Definition. The normal vector of a face:

[0249]

[0250] Distance from point to surface:

[0251] d i =(R·p i +tq i1 )·n i

[0252] Objective function:

[0253]

[0254] Minimize the objective function and solve the optimal rotation matrix and translation vector:

[0255] (R opt ,t opt ) = argminE pte (R,t)

[0256] (4) Perform point-to-point, point-to-line, and point-to-surface joint optimization on two adjacent frame point clouds to obtain the optimal pose:

[0257] E(R,t(=λ ptp ·E ptp (R,t)+λ ptl ·E ptl (R,t)+λ pte ·E pte (R,t)

[0258] In the S4 module, the present invention uses IMU pre-integration to optimize the posture. IMU provides acceleration a t and angular velocity w t , we use pre-integration technology to integrate these data between two key frames. The specific process is as follows:

[0259] (1) De-noising and calibration of IMU data,

[0260] Calculate IMU pre-integration and calculate the IMU pre-integration value between key frames, including displacement, rotation and velocity changes.

[0261]

[0262] Among them, R represents the rotation matrix, v represents the velocity, p represents the position, g represents the gravitational acceleration, Δt ij Indicates the time difference.

[0263] (2) Sliding window optimization:

[0264] Sliding window optimization is used to optimize the current key frame and historical key frames together. All frames in the window are optimized together to improve estimation accuracy and robustness. The optimization goal is to minimize the following loss function:

[0265]

[0266] Among them, T i Indicates t i The position at the moment, T j Indicates t j The position at the moment, fi Indicates t i All frame point clouds within the sliding window at time instant.

[0267] (3) Covariance Propagation:

[0268] Calculate the covariance of posture and velocity during the optimization process. Define the state vector x, including posture (position p and rotation q), velocity v and IMU bias (acceleration bias b a and angular velocity bias b g ).

[0269]

[0270] The state covariance matrix P is defined as:

[0271]

[0272] Assume that the state transition equation of the system is:

[0273] x k+1 =f(x k ,u k )+w k

[0274] Among them, u k is the control input, w k is the process noise, whose covariance is Q k , the covariance propagation formula is:

[0275]

[0276] Among them, F k , G k are the state transfer functions of the state and noise respectively.

[0277]

[0278] (4) Real-time update:

[0279] Real-time updating includes adding new keyframes to the sliding window and optimizing them. Set the sliding window size to N. When a new keyframe x k+1 When entering the sliding window, all states and covariances in the sliding window are updated. If the number of key frames in the window exceeds N, the earliest key frame is removed to keep the window size fixed.

[0280] (5) Relocation:

[0281] When the system fails to track, it uses IMU data and existing maps to relocalize the system and calculate the relative pose between the current frame and the matching historical keyframes.

[0282]

[0283] Use the relative pose to update the current frame pose:

[0284] T new =T history T rel

[0285] Recalculate the covariance matrix using the relocated pose. Let the state after relocation be: X new , the state before relocation is: X old , the status is updated to:

[0286] X new =X old +ΔX

[0287] The covariance is updated as:

[0288] P new =P old +K(R-HP old H T )K T

[0289] Where K is the Kalman gain, R is the measurement noise covariance, and H is the measurement matrix. Through covariance propagation, real-time update, and relocalization, the pose estimation accuracy and robustness of the SLAM system can be significantly improved. These steps and formulas ensure the stability and reliability of the system in a dynamic environment.

[0290] In the S5 module, a loop detection factor is designed to achieve pose optimization. First, the point cloud is segmented, and the probability density and information entropy features of the local point cloud after segmentation are extracted. The probability density score is calculated by kernel density estimation (KDE). Assuming the kernel function is K and the bandwidth is h, the probability density score is:

[0291]

[0292] Among them, P i For point p i Neighborhood, N i is the number of points in the neighborhood. Information entropy can be calculated by the probability distribution of the local area in the point cloud. Suppose the neighborhood probability distribution of a point in the point cloud is P i :

[0293]

[0294] The above eigenvalues ​​are weighted to obtain the comprehensive eigenvalue F i :

[0295] F i =w f f i+w h H i

[0296] Among them, w f and w h is the weight of each feature. In addition, the present invention designs a new weighted similarity calculation method, which is to calculate the cosine distance and correlation coefficient by weighting:

[0297]

[0298] Where α represents the weight of the cosine distance in the two similarity scores, and β represents the weight of the correlation coefficient in the two similarity scores. i Indicates the weighted value of the cosine distance and the correlation coefficient. Determines whether there is a loop based on the calculated similarity. Sets the threshold like Then it is considered that there is a loop. The loop error factor is optimized by optimizing the loop error term:

[0299]

[0300] Among them, Δp ij and ΔR ij The relative displacement and rotation are obtained by loop detection. The loop detection factor is added to the sliding window.

[0301] In the S6 module, a keyframe sliding window based on linear interpolation is used to optimize the factor graph. The specific steps are as follows:

[0302] (1) Determine the key frame insertion position: Assume the time interval between key frames: Δt k , for each new key frame P i , calculate its difference with the previous key frame P k-1 The time difference t i -t k-1 , t i -t k-1 ≥Δt k , a new keyframe is inserted.

[0303] (2) Calculate the interpolation factor α:

[0304]

[0305] (3) Calculate the linear interpolation pose T i :

[0306] T i =(1-α)T k-1 +αT k+1

[0307] Where Tk-1 and T k+1 are the poses of the previous key frame and the next key frame respectively. Designing the sliding window according to this method reduces the computational complexity.

[0308] Further, in step S6, the method for adaptive simultaneous localization and mapping of scenes based on dynamic objects, the factors include visual odometer factors LiDAR Odometry Factor Inertial Measurement Unit Pre-Integration Factor Loopback Factor This method automatically selects the appropriate sensor under the current environmental conditions according to the designed adaptive environmental degradation coefficient. The construction of the above four factors is as follows:

[0309] (1) The visual odometry factor can be expressed as:

[0310]

[0311] Among them, u i is the ith observation point. wi is the i-th 3D point in the world coordinate system. cw and t cw Refers to the rotation matrix and translation vector between two frames. K is the camera intrinsic parameter matrix. Through nonlinear least squares optimization methods (such as Levenberg-Marquardt), the reprojection error of all observation points is minimized to obtain the optimized pose estimate T cw (Including R cw and t cw ).

[0312] (2) The laser radar odometer factor can be expressed as:

[0313]

[0314] Among them, X t Indicates the current key frame, X t+1 The frame collected by the LiDAR at time t+1, p i Indicates key frame X t The i-th point in the image, R is the rotation matrix, t is the translation vector, q i Indicates key frame X t+1 The i-th point in the i Indicates key frame X t+1 The distance from the corresponding point to the corresponding line. i Indicates key frame X t+1 Any three points form the normal vector of the surface. i Is keyframe X t+1 A little inside.

[0315] (3) The construction process of the inertial measurement unit pre-integration factor is as follows:

[0316] 1) According to the inertial measurement unit pre-integration method, the relative motion Δv of the body in adjacent time stamps is obtained ij , Δp ij and ΔR ij :

[0317]

[0318] Among them, w k and a k are the measurements from the gyroscope and accelerometer, respectively, and are used to minimize the error between the pre-integrated value and the current estimate.

[0319] 2) Construct an error function of the inertial measurement unit pre-integration factor to minimize the error between the pre-integration value and the current estimate:

[0320]

[0321] Where Δt is the time interval between two key frames, and the Log function represents the logarithmic mapping of the rotation matrix, which converts the rotation error into a vector form in the Lie algebraic space. i represents the rotation matrix of the i-th frame, p i represents the position of the i-th frame, v i Indicates the speed of the i-th frame. b a represents the bias of the accelerometer, b g Represents the bias of the gyroscope. (4) The construction of the loopback factor is as follows:

[0322] The loop closure factor often uses loop detection as a factor in the factor graph to optimize mapping and positioning. It is usually divided into vision-based loop detection and lidar-based loop detection, which aims to eliminate the time-accumulated pose drift. Compared with cameras, lidar is more robust and can work in different environments, including weak lighting, uneven lighting, multiple light sources, etc. At the same time, lidar can provide more accurate three-dimensional information, so it is more effective in actual three-dimensional scenes. In the present invention, lidar data is used for loop detection instead of image data. When a loop is detected, it is necessary to add a loop detection factor to the factor graph, and add the constraint between the current frame and the loop keyframe to the factor graph. The error function of the loop detection factor can be expressed as:

[0323]

[0324] The Log function represents the logarithmic mapping in the Lie algebraic space, which converts the pose transformation matrix into a vector error representation. ijRepresents the relative pose transformation matrix from frame i to frame j, including the rotation R ij and pan t ij . T i represents the pose transformation matrix of frame i, T j Represents the pose transformation matrix of frame j. Represents the relative measured pose obtained through loop closure detection. Through this formula and steps, the loop closure factor can be constructed and optimized, thereby significantly reducing the cumulative error in the SLAM system and improving the accuracy of global pose estimation.

[0325] In step 6, after the factor graph is constructed, a dynamic adaptive Levenberg-Marquardt method is used to perform pose optimization, which adaptively adjusts the damping coefficient to make the algorithm have better convergence performance at different stages. The specific steps are as follows:

[0326] (1) Initialization: set the initial estimate x0, initialize the damping coefficient λ and the adjustment parameter v>1.

[0327] (2) Calculate the error and Jacobian matrix: At the current estimate x k At this point, calculate the error vector r(x k )

[0328]

[0329] (3) Calculate the Jacobian matrix J(x k ).

[0330]

[0331] (4) Construct the linear equation of damping:

[0332] (J T J+λI)δx=-J T e

[0333] (5) Update the estimated value:

[0334] x k+1 =x k +δx

[0335] (6) Adjust the damping coefficient:

[0336]

[0337] Through the above steps and formulas, the dynamic adaptive Levenberg-Marquardt method can effectively perform factor graph optimization, combining the measurement data of visual odometry, lidar odometry, IMU and loop closure detection to improve the global pose estimation accuracy and robustness of the SLAM system.

Claims

1. A method for scene adaptive simultaneous localization and mapping based on dynamic objects comprises the following steps: S1. Obtain multiple sensor data and perform preprocessing; S2. Build a visual odometry; S3. Build a laser radar odometer; S4. Construct inertial navigation system factors; S5. Construct loop detection factor; S6. Use the factor graph to jointly optimize the radar, vision, inertial measurement instrument pre-integration factors and closed-loop detection factors to obtain the optimal posture transformation of the unmanned vehicle; S7. Build an environment map.

2. According to the method for scene adaptive simultaneous positioning and mapping based on dynamic objects in claim 1, a method for adaptively selecting odometers is designed in step S1, and different sensors are adaptively selected according to the complexity of the environment.

3. According to the method for scene-adaptive simultaneous positioning and mapping based on dynamic objects in claim 1, a method for filtering out dynamic objects in a visual odometer is designed in step S2, and in particular, it relates to a method and system for dynamic object detection using an RGB-D camera in combination with RGB images and point cloud data, and the mapping accuracy is improved by first filtering out dynamic objects and then optimizing the posture of the visual odometer.

4. According to the method for scene-adaptive simultaneous positioning and mapping based on dynamic objects in claim 1, a multi-strategy pose optimization method is designed in step S3; first, the original point cloud is clustered, and then the clustered point cloud is subjected to point-to-point, point-to-line, and point-to-surface pose optimization methods to obtain the optimal pose.

5. a kind of scene-adaptive synchronous positioning and mapping method based on dynamic objects according to claim 1, wherein a kind of inertial navigation unit factor is designed in step S4, and the mode of improved IMU pre-integration is adopted to optimize posture; Then by covariance propagation, real-time updating and relocation, the posture estimation accuracy and the robustness of SLAM system are significantly improved.

6. According to the method of synchronous positioning and mapping based on scene adaptation of dynamic objects in claim 1, a loop detection factor is designed in step S5, and the method adopts the probability density and information entropy characteristics of point clouds, and combines with a high-precision similarity calculation method to significantly improve the pose optimization accuracy.

7. According to a scene-adaptive synchronous positioning and mapping method based on dynamic objects as described in claim 1, wherein a factor graph optimization method of multi-sensor fusion is designed in step S6, which adopts the dynamic adaptive Levenberg-Marquardt method to effectively optimize the factor graph, and combines the measurement data of visual odometer, lidar odometer, IMU and loop detection to improve the global pose estimation accuracy and robustness of the SLAM system.