Robot positioning method, system and equipment based on laser SLAM (Simultaneous Localization and Mapping) and medium

Abstract

The invention relates to the technical field of laser radar positioning, and particularly discloses a robot positioning method, system, equipment and medium based on laser SLAM. 3D feature point clouds from a laser radar are collected, IMU data are synchronously received, preliminary posture correction is conducted on feature points through posture pre-transformation, the corrected 3D point clouds are projected to a two-dimensional plane, and the 3D point clouds are projected to the two-dimensional plane; forming projection feature points containing height information, and extracting the projection feature points as two-dimensional feature point cloud for registration; matching the two-dimensional feature points of the current frame with a historical sub-map by adopting an RANSAC point cloud registration algorithm, and updating the sub-map according to a registration result; according to the method, the initial speedometer output is filtered based on Kalman filtering to obtain the final speedometer output, the estimation precision is improved, a smooth and high-precision speedometer result is output, support is provided for accurate positioning and navigation of a robot in a complex environment, and the robust SLAM mapping capability in a dynamic, sparse or strenuous motion environment is achieved.

Description

Technical Field

[0001] The present invention relates to the field of laser radar positioning technology, and more specifically, to a robot positioning method, system, device and medium based on laser SLAM. Background Art

[0002] SLAM (Simultaneous Localization and Mapping) is one of the indispensable key technologies in the field of mobile robotics. It enables robots to incrementally build maps in unknown environments and simultaneously locate themselves based on their own state estimates and maps.

[0003] However, in complex environments, such as those with sparse features, those with more dynamic objects, or scenarios where robots are in intense motion, 3D laser SLAM can use information within the three-dimensional space for positioning, offering better performance. However, for robots operating on a flat surface, 3D laser SLAM calculates six degrees of freedom, resulting in a waste of computing resources. Existing technologies address this issue by converting 3D point cloud slices into 2D lasers and running 2D laser SLAM. However, this approach results in information loss in the spatial environment, reducing algorithm performance.

[0004] Therefore, it is necessary to provide a robot positioning method, system, equipment and medium based on laser SLAM to solve the above technical problems. In order to solve the above problems, a technical solution is now provided. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, the present invention provides a robot positioning method, system, device and medium based on laser SLAM, which is used to solve the problem that the existing 3DSLAM wastes resources in calculating redundant degrees of freedom on a planar robot, while the 2DSLAM suffers from performance degradation due to information loss, making it difficult to strike a balance between efficiency and accuracy.

[0006] To achieve the above object, the present invention provides the following technical solutions: The robot positioning method based on laser SLAM includes the following steps: Collect 3D feature point clouds in the robot's moving space and obtain IMU data based on the inertial measurement unit; The 3D feature point cloud is pre-transformed using IMU data, projected onto a 2D plane, and the height attribute is retained to obtain a 2D feature point cloud. The 2D feature point cloud is then registered, and the point cloud registration result of the current frame is output as the preliminary odometry. The preliminary odometer output is filtered based on the Kalman filter to obtain the final odometer output; The sub-map is updated after each point cloud registration is completed, and the feature point clouds completed by all frames are superimposed to form a local sub-map.

[0007] As a further solution of the present invention, the RANSAC point cloud registration method is used to register the 2D feature point cloud.

[0008] As a further solution of the present invention, the operation process of registering the 2D feature point cloud is as follows: Randomly select several feature point clouds, and query the nearest feature points in the stored local submap, add them to a queue as corresponding point pairs, and extract several corresponding point pairs; the feature points of the current frame are called source point sets ,in, is the feature point of the current frame in the i-th corresponding point pair in the source point set, is the number of corresponding point pairs, and the feature points from the local submap are called the target point set ,in, For the feature point of the i-th feature point in the source point set, the feature point from the local submap is centered. The mean of the source point set and the target point set is calculated respectively, and then the first data matrix is ​​obtained by centralization. and the second data matrix ; Based on the first data matrix and the second data matrix Construct the covariance matrix; Perform SVD decomposition on the covariance matrix, obtain the orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector, and construct the homogeneous transformation matrix based on the rotation matrix and translation vector ; Apply a rigid body transformation to all feature points in the current frame, find the number of inliers and iteratively optimize the odometry output.

