A crane track inertial combined measurement method and system based on motion and scene constraints

Abstract

The application discloses a crane track inertia combined measurement method and system based on motion and scene constraints. The method synchronously collects IMU, an odometer, RTK, laser ranging, control point and revisit point data by detecting a carrier, and then constructs inertia constraints based on IMU pre-integration, and combines zero speed, attitude locking, non-integrity, mileage increment and other motion constraints and control point attachment, revisit consistency, laser ranging and multiple measurement weighting and other scene constraints to perform combined calculation. Meanwhile, the constraint weight is adaptively adjusted according to acceleration, angular velocity and residual characteristics, and track parameters such as track gauge, horizontal height difference, straightness and joint misalignment are output through EKF, graph optimization, RTS reverse smoothing and global adjustment. The scheme can improve track dynamic measurement efficiency, suppress inertia error divergence and reduce high-altitude artificial detection risk.

Description

Technical Field

[0001] This invention relates to the field of crane inspection technology, and in particular to a method and system for measuring the combined inertia of crane tracks based on motion and scene constraints. Background Technology

[0002] Crane rails are the fundamental load-bearing and guiding structure for the operation of large lifting equipment such as bridge cranes, gantry cranes, quay cranes, and telescopic cranes. Their geometric parameters, such as track gauge, horizontal elevation difference, straightness, rail top elevation variation, and track joint misalignment, directly affect the crane's operational stability, wheel wear, structural fatigue life, and operational safety. With the increasing frequency of crane operation in large factories, ports, steel metallurgy, and logistics warehousing environments, long-term service of crane rails can easily lead to problems such as foundation settlement, track misalignment, track joint misalignment, localized wear, and loose connections. Therefore, regular inspection and condition assessment of crane rails are necessary.

[0003] Current crane track inspection methods typically rely on manual point-by-point measurements using tools such as total stations, levels, track gauges, and laser collimators. While this method can achieve high accuracy at local measurement points, it requires inspectors to set up, aim, read, and record measurements near the elevated track beams, factory roof trusses, or crane travel areas. This process is cumbersome, labor-intensive, and inefficient, and carries safety and production risks such as falls from heights, equipment interference, and prolonged downtime. For long-distance or multi-span tracks, manual point-by-point measurements are also susceptible to variations in measurement point spacing, line-of-sight obstruction, operator experience, and changes in the site environment, making it difficult to generate a continuous, dense, and dynamic sequence of track geometric parameters.

[0004] In recent years, inertial navigation measurement technology has been explored for the inspection of linear structures such as tracks, pipe racks, and roads. Inertial measurement units (IMUs) can continuously output triaxial acceleration and triaxial angular velocity, offering advantages such as high sampling frequency, no need for external line-of-sight requirements, and suitability for dynamic continuous measurement. However, when relying solely on inertial measurement for crane track inspection, IMU integration errors accumulate rapidly over time, leading to drift in position, velocity, and attitude estimates. Particularly during long-distance inspections, even small zero-bias errors can be amplified through multiple integrations, causing the calculated track geometry parameters to gradually deviate from the actual track condition.

[0005] Furthermore, crane track inspection sites typically present significant engineering-related interference. For instance, when the inspection vehicle passes over track welds, pressure plate areas, misaligned joints, worn rail surfaces, or locally contaminated areas, the IMU (Installation Unit) is affected by instantaneous impacts and high-frequency vibrations. Factors such as wheel slippage, rail surface deposits, and localized track deformation can also cause anomalies in odometer or laser ranging observations. If these abnormal data are weighted equally with normal data during combined measurements, it can easily lead to abrupt changes in state estimation, causing localized false anomalies in parameters such as track straightness, gauge, and elevation difference, thus reducing the stability and reliability of the measurement results.

[0006] While some existing combined measurement schemes incorporate auxiliary information such as odometers, RTK, or laser ranging, they typically only employ simple filtering or local correction methods to handle multi-source observations, failing to fully utilize the inherent motion and scene constraints in crane track inspection. For example, when the inspection vehicle moves along the track, its lateral and vertical velocities are limited; zero-velocity constraints can be formed when the inspection vehicle is stationary or in low-speed sections; track control points, revisit points, and multiple measurement sections can provide globally consistent constraints; and the track gauge, elevation difference, and relative positional relationship between the left and right rails can also provide effective constraints for solving track geometric parameters. If these constraints are not systematically incorporated into the inertial combined measurement model, it will be difficult to effectively suppress inertial error divergence and error propagation caused by local abnormal observations.

[0007] Therefore, there is an urgent need to propose a crane track inertia combined measurement method and system based on motion and scene constraints that can integrate multi-source information to improve track measurement efficiency, reduce the risks of manual high-altitude operations, and achieve high-quality track geometric parameter measurement. Summary of the Invention

[0008] In view of this, the purpose of this invention is to propose a crane track inertial combined measurement method and system based on motion and scene constraints. This solution integrates multi-source information such as IMU, odometer, RTK, laser ranging, control points and revisit points, and combines motion constraints and scene constraints in the crane track detection process. It can also perform robust adaptive processing on dynamic disturbances and abnormal observations to improve track measurement efficiency, reduce the risk of manual high-altitude operations, and achieve long-distance, continuous, high-precision and high-consistency track geometric parameter measurement.

[0009] To achieve the above-mentioned technical objectives, the technical solution adopted by this invention is as follows: A combined inertial measurement method for crane tracks based on motion and scene constraints is disclosed. The crane track is equipped with a detection carrier that runs along the track. The detection carrier is equipped with an inertial measurement unit, an odometer, a laser ranging module, an RTK positioning module, and a unified timeline acquisition unit. The method includes the following steps: S1. When the detection carrier runs along the crane track, the unified timeline acquisition unit collects and records inertial measurement data, odometer incremental data, laser ranging data, RTK positioning data, control point data and revisit point data simultaneously to form a multi-source synchronous measurement data sequence. S2. Calibrate the external parameters of each sensor relative to the carrier coordinate system according to preset conditions, and compensate for the time deviation of each sensor sampling time, so as to convert the multi-source synchronous measurement data sequence to a unified track measurement reference. S3. Perform IMU pre-integration based on the inertial measurement data between adjacent keyframes, construct preset state variables, and establish inertial constraint factors based on the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment. S4. Based on the characteristics of the restricted movement of the detection carrier along the crane track, establish the motion constraint factor; S5. Establish scenario constraint factors based on control points, revisit points, laser ranging results, and multiple measurement results in the crane track detection scenario; S6. Calculate the dynamic excitation index based on the acceleration characteristics, angular velocity characteristics, jerk characteristics, and residual characteristics of each constraint factor output by the inertial measurement unit, and adaptively adjust the weights of the motion constraint factor and the scene constraint factor based on the dynamic excitation index. S7. Input the inertial constraint factor, the motion constraint factor adjusted by adaptive weight, and the scene constraint factor adjusted by adaptive weight into the combined solution model, perform real-time state recursion through extended Kalman filtering, and perform consistency optimization of key frame states through graph optimization to obtain the optimized trajectory of the detection vehicle. S8. Perform RTS reverse smoothing and global adjustment on the optimized trajectory to correct the accumulated error of long-distance measurement and obtain a globally consistent track measurement trajectory. S9. Calculate the geometric parameters of the crane track based on the globally consistent track measurement trajectory and laser ranging data.

[0010] As one possible implementation, further, in step S1 of this solution, the multi-source synchronous measurement data sequence is represented as: in, Indicates the first A collection of multi-source measurement data at each sampling time. Indicates triaxial acceleration, Indicates the angular velocity of the three axes. Indicates the odometer increment. Represents laser ranging data, This represents RTK positioning data. This represents control point data or revisit point data. This represents a unified timestamp.

[0011] As one possible implementation, step S2 of this scheme further includes: establishing a navigation coordinate system, a carrier coordinate system, an inertial measurement unit coordinate system, an odometer coordinate system, a laser ranging coordinate system, and an orbit coordinate system; calibrating the external parameters of each sensor relative to the carrier coordinate system; compensating for time deviations in the sampling time of each sensor; and converting the multi-source synchronous measurement data sequence to a unified orbit measurement reference. In step S2, the extrinsic parameters of each sensor relative to the carrier coordinate system are expressed as follows: in, Indicates the first The pose transformation matrix of the sensor-like coordinate system relative to the carrier coordinate system. Represents the rotation matrix. Represents the translation vector; The sensor sampling time is also compensated, and the compensation formula is defined as follows: in, Indicates the first The original sampling time of the sensor-like sensor Indicates the first Time deviation of a sensor relative to a unified time reference.

