A multi-sensor fusion slam method in a dynamic scene

Abstract

This invention discloses a multi-sensor fusion SLAM method for dynamic scenes, belonging to the field of robot perception. The method includes the following steps: preprocessing visual, laser, and IMU data, including time and spatial synchronization; establishing sensor mathematical models and simultaneously constructing a visual-inertial subsystem and a laser-inertial subsystem, which are fused in a tightly coupled manner. The laser-inertial subsystem assists the visual-inertial subsystem in scale recovery, and the visual-inertial subsystem assists the laser-inertial subsystem in loop closure detection. Finally, a globally nonlinear optimization factor graph model is established through graph optimization, adding laser odometry factors, IMU pre-integration factors, visual factors, and loop closure detection factors to the factor graph to obtain globally optimal pose estimation and a 3D map. In dynamic environments, this invention reduces the impact of dynamic objects on localization by fusing information from multiple sensors, resulting in higher localization accuracy and robustness.

Description

Technical Field

[0001] This invention relates to the field of robot perception, and more specifically to a multi-sensor fusion SLAM method for dynamic scenes. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology enables real-time pose estimation and 3D scene construction in unknown environments using information acquired by sensors. Over the past few decades, SLAM technology has been widely applied in mobile robotics, autonomous driving, and drones. However, using a single sensor for localization and map building faces many challenges. For example, LiDAR-based methods can obtain details in distant environments and are insensitive to changes in lighting, but their self-relocalization capabilities are poor; they cannot relocalize when tracking is lost, and their ability to handle dynamic objects is limited. Vision sensor-based methods perform well in feature-rich scenes, but in dynamic environments, especially those dominated by dynamic objects, localization failures occur, and they are highly susceptible to changes in lighting and have limited environmental perception range. Inertial Measurement Units (IMUs) can obtain relatively accurate poses through integration in a short time, but accumulated errors cannot be eliminated over time.

[0003] Existing multi-sensor fusion schemes still suffer from inaccurate dynamic recognition and insufficient fusion in dynamic environments. Chinese Patent Publication No. CN110553652A discloses a robot multi-sensor fusion localization method and its application, utilizing LiDAR, IMU, and camera for fusion localization and obtaining the optimal pose through graph optimization. However, this patent lacks a step for processing dynamic objects in dynamic environments, resulting in low localization accuracy. Chinese Patent Publication No. CN112991447A discloses a method and system for visual localization and static map construction in dynamic environments, performing localization after removing dynamic objects. While this reduces the impact of dynamic objects on localization, this patent relies on the type of prior annotation, which significantly reduces the system's localization accuracy when semantic information is lacking. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention proposes a multi-sensor fusion SLAM method for dynamic scenes. By identifying dynamic features in the environment and fully fusing information from different sensors, the system's positioning accuracy and robustness are improved.

[0005] To achieve the above objectives, the present invention provides the following technical solutions, including:

[0006] A multi-sensor fusion SLAM method for dynamic scenes includes the following steps:

[0007] Step s1: Construct a multi-sensor fusion system, including a visual inertial subsystem and a laser inertial subsystem;

[0008] Step s2: Construct the visual inertial subsystem to obtain the initial pose estimation and provide loop closure detection for the laser inertial subsystem;

[0009] Step s3: Construct a laser inertial subsystem to acquire environmental point cloud information, build a 3D point cloud map, and complete localization;

[0010] Step s4: When the visual inertial subsystem fails to locate, the laser inertial subsystem provides the location information; when the laser inertial subsystem fails to locate, the visual inertial subsystem completes the location; when both the visual inertial subsystem and the laser inertial subsystem are effective, the visual, laser and inertial measurement unit information are fused in a tightly coupled manner to obtain the robot's optimal pose and global map.

[0011] Preferably, step s2 specifically refers to:

[0012] First, FAST feature points are extracted from the images acquired by the camera, and then the FLANN method is used to match the feature points.

[0013] Next, the visual inertial subsystem is initialized, which mainly includes: aligning the timestamps of the visual and inertial measurement unit (IMU, usually a combined unit consisting of 3 accelerometers and 3 gyroscopes), recovering the visual scale using IMU data, and calibrating the external parameters of the visual and IMU online;

