A method, apparatus, and device for simultaneous localization and mapping (SLAM) of a mobile robot.

Abstract

This invention discloses a method, apparatus, and device for simultaneous localization and mapping (SLAM) of a mobile robot. The method involves acquiring depth information, LiDAR scanning information, and odometry information of the robot's environment; performing 3D projection on sparse point cloud data to obtain 2D information of the robot's environment; performing spatiotemporal synchronization processing on the 2D information, LiDAR scanning information, and odometry information; establishing pose nodes using the spatiotemporally synchronized 2D information, LiDAR scanning information, and odometry information; performing point cloud registration on the spatiotemporally synchronized 2D information and LiDAR scanning information using an iterative nearest point method; optimizing the pose nodes in real time using a second iterative nearest point method; and drawing a map based on the optimized pose nodes and pose transformation matrix; and completing loop closure screening using a third iterative nearest point method, with the selected loops used for loop closure detection. This invention can simultaneously leverage the advantages of LiDAR and visual sensors in unknown and complex environments.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot synchronous localization and mapping technology, specifically relating to a method, apparatus and device for mobile robot synchronous localization and mapping. Background Technology

[0002] Autonomous mobile robots are among the most common intelligent robots, widely used in industrial manufacturing, logistics, home services, and entertainment. The ability of mobile robots to accurately and in real-time locate their own position in unknown and complex environments, and to perceive and construct accurate and complete environmental maps, is a key technology for achieving intelligence in mobile robots. The accuracy and completeness of simultaneous localization and map building are prerequisites for autonomous navigation of mobile robots.

[0003] Currently, the application scenarios of mobile robots are becoming increasingly complex, making it difficult for SLAM algorithms based on a single sensor to operate robustly in complex environments. For example, visual cameras may fail to track in scenarios with insufficient lighting, missing features, or interference from moving objects; two-dimensional LiDAR can only scan planar information and cannot fully reconstruct scene information; and inertial measurement units (IMUs) have zero bias, and using integration to solve pose will produce cumulative errors. Summary of the Invention

[0004] To address the problems existing in the prior art, the present invention provides a method, apparatus and device for simultaneous localization and mapping of mobile robots, which can simultaneously leverage the advantages of lidar and visual sensors in unknown and complex environments to accurately and in real time locate its own position and perceive and construct an accurate and complete environmental map.

[0005] To solve the above-mentioned technical problems, the present invention is achieved through the following technical solution:

[0006] A method for simultaneous localization and mapping (SLAM) of a mobile robot includes:

[0007] Acquire depth information, LiDAR scanning information, and mileage information of the environment in which the mobile robot is located;

[0008] The depth information is converted into point cloud data, and the point cloud data is downsampled to obtain sparse point cloud data.

[0009] The sparse point cloud data is projected in three dimensions to obtain two-dimensional information about the environment in which the mobile robot is located.

[0010] The two-dimensional information, the lidar scanning information, and the mileage information are subjected to spatiotemporal synchronization processing;

[0011] Pose nodes are established using the spatiotemporally synchronized two-dimensional information, lidar scanning information, and odometer information.

[0012] The iterative nearest point method is used to perform point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information to obtain the pose transformation matrix of the mobile robot;

[0013] The pose nodes are optimized in real time using the quadratic iterative nearest point method, and a map is drawn based on the optimized pose nodes and the pose transformation matrix.

[0014] The loop closure screening is performed using the three-iteration nearest point method. The screened loops are used for loop closure detection to correct the error of the map.

[0015] Further, the step of downsampling the point cloud data to obtain sparse point cloud data specifically involves:

[0016] The point cloud data is downsampled using a voxel filtering method to obtain sparse point cloud data.

[0017] Furthermore, the real-time optimization of the pose node using the quadratic iterative nearest point method includes:

