High-efficiency registration laser radar and IMU mobile robot three-dimensional tight coupling SLAM method

Abstract

The invention provides a laser radar and IMU (inertial measurement unit) mobile robot three-dimensional tight coupling SLAM (simultaneous localization and mapping) method with high-efficiency registration, which relates to the technical field of robots and comprises the following steps: assembling a laser radar and an IMU sensor on a robot, connecting the laser radar and the IMU sensor with an operating system platform to receive and subscribe information data from the sensor, pre-processing data acquired by the sensor, and acquiring the acquired data; in the collection process of laser point cloud data, deformity correction is carried out through IMU data, feature extraction is carried out on the processed data, key feature points in laser point cloud are found out, the initial position of point cloud registration is set through increment of inter-frame IMU pre-integration data, and key frames are selected according to pose changes calculated by a laser speedometer. The invention provides a laser radar and IMU mobile robot three-dimensional tight coupling SLAM method and system with high-efficiency registration, and aims to solve the problem of limited positioning and map construction performance in a complex environment in the prior art and provide more accurate and reliable technical support for application of a mobile robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robotics, and particularly to a three-dimensional tightly-coupled SLAM method for a lidar and IMU mobile robot with high-efficiency registration. Background Art

[0002] With the continuous development of robotics, Simultaneous Localization and Mapping (SLAM) has become a core technology in the field of mobile robots. The SLAM technology aims to solve the problem of a robot simultaneously performing self-localization and environmental map construction in an unknown environment. In recent years, the fusion application of lidar and Inertial Measurement Unit (IMU) has received extensive attention in this field.

[0003] Lidar can provide accurate environmental distance information to help a robot perceive the surrounding environment and construct a two-dimensional or three-dimensional map. However, lidar data may be affected by distortion in a high-speed movement or complex dynamic environment, resulting in a decline in map construction and positioning accuracy. To solve this problem, researchers have begun to attempt to fuse IMU data with lidar data.

[0004] IMU can provide motion information such as the attitude, speed, and position of a robot, and its data update frequency is high, which has a good complementary effect on a robot in rapid motion. By fusing lidar and IMU data, the distortion of lidar data can be effectively corrected, and the positioning accuracy and map construction quality of the robot can be improved.

[0005] Currently, many studies have been dedicated to the fusion SLAM technology of lidar and IMU. However, existing methods still have certain deficiencies in aspects such as data preprocessing, feature extraction, key frame selection, loop detection, and global optimization, resulting in limited performance in positioning and map construction in complex environments. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a personalized questionnaire generation and intelligent diagnosis system for psychological screening, which can improve the fineness of data preprocessing to ensure the accuracy and comprehensiveness of deformation monitoring.

[0007] To solve the above technical problem, the technical solution of the present invention is as follows:

[0008] In a first aspect, a three-dimensional tightly-coupled SLAM method for a lidar and IMU mobile robot with high-efficiency registration, the method includes:

[0009] Assemble a lidar and an IMU sensor onto the robot and connect them to the operating system platform to receive and subscribe to information data from the sensors;

[0010] Preprocess the data collected by the sensors. During the acquisition process of the lidar point cloud data, distortion correction is performed using the IMU data.

[0011] Extract features from the processed data, find the key feature points in the lidar point cloud, and use the increment of the inter-frame IMU pre-integrated data to set the initial position of the point cloud registration.

[0012] According to the pose changes calculated by the lidar odometry, select key frames, use the FLANN and kd-tree structures to quickly find key frames similar to the current frame, and determine whether the robot has returned to a previous position through loop closure detection.

[0013] Through the factor graph optimization method, combine the factors of the lidar odometry, the loop closure detection factors, and the pre-integrated factors of the IMU to optimize the global pose and generate a three-dimensional point cloud map.

[0014] Furthermore, install the lidar and the IMU sensor on the robot and connect them to the operating system platform to receive and subscribe to the information data from the sensors, including:

[0015] Install the lidar on the robot so that it can scan the surrounding environment omnidirectionally.

[0016] Connect the power cable of the lidar to the power system of the robot to ensure normal operation.

[0017] Install the IMU sensor at the central position of the robot.

