A Multi-Sensor Fusion Localization Method and System Based on Graph Optimization SLAM

By employing an asymmetric computing architecture and a multi-sensor fusion localization method with a unified clock reference, the problem of insufficient localization accuracy and robustness of SLAM in complex environments is solved, achieving high-precision mapping and rapid relocalization, thus enhancing the system's adaptability and stability.

CN123111233APending Publication Date: 2026-11-17SHANDONG INST OF COMMERCE & TECH
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202611303088.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-08-26
Publication Date
2026-11-17

AI Technical Summary

Technical Problem

Existing SLAM methods lack sufficient localization accuracy and robustness in complex environments, especially in scenarios with insufficient geometric features or degraded observations where scanning matching is unstable. Furthermore, the lack of a complete multi-sensor collaborative scheme makes it difficult to balance mapping accuracy and real-time performance.

Method used

An asymmetric computing architecture is adopted. The lower-level computer generates wheel odometry data, while the upper-level computer directly receives laser point cloud and inertial data. A unified clock reference is used for time alignment and point-by-point motion distortion compensation is performed. Local scan matching and global graph optimization are performed by combining multi-source predicted pose. Parameter optimization and arbitrary position repositioning are performed using a comprehensive evaluation function.

Benefits of technology

It improves the positioning accuracy and mapping consistency of mobile robots in complex environments, enhances the robustness and adaptability of the system, and enables rapid recovery of global pose in specific environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123111233A_ABST
    Figure CN123111233A_ABST
Patent Text Reader

Abstract

This invention relates to the field of mobile robot localization and navigation technology, and discloses a multi-sensor fusion localization method and system based on graph-optimized SLAM. The method involves a lower-level computer reading motor feedback and calculating wheel odometer readings, while a higher-level computer directly receives laser point clouds and inertial data, mapping these three types of data to a unified clock domain. Based on the acquisition time of each laser point, the inertial attitude and odometer translation are interpolated, and point-by-point motion distortion compensation is performed. The system uses multi-source predicted pose as the initial prior for local scan matching to construct a sub-map, and obtains a globally consistent trajectory and map through loop closure detection and sparse graph optimization. Simultaneously, parameters are optimized based on trajectory error and loop closure mismatch rate, and arbitrary position relocalization based on a preliminary initial pose is supported. This invention improves localization accuracy, mapping consistency, and operational robustness in complex environments through asymmetric allocation of computing power between the upper and lower-level computers, unified time alignment, and multi-source constraint optimization.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of mobile robot localization and navigation technology, specifically to a multi-sensor fusion localization method and system based on graph-optimized SLAM (Simultaneous Localization and Mapping). Background Technology

[0002] When mobile robots perform autonomous operations in complex indoor and outdoor environments, they need to estimate their own pose and build environmental maps in real time. Existing SLAM methods mainly include filtering-based methods and graph optimization-based methods. Graph optimization SLAM can perform batch optimization of the robot's complete motion trajectory by constructing laser point cloud nodes, sub-map nodes, and odometry constraints, scan matching constraints, and loop closure constraints, thus making it suitable for mapping and localization in a large-scale environment.

[0003] However, relying solely on lidar can lead to unstable scan matching in scenarios with insufficient geometric features or degraded observation, such as long corridors, large flat areas, sparse vegetation, glass, or highly reflective objects. Relying solely on wheeled odometers can result in cumulative drift due to wheel slippage, uneven ground, and tire deformation. Relying solely on inertial measurement units (IMUs) can cause attitude errors to increase over time due to zero bias and noise integration. Integrating lidar, IMUs, and wheeled odometers can leverage the complementary characteristics of these different sensors to improve positioning accuracy and robustness.

[0004] Existing graph-optimized SLAM software typically allows the reception of laser, inertial, and odometry data, but it lacks a complete collaborative solution for specific robots, from low-level motor feedback to high-level pose graph optimization. Specifically, it lacks the following: computational resource sharing between the lower and upper-level machines; a unified clock reference and point-by-point time alignment across multiple communication links; point-by-point motion distortion compensation for laser point clouds based on inertial attitude and wheel odometry translations; parameter search methods tailored to specific sensor frequencies, robot speeds, and environmental characteristics; and a rapid relocalization mechanism when starting from a non-mapping origin. Therefore, simply using existing SLAM software for routine calls or static parameter configuration is insufficient to simultaneously achieve mapping accuracy, real-time performance, and environmental adaptability. Summary of the Invention

[0005] The purpose of this invention is to provide a multi-sensor fusion localization method and system based on graph-optimized SLAM. Through asymmetric computing architecture, data alignment with a unified clock reference, point-by-point motion distortion compensation, local scan matching with multi-source priors, global graph optimization under closed-loop constraints, parameter optimization based on a comprehensive evaluation function, and arbitrary position relocalization, this method improves the localization accuracy of mobile robots in complex environments and enhances the system's capabilities. Figure 1 Consistency and operational robustness.

[0006] To achieve the above objectives, the present invention adopts the following technical solution.