[0018] The two-dimensional information and lidar scanning information after spatiotemporal synchronization processing at time i+1 are transformed into the global coordinate system, denoted as Z. i+1 Find the nearest neighbor points of each laser point within a set radius on the map, transfer all nearest neighbor points to the local coordinate system and save them as m. i+1 ;

[0019] Z i+1 and m i+1 As input to formula (1), the transformation matrix T for the registration of two frames of point cloud data is obtained. opt ;

[0020]

[0021] The transformation matrix T opt Substitute into formula (2) to optimize the pose of the pose node at time i+1;

[0022]

[0023] Among them, T i W This represents the robot's pose in the global coordinate system at time i; Z represents the pose transformation matrix of the mobile robot from time i to time i+1, which is the pose transformation matrix of the mobile robot obtained by point cloud registration of the spatiotemporally synchronized two-dimensional information and the lidar scanning information using the iterative nearest point method; i+1 This represents the laser observation data at time i+1.

[0024] Furthermore, the approximate nearest neighbor algorithm in the PCL library is used to find the nearest neighbor points of each laser point within a set radius on the map.

[0025] Furthermore, the loop closure screening is performed using the three-iteration nearest point method, and the screened loops are used for loop closure detection to correct the error of the map, including:

[0026] Determine the key frame Mainscan and the processing frequency f, wherein the key frame is defined as a frame after the mobile robot has traveled a set distance. The key frame that meets the processing frequency f is then subjected to three subsequent iterations of nearest point matching.

[0027] During the three iterations of nearest point matching, all historical keyframes are traversed and matched. By using proximity detection based on LiDAR, the historical keyframes that are closest to the keyframes determined by the current mobile robot are found. Then, the ICP algorithm is applied to solve the transformation matrix and registration distance. Historical keyframes with a registration distance less than a set threshold are added to the loop closure candidate set. The selected loops are used for loop closure detection to complete the error correction of the map.

[0028] A mobile robot synchronous localization and mapping device, comprising:

[0029] The acquisition module is used to acquire depth information, LiDAR scanning information, and mileage information of the environment in which the mobile robot is located;

[0030] The preprocessing module is used to convert the depth information into point cloud data, and to downsample the point cloud data to obtain sparse point cloud data.

[0031] The projection module is used to project the sparse point cloud data in three dimensions to obtain two-dimensional information about the environment in which the mobile robot is located.

[0032] The synchronization module is used to perform spatiotemporal synchronization processing on the two-dimensional information, the lidar scanning information, and the mileage information;

[0033] The pose node establishment module is used to establish pose nodes using the spatiotemporally synchronized two-dimensional information, lidar scanning information and odometer information.

[0034] The registration module is used to perform point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information using the iterative nearest point method to obtain the pose transformation matrix of the mobile robot.

[0035] The optimization module is used to optimize the pose nodes in real time using the quadratic iterative nearest point method, and to draw a map based on the optimized pose nodes and the pose transformation matrix.

[0036] The correction module is used to perform loop closure screening using the three-iteration nearest point method. The screened loops are used for loop closure detection to complete the error correction of the map.

[0037] An apparatus includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the mobile robot synchronous localization and mapping method.

[0038] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the mobile robot synchronous localization and map building method.

[0039] Compared with the prior art, the present invention has at least the following beneficial effects:

[0040] This invention provides a method for simultaneous localization and mapping (SLAM) of a mobile robot. It acquires depth information, LiDAR scanning information, and odometry information of the robot's environment and preprocesses this information. Pose nodes are then established using the preprocessed 2D information, LiDAR scanning information, and odometry information. An iterative nearest-point method is used to register the spatiotemporally synchronized 2D information and LiDAR scanning information into point clouds, resulting in the robot's pose transformation matrix. A second iterative nearest-point method is used to optimize the pose nodes in real time, and a map is drawn based on the optimized pose nodes and the pose transformation matrix. A third iterative nearest-point method is used to filter loop closures, which are then used for loop closure detection to correct errors in the map. This allows the mobile robot to leverage the advantages of both LiDAR and vision sensors in unknown and complex environments, accurately and in real-time locating its own position and perceiving and constructing an accurate and complete environmental map. This invention is simple in principle and easy to implement, solving the problems of weak anti-interference capability and environmental information loss in single-sensor SLAM algorithms in complex environments. It achieves high-precision localization and robust multimodal SLAM, meeting the real-time performance requirements of mobile robots in practical applications.