[0014] After the visual-inertial subsystem completes initialization, the optimization equations for the visual-inertial subsystem are established as follows:

[0015]

[0016] in:

[0017]

[0018] Where, r prior Representing the prior error, H is constructed after marginalizing the sliding window. p Represents the marginalization matrix, r I Represents the pre-integration error of the IMU. The reprojection error of the j-th feature point is represented by ||·|| 2 Represents the L2 norm, ρ m χ represents the robust kernel function, and x represents the variable to be optimized. ω represents the observed value of the IMU. j The weight representing the j-th feature of the image. Let λ represent the weight obtained from the previous iteration of the j-th feature, k represent the current iteration number, and λ represent the weight obtained from the previous iteration.w ,λ m Let Φ(ω) be a constant. j ) is a regular expression, Ψ(ω) j ) is a momentum equation related to the weights and the number of iterations, ν(T) k ,T k+1 () is a motion compensation term used to compensate for larger measurement errors of the IMU during strenuous bodily movement;

[0019] The variable χ to be optimized in the system is defined as follows:

[0020] χ=[x n ,x n+1 ,…,x n+N ,λ m ,λ m+1 …λ m+M ,ω m ,ω m+1 …ω m+M ]

[0021] n represents the current time of the sliding window, N represents the size of the sliding window (10 in this invention), M represents the number of features within the sliding window, and x i Let λ represent the state variables to be optimized, including pose, velocity, accelerometer bias, and angular velocity bias. Let λ be the inverse depth of the environmental features, and ω be the weight of each feature. To prevent excessive weights from interfering with the optimization process, the weights need to be set between 0 and 1, with dynamic feature weights approaching 0 and static feature weights approaching 1. The entire optimization is divided into two parts: optimization of the state variables and inverse depth, and optimization of the weights.

[0022] Finally, in order to reduce the cumulative error of the robot, loop closure detection is performed on the visual inertial subsystem, and the detected loop closures are published as topics for use by the laser inertial subsystem and the global optimization module.

[0023] Preferably, step s3 further includes the following steps:

[0024] Step s31: Align the data from the lidar and IMU using timestamps, and integrate the acceleration and angular velocity obtained from the IMU to obtain the position and attitude. The calculation process is as follows:

[0025]

[0026] in:

[0027]

[0028] In the formula, The rotation matrix t represents the rotation from the world coordinate system to the robot coordinate system. k and tk+1 Δt represents the time at time k and time (k+1), respectively. k This represents the time interval between two frames of the IMU. and These represent the position vectors in the world coordinate system at times k+1 and k, respectively. and These represent the attitudes in the world coordinate system at times k+1 and k, respectively. The accelerometer output of the inertial measurement unit at time t. ω represents the angular velocity output of the inertial measurement unit at time t. x ω y and ω z This represents the three-axis angular velocity output of the IMU. This represents the deviation of the accelerometer at time t. g represents the deviation of the angular velocity meter at time t. w The vector represents the gravitational acceleration in the world coordinate system; a uniform velocity model is used to remove motion distortion from the laser point cloud point by point. Considering that the point cloud obtained by the lidar contains point clouds of dynamic objects, this invention identifies and removes the dynamic features contained in the point cloud, and then extracts line features and surface features from the laser point cloud.

[0029] Step s32: Construct the laser residual equation, establish the residual equations for the current frame and the previous frame, and define them as follows:

[0030]

[0031] In the formula, The line feature residuals representing the laser point cloud, The surface feature residuals represent the laser point cloud, where k, i, j, l, and m are all indices of the point cloud. and Representative line characteristics, and Representative surface features;

[0032] Furthermore, the transformation relationship between the point cloud at time k and the point cloud at time k+1 is as follows:

[0033]

[0034] Where R and t are the robot's rotation matrix and translation vector, respectively, and are the variables for optimizing the laser inertial subsystem;

[0035] Step s33: Construct a laser odometry system, and optimize the equations as follows:

[0036]

[0037] Where, ω e ,ωp ∈[0,1] represents the weight of the point cloud features;

[0038] The Gauss-Newton method is used to optimize the optimization equations, optimizing the robot's rotation matrix and translation vector. When the optimization equations converge, the robot's pose transformation relationship can be obtained.

