A lateral deviation data fusion system and method based on Kalman filtering

Abstract

This application relates to the field of deviation data fusion technology, and discloses a lateral deviation data fusion system and method based on Kalman filtering. The method includes: acquiring the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle; based on a preset lateral kinematic model of the vehicle, performing state prediction through the angular rate and longitudinal velocity of the inertial measurement unit to obtain a first state estimate at the current moment; based on the attitude angle of the inertial measurement unit, projecting the lateral deviation in the first state estimate onto the measurement direction of the ultrasonic sensor to generate a corresponding first distance measurement, and calculating the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset; and performing continuous lateral correction of the rail vehicle between two absolute position beacon points. This application can continuously correct the heading offset by the distance measurement of the rail wall and suppress long-term drift of the heading offset.

Description

Technical Field

[0001] This application relates to the field of deviation data fusion technology, and more specifically to a lateral deviation data fusion system and method based on Kalman filtering. Background Technology

[0002] Currently, lateral position monitoring of rail vehicles on operating lines is a crucial link in ensuring operational safety and preventing derailments. In switch areas and platform areas, absolute position beacons are installed along the line, providing vehicles with precise lateral position and heading references as they pass. However, in long sections between two beacons, vehicles cannot directly obtain absolute lateral references and must rely on onboard sensors for continuous calculations. Inertial measurement units (IMUs) can output angular velocity and acceleration at high frequency, and through integration and recursion, the vehicle's attitude and displacement changes can be obtained. However, the gyroscope's zero bias is accumulated cycle by cycle during integration, causing the estimated value to drift over time, which can affect the safety and accuracy of the condition monitoring process.

[0003] The existing technology has the following problems: the combination of inertial measurement unit and ultrasonic ranging usually adopts a loose coupling method, and each is calculated independently before the results are weighted averaged or Kalman filtered. Ultrasonic ranging cannot effectively constrain the divergence of heading offset in inertial recursion. For the handling of occasional outliers in ultrasonic ranging, the judgment is made by using a fixed threshold or chi-square test based on the innovation variance. This can easily misjudge normal fluctuations as outliers or miss real anomalies, which limits the long-term accuracy and robustness of lateral deviation and heading offset estimation in beacon-free intervals and affects the reliability of active correction. In order to solve at least one of the above problems, this application proposes a lateral deviation data fusion system and method based on Kalman filtering. Summary of the Invention

[0004] To address the shortcomings of existing technologies, the purpose of this application is to provide a lateral deviation data fusion system and method based on Kalman filtering, which can effectively solve the problems in the background technology. The specific technical solution of this application is as follows:

[0005] Lateral bias data fusion methods based on Kalman filtering include:

[0006] The system acquires the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle.

[0007] Based on the preset vehicle lateral kinematics model, the state is predicted by the angular rate and longitudinal velocity of the inertial measurement unit, and the first state estimate at the current moment is obtained.

[0008] Based on the attitude angle of the inertial measurement unit, the lateral deviation in the first state estimation is projected onto the measurement direction of the ultrasonic sensor to generate the corresponding first distance measurement, and the residual between the first distance measurement and the corresponding distance is calculated to obtain the first lateral deviation and the first heading offset.

[0009] By combining the first lateral deviation and the first heading offset, the rail vehicle is continuously corrected laterally between the two absolute position beacon points.

[0010] Specifically, the acquisition of angular velocity and acceleration output by the inertial measurement unit on the rail vehicle includes: receiving the angular velocity measurement value and acceleration measurement value output by the inertial measurement unit in the form of differential signals, converting the differential signals into single-ended signals, and performing low-pass filtering to obtain the angular velocity and acceleration.

[0011] Specifically, the process of predicting the state based on a preset vehicle lateral kinematics model and using the angular rate and longitudinal velocity of the inertial measurement unit to obtain the first state estimate at the current moment includes:

[0012] Construct a state vector, which includes lateral deviation, heading offset, and gyro zero bias;

[0013] Based on the preset vehicle lateral kinematics model, the effective heading angular rate is obtained by removing the gyroscope zero bias from the angular rate of the inertial measurement unit.

[0014] The product of longitudinal velocity and heading deviation is used as the rate of change of lateral deviation. Combined with the effective heading angular rate, the first state estimate at the current moment is obtained by recursion.

[0015] Specifically, the state vector also includes the wheel diameter ratio factor of the left and right wheels of the rail vehicle. When performing state prediction, if the rail vehicle is equipped with left and right wheel speed sensors, the difference between the left and right wheel speeds output by the left and right wheel speed sensors is used to construct a virtual heading angular rate observation. The virtual heading angular rate observation is weighted and fused with the effective heading angular rate and used as the input angular rate for state prediction.

[0016] Specifically, the attitude angle based on the inertial measurement unit projects the lateral deviation in the first state estimation onto the measurement direction of the ultrasonic sensor to generate the corresponding first range, and calculates the residual between the first range and the corresponding distance to obtain the first lateral deviation and the first heading offset, including:

[0017] Based on the installation height and installation angle of the ultrasonic sensor on the vehicle body, determine the unit vector of the measurement direction relative to the vehicle body coordinate system;

[0018] Based on the unit vector, using the roll angle and pitch angle currently output by the inertial measurement unit, the lateral deviation in the first state estimation is transformed from the vehicle coordinate system to the rail coordinate system and projected onto the measurement direction to obtain the first distance measurement.

[0019] Calculate the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset.

[0020] Specifically, calculating the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset includes:

[0021] The dynamic limits are determined based on the prior error covariance matrix corresponding to the first state estimate, the observation noise variance of the ultrasonic sensor, and the preset confidence level.

[0022] The absolute value of the residual is compared with the dynamic limit. When the absolute value of the residual exceeds the dynamic limit, if the change in lateral acceleration output by the inertial measurement unit within the preset time window is less than the preset change threshold, the residual is determined to be an outlier. The observation noise variance of the ultrasonic sensor in this measurement is increased by a preset factor and then used for Kalman gain calculation to obtain an updated Kalman filter.

[0023] The first lateral deviation and the first heading offset are calculated based on the updated Kalman filter.

[0024] Specifically, after determining that the residual is an outlier, the following steps are also included:

[0025] Acquire the echo signal detected by the ultrasonic sensor within the current measurement cycle, and filter out multiple echoes in the echo signal;

[0026] From the multiple echo distances corresponding to the multiple echoes, select the echo distance with the smallest difference from the first distance measurement and replace the distance between the car body and the rail wall.

[0027] Specifically, the step of combining the first lateral deviation and the first heading offset to continuously correct the track vehicle's lateral deviation between two absolute position beacon points includes:

[0028] The first lateral deviation and the first heading deviation are compared with the first safety threshold and the second safety threshold, respectively.

[0029] When the first lateral deviation exceeds the first safety threshold or the first heading deviation exceeds the second safety threshold, a correction torque command is output to the vehicle controller, and a warning signal is generated at the same time.

[0030] When the first lateral deviation exceeds the preset emergency braking threshold, an emergency braking trigger signal is output to the vehicle controller.

[0031] Specifically, the step of continuously correcting the lateral deviation of the rail vehicle between two absolute position beacon points by combining the first lateral deviation and the first heading offset further includes: determining the target lateral displacement and target heading angle change based on the first lateral deviation and the first heading offset, and generating a reference trajectory command sequence for the lateral controller to perform correction.

[0032] A Kalman filter-based lateral deviation data fusion system, used to implement the Kalman filter-based lateral deviation data fusion method, includes:

[0033] The data acquisition module acquires the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle.

[0034] The state estimation module, based on a preset vehicle lateral kinematics model, predicts the state using the angular rate and longitudinal velocity of the inertial measurement unit to obtain the first state estimate at the current moment.

[0035] The deviation analysis module projects the lateral deviation in the first state estimation onto the measurement direction of the ultrasonic sensor based on the attitude angle of the inertial measurement unit, generates the corresponding first distance measurement, and calculates the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset.

[0036] The lateral correction module, combining the first lateral deviation and the first heading offset, continuously corrects the lateral deviation of the rail vehicle between two absolute position beacon points.

[0037] The beneficial effects of this application are as follows: By using a coupled Kalman filter structure of an inertial measurement unit and an ultrasonic sensor, a gyroscope zero-bias online estimation channel is established at the state vector level. Combined with the coupling of the lateral deviation change rate and heading offset in the vehicle's lateral kinematics model, the heading offset can be corrected based on the continuous ranging of ultrasonic waves to the rail wall. In the measurement update stage, outliers are screened out. Based on the dynamic limit of the filter uncertainty adaptive adjustment, the occasional outliers in ultrasonic ranging are effectively distinguished from the actual lateral dynamic changes of the vehicle. This maintains the numerical stability of the filtering algorithm, suppresses long-term drift of heading offset, and adapts to changes in wheel wear status. Attached Figure Description