[0041] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0042] To more clearly illustrate the technical solutions in the specific embodiments of the present invention, the drawings used in the description of the specific embodiments will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0043] Figure 1 A flowchart illustrating a method for simultaneous localization and mapping of a mobile robot.

[0044] Figure 2 This is a principle block diagram of a mobile robot synchronous localization and map building method according to the present invention;

[0045] Figure 3 shows the physical image of the sensor and the schematic diagram of the joint calibration; among them, Figure 3(a) is the Kinect V2 depth camera image, Figure 3(b) is the physical image of the RPlidar a2 lidar, Figure 3(c) is the camera intrinsic parameter calibration diagram, and Figure 3(d) is the joint calibration diagram;

[0046] Figure 4 shows the processing results of voxel filtering after downsampling the point cloud data. Figure 4(a) is the initial point cloud image obtained by the depth camera, and Figure 4(b) is the image after voxel filtering is applied to the initial point cloud.

[0047] Figure 5(a) is a schematic diagram of approximate synchronization, and Figure 5(b) is a schematic diagram of the closest matching process in the second iteration;

[0048] Figure 6 compares the single-modal Gmapping algorithm with the synchronous localization and mapping method for mobile robots of this invention in the simulation environment of the robot operating system Gazebo. Figure 6(a) and 6(b) These are world model diagrams created from different perspectives. Figure 6(c) is the map constructed by the Gmapping algorithm, and Figure 6(d) is the map constructed by this invention.

[0049] Figure 7 compares the maps constructed by the single-modal Gmapping algorithm and the multi-modal fusion algorithm proposed in this invention under real-world conditions. Figure 7(a) and 7(b) Figure 7(c) shows an environmental map constructed using the Gmapping algorithm, and Figure 7(d) shows an environmental map constructed using the fusion algorithm proposed in this invention. Figure 7(e) and 7(f) These are magnified views of the environmental maps constructed by the two algorithms. Detailed Implementation

[0050] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions 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, 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.

[0051] To meet diverse application scenarios and functional requirements, mobile robots typically employ multiple sensors to acquire their own position and environmental information. Furthermore, by integrating multimodal information based on the strengths and weaknesses of each sensor, the robot's navigation accuracy, stability, and anti-interference capabilities can be improved in complex and open industrial environments, enabling intelligent navigation in a wider range of applications. This invention researches a machine vision and LiDAR multimodal SLAM technology for dynamic and complex scenarios. This technology is simple in principle and easy to implement, addressing the issues of weak anti-interference capabilities and environmental information loss inherent in single-sensor SLAM algorithms in complex environments. It achieves high-precision positioning and robust multimodal SLAM, meeting the real-time performance requirements of mobile robots in practical applications.

[0052] As a specific embodiment of the present invention, such as Figure 1 As shown, a method for simultaneous localization and mapping (SLAM) of a mobile robot includes the following steps:

[0053] Step 1: Obtain depth information, LiDAR scanning information, and mileage information of the environment in which the mobile robot is located.

[0054] In this embodiment, the Turtlebot mobile robot is used in conjunction with the RPlidar a2 lidar and Kinect V2 depth camera to obtain the above information.

[0055] Step 2: Convert the depth information into point cloud data, and downsample the point cloud data to obtain sparse point cloud data.

[0056] Specifically, the point cloud data is downsampled using a voxel filtering method to obtain sparse point cloud data.

[0057] Step 3: Perform a 3D projection on the sparse point cloud data to obtain 2D information of the environment in which the mobile robot is located.

