Locomotive multi-source fusion positioning method, device and system for private railway

Abstract

The application relates to the technical field of multi-source fusion positioning, and specifically discloses a locomotive multi-source fusion positioning method, device and system for a special railway, which comprises the following steps: acquiring multi-source positioning observation data of a locomotive, wherein the multi-source positioning observation data comprises interframe pose increments, interframe relative pose transformation and absolute position information; inputting the multi-source positioning observation data into an adaptive multi-source fusion filter; constructing a redundant observation constraint, and comparing a predicted state estimation of the locomotive with the redundant observation constraint to realize fault detection and confidence evaluation of a wheel speed inertial odometry and a laser odometry; determining the confidence of a global odometry, and adaptively adjusting the fusion weights of the odometries; and performing observation update correction on the predicted state estimation of the locomotive to obtain a final fusion positioning result of the locomotive. The locomotive multi-source fusion positioning method for the special railway can improve positioning precision.

Description

Technical Field

[0001] This invention relates to the field of multi-source fusion positioning technology, and in particular to a multi-source fusion positioning method, a multi-source fusion positioning device, and a multi-source fusion positioning system for locomotives used in dedicated railways. Background Technology

[0002] In the freight transportation sector, dedicated railway transportation has become a core mode of transport for large-volume freight due to its advantages such as large capacity, high efficiency, and strong stability. With the rapid iteration of autonomous driving technology, its application in the dedicated railway transportation industry is gradually deepening. Positioning technology, as the core support of locomotive automatic driving systems, directly determines the driving safety, operational accuracy, and running efficiency of automatic trains, and is a key core technology for realizing autonomous driving on dedicated railways.

[0003] Currently, positioning technology for dedicated railway automatic driving systems employs different positioning schemes depending on the scenario. In unobstructed operation scenarios with stable communication signals, real-time dynamic positioning (RTK) data corrected by differential data received from the Global Navigation Satellite System (GNSS) is typically used for positioning. However, the actual operating environment of dedicated railways is complex and variable, with numerous interference factors. Existing positioning technologies all have insurmountable technical defects and cannot meet the stringent requirements of dedicated railway automatic driving systems for positioning accuracy, stability, and real-time performance.

[0004] For GNSS-RTK positioning, its positioning accuracy is highly dependent on a stable communication link and real-time, stable reception of differential data. However, there are usually large loading and unloading machinery and other obstructions in the dedicated railway operation area, and the electromagnetic environment near the locomotive is complex and susceptible to electromagnetic interference from high-voltage transmission lines, radio transmitting equipment, etc., which can easily lead to communication signal attenuation and interruption. As a result, the GNSS receiver cannot receive differential data normally and cannot enter a stable fixed solution state. Ultimately, this leads to abnormal situations such as increased positioning deviation, positioning drift, or even positioning failure, which directly threaten the driving safety of automatic trains and reduce the efficiency of dedicated operations.

[0005] While the multi-line LiDAR SLAM positioning method used in the aforementioned occlusion scenarios can reduce reliance on satellite signals to some extent, in dedicated, densely occluded environments with complex operating conditions, LiDAR sensors are easily obstructed by stacked containers, machinery, or other obstacles. This leads to missing LiDAR point cloud data, hindering the effective extraction of environmental feature points and affecting map construction accuracy and positioning precision. Furthermore, in scenarios with prolonged occlusion and scarce environmental feature points, LiDAR odometry is prone to degradation, failing to construct effective point cloud maps. This results in a continuous accumulation of positioning errors, making it difficult to maintain stable positioning over long periods. Moreover, LiDAR SLAM positioning based on global maps relies on the global positioning of the point cloud map; larger map errors lead to larger positioning errors. In the large-scale, open environment of dedicated railways, global maps, due to their space requirements and sparse LiDAR point clouds, cannot meet the needs of continuous autonomous driving operations. Visual SLAM localization also faces similar technical bottlenecks. Its core relies on cameras to capture environmental images and extract image feature points to complete localization and map construction. However, the working environment of dedicated railways contains a large amount of dust and moisture, which easily adhere to the surface of camera lenses. At the same time, obstacles can also cause the images captured by the camera to be blurry and feature point extraction to fail. In addition, when there are weak texture areas or dynamic interference in the working scene, visual odometry is prone to degradation, resulting in a significant decrease in positioning accuracy and inability to guarantee real-time performance. It is also difficult to adapt to the core requirements of automatic driving of dedicated railways.

[0006] In summary, existing single or single-type combination positioning technologies all suffer from problems such as unstable positioning, insufficient accuracy, and weak anti-interference ability in actual operating scenarios such as complex obstructions and poor communication signals on dedicated railways.

[0007] Therefore, how to solve the problems of insufficient positioning accuracy, poor continuity, low reliability, and redundant and inefficient map construction in mixed operation scenarios with obstructions and large areas for unmanned dedicated railway locomotives has become a technical problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0008] This invention provides a multi-source fusion positioning method, a multi-source fusion positioning device, and a multi-source fusion positioning system for locomotives on dedicated railways. It aims to solve one of the problems existing in related technologies, such as insufficient positioning accuracy, poor continuity, low reliability, and redundant and inefficient map construction, in mixed operation scenarios with obstructions and large areas for unmanned locomotives on dedicated railways.

[0009] As a first aspect of the present invention, a multi-source fusion positioning method for locomotives on dedicated railways is provided, comprising:

[0010] The system acquires multi-source positioning observation data for the locomotive. This multi-source positioning observation data includes inter-frame pose increments, inter-frame relative pose transformations, and absolute position information. The inter-frame pose increments are obtained through wheel speed inertial odometers. The inter-frame relative pose transformations are obtained by laser odometers after distortion correction of point cloud data based on the inter-frame pose increments and matching them with a local map. The absolute position information is obtained through a global odometer module, which includes a GNSS-RTK odometer and a UWB odometer. The GNSS-RTK odometer provides global absolute pose information, and the UWB odometer provides local absolute position information.

[0011] The multi-source positioning observation data is input into an adaptive multi-source fusion filter to recursively predict the system state variables and obtain the predicted state estimate of the locomotive.

[0012] Redundant observation constraints are constructed based on the absolute position information of the global odometer module, and the predicted state estimate of the locomotive is compared with the redundant observation constraints to achieve fault detection and confidence assessment of wheel speed inertial odometer and laser odometer, and obtain the confidence assessment result of local odometer.

[0013] The confidence level of the global odometer is determined based on the operating status of the global odometer module, and the fusion weight of each odometer is adaptively adjusted based on the confidence level evaluation results of the local odometers and the confidence level of the global odometer.

[0014] The predicted state estimate of the locomotive is updated and corrected based on the multi-source positioning observation data and the fusion weights of each odometry to obtain the final fused positioning result of the locomotive. Specifically, when the current position of the locomotive is in an occluded environment, the locomotive's pose is updated based on the inter-frame relative pose transformation of the laser odometry and the local absolute position information of the UWB odometry. When the current position of the locomotive is in an unoccluded environment, the locomotive's pose is updated based on the global absolute pose information of the GNSS-RTK odometry.

[0015] Furthermore, the inter-frame pose increment is obtained through wheel speed inertial odometry, including:

[0016] Acquire inertial measurement unit (IMU) measurement data, and perform inter-frame pre-integration processing on the IMU measurement data to obtain inter-frame inertial measurement pre-integration results;

[0017] Acquire wheel speed meter measurement data and extract the forward speed observation value of the locomotive from the wheel speed meter measurement data;

[0018] The velocity observation equation is constructed based on the nonholonomic constraint model, and the constraints of the nonholonomic constraint model are determined. The corresponding velocity observation noise covariance matrix is ​​constructed based on the velocity observation equation under the constraints of the nonholonomic constraint model. The constraint of the nonholonomic constraint model is that the lateral velocity observation value and the axial velocity observation value of the locomotive are both set to zero.

[0019] The noise variance corresponding to the horizontal and upward zero velocities in the velocity observation noise covariance matrix is ​​adaptively adjusted according to the magnitude of the velocity observation residuals.

[0020] Using the pre-integrated results of the inter-frame inertial measurement as the state prediction, the velocity observation equation, the adaptively adjusted velocity observation noise covariance matrix, and the locomotive's forward velocity observation value are used as observation constraints to perform extended Kalman filtering state update, thereby obtaining the inter-frame pose increment.

[0021] Furthermore, the inter-frame relative pose transformation is obtained by laser odometry after performing distortion correction processing on the point cloud data based on the inter-frame pose increment and matching it with the local map, including:

[0022] Acquire multi-line lidar point cloud data, and calculate the locomotive pose change at each scanning point acquisition time by linear interpolation based on the inter-frame pose increment obtained from the wheel speed inertial odometer.

[0023] Based on the locomotive pose changes at each scanning point acquisition time, all scanning points are uniformly transformed to the same coordinate system at the same time to obtain the distortion-free point cloud.

[0024] Extract line feature point sets and surface feature point sets from the distortion-free point cloud to form the current feature point cloud;

[0025] The current frame feature point cloud is transformed to the previous frame coordinate system using the inter-frame pose transformation of the wheel speed inertial odometer as the initial estimate, and then matched with the feature point cloud of the previous frame to obtain the inter-frame relative pose transformation of the laser odometer. The inter-frame pose transformation of the wheel speed inertial odometer is obtained by the inter-frame pose increment transformation of the wheel speed inertial odometer.

[0026] The current feature point cloud is transformed to the world coordinate system based on the inter-frame relative pose transformation of the laser odometry to construct a local map;

[0027] Search for the corresponding point set of each feature point in the current feature point cloud in the local map, and iteratively match the corresponding point set with each feature point in the current frame feature point cloud to update the inter-frame relative pose transformation.

[0028] Furthermore, the absolute position information is obtained through a global odometer module, including:

[0029] The locomotive's GNSS-RTK odometer is acquired in the WGS84 coordinate system, along with its latitude, longitude, altitude, and angle information. This information is then converted to the UTM Cartesian coordinate system to obtain the locomotive's global absolute pose information, which includes the locomotive's absolute position and heading angle.

