Multi-sensor fusion based heavy lifting equipment centimeter level positioning method and system

Abstract

This application relates to the fields of sensor fusion and industrial automation technology, and in particular to a centimeter-level positioning method and system for heavy-duty hoisting equipment based on multi-sensor fusion. The method includes: acquiring raw observation data from BeiDou RTK, lidar, millimeter-wave radar, high-definition cameras, and inertial measurement units; performing spatiotemporal synchronization processing to generate fused observation data; constructing an initial factor graph with equipment pose as the core state variable; real-time monitoring of the quality indicators of each sensor, dynamically adjusting the weights of factor nodes or removing nodes to generate a dynamic topology factor graph; and incrementally optimizing the dynamic topology factor graph using the iSAM2 algorithm to output equipment pose estimation and covariance matrix. This application achieves centimeter-level positioning under incomplete observation conditions through tightly coupled fusion of five sensors and adaptive dynamic topology adjustment, providing uncertainty information for downstream safety control.

Description

Technical Field

[0001] This application relates to the field of sensor fusion and industrial automation technology, and in particular to a centimeter-level positioning method and system for heavy-duty hoisting equipment based on multi-sensor fusion. Background Technology

[0002] Heavy-duty lifting equipment requires high-precision positioning information to support autonomous operation and safety control when operating in ports, shipyards, and other similar settings. Existing positioning technologies have the following shortcomings: single sensors, such as GPS / real-time dynamic differential positioning, are prone to loss of lock in obstructed environments; lidar degrades in dusty or foggy environments; and visual sensors are greatly affected by changes in lighting conditions. Existing extended Kalman filter / unscented Kalman filter fusion methods are based on the assumption of complete observation, leading to a sharp drop in positioning accuracy when sensors intermittently fail. Furthermore, general positioning methods are not optimized for the high inertia and slow dynamic characteristics of heavy-duty lifting scenarios.

[0003] In existing patent literature, CN121717273A discloses a crane positioning system that uses a rotary encoder, inertial measurement unit, and ultra-wideband three-source sensors, and fuses the positioning data using Kalman filtering. However, this method relies on the assumption of complete observation, and its positioning accuracy drops sharply when the sensors fail intermittently. Furthermore, it does not output a covariance matrix, preventing downstream safety control modules from utilizing the uncertainty information. CN117889849A discloses a sensor fusion factor map positioning method for indoor mobile robots, but this method is geared towards indoor mobile robots and does not include BeiDou real-time dynamic differential positioning or millimeter-wave radar, lacking a centimeter-level absolute positioning reference. CN120831111A discloses an intelligent and precise positioning system and method for UAVs based on multi-source information fusion, but this method is geared towards UAV scenarios and lacks an adaptive factor weight adjustment mechanism for sensor degradation. None of the existing technologies propose a tightly coupled fusion positioning scheme for multi-source heterogeneous sensors specifically for heavy-duty lifting scenarios. Summary of the Invention

[0004] To facilitate centimeter-level positioning of heavy-duty hoisting equipment under incomplete observation conditions and to provide positioning uncertainty information to downstream safety control modules, this application provides a centimeter-level positioning method and system for heavy-duty hoisting equipment based on multi-sensor fusion.

[0005] Firstly, this application provides a centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion, employing the following technical solution: A centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion, comprising: The system collects raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment. These multiple sensors include BeiDou RTK, lidar, millimeter-wave radar, high-definition cameras, and inertial measurement units. The raw observation data from the multi-source sensors are processed in a spatiotemporal synchronization manner to generate fused observation data with a unified spatiotemporal reference. Based on the fused observation data, an initial factor map is constructed with the device pose as the core state variable. The device pose includes three-dimensional position, three-dimensional attitude, velocity, and sensor bias. The quality indicators of the raw observation data of each sensor are monitored in real time. When any quality indicator is lower than the corresponding availability threshold, the weight of the corresponding factor node in the initial factor graph is dynamically adjusted or the factor node is temporarily removed to generate a dynamic topological factor graph. The dynamic topology factor graph is iteratively optimized using an incremental smoothing optimization algorithm. Each time a new frame of fused observation data arrives, only the state variables in the dynamic topology factor graph affected by the corresponding frame of fused observation data are updated, and the optimized device pose estimate and corresponding covariance matrix are output. The device pose estimation and covariance matrix are output to the downstream safety control module for autonomous operation and safety control of heavy-duty hoisting equipment.

[0006] Optionally, before collecting raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment, the following may also be included: A preset scanning path is executed in the operating area of ​​the heavy-duty hoisting equipment, and three-dimensional point cloud data is collected in multiple cycles using LiDAR, while image sequences in multiple cycles are collected using a high-definition camera. Based on the three-dimensional point cloud data and image sequence, a map is constructed to generate a priori point cloud map covering the entire work area. The point cloud density of the priori point cloud map is not lower than a preset density threshold. Acquire the real-time point cloud frame collected by the current lidar, and perform iterative nearest point registration between the real-time point cloud frame and the prior point cloud map to calculate the absolute pose transformation of the real-time point cloud frame relative to the prior point cloud map. Based on the absolute pose transformation, generate prior map constraint factors; The prior map constraint factors are added to the initial factor map as additional absolute position constraint nodes; When the BeiDou RTK positioning status is out of lock and the inter-frame matching score of the lidar is lower than the preset matching score threshold, the weight of the prior map constraint factor is increased to a preset multiple higher than the weight of all other factor nodes, and the device pose estimation is output based on the prior map constraint factor as the main positioning basis.

[0007] Optionally, the step of performing spatiotemporal synchronization processing on the raw observation data from the multi-source sensors to generate fused observation data with a unified spatiotemporal reference includes: Using the internal clock of the inertial measurement unit as the primary time reference, it receives time stamp data from BeiDou RTK, lidar, millimeter-wave radar, and high-definition cameras. Based on the time deviation between the timestamp data and the internal clock, the time delay of each sensor relative to the inertial measurement unit is calculated. For sensors with a delay of less than one sampling period, a hardware trigger signal is used to force alignment of their data acquisition time to generate hardware-synchronized observation data. For sensors with a delay greater than or equal to one sampling period, acquire two consecutive historical observation data of the sensor, perform linear interpolation calculation based on the time delay, and generate interpolated synchronous observation data. The hardware synchronous observation data and the interpolated synchronous observation data are organized according to a unified time axis to form a synchronous observation frame; Obtain a pre-stored multi-sensor extrinsic parameter calibration file, which includes coordinate transformation matrices from lidar to camera, lidar to inertial measurement unit, and camera to inertial measurement unit; Using the coordinate transformation matrix, the observation data of all sensors in the synchronous observation frame are transformed into the coordinate system of the inertial measurement unit, generating fused observation data with a unified spatiotemporal reference.

[0008] Optionally, constructing an initial factor graph with device pose as the core state variable based on the fused observation data includes: Extract fused observation data from multiple consecutive time points to construct a sequence of state nodes for device pose. The state nodes at each time point include a 3D position node, a 3D attitude node, a velocity node, and an inertial measurement unit offset node. Acquire inertial measurement unit pre-integration data between adjacent time points, and construct inertial constraint factors between adjacent state nodes based on the inertial measurement unit pre-integration data; Extract point cloud data from a single frame of lidar, perform inter-frame matching between the point cloud data and the point cloud data from the previous moment, and construct an inter-frame matching factor for lidar based on the relative pose transformation obtained from the matching. Extract image data from a single frame of a high-definition camera and extract feature points from the image data. When there are matching feature points in two consecutive frames, construct a visual reprojection factor based on the reprojection error of the feature points. When the positioning state of BeiDou RTK is a fixed solution, the absolute position coordinates output by BeiDou RTK are extracted, and an absolute position factor is constructed based on the difference between the absolute position coordinates and the three-dimensional position node of the current state node. Extract the target radial velocity data output by the millimeter-wave radar, and construct the Doppler velocity factor based on the Doppler effect deviation between the target radial velocity data and the velocity node of the current state node; The inertial constraint factor, lidar inter-frame matching factor, visual reprojection factor, absolute position factor, and Doppler velocity factor are used as factor nodes and connected to the state node sequence to form an initial factor graph.

[0009] Optionally, the real-time monitoring of the quality indicators of the raw observation data from each sensor, and dynamically adjusting the weight of the corresponding factor node in the initial factor graph or temporarily removing the factor node when any quality indicator falls below the corresponding availability threshold, to generate a dynamic topological factor graph includes: Obtain the positioning status flag bit output by Beidou RTK, the positioning status flag bit includes fixed solution status, floating-point solution status and unlock status; When the positioning status flag changes from a fixed solution state to a non-fixed solution state and the duration exceeds a preset duration threshold, the quality index of BeiDou RTK is determined to be lower than the availability threshold, and the absolute position factor node is removed from the initial factor graph. The point cloud matching score output by the lidar during inter-frame matching is obtained, and the point cloud matching score represents the degree of overlap between the point cloud of the current frame and the previous frame. When the point cloud matching score is lower than the preset matching score threshold, it is determined that the quality index of the lidar is lower than the availability threshold, and the weight of the lidar inter-frame matching factor in the initial factor map is reduced to the preset ratio range of the original weight. Obtain the number of valid feature points extracted from the current frame image of the high-definition camera; When the number of effective feature points is lower than the preset feature point number threshold, it is determined that the quality index of the high-definition camera is lower than the availability threshold, and the visual reprojection factor node is temporarily removed from the initial factor map. When the sensor quality index corresponding to the removed absolute position factor node, the reduced weight of the LiDAR inter-frame matching factor, or the temporarily removed visual reprojection factor node recovers to above the availability threshold, the corresponding factor node will be added back to the factor graph and its original weight will be restored. A dynamic topological factor graph is generated based on the adjustment status of the factor nodes.