[0018] Connect the data cable of the IMU sensor to the data acquisition module of the robot.

[0019] Determine that the operating system of the robot supports the driver programs of the lidar and the IMU sensor.

[0020] Communicate the robot with the remote server through the wireless network to ensure that the network connection is stable and secure.

[0021] Assign a unique IP address to the robot for identification and access in the network.

[0022] Optimize the frequency and amount of data transmission according to the network bandwidth and latency to ensure real-time performance and reliability.

[0023] Test the functions of the lidar and the IMU sensor respectively to ensure that they can work properly and transmit correct data.

[0024] Connect the lidar and the IMU sensor to the operating system simultaneously to receive and subscribe to the information data from the sensors.

[0025] Further, preprocess the data collected by the sensor. During the collection of the laser point cloud data, distortion correction is performed using the IMU data, including:

[0026] The preprocessing includes methods such as removing noise points, filtering, and downsampling;

[0027] Use the IMU information to correct the distortion of the point cloud information. The formula is as follows:

[0028]

[0029] Among them, t and t k-1 respectively represent the start and end timestamps of a frame of point cloud; σ k,k-1 represents the translational change amount between the current frames; σ k-1,k-2 represents the change amount between the previous frames.

[0030] Further, the IMU data processing formula:

[0031]

[0032] Among them, ΔR ik 、Δv ij 、Δp ij represent the increments of rotation, velocity, and position between time i and j;

[0033] ΔR ik represents the change amount of the matrix R k at time point k; represents the dynamic change of a certain parameter or value at time point k;

[0034] Among them, represents the transpose of the rotation matrix at position i; p j -p i represents the position vector difference between position j and position i; v i Δt ij represents the velocity vector v at position i i multiplied by the time difference represents the displacement generated due to the action of gravity within the time t ij ; Δv ik Δt represents the velocity change amount Δv from position i to position k ik multiplied by the time difference Δt; represents the product of the rotation change amount ΔR ik and the difference between the acceleration measurement value, bias, and noise, and then multiplied by half of the square of the time difference.

[0035] Further, according to the pose change calculated by the laser odometer, key frames are selected, and the FLANN and kd-tree structures are used to quickly find key frames similar to the current frame. Loop closure detection is used to determine whether the robot has returned to a previous position, including:

[0036] By calculating the normal vector Curvature Covariance All three of them are also involved in the construction of the local map and are transformed using the direct projection method. The formula is:

[0037]

[0038] Among them, represents a certain weighted quantity or intensity, i represents a specific category or dimension, and w represents weighting; represents the base quantity or intensity, i represents the category or dimension, and L represents the "unweighted" or "base" state; represents the weighting factor or ratio;

[0039] Among them, The σ in represents the standard deviation, i represents a specific variable or data point, and w represents that the superscript may represent a certain specific situation or data set; The σ in represents the standard deviation, i represents the same variable or data point, and L represents the data set;

[0040] Among them, respectively represent the representations of a certain quantity C in the world coordinate system w and in the local coordinate system L relative to a certain reference frame i.

[0041] Further, the formula for loop closure detection:

[0042]

[0043] Among them, d(I q ,I c ) represents the conditional entropy; N s represents calculating a value related to the conditional entropy each time; respectively represent the entropy values under specific conditions; represents the modulus of the entropy value; represents that the result of the summation is divided by N s .

[0044] Further, through the factor graph optimization method, the factors of the laser odometer, the loop closure detection factors, and the pre-integration factors of the IMU are combined to optimize the global pose and generate a three-dimensional point cloud map, including:

[0045] The factor formula of the laser odometer is:

[0046]

[0047] Among them, L(χ) represents a linear transformation; represents a transformation increment from time step i to time step i + 1, and W represents a specific matrix; and represents the transformation matrix from time step i to time step i + 1; represents the transpose matrix of

[0048] The formula for the loop detection factor is:

[0049] G(X) = ΔT i,j = (T i ) T ΔTT j ;