[0030] UWB base stations are calibrated to synchronize the location of UWB base stations with the location of GNSS-RTK odometry, and to obtain the absolute position information of each UWB base station in the world coordinate system.

[0031] The locomotive's local absolute position information is obtained by the UWB odometer based on the distance data between the UWB tag on the locomotive and each UWB base station. The local absolute position information of the locomotive includes the absolute position of the locomotive.

[0032] Further, the multi-source positioning observation data is input into an adaptive multi-source fusion filter to recursively predict the system state variables, thereby obtaining a predicted state estimate of the locomotive, including:

[0033] An adaptive multi-source fusion filter based on error state Kalman filtering is constructed, and system state variables are defined, including the locomotive's position, speed, attitude, and the zero bias of the acceleration of the inertial measurement unit and the zero bias of the gyroscope.

[0034] The error state equation is constructed based on the kinematic model of the locomotive, and the state covariance matrix and process noise matrix are initialized.

[0035] The multi-source positioning observation data is input into the adaptive multi-source fusion filter;

[0036] Based on the error state equation, the system state variables are recursively predicted using the inter-frame pose increment in the multi-source positioning observation data as the driving force, and the state covariance matrix and process noise matrix are synchronously recursively updated to obtain the predicted state estimate of the locomotive.

[0037] Furthermore, redundant observation constraints are constructed based on the absolute position information of the global odometer module, and the predicted state estimate of the locomotive is compared with the redundant observation constraints to achieve fault detection and confidence assessment of the wheel speed inertial odometer and laser odometer, obtaining the confidence assessment result of the local odometer, including:

[0038] Based on the absolute position information output by the global odometer module, the low-frequency absolute position information of the global odometer is interpolated onto the high-frequency timestamp of the local odometer through linear interpolation, resulting in a global redundant observation sequence that is aligned with the local odometer time.

[0039] The predicted state estimate of the locomotive is compared with the global redundant observation sequence to calculate the pose deviation of the wheel speed inertial odometer and the laser odometer.

[0040] When the pose deviation of the wheel speed inertial odometer and / or laser odometer exceeds a preset pose deviation threshold, it is determined that the corresponding local odometer is faulty, and the confidence level of the faulty local odometer is adaptively reduced according to the amplitude of the pose deviation to obtain the confidence level evaluation result of the local odometer.

[0041] Furthermore, the confidence level of the global odometer is determined based on the operating status of the global odometer module, and the fusion weights of each odometer are adaptively adjusted based on the confidence level evaluation results of the local odometers and the confidence level of the global odometer, including:

[0042] The confidence level of the GNSS-RTK odometry is determined based on its status and the number of satellites searched. Specifically, a high confidence level is set when the GNSS-RTK odometry status is a fixed solution, a medium confidence level is set when the GNSS-RTK odometry status is a floating-point solution, and an unreliable level is set when the GNSS-RTK odometry status is a single-point solution or the number of satellites searched is less than a preset number of satellites searched.

[0043] The confidence level of the UWB odometer is determined based on the number of UWB base station distance data received by the UWB odometer. When the number of UWB base station distance data received by the UWB odometer is less than a preset number of distance data, it is set as unreliable.

[0044] The occlusion status of the locomotive's current position is determined based on the confidence levels of the GNSS-RTK odometer and the UWB odometer.

[0045] When the GNSS-RTK odometer is set to untrusted and the UWB odometer is not untrusted, the locomotive's current position is determined to be an obstructed area.

[0046] When the GNSS-RTK odometer reading is high or medium confidence, the locomotive's current position is determined to be an unobstructed area;

[0047] The fusion weights of each odometer are adaptively adjusted based on the occlusion status of the locomotive's current position, the confidence evaluation results of the local odometer, and the confidence of the global odometer.

[0048] Further, based on the multi-source positioning observation data and the fusion weights of each odometer, the predicted state estimate of the locomotive is updated and corrected to obtain the final fused positioning result of the locomotive, including:

[0049] Available observation sources are determined based on the occlusion status of the locomotive's current position. Specifically, when the locomotive's current position is in an occluded environment, the inter-frame relative pose transformation of the laser odometry and the local absolute position information of the UWB odometry are determined as available observation sources. When the locomotive's current position is in an unoccluded environment, the global absolute pose information of the GNSS-RTK odometry is determined as available observation sources.

[0050] Construct the corresponding observation equations based on the available observation sources;

[0051] The observation noise covariance matrix corresponding to the observation equation is adjusted according to the fusion weight of each odometer;

[0052] The Kalman gain is calculated based on the observation equation, the adjusted observation noise covariance matrix, and the predicted state estimate of the locomotive.

[0053] The predicted state estimate of the locomotive is corrected and updated based on the Kalman gain to obtain the final fusion positioning result of the locomotive.

[0054] As another aspect of the present invention, a multi-source fusion positioning device for locomotives in dedicated railways is provided, for implementing the multi-source fusion positioning method for locomotives in dedicated railways described above, wherein the device includes:

[0055] The data acquisition module is used to acquire multi-source positioning observation data of the locomotive. The multi-source positioning observation data includes inter-frame pose increment, inter-frame relative pose transformation, and absolute position information. The inter-frame pose increment is obtained by wheel speed inertial odometer. The inter-frame relative pose transformation is obtained by laser odometer after distortion correction of point cloud data based on the inter-frame pose increment and matching and solving with a local map. The absolute position information is obtained by global odometer module, which includes GNSS-RTK odometer and UWB odometer. The GNSS-RTK odometer is used to provide global absolute pose information, and the UWB odometer is used to provide local absolute position information.

[0056] The prediction state module is used to input the multi-source positioning observation data into an adaptive multi-source fusion filter to recursively predict the system state variables and obtain the predicted state estimate of the locomotive.

[0057] The local odometer confidence assessment module is used to construct redundant observation constraints based on the absolute position information of the global odometer module, and compare the predicted state estimate of the locomotive with the redundant observation constraints to realize fault detection and confidence assessment of wheel speed inertial odometer and laser odometer, and obtain the confidence assessment result of local odometer.

[0058] The global odometer confidence assessment module is used to determine the confidence of the global odometer based on the operating status of the global odometer module, and adaptively adjust the fusion weight of each odometer based on the confidence assessment results of the local odometers and the confidence of the global odometer.

[0059] The fusion positioning module is used to perform observation update correction on the predicted state estimate of the locomotive based on the multi-source positioning observation data and the fusion weights of each odometer to obtain the final fusion positioning result of the locomotive. Specifically, when the current position of the locomotive is in an occluded environment, the locomotive's pose is updated based on the inter-frame relative pose transformation of the laser odometer and the local absolute position information of the UWB odometer. When the current position of the locomotive is in an unoccluded environment, the locomotive's pose is updated based on the global absolute pose information of the GNSS-RTK odometer.

[0060] As another aspect of the present invention, a multi-source fusion positioning system for locomotives in dedicated railways is provided, comprising: a wheel speed inertial odometer, a laser odometer, a global odometer module, and the aforementioned multi-source fusion positioning device for locomotives in dedicated railways, wherein the wheel speed inertial odometer, the laser odometer, and the global odometer module are all communicatively connected to the multi-source fusion positioning device for locomotives in dedicated railways.

[0061] The wheel speed inertial odometry is used to obtain the locomotive's inter-frame pose increment;

[0062] The laser odometry is used to obtain the locomotive's inter-frame relative pose transformation;

[0063] The global odometer module is used to obtain the absolute position information of the locomotive;

[0064] The locomotive multi-source fusion positioning device for dedicated railways is used to perform multi-source fusion based on the locomotive's inter-frame pose increment, inter-frame relative pose transformation, and absolute position information, and to evaluate the confidence level of each odometer to obtain the final fusion positioning result.

[0065] The present invention provides a multi-source fusion localization method for locomotives on dedicated railways. This method acquires multi-source localization observation data of the locomotive and inputs this data into an adaptive multi-source fusion filter to recursively predict system state variables, thereby obtaining a predicted locomotive state estimate. Then, based on the global and local cross-validation mechanism in the multi-source localization observation data, it completes the optimal update of the system pose estimation and covariance matrix. Based on the operating status of the global odometer module, it determines the confidence level of the global odometer and derives the occlusion state of the locomotive's current position. Finally, based on the multi-source localization observation data and the fusion weights of each odometer, it performs observation update correction on the predicted locomotive state estimate to obtain the final fusion localization result. Based on the occlusion state of the locomotive's current position, it determines the odometer corresponding to the locomotive's pose update under different occlusion environments. This multi-source fusion positioning method for locomotives on dedicated railways achieves multi-source fusion positioning in all scenarios of dedicated railways. The wheel speed inertial odometer serves as the main odometer, providing the prediction basis throughout the entire operation. The global odometer provides absolute position constraints for cross-verification and observation correction. The selection of observation sources in occluded and unoccluded states ensures the continuity and accuracy of positioning in different environments. Attached Figure Description

[0066] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof.

[0067] Figure 1 The flowchart illustrates the multi-source fusion positioning method for locomotives used in dedicated railways provided by this invention.

[0068] Figure 2 The flowchart for obtaining the inter-frame pose increment provided by the present invention is shown.

[0069] Figure 3 The flowchart for obtaining inter-frame relative pose transformation provided by the present invention is shown.

[0070] Figure 4 A flowchart illustrating the process of obtaining absolute position information provided by this invention.

[0071] Figure 5 This is a flowchart illustrating how multi-source positioning observation data, provided by this invention, is input into an adaptive multi-source fusion filter for recursive prediction.

[0072] Figure 6 This is a flowchart for obtaining the confidence assessment results of a local odometer, as provided by the present invention.

[0073] Figure 7 The flowchart provided by the present invention describes the determination of the confidence level of the global odometer, the adaptive adjustment of the odometer fusion weights, and the determination of the occlusion status of the locomotive's current position.

[0074] Figure 8 The flowchart provided by the present invention illustrates the process of obtaining the final fusion positioning result of the locomotive.

[0075] Figure 9 The structural block diagram of the multi-source fusion positioning device for locomotives used in dedicated railways provided by the present invention.