[0007] A multi-sensor fusion localization method based on graph optimization SLAM includes the following steps: S1. Construct an asymmetric computing architecture and collect multi-source data: The lower-level computer control unit reads the motion feedback information of the motor controller and calculates the wheel odometer data of the robot; the upper-level computer processing unit directly receives the laser point cloud data output by the lidar and the inertial data output by the inertial measurement unit, and receives the wheel odometer data. S2. Unify clock reference and perform time alignment: Map the timestamps of laser point cloud data, inertial data, and wheel odometer data to the same clock domain, and establish inertial data buffer queues and wheel odometer data buffer queues respectively; based on the acquisition time of each laser point in the laser point cloud, interpolate the inertial data and wheel odometer data so that the acquisition time of each laser point is within the interpolation interval formed by adjacent valid samples of the corresponding sensor, and ensure that the clock mapping residual is not greater than the preset synchronization threshold. S3. Laser point cloud preprocessing and motion distortion compensation: Distance filtering and voxel downsampling are performed on the laser point cloud. Based on the time offset of each laser point relative to the reference time of a frame of point cloud, the relative pose between the acquisition time of the laser point and the reference time is calculated using the interpolated inertial attitude and the translation amount of the wheel odometer. Each laser point is then transformed to the coordinate system of the reference time. S4. Local localization and sub-map construction: The predicted pose of the current frame is formed based on inertial data and wheel odometer data. The predicted pose is used as the initial prior for scanning matching. The laser point cloud after motion distortion compensation is matched with the current sub-map. The local observation pose is obtained through nonlinear least squares optimization. The laser point cloud of the current frame is then inserted into the current sub-map. S5. Loop Detection and Global Graph Optimization: Maintain pose nodes of completed sub-maps and historical laser point clouds, generate loop candidate constraints within the candidate search window, perform fine matching on loop candidate constraints that meet the matching score threshold, and add odometry constraints, local matching constraints and loop constraints to the pose graph. Obtain globally consistent robot trajectory and map through sparse graph optimization. S6. Parameter Optimization and Relocation: The parameter optimization is performed during the system deployment or calibration phase; mapping tests are conducted for multiple candidate parameter combinations, based on trajectory error, location... Figure 1A comprehensive evaluation function is constructed based on consistency, processing latency, and closed-loop mismatch rate. The target parameter combination is obtained through coarse-to-fine parameter search. The relocalization is triggered as needed during system operation. After loading the pre-built global map or historical sub-map set, when the robot starts from a non-map origin or loses localization, multiple candidate poses are generated within a preset position range and heading angle range based on the input approximate initial pose, and map matching is performed. The candidate pose with the highest matching score and exceeding the relocalization threshold is determined as the new tracking starting point.

[0008] Furthermore, S1 specifically includes the following steps: S11, Wheel Odometer Calculation: The lower-level control unit reads the rotational speed and rotor position information fed back by the motor controller through the bus, performs forward kinematics calculation according to the robot drive form, obtains the linear velocity and yaw rate in the robot body coordinate system, and generates wheel odometer data with timestamps through discrete integration. S12. Direct Multi-Source Data Acquisition: The host computer processing unit directly receives laser point cloud data output by the lidar and inertial data output by the inertial measurement unit through different communication interfaces, and also receives wheel odometer data sent by the lower-level control unit. The asymmetric computing architecture configures motor feedback reading, basic kinematics calculation, and wheel odometer generation in the lower-level control unit, while configuring laser point cloud processing, inertial data processing, time alignment, scan matching, and pose map optimization in the host computer processing unit. This leverages the strong real-time performance of the lower-level computer and avoids communication congestion and additional delays caused by high-frequency, high-bandwidth sensor data being relayed through the lower-level computer.

[0009] S13. Data Encapsulation and Caching: The host computer processing unit performs format verification, unit conversion, coordinate system identification, and timestamp addition or correction on the laser point cloud data, inertial data, and wheel odometer data, respectively, and encapsulates them into standard data messages that the positioning algorithm can subscribe to before writing them into the corresponding data buffer queue.

[0010] Furthermore, in S11, for a differential drive robot, the linear velocity in the robot's body coordinate system is calculated based on the rotational speeds of the left and right drive wheels. v and yaw rate ω ,satisfy v =( v R + v L ) / 2 and ω =( v R - v L ) / B, where, v R Let be the equivalent linear velocity of the right drive wheel. vL Let B be the equivalent linear velocity of the left drive wheel and B be the equivalent wheelbase of the left and right drive wheels. The lower-level control unit performs discrete integration on the linear velocity and yaw rate over a sampling period Δt to obtain the translational and rotational increments between adjacent moments, and generates wheel odometry data containing position, attitude, linear velocity, angular velocity, and covariance information. For a four-wheel differential robot, the velocities of the front and rear drive wheels on the same side are first averaged or weighted and fused before the linear velocity is calculated. v and yaw rate ω .

[0011] Furthermore, S2 specifically includes the following steps: S21. Master Clock Determination and Clock Mapping: The unified clock reference adopts a host computer monotonic system clock, a local clock synchronized by external pulses, a network precise time protocol clock, or a combination thereof. In a preferred embodiment, the host computer monotonic system clock is used as the master clock, and a mapping relationship is established between the clocks of each sensor device and the master clock.

[0012] S22. Clock Offset and Transmission Delay Correction: For data with device timestamps, the clock offset between the device clock and the master clock is estimated based on a sliding time window, and abnormal transmission delay samples are removed. For data without device timestamps, the corrected timestamp is obtained by subtracting the estimated transmission delay from the received time. The sliding time window contains N consecutive data packets, preferably N between 20 and 200. The difference between the received time and the device time of each data packet is calculated, and the clock offset is estimated using the median or truncated mean. Samples that deviate from the median by more than a preset multiple of the median absolute deviation are removed, thereby reducing the impact of operating system scheduling jitter and network burst delays on time correction. S23. Interpolation Alignment and Synchronization Determination: The synchronization process uses the scanning period of each frame of laser point cloud as the synchronization period. Based on the acquisition time of each laser point in the laser point cloud, adjacent time data are read from the inertial data buffer queue and the wheel odometer data buffer queue and interpolated. When the acquisition time of the laser point is within the interpolation interval formed by the adjacent valid samples of the corresponding sensor, and the clock mapping residual is not greater than the preset synchronization threshold, it is determined that the set of data meets the time alignment requirements. The preset synchronization threshold is preferably 1 to 10 ms.