[0050] Among them, G(X) represents the value of a function G at X; ΔT i,j is the element in the i-th row and j-th column of the matrix ΔT; T represents a matrix, and T i represents the i-th row of T, which is a row vector; T j represents the j-th column of T, which is a column vector; (T i ) T represents the transpose of T i , that is, changing from a row vector to a column vector; (T i ) T ΔTT j represents the multiplication of a row vector and a column vector;

[0051] The formula for the pre-integration factor of the IMU is:

[0052] I(X) = [(ΔR i,i+1 ) T , (Δv i,i+1 ) T , (Δp i,i+1 ) T T ;

[0053] Among them, I(X) is a matrix composed of three transposed matrices or vectors; ΔR i,i+1 represents a rotation change matrix, indicating the rotation change from time i to time i + 1; Δv i,i+1 represents a velocity change vector, indicating the velocity change from time i to time i + 1; Δp i,i+1 represents a position change vector, indicating the position change from time i to time i + 1; T means that each matrix or vector is first transposed.

[0054] ​Second aspect, a three-dimensional tightly coupled SLAM system for a lidar and IMU mobile robot with high-efficiency registration, which is applied to the method described above, includes:

[0055] Mount the lidar and IMU sensors on the robot and connect them to the operating system platform, and the operating system needs to be able to smoothly receive and subscribe to the information data from the sensors;

[0056] Preprocess the data collected by the sensors, and correct the distortion of the lidar point cloud data through the IMU data during the collection process;

[0057] Extract features from the processed data, find the key feature points in the lidar point cloud, and use the increment of the inter-frame IMU pre-integration data to set the initial position of the point cloud registration;

[0058] According to the pose change calculated by the lidar odometry, select key frames, use the FLANN and kd-tree structures to quickly find key frames similar to the current frame, and judge whether the robot has returned to the previous position through loop detection;

[0059] Through the factor graph optimization method, combine the factors of the lidar odometry, the loop detection factors, and the pre-integration factors of the IMU to optimize the global pose and generate a three-dimensional point cloud map.

[0060] Third aspect, a computing device, includes:

[0061] One or more processors;

[0062] A storage device for storing one or more programs, and when the one or more programs are executed by the one or more processors, the one or more processors implement the method as claimed in claim 8.

[0063] Fourth aspect, a computer-readable storage medium, in which a program is stored, and when the program is executed by a processor, the method described above is implemented.

[0064] The above solution of the present invention has at least the following beneficial effects.

[0065] 1. The present invention realizes precise environmental perception and positioning by efficiently integrating lidar and IMU sensor data. By assembling lidar and IMU sensors onto a robot and connecting them to an operating system platform, the robot can receive and subscribe to sensor information in real time. During the acquisition process of lidar point cloud data, distortion correction is performed using IMU data, improving the accuracy of the data. In addition, the increment of inter-frame IMU pre-integrated data is used to set the initial position of point cloud registration, further enhancing the accuracy of positioning. This efficient data integration and processing method enables the robot to achieve more reliable and accurate positioning and navigation in complex environments.

[0066] 2. The present invention adopts a factor graph optimization method, which combines factors of lidar odometry, loop closure detection factors, and IMU pre-integration factors to optimize the global pose and generate a high-quality three-dimensional point cloud map. By selecting key frames and using FLANN and kd-tree structures to quickly find similar key frames, the efficiency of map construction is improved. At the same time, the loop closure detection mechanism effectively determines whether the robot returns to a previous position, avoiding map drift and error accumulation. This optimization method not only improves the accuracy and consistency of the map but also provides reliable map support for the path planning and navigation of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 FIG. is a schematic flow chart of a three-dimensional tightly coupled SLAM method for a lidar and IMU mobile robot with high-efficiency registration provided by an embodiment of the present invention.

[0068] Figure 2 FIG. is a schematic diagram of a three-dimensional tightly coupled SLAM system for a lidar and IMU mobile robot with high-efficiency registration provided by an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0069] Hereinafter, exemplary embodiments of the present disclosure will be described in more detail with reference to the accompanying drawings. Although the exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided so that the present disclosure can be more thoroughly understood and the scope of the present disclosure can be completely conveyed to those skilled in the art.

[0070] As Figure 1 shown, an embodiment of the present invention proposes a three-dimensional tightly coupled SLAM method for a lidar and IMU mobile robot with high-efficiency registration, and the method includes the following steps:

[0071] Assemble a lidar and an IMU sensor onto a robot and connect them to an operating system platform to receive and subscribe to information data from the sensors;

[0072] Preprocess the data collected by the sensors. During the acquisition of the LiDAR point cloud data, perform distortion correction through the IMU data;

[0073] Extract features from the processed data, find the key feature points in the LiDAR point cloud, and use the increment of the inter-frame IMU pre-integrated data to set the initial position of the point cloud registration;

[0074] According to the pose changes calculated by the LiDAR odometry, select the key frames, use the FLANN and kd-tree structures to quickly find the key frames similar to the current frame, and judge whether the robot has returned to the previous position through loop detection;

[0075] Through the factor graph optimization method, combine the factors of the LiDAR odometry, the loop detection factors, and the pre-integrated factors of the IMU to optimize the global pose and generate a 3D point cloud map.

[0076] In the embodiment of the present invention, by integrating the LiDAR and the IMU sensor, an efficient and accurate robot positioning and mapping system is constructed. This system can receive and process sensor data in real time, perform distortion correction on the LiDAR point cloud through the IMU data, and improve the data accuracy. At the same time, by using feature extraction and inter-frame IMU pre-integrated data, high-precision initial positioning of the point cloud registration is achieved. Through the LiDAR odometry, loop detection, and factor graph optimization methods, the system can generate a high-quality 3D point cloud map, effectively improving the navigation and positioning capabilities of the robot in complex environments.

[0077] In a preferred embodiment of the present invention, install the LiDAR on the robot so that it can scan the surrounding environment omnidirectionally;

[0078] Connect the power cord of the LiDAR to the power system of the robot to ensure normal operation;

[0079] Assemble the IMU sensor at the center position of the robot;

[0080] Connect the data cable of the IMU sensor to the data acquisition module of the robot;

[0081] Determine that the operating system of the robot supports the driver programs of the LiDAR and the IMU sensor;

[0082] Connect the robot to communicate with the remote server through the wireless network, and ensure that the network connection is stable and secure;

[0083] Assign a unique IP address to the robot for identification and access in the network;

[0084] According to the network bandwidth and latency, optimize the frequency and amount of data transmission to ensure real-time performance and reliability;

[0085] Test the functions of the lidar and the IMU sensor separately to determine that they can work properly and transmit correct data;

[0086] Connect the lidar and the IMU sensor to the operating system simultaneously to receive and subscribe to the information data from the sensors.

[0087] In the embodiment of the present invention, the IMU sensor is assembled at the central position of the robot, which helps to accurately capture the overall motion state and posture changes of the robot, improve the accuracy and stability of dynamic perception. The data line of the IMU sensor is directly connected to the data acquisition module of the robot to ensure that data can be transmitted efficiently and accurately, reduce the delay and error in the transmission process. Confirm that the operating system of the robot supports the driver programs of the lidar and the IMU sensor to ensure seamless compatibility between the sensors and the system, improve the fluency and stability of the overall operation. The robot communicates with the remote server through the wireless network to achieve remote monitoring and control functions, and at the same time ensure the stability and security of the network connection, which is convenient for remote management and operation. Assign a unique IP address to the robot to facilitate accurate identification and access to the robot in the network, improve the convenience and efficiency of network management. Optimize the frequency and amount of data transmission according to the network bandwidth and delay conditions to ensure the real-time and reliability of the data, meet the requirements of the robot for data processing and response speed. Test the functions of the lidar and the IMU sensor separately to ensure that they can work properly and transmit accurate data, providing reliable support for the perception and decision-making of the robot. The lidar and the IMU sensor are connected to the operating system simultaneously to realize the fusion processing of multi-sensor information, provide more comprehensive and accurate environmental perception data, and enhance the perception ability and decision-making accuracy of the robot.

[0088] In a preferred embodiment of the present invention, the preprocessing includes noise point removal, filtering and downsampling processing methods;

[0089] Use the IMU information to correct the distortion of the point cloud information. The formula is as follows:

[0090]