[0058] In this embodiment, information about a set height is extracted from sparse point cloud data and converted into two-dimensional laser scanning data in the robot operating system to obtain two-dimensional information about the environment in which the mobile robot is located.

[0059] Step 4: Perform spatiotemporal synchronization processing on the two-dimensional information, the lidar scanning information, and the mileage information.

[0060] In this embodiment, time synchronization is achieved using an approximate synchronization method, as shown in Figure 5(a). Approximate synchronization is a synchronization method for acquiring data from multiple sensors near a certain moment. The two-dimensional information, the lidar scanning information, and the odometer information are stored in a stack data structure according to the order in which they are acquired. Once the stacks storing the three types of sensor data are full (i.e., the stacks are not empty), the latest data (the data at the top of the stack) is retrieved, and the timestamp is obtained. The timestamps of the three types of sensor data are calculated. If the three timestamps differ significantly, the current time is obtained using the Robot Operating System (ROS) to assign values ​​to each data timestamp. Through the above steps, approximate synchronization of sensor perception information near a certain moment can be achieved.

[0061] In this embodiment, spatial synchronization is achieved by using a joint calibration method of lidar and depth camera.

[0062] Step 5: Establish pose nodes using the spatiotemporally synchronized two-dimensional information, lidar scanning information, and odometer information.

[0063] Step 6: Use the iterative nearest point method to perform point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information to obtain the pose transformation matrix of the mobile robot.

[0064] Step 7: Use the quadratic iterative nearest point method to optimize the pose nodes in real time, and draw a map based on the optimized pose nodes and the pose transformation matrix.

[0065] In this embodiment, the two-dimensional information and lidar scanning information after spatiotemporal synchronization processing at time i+1 are transformed into the global coordinate system, denoted as Z. i+1 For each laser point, the approximate nearest neighbor algorithm from the PCL library is used to find the nearest neighbor points within a set radius on the map. All nearest neighbor points are then transferred to the local coordinate system and saved as m. i+1 ;

[0066] Z i+1 and m i+1 As input to formula (1), the transformation matrix T for the registration of two frames of point cloud data is obtained. opt ;

[0067]

[0068] Finally, the transformation matrix T opt Substituting into formula (2), the pose of the pose node at time i+1 is optimized:

[0069]

[0070] Among them, T i W This represents the robot's pose in the global coordinate system at time i; Z represents the pose transformation matrix of the mobile robot from time i to time i+1, which is the pose transformation matrix of the mobile robot obtained by point cloud registration of the spatiotemporally synchronized two-dimensional information and the lidar scanning information using the iterative nearest point method; i+1 This represents the laser observation data at time i+1.

[0071] Figure 5(b) is a schematic diagram of the second-order iterative nearest matching process, where M... i T represents the global map that has been constructed at time i. i W T represents the robot's pose in the global coordinate system at time i. i+1 L Z represents the transformation matrix of the robot from time i to time i+1, which is the result of the previous calculation. i+1 This represents the laser observation data at time i+1. It can be seen that the error in the first ICP matching may lead to map inconsistency. The error correction can be completed by using the second iteration nearest point method to optimize the pose node in real time.

[0072] Step 8: Use the three-iteration nearest point method to complete the loop closure screening. The screened loops are used for loop closure detection to complete the error correction of the map.

[0073] In this embodiment, the key frame Mainscan and the processing frequency f are first determined. A key frame is defined as a frame that is considered to be a key frame only after the robot has traveled a certain distance (0.5m in the experiment). For the determined key frames, the processing frequency f (f = 6Hz in the experiment) is met before the subsequent three iterations of nearest point matching are performed.

[0074] During the three-iteration nearest point matching, all historical keyframes are traversed and matched. Through proximity detection based on LiDAR, the historical keyframes that are closest to the keyframes determined by the current robot are found. Then, the ICP algorithm is applied to solve the transformation matrix and registration distance. Historical keyframes with a registration distance less than a certain threshold are added to the loop closure candidate set. The selected loops are used for loop closure detection to complete the error correction of the map.