[0012] As one possible implementation, further, in step S3 of this solution, the preset state variables include position, velocity, attitude, accelerometer bias, gyroscope bias and / or odometer proportional error coefficient; In step S3, the state quantity Represented as: in, Indicates location, Indicates speed, Represents attitude quaternions, This indicates that the accelerometer has zero bias. This indicates that the gyroscope has zero bias. This represents the proportional error coefficient of the odometer; The IMU pre-integration includes calculating the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment between adjacent keyframes, and constructing the inertial residual between adjacent keyframes using the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment.

[0013] As one possible implementation, further, in step S4 of this solution, the motion constraint factors include a zero-speed constraint factor, an attitude lock constraint factor, a non-integrity constraint factor, and / or an odometry increment constraint factor. The zero-speed constraint factor is used to constrain the speed state when the detection carrier is in a stationary state or a low-speed dwell state. The attitude locking constraint factor is used to constrain the changes in roll angle, pitch angle or heading angle of the detection vehicle in the stable track section. The non-integrity constraint factor is used to constrain the velocity components of the detection vehicle in the lateral and vertical directions of the track. The odometer increment constraint factor is used to constrain the displacement increment along the track longitudinal direction between adjacent keyframes.

[0014] As one possible implementation, further, in step S5 of this solution, the scene constraint factors include control point adherence constraint factors, revisit point pose consistency constraint factors, laser ranging constraint factors, and / or multiple measurement weighted constraint factors. The control point binding constraint factor is constructed by the difference between the control point coordinates and the estimated position of the detection vehicle. The revisit point pose consistency constraint factor is constructed by detecting the position difference and attitude difference when the carrier passes through the same track point multiple times. The laser ranging constraint factor is constructed by the difference between the laser ranging observation value and the distance value predicted by the detection carrier state and the external parameters of the laser ranging module. The weighted constraint factor for multiple measurements is constructed by weighting the consistency of multiple measurement results in the same orbital segment.

[0015] As a preferred implementation method, in step S6 of this solution, the dynamic disturbance index is expressed as: in, Indicates the first Dynamic excitation index at each moment This represents the average acceleration within the sliding window. This represents the average angular velocity within the sliding window. Indicates the standard deviation of acceleration. Indicates the standard deviation of angular velocity. This represents the standard deviation of jerk. This represents the current constraint factor residual. Represents the standard deviation of the residuals. Denotes the weight coefficients, and satisfies .

[0016] As a possible implementation, further, in step S6 of this solution, the constraint factor weights are determined according to the dynamic disturbance index in the following manner: in, Indicates the first The weights of each constraint factor, This indicates the threshold for mild perturbation. This indicates the severe perturbation threshold. Indicates the first The dynamic excitation index at each moment; when When the corresponding constraint factors participate normally in the combination solution, When the corresponding constraint factor is reduced in weight, it participates in the combination solution. The corresponding constraint factor is removed from the current optimization window.

[0017] As one possible implementation, further, in step S9 of this scheme, the geometric parameters of the crane track include the coordinates of the track centerline, track gauge, track horizontal height difference, track straightness, track top elevation change and / or track joint misalignment.

[0018] Based on the above, this solution also proposes a crane track inertial combined measurement system based on motion and scene constraints, which includes: The multi-source data acquisition module is used to acquire inertial measurement data, odometry incremental data, laser ranging data, RTK positioning data, control point data, and revisit point data. The time synchronization module is used to synchronize the time of data from multiple sources. The coordinate calibration module is used for extrinsic parameter calibration and coordinate transformation of multi-source data; The inertial pre-integration module is used to calculate the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment based on the inertial measurement data between adjacent keyframes, and to construct the inertial constraint factor. The motion constraint construction module is used to construct zero-speed constraint factors, attitude lock constraint factors, non-integrity constraint factors, and odometry increment constraint factors. The scene constraint construction module is used to construct control point adherence constraint factors, revisit point pose consistency constraint factors, laser ranging constraint factors, and multiple measurement weighted constraint factors. The robust adaptive fusion module is used to adjust the weights of motion constraint factors and scene constraint factors based on dynamic perturbation indicators; The combined solution module is used to obtain the optimized trajectory of the detection carrier based on extended Kalman filtering and graph optimization; The smoothing and adjustment module is used to perform RTS reverse smoothing and global adjustment on the optimized trajectory; The track parameter output module is used to output the coordinates of the track centerline, track gauge, track horizontal elevation difference, track straightness, track top elevation change, and track joint misalignment.

[0019] By adopting the above technical solution, the present invention has the following beneficial effects compared with the prior art: This solution integrates an IMU, odometer, RTK positioning module, laser ranging module, and unified timeline acquisition unit onto the detection carrier, enabling the carrier to continuously move along the crane track and automatically collect multi-source measurement data. Compared to the traditional method of manually setting up measurement points, aiming, reading, and recording data, this solution enables dynamic and continuous measurement of track geometry parameters, reducing manual climbing, stopping, and close contact with the track, thereby reducing the intensity and safety risks of high-altitude operations.

[0020] In addition, this scheme does not simply rely on the IMU for position and attitude integration. Instead, based on the characteristics of the crane track detection vehicle's restricted motion along the track, it introduces zero-speed constraints, attitude locking constraints, non-integrity constraints, and odometer increment constraints. These constraints can correct for speed drift, attitude drift, and track displacement errors. In particular, they can limit the unreasonable drift of the detection vehicle in the lateral and vertical directions of the track, thereby improving the stability of the inertial combined measurement.

[0021] To enhance the global consistency of long-distance measurements through scene constraints, this scheme constructs scene constraint factors using control points, revisit points, laser ranging results, and multiple measurement results. This incorporates the objectively existing spatial benchmarks and repeated observation relationships within the track inspection site into the combined solution model. Control point alignment constraints bind the inspection trajectory to the global benchmark; revisit point pose consistency constraints correct closure errors; weighted constraints from multiple measurements improve the consistency of repeatedly inspected sections; and laser ranging constraints enhance the computational reliability of track edge, gauge, and elevation difference parameters. This reduces cumulative errors in long-distance measurements and improves the overall consistency of track geometric parameters.

[0022] This scheme also establishes a dynamic excitation index based on acceleration characteristics, angular velocity characteristics, jerk characteristics, and constraint residual characteristics, and adaptively adjusts the weights of each constraint factor according to this index. When the detection vehicle passes through track welds, joint misalignments, wear zones, or high-frequency vibration sections, the system can identify the impact of abnormal excitations on IMU, odometer, or laser ranging data, and reduce or eliminate abnormal constraints to avoid abnormal observations contaminating the overall solution results. This improves the robustness of track detection in complex field environments.

[0023] To improve data accuracy, this solution employs a combination of extended Kalman filtering and graph optimization. Extended Kalman filtering is used for real-time state recursion, meeting the real-time output requirements for trajectory and parameters during on-site detection. Graph optimization integrates inertial constraints, motion constraints, and scene constraints into a unified global objective function, improving the consistency and accuracy of keyframe state estimation. Furthermore, RTS reverse smoothing and global adjustment correct for insufficient unidirectional filtering, further reducing cumulative errors in long-distance detection.

[0024] This solution can also continuously calculate the track centerline coordinates, track gauge, horizontal elevation difference, track top elevation change, straightness deviation, and track joint misalignment based on the optimized detection trajectory and laser ranging data, no longer limited to discrete measurement point results. The detection results can generate continuous curves, abnormal section markers, and re-measurement suggestions, facilitating maintenance personnel to quickly locate abnormal track areas and providing data support for crane track adjustment, maintenance, and safety assessment.

[0025] In terms of engineering adaptability and scalability, this solution establishes a combined measurement framework of "inertial pre-integration - motion constraint - scenario constraint - robust adaptive fusion - smooth adjustment". This framework is not only applicable to crane tracks of different models, spans and operating scenarios, but can also be extended to linear structure inspection scenarios such as elevator guide rails, construction hoist guide rails, roller coaster tracks and factory logistics tracks, and has good versatility and engineering promotion value. Attached Figure Description

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

[0027] Figure 1 This is a simplified implementation flowchart of the measurement method in this scheme; Figure 2 This is a schematic diagram of the internal unit module connection of the multi-source data acquisition module deployed on the detection carrier in this solution. Other basic existing technology components used to realize the module's function are hidden, such as power supply and communication interface. Figure 3 This is a schematic diagram of the unit module connections of the measurement system in this scheme. Detailed Implementation