[0091] Among them, t and t k-1 respectively represent the start and end timestamps of a frame of point cloud; σ k,k-1 represents the translational change amount between the current frames; σ k-1,k-2 represents the change amount between the previous frames.

[0092] In the embodiments of the present invention, by using IMU information for point cloud distortion correction, the distortion of point cloud data caused by the movement of the robot can be effectively reduced or eliminated, thereby improving the accuracy and reliability of the point cloud data. The IMU data processing formula can calculate the rotation, speed, and position increment of the robot in real time, so as to quickly and accurately compensate for the dynamic changes of the robot. By combining the data of the lidar and the IMU sensor, the effective fusion of multi-source information can be achieved. The IMU sensor has a high update frequency and response speed, and can provide stable dynamic information when the lidar data is limited or interfered. Accurate IMU data processing and point cloud distortion correction help to improve the positioning accuracy and map construction quality of the robot. By reducing distortion and dynamic errors, the robot can more accurately draw a three-dimensional map of the surrounding environment, providing strong support for autonomous navigation and path planning.

[0093] In a preferred embodiment of the present invention, the IMU data processing formula:

[0094]

[0095] Where, ΔR ik , Δv ij , Δp ij represent the increments of rotation, speed, and position between time points i and j;

[0096] ΔR ik represents the change amount of matrix R k at time point k; represents the dynamic change of a certain parameter or value at time point k;

[0097] Where, represents the transpose of the rotation matrix at position i; p j -p i represents the position vector difference between position j and position i; v i Δt ij represents the velocity vector v at position i i multiplied by the time difference represents the displacement generated due to the action of gravity within time t ij ; Δv ik Δt represents the change amount of velocity Δv from position i to position k ik multiplied by the time difference Δt; represents the product of the rotation change amount ΔR ik and the difference between the acceleration measurement value, bias, and noise, and then multiplied by half of the square of the time difference.

[0098] In an embodiment of the present invention, this formula can accurately calculate the increments of rotation, speed, and position from one moment to another. Each parameter and variable in the formula can capture the dynamic changes of the robot at different time points in real time, including rotation, speed, and position. The formula specifically considers the influence of gravity on the displacement of the robot, compensates for the change in the actual position by calculating the displacement generated by gravity. When calculating the relationship between the change in rotation and acceleration, the formula suppresses the influence of these adverse factors by considering the differences in acceleration measurements, biases, and noises, and multiplying by half of the square of the time difference. This formula comprehensively considers multiple factors such as the rotation, speed, position, and gravity of the robot, providing a powerful tool for comprehensively analyzing the motion state of the robot.

[0099] In a preferred embodiment of the present invention, by calculating the normal vector Curvature Covariance These three also participate in the construction of the local map and are transformed using the direct projection method. The formula is:

[0100]

[0101] Among them, represents a certain weighted quantity or intensity, i represents a specific category or dimension, and w represents weighting; represents the basic quantity or intensity, i represents the category or dimension, and L represents the "unweighted" or "basic" state; represents the weighting factor or ratio;

[0102] Among them, In, σ represents the standard deviation, i represents a specific variable or data point, and w represents the superscript that may represent a certain specific situation or data set; In, σ represents the standard deviation, i represents the same variable or data point, and L represents the data set;

[0103] Among them, respectively represent the representations of a certain quantity C in the world coordinate system w and in the local coordinate system L with respect to a certain reference frame i.

[0104] In an embodiment of the present invention, by calculating the normal vector, curvature, and covariance, the geometric characteristics and spatial relationships of each point in the map can be described more accurately. Using the direct projection method for transformation, the map data can be flexibly converted from one coordinate system to another, enabling the map to adapt to different application scenarios and requirements. By introducing a weighting factor or ratio, targeted weighting processing can be performed on the map data. Considering the standard deviation in the calculation process can quantify the degree of dispersion of the data, and thus evaluate the stability and reliability of the map data. This formula supports the fusion processing of data from different sources and in different formats to generate a unified local map.

[0105] In a preferred embodiment of the present invention, the formula for loop detection is:

[0106]

[0107] where d(I q , I c ) represents conditional entropy; N s represents calculating a value related to conditional entropy each time; respectively represent entropy values under specific conditions; represents the modulus of the entropy value; represents that the result of the summation is divided by N s .