[0013] Furthermore, S3 specifically includes the following steps: S31. Point cloud preprocessing: Perform minimum range filtering, maximum range filtering, and voxel downsampling on the original laser point cloud to remove robot-occluded points, overrange points, and redundant points; at the same time, read the frame start time, frame end time, or relative time of each laser point in a frame of point cloud; if the lidar does not directly output the point-by-point relative time, estimate the relative time of each laser point based on the change in the azimuth angle of each laser point relative to the frame start azimuth angle, the scanning cycle, and the point sequence information. S32. Pose Interpolation and Extrinsic Parameter Transformation: Let the reference time of a frame of laser point cloud be... t ref , No. i The acquisition time of each laser point is t i The attitude was interpolated using quaternion spherical linear interpolation based on inertial data, and the translation was interpolated using wheel odometry data. Combined with pre-calibrated fixed extrinsic parameters between the lidar coordinate system and the robot's body coordinate system, the lidar coordinate system was obtained in [the following context]. t i and t ref Position at any moment T ( t i )and T ( t ref ),in T ( t ) indicates that the lidar coordinate system is at time 10:00. t Pose transformation matrix relative to a unified reference coordinate system; S33, Point-by-point motion distortion compensation: ... i laser points p i Transform to the reference time using the following formula: p ' i =T -1 ( t ref ) T ( t i ) p i ; The above transformation is performed sequentially on each laser point in the laser point cloud of the same frame, unifying laser points collected at different times within the same frame to the reference time, reducing point cloud stretching, misalignment, and ghosting caused by robot translation, turning, and vibration. Inertial data mainly provides high-frequency rotational changes, while wheeled odometry mainly provides short-term translational changes; when wheeled odometry is determined to be slipping, the weight of that wheeled odometry data in the predicted pose calculation and scan matching translation prior terms is reduced.

[0014] Furthermore, S4 specifically includes the following steps: S41. Predicted Pose Generation: Based on the time-aligned inertial data and wheel odometry data, calculate the attitude increment and translation increment of the current laser frame relative to the previous laser frame, and form the predicted pose of the current laser frame. T pre .

