SLAM method for multi-sensor fusion in dynamic scene

Abstract

The invention discloses a multi-sensor fusion SLAM (Simultaneous Localization and Mapping) method in a dynamic scene, which belongs to the field of robot perception and comprises the following steps of: preprocessing visual, laser and IMU (Inertial Measurement Unit) data, including time synchronization and space synchronization; a sensor mathematical model is established, a visual inertia subsystem and a laser inertia subsystem are constructed at the same time, and the two systems are fused in a tight coupling mode. The laser inertial subsystem assists the visual inertial subsystem to complete scale recovery, and the visual inertial subsystem assists the laser inertial subsystem to carry out loopback detection. And finally, establishing a global nonlinear optimized factor graph model in a graph optimization mode, and adding a laser odometer factor, an IMU pre-integration factor, a visual factor and a loopback detection factor into a factor graph to obtain global optimal pose estimation and a three-dimensional map. In a dynamic environment, information of a plurality of sensors is fused, so that the influence of a dynamic object on positioning is reduced, and higher positioning precision and robustness are achieved.

Description

Technical Field

[0001] The present invention relates to the field of robot perception, and in particular to a SLAM method of multi-sensor fusion in dynamic scenes. Background Art

[0002] Simultaneous localization and mapping (SLAM) technology can complete its own pose estimation and three-dimensional scene construction in real time in unknown environments through information obtained by sensors. In the past few decades, SLAM technology has been widely used in mobile robots, autonomous driving, drones and other fields. However, there are many challenges in using a single sensor for positioning and mapping. For example, the lidar-based method can obtain details in a long-distance environment and is insensitive to changes in illumination, but its own relocation ability is poor, it cannot be relocated when tracking is lost, and its ability to handle dynamic objects is limited. The visual sensor-based method performs well in feature-rich scenes, but in dynamic environments, especially those dominated by dynamic objects, it will lead to positioning failures, and is extremely susceptible to changes in illumination and has a limited range of environmental perception. The inertial measurement unit can obtain a relatively accurate pose by integrating in a short period of time, but the accumulated error cannot be eliminated over time.

[0003] Existing multi-sensor fusion solutions still have problems with inaccurate dynamic recognition and insufficient fusion in dynamic environments. The Chinese patent publication number CN110553652A discloses a robot multi-sensor fusion positioning method and its application, which uses lidar, IMU and camera for fusion positioning, and obtains the optimal posture through graph optimization methods. However, the patent lacks the link for processing dynamic objects in a dynamic environment, resulting in low positioning accuracy. The Chinese patent publication number CN112991447A discloses a method and system for visual positioning and static map construction in a dynamic environment. Although positioning is performed after removing dynamic objects, the impact of dynamic objects on positioning can be reduced, but the patent relies on the type of prior annotations, which will greatly reduce the positioning accuracy of the system when semantic information is lacking. Summary of the invention

[0004] In order to overcome the defects of the prior art, the present invention proposes a multi-sensor fusion SLAM method in dynamic scenes, which improves the positioning accuracy and robustness of the system by identifying dynamic features in the environment and fully fusing information from different sensors.

[0005] In order to achieve the above object, the following technical solutions of the present invention include:

[0006] A multi-sensor fusion SLAM method in a dynamic scene comprises 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: Build 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 obtain environmental point cloud information, establish a three-dimensional point cloud map and complete positioning;

[0010] Step s4: When the visual-inertial subsystem fails to locate, the laser-inertial subsystem provides positioning information; when the laser-inertial subsystem fails to locate, the visual-inertial subsystem completes the positioning; when both the visual-inertial subsystem and the laser-inertial subsystem are valid, the visual, laser and inertial measurement unit information are fused in a tightly coupled manner to obtain the optimal posture and global map of the robot.

[0011] Preferably, the step s2 specifically refers to:

[0012] First, extract FAST feature points from the image acquired by the camera and use the FLANN method to match the feature points;

[0013] Then 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 three accelerometers and three gyroscopes) data, restoring the visual scale using IMU data, and online calibrating the external parameters of the visual and IMU;

[0014] After the visual-inertial subsystem is initialized, the optimization equation of the visual-inertial subsystem is established. The optimization equation is as follows:

[0015]

[0016] in:

[0017]