[0108] In an embodiment of the present invention, by calculating conditional entropy, this formula can accurately measure the uncertainty of information under specific conditions. Each calculation in the formula is related to conditional entropy, which means it can remain efficient when processing a large amount of data. Since the formula takes into account the entropy values under specific conditions, it has good flexibility and adaptability. By performing modulus operation on the entropy values, summing them up, and then dividing by the corresponding quantity, this formula can, to a certain extent, suppress the influence of noise and outliers. The result of loop detection is crucial for the path planning and navigation of the robot.

[0109] In a preferred embodiment of the present invention, the factor formula of the laser odometer is:

[0110]

[0111] where L(χ) represents a linear transformation; represents a transformation increment from time step i to time step i + 1, and W represents a specific matrix; and represent the transformation matrices from time step i to time step i + 1; represents the transpose matrix of;

[0112] The factor formula for loop detection is:

[0113] G(X) = ΔT i,j = (T i ) T ΔTT j ;

[0114] where G(X) represents the value of a function G at X; ΔT i,j is the element in the i-th row and j-th column of the matrix ΔT; T represents a matrix, T i represents the i-th row of T, which is a row vector; T j represents the j-th column of T, which is a column vector; (Ti ) T represents the transpose of T i , i.e., changing from a row vector to a column vector; (T i ) T ΔTT j represents the multiplication of a row vector by a column vector;

[0115] The pre-integration factor formula of the IMU is:

[0116] I(X) = [(ΔR i,i+1 ) T ,(Δv i,i+1 ) T ,(Δp i,i+1 ) T ) T ;

[0117] where I(X) is a matrix composed of three transposed matrices or vectors; ΔR i,i+1 represents a rotation change matrix, indicating the rotation change from time i to time i + 1; Δv i,i+1 represents a velocity change vector, indicating the velocity change from time i to time i + 1; Δp i,i+1 represents a position change vector, indicating the position change from time i to time i + 1; T means that each matrix or vector is first transposed.

[0118] In the embodiments of the present invention, through linear transformation and matrix operations, the laser odometry factor formula can accurately estimate the motion state of the robot, including position, velocity, and orientation, etc., providing important information for the autonomous navigation and positioning of the robot. This formula expresses the motion changes of the robot through concise matrix operations, making the calculation process efficient and highly real-time, and is applicable to robot systems that require quick response. The linear transformation and matrix operations in the formula can adapt to different motion models and scenarios, enabling the laser odometry to provide accurate motion estimation in various environments. By calculating the product of the function value and the matrix transpose, this formula can accurately detect whether the robot has returned to a previously visited position, that is, loop detection is achieved, which helps to improve the accuracy and consistency of map construction. This formula processes data through matrix operations, can suppress the influence of noise and errors to a certain extent, and improves the robustness of loop detection. The loop detection factor formula is implemented through concise matrix operations, with high calculation efficiency and can meet the real-time requirements. By considering the changes in rotation, velocity, and position, this formula can accurately calculate the pre-integration result of the IMU over a period of time, providing an accurate data basis for subsequent robot state estimation. Through the pre-integration technology, the IMU data over a period of time can be integrated to reduce the calculation amount in the subsequent optimization process and improve the real-time performance. The IMU pre-integration factor formula can make full use of the high-frequency data characteristics of the IMU to improve the stability and robustness of the robot in complex environments.

[0119] As Figure 2 shown, the embodiments of the present invention also provide a personalized questionnaire intelligent diagnosis system for psychological screening, including:

[0120] Install the lidar and IMU sensors on the robot and connect them to the operating system platform to receive and subscribe to information data from the sensors;

[0121] Preprocess the data collected by the sensors, and correct the distortion of the lidar point cloud data through the IMU data during the collection process;

[0122] Extract features from the processed data, find the key feature points in the lidar point cloud, and use the increment of the inter-frame IMU pre-integration data to set the initial position of the point cloud registration;

[0123] According to the pose changes calculated by the laser odometry, select key frames, use the FLANN and kd-tree structures to quickly find key frames similar to the current frame, and judge whether the robot has returned to the previous position through loop detection;