[0015] S42. Scan Matching and Pose Optimization: Using the predicted pose as the initial prior for scan matching, construct an objective function that includes the matching residual between the laser point cloud and the probability grid or voxel map, the translational prior residual between the current pose and the predicted pose, and the rotational prior residual; the objective function is expressed as: ; Where M represents the occupancy probability of the probability grid or voxel map. T = ( R , t ( ) represents the position to be determined. T pre = ( R pre , t pre The predicted pose is formed by inertial data and wheel odometer readings. λ t and λ r These are translational prior weights and rotational prior weights, respectively; the objective function is solved using at least one of the Gauss-Newton method, the Levenberg-Marquardt method, or the trust region method to obtain the local observation pose. S43. Sub-map update: Insert the current laser point cloud after the local observation pose transformation into the active sub-map; when the active sub-map meets the preset switching conditions, create a new active sub-map, and set the sub-map that meets the completion conditions to the completed state according to the overlapping update mechanism.

[0016] Furthermore, S5 specifically includes the following steps: S51. Historical Map Maintenance: Store the completed sub-map and its associated historical laser point cloud pose nodes into the backend historical set, and construct a pose map based on odometry constraints and local scan matching constraints.

[0017] S52, Closed-loop candidate search: The global optimization thread searches for closed-loop candidates in the historical submap. First, it uses correlation scanning matching and branch and bound search to obtain candidate poses and matching scores. Then, it uses nonlinear scanning matching to refine the poses of candidate constraints that meet the matching score threshold.

[0018] S53. Global Graph Optimization: The refined closed-loop constraints are added to the pose graph. Sparse Pose Adjustment (SPA) or an equivalent sparse nonlinear graph optimization method is used to jointly optimize the pose nodes of the historical laser point cloud and the pose nodes of the sub-map to obtain a globally consistent robot trajectory and map.

[0019] Furthermore, S6 specifically includes the following steps: S61. Parameter Optimization: Performed during system deployment or calibration, using at least one of the following parameters as optimization parameters: voxel filter size, submap switching threshold, linear search window for local correlation scan matching, angle search window for local correlation scan matching, number of global optimization trigger nodes, and closed-loop matching score threshold; performing mapping tests on multiple candidate parameter combinations, based on trajectory error, ground... Figure 1 A comprehensive evaluation function is constructed based on consistency, processing latency, and closed-loop mismatch rate. A coarse grid search is first performed, followed by a fine search with a smaller search step size for the top 10% of parameter combinations in the comprehensive evaluation function. Specifically, parameter optimization is performed using a calibration dataset, which includes at least three motion scenarios: linear motion, turning motion, in-situ rotation, and feature-rich and feature-sparse regions. It may also include scenarios involving slippery ground, slopes, or highly reflective objects. A complete mapping process is run for each candidate parameter combination, and the comprehensive evaluation function is calculated. ; in, E traj The trajectory error is related to the high-precision reference trajectory or the start and end point error of the closed loop. E map This refers to the overlap error of point clouds in overlapping areas or map entropy. T delay The average processing delay per laser frame. R false This represents the closed-loop mismatch rate. w 1- w 4 represents the normalized weights. First, a coarse grid search is performed within the candidate range. Then, a fine search is performed by reducing the search step size for the parameter combinations with the top 10% F-values, until the F-value improvement rate is less than 1% for two consecutive rounds or the preset number of iterations is reached. Preferably, the voxel filter size is 0.03–0.10 m, the sub-map switching threshold is 60–120 frames, the linear search window for local correlation scanning matching is 0.05–0.20 m, the angle search window is 0.05–0.25 rad, the number of global optimization trigger nodes is 60–120, and the closed-loop matching score threshold is 0.50–0.70. The above ranges are used to provide an feasible initial search domain and do not constitute a limitation on sensor model, robot size, or environment type. S62, Arbitrary Position Relocation: Triggered on demand during system operation; after loading a pre-built global map or historical sub-map set, it receives a rough initial pose ( x 0, y 0, θ 0), in [ x 0- Δ x , x 0+ Δ x ]、[ y0- Δ y , y 0+ Δ y ]and[ θ 0- Δ θ , θ 0+ Δ θ Layered sampling is performed within the search space formed by [the system]. First, correlation scanning matching is used to calculate the candidate pose score. Then, the preset number of candidate poses with the highest score are refined nonlinearly. When the optimal score reaches the relocation threshold, the global transformation relationship between the map coordinate system and the odometry coordinate system is updated. If the system does not set an independent odometry coordinate system, the transformation relationship between the map coordinate system and the robot body coordinate system is updated.

[0020] The present invention also provides a multi-sensor fusion positioning system, comprising: The sensing unit includes a lidar and an inertial measurement unit, which are used to output laser point cloud data and inertial data, respectively. The execution unit, including a drive motor and a motor controller, is used to drive the robot to move and output motion feedback information; The lower-level control unit communicates with the execution unit and is used to read the motion feedback information, perform kinematic calculations, and generate wheel odometer data. The host computer processing unit communicates with both the sensing unit and the lower computer control unit. The host computer processing unit includes a data receiving and caching module, a time synchronization module, a point cloud preprocessing and motion distortion compensation module, a local positioning and sub-map construction module, a loop closure detection and global map optimization module, a parameter optimization module, and a repositioning module.

[0021] Furthermore, the lower-level control unit is a microcontroller or a real-time controller, and the upper-level processing unit is an edge computing platform with a central processing unit and / or a graphics processing unit; the lower-level control unit communicates with the motor controller via a controller area network bus, and the upper-level processing unit communicates with the lidar via Ethernet, with the inertial measurement unit via a serial interface or a universal serial bus, and with the lower-level control unit via a serial interface, Ethernet, or a universal serial bus.

[0022] Technical effects of the present invention: Compared with existing technologies, the multi-sensor fusion localization method and system based on graph-optimized SLAM of the present invention has the following advantages: (1) Asymmetric computing power allocation is adopted, with the real-time motor feedback reading and kinematic calculation configured on the lower computer, and the high-bandwidth laser point cloud, high-frequency inertial data and graph optimization configured on the upper computer, reducing bus congestion and time jitter caused by intermediate forwarding.

[0023] (2) Instead of relying on the order of message arrival, it maps multi-source data to a unified clock domain and interpolates the inertial attitude and odometer translation according to the actual acquisition time of the laser point to achieve point-by-point motion distortion compensation and improve the geometric consistency of the local sub-map.

[0024] (3) Using multi-source predicted pose as the initial prior for local scanning matching, and using odometry constraints, local matching constraints and closed-loop constraints in conjunction with global backend, can maintain stable positioning under laser degradation or short-term slippage conditions.

[0025] (4) A comprehensive evaluation function that includes accuracy, map quality, real-time performance and closed-loop reliability is used for parameter search to avoid statically setting parameters based solely on experience, so that the algorithm can adapt to different robot speeds, sensor frequencies and environmental characteristics.

[0026] (5) A hierarchical sampling and map matching mechanism based on the approximate initial pose is set up, which can quickly restore the global pose after starting at a non-map origin or after a short period of positioning loss. Attached Figure Description

[0027] Figure 1 This is a hardware architecture diagram of a multi-sensor fusion positioning system according to a specific embodiment of the present invention. Detailed Implementation

[0028] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the invention will be further described below with reference to the accompanying drawings and specific embodiments. These embodiments are used to explain the invention and are not intended to limit the scope of protection of the invention.

[0029] Example 1: This embodiment relates to a multi-sensor fusion localization method based on graph-optimized SLAM.

[0030] like Figure 1As shown, this embodiment employs an asymmetric computing architecture. The host computer processing unit uses a Jetson Orin NX, and the slave control unit uses an STM32F407. The sensing units include a LeiShen C16 lidar and a WHEELTEC N100 inertial measurement unit (IMU). The execution units include a DJI-3508 motor and a DJI C620 motor controller. The STM32F407 reads the motor speed and rotor position feedback via the Controller Area Network (CAN) bus and generates a wheel odometer through forward kinematics calculation. The Jetson Orin NX directly receives the laser point cloud via Ethernet, directly receives IMU data via a Universal Serial Bus (USB) or serial interface, and receives the wheel odometer data via a serial interface or USB-to-serial link. The display is used for operational status monitoring, and the remote control and input / output expansion interfaces are used for chassis debugging and safety control.

[0031] In this embodiment, the graph-optimized SLAM framework is implemented using Cartographer. Local localization uses the Ceres nonlinear scanning matcher, and closed-loop candidate constraints are obtained using a fast correlation scanning matcher and branch and bound search. The running parameters are loaded through a Lua configuration file.

[0032] In this embodiment, the lidar scanning frequency is 10Hz, the IMU output frequency is 200Hz, and the wheel odometer output frequency is 100Hz. These frequencies are merely one set of optional implementation parameters. The laser point cloud can be published as a 3D point cloud topic or projected and published as a 2D laser scan topic; the inertial data and wheel odometer data are encapsulated as standard inertial messages and odometer messages, respectively.

[0033] Step S11, Wheel Odometer Calculation: The STM32F407 reads the speed and rotor position of the four DJI-3508 motors, averages the speeds of the front and rear wheels on the same side, or weights them according to the effective tire radius to obtain the equivalent linear velocity on both sides. v L and v R ;according to v =( v R + v L ) / 2 and ω =( v R - v LThe system calculates the linear velocity and yaw rate of the vehicle body and integrates them at a frequency of 100Hz, outputting wheel odometer data including position, attitude, velocity and covariance.

[0034] Step S12, Direct Data Acquisition from Multiple Sources: Jetson Orin NX directly receives laser point cloud data output from the LeiShen C16 LiDAR via Ethernet, directly receives high-frequency inertial data output from the WHEELTEC N100 inertial measurement unit via a universal serial bus or serial interface, and receives wheel odometer data sent by the STM32F407 via a serial interface or universal serial bus, thereby avoiding the need for laser point cloud and high-frequency inertial data to be relayed through a lower-level machine.

[0035] Step S13, Data Encapsulation and Caching: Jetson Orin NX performs data frame verification, unit conversion, and coordinate system identification on the three types of data, encapsulates them into laser point cloud messages, inertial messages, and wheel odometer messages, and writes them into the corresponding circular buffer queues for subsequent reading by the time synchronization module.

[0036] Step S21, Master Clock Determination and Clock Mapping: The Jetson Orin NX uses the host computer's monotonic system clock as the master clock, and establishes mapping relationships between the LiDAR device clock, the inertial measurement unit device clock, and the STM32F407 clock and the master clock respectively; if subsequent hardware provides a Pulse Per Second (PPS) or Precision Time Protocol (PTP) interface, a unified hardware clock can also be used as the master clock.

[0037] Step S22, Clock Offset and Transmission Delay Correction: Maintain circular buffer queues for inertial data and wheel odometer data respectively in each laser scanning cycle. Calculate the clock offset for the inertial measurement unit device timestamp and wheel odometer timestamp. Set the sliding time window length to 100 samples, estimate the clock offset using the median, and remove abnormal transmission delay samples. For data without device timestamps, generate a correction timestamp based on the reception time and estimated transmission delay.

[0038] Step S23, Interpolation Alignment and Synchronization Determination: Based on the acquisition time of each laser point, the system reads adjacent time data from the inertial data buffer queue and the wheel odometer data buffer queue and performs interpolation. In this embodiment, the acquisition time of each laser point is within the interpolation interval formed by the adjacent effective samples of the corresponding sensor, and the clock mapping residual is controlled within 5ms. When the acquisition time of the laser point exceeds the effective interpolation interval, the clock mapping residual exceeds the synchronization threshold, or the buffer queue data is incomplete, the processing of the laser point cloud of that frame is temporarily suspended or its fusion weight is reduced.

[0039] Step S31, Point Cloud Preprocessing: Perform distance filtering and voxel downsampling on each frame of the original laser point cloud. The lower and upper limits of the distance filtering are set according to the occlusion range of the robot structure and the effective range of the lidar. In this embodiment, the voxel filtering size is set to 0.05m. If the lidar does not directly output the point-by-point relative time, the relative time of each laser point is estimated based on the change of the azimuth angle of each laser point relative to the frame start azimuth angle, the scanning period, and the point sequence information.

[0040] Step S32, Pose Interpolation and Extrinsic Parameter Transformation: Based on the relative time of each laser point, quaternion spherical linear interpolation is performed on the attitude output by the inertial measurement unit, and linear interpolation is performed on the translation output by the wheel odometer. Combined with the pre-calibrated fixed extrinsic parameters between the lidar coordinate system and the robot body coordinate system, the lidar coordinate system at the laser point acquisition time is obtained. t i With frame reference time t ref pose relative to a unified reference coordinate system T ( t i )and T ( t ref ),in T ( t ) indicates that the lidar coordinate system is at time 10:00. t The pose transformation matrix relative to a unified reference coordinate system.

[0041] Step S33, Point-by-point motion distortion compensation: According to p ' i =T -1 ( t ref ) T ( t i ) p i The first i The laser points are transformed to the frame reference time, and the transformation is performed sequentially on all laser points in a frame of laser point cloud, thereby reducing point cloud stretching, misalignment, and ghosting caused by robot translation, turning, and vibration.

[0042] After time synchronization and motion distortion compensation, the laser point cloud, inertial data, and wheel odometry data are fed into the graph-optimized SLAM core algorithm layer.

[0043] Step S41, Predicted pose generation: Local SLAM, i.e., the local SLAM front end, calculates the attitude increment and translation increment of the current laser frame relative to the previous laser frame based on the inertial angular velocity and wheel odometry velocity, and forms the predicted pose.

[0044] Step S42, Scan Matching and Pose Optimization: The system uses the predicted pose as the initial value for scan matching, and adopts the Ceres nonlinear scan matcher to match the current laser point cloud with the probability grid of the active sub-map, and obtains the local observation pose of the current laser frame through nonlinear least squares optimization.

[0045] Step S43, Submap Update: Insert the current laser point cloud after local observation pose transformation into the active submap; when the active submap meets the preset switching conditions, create a new active submap, and set the submap that meets the completion conditions to the completed state according to the overlapping update mechanism.

[0046] Step S51, Historical Map Maintenance: Global SLAM backend maintenance has been completed, including sub-map sets, historical laser point cloud pose nodes, odometry constraints, and local scan matching constraints.

[0047] Step S52, Loop Closed Candidate Search: The system searches for loop closed candidates in the historical sub-map using a fast correlation scanning matcher and branch and bound method, filters reliable loop closed candidates based on the matching score threshold, and refines the pose using nonlinear scanning matching.

[0048] Step S53, Global Graph Optimization: Add the refined closed-loop constraints to the global pose graph, and use Sparse Pose Adjustment (SPA) to optimize all historical laser point cloud pose nodes and sub-map pose nodes to obtain a globally consistent trajectory and map.

[0049] In this embodiment, Cartographer is preferably used to implement the above-mentioned graph-optimized SLAM. Its configuration file is written in Lua, but the Lua configuration file is only used to load parameters and does not constitute a necessary limitation on the independent scope of protection of this invention. Coordinate system parameters can be set to map_frame="map", tracking_frame="imu_link", published_frame="odom", and odom_frame="odom". When the external wheeled odometer has already provided the odom-to-base_link transformation, provide_odom_frame=false, and use_odometry=true and use_imu_data=true can be set. When the system does not have an external module providing the odom-to-base_link transformation, the SLAM node can provide the corresponding odometer coordinate transformation according to the frame configuration, and published_frame can be set to base_link.

[0050] Step S61, Parameter Optimization: This step is performed during system deployment or calibration. To obtain parameters suitable for the robot and sensor combination, this embodiment establishes a calibration dataset including straight lines, turns, in-situ rotations, feature-rich regions, and feature-sparse regions. A coarse mesh search is performed within the following ranges: voxel filter size 0.03–0.10 m, sub-map switching threshold 60–120 frames, linear search window for local correlation scanning matching 0.05–0.20 m, angle search window 0.05–0.25 rad, number of global optimization trigger nodes 60–120, and closed-loop matching score threshold 0.50–0.70. A comprehensive evaluation function normalizes and weights the trajectory error, overlapping error of point clouds in repeated regions, average processing latency per frame, and closed-loop mismatch rate. After the coarse search, the top 10% of parameter combinations are retained, and the search step size is halved for a fine search.

[0051] Based on the above optimization, the following set of parameters can be selected in this embodiment: voxel filter size 0.05m, sub-map switching parameter num_range_data 90, linear search window for local correlation scan matching 0.10m, angle search window 0.15rad, global optimization triggered once every 90 nodes, minimum score for closed-loop matching 0.55, and minimum score for relocalization 0.60. These values ​​are example values; in actual applications, they can be re-optimized based on robot speed, point cloud density, and environmental characteristics.

[0052] Step S62, Arbitrary Location Repositioning: This step is triggered as needed during system operation. After loading the pre-built global map or historical sub-map set, when the robot starts at a location other than the mapping origin or experiences positioning loss, the operator or the upper-level task system provides a rough initial pose (…). x 0, y 0, θ 0). The system performs low-resolution layered sampling within a position range of ±1.0m and a heading angle of ±30°. Correlation-based scan matching is used to calculate candidate scores, and the top 10 candidates are retained for Ceres nonlinear pose refinement. When the optimal score is not lower than 0.60, the global pose relationship between the map and the odom is updated. If the system does not have an independent odom coordinate system, the pose relationship between the map and the base link is updated, and normal tracking is restored. Visualization tools are used to monitor the coordinate transformation tree, point cloud, active submap, and global map status.

[0053] Example 2: This embodiment relates to a multi-sensor fusion localization system based on graph-optimized SLAM, including a sensing unit, an execution unit, a lower-level control unit, and a higher-level processing unit. The sensing unit includes a lidar and an inertial measurement unit, used to output lidar point cloud data and inertial data, respectively. The execution unit includes a drive motor and a motor controller, used to drive robot movement and output motion feedback information. The lower-level control unit communicates with the execution unit, used to read the motion feedback information, perform kinematic calculations, and generate wheel odometer data. The higher-level processing unit communicates with both the sensing unit and the lower-level control unit, and includes a data receiving and caching module, a time synchronization module, a point cloud preprocessing and motion distortion compensation module, a local localization and sub-map construction module, a loop closure detection and global graph optimization module, a parameter optimization module, and a relocalization module.

[0054] In other embodiments, the host computer processing unit can be other edge computing platforms with a central processing unit and / or a graphics processing unit, the lower-level control unit can be other microcontrollers, digital signal processors, or real-time controllers, the lidar can be a two-dimensional or three-dimensional lidar, and the IMU, motor controller, and drive motor can also be other models. As long as unified time alignment, point-by-point motion distortion compensation, multi-source prior scan matching, and pose graph optimization can be achieved, they all fall under the implementation of the technical concept of this invention.

[0055] It should be noted that this invention is not a simple application of the existing Cartographer software. While Cartographer can serve as one implementation of a graph-optimized SLAM framework, the technical contribution of this invention focuses on the hardware-software collaboration before and during framework input: the lower-level computer generates wheel odometry in real time; the upper-level computer directly connects to high-frequency inertial data and laser point clouds; point-by-point time alignment and motion distortion compensation are performed within a unified clock domain; local scan matching is achieved using multi-source predicted pose constraints; running parameters are searched through a comprehensive evaluation function; and arbitrary location relocalization is provided. These technical features can also be deployed in other SLAM frameworks with local sub-map and global pose graph optimization capabilities.

[0056] This invention uses a lower-level computer to read motor feedback and calculate wheel odometer readings, while a higher-level computer directly receives laser point cloud and inertial data, mapping these three types of data to a unified clock domain. Based on the acquisition time of each laser point, the system interpolates the inertial attitude and odometer translation and performs point-by-point motion distortion compensation. The system uses multi-source predicted pose as the initial prior for local scan matching to construct a sub-map, and obtains a globally consistent trajectory and map through loop closure detection and sparse graph optimization. Simultaneously, based on trajectory error and location... Figure 1The invention optimizes operating parameters from coarse to fine, considering consistency, processing latency, and closed-loop mismatch rate, and supports arbitrary position repositioning based on a rough initial pose. Through asymmetric allocation of computing power between upper and lower computers, unified time alignment, and multi-source constraint optimization, the invention improves positioning accuracy and system construction in complex environments. Figure 1 Consistency and operational robustness.

[0057] The above description is merely a preferred embodiment of the present invention. Any equivalent substitutions and appropriate modifications made by those skilled in the art to the sensor model, communication interface, optimizer, evaluation index weights, or parameter ranges without departing from the spirit and essence of the present invention should fall within the protection scope of the claims of the present invention.

Claims

1. A multi-sensor fusion localization method based on graph-optimized SLAM, characterized in that, include: S1. Construct an asymmetric computing architecture and collect multi-source data: The lower-level computer control unit reads the motion feedback information of the motor controller and calculates the wheel odometer data of the robot; the upper-level computer processing unit directly receives the laser point cloud data output by the lidar and the inertial data output by the inertial measurement unit, and receives the wheel odometer data. S2. Unify clock reference and perform time alignment: Map the timestamps of laser point cloud data, inertial data, and wheel odometer data to the same clock domain, and establish inertial data buffer queues and wheel odometer data buffer queues respectively; based on the acquisition time of each laser point in the laser point cloud, interpolate the inertial data and wheel odometer data so that the acquisition time of each laser point is within the interpolation interval formed by adjacent valid samples of the corresponding sensor, and ensure that the clock mapping residual is not greater than the preset synchronization threshold. S3. Laser point cloud preprocessing and motion distortion compensation: Distance filtering and voxel downsampling are performed on the laser point cloud. Based on the time offset of each laser point relative to the reference time of a frame of point cloud, the relative pose between the acquisition time of the laser point and the reference time is calculated using the interpolated inertial attitude and the translation amount of the wheel odometer. Each laser point is then transformed to the coordinate system of the reference time. S4. Local localization and sub-map construction: The predicted pose of the current frame is formed based on inertial data and wheel odometer data. The predicted pose is used as the initial prior for scanning matching. The laser point cloud after motion distortion compensation is matched with the current sub-map. The local observation pose is obtained through nonlinear least squares optimization. The laser point cloud of the current frame is then inserted into the current sub-map. S5. Loop Detection and Global Graph Optimization: Maintain pose nodes of completed sub-maps and historical laser point clouds, generate loop candidate constraints within the candidate search window, perform fine matching on loop candidate constraints that meet the matching score threshold, and add odometry constraints, local matching constraints and loop constraints to the pose graph. Obtain globally consistent robot trajectory and map through sparse graph optimization. S6. Parameter Optimization and Relocation: The parameter optimization is performed during the system deployment or calibration phase. Mapping tests are conducted for multiple candidate parameter combinations. A comprehensive evaluation function is constructed based on trajectory error, map consistency, processing latency, and closed-loop mismatch rate. The target parameter combination is obtained through coarse-to-fine parameter search. Relocation is triggered as needed during the system operation phase. After loading the pre-built global map or historical sub-map set, when the robot starts from a non-mapped origin or loses its localization, multiple candidate poses are generated within a preset position range and heading angle range based on the input approximate initial pose. Map matching is then performed, and the candidate pose with the highest matching score that exceeds the relocation threshold is determined as the new tracking starting point.

2. The multi-sensor fusion positioning method according to claim 1, characterized in that, S1 specifically includes the following steps: S11, Wheel Odometer Calculation: The lower-level control unit reads the rotational speed and rotor position information fed back by the motor controller through the bus, performs forward kinematics calculation according to the robot drive form, obtains the linear velocity and yaw rate in the robot body coordinate system, and generates wheel odometer data with timestamps through discrete integration. S12. Multi-source data direct acquisition: The host computer processing unit directly receives the laser point cloud data output by the lidar through the first communication interface, directly receives the inertial data output by the inertial measurement unit through the second communication interface, and receives the wheel odometer data sent by the lower computer control unit through the third communication interface. S13. Data encapsulation and caching: The host computer processing unit performs format verification, timestamp appending or correction on the laser point cloud data, inertial data and wheel odometer data respectively, and encapsulates them into standard data messages that the positioning algorithm can subscribe to before writing them into the corresponding data buffer queue.

3. The multi-sensor fusion positioning method according to claim 1, characterized in that, S2 specifically includes the following steps: S21. Master clock determination and clock mapping: Use at least one of the following as the master clock: the monotonic system clock of the host computer processing unit, the local clock synchronized by an external pulse, or the network precise time protocol clock, and establish a mapping relationship between the clocks of each sensor device and the master clock. S22, Clock skew and transmission delay correction: For data with device timestamps, the clock skew between the device clock and the master clock is estimated based on a sliding time window, and abnormal transmission delay samples are removed; for data without device timestamps, the correction timestamp is obtained by subtracting the estimated transmission delay from the receiving time. S23. Interpolation Alignment and Synchronization Judgment: The scanning period of each frame of laser point cloud is used as the synchronization period. Based on the acquisition time of each laser point, adjacent time data are read from the inertial data buffer queue and the wheel odometer data buffer queue and interpolated. When the acquisition time of the laser point is within the interpolation interval formed by the adjacent valid samples of the corresponding sensor, and the clock mapping residual is not greater than the preset synchronization threshold, the data set is determined to meet the time alignment requirements.

4. The multi-sensor fusion positioning method according to claim 1, characterized in that, S3 specifically includes the following steps: S31. Point cloud preprocessing: Perform minimum range filtering, maximum range filtering and voxel downsampling on the original laser point cloud to remove robot body occlusion points, over-range points and redundant points. S32. Pose Interpolation and Extrinsic Parameter Transformation: Let the reference time of a frame of laser point cloud be... t ref , No. i The acquisition time of each laser point is t i The attitude was interpolated using quaternion spherical linear interpolation based on inertial data, and the translation was interpolated using wheel odometry data. Combined with pre-calibrated fixed extrinsic parameters between the lidar coordinate system and the robot's body coordinate system, the lidar coordinate system was obtained in [the following context]. t i and t ref Position at any moment T ( t i )and T ( t ref ),in T ( t ) indicates that the lidar coordinate system is at time 10:

00. t Pose transformation matrix relative to a unified reference coordinate system; S33, Point-by-point motion distortion compensation: ... i laser points p i according to p ' i =T -1 ( t ref ) T ( t i ) p i Transformed into laser point at reference time p ' i The transformation is then performed sequentially on each laser point in the laser point cloud of that frame.

5. The multi-sensor fusion positioning method according to claim 1, characterized in that, S4 specifically includes the following steps: S41. Predicted pose generation: Calculate the attitude increment and translation increment of the current laser frame relative to the previous laser frame based on the time-aligned inertial data and wheel odometer data, and generate the predicted pose of the current laser frame. S42. Scan Matching and Pose Optimization: Using the predicted pose as the initial prior for scan matching, construct an objective function including the matching residual between the laser point cloud and the probability grid or voxel map, the translational prior residual between the current pose and the predicted pose, and the rotational prior residual. Then, use at least one of the Gauss-Newton method, the Levenberg-Marquardt method, or the trust region method to solve for the local observation pose. S43. Sub-map update: Insert the current laser point cloud after the local observation pose transformation into the active sub-map. When the active sub-map meets the preset switching conditions, create a new active sub-map and set the sub-map that meets the completion conditions to the completed state according to the overlapping update mechanism.

6. The multi-sensor fusion positioning method according to claim 5, characterized in that, S5 specifically includes the following steps: S51. Historical map maintenance: Store the completed sub-map and its associated historical laser point cloud pose nodes into the backend historical set, and construct the pose map based on odometry constraints and local scan matching constraints. S52, Closed-loop candidate search: The global optimization thread searches for closed-loop candidates in the historical submap. First, it uses correlation scanning matching and branch and bound search to obtain candidate poses and matching scores. Then, it uses nonlinear scanning matching to refine the poses of candidate constraints that meet the matching score threshold. S53. Global graph optimization: The refined closed-loop constraints are added to the pose graph. The sparse pose adjustment or equivalent sparse nonlinear graph optimization method is used to jointly optimize the historical laser point cloud pose nodes and sub-map pose nodes to obtain a globally consistent robot trajectory and map.

7. The multi-sensor fusion positioning method according to claim 1, characterized in that, S6 specifically includes the following steps: S61. Parameter Optimization: Performed during system deployment or calibration, at least one of the following parameters is selected as the optimization parameters: voxel filter size, submap switching threshold, linear search window for local correlation scan matching, angle search window for local correlation scan matching, number of global optimization trigger nodes, and closed-loop matching score threshold. Mapping tests are performed on multiple candidate parameter combinations, and a comprehensive evaluation function is constructed based on trajectory error, map consistency, processing latency, and closed-loop mismatch rate. Grid search is then performed. S62, Arbitrary Position Relocation: Triggered on demand during system operation; after loading a pre-built global map or historical sub-map set, it receives a rough initial pose ( x 0, y 0, θ 0), perform hierarchical sampling within the neighborhood of position and heading angle; firstly, use correlation scanning matching to calculate candidate pose scores, then perform nonlinear pose refinement on the preset number of candidate poses with the highest scores, and update the global transformation relationship between the map coordinate system and the odometry coordinate system when the optimal score reaches the relocation threshold; if the system does not set an independent odometry coordinate system, then update the transformation relationship between the map coordinate system and the robot body coordinate system.

8. The multi-sensor fusion positioning method according to claim 2, characterized in that, In step S11, for a differential drive robot, the linear velocity in the robot's body coordinate system is calculated based on the rotational speeds of the left and right drive wheels. v and yaw rate ω ,satisfy v =( v R + v L ) / 2 and ω =( v R - v L ) / B, where, v R Let be the equivalent linear velocity of the right drive wheel. v L Let B be the equivalent linear velocity of the left drive wheel, and B be the equivalent wheelbase of the left and right drive wheels. For a four-wheel differential robot, the velocities of the front and rear drive wheels on the same side are first averaged or weighted and fused before calculating the linear velocity. v and yaw rate ω .

9. A multi-sensor fusion positioning system for implementing the multi-sensor fusion positioning method according to any one of claims 1 to 8, characterized in that, The system includes: The sensing unit includes a lidar and an inertial measurement unit, which are used to output laser point cloud data and inertial data, respectively. The execution unit, including a drive motor and a motor controller, is used to drive the robot to move and output motion feedback information; The lower-level control unit communicates with the execution unit and is used to read the motion feedback information, perform kinematic calculations, and generate wheel odometer data. The host computer processing unit communicates with both the sensing unit and the lower computer control unit. The host computer processing unit includes a data receiving and caching module, a time synchronization module, a point cloud preprocessing and motion distortion compensation module, a local positioning and sub-map construction module, a loop closure detection and global map optimization module, a parameter optimization module, and a repositioning module.

10. The multi-sensor fusion positioning system according to claim 9, characterized in that, The lower-level control unit is a microcontroller or a real-time controller, and the upper-level processing unit is an edge computing platform with a central processing unit and / or a graphics processing unit. The lower-level control unit communicates with the motor controller via a controller area network bus, and the upper-level processing unit communicates with the lidar via Ethernet, with the inertial measurement unit via a serial interface or a universal serial bus, and with the lower-level control unit via a serial interface, Ethernet, or a universal serial bus.