Fusion positioning method and device of robot

Abstract

The application provides a fusion positioning method and device of a robot, and belongs to the technical field of robot positioning. The method comprises the following steps: acquiring inertial measurement data, point cloud data and a three-dimensional global point cloud map; in response to the robot being stationary, determining initialization parameters of an inertial measurement unit according to the inertial measurement data; removing motion distortion from the point cloud data according to the initialization parameters to generate a de-distorted point cloud; registering the de-distorted point cloud with a global submap of the three-dimensional global point cloud map to obtain global observation values, and registering a local key frame map constructed online to obtain odometry observation values; driving an error state filter according to the initialization parameters and the inertial measurement data to perform state prediction, and updating the state of the error state filter according to the global observation values and the odometry observation values to obtain a pose estimation of the robot; integrating the inertial measurement data to output pose incremental information of the pose estimation, and fusing the pose estimation and the pose incremental information to output a positioning result.

Description

Technical Field

[0001] This application belongs to the technical field of robot localization, and specifically relates to a fusion localization method and apparatus for a robot. Background Technology

[0002] With the widespread application of intelligent robots in security, inspection, and logistics, their operating environments are expanding from structured, small-scale indoor scenarios to unstructured, large-scale, and complex mixed indoor-outdoor scenarios. In such complex, large-space environments, such as large factories, industrial parks, large warehouses, underground parking lots, and airport terminals, covering areas of hundreds of thousands or even millions of square meters, robots need to possess the ability to continuously, reliably, and with high accuracy estimate their own pose within that area. However, when facing large-scale complex scenarios, a single sensor often struggles to overcome challenges such as changes in ambient lighting, missing structural features, and dense dynamic obstacles. Existing positioning technologies still have the following shortcomings in addressing these requirements: 1. Existing odometry-based positioning schemes obtain relative positioning relationships by fusing and accumulating the relative pose changes between LiDAR sensor frames and the integral of instantaneous measurement results from the inertial measurement unit. Due to sensor noise, feature mismatch, and simplification of motion and observation models, this type of scheme lacks a global correction mechanism. As robots operate in large-scale scenes for extended periods, LiDAR odometry exhibits significant positioning drift, with positioning errors continuously increasing over time, failing to guarantee globally consistent positioning in large spaces and long-term operation.

[0003] 2. Existing 3D localization methods based on LiDAR and relying on global maps (such as HDL methods) typically assume that sensor observations can always effectively match a pre-built high-precision global point cloud map. However, in actual deployments, robots often enter unmapped areas or areas where the map is dynamically changing. In satellite-denied unstructured environments, decreased accuracy or even failure of LiDAR SLAM is a common problem. These methods, lacking effective prior map constraints, cannot provide reliable localization references in unmapped or map-mismatched areas, leading to localization loss.

[0004] 3. Existing 3D localization methods based on LiDAR and relying on global maps typically load an entire global map at system startup. When dealing with large spatial scenes, the sheer volume of 3D point cloud map data significantly reduces the computational efficiency of point cloud registration algorithms such as ICP (Iterative Closest Point) and NDT (Normal Distributions Transform). Furthermore, system memory limitations and the constraints of third-party point cloud processing libraries (such as PCL) on data scale also make it difficult to load large-scale point cloud data into memory at high resolution all at once.

[0005] 4. Existing localization methods based on 3D LiDAR typically use the acquisition frequency of the raw LiDAR point cloud (usually 10Hz) as the upper limit for pose output. However, in practical applications, robot chassis control usually requires pose feedback of at least 50Hz, while UAV flight control requires attitude / position feedback of over 100Hz. Relying solely on a 10Hz LiDAR output frequency is insufficient to meet the requirements of high-dynamic systems for high-frequency, continuous, and smooth state estimation.

[0006] 5. During the scanning process of LiDAR, due to the continuous movement of the robot, the acquisition time of different laser points in a single frame of point cloud corresponds to different robot poses, resulting in motion distortion of the point cloud. Existing methods lack effective means to remove motion distortion before point cloud registration. Directly using distorted point clouds for registration will lead to a decrease in matching accuracy and affect the accuracy of the positioning results.

[0007] 6. While some existing solutions attempt to fuse IMU (Inertial Measurement Unit) and LiDAR data, they often employ loose coupling, failing to fully leverage the complementary advantages of the IMU's high-frequency characteristics and the LiDAR's precise sensing capabilities. Particularly in satellite-denied environments, the lack of an effective architecture for tightly coupled fusion of multi-source heterogeneous information limits the system's positioning accuracy and robustness in complex scenarios.

[0008] The content of the background section is merely the technology known to the inventor and does not necessarily represent the prior art in this field. Summary of the Invention

[0009] This application aims to provide a fusion localization method and apparatus for robots to solve at least one of the above-mentioned technical problems.

[0010] According to one aspect of this application, a fusion localization method for a robot is provided. The robot is equipped with an inertial measurement unit (IMU), a lidar, and an error state filter. The method includes: acquiring inertial measurement data from the IMU, point cloud data from the lidar, and a 3D global point cloud map of the target scene; determining initialization parameters of the IMU based on the inertial measurement data in response to the robot being stationary; performing motion distortion removal on the point cloud data based on the initialization parameters to generate a distorted point cloud; registering the distorted point cloud with a global sub-map of the 3D global point cloud map to obtain global observations, and registering it with an online constructed local keyframe map to obtain odometry observations; driving the error state filter to perform state prediction based on the initialization parameters and the inertial measurement data, and updating the state of the error state filter based on the global observations and odometry observations to obtain a pose estimate of the robot; integrating the inertial measurement data to output pose increment information of the pose estimate, and fusing the pose estimate with the pose increment information to output the localization result at a frequency higher than the lidar frame rate.

[0011] According to some embodiments of this application, motion distortion removal is performed on point cloud data based on initialization parameters to generate a dedistorted point cloud, including: acquiring inertial measurement data within the acquisition period corresponding to each frame of point cloud data; integrating the inertial measurement data within the acquisition period according to the initialization parameters to obtain the robot's pose information within the acquisition period; interpolating the pose information based on the time offset between the acquisition time of the laser point in each frame of point cloud data and the end time of the acquisition period to obtain the laser point distortion removal matrix; and projecting the coordinates of the laser point onto the coordinate system at the end time of the acquisition period based on the distortion removal matrix to generate a dedistorted point cloud.

[0012] According to some embodiments of this application, registering the distorted point cloud with a global sub-map of a 3D global point cloud map to obtain global observations includes: registering the distorted point cloud with the global sub-map using a normal distribution transformation algorithm to obtain the robot's six-degree-of-freedom pose in the global map coordinate system of the 3D global point cloud map, which is then used as global observations.

[0013] According to some embodiments of this application, the process of constructing a local keyframe map includes: storing the first frame in the distorted point cloud as an initial keyframe in a keyframe queue, and using the pose of the initial keyframe as a comparison reference; sequentially acquiring subsequent frames in the distorted point cloud, and calculating the translation distance and / or rotation angle between the pose of the subsequent frames and the comparison reference; when the translation distance exceeds a preset translation distance threshold or the rotation angle exceeds a preset rotation angle threshold, storing the corresponding subsequent frame as a new keyframe in the keyframe queue, and updating the comparison reference with the pose of the new keyframe; when the length of the keyframe queue exceeds a preset length threshold, removing the keyframe with the earliest storage time in the keyframe queue; and stitching together the point cloud data of all keyframes in the keyframe queue to generate a local keyframe map.

[0014] According to some embodiments of this application, registering the odometer observations with an online constructed local keyframe map includes: registering the distorted point cloud with the local keyframe map using a normal distribution transformation algorithm to obtain the first pose of the distorted point cloud in the coordinate system of the local keyframe map for the current frame; obtaining the second pose obtained by registering the distorted point cloud with the local keyframe map for the previous frame; and calculating the inter-frame relative pose transformation based on the first pose and the second pose, which is used as the odometer observation.

[0015] According to some embodiments of this application, the initialization parameters include zero bias and the direction of gravitational acceleration. Driving the error state filter to predict the state based on the initialization parameters and inertial measurement data includes: correcting the inertial measurement data according to the zero bias in the initialization parameters to obtain corrected inertial measurement data; constructing a state prediction model for the error state filter based on the direction of gravitational acceleration in the initialization parameters and the corrected inertial measurement data; and iteratively updating the state variables of the error state filter according to the state prediction model to drive the error state filter to predict the state. According to some embodiments of this application, updating the state of the error state filter based on global observations and odometry observations to obtain the robot's pose estimate includes: constructing a first observation equation for the error state filter based on global observations; constructing a second observation equation for the error state filter based on odometry observations; calculating the update amount of the state variables of the error state filter based on the first and second observation equations; correcting the state variables of the error state filter based on the update amount; and converting the corrected state variables into the robot's pose estimate.