[0010] Optionally, when the BeiDou RTK lock-on failure, the lidar point cloud matching score is lower than a preset matching score threshold, and the number of effective feature points of the high-definition camera is lower than a preset feature point number threshold occur simultaneously, the following additional steps are also included: The availability of the inertial measurement unit is checked. If the inertial measurement unit is working properly, the historical state nodes within the most recent preset time period are extracted. Based on the historical state nodes and the pre-integrated data of the inertial measurement unit, the predicted state nodes are generated through a pure inertial recursive method. The predicted state node is added to the dynamic topology factor graph as a temporary absolute constraint factor, replacing the removed absolute position factor node, lidar inter-frame matching factor node, and visual reprojection factor node. When any removed or deweighted sensor recovers to above the availability threshold, the pure inertial recursion is immediately stopped, the temporary absolute constraint factor is removed, and the corresponding sensor factor node is restored.

[0011] Optionally, the incremental smoothing optimization algorithm is used to iteratively optimize the dynamic topology factor graph. Each time a new frame of fused observation data arrives, only the state variables in the dynamic topology factor graph affected by the corresponding frame of fused observation data are updated. The optimized device pose estimate and corresponding covariance matrix are output, including: Upon receiving a newly arrived frame of fused observation data, add a new state node and associated factor node corresponding to the corresponding frame data to the dynamic topology factor graph; Identify the first-layer neighboring state nodes in the dynamic topology factor graph that have a direct connection relationship with the new state node, and the second-layer neighboring state nodes that have a connection relationship with the first-layer neighboring state nodes. Mark the new state node, the first-level neighboring state nodes, the second-level neighboring state nodes, and all factor nodes between the new state node and the first-level neighboring state nodes, and between the first-level neighboring state nodes and the second-level neighboring state nodes, as subgraph regions to be updated; Nonlinear least squares optimization is performed on all state variables within the subgraph region to be updated, while keeping the state variables outside the subgraph region unchanged. After optimization, the updated values ​​of the new state nodes and their corresponding first-layer neighboring state nodes in the subgraph region to be updated are extracted and used as the device pose estimate at the current moment. The numerical transfer relationship between the corresponding new state node and the first layer neighboring state node in the optimization solution process is extracted and used as the covariance matrix of the device pose estimation. Output the device pose estimate and covariance matrix.

[0012] Optionally, before performing nonlinear least squares optimization on all state variables within the subgraph region to be updated, the following steps are also included: Calculate the observation residual for each factor node in the subgraph region to be updated, where the observation residual is the difference between the predicted and actual observation values ​​of the factor node. Based on the dimensions of the factor nodes, determine the degrees of freedom parameters for the chi-square test and set the preset reliability threshold; Based on the aforementioned degrees of freedom parameters and the preset confidence threshold, the corresponding chi-square threshold is obtained by querying the chi-square distribution table. The Mahalanobis distance squared is calculated by dividing the observed residual of each factor node by the covariance matrix of the corresponding factor node, and then the Mahalanobis distance squared is compared with the chi-square threshold. When the squared Mahalanobis distance value is greater than the chi-square threshold, the observation data corresponding to the factor node is determined to be an abnormal observation, and the factor node is marked as an abnormal factor node. Temporarily remove the abnormal factor nodes from the subgraph region to be updated, and then perform nonlinear least squares optimization on the subgraph region to be updated after removal. After the optimization solution is completed, check whether the observation residuals of the abnormal factor nodes have decreased to below the chi-square threshold; If the abnormal factor node has been reduced to below the chi-square threshold, then the abnormal factor node is added back to the subgraph region to be updated and optimized again. If the value is not reduced below the chi-square threshold, the removal status will remain and an abnormal alarm signal will be output.

[0013] Secondly, this application also discloses a centimeter-level positioning system for heavy-duty hoisting equipment based on multi-sensor fusion, which adopts the following technical solution: A centimeter-level positioning system for heavy-duty hoisting equipment based on multi-sensor fusion, comprising: The data acquisition module is used to collect raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment. The multiple sensors include a Beidou RTK receiver, a lidar, a millimeter-wave radar, a high-definition camera, and an inertial measurement unit. The spatiotemporal synchronization module is used to perform spatiotemporal synchronization processing on the raw observation data from the multi-source sensors to generate fused observation data with a unified spatiotemporal reference. The factor graph construction module is used to construct an initial factor graph with device pose as the core state variable based on the fused observation data. The device pose includes three-dimensional position, three-dimensional attitude, velocity, and sensor bias. The dynamic topology adjustment module is used to monitor the quality indicators of the raw observation data of each sensor in real time. When any quality indicator is lower than the corresponding availability threshold, the weight of the corresponding factor node in the initial factor graph is dynamically adjusted or the factor node is temporarily removed to generate a dynamic topology factor graph. The incremental optimization engine is used to iteratively optimize the dynamic topology factor graph using an incremental smoothing optimization algorithm. Each time a new frame of fused observation data arrives, it only updates the state variables in the dynamic topology factor graph that are affected by the corresponding frame of fused observation data, and outputs the optimized device pose estimate and the corresponding covariance matrix. The positioning output interface is used to output the device pose estimation and covariance matrix to the downstream safety control module for use in the autonomous operation and safety control of the heavy-duty hoisting equipment.

[0014] In summary, this application includes the following beneficial technical effects: By collecting raw observation data from five sensors—BeiDou RTK, LiDAR, millimeter-wave radar, high-definition camera, and inertial measurement unit—and performing spatiotemporal synchronization processing, a factor graph with equipment pose as the core state variable is constructed. This allows for full utilization of the complementary characteristics of multi-source heterogeneous sensors under a unified spatiotemporal reference, achieving centimeter-level positioning accuracy. Furthermore, by real-time monitoring of the quality indicators of each sensor and dynamically adjusting the weights of factor nodes or temporarily removing unreliable nodes, the factor graph topology can adapt to sensor degradation environments. This overcomes the shortcomings of traditional fusion methods that rely on the assumption of complete observations, maintaining positioning consistency even under incomplete observation conditions such as BeiDou RTK lock loss, LiDAR degradation, or insufficient visual features. Incremental smoothing optimization algorithms are further employed to update only the state variables affected by new observation data, significantly reducing computational complexity and meeting the real-time control requirements of heavy-duty lifting equipment. Finally, the equipment pose estimate and its covariance matrix are output, enabling the downstream safety control module to simultaneously obtain positioning results and uncertainty information. This allows for timely implementation of safety measures such as deceleration or shutdown when positioning accuracy deteriorates, effectively ensuring the autonomous operation safety of heavy-duty lifting equipment in complex working environments such as ports and shipyards. Attached Figure Description

[0015] Figure 1 This is a main flowchart of a centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to an embodiment of this application; Figure 2 This is a flowchart illustrating the steps involved in generating fused observation data with a unified spatiotemporal reference. Figure 3 It is a flowchart of the steps to construct an initial factor graph with device pose as the core state variable; Figure 4 This is a flowchart of the steps involved in generating a dynamic topology factor graph; Figure 5 This is a flowchart showing the steps to output the optimized device pose estimate and the corresponding covariance matrix; Figure 6 This is a block diagram of a centimeter-level positioning system for heavy-duty hoisting equipment based on multi-sensor fusion, according to an embodiment of this application.

[0016] Explanation of reference numerals in the attached figures: 1. Data acquisition module; 2. Spatiotemporal synchronization module; 3. Factor graph construction module; 4. Dynamic topology adjustment module; 5. Incremental optimization module; 6. Positioning output module. Detailed Implementation

[0017] In the first aspect, this application discloses a centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion.

[0018] Reference Figure 1 A centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion, comprising steps S101 to S106: Step S101: Collect raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment.

[0019] Specifically, in operational scenarios such as ports and shipyards, heavy-duty lifting equipment is equipped with multi-source sensor arrays. The BeiDou real-time dynamic differential positioning module receives BeiDou satellite signals and uses ground reference stations for differential correction, outputting centimeter-level absolute position coordinates with a sampling frequency of 10 Hz. LiDAR acquires three-dimensional point cloud structure information of the surrounding environment by emitting laser beams and measuring the time of flight of the reflected light; each frame of the point cloud contains thousands to tens of thousands of spatial point coordinates, with a sampling frequency of 10 Hz. Millimeter-wave radar measures the radial velocity of a target relative to the radar by emitting millimeter-wave electromagnetic waves and receiving the echoes, with a sampling frequency of 20 Hz. High-definition cameras acquire environmental images through optical lenses and image sensors, providing texture and feature information, with a sampling frequency of 30 Hz. The inertial measurement unit integrates accelerometers and gyroscopes, measuring the linear acceleration of the device in three orthogonal directions and the angular velocity about three orthogonal axes, respectively; its internal clock has high short-term stability, with a sampling frequency of 100 Hz. Raw observation data refers to the unprocessed data directly output by each sensor after it is independently collected at its own sampling frequency.

[0020] Step S102: Perform spatiotemporal synchronization processing on the raw observation data from multiple sources to generate fused observation data with a unified spatiotemporal reference.