[0028] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be particularly noted that the following embodiments are for illustrative purposes only and do not limit the scope of the invention. Similarly, the following embodiments are only some, not all, embodiments of the present invention, and all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0029] Combination Figure 1 As shown, this embodiment proposes a combined inertial measurement method for crane tracks based on motion and scene constraints. A detection carrier running along the crane track is deployed on the track. The detection carrier is equipped with an inertial measurement unit, an odometer, a laser ranging module, an RTK positioning module, and a unified timeline acquisition unit (see reference). Figure 2 As shown), the method of this solution includes the following steps: S1. When the detection carrier runs along the crane track, the unified timeline acquisition unit collects and records inertial measurement data, odometer incremental data, laser ranging data, RTK positioning data, control point data and revisit point data simultaneously to form a multi-source synchronous measurement data sequence. S2. Calibrate the external parameters of each sensor relative to the carrier coordinate system according to preset conditions, and compensate for the time deviation of each sensor sampling time, so as to convert the multi-source synchronous measurement data sequence to a unified track measurement reference. S3. Perform IMU pre-integration based on the inertial measurement data between adjacent keyframes, construct preset state variables, and establish inertial constraint factors based on the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment. S4. Based on the characteristics of the restricted movement of the detection carrier along the crane track, establish the motion constraint factor; S5. Establish scenario constraint factors based on control points, revisit points, laser ranging results, and multiple measurement results in the crane track detection scenario; S6. Calculate the dynamic excitation index based on the acceleration characteristics, angular velocity characteristics, jerk characteristics, and residual characteristics of each constraint factor output by the inertial measurement unit, and adaptively adjust the weights of the motion constraint factor and the scene constraint factor based on the dynamic excitation index. S7. Input the inertial constraint factor, the motion constraint factor adjusted by adaptive weight, and the scene constraint factor adjusted by adaptive weight into the combined solution model, perform real-time state recursion through extended Kalman filtering, and perform consistency optimization of key frame states through graph optimization to obtain the optimized trajectory of the detection vehicle. S8. Perform RTS reverse smoothing and global adjustment on the optimized trajectory to correct the accumulated error of long-distance measurement and obtain a globally consistent track measurement trajectory. S9. Calculate the geometric parameters of the crane track based on the globally consistent track measurement trajectory and laser ranging data.

[0030] Traditional crane track measurement relies on manual point-by-point deployment and discrete readings, making it difficult to obtain continuous dynamic information on the track geometry. At the same time, different sensors have different sampling frequencies, different time bases, and different data formats. If they are not collected and synchronized in a unified manner, it will lead to time misalignment between subsequent inertial calculations, mileage constraints, laser ranging constraints, and control point constraints, thereby introducing errors in track position calculation.

[0031] Therefore, step S1 of this scheme provides the original data foundation for subsequent coordinate unification, IMU pre-integration, motion constraint modeling, scene constraint modeling and combinatorial optimization solution by constructing a multi-source measurement data sequence under a unified time line.

[0032] This solution involves installing an inertial measurement unit (IMU), an odometer, a laser ranging module, an RTK positioning module, and a unified timeline acquisition unit on a detection vehicle running along the crane track. The IMU collects triaxial acceleration and angular velocity data; the odometer collects the longitudinal travel increment of the detection vehicle along the track; the laser ranging module collects the distance from the detection vehicle to the track side, track top surface, or adjacent track; the RTK positioning module collects the absolute coordinates near control points or the positioning information of track sections; and the unified timeline acquisition unit timestamps, buffers, and synchronizes the data from different sensors.

[0033] During the operation of the detection carrier along the crane track, data is collected and recorded synchronously by the unified timeline acquisition unit. At each sampling time, the following multi-source measurement data set is formed: in, Indicates the first A collection of multi-source measurement data at each sampling time. This represents the triaxial acceleration, specifically the triaxial acceleration vector output by the inertial measurement unit. This represents the three-axis angular velocity, i.e., the three-axis angular velocity vector output by the inertial measurement unit; This indicates the odometer increment, that is, the increment of travel output by the odometer; This represents laser ranging data, specifically the distance measurement output by the laser ranging module. This represents RTK positioning data, specifically the position observations output by the RTK positioning module. This indicates control point data or revisit point data, i.e., control point identifier, revisit point identifier, or track scene identifier; This indicates a unified timestamp, which is the unified timestamp assigned by the unified timeline acquisition unit.

[0034] Because multi-sensor data acquisition involves differences in sampling frequencies—for example, IMUs typically use high-frequency sampling, while odometers, laser rangefinders, and RTKs use medium- to low-frequency sampling—a unified timestamp is required. Based on this, data from different sampling times are interpolated or resampled onto the same timeline.

[0035] For a certain sensor At the original moment and Observations were obtained respectively and If unified timestamp satisfy: Then, the synchronous observations of the sensor at a unified timestamp can be obtained through linear interpolation: in, Indicates the first Sensor-like devices at a unified timestamp Synchronous observations below; , They represent the first The observations of the sensor at adjacent raw sampling times; , They represent the first Adjacent raw sampling times of the sensor-like device; This represents a unified timestamp.

[0036] In this scheme, when the interval between adjacent samplings is short, the sensor observations can be approximately linearly varied within a local time period. Therefore, by using the interpolation method described above, sensor observations from different frequencies and sampling times can be unified to the same time node, reducing observation mismatches caused by time asynchrony.

[0037] Through step S1, this scheme obtains a multi-source synchronous measurement data sequence. Its definition is: Among them, the definition For the first A multi-source measurement data set, This represents the total number of data collection moments.

[0038] Since IMU, odometer, laser ranging module, and RTK antenna are usually installed at different locations on the detection vehicle and their respective coordinate axes are different, directly fusing their observation data would result in the inability to establish a unified geometric relationship between position, attitude, distance, and odometer increments.

[0039] Therefore, in step S2 of this scheme, a unified orbit measurement coordinate system is established, and through external parameter calibration and time compensation, the multi-source synchronous data obtained in step S1 is transformed into a unified orbit coordinate system, providing a unified coordinate basis for subsequent IMU pre-integration, constraint construction and combined solution.

[0040] As one possible implementation, step S2 of this scheme further includes: establishing a navigation coordinate system, a carrier coordinate system, an inertial measurement unit coordinate system, an odometer coordinate system, a laser ranging coordinate system, and an orbit coordinate system; calibrating the external parameters of each sensor relative to the carrier coordinate system; compensating for time deviations in the sampling time of each sensor; and converting the multi-source synchronous measurement data sequence to a unified orbit measurement reference. In step S2, the extrinsic parameters of each sensor relative to the carrier coordinate system are expressed as follows: in, Indicates the first The pose transformation matrix of the sensor-like coordinate system relative to the carrier coordinate system. Represents the rotation matrix. Represents the translation vector; The sensor sampling time is also compensated, and the compensation formula is defined as follows: in, Indicates the first The original sampling time of the sensor-like sensor Indicates the first Time deviation of a sensor relative to a unified time reference.

[0041] As an example of implementation, step S2 of this solution includes the following sub-steps: Establish navigation coordinate system Carrier coordinate system Inertial Measurement Unit Coordinate System Odometer coordinate system Laser ranging coordinate system RTK antenna coordinate system and orbital coordinate system .

[0042] Among them, the orbital coordinate system Preferably, the direction of the track design centerline is taken as The axis is defined by the track gauge direction. The axis is the vertical direction. The track coordinate system can directly express track displacement, gauge deviation, horizontal elevation difference, and track straightness deviation.

[0043] For any sensor Its extrinsic parameters relative to the carrier coordinate system are expressed as: in, Indicates the first Homogeneous pose transformation matrix of sensor-like coordinate system relative to carrier coordinate system; Indicates the first The rotation matrix of the sensor-like coordinate system relative to the carrier coordinate system; Indicates the first The translation vector of the sensor's coordinate origin relative to the carrier's coordinate origin; It can represent an IMU, odometer, laser ranging module, or RTK antenna.

[0044] The coordinates of the measuring point obtained by the sensor in its own coordinate system are as follows .

[0045] If it is necessary to transform it to the orbit coordinate system, the transformation should be performed sequentially from the sensor coordinate system to the carrier coordinate system, from the carrier coordinate system to the navigation coordinate system, and from the navigation coordinate system to the orbit coordinate system, to obtain: in, Indicates the coordinates of the measuring point in the orbital coordinate system; Indicates the coordinates of the measuring point in the sensor coordinate system; The transformation matrix represents the transformation from the sensor coordinate system to the carrier coordinate system; The transformation matrix represents the transformation from the vehicle coordinate system to the navigation coordinate system; This represents the transformation matrix from the navigation coordinate system to the orbital coordinate system.