[0038] Figure 1 This is a flowchart illustrating the workflow of the lateral deviation data fusion method based on Kalman filtering in the embodiments of this application.

[0039] Figure 2 This is a schematic diagram of the structure of the lateral deviation data fusion system based on Kalman filtering in an embodiment of this application. Detailed Implementation

[0040] The present application will be further described in detail below with reference to the accompanying drawings and embodiments.

[0041] In the embodiments of this application, the terms "exemplary" or "for example" are used to indicate that something is an example, illustration, or description. Any embodiment or design that is described as "exemplary" or "for example" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or design. Specifically, the use of the terms "exemplary" or "for example" is intended to present the relevant concepts in a specific manner.

[0042] Hereinafter, the terms "first," "second," and other generic terms are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0043] refer to Figure 1 The diagram illustrates a specific implementation of the lateral deviation data fusion method based on Kalman filtering in this application, including:

[0044] S101. Obtain the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle.

[0045] S102. Based on the preset vehicle lateral kinematics model, the state is predicted by the angular rate and longitudinal velocity of the inertial measurement unit to obtain the first state estimate at the current moment.

[0046] S103. Based on the attitude angle of the inertial measurement unit, the lateral deviation in the first state estimation is projected onto the measurement direction of the ultrasonic sensor to generate the corresponding first distance measurement, and the residual between the first distance measurement and the corresponding distance is calculated to obtain the first lateral deviation and the first heading offset.

[0047] S104. Combining the first lateral deviation and the first heading offset, the rail vehicle is continuously laterally corrected between the two absolute position beacon points.

[0048] In this embodiment, a Kalman filter-based lateral deviation data fusion method is used for rail vehicles traveling between two adjacent absolute position beacon points to acquire real-time lateral deviation and heading offset of the vehicle relative to the track, providing continuous state input for the vehicle's active lateral correction. The attitude recursion of the inertial measurement unit and the distance measurement of the ultrasonic sensor are incorporated into a unified Kalman filter framework, achieving tight coupling at the state vector level. Simultaneously, a dynamic robustness mechanism bound to inertial prediction information is established in the measurement update stage, ensuring the reliability of the estimation even under adverse conditions such as abnormal track wall reflections.

[0049] In this embodiment, three signals are acquired from onboard sensors and the vehicle bus. The inertial measurement unit outputs angular velocity and acceleration measurements in differential signal form. After receiving the differential signals, they are first converted into single-ended signals, and then filtered by a second-order low-pass filter with a cutoff frequency of 30 Hz to remove high-frequency components introduced by wheel-rail impact and vehicle body vibration. The filtered angular velocity includes two components: heading angular rate and roll angular rate, and the filtered acceleration includes a lateral acceleration component. After low-pass filtering, glitches in the signals entering subsequent state derivations are significantly suppressed.

[0050] Ultrasonic sensors are mounted on the side of the vehicle body, emitting ultrasonic pulses with a center frequency of 40 kHz towards the rail wall at fixed intervals and receiving the echoes. Based on the flight time between transmission and reception, and combined with the ambient sound speed, the sensor calculates the distance from the mounting point on the vehicle body along the beam direction to the rail wall, outputting the distance between the vehicle body and the rail wall. In the case of multiple ultrasonic sensors arranged on one side of the vehicle body, each sensor is installed at intervals along the longitudinal direction of the vehicle, and each outputs a distance value in each measurement cycle. The longitudinal speed of the rail vehicle is obtained from the vehicle communication bus. The source can be the vehicle speed calculated by the traction control unit based on the motor speed, or the average of the wheel speeds measured by the left and right wheel speed sensors after wheel diameter calibration.

[0051] In this embodiment, the construction of the state vector is the foundation of state prediction. The state vector includes three components: lateral deviation, heading offset, and gyro zero bias. Lateral deviation is defined as the distance between the projection point of the vehicle's center of mass on the track surface and the track centerline; heading offset is defined as the angle between the vehicle's longitudinal axis and the tangent to the track centerline; gyro zero bias is the constant deviation of the output of the gyro channel in the inertial measurement unit that is sensitive to the heading angular rate. This deviation is accumulated synchronously during the angular rate integration process and is the source of error in the heading offset estimation that diverges over time. By using the gyro zero bias as an online state variable for estimation, this deviation can be identified and subtracted during the filtering process, rather than relying on static calibration values.

[0052] Specifically, the state prediction is based on a preset vehicle lateral kinematics model. The rate of change of lateral deviation is determined by the longitudinal velocity and heading offset. Under operating conditions with a small heading offset angle, the rate of change is taken as the product of the longitudinal velocity and the heading offset. The rate of change of heading offset is equal to the true heading angular rate of the vehicle body. This true heading angular rate is obtained by subtracting the gyro zero bias in the state vector from the heading angular rate measured by the inertial measurement unit. The change of the gyro zero bias itself in a short time is modeled as a random walk, driven by process noise.

[0053] During the time update process of Kalman filtering, the prior values ​​of each state component at the current time are recursively obtained based on the posterior state of the previous time step and the aforementioned kinematic relationship. Simultaneously, the state transition matrix drives the error covariance matrix to propagate forward, obtaining the prior error covariance matrix. The first state estimate, along with the prior error covariance matrix, is then passed to the measurement update stage.

[0054] With left and right wheel speed sensors, additional angular rate information provided by the wheel speed difference can be introduced during the state prediction stage. The left and right wheel speed sensors output the left and right wheel speeds respectively; the difference between them, divided by the wheelbase, yields a yaw rate value, which serves as the virtual yaw rate observation. This observation, along with the effective yaw rate obtained by subtracting gyro zero bias from the inertial measurement unit, reflects vehicle rotation through different physical pathways, resulting in differences in their noise characteristics. The virtual yaw rate observation and the effective yaw rate are weighted and fused according to a weight inversely proportional to their respective noise variances. The fused angular rate replaces the individual effective yaw rate as the driving input for state prediction. Channels with higher noise variance contribute less to the fusion. This process improves the long-term stability of the yaw rate estimation and helps suppress yaw deviation divergence between long-distance beacon points.

[0055] In the implementation where the state vector extension includes the wheel diameter ratio factor of the left and right wheels, the wheel diameter ratio factor participates in the recursion and update along with the state vector, gradually approaching the true ratio of the equivalent radii of the left and right wheels, and compensating for the wheel speed difference conversion ratio error caused by the difference in wheel wear.

[0056] Before constructing the observation equations, a geometric relationship needs to be established between the lateral deviation in the prior state and the measurement directions of each ultrasonic sensor. Based on the installation height and installation angle of the ultrasonic sensors on the vehicle body, the unit vector of their beam direction in the vehicle coordinate system is determined. The installation height is the vertical distance from the sensor's transmitting surface to the rail surface, and the installation angle includes the horizontal deflection angle of the beam relative to the lateral vertical plane of the vehicle body and the pitch angle relative to the horizontal plane; these two deflection angle parameters together describe the spatial pointing of the beam. Combining these parameters, a set of direction cosines for the measurement directions of each sensor is calculated in the vehicle coordinate system.

[0057] Specifically, the roll and pitch angles output in real time by the inertial measurement unit reflect the vehicle's roll and pitch attitude relative to the rail surface. A rotational transformation relationship is constructed from the vehicle coordinate system to the rail coordinate system using the roll and pitch angles; the rail coordinate system uses the track lateral, track longitudinal, and rail surface normal axes as coordinate axes. The lateral deviation in the prior state is mapped to the rail coordinate system through this rotational transformation, eliminating projection errors caused by vehicle roll and pitch. The lateral deviation in the rail coordinate system is projected onto the measurement directions of each ultrasonic sensor to obtain the predicted distance for each sensor, called the first ranging.

[0058] The difference between the actual distance between the vehicle body and the rail wall output by each ultrasonic sensor and the corresponding first distance measurement is calculated to obtain the residual. The residual reflects the degree of deviation between the prior state and the current measurement. In actual tracks, the rail wall may have welded joints, auxiliary structural components, or abnormal surface conditions. Due to multipath reflections or partial obstruction, the ultrasonic sensors may output distance values ​​that significantly deviate from the true geometric distance. Directly introducing such abnormal distance values ​​into the Kalman filter measurement update would contaminate the state estimation. This implementation sets a consistency check before measurement update.

[0059] Specifically, the consistency test process includes: calculating the innovation variance of the predicted distance using the prior error covariance matrix, the observation noise variance calibrated by the ultrasonic sensor, and preset confidence level parameters; taking the square root of the innovation variance and multiplying it by the quantile factor corresponding to the confidence level to obtain the dynamic limit. The confidence level is set to three times the standard deviation, and the quantile factor is approximately 3. The dynamic limit adaptively adjusts with changes in prior uncertainty: after beacon point correction, the prior uncertainty is small, and the dynamic limit is narrow; after long-distance extrapolation, the prior uncertainty increases, and the dynamic limit widens accordingly.