[0021] Specifically, due to differences in time references, sampling frequencies, and installation locations among the sensors, directly using the raw observation data can lead to fusion biases. Therefore, spatiotemporal synchronization processing is necessary. This process includes two stages: time synchronization and spatial synchronization. Time synchronization uses the internal clock of the inertial measurement unit (IMU) as the primary time reference, aligning the observation data from each sensor to a unified time axis through hardware triggering or linear interpolation. Spatial synchronization uses a pre-calibrated multi-sensor extrinsic parameter calibration file to convert the observation data from all sensors to the IMU coordinate system, ensuring consistent coordinate representation for observations of the same spatial point from different sensors. After spatiotemporal synchronization processing, fused observation data with a unified spatiotemporal reference is generated.

[0022] Step S103: Based on the fused observation data, construct an initial factor graph with device pose as the core state variable.

[0023] Specifically, fused observation data from multiple consecutive time points are extracted to construct a sequence of state nodes for device pose. Each state node at any given time point includes a 3D position node, a 3D attitude node, a velocity node, and an inertial measurement unit (IMU) bias node. Based on the fused observation data, various constraint factors are constructed, including inertial constraint factors, lidar inter-frame matching factors, visual reprojection factors, absolute position factors, and Doppler velocity factors. These factors are then connected as factor nodes to the state node sequence to form an initial factor graph.

[0024] Step S104: Monitor the quality indicators of the raw observation data of each sensor in real time. When any quality indicator is lower than the corresponding availability threshold, dynamically adjust the weight of the corresponding factor node in the initial factor graph or temporarily remove the factor node to generate a dynamic topology factor graph.

[0025] Specifically, during heavy-duty hoisting operations, environmental changes can lead to performance degradation in different sensors, necessitating real-time monitoring of data quality from each sensor. For BeiDou real-time dynamic differential positioning, its positioning status flags are monitored, including fixed solution status, floating-point solution status, and unlocked status. If a fixed solution is lost for more than one second, it is deemed unusable. For LiDAR, the point cloud matching score output during inter-frame matching is monitored, ranging from 0 to 1. A point cloud matching score below 0.6 indicates a degraded state. For high-definition cameras, the number of valid feature points extracted from the current frame image is monitored. If the number of valid feature points is less than 50, it is deemed unusable. When any quality indicator falls below the corresponding availability threshold, the corresponding factor node in the initial factor graph undergoes weight adjustment or temporary removal. When the sensor quality indicator recovers, the corresponding factor node is re-added to the factor graph and its original weights are restored. Based on the adjusted status of the factor nodes, a dynamic topology factor graph is generated.

[0026] Step S105: The dynamic topology factor graph is iteratively optimized using an incremental smoothing optimization algorithm. Each time a new frame of fused observation data arrives, only the state variables in the dynamic topology factor graph affected by the corresponding frame of fused observation data are updated, and the optimized device pose estimate and corresponding covariance matrix are output.

[0027] Specifically, the iSAM2 incremental smoothing optimization algorithm is used to iteratively optimize the dynamic topology factor graph. Upon receiving a new frame of fused observation data, corresponding new state nodes and associated factor nodes are added to the dynamic topology factor graph. Neighboring state nodes associated with the new state nodes are identified, and the subgraph region to be updated is delineated. Nonlinear least squares optimization is performed on the state variables within the subgraph region to be updated, while keeping the state variables outside the region unchanged. After optimization, the updated values ​​of the new state nodes are extracted as the device pose estimate for the current time step, and the corresponding numerical transitivity is extracted as the covariance matrix. The device pose estimate and covariance matrix are then output.

[0028] Step S106: Output the equipment pose estimation and covariance matrix to the downstream safety control module for autonomous operation and safety control of heavy-duty hoisting equipment.

[0029] Specifically, the downstream safety control module includes a collision avoidance protection module, an automatic deviation correction module, and a path planning module. The collision avoidance protection module uses equipment pose estimation to determine the distance between the hoisting equipment and surrounding obstacles. The automatic deviation correction module uses equipment pose estimation to detect and adjust the deviation between the equipment's trajectory and the preset trajectory. The path planning module uses equipment pose estimation and the covariance matrix to generate the optimal operating path. Outputting the covariance matrix allows downstream modules to perceive the reliability of the positioning results, thereby making more reasonable control decisions.

[0030] In one embodiment of this invention, before collecting raw observation data from multiple sensors during the operation of the heavy-duty hoisting equipment, steps S201 to S206 are further included: Step S201: Execute the preset scanning path in the working area of ​​the heavy-duty hoisting equipment, use LiDAR to collect three-dimensional point cloud data in multiple rotations, and use a high-definition camera to collect image sequences in multiple rotations.

[0031] Specifically, the operating area of ​​heavy-duty hoisting equipment refers to the site area covered by the equipment during daily operation, including storage yards, loading and unloading areas, and areas along the tracks. The preset scanning path refers to the pre-planned movement trajectory of the LiDAR and high-definition cameras. This path needs to cover the entire operating area, ensuring that every location within the operating area is scanned at least once. Multi-loop acquisition refers to performing multiple complete scans along the preset scanning path. Each complete scan is called a loop. Multi-loop acquisition is used to reduce occlusion and blind spots in a single acquisition, improving the uniformity of point cloud density.

[0032] Step S202: Based on the 3D point cloud data and image sequence, construct a map to generate a priori point cloud map covering the entire work area.

[0033] Specifically, map building employs an offline simultaneous localization and mapping (SLT / Map) method. "Offline" means the map building process is completed before the device is officially operational and is not part of the real-time localization process. SLT / Map is a technique that simultaneously estimates the sensor's own motion trajectory and builds an environmental map. By analyzing the relative motion relationships between consecutive frames, loop closure detection, and global optimization to eliminate accumulated errors, a globally consistent map is gradually formed. 3D point cloud data provides geometric information about the environment, while image sequences provide textural information; their fusion allows for the construction of a richer and more accurate map. The prior point cloud map refers to a pre-built 3D point cloud map that exists before real-time localization. This map has a point density of at least 100 points per square meter to ensure sufficient detail to support centimeter-level localization.

[0034] Step S203: Obtain the real-time point cloud frame acquired by the current lidar, and perform iterative nearest point registration between the real-time point cloud frame and the prior point cloud map, and calculate the absolute pose transformation of the real-time point cloud frame relative to the prior point cloud map.

[0035] Specifically, a real-time point cloud frame refers to a frame of 3D point cloud data acquired by the LiDAR at the current moment, reflecting the structural information of the environment surrounding the device. Iterative nearest point registration is a point cloud alignment algorithm that iteratively finds the nearest point pair and calculates the optimal rotation and translation until convergence. Point cloud registration aligns the current LiDAR frame with the prior point cloud map, providing absolute positional constraints. The rotation matrix is ​​an orthogonal matrix describing the rotation transformation in 3D space, and the translation vector is a vector describing the translation transformation in 3D space; together, they are called pose transformation. Absolute pose transformation refers to the pose transformation of the real-time point cloud frame relative to the prior point cloud map coordinate system; "absolute" means that the pose transformation is relative to a fixed prior map coordinate system.

[0036] Step S204: Generate prior map constraint factors based on absolute pose transformation.

[0037] Specifically, the absolute pose transformation includes three rotational degrees of freedom and three translational degrees of freedom, for a total of six degrees of freedom. The prior map constraint factor is a factor node that adds this absolute pose transformation as an observation to the factor graph. Its constraint object is the device pose state node at the current moment. The role of the prior map constraint factor is to provide an absolute position reference to the factor graph. Its accuracy is unaffected by satellite signal quality and remains stable even indoors or in occluded environments, thus compensating for the shortcomings of BeiDou real-time dynamic differential positioning in occluded environments.

[0038] Step S205: Add the prior map constraint factors to the initial factor map as additional absolute position constraint nodes.

[0039] Specifically, adding prior map constraint factors to the initial factor map means adding a new factor node outside the existing set of factor nodes. This new node is connected to the device pose state node at the current moment. Absolute position constraint nodes are factor nodes that provide the absolute position information of the device in the global coordinate system. In contrast, relative constraint nodes only provide the relative relationships between state nodes and do not provide absolute references. Within the operational area where the prior point cloud map has been constructed, the prior map constraint factors and the BeiDou real-time dynamic differential positioning factors complement each other.

[0040] Step S206: When the positioning status of BeiDou RTK is out of lock and the inter-frame matching score of the lidar is lower than the preset matching score threshold, the weight of the prior map constraint factor is increased to a preset multiple higher than the weight of all other factor nodes, and the device pose estimation is output based on the prior map constraint factor as the main positioning basis.

[0041] Specifically, BeiDou real-time dynamic differential positioning loss of lock refers to the receiver's inability to calculate a fixed or floating-point solution, thus failing to provide reliable position information. A lidar inter-frame matching score below 0.6 indicates lidar degradation in environments such as dust, rain, and fog. In this extreme case of dual degradation, the prior map constraint factor, unaffected by satellite signals and dusty environments, provides a relatively reliable positioning basis. The preset multiplier refers to the amplification factor of the prior map constraint factor relative to the weights of other factor nodes; for example, setting it to 5 times increases the weight, making the optimization result more inclined to satisfy the constraint. Using the prior map constraint factor as the primary positioning basis means that, during the optimization process, the influence of the prior map constraint factor on the final pose estimation exceeds the sum of all other factor nodes.

[0042] Reference Figure 2 In one embodiment of this invention, the process of performing spatiotemporal synchronization processing on the raw observation data from multiple sensors to generate fused observation data with a unified spatiotemporal reference includes steps S301 to S307: Step S301: Using the internal clock of the inertial measurement unit as the primary time reference, receive the timestamp data from BeiDou RTK, lidar, millimeter-wave radar, and high-definition camera.