[0046] Due to the aforementioned external parameters To represent the pose relationship between the sensor coordinate system and the carrier coordinate system, it can be defined as follows: The reasoning behind this transformation is based on the principle of rigid body coordinate transformation, which states that the representation of the same spatial point in different coordinate systems can be unified through rotation and translation. Through this transformation, IMU attitude, odometer displacement, laser ranging points, and RTK positioning points can be mapped to the orbital coordinate system.

[0047] Even when different sensors record data using a unified timestamp, hardware triggering delays or communication delays may still exist. To eliminate this effect, time deviation compensation is performed on the (s)th type of sensor: in, Indicates the first The actual sampling time of the sensor-like device; Indicates the target time under a unified time base; Indicates the first Time deviation of a sensor relative to a unified time reference.

[0048] If the detection carrier has a time deviation Memory speed If the uncompensated time deviation is not compensated, it will cause an approximate spatial error, which is defined as: in, This indicates spatial error caused by time deviation; Indicates the detection vector in the first The velocity magnitude at time t.

[0049] As can be seen from the above formula, when the detection carrier is in a dynamic measurement state, even a small time deviation will cause spatial position error. Therefore, time compensation needs to be completed in step S2.

[0050] In this step, the multi-source synchronous measurement data sequence output in step S1 is... After unifying coordinates and time references, a standardized measurement data sequence is output, defined as follows: in, Indicates the first Standardized observation data at each sampling time under the orbital coordinate reference, defined For the first A standardized multi-source measurement dataset The total number of acquisition times; this standardized data sequence serves as the input for IMU pre-integration in subsequent step S3, and also as the data basis for motion constraint modeling in step S4 and scene constraint modeling in step S5.

[0051] In this scheme, the IMU can provide continuous acceleration and angular velocity information, but if global optimization is performed directly on each high-frequency sampling point, the computational load will be too large; if only simple integration is performed, the accelerometer bias and gyroscope bias will accumulate over time and result in significant position and attitude drift.

[0052] To address the issues of high computational cost of IMU high-frequency integration, noise accumulation over time, and difficulty in maintaining stable inertial states between adjacent keyframes, step S3 of this scheme utilizes IMU pre-integration to compress high-frequency inertial observations into position, velocity, and attitude increments between adjacent keyframes, and constructs inertial constraint factors to provide continuous motion priors for subsequent combined solutions.

[0053] As an example of implementation, step S3 of this solution includes the following sub-steps: In step S3 of this scheme, the preset state variables include position, velocity, attitude, accelerometer bias, gyroscope bias and / or odometer proportional error coefficient. Wherein, the state quantity Represented as: in, Indicates the first State variables of each keyframe; Indicates position, that is, the position of the detection vehicle under the track measurement reference; Indicates speed, specifically the speed of the detection carrier; This represents the attitude quaternion, i.e., the attitude quaternion of the detection vehicle; This indicates that the accelerometer has zero bias. This indicates that the gyroscope has zero bias. This represents the proportional error coefficient of the odometer.

[0054] In this scheme, the IMU pre-integration includes calculating the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment between adjacent keyframes, and constructing the inertial residual between adjacent keyframes using the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment.

[0055] In this scheme, the raw IMU observations can be expressed as: in, This represents the acceleration measured by the IMU; Represents actual acceleration; This indicates that the accelerometer has zero bias. This indicates random noise from the accelerometer; This represents the angular velocity measured by the IMU; Represents the true angular velocity; This indicates that the gyroscope has zero bias. This represents random noise from the gyroscope.

[0056] Therefore, the acceleration and angular velocity after zero bias correction are: in, This represents the acceleration after zero bias correction; This represents the angular velocity after zero bias correction.

[0057] In this scheme, based on the kinematics of inertial navigation, position, velocity, and attitude satisfy the following definitions: in, Indicates the position at the next moment; Indicates the velocity at the next moment; Indicates the attitude quaternion at the next moment; Represents the attitude quaternion The resulting rotation matrix; Represents the gravitational acceleration vector; Indicates the time interval between adjacent IMU sampling times; Represents quaternion multiplication; This represents the exponential mapping from angular velocity increments to attitude increments.

[0058] Due to keyframes With keyframes There may be multiple IMU sampling points between keyframes, and optimizing point by point would significantly increase the computational cost. Therefore, compressing the IMU information between keyframes into a pre-integrated quantity is defined as follows: in, Keyframe To keyframe The pre-integral velocity increment between; Keyframe To keyframe Pre-integral position increments between; Keyframe To keyframe Pre-integral attitude increments between; Indicates from keyframe To the The relative rotation matrix at each IMU sampling time; Keyframe To the Intermediate pre-integral velocity at each IMU sampling time.

[0059] The reasoning behind the above formula is as follows: since velocity is the first integral of acceleration over time and position is the first integral of velocity over time, we know that IMU acceleration can be recursively used to deduce velocity and position after attitude transformation; since angular velocity is the rate of attitude change, we know that angular velocity can be recursively used to deduce attitude after exponential mapping. Therefore, by accumulating or multiplying multiple IMU sampling points between keyframes, we can obtain the relative motion constraints between keyframes.

[0060] Regarding the construction of the inertial constraint factor, this scheme constructs the inertial residual between adjacent keyframes based on the pre-integral quantity: in, Keyframe With keyframes The inertial constraint residual between them, i.e., the inertial constraint factor; , These represent keyframes. , Location; These represent keyframes. , speed; Keyframe The attitude rotation matrix; Keyframe To keyframe The time interval between; Represents the gravitational acceleration vector; These represent the pre-integration position, velocity, and attitude increments, respectively. This represents the Lie algebra mapping that maps attitude errors to a three-dimensional error vector.

[0061] The residual is used to measure the consistency between the motion of adjacent keyframes inferred from the state variables and the IMU pre-integration observations.

[0062] This step, based on the standardized IMU data and time series output from step S2, outputs the inertial constraint factor. and initial values ​​of keyframe states .

[0063] This inertial constraint factor will serve as a fundamental constraint for the combined optimization objective function in subsequent steps; keyframe state It will continue to be corrected by motion constraints and scene constraints in subsequent steps.

[0064] Since the crane track detection vehicle typically moves longitudinally along the track, its motion is subject to significant constraints: therefore, its speed should be close to zero in stationary sections, its lateral and vertical speeds should be limited in normal travel sections, and its attitude should not exhibit unreasonable jumps in stable track sections. Without utilizing these prior knowledge of motion, IMU errors will continuously accumulate in the lateral, elevation, and attitude directions along the track.

[0065] To address the issues of velocity drift, attitude drift, and lateral position divergence in inertial calculations, step S4 of this scheme transforms the constrained motion characteristics of the crane track detection carrier into motion constraint factors that can participate in optimization.

[0066] As one possible implementation, further, in step S4 of this solution, the motion constraint factors include a zero-speed constraint factor, an attitude lock constraint factor, a non-integrity constraint factor, and / or an odometry increment constraint factor. The zero-speed constraint factor is used to constrain the speed state when the detection carrier is in a stationary state or a low-speed dwell state. The attitude locking constraint factor is used to constrain the changes in roll angle, pitch angle or heading angle of the detection vehicle in the stable track section. The non-integrity constraint factor is used to constrain the velocity components of the detection vehicle in the lateral and vertical directions of the track. The odometer increment constraint factor is used to constrain the displacement increment along the track longitudinal direction between adjacent keyframes.

[0067] As an example of implementation, step S4 of this solution includes the following sub-steps: When the detection carrier is stationary, briefly stationary, or in a low-speed stable state, the zero-velocity state can be determined based on changes in acceleration and angular velocity. The zero-velocity state determination criteria can be set as follows: in, This indicates the addition measured by the IMU; This represents the angular velocity measured by the IMU; This represents the average acceleration within the sliding window; Indicates the acceleration fluctuation threshold; Indicates the angular velocity threshold; Indicates the odometer increment threshold; Indicates the odometer at the 1st The walking increment at each sampling time.

[0068] When the above conditions are met, the detection carrier is considered to be in a zero-velocity state, and a zero-velocity residual is constructed, which is defined as: in, This represents the zero-velocity constraint residual, i.e., the zero-velocity constraint factor. Indicates the first The speed of each keyframe.

[0069] The reasoning behind this formula is that when the detection vehicle is stationary, its true velocity should be zero. Therefore, the velocity quantity in the state estimation should be constrained to near zero to eliminate velocity drift caused by inertial integral.