[0016] According to some embodiments of this application, inertial measurement data is integrated to output pose increment information for pose estimation, and the pose estimation and pose increment information are fused to output the positioning result at a frequency higher than the lidar frame rate. This includes: using the pose estimation output by the error state filter in the previous step as the pose estimation reference, integrating the inertial measurement data between two adjacent frames of distorted point clouds to obtain the pose increment information between the two adjacent frames of distorted point clouds; and superimposing the pose estimation reference and the pose increment information to output the positioning result at a frequency higher than the lidar frame rate.

[0017] According to some embodiments of this application, the global subgraph is constructed as follows: the three-dimensional global point cloud map is divided into multiple map blocks according to a preset size, and a spatial index identifier is assigned to each map block; the current map block is determined according to the robot's current position; with the current map block as the center, the map blocks within a preset range around the current map block are loaded and stitched together to form a global subgraph.

[0018] According to another aspect of this application, a fusion positioning apparatus for performing any of the fusion positioning methods described above is also provided, comprising: a data acquisition module for acquiring inertial measurement data, point cloud data, and a three-dimensional global point cloud map; an initialization module for determining initialization parameters based on the inertial measurement data in response to the robot being stationary; a motion distortion removal module for removing motion distortion from the point cloud data according to the initialization parameters to generate a distorted point cloud; a registration module for registering the distorted point cloud with a global sub-map of the three-dimensional global point cloud map to obtain global observations, and registering it with an online constructed local keyframe map to obtain odometry observations; a state estimation module for driving an error state filter to predict the state based on the initialization parameters and inertial measurement data, and updating the state of the error state filter based on the global observations and odometry observations to obtain a pose estimate of the robot; a trajectory calculation module for integrating the inertial measurement data and outputting pose increment information of the pose estimate; and an output module for fusing the pose estimate and the pose increment information to output the positioning result at a frequency higher than the lidar frame rate.

[0019] According to another aspect of this application, an electronic device is also provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the fusion positioning methods described above.

[0020] According to another aspect of this application, a computer-readable medium having processor-executable non-volatile program code is also provided, the program code causing the processor to perform any of the fusion positioning methods described above.

[0021] According to another aspect of this application, a computer program product is also provided, including a computer program stored on a computer-readable medium, the computer program including program instructions that, when executed by a computer, cause the computer to perform any of the fusion positioning methods described above.

[0022] The beneficial effects of the technical solutions provided in this application are: This application registers the distorted point cloud with both a global submap and an online-constructed local keyframe map to obtain global and odometry observations, which are then used together to update the state of the error state filter. This approach effectively suppresses long-term drift by using the global map as an absolute reference, addressing the problem of continuously increasing errors over time due to the lack of global correction in existing odometry schemes. Furthermore, the online-constructed local keyframe map maintains continuous relative motion observations in unmapped areas or when the global map fails, avoiding the localization loss in unmapped areas found in existing methods. Simultaneously, motion distortion removal ensures point cloud quality and improves registration accuracy, overcoming the problem of decreased registration accuracy due to the lack of effective distortion removal methods in existing technologies. In addition, by integrating the inertial measurement data and fusing it with the pose estimation output from the error state filter, the localization results are output at a frequency higher than the lidar frame rate, significantly improving the smoothness and high-frequency response capability of the localization. This breaks through the limitation of existing methods using the lidar frame rate (10Hz) as the upper limit for pose output. Moreover, the error state filter further improves the localization smoothness and high-frequency response capability by integrating the inertial measurement data and fusing it with the pose estimation output from the error state filter. Figure 3 By tightly coupling and fusing heterogeneous information sources, the problem of insufficient accuracy and robustness of existing loosely coupled schemes in satellite-denied environments is overcome, thereby realizing high-precision, high-robustness, and high-frequency six-degree-of-freedom pose estimation in complex large-space scenarios. Attached Figure Description

[0023] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0024] Figure 1 A flowchart illustrating the robot fusion localization method according to an embodiment of this application is shown; Figure 2 This paper shows a schematic diagram of the overall architecture of the robot fusion positioning device according to an embodiment of this application; Figure 3 A flowchart illustrating step S300 of the fusion positioning method according to an embodiment of this application is shown; Figure 4 A schematic diagram illustrating the motion distortion removal principle of an embodiment of this application is shown; Figure 5 A flowchart illustrating step S400 of the fusion positioning method according to an embodiment of this application is shown. Figure 6 This illustration shows another flowchart of step S400 of the fusion positioning method according to an embodiment of this application; Figure 7A schematic diagram illustrating the workflow of the incremental odometer module according to an embodiment of this application is shown; Figure 8 A flowchart illustrating step S500 of the fusion positioning method according to an embodiment of this application is shown. Figure 9 This illustration shows another flowchart of step S500 of the fusion positioning method according to an embodiment of this application; Figure 10 A flowchart illustrating step S600 of the fusion positioning method according to an embodiment of this application is shown; Figure 11 This illustration shows another flowchart of step S400 of the fusion positioning method according to an embodiment of this application; Figure 12 A flowchart illustrating the spatial index-based map tile strategy of this application is shown. Figure 13 A schematic diagram of a global subgraph according to an embodiment of this application is shown; Figure 14 A schematic diagram of the dual-pointer map cross-loading mechanism according to an embodiment of this application is shown; Figure 15 A schematic diagram of the fusion positioning device according to an embodiment of this application is shown.

[0025] Explanation of reference numerals in the attached figures: Fusion positioning device 1; data acquisition module 11; motion distortion removal module 12; registration module 13; state estimation module 14; trajectory calculation module 15; output module 16. Detailed Implementation

[0026] The present application will now be described in detail with reference to the accompanying drawings and embodiments. Various examples are provided by way of interpretation and not by way of limitation. In fact, those skilled in the art will recognize that modifications and variations can be made to the present application without departing from the scope or spirit thereof. For example, a feature represented or described as part of one embodiment may be used in another embodiment to produce yet another embodiment. Therefore, it is desirable that the present application encompass such modifications and variations that fall within the scope of the appended claims and their equivalents.

[0027] The accompanying drawings illustrate one or more examples of this application. The detailed description uses numerical and alphabetic designations to refer to features in the drawings. Similar or analogous designations in the drawings and description have been used to refer to similar or analogous parts of this application. As used herein, the terms “first,” “second,” “third,” and “fourth,” etc., are used interchangeably to distinguish one component from another and are not intended to indicate the location or importance of a single component.

[0028] The accompanying drawings in the following embodiments clearly and completely describe the technical solutions of this application. Obviously, the described embodiments are only some, not all, of the embodiments of this application. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0029] Figure 1 A flowchart illustrating the robot fusion localization method according to an embodiment of this application is shown. Figure 2 A schematic diagram of the overall architecture of a robot fusion localization device according to an embodiment of this application is shown. According to an example embodiment, such as… Figure 1 and Figure 2 As shown, the robot fusion localization method includes steps S100-S600. Exemplarily, the fusion localization method of this application can be executed in a computer system such as a set of executable instructions. Exemplarily, the computer system is a fusion localization apparatus. Furthermore, although the logical sequence is shown in the flowchart, under default conditions, the steps shown or described can be executed in a different order than that shown in the flowchart.

[0030] See Figure 2 The fusion positioning device consists of three parts: a data source, a multi-sensor fusion positioning framework based on error state Kalman filter, and a segmented 3D map management framework based on spatial index.

[0031] The data sources include an inertial measurement unit (IMU), a 3D LiDAR, and a 3D global point cloud map. The IMU provides inertial measurement data, the 3D LiDAR provides point cloud data, and the 3D global point cloud map provides prior map information of the target scene. The multi-sensor fusion localization framework based on an error-state Kalman filter includes static initialization, motion distortion removal, trajectory estimation, incremental laser odometry, and an error-state Kalman filter. It is used to fuse the multimodal data provided by the data sources and output the robot's six-degree-of-freedom pose estimate. The spatial index-based segmented 3D map management framework includes map segmentation, map update determination, and dual-pointer map cross-loading. It is used to segment and store, dynamically load, and switch the 3D global point cloud map in real time, providing the localization framework with a global sub-map around the current robot position as a registration reference. These functional frameworks cooperate to execute the fusion localization method described in the above embodiments of this application, achieving high global consistency, high robustness, and high frequency of localization output for the robot in large-space scenarios.

[0032] According to the example embodiment, the robot is equipped with an inertial measurement unit (IMU), a lidar system, and an error state filter. An inertial measurement unit (IMU) is an electronic sensor device used to measure the angular velocity and acceleration of an object in three-dimensional space, and consists of a gyroscope and an accelerometer. The gyroscope measures the rotational angular velocity (in rad / s) about the X, Y, and Z axes, while the accelerometer measures the linear acceleration (in m / s²) along the X, Y, and Z axes. 2 The data frequency is 200Hz. The IMU's measurement data is instantaneous, but due to the IMU's own zero bias (i.e., a constant error in which the output is not zero when at rest) and measurement noise, long-term integration of the zero bias will produce cumulative drift.