[0060] The absolute value of each residual is compared with its corresponding dynamic limit. When the absolute value of the residual exceeds the dynamic limit, the change in lateral acceleration output by the inertial measurement unit within a preset time window is further examined. The window length covers 0.1 seconds prior to the current time and includes several sampling points. If the change in lateral acceleration within the window is less than a preset threshold, it indicates that the vehicle has not undergone actual lateral movement corresponding to the abnormal residual amplitude, and the residual is determined to be an outlier. Conversely, if the change in lateral acceleration exceeds the threshold, the abnormal residual may be related to the transient lateral dynamics of the vehicle and is not determined to be an outlier.

[0061] For residuals identified as outliers, the observation noise variance of the current measurement by the ultrasonic sensor is increased by a preset factor. Since the observation noise variance appears in the denominator of the Kalman gain calculation, its increase brings the Kalman gain of that measurement close to zero. This suppresses the actual contribution of the measurement information to the state correction, effectively ignoring the outlier measurement. By not directly discarding the measured values, the stability of the observation equation's dimension is maintained. For measurements not identified as outliers, the observation noise variance remains unchanged.

[0062] Furthermore, using the verified and processed residuals and the adjusted observation noise variance, the measurement update equation of Kalman filtering is executed to calculate the Kalman gain matrix, which is then applied to the prior state to obtain the posterior state. The lateral deviation component and heading offset component in the posterior state are the first lateral deviation and the first heading offset output in this step.

[0063] It should be noted that although the heading deviation is not a direct observation of the ultrasonic sensor, in the observation equation, the lateral deviation and heading deviation are coupled through the vehicle attitude projection. The measurement update's correction of the lateral deviation will indirectly correct the heading deviation estimate and improve the observability of the heading deviation in the beacon-free section.

[0064] In implementations where ultrasonic sensors provide echo signal envelopes, once a measurement is identified as an outlier, the echo signal can be further analyzed. If multiple peak values ​​exceeding the detection threshold are detected in the echo envelope, it indicates the presence of multiple echoes. Multiple echoes typically originate from primary reflections caused by foreign objects or attachments in front of the track wall and secondary reflections from the track wall itself. The flight time corresponding to each echo is extracted from the multiple echoes and converted into multiple echo distances. These echo distances are compared one by one with the first distance measurement, and the echo distance with the smallest difference from the first distance measurement is selected to replace the original distance between the vehicle body and the track wall. The residual is then recalculated and a measurement update is performed. This process allows for the extraction of distance values ​​closer to the true geometric relationship from suboptimal reflection paths when redundant information exists in the echoes, improving the utilization rate of measurement data under abnormal conditions.

[0065] Furthermore, the first lateral deviation is compared with a first safety threshold, and the first heading deviation is compared with a second safety threshold. The first safety threshold is the maximum permissible lateral deviation under normal operation, and the second safety threshold is the maximum permissible heading angle. When the first lateral deviation exceeds the first safety threshold or the first heading deviation exceeds the second safety threshold, a corrective torque command is output to the vehicle controller via a communication message, and a warning signal is generated. The vehicle controller drives the steering actuator to adjust the wheel steering angle according to the corrective torque command, gradually guiding the vehicle back to the track centerline.

[0066] When the first lateral deviation exceeds the preset emergency braking threshold, an emergency braking trigger signal is output to the vehicle controller. This signal directly drives the braking system to perform an emergency stop via a safety circuit.

[0067] Within the normal operating range where emergency braking is not triggered, a reference trajectory command sequence is generated at the same cycle as the Kalman measurement update, based on the first lateral deviation and the first heading offset. This reference trajectory command sequence contains a set of time-ordered changes in target lateral displacement and target heading angle, describing a smooth path from the current offset state to the zero-offset state. Upon receiving this command sequence, the lateral controller uses tracking control to guide the vehicle along the reference trajectory, avoiding lateral shocks caused by step corrections.

[0068] As the vehicle travels between two adjacent absolute position beacon points, the above four steps are continuously and cyclically executed. When the vehicle passes a beacon point, the absolute lateral position and absolute heading provided by the beacon serve as external strong observations to correct the filter, eliminating the accumulated errors in the calculation within the section. After leaving the beacon point, the vehicle enters the next continuous estimation cycle between beacons, forming a closed-loop process that provides continuous and reliable state awareness support for the active lateral correction of the rail vehicle.

[0069] This application establishes a gyroscope zero-bias online estimation channel at the state vector level through a coupled Kalman filter structure of an inertial measurement unit and an ultrasonic sensor. By combining the coupling of the lateral deviation change rate and heading offset in the vehicle's lateral kinematics model, the heading offset can be corrected based on the continuous ultrasonic ranging of the rail wall. In the measurement update stage, outliers are screened out. Based on the dynamic limit of the filter uncertainty adaptive adjustment, the occasional outliers in ultrasonic ranging are effectively distinguished from the actual lateral dynamic changes of the vehicle. This maintains the numerical stability of the filtering algorithm, suppresses long-term heading offset drift, and adapts to changes in wheel wear.

[0070] Furthermore, acquiring the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle includes: receiving the angular velocity and acceleration measurements output by the inertial measurement unit in the form of differential signals, converting the differential signals into single-ended signals, and performing low-pass filtering to obtain the angular velocity and acceleration.

[0071] In this embodiment, the signal output by the inertial measurement unit (IMU) is conditioned to obtain angular velocity and acceleration data suitable for Kalman filtering. The IMU's sensing devices, which are sensitive to angular velocity and acceleration, output two analog signals of opposite polarity and equal amplitude in the form of differential pairs. The two transmission lines of the differential signals are spatially tightly coupled, and the noise voltages induced by external electromagnetic interference on these two lines have the same polarity and approximately the same amplitude, exhibiting common-mode characteristics.

[0072] The differential signal is fed into a differential amplifier with a high common-mode rejection ratio. This differential amplifier performs a subtraction operation on the two input signals: subtracting the negative polarity signal from the positive polarity signal. The useful components corresponding to the physical quantities double in amplitude due to opposite polarities, while common-mode noise cancels out due to identical polarities. The result of the subtraction operation is a single-ended voltage signal referenced to ground potential. The amplitude of this single-ended voltage signal is proportional to the instantaneous value of the angular velocity or acceleration on the sensitive axis of the inertial measurement unit. The differential signal is converted into a single-ended signal, common-mode noise is suppressed, and the processed values ​​are angular velocity and acceleration measurements in differential signal form. The output is a single-ended voltage signal.

[0073] Before entering the analog-to-digital converter, the single-ended voltage signal passes through an analog anti-aliasing filter constructed from operational amplifiers. The cutoff frequency of this analog filter is set to a value that coordinates with the subsequent digital filtering. Its function is to attenuate components in the signal spectrum above half the sampling rate before sampling, preventing high-frequency noise from folding into the low-frequency effective signal band after sampling.

[0074] The single-ended voltage signal, processed by an analog anti-aliasing filter, is converted into a digital sequence by an analog-to-digital converter at a predetermined sampling rate. The sampling rate is selected by comprehensively considering the highest frequency component of the vehicle's lateral motion and the design requirements of the subsequent digital filter. After conversion, a set of discrete digital sequences in time order is obtained, with values ​​representing the sampled values ​​of angular velocity or acceleration at the corresponding time points. The processed object is the analog-filtered single-ended voltage signal, and the output is a digital sequence.

[0075] The digital sequence contains high-frequency vibration components excited by factors such as track joint impact and wheel-rail surface roughness. These high-frequency components are separable from the effective information of the vehicle's lateral motion in the frequency domain. A second-order low-pass digital filter is performed on the digital sequence. The coefficients of this filter are determined by a preset cutoff frequency and sampling rate, with a typical Butterworth response as the design goal, making the amplitude response within the passband as flat as possible. In each sampling period, the processor reads the current sample value, sums it with historical sample values ​​from several previous periods according to the filter coefficients, and replaces the current sample value with the weighted sum. High-frequency components tend to approach zero during the summation process due to the alternating positive and negative values ​​of the weighting terms, while low-frequency components are retained because the weighting terms are superimposed in the same direction. The cutoff frequency is set to 30 Hz. Signal components above this frequency belong to high-frequency vibration noise, while those below this frequency are the effective frequency band of the vehicle's lateral motion. After filtering, smoothed angular velocity and acceleration measurements are obtained, and the filtered angular velocity and acceleration are output.

[0076] Preferably, the differential analog signal output by the inertial measurement unit is converted into a low-noise, smooth digital signal through common-mode noise suppression, anti-aliasing protection, and high-frequency vibration filtering, providing stable and reliable data input for state prediction.