[0075] In the three iterations of nearest point matching, to avoid false detections caused by high similarity between adjacent frames, several key frames in the nearest neighbor are ignored.

[0076] During the three iterations of nearest-point matching, the candidate set of loop closures detected in the keyframe determined by the current robot may contain 0, 1, or more loop closures. If there are 0, it means no loop closures were detected. If there are not 0, the loop closure with the smallest registration distance S is selected. min A set of loopback information, because the current loopback frame pair has the best registration effect.

[0077] This invention provides a mobile robot synchronous localization and mapping device, comprising:

[0078] The acquisition module is used to acquire depth information, LiDAR scanning information, and mileage information of the environment in which the mobile robot is located.

[0079] The preprocessing module is used to convert the depth information into point cloud data, and to downsample the point cloud data to obtain sparse point cloud data.

[0080] The projection module is used to project the sparse point cloud data in three dimensions to obtain two-dimensional information about the environment in which the mobile robot is located.

[0081] The synchronization module is used to perform spatiotemporal synchronization processing on the two-dimensional information, the lidar scanning information, and the mileage information.

[0082] The pose node establishment module is used to establish pose nodes using the spatiotemporally synchronized two-dimensional information, lidar scanning information, and odometer information.

[0083] The registration module is used to perform point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information using the iterative nearest point method to obtain the pose transformation matrix of the mobile robot.

[0084] The optimization module is used to optimize the pose nodes in real time using the quadratic iterative nearest point method, and to draw a map based on the optimized pose nodes and the pose transformation matrix.

[0085] The correction module is used to perform loop closure screening using the three-iteration nearest point method. The screened loops are used for loop closure detection to complete the error correction of the map.

[0086] This invention effectively solves the problems of weak positioning error, map accuracy, and anti-interference ability caused by the inherent shortcomings of 2D LiDAR and depth cameras (2D LiDAR loses three-dimensional environmental information, and machine vision equipment is susceptible to light interference).

[0087] The following details the formula used for real-time optimization of pose nodes using the quadratic iterative nearest point method:

[0088]

[0089] The specific operational design.

[0090] Let X represent the robot pose sequence, L represent environmental landmarks, U represent control input, and Z represent landmark measurements. The SLAM problem is equivalent to estimating the following posterior probability distribution:

[0091] P(X,L|Z,U)

[0092] =P(L|Z,X,U)P(X|Z,U) (1)

[0093] =P(L|Z,X)P(X|Z,U)

[0094] The SLAM problem is equivalent to the following posterior probability distribution:

[0095]

[0096] Where P(x0) represents the initial pose P(x) i |x i-1 ,u i P(z) represents the motion model. i |x i ) represents the measurement model. The SLAM problem is transformed into a least squares estimation problem based on maximum a posteriori probability, where the maximum a posteriori estimated pose sequence X* and environment map L* are obtained by taking the negative logarithm of the joint probability of equation (2):

[0097]

[0098] Typically, motion and measurement models are assumed to follow a Gaussian distribution, so they can be transformed into the following form:

[0099] x j ~N(x) i ⊕T ij ,∑ ij (4)

[0100] Where the operator ⊕ represents coordinate transformation, T ij and ∑ ij Representing pose x i and pose x j The transformation matrix and covariance matrix of equation (4) can also be written in the following form:

[0101]

[0102] Where, m i It is motion noise, n i The observation noise leads to the following nonlinear least squares problem:

[0103]

[0104] Linearization using the Guass-Newton method transforms the problem into the following least squares problem:

[0105]

[0106] Here, vector θ contains all pose variables, and matrix A is a large sparse Jacobian matrix.

[0107] The Cholesky decomposition method can be used to solve equation (7), but it requires calculating matrix ATA, which would result in a huge computational burden. Therefore, the QR decomposition method is used for matrix A:

[0108]