[0124] Through the factor graph optimization method, combine the factors of the laser odometry, the loop detection factor, and the pre-integration factor of the IMU to optimize the global pose and generate a three-dimensional point cloud map.

[0125] It should be noted that this system corresponds to the above method, and all implementation manners in the above method embodiments are applicable to this embodiment and can achieve the same technical effects.

[0126] The above is the preferred embodiment of the present invention. It should be pointed out that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A three-dimensional tightly-coupled SLAM method for a lidar and IMU mobile robot with high-efficiency registration, characterized in that, Including: Assemble a lidar and an IMU sensor onto a robot and connect them to an operating system platform to receive and subscribe to information data from the sensors; Preprocess the data collected by the sensors. During the collection of lidar point cloud data, perform distortion correction using IMU data; Extract features from the processed data, find the key feature points in the lidar point cloud, and use the increment of inter-frame IMU pre-integrated data to set the initial position of point cloud registration; According to the pose change calculated by the lidar odometry, select key frames, and use the FLANN and kd-tree structures to quickly find key frames similar to the current frame, and determine whether the robot has returned to a previous position through loop closure detection; Through the factor graph optimization method, combine the factors of lidar odometry, loop closure detection factors, and IMU pre-integrated factors to optimize the global pose and generate a three-dimensional point cloud map.

2. The three-dimensional tight-coupling SLAM method for a lidar and IMU mobile robot with high-efficiency registration according to claim 1, characterized in that, Assemble a lidar and an IMU sensor onto a robot and connect them to an operating system platform to receive and subscribe to information data from the sensors, including: Install the lidar on the robot to enable omnidirectional scanning of the surrounding environment; Connect the power cord of the lidar to the power system of the robot to ensure normal operation; Assemble the IMU sensor at the central position of the robot; Connect the data cable of the IMU sensor to the data acquisition module of the robot; Determine that the operating system of the robot supports the driver programs of the lidar and the IMU sensor; Communicate the robot with a remote server through a wireless network to determine that the network connection is stable and secure; Assign a unique IP address to the robot for identification and access in the network; Optimize the frequency and amount of data transmission according to the network bandwidth and latency to ensure real-time performance and reliability; Test the functions of the lidar and the IMU sensor respectively to determine that they can operate normally and transmit correct data; Simultaneously connect the lidar and the IMU sensor to the operating system to receive and subscribe to information data from the sensors.

3. The high-efficiency registration lidar and IMU mobile robot three-dimensional tight-coupling SLAM method according to claim 2, characterized in that, Preprocess the data collected by the sensors. During the collection of lidar point cloud data, perform distortion correction using IMU data, including: The preprocessing includes methods such as removing noise points, filtering, and downsampling; Use IMU information for point cloud information distortion correction. The formula is as follows: Among them, t and t k-1 respectively represent the start and end timestamps of a frame of point cloud; σ k,k-1 represents the translational change amount between the current frames; σ k-1,k-2 represents the change amount between the previous frames.

4. The high-efficiency registration lidar and IMU mobile robot three-dimensional tight-coupling SLAM method according to claim 3, characterized in that, IMU data processing formula: where, ΔR ik , Δv ij , Δp ij represent the increments of rotation, velocity, and position from time i to time j; ΔR ik represents the matrix R k the change amount at time point k; represents the dynamic change of a certain parameter or value at time point k; Among them, represents the transpose of the rotation matrix at position i; p j -p i represents the position vector difference between position j and position i; v i Δt ij represents the velocity vector v at position i i multiplied by the time difference Δt ij ; represents the displacement generated by the action of gravity within time t ij ; Δv ik Δt represents the change in velocity Δv from position i to position k ik multiplied by the time difference Δt; represents the change in rotation ΔR ik which is the product of the difference between the acceleration measurement, bias, and noise, and then multiplied by half of the square of the time difference.