[0018] Among them, r prior represents the prior error, constructed after marginalizing the sliding window, H p represents the marginalization matrix, r I represents the pre-integration error of the IMU, represents the reprojection error of the jth feature point, ||·|| 2 represents the bi-norm, ρ m represents the robust kernel function, χ represents the variable to be optimized, Represents the observation value of IMU, ω j represents the weight of the jth feature of the image, represents the weight of the jth feature obtained in the previous iteration, k represents the current number of iterations, and λw ,λ m is a constant, Φ(ω j ) is a regular expression, Ψ(ω j ) is a momentum equation related to weight and number of iterations, ν(T k ,T k+1 ) is a motion compensation term, which is used to compensate for the large measurement error of the IMU when the body moves violently;

[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 moment of the sliding window, N represents the size of the sliding window, which is 10 in the present invention, M represents the number of features in the sliding window, and x i Represents the state quantity that needs to be optimized, including posture, velocity, accelerometer deviation and angular velocity meter deviation, λ is the inverse depth of environmental features, ω is the weight of each feature, to prevent the weight from being too large and interfering with the entire optimization process, the weight needs to be set between 0 and 1, the dynamic feature weight is close to 0, and the static feature weight is close to 1. The entire optimization is divided into two parts: optimization of state quantity and inverse depth and optimization of weight.

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

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

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

[0025]

[0026] in:

[0027]

[0028] In the formula, Represents the rotation matrix from the world coordinate system to the robot coordinate system, t k and tk+1 Represent the time of the kth moment and the k+1th moment respectively, Δt k Represents the time interval between two IMU frames, and Represent the position vectors at time k+1 and k in the world coordinate system respectively, and Represent the postures at time k+1 and k in the world coordinate system respectively, represents 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 Represents the three-axis angular velocity output of the IMU, represents the deviation of the accelerometer at time t, represents the deviation of the angular velocity meter at time t, g w Represents the gravity acceleration vector in the world coordinate system; uses a uniform velocity model to remove motion distortion from the laser point cloud point by point. Considering that the point cloud obtained by the laser radar contains point clouds of dynamic objects, the present 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 equation of the current frame and the previous frame, and define it as follows:

[0030]

[0031] In the formula, Represents the line feature residual of the laser point cloud, Represents the surface feature residual of the laser point cloud, k,i,j,l,m all represent the index of the point cloud, and Represents line features, and Representative surface features;

[0032] In addition, the transformation relationship between the point cloud at the kth moment and the point cloud at the k+1th moment is as follows:

[0033]

[0034] Among them, R and t are the rotation matrix and translation vector of the robot, respectively, and are the variables optimized by the laser inertial subsystem;

[0035] Step s33: Construct a laser odometer, and the optimization equation is as follows:

[0036]

[0037] Among them, ω e ,ωp ∈[0, 1] represents the weight of the point cloud feature;

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

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

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

[0041] Step s42: testing 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 the two subsystems are effective at the same time, the data of vision, laser and inertial measurement unit are fused in a tightly coupled manner to establish a global optimization factor graph model. Different error equations are converted into their respective factors through the graph optimization library, and the laser odometer factor, pre-integration factor, vision factor and loop detection factor are added to the factor graph. The global optimal position and posture of the robot are finally obtained through graph optimization.

[0043] The present invention also provides a readable storage medium on which a computer program is stored. When the computer program is executed, the SLAM method of multi-sensor fusion in a dynamic scene is implemented.

[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, the SLAM method of multi-sensor fusion in a dynamic scene is implemented.

[0045] The present invention also provides a computer program product, which includes a computer program / instruction. When the computer program / instruction is executed by a processor, the SLAM method of multi-sensor fusion in a dynamic scene is implemented.

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

[0047] 1. The present invention divides the multi-sensor fusion system into two subsystems, namely the visual inertial subsystem and the laser inertial subsystem. When the visual inertial subsystem fails, the laser inertial subsystem completes the positioning. Conversely, when the laser inertial subsystem fails, the visual inertial subsystem completes the positioning. In this way, when a single sensor fails, the fusion system can still perform positioning.

[0048] 2. The present invention considers the influence of dynamic objects on positioning from two aspects, the visual inertial subsystem and the laser inertial subsystem, in the multi-sensor fusion system. In the visual inertial subsystem, the dynamic features in the environment are identified and removed through a robust optimization function, and in the laser inertial subsystem, the dynamic objects in the point cloud are also removed, and the static environment information is retained, thereby improving the positioning accuracy of the system in a dynamic environment.

[0049] 3. The present invention achieves positioning 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 visual inertial subsystem provides initial pose estimation and loop detection information for the laser inertial subsystem, and the laser inertial subsystem provides depth information for the visual inertial subsystem. The two subsystems complement each other and improve the robustness of the positioning system in dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a schematic flow chart of a SLAM method of multi-sensor fusion in a dynamic scene of 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 It is a three-dimensional map established by the present invention on the UrbanNavDataset dataset. DETAILED DESCRIPTION

[0053] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0054] Example 1

[0055] The multi-sensor fusion system of the present invention is composed of two subsystems, namely a visual inertial subsystem and a laser inertial subsystem.

[0056] like Figure 1As shown in the figure, a SLAM method of multi-sensor fusion in dynamic scenes, the specific process is as follows:

[0057] S1: Collects data from cameras, inertial measurement units (IMU, usually 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 a sensor model, 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 represents the visual reprojection error, represents the observed value of the visual jth frame, χ represents the variable to be optimized, and represents the 3D coordinates of the feature point in the jth frame, estimated by the camera, and Represents the pixel coordinates of the feature point in the jth frame.

[0061] S3: Construct the visual inertial subsystem. First, extract the image features after performing histogram equalization on the image acquired by the camera, align the IMU timestamp with the camera timestamp, and obtain the position and posture relationship of the camera at adjacent moments through the pre-integration model. The pre-integration calculation is as follows:

[0062]

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

[0064]

[0065] in:

[0066]

[0067] Among them, r prior represents the prior error, constructed after marginalizing the sliding window, Hp represents the marginalization matrix, r I Represents the pre-integration error of IMU, r p represents the reprojection error, ||·|| 2 represents the bi-norm, ρ m represents the robust kernel function, χ represents the variable to be optimized, Represents the observation value of IMU, ω j represents the weight of the jth feature of the image, represents the weight of the jth feature obtained in the previous iteration, k represents the current number of iterations, and λ w ,λ m is a constant, Φ(ω j ) is a regular expression, Ψ(ω j ) is a momentum equation related to weight and number of iterations, ν(T k ,T k+1 ) is a motion compensation term, which is used to compensate for the large measurement error of the IMU when the body moves violently.

[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 moment of the sliding window, N represents the size of the sliding window, which is 10 in the present invention, M represents the number of features in the sliding window, and x iRepresents the state quantity that needs to be optimized, including posture, speed, accelerometer deviation and angular velocity meter deviation, λ is the inverse depth of environmental features, ω is the weight of each feature, to prevent the weight from being too large and interfering with the entire optimization process, it is necessary to set the weight between 0 and 1, the dynamic feature weight is close to 0, and the static feature weight is close to 1. The present invention uses the alternating optimization method to optimize the above optimization equation, and the entire optimization is divided into two parts: optimization of state quantity and inverse depth, and optimization of weight. When optimizing the state quantity and inverse depth, the feature weight is kept unchanged, and when optimizing the weight, the state quantity and inverse depth are kept unchanged. In this way, as the optimization proceeds, the least squares equation finally converges, and the weights of the robot posture and environmental features are obtained at the same time, and this posture is used as the initial posture of the laser inertial subsystem. When the feature weight is greater than the threshold, it is considered to be a static feature, and when it is less than the threshold, it is considered to be a dynamic feature. In this way, the dynamic and static features of the environment can be separated. The advantage of this method is that no prior information is required and the applicable scenarios are wider. Since dynamic objects in the environment may cause the system to erroneously detect loops, dynamic features do not participate in loop detection and mapping. The present invention only performs loop detection and mapping on static features. Figure 2 This is the result of the visual-inertial subsystem identifying the dynamic features of the environment, where red represents dynamic features, green represents static features, and the car is a dynamic object. It can be seen from the figure that the method of the present invention can accurately identify the dynamic features of the environment.

[0071] S4: Select key frames and perform loop detection. The key frame selection criteria include a certain time interval with the previous key frame, sufficient feature points in the current frame, and sufficient common view area with the previous key frame. Perform loop detection, and the loop detected here will be used in the final multi-sensor fusion system.

[0072] S5: Construct a laser inertial subsystem, and remove the motion distortion of the laser point cloud through the posture obtained by pre-integration of the inertial measurement unit. The present invention takes into account that the point cloud obtained by the laser radar contains point clouds of dynamic objects in the environment, so the point cloud is identified and dynamic objects are removed, and then the line features and surface features of the point cloud are extracted. The pre-processed point cloud is registered using the ICP algorithm to obtain the posture relationship between adjacent moments.

[0073] S6: Multi-sensor fusion system detection: When the visual-inertial subsystem fails in a dynamic environment, the laser-inertial subsystem completes the positioning, and vice versa. When both subsystems are effective at the same time, the data of the visual, laser and inertial sensors are fused in a tightly coupled manner.

[0074] S7: Construct a tightly coupled multi-sensor fusion system: First, initialize the system, including using the depth information of the laser point cloud to assist visual restoration of scale, using the loop closure detected by vision to assist the lidar to complete loop closure detection, and using the visually estimated pose to calibrate the deviation of the inertial measurement unit. Through the factor graph model, various residuals are converted into various factors, and a globally optimized factor graph model is established. The laser odometer factor, inertial measurement unit pre-integration factor, visual factor, and loop closure detection factor are added to the factor graph. The factor graph is optimized using the graph optimization method, and the optimized initial value is provided by the initial pose obtained by optimizing the visual inertial subsystem and the pose obtained by pre-integration of the inertial measurement unit. By optimizing the factor graph, the optimal pose estimation and three-dimensional map of the system can be obtained. Figure 3 The three-dimensional point cloud map established for the present invention shows that the multi-sensor fusion system only maps static point clouds, which can reduce the impact of dynamic point clouds on robot positioning. Buildings, roads and landmarks can be clearly seen in detail.

[0075] Example 2

[0076] An electronic device comprises a processor, a memory and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, a SLAM method of multi-sensor fusion in a dynamic scene is implemented in the above-mentioned embodiment 1.

[0077] The electronic device of the embodiment of the present application can be the mobile device itself, or a stand-alone device independent of the mobile device, which can communicate with the mobile device to receive the collected input signals from them and send the selected target decision behavior to them.

[0078] The electronic device includes one or more processors and memory. The processor may be a central processing unit (CPU) or other forms of processing units 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 (cache), etc. 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 run the program instructions to implement the decision behavior decision method and / or other desired functions of the various embodiments of the present application described above.

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

[0080] Example 3

[0081] In addition to the above-mentioned methods and devices, an embodiment of the present application may also be a computer program product, which includes computer program instructions, which, when executed by a processor, enable the processor to execute the steps of the decision-making behavior decision method according to various embodiments of the present application described in the above-mentioned embodiment 1 of this specification.

[0082] The computer program product may be written in any combination of one or more programming languages ​​to write program codes for performing the operations of the embodiments of the present application, including object-oriented programming languages, such as Java, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as an independent software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0083] Example 4

[0084] An embodiment of the present application may also be a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes the steps of the decision-making behavior decision method according to various embodiments of the present application described in the above-mentioned embodiment 1.

[0085] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

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

Claims

1. A multi-sensor fusion SLAM method in a dynamic scene, characterized by: The following steps are involved: Step s1: construct a multi-sensor fusion system, including a visual inertial subsystem and a laser inertial subsystem; Step s2: Build 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 obtain environmental point cloud information, establish a three-dimensional point cloud map and complete positioning; Step s4: When the visual-inertial subsystem fails to locate, the laser-inertial subsystem provides positioning information; when the laser-inertial subsystem fails to locate, the visual-inertial subsystem completes the positioning; when both the visual-inertial subsystem and the laser-inertial subsystem are valid, the visual, laser and inertial measurement unit information are fused in a tightly coupled manner to obtain the optimal posture and global map of the robot.

2. A multi-sensor fusion SLAM method in a dynamic scene, characterized by: The step s2 specifically refers to: First, extract FAST feature points from the image acquired by the camera and use the FLANN method to match the feature points; Then the visual inertial subsystem is initialized, which mainly includes: aligning the timestamps of the visual and inertial measurement unit (IMU) data, restoring the visual scale using IMU data, and calibrating the external parameters of the visual and IMU online; After the visual-inertial subsystem is initialized, the optimization equation of the visual-inertial subsystem is established. The optimization equation is as follows: in: Among them, r prior represents the prior error, constructed after marginalizing the sliding window, H p represents the marginalization matrix, r I represents the pre-integration error of the IMU, represents the reprojection error of the jth feature point, ||·|| 2 represents the bi-norm, ρ m represents the robust kernel function, χ represents the variable to be optimized, Represents the observation value of IMU, ω j represents the weight of the jth feature of the image, represents the weight of the jth feature obtained in the previous iteration, k represents the current number of iterations, and λ w ,λ m is a constant, Φ(ω j ) is a regular expression, Ψ(ω j ) is a momentum equation related to weight and number of iterations, ν(T k ,T k+1 ) is a motion compensation term; The variable χ to be optimized in the system is defined as follows: x=[x n ,x n+1 ,…,x n+N ,l m ,l m+1 …l m+M ,oh m ,oh m+1 …oh m+M ] n represents the current moment of the sliding window, N represents the size of the sliding window, which is 10 in the present invention, M represents the number of features in the sliding window, and x i Represents the state quantity that needs to be optimized, including posture, speed, accelerometer deviation and angular velocity meter deviation. λ is the inverse depth of environmental features. ω is the weight of each feature to prevent excessive weight from interfering with the entire optimization process. Here, the weight needs to be set between 0 and 1. The weight of dynamic features approaches 0, and the weight of static features approaches 1. The entire optimization is divided into two parts: optimization of state quantity and inverse depth and optimization of weight. Finally, in order to reduce the cumulative error of the robot, the visual inertial subsystem is subjected to loop detection, and the detected loops are published through topics for use by the laser inertial subsystem and the global optimization module.

3. A multi-sensor fusion SLAM method in a dynamic scene, characterized by: The step s3 further comprises the following steps: Step s31: Align the data of the lidar and IMU through the timestamp, integrate the acceleration and angular velocity obtained by the IMU to obtain the position and attitude. The calculation process is as follows: in: In the formula, Represents the rotation matrix from the world coordinate system to the robot coordinate system, t k and t k+1 Represent the time of the kth moment and the k+1th moment respectively, Δt k Represents the time interval between two IMU frames, and Represent the position vectors at time k+1 and k in the world coordinate system respectively, and Represent the postures at time k+1 and k in the world coordinate system respectively, represents 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 Represents the three-axis angular velocity output of the IMU, represents the deviation of the accelerometer at time t, represents the deviation of the angular velocity meter at time t, g w Represents the gravity acceleration vector in the world coordinate system; uses a uniform velocity model to remove motion distortion from the laser point cloud point by point. Considering that the point cloud obtained by the laser radar contains point clouds of dynamic objects, the present 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; Step s32: Construct the laser residual equation, establish the residual equation of the current frame and the previous frame, and define it as follows: In the formula, Represents the line feature residual of the laser point cloud, Represents the surface feature residual of the laser point cloud, k,i,j,l,m all represent the index of the point cloud, and Represents line features, and Representative surface features; In addition, the transformation relationship between the point cloud at the kth moment and the point cloud at the k+1th moment is as follows: Among them, R and t are the rotation matrix and translation vector of the robot, respectively, and are the variables optimized by the laser inertial subsystem; Step s33: Construct a laser odometer, and the optimization equation is as follows: Among them, ω e ,ω p ∈[0, 1] represents the weight of the point cloud feature; The Gauss-Newton method is used to optimize the optimization equation to optimize the robot's rotation matrix and translation vector. When the optimization equation converges, the robot's posture transformation relationship can be obtained.

4. A multi-sensor fusion SLAM method in a dynamic scene, characterized by: The step s4 further comprises the following steps: Step s41: completing the initialization of the multi-sensor fusion system, including aligning the timestamps of vision, lidar and IMU, and calibrating the position relationship of the three sensors; Step s42: testing 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 the two subsystems are effective at the same time, the data of 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 converted into their respective factors through the graph optimization library, and the laser odometry factor, pre-integration factor, vision factor and loop detection factor are added to the factor graph, and the global optimal position and posture of the robot are finally obtained through graph optimization.

5. A readable storage medium, characterized in that: A computer program is stored thereon, and when the computer program is executed, the SLAM method of multi-sensor fusion in a dynamic scene as described in any one of claims 1 to 4 is implemented.

6. An electronic device, characterized in that: It comprises a processor, a memory and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, a SLAM method for multi-sensor fusion in a dynamic scene as described in any one of claims 1 to 4 is implemented.

7. A computer program product, characterized in that The invention comprises a computer program / instruction, which, when executed by a processor, implements a SLAM method for multi-sensor fusion in a dynamic scene as described in any one of claims 1 to 4.

Images