[0009] As a further solution of the present invention, the source point set and the target point set The process of calculating the mean and then centering is as follows: Source point set and the target point set Calculate the mean separately: Where, is the mean of the source point set, is the mean of the target point set; The source point set and the target point set are centralized according to the mean calculation results: Where, is the feature point of the new frame in the i-th corresponding point pair in the centralized source point set, is the feature point originating from the local submap in the ith feature point pair in the centralized source point set; Based on the source point set and the target point set after the centralization processing, the data matrix is ​​constructed respectively to obtain the first data matrix Based on the source point set and the target point set after the centralization processing, the data matrix is ​​constructed respectively to obtain the first data matrix and the second data matrix .

[0010] As a further solution of the present invention, based on the first data matrix and the second data matrix The expression for constructing the covariance matrix is ​​as follows: Where, is the covariance matrix, is the feature point of the new frame in the i-th corresponding point pair in the centralized source point set, is the feature point from the local submap in the ith feature point pair in the centralized source point set, is the transpose operation.

[0011] As a further solution of the present invention, the covariance matrix is ​​decomposed by SVD, and an orthogonal matrix is ​​obtained by decomposing the covariance matrix. Then, the rotation matrix is ​​calculated and the translation vector is solved. A homogeneous transformation matrix is ​​constructed based on the rotation matrix and the translation vector. The specific process is as follows: the covariance matrix is ​​decomposed by SVD to obtain the orthogonal matrix and the singular value diagonal matrix. The operation formula is: Where, is an orthogonal matrix, is a diagonal matrix of singular values; Calculate the rotation matrix based on the orthogonal matrix: Where, is the rotation matrix; Calculate the translation vector from the rotation matrix: Where, is the translation vector; Obtain the homogeneous transformation matrix through the translation vector and the rotation matrix: Where, is the homogeneous transformation matrix.

[0012] As a further solution of the present invention, a rigid body transformation is applied to all feature points in the current frame, the number of inliers is found, and the odometry output is iteratively optimized, specifically including: Apply rigid body transformation to all feature points in the current frame, and find the number of feature points in the submap within a fixed distance of all feature points in the feature point cloud of the current frame after transformation, as the number of inliers; Repeatedly randomly extract corresponding point pairs and apply rigid body transformation. When the number of inliers is greater than the number of inliers generated by the previous transformation, receive a new transformation. , and store the transformed feature point cloud to construct a local sub-map; When receiving a new transformation, the formula for calculating the new robot odometry output is: Where, is the transformation matrix at the i-th moment, is the transformation matrix at the i-1th moment.

[0013] A robot positioning system based on laser SLAM, the system comprising: The acquisition and measurement module is used to collect 3D feature point clouds of the mobile robot and obtain IMU data based on the inertial measurement unit. The transformation and registration module is used to pre-transform the pose of the 3D feature point cloud using the IMU data, project the 3D feature point cloud onto a 2D plane, retain the height attribute to obtain a 2D feature point cloud, register the 2D feature point cloud, and output the point cloud registration result of the current frame as the preliminary odometry. The filter output module is used to filter the preliminary odometer output based on Kalman filtering to obtain the final odometer output; The update overlay module is used to update the submap after each point cloud registration is completed, and to overlay the feature point clouds of all frame registrations to form a local submap.

[0014] An electronic device includes a memory and a processor, wherein a computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes the steps of the above-mentioned laser SLAM-based robot positioning method.

[0015] A readable storage medium stores a computer program, which, when executed by a processor, is used to implement the steps of the above-mentioned robot positioning method based on laser SLAM.

[0016] The technical effects and advantages of the robot positioning method, system, equipment and medium based on laser SLAM of the present invention are as follows: the present invention collects 3D feature point clouds from laser radar and synchronously receives IMU data, performs preliminary posture correction on the feature points through posture pre-transformation, and projects the corrected three-dimensional point cloud onto a two-dimensional plane to form projected feature points containing height information, which are extracted as two-dimensional feature point clouds for alignment; adopts the RANSAC point cloud alignment algorithm to match the two-dimensional feature points of the current frame with the historical submap, and updates the submap according to the alignment results; filters the preliminary odometer output based on Kalman filtering to obtain the final odometer output, improves the estimation accuracy, and outputs smooth and high-precision odometer results, providing support for the robot's precise positioning and navigation in complex environments, and realizing robust SLAM mapping capabilities in dynamic, sparse or violent motion environments.