[0070] Since the detection carrier moves longitudinally along the crane track, its lateral and vertical velocities should ideally be close to zero. The velocity, transformed from the navigation coordinate system to the track coordinate system, is defined as follows: in, Represents the velocity in the orbital coordinate system; Represents the rotation matrix from the navigation coordinate system to the orbit coordinate system; This indicates the velocity in the navigation coordinate system.

[0071] velocity in orbital coordinate system It can be written as: in, Indicates the longitudinal velocity of the track; Indicates the lateral velocity of the track; This indicates vertical velocity.

[0072] Since the detection carrier mainly moves along the track, a non-integrity constraint residual is constructed, which is defined as: in, This represents the residual of the non-integrity constraint, i.e., the non-integrity constraint factor; Represents the horizontal unit vector of the track; This represents the vertical unit vector of the orbit.

[0073] Under the reasoning logic of this formula, the track constraint restricts the free movement direction of the detection vehicle, and the main degree of freedom of the detection vehicle is concentrated in the longitudinal direction of the track. Therefore, by constraining the lateral and vertical velocities, lateral drift and elevation drift caused by IMU integration can be suppressed.

[0074] In the stable sections of the track, the changes in the attitude of the detection vehicle should be consistent with the track alignment, rail surface slope, and installation attitude, and there should be no abrupt changes caused by IMU noise. Let the... Estimating attitude angles in keyframes for: in, Indicates the roll angle; Indicates the pitch angle; Indicates the heading angle.

[0075] The reference attitude angle is obtained from the track design direction, control point direction, or sliding window statistical attitude. : , , These are the roll angle, pitch angle, and yaw angle corresponding to the reference attitude angle, respectively.

[0076] Constructing attitude locking residuals: in, This represents the attitude locking constraint residual, i.e., the attitude locking constraint factor; Indicates the current estimated attitude angle; Indicates the reference attitude angle.

[0077] Under the reasoning logic of this formula, in sections where the track is smooth and the detection vehicle is operating stably, attitude changes are continuous and small in amplitude. Attitude locking constraints can prevent the attitude estimation from slowly drifting due to gyroscope zero bias.

[0078] The odometer provides the distance traveled by the carrier along the longitudinal direction of the track. Let the cumulative distance traveled by the odometer between keyframe (i) and keyframe (j) be: in, Keyframe To keyframe The odometer shows the cumulative distance traveled between the two points. Indicates the first The odometer increment at each sampling time.

[0079] The orbital displacement obtained by estimating the state of the detected carrier is: in, Keyframe To keyframe The orbital displacement between them; Represents the longitudinal unit vector of the orbit; These represent keyframes. , The location.

[0080] Considering the proportional error coefficient of the odometer The odometer residual is constructed and defined as follows: in, This represents the residual of the odometer increment constraint, i.e., the odometer increment constraint factor; Keyframe The odometer proportional error coefficient at the location.

[0081] Under the reasoning logic of this formula, the track displacement between keyframes should be consistent with the cumulative travel distance of the odometer. However, the odometer may have wheel diameter errors, slippage, or installation errors. Therefore, a proportional error coefficient is introduced for compensation.

[0082] Based on the keyframe state obtained in step S3 Based on the odometry and IMU standardized data output from step S2, this step obtains the set of motion-constrained residuals, which is defined as: The set of motion constraints will undergo credibility evaluation and weight adjustment in subsequent steps, and will participate in the combined solution together with inertial constraints and scene constraints.

[0083] Crane track sites typically contain control points, fixed track structures, repeated crossing points, and multiple measurement sections. This scenario information can provide global constraints and closure corrections for combined measurements.

[0084] To address the issues of insufficient global reference, difficulty in eliminating closure errors in revisited sections, and difficulty in unifying laser ranging results with inertial trajectories during long-distance track measurements, step S5 of this scheme transforms control point homing relationships, revisited point pose consistency, laser ranging geometric relationships, and multiple measurement consistency into scene constraint factors.

[0085] As one possible implementation, further, in step S5 of this solution, the scene constraint factors include control point adherence constraint factors, revisit point pose consistency constraint factors, laser ranging constraint factors, and / or multiple measurement weighted constraint factors. The control point binding constraint factor is constructed by the difference between the control point coordinates and the estimated position of the detection vehicle. The revisit point pose consistency constraint factor is constructed by detecting the position difference and pose difference when the carrier passes through the same track point multiple times. The laser ranging constraint factor is constructed by the difference between the laser ranging observation value and the distance value predicted by the detection carrier state and the external parameters of the laser ranging module. The weighted constraint factor for multiple measurements is constructed by weighting the consistency of multiple measurement results in the same orbital segment.

[0086] As an example of implementation, step S5 of this solution includes the following sub-steps: When the detection vehicle passes near a known control point, the estimated position can be matched with the coordinates of the control point. Let the first... The coordinates of the control points corresponding to each keyframe are: Then the control point residuals are: in, This represents the residuals of the control point adherence constraints; Indicates the first Estimated location of each keyframe; This indicates the known coordinates of the control point in the orbital coordinate system.

[0087] According to the reasoning logic of this formula, when the detection vehicle passes through a control point, its estimated position should be consistent with the coordinates of the control point. By constraining the control point, the inertial trajectory can be bound to a global measurement reference, preventing long-distance drift.

[0088] Construction of pose consistency constraint factors for revisit points If the detection vehicle passes through the same track point in one or multiple detections, the estimated position and attitude corresponding to that point should remain consistent. Let the first... Second and third The statuses of each visit to the same revisit point are as follows: and The pose consistency residual of the revisit point is: in, This represents the pose consistency residual at the revisit point, i.e., the pose consistency constraint factor at the revisit point. These represent the positions when passing through the same revisit point twice; Let each represent the attitude quaternion when passing through the same revisit point twice; Representing attitude quaternions The reverse; Represents quaternion multiplication; This represents the mapping from attitude error to a three-dimensional vector.

[0089] The reasoning behind this formula is that the same physical point should have a unique spatial position within the same orbital coordinate system; if the position and attitude obtained by passing the same point multiple times are inconsistent, the difference reflects inertial drift, odometer error, or local observation anomalies. Revisiting constraints can establish closed relationships, improving overall consistency.

[0090] Construction of laser ranging constraint factors The laser ranging module can measure the distance from the detection carrier to the side of the track, the top surface of the track, or adjacent tracks. Let the extrinsic parameters of the laser ranging module in the carrier coordinate system be... The carrier status is detected as follows The laser distance can then be predicted based on the geometric projection relationship, and it is defined as follows: in, This indicates the laser distance predicted based on the current state. This represents the geometric prediction function for laser ranging. Indicates the first Each keyframe state; (T_b^m) represents the extrinsic parameter of the laser ranging module relative to the carrier coordinate system; It represents the geometric reference surface or reference line formed by the edge of the track, the top surface of the track, or adjacent tracks.

[0091] The laser ranging residual is: in, This represents the laser ranging constraint residual, i.e., the laser ranging constraint factor; This represents the actual observation value from the laser ranging module.

[0092] Under the reasoning logic of this formula, if the state estimation of the detection carrier is accurate, the distance calculated from the state and the external parameters of the laser module should be consistent with the actual distance measurement value. This constraint enhances the reliability of measurements of geometric parameters such as track gauge, elevation difference, and track edge position.

[0093] Construction of weighted constraint factors based on multiple measurements For the same track section, multiple round trip measurements can be performed. Let the first measurement be the first measurement of the same track section. The orbital parameters obtained from this measurement are: Its credibility weight is The weighted fusion result is: in, Indicates the first Weighted results of multiple measurements of a single track section; Indicates the first The orbital parameters obtained from this measurement; Indicates the first The credibility weight of the measurement; Indicates the number of measurements.

[0094] Therefore, the consistency residual of multiple measurements is constructed, which is defined as: in, Indicates the first The residual between the individual measurement results and the weighted fusion results.

[0095] Under the reasoning logic of this formula, the same track section should have consistent geometric parameters in a steady state. Differences in multiple measurements mainly arise from sensor noise, local disturbances, and trajectory estimation errors. By using weighted consistency constraints, the stability of results for repeated measurement sections can be improved.

[0096] This step, based on the control points, revisit points, and laser ranging data obtained in step S2, and the keyframe states obtained in step S3, outputs a set of scene constraint residuals, defined as follows: This set will be subjected to robust adaptive weight adjustment along with motion constraints in step S6, and will serve as an important observation factor for graph optimization and filter update in step S7.

