A method and system for high-frequency real-time positioning of mobile robots indoors and outdoors

Abstract

This invention provides a pose optimization and localization method for robots operating indoors and outdoors with multiple sensors and few parameters. It utilizes Ceres to optimize and remove point cloud data from the ground point cloud. A plane is fitted between the current point and its n neighboring points. An abstract model of the localization system is established, and the `solve` function from the Ceres library is called to solve for the corresponding localization result `x0`. The localization calculation is repeated until convergence to a certain threshold, yielding the transformation relationship between `Xrobot` and `Map`, representing the current localization data. This invention, based on an iterative optimization method for removing ground point cloud feature points using Ceres, significantly improves the processing speed of real-time localization algorithms. It requires less computational power, offers high real-time performance, and is robust. It is compatible with daytime, nighttime, and low-light conditions such as rain and fog.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, specifically to a pose optimization method and a positioning method for robots with multiple sensors and few parameters, both indoors and outdoors. Background Technology

[0002] With the development of robotics technology, autonomous mobile robots (AMRs) have emerged. AMRs are machines with see-think-act capabilities, which can perceive the environment, autonomously build an environmental model, thereby obtaining their own pose information in the environment, and planning and controlling the robot's movement.

[0003] In traditional technology, the robot is first located to determine its current position; then, based on its current position, the robot's direction of movement is determined. The robot's current position can be determined using GPS (Global Positioning System) to locate it on a world map, or by using LiDAR to scan point cloud data and matching the point cloud data with a point cloud map to obtain the robot's position on the point cloud map.

[0004] In recent years, many companies have invested in the research of mobile robots, which has led to their widespread adoption in people's production and daily lives. Examples include JD.com's outdoor unmanned delivery robots, Roborock's intelligent sweeping robots, and intelligent AGV forklifts in major ports. However, the environmental perception and navigation designs of these robots are only applicable to a single indoor or outdoor environment, making it difficult to achieve seamless positioning and navigation when switching between indoor and outdoor environments.

[0005] Current indoor and outdoor positioning algorithms offer several solutions, such as using GPS to compensate for the limitations of outdoor positioning algorithms. However, GPS applications indoors are subject to significant interference, and LiDAR applications outdoors are affected by obstacles; neither can be simultaneously applied to both indoor and outdoor scenarios. GPS increases the hardware cost of the system. Furthermore, positioning based on GPS and LiDAR involves two separate positioning systems. GPS is typically only used to obtain a reference position at an initial location, requiring fusion of the GPS position and map coordinates during map creation, further increasing algorithm complexity. Using visual information to compensate for the lack of information richness in addressing indoor and outdoor positioning problems is also problematic. However, in low-light conditions such as nighttime or foggy / rainy weather, introducing visual information can introduce even greater errors, severely impacting system robustness. Additionally, traditional robot positioning algorithms using loosely or tightly coupled multi-sensor fusion methods require designing the robot system and calibrating the position and attitude relationships between multiple sensors. This is labor-intensive, involves numerous fusion parameters, and improper parameter design or configuration can lead to system divergence, further increasing system complexity and workload, and reducing robustness. Traditional filter-based data fusion requires dynamic maintenance and adjustment of the 6-dimensional (XYZRPY) covariance matrix or information matrix of each sensor data in imu, odom, and laser_pose, which is a total of 18*18 dimensions. This parameter matrix needs to be continuously maintained, otherwise the sensor fusion process will have poor robustness and the data will be prone to divergence. Summary of the Invention

[0006] The technical problem to be solved by this invention is to address the problem of seamless indoor and outdoor positioning optimization for robots using a multi-sensor, low-parameter fusion positioning algorithm.

[0007] The present invention solves the above-mentioned technical problems through the following technical means:

[0008] A pose optimization method for robots with multiple sensors and few parameters, both indoors and outdoors, includes the following steps:

[0009] Step 1. Preprocess the measurement data acquired by the lidar on the robot to obtain point cloud data; and optimize the ground point cloud by removing the point cloud data based on Ceres.

[0010] Step 2. Traverse the point cloud data after removing the ground point cloud, process each point, and take the current point as the initial positioning data given by the user or the previous calculation result as input to transform the lidar coordinate system to the map coordinate system;

[0011] Step 3. Use a kd-tree to find the n nearest neighbors of the current point in the Map. If the distance between the farthest neighbor and the current point is less than a threshold m, continue processing; otherwise, skip the current point. A plane is fitted based on the n neighbors.

[0012] Step 4. Calculate the distance dist from the current point to the plane, and calculate the projection point point_proj of the current point on the plane;

[0013] Step 5. Construct the localization model: Create a cost function that defines a structure named CostPlan. Within the CostPlan structure, the operator() function is configured to accept two parameters:

[0014] const T*PRi: is a pointer to a 6-dimensional vector representing the robot's pose. The first three elements of the 6-dimensional vector are the position, and the last three elements are the logarithmic mapping of rotations.

[0015] T*residual: A pointer to a 3-dimensional vector used to store the calculated residual;

[0016] operator() first converts the input current point and projection point into template type T, then maps the input pose into a 6-dimensional vector, and uses the Sophus library developed based on C++ and Eigen library, where the three-dimensional rotation group SO(3) class is used to convert the logarithmic mapping of the rotation part into a quaternion, then calculates the rotation and translation from the world coordinate system to the local coordinate system, and finally calculates the position of the current point on the Map and the distance from the current point to the projection point, and saves it into the residual;

[0017] Step 6. In order to minimize the sum of squares of the residuals, call the solve function in the Ceres library to solve for the corresponding location result x0.

[0018] Step 7. Output the obtained positioning result x0 as the input of Step 2. Repeat Steps 2-7 until convergence to a certain threshold to obtain the transformation relationship between the robot's position and pose and the Map, which is the positioning data at the current moment.

[0019] Furthermore, the specific preprocessing method for the measurement data acquired by the lidar is as follows:

[0020] First, the measurement data is converted to PCL format. Based on the acquired measurement data, i.e. point cloud data, the row number of each point cloud in the point cloud is calculated and then reprojected.

[0021] Then, ground points are calculated. Points below the horizontal angle are extracted from all points. For a laser point in beam i, the corresponding point is found in beam i+1. The difference vector between the lines connecting these two points and the radar is calculated. If the angle between the difference vector and the horizontal plane is less than a set value, it is considered a ground point.

[0022] Cluster the remaining point cloud and record the clustered point cloud. Sparsify the ground point cloud and save the segmented point cloud.

[0023] Laser distortion correction based on IMU: calculate curvature, calculate smoothness, and remove unreliable points;

[0024] For the current point cloud frame, extract features by line bundle, arrange the curvature of the point cloud in each bundle from smallest to largest, extract line features, set all remaining points as surface points, then perform downsampling to reduce the amount of computation, and finally publish the point cloud.

[0025] Furthermore, the specific method for laser distortion correction is as follows:

[0026] The system subscribes to IMU sensor data in real time and stores it in a queue. It obtains the attitude, angle, angular velocity, and angular acceleration of each frame of IMU sensor data and records the IMU sensor timestamp for each frame. It records the IMU sensor attitude at the initial point of the LiDAR. It analyzes the data of each point of the LiDAR to obtain the timestamp corresponding to each point in the point cloud. It searches for the previous and next IMU data in the IMU queue for the current timestamp. It interpolates the attitude to the corresponding point using SLERP. It obtains the attitude corresponding to each point cloud and obtains the position of each point relative to the initial point cloud by inversely transforming the current point to the initial pose. After traversing all frames of point cloud data, it obtains the position of the entire point cloud relative to the initial time of the LiDAR. It assigns the calculated new point cloud data to the original stored point cloud position as the distortion-free point cloud.

[0027] Furthermore, the method for fitting the plane in step 3 is as follows:

[0028] For the n neighboring points found, calculate their mean and construct an n x m matrix matA0, where each row represents the coordinates of a neighboring point. Solve the linear equation system matA0*matX0=matB0 using column pivoting QR decomposition, where matB0 is an n x 1 matrix with all elements being -1, and the solution matX0 represents the parameters of the plane. Obtain the plane's normal vector (pa, pb, pc) and distance pd from the origin from the solution matX0, and normalize them, ensuring their length is 1, to fit a plane.

[0029] This invention also provides a method for real-time indoor and outdoor high-frequency positioning of mobile robots, comprising the following steps:

[0030] S01. Obtain the initial LiDAR positioning data of the mobile robot and save it as the initial pose;

[0031] S02. Based on the positioning data, obtain key points within a certain range nearby, and based on the key points, find the point cloud map data corresponding to the corner points and surface points;

[0032] S03. Convert the point cloud map data to the Map coordinate system to obtain the converted point cloud map;

[0033] S04. Perform ICP algorithm matching between the transformed point cloud map and the Map. If the matching score is higher than the second threshold, proceed to step S01. Otherwise, use the obtained initial pose of the ICP algorithm as the initial value and bring it into step S05.

[0034] S05. Based on the initial pose as input to the real-time localization algorithm, execute the method described in any one of claims 1 to 4; finally, calculate the real-time pose solution score.

[0035] S06. When the score is greater than the third threshold, the LiDAR data, IMU data, and odometer data are fused together. Otherwise, the result calculated by the LiDAR is used as the fused result. At this time, the IMU and odometer do not participate in the calculation. The fused data is transmitted to step S05 in real time as the prediction value for the next iteration calculation.

[0036] Furthermore, the fusion method in step S06 is as follows:

[0037] IMU data processing: IMU data is used for integration calculations to obtain the robot's displacement and rotation;

[0038] Odometry data processing: Odometry data is used for integration calculations to obtain the robot's displacement;

[0039] LiDAR data processing: The data calculated by matching LiDAR data with map data is assigned to a variable, and the obtained data is the robot's pose;

[0040] Localization data fusion: First, if the LiDAR pose is different from the previous pose and the real-time pose calculation score is less than the third threshold, then the LiDAR pose is considered accurate and is directly used as the fused pose. At the same time, IMU and odometry data are used to estimate the robot's pose between the LiDAR in the previous frame and the current frame. These poses are then used as the starting point for Riemann integration to obtain the fused pose.

[0041] Data release: The fused pose is released on the corresponding topic, and the final data frequency is no less than 100Hz.

[0042] This invention also provides a high-frequency real-time positioning system for mobile robots indoors and outdoors, comprising:

[0043] The initial pose acquisition module is used to acquire the initial LiDAR positioning data of the mobile robot and save it as the initial pose.

[0044] The point cloud map data acquisition module is used to acquire key points within a certain range in the vicinity based on the positioning data, and to find the point cloud map data corresponding to the corner points and polygon points based on the key points.

[0045] The map conversion module is used to convert point cloud map data to the Map coordinate system to obtain a converted point cloud map.

[0046] The pose matching module is used to match the transformed point cloud map with the Map using the ICP algorithm. If the matching score is higher than the second threshold, the initial pose acquisition module is executed; otherwise, the acquired initial pose of the ICP algorithm is used as the initial value and brought into the localization calculation module.

[0047] The positioning calculation module is used to execute the method described in any one of claims 1 to 4 based on the initial pose as input to the real-time positioning algorithm; and finally calculates the real-time pose calculation score.

[0048] The fusion calculation module is used to fuse LiDAR data with IMU data and odometer data when the score is greater than the third threshold; otherwise, the result calculated by LiDAR is used as the fused result, and IMU and odometer do not participate in the calculation. The fused data is transmitted to step S05 in real time as the prediction value for the next iteration calculation.

[0049] Furthermore, the fusion method in the fusion computing module is as follows:

[0050] IMU data processing: IMU data is used for integration calculations to obtain the robot's displacement and rotation;

[0051] Odometry data processing: Odometry data is used for integration calculations to obtain the robot's displacement;

[0052] LiDAR data processing: The data calculated by matching LiDAR data with map data is assigned to a variable, and the obtained data is the robot's pose;

[0053] Localization data fusion: First, if the LiDAR pose is different from the previous pose and the real-time pose calculation score is less than the third threshold, then the LiDAR pose is considered accurate and is directly used as the fused pose. At the same time, IMU and odometry data are used to estimate the robot's pose between the LiDAR in the previous frame and the current frame. These poses are then used as the starting point for Riemann integration to obtain the fused pose.

[0054] Data release: The fused pose is released on the corresponding topic, and the final data frequency is no less than 100Hz.

[0055] The present invention also provides a processing device, comprising at least one processor and at least one memory communicatively connected to the processor, wherein the memory stores program instructions executable by the processor, and the processor can execute the above-described method by invoking the program instructions.

[0056] The present invention also provides a computer-readable storage medium storing computer instructions that cause the computer to perform the above-described method.

[0057] The advantages of this invention are:

[0058] This invention is based on an iterative optimization method for removing ground point cloud feature points using Ceres, which significantly improves the processing speed of real-time positioning algorithms; it has low computational requirements, high real-time performance, and high robustness; it is compatible with daytime, nighttime, and low-light rain and fog weather.

[0059] The sensor fusion process is robust, avoiding divergence after fusion. It adjusts and maintains a 6-dimensional covariance matrix or information matrix for each variable in IMU, ODOM, and Laser_pose, totaling 18*18 dimensions. It can directly match and locate the 3D map without switching to 2D maps. It can still output 100Hz fusion positioning results even in areas where LiDAR positioning is poor in charging rooms. This algorithm appropriately reduces the amount of data processing and optimizes the processing logic. Finally, the processing utilization rate is less than 50% on a single Intel processor CPU, supporting real-time operation on embedded processors.

[0060] This invention uses a fusion algorithm that combines IMU, odometer, and lidar matching, eliminating the need for traditional filtering algorithms. It has fewer parameters, is faster, more robust, and outputs 100Hz positioning data in real time. Attached Figure Description

[0061] Figure 1 This is a logic block diagram of lidar data preprocessing in Embodiment 1 of the present invention;

[0062] Figure 2 This is a flowchart of the laser lidar distortion correction process in Embodiment 1 of the present invention;

[0063] Figure 3 This is a flowchart illustrating the overall workflow of Embodiment 2 of the present invention;

[0064] Figure 4 This is a flowchart of the data fusion process of IMU, odometer, and lidar in Embodiment 2 of the present invention. Detailed Implementation

[0065] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. 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.

[0066] Example 1

[0067] This embodiment describes a pose optimization method for robots operating indoors and outdoors with multiple sensors and few parameters, which is well-suited for narrow or enclosed spaces. The method includes the following steps:

[0068] Step 1, data preprocessing, such as Figure 1 As shown:

[0069] 1) Initially, the lidar rotates one full circle, collecting point cloud data. After receiving a frame of point cloud data, it is first converted into an easily processed PCL format. The lidar's rotation is not a complete 360-degree rotation from 0 degrees in one direction; there is an initial deviation. Therefore, the starting and ending angles of this point cloud frame need to be determined. Based on the acquired point cloud data, the row number of each point cloud element is calculated, and reprojection is performed.

[0070] 2) Calculate ground points. Extract points below the horizontal angle from all points. For a laser point in beam i, find the corresponding point in beam i+1. Calculate the difference vector formed by the lines connecting these two points and the radar. If the angle between this difference vector and the horizontal plane is less than 10 degrees, it is considered a ground point.

[0071] 3) Cluster the remaining point cloud and record the clustered point cloud. Then, sparsify the ground point cloud and save the segmented point cloud.

[0072] 4) IMU-based laser distortion correction: calculate curvature, calculate smoothness, and remove unreliable points.

[0073] 5) To prevent feature extraction from being too dense, the current point cloud frame is divided into 6 parts according to line bundles for feature extraction. The curvature of the point cloud in each part is arranged from smallest to largest, and line features are extracted. All remaining points are set as surface points, and then downsampling is performed to reduce the amount of computation. Finally, the point cloud is published.

[0074] The laser distortion correction process based on the IMU in the previous step is as follows: Figure 2 As shown:

[0075] This example only addresses laser point cloud distortion during rotation using Slerp interpolation and inverse transformation. Considering the significant increase in computational load during robot rotation, and the noticeable distortion between two consecutive laser frames during robot rotation, this example performs laser distortion correction for rotation. For translation, the algorithm has good convergence and low computational cost, so distortion correction is not considered. The implementation involves real-time subscription to IMU data and storage in a queue. The IMU data includes attitude angles, angular velocities, and angular accelerations. Angles exhibit some drift, and angular velocities and angular accelerations can be integrated over a certain time to calculate the corresponding angles and displacements (assuming the lidar is converted to the IMU coordinate system via extrinsic parameters between the IMU and the radar). Given the high frequency of IMU data (>200Hz), to reduce algorithm complexity, this example only processes the IMU's attitude angles. To address angle drift, a method of calculating relative angle values ​​over a period of time is used. The Euler angles of each frame of IMU data are acquired, and the IMU timestamp for each frame is recorded. Simultaneously, the IMU pose at the initial point of the LiDAR is recorded (considering time alignment issues, the initial point is interpolated to the pose corresponding to the LiDAR's initial timestamp using the SLERP interpolation algorithm). The LiDAR outputs data at 10Hz, with each frame of LiDAR data being 100ms by default. Within 100ms, the LiDAR acquires data at a constant speed. The data for each point in the LiDAR is analyzed to obtain the timestamp corresponding to each point in the point cloud. For the current timestamp, the preceding and following IMU data in the IMU queue are searched, and the pose corresponding to the corresponding point is interpolated using SLERP. The interpolation coefficient is the ratio of the timestamp of the current point cloud to the timestamps of the preceding and following IMU data. Finally, the pose corresponding to each point cloud is obtained. By inversely transforming the current point to the initial pose, the position of each point relative to the initial point cloud is obtained. After traversing all the point cloud data in a frame, the position of the entire point cloud relative to the initial moment of the laser is obtained. The point cloud is then subjected to distortion correction processing based on its rotation state. The calculated new point cloud data is then assigned to the original stored point cloud position as the distorted point cloud.

[0076] Step 2, Scene Degradation Handling Method

[0077] Based on Ceres optimization to remove ground points and perform face point matching and localization, a point-to-face distance constraint is constructed, and a nonlinear optimization library is used to accelerate the process, reducing complexity and improving processing efficiency.

[0078] The previous step yielded point clouds with different features, including polygon point clouds, ground point clouds, and corner point clouds. The map also contains polygon point cloud maps, corner point cloud maps, and overall point cloud maps. Due to the large volume of global point cloud data, the incomplete data feature information represented by corner point clouds, and the large overall volume of polygon point clouds, while ground point clouds can serve as constraints to prevent Z-direction drift during mapping, Z-direction drift is no longer relevant in the localization algorithm. Therefore, removing ground point cloud data during point cloud matching significantly reduces the data volume, improves computational efficiency, and increases robustness. This example uses a method of removing ground point cloud information from the polygon point cloud map for matching and localization, employing a nonlinear optimization algorithm based on the Ceres library. The specific algorithm steps are as follows:

[0079] 1. Iterate through the input point cloud data after removing the ground point cloud (if the point cloud data is too small, less than 200 points, do not process it and exit the processing flow directly). Process each point, taking the current point as input based on the user-provided initial positioning data or the previous calculation result (if this is the first time the algorithm is executed, the data is the user-provided initial positioning value; otherwise, it is the positioning result calculated in the previous calculation), and transform the radar coordinate system to the global map coordinate system.

[0080] 2. Use a kd-tree to find the 5 nearest neighbors of the current point on the global map. If the distance between the farthest neighbor and the current point is less than the threshold of 3 meters, continue processing; otherwise, skip the current point.

[0081] 3. For the five neighboring points found, calculate their mean and construct a 5x3 matrix matA0, where each row represents the coordinates of a neighboring point. This matrix can be viewed as the coefficient matrix of a linear system. matB0 is a 5x1 matrix with all elements being -1. This matrix can be viewed as the constant term of the linear system. Solve the linear equation system matA0*matX0=matB0 using QR decomposition with column pivoting, where matB0 is a 5x1 matrix with all elements being -1, and the solution matX0 represents the parameters of the plane. Obtain the plane's normal vector (pa, pb, pc) and the distance pd from the origin from the solution matX0, and normalize them. QR decomposition with column pivoting is a method for solving linear equation systems that can handle cases where the coefficient matrix is ​​rectangular (i.e., the number of equations and the number of unknowns are not equal). In this problem, we have 5 equations (5 neighboring points) and 3 unknowns (the plane's parameters). Obtain the plane's normal vector (pa, pb, pc) and the distance pd from the origin from the solution matX0. Here, pa, pb, and pc are a, b, and c in the plane equation ax + by + cz + d = 0, and pd is d. Then, the normal vector is normalized to ensure that its length is 1.

[0082] In summary, a plane was fitted using the least squares method, and this plane conforms to the distribution of neighboring points.

[0083] 4. Check if all neighboring points are on the calculated plane (i.e., the distance from the plane is less than 0.2). If so, a valid planar feature has been found.

[0084] 5. If a valid planar feature is found, calculate the distance dist from the current point to the plane, and calculate the projection point point_proj of the current point on the plane.

[0085] 6. Create a cost function e, representing the distance from the current point to the plane. The process of creating the cost function is as follows:

[0086] The cost function defines a structure called CostPlan, which is used to compute the Ceres cost function between the planar features of the point cloud and the map cloud. Ceres is a library for solving nonlinear least squares problems.

[0087] The `Create` static function takes the current point, the projection point, and the transformation matrix as parameters, creates a new `CostPlan` object, and wraps it as a Ceres cost function. This function returns a pointer to `ceres::CostFunction`, which can be directly used in Ceres optimization problems.

[0088] In the CostPlan struct, the operator() function is a template function that accepts two parameters:

[0089] -const T*PRi: This is a pointer to a 6-dimensional vector representing the robot's pose, including position and rotation. The first three elements of this vector are the position, and the last three elements are the logarithmic mapping of the rotations.

[0090] -T*residual: This is a pointer to a 3-dimensional vector used to store the calculated residuals.

[0091] First, the input current point and projection point are converted into template type T. Then, the input pose is mapped into a 6-dimensional vector. The Sophus library, developed based on C++ and Eigen library, is used to represent the 3D rotation group SO(3). The logarithmic mapping of the rotation part is converted into a quaternion. Then, the rotation and translation from the world coordinate system to the local coordinate system are calculated. Finally, the position of the point on the map is calculated, and the distance from the point to the projection point is calculated and saved into the residual.

[0092] 7. The above process establishes an abstract model of the positioning system. The number of processing threads used is set to 4-6, and the number of iterations is 5. In order to minimize the sum of squares of the residuals (i.e., the difference between the observed data and the model prediction), the solve function in the Ceres library is called to solve for the corresponding positioning result x0.

[0093] 8. Output the positioning result x0 obtained in step 7, repeat step 1, and repeat the above steps until convergence to a certain threshold (rotation threshold is 0.5 degrees, translation threshold is 0.05m, iteration number threshold is 5 times) to obtain the transformation relationship between Xrobot and Map, that is, the positioning data at the current time.

[0094] In summary, this process primarily involves extracting planar features from the input point cloud and calculating the distance from each point to its corresponding plane. These distances are used to subsequently calculate the Ceres cost function between the point cloud planar features and the map cloud. This cost function can be used to optimize the robot's pose by minimizing it. The iterative optimization method for removing ground point cloud feature points based on Ceres significantly improves the processing speed of real-time localization algorithms.

[0095] Example 2

[0096] Based on the solution to the scene degradation problem in Embodiment 1, this embodiment provides a method for rapid robot localization, such as... Figure 3 As shown, the specific steps include:

[0097] Step 1: Build a 3D map based on LOAM or related algorithms such as Lego-LOAM and Lio-SAM, and save the data as Map data, corner map, polygon map, and point cloud data corresponding to key points;

[0098] Step 2: During localization, first provide initial localization data or read real-time saved data as the initial pose;

[0099] Step 3: Based on this initial pose, obtain key points within 3 meters of the key points, and find the map point cloud corresponding to the corner points and face points;

[0100] Step 4: Convert the point cloud map to the Map coordinate system using this key point to obtain the converted point cloud map;

[0101] Step 5: Perform ICP matching on the obtained transformed point cloud map and Map. If the matching score is higher than the second threshold, proceed to step 2 (at this point, manual intervention can be used to give a new initial value). If it is lower than the second threshold, use the obtained ICP algorithm initial pose as the initial value to enter step 6.

[0102] Step 6: Transmit the precise initial pose to the real-time positioning algorithm;

[0103] Step 7: The real-time positioning algorithm iteratively solves the real-time pose, which is the method in Example 1, and calculates the real-time pose solution score.

[0104] Step 8: If the score is greater than the third threshold, fuse the LiDAR data with the IMU data and the odometer data; otherwise, use the result calculated by the LiDAR as the fused result (at this time, the IMU and odometer do not participate in the calculation).

[0105] Step 9: Fuse the attitude and odometry data from the IMU to output real-time position data at a frequency of not less than 100Hz.

[0106] Step 10: The fused data is transmitted to Step 6 in real time and used as the predicted value for the next iteration calculation.

[0107] The data fusion process for IMU, odometer, and lidar is as follows: Figure 4 As shown:

[0108] 1. IMU Data Processing: IMU data is used to estimate the robot's pose and velocity. This data is stored in a locked double-ended queue and can be retrieved via specific functions. The retrieved IMU data is then used for integration calculations to obtain the robot's displacement and rotation.

[0109] 2. Odometry Data Processing: Odometry data provides the robot's linear and angular velocities. This data is stored in a separate locked double-ended queue and can be retrieved via functions. The retrieved odometry data is used for integration calculations to obtain the robot's displacement.

[0110] 3. LiDAR data processing: The data calculated by matching the LiDAR data with the map data is assigned to a variable, and the obtained data is the robot's pose.

[0111] 4. Localization Data Fusion: First, if the LiDAR pose differs from the previous pose, and the map matching cost is less than a threshold, then the LiDAR pose is considered accurate and directly used as the fused pose. Simultaneously, IMU and odometry data are used to estimate the robot's pose between the LiDAR in the previous frame and the current frame. These poses are then used as a starting point for Riemann integration to obtain the fused pose.

[0112] 5. Data Release: The fused pose is released on the corresponding topic, and the final data frequency is no less than 100Hz.

[0113] This embodiment first uses LOAM or related algorithms such as Lego-LOAM and Lio-SAM to build a 3D map using LiDAR data. This map data includes map data, corner maps, polygon maps, and point cloud data corresponding to key points, providing detailed map information for subsequent positioning.

[0114] At the start of localization, the system acquires initial localization data or reads real-time saved data as the initial pose. Next, the system extracts key points within 3 meters of key points and finds the corresponding corner and area point clouds on the map.

[0115] By transforming the point cloud map to the Map coordinate system using the initial pose, the system achieves the transformation from the sensor coordinate system to the global coordinate system. Next, the system performs ICP matching. If the matching score is higher than a second threshold, the initial pose is updated and re-localization is performed; if it is lower than the threshold, the initial pose of the ICP algorithm is used as the initial value.

[0116] The precise initial pose is transmitted to the real-time localization algorithm, and the system begins iteratively solving for the real-time pose. The Ceres optimization algorithm is used to calculate the real-time pose solution score. If the score exceeds a threshold, the system fuses the attitude data from the IMU and the pose data from the odometry to improve the accuracy of the pose estimation.

[0117] Ultimately, the system outputs real-time location data at a frequency of at least 100Hz and transmits the fused data to the next round of iterative calculations as a prediction value. In this way, the system achieves efficient and accurate real-time positioning, and by fusing information from multiple sensors, it improves the system's robustness and stability.

[0118] Throughout the process, key steps such as point cloud processing, key point extraction, pose estimation, pose optimization, data fusion, and real-time updates and deployment collaborate to ensure the system's reliable operation in complex environments. This integrated system based on advanced algorithms provides an efficient and real-time solution for mobile robots to navigate and locate in unknown environments.

[0119] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications 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.

Claims

1. A method for high-frequency real-time positioning of a mobile robot indoors and outdoors, characterized in that, Includes the following steps: S01. Obtain the initial LiDAR positioning data of the mobile robot and save it as the initial pose; S02. Based on the positioning data, obtain key points within a certain range nearby, and based on the key points, find the point cloud map data corresponding to the corner points and surface points; S03. Convert the point cloud map data to the Map coordinate system to obtain the converted point cloud map; S04. Perform ICP algorithm matching between the transformed point cloud map and the Map. If the matching score is higher than the second threshold, proceed to step S01. Otherwise, use the obtained initial pose of the ICP algorithm as the initial value and bring it into step S05. S05. Based on the initial pose as the input to the real-time localization algorithm, execute steps 1 to 7; finally, calculate the real-time pose solution score. Step 1. Preprocess the measurement data acquired by the lidar on the robot to obtain point cloud data; and optimize the ground point cloud by removing the point cloud data based on Ceres. Step 2. Traverse the point cloud data after removing the ground point cloud, process each point, and take the current point as the initial positioning data given by the user or the previous calculation result as input to transform the lidar coordinate system to the map coordinate system; Step 3. Use a kd-tree to find the n nearest neighbors of the current point in the Map; if the distance between the farthest neighbor and the current point is less than the threshold m, continue processing; otherwise, skip the current point; a plane is fitted based on the n neighbors. Step 4. Calculate the distance dist from the current point to the plane, and calculate the projection point point_proj of the current point on the plane; Step 5. Construct the localization model: Create a cost function that defines a structure named CostPlan. Within the CostPlan structure, the operator() function is configured to accept two parameters: const T*PRi: is a pointer to a 6-dimensional vector representing the robot's pose. The first three elements of the 6-dimensional vector are the position, and the last three elements are the logarithmic mapping of rotations. T*residual: A pointer to a 3-dimensional vector used to store the calculated residual; operator() first converts the input current point and projection point into template type T, then maps the input pose into a 6-dimensional vector, and uses the Sophus library developed based on C++ and Eigen library, where the three-dimensional rotation group SO(3) class is used to convert the logarithmic mapping of the rotation part into a quaternion, then calculates the rotation and translation from the world coordinate system to the local coordinate system, and finally calculates the position of the current point on the Map and the distance from the current point to the projection point, and saves it into the residual; Step 6. To minimize the sum of squares of the residuals, call the solve function in the Ceres library to solve for the corresponding location result x0; Step 7. Output the obtained positioning result x0 as the input of step 2. Repeat steps 2-7 until convergence to a certain threshold to obtain the transformation relationship between the robot's position and pose and the Map, i.e. the positioning data at the current moment. S06. When the score is greater than the third threshold, the LiDAR data is fused with the IMU data and the odometer data. Otherwise, the result calculated by the LiDAR is used as the fused result. At this time, the IMU and odometer do not participate in the calculation. The fused data is transmitted to step S05 in real time as the prediction value for the next iteration calculation.

2. The method for real-time indoor and outdoor high-frequency positioning of a mobile robot according to claim 1, characterized in that, The specific preprocessing method for measurement data acquired by lidar is as follows: First, the measurement data is converted to PCL format. Based on the acquired measurement data, i.e. point cloud data, the row number of each point cloud in the point cloud is calculated and then reprojected. Then, ground points are calculated. Points below the horizontal angle are extracted from all points. For a laser point in beam i, the corresponding point is found in beam i+1. The difference vector between the lines connecting these two points and the radar is calculated. If the angle between the difference vector and the horizontal plane is less than a set value, it is considered a ground point. Cluster the remaining point cloud and record the clustered point cloud. Sparsify the ground point cloud and save the segmented point cloud. Laser distortion correction based on IMU: calculate curvature, calculate smoothness, and remove unreliable points; For the current point cloud frame, extract features by line bundle, arrange the curvature of the point cloud in each bundle from smallest to largest, extract line features, set all remaining points as surface points, then perform downsampling to reduce the amount of computation, and finally publish the point cloud.

3. The method for indoor and outdoor high-frequency real-time positioning of a mobile robot according to claim 2, characterized in that, The specific method for laser distortion correction is as follows: The system subscribes to IMU sensor data in real time and stores it in a queue. It obtains the attitude, angle, angular velocity, and angular acceleration of each frame of IMU sensor data and records the IMU sensor timestamp for each frame. It records the IMU sensor attitude at the initial point of the LiDAR. It analyzes the data of each point of the LiDAR to obtain the timestamp corresponding to each point in the point cloud. It finds the previous and next IMU data in the IMU queue for the current timestamp. It then interpolates the corresponding attitude to the corresponding point using SLEP. The pose corresponding to each point cloud is obtained, and the current point is transformed to the initial pose through inverse pose transformation to obtain the position of each point relative to the initial point cloud. After traversing all the point cloud data of a frame, the position of the entire point cloud relative to the initial moment of the laser is obtained. The calculated new point cloud data is assigned to the original stored point cloud position as the distortion-free point cloud.

4. A method for real-time indoor and outdoor high-frequency positioning of a mobile robot according to any one of claims 1 to 3, characterized in that, The method for fitting the plane in step 3 is as follows: For the n neighboring points found, calculate their mean and construct an n x m matrix matA0, where each row represents the coordinates of a neighboring point. Solve the linear equation system matA0*matX0=matB0 using column pivoting QR decomposition, where matB0 is an n x 1 matrix with all elements being -1, and the solution matX0 of the equation system represents the parameters of the plane. Obtain the plane's normal vector (pa, pb, pc) and distance pd from the origin from the solution matX0, and normalize them. Ensure that its length is 1, and fit a plane.

5. A mobile robot indoor and outdoor high-frequency real-time positioning system, characterized in that, include: The initial pose acquisition module is used to acquire the initial LiDAR positioning data of the mobile robot and save it as the initial pose. The point cloud map data acquisition module is used to acquire key points within a certain range in the vicinity based on the positioning data, and to find the point cloud map data corresponding to the corner points and polygon points based on the key points. The map conversion module is used to convert point cloud map data to the Map coordinate system to obtain a converted point cloud map. The pose matching module is used to match the transformed point cloud map with the Map using the ICP algorithm. If the matching score is higher than the second threshold, the initial pose acquisition module is executed; otherwise, the acquired initial pose of the ICP algorithm is used as the initial value and brought into the localization calculation module. The localization calculation module is used to execute steps 1 to 7 based on the initial pose as input to the real-time localization algorithm; finally, it calculates the real-time pose solution score. Step 1. Preprocess the measurement data acquired by the lidar on the robot to obtain point cloud data; and optimize the ground point cloud by removing the point cloud data based on Ceres. Step 2. Traverse the point cloud data after removing the ground point cloud, process each point, and take the current point as the initial positioning data given by the user or the previous calculation result as input to transform the lidar coordinate system to the map coordinate system; Step 3. Use a kd-tree to find the n nearest neighbors of the current point in the Map; if the distance between the farthest neighbor and the current point is less than the threshold m, continue processing; otherwise, skip the current point; a plane is fitted based on the n neighbors. Step 4. Calculate the distance dist from the current point to the plane, and calculate the projection point point_proj of the current point on the plane; Step 5. Construct the localization model: Create a cost function that defines a structure named CostPlan. Within the CostPlan structure, the operator() function is configured to accept two parameters: const T*PRi: is a pointer to a 6-dimensional vector representing the robot's pose. The first three elements of the 6-dimensional vector are the position, and the last three elements are the logarithmic mapping of rotations. T*residual: A pointer to a 3-dimensional vector used to store the calculated residual; operator() first converts the input current point and projection point into template type T, then maps the input pose into a 6-dimensional vector, and uses the Sophus library developed based on C++ and Eigen library, where the three-dimensional rotation group SO(3) class is used to convert the logarithmic mapping of the rotation part into a quaternion, then calculates the rotation and translation from the world coordinate system to the local coordinate system, and finally calculates the position of the current point on the Map and the distance from the current point to the projection point, and saves it into the residual; Step 6. To minimize the sum of squares of the residuals, call the solve function in the Ceres library to solve for the corresponding location result x0; Step 7. Output the obtained positioning result x0 as the input of step 2. Repeat steps 2-7 until convergence to a certain threshold to obtain the transformation relationship between the robot's position and pose and the Map, i.e. the positioning data at the current moment. The fusion calculation module is used to fuse LiDAR data with IMU data and odometer data when the score is greater than the third threshold; otherwise, the result calculated by LiDAR is used as the fused result, and IMU and odometer do not participate in the calculation. The fused data is transmitted to step S05 in real time as the prediction value for the next iteration calculation.

6. The indoor and outdoor high-frequency real-time positioning system for a mobile robot according to claim 5, characterized in that, The fusion method in the fusion computing module is as follows: IMU data processing: IMU data is used for integration calculations to obtain the robot's displacement and rotation; Odometry data processing: Odometry data is used for integration calculations to obtain the robot's displacement; LiDAR data processing: The data calculated by matching LiDAR data with map data is assigned to a variable, and the obtained data is the robot's pose; Localization data fusion: First, if the LiDAR pose is different from the previous pose and the real-time pose calculation score is less than the third threshold, then the LiDAR pose is considered accurate and is directly used as the fused pose. At the same time, IMU and odometry data are used to estimate the robot's pose between the LiDAR in the previous frame and the current frame. These poses are then used as the starting point for Riemann integration to obtain the fused pose. Data release: The fused pose is released on the corresponding topic, and the final data frequency is no less than 100Hz.

7. A processing device, characterized in that, It includes at least one processor and at least one memory communicatively connected to the processor, wherein: the memory stores program instructions executable by the processor, and the processor can execute the method as described in any one of claims 1 to 4 by invoking the program instructions.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that cause the computer to perform the method as described in any one of claims 1 to 4.

Images