[0076] Figure 10 The structural block diagram of the multi-source fusion positioning system for locomotives used in dedicated railways provided by the present invention. Detailed Implementation

[0077] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other. The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0078] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0079] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate for the embodiments of the invention described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0080] This embodiment provides a multi-source fusion positioning method for locomotives on dedicated railways. Figure 1 This is a flowchart of a multi-source fusion positioning method for locomotives on dedicated railways provided by an embodiment of the present invention, as shown below. Figure 1 As shown, it includes:

[0081] S100. Acquire multi-source positioning observation data of the locomotive. The multi-source positioning observation data includes inter-frame pose increment, inter-frame relative pose transformation, and absolute position information. The inter-frame pose increment is obtained by wheel speed inertial odometer. The inter-frame relative pose transformation is obtained by laser odometer after distortion correction of point cloud data based on the inter-frame pose increment and matching with local map. The absolute position information is obtained by global odometer module. The global odometer module includes GNSS-RTK odometer and UWB odometer. The GNSS-RTK odometer is used to provide global absolute pose information, and the UWB odometer is used to provide local absolute position information.

[0082] In this embodiment of the invention, the inter-frame pose increment is obtained through a wheel speed inertial odometry (WSO). Specifically, the WSO can consist of a wheel speed meter, an inertial measurement unit (IMU), and a data fusion algorithm. The wheel speed meter can only output the locomotive's longitudinal velocity information and cannot directly obtain attitude angle information; while the IMU gyroscope can provide angular velocity information, which can be integrated to obtain attitude angle information. Therefore, this invention employs a scheme of fusing wheel speed meter and IMU data to output the inter-frame pose increment, thereby providing stable and reliable inter-frame pose constraint information. The inter-frame relative pose transformation is obtained by laser odometry performing distortion correction processing on the point cloud data based on the inter-frame pose increment and then matching and solving it with a local map. It is important to note that the inter-frame relative pose transformation of the laser odometry relies on the inter-frame pose increment provided by the wheel speed inertial odometry. This wheel speed inertial odometry's inter-frame pose increment is not only the prediction input for the fusion filter but also directly supplies the laser odometry for point cloud distortion correction. This creates an upstream-to-downstream data dependency between the wheel speed inertial odometry and the laser odometry, rather than them being independently operating parallel sensor modules. The absolute position information is obtained through a global odometry module, which includes a GNSS-RTK odometry and a UWB odometry. The GNSS-RTK odometry provides the locomotive's global absolute pose information, including its absolute position and heading angle. The UWB odometry provides the locomotive's local absolute position information, which only includes the locomotive's absolute position and not its angle information. Both GNSS-RTK odometry and UWB odometry can provide absolute position information, but the absolute position information they provide differs in terms of information dimension and reference coordinate system: GNSS-RTK odometry provides global absolute pose information including position and heading angle based on the satellite coordinate system, while UWB odometry provides only local absolute position information in the position dimension based on the UWB base station coordinate system using the trilateration method.

[0083] S200. Input the multi-source positioning observation data into an adaptive multi-source fusion filter to recursively predict the system state variables and obtain the predicted state estimate of the locomotive.

[0084] In this embodiment of the invention, the multi-source positioning observation data obtained in step S100 is input to an adaptive multi-source fusion filter. Specifically, the adaptive multi-source fusion filter builds a fusion framework based on Error State Kalman Filter (ESKF), uses the inter-frame pose increment obtained from the wheel speed inertial odometry as the system's high-frequency pose prediction unit, and recursively predicts the system state variables to obtain the locomotive's predicted state estimate. Since the output of the wheel speed inertial odometry is not affected by external environmental interference and has high-frequency output characteristics, it can stably complete the real-time recursive update of the system state variables, state covariance matrix, and process noise matrix. Because the wheel speed inertial odometry has low computational overhead and local stability, it runs as the main odometry throughout the entire process.

[0085] It should be noted that when inputting the multi-source positioning observation data, the inter-frame pose increment obtained by the wheel speed inertial odometry is used as the driving input for the recursive prediction of the system state variables, while the inter-frame relative pose transformation and absolute position information are reserved for the observation update and correction steps in the subsequent step S500.

[0086] S300. Construct redundant observation constraints based on the absolute position information of the global odometer module, and compare the predicted state estimate of the locomotive with the redundant observation constraints to achieve fault detection and confidence assessment of the wheel speed inertial odometer and laser odometer, and obtain the confidence assessment result of the local odometer.

[0087] In this embodiment of the invention, a global and local cross-validation mechanism is implemented. Using the absolute position information output by the global odometer module as a reference, low-frequency global odometer absolute position information is interpolated to the high-frequency timestamp of the local odometer through linear interpolation, resulting in redundant observation constraints aligned with the local odometer time. Then, the predicted locomotive state estimate obtained in step S200 is compared with the redundant observation constraints to calculate the pose deviations of the wheel speed inertial odometer and the laser odometer. When the pose deviation exceeds a preset threshold, the corresponding local odometer is determined to be faulty, for example, the laser odometer is determined to have degraded or the wheel speed inertial odometer error exceeds the limit. The confidence level of the faulty local odometer is adaptively reduced based on the amplitude of the pose deviation, obtaining the confidence evaluation result of the local odometer.

[0088] Through the aforementioned global and local cross-validation mechanism, using the global odometry as the external observation benchmark of the system, the optimal update of the system pose estimation and covariance matrix is ​​achieved, realizing the adaptive and robust fusion of information from multiple heterogeneous sensors.

[0089] S400. Determine the confidence level of the global odometer based on the operating status of the global odometer module, and adaptively adjust the fusion weight of each odometer based on the confidence level evaluation results of the local odometers and the confidence level of the global odometer.

[0090] In this embodiment of the invention, the confidence level of the global odometer is determined based on the operating status of the global odometer module.

[0091] S500: Based on the multi-source positioning observation data and the fusion weights of each odometer, the predicted state estimate of the locomotive is updated and corrected to obtain the final fusion positioning result of the locomotive. Specifically, when the current position of the locomotive is in an occluded environment, the locomotive's pose is updated based on the inter-frame relative pose transformation of the laser odometer and the local absolute position information of the UWB odometer. When the current position of the locomotive is in an unoccluded environment, the locomotive's pose is updated based on the global absolute pose information of the GNSS-RTK odometer.

[0092] In summary, the multi-source fusion localization method for locomotives on dedicated railways provided by this invention acquires multi-source localization observation data of the locomotive and inputs this data into an adaptive multi-source fusion filter to recursively predict the system state variables, thereby obtaining the predicted state estimate of the locomotive. Then, based on the global and local cross-validation mechanism in the multi-source localization observation data, it completes the optimal update of the system pose estimation and covariance matrix. Based on the operating state of the global odometer module, it determines the confidence level of the global odometer and derives the occlusion state of the locomotive's current position. Finally, based on the multi-source localization observation data and the fusion weights of each odometer, it performs observation update correction on the predicted state estimate of the locomotive to obtain the final fusion localization result of the locomotive. Based on the occlusion state of the locomotive's current position, it determines the odometer corresponding to the locomotive's pose update under different occlusion environments. This multi-source fusion positioning method for locomotives on dedicated railways achieves multi-source fusion positioning in all scenarios of dedicated railways. The wheel speed inertial odometer serves as the main odometer, providing the prediction basis throughout the entire operation. The global odometer provides absolute position constraints for cross-verification and observation correction. The selection of observation sources in occluded and unoccluded states ensures the continuity and accuracy of positioning in different environments.

[0093] In this embodiment of the invention, the inter-frame pose increment is obtained through wheel speed inertial odometry, such as... Figure 2 As shown, it includes:

[0094] S110a. Acquire inertial measurement unit measurement data, and perform inter-frame pre-integration processing on the inertial measurement unit measurement data to obtain inter-frame inertial measurement pre-integration results.

[0095] Specifically, the inertial measurement unit (IMU) measurement data is pre-integrated between adjacent frames to obtain the inter-frame inertial measurement pre-integration result. The angular velocity information output by the IMU gyroscope is pre-integrated to obtain the inter-frame angle increment, and the acceleration information output by the IMU accelerometer is pre-integrated to obtain the inter-frame velocity and position increment. The above increment information together constitute the inter-frame inertial measurement pre-integration result.

[0096] S120a. Acquire wheel speed meter measurement data and extract the forward speed observation value of the locomotive from the wheel speed meter measurement data;

[0097] Specifically, wheel speed meter measurement data is acquired, and the locomotive's forward speed observation value is extracted from the wheel speed meter measurement data. The wheel speed meter measurement data reflects the rotational speed of the locomotive wheels, and after conversion, the forward speed observation value of the locomotive along the direction of travel is obtained.

[0098] S130a. Construct the velocity observation equation based on the nonholonomic constraint model and determine the constraints of the nonholonomic constraint model. Construct the corresponding velocity observation noise covariance matrix based on the velocity observation equation under the constraints of the nonholonomic constraint model. The constraints of the nonholonomic constraint model are that the lateral velocity observation value and the axial velocity observation value of the locomotive are both set to zero.

[0099] S140a. Adaptively adjust the noise variance in the velocity observation noise covariance matrix corresponding to the horizontal and upward zero velocities according to the magnitude of the velocity observation residuals;

[0100] Specifically, a velocity observation equation is constructed based on a nonholonomic constraint model. The lateral and overhead velocity observations of the locomotive are set to zero. A corresponding velocity observation noise covariance matrix is ​​constructed based on the velocity observation equation. The noise variance corresponding to the zero-velocity lateral and overhead observations in the velocity observation noise covariance matrix is ​​adaptively adjusted according to the magnitude of the velocity observation residuals. During normal operation, the locomotive has no lateral or overhead velocity; therefore, the velocity observations in these two directions are set to zero as NHC constraints. The velocity observation noise covariance matrix is ​​paired with the velocity observation equation, defining the degree of confidence for each velocity observation component. The velocity observation residuals reflect the consistency between the NHC constraints and the actual state. When the residuals are small, it indicates that the locomotive indeed satisfies the NHC constraints, and the noise variance corresponding to the zero-velocity lateral and overhead observations remains small, giving the NHC constraints a higher weight. When the residuals are large, it indicates that the locomotive may be in abnormal operating conditions such as coasting or slippage, and the NHC constraints do not match the actual state. In this case, the noise variance corresponding to the zero-velocity lateral and overhead observations is adaptively increased, and the weight of the NHC constraints is reduced to avoid forcibly applying erroneous constraints under abnormal operating conditions to the state estimation.