[0039] Preferably, step s4 further includes the following steps:

[0040] Step s41: Complete the initialization of the multi-sensor fusion system, including aligning the timestamps of vision, lidar and IMU, and calibrating the positional relationship of the three sensors;

[0041] Step s42: Detect the visual inertial subsystem and the laser inertial subsystem. If the visual inertial subsystem fails to locate, the laser inertial subsystem is used to complete the positioning function alone; if the laser inertial subsystem fails to locate, the visual inertial subsystem is used to provide the positioning function.

[0042] Step s43: When both subsystems are effective simultaneously, the data from the vision, laser, and inertial measurement units are fused in a tightly coupled manner to establish a globally optimized factor graph model. Different error equations are transformed into their respective factors using a graph optimization library, and laser odometry factors, pre-integration factors, vision factors, and loop closure detection factors are added to the factor graph. Finally, the robot's globally optimal position and attitude are obtained through graph optimization.

[0043] The present invention also provides a readable storage medium having a computer program stored thereon, which, when executed, implements the above-described multi-sensor fusion SLAM method for dynamic scenes.

[0044] The present invention also provides an electronic device, which includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the above-described multi-sensor fusion SLAM method in a dynamic scene.

[0045] The present invention also provides a computer program product, which includes a computer program / instructions that, when executed by a processor, implement the above-described multi-sensor fusion SLAM method in a dynamic scene.

[0046] Compared with existing technologies, the present invention has the following advantages:

[0047] 1. This invention divides the multi-sensor fusion system into two subsystems: a visual-inertial subsystem and a laser-inertial subsystem. When the visual-inertial subsystem fails, the laser-inertial subsystem performs the positioning; conversely, when the laser-inertial subsystem fails, the visual-inertial subsystem performs the positioning. This allows the fusion system to still perform positioning even when a single sensor fails.

[0048] 2. This invention considers the impact of dynamic objects on localization in a multi-sensor fusion system from both the visual-inertial subsystem and the laser-inertial subsystem. In the visual-inertial subsystem, a robust optimization function is used to identify and remove dynamic features in the environment. Similarly, in the laser-inertial subsystem, dynamic objects in the point cloud are removed while retaining static environmental information, thus improving the system's localization accuracy in dynamic environments.

[0049] 3. This invention achieves localization and mapping by fusing information from laser, vision, and inertial measurement units in a tightly coupled manner, solving the problems of inaccurate dynamic recognition and insufficient fusion in dynamic scenes. The vision-inertial subsystem provides initial pose estimation and loop closure detection information to the laser-inertial subsystem, while the laser-inertial subsystem provides depth information to the vision-inertial subsystem. The two subsystems complement each other, improving the robustness of the localization system in dynamic environments. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the multi-sensor fusion SLAM method for dynamic scenes according to the present invention.

[0051] Figure 2 It is the result of the visual inertial subsystem recognizing the dynamic features of the environment.

[0052] Figure 3 This invention creates a 3D map on the UrbanNavDataset dataset. Detailed Implementation

[0053] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0054] Example 1

[0055] The multi-sensor fusion system of the present invention consists of two subsystems: a visual inertial subsystem and a laser inertial subsystem.

[0056] like Figure 1As shown, a multi-sensor fusion SLAM method for dynamic scenes is described below, with the specific process as follows:

[0057] S1: Acquires data from cameras, inertial measurement units (IMUs, typically a combination of three accelerometers and three gyroscopes) and lidar in dynamic environments, processes the sensor data, and aligns the camera, lidar, and IMU data in time and space through calibration.

[0058] S2: Establish sensor models, including: visual reprojection residual model, IMU pre-integration model, and laser point cloud residual model, where the visual reprojection error is as follows:

[0059]

[0060] In the formula, r C This represents visual reprojection error. χ represents the observation value of the j-th visual frame, and χ represents the variable to be optimized. and The three-dimensional coordinates of the representative feature point in frame j are estimated by the camera. and This represents the pixel coordinates of the feature point in the j-th frame.