[0033] LiDAR (Light Detection and Ranging) is a sensor that measures three-dimensional spatial information of the surrounding environment by emitting laser beams and receiving reflected signals, with a data frequency of 10Hz. The point cloud data output by LiDAR consists of three-dimensional spatial points, each containing X, Y, and Z axis coordinate information. Since scanning one frame of point cloud data requires approximately 100ms, and the robot may move during this acquisition time, motion distortion may occur in the point cloud.

[0034] Error-State Kalman Filter (ESKF) is a state estimation algorithm used for multi-sensor fusion. Its core idea is to decompose the system state (the set of robot motion state variables estimated by ESKF) into nominal state and error state. The smaller error state is then linearized using Kalman filtering, thus avoiding direct linearization of complex nonlinear systems. This approach offers advantages such as high computational efficiency and good linearization accuracy.

[0035] In step S100, inertial measurement data from the inertial measurement unit, point cloud data from the lidar, and a three-dimensional global point cloud map of the target scene are acquired.

[0036] According to the example embodiment, the target scenario is the working environment in which the robot performs localization and navigation tasks, including but not limited to complex large-space scenarios encompassing mixed indoor and outdoor environments such as factories, industrial parks, warehouses, underground parking lots, airport terminals, shopping malls, mining areas, and outdoor parks. The 3D global point cloud map is a pre-constructed 3D global point cloud map of the target scenario using a globally consistent 3D laser SLAM (Simultaneous Localization and Mapping) algorithm. This 3D global point cloud map serves as a global prior map, providing absolute coordinate references for robot localization, eliminating cumulative drift caused by long-term operation, and ensuring global consistency in localization.

[0037] For example, the inertial measurement unit (IMU) provides acceleration information and angular velocity information along the X, Y, and Z axes in a right-handed coordinate system at a data frequency of 200 Hz. The lidar provides three-dimensional laser point cloud data at a data frequency of 10 Hz. For example, the fusion positioning device acquires inertial measurement data from the IMU, acquires point cloud data from the lidar, and loads the three-dimensional global point cloud map from the map storage module corresponding to the three-dimensional global point cloud map via an internal bus or network communication.

[0038] In step S200, in response to the robot coming to a standstill, the initialization parameters of the inertial measurement unit are determined based on the inertial measurement data.

[0039] According to the example embodiment, the initialization parameters include at least the zero bias of the inertial measurement unit (IMU) and the direction of gravitational acceleration. Zero bias refers to a non-zero constant error output by the IMU when stationary. Ideally, the gyroscope should output 0 rad / s and the accelerometer should output 0 m / s² when stationary. 2 (Ignoring gravity), due to factors such as sensor manufacturing processes and temperature variations, there is a constant offset in the actual output, called zero bias. Zero bias changes slowly over time and needs to be initially estimated when the inertial measurement unit starts up, and continuously estimated and updated by ESKF during operation.

[0040] The direction of gravitational acceleration refers to the direction vector of Earth's gravitational acceleration in the robot's coordinate system. When the robot is stationary, the specific force (i.e., the non-gravitational force per unit mass) measured by the robot's accelerometer is in the opposite direction of gravitational acceleration, thus determining the direction of gravitational acceleration.

[0041] For example, within a preset time (e.g., 1 second) before the fusion positioning device is activated, the robot remains stationary. The stationary initialization module in the fusion positioning device collects all inertial measurement data within this preset time. The variance of the angular velocity and acceleration data in the inertial measurement data is calculated to determine whether the robot is in a stable stationary state. The module also calculates whether the accelerometer reading modulus is within the theoretical value of gravitational acceleration (9.8 m / s²). 2 The data is then examined near the sensor's location to verify its validity. A zero-bias estimate is obtained by calculating the mean of the inertial measurement data and determining whether it falls within the sensor's preset measurement range. For example, when the above three conditions are met—data variance less than a preset threshold, gravitational modulus within 9.8 ± 0.5 m / s²—the zero-bias estimate is determined. 2 When both the zero bias estimate and the zero bias value are within the preset measurement range, the initialization is considered successful, and the initial zero bias and noise parameters are passed to the error state Kalman filter. If either condition is not met, the initialization is considered to have failed, and the fusion positioning device prompts the user to keep the robot stationary and continues initialization in the next second until the initialization is successful.

[0042] In step S300, motion distortion removal is performed on the point cloud data according to the initialization parameters to generate a distortion-free point cloud.

[0043] According to the example embodiment, motion distortion refers to the fact that, due to the scanning time (approximately 100ms) required for the LiDAR to scan a frame of point cloud, during which the robot continues to move, different LiDAR points in that frame of point cloud correspond to different robot poses at different acquisition times. If no correction is performed, the points in a frame of point cloud will be distributed in different coordinate systems, causing point cloud "tailing" or "deformation," which seriously affects the accuracy of subsequent point cloud registration.

[0044] For example, the motion distortion removal process of the fusion positioning device includes the following steps: (1) Obtain the inertial measurement data within the acquisition period corresponding to each frame of point cloud data, and integrate the inertial measurement data within the acquisition period according to the initialization parameters to obtain the pose information at each moment within the acquisition period.

[0045] (2) For each laser point in a frame of point cloud, based on the offset time of the laser point acquisition time relative to the start time, query the integral result of the corresponding inertial measurement data, and calculate the pose transformation matrix of the laser point relative to the end time of the frame by performing linear interpolation on the translation amount and spherical linear interpolation (SLERP) on the rotation amount.

[0046] (3) The original laser point coordinates are mapped to the unified coordinate system at the end of the frame using the pose transformation matrix. After performing the above transformation on all laser points, all the transformed laser points are organized into a complete point cloud data frame according to their spatial positions, thus generating the distortion-free point cloud. The coordinates of all points in the distortion-free point cloud are referenced to the lidar coordinate system at the end of the frame, which is equivalent to the point cloud obtained by the robot's stationary scanning at that moment. The end of the frame is used as the timestamp of the distortion-free point cloud.

[0047] In step S400, the distorted point cloud is registered with the global sub-map of the 3D global point cloud map to obtain global observations, and registered with the online constructed local keyframe map to obtain odometry observations.

[0048] According to the example embodiment, the global subgraph is a local map fragment centered on the robot's current position, dynamically loaded from the 3D global point cloud map, used to provide absolute coordinate reference. The local keyframe map is a local map constructed and maintained online by the system, stitched together from multiple recent keyframe point clouds, independent of the global prior map, used to provide continuous relative motion observation. The global subgraph is dynamically loaded from the pre-segmented and stored 3D global point cloud map by the map server of the 3D global point cloud map according to the robot's current position, and serves as the reference point cloud for the current NDT registration after loading.

[0049] According to the example embodiment, the fusion localization device uses the Normal Distributions Transform (NDT) algorithm to register the distorted point cloud with a global sub-map of the 3D global point cloud map, and to register the distorted point cloud with an online-constructed local keyframe map. The fusion localization device divides the reference point cloud into multiple grid cells using the NDT algorithm, calculates the normal distribution parameters (mean and covariance matrix) of the point cloud within each grid cell, and then transforms the point cloud to be registered into the reference point cloud coordinate system. The optimal transformation is solved by maximizing the probability density of the point cloud to be registered within each grid cell.

[0050] For example, the process of acquiring global observations is as follows: The fusion localization device inputs the distorted point cloud and the currently loaded global sub-map into the NDT algorithm. The NDT algorithm uses the pose estimate output by the error state filter at the previous moment as the initial registration value. Through iterative optimization, it solves for the optimal pose transformation of the point cloud to be registered (distorted point cloud) in the global map coordinate system of the 3D global point cloud map, and outputs the robot's six-degree-of-freedom pose in the global map coordinate system as the global observation. This global observation is then sent to the ESKF update phase.

[0051] For example, the process of acquiring odometry observations is as follows: The incremental odometry module in the fusion positioning device uses the pose estimate output by the ESKF at the current time as the initial value for registration. The distortion-free point cloud is registered with the local keyframe map at the current time using the NDT algorithm to obtain the first pose T of the current frame in the local keyframe map coordinate system. t Simultaneously, the second pose T obtained by registering the distortion-free point cloud from the previous frame with the local keyframe map is read. t-1 Then, according to formula (1): The inter-frame relative pose transformation ΔT is calculated, and this relative pose transformation is the odometry observation. This odometry observation is then fed into the ESKF update phase to maintain the continuity of positioning in areas without global map coverage.

[0052] In step S500, the error state filter is driven to predict the state based on the initialization parameters and inertial measurement data, and the state of the error state filter is updated based on the global observations and odometry observations to obtain the robot's pose estimate.

[0053] According to the example embodiment, ESKF uses the robot's motion state as the estimation object. The motion state is mathematically represented as system state variables (i.e., system state), including the robot's position, attitude, velocity, inertial measurement unit (IMU) zero bias, and the direction of gravitational acceleration in the global coordinate system. State prediction is the first step in ESKF's operation, using the integration of instantaneous IMU measurement data (acceleration and angular velocity) to calculate the evolution of the system state variables over time. Specifically, ESKF uses the integration process of IMU measurements as a motion model, combining the current state estimate and the IMU input to calculate the prior state estimate for the next moment. Since the inertial measurement data frequency is 200Hz, the state prediction step also runs at a frequency of 200Hz.