[0101] S150a. Using the pre-integration result of the inter-frame inertial measurement as the state prediction, the extended Kalman filter state update is performed using the velocity observation equation, the adaptively adjusted velocity observation noise covariance matrix, and the locomotive's forward velocity observation value as observation constraints to obtain the inter-frame pose increment.

[0102] Specifically, using the pre-integrated results of the inter-frame inertial measurement as the state prediction, the velocity observation equation, the adjusted velocity observation noise covariance matrix, and the forward velocity observation value are used as observation constraints to perform extended Kalman filtering state updates, outputting the inter-frame pose increment. The pre-integrated results of the inter-frame inertial measurement provide state prediction as the prediction stage of the EKF, the velocity observation equation provides the observation model (lateral velocity = 0, upward velocity = 0, forward velocity = wheel speed meter forward velocity observation value), and the adjusted velocity observation noise covariance matrix provides the observation weights. These three together serve as observation constraints to correct and update the state prediction, ultimately outputting the inter-frame pose increment.

[0103] Therefore, this embodiment of the invention addresses the locomotive's operating characteristics, where the locomotive has no lateral or axial speed, satisfying the nonholonomic constraint (NHC) model. An adaptive nonholonomic constraint model is introduced in the speed observation update stage of the extended Kalman filter. By adaptively adjusting the speed observation noise covariance matrix, the measurement error caused by abnormal operating conditions such as locomotive skidding and slippage is effectively suppressed, thereby improving the accuracy and robustness of pose estimation.

[0104] In this embodiment of the invention, the inter-frame relative pose transformation is obtained by laser odometry performing distortion correction processing on point cloud data based on the inter-frame pose increment and then matching and solving it with a local map. Figure 3 As shown, it includes:

[0105] S110b: Acquire multi-line lidar point cloud data, and calculate the locomotive pose change at each scanning point acquisition time by linear interpolation based on the inter-frame pose increment obtained from the wheel speed inertial odometer.

[0106] Specifically, the system receives point cloud data from a multi-line lidar system and calculates the locomotive pose change at each scanning point's acquisition time using linear interpolation based on the inter-frame pose increments obtained from the wheel speed inertial odometry. The inter-frame pose increments provided by the wheel speed inertial odometry include information on the locomotive's position, velocity, and attitude changes between frames. By using linear interpolation, the inter-frame pose increments are decomposed onto the timestamps of each scanning point, thereby obtaining an estimate of the locomotive pose change at each scanning point's acquisition time.

[0107] S120b: Based on the locomotive pose change at each scanning point acquisition time, all scanning points are uniformly transformed to the same coordinate system at the same time to obtain the distortion-free point cloud.

[0108] Specifically, based on the locomotive pose changes at each scanning point acquisition time, all scanning points are uniformly transformed to the same coordinate system at the same time, resulting in a distortion-free point cloud. By utilizing the locomotive pose changes at each scanning point acquisition time, scanning points acquired at different times are uniformly projected to the same reference coordinate system, eliminating point cloud distortion caused by locomotive motion.

[0109] S130b: Extract line feature point sets and surface feature point sets from the distortion-free point cloud to form the current feature point cloud;

[0110] Specifically, line feature point sets and surface feature point sets are extracted from the distortion-free point cloud to form the feature point cloud of the current frame. In the distortion-free point cloud, line feature points (points distributed along a linear structure) and surface feature points (points distributed along a planar structure) are extracted based on the local geometric features of the point cloud to form the feature point cloud of the current frame.

[0111] S140b: Transform the current frame feature point cloud to the previous frame coordinate system using the inter-frame pose transformation of the wheel speed inertial odometer as the initial estimate, and match and solve with the feature point cloud of the previous frame to obtain the inter-frame relative pose transformation of the laser odometer, wherein the inter-frame pose transformation of the wheel speed inertial odometer is obtained based on the inter-frame pose increment transformation of the wheel speed inertial odometer.

[0112] Specifically, the current frame feature point cloud is transformed to the previous frame coordinate system using the inter-frame pose transformation of the wheel speed inertial odometry as the initial estimate, and then matched with the feature point cloud of the previous frame to obtain the inter-frame relative pose transformation of the laser odometry. Since the point clouds of the current frame and the previous frame are in different coordinate systems, the current frame feature point cloud needs to be transformed to the previous frame coordinate system for matching. The inter-frame pose transformation provided by the wheel speed inertial odometry provides an approximate alignment relationship between the two frames as the initial estimate. After transforming the current frame feature point cloud to the previous frame coordinate system using this initial estimate, the feature point clouds of the two frames are roughly aligned. At this point, least squares iterative matching is performed to gradually optimize the initial estimate and obtain the inter-frame relative pose transformation of the laser odometry.

[0113] In this embodiment of the invention, the current line feature points Surface feature points Inter-frame pose obtained through wheel speed inertial odometry Feature points from the previous frame and The closest point least squares is shown below:

[0114] ,

[0115] in, The scaling factor representing the confidence level of surface features is used to obtain the precise pose change of the lidar's current pose relative to the previous frame, i.e., the lidar odometry. The feature point cloud in the lidar coordinate system is transformed into the world coordinate system and saved as a local map of fixed size. This process is repeated to match the point cloud of the current frame with the local map and calculate the lidar odometry between frames.

[0116] It should be noted that the confidence ratio of the surface feature is a proportional coefficient. There are several ways to determine the proportionality coefficient λ. For example, it can be set as an empirical constant based on the environment; for instance, if the number of surface features exceeds the number of line features in the environment, then this value should be set larger. Alternatively, it can be based on an adaptive weighting of the feature quantity distribution, where the number of valid line feature points and surface feature points extracted in each frame is counted after feature extraction. Therefore, the proportionality coefficient λ is used to balance the contribution of the residuals of line features and surface features to the state estimation. Its determination can be based on an empirical constant, or an adaptive estimation based on the distribution ratio of feature points, depending on dynamic changes in the environment. Alternatively, it can utilize the coplanarity of surface feature points to fit the residuals and perform uncertainty weighting to ensure the accuracy and robustness of pose estimation under different environments to the greatest extent.

[0117] Therefore, in this embodiment of the invention, the line feature points and surface feature points of the current frame are transformed to the same coordinate system as the feature points of the previous frame through inter-frame pose transformation using wheel speed inertial odometry. Least squares calculation is then performed with the nearest point from the previous frame, where the confidence level of the surface features is weighted using a scaling factor. Specifically, the line feature points of the current frame find the nearest line feature point in the coordinate system of the previous frame to form a line feature correspondence, and the surface feature points of the current frame find the nearest surface feature point in the coordinate system of the previous frame to form a surface feature correspondence. Through least squares calculation, the distance between each feature point in the current frame and its corresponding point is minimized, resulting in the optimal inter-frame relative pose transformation. The confidence level of the surface features is weighted using a scaling factor; that is, surface feature points with high confidence are given a larger weight in the least squares calculation, while surface feature points with low confidence are given a smaller weight, thereby improving the robustness of the matching solution.

[0118] S150b: Transform the current feature point cloud to the world coordinate system according to the inter-frame relative pose transformation of the laser odometry to construct a local map;

[0119] S160b: Search for the corresponding point set of each feature point in the current feature point cloud in the local map, and perform iterative matching and solution between the corresponding point set and each feature point in the current frame feature point cloud to update the inter-frame relative pose transformation.

[0120] Specifically, the current frame feature point cloud is transformed to the world coordinate system based on the inter-frame relative pose transformation of the laser odometry to construct a local map of fixed size. The corresponding point set of each feature point in the current frame feature point cloud is searched in the local map. The inter-frame relative pose transformation is updated by iteratively matching the corresponding point set with each feature point in the current frame feature point cloud.

[0121] It should be understood that the inter-frame matching (between frames) in step S140b provides a coarser inter-frame relative pose transformation, while the frame-map matching (between a frame and a local map) in step S160b further optimizes the pose estimation using richer reference information accumulated in the local map. The current frame's feature point cloud is projected onto the world coordinate system through the inter-frame relative pose transformation and added to a fixed-size local map. Then, a corresponding point set is searched for each feature point in the current frame within the local map. Through iterative matching between the corresponding point set and each feature point in the current frame, the inter-frame relative pose transformation is updated, resulting in a more accurate pose estimation.

[0122] In this embodiment of the invention, the absolute position information is obtained through a global odometer module, such as... Figure 4 As shown, it includes:

[0123] S110c: Obtain the latitude, longitude, altitude, and angle information of the locomotive's GNSS-RTK odometer in the WGS84 coordinate system, and convert the latitude, longitude, altitude, and angle information in the WGS84 coordinate system to the UTM rectangular coordinate system to obtain the locomotive's global absolute pose information, which includes the locomotive's absolute position and heading angle.

[0124] Specifically, the GNSS-RTK odometry receives differential data from the base station and outputs centimeter-level latitude, longitude, altitude, and angle information in the WGS84 coordinate system. Then, the latitude, longitude, altitude, and angle information in the WGS84 coordinate system are converted into coordinates (x, y, z) and heading angle in the UTM Cartesian coordinate system for absolute pose observation in an adaptive multi-source fusion filter. In an unobstructed environment, the GNSS-RTK odometry achieves an absolute position accuracy of 2 cm and an angle accuracy of 0.2°. The locomotive's global absolute pose information includes its absolute position and heading angle, provided based on the satellite coordinate system.

[0125] S120c calibrates UWB base stations to synchronize the location of UWB base stations with the location of GNSS-RTK odometers, and obtains the absolute position information of each UWB base station in the world coordinate system.

[0126] Specifically, it is necessary to calibrate the location of UWB base stations, synchronize the location of UWB base stations with the GNSS-RTK location, and record the absolute position information of each UWB base station in the world coordinate system.