5. The method for three-dimensional tight-coupling SLAM of a lidar and an IMU mobile robot with high-efficiency registration according to claim 4, wherein According to the pose change calculated by the lidar odometry, select key frames, and use the FLANN and kd-tree structures to quickly find key frames similar to the current frame, and determine whether the robot has returned to a previous position through loop closure detection, including: By calculating the normal vector Curvature Covariance All three of them also participate in the construction of the local map and are transformed using the direct projection method. The formula is as follows: Among them, represents a certain weighted quantity or intensity, i represents a specific category or dimension, and w represents weighting; represents the base quantity or intensity, i represents the category or dimension, and L represents the "unweighted" or "base" state; represents the weighting factor or ratio; Among them, in which, σ represents the standard deviation, i represents a specific variable or data point, and w represents a superscript that may represent a certain specific situation or data set; in which, σ represents the standard deviation, i represents the same variable or data point, and L represents the data set; wherein, respectively represent the representations of a certain quantity C in the world coordinate system w and the local coordinate system L with respect to a certain reference system i.

6. The three-dimensional tight-coupling SLAM method for a lidar and IMU mobile robot with high-efficiency registration according to claim 5, characterized in that, Formula for loop closure detection: Among them, d(I q , I c ) represents the conditional entropy; N s represents calculating a value related to the conditional entropy each time; respectively represent the entropy values under specific conditions; represents the modulus of the entropy value; represents that the result of the summation is divided by N s .

7. The method for three-dimensional tight coupling SLAM of a lidar and an IMU mobile robot with high-efficiency registration according to claim 6, wherein Through the factor graph optimization method, combine the factors of lidar odometry, loop closure detection factors, and IMU pre-integrated factors to optimize the global pose and generate a three-dimensional point cloud map, including: The factor formula of lidar odometry is: Among them, \(L(\chi)\) represents a linear transformation; represents a transformation increment from time step \(i\) to time step \(i + 1\), and \(W\) represents a specific matrix; and represents the transformation matrix from time step \(i\) to time step \(i + 1\); represents the transpose matrix of The loop closure detection factor formula is: G(X) = ΔT i,j = (T i ) T ΔTT j ; where G(X) represents the value of a function G at X; ΔT i,j is the element in the i-th row and j-th column of the matrix ΔT; T represents a matrix, T i represents the i-th row of T, which is a row vector; T j represents the j-th column of T, which is a column vector; (T i ) T represents T i 's transpose, that is, changing from a row vector to a column vector; (T i ) T ΔTT j represents the multiplication of a row vector and a column vector; The IMU pre-integrated factor formula is: I(X) = [(ΔR i,i+1 ) T , (Δv i,i+1 ) T , (Δp i,i+1 ) T T ;​ where, I(X) is a matrix composed of three transposed matrices or vectors; ΔR i,i+1 represents a rotation transformation matrix, representing the rotation transformation from time i to time i + 1; Δv i,i+1 represents a velocity change vector, representing the velocity change from time i to time i + 1; Δp i,i+1 represents a position change vector, representing the position change from time i to time i + 1; T means that each matrix or vector is first transposed.

8. A three-dimensional tightly-coupled SLAM system for a lidar and IMU mobile robot with high-efficiency registration, characterized in that, Applied to the method according to any one of claims 1 to 7, including: Assemble a lidar and an IMU sensor onto a robot and connect them to an operating system platform, which needs to be able to smoothly receive and subscribe to information data from the sensors; Preprocess the data collected by the sensors. The lidar point cloud data is corrected for distortion during acquisition using IMU data; Extract features from the processed data, find the key feature points in the lidar point cloud, and use the increment of the inter-frame IMU pre-integrated data to set the initial position for point cloud registration; Select key frames based on the pose changes calculated by the lidar odometry, use the FLANN and kd-tree structures to quickly find key frames similar to the current frame, and determine whether the robot has returned to a previous position through loop closure detection; Combine the factors of the lidar odometry, the loop closure detection factors, and the IMU pre-integration factors through the factor graph optimization method to optimize the global pose and generate a three-dimensional point cloud map.

9. A computing device, characterized in that, Comprising: One or more processors; A storage device for storing one or more programs, which when executed by the one or more processors cause the one or more processors to implement the method as claimed in claim 7.

10. A computer-readable storage medium, characterized in that, A program is stored in the computer-readable storage medium, and when the program is executed by a processor, the method as claimed in claim 7 is implemented.

Images