[0043] Specifically, the BeiDou real-time dynamic differential positioning module records a timestamp each time it calculates a positioning result; the lidar records a timestamp when it completes a point cloud scan; the millimeter-wave radar records a timestamp when it outputs target velocity data; and the high-definition camera records a timestamp when it exposes and acquires an image. These timestamps are time stamp information output by each sensor along with the observation data, typically in units of seconds, including both integer and fractional seconds. The inertial measurement unit's internal clock serves as the primary time reference, used for comparison with the timestamps of other sensors.

[0044] Step S302: Calculate the time delay of each sensor relative to the inertial measurement unit based on the time deviation between the timestamp data and the internal clock.

[0045] Specifically, the timestamp data output by each sensor is compared with the reading of the internal clock of the inertial measurement unit at the same moment to obtain the time deviation between the two. Due to differences in the clock start-up time of each sensor, variations in crystal oscillator frequencies, and delays during data acquisition and transmission, this time deviation is not a fixed value. By statistically analyzing the deviation data at multiple consecutive moments, a pattern of time deviation variation over time is fitted, thereby calculating the current time delay of each sensor relative to the inertial measurement unit. The time delay can be positive or negative; a positive value indicates that the sensor's data acquisition time is later than the inertial measurement unit's time reference, while a negative value indicates that it is earlier than the time reference.

[0046] Step S303: For sensors with a delay of less than one sampling period, a hardware trigger signal is used to force alignment of their data acquisition time to generate hardware-synchronized observation data.

[0047] Specifically, the sampling period refers to the time interval between two consecutive data acquisitions by the sensor, and its value is equal to the reciprocal of the sampling frequency. A delay of less than one sampling period means that the sensor's time deviation does not exceed its own sampling interval, allowing for high-precision synchronization at the microsecond level through hardware. The inertial measurement unit generates a pulse signal and sends it to other sensors via a synchronization signal line. This pulse signal is in square wave form, with the rising or falling edge serving as the trigger moment. Upon receiving the trigger signal, other sensors immediately perform data acquisition, thereby forcibly aligning the data acquisition time to the trigger signal edge, generating hardware-synchronized observation data.

[0048] Step S304: For sensors with a delay greater than or equal to one sampling period, acquire two consecutive historical observation data of the sensor, perform linear interpolation calculation based on the time delay, and generate interpolated synchronous observation data.

[0049] Specifically, a delay greater than or equal to one sampling period means that the sensor's time deviation has exceeded a data acquisition interval, making hardware triggering unsuitable. The two most recent observations are extracted from the sensor's historical data buffer and denoted as the previous and next observations, respectively. The time interval between the two observations equals the sensor's sampling period. The proportion of the time difference between the target alignment moment and the previous observation moment to the sampling period is calculated. The two observations are then weighted according to this proportion, i.e., multiplied by the difference between the two observations and added to the previous observation, to obtain the interpolated observation value for the target alignment moment. The time point corresponding to this interpolated observation value is aligned with the inertial measurement unit's time reference.

[0050] Step S305: Organize the hardware synchronous observation data and the interpolated synchronous observation data according to a unified time axis to form a synchronous observation frame.

[0051] Specifically, using the inertial measurement unit's time reference, a unified time axis is established, divided into fixed time intervals starting from the initial moment, with each time point corresponding to a discrete moment. Hardware-synchronized observation data and interpolated-synchronized observation data are then assigned to their respective aligned time points on the unified time axis, and observation data from all sensors are collected at each moment. A synchronized observation frame refers to the collection of all sensor observation data corresponding to a specific moment on the unified time axis.

[0052] Step S306: Obtain the pre-stored multi-sensor extrinsic parameter calibration file, which includes the coordinate transformation matrix from LiDAR to the camera, from LiDAR to the inertial measurement unit, and from the camera to the inertial measurement unit.

[0053] Specifically, the multi-sensor extrinsic parameter calibration file records the parameters of the relative position and attitude relationships between each sensor. Extrinsic parameters, or external parameters, describe the spatial transformation relationships between different sensor coordinate systems. Calibration employs a checkerboard-based multi-sensor joint calibration method. The calibration error from LiDAR to the camera is less than 1 pixel, and the calibration rotation error from LiDAR to the inertial measurement unit (IMU) is less than 0.1 degrees, and the translation error is less than 2 centimeters. Calibration is performed quarterly or after each sensor disassembly and reassembly. The coordinate transformation matrix from LiDAR to the camera transforms the point coordinates in the LiDAR coordinate system to the camera coordinate system; the coordinate transformation matrix from LiDAR to the IMU transforms the point coordinates in the LiDAR coordinate system to the IMU coordinate system; and the coordinate transformation matrix from the camera to the IMU transforms the point coordinates in the camera coordinate system to the IMU coordinate system.

[0054] Step S307: Using the coordinate transformation matrix, the observation data of all sensors in the synchronous observation frame are transformed into the coordinate system of the inertial measurement unit to generate fused observation data with a unified spatiotemporal reference.

[0055] Specifically, for point cloud coordinates observed by lidar, they are multiplied by the coordinate transformation matrix from lidar to inertial measurement unit (IMU) to obtain the three-dimensional coordinates of the point in the IMU coordinate system. For image feature points observed by high-definition cameras, they are first converted from pixel coordinates to three-dimensional coordinates in the camera coordinate system, and then multiplied by the coordinate transformation matrix from camera to IMU to obtain the three-dimensional coordinates of the point in the IMU coordinate system. For targets observed by millimeter-wave radar, their polar coordinates are converted to rectangular coordinates and then multiplied by the coordinate transformation matrix from millimeter-wave radar to IMU. For latitude, longitude, and altitude output by BeiDou real-time dynamic differential positioning, they are first converted to coordinates in the geocentric-ground-fixed coordinate system using a coordinate transformation formula, and then converted to coordinates in the IMU coordinate system. The data of the IMU itself does not need to be converted. After all sensor observation data is converted, fused observation data with a unified spatiotemporal reference is generated.

[0056] Reference Figure 3 In one embodiment of this example, constructing an initial factor graph with device pose as the core state variable based on fused observation data includes steps S401 to S407: Step S401: Extract fused observation data from multiple consecutive time points and construct a sequence of state nodes for device pose. The state nodes at each time point include a 3D position node, a 3D attitude node, a velocity node, and an inertial measurement unit offset node.

[0057] Specifically, from the fused observation data output by the spatiotemporal synchronization module, all data frames from the start time to the current time are extracted in chronological order, with each frame corresponding to a discrete time point. A state node is created for each time point, and all state nodes are connected in chronological order to form a sequence. Each state node contains multiple variable components: the 3D position node contains X-axis, Y-axis, and Z-axis coordinates; the 3D attitude node contains roll, pitch, and yaw angles; the velocity node contains the linear velocity components of the device in the three axes; and the inertial measurement unit bias node contains accelerometer bias and gyroscope bias, which change slowly over time and need to be estimated in real time as state variables.

[0058] Step S402: Obtain the inertial measurement unit pre-integration data between adjacent time points, and construct the inertial constraint factor between adjacent state nodes based on the inertial measurement unit pre-integration data.

[0059] Specifically, the acceleration and angular velocity output by the inertial measurement unit (IMU) are integrated between corresponding moments of two adjacent state nodes. Using the earlier state as initial conditions, all acceleration and angular velocity measurements within the time interval are substituted into the kinematic equations, and the relative position change, relative velocity change, and relative attitude change are calculated using numerical integration methods. These calculation results are called IMU pre-integration data. The advantage of pre-integration is that it decouples the IMU's integration calculation from state optimization, avoiding recalculation in each optimization iteration. The inertial constraint factor uses the pre-integration data as observations, connecting two adjacent state nodes, requiring that the pose and velocity of the later state node satisfy the relative motion relationship determined by the pre-integration.

[0060] Step S403: Extract the point cloud data of a single frame of lidar, perform inter-frame matching between the point cloud data and the point cloud data of the previous moment, and construct the lidar inter-frame matching factor based on the relative pose transformation obtained from the matching.

[0061] Specifically, lidar point cloud data is extracted from the fused observation data at the current moment, and lidar point cloud data from the previous moment is extracted from historical data. The current frame point cloud is registered with the previous frame point cloud, and the optimal rotation matrix and translation vector between the two sets of point clouds are found through an iterative nearest-point algorithm. This rotation matrix and translation vector describe the relative motion of the device from the previous moment to the current moment, which is called the relative pose transformation. The lidar inter-frame matching factor uses this relative pose transformation as the observation value to connect the current state node and the previous state node, requiring that the relative pose transformation between these two state nodes is consistent with the relative pose transformation obtained from the inter-frame matching.

[0062] Step S404: Extract image data from a single frame of a high-definition camera and extract feature points from the image data. When there are matching feature points in two consecutive frames, construct a visual reprojection factor based on the reprojection error of the feature points.

[0063] Specifically, high-resolution camera image data is extracted from the fused observation data at the current moment, while high-resolution camera image data from the previous moment is extracted from historical data. Feature points are extracted from both frames, identifying the locations of pixels with significant texture features such as corner points and edge points, and a descriptor vector is calculated for each feature point. By comparing the similarity of the descriptor vectors, feature point pairs corresponding to the same 3D spatial point in the two frames are identified; these pairs are called matched feature points. For each pair of matched feature points, the feature points from the previous frame are projected onto the current frame image plane based on the estimated pose of the current state node, and the Euclidean distance between the projected point and the actual feature point is calculated; this distance is called the reprojection error. The visual reprojection factor uses the sum of squared reprojection errors of all matched feature points as the residual term, connecting two adjacent state nodes, aiming to minimize the reprojection error.