[0061] S3: Constructing the visual-inertial subsystem. First, after histogram equalization of the images acquired by the camera, image features are extracted. The IMU timestamps and camera timestamps are aligned. The pose relationship between adjacent camera time steps is obtained through a pre-integration model. The pre-integration calculation is as follows:

[0062]

[0063] in, This represents the pre-integral quantity of the position in the coordinate system from time k to k+1. This represents the pre-integral velocity in the coordinate system from time k to k+1. This represents the pre-integral value of the attitude in the coordinate system from time k to k+1. This represents the deviation of the accelerometer at time t. This represents the deviation of the angular velocity meter at time t. Then, by simultaneously solving the visual reprojection residual and the IMU pre-integration residual, the joint optimization equation is as follows:

[0064]

[0065] in:

[0066]

[0067] Where, r prior Representing the prior error, H is constructed after marginalizing the sliding window.p Represents the marginalization matrix, r I r represents the pre-integration error of the IMU. p Represents reprojection error, ||·|| 2 Represents the L2 norm, ρ m χ represents the robust kernel function, and x represents the variable to be optimized. ω represents the observed value of the IMU. j The weight representing the j-th feature of the image. Let λ represent the weight obtained from the previous iteration of the j-th feature, k represent the current iteration number, and λ represent the weight obtained from the previous iteration. w ,λ m Let Φ(ω) be a constant. j ) is a regular expression, Ψ(ω) j ) is a momentum equation related to the weights and the number of iterations, ν(T) k ,T k+1 () is a motion compensation term used to compensate for larger measurement errors of the IMU during strenuous bodily movement.

[0068] The variable χ to be optimized in the system is defined as follows:

[0069] χ=[x n ,x n+1 ,…,x n+N ,λ m ,λ m+1 …λ m+M ,ω m ,ω m+1 …ω m+M ]

[0070] n represents the current time of the sliding window, N represents the size of the sliding window (10 in this invention), M represents the number of features within the sliding window, and x iLet λ represent the state variables to be optimized, including pose, velocity, accelerometer deviation, and angular velocity deviation. Let λ be the inverse depth of the environmental features, and ω be the weight of each feature. To prevent excessive weights from interfering with the optimization process, the weights are set between 0 and 1, with dynamic feature weights approaching 0 and static feature weights approaching 1. This invention uses an alternating optimization method to optimize the above equations. The entire optimization is divided into two parts: optimization of the state variables and inverse depth, and optimization of the weights. When optimizing the state variables and inverse depth, the feature weights are kept constant; when optimizing the weights, the state variables and inverse depth are kept constant. As the optimization progresses, the least squares equation eventually converges, yielding the weights of the robot's pose and environmental features. This pose is then used as the initial pose of the laser inertial subsystem. When the feature weight is greater than a threshold, it is considered a static feature; when it is less than the threshold, it is considered a dynamic feature. This allows for the separation of dynamic and static environmental features. The advantage of this method is that it does not require prior information and is applicable to a wider range of scenarios. Since dynamic objects in the environment can cause the system to erroneously detect loop closures, dynamic features are not involved in loop closure detection and mapping. This invention only performs loop closure detection and mapping on static features. Figure 2 The image shows the results of the visual-inertial subsystem's recognition of dynamic environmental features. Red represents dynamic features, green represents static features, and the car represents a dynamic object. As can be seen from the image, the method of this invention can accurately identify the dynamic features of the environment.

[0071] S4: Select keyframes and perform loop closure detection. Keyframe selection criteria include a certain time interval between the current and previous keyframes, a sufficient number of feature points in the current frame, and a sufficient shared viewing area with the previous keyframe. Loop closure detection is performed here, and the detected loops will be used in the final multi-sensor fusion system.

[0072] S5: Construct a laser inertial subsystem. Remove motion distortion of the laser point cloud by pre-integrating the pose obtained by the inertial measurement unit. This invention takes into account that the point cloud obtained by the lidar contains point clouds of dynamic objects in the environment. Therefore, the dynamic objects are identified and removed from the point cloud. Then, the line features and surface features of the point cloud are extracted. The ICP algorithm is used to register the preprocessed point cloud to obtain the pose relationship between adjacent time steps.