[0077] Furthermore, based on a pre-defined vehicle lateral kinematics model, state prediction is performed using the angular rate and longitudinal velocity of the inertial measurement unit to obtain the first state estimate at the current moment, including:

[0078] S301. Construct a state vector, which includes lateral deviation, heading offset, and gyro zero bias;

[0079] S302. Based on the preset vehicle lateral kinematics model, the effective heading angular rate is obtained by removing the gyroscope zero bias from the angular rate of the inertial measurement unit.

[0080] S303. The product of longitudinal velocity and heading deviation is used as the rate of change of lateral deviation. Combined with the effective heading angular rate, the first state estimate at the current moment is obtained by recursion.

[0081] In this embodiment, the Kalman filter is updated in time. Using the posterior state from the previous time step and the input data from the current time step, the prior state estimate for the current time step, i.e., the first state estimate, is calculated. The time update consists of three functional units: state vector construction, effective heading angular rate calculation, and kinematic recursion. The Kalman filter mathematically describes the system using a state vector, which contains all the physical quantities that need to be estimated in real time. A floating-point array is defined in memory as the state vector, containing three components: lateral deviation, heading offset, and gyro zero bias.

[0082] Specifically, lateral deviation is defined as the lateral distance between the projection of the vehicle's center of mass onto the track surface and the track centerline. Positive and negative values ​​correspond to the two sides of the track centerline, respectively. Heading offset is defined as the angle between the vehicle's longitudinal axis and the tangent direction of the track centerline; a zero value indicates that the vehicle's axial direction is consistent with the track direction. Gyro zero bias is defined as the continuous deviation value output by the heading angular rate sensitive channel in the inertial measurement unit when there is no actual rotational input. This deviation value is superimposed with the actual angular rate and collected by the measured circuit. It is accumulated cycle by cycle during the angular rate integration process, constituting the error source of the heading offset estimation diverging over time.

[0083] Furthermore, initial values ​​are assigned to the state vector in the initial stage. The initial values ​​for lateral deviation and heading offset can be taken from the absolute position and heading reference provided by the beacon point where the vehicle starts. The initial value for gyro zero bias can be taken as the arithmetic mean of the heading angular rate data collected continuously for several seconds while the vehicle is stationary, with the collection duration ensuring the stability of the statistical mean.

[0084] Alongside the state vector, an error covariance matrix is ​​maintained. This matrix is ​​a square matrix corresponding to the dimension of the state vector. Its diagonal elements represent the variance of the estimated values ​​of each state component, while the off-diagonal elements represent the correlation between the estimation errors of different state components. During initialization, the variance of each state component is set according to the precision of its initial value: the absolute reference value provided by the beacon point has high precision, so its corresponding variance is set to a smaller value; the gyroscope zero bias, due to the potential difference between the mean value collected from static conditions and the actual operating value, has a corresponding variance set to a larger value.

[0085] Preferably, the gyroscope zero bias is incorporated into the state vector processing. During continuous vehicle operation, the gyroscope zero bias slowly drifts due to changes in temperature, device aging, and mechanical stress. If the factory calibration value is always used, the drift will be superimposed on the effective heading angular rate, resulting in a systematic increase in heading deviation after integration. By listing the gyroscope zero bias as a state variable to be estimated, the filter, in the measurement update step, uses information from the ultrasonic ranging residual to make minor adjustments to the estimated gyroscope zero bias value along the relevant direction indicated by the covariance matrix. This allows the zero bias estimate to gradually track the actual drift of the device, thereby maintaining the accuracy of heading deviation estimation over long distances between beacon points.

[0086] After constructing the state vector, the angular rate component corresponding to the heading-sensitive axis is extracted from the angular velocity data in each processing cycle. This component is a digital quantity directly output by the inertial measurement unit, and its value is the superposition of the vehicle's true heading angular rate and the gyroscope zero bias. The estimated value of the gyroscope zero bias is read from the current state vector, and this estimated value is subtracted from the original angular rate component to obtain the effective heading angular rate. The effective heading angular rate represents the true rotational speed and direction of the vehicle around the vertical axis, after deducting the influence of the inertial device's own bias. The subtracted gyroscope zero bias is not a static constant, but a state quantity updated online by the filter based on historical measurement information, and its value gradually approaches the actual physical bias of the device as the filter converges.

[0087] Preferably, the calculation of the effective heading angular rate establishes a real-time correction loop between the inertial measurement output and the filter state. In the initial stage of filter operation, the gyroscope zero-bias estimate may have deviations. Residual errors are reflected in the prior lateral deviation and heading offset through state recursion, thus affecting the residuals between the gyroscope and ultrasonic measurements. Measurement updates, while correcting for lateral deviation and heading offset, also correct the gyroscope zero-bias along the gradient direction through the covariance correlation between various state quantities. The corrected zero-bias value is used in this stage of the next cycle to calculate the new effective heading angular rate, forming a closed-loop correction. This allows for online identification of the gyroscope zero-bias without the need for dedicated calibration procedures or additional sensors, relying solely on continuous observation of the track wall by ultrasonic sensors during normal operation.

[0088] After obtaining the effective heading angular rate, the prior state at the current moment is recursively derived from the posterior state of the previous moment based on the vehicle's lateral kinematics model. The vehicle's lateral kinematics model is based on two physical relationships. The first relationship is the change of lateral deviation over time: when the heading deviation is not zero, the vehicle's longitudinal speed along the track will generate a component in the lateral direction. The magnitude of this component, under the condition of a small heading deviation angle, is taken as the product of the longitudinal speed and the heading deviation. The increment of the lateral deviation is the integral of this component over one processing cycle. The second relationship is the change of heading deviation over time: the rate of change of the heading deviation is equal to the vehicle's true heading angular rate, i.e., the effective heading angular rate. The increment of the heading deviation is the integral of the effective heading angular rate over one processing cycle.

[0089] In each processing cycle, the following recursion is performed: The lateral deviation posterior value, heading offset posterior value, and gyro zero-bias posterior value are extracted from the posterior state obtained from the measurement update at the previous moment; the lateral deviation posterior value is added to the product of the longitudinal velocity and the heading offset posterior value, and then multiplied by the processing cycle to obtain the prior value of the lateral deviation at the current moment; the heading offset posterior value is added to the effective heading angular rate and multiplied by the processing cycle to obtain the prior value of the heading offset at the current moment; the gyro zero-bias posterior value is directly used as the prior value at the current moment, as its short-term variation is modeled as a random walk. During the recursion, the longitudinal velocity is taken from the vehicle bus data obtained in step S101, and the processing cycle is set to a value matching the sensor data update rate.

[0090] Specifically, after recursively updating the state vector over time, the error covariance matrix is ​​updated synchronously. The state transition matrix, the posterior error covariance matrix of the previous time step, and the process noise covariance matrix are combined according to the Kalman filter time update equation to obtain the prior error covariance matrix. The elements of the state transition matrix reflect the transmission relationship between the state components, where the partial derivative of the lateral deviation state with respect to the heading deviation state includes the product of the longitudinal velocity and the processing period. The diagonal elements of the process noise covariance matrix reflect the process noise variances of lateral acceleration and heading angular rate, and their values ​​are selected based on the degree of track surface irregularity and the noise index of the inertial measurement unit.

[0091] Upon completion of the recursion, the prior state estimate and prior error covariance matrix are obtained and passed to the measurement update stage. The deterministic coupling between the lateral deviation rate of change and the heading offset in the kinematic model ensures that the heading offset estimation error is converted into a predicted lateral deviation error through the longitudinal velocity. This error, captured by ultrasonic ranging in subsequent measurement updates, is then used to inversely correct the heading offset via Kalman gain, forming an indirect correction channel for indirectly observed state quantities. This alleviates the problem of insufficient observability of heading offset in beacon-free regions.

[0092] Furthermore, the state vector also includes the wheel diameter ratio factor of the left and right wheels of the rail vehicle. When performing state prediction, if the rail vehicle is equipped with left and right wheel speed sensors, the difference between the left and right wheel speeds output by the left and right wheel speed sensors is used to construct a virtual heading angular rate observation. The virtual heading angular rate observation is then weighted and fused with the effective heading angular rate, and used as the input angular rate for state prediction.

[0093] In this embodiment, the effective heading angular rate is used as the input angular rate for state prediction. With the rail vehicle equipped with left and right wheel speed sensors, the state vector can be further extended and wheel speed difference information can be introduced to construct redundant heading angular rate observations. These observations are then weighted and fused to generate the input angular rate, thereby improving the state prediction's ability to suppress long-term heading drift.