[0064] Step S405: When the positioning state of BeiDou RTK is a fixed solution, extract the absolute position coordinates output by BeiDou RTK, and construct the absolute position factor based on the difference between the absolute position coordinates and the three-dimensional position node of the current state node.

[0065] Specifically, check the positioning status flags output by the BeiDou real-time dynamic differential positioning system. When the flag indicates a fixed solution, it means the current positioning accuracy has reached the centimeter level and the data is usable. Extract the longitude, latitude, and altitude values ​​output by the BeiDou real-time dynamic differential positioning system, and convert these geographic coordinates into X, Y, and Z coordinates in the inertial measurement unit coordinate system using coordinate transformation formulas. Compare the transformed coordinates with the current estimated values ​​of the 3D position nodes in the current state node, and calculate the differences between the two in the X, Y, and Z directions. The absolute position factor uses this difference as a residual term, connecting only the current state node, requiring that the difference between the values ​​of the 3D position nodes and the absolute position coordinates output by the BeiDou real-time dynamic differential positioning system be as small as possible.

[0066] Step S406: Extract the target radial velocity data output by the millimeter-wave radar, and construct the Doppler velocity factor based on the Doppler effect deviation between the target radial velocity data and the velocity node of the current state node.

[0067] Specifically, the radial velocity data of the target output from the millimeter-wave radar is extracted from the fused observation data at the current moment. Radial velocity refers to the velocity component of the target relative to the radar line of sight. Positive radial velocity indicates that the target is moving away from the radar, while negative radial velocity indicates that the target is moving towards the radar. The millimeter-wave radar uses the Doppler effect to measure the radial velocity of the target relative to the radar. Based on the estimated value of the velocity node in the current state node, the velocity projection of the device in the radar line-of-sight direction is calculated, and this projection value is used as the predicted radial velocity value. The difference between the actual radial velocity measured by the millimeter-wave radar and the predicted radial velocity value is calculated; this difference is called the Doppler effect bias. The Doppler velocity factor uses this bias as a residual term and connects only to the state node at the current moment, requiring that the value of the velocity node minimizes the deviation between the predicted radial velocity value and the actual radial velocity value measured by the millimeter-wave radar.

[0068] Step S407: Connect the inertial constraint factor, lidar inter-frame matching factor, visual reprojection factor, absolute position factor, and Doppler velocity factor as factor nodes with the state node sequence to form an initial factor map.

[0069] Specifically, in the factor graph structure, each state node in the state node sequence represents the device pose at a given moment. Inertial constraint factors connect adjacent state nodes, LiDAR inter-frame matching factors connect adjacent state nodes, visual reprojection factors connect adjacent state nodes, absolute position factors connect to their respective single state nodes, and Doppler velocity factors connect to their respective single state nodes. Associating all factor nodes with state nodes according to these relationships forms a bipartite graph structure composed of state nodes and factor nodes, i.e., the initial factor graph.

[0070] Reference Figure 4 In one embodiment of this invention, the quality index of the raw observation data of each sensor is monitored in real time. When any quality index is lower than the corresponding availability threshold, the weight of the corresponding factor node in the initial factor graph is dynamically adjusted or the factor node is temporarily removed to generate a dynamic topology factor graph, including steps S501 to S508: Step S501: Obtain the positioning status flag bit output by Beidou RTK. The positioning status flag bit includes fixed solution status, floating-point solution status and unlock status.

[0071] Specifically, the BeiDou real-time dynamic differential positioning receiver outputs a status indicator code each time it calculates a positioning result; this code is the positioning status flag. A fixed resolution status indicates that the receiver has successfully fixed the carrier phase integer ambiguity, with positioning errors typically at the centimeter level; this is the highest accuracy positioning status. A floating-point resolution status indicates that the receiver failed to fix the integer ambiguity and instead used floating-point estimation; positioning errors typically at the decimeter level. A lost-lock status indicates that the receiver cannot track sufficient satellite signals or that differential correction data is interrupted, preventing the output of a valid positioning result.

[0072] Step S502: When the positioning status flag changes from a fixed solution state to a non-fixed solution state and the duration exceeds a preset duration threshold, the quality index of BeiDou RTK is determined to be lower than the availability threshold, and the absolute position factor node is removed from the initial factor graph.

[0073] Specifically, non-fixed solution states refer to all states other than fixed solution states, including floating-point solution states and unlocked states. When the positioning status flag changes from a fixed solution to a floating-point solution or unlocked state, timing begins. If the duration of this non-fixed solution state exceeds a preset duration threshold, the quality index of BeiDou real-time dynamic differential positioning is determined to be below the availability threshold. In this embodiment, the preset duration threshold can be set to 1 second. The 1-second duration threshold is used to avoid frequent switching caused by momentary signal obstruction or brief interference. After the determination is passed, the absolute position factor node is found in the initial factor graph and deleted from the factor graph so that the factor node no longer participates in subsequent optimization solutions. When a fixed solution is lost, the factor graph automatically switches from the BeiDou-LiDAR-Vision fusion mode to the LiDAR-Vision-Inertial Measurement Unit fusion mode.

[0074] Step S503: Obtain the point cloud matching score output by the lidar when performing inter-frame matching. The point cloud matching score represents the degree of overlap between the point clouds of the current frame and the previous frame.

[0075] Specifically, during the inter-frame matching process of the LiDAR, the current frame's point cloud is transformed into the coordinate system of the previous frame's point cloud according to the pose transformation obtained from the matching. For each transformed point, the nearest point in the previous frame's point cloud is found, and the Euclidean distance between the two is calculated. After statistical analysis of the distances of all points, a quantitative score is obtained, which is the point cloud matching score. The point cloud matching score typically ranges from 0 to 1. The closer the score is to 1, the higher the degree of overlap between the two sets of point clouds, and the more reliable the matching result. When the point cloud matching score is lower than a preset matching score threshold, it indicates that the LiDAR is in a degraded state.

[0076] Step S504: When the point cloud matching score is lower than the preset matching score threshold, it is determined that the quality index of the LiDAR is lower than the availability threshold, and the weight of the LiDAR inter-frame matching factor in the initial factor map is reduced to the preset ratio range of the original weight.

[0077] Specifically, the point cloud matching score is compared with a preset matching score threshold. In this embodiment, the preset matching score threshold can be set to 0.6. If the point cloud matching score is lower than 0.6, the quality index of the lidar is determined to be lower than the availability threshold, indicating that the lidar may be in a degraded environment such as dust, rain, or fog. The lidar inter-frame matching factor node is found in the initial factor graph, and its weight coefficient is reduced to a preset proportion range of the original weight. In this embodiment, the preset proportion range can be set to 30% to 50%. The weight is reduced rather than completely removed because lidar data in a degraded state still provides some constraint information, but its reliability is low. After the weight is reduced, the influence of this factor node in optimizing the objective function is correspondingly reduced. When the lidar point cloud matching score rises back above 0.6, its original weight is automatically restored.

[0078] Step S505: Obtain the number of valid feature points extracted from the current frame image of the high-definition camera.

[0079] Specifically, feature point extraction is performed on the current frame image from the high-definition camera to identify the locations of pixels with significant texture features, such as corner points and edge points. All extracted feature points undergo quality screening, discarding those located at image edges or with unstable textures. The remaining feature points that can be successfully extracted and stably matched across consecutive frames are called valid feature points. The total number of valid feature points is then counted; this value represents the number of valid feature points in the current frame image.

[0080] Step S506: When the number of effective feature points is lower than the preset feature point number threshold, the quality index of the high-definition camera is determined to be lower than the availability threshold, and the visual reprojection factor node is temporarily removed from the initial factor map.

[0081] Specifically, the number of effective feature points is compared with a preset feature point threshold. In this embodiment, the preset feature point threshold can be set to 50. If the number of effective feature points is less than 50, the quality index of the high-definition camera is determined to be lower than the usability threshold, indicating that the current environment may have conditions unfavorable to visual positioning, such as excessively strong or dark lighting, or monotonous texture. The visual reprojection factor node is found in the initial factor graph and temporarily removed from the factor graph. Temporary removal means that the factor node is deleted at the current moment, but can be re-added after the sensor quality recovers, which is different from permanent deletion. When the number of effective feature points of the high-definition camera rises back to more than 50, the visual reprojection factor node is re-added to the factor graph.

[0082] Step S507: When the sensor quality index corresponding to the removed absolute position factor node, the reduced weight of the LiDAR inter-frame matching factor, or the temporarily removed visual reprojection factor node recovers to above the availability threshold, the corresponding factor node is added back to the factor graph and its original weight is restored.

[0083] Specifically, the quality indicators of each sensor are continuously monitored. For BeiDou real-time dynamic differential positioning, the quality indicator is considered to have recovered when the positioning status flag returns to a fixed solution and remains stable. For LiDAR, the quality indicator is considered to have recovered when the point cloud matching score rises above 0.6. For HD cameras, the quality indicator is considered to have recovered when the number of effective feature points rises above 50. After the quality indicator is recovered, previously removed factor nodes are added back to the factor graph, or the weights of factor nodes whose weights were reduced are restored to their original values.

[0084] Step S508: Generate a dynamic topological factor graph based on the adjustment status of the factor nodes.

[0085] Specifically, the adjustment status of factor nodes includes the following information: which absolute position factor nodes have been removed, which LiDAR inter-frame matching factor nodes have had their weights reduced and their current weight values, which visual reprojection factor nodes have been temporarily removed, and which factor nodes have been reinstated. Based on these adjustment statuses, the topology of the initial factor graph is modified to generate a dynamic topology factor graph. The topology of this factor graph dynamically changes with sensor availability, achieving adaptive fusion under incomplete observation conditions.