[0097] During the dynamic measurement of crane tracks, IMU, odometer, or laser ranging observations may be affected by transient disturbances at certain moments. If abnormal observations are still included in the combined solution with normal weights, it will cause abrupt changes in the trajectory or misjudgment of track parameters.

[0098] To address constraint contamination issues caused by track welds, joint misalignment, rail wear, wheel vibration, high-frequency impacts, and transient sensor anomalies, step S6 of this solution adaptively adjusts the weights of motion constraint factors and scene constraint factors based on the degree of dynamic disturbance and the magnitude of constraint residuals, thereby reducing the weight of abnormal observations or eliminating them.

[0099] As a preferred implementation method, in step S6 of this solution, the dynamic disturbance index is expressed as: in, Indicates the first Dynamic excitation index at each moment This represents the average acceleration within the sliding window. This represents the average angular velocity within the sliding window. Indicates the standard deviation of acceleration. Indicates the standard deviation of angular velocity. This represents the standard deviation of jerk. This represents the current constraint factor residual. Represents the standard deviation of the residuals. Denotes the weight coefficients, and satisfies .

[0100] As a possible implementation, further, in step S6 of this solution, the constraint factor weights are determined according to the dynamic disturbance index in the following manner: in, Indicates the first The weights of each constraint factor, This indicates the threshold for mild perturbation. This indicates the severe perturbation threshold. Indicates the first The dynamic excitation index at each moment; when When the corresponding constraint factors participate normally in the combination solution, When the corresponding constraint factor is reduced in weight, it participates in the combination solution. The corresponding constraint factor is removed from the current optimization window.

[0101] As an example of implementation, step S6 of this solution includes the following sub-steps: First, a dynamic excitation index is constructed based on IMU acceleration, angular velocity, jerk, and constraint residuals, and is defined as follows: in, Indicates the first Dynamic disturbance index at each moment; Indicates the first Acceleration at any given moment; This represents the average acceleration within the sliding window; Indicates the first angular velocity at each instant; This represents the average angular velocity within the sliding window; This represents the second-order difference of acceleration, used to characterize changes in impact or vibration. This represents the standard deviation of acceleration within the sliding window; This represents the standard deviation of the angular velocity within the sliding window; Indicates the standard deviation of jerk; This represents the current constraint factor residual; Indicates the standard deviation of the residuals; Let represent the weighting coefficients, and satisfy: .

[0102] The aforementioned dynamic disturbance index consists of four types of normalized quantities.

[0103] The first item is represented as: It measures the degree of deviation of the current acceleration from a local steady state. If the detection vehicle passes through a track weld or joint misalignment, the acceleration will experience a sudden change, and this value will increase.

[0104] The second item is represented as: It is used to measure abnormal fluctuations in attitude angular velocity. If the detection vehicle experiences impact, shaking, or attitude jitter, this value increases.

[0105] The third item is represented as: This is a second-order difference term for acceleration, used to describe the abrupt change in the rate of acceleration change. Since track joint impacts typically manifest as rapid changes in acceleration over a short period, this term can be used to identify high-frequency disturbances.

[0106] The fourth item is represented as: It is used to describe the degree of anomaly in the current constraint factor residuals. If an observation is significantly inconsistent with the current state prediction, it indicates that the observation may be subject to abnormal interference or mismatch.

[0107] By weighting and summing the above four types of indicators, a comprehensive indicator that simultaneously reflects motion perturbation and constraint consistency can be obtained. .

[0108] Determine the constraint factor weights based on the dynamic disturbance index: in, Indicates the first The credible weights of each constraint factor; This indicates the threshold for mild disturbance. Indicates the severe perturbation threshold; Indicates the first The dynamic disturbance index at each moment.

[0109] Under the reasoning logic of this formula, when When this occurs, it indicates that the current observation is within the normal fluctuation range and should participate in the solution with full weights; when This indicates that the current observation has been disturbed to some extent, but still retains some valid information, therefore it is... Reduce the weight; when If this occurs, it indicates that the current observation may be severely anomalous data and should be removed from the current optimization window.

[0110] EKF is suitable for real-time recursive state estimation, but as a forward filter, it is easily affected by early linearization errors and local anomalies; graph optimization can uniformly handle multiple types of constraints and improve global consistency, but direct full optimization has a large computational cost.

[0111] To address the challenge of balancing real-time measurement and global consistency, this step employs EKF for real-time state recursion and graph optimization to correct the consistency of keyframe states, thereby achieving a balance between real-time performance and accuracy.

[0112] As an example of implementation, step S7 of this solution includes the following sub-steps: EKF State Prediction State prediction is performed based on the IMU pre-integration results from step S3, and is defined as follows: in, Indicates the first The predicted state at each moment; Indicates the first The posterior state at each moment; This represents the inertial input provided by IMU pre-integration; This represents the state recursion function.

[0113] The state covariance prediction is: in, This represents the predicted state covariance; This represents the covariance of the posterior state at the previous time step; Represents the state transition Jacobian matrix; This represents the process noise covariance matrix.

[0114] Under the reasoning logic of this formula, state uncertainty will propagate recursively with inertia. At the same time, IMU noise and zero bias noise will introduce new process uncertainties. Therefore, it is necessary to update the covariance through the state transition matrix and the process noise matrix.

[0115] EKF Measurement Update The weighted constraints output from step S6 are used as measurement update terms, and the measurement residuals are defined as follows: in, Indicates measurement residual; Represents actual observations; This represents the observation prediction function obtained based on the predicted state.

[0116] The Kalman gain is: in, Indicates Kalman gain; Represents the Jacobian matrix of the measurement model; This represents the measurement noise covariance matrix.

[0117] The update status is: The updated covariance is: in, Indicates the first The posterior state at each moment; Indicates the posterior state covariance; Represents the identity matrix.

[0118] When the constraint factor weight is At that time, the measurement covariance can be adjusted equivalently: in, This represents the adaptively adjusted measurement noise covariance; This indicates an extremely small positive number that prevents the denominator from being zero; Indicates the first The credible weights of each constraint factor; This represents the measurement noise covariance matrix.

[0119] Under the reasoning logic of this formula, the smaller the weight, the lower the confidence of the observation, and the larger the corresponding measurement noise covariance should be, and the weaker its impact on state updates.

[0120] Graph optimization objective function Construct a graph optimization objective function on a local window or a global set of keyframes: in, Represent the objective function of graph optimization; This represents the set of all keyframe states to be optimized. Represents the inertial constraint residual; Represents the zero-velocity constraint residual; Represents the residual of non-integrity constraints; This represents the attitude locking constraint residual; This represents the residual of the odometer increment constraint; This represents the control point alignment residuals; This indicates the pose consistency residual at the revisit point; This represents the laser ranging residual; Let each represent the covariance matrix of the residuals under various constraints; These represent the adaptive weights of the corresponding constraint factors.

[0121] In this step, the reasoning for the objective function is based on the maximum a posteriori estimation concept. Various sensor observations and constraints can be viewed as probabilistic constraints on state variables, and minimizing the weighted sum of squared residuals is equivalent to finding the most probable state sequence.

[0122] This step is based on the inertial constraint factor in step S3, the weighted motion constraint in step S6, and the weighted scene constraint. The optimized trajectory of the output detection vehicle is defined as follows: in, Represents the optimized detection carrier trajectory; Define Indicates the first The optimized state after each keyframe This represents the number of samples.

[0123] EKF forward recursion can only utilize information prior to the current time step. After the measurement is completed, subsequent control points, revisit points, and closure information should also be used to correct the preceding state in reverse.

[0124] To address the issues of insufficient utilization of posterior information from unidirectional filtering, residual cumulative errors remaining in long-distance trajectories, and incompletely balanced distribution of control points and revisit closure relationships, this scheme employs RTS reverse smoothing in step S8 to correct the state sequence using full-time information, and then uses global adjustment to rationally distribute control point alignment errors and revisit closure errors across the entire track measurement trajectory.

[0125] As an example of implementation, step S8 of this solution includes the following sub-steps: After completing the forward filtering, smoothing is performed from the last keyframe backwards. The RTS smoothing gain is: in, Indicates the first RTS smoothing gain for each keyframe; Indicates the first The posterior covariance of each keyframe; Indicates the first From the first keyframe to the... The state transition Jacobian matrix for each keyframe; Indicates by the first The keyframe prediction reached the [number]th keyframe. The covariance of each keyframe.

[0126] The smooth state is: in, Indicates the first The smoothing state of each keyframe; Indicates the first The forward-filtered posterior state of each keyframe; Indicates the first The smoothing state of each keyframe; Indicates by the first The first keyframe prediction obtained Keyframe states.