[0127] S130c: Obtain the local absolute position information of the locomotive calculated by the UWB odometer based on the distance data between the UWB tag on the locomotive and each UWB base station. The local absolute position information of the locomotive includes the absolute position of the locomotive.

[0128] Specifically, the UWB odometer receives distance data between the vehicle-mounted UWB tag and each UWB base station. The data received by the vehicle-mounted UWB tag is the distance value between the tag and each base station. Based on the absolute positions of at least four known UWB base stations in the world coordinate system and the distance values ​​between each base station and the vehicle-mounted UWB tag, the local absolute position information of the vehicle-mounted UWB tag is calculated using the trilateration method. This local absolute position information only includes the absolute position of the vehicle and does not include angle information; it is provided based on the UWB base station coordinate system using the trilateration method.

[0129] In this embodiment of the invention, the absolute location information of the current UWB tag can be calculated using the trilateration method based on the known absolute locations of at least four base stations. The calculation formula for the trilateration method is as follows:

[0130] ,

[0131] in, This represents the coordinates of the nth base station. This represents the distance between the nth base station and the tag on the vehicle, thus indicating the current two-dimensional position of the UWB tag on the vehicle. for:

[0132] .

[0133] It should be understood that the difference between GNSS-RTK odometry and UWB odometry in this embodiment of the invention is as follows: GNSS-RTK odometry provides global absolute pose information based on a satellite coordinate system, including information in two dimensions: position and heading angle; UWB odometry, based on a UWB base station coordinate system, provides only local absolute position information in the position dimension using the trilateration method, without including angle information. This difference in information dimensions causes them to play different roles in fusion positioning, i.e., GNSS-RTK odometry provides both position and heading constraints, while UWB odometry only provides position constraints.

[0134] In this embodiment of the invention, the multi-source positioning observation data is input to an adaptive multi-source fusion filter to recursively predict the system state variables, thereby obtaining a predicted state estimate of the locomotive, such as... Figure 5 As shown, it includes:

[0135] S210. Construct an adaptive multi-source fusion filter with error state Kalman filtering as the core, and define system state variables, including the locomotive's position, speed, attitude, and the zero bias of the acceleration of the inertial measurement unit and the zero bias of the gyroscope.

[0136] Specifically, in this embodiment of the invention, the system state variables are defined as error state forms, that is, the deviation between the true value and the nominal value of the state variables, including position error, velocity error, attitude error, acceleration zero bias error and gyroscope zero bias error.

[0137] S220. Construct the error state equation based on the locomotive's kinematic model, and initialize the state covariance matrix and process noise matrix;

[0138] Specifically, the system state variables are defined as follows:

[0139] ,

[0140] in, Indicates position error (in navigation system n, three-dimensional, unit m); Indicates velocity error (in navigation system n, three-dimensional, unit: meters); Represents attitude error (equivalent rotation vector / misalignment angle, three-dimensional, unit: rad); This indicates the zero bias error of the accelerometer (three-dimensional, unit m / s²). This represents the gyroscope's zero bias error (three-dimensional, unit: rad / s).

[0141] Error state equation:

[0142] ,

[0143] in, The state transition matrix (continuous) represents the coupling relationship between errors; G(t) represents the noise driving matrix; w(t) represents the system process noise (zero-mean Gaussian white noise), including the measurement noise from the accelerometer and gyroscope, as well as the zero-bias walk noise.

[0144] ,

[0145] Process noise matrix (continuous-time spectral density):

[0146] ,

[0147] in, This represents a 12×12 diagonal or block diagonal matrix, typically taken as:

[0148] ,

[0149] in, This indicates the noise power spectral density measured by the accelerometer, in units of... ; This indicates the noise power spectral density measured by the gyroscope, in units of... ; This represents the zero-bias walk power spectral density of the accelerometer, in units of... or ; This represents the zero-bias walk power spectral density of a gyroscope, in units of... .

[0150] In this embodiment of the invention, an error state equation is constructed based on the locomotive's kinematic model, and the state covariance matrix and process noise matrix are initialized. The error state equation describes the propagation law of the system error state over time and is derived based on the locomotive's kinematic model. The state covariance matrix is ​​initialized as a diagonal matrix, with each element corresponding to the initial uncertainty of each state variable. The process noise matrix defines the noise level of the system model itself.

[0151] S230. Input the multi-source positioning observation data into the adaptive multi-source fusion filter;

[0152] In this embodiment of the invention, the multi-source positioning observation data is input to the adaptive multi-source fusion filter, wherein the inter-frame pose increment obtained by the wheel speed inertial odometry is used as the driving input, and the inter-frame relative pose transformation and the absolute position information are reserved for use in subsequent observation update and correction steps. The wheel speed inertial odometry operates as the system's high-frequency pose prediction unit throughout the entire process, and its inter-frame pose increment is used to drive the recursive prediction of system state variables; the inter-frame relative pose transformation of the laser odometry and the absolute position information of the global odometry module are not used in the current recursive prediction step, but are reserved for use as observation constraints in subsequent observation update and correction steps.

[0153] S240. Based on the error state equation, the system state variables are recursively predicted using the inter-frame pose increment in the multi-source positioning observation data as the driving force, and the state covariance matrix and process noise matrix are synchronously recursively updated to obtain the predicted state estimate of the locomotive.

[0154] In this embodiment of the invention, the system state variables are recursively predicted based on the error state equation using the inter-frame pose increment as the driving input, and the state covariance matrix and the process noise matrix are synchronously recursively updated to obtain the predicted state estimate of the locomotive. At each time step, the system state variables are recursively predicted using the inter-frame pose increment to drive the error state equation, and the state covariance matrix is ​​synchronously recursively predicted and updated based on the process noise matrix. Finally, the predicted state estimate and predicted state covariance matrix of the locomotive are output, providing a predictive basis for subsequent observation update correction.

[0155] In this embodiment of the invention, redundant observation constraints are constructed based on the absolute position information of the global odometer module, and the predicted state estimate of the locomotive is compared with the redundant observation constraints to achieve fault detection and confidence assessment of the wheel speed inertial odometer and laser odometer, thereby obtaining the confidence assessment result of the local odometer, such as... Figure 6 As shown, it includes:

[0156] S310. Using the absolute position information output by the global odometer module as a reference, the low-frequency global odometer absolute position information is interpolated to the high-frequency timestamp of the local odometer through linear interpolation to obtain a global redundant observation sequence that is aligned with the local odometer time.

[0157] Specifically, using the absolute position information output by the global odometer module as a reference, low-frequency global odometer absolute position information is interpolated to the high-frequency timestamp of the local odometer through linear interpolation, resulting in redundant observation constraints aligned with the local odometer time. The global odometer module (GNSS-RTK odometer and UWB odometer) has a low output frequency (typically 1Hz~10Hz), while the local odometer (wheel speed inertial odometer and laser odometer) has a high output frequency (typically 50Hz~200Hz). To enable effective comparison of the two sets of data, it is necessary to interpolate the low-frequency global odometer absolute position information to the local odometer timestamp through linear interpolation, thus obtaining time-aligned redundant observation constraints.

[0158] S320. The predicted state estimate of the locomotive is compared with the global redundant observation sequence to calculate the pose deviation of the wheel speed inertial odometer and the laser odometer.

[0159] Specifically, the predicted state estimate of the locomotive obtained in step S200 is compared with the redundant observation constraints to calculate the pose deviations of the wheel speed inertial odometer and the laser odometer. The pose component contributed by the wheel speed inertial odometer in the predicted state estimate is compared with the redundant observation constraints to calculate the pose deviation of the wheel speed inertial odometer, and the pose component contributed by the laser odometer is compared with the redundant observation constraints to calculate the pose deviation of the laser odometer.

[0160] S330. When the position deviation of the wheel speed inertial odometer and / or laser odometer exceeds the preset position deviation threshold, it is determined that the corresponding local odometer is faulty, and the confidence level of the faulty local odometer is adaptively reduced according to the amplitude of the position deviation to obtain the confidence level evaluation result of the local odometer.

[0161] Specifically, when the pose deviation exceeds a preset pose deviation threshold, the corresponding local odometer is determined to be faulty. The confidence level of the faulty local odometer is adaptively reduced based on the amplitude of the pose deviation, thus obtaining a confidence level assessment result for the local odometer. When the pose deviation of the wheel speed inertial odometer exceeds a preset threshold, the wheel speed inertial odometer error is determined to be out of limit. When the pose deviation of the laser odometer exceeds a preset threshold, the laser odometer is determined to have degraded. A larger deviation amplitude results in a greater reduction in confidence level, while a smaller deviation amplitude results in a smaller reduction in confidence level, achieving adaptive confidence level adjustment based on the physical deviation amplitude.

[0162] It should be understood that the preset position deviation threshold is the key judgment boundary for determining whether a local odometer (such as a wheel speed inertial odometer or a laser odometer) has failed. There is no unified standard for its value. It is mainly determined based on sensor characteristics, application scenarios and system fault tolerance requirements. Commonly used setting methods include: (1) Based on experience, a fixed threshold (such as a position deviation of 0.5 meters or an attitude deviation of 3 degrees) is given directly according to different working conditions (such as vehicle speed and road surface). The reference system accuracy is required to be higher than that of the odometer under test and the threshold is greater than the normal noise fluctuation; (2) Based on a statistical model, assuming that the deviation residual follows a normal distribution, the confidence interval boundary is calculated according to the set confidence level (such as 95%) as a dynamic threshold. If it exceeds the threshold, it is judged as abnormal; (3) Based on the comparison of the reference system, the estimated value of the main positioning system (such as an IMU / GNSS combination) is used as the benchmark. The Mahalanobis distance between the pre-integrated value of the wheel speed or laser odometer and the benchmark value is calculated. When the distance exceeds the preset threshold, the odometer is judged to be faulty or degraded. In this embodiment of the invention, adaptive weight reduction is triggered when the pose deviation exceeds the threshold, and the greater the deviation amplitude, the greater the confidence reduction, thereby realizing intelligent fault isolation based on physical deviation.