[0086] In one embodiment of this invention, when the BeiDou RTK lock-on failure, the lidar point cloud matching score is lower than a preset matching score threshold, and the number of effective feature points of the high-definition camera is lower than a preset feature point number threshold occur simultaneously, steps S601 to S604 are further included: Step S601: Detect the availability of the inertial measurement unit. If the inertial measurement unit is working properly, extract the historical state nodes within the most recent preset time period.

[0087] Specifically, when the BeiDou real-time dynamic differential positioning, LiDAR, and HD camera all fail simultaneously—that is, when the BeiDou real-time dynamic differential positioning is in a non-fixed solution state, the LiDAR point cloud matching score is below 0.6, and the number of effective feature points of the HD camera is below 50—the inertial measurement unit (IMU) is checked for proper operation. This includes checking the IMU's power supply status, communication status, and whether the output data is within a reasonable range. If the IMU is functioning normally, historical state nodes within the most recent preset time period are extracted from the state node sequence of the factor graph. The estimated values ​​of these state nodes have been previously optimized and deemed reliable. In this embodiment, a preset time period can be set to 5 seconds.

[0088] Step S602: Based on the historical state nodes and the pre-integrated data of the inertial measurement unit, generate the predicted state nodes through a pure inertial recursive method.

[0089] Specifically, using the latest extracted historical state node as the initial state, all inertial measurement unit (IMU) pre-integrated data from the corresponding time of that historical state node to the current time are acquired. Substituting the initial state into the kinematic equations, each pre-integrated data point is applied sequentially in chronological order to progressively calculate the position, velocity, and attitude changes within each tiny time step. The changes from each step are accumulated to obtain the total change from the initial time to the current time. This total change is then superimposed onto the pose of the historical state node to obtain the predicted pose for the current time. This predicted pose constitutes the predicted state node. Pure inertial recursion refers to a recursive method that relies solely on IMU data and does not depend on any other sensors.

[0090] Step S603: Add the predicted state node as a temporary absolute constraint factor to the dynamic topology factor graph, replacing the removed absolute position factor node, LiDAR inter-frame matching factor node, and visual reprojection factor node.

[0091] Specifically, a special type of temporary absolute constraint factor is created, using the pose of the predicted state node as the observation value of this factor, which is then connected to the state node at the current time step. This temporary absolute constraint factor is added to the dynamic topology factor graph, while ensuring that the removed absolute position factor nodes, LiDAR inter-frame matching factor nodes, and visual reprojection factor nodes remain in the removed state. The temporary absolute constraint factor serves to provide constraints in the dynamic topology factor graph, preventing the factor graph from degenerating due to a lack of constraints. The weight of this constraint factor is set lower than that of normal sensor factors to reflect the uncertainty of pure inertial recursion.

[0092] Step S604: When any sensor that has been removed or had its weight reduced recovers to above the availability threshold, immediately stop pure inertial recursion, remove the temporary absolute constraint factor, and restore the corresponding sensor factor node.

[0093] Specifically, the quality indicators of each sensor are continuously monitored. Once any one of the following conditions is met: BeiDou real-time dynamic differential positioning regains a fixed solution, the lidar point cloud matching score rises above 0.6, or the number of effective feature points from the high-definition camera rises above 50, the pure inertial recursion process is immediately terminated. The temporary absolute constraint factor added in step S603 is removed from the dynamic topology factor graph. Simultaneously, the factor nodes corresponding to the sensors whose quality has been restored are re-added to the dynamic topology factor graph. If the weight of this factor node was previously reduced, its original weight is restored. The system switches back to normal factor graph fusion mode, and the newly restored sensor data begins to participate in the optimization solution.

[0094] Reference Figure 5 In one embodiment of this invention, an incremental smoothing optimization algorithm is used to iteratively optimize the dynamic topology factor graph. Each time a new frame of fused observation data arrives, only the state variables in the dynamic topology factor graph affected by the corresponding frame of fused observation data are updated, and the optimized device pose estimate and corresponding covariance matrix are output, including steps S701 to S707: Step S701: Receive a newly arrived frame of fused observation data, and add a new state node and associated factor node corresponding to the corresponding frame data in the dynamic topology factor graph.

[0095] Specifically, when the spatiotemporal synchronization module outputs a new frame of fused observation data, this frame of data is received. A new state node is created at the end of the state node sequence in the dynamic topology factor graph; this node represents the device pose at the current moment. Based on the sensor observations included in the newly arrived fused observation data, corresponding associated factor nodes are constructed, including absolute position factor nodes, LiDAR inter-frame matching factor nodes, visual reprojection factor nodes, millimeter-wave radar Doppler factor nodes, and any possible prior map constraint factor nodes. These newly created factor nodes are then connected to the new state node and any possible historical state nodes according to their respective connection rules.

[0096] Step S702: Identify the first-layer neighboring state nodes in the dynamic topology factor graph that have a direct connection with the new state node, and the second-layer neighboring state nodes that have a connection with the first-layer neighboring state nodes.

[0097] Specifically, in the dynamic topology factor graph, starting from the new state node, the system searches for directly connected state nodes along the factor nodes. Since the inertial constraint factor, the lidar inter-frame matching factor, and the visual reprojection factor all connect the new state node to the state node of the previous time step, the state node of the previous time step is the first-layer neighboring state node with a direct connection to the new state node. Then, starting from the first-layer neighboring state node, the system searches for directly connected historical state nodes along the factor nodes. Since the inertial constraint factor connects the first-layer neighboring state node to the state node of the time step before that, the state node of the time step before that is the second-layer neighboring state node with a connection to the first-layer neighboring state node.

[0098] Step S703: Mark the new state node, the first-layer neighboring state nodes, the second-layer neighboring state nodes, and all factor nodes between the new state node and the first-layer neighboring state nodes, and between the first-layer neighboring state nodes and the second-layer neighboring state nodes, as subgraph regions to be updated.

[0099] Specifically, a local region is delineated in the dynamic topological factor graph. This region includes three types of state nodes: new state nodes, first-layer neighboring state nodes, and second-layer neighboring state nodes. It also includes all factor nodes between the new state node and its first-layer neighboring state nodes, as well as all factor nodes between the first-layer and second-layer neighboring state nodes. This local region is called the subgraph region to be updated. Since connections only exist between adjacent state nodes in the factor graph, the influence of a new state node is limited to its two adjacent layers of state nodes and does not spread to earlier historical state nodes. Therefore, the subgraph region to be updated is much smaller than the entire factor graph, which is the fundamental reason why the iSAM2 algorithm can achieve efficient computation.

[0100] Step S704: Perform nonlinear least squares optimization on all state variables within the subgraph region to be updated, while keeping the state variables outside the subgraph region unchanged.

[0101] Specifically, the variables of all state nodes within the subgraph region to be updated are used as optimization variables, and the sum of squared errors of all factor nodes within the region is used as the objective function. The variable values ​​of state nodes outside the subgraph region to be updated are kept unchanged and not optimized. A nonlinear least squares optimization algorithm is used to solve the objective function, and the state variable values ​​that minimize the objective function are found through iterative calculation. Each time a new observation arrives, only the affected state nodes are updated, with a data processing delay of no more than 80 milliseconds, meeting real-time control requirements.

[0102] Step S705: After the optimization solution is completed, extract the updated values ​​of the new state nodes and the corresponding first-layer neighboring state nodes in the subgraph region to be updated, as the device pose estimate at the current moment.

[0103] Specifically, after the optimization solution converges, the updated values ​​of 3D position, 3D attitude, velocity, and inertial measurement unit bias contained in the new state node are extracted from the optimization results. These values ​​are used as the device pose estimate at the current moment. Simultaneously, the updated values ​​of the first-layer neighboring state nodes are extracted to update the historical trajectory records. The device pose estimate includes X-coordinate, Y-coordinate, Z-coordinate, roll angle, pitch angle, yaw angle, linear velocity components in the three directions, and sensor bias values.

[0104] Step S706: Extract the numerical transmission relationship between the corresponding new state node and the first layer neighboring state node in the optimization solution process, as the covariance matrix for device pose estimation.

[0105] Specifically, in the nonlinear least squares optimization process, the Hessian matrix of the objective function reflects the mutual constraints between state variables, and the inverse of the Hessian matrix reflects the uncertainty propagation characteristics of the state variable estimates. A sub-matrix block corresponding to the new state node and its first-layer neighboring state nodes is extracted from this inverse matrix. The size of this sub-matrix block is equal to the sum of the dimensions of these state nodes, with its diagonal elements representing the variance of the corresponding state component and its off-diagonal elements representing the covariance between different state components. This sub-matrix block is then used as the covariance matrix for device pose estimation.

[0106] Step S707: Output the device pose estimate and covariance matrix.

[0107] Specifically, the device pose estimate obtained in step S705 and the covariance matrix obtained in step S706 are organized into an output data format. The device pose estimate contains pose information for 6 degrees of freedom, and the covariance matrix is ​​a 6x6 square matrix that reflects the uncertainty level of the pose estimate. These two parts of data are sent to the downstream safety control module through a communication interface.

[0108] In one embodiment of this invention, before performing nonlinear least squares optimization on all state variables within the subgraph region to be updated, steps S801 to S809 are further included: Step S801: Calculate the observation residual for each factor node in the subgraph region to be updated. The observation residual is the difference between the predicted observation and the actual observation of the factor node.