[0073] S6: Multi-sensor fusion system detection: When the visual-inertial subsystem fails in a dynamic environment, the laser-inertial subsystem completes the positioning; conversely, the visual-inertial subsystem completes the positioning. When both subsystems are effective simultaneously, the data from the visual, laser, and inertial sensors are fused in a tightly coupled manner.

[0074] S7: Constructing a tightly coupled multi-sensor fusion system: First, the system is initialized, including using depth information from laser point clouds to assist visual scale reconstruction, using visually detected loop closures to assist the LiDAR in loop closure detection, and using visually estimated poses to calibrate the inertial measurement unit's (IMU) deviations. Various residuals are converted into various factors using a factor graph model, and a globally optimized factor graph model is established, adding laser odometry factors, IMU pre-integration factors, visual factors, and loop closure detection factors to the factor graph. Graph optimization is used to optimize the factor graph, with initial values ​​provided by the initial pose obtained from the visual-inertial subsystem optimization and the pose obtained from the IMU pre-integration. Graph optimization of the factor graph yields the optimal pose estimate and 3D map of the system. Figure 3 The three-dimensional point cloud map established for this invention shows that the multi-sensor fusion system only maps static point clouds, which can reduce the impact of dynamic point clouds on robot localization. Buildings, roads and landmarks can be clearly seen in the details.

[0075] Example 2

[0076] An electronic device includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements a multi-sensor fusion SLAM method for dynamic scenes as described in Embodiment 1 above.

[0077] The electronic device in this application embodiment may be the mobile device itself, or a standalone device independent of it. The standalone device may communicate with the mobile device to receive the collected input signals from it and send the selected target decision behavior to it.

[0078] An electronic device includes one or more processors and memory. The processor may be a central processing unit (CPU) or other processing unit with data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device to perform desired functions. The memory may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may, for example, include random access memory (RAM) and / or cache memory. The non-volatile memory may, for example, include read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor may execute the program instructions to implement the decision-making behavior and decision-making methods of the various embodiments of this application described above, and / or other desired functions.

[0079] Electronic devices may also include input devices and output devices.

[0080] Example 3

[0081] In addition to the methods and devices described above, embodiments of this application may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the steps in the decision-making behavior decision-making method according to various embodiments of this application as described in Embodiment 1 above.

[0082] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0083] Example 4

[0084] Embodiments of this application may also be computer-readable storage media storing computer program instructions thereon, which, when executed by a processor, cause the processor to perform the steps in the decision-making behavior decision-making method according to various embodiments of this application described in Embodiment 1 above.