[0017] The robot positioning method based on laser SLAM in the present invention takes into account the spatial information integrity of 3D point clouds and the computational efficiency of 2D algorithms. It can reduce redundant calculations of six degrees of freedom while retaining key height features, significantly improving the system's positioning robustness and real-time performance in environments with sparse features, dynamic object interference or intense motion, while avoiding the performance degradation of traditional 2DSLAM due to information loss. It is an efficient solution for mapping and positioning of ground mobile robots in complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 Flowchart of a robot positioning method based on laser SLAM provided in an embodiment of the present invention; Figure 2 A flowchart of odometer calculation provided by an embodiment of the present invention; Figure 3 A system block diagram of a robot positioning system based on laser SLAM provided in an embodiment of the present invention; Figure 4 A schematic diagram of the hardware structure of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0019] The following will clearly and completely describe the technical solutions of the present invention in conjunction with the accompanying drawings. Obviously, the technical solutions described are only part of the present invention, not the entire invention. Based on the technical solutions of the present invention, all other technical solutions obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0020] like Figure 1 FIG. 1 is a flow chart of a robot positioning method based on laser SLAM provided by an embodiment of the present invention. Figure 1 The execution subject of the method shown may be a software and / or hardware device. The execution subject of the present application may include but is not limited to at least one of the following: user equipment, network equipment, etc. Among them, user equipment may include but is not limited to computers, smart phones, personal digital assistants (PDAs) and the electronic devices mentioned above. Network equipment may include but is not limited to a single network server, a server group consisting of multiple network servers, or a cloud based on cloud computing consisting of a large number of computers or network servers, wherein cloud computing is a type of distributed computing, a super virtual computer composed of a group of loosely coupled computers. This embodiment does not limit this. It includes steps S1 to S4, as follows: S1, collects 3D feature point clouds in the robot's moving space and obtains IMU data based on the inertial measurement unit; First, a 3D LiDAR sensor deployed on a mobile robot platform scans the surrounding environment and collects a 3D feature point cloud at the current moment. To improve processing efficiency and robustness, the original 3D feature point cloud is filtered, dedistorted, and feature extracted. Key geometric feature points, such as edge points and surface points, are extracted to form a 3D feature point cloud for the current frame.

[0021] At the same time, the onboard inertial measurement unit (IMU) is used to collect the angular velocity and acceleration information at the current moment, and the IMU data and laser point cloud data are synchronized through timestamps to provide inertial information support for subsequent posture estimation.

[0022] S2, pre-transform the pose of the 3D feature point cloud through IMU data, project the 3D feature point cloud to a 2D plane, retain the height attribute to obtain a 2D feature point cloud, align the 2D feature point cloud, and output the point cloud registration result of the current frame as the preliminary odometry; After completing the 3D feature point cloud and IMU data acquisition, the three-dimensional feature point cloud of the current frame is pre-transformed based on the angular velocity and acceleration information provided by the IMU data to obtain an initial pose estimate. Specifically, the pose change between the previous moment and the current moment can be estimated by IMU pre-integration to obtain the estimated pose and posture transformation matrix of the current frame. Subsequently, the three-dimensional feature point cloud is projected onto a two-dimensional horizontal plane based on the pose estimation transformation matrix to form a 2D feature point cloud. In order to retain the height features in the three-dimensional structural information, the generated two-dimensional point cloud still retains its Z-axis height value in the attributes of each point to achieve 2.5D feature expression. Based on the two-dimensional feature point cloud, the system uses a robust point cloud matching method to align the current frame point cloud with the previous frame or local map to obtain an estimate of the pose increment of the current frame relative to the previous frame. The alignment result is the preliminary odometry output of this frame, which is used to represent the relative motion information of the robot in the two-dimensional plane.

[0023] S3, filtering the preliminary odometer output based on Kalman filtering to obtain the final odometer output; To improve robustness against noise, dynamic changes, and sensor errors, a Kalman filter is introduced to fuse and optimize the preliminary odometry output. The predicted value of the IMU data is used as the state prediction term, and the pose obtained from point cloud registration is input into the Kalman filter as the observation term. By executing a prediction update loop, the filter can optimally estimate the system state, thereby suppressing estimation jitter caused by local registration errors, motion blur, or missing features. The final odometry output is smoother and more continuous for the current frame. The output also contains the robot's current position and heading in the two-dimensional plane, which can be used by subsequent navigation, mapping, or control modules.

[0024] S4, after each point cloud registration is completed, the sub-map is updated, and the feature point clouds completed by all frame registrations are superimposed to form a local sub-map.

[0025] After obtaining the final odometry output, the feature point cloud of this frame after registration and correction is converted to the sub-map coordinate system and fused with the existing local map to complete the sub-map update operation. Specifically, the two-dimensional feature point cloud of the current frame is transformed into a unified reference coordinate system according to the final pose of the current frame, and it is superimposed with the point cloud after registration of several historical frames to form a local sub-map in the current time window. The local sub-map is used for point cloud matching and registration of subsequent frames. It also supports the construction of a sliding window mechanism to control the map size to improve the system's operating efficiency and long-term stability. By continuously superimposing updated sub-maps, it is possible to maintain and expand structural information in complex environments and enhance positioning and tracking capabilities.

[0026] The RANSAC (random sampling consensus) point cloud registration method is used to register the 2D feature point cloud.

[0027] The RANSAC algorithm is specifically designed to handle data containing a large number of outliers. In point cloud registration tasks, the environment often contains dynamic obstacles or perception errors that cause mismatched point pairs. By using iterative sampling and the "number of inliers" as a scoring criterion, RANSAC can effectively remove these outliers, improving the stability and accuracy of the registration results.

[0028] The operation process of registering 2D feature point cloud is as follows: Randomly select several feature point clouds, and query the nearest feature points in the stored local submap, add them to a queue as corresponding point pairs, and extract several corresponding point pairs; The feature points of the current frame are called the source point set ,in, is the feature point of the current frame in the i-th corresponding point pair in the source point set, is the number of corresponding point pairs, and the feature points from the local submap are called the target point set ,in, For the feature point of the i-th feature point in the source point set, the feature point from the local submap is centered. The mean of the source point set and the target point set is calculated respectively, and then the first data matrix is ​​obtained by centralization. and the second data matrix ; Based on the first data matrix and the second data matrix Construct the covariance matrix; Perform SVD decomposition on the covariance matrix, obtain the orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector, and construct the homogeneous transformation matrix based on the rotation matrix and translation vector ; Apply a rigid body transformation to all feature points in the current frame, find the number of inliers and iteratively optimize the odometry output.

[0029] Perform SVD (singular value decomposition) on the covariance matrix to obtain an orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector, and construct a homogeneous transformation matrix based on the rotation matrix and translation vector; Apply a rigid body transformation to all feature points in the current frame, find the number of inliers and iteratively optimize the odometry output.

[0030] To achieve highly robust registration of two-dimensional feature point clouds, this implementation employs a point cloud registration method based on the RANSAC algorithm. This method establishes correspondences between the feature points of the current frame and historical local submaps, estimates the transformation model, and evaluates the number of inliers to complete the estimation of the relative pose of the current frame. By centering the source point set P and the target point set Q and performing SVD decomposition based on the covariance matrix, the resulting rotation matrix and displacement vector meet the minimum mean square error criterion, exhibiting excellent numerical stability and analytical solution accuracy, avoiding the potential for local optima encountered by gradient descent methods.

[0031] Combining the obtained rotation matrix and translation vector, a homogeneous transformation matrix of the current frame relative to the previous frame or submap is constructed to represent the motion transformation of the robot in the local coordinate system.

[0032] After obtaining the homogeneous transformation matrix, it is applied to all feature points in the current frame, performing a rigid body transformation to obtain the transformed point cloud of the current frame in the submap reference frame. Next, the number of point pairs between the transformed feature points and the corresponding points in the submap whose distance is within a set threshold is counted, which serves as the evaluation metric for the number of inliers in the current iteration.

[0033] In order to obtain a better registration effect, the RANSAC process aims to "maximize the number of inliers" and iterates the above steps several times. Usually, a maximum number of iterations is set or the early convergence condition is met. Finally, the transformation matrix that maximizes the number of inliers is selected as the optimal registration result for the current frame. The optimal transformation result is used to output the relative pose estimate of the current frame, that is, the preliminary odometry output of this frame.

[0034] Source point set and the target point set The process of calculating the mean and then centering is as follows: Calculate the mean for the source point set and the target point set respectively: Where, is the mean of the source point set, is the mean of the target point set; The source point set and the target point set are centralized according to the mean calculation results: where, is the feature point of the new frame in the i-th corresponding point pair in the centralized source point set, is the feature point originating from the local submap in the ith feature point pair in the centralized source point set; Based on the source point set and the target point set after the centralization processing, data matrices are respectively constructed to obtain a first data matrix. Based on the source point set and the target point set after the centralization processing, data matrices are respectively constructed to obtain a first data matrix and a second data matrix.

[0035] In order to estimate the rigid body transformation of the 2D point cloud between the current frame and the submap, we first perform mean calculation and centering on the two key sets of point cloud registration: the source point set and the target point set, to eliminate the influence of the global translation, so that the subsequent calculation focuses on the posture rotation information, thereby improving the accuracy and stability of the rigid body registration. In practical applications, if there are 3 matching point pairs, that is, n=3, then the source point set , target point set , calculate the mean of the source point set and the target point set respectively to get the mean of the source point set , the mean of the target point set , and then the source point set and the target point set are centralized according to the mean calculation results to obtain , , then we get the first data matrix and the second data matrix .

[0036] Centralization effectively eliminates the overall translation bias by converting the point set into a coordinate system centered on its respective mean, making subsequent rotation estimation more accurate. After centering the source point set and the target point set, the construction of the covariance matrix is ​​more in line with the minimum mean square error criterion, which can improve the numerical stability and robustness of the SVD solution process. The point cloud registration process after centering mainly estimates the posture rotation information, which can avoid unnecessary translation interference and make the geometric meaning of the rotation matrix clearer and more independent. Compared with directly processing the original point cloud, centering helps to reduce the impact caused by the distribution offset of a single point, and has a certain inhibitory effect on the interference of abnormal points, thereby improving the final registration quality. The feature points after centering are easier to achieve unified processing during sub-image construction and multi-frame fusion, which helps to build a smoother and more accurate local map.

[0037] The expression for constructing the covariance matrix based on the first data matrix and the second data matrix is ​​as follows: Where, is the covariance matrix, is the feature point of the new frame in the i-th corresponding point pair in the centralized source point set, is the feature point from the local submap in the ith feature point pair in the centralized source point set, is the transpose operation.

[0038] By performing collaborative statistics on the centralized source point set and target point set, the covariance matrix effectively captures the overall correspondence and spatial structure change trend of the two sets of points in two-dimensional or three-dimensional space, thereby providing a stable foundation for subsequent pose estimation. Specifically, the covariance matrix can accurately express the spatial alignment direction between the current frame point cloud and the corresponding points in the map. By performing SVD on the matrix, the optimal rotation matrix can be obtained, and then the translation vector is solved in conjunction with the mean shift to construct a rigid body transformation matrix to achieve high-precision point cloud pose estimation. The introduction of the covariance matrix not only improves the accuracy and stability of pose estimation, but also has good robustness. In particular, when the feature points are sparsely distributed or there are some mismatches, the main direction can still be extracted through global statistical characteristics, thereby significantly improving the robustness and practicality of the point cloud registration algorithm in complex environments.

[0039] Perform SVD decomposition on the covariance matrix, obtain the orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector. Based on the rotation matrix and translation vector, a homogeneous transformation matrix is ​​constructed. The specific process is as follows: Perform SVD decomposition on the covariance matrix, obtain the orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector. Based on the rotation matrix and translation vector, a homogeneous transformation matrix is ​​constructed. The specific process is as follows: The calculation formula for the orthogonal matrix and the singular value diagonal matrix obtained by SVD decomposition of the covariance matrix is: Where, is an orthogonal matrix, is a diagonal matrix of singular values; Calculate the rotation matrix based on the orthogonal matrix: Where, is the rotation matrix; Calculate the translation vector from the rotation matrix: Where, is the translation vector; Obtain the homogeneous transformation matrix through the translation vector and the rotation matrix: Where, is the homogeneous transformation matrix.

[0040] By performing singular value decomposition (SVD) on the covariance matrix and calculating the rotation matrix and translation vector in sequence, and finally constructing the homogeneous transformation matrix, the optimal rigid body transformation estimation between the source point set and the target point set is essentially achieved. Its technical effect is reflected in the high-precision, low-drift, stable and robust point cloud registration capability.

[0041] By decomposing the covariance matrix, the extracted orthogonal matrix describes the principal directions and spatial distribution characteristics of the two sets of points. The rotation matrix constructed further precisely aligns the pose relationship of the point cloud; the resulting translation vector corrects the relative position of the point cloud in space. The resulting homogeneous transformation matrix unifies rotation and translation into a single four-dimensional matrix, facilitating efficient use in multi-frame data fusion, map stitching, and robot motion estimation.

[0042] Overall, the covariance matrix solution process not only avoids the local extreme value trap common in iterative optimization, but also has high mathematical stability and engineering practicality. It is one of the key steps in realizing high-precision laser SLAM algorithms, point cloud map construction and motion estimation.

[0043] Apply rigid body transformation to all feature points in the current frame, find the number of inliers and iteratively optimize the odometry output, specifically including: Apply rigid body transformation to all feature points in the current frame, and find the number of feature points in the submap within a fixed distance of all feature points in the feature point cloud of the current frame after transformation, as the number of inliers; Repeatedly randomly extract corresponding point pairs and apply rigid body transformation. When the number of inliers is greater than the number of inliers generated by the previous transformation, receive a new transformation. , and store the transformed feature point cloud to construct a local sub-map; When receiving a new transformation , the calculation formula for the new robot odometry output is: Where, is the transformation matrix at the i-th moment, is the transformation matrix at the i-1th moment.

[0044] By applying rigid body transformation to the feature points of the current frame and combining iterative optimization with the evaluation of the number of inliers, a robust point cloud registration verification and optimization mechanism is essentially formed. Its core technical effect is reflected in the adaptive screening and refinement of the uncertainty of the initial transformation estimate, thereby improving the stability and accuracy of the odometry output. Specifically, the rigid body transformation is first applied to all feature points of the current frame, and matching points within a certain threshold are searched in the submap, and they are counted as the "number of inliers" to measure the registration effect. When a new transformation is generated by randomly extracting point pairs and the number of inliers increases, the system automatically replaces the current transformation with, reflecting the random optimal screening characteristics of RANSAC, which effectively eliminates the erroneous posture estimation caused by mismatching. Then through To achieve the temporal update of the robot's posture, the formula continuously multiplies the incremental posture of each frame with the cumulative posture of the previous frame to construct the continuous motion trajectory of the robot in the global coordinate system, thereby realizing a robust SLAM process.

[0045] Overall, this process effectively enhances the system's ability to resist abnormal matching, local occlusion, and dynamic interference, and improves the accuracy of motion estimation through the interior point maximization criterion. It is one of the key steps in building a robust, high-performance laser SLAM algorithm.

[0046] like Figure 2 As shown in FIG, it is a flow chart of the odometry calculation provided by an embodiment of the present invention. First, the three-dimensional feature point cloud from the lidar is collected, and the IMU data is received synchronously, and the feature points are preliminarily corrected in posture through the "pose pre-transformation" module. The corrected three-dimensional point cloud is projected onto a two-dimensional plane to form "projected feature points" containing height information, which are then extracted as two-dimensional feature point clouds for registration. Next, the RANSAC point cloud registration algorithm is used to match the two-dimensional feature points of the current frame with the historical sub-map, and the sub-map is dynamically updated based on the registration results. Subsequently, the preliminary pose information obtained by the registration is multi-source fused and noise suppressed through the Kalman filter to improve the estimation accuracy, and finally a smooth and high-precision odometry result is output to provide support for the robot's precise positioning and navigation in complex environments. The entire process combines the dynamic stability of the IMU with the spatial geometric characteristics of the laser point cloud to achieve robust SLAM mapping capabilities in dynamic, sparse or violent motion environments.

[0047] A robot positioning system based on laser SLAM, the system comprising: The acquisition and measurement module is used to collect 3D feature point clouds of mobile robots and obtain IMU data based on the inertial measurement unit; The transformation and registration module is used to pre-transform the pose of the 3D feature point cloud using IMU data, project the 3D feature point cloud onto a 2D plane, retain the height attribute to obtain a 2D feature point cloud, register the 2D feature point cloud, and output the point cloud registration result of the current frame as the preliminary odometry. The filter output module is used to filter the preliminary odometer output based on Kalman filtering to obtain the final odometer output; The update overlay module is used to update the submap after each point cloud registration is completed, and to overlay the feature point clouds of all frame registrations to form a local submap.

[0048] like Figure 3 FIG. 1 is a system block diagram of a robot positioning system based on laser SLAM according to an embodiment of the present invention, which can be used to perform Figure 1 The implementation principles and technical effects of the steps in the method embodiment shown are similar and will not be repeated here.

[0049] An electronic device includes a memory and a processor, wherein a computer program is stored in the memory. When the processor runs the computer program stored in the memory, the processor executes the steps of the above-mentioned laser SLAM-based robot positioning method.

[0050] like Figure 4 FIG. 1 is a schematic diagram of the hardware structure of an electronic device provided by an embodiment of the present invention. The electronic device includes: a processor 41, a memory 42, and a computer program; wherein: The memory 42 is used to store the computer program, which may also be a flash memory. The computer program is, for example, an application program or a functional module for implementing the above method.

[0051] The processor 41 is configured to execute the computer program stored in the memory to implement the various steps performed by the device in the above method. For details, please refer to the relevant description in the above method embodiment.

[0052] Optionally, the memory 42 may be independent or integrated with the processor 41 .

[0053] When the memory 42 is a device independent of the processor 41, the device may further include: The bus 43 is used to connect the memory 42 and the processor 41 .

[0054] A readable storage medium stores a computer program, which, when executed by a processor, is used to implement the steps of the above-mentioned robot positioning method based on laser SLAM.

[0055] The readable storage medium may be a computer storage medium or a communication medium. Communication media include any medium that facilitates the transfer of computer programs from one location to another. Computer storage media may be any available medium that can be accessed by a general-purpose or special-purpose computer. For example, a readable storage medium is coupled to a processor, enabling the processor to read information from and write information to the readable storage medium. Of course, the readable storage medium may also be an integral part of the processor. The processor and the readable storage medium may be located in an application-specific integrated circuit (ASIC). In addition, the ASIC may be located in a user device. Of course, the processor and the readable storage medium may also exist as discrete components in a communication device. The readable storage medium may be a read-only memory (ROM), a random access memory (RAM), a CD-ROM, a magnetic tape, a floppy disk, an optical data storage device, and the like.

[0056] The present invention also provides a program product, which includes execution instructions stored in a readable storage medium. At least one processor of a device can read the execution instructions from the readable storage medium, and at least one processor executes the execution instructions so that the device implements the methods provided in the various embodiments described above.

[0057] In the embodiments of the above-mentioned devices, it should be understood that the processor may be a central processing unit (CPU), other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASICs), etc. The general-purpose processor may be a microprocessor or any conventional processor. The steps of the method disclosed in the present invention may be directly executed by a hardware processor or by a combination of hardware and software modules within the processor.

[0058] Through the introduction of the above embodiments, the present invention collects 3D feature point clouds from laser radars through a robot positioning method based on laser SLAM, and synchronously receives IMU data. The feature points are preliminarily corrected in posture through posture pre-transformation. The corrected three-dimensional point cloud is projected onto a two-dimensional plane to form projected feature points containing height information, which are extracted as two-dimensional feature point clouds for registration; the RANSAC point cloud registration algorithm is used to match the two-dimensional feature points of the current frame with the historical sub-map, and the sub-map is updated according to the registration results; the preliminary odometer output is filtered based on the Kalman filter to obtain the final odometer output, thereby improving the estimation accuracy and outputting smooth and high-precision odometer results, providing support for the robot's precise positioning and navigation in complex environments, and realizing robust SLAM mapping capabilities in dynamic, sparse or violent motion environments.

[0059] The robot positioning method based on laser SLAM in the embodiment of the present invention takes into account the spatial information integrity of 3D point clouds and the computational efficiency of 2D algorithms. It can reduce redundant calculations of six degrees of freedom while retaining key height features, significantly improving the system's positioning robustness and real-time performance in environments with sparse features, dynamic object interference or intense motion, while avoiding the performance degradation of traditional 2DSLAM due to information loss. It is an efficient solution for mapping and positioning of ground mobile robots in complex scenarios.

[0060] The above is only a specific implementation method of the present application, but the scope of protection of the present application is not limited thereto. Any technician familiar with this technical field can easily think of changes or replacements within the technical scope disclosed in this application, which should be covered by the scope of protection of the present application.

[0061] Finally: The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A robot positioning method based on laser SLAM, characterized in that: The following steps are involved: Collect 3D feature point clouds in the robot's moving space and obtain IMU data based on the inertial measurement unit; The 3D feature point cloud is pre-transformed using IMU data, projected onto a 2D plane, and the height attribute is retained to obtain a 2D feature point cloud. The 2D feature point cloud is then registered, and the point cloud registration result of the current frame is output as the preliminary odometry. The preliminary odometer output is filtered based on the Kalman filter to obtain the final odometer output; The sub-map is updated after each point cloud registration is completed, and the feature point clouds completed by all frames are superimposed to form a local sub-map.

2. The robot positioning method based on laser SLAM according to claim 1, characterized in that: The RANSAC point cloud registration method is used to register the 2D feature point cloud.

3. The robot positioning method based on laser SLAM according to claim 2, characterized in that: The operation process of registering 2D feature point cloud is as follows: Randomly select several feature point clouds and query the nearest feature point in the stored local submap. Add the feature point and the nearest feature point in the local submap into a queue as a corresponding point pair, and extract several corresponding point pairs. The feature points of the current frame are called the source point set ,in, is the feature point of the current frame in the i-th corresponding point pair in the source point set, is the number of corresponding point pairs, and the feature points from the local submap are called the target point set ,in, For the feature point of the i-th feature point in the source point set, the feature point from the local submap is centered. The mean of the source point set and the target point set is calculated respectively, and then the first data matrix is ​​obtained by centralization. and the second data matrix ; Based on the first data matrix and the second data matrix Construct the covariance matrix; Perform SVD decomposition on the covariance matrix, obtain the orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector, and construct the homogeneous transformation matrix based on the rotation matrix and translation vector ; Apply a rigid body transformation to all feature points in the current frame, find the number of inliers and iteratively optimize the odometry output.

4. The robot positioning method based on laser SLAM according to claim 3, characterized in that: Source point set and the target point set The process of calculating the mean and then centering is as follows: Source point set and the target point set Calculate the mean separately: Where, is the mean of the source point set, is the mean of the target point set; The source point set and the target point set are centralized according to the mean calculation results: Where, is the feature point of the new frame in the i-th corresponding point pair in the centralized source point set, is the feature point originating from the local submap in the ith feature point pair in the centralized source point set; Based on the source point set and the target point set after the centralization processing, the data matrix is ​​constructed respectively to obtain the first data matrix Based on the source point set and the target point set after the centralization processing, the data matrix is ​​constructed respectively to obtain the first data matrix and the second data matrix .

5. The robot positioning method based on laser SLAM according to claim 3, characterized in that: Based on the first data matrix and the second data matrix The expression for constructing the covariance matrix is ​​as follows: Where, is the covariance matrix, is the feature point of the new frame in the i-th corresponding point pair in the centralized source point set, is the feature point from the local submap in the ith feature point pair in the centralized source point set, is the transpose operation.

6. The robot positioning method based on laser SLAM according to claim 3, characterized in that: Perform SVD decomposition on the covariance matrix, obtain the orthogonal matrix through covariance matrix decomposition, and then calculate the rotation matrix and solve the translation vector. Based on the rotation matrix and translation vector, construct the homogeneous transformation matrix. The specific process is as follows: The calculation formula for the orthogonal matrix and the singular value diagonal matrix obtained by SVD decomposition of the covariance matrix is: Where, is an orthogonal matrix, is a diagonal matrix of singular values; Calculate the rotation matrix based on the orthogonal matrix: Where, is the rotation matrix; Calculate the translation vector from the rotation matrix: Where, is the translation vector; Obtain the homogeneous transformation matrix through the translation vector and the rotation matrix: Where, is the homogeneous transformation matrix.

7. The robot positioning method based on laser SLAM according to claim 3, characterized in that: Apply rigid body transformation to all feature points in the current frame, find the number of inliers and iteratively optimize the odometry output, specifically including: Apply rigid body transformation to all feature points in the current frame, and find the number of feature points in the submap within a fixed distance of all feature points in the feature point cloud of the current frame after transformation, as the number of inliers; Repeatedly randomly extract corresponding point pairs and apply rigid body transformation. When the number of inliers is greater than the number of inliers generated by the previous transformation, receive a new transformation. , and store the transformed feature point cloud to construct a local sub-map; When receiving a new transformation , the calculation formula for the new robot odometry output is: Where, is the transformation matrix at the i-th moment, is the transformation matrix at the i-1th moment.

8. A robot positioning system based on laser SLAM, applied to the robot positioning method based on laser SLAM according to any one of claims 1 to 7, characterized in that: The system comprises: The acquisition and measurement module is used to collect 3D feature point clouds of mobile robots and obtain IMU data based on the inertial measurement unit; The transformation and registration module is used to pre-transform the pose of the 3D feature point cloud using IMU data, project the 3D feature point cloud onto a 2D plane, retain the height attribute to obtain a 2D feature point cloud, register the 2D feature point cloud, and output the point cloud registration result of the current frame as the preliminary odometry. The filter output module is used to filter the preliminary odometer output based on Kalman filtering to obtain the final odometer output; The update overlay module is used to update the submap after each point cloud registration is completed, and to overlay the feature point clouds of all frame registrations to form a local submap.

9. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor runs the computer program stored in the memory, the processor executes the steps of the robot positioning method based on laser SLAM according to any one of claims 1 to 7.

10. A readable storage medium storing a computer program, wherein: When the computer program is executed by a processor, it is used to implement the steps of the robot positioning method based on laser SLAM as described in any one of claims 1 to 7.

Images