[0054] State update is the second step in ESKF's operation, which uses global and odometry observations to correct the state errors generated in the prediction step. Specifically, when global and odometry observations are received, ESKF compares the observations with the predicted values, calculates the optimal estimate of the state error, corrects the predicted state, and outputs the corrected posterior state estimate.

[0055] For example, ESKF uses the robot's six-DOF pose (position and attitude), velocity, IMU bias, and gravitational acceleration on the SE(3) (Special Euclidean Group in 3 dimensions) manifold as system state variables, and the integration process of the instantaneous IMU measurements as the motion model. The registration results of the distorted point cloud and the global subgraph (i.e., global observations) and the relative pose results calculated by the incremental odometry module (i.e., odometry observations) are introduced into the filtering and updating stage of ESKF as observations. Through the above-mentioned "prediction-update" iterative process, ESKF fuses three types of heterogeneous information sources and outputs the robot's six-DOF pose estimate.

[0056] In step S600, the inertial measurement data is integrated to output the pose increment information of the pose estimation, and the pose estimation and pose increment information are fused to output the positioning result at a frequency higher than the lidar frame rate.

[0057] According to the example embodiment, pose increment information refers to the relative pose change of the robot (including incremental attitude, incremental velocity, and incremental position) obtained by integrating the IMU measurements between two adjacent frames of LiDAR point clouds. This pose increment information is used to fill the positioning gap between two LiDAR frames.

[0058] For example, the trajectory extrapolation module in the fusion positioning device operates at the inertial measurement data frequency (200Hz) and uses the Euler method to discretely integrate the IMU measurements. The Euler method assumes that the robot's angular velocity and acceleration remain constant within a small time interval Δt (e.g., 5ms). When the ESKF finishes an observation of the laser point cloud, it updates the trajectory extrapolation module with the currently estimated IMU bias and pose estimates and resets the incremental state of the trajectory extrapolation module. The integration results of the trajectory extrapolation module are superimposed with the previous global registration observation results of the ESKF, achieving a high-frequency, high-smoothness positioning output of 200Hz, thereby overcoming the limitation of the lidar frame rate (10Hz) on the positioning output frequency.

[0059] This application integrates ESKF with IMU, LiDAR, and global positioning. Figure 3This application employs a heterogeneous information source, using global sub-map registration results as global observations for periodic correction, effectively eliminating cumulative drift and ensuring globally consistent localization in large-space scenarios. Simultaneously, by constructing local keyframe maps online, the incremental odometry module calculates inter-frame relative pose transformations without relying on prior maps, enabling continuous localization even in unmapped areas, achieving robust and seamless localization in mixed indoor and outdoor scenarios. The trajectory estimation module integrates inertial measurement data at a frequency of 200Hz and superimposes ESKF pose estimation, increasing the localization output frequency to 20 times the lidar frame rate, significantly improving the smoothness and continuity of the localization trajectory. Point-by-point motion distortion compensation for each lidar point eliminates point cloud "tailing" or "deformation," providing high-quality observation data for subsequent NDT registration. This application achieves high global consistency, high robustness, high frequency, and high accuracy in robot localization in complex, large-space scenarios.

[0060] Figure 3 A flowchart illustrating step S300 of the fusion localization method according to an embodiment of this application is shown. According to an example embodiment, such as... Figure 3 As shown, step S300 includes steps S310-S340.

[0061] In step S310, inertial measurement data within the acquisition period corresponding to each frame of point cloud data is acquired.

[0062] According to the example embodiment, the LiDAR data frequency is 10Hz, meaning the acquisition time for each frame of point cloud data is 100ms. The inertial measurement data frequency is 200Hz, meaning one inertial measurement data point is generated every 5ms. Therefore, within the acquisition time of one frame of LiDAR point cloud data, the fusion positioning device will simultaneously acquire 20 inertial measurement data points. Exemplarily, the fusion positioning device synchronizes the LiDAR and inertial measurement units in time, that is, it receives one frame of LiDAR point cloud data and simultaneously acquires all the inertial measurement data points within the acquisition time of that frame.

[0063] In step S320, the inertial measurement data during the acquisition period is integrated according to the initialization parameters to obtain the robot's pose information during the acquisition period.

[0064] According to the example embodiment, the pose information includes the robot's position (three-dimensional coordinates) and attitude (rotation matrix or quaternion), which are used to describe the robot's motion trajectory over time.