[0163] In this embodiment of the invention, the confidence level of the global odometer is determined based on the operating status of the global odometer module, and the fusion weights of each odometer are adaptively adjusted based on the confidence level evaluation results of the local odometers and the confidence level of the global odometer, such as... Figure 7 As shown, it includes:

[0164] S410. Determine the confidence level of the GNSS-RTK odometer based on its status and the number of satellites searched. Specifically, set a high confidence level when the GNSS-RTK odometer status is a fixed solution, set a medium confidence level when the GNSS-RTK odometer status is a floating-point solution, and set an unreliable level when the GNSS-RTK odometer status is a single-point solution or the number of satellites searched is less than a preset number of satellites searched.

[0165] Specifically, the confidence level of the GNSS-RTK odometry is determined based on its status and the number of satellites searched. The GNSS-RTK odometry status and the number of satellites searched are checked. When the status is a fixed solution, a high confidence level is set, meaning the GNSS-RTK odometry can provide centimeter-level high-precision absolute pose information. When the status is a floating-point solution, a medium confidence level is set, meaning the accuracy of the GNSS-RTK odometry decreases but it can still provide effective absolute pose information. When the status is a single-point solution or the number of satellites searched is less than 4, it is set to unreliable, meaning the GNSS-RTK odometry cannot provide reliable absolute pose information.

[0166] S420. Determine the confidence level of the UWB odometer based on the number of UWB base station distance data received by the UWB odometer, wherein when the number of UWB base station distance data received by the UWB odometer is less than the preset number of distance data, it is set to unreliable.

[0167] Specifically, the confidence level of the UWB odometry is determined based on the number of UWB base station distance data points received. UWB devices are deployed in obstructed areas, and the availability of the UWB odometry is judged by detecting the number of received UWB base station distance data points. If the number of received UWB base station distance data points is less than three, it is set to unreliable; that is, the trilateration method requires at least three known base station locations and distance values ​​to calculate the two-dimensional location, and less than three will prevent location calculation. If the number of received UWB base station distance data points is at least four, it is set to reliable; that is, four base stations provide redundancy constraints to improve the accuracy of location calculation.

[0168] S430. Determine the occlusion status of the locomotive's current position based on the confidence level of the GNSS-RTK odometer and the confidence level of the UWB odometer.

[0169] Specifically, the occlusion status of the locomotive's current position is determined based on the confidence levels of the GNSS-RTK odometer and the UWB odometer. The occlusion status is derived causally from the confidence levels of the global odometers.

[0170] S440. When the GNSS-RTK odometer is set to untrusted and the UWB odometer is not untrusted, determine that the locomotive's current position is an obstructed area.

[0171] Specifically, when the GNSS-RTK odometer is set to unreliable and the UWB odometer is not unreliable, the locomotive's current position is determined to be an obstructed area. That is, the unreliability of GNSS-RTK indicates that the satellite signal is obstructed, while the reliability of UWB indicates that the UWB base station can still provide absolute position reference. The combination of the two indicates that the locomotive is in an operating area with obstruction but UWB coverage.

[0172] S450: When the GNSS-RTK odometer is at a high or medium confidence level, the locomotive's current position is determined to be an unobstructed area.

[0173] Specifically, when the GNSS-RTK odometer is at a high or medium confidence level, it is determined that the locomotive's current position is an unobstructed area, meaning that the satellite signal is unobstructed and the GNSS-RTK can work normally.

[0174] S460. The fusion weights of each odometer are adaptively adjusted based on the occlusion status of the locomotive's current position, the confidence evaluation results of the local odometer, and the confidence of the global odometer.

[0175] Specifically, when the locomotive is in an obstructed area, the fusion weight of the wheel speed inertial odometry and UWB odometry is increased, while the fusion weight of the GNSS-RTK odometry is decreased. That is, in this case, the GNSS-RTK odometry is unreliable and its weight should be reduced, while the wheel speed inertial odometry, as the primary odometry, should maintain a higher weight, and the UWB odometry, being reliable, should have its weight increased to provide absolute position constraints. When the locomotive is in an unobstructed area, the fusion weight of the GNSS-RTK odometry is increased. That is, in this case, the GNSS-RTK is reliable and has high accuracy, and its weight should be increased to provide global absolute pose constraints.

[0176] In summary, the fusion strategies for the three typical scenarios are as follows:

[0177] Scenario 1: When the GNSS-RTK odometry experiences brief floating-point solutions or occlusion, the accumulated error can be effectively reduced by fusing local absolute position information from the wheel speed inertial odometry and the UWB odometry. In this situation, the GNSS-RTK is unreliable or its accuracy decreases. The wheel speed inertial odometry serves as the primary odometry, providing the prediction basis, while the UWB odometry provides absolute position constraints to suppress the accumulated error of the wheel speed inertial odometry.

[0178] Scenario 2: In open outdoor environments or when the LiDAR is obstructed, the global absolute pose information from wheel speed inertial odometry and GNSS-RTK odometry should be used to reduce the impact of the LiDAR odometry on the overall positioning. In this case, GNSS-RTK reliably provides global absolute pose constraints, but obstruction in front of the LiDAR causes the LiDAR odometry to degrade, so the weight of the LiDAR odometry should be reduced.

[0179] Scenario 3: When GNSS-RTK and LiDAR are obstructed, the effective accuracy of the positioning data is maintained by fusing local absolute position information from wheel speed inertial odometry and UWB odometry. In this case, both GNSS-RTK and LiDAR odometry are unreliable. Wheel speed inertial odometry serves as the primary odometry to provide the prediction basis, while UWB odometry provides absolute position constraints to maintain positioning accuracy.

[0180] In this embodiment of the invention, the predicted state estimate of the locomotive is updated and corrected based on the multi-source positioning observation data and the fusion weights of each odometer to obtain the final fused positioning result of the locomotive, such as... Figure 8 As shown, it includes:

[0181] S510. Determine available observation sources based on the occlusion status of the locomotive's current position. Specifically, when the locomotive's current position is in an occluded environment, determine the inter-frame relative pose transformation of the laser odometer and the local absolute position information of the UWB odometer as available observation sources. When the locomotive's current position is in an unoccluded environment, determine the global absolute pose information of the GNSS-RTK odometer as available observation sources.

[0182] Specifically, available observation sources are determined based on the occlusion status of the locomotive's current position. When the locomotive is in an occluded environment, the inter-frame relative pose transformation of the laser odometry and the local absolute position information of the UWB odometry are identified as available observation sources. In this case, the GNSS-RTK odometry is unreliable, and the laser odometry provides inter-frame relative pose constraints, while the UWB odometry provides local absolute position constraints. When the locomotive is in an unoccluded environment, the global absolute pose information of the GNSS-RTK odometry is identified as an available observation source. In this case, the GNSS-RTK odometry is reliable and highly accurate, and its global absolute pose information is used to provide position and heading constraints. The wheel speed inertial odometry is used as the prediction basis for fusion in all scenarios. Its inter-frame pose increment has already been used as a driving input for the recursive prediction of system state variables in step S200, and the observation correction in step S500 is superimposed on the predicted state estimation.

[0183] S520. Construct the corresponding observation equations based on the available observation sources;

[0184] Specifically, in an occluded environment, an observation equation is constructed using the relative pose transformation between laser odometry frames and the local absolute position information of UWB odometry as the observation quantities; in an unoccluded environment, an observation equation is constructed using the global absolute pose information of GNSS-RTK odometry as the observation quantities.

[0185] S530. Adjust the observation noise covariance matrix corresponding to the observation equation according to the fusion weight of each odometer;

[0186] In this embodiment of the invention, the observation noise variance of the odometer with a higher fusion weight is set to a smaller value, indicating that the observation information of the odometer is given higher confidence; the observation noise variance of the odometer with a lower fusion weight is set to a larger value, indicating that the observation information of the odometer is given lower attention.

[0187] S540. Calculate the Kalman gain based on the observation equation, the adjusted observation noise covariance matrix, and the predicted state estimate of the locomotive.

[0188] S550. The predicted state estimate of the locomotive is corrected and updated according to the Kalman gain to obtain the final fusion positioning result of the locomotive.

[0189] In this embodiment of the invention, the Kalman gain is calculated based on the predicted state covariance matrix and the adjusted observation noise covariance matrix, determining the degree of correction of the predicted state estimate by the observation correction. The final fused positioning result is the optimal pose estimate of the locomotive, which is the high-frequency continuous positioning information output by the adaptive multi-source fusion filter.

[0190] In summary, the multi-source fusion positioning method for locomotives on dedicated railways provided by this invention has the following technical effects:

[0191] (1) This invention achieves continuous, stable, and high-precision positioning across all scenarios of dedicated railways by fusing multi-source information from wheel speed inertial odometers, laser odometers, GNSS-RTK odometers, and UWB odometers, combined with an adaptive multi-source fusion filter based on error state Kalman filtering. In unobstructed environments, the use of GNSS-RTK odometers fused with wheel speed inertial odometers and laser odometers effectively improves positioning accuracy in outdoor scenarios and maintains long-term positioning accuracy even in the presence of GNSS signal interference. In obstructed environments, the use of laser odometers, wheel speed inertial odometers, and UWB odometers for fusion positioning enables high-precision positioning even without GNSS satellite search.

[0192] (2) This invention uses a wheel speed inertial odometry as the high-frequency pose prediction unit of the adaptive multi-source fusion filter. Its output is not affected by external environmental interference, and it has high-frequency output characteristics, which can stably complete the real-time recursive update of system state variables, state covariance matrix and process noise matrix. Since the wheel speed inertial odometry has low computational overhead and local stability, it runs as the main odometry throughout the entire process, providing a continuous and reliable prediction basis for the fusion system.

[0193] (3) In this invention, an adaptive nonholonomic constraint model is introduced into the wheel speed inertial odometer. In view of the locomotive’s characteristic of having no speed in the lateral and axial directions, the noise variance corresponding to the zero speed observation in the lateral and axial directions in the speed observation noise covariance matrix is ​​adaptively adjusted to effectively suppress the measurement error caused by abnormal working conditions such as locomotive skidding and slipping, and improve the accuracy and robustness of pose estimation.