[0127] Under the reasoning logic of this formula, if the first... If the first keyframe is corrected using subsequent observations, then the second keyframe... Each keyframe should also be reverse-corrected based on the state transition relationship between the two. RTS smoothing gain It is used to measure the degree of influence of the correction amount at the next time step on the state at the previous time step.

[0128] After RTS smoothing, a global adjustment model is further established: in, This represents the global state correction to be solved; Indicates the first The coefficient matrix of the trajectory observation equation; Indicates the first The closure error of the trajectory observation equation; This represents the weight matrix of the corresponding trajectory observation equation; Indicates the first The coefficient matrix of constraint equations for control points, revisit points, or multiple measurements; This represents the residual or closure error of the corresponding constraint equation; This represents the weight matrix corresponding to the constraint equation.

[0129] According to the principle of least squares adjustment, the normal equation is: in, Represents the coefficient matrix of the integrated observation equation; Represents the comprehensive weight matrix; Represents the combined observation closed difference vector; This represents the correction quantity for the state to be determined.

[0130] Seek Then, the trajectory status is corrected: in, Represents the first step after global adjustment. Keyframe states; Represents the RTS smoothed th Keyframe states; Indicates the first The state correction amount corresponding to each keyframe.

[0131] This step is based on the optimized trajectory output in step S7. The control points, revisit points, and multiple measurement consistency constraints from step S5 are utilized again. The output is a globally consistent track measurement trajectory. Its definition is: Among them, the definition Indicates the first The optimized state after each keyframe This represents the number of samples.

[0132] The aforementioned steps yield the trajectory of the detection vehicle and the fusion status of various sensors. However, track maintenance and safety assessment actually require specific geometric parameters such as track gauge, horizontal height difference, track centerline offset, straightness, track top elevation change, and joint misalignment.

[0133] To address the issue of how to convert inertial measurement results into usable track inspection indicators for engineering applications, step S9 of this scheme combines the globally consistent trajectory output from step S8 with laser ranging data to calculate the engineering inspection parameters of the crane track and generate an inspection report.

[0134] As one possible implementation, further, in step S9 of this scheme, the geometric parameters of the crane track include the coordinates of the track centerline, track gauge, track horizontal height difference, track straightness, track top elevation change and / or track joint misalignment.

[0135] As an example of implementation, step S9 of this solution includes the following sub-steps: Based on the state of the detection carrier after global adjustment And the extrinsic parameters of the laser ranging module, calculate the first Coordinates of the measuring points on the left and right rails at each cross-section: in, Indicates the first Coordinates of the left rail measuring point at each cross section; Indicates the first Coordinates of the right-rail measuring points at each cross-section; This represents the function for calculating the coordinates of the measurement points on the left track. This represents the function for calculating the coordinates of the measurement points on the right track. This represents the state after global adjustment output in step S8; This represents the extrinsic parameters of the laser ranging module relative to the carrier coordinate system; This indicates the laser ranging observation value in the left rail direction; This indicates the laser ranging observation value in the right-hand rail direction.

[0136] Under the reasoning logic of this formula, once the global pose of the detection carrier is determined, the position and orientation of the laser ranging module relative to the carrier are known. By combining the laser distance, the spatial coordinates of the track measuring point in the track coordinate system can be calculated.

[0137] Track gauge calculation Track gauge is defined as the lateral distance between the left and right rails at the same cross section, and can be expressed as: in, Indicates the first Track gauge at each track section; Indicates the coordinates of the measuring point on the left rail; Indicates the coordinates of the measuring points on the right track; This represents the Euclidean distance.

[0138] If only the lateral component of the track is considered, it can also be expressed as: in, Indicates the lateral coordinates of the measuring point on the left rail; This indicates the lateral coordinates of the measuring point on the right rail.

[0139] Under the reasoning logic of this formula, the track gauge is the spatial distance between the left and right tracks, which is mainly represented as the difference in lateral coordinates in the track coordinate system.

[0140] Calculation of horizontal elevation difference The track level difference represents the difference in elevation between the tops of the left and right rails at the same cross-section. in, Indicates the first The horizontal elevation difference at each track section; Indicates the elevation of the top of the left rail; This indicates the elevation of the top of the right rail.

[0141] Under the reasoning logic of this formula, the elevation difference between the left and right rails at the same cross-section directly reflects the lateral tilting trend of the crane during operation, and is an important parameter for judging the installation and settlement status of the rails.

[0142] Calculation of track centerline coordinates The center point of the track at the same cross section can be represented as: in, Indicates the first Coordinates of the center point of the track at each cross section; Indicates the coordinates of the measuring point on the left rail; This indicates the coordinates of the measuring point on the right track.

[0143] Under the reasoning logic of this formula, the track centerline is formed by connecting the midpoints of the corresponding sections of the left and right rails. Therefore, the average value of the coordinates of the measuring points on the left and right rails can be used as the coordinates of the track centerline of that section.

[0144] Track straightness deviation calculation Let the track design centerline be , No. The center point of each cross-section of the track The projection point on the design center line is: in, This represents the nearest projection point of the track center point onto the design centerline; Indicates the centerline of the track design; This represents the point that minimizes the distance.

[0145] The deviation of track straightness is: in, Indicates the first Track straightness deviation at each cross-section; Indicates the first The center point of the track at each cross section; This indicates its projection point on the design center line.

[0146] Under the reasoning logic of this formula, track straightness can be understood as the degree to which the actual track center point deviates from the design center line. Therefore, the distance between the actual center point and the nearest point of the design center line can be used as the straightness deviation.

[0147] Calculation of track top elevation change The change in rail top elevation can be calculated based on the elevation difference between adjacent sections: in, Indicates the first The change in the rail top elevation of each section relative to the previous section; This represents the elevation of the center point of the track at the (k)th cross section; Indicates the first Elevation of the center point of the track section.

[0148] The gradient can also be calculated based on the longitudinal distance of the track, and it is defined as follows: in, Indicates the first The rail top slope between the current section and the previous section; , These represent the orbital coordinates of adjacent cross sections.

[0149] Under the reasoning logic of this formula, the continuous change in the track top elevation reflects the settlement or local uplift of the track foundation, and the elevation difference and slope of adjacent sections can be used to identify sections with abnormal elevation changes.

[0150] Calculation of track joint misalignment For the track joint location, let the measuring points on the front and rear sides of the joint be respectively... and Their elevations are respectively and The vertical misalignment is: in, Indicates the first Vertical misalignment at each joint location; Indicates the elevation of the rail top on the front side of the joint; This indicates the elevation of the top of the rail behind the joint.

[0151] The lateral misalignment can be expressed as: in, Indicates the first Lateral misalignment at each joint location; Indicates the lateral coordinates of the front side of the joint; This indicates the lateral coordinates of the back side of the seam.

[0152] The total misalignment can be expressed as: in, Indicates the first The total misalignment at each joint location.

[0153] Under the reasoning logic of this formula, track joint misalignment is usually manifested as discontinuity in the horizontal or vertical direction of the track on both sides before and after the joint. Therefore, the misalignment can be calculated by the difference in coordinates of the measuring points before and after the joint.

[0154] This step is based on the globally consistent orbit measurement trajectory output in step S8. This is combined with the standardized laser ranging data from step S2. The calculated set of orbital geometric parameters is then output, defined as follows: in, This represents the set of geometric parameters of the crane track; Indicates track gauge; Indicates the difference in horizontal level; Indicates deviation in track straightness; Indicates the change in rail top elevation; Indicates the misalignment of the track joint; Indicates the coordinates of the center point of the track; Indicates the number of track sections.

[0155] Finally, a track inspection report is generated based on the above track geometric parameters. The report may include the track centerline coordinate sequence, track gauge variation curve, horizontal elevation difference variation curve, straightness deviation curve, track top elevation variation curve, joint misalignment location, abnormal section identification, and re-measurement or maintenance recommendations.

[0156] Combination Figure 3 As shown, based on the above, this solution also proposes a crane track inertial combined measurement system based on motion and scene constraints, which includes: The multi-source data acquisition module is used to acquire inertial measurement data, odometry incremental data, laser ranging data, RTK positioning data, control point data, and revisit point data. The time synchronization module is used to synchronize data from multiple sources. The coordinate calibration module is used for extrinsic parameter calibration and coordinate transformation of multi-source data; The inertial pre-integration module is used to calculate the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment based on the inertial measurement data between adjacent keyframes, and to construct the inertial constraint factor. The motion constraint construction module is used to construct zero-speed constraint factors, attitude lock constraint factors, non-integrity constraint factors, and odometry increment constraint factors. The scene constraint construction module is used to construct control point adherence constraint factors, revisit point pose consistency constraint factors, laser ranging constraint factors, and multiple measurement weighted constraint factors. The robust adaptive fusion module is used to adjust the weights of motion constraint factors and scene constraint factors based on dynamic perturbation indicators; The combined solution module is used to obtain the optimized trajectory of the detection carrier based on extended Kalman filtering and graph optimization; The smoothing and adjustment module is used to perform RTS reverse smoothing and global adjustment on the optimized trajectory; The track parameter output module is used to output the coordinates of the track centerline, track gauge, track horizontal elevation difference, track straightness, track top elevation change, and track joint misalignment.