[0109] Applying equations (8) to (9), we obtain the following formula. Because Q is an orthogonal matrix, Q... T Q = I (where I is the identity matrix), therefore, an extra term Q is added. T The second line of equation (9) does not change the magnitude of vector Aθ-b.

[0110]

[0111] Among them, [d,e] T =Q T b is defined if and only if Rθ = d, the above equation reaches its minimum value ||e||. 2 Based on the above analysis, the SLAM problem has been solved. Meanwhile, when new observations are input, the Jacobian matrix A does not change significantly; therefore, the result of the previous QR decomposition can be used as the initial value for the current iteration, and the optimization process is incremental. This incremental QR decomposition method avoids the need to solve for the Jacobian matrix A every time, ensuring the real-time nature of the optimization.

[0112] Figures 6(a) and 6(b) show the world model constructed in the Gazebo simulation environment. To compare the superiority of the design scheme of this invention, obstacles below the scanning plane of the LiDAR were added to the world model (circled in the figure). Figure 6(c) shows the simulation effect of map construction by a single LiDAR sensor. When using a single LiDAR sensor for map construction, information about obstacles below the scanning plane will be lost, and subsequent navigation will also generate unnecessary errors and computational losses. Figure 6(d) shows the map constructed by the multimodal fusion SLAM algorithm designed in this invention in the simulation environment. By comparing with Figure 6(c), the multimodal fusion SLAM algorithm can effectively preserve obstacle information that the LiDAR cannot detect. At the same time, when the external scene is large, it can greatly eliminate the positioning error, thus providing superior initial conditions for autonomous navigation of mobile robots.

[0113] Building real-world scenarios, such as Figure 7(a) and 7(b) This invention was verified by using different SLAM algorithms for map construction (e.g., Figure 7(c) and 7(d) It is evident that in complex scenarios, the multimodal fusion SLAM algorithm has the advantages of high accuracy and high information preservation: it can preserve the information of obstacles (obstacles 1, 2, 4) that cannot be detected by LiDAR, and it also performs well in locations with complex features such as doorways, while maintaining good accuracy in environments with high light intensity.

[0114] In one embodiment of the present invention, a computer device is provided, comprising a processor and a memory. The memory stores a computer program, which includes program instructions. The processor executes the program instructions stored in the computer storage medium. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing and control core of the terminal, suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions to achieve a corresponding method flow or corresponding function. The processor described in this embodiment of the present invention can be used to implement the operation of a mobile robot synchronous localization and map building method.

[0115] In one embodiment of the present invention, a mobile robot synchronous localization and map building method, if implemented as a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable storage medium includes permanent and non-permanent, removable and non-removable media, and information storage can be implemented by any method or technology. Information can be computer-readable instructions, data structures, program modules, or other data.

[0116] The computer storage medium can be any available medium or data storage device that a computer can access, including but not limited to magnetic storage (e.g., floppy disks, hard disks, magnetic tapes, magneto-optical disks (MOs)), optical storage (e.g., CDs, DVDs, BDs, HVDs), and semiconductor storage (e.g., ROMs, EPROMs, EEPROMs, non-volatile memory (NAND flash), solid-state drives (SSDs)).

[0117] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0118] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create a machine for implementing the flowchart illustrations. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0119] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0120] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0121] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the technical scope disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of protection of the claims.

Claims

1. A mobile robot simultaneous localization and mapping method, characterized by, include: Acquire depth information, LiDAR scanning information, and mileage information of the environment in which the mobile robot is located; The depth information is converted into point cloud data, and the point cloud data is downsampled to obtain sparse point cloud data. The sparse point cloud data is projected in three dimensions to obtain two-dimensional information about the environment in which the mobile robot is located. The two-dimensional information, the lidar scanning information, and the mileage information are subjected to spatiotemporal synchronization processing; Pose nodes are established using the spatiotemporally synchronized two-dimensional information, lidar scanning information, and odometer information. The iterative nearest point method is used to perform point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information to obtain the pose transformation matrix of the mobile robot; The pose nodes are optimized in real time using the quadratic iterative nearest point method, and a map is drawn based on the optimized pose nodes and the pose transformation matrix. The loop closure screening is performed using the three-iteration nearest point method. The screened loops are used for loop closure detection to correct the error of the map.