[0194] (4) The present invention realizes odometer fault detection and confidence assessment through global and local cross-validation mechanism: Based on the absolute position information output by the global odometer module, redundant observation constraints are constructed by linear interpolation. The predicted state estimate is compared with the redundant observation constraints to calculate the pose deviation. When the deviation exceeds the preset threshold, it is determined that the local odometer is faulty. The confidence of the faulty odometer is adaptively reduced according to the deviation amplitude, so as to realize the adaptive robust fusion of multi-source heterogeneous sensor information.

[0195] (5) The present invention determines the occlusion status of the locomotive's current position based on the confidence level of the global odometer and adaptively adjusts the fusion weight of each odometer: when the GNSS-RTK odometer has a brief floating-point solution or occlusion, the cumulative error is effectively reduced by fusing the local absolute position information of the wheel speed inertial odometer and the UWB odometer; in open outdoor scenes and in the environment where the lidar is occluded, the influence of the lidar on the overall positioning is reduced by using the global absolute pose information of the wheel speed inertial odometer and the GNSS-RTK odometer; in the case of GNSS-RTK occlusion and lidar occlusion in front, the effective accuracy of the positioning data is maintained by fusing the local absolute position information of the wheel speed inertial odometer and the UWB odometer.

[0196] As another embodiment of the present invention, a multi-source fusion positioning device 100 for locomotives in dedicated railways is provided to implement the multi-source fusion positioning method for locomotives in dedicated railways described above, wherein, as Figure 9 As shown, it includes:

[0197] The data acquisition module 110 is used to acquire multi-source positioning observation data of the locomotive. The multi-source positioning observation data includes inter-frame pose increment, inter-frame relative pose transformation, and absolute position information. The inter-frame pose increment is obtained by wheel speed inertial odometer. The inter-frame relative pose transformation is obtained by laser odometer after distortion correction of point cloud data based on the inter-frame pose increment and matching and solving with a local map. The absolute position information is obtained by global odometer module, which includes GNSS-RTK odometer and UWB odometer. The GNSS-RTK odometer is used to provide global absolute pose information, and the UWB odometer is used to provide local absolute position information.

[0198] The prediction state module 120 is used to input the multi-source positioning observation data into an adaptive multi-source fusion filter to recursively predict the system state variables and obtain the predicted state estimate of the locomotive.

[0199] The local odometer confidence assessment module 130 is used to construct redundant observation constraints based on the absolute position information of the global odometer module, and compare the predicted state estimate of the locomotive with the redundant observation constraints to realize fault detection and confidence assessment of the wheel speed inertial odometer and laser odometer, and obtain the confidence assessment result of the local odometer.

[0200] The global odometer confidence assessment module 140 is used to determine the confidence of the global odometer based on the operating status of the global odometer module, and adaptively adjust the fusion weight of each odometer based on the confidence assessment results of the local odometers and the confidence of the global odometer.

[0201] The fusion positioning module 150 is used to perform observation update correction on the predicted state estimate of the locomotive based on the multi-source positioning observation data and the fusion weights of each odometer to obtain the final fusion positioning result of the locomotive. Specifically, when the current position of the locomotive is in an occluded environment, the locomotive's pose is updated based on the inter-frame relative pose transformation of the laser odometer and the local absolute position information of the UWB odometer. When the current position of the locomotive is in an unoccluded environment, the locomotive's pose is updated based on the global absolute pose information of the GNSS-RTK odometer.

[0202] The multi-source fusion positioning device for locomotives on dedicated railways provided by this invention acquires multi-source positioning observation data of the locomotive and inputs the multi-source positioning observation data into an adaptive multi-source fusion filter to achieve recursive prediction of system state variables, thereby obtaining the predicted state estimate of the locomotive. Then, based on the global and local cross-validation mechanism in the multi-source positioning observation data, it completes the optimal update of the system pose estimation and covariance matrix. Based on the operating status of the global odometer module, it determines the confidence level of the global odometer and derives the occlusion state of the locomotive's current position. Finally, based on the multi-source positioning observation data and the fusion weights of each odometer, it performs observation update correction on the predicted state estimate of the locomotive to obtain the final fusion positioning result of the locomotive. Based on the occlusion state of the locomotive's current position, it determines the odometer corresponding to the locomotive's pose update under different occlusion environments. This multi-source fusion positioning device for locomotives used in dedicated railways enables multi-source fusion positioning in all scenarios of dedicated railways. The wheel speed inertial odometer serves as the main odometer, providing the prediction basis throughout the entire operation. The global odometer provides absolute position constraints for cross-verification and observation correction. The selection of observation sources in occluded and unoccluded states ensures the continuity and accuracy of positioning in different environments.

[0203] The specific working principle of the multi-source fusion positioning device for locomotives of dedicated railways according to the present invention can be referred to the description of the multi-source fusion positioning method for locomotives of dedicated railways above, and will not be repeated here.

[0204] As another embodiment of the present invention, a multi-source fusion positioning system 10 for locomotives on dedicated railways is provided, wherein, as Figure 10 As shown, it includes: wheel speed inertial odometer 200, laser odometer 300, global odometer module 400 and the locomotive multi-source fusion positioning device 100 for dedicated railways mentioned above. The wheel speed inertial odometer 200, laser odometer 300 and global odometer module 400 are all communicatively connected to the locomotive multi-source fusion positioning device 100 for dedicated railways.

[0205] The wheel speed inertial odometer 200 is used to obtain the locomotive's inter-frame pose increment;

[0206] The laser odometry 300 is used to obtain the inter-frame relative pose transformation of the locomotive;

[0207] The global odometer module 400 is used to obtain the absolute position information of the locomotive;

[0208] The locomotive multi-source fusion positioning device 100 for dedicated railways is used to perform multi-source fusion based on the locomotive's inter-frame pose increment, inter-frame relative pose transformation, and absolute position information, and to evaluate the confidence level of each odometer to obtain the final fusion positioning result.

[0209] The multi-source fusion positioning system for locomotives in dedicated railways provided by this invention adopts the multi-source fusion positioning device for locomotives in dedicated railways described above. It acquires the locomotive's inter-frame pose increment, inter-frame relative pose transformation, and absolute position information, and inputs these information into an adaptive multi-source fusion filter to recursively predict the system state variables, obtaining a predicted locomotive state estimate. Then, based on the global and local cross-validation mechanism in the locomotive's inter-frame pose increment, inter-frame relative pose transformation, and absolute position information, it completes the optimal update of the system pose estimation and covariance matrix. Based on the operating state of the global odometer module, it determines the confidence level of the global odometer and derives the occlusion state of the locomotive's current position. Finally, based on the locomotive's inter-frame pose increment, inter-frame relative pose transformation, and absolute position information, and the fusion weights of each odometer, it performs observation update correction on the predicted locomotive state estimate to obtain the final fusion positioning result. Based on the occlusion state of the locomotive's current position, it determines the odometer corresponding to the locomotive's pose update under different occlusion environments. This multi-source fusion positioning system for locomotives used in dedicated railways achieves multi-source fusion positioning in all scenarios of dedicated railways. The wheel speed inertial odometer serves as the main odometer, providing the prediction basis throughout the entire operation. The global odometer provides absolute position constraints for cross-verification and observation correction. The selection of observation sources in occluded and unoccluded states ensures the continuity and accuracy of positioning in different environments.

[0210] The specific working principle of the multi-source fusion positioning system for locomotives of dedicated railways according to the present invention can be referred to the description of the multi-source fusion positioning method for locomotives of dedicated railways above, and will not be repeated here.

[0211] It is understood that the above embodiments are merely exemplary embodiments used to illustrate the principles of the present invention, and the present invention is not limited thereto. For those skilled in the art, various modifications and improvements can be made without departing from the spirit and essence of the present invention, and these modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. A multi-source fusion positioning method for locomotives on dedicated railways, characterized in that, include: The system acquires multi-source positioning observation data for the locomotive. This multi-source positioning observation data includes inter-frame pose increments, inter-frame relative pose transformations, and absolute position information. The inter-frame pose increments are obtained through wheel speed inertial odometers. The inter-frame relative pose transformations are obtained by laser odometers after distortion correction of point cloud data based on the inter-frame pose increments and matching them with a local map. The absolute position information is obtained through a global odometer module, which includes a GNSS-RTK odometer and a UWB odometer. The GNSS-RTK odometer provides global absolute pose information, and the UWB odometer provides local absolute position information. The multi-source positioning observation data is input into an adaptive multi-source fusion filter to recursively predict the system state variables and obtain the predicted state estimate of the locomotive. Redundant observation constraints are constructed based on the absolute position information of the global odometer module, and the predicted state estimate of the locomotive is compared with the redundant observation constraints to achieve fault detection and confidence assessment of wheel speed inertial odometer and laser odometer, and obtain the confidence assessment result of local odometer. The confidence level of the global odometer is determined based on the operating status of the global odometer module, and the fusion weight of each odometer is adaptively adjusted based on the confidence level evaluation results of the local odometers and the confidence level of the global odometer. The predicted state estimate of the locomotive is updated and corrected based on the multi-source positioning observation data and the fusion weights of each odometry to obtain the final fused positioning result of the locomotive. Specifically, when the current position of the locomotive is in an occluded environment, the locomotive's pose is updated based on the inter-frame relative pose transformation of the laser odometry and the local absolute position information of the UWB odometry. When the current position of the locomotive is in an unoccluded environment, the locomotive's pose is updated based on the global absolute pose information of the GNSS-RTK odometry.

2. The multi-source fusion positioning method for locomotives on dedicated railways according to claim 1, characterized in that, The inter-frame pose increment is obtained through wheel speed inertial odometry and includes: Acquire inertial measurement unit (IMU) measurement data, and perform inter-frame pre-integration processing on the IMU measurement data to obtain inter-frame inertial measurement pre-integration results; Acquire wheel speed meter measurement data and extract the forward speed observation value of the locomotive from the wheel speed meter measurement data; The velocity observation equation is constructed based on the nonholonomic constraint model, and the constraints of the nonholonomic constraint model are determined. The corresponding velocity observation noise covariance matrix is ​​constructed based on the velocity observation equation under the constraints of the nonholonomic constraint model. The constraint of the nonholonomic constraint model is that the lateral velocity observation value and the axial velocity observation value of the locomotive are both set to zero. The noise variance corresponding to the horizontal and upward zero velocities in the velocity observation noise covariance matrix is ​​adaptively adjusted according to the magnitude of the velocity observation residuals. Using the pre-integrated results of the inter-frame inertial measurement as the state prediction, the velocity observation equation, the adaptively adjusted velocity observation noise covariance matrix, and the locomotive's forward velocity observation value are used as observation constraints to perform extended Kalman filtering state update, thereby obtaining the inter-frame pose increment.