[0157] Furthermore, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0158] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods of various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0159] The above description is only a part of the embodiments of the present invention and does not limit the scope of protection of the present invention. Any equivalent device or equivalent process transformation made based on the content of the present invention specification and drawings, or direct or indirect application in other related technical fields, are similarly included within the patent protection scope of the present invention.

Claims

1. A combined inertial measurement method for crane tracks based on motion and scene constraints, wherein a detection carrier running along the crane track is deployed on the track, and the detection carrier is equipped with an inertial measurement unit, an odometer, a laser ranging module, an RTK positioning module, and a unified timeline acquisition unit, characterized in that, It includes: S1. When the detection carrier runs along the crane track, the unified timeline acquisition unit collects and records inertial measurement data, odometer incremental data, laser ranging data, RTK positioning data, control point data and revisit point data simultaneously to form a multi-source synchronous measurement data sequence. S2. Calibrate the external parameters of each sensor relative to the carrier coordinate system according to preset conditions, and compensate for the time deviation of each sensor sampling time, so as to convert the multi-source synchronous measurement data sequence to a unified track measurement reference. S3. Perform IMU pre-integration based on the inertial measurement data between adjacent keyframes, construct preset state variables, and establish inertial constraint factors based on the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment. S4. Based on the characteristics of the restricted movement of the detection carrier along the crane track, establish the motion constraint factor; S5. Establish scenario constraint factors based on control points, revisit points, laser ranging results, and multiple measurement results in the crane track detection scenario; S6. Calculate the dynamic excitation index based on the acceleration characteristics, angular velocity characteristics, jerk characteristics, and residual characteristics of each constraint factor output by the inertial measurement unit, and adaptively adjust the weights of the motion constraint factor and the scene constraint factor based on the dynamic excitation index. S7. Input the inertial constraint factor, the motion constraint factor adjusted by adaptive weight, and the scene constraint factor adjusted by adaptive weight into the combined solution model, perform real-time state recursion through extended Kalman filtering, and perform consistency optimization of key frame states through graph optimization to obtain the optimized trajectory of the detection vehicle. S8. Perform RTS reverse smoothing and global adjustment on the optimized trajectory to correct the accumulated error of long-distance measurement and obtain a globally consistent track measurement trajectory. S9. Calculate the geometric parameters of the crane track based on the globally consistent track measurement trajectory and laser ranging data.

2. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, In step S1, the multi-source synchronous measurement data sequence is represented as follows: in, Indicates the first A collection of multi-source measurement data at each sampling time. Indicates triaxial acceleration, Indicates the angular velocity of the three axes. Indicates the odometer increment. Represents laser ranging data, This represents RTK positioning data. This represents control point data or revisit point data. This represents a unified timestamp.

3. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, Step S2 includes: establishing a navigation coordinate system, a carrier coordinate system, an inertial measurement unit coordinate system, an odometer coordinate system, a laser ranging coordinate system, and an orbit coordinate system; calibrating the external parameters of each sensor relative to the carrier coordinate system; compensating for time deviations in the sampling time of each sensor; and converting the multi-source synchronous measurement data sequence to a unified orbit measurement reference. In step S2, the extrinsic parameters of each sensor relative to the carrier coordinate system are expressed as follows: in, Indicates the first The pose transformation matrix of the sensor-like coordinate system relative to the carrier coordinate system. Represents the rotation matrix. Represents the translation vector; The sensor sampling time is also compensated, and the compensation formula is defined as follows: in, Indicates the first The original sampling time of the sensor-like sensor Indicates the first Time deviation of a sensor relative to a unified time reference.

4. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, In step S3, the preset state variables include position, velocity, attitude, accelerometer bias, gyroscope bias and / or odometer proportional error coefficient; In step S3, the state quantity Represented as: in, Indicates location, Indicates speed, Represents attitude quaternions, This indicates that the accelerometer has zero bias. This indicates that the gyroscope has zero bias. This represents the proportional error coefficient of the odometer; The IMU pre-integration includes calculating the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment between adjacent keyframes, and constructing the inertial residual between adjacent keyframes using the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment.

5. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, In step S4, the motion constraint factors include zero-speed constraint factors, attitude lock constraint factors, non-integrity constraint factors, and / or odometry increment constraint factors. The zero-speed constraint factor is used to constrain the speed state when the detection carrier is in a stationary state or a low-speed dwell state. The attitude locking constraint factor is used to constrain the changes in roll angle, pitch angle or heading angle of the detection vehicle in the stable track section. The non-integrity constraint factor is used to constrain the velocity components of the detection vehicle in the lateral and vertical directions of the track. The odometer increment constraint factor is used to constrain the displacement increment along the track longitudinal direction between adjacent keyframes.

6. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, In step S5, the scene constraint factors include control point adherence constraint factors, revisit point pose consistency constraint factors, laser ranging constraint factors, and / or multiple measurement weighted constraint factors. The control point binding constraint factor is constructed by the difference between the control point coordinates and the estimated position of the detection vehicle. The revisit point pose consistency constraint factor is constructed by detecting the position difference and attitude difference when the carrier passes through the same track point multiple times. The laser ranging constraint factor is constructed by the difference between the laser ranging observation value and the distance value predicted by the detection carrier state and the external parameters of the laser ranging module. The weighted constraint factor for multiple measurements is constructed by weighting the consistency of multiple measurement results in the same orbital segment.

7. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 2, characterized in that, In step S6, the dynamic disturbance index is expressed as: in, Indicates the first Dynamic excitation index at each moment This represents the average acceleration within the sliding window. This represents the average angular velocity within the sliding window. Indicates the standard deviation of acceleration. Indicates the standard deviation of angular velocity. This represents the standard deviation of jerk. This represents the current constraint factor residual. Represents the standard deviation of the residuals. Denotes the weight coefficients, and satisfies .

8. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, In step S6, the constraint factor weights are determined based on the dynamic perturbation index in the following manner: in, Indicates the first The weights of each constraint factor, This indicates the threshold for mild perturbation. This indicates the severe perturbation threshold. Indicates the first The dynamic excitation index at each moment; when When the corresponding constraint factors participate normally in the combination solution, When the corresponding constraint factor is reduced in weight, it participates in the combination solution. The corresponding constraint factor is removed from the current optimization window.

9. The crane track inertia combined measurement method based on motion and scene constraints as described in claim 1, characterized in that, In step S9, the geometric parameters of the crane track include the coordinates of the track centerline, track gauge, track horizontal elevation difference, track straightness, track top elevation change, and / or track joint misalignment.

10. A crane track inertial combined measurement system based on motion and scene constraints, characterized in that, It includes: The multi-source data acquisition module is used to acquire inertial measurement data, odometry incremental data, laser ranging data, RTK positioning data, control point data, and revisit point data. The time synchronization module is used to synchronize the time of data from multiple sources. The coordinate calibration module is used for extrinsic parameter calibration and coordinate transformation of multi-source data; The inertial pre-integration module is used to calculate the pre-integrated position increment, pre-integrated velocity increment, and pre-integrated attitude increment based on the inertial measurement data between adjacent keyframes, and to construct the inertial constraint factor. The motion constraint construction module is used to construct zero-speed constraint factors, attitude lock constraint factors, non-integrity constraint factors, and odometry increment constraint factors. The scene constraint construction module is used to construct control point adherence constraint factors, revisit point pose consistency constraint factors, laser ranging constraint factors, and multiple measurement weighted constraint factors. The robust adaptive fusion module is used to adjust the weights of motion constraint factors and scene constraint factors based on dynamic perturbation indicators; The combined solution module is used to obtain the optimized trajectory of the detection carrier based on extended Kalman filtering and graph optimization; The smoothing and adjustment module is used to perform RTS reverse smoothing and global adjustment on the optimized trajectory; The track parameter output module is used to output the coordinates of the track centerline, track gauge, track horizontal elevation difference, track straightness, track top elevation change, and track joint misalignment.

Images