[0094] Specifically, the state vector is expanded from three components in the basic scheme to four components, with the addition of a wheel diameter scaling factor. The wheel diameter scaling factor is defined as the ratio of the equivalent rolling radius of the left wheel to the equivalent rolling radius of the right wheel. Ideally, this ratio is 1, indicating that the diameters of the left and right wheels are the same. During operation, due to factors such as unequal travel distances of the inner and outer wheels when cornering, asymmetrical wear of the left and right wheels caused by tread braking, and different degrees of recovery on both sides after wheelset re-turning, the actual rolling radii of the left and right wheels will differ. When using the difference in wheel speeds to calculate the heading angular rate, the wheel diameter difference directly introduces a scaling error proportional to the deviation of the ratio from 1. This error, after integration, manifests as a continuous increase in the estimated heading deviation.

[0095] During initialization, the wheel diameter scaling factor is assigned an initial value of 1. The error covariance matrix is ​​simultaneously expanded by one row and one column. The newly added variance elements are set according to the historical statistical range of wheel wear, and the initial value of the newly added covariance elements is zero. During the filtering operation, the ultrasonic ranging residual is corrected online for the wheel diameter scaling factor by the element corresponding to the wheel diameter scaling factor in the Kalman gain matrix. The estimated value fluctuates around 1, and the fluctuation amplitude reflects the actual difference between the current left and right wheel diameters.

[0096] Furthermore, after determining that the rail vehicle is equipped with left and right wheel speed sensors through the vehicle bus configuration information, the left and right wheel speeds are synchronously collected in each processing cycle. The wheel speed sensors are installed at the ends of the left and right wheel axles and output pulse or frequency signals proportional to the rotational speed, which are then converted into angular velocity values.

[0097] Specifically, the process of constructing a virtual heading angular rate observation includes: multiplying the left wheel speed by the equivalent rolling radius of the left wheel to obtain the linear velocity of the left wheel; multiplying the right wheel speed by the equivalent rolling radius of the right wheel to obtain the linear velocity of the right wheel; and dividing the difference between the two linear velocities by the vehicle's wheelbase. The result is the heading angular rate calculated from the wheel speed difference. The vehicle's wheelbase is the lateral distance between the contact points of the left and right wheels. The left and right equivalent rolling radii used in the calculation are obtained by converting the nominal wheel diameter and the estimated value of the current wheel diameter scaling factor, ensuring that the ratio of the left and right radii is consistent with the definition of the wheel diameter scaling factor. The heading angular rate obtained through the above construction process is called the virtual heading angular rate observation. Its information comes from the wheel rotation geometry and is physically independent of the gyro channels of the inertial measurement unit.

[0098] Each processing cycle includes two data sources: effective heading angular rate and virtual heading angular rate observations. The effective heading angular rate is obtained by subtracting the gyroscope zero bias from the inertial measurement unit's angular rate, resulting in a fast response speed, but its long-term accuracy is affected by gyroscope drift. The virtual heading angular rate observation is calculated from the wheel speed difference, is not affected by gyroscope zero bias, and has good long-term stability, but its accuracy is affected by wheel diameter scaling factor estimation errors and wheel-rail creep.

[0099] The fusion weights are determined based on the current estimated variances of the two data sources. The variance of the effective heading angular rate channel is the sum of the variance of the gyro angular rate white noise and the variance of the gyro zero-bias estimate. The variance of the virtual heading angular rate channel is determined by comprehensively considering the variance of the wheel speed sensor measurement noise, the variance of the wheel diameter scaling factor estimate, and the tolerance of the wheel track parameter. The ratio of the reciprocal of each variance to the sum of the reciprocals of the total variances is the fusion weight of the corresponding data source, with data sources having larger variances receiving smaller weights. The two data sources are multiplied by their respective weights and then summed. The fusion result serves as the input angular rate for state prediction, replacing the practice of directly using the effective heading angular rate in the basic scheme.

[0100] Preferably, redundant wheel speed information independent of the inertial channel is introduced in the state prediction stage. This allows the prior estimate of the heading angular rate to be constrained by the wheel rotation geometry. When the gyroscope zero-bias estimation has not fully converged or residual drift exists, the virtual heading angular rate observation can provide an alternative angular rate reference independent of the gyroscope output, suppressing the unidirectional drift trend of the heading offset prior value in long-distance beacon-free regions. Simultaneously, the wheel diameter scaling factor participates in the online estimation as an extended state variable. Its value is automatically adjusted according to the wheel wear condition, maintaining the construction accuracy of the virtual observation throughout the entire operating cycle and avoiding the maintenance requirements of periodic manual measurement and parameter updates.

[0101] Furthermore, based on the attitude angle of the inertial measurement unit, the lateral deviation in the first state estimation is projected onto the measurement direction of the ultrasonic sensor to generate the corresponding first range, and the residual between the first range and the corresponding distance is calculated to obtain the first lateral deviation and the first heading offset, including:

[0102] S501. Determine the unit vector of the measurement direction relative to the vehicle coordinate system based on the installation height and installation angle of the ultrasonic sensor on the vehicle body.

[0103] S502. Based on the unit vector, using the roll angle and pitch angle currently output by the inertial measurement unit, the lateral deviation in the first state estimation is transformed from the vehicle coordinate system to the rail coordinate system and projected onto the measurement direction to obtain the first distance measurement.

[0104] S503. Calculate the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset.

[0105] In this embodiment, the prior state estimate is corrected using the distance between the vehicle body and the track wall measured by ultrasonic sensors to obtain the posterior lateral deviation and heading offset, namely the first lateral deviation and the first heading offset. The correction process is implemented through three functional units: first, the measurement direction of each sensor in the vehicle coordinate system is determined; then, the prior lateral deviation is projected as the predicted distance after attitude compensation; and finally, the residual between the predicted distance and the actual distance drives the measurement update of the Kalman filter.

[0106] Specifically, the mounting posture of each ultrasonic sensor on the vehicle body is described by two geometric parameters: mounting height and mounting angle. The mounting height is the vertical distance from the center of the sensor's acoustic emitting surface to the rail surface. The mounting angle includes horizontal and pitch angles. The horizontal angle is the deflection angle of the beam centerline relative to the vehicle's transverse vertical plane in the horizontal plane, and the pitch angle is the tilt angle of the beam centerline relative to the horizontal plane. These parameters are measured and determined during system installation and calibration, and are stored in the memory as fixed configuration data.

[0107] After reading the installation offset parameters of any sensor, a unit vector representing the sensor's measurement direction is constructed in the vehicle coordinate system. The longitudinal axis of the vehicle coordinate system is along the vehicle's forward direction, the transverse axis is perpendicular to the longitudinal axis pointing to the right side of the track, and the vertical axis follows the right-hand rule, pointing upwards. The construction process is as follows: starting from the initial direction pointing towards the vehicle's transverse axis, rotations around the vertical axis corresponding to the horizontal offset angle and rotations around the transverse axis corresponding to the pitch offset angle are applied sequentially. After rotation, a three-dimensional vector with a magnitude of 1 is obtained. The three components of this vector are the direction cosines of the measurement direction on the vehicle's longitudinal, transverse, and vertical axes, respectively. This unit vector serves as a fixed attribute for each sensor and is directly read and used in each processing cycle without recalculation. By parameterizing and solidifying the sensor installation attitude into a unit vector, sensors with different installation positions and pointing angles each correspond to a set of determined direction cosines. During measurement updates, the observation matrix elements between each sensor and the state variables are determined by these direction cosines, and the sensor's geometric characteristics are accurately incorporated into the filtering calculation.

[0108] Within each processing cycle, the current output roll and pitch angles are obtained from the inertial measurement unit. The roll angle is the angle of rotation of the vehicle body around its longitudinal axis, reflecting the degree of lateral tilt when the vehicle passes through a curve with superelevation. The pitch angle is the angle of rotation of the vehicle body around its lateral axis, reflecting the pitch attitude of the vehicle on slopes or where the track surface is vertically uneven.

[0109] Furthermore, a rotational transformation from the vehicle coordinate system to the rail coordinate system is constructed using roll and pitch angles. The rail coordinate system uses the transverse, longitudinal, and rail surface normal axes as coordinate axes. The rotational transformation consists of two steps: rotating the vehicle around its longitudinal axis by an angle equal to the roll angle, aligning the vehicle's transverse axis with the rail surface's transverse axis; and rotating the vehicle around the transformed transverse axis by an angle equal to the pitch angle, aligning the vehicle's vertical axis with the rail surface's normal axis.

[0110] The prior value of the lateral deviation is extracted from the first-state estimate; this value is defined in the vehicle coordinate system. The prior value of the lateral deviation is mapped to the rail coordinate system through the aforementioned rotation transformation, yielding the lateral deviation in the rail coordinate system. The rail-plane lateral deviation eliminates the projection distortion caused by vehicle roll and pitch. The rail-plane lateral deviation is then projected onto the unit vector of the measurement direction of each sensor as a scalar; the projected value is the distance change caused by the lateral deviation. This projected value is added to the reference distance measured by the sensor under ideal vehicle alignment conditions; the reference distance is measured and stored during system calibration. The sum is the predicted distance, called the first distance, representing the distance value that the sensor should measure assuming the prior state is accurate.

[0111] Preferably, by introducing roll and pitch angles, the construction of the predicted ranging synchronously compensates for the vehicle body roll effect caused by cornering superelevation. The attitude information of the inertial measurement unit and the ultrasonic ranging information form constraints in the construction process of the predicted ranging: when there is an error in the roll angle, the predicted ranging deviates from the true geometric relationship, and the resulting residual will be updated by measurement to correct the attitude-related state components in reverse.

[0112] Furthermore, the actual distance between the vehicle body and the track wall output by each ultrasonic sensor in the current cycle is read and subtracted from the corresponding first distance measurement to obtain the residual for each sensor. A positive residual indicates that the predicted distance is greater than the actual measured value, and the prior lateral bias may be too large; a negative residual indicates that the predicted distance is less than the actual measured value, and the prior lateral bias may be too small. Using the residual sequence as driving information, combined with the prior error covariance matrix, the observation matrix, and the observation noise covariance matrix, Kalman filtering is performed to update the measurement. The rows of the observation matrix correspond to each sensor, and the columns correspond to each state component. The matrix elements are determined by the lateral component of the unit vector of each sensor's measurement direction on the track surface. The Kalman gain matrix is ​​calculated, and each element in the Kalman gain matrix determines the distribution ratio of residual information among lateral bias, heading offset, and gyro zero bias.

[0113] Furthermore, the state correction is obtained by multiplying the Kalman gain matrix by the residual vector. This correction is then added to the prior state estimate to obtain the posterior state estimate. The lateral deviation component in the posterior state is the first lateral deviation, and the heading deviation component is the first heading deviation. Simultaneously, the error covariance matrix is ​​updated to obtain the posterior error covariance matrix.

[0114] Preferably, although the heading deviation is not a direct observation of the ultrasonic sensor, the covariance element between the lateral deviation and the heading deviation in the prior error covariance matrix records the error correlation between the two during the kinematic recursion: the estimation error of the heading deviation is mapped to the prior prediction error of the lateral deviation through the longitudinal velocity and the recursion period. When the measurement update corrects the lateral deviation based on the residual, the Kalman gain matrix automatically allocates a portion of the correction to the heading deviation through this covariance element, with the allocation proportion proportional to the longitudinal velocity. This mechanism enables the heading deviation to be indirectly corrected by continuous ultrasonic observation in beacon-free areas, avoiding divergence under pure inertial recursion, and the correction strength is adaptively adjusted with vehicle speed.

[0115] Furthermore, the residual between the first distance measurement and the corresponding distance is calculated to obtain the first lateral deviation and the first heading offset, including:

[0116] S601. Determine the dynamic limits based on the prior error covariance matrix corresponding to the first state estimate, the observation noise variance of the ultrasonic sensor, and the preset confidence level.

[0117] S602. Compare the absolute value of the residual with the dynamic limit. When the absolute value of the residual exceeds the dynamic limit, if the change in lateral acceleration output by the inertial measurement unit within the preset time window is less than the preset change threshold, the residual is determined to be an outlier. The observation noise variance of the ultrasonic sensor in this measurement is increased by a preset factor and used for Kalman gain calculation to obtain an updated Kalman filter.

[0118] S603. Calculate the first lateral deviation and the first heading offset based on the updated Kalman filter.

[0119] In this embodiment, after obtaining the residuals of each ultrasonic sensor, before proceeding to the measurement update, a consistency check is performed to determine the reliability of the residuals. Based on the determination result, the observation noise variance in the measurement update is adjusted to suppress outlier interference introduced by track wall reflection anomalies. This consistency check includes, in sequence, determining the dynamic limit, adjusting parameters based on dual discrimination, and performing the measurement update using the adjusted parameters.

[0120] From the prior error covariance matrix output in step S102, the predicted ranging variance corresponding to each sensor is extracted in conjunction with the observation matrix. The observation matrix consists of the components of the unit vectors of each sensor's measurement direction in the transverse direction on the track surface, and is a known constant matrix. The extracted predicted ranging variance reflects the mapping of prior state uncertainty in the ranging space. The observation noise variance of each ultrasonic sensor is a parameter that has been calibrated and stored beforehand. This parameter is obtained by collecting ranging data in the smooth section of the track wall and calculating its statistical variance, and is stored as a constant value after calibration. When the sensor characteristics change due to long-term use, the observation noise variance can be periodically updated using ranging data when the vehicle is stationary and centered.

[0121] Specifically, the predicted ranging variance is added to the observation noise variance of the sensor to obtain the innovation variance. The square root of the innovation variance is taken to obtain the innovation standard deviation, which is then multiplied by a preset confidence factor to obtain the dynamic limit. The confidence factor is set as a multiple of the standard deviation; for example, a value of 3 corresponds to approximately 99.7% confidence. The value of the dynamic limit adaptively adjusts as the prior error covariance matrix changes: after beacon point correction, the prior uncertainty is small, the dynamic limit narrows, and the discrimination is more sensitive; after long-distance extrapolation, the prior uncertainty increases, and the dynamic limit widens accordingly to avoid misclassifying normal prediction fluctuations as outliers.

[0122] The absolute value of each residual is compared with its corresponding dynamic limit. When the absolute value of the residual exceeds the dynamic limit, the lateral acceleration data output by the inertial measurement unit (IMU) within a preset time window is further read. The time window length is 0.1 seconds. If the IMU's data output rate is 100 Hz, the window contains 10 sampling points. The difference between the maximum and minimum lateral acceleration values ​​within the window is calculated as the change in lateral acceleration. The change threshold is determined based on the statistical distribution of the change in lateral acceleration within the same time window when the vehicle is traveling normally on a smooth track.

[0123] If the change in lateral acceleration is less than the threshold value, it indicates that the vehicle did not experience lateral dynamics matching the residual amplitude during that period. The anomaly in the residual lacks physical motion support and is therefore classified as an outlier. After being classified as an outlier, the observation noise variance corresponding to this sensor measurement is increased by a preset factor, such as 100 times, significantly compressing the contribution of this measurement to subsequent Kalman gain calculations, essentially ignoring the measurement. If the change in lateral acceleration reaches or exceeds the threshold value, the abnormal residual may be related to actual lateral motion; therefore, no outlier treatment is applied, and the observation noise variance remains unchanged.

[0124] When the absolute value of the residual does not exceed the dynamic limit, the measurement is considered normal, and the variance of the observation noise remains unchanged.

[0125] Furthermore, using the observation noise variance, prior error covariance matrix, and observation matrix after the above discrimination and adjustment as inputs, the Kalman gain matrix is ​​calculated. For sensor channels with inflated observation noise variance, the Kalman gain is correspondingly compressed, weakening its effect on state correction. The state correction amount is calculated using the Kalman gain matrix and residual vector to correct the prior state estimate, resulting in a posterior state estimate, where the lateral deviation component and heading offset component are the first lateral deviation and the first heading offset, respectively. Simultaneously, the error covariance matrix is ​​updated to the posterior error covariance matrix, completing the measurement update for this cycle.

[0126] Preferably, a dual discrimination method combining statistical limits and physical consistency verification suppresses occasional outliers from ultrasonic sensors while retaining effective measurement information reflecting the true dynamics of the vehicle. The dynamic limits adaptively adjust with filter uncertainty, ensuring that the sensitivity of outlier identification matches the current estimation accuracy. The introduction of lateral acceleration variation provides a physically redundant criterion independent of statistical tests, reducing the risk of misjudging outliers due to actual lateral impacts when relying solely on innovation statistics. After outlier determination, the method of amplifying the observation noise variance rather than directly deleting measurements ensures the consistency of the observation equation's dimensions and the numerical stability of the filtering algorithm.

[0127] Furthermore, after determining that the residual is an outlier, the following steps are also included:

[0128] S701. Acquire the echo signal detected by the ultrasonic sensor in the current measurement cycle, and filter out multiple echoes in the echo signal;

[0129] S702. From the multiple echo distances corresponding to the multiple echoes, select the echo distance with the smallest difference from the first distance measurement, and replace the distance between the car body and the rail wall.

[0130] In this embodiment, when the residual of an ultrasonic sensor is determined to be an outlier, the measurement is not discarded directly. Instead, multiple echo information is extracted from the echo signal of the sensor, and an attempt is made to replace the original measurement value contaminated by the outlier with the echo distance corresponding to the secondary reflection path. This optional processing method is performed after the aforementioned outlier determination step and before the measurement update is executed.

[0131] Specifically, after completing transmission and reception in each measurement cycle, the ultrasonic sensor, in addition to outputting the distance between the vehicle body and the track wall calculated based on the first echo time, can also upload sampled data of the echo signal envelope. The echo signal envelope is a time-ordered sequence of amplitudes, with the amplitude at each sampling point representing the received echo sound pressure intensity at the corresponding moment. The time axis is mapped to the flight time of the ultrasonic wave from transmission through reflection to return.

[0132] Furthermore, a pre-configured detection threshold is read. This threshold is determined based on the amplitude of the base noise collected by the sensor in an open, targetless environment, and is set to six times the root mean square (RMS) noise value. All sampling points within the echo signal envelope are traversed, and the amplitude at each point is compared to the detection threshold. Continuous intervals where the amplitude exceeds the threshold are marked, and the sampling point with the largest amplitude within each interval is taken as the arrival time of an echo. The flight time corresponding to each echo arrival time is recorded. The flight time is calculated by dividing the sampling point number by the sampling rate of the echo signal envelope, and then multiplying the flight time by the ambient sound speed and dividing by 2 to obtain the distance value corresponding to each echo.

[0133] When the number of identified echoes is greater than or equal to 2, multiple echoes are determined to exist, and the distance values ​​corresponding to all echoes are combined into a candidate echo distance set. When the number of echoes is 1, there are no multiple echoes, this optional processing method terminates, and the field value determination result remains unchanged. The first ranging value generated for the sensor in step S502 is read. The first ranging value is the predicted distance value obtained after attitude compensation and projection based on the lateral deviation in the prior state estimation, representing the current filter's estimate of the true geometric distance from the sensor to the track wall. Each echo distance in the candidate echo distance set is subtracted from the first ranging value, and the absolute value is taken to obtain the degree of deviation between each echo distance and the predicted distance. The magnitudes of each deviation degree are compared, and the echo distance with the smallest deviation degree is selected as the replacement value.

[0134] The replacement value is used to overwrite the original measured distance between the vehicle body and the rail wall. The residual is then recalculated using the replaced distance and the first distance measurement. The replaced residual is no longer subject to consistency checks and directly replaces the original residual that was judged as an outlier, entering the measurement update stage. Among the multiple echoes, the echo that is closest to the prior predicted distance has the highest geometric consistency with the current state estimate of the vehicle, and its probability of reflecting the true rail wall distance is greater than that of the first echo contaminated by abnormal reflections.

[0135] Preferably, through the above processing, when the ultrasonic sensor generates outlier measurements due to obstruction or interference of the main reflection path, the redundant secondary reflection path information in the echo signal envelope is used, combined with the filter's prediction of the current geometric distance, to recover an effective distance measurement that is closer to the true value. Without adding additional sensors, the usable proportion of effective measurement data under abnormal working conditions is increased, and the loss of measurement information caused by the deterioration of local reflection conditions on the track wall is reduced.

[0136] Furthermore, combining the first lateral deviation and the first heading offset, continuous lateral correction is performed on the rail vehicle between the two absolute position beacon points, including:

[0137] S801. Compare the first lateral deviation and the first heading deviation with the first safety threshold and the second safety threshold, respectively.

[0138] S802. When the first lateral deviation exceeds the first safety threshold or the first heading deviation exceeds the second safety threshold, a correction torque command is output to the vehicle controller, and a warning signal is generated at the same time.

[0139] S803. When the first lateral deviation exceeds the preset emergency braking threshold, an emergency braking trigger signal is output to the vehicle controller.

[0140] In this embodiment, by utilizing the first lateral deviation and the first heading offset, in the section between two absolute position beacon points, the corresponding lateral correction action or safety protection action is triggered based on the graded determination of the degree of deviation.

[0141] In each processing cycle, a first lateral deviation and a first heading offset are acquired. Pre-configured first safety threshold, second safety threshold, and emergency braking threshold are read from memory. The first safety threshold is the lateral deviation threshold that triggers active correction. Its value is determined by the minimum permissible clearance between the vehicle's dynamic envelope and the rail wall. Within this clearance range, lateral deviation of the vehicle will not cause mechanical interference with the rail wall. The second safety threshold is the heading offset threshold that triggers active correction. Its value is jointly determined by the vehicle's wheelbase and the rail wall clearance. It indicates that at this heading angle, the vehicle can complete attitude correction through the steering mechanism within a steering rate range that meets ride comfort limitations. The emergency braking threshold is the lateral deviation threshold that triggers emergency braking. Its value is greater than the first safety threshold and is determined by the critical lateral offset at which the vehicle's body structure physically contacts the rail wall in the most unfavorable roll posture. The three thresholds satisfy a progressively increasing relationship.

[0142] Specifically, the absolute value of the first lateral deviation is compared with a first safety threshold, the absolute value of the first heading deviation is compared with a second safety threshold, and the absolute value of the first lateral deviation is compared with an emergency braking threshold. The comparison results correspond to three response levels: if both deviations are within their respective safety thresholds, no correction action is triggered; if either deviation exceeds its corresponding safety threshold but the lateral deviation does not exceed the emergency braking threshold, a correction action is triggered; if the lateral deviation exceeds the emergency braking threshold, emergency braking is triggered.

[0143] When the comparison result triggers a correction action, a correction torque command is determined based on the first lateral deviation and the first heading offset. Using the first lateral deviation and the first heading offset as inputs, the target steering torque is calculated according to the control relationship between the steering torque and the lateral deviation and heading offset in the vehicle's lateral dynamics model. The control relationship is determined based on preset parameters such as vehicle mass and rail friction coefficient, ensuring that the lateral restoring force generated by the calculated steering torque can drive the vehicle body towards the track centerline without exceeding the rail adhesion limits. The correction torque command message, containing the amplitude and direction of the target steering torque, is sent to the vehicle controller via the vehicle communication bus. The vehicle controller then drives the steering actuator to adjust the wheel steering angle according to this command.

[0144] Simultaneously, an early warning signal is generated, triggering the on-board data recorder to store the status data for that period at high density, and to report the deviation event to the dispatch center through the vehicle-to-ground communication link. The reported content includes the timestamp of the deviation, location information, and deviation value.

[0145] When the comparison result triggers emergency braking, an emergency braking trigger signal is output to the vehicle controller via a signal channel independent of the vehicle communication bus. This independent signal channel is physically separated from the vehicle communication bus to ensure that signal transmission is not affected in the event of a communication bus failure. Upon receiving the emergency braking trigger signal, the vehicle controller cuts off traction and applies maximum braking force to execute an emergency stop. The emergency braking state requires manual confirmation to reset after the vehicle has come to a complete stop and does not automatically resume operation.

[0146] Preferably, through the aforementioned graded judgment and response mechanism, the deviation estimation result is converted into control actions matching the risk level. Within the range between the safety threshold and the emergency braking threshold, active correction attempts are made to restore the vehicle's track alignment, ensuring operational continuity. When the emergency braking threshold is exceeded, an irreversible safety stop is triggered via a signal channel independent of the communication bus, ensuring a safety baseline. The actions at both levels are driven by a unified deviation estimation result, completing judgment and output within the same processing cycle, ensuring the time determinism of perception and execution.

[0147] Furthermore, combining the first lateral deviation and the first heading offset, the continuous lateral correction of the rail vehicle between two absolute position beacon points also includes: determining the target lateral displacement and target heading angle change based on the first lateral deviation and the first heading offset, and generating a reference trajectory command sequence for the lateral controller to perform correction.

[0148] In this embodiment, after triggering the correction action, a correction torque command can be output, or a reference trajectory command sequence can be further generated for the lateral controller to execute. As an optional implementation, when it is determined that a correction action needs to be triggered, the target steering torque is not directly calculated. Instead, a reference trajectory is generated based on the first lateral deviation and the first heading offset, transitioning from the current offset state to the target state at the track centerline. The target lateral displacement and target heading angle change are output in the form of a time series.

[0149] Specifically, the first lateral deviation and the first heading offset at the current moment are taken as the starting state for trajectory planning, and the state where the lateral deviation is zero and the heading offset is zero is taken as the target state for trajectory planning. In the starting state, the value and direction of the lateral deviation are determined by the first lateral deviation, and the value and direction of the heading offset are determined by the first heading offset.

[0150] Furthermore, three sets of motion constraints are applied during trajectory planning. The upper limit of lateral velocity limits the maximum rate of lateral movement of the vehicle body, and its value is determined based on the mechanical speed limit of the steering actuator driving the wheels lateral movement. The upper limit of lateral acceleration limits the maximum amplitude of lateral acceleration of the vehicle body, and its value is determined by the rail adhesion coefficient and the vehicle mass, ensuring that the lateral force between the wheels and the rail does not exceed the available adhesion. The upper limit of lateral jerk limits the rate of change of lateral acceleration, and its value is determined based on the requirements for lateral jerk limits in ride comfort standards.

[0151] Recursive planning is performed with fixed time steps. At each time step, the lateral displacement margin and heading angle margin required to move from the current recursive state to the target state are calculated. Under the condition of satisfying the upper limit constraint of lateral acceleration, the executable direction and amount of lateral acceleration adjustment for this time step are determined to continuously approach the target state from the recursive state. When the lateral displacement margin decreases to the interval where deceleration and stopping are required, the deceleration curve is planned with the upper limit of lateral acceleration as the limit so that the lateral velocity synchronously returns to zero when reaching the target state. The planning of the heading angle dimension is coordinated with the lateral displacement dimension to ensure that the changes in lateral displacement and heading angle at the same trajectory point are consistent in vehicle kinematics.

[0152] After the recursion is completed, a sequence of trajectory points arranged by time steps is obtained. Each trajectory point contains the target's lateral displacement and heading angle values ​​at that moment, and is accompanied by a timestamp synchronized with the processing cycle. The entire sequence of trajectory points is used as a reference trajectory command sequence and output to the lateral controller point by point at the same cycle as the Kalman measurement update. The lateral controller executes the tracking of each trajectory point sequentially according to the timestamp.

[0153] Preferably, by generating a sequence of reference trajectory commands, the lateral controller obtains a set of predefined tracking targets, rather than a single torque command. The lateral controller can utilize the target value sequence for several future cycles to compensate for the response delay of the steering mechanism in advance through feedforward control, thereby improving the tracking accuracy of the reference trajectory. The acceleration constraints applied during trajectory planning ensure the continuity of lateral acceleration in the reference trajectory, preventing abrupt changes in the steering torque command during execution. This reduces the impact of sudden lateral movements of the vehicle body on passenger comfort and the impact load on the steering actuator.

[0154] like Figure 2 As shown, a Kalman filter-based lateral deviation data fusion system is used to implement a Kalman filter-based lateral deviation data fusion method, including:

[0155] The data acquisition module acquires the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle.

[0156] The state estimation module, based on a preset vehicle lateral kinematics model, predicts the state using the angular rate and longitudinal velocity of the inertial measurement unit to obtain the first state estimate at the current moment.

[0157] The deviation analysis module projects the lateral deviation in the first state estimation onto the measurement direction of the ultrasonic sensor based on the attitude angle of the inertial measurement unit, generates the corresponding first distance measurement, and calculates the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset.

[0158] The lateral correction module, combining the first lateral deviation and the first heading offset, continuously corrects the lateral deviation of the rail vehicle between two absolute position beacon points.

[0159] The above description is merely a preferred embodiment of this application. The scope of protection of this application is not limited to the above embodiments. All technical solutions falling within the scope of this application's concept are within the scope of protection of this application. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of this application should also be considered within the scope of protection of this application.

Claims

1. A lateral bias data fusion method based on Kalman filtering, characterized in that, include: The system acquires the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle. Based on the preset vehicle lateral kinematics model, the state is predicted by the angular rate and longitudinal velocity of the inertial measurement unit, and the first state estimate at the current moment is obtained. Based on the attitude angle of the inertial measurement unit, the lateral deviation in the first state estimation is projected onto the measurement direction of the ultrasonic sensor to generate the corresponding first distance measurement, and the residual between the first distance measurement and the corresponding distance is calculated to obtain the first lateral deviation and the first heading offset. By combining the first lateral deviation and the first heading offset, the rail vehicle is continuously corrected laterally between the two absolute position beacon points.

2. The lateral deviation data fusion method based on Kalman filtering according to claim 1, characterized in that, The process of obtaining the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle includes: receiving the angular velocity measurement value and acceleration measurement value output by the inertial measurement unit in the form of differential signals, converting the differential signals into single-ended signals, and performing low-pass filtering to obtain the angular velocity and acceleration.

3. The lateral deviation data fusion method based on Kalman filtering according to claim 1, characterized in that, The vehicle's lateral kinematics model, based on a preset model, uses the angular rate and longitudinal velocity of an inertial measurement unit to predict the state and obtain a first state estimate for the current moment, including: Construct a state vector, which includes lateral deviation, heading offset, and gyro zero bias; Based on the preset vehicle lateral kinematics model, the effective heading angular rate is obtained by removing the gyroscope zero bias from the angular rate of the inertial measurement unit. The product of longitudinal velocity and heading deviation is used as the rate of change of lateral deviation. Combined with the effective heading angular rate, the first state estimate at the current moment is obtained by recursion.

4. The lateral deviation data fusion method based on Kalman filtering according to claim 3, characterized in that, The state vector also includes the wheel diameter ratio factor of the left and right wheels of the rail vehicle. When performing state prediction, if the rail vehicle is equipped with left and right wheel speed sensors, the difference between the left and right wheel speeds output by the left and right wheel speed sensors is used to construct a virtual heading angular rate observation. The virtual heading angular rate observation is weighted and fused with the effective heading angular rate and used as the input angular rate for state prediction.

5. The lateral deviation data fusion method based on Kalman filtering according to claim 1, characterized in that, The attitude angle based on the inertial measurement unit projects the lateral deviation in the first state estimation onto the measurement direction of the ultrasonic sensor to generate the corresponding first range, and calculates the residual between the first range and the corresponding distance to obtain the first lateral deviation and the first heading offset, including: Based on the installation height and installation angle of the ultrasonic sensor on the vehicle body, determine the unit vector of the measurement direction relative to the vehicle body coordinate system; Based on the unit vector, using the roll angle and pitch angle currently output by the inertial measurement unit, the lateral deviation in the first state estimation is transformed from the vehicle coordinate system to the rail coordinate system and projected onto the measurement direction to obtain the first distance measurement. Calculate the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset.

6. The lateral deviation data fusion method based on Kalman filtering according to claim 5, characterized in that, The calculation of the residual between the first distance measurement and the corresponding spacing to obtain the first lateral deviation and the first heading offset includes: The dynamic limits are determined based on the prior error covariance matrix corresponding to the first state estimate, the observation noise variance of the ultrasonic sensor, and the preset confidence level. The absolute value of the residual is compared with the dynamic limit. When the absolute value of the residual exceeds the dynamic limit, if the change in lateral acceleration output by the inertial measurement unit within the preset time window is less than the preset change threshold, the residual is determined to be an outlier. The observation noise variance of the ultrasonic sensor in this measurement is increased by a preset factor and then used for Kalman gain calculation to obtain an updated Kalman filter. The first lateral deviation and the first heading offset are calculated based on the updated Kalman filter.

7. The lateral deviation data fusion method based on Kalman filtering according to claim 6, characterized in that, After determining that the residual is an outlier, the following steps are also included: Acquire the echo signal detected by the ultrasonic sensor within the current measurement cycle, and filter out multiple echoes in the echo signal; From the multiple echo distances corresponding to the multiple echoes, select the echo distance with the smallest difference from the first distance measurement and replace the distance between the car body and the rail wall.

8. The lateral deviation data fusion method based on Kalman filtering according to claim 1, characterized in that, The method of combining the first lateral deviation and the first heading offset to continuously correct the track vehicle's lateral deviation between two absolute position beacon points includes: The first lateral deviation and the first heading deviation are compared with the first safety threshold and the second safety threshold, respectively. When the first lateral deviation exceeds the first safety threshold or the first heading deviation exceeds the second safety threshold, a correction torque command is output to the vehicle controller, and a warning signal is generated at the same time. When the first lateral deviation exceeds the preset emergency braking threshold, an emergency braking trigger signal is output to the vehicle controller.

9. The lateral deviation data fusion method based on Kalman filtering according to claim 8, characterized in that, The method of combining the first lateral deviation and the first heading offset to continuously correct the track vehicle between two absolute position beacon points further includes: determining the target lateral displacement and target heading angle change based on the first lateral deviation and the first heading offset, and generating a reference trajectory command sequence for the lateral controller to perform correction.

10. A lateral deviation data fusion system based on Kalman filtering, used to implement the lateral deviation data fusion method based on Kalman filtering as described in any one of claims 1 to 9, characterized in that, include: The data acquisition module acquires the angular velocity and acceleration output by the inertial measurement unit on the rail vehicle, the distance between the vehicle body and the rail wall measured by the ultrasonic sensor, and the longitudinal velocity of the rail vehicle. The state estimation module, based on a preset vehicle lateral kinematics model, predicts the state using the angular rate and longitudinal velocity of the inertial measurement unit to obtain the first state estimate at the current moment. The deviation analysis module projects the lateral deviation in the first state estimation onto the measurement direction of the ultrasonic sensor based on the attitude angle of the inertial measurement unit, generates the corresponding first distance measurement, and calculates the residual between the first distance measurement and the corresponding distance to obtain the first lateral deviation and the first heading offset. The lateral correction module, combining the first lateral deviation and the first heading offset, continuously corrects the lateral deviation of the rail vehicle between two absolute position beacon points.

Images