3. The multi-source fusion positioning method for locomotives on dedicated railways according to claim 1, characterized in that, The inter-frame relative pose transformation is obtained by laser odometry after performing distortion correction processing on the point cloud data based on the inter-frame pose increment and matching it with the local map, including: Acquire multi-line lidar point cloud data, and calculate the locomotive pose change at each scanning point acquisition time by linear interpolation based on the inter-frame pose increment obtained from the wheel speed inertial odometer. Based on the locomotive pose changes at each scanning point acquisition time, all scanning points are uniformly transformed to the same coordinate system at the same time to obtain the distortion-free point cloud. Extract line feature point sets and surface feature point sets from the distortion-free point cloud to form the current feature point cloud; The current frame feature point cloud is transformed to the previous frame coordinate system using the inter-frame pose transformation of the wheel speed inertial odometer as the initial estimate, and then matched with the feature point cloud of the previous frame to obtain the inter-frame relative pose transformation of the laser odometer. The inter-frame pose transformation of the wheel speed inertial odometer is obtained by the inter-frame pose increment transformation of the wheel speed inertial odometer. The current feature point cloud is transformed to the world coordinate system based on the inter-frame relative pose transformation of the laser odometry to construct a local map; Search for the corresponding point set of each feature point in the current feature point cloud in the local map, and iteratively match the corresponding point set with each feature point in the current frame feature point cloud to update the inter-frame relative pose transformation.

4. The multi-source fusion positioning method for locomotives on dedicated railways according to claim 1, characterized in that, The absolute position information is obtained through the global odometer module and includes: The locomotive's GNSS-RTK odometer is acquired in the WGS84 coordinate system, along with its latitude, longitude, altitude, and angle information. This information is then converted to the UTM Cartesian coordinate system to obtain the locomotive's global absolute pose information, which includes the locomotive's absolute position and heading angle. UWB base stations are calibrated to synchronize the location of UWB base stations with the location of GNSS-RTK odometry, and to obtain the absolute position information of each UWB base station in the world coordinate system. The locomotive's local absolute position information is obtained by the UWB odometer based on the distance data between the UWB tag on the locomotive and each UWB base station. The local absolute position information of the locomotive includes the absolute position of the locomotive.

5. The multi-source fusion positioning method for locomotives on dedicated railways according to any one of claims 1 to 4, characterized in that, The multi-source positioning observation data is input into an adaptive multi-source fusion filter to recursively predict the system state variables, thereby obtaining a predicted state estimate of the locomotive, including: An adaptive multi-source fusion filter based on error state Kalman filtering is constructed, and system state variables are defined, including the locomotive's position, speed, attitude, and the zero bias of the acceleration of the inertial measurement unit and the zero bias of the gyroscope. The error state equation is constructed based on the kinematic model of the locomotive, and the state covariance matrix and process noise matrix are initialized. The multi-source positioning observation data is input into the adaptive multi-source fusion filter; Based on the error state equation, the system state variables are recursively predicted using the inter-frame pose increment in the multi-source positioning observation data as the driving force, and the state covariance matrix and process noise matrix are synchronously recursively updated to obtain the predicted state estimate of the locomotive.

6. The multi-source fusion positioning method for locomotives on dedicated railways according to any one of claims 1 to 4, characterized in that, Redundant observation constraints are constructed based on the absolute position information of the global odometer module, and the predicted state estimate of the locomotive is compared with the redundant observation constraints to achieve fault detection and confidence assessment of the wheel speed inertial odometer and laser odometer, obtaining the confidence assessment result of the local odometer, including: Based on the absolute position information output by the global odometer module, the low-frequency absolute position information of the global odometer is interpolated onto the high-frequency timestamp of the local odometer through linear interpolation, resulting in a global redundant observation sequence that is aligned with the local odometer time. The predicted state estimate of the locomotive is compared with the global redundant observation sequence to calculate the pose deviation of the wheel speed inertial odometer and the laser odometer. When the pose deviation of the wheel speed inertial odometer and / or laser odometer exceeds a preset pose deviation threshold, it is determined that the corresponding local odometer is faulty, and the confidence level of the faulty local odometer is adaptively reduced according to the amplitude of the pose deviation to obtain the confidence level evaluation result of the local odometer.

7. The multi-source fusion positioning method for locomotives on dedicated railways according to any one of claims 1 to 4, characterized in that, The confidence level of the global odometer is determined based on the operating status of the global odometer module, and the fusion weights of each odometer are adaptively adjusted based on the confidence level evaluation results of the local odometers and the confidence level of the global odometer, including: The confidence level of the GNSS-RTK odometry is determined based on its status and the number of satellites searched. Specifically, a high confidence level is set when the GNSS-RTK odometry status is a fixed solution, a medium confidence level is set when the GNSS-RTK odometry status is a floating-point solution, and an unreliable level is set when the GNSS-RTK odometry status is a single-point solution or the number of satellites searched is less than a preset number of satellites searched. The confidence level of the UWB odometer is determined based on the number of UWB base station distance data received by the UWB odometer. When the number of UWB base station distance data received by the UWB odometer is less than a preset number of distance data, it is set as unreliable. The occlusion status of the locomotive's current position is determined based on the confidence levels of the GNSS-RTK odometer and the UWB odometer. When the GNSS-RTK odometer is set to untrusted and the UWB odometer is not untrusted, the locomotive's current position is determined to be an obstructed area. When the GNSS-RTK odometer reading is high or medium confidence, the locomotive's current position is determined to be an unobstructed area; The fusion weights of each odometer are adaptively adjusted based on the occlusion status of the locomotive's current position, the confidence evaluation results of the local odometer, and the confidence of the global odometer.

8. The multi-source fusion positioning method for locomotives on dedicated railways according to any one of claims 1 to 4, characterized in that, The predicted state estimate of the locomotive is updated and corrected based on the multi-source positioning observation data and the fusion weights of each odometer to obtain the final fused positioning result of the locomotive, including: Available observation sources are determined based on the occlusion status of the locomotive's current position. Specifically, when the locomotive's current position is in an occluded environment, the inter-frame relative pose transformation of the laser odometry and the local absolute position information of the UWB odometry are determined as available observation sources. When the locomotive's current position is in an unoccluded environment, the global absolute pose information of the GNSS-RTK odometry is determined as available observation sources. Construct the corresponding observation equations based on the available observation sources; The observation noise covariance matrix corresponding to the observation equation is adjusted according to the fusion weight of each odometer; The Kalman gain is calculated based on the observation equation, the adjusted observation noise covariance matrix, and the predicted state estimate of the locomotive. The predicted state estimate of the locomotive is corrected and updated based on the Kalman gain to obtain the final fusion positioning result of the locomotive.

9. A multi-source fusion positioning device for locomotives on dedicated railways, used to implement the multi-source fusion positioning method for locomotives on dedicated railways as described in any one of claims 1 to 8, characterized in that, include: The data acquisition module is used to acquire multi-source positioning observation data of the locomotive. The multi-source positioning observation data includes inter-frame pose increment, inter-frame relative pose transformation, and absolute position information. The inter-frame pose increment is obtained by wheel speed inertial odometer. The inter-frame relative pose transformation is obtained by laser odometer after distortion correction of point cloud data based on the inter-frame pose increment and matching and solving with a local map. The absolute position information is obtained by global odometer module, which includes GNSS-RTK odometer and UWB odometer. The GNSS-RTK odometer is used to provide global absolute pose information, and the UWB odometer is used to provide local absolute position information. The prediction state module is used to input the multi-source positioning observation data into an adaptive multi-source fusion filter to recursively predict the system state variables and obtain the predicted state estimate of the locomotive. The local odometer confidence assessment module is used to construct redundant observation constraints based on the absolute position information of the global odometer module, and compare the predicted state estimate of the locomotive with the redundant observation constraints to realize fault detection and confidence assessment of wheel speed inertial odometer and laser odometer, and obtain the confidence assessment result of local odometer. The global odometer confidence assessment module is used to determine the confidence of the global odometer based on the operating status of the global odometer module, and adaptively adjust the fusion weight of each odometer based on the confidence assessment results of the local odometers and the confidence of the global odometer. The fusion positioning module is used to perform observation update correction on the predicted state estimate of the locomotive based on the multi-source positioning observation data and the fusion weights of each odometer to obtain the final fusion positioning result of the locomotive. Specifically, when the current position of the locomotive is in an occluded environment, the locomotive's pose is updated based on the inter-frame relative pose transformation of the laser odometer and the local absolute position information of the UWB odometer. When the current position of the locomotive is in an unoccluded environment, the locomotive's pose is updated based on the global absolute pose information of the GNSS-RTK odometer.

10. A multi-source fusion positioning system for locomotives on dedicated railways, characterized in that, include: The wheel speed inertial odometer, laser odometer, global odometer module, and the locomotive multi-source fusion positioning device for dedicated railway as described in claim 9, wherein the wheel speed inertial odometer, laser odometer, and global odometer module are all communicatively connected to the locomotive multi-source fusion positioning device for dedicated railway. The wheel speed inertial odometry is used to obtain the locomotive's inter-frame pose increment; The laser odometry is used to obtain the locomotive's inter-frame relative pose transformation; The global odometer module is used to obtain the absolute position information of the locomotive; The locomotive multi-source fusion positioning device for dedicated railways is used to perform multi-source fusion based on the locomotive's inter-frame pose increment, inter-frame relative pose transformation, and absolute position information, and to evaluate the confidence level of each odometer to obtain the final fusion positioning result.

Images