[0109] Specifically, for each factor node in the subgraph region to be updated, the predicted observation value corresponding to that factor node is calculated based on the estimated value of the current state variable. The predicted observation value is then subtracted from the actual observation value output by the sensor in each dimension of the observation space to obtain a vector, which is the observation residual.

[0110] Step S802: Determine the degrees of freedom parameters for the chi-square test based on the dimensions of the factor nodes, and set the preset reliability threshold.

[0111] Specifically, the dimension of a factor node refers to the number of observations included in that factor. For example, the absolute position factor includes position observations in the X, Y, and Z directions, with a dimension of 3; the visual reprojection factor includes pixel coordinate observations in the U and V directions, with a dimension of 2; and the lidar inter-frame matching factor includes 3 rotational degrees of freedom and 3 translational degrees of freedom, with a dimension of 6. The dimension value of the factor node is used as the degree of freedom parameter for the chi-square test. In this embodiment, the preset confidence threshold is set to 95%, meaning that under normal circumstances, 95% of the observation residuals should fall within the normal range.

[0112] Step S803: Based on the degree of freedom parameter and the preset confidence threshold, query the chi-square distribution table to obtain the corresponding chi-square threshold.

[0113] Specifically, the chi-square distribution is a probability distribution used to describe the sum of squares distribution of a standard normal random variable. The chi-square distribution table provides critical values ​​for different degrees of freedom and confidence thresholds; these critical values ​​are called the chi-square threshold. For example, when the degrees of freedom are 6 and the confidence threshold is 95%, the chi-square threshold is 12.59. The chi-square threshold is the boundary value for determining whether an observation is anomalous.

[0114] Step S804: Divide the observed residual of each factor node by the covariance matrix of the corresponding factor node and calculate the squared Mahalanobis distance. Compare the squared Mahalanobis distance with the chi-square threshold.

[0115] Specifically, Mahalanobis distance is a distance metric that considers the correlation between variables. It normalizes each dimension using a covariance matrix, eliminating the effects of scale differences and correlations between different dimensions. After calculating the squared Mahalanobis distance, it is compared with a chi-square threshold; a larger squared Mahalanobis distance indicates a greater deviation of the observation from the prediction.

[0116] Step S805: When the squared Mahalanobis distance value is greater than the chi-square threshold, the observation data corresponding to the factor node is determined to be an abnormal observation, and the factor node is marked as an abnormal factor node.

[0117] Specifically, when the squared Mahalanobis distance exceeds the chi-square threshold, it means that at a 95% confidence level, the probability of this observation residual occurring is less than 5%, which is a low-probability event. Therefore, this observation is judged as an anomalous observation. Anomalous observations may be caused by transient sensor failures, communication packet loss, environmental interference, etc. After marking this factor node as an anomalous factor node, subsequent processing will take special measures for this node to prevent it from negatively impacting state estimation.

[0118] Step S806: Temporarily remove abnormal factor nodes from the subgraph region to be updated, and perform nonlinear least squares optimization on the removed subgraph region to be updated.

[0119] Specifically, if anomalous observations are involved in optimization, they will negatively impact state estimation because the optimization algorithm attempts to satisfy these unreasonable constraints, causing the estimated state value to deviate from the true value. Temporarily removing anomalous factor nodes from the subgraph region to be updated leaves only factor nodes considered normal. The optimization solution is no longer affected by anomalous observations, resulting in a more reliable state estimation. Temporary removal rather than permanent deletion is chosen because the observed data may only be temporarily anomalous and may return to normal in subsequent moments.

[0120] Step S807: After the optimization solution is completed, check whether the observation residuals of the outlier factor nodes have decreased to below the chi-square threshold.

[0121] Specifically, after optimization, the state variables change, and the observation residuals of the anomalous factor nodes are recalculated. Since the state estimates have been updated, the observation residuals previously identified as anomalous may decrease with the change in state. Checking whether the residuals have decreased below the chi-square threshold is used to determine whether the observation data is truly anomalous or merely a false anomaly due to inaccurate state estimation. If the residuals have decreased below the threshold, it indicates that the previous state estimation bias led to a large residual, and the observation data itself is not anomalous.

[0122] Step S808: If the value has been reduced to below the chi-square threshold, the abnormal factor node is added back to the subgraph region to be updated and optimized again.

[0123] Specifically, if the observation residuals of the anomalous factor nodes decrease to below the chi-square threshold, it indicates that the previously marked observation data are reasonable under the new state estimation, and the misjudgment may have been caused by bias in the initial state estimation. Re-adding the factor node to the subgraph region to be updated and then optimizing again can fully utilize the observation data and improve the accuracy of the state estimation. The optimization process is the same as in step S804, except that only the state variables within the subgraph region to be updated are solved.

[0124] Step S809: If the value is not reduced below the chi-square threshold, maintain the removal state and output an abnormal alarm signal.

[0125] Specifically, if the observation residual of an anomalous factor node consistently exceeds the chi-square threshold, it indicates that the observation data is indeed abnormal and not caused by state estimation bias. This anomalous factor node remains in the removed state and does not participate in subsequent optimization. Simultaneously, an anomaly alarm signal is output. This signal is used to prompt operators to check the operating status of the corresponding sensor, or to allow downstream safety control modules to take appropriate fault-tolerant measures, such as reducing the control decision weight that relies on this sensor.

[0126] Secondly, this application also discloses a centimeter-level positioning system for heavy-duty hoisting equipment based on multi-sensor fusion.

[0127] Reference Figure 6 A centimeter-level positioning system for heavy-duty hoisting equipment based on multi-sensor fusion, comprising: The data acquisition module is used to collect raw observation data during the operation of heavy-duty hoisting equipment. The multi-source sensors include a Beidou RTK receiver, lidar, millimeter-wave radar, high-definition camera, and inertial measurement unit. The spatiotemporal synchronization module is used to perform spatiotemporal synchronization processing on the raw observation data from multiple sources to generate fused observation data with a unified spatiotemporal reference. The factor graph construction module is used to construct an initial factor graph with device pose as the core state variable based on fused observation data. Device pose includes three-dimensional position, three-dimensional attitude, velocity and sensor bias. The dynamic topology adjustment module is used to monitor the quality indicators of the raw observation data of each sensor in real time. When any quality indicator is lower than the corresponding availability threshold, the weight of the corresponding factor node in the initial factor graph is dynamically adjusted or the factor node is temporarily removed to generate a dynamic topology factor graph. The incremental optimization module is used to iteratively optimize the dynamic topology factor graph using an incremental smoothing optimization algorithm. Each time a new frame of fused observation data arrives, it only updates the state variables in the dynamic topology factor graph that are affected by the corresponding frame of fused observation data, and outputs the optimized device pose estimate and the corresponding covariance matrix. The positioning output module is used to output the equipment pose estimation and covariance matrix to the downstream safety control module for the autonomous operation and safety control of heavy-duty hoisting equipment.

[0128] The above are all preferred embodiments of this application, and are not intended to limit the scope of protection of this application. Therefore, all equivalent changes made in accordance with the structure, shape and principle of this application should be covered within the scope of protection of this application.

Claims

1. A multi-sensor fusion-based centimeter-level positioning method for heavy lifting equipment, characterized in that, include: The system collects raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment. These multiple sensors include BeiDou RTK, lidar, millimeter-wave radar, high-definition cameras, and inertial measurement units. The raw observation data from the multi-source sensors are processed in a spatiotemporal synchronization manner to generate fused observation data with a unified spatiotemporal reference. Based on the fused observation data, an initial factor map is constructed with the device pose as the core state variable. The device pose includes three-dimensional position, three-dimensional attitude, velocity, and sensor bias. The quality indicators of the raw observation data of each sensor are monitored in real time. When any quality indicator is lower than the corresponding availability threshold, the weight of the corresponding factor node in the initial factor graph is dynamically adjusted or the factor node is temporarily removed to generate a dynamic topological factor graph. The dynamic topology factor graph is iteratively optimized using an incremental smoothing optimization algorithm. Each time a new frame of fused observation data arrives, only the state variables in the dynamic topology factor graph affected by the corresponding frame of fused observation data are updated, and the optimized device pose estimate and corresponding covariance matrix are output. The device pose estimation and covariance matrix are output to the downstream safety control module for autonomous operation and safety control of heavy-duty hoisting equipment.

2. The multi-sensor fusion based centimeter-level positioning method for heavy lifting equipment according to claim 1, characterized in that, Before collecting raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment, the following steps are also included: A preset scanning path is executed in the operating area of ​​the heavy-duty hoisting equipment, and three-dimensional point cloud data is collected in multiple cycles using LiDAR, while image sequences in multiple cycles are collected using a high-definition camera. Based on the three-dimensional point cloud data and image sequence, a map is constructed to generate a priori point cloud map covering the entire work area. The point cloud density of the priori point cloud map is not lower than a preset density threshold. Acquire the real-time point cloud frame collected by the current lidar, and perform iterative nearest point registration between the real-time point cloud frame and the prior point cloud map to calculate the absolute pose transformation of the real-time point cloud frame relative to the prior point cloud map. Based on the absolute pose transformation, generate prior map constraint factors; The prior map constraint factors are added to the initial factor map as additional absolute position constraint nodes; When the BeiDou RTK positioning status is out of lock and the inter-frame matching score of the lidar is lower than the preset matching score threshold, the weight of the prior map constraint factor is increased to a preset multiple higher than the weight of all other factor nodes, and the device pose estimation is output based on the prior map constraint factor as the main positioning basis.