[0085] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0086] The above are merely preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A multi-sensor fusion SLAM method for dynamic scenes, characterized in that: Includes the following steps: Step s1: Construct a multi-sensor fusion system, including a visual inertial subsystem and a laser inertial subsystem; Step s2: Construct the visual inertial subsystem to obtain the initial pose estimation and provide loop closure detection for the laser inertial subsystem; Step s3: Construct a laser inertial subsystem to acquire environmental point cloud information, build a 3D point cloud map, and complete localization; Step s4: When the visual inertial subsystem fails to locate, the laser inertial subsystem provides the location information; when the laser inertial subsystem fails to locate, the visual inertial subsystem completes the location; when both the visual inertial subsystem and the laser inertial subsystem are effective, the visual, laser and inertial measurement unit information are fused in a tightly coupled manner to obtain the robot's optimal pose and global map. Step s2 specifically refers to: First, FAST feature points are extracted from the images acquired by the camera, and then the FLANN method is used to match the feature points. Next, the visual-inertial subsystem is initialized, which mainly includes: aligning the timestamps of the visual and inertial measurement unit (IMU) data, recovering the visual scale using IMU data, and calibrating the external parameters of the visual and IMU online. After the visual-inertial subsystem completes initialization, the optimization equations for the visual-inertial subsystem are established as follows: in: in, Representing the prior error, constructed after marginalizing the sliding window. Represents the marginalization matrix. Represents the pre-integration error of the IMU. Representing the Reprojection error of each feature point Represents the L2 norm, Represents a robust kernel function. The variable representing the variable to be optimized. Observations representing the IMU Representing the image number The weights of each feature, Representing the The weights obtained from the previous iteration of each feature. This represents the current iteration number. It is a constant. It is a regular expression. It is a momentum equation related to the weights and the number of iterations. It is a motion compensation item; Variables to be optimized in the system The definition is as follows: Represents the current moment of the sliding window. This represents the size of the sliding window; in this application, it is 10. Represents the number of features within the sliding window. This represents the state variables that need to be optimized, including pose, velocity, accelerometer bias, and angular velocity bias. The inverse depth of environmental characteristics To prevent excessive weights from interfering with the optimization process, weights need to be set between 0 and 1, with dynamic feature weights approaching 0 and static feature weights approaching 1. The entire optimization is divided into two parts: optimization of state variables and inverse depth, and optimization of weights. An alternating optimization method is used, keeping the weights constant when optimizing state variables and inverse depth, and keeping the state variables and inverse depth constant when optimizing weights. Finally, in order to reduce the cumulative error of the robot, loop closure detection is performed on the visual inertial subsystem, and the detected loop closures are published as topics for use by the laser inertial subsystem and the global optimization module. Step s3 further includes the following steps: Step s31: Align the data from the lidar and IMU using timestamps, and integrate the acceleration and angular velocity obtained from the IMU to obtain the position and attitude. The calculation process is as follows: in: In the formula, This represents the rotation matrix from the world coordinate system to the robot coordinate system. and Representing the first Time and the The time of moment, This represents the time interval between two frames of the IMU. and Represent and The position vector in the world coordinate system at any given moment. and Represent and The posture of constantly being within the global coordinate system. represent The accelerometer output of the inertial measurement unit at any given time. represent The angular velocity output of the inertial measurement unit at any given moment. , and This represents the three-axis angular velocity output of the IMU. represent The deviation of the accelerometer at any moment, represent The deviation of the angular velocity meter at any given moment. The vector represents the gravitational acceleration in the world coordinate system; a uniform velocity model is used to remove motion distortion from the laser point cloud point by point. Considering that the point cloud obtained by the lidar contains point clouds of dynamic objects, the dynamic features contained in the point cloud are identified and removed, and then line features and surface features are extracted from the laser point cloud. Step s32: Construct the laser residual equation, establish the residual equations for the current frame and the previous frame, and define them as follows: In the formula, The line feature residuals representing the laser point cloud, The surface feature residuals representing the laser point cloud, Both represent indexes of point clouds. , and Representative line characteristics, and Representative surface features; In addition, the Moment Cloud and the First The temporal point cloud transformation relationship is as follows: in, and , respectively, are the robot's rotation matrix and translation vector, and are the variables for optimizing the laser inertial subsystem; Step s33: Construct a laser odometry system, and optimize the equations as follows: in, Weights representing point cloud features; The Gauss-Newton method is used to optimize the optimization equations, optimizing the robot's rotation matrix and translation vector. When the optimization equations converge, the robot's pose transformation relationship can be obtained. Step s4 also includes the following steps: Step s41: Complete the initialization of the multi-sensor fusion system, including aligning the timestamps of vision, lidar and IMU, and calibrating the positional relationship of the three sensors; Step s42: Detect the visual inertial subsystem and the laser inertial subsystem. If the visual inertial subsystem fails to locate, the laser inertial subsystem is used to complete the positioning function alone; if the laser inertial subsystem fails to locate, the visual inertial subsystem is used to provide the positioning function. Step s43: When both subsystems are effective simultaneously, the data from the vision, laser, and inertial measurement units are fused in a tightly coupled manner to establish a globally optimized factor graph model. Different error equations are transformed into their respective factors through a graph optimization library, and the laser odometry factor, pre-integration factor, vision factor, and loop closure detection factor are added to the factor graph. The robot's global optimal position and attitude are finally obtained through graph optimization.

2. A readable storage medium, characterized in that, It stores a computer program, which, when executed, implements the SLAM method for multi-sensor fusion in a dynamic scene as described in claim 1.

3. An electronic device, characterized in that, It includes a processor, a memory, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the SLAM method for multi-sensor fusion in a dynamic scene as described in claim 1.

4. A computer program product, characterized in that, It includes a computer program / instruction that, when executed by a processor, implements the multi-sensor fusion SLAM method for dynamic scenes as described in claim 1.

Images