2. The method of claim 1, wherein, The step of downsampling the point cloud data to obtain sparse point cloud data specifically involves: The point cloud data is downsampled using a voxel filtering method to obtain sparse point cloud data.

3. The method of claim 1, wherein, The real-time optimization of the pose node using the quadratic iterative nearest point method includes: Will The two-dimensional information and lidar scanning information after spatiotemporal synchronization processing at each moment are transformed into the global coordinate system, denoted as . And find the nearest neighbor points of each laser point within a set radius on the map, transfer all nearest neighbor points to the local coordinate system and save them as ; Will and As input to formula (1), the transformation matrix for registration of two frames of point cloud data is obtained. ; （1） in, The transformation matrix Substituting into formula (2) Optimize the pose of the pose nodes at each time step; （2） in, Represents the maximum a posteriori estimated pose sequence; Represents the robot's pose sequence; Represents a motion model; Represents the measurement model; Express posture; Indicates motion noise; Indicates observation noise; express The robot's pose in the global coordinate system at any given moment; express Time's up The pose transformation matrix of the mobile robot is obtained by performing point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information using the iterative nearest point method. express Laser observation data at any given time.

4. The method for simultaneous localization and mapping of a mobile robot according to claim 3, characterized in that, The approximate nearest neighbor algorithm in the PCL library is used to find the nearest neighbor points of each laser point within a set radius on the map.

5. The method for simultaneous localization and mapping of a mobile robot according to claim 1, characterized in that, The method employs a three-iteration nearest-point approach to filter loop closures. The filtered loop closures are used for loop closure detection to correct errors in the map, including: Determine keyframes and processing frequency Here, a keyframe is defined as a frame formed when the mobile robot has traveled a set distance, and the keyframes are processed at a certain frequency. Perform three more iterations of nearest-point matching; During the three iterations of nearest point matching, all historical keyframes are traversed and matched. By using proximity detection based on LiDAR, the historical keyframes that are closest to the keyframes determined by the current mobile robot are found. Then, the ICP algorithm is applied to solve the transformation matrix and registration distance. Historical keyframes with a registration distance less than a set threshold are added to the loop closure candidate set. The selected loops are used for loop closure detection to complete the error correction of the map.

6. A mobile robot synchronous localization and mapping device, characterized in that, include: The acquisition module is used to acquire depth information, LiDAR scanning information, and mileage information of the environment in which the mobile robot is located; The preprocessing module is used to convert the depth information into point cloud data, and to downsample the point cloud data to obtain sparse point cloud data. The projection module is used to project the sparse point cloud data in three dimensions to obtain two-dimensional information about the environment in which the mobile robot is located. The synchronization module is used to perform spatiotemporal synchronization processing on the two-dimensional information, the lidar scanning information, and the mileage information; The pose node establishment module is used to establish pose nodes using the spatiotemporally synchronized two-dimensional information, lidar scanning information and odometer information. The registration module is used to perform point cloud registration on the spatiotemporally synchronized two-dimensional information and the lidar scanning information using the iterative nearest point method to obtain the pose transformation matrix of the mobile robot. The optimization module is used to optimize the pose nodes in real time using the quadratic iterative nearest point method, and to draw a map based on the optimized pose nodes and the pose transformation matrix. The correction module is used to perform loop closure screening using the three-iteration nearest point method. The screened loops are used for loop closure detection to complete the error correction of the map.

7. A mobile robot synchronous localization and mapping device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of a mobile robot synchronous localization and map building method as described in any one of claims 1 to 5.

8. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of a mobile robot synchronous localization and map building method as described in any one of claims 1 to 5.

Images