3. The centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to claim 1, characterized in that, The process of performing spatiotemporal synchronization processing on the raw observation data from the multi-source sensors to generate fused observation data with a unified spatiotemporal reference includes: Using the internal clock of the inertial measurement unit as the primary time reference, it receives time stamp data from BeiDou RTK, lidar, millimeter-wave radar, and high-definition cameras. Based on the time deviation between the timestamp data and the internal clock, the time delay of each sensor relative to the inertial measurement unit is calculated. For sensors with a delay of less than one sampling period, a hardware trigger signal is used to force alignment of their data acquisition time to generate hardware-synchronized observation data. For sensors with a delay greater than or equal to one sampling period, acquire two consecutive historical observation data of the sensor, perform linear interpolation calculation based on the time delay, and generate interpolated synchronous observation data. The hardware synchronous observation data and the interpolated synchronous observation data are organized according to a unified time axis to form a synchronous observation frame; Obtain a pre-stored multi-sensor extrinsic parameter calibration file, which includes coordinate transformation matrices from lidar to camera, lidar to inertial measurement unit, and camera to inertial measurement unit; Using the coordinate transformation matrix, the observation data of all sensors in the synchronous observation frame are transformed into the coordinate system of the inertial measurement unit, generating fused observation data with a unified spatiotemporal reference.

4. The centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to claim 1, characterized in that, The construction of an initial factor graph with device pose as the core state variable based on the fused observation data includes: Extract fused observation data from multiple consecutive time points to construct a sequence of state nodes for device pose. The state nodes at each time point include a 3D position node, a 3D attitude node, a velocity node, and an inertial measurement unit offset node. Acquire inertial measurement unit pre-integration data between adjacent time points, and construct inertial constraint factors between adjacent state nodes based on the inertial measurement unit pre-integration data; Extract point cloud data from a single frame of lidar, perform inter-frame matching between the point cloud data and the point cloud data from the previous moment, and construct an inter-frame matching factor for lidar based on the relative pose transformation obtained from the matching. Extract image data from a single frame of a high-definition camera and extract feature points from the image data. When there are matching feature points in two consecutive frames, construct a visual reprojection factor based on the reprojection error of the feature points. When the positioning state of BeiDou RTK is a fixed solution, the absolute position coordinates output by BeiDou RTK are extracted, and an absolute position factor is constructed based on the difference between the absolute position coordinates and the three-dimensional position node of the current state node. Extract the target radial velocity data output by the millimeter-wave radar, and construct the Doppler velocity factor based on the Doppler effect deviation between the target radial velocity data and the velocity node of the current state node; The inertial constraint factor, lidar inter-frame matching factor, visual reprojection factor, absolute position factor, and Doppler velocity factor are used as factor nodes and connected to the state node sequence to form an initial factor graph.

5. The centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to claim 1, characterized in that, The process of real-time monitoring of the quality indicators of the raw observation data from each sensor, and dynamically adjusting the weight of the corresponding factor node in the initial factor graph or temporarily removing the factor node when any quality indicator falls below the corresponding availability threshold, to generate a dynamic topological factor graph includes: Obtain the positioning status flag bit output by Beidou RTK, the positioning status flag bit includes fixed solution status, floating-point solution status and unlock status; When the positioning status flag changes from a fixed solution state to a non-fixed solution state and the duration exceeds a preset duration threshold, the quality index of BeiDou RTK is determined to be lower than the availability threshold, and the absolute position factor node is removed from the initial factor graph. The point cloud matching score output by the lidar during inter-frame matching is obtained, and the point cloud matching score represents the degree of overlap between the point cloud of the current frame and the previous frame. When the point cloud matching score is lower than the preset matching score threshold, it is determined that the quality index of the lidar is lower than the availability threshold, and the weight of the lidar inter-frame matching factor in the initial factor map is reduced to the preset ratio range of the original weight. Obtain the number of valid feature points extracted from the current frame image of the high-definition camera; When the number of effective feature points is lower than the preset feature point number threshold, it is determined that the quality index of the high-definition camera is lower than the availability threshold, and the visual reprojection factor node is temporarily removed from the initial factor map. When the sensor quality index corresponding to the removed absolute position factor node, the reduced weight of the LiDAR inter-frame matching factor, or the temporarily removed visual reprojection factor node recovers to above the availability threshold, the corresponding factor node will be added back to the factor graph and its original weight will be restored. A dynamic topological factor graph is generated based on the adjustment status of the factor nodes.

6. The centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to claim 5, characterized in that, When simultaneously, the following occurs: BeiDou RTK lock failure, LiDAR point cloud matching score below a preset matching score threshold, and the number of effective feature points from the high-definition camera below a preset feature point number threshold: The availability of the inertial measurement unit is checked. If the inertial measurement unit is working properly, the historical state nodes within the most recent preset time period are extracted. Based on the historical state nodes and the pre-integrated data of the inertial measurement unit, the predicted state nodes are generated through a pure inertial recursive method. The predicted state node is added to the dynamic topology factor graph as a temporary absolute constraint factor, replacing the removed absolute position factor node, lidar inter-frame matching factor node, and visual reprojection factor node. When any removed or deweighted sensor recovers to above the availability threshold, the pure inertial recursion is immediately stopped, the temporary absolute constraint factor is removed, and the corresponding sensor factor node is restored.

7. The centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to claim 1, characterized in that, The incremental smoothing optimization algorithm is used to iteratively optimize the dynamic topology factor graph. Each time a new frame of fused observation data arrives, only the state variables in the dynamic topology factor graph affected by the corresponding frame of fused observation data are updated. The optimized device pose estimate and corresponding covariance matrix are output, including: Upon receiving a newly arrived frame of fused observation data, add a new state node and associated factor node corresponding to the corresponding frame data to the dynamic topology factor graph; Identify the first-layer neighboring state nodes in the dynamic topology factor graph that have a direct connection relationship with the new state node, and the second-layer neighboring state nodes that have a connection relationship with the first-layer neighboring state nodes. Mark the new state node, the first-level neighboring state nodes, the second-level neighboring state nodes, and all factor nodes between the new state node and the first-level neighboring state nodes, and between the first-level neighboring state nodes and the second-level neighboring state nodes, as subgraph regions to be updated; Nonlinear least squares optimization is performed on all state variables within the subgraph region to be updated, while keeping the state variables outside the subgraph region unchanged. After optimization, the updated values ​​of the new state nodes and their corresponding first-layer neighboring state nodes in the subgraph region to be updated are extracted and used as the device pose estimate at the current moment. The numerical transfer relationship between the corresponding new state node and the first layer neighboring state node in the optimization solution process is extracted and used as the covariance matrix of the device pose estimation. Output the device pose estimate and covariance matrix.

8. A centimeter-level positioning method for heavy-duty hoisting equipment based on multi-sensor fusion according to claim 7, characterized in that, Before performing nonlinear least squares optimization on all state variables within the region of the subgraph to be updated, the following steps are also included: Calculate the observation residual for each factor node in the subgraph region to be updated, where the observation residual is the difference between the predicted and actual observation values ​​of the factor node. Based on the dimensions of the factor nodes, determine the degrees of freedom parameters for the chi-square test and set the preset reliability threshold; Based on the aforementioned degrees of freedom parameters and the preset confidence threshold, the corresponding chi-square threshold is obtained by querying the chi-square distribution table. The Mahalanobis distance squared is calculated by dividing the observed residual of each factor node by the covariance matrix of the corresponding factor node, and then the Mahalanobis distance squared is compared with the chi-square threshold. When the squared Mahalanobis distance value is greater than the chi-square threshold, the observation data corresponding to the factor node is determined to be an abnormal observation, and the factor node is marked as an abnormal factor node. Temporarily remove the abnormal factor nodes from the subgraph region to be updated, and then perform nonlinear least squares optimization on the subgraph region to be updated after removal. After the optimization solution is completed, check whether the observation residuals of the abnormal factor nodes have decreased to below the chi-square threshold; If the abnormal factor node has been reduced to below the chi-square threshold, then the abnormal factor node is added back to the subgraph region to be updated and optimized again. If the value is not reduced below the chi-square threshold, the removal status will remain and an abnormal alarm signal will be output.

9. A centimeter-level positioning system for heavy-duty hoisting equipment based on multi-sensor fusion, characterized in that, include: The data acquisition module is used to collect raw observation data from multiple sensors during the operation of heavy-duty hoisting equipment. The multiple sensors include a Beidou RTK receiver, a lidar, a millimeter-wave radar, a high-definition camera, and an inertial measurement unit. The spatiotemporal synchronization module is used to perform spatiotemporal synchronization processing on the raw observation data from the multi-source sensors to generate fused observation data with a unified spatiotemporal reference. The factor graph construction module is used to construct an initial factor graph with device pose as the core state variable based on the fused observation data. The device pose includes three-dimensional position, three-dimensional attitude, velocity, and sensor bias. The dynamic topology adjustment module is used to monitor the quality indicators of the raw observation data of each sensor in real time. When any quality indicator is lower than the corresponding availability threshold, the weight of the corresponding factor node in the initial factor graph is dynamically adjusted or the factor node is temporarily removed to generate a dynamic topology factor graph. The incremental optimization module is used to iteratively optimize the dynamic topology factor graph using an incremental smoothing optimization algorithm. Each time a new frame of fused observation data arrives, it only updates the state variables in the dynamic topology factor graph that are affected by the corresponding frame of fused observation data, and outputs the optimized device pose estimate and the corresponding covariance matrix. The positioning output module is used to output the device pose estimation and covariance matrix to the downstream safety control module for use in the autonomous operation and safety control of the heavy-duty hoisting equipment.

Images