[0065] For example, let t be the start time of the point cloud data of the k-th frame of the LiDAR. k The end time is t k+1 For t k to t k+1All inertial measurement data within the time period are integrated using the following discrete-time integration expression (Euler method): The attitude update expression is formula (2): R( ; The position update expression is formula (3): ; The speed update expression is formula (4): ; Where t is t k With t k+1 At any integration time between; w m and a m These are the current angular velocity and acceleration measurements of the IMU; b g and b a These are the zero biases of the IMU's gyroscope and accelerometer (estimated by the initialization module); g is the direction vector of gravitational acceleration; R(t), p(t), and v(t) are the attitude, position, and velocity at time t, respectively. The integration step size (e.g., 5ms); exp() ∧ This is an exponential mapping with the natural number e as the base, used to convert the angular velocity vector into a rotation matrix. Through the above recursive formulas (2), (3) and (4), the robot's pose information at each moment during the point cloud data acquisition period of this frame can be obtained.

[0066] In step S330, the pose information is interpolated based on the time offset between the acquisition time of the laser points in each frame of point cloud data and the end time of the acquisition period to obtain the distortion removal matrix of the laser points.

[0067] According to the example embodiment, the distortion removal matrix is ​​a 4×4 homogeneous transformation matrix containing rotation and translation components, used to transform the laser point from the coordinate system at the acquisition time to the unified coordinate system at the end of the frame. Interpolation includes linear interpolation of the translation and spherical linear interpolation of the rotation. Linear interpolation refers to linearly weighting the translation in the pose information according to the time ratio, suitable for the assumption that the translation motion is approximately uniform over a short time. Spherical linear interpolation (SLERP) refers to interpolating the rotation in the pose information in a unit quaternion space, performing rotational interpolation along the shortest arc, ensuring that the interpolation result is always a valid rotation matrix.

[0068] For example, for two inertial measurement timestamps t i and t i+1 laser point p between j(The original coordinates of the laser point at the acquisition time) are found by interpolating based on the two nearest inertial measurement values ​​and the inertial measurement timestamps. Let the acquisition time of the laser point be t. j The frame end time of this laser point is t. k+1 The two IMU measurement times before and after are t i and t i+1 And obtain the linear difference from t based on the inertial measurement timestamp. j Time to t k+1 Pose transformation matrix at time step The pose transformation matrix is ​​the distortion removal matrix for the laser point, as shown in formula (5): Formula (5) in, For t j Time to t k+1 The pose transformation matrix at time t; For t k+1 Time to t i+1 The pose transformation matrix at time t; For t i Time to t k+1 The pose transformation matrix at time step; the interpolation operation includes linear interpolation of the translation component and spherical linear interpolation of the rotation component.

[0069] In step S340, the coordinates of the laser points are projected onto the coordinate system at the end of the acquisition period according to the distortion removal matrix to generate a distortion-removed point cloud.

[0070] According to the example embodiment, projection refers to transforming the three-dimensional coordinates of the laser point from the lidar coordinate system at the acquisition time to the lidar coordinate system at the end of the frame through homogeneous coordinate transformation.

[0071] For example, let the laser point p j The original coordinates at the acquisition time are p j The distortion removal matrix is After obtaining the pose transformation matrix, the motion distortion removed and projected onto t can be obtained according to equation (6). k+1 The laser point at time t is given by formula (6) as follows: Formula (6) Where, p k+1，j For projection onto t k+1 A single laser point p at time t j The coordinate transformations described above are applied to all laser points within a frame to generate a complete distortion-free point cloud. All points in this distortion-free point cloud are located at the end of the frame at time t.k+1 Under a unified coordinate system, point cloud distortion caused by robot motion is eliminated.

[0072] Figure 4 A schematic diagram illustrating the motion distortion removal principle of an embodiment of this application is shown. According to an example embodiment, such as… Figure 4 As shown, the acquisition time period for one frame of raw lidar point cloud data is t. k To t k+1 During this period, the robot moves continuously, resulting in laser points (as shown by solid circles) collected at different times being distributed in different coordinate systems. The fusion positioning device acquires all inertial measurement data within this period, integrates the inertial measurement data according to the initialization parameters, and obtains the pose information at each moment within this period; for each laser point, based on its acquisition time t... j Relative to the initial time t k The offset time is calculated by using linear interpolation of the translation and spherical linear interpolation of the rotation to determine the laser point's position relative to the end of the frame, t. k+1 The distortion removal matrix is ​​then used to map the original coordinates of the laser point to the frame end time t using the distortion removal matrix. k+1 Under a unified coordinate system. As shown in the figure, the point cloud before distortion compensation (solid circles on the left) produces a "tailing" or "deformation" phenomenon due to the robot's movement. After point-by-point projection of the distortion removal matrix, all laser points are transformed to the unified coordinate system at the end of the frame, forming a distortion-free point cloud (solid circles on the right), which is equivalent to the point cloud obtained by the robot's stationary scanning at that moment.

[0073] Through the above embodiments, this application uses the instantaneous measurement values ​​of the IMU to perform point-by-point motion distortion compensation on the laser point cloud, projecting all points in a frame of point cloud onto the coordinate system at the same moment, thereby eliminating the point cloud distortion caused by robot motion.

[0074] According to the example embodiment, step S400 includes step S410. In step S410, the distorted point cloud and the global sub-map are registered using a normal distribution transformation algorithm to obtain the robot's six-degree-of-freedom pose in the global map coordinate system of the three-dimensional global point cloud map, which is used as the global observation value.

[0075] According to the example embodiment, six degrees of freedom pose refers to the robot's position (X, Y, Z coordinates) and attitude (roll, pitch, yaw) in three-dimensional space, totaling six degrees of freedom, used to fully describe the robot's pose state.

[0076] All map tiles and global subgraphs in the global map coordinate system have definite absolute coordinates. The global map coordinate system provides an absolute reference datum for robot localization.

[0077] For example, the fusion localization device registers the distorted point cloud with the global sub-map using the NDT algorithm, iteratively solving for the optimal pose transformation to maximize the probability of matching the overlapping portion of the distorted point cloud and the global sub-map. The six-DOF pose transformation (relative to the global map coordinate system) output by registration is modeled as a global registration observation and used as a nonlinear measurement input to the update stage of ESKF to correct the cumulative drift caused by long-term system operation and ensure global consistency of localization.

[0078] Through the above embodiments, this application obtains absolute pose observation in the global map coordinate system by performing NDT registration between the real-time distorted point cloud and the dynamically loaded global sub-graph, thereby effectively suppressing robot localization drift in large space scenes and ensuring high global consistency of localization results.

[0079] Figure 5 A flowchart illustrating step S400 of the fusion localization method according to an embodiment of this application is shown. According to an example embodiment, such as… Figure 5 As shown, the process of constructing the local keyframe map in step S400 includes steps S420-S460.

[0080] In step S420, the first frame in the distorted point cloud is stored in the keyframe queue as the initial keyframe, and the pose of the initial keyframe is used as the comparison reference.

[0081] According to the example embodiment, a keyframe refers to a representative frame selected from a continuous point cloud sequence, used to construct a local point cloud map. The selection of keyframes is based on a pose change threshold criterion, that is, calculating the translation distance and rotation angle between the pose of the current frame and the pose of the previous keyframe. When the translation distance exceeds a preset translation distance threshold (e.g., 1 meter) or the rotation angle exceeds a preset rotation angle threshold (e.g., 10°), the current frame is determined to be a new keyframe.

[0082] A keyframe queue is a first-in-first-out (FIFO) data structure used to store the most recently selected keyframes and their corresponding poses. The capacity of the keyframe queue is capped by a preset length threshold (e.g., 10 frames). When the queue is full, the oldest keyframe stored is removed.

[0083] The reference frame is the current pose used to determine whether a subsequent frame is a keyframe. Initially, it is the pose of the initial keyframe. Whenever a new keyframe is added to the queue, the reference frame is updated to the pose of the new keyframe.

[0084] For example, when the incremental odometry module receives the first frame of laser point cloud after motion distortion compensation, it uses the robot's initial pose in the global map coordinate system to transform the point cloud to the global map coordinate system and stores it as the initial key frame in the key frame queue. At the same time, it records the corresponding pose as the first registration result and the initial comparison benchmark, thus forming the starting point of the local key frame map.

[0085] In step S430, subsequent frames in the distorted point cloud are acquired sequentially, and the translation distance and / or rotation angle between the pose of the subsequent frames and the comparison reference are calculated.

[0086] According to the example embodiment, the translation distance refers to the three-dimensional Euclidean distance between the pose of the current frame and the comparison reference pose, i.e., d The rotation angle refers to the absolute value of the difference between the yaw angle and the comparison reference in the current frame. .

[0087] Where d is the translation distance; T t The pose of the current frame (i.e., the current registration frame); T kf For comparison reference pose; This is the rotation angle (i.e., the difference in yaw angle); The yaw angle for the current frame; The yaw angle serves as the benchmark for comparison.

[0088] For example, during subsequent operation, whenever the fusion positioning device acquires a new frame of distortion-free point cloud, it first uses the pose estimate output by the ESKF at the current moment as the initial value, and then uses the NDT algorithm to register the point cloud with the currently maintained local keyframe map to obtain the optimal pose estimate of the current frame in the local map coordinate system. Then, it calculates the translation distance and rotation angle between the current frame pose and the comparison reference.

[0089] In step S440, when the translation distance exceeds a preset translation distance threshold or the rotation angle exceeds a preset rotation angle threshold, the corresponding subsequent frame is stored as a new key frame in the key frame queue, and the comparison reference is updated with the pose of the new key frame.

[0090] According to the example embodiment, the preset translation distance threshold is a position change threshold used to determine whether to select a new keyframe, and is exemplarily set to 1 meter. The preset rotation angle threshold is an attitude change threshold used to determine whether to select a new keyframe, and is exemplarily set to 10°. When the translation distance exceeds 1 meter or the rotation angle exceeds 10°, the current frame is determined to be a new keyframe.

[0091] In step S450, when the length of the keyframe queue exceeds a preset length threshold, the keyframe with the earliest entry time in the keyframe queue is removed.

[0092] According to the example embodiment, the preset length threshold is the upper limit of the capacity of the keyframe queue, which is exemplarily set to 10 frames. When the length of the keyframe queue exceeds 10 frames, the fusion positioning device removes the keyframe with the earliest entry time from the queue, so that the queue length is restored to within the preset threshold range.

[0093] For example, in order to maintain the timeliness and controllable scale of the local map, when the keyframe queue length exceeds 10 frames, the fusion positioning device adopts a first-in-first-out (FIFO) strategy to automatically remove the earliest inserted keyframe, ensuring that the most recently acquired keyframe is always retained in the queue.

[0094] In step S460, the point cloud data of all key frames in the key frame queue are stitched together to generate a local key frame map.

[0095] According to the example embodiment, stitching refers to transforming each keyframe point cloud retained in the keyframe queue to the same coordinate system according to the pose corresponding to each keyframe, and then merging them into a complete point cloud map.

[0096] For example, the fusion positioning device performs unified coordinate transformation and stitching on the keyframe point clouds retained in the keyframe queue according to their corresponding poses to construct a local keyframe map at the current moment, which is used as a reference map for the next NDT registration.

[0097] Through the above embodiments, this application stores the first frame in the distortion-reduced point cloud as the initial keyframe in the keyframe queue, and sequentially filters keyframes in subsequent frames using translation distance thresholds and rotation angle thresholds as judgment criteria. Only frames that meet the condition of "translation distance exceeding 1 meter or rotation angle exceeding 10°" are stored in the queue, effectively reducing data redundancy while ensuring the local map coverage density. At the same time, the automatic elimination of keyframes is achieved through the first-in-first-out queue and a preset length threshold (such as 10 frames), ensuring that the local map always reflects the environment recently traversed by the robot, combining timeliness and scale controllability. Finally, the keyframe point clouds retained in the queue are stitched together according to their respective poses to the same coordinate system to generate a local keyframe map that can be used for subsequent NDT registration. This provides the incremental odometry module with a local reference benchmark that does not rely on a global prior map, enhancing the system's positioning robustness and continuity in areas without global map coverage.

[0098] Figure 6 This illustration shows another flowchart of step S400 of the fusion positioning method according to an embodiment of this application. According to an example embodiment, such as... Figure 6 As shown, step S400 also includes steps S470-S490.

[0099] In step S470, the distorted point cloud is registered with the local keyframe map using a normal distribution transformation algorithm to obtain the first pose of the current frame of the distorted point cloud in the coordinate system of the local keyframe map.

[0100] According to the example embodiment, the first pose refers to the pose estimate obtained after registering the distortion-free point cloud of the current frame with the local keyframe map, denoted as T. t This indicates the position and orientation of the current frame in the local keyframe map coordinate system.

[0101] For example, the incremental odometry module uses the pose estimate output by the ESKF at the current time as the initial value for NDT registration, and registers the current distorted point cloud with the currently maintained local keyframe map to obtain the optimal pose estimate (i.e., the first pose) T of the current frame in the local map coordinate system. t .

[0102] In step S480, the second pose is obtained by registering the previous frame's distorted point cloud with the local keyframe map.

[0103] According to the example embodiment, the second pose refers to the pose estimate obtained after registering the distortion-free point cloud of the previous frame with the local keyframe map, denoted as T. t-1 This represents the position and pose of the previous frame in the local keyframe map coordinate system. For example, the fusion positioning device stores the pose T obtained by registering the laser point cloud of the previous frame with the local keyframe map. t-1 This is used when calculating the relative motion between frames in the current frame.

[0104] In step S490, the inter-frame relative pose transformation is calculated based on the first pose and the second pose, and used as the odometry observation value.

[0105] According to the example embodiment, inter-frame relative pose transformation refers to the change in the robot's relative position and attitude from the previous frame to the current frame, denoted as ΔT as an odometry observation value, and is expressed as ΔT=T t-1 -T t The calculation yields the odometer observation, which is then input into the ESKF as a relative motion result for state updates.

[0106] Figure 7 A schematic diagram illustrating the workflow of an incremental odometer module according to an embodiment of this application is shown. Based on an example embodiment, such as... Figure 7 As shown, the incremental odometry module uses the pose estimate output by the ESKF at the current time as the initial value for NDT registration, registers the distorted point cloud with the local keyframe map to obtain the pose of the current frame, and calculates the incremental pose as the odometry observation value input to the ESKF in combination with the previous registration result; at the same time, it determines whether the current frame should be stored as a keyframe in the keyframe queue according to the pose change threshold in order to update the local keyframe map.

[0107] Through the above embodiments, this application provides continuous and reliable odometry observations for ESKF by calculating the relative pose transformation between frames, thereby enhancing the system's positioning robustness in areas without global map coverage.

[0108] Figure 8 A flowchart illustrating step S500 of the fusion positioning method according to an embodiment of this application is shown. According to an example embodiment, such as… Figure 8 As shown, step S500 also includes steps S510-S530.

[0109] In step S510, the inertial measurement data is corrected according to the zero bias in the initialization parameters to obtain the corrected inertial measurement data.

[0110] Specifically, the true angular velocity of the gyroscope is w = w m -b g The actual acceleration of the accelerometer is a = a m -b a For example, the state variables of ESKF include the IMU zero bias b. g and b a The fusion positioning device uses the initial zero bias as the initial value of the state variable and continuously estimates and updates the zero bias state in each filtering cycle. In the state prediction stage, the inertial measurement data is corrected using the currently estimated zero bias.

[0111] In step S520, a state prediction model for the error state filter is constructed based on the direction of gravitational acceleration in the initialization parameters and the corrected inertial measurement data.

[0112] According to the example embodiment, the state prediction model is a mathematical model describing the evolution of the system state over time, which in this application is composed of IMU kinematic equations. This state prediction model uses modified inertial measurement data (angular velocity and acceleration) to integrate and predict the system state at the next moment.

[0113] For example, the state prediction model is constructed based on the following IMU kinematic equations. Within a preset time interval (e.g., 5ms), assuming that the angular velocity and acceleration remain constant, the robot's attitude, position, and velocity are recursively derived according to equations (2) to (4).

[0114] In step S530, the state variables of the error state filter are iteratively updated according to the state prediction model to drive the error state filter to perform state prediction.

[0115] For example, ESKF performs the state prediction step at a frequency of 200Hz, and updates the system state variables iteratively each time new inertial measurement data is received, based on the state prediction model.

[0116] Through the above embodiments, this application achieves high-precision high-frequency state prediction by introducing IMU zero-bias correction and gravitational acceleration direction into the ESKF state prediction model.

[0117] Figure 9 This illustration shows another flowchart of step S500 of the fusion positioning method according to an embodiment of this application. According to the example embodiment, as... Figure 9 As shown, step S500 also includes steps S540-S570.

[0118] In step S540, the first observation equation of the error state filter is constructed based on the global observations.

[0119] According to the example embodiment, the first observation equation is an equation describing the mathematical relationship between the global observations (i.e., the global pose obtained from NDT registration) and the ESKF system state. This first observation equation treats the global registration observations as a nonlinear measurement function to correct the accumulated error of the system state during the filtering update phase of the ESKF.

[0120] In step S550, the second observation equation of the error state filter is constructed based on the odometer observations.

[0121] According to the example embodiment, the second observation equation is an equation describing the mathematical relationship between odometry observations (i.e., inter-frame relative pose transformations) and the ESKF system state. This second observation equation uses odometry observations as relative motion constraints to maintain state continuity during the ESKF's filtering update phase.

[0122] In step S560, the update amount of the state variables of the error state filter is calculated according to the first observation equation and the second observation equation.

[0123] According to the example embodiment, the update amount of the state variable is the optimal estimate of the error state calculated in the filtering update stage of ESKF, which is used to correct the state error generated in the filtering update stage. During the filtering update, ESKF first calculates the observed predicted value based on the current state variable, and then compares the global observation value or odometry observation value with the predicted value. The difference obtained from the comparison is the observation residual, which reflects the deviation of the state estimate. Next, ESKF calculates the Kalman gain matrix based on the observation noise covariance and the state covariance, which is used to balance the reliability of the predicted value and the reliability of the observed value. Finally, the observation residual is multiplied by the Kalman gain matrix to obtain the optimal estimate of the state error (i.e., the update amount of the state variable), thus completing the correction of the state variable.

[0124] In step S570, the state variables of the error state filter are corrected according to the update amount, and the corrected state variables are converted into the robot's pose estimate.

[0125] For example, ESKF divides the system state into two parts: a nominal state and an error state. The nominal state is the main state directly driven by IMU integration (i.e., the large-signal part of the state variables), while the error state is the deviation between the nominal state and the true state (i.e., the small-signal part). ESKF applies the state update (i.e., the optimal estimate of the error state) calculated in step S560 to the nominal state, completing the correction of the nominal state and obtaining the robot's pose estimate. The corrected state variables include the robot's six-DOF pose, velocity, IMU zero bias, etc. Then, the corrected state variables are converted into the robot's six-DOF pose estimate, which is output as the localization result.

[0126] Through the above embodiments, this application achieves optimal fusion of multi-sensor information by fusing global observations and odometer observations through the ESKF update steps, thus ensuring high positioning accuracy and robustness.

[0127] Figure 10 A flowchart illustrating step S600 of the fusion localization method according to an embodiment of this application is shown. According to an example embodiment, such as... Figure 10 As shown, step S600 also includes steps S610-S620.

[0128] In step S610, the pose estimation of the previous output of the error state filter is used as the pose estimation reference. The inertial measurement data between two adjacent frames of distorted point clouds are integrated to obtain the pose increment information between two adjacent frames of distorted point clouds.

[0129] According to the example embodiment, the pose estimation reference refers to the optimal pose estimate output by the ESKF at the time of the previous lidar frame, which serves as the starting point for the high-frequency integration of the trajectory estimation module. The trajectory estimation module uses this reference as its initial value and calculates the current position by integrating subsequent inertial measurement data.

[0130] For example, the trajectory estimation module is an IMU integration thread that runs at the frequency (200Hz) of the inertial measurement data. When the ESKF finishes an observation of the laser point cloud, it updates the zero bias of the system state variables to the trajectory estimation module and resets the incremental attitude, velocity, and position of the trajectory estimation module to zero. Thereafter, the trajectory estimation module integrates the IMU measurements between two adjacent frames of distorted point clouds, using the pose estimate output by the ESKF as a reference, to calculate the cumulative amount of IMU measurements, i.e., the pose increment information.

[0131] In step S620, the pose estimation reference is superimposed with the pose increment information, and the positioning result is output at a frequency higher than the lidar frame rate.

[0132] For example, the trajectory estimation module integrates the results (pose increment information) of each iteration and superimposes the pose estimation reference from the previous ESKF output to obtain the high-frequency pose estimation for the current moment, which is then published at a frequency of 200Hz. This process fully utilizes the IMU's raw measurement frequency of up to 200Hz to perform high-frequency interpolation and publication of the system state, thereby overcoming the limitation of the lidar frame rate (10Hz) on the positioning output frequency and achieving high-frequency, high-smoothness positioning output.

[0133] Through the above embodiments, this application achieves a high-frequency positioning output of 200Hz through the trajectory estimation module, which meets the requirements of high dynamic systems for continuous and smooth state estimation.

[0134] Figure 11 This illustration shows another flowchart of step S400 of the fusion positioning method according to an embodiment of this application. According to an example embodiment, such as... Figure 11 As shown, the process of constructing the global subgraph in step S400 includes steps S401-S403.

[0135] In step S401, the three-dimensional global point cloud map is divided into multiple map blocks according to a preset size, and a spatial index identifier is assigned to each map block.

[0136] Figure 12 A flowchart illustrating a spatial index-based map tile strategy according to an embodiment of this application is shown. According to an example embodiment, such as... Figure 12 As shown, the fusion positioning device first creates a two-dimensional grid-mapped map data structure, with spatial index identifiers as keys and point cloud data of map tiles as values. Then, it iterates through each 3D point in each keyframe, calculating the spatial index identifier of its corresponding map tile based on the point's coordinates. If the map tile corresponding to the identifier already exists, the point is added to that tile; otherwise, a new map tile is created. After the iteration is complete, each map tile undergoes voxel filtering at a preset resolution to reduce point cloud density. The processed map tiles are then stored in a designated directory with the spatial index identifier as the filename. Simultaneously, all spatial index identifiers are stored in a text file within that directory, completing the map tile segmentation.

[0137] In one specific embodiment, a map tile refers to a square sub-map formed by dividing a large-scale global 3D point cloud map according to rules. Each map tile occupies a unique geographical area on the horizontal plane and retains a complete point cloud structure along the vertical direction. The preset size is exemplarily set to 50 meters × 50 meters.

[0138] The spatial index identifier is a unique identifier for each map tile, determined by its position within the spatial grid. Specifically, for any point coordinate (x, y) within the coverage area of ​​a map tile, its spatial index identifier (m) is the spatial index identifier of the map tile it belongs to. x m y ). (During the day, through m) x Calculated using formula (7), m y Calculated using formula (8): Formula (7) Formula (8) Where, m x m is the index of the map tile in the x-direction. y `pt.x` is the index of the map tile in the y-direction; `floor()` is the function to round down to the nearest integer (also known as the floor function); `pt.x` and `pt.y` are the coordinates of the radar point in the keyframe; `block_size` is the side length of the map tile (e.g., 50m).

[0139] For example, the fusion localization device first performs global 3D reconstruction based on SLAM and generates point cloud map keyframes. Then, it transforms the global 3D point cloud map into segmented global sub-map tiles using a spatial index-based map tiling strategy. This map is organized using the following data structure: std:map <Eigen::Vector2d, pcl::PointCloud <pcl::pointxyzi>The data structure `::Ptr, less_vec>` uses a two-dimensional spatial location index identifier as the key and a point cloud pointer to a map tile as the value, with the two-dimensional vector comparator `less_vec` as the key comparator. During map tiling, for each keyframe, the keyframe position obtained during the mapping phase is projected onto the map coordinate system. Each point in each projected keyframe calculates its corresponding spatial index identifier for the map tile according to the formulas (7) and (8) above. If the calculated map tile already exists, the point is added to the map tile; otherwise, a new map tile is constructed using the spatial index identifier. Subsequently, a filter is created to perform voxel filtering on each map tile at a preset resolution (e.g., 0.1-meter resolution) to reduce the point cloud density of each map tile. The voxel-filtered point clouds of the map tiles are stored in a preset directory with the spatial index identifier as the filename, and all spatial index identifiers are stored in a text file in that preset directory, thus completing the map tiling step.

[0140] In step S402, the corresponding current map tile is determined based on the robot's current position.

[0141] According to the example embodiment, the current map tile refers to the map tile where the robot's current position is located. This is determined by calculating the spatial index identifier (m) to which the robot's current position (x, y) belongs. x m y To determine.

[0142] In step S403, with the current map tile as the center, the current map tile and map tiles within a preset range around the current map tile are loaded and stitched together to form a global sub-map.

[0143] Figure 13 A schematic window diagram of a global subgraph according to an embodiment of this application is shown. According to an example embodiment, such as... Figure 13 As shown, the global submap consists of nine map tiles in eight adjacent directions (front, right-front, right, right-back, back, left-back, left, left-front) centered on the current map tile. The global submap is a local point cloud map composed of these nine map tiles, centered on the center point of the map tile where the robot is located. In a specific embodiment, the side length of the global submap is 150 meters.

[0144] According to the example embodiment, the steps for loading and switching the global subgraph are as follows: Step 1: Determine the corresponding current map tile based on the robot's current position, and determine the map tiles in eight adjacent directions around the current map tile as the center, which together form the following map window.

[0145] Specifically, the following map window is a square window centered on the map tile at the robot's current position, covering a preset range of nine map tiles in eight adjacent directions (front, right front, right, right back, back, left back, left, left front) in the horizontal direction.

[0146] Step 2: Monitor the robot's real-time pose at a preset period, calculate the corresponding spatial index identifier based on the robot's current position, and make a map update judgment based on the spatial index identifier.

[0147] Specifically, the fusion positioning device continuously monitors the robot's real-time pose at preset intervals (e.g., 1 second), and calculates the corresponding spatial index identifier (m) based on the robot's current position during each monitoring process. x m y ).

[0148] Step 3: Perform map loading judgment: Taking the map tile where the robot is currently located as the center, generate spatial index identifiers for nine target map tiles, including the current map tile and its eight surrounding adjacent map tiles; traverse these nine target map tiles, mark the map tiles that do not yet exist in the global subgraph pointed to by the main pointer but exist in the constructed global map tile index list as to be loaded, and store their spatial index identifiers in the spatial index identifier queue of the preload pointer.

[0149] Figure 14 A schematic diagram of a dual-pointer map cross-loading mechanism according to an embodiment of this application is shown. According to an example embodiment, such as… Figure 14 As shown, the fusion positioning device maintains two independent global subgraph pointers: a primary pointer and a preload pointer.

[0150] The primary pointer points to the currently used global subgraph (i.e., the reference point cloud currently used for NDT registration). The localization thread reads the global subgraph data through the primary pointer and performs point cloud registration. The preload pointer points to a new global subgraph that is loaded asynchronously and used for the next stage of localization.

[0151] In the map loading judgment section, the fusion localization device first generates a spatial index identifier (c) corresponding to the robot's current pose. x c y (here c) x That is, m x； c y That is, m y Spatial index identifiers for nine sub-tiles centered on (c) x c y ), (c x c y+1 ), (c x+1 c y ) 、 (c x+1 c y+1 ), (c x c y-1 ), (c x-1 c y ), (c x-1 c y+1 ), (c x+1 c y-1 ) and (c x-1 c y-1 Then, the nine spatial index identifiers are traversed. For each map tile, it is first determined whether the map tile is already in the global subgraph pointed to by the main pointer, that is, whether the spatial index identifier of the map tile is in the ordered queue pointed to by the main pointer. If it exists, it remains unchanged; if it does not exist, it is further determined whether the map tile is contained in the pre-built global map, that is, whether the spatial index identifier of the map tile is in the list of spatial index identifiers built in the map tile operation. If it exists, the map loading flag is set to True, and the spatial index identifier is stored in the spatial index identifier queue of the pre-load pointer.

[0152] Step 4: Perform map unloading judgment: The fusion positioning device copies the elements in the spatial index identifier queue of the global submap pointed to by the master pointer to the spatial index identifier queue of the preload pointer, traverses the spatial index identifier queue, calculates the two-dimensional Euclidean distance between the spatial index identifier of each map block and the spatial index identifier of the block where the robot is currently located, and removes map blocks whose distance is greater than the preset unloading threshold from the preload queue. For example, the preset unloading threshold is 3 map block units.

[0153] Specifically, to avoid repeated map switching, the fusion positioning device implements a delayed unloading strategy for existing map tiles. First, each element in the spatial index identifier queue of the global submap currently pointed to by the primary pointer is inserted into the spatial index identifier queue of the preload pointer (this queue is ordered). At this point, the preload queue contains both map tile information from the current primary map and information about newly loaded map tiles. Then, the queue is traversed, and the spatial index identifier of each map tile is calculated to match the spatial index identifier of the tile the robot is currently in (c...). x c y The two-dimensional Euclidean distance between the two map tiles is used to remove map tiles from the list if the distance is greater than a preset unloading threshold (e.g., 3 map tile units).

[0154] Step 5: When the robot pose crosses the map tile boundary and triggers the map loading flag, the corresponding map tile is read according to the list of spatial index identifiers in the preloaded pointer, a new global subgraph is constructed by splicing the tiles, and then a pointer swap operation is performed to promote the preloaded pointer to the primary pointer and reclaim the old global subgraph pointed to by the original primary pointer.

[0155] According to the example embodiment, when the robot's pose crosses the boundary of a map tile, triggering a map loading flag, the fusion localization device reads the corresponding map tile point cloud data from the list of spatial index identifiers pointed to by the preloaded pointer. It then stitches the data together based on the pose corresponding to the map tile to construct a new global sub-map and points the preloaded pointer to this new global sub-map. After completing the map tile loading and unloading operations, the fusion localization device performs a pointer swap operation, promoting the preloaded pointer to the primary pointer, and reclaiming the global sub-map point cloud data previously pointed to by the primary pointer. This dual-pointer mechanism avoids data contention between the old and new global sub-maps during map switching.

[0156] Through the above embodiments, this application achieves on-demand loading and dynamic switching of map tiles in large scenes by adopting a spatial index-based segmented 3D map management strategy and a dual-pointer map cross-loading mechanism, thereby reducing the latency of the positioning thread and improving high throughput.

[0157] Figure 15 A schematic diagram of the fusion positioning device according to an embodiment of this application is shown. According to an example embodiment, such as… Figure 15 As shown, the fusion positioning device 1 includes a data acquisition module 11, a motion distortion removal module 12, a registration module 13, a state estimation module 14, a trajectory calculation module 15, and an output module 16.

[0158] The data acquisition module 11 is used to acquire inertial measurement data, point cloud data, and a 3D global point cloud map; the initialization module is used to determine initialization parameters based on the inertial measurement data in response to the robot coming to a standstill. The execution content of the data acquisition module 11 has been described in detail in steps S100 and S200, and will not be repeated here.

[0159] The motion distortion removal module 12 is used to remove motion distortion from the point cloud data according to the initialization parameters, generating a distorted point cloud. The execution content of the motion distortion removal module 12 has been described in detail in step S300, and will not be repeated here.

[0160] The registration module 13 is used to register the distorted point cloud with the global sub-map of the 3D global point cloud map to obtain global observations, and to register with the online constructed local keyframe map to obtain odometry observations. The execution content of the registration module 13 has been described in detail in step S400, and will not be repeated here.

[0161] The state estimation module 14 is used to drive the error state filter to predict the state based on the initialization parameters and inertial measurement data, and to update the state of the error state filter based on the global observations and odometry observations to obtain the robot's pose estimate. The execution content of the state estimation module 14 has been described in detail in step S500, and will not be repeated here.

[0162] The trajectory estimation module 15 is used to integrate the inertial measurement data and output the pose increment information of the pose estimation. The output module 16 is used to fuse the pose estimation and pose increment information and output the positioning result at a frequency higher than the lidar frame rate. The execution content of the trajectory estimation module 15 and the output module 16 has been described in detail in step S600 and will not be repeated here.

[0163] According to another aspect of this application, an electronic device is also provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the fusion positioning methods described above.

[0164] According to another aspect of this application, a computer-readable medium having processor-executable non-volatile program code is also provided, the program code causing the processor to perform any of the fusion positioning methods described above.

[0165] According to another aspect of this application, a computer program product is also provided, including a computer program stored on a computer-readable medium, the computer program including program instructions that, when executed by a computer, cause the computer to perform any of the fusion positioning methods described above.

[0166] As is known from common technical knowledge, this application can be implemented through other embodiments that do not depart from its spirit or essential characteristics. Therefore, the disclosed embodiments described above are merely illustrative in all respects and are not the only ones. All modifications within the scope of this application or equivalent to this application are included in this application.< / pcl::pointxyzi>

Claims

1. A fusion localization method for a robot, characterized in that, The robot is equipped with an inertial measurement unit, a lidar, and an error state filter. The method includes: Acquire inertial measurement data from the inertial measurement unit, point cloud data from the lidar, and a three-dimensional global point cloud map of the target scene; In response to the robot coming to a stop, the initialization parameters of the inertial measurement unit are determined based on the inertial measurement data; Based on the initialization parameters, motion distortion removal is performed on the point cloud data to generate a distortion-free point cloud; The distortion-free point cloud is registered with the global sub-map of the 3D global point cloud map to obtain global observations, and registered with the online constructed local keyframe map to obtain odometry observations. The error state filter is driven to predict the state based on the initialization parameters and the inertial measurement data, and the state of the error state filter is updated based on the global observation and the odometry observation to obtain the pose estimate of the robot. The inertial measurement data is integrated to output the pose increment information of the pose estimation, and the pose estimation and the pose increment information are fused to output the positioning result at a frequency higher than the lidar frame rate.

2. The fusion positioning method according to claim 1, characterized in that, The step of removing motion distortion from the point cloud data according to the initialization parameters to generate a distortion-free point cloud includes: Acquire inertial measurement data for each frame of point cloud data within the corresponding acquisition period; The robot's pose information during the acquisition period is obtained by integrating the inertial measurement data during the acquisition period based on the initialization parameters. Based on the time offset between the acquisition time of the laser points in each frame of point cloud data and the end time of the acquisition period, the pose information is interpolated to obtain the distortion removal matrix of the laser points; The coordinates of the laser point are projected onto the coordinate system at the end of the acquisition period based on the distortion removal matrix to generate the distortion-removed point cloud.

3. The fusion positioning method according to claim 1, characterized in that, The step of registering the distorted point cloud with the global sub-map of the 3D global point cloud map to obtain global observations includes: The distortion-free point cloud is registered with the global sub-map using a normal distribution transformation algorithm to obtain the robot's six-degree-of-freedom pose in the global map coordinate system of the three-dimensional global point cloud map, which is then used as the global observation value.

4. The fusion positioning method according to claim 1, characterized in that, The process of constructing the local keyframe map includes: The first frame in the distortion-free point cloud is stored in the keyframe queue as the initial keyframe, and the pose of the initial keyframe is used as the comparison reference. Sequentially acquire subsequent frames in the distortion-free point cloud, and calculate the translation distance and / or rotation angle between the pose of the subsequent frame and the comparison reference. When the translation distance exceeds a preset translation distance threshold or the rotation angle exceeds a preset rotation angle threshold, the corresponding subsequent frame is stored as a new keyframe in the keyframe queue, and the comparison benchmark is updated with the pose of the new keyframe. When the length of the keyframe queue exceeds a preset length threshold, the keyframe with the earliest entry time in the keyframe queue is removed. The point cloud data of all keyframes in the keyframe queue are stitched together to generate the local keyframe map.

5. The fusion positioning method according to claim 1, characterized in that, The process of registering the odometry observations with the online-constructed local keyframe map includes: The distortion-free point cloud is registered with the local keyframe map using a normal distribution transformation algorithm to obtain the first pose of the current frame of the distortion-free point cloud in the coordinate system of the local keyframe map. Obtain the second pose by registering the previous frame's distortion-free point cloud with the local keyframe map; Based on the first pose and the second pose, the inter-frame relative pose transformation is calculated and used as the odometry observation.

6. The fusion positioning method according to claim 1, characterized in that, The initialization parameters include zero bias and the direction of gravitational acceleration. The step of driving the error state filter to perform state prediction based on the initialization parameters and the inertial measurement data includes: The inertial measurement data is corrected based on the zero bias in the initialization parameters to obtain the corrected inertial measurement data. Based on the direction of gravitational acceleration in the initialization parameters and the corrected inertial measurement data, a state prediction model for the error state filter is constructed. The state variables of the error state filter are iteratively updated according to the state prediction model to drive the error state filter to perform state prediction.

7. The fusion positioning method according to claim 1, characterized in that, The step of updating the state of the error state filter based on the global observations and the odometry observations to obtain the robot's pose estimate includes: The first observation equation of the error state filter is constructed based on the global observations; The second observation equation for the error state filter is constructed based on the odometer observations; The update amount of the state variable of the error state filter is calculated based on the first observation equation and the second observation equation. The state variables of the error state filter are corrected according to the update amount, and the corrected state variables are converted into the pose estimate of the robot.

8. The fusion positioning method according to claim 1, characterized in that, The process of integrating the inertial measurement data, outputting the pose increment information of the pose estimation, and fusing the pose estimation with the pose increment information to output the positioning result at a frequency higher than the lidar frame rate includes: Using the pose estimation of the previous output of the error state filter as the pose estimation reference, the inertial measurement data between two adjacent frames of distorted point clouds are integrated to obtain the pose increment information between the two adjacent frames of distorted point clouds. The pose estimation benchmark is superimposed with the pose increment information, and the positioning result is output at a frequency higher than the lidar frame rate.

9. The fusion positioning method according to claim 1, characterized in that, The global subgraph is constructed in the following manner: The three-dimensional global point cloud map is divided into multiple map blocks according to a preset size, and a spatial index identifier is assigned to each map block; Based on the robot's current position, determine the corresponding current map tile; Centered on the current map tile, load the current map tile and map tiles within a preset range around the current map tile, and stitch them together to form the global sub-map.

10. A fusion positioning apparatus for performing the fusion positioning method according to any one of claims 1-9, characterized in that, include: The data acquisition module is used to acquire the inertial measurement data, the point cloud data, and the three-dimensional global point cloud map; An initialization module is used to determine the initialization parameters based on the inertial measurement data in response to the robot coming to a standstill. The motion distortion removal module is used to remove motion distortion from the point cloud data according to the initialization parameters, and generate the distortion-removed point cloud; The registration module is used to register the distortion-free point cloud with a global sub-map of the 3D global point cloud map to obtain the global observation value, and to register it with the online constructed local keyframe map to obtain the odometry observation value. The state estimation module is used to drive the error state filter to perform state prediction based on the initialization parameters and the inertial measurement data, and update the state of the error state filter based on the global observation value and the odometry observation value to obtain the pose estimate of the robot. The trajectory estimation module is used to integrate the inertial measurement data and output the pose increment information of the pose estimation. The output module is used to fuse the pose estimation with the pose increment information and output the positioning result at a frequency higher than the lidar frame rate.

Images