System and method for dead reckoning correction based on radio frequency identification zero-reset
By combining RFID zero-point reset and lateral ranging sensors, the problem of dead reckoning error divergence in the absence of satellite signals by inertial measurement units was solved, enabling accurate attitude reckoning of rail vehicles and ensuring the validity of detection data and driving safety.
Patent Information
- Application Number
- CN202610829535.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2026-06-10
- Publication Date
- 2026-08-25
- Estimated Expiration
- 2046-06-10
AI Technical Summary
In areas without satellite signal obstruction, the dead reckoning error of the inertial measurement unit diverges over time, causing the estimated position of the rail vehicle to deviate from the track centerline, affecting the validity of the detection data and driving safety.
A dead reckoning correction method based on RFID zero-point reset is adopted. By using a lateral ranging sensor and RFID tags, the observation equations of lateral position error and heading angle error are constructed to continuously correct the predicted pose of the inertial measurement unit. When the RFID signal is received, the track position state is reset to keep the lateral and heading states unchanged. The heading angle rate zero bias is corrected by using the mileage difference between adjacent tags.
In long-distance, label-free areas without satellite positioning, it effectively suppresses dead reckoning errors, maintains lateral and heading position accuracy, avoids attitude jumps, and improves the accuracy and security of orbit detection data.
Smart Images

Figure CN122360516B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of dead reckoning correction technology, and more specifically to a dead reckoning correction system and method based on radio frequency identification zero-point reset. Background Technology
[0002] Currently, for track inspection vehicles, engineering vehicles, and operating trains operating in tunnels, underground lines, or sections obscured by buildings, onboard positioning systems are relied upon to obtain real-time mileage, lateral offset, and heading angle along the track direction to support functions such as track geometry inspection, train operation control, and safety protection. In most sections of the line, Global Navigation Satellite System (GNSS) signals are blocked or severely attenuated, making it impossible to provide a continuous position reference. Inertial Measurement Units (IMUs) are typically used for dead reckoning, recursively estimating attitude by integrating the angular rate and acceleration of the inertial devices. Dead reckoning can operate independently without external signals, but the zero bias and scaling factor errors in the angular rate and acceleration accumulate continuously during integration, causing position errors to diverge over time and affecting the accuracy of the position reckoning results.
[0003] The existing technology has the following problems: the error of dead reckoning in all degrees of freedom in the tagless section continues to diverge. Among them, the heading angle error caused by the zero offset of the gyroscope heading axis will cause the estimated lateral position of the vehicle to deviate from the track centerline, and the deviation increases quadratically with the increase of the travel distance; the RFID tag only provides the absolute position along the track direction. If the entire state is reset directly when the tag is triggered, the lateral position and heading angle will also be overwritten, which will destroy the continuity of the original estimate; if only the position along the track is reset without correcting the heading angle error, the lateral and heading drifts will still accumulate over time, causing the estimated lateral position of the vehicle in the tagless section to gradually deviate from the actual track centerline, affecting the validity of track detection data and the guarantee of driving safety. In order to solve at least one of the above problems, this application proposes a dead reckoning correction system and method based on RFID zero-point reset. Summary of the Invention
[0004] To address the shortcomings of existing technologies, the purpose of this application is to provide a dead reckoning correction system and method based on radio frequency identification zero-point reset, which can effectively solve the problems in the background technology. The specific technical solution of this application is as follows:
[0005] Dead reckoning correction methods based on RFID zero-point reset include:
[0006] The angular rate and acceleration from the inertial measurement unit are used to perform dead reckoning to generate the first predicted pose of the vehicle, which includes the position along the track, the lateral position, and the heading angle.
[0007] In the interval where no RFID reader receives a signal indicating that the RFID tag along the track has been read, an observation equation for the lateral position error and heading angle error is constructed based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information. The first predicted pose is then continuously corrected to obtain the intermediate pose.
[0008] When a read event signal is received, the track position state of the intermediate pose is reset to the track position corresponding to the read RFID tag, the track position covariance is reset to the preset initial value, and the lateral position state, heading angle state and covariance of the intermediate pose are kept unchanged to obtain the reset pose.
[0009] Output the reset pose, and use the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag.
[0010] Specifically, the dead reckoning of angular rates and accelerations from the inertial measurement unit includes:
[0011] The differential angular rate signal and differential acceleration signal output by the inertial measurement unit are converted into single-ended angular rate signal and single-ended acceleration signal, respectively.
[0012] The single-ended angular rate signal and single-ended acceleration signal are compensated based on the pre-calibrated scaling factor and zero bias to obtain the compensated angular rate and acceleration.
[0013] Dead reckoning is performed on the compensated angular rate and acceleration.
[0014] Specifically, the observation equations for lateral position error and heading angle error, constructed based on the lateral distance collected by the lateral ranging sensor and the preset orbital geometry information, include:
[0015] The difference between the lateral distance and the reference lateral distance determined based on the track geometry information is calculated as the lateral position error;
[0016] The heading angle error is calculated based on the ratio of the time rate of change of the lateral distance to the along-track velocity in the first predicted pose.
[0017] Using the lateral position error and heading angle error as observations, an observation matrix is constructed that reflects the mapping relationship between the lateral position state, heading angle state and the observations.
[0018] Specifically, before constructing the observation equation, the method also includes: identifying abrupt changes in the lateral distance; when the rate of change of the lateral distance exceeds a preset rate of change threshold within a sliding window of a preset duration and continues for a preset duration, the lateral distance within the corresponding time period is marked as a turnout area measurement value and deleted.
[0019] Specifically, the read event signal includes received signal strength indication information; upon receiving the read event signal, it also includes:
[0020] Generate a signal strength sequence based on the received signal strength indication information;
[0021] Identify the peak point in the signal intensity sequence and use the time corresponding to the peak point as the reset time;
[0022] Based on the reset time, perform time interpolation on the intermediate pose sequence to obtain the aligned reset intermediate pose;
[0023] The step of resetting the track position state of the intermediate pose to the track position corresponding to the RFID tag being read is performed on the aligned intermediate pose.
[0024] Specifically, when a read event signal is received, the intermediate pose's track position state is reset to the track position corresponding to the read RFID tag, the track position covariance is reset to a preset initial value, and the intermediate pose's lateral position state, heading angle state, and covariance remain unchanged to obtain a reset pose. This process further includes:
[0025] Obtain the mileage difference between the RFID tag read this time and the RFID tag read last time;
[0026] Obtain the cumulative displacement along the track between two readings of the intermediate pose sequence;
[0027] The correction amount for zero deviation of heading angular rate is calculated based on the difference between the mileage difference and the displacement along the track, as well as the time interval between the two readings.
[0028] The correction is superimposed on the zero-bias state of the heading angular rate of the nonlinear filter, and the covariance of the zero-bias state of the heading angular rate is reset.
[0029] Specifically, resetting the position covariance along the track to a preset initial value includes:
[0030] Determine the mileage interval between the RFID tag read this time and the RFID tag read last time;
[0031] Based on the mileage interval, a preset initial value is determined according to a pre-defined functional relationship.
[0032] Specifically, the output reset pose includes: synchronizing the reset pose with the system timestamp, writing it into the vehicle positioning data output buffer, and triggering the state variables stored in the dead reckoning integrator to be overwritten by the reset pose.
[0033] Specifically, the step of using the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag includes: before receiving the next read event signal, using the reset pose as the integration starting point for the dead reckoning process in the nonlinear filter, and compensating the angular rate output by the inertial measurement unit with the updated zero bias of the heading angular rate before performing dead reckoning.
[0034] The dead reckoning correction system based on RFID zero-point reset is used to implement the dead reckoning correction method based on RFID zero-point reset, including:
[0035] The dead reckoning module performs dead reckoning on the angular rate and acceleration from the inertial measurement unit to generate the vehicle's first predicted pose, which includes the position along the track, the lateral position, and the heading angle.
[0036] The pose correction module, in the interval where no RFID reader has received a reading event signal for RFID tags along the track, constructs an observation equation for lateral position error and heading angle error based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information, and continuously corrects the first predicted pose to obtain the intermediate pose.
[0037] When the pose reset module receives a read event signal, it resets the track position state of the intermediate pose to the track position corresponding to the read RFID tag, resets the track position covariance to a preset initial value, and keeps the lateral position state, heading angle state and covariance of the intermediate pose unchanged to obtain the reset pose.
[0038] The pose feedback module outputs a reset pose and uses the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag.
[0039] The beneficial effects of this application are as follows: By decoupling and coordinating continuous constraints of lateral ranging with discrete resets of radio frequency identification (RFID), the entire trajectory of the rail vehicle's pose can be estimated in long-distance, tagless intervals without satellite positioning; lateral ranging continuously corrects the lateral position and heading angle, keeping the error within a small range in the tagless interval; when the RFID tag is triggered, only the track position is reset while the lateral and heading states remain unchanged, avoiding attitude jumps introduced by full-state resets; the known mileage difference between adjacent tags is used as an external scale reference to correct the zero bias of the heading angular rate online and reduce the heading drift rate interval by interval; after reset, the track position covariance is dynamically adjusted according to the tag spacing, so that the filter's confidence in each reset matches the interval length, suppressing full-degree-of-freedom drift in dead reckoning and improving the accuracy of pose estimation. Attached Figure Description
[0040] Figure 1This is a flowchart illustrating the dead reckoning correction method based on radio frequency identification zero-point reset in the embodiments of this application.
[0041] Figure 2 This is a schematic diagram of the dead reckoning correction system based on radio frequency identification zero-point reset in the embodiments of this application. Detailed Implementation
[0042] The present application will be further described in detail below with reference to the accompanying drawings and embodiments.
[0043] 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.
[0044] 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.
[0045] refer to Figure 1 The image shows a specific implementation of the dead reckoning correction method based on RFID zero-point reset in this application, including:
[0046] S101. Perform dead reckoning on the angular rate and acceleration from the inertial measurement unit to generate the first predicted pose of the vehicle, the first predicted pose including the position along the track, the lateral position and the heading angle;
[0047] S102. In the interval where no RFID reader receives a signal indicating that the RFID tag along the track has been read, based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information, an observation equation for the lateral position error and heading angle error is constructed, and the first predicted pose is continuously corrected to obtain the intermediate pose.
[0048] S103. When a read event signal is received, the track position state of the intermediate pose is reset to the track position corresponding to the read RFID tag, the track position covariance is reset to the preset initial value, and the lateral position state, heading angle state and covariance of the intermediate pose are kept unchanged to obtain the reset pose.
[0049] S104. Output the reset pose and use the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag.
[0050] This embodiment provides a method for zero-point reset and dead reckoning correction based on radio frequency identification (RFID). The on-board positioning processing unit of the rail vehicle processes signals from the inertial measurement unit, lateral ranging sensor and RFID reader, and outputs the corrected vehicle position and attitude.
[0051] Specifically, the onboard positioning processing unit receives angular rate and acceleration signals output by the inertial measurement unit according to the sampling period. Starting from the initial pose or the pose corrected in the previous period, it updates the attitude by integrating the angular rate to obtain the vehicle attitude including the heading angle. After transforming the acceleration to the navigation coordinate system through the attitude matrix and subtracting the gravity component, it performs a first integration to obtain the velocity increment and a second integration to obtain the position increment, and accumulates them to generate the first predicted pose. The first predicted pose includes the track position, lateral position, and heading angle. The track position is the vehicle's coordinate in the direction of track mileage, the lateral position is the vehicle's lateral offset relative to the track centerline, and the heading angle is the angle between the vehicle's longitudinal axis and the tangent direction of the track centerline.
[0052] Specifically, the signal output by the inertial measurement unit (IMU) can be in differential form. As an optional implementation, the positioning processing unit first converts the differential angular rate and differential acceleration signals into single-ended signals, and then uses the scaling factor and zero-bias parameter stored in non-volatile memory to compensate for the single-ended signals: the single-ended angular rate is subtracted from the gyro zero-bias and then multiplied by the gyro scaling factor; the single-ended acceleration is subtracted from the accelerometer zero-bias and then multiplied by the accelerometer scaling factor. The compensated angular rate and acceleration are used to perform dead reckoning. The scaling factor and zero-bias parameter are obtained through factory calibration. During the calibration process, the IMU is placed on a temperature-controlled turntable, and the output is collected at different temperatures and rotational speeds. The parameters are determined by least-squares fitting.
[0053] When no RFID reader signal is received indicating a read event of RFID tags along the track, the positioning processing unit executes a continuous calibration process. On the input side, a lateral ranging sensor collects the lateral distance from the vehicle to the track sidewall or tunnel wall and sends it to the positioning processing unit. The sensor is mounted on the side of the vehicle and emits laser pulses perpendicular to the vehicle's longitudinal axis, calculating the lateral distance using time-of-flight. The positioning processing unit's memory stores pre-set track geometry information, consisting of a data table of discrete samples along the track mileage. Each sampling point records the reference lateral distance at that mileage. The reference lateral distance is determined by the design coordinates of the track centerline, the design track gauge, and the sensor's installation offset on the vehicle.
[0054] Specifically, the processing consists of two steps: observation generation and filter update. Observation generation: The positioning processing unit takes the currently acquired lateral distance and subtracts it from the reference lateral distance obtained by linear interpolation from the orbital geometry information table based on the current odometer estimate, to obtain the lateral position error. It also takes the lateral distance sequence from the most recent period and subtracts it to obtain the rate of change over time, dividing it by the along-track velocity in the first predicted pose to obtain the heading angle error. The along-track velocity is provided by the velocity state component calculated from dead reckoning. Filter update: The positioning processing unit constructs observation equations using the lateral position error and heading angle error as observations, driving the extended Kalman filter to perform state updates and covariance updates.
[0055] The Extended Kalman Filter (EKF) is a recursive nonlinear state estimator whose state vector includes at least track position, lateral position, and heading angle, and may further include error states such as gyroscope zero bias. In the observation matrix, the lateral position state corresponds to the lateral position error observation, and the corresponding position is assigned a value of 1; the heading angle state corresponds to the heading angle error observation, and the corresponding position is assigned a value of 1; the corresponding elements for other states are 0. After the filter is updated, the intermediate pose is output.
[0056] Preferably, the lateral position error and heading angle error are continuously observed and corrected, and their covariance remains at a low level; the position along the track accumulates over time due to the lack of direct observation.
[0057] Specifically, the lateral distance in the turnout area exhibits abrupt changes, and directly using it in the observation equation would introduce error correction. As an optional implementation, before constructing the observation equation, the positioning processing unit performs the following identification and rejection of lateral distances: It maintains a fixed-length sliding window storing the lateral distance values for the most recent several periods. For each period, it calculates the difference between the maximum and minimum values within the window, divides it by the window duration, and obtains the amplitude change rate. When the amplitude change rate exceeds a preset change rate threshold and persists for a preset duration, it is determined that the current turnout area has been traversed, and the lateral distance for the corresponding time period is marked and discarded, not participating in the construction of the observation equation. The change rate threshold and preset duration are calibrated through real-vehicle testing. Lateral distance data are collected in known turnout sections, and the change rate distributions of normal sections and turnout sections are statistically analyzed. The boundary value distinguishing the two is selected as the threshold, and the duration is the shortest time for a vehicle to pass through the turnout.
[0058] Furthermore, the RFID reader continuously transmits radio frequency signals. When a vehicle approaches an RFID tag deployed along the track, the tag reflects a signal carrying the tag ID, which the reader decodes to generate a read event signal. The read event signal carries the tag ID and may include received signal strength indication information, i.e., the strength value of the signal received by the reader after being emitted by the reader, reflected by the tag, and received by the reader.
[0059] After receiving the read event signal, the positioning processing unit extracts the tag ID and retrieves it from a pre-set tag-mileage mapping table to obtain the absolute track position corresponding to that tag. The tag-mileage mapping table is determined and written into the table by surveyors using high-precision mapping equipment during track construction or maintenance.
[0060] The positioning processing unit assigns the track-side position component in the filter state vector to the retrieved absolute track-side position, resets the diagonal elements corresponding to the track-side position state in the covariance matrix to preset initial values, and retains the lateral position state, heading angle state, and the diagonal elements and cross-correlation elements corresponding to these two states in the covariance matrix unchanged. The diagonal elements of the covariance matrix represent the variance of the estimation error for each state. The result of this operation is that the cumulative error along the track direction is reduced to zero and the uncertainty is reduced to a basic level. Since the lateral and heading directions have been continuously constrained in step S102 and the error variance is already within a small range, they are not forcibly modified during reset to avoid introducing artificial jumps.
[0061] Specifically, there is a discrepancy between the time of occurrence of the read event signal and the time when the antenna is directly facing the tag. As an optional implementation, the positioning processing unit uses the received signal strength indication sequence for time alignment: the reader collects the received signal strength value at a higher frequency within the tag's range, forming a sequence that first rises and then falls as the vehicle moves. The positioning processing unit finds the maximum value point in the sequence, takes the timestamp of that point as the reset time, retrieves the intermediate pose corresponding to that time from the filter buffer, and performs the track-side position state reset using the aligned intermediate pose.
[0062] As an optional implementation, after resetting the track position, the positioning processing unit performs online correction of the zero-bias of the heading angular rate using the known spacing between adjacent tags: It obtains the known mileage difference between the tag read this time and the tag read last time, calculated by subtracting the results of the two queries in the tag-mileage mapping table; simultaneously, it obtains the accumulated track displacement of the stored intermediate pose sequence between the two read events, i.e., the difference in track position between the intermediate poses corresponding to the two reset times; it subtracts the accumulated track displacement from the known mileage difference to obtain the displacement error; based on the displacement error, the time interval between the two reads, and the average track velocity within the interval, it calculates the zero-bias correction amount of the heading angular rate; it then superimposes this correction amount onto the zero-bias state of the heading angular rate in the filter state vector and resets the state covariance to a smaller initial value. The average track velocity is obtained by dividing the track displacement between the two reads by the time interval.
[0063] As an optional implementation, the preset initial value for resetting the track position covariance is not fixed, but dynamically determined based on the mileage distance between the current tag and the previous tag. The positioning processing unit calculates the mileage distance between the two readings and substitutes it into a pre-calibrated functional relationship to obtain the preset initial value for this reset. This functional relationship causes the preset initial value to increase with the increase of the mileage distance, reflecting the physical reality that the residual uncertainty of dead reckoning increases accordingly with long distances. The functional relationship can be a linear mapping, and the slope and intercept are determined by fitting multiple sets of measured data at different distances.
[0064] Furthermore, the positioning processing unit adds a system timestamp to the generated reset pose and writes it to the vehicle positioning data output buffer for reading by the upper-level control or data recording unit. Simultaneously, it overwrites the current state variables stored in the dead reckoning integrator with the state variable values corresponding to the reset pose. The state variables include position, velocity, and attitude quaternions.
[0065] Once the vehicle passes the current tag and enters the next tagless zone, the positioning processing unit re-enters the continuous correction mode in step S102. Before receiving the next read event signal, dead reckoning uses the overwritten state variables as initial values, compensates for the angular rate output by the inertial measurement unit using the zero bias of the heading angular rate updated in step S103, and then performs recursion; lateral ranging continuously provides lateral and heading observation constraints. Each step runs cyclically, forming a correction mechanism of continuous lateral and heading constraints, discrete longitudinal zero-point reset, online zero bias correction, and closed-loop feedback.
[0066] This application achieves full-range attitude estimation of rail vehicles in long-distance, tagless intervals without satellite positioning by decoupling and coordinating continuous constraints of lateral ranging with discrete resets using radio frequency identification (RFID). Lateral ranging continuously corrects the lateral position and heading angle, keeping the error within a small range within the tagless interval. When an RFID tag is triggered, only the track position is reset while the lateral and heading states remain unchanged, avoiding attitude jumps introduced by full-state resets. The known mileage difference between adjacent tags is used as an external scale reference to correct the zero bias of the heading angular rate online and reduce the heading drift rate interval by interval. After reset, the track position covariance is dynamically adjusted according to the tag spacing, so that the filter's confidence level for each reset matches the interval length, suppressing full-degree-of-freedom drift in dead reckoning and improving the accuracy of attitude estimation.
[0067] Furthermore, dead reckoning is performed on the angular rate and acceleration from the inertial measurement unit, including:
[0068] S201. Convert the differential angular rate signal and differential acceleration signal output by the inertial measurement unit into single-ended angular rate signal and single-ended acceleration signal, respectively.
[0069] S202. Based on the pre-calibrated scaling factor and zero bias, the single-ended angular rate signal and single-ended acceleration signal are compensated to obtain the compensated angular rate and acceleration.
[0070] S203. Perform dead reckoning on the compensated angular rate and acceleration.
[0071] In this embodiment, the angular rate and acceleration output by the inertial measurement unit are provided to the positioning processing unit in the form of differential signal pairs. Each signal contains voltages on two physical cables that are out of phase. The analog front end of the positioning processing unit is equipped with a differential amplifier. The non-inverting and inverting inputs of the differential amplifier are respectively connected to the two paths of the differential signal pair. Internally, the non-inverting voltage is subtracted from the inverting voltage, and a single-ended voltage relative to the reference ground is output. Common-mode noise is canceled during the subtraction process, and the differential signal is retained and amplified. This single-ended voltage is sent to an analog-to-digital converter and converted into discrete digital sampled values to obtain single-ended angular rate and single-ended acceleration signals. The common-mode component in the noise of the output single-ended signal has been suppressed at the front end, reducing the noise floor for subsequent compensation and integration operations.
[0072] Furthermore, the non-volatile memory of the positioning processing unit stores a factory-calibrated parameter table, which includes the zero bias and scaling factor for each axis of the gyroscope, as well as the zero bias and scaling factor for each axis of the accelerometer. The calibration process for these parameters is as follows: the inertial measurement unit is installed on a temperature-controlled turntable, which operates according to a set angular rate sequence and temperature sequence, synchronously acquiring the output of each axis of the inertial measurement unit and the true value of the turntable; using the true value of the turntable as a reference, a least-squares linear fit is performed on the acquired data, with the slope of the fitted line representing the scaling factor parameter and the intercept representing the zero bias parameter. The calibrated parameters are organized into a lookup table according to temperature ranges.
[0073] In each processing cycle, the positioning processing unit reads the corresponding zero-bias and scaling factor parameters at the current temperature. For each sampled value of the single-ended angular rate signal, the gyro zero-bias of the corresponding axis is subtracted first, and then the result is multiplied by the gyro scaling factor of the corresponding axis to obtain the compensated angular rate. For each sampled value of the single-ended acceleration signal, the accelerometer zero-bias of the corresponding axis is subtracted first, and then the result is multiplied by the accelerometer scaling factor of the corresponding axis to obtain the compensated acceleration. The compensation operation maps the sensor output from digital quantities containing individual biases to angular rate and acceleration in the physical coordinate system. After compensation, the zero-bias DC component of the angular rate and acceleration is eliminated and the scaling factor is corrected, reducing the tilt of the integral trajectory caused by individual differences between different sensors.
[0074] The positioning processing unit uses attitude quaternions, velocity, and position as state variables. The initial state variables are obtained from the alignment at system startup or given by the corrected pose from the previous cycle. The attitude quaternion is a mathematical object consisting of four elements, representing the rotational relationship between the vehicle coordinate system and the local navigation coordinate system. The navigation coordinate system here is the local horizontal coordinate system, that is, a rectangular coordinate system with the origin located at the vehicle position and the coordinate axes pointing east, north, and sky, respectively.
[0075] In each processing cycle, the positioning processing unit multiplies the compensated angular velocity by the sampling period to obtain the angle increment vector for that cycle. An incremental quaternion is constructed using the magnitude of the angle increment vector as the rotation angle and the direction as the rotation axis. This incremental quaternion is then multiplied by the previous attitude quaternion to obtain the updated attitude quaternion. The heading angle is extracted from the attitude quaternion. The compensated acceleration is taken and rotated from the vehicle coordinate system to the navigation coordinate system using the attitude quaternion. The local gravitational acceleration vector is subtracted from the rotational acceleration in the navigation coordinate system to obtain the specific force vector. This specific force is multiplied by the sampling period to obtain the velocity increment, which is accumulated to the previous velocity. The updated velocity is multiplied by the sampling period to obtain the position increment, which is accumulated to the previous position. The projection component of the position vector along the track direction is the track position, and the projection component perpendicular to the track centerline is the lateral position. The track position, lateral position, and heading angle output in each cycle together constitute the first predicted pose. The first predicted pose can be continuously recursively derived without relying on external signals, maintaining uninterrupted pose output in satellite-free sections such as tunnels or underground, providing a temporally continuous and unobstructed prediction basis for continuous lateral ranging correction and discrete RFID reset.
[0076] Furthermore, based on the lateral distance collected by the lateral ranging sensor and the preset orbital geometry information, observation equations for lateral position error and heading angle error are constructed, including:
[0077] S301. Calculate the difference between the lateral distance and the reference lateral distance determined based on the track geometry information, and use it as the lateral position error;
[0078] S302. Calculate the heading angle error based on the ratio of the time change rate of the lateral distance to the along-track velocity in the first predicted pose.
[0079] S303. Using the lateral position error and heading angle error as observations, construct an observation matrix that reflects the mapping relationship between the lateral position state, heading angle state and the observations.
[0080] In this embodiment, the positioning processing unit's memory stores track geometry information, which is a data table sampled at fixed intervals along the mileage direction. Each sampling point records the reference lateral distance that the lateral ranging sensor should measure when the vehicle is traveling normally along the track centerline at that mileage. The reference lateral distance is determined by three factors: the design plane coordinates of the track centerline, the track gauge design value, and the offset of the lateral ranging sensor's installation position on the vehicle body along the vehicle width direction. The track centerline gives the direction of the track in the plane, the track gauge design value determines the position of the rails on both sides relative to the centerline, and the installation offset determines the lateral distance of the sensor's optical window relative to the longitudinal axis of the vehicle body. The sum of these three factors yields the theoretical distance from the sensor to the track sidewall or tunnel wall.
[0081] Specifically, during the track construction surveying phase, track geometry information is collected along the line at preset intervals using a total station to gather centerline coordinates and track gauge values. This data is then combined with sensor installation dimensions from the vehicle design drawings for offline calculation and stored in the solid-state memory of the positioning processing unit, indexed by mileage. During vehicle operation, the positioning processing unit acquires the current lateral distance collected by the lateral ranging sensor in each processing cycle. Using the current track position output from dead reckoning as a mileage estimate, linear interpolation is performed between two adjacent sampling points in the track geometry information table to obtain the reference lateral distance corresponding to the current mileage estimate. The difference between the currently collected lateral distance and the reference lateral distance is calculated; this difference is the lateral position error, directly reflecting the direction and amount of the vehicle's actual lateral position offset from the expected position of the track centerline. This difference eliminates the interference of track geometry changes on lateral constraints, ensuring that subsequent filters only reflect the vehicle's own motion deviations rather than distance changes caused by track design geometry when observing the lateral position.
[0082] During vehicle operation, when there is a heading angle deviation (i.e., the vehicle's longitudinal axis does not coincide with the tangent direction of the track centerline), the lateral distance measured by the lateral ranging sensor will change with the travel distance. When the heading angle deviation is positive, the vehicle faces the side deviating from the track centerline, and the lateral distance gradually increases or decreases along the travel direction. The geometric relationship between the rate of change and the track speed and heading angle deviation is as follows: the rate of change of the lateral distance with respect to time is approximately equal to the track speed multiplied by the sine of the heading angle deviation; at small angles, the sine is close to the angle itself. Based on this geometric relationship, the positioning processing unit takes the lateral distance sequence of the most recent several periods and performs a differential operation to obtain the time rate of change of the lateral distance. The differential operation uses a central difference format, that is, subtracting the lateral distance of the previous period from the lateral distance of the period following the current period and dividing by twice the sampling period to suppress high-frequency noise.
[0083] The track-side velocity component in the first predicted pose is taken. When the track-side velocity component is greater than a preset velocity threshold, the lateral distance time change rate is divided by the track-side velocity, and the quotient is the observed value of the heading angle error. The preset velocity threshold is set based on the noise level of the lateral ranging sensor and the error propagation characteristics of differential calculation. A typical value is taken near the vehicle's slow crawling speed. Below this speed, the noise proportion in the lateral distance time change rate is too large, and the calculated heading angle error is unreliable. At this time, the heading angle error observation update is paused. The observed value of the heading angle error characterizes the magnitude and direction of the vehicle's current heading angle deviation from the track direction.
[0084] Furthermore, the extended Kalman filter maintains a state vector containing track position, lateral position, heading angle, and optional error states such as gyroscope bias and accelerometer bias. The observation matrix maps the state vector to observations, describing which elements in the state vector contribute to the current observations and the linear relationship between their contributions. The positioning processing unit constructs a two-row, multi-column observation matrix based on the two scalar observations: lateral position error and heading angle error. Rows in the matrix correspond to the two observations, and columns correspond to the elements of the state vector. The lateral position error observation is contributed by the lateral position state, with a one-to-one correspondence. The intersection of the row containing the lateral position error and the column containing the lateral position state in the observation matrix is assigned a value of 1, and the remaining columns in that row are assigned 0. Similarly, the heading angle error observation is contributed by the heading angle state, also with a one-to-one correspondence. The intersection of the row containing the heading angle error and the column containing the heading angle state in the observation matrix is assigned a value of 1, and the remaining columns in that row are assigned 0. If the state vector contains other states, the corresponding column elements are all kept to 0. The observation matrix structure is clear. The independent lateral distance measurement and the derived heading angle measurement are respectively bound to the lateral position state and heading angle state of the filter, thus constructing a corresponding bridge from the measurement space to the state space.
[0085] Preferably, the difference between the current lateral distance and the reference lateral distance eliminates the interference of track design geometric changes on lateral observation, allowing a unified observation model to be used for both curved and straight sections. By using the trend of lateral distance change with travel distance to infer the heading angle deviation, the heading angle information that usually requires dual antennas or multi-point ranging is extracted from single-point lateral ranging, reducing the complexity of sensor configuration. The observation matrix clearly defines a unidirectional binding structure in which lateral position error is used only to correct lateral position and heading angle error is used only to correct heading angle, avoiding cross-contamination caused by measurement anomalies propagating through the filter covariance, and making the state estimation of the filter converge more stably.
[0086] Furthermore, before constructing the observation equation, the method also includes: identifying abrupt changes in the lateral distance; when the rate of change of the lateral distance exceeds a preset rate of change threshold within a sliding window of a preset duration and continues for a preset duration, the lateral distance within the corresponding time period is marked as a turnout area measurement value and deleted.
[0087] In this embodiment, before constructing the observation equation, the lateral distance collected by the lateral ranging sensor is processed for turnout area identification and rejection. The track geometry in the turnout section has discontinuous changes. Structures such as guard rails, switch rails, and frogs cause steep jumps in the lateral distance from the track sidewall to the sensor. These jumps are not caused by the lateral movement of the vehicle itself or changes in the heading angle. If they are used indiscriminately as the observation input for lateral position error or heading angle error, incorrect observation information will be injected into the extended Kalman filter, resulting in incorrect correction of the lateral position state and heading angle state.
[0088] Specifically, the positioning processing unit maintains a fixed-length sliding window in memory. The window length is calculated as the product of time and sampling period, which is then converted into the number of sampling points. In each processing cycle, the lateral distance samples from the lateral ranging sensor are pushed into this window, which holds a sequence of lateral distances from the most recent few cycles. Each cycle, the difference between the maximum and minimum lateral distance values within the window is calculated and divided by the actual time length corresponding to the window to obtain the amplitude change rate, expressed as distance per unit time. The amplitude change rate reflects the overall fluctuation of the lateral distance within the time period covered by the window.
[0089] It should be noted that the preset rate of change threshold and preset duration are determined as follows: Real-vehicle data is collected in known turnout sections and normal sections of the track, recording the lateral distance output of the lateral ranging sensor in different sections. The collected data is analyzed offline to statistically analyze the amplitude change rate distribution caused by normal lateral vehicle movement and sensor noise in the normal section, and the amplitude change rate distribution caused by gauge changes and guardrail introduction in the turnout section. The boundary point between the upper quantile of the amplitude change rate distribution in the normal section and the lower quantile of the amplitude change rate distribution in the turnout section is taken as the rate of change threshold, ensuring that the amplitude change rate in the normal section is below the threshold by a sufficiently high proportion, and the amplitude change rate in the turnout section is above the threshold by a sufficiently high proportion. The preset duration is set with reference to the time required for a vehicle to pass through the shortest turnout at the minimum allowable operating speed of the track, ensuring that brief sensor spike noise will not trigger false identification, while continuous turnout geometric changes are fully covered.
[0090] In each processing cycle, the positioning processing unit compares the amplitude change rate calculated for the current sliding window with a preset change rate threshold. If the amplitude change rate does not exceed the change rate threshold, the lateral distance within the window is sent to subsequent stages S301 to S303 for processing according to the normal procedure. If the amplitude change rate exceeds the change rate threshold, timing accumulation is initiated, continuously comparing the amplitude change rate with the change rate threshold for each subsequent cycle window. When the cumulative time for the amplitude change rate to exceed the change rate threshold reaches a preset duration, it is determined that the vehicle is currently passing through the turnout area, and all lateral distance sampling values covered from the start of the preset duration to the current time are marked as turnout area measurement values. The marked lateral distances are directly deleted from the observation data stream and are not included in the calculation of the reference lateral distance difference in S301 and the lateral distance time change rate in S302. The deletion of turnout area measurement values continues until the amplitude change rate falls back below the change rate threshold, at which point it is determined that the vehicle has left the turnout area, and subsequent lateral distance processing resumes normal operation.
[0091] Preferably, after the abrupt change in lateral distance in the turnout section is identified and eliminated, the extended Kalman filter will not receive false observations caused by track geometry jumps during the turnout passage. The lateral position and heading angle states are maintained in the turnout section by the short-term accuracy of dead reckoning without being erroneously deviated. After leaving the turnout, the lateral distance measurement constraint returns to normal operation, and the filter state continues to converge smoothly. The turnout elimination threshold is calibrated based on the quantile statistical method of real vehicle data, making the identification logic adaptable to different lines and different operating speeds.
[0092] Furthermore, the read event signal includes received signal strength indication information; upon receiving the read event signal, it also includes:
[0093] S501. Generate a signal strength sequence based on the received signal strength indication information;
[0094] S502. Identify the peak point in the signal strength sequence and use the time corresponding to the peak point as the reset time;
[0095] S503. Perform time interpolation on the intermediate pose sequence based on the reset time to obtain the aligned reset intermediate pose;
[0096] S504, The step of resetting the track position state of the intermediate pose to the track position corresponding to the RFID tag being read is performed on the aligned intermediate pose.
[0097] In this embodiment, a signal strength sequence is generated based on the received signal strength indication information. During the process of establishing communication with RFID tags along the track, the RFID reader outputs a data frame containing the tag identifier and the received signal strength indication value after each read / write operation or at fixed query intervals. The received signal strength indication value is the quantized result of the radio frequency power measured by the reader when demodulating the tag's backscattered signal from the receiving link, measured in decibels and milliwatts, characterizing the strength of the tag's reflected signal reaching the reader's antenna port.
[0098] The closer the relative distance between the reader and the tag, the higher the received signal strength indication (RSS) value. The shortest distance occurs when the antenna is directly facing the tag, resulting in a local maximum RSS value. After detecting the initial boundary of the read event signal, the positioning processing unit continuously records the RSS value output by the reader, along with a system timestamp for each value. As the vehicle moves, the RSS value experiences a gradual increase from the background noise level, reaching a peak, then gradually decreasing back to the background noise level. The positioning processing unit stores the chronologically ordered RSS values along with their timestamps as a signal strength sequence. The sequence length depends on the reader's query frequency and the time taken for the vehicle to pass through the tag's range.
[0099] Furthermore, peak points in the signal strength sequence are identified to determine the reset time. The signal strength sequence exhibits a single-peak shape, rising first and then falling, with the peak point corresponding to the instantaneous position where the antenna and tag are directly opposite each other, serving as the ideal time reference for resetting along the track direction. The positioning processing unit performs peak point identification on the signal strength sequence: starting from the second sampling point of the sequence, it sequentially compares the received signal strength indication value of the current sampling point with the received signal strength indication values of the previous and next sampling points. If the indication value of the current sampling point is greater than the indication values of both the previous and next points, then the current sampling point is a peak point. At the sequence boundaries, only available neighboring points are compared.
[0100] When multiple sampling points in the sequence meet the peak point condition, the sampling point with the highest received signal strength indication value is selected as the final peak point to eliminate secondary peak interference caused by multipath reflection or temporary obstruction. The system timestamp corresponding to this peak point is extracted and used as the reset time. The reset time represents the position of the physical event where the vehicle antenna and tag are facing each other on the system timeline. Compared with directly using the trigger time of reading the event signal or the start time of the signal sequence, the correspondence between the reset time and the tag's actual mileage position is more accurate.
[0101] Specifically, the intermediate pose sequence is interpolated based on the reset time to obtain aligned reset intermediate poses. The extended Kalman filter outputs an intermediate pose with a system timestamp in each filtering cycle. These intermediate poses are arranged chronologically to form an intermediate pose sequence, which is stored in the memory buffer of the localization processing unit. The reset time and the system timestamps of each sampling point in the intermediate pose sequence typically do not coincide; that is, there is a time deviation between the time corresponding to the intermediate pose output by the filter and the physical time when the antenna is directly facing the tag.
[0102] The localization processing unit searches the intermediate pose sequence for the nearest intermediate pose before the reset time and the nearest intermediate pose after the reset time, using these two intermediate poses as interpolation reference points. Linear interpolation is performed between these two reference points, with the interpolation weight determined by the ratio of the time difference between the reset time and the timestamps of the two reference points: the time difference between the previous reference point's timestamp and the reset time is divided by the time interval between the two reference points to obtain the interpolation weight coefficient. The aligned reset intermediate pose is equal to the previous reference point's intermediate pose plus this weight coefficient multiplied by the difference between the subsequent reference point's intermediate pose and the previous reference point's intermediate pose. If the reset time happens to coincide with the timestamp of a certain intermediate pose, that intermediate pose is directly taken as the aligned reset intermediate pose. After interpolation, a filter state estimate that is precisely aligned in time with the antenna-to-tag event is obtained, and subsequent zero-point resets will be performed based on this aligned state.
[0103] The track-side position state reset is performed on the aligned intermediate reset pose. After obtaining the aligned intermediate reset pose, the positioning processing unit extracts the tag identifier from the read event signal and queries the tag-odot mapping table to obtain the absolute track-side position corresponding to the tag. The positioning processing unit assigns the track-side position component in the state vector of the aligned intermediate reset pose to this absolute track-side position, resets the diagonal elements corresponding to the track-side position in the covariance matrix to preset initial values, and keeps the lateral position state, heading angle state, and their covariance unchanged. Since the aligned intermediate reset pose corresponds precisely in time to the instant when the antenna is directly facing the tag, the absolute track-side position applied by the reset operation matches the actual spatial position of the tag, and the reset deviation in the track-side direction is controlled.
[0104] Preferably, the reset time is precisely located from the coarse trigger boundary of the read event signal to the instant when the antenna is directly facing the tag by utilizing the existing received signal strength indication information of the reader, and the filter state is aligned to this instant by time interpolation, so that the accuracy of discrete RFID zero-point reset in the time dimension is no longer affected by the size of the tag's effective range and the change of vehicle speed, and the accuracy of track-side reset is maintained.
[0105] Furthermore, upon receiving a read event signal, the track position state of the intermediate pose is reset to the track position corresponding to the read RFID tag, the track position covariance is reset to a preset initial value, and the lateral position state, heading angle state, and their covariance of the intermediate pose remain unchanged, thus obtaining the reset pose. This process also includes:
[0106] S601. Obtain the mileage difference between the RFID tag read this time and the RFID tag read last time;
[0107] S602. Obtain the cumulative displacement along the track between two readings of the intermediate pose sequence;
[0108] S603. Calculate the correction amount for zero deviation of heading angular rate based on the difference between the mileage difference and the displacement along the track, as well as the time interval between the two readings.
[0109] S604. The correction amount is superimposed on the zero-bias state of the heading angular rate of the nonlinear filter, and the covariance of the zero-bias state of the heading angular rate is reset.
[0110] In this embodiment, the mileage difference between the currently read RFID tag and the previously read RFID tag is obtained. After each successful RFID tag read and zero-point reset, the positioning processing unit records the tag identifier and corresponding absolute track position in a tag history table in memory. The tag history table retains at least the tag information from the two most recent reads. When the zero-point reset corresponding to the current read event is completed, the positioning processing unit retrieves the absolute track position corresponding to the previously read tag from the tag history table, subtracts the absolute track position of the previous tag from the current tag's absolute track position, and the resulting difference is the known mileage difference between the two tags. This mileage difference is measured on-site using high-precision surveying equipment during track construction or maintenance and written into the tag-mileage mapping table. It does not depend on the vehicle's own positioning results and has an independent external reference attribute. The mileage difference provides the true distance between two spatially known reference points for subsequent zero-bias estimation, constituting a spatial scale reference for online calibration.
[0111] Furthermore, the accumulated track displacement of the intermediate pose sequence between the two read events is obtained. In the zero-point reset operation of S103, the positioning processing unit has retained the intermediate pose sequence output by the filter before reset in memory before completing the track position reset. Each sampling point of the intermediate pose sequence contains the track position component and the corresponding timestamp.
[0112] The positioning processing unit extracts the track position corresponding to the reset time of the previous reading event and the track position corresponding to the reset time of the current reading event from the intermediate pose sequence. It subtracts the track position of the former from the latter to obtain the cumulative track displacement between the two reading events, resulting from the combined effects of dead reckoning and continuous lateral ranging correction. The cumulative track displacement reflects the estimated track distance actually traveled by the vehicle between two adjacent tags. Since the continuous correction process in step S102 has continuously constrained the lateral position error and heading angle error, the lateral coupling error in this cumulative track displacement has been suppressed. The remaining track displacement error mainly originates from the projection accumulation of the uncorrected inertial measurement unit system error—the zero bias of the heading angle rate—in the track direction.
[0113] Specifically, the correction for zero deviation of the heading angular rate is calculated based on the difference between the mileage difference and the displacement along the track, as well as the time interval between two readings. The positioning processing unit subtracts the accumulated displacement along the track from the known mileage difference to obtain the displacement error. The sign of this error indicates whether the dead reckoning displacement along the track is too large or too small relative to the actual mileage.
[0114] Based on kinematic analysis, a constant heading angular rate zero bias causes the vehicle's heading angle to deflect at a constant rate, resulting in a quadratic increase in lateral position over time. This lateral offset, projected onto the track direction, manifests as a track displacement error. The relationship between this error and the heading angular rate zero bias is: the displacement error is proportional to the heading angular rate zero bias, the mean of the track velocity, and the square of the time interval. The positioning processing unit takes the time interval between two read events, i.e., the difference between the reset time of the current read and the reset time of the previous read. The arithmetic mean of the track velocities at each sampling point in the intermediate pose sequence between the two read events is taken as the average track velocity, extracted from the velocity state component of the filter. The displacement error is multiplied by a constant coefficient and then divided by the product of the average track velocity and the square of the time interval to calculate the correction amount for the heading angular rate zero bias, expressed in angular rate units. The constant coefficient is determined by kinematic relationships and is set to 2. This inverse calculation operation is performed once at each tag, and a single calculation provides an estimate of the current heading angular rate zero bias deviation.
[0115] Furthermore, the correction is superimposed on the zero-bias state of the extended Kalman filter's heading rate, and the covariance of the zero-bias state of the heading rate is reset. The state vector of the extended Kalman filter maintains a state element specifically for estimating the zero bias of the gyro heading axis. This state element is set to zero or the residual value of the last calibration during filter initialization and is gradually estimated by the filter based on observations during normal operation.
[0116] The calculated zero-bias correction for the heading angular rate is directly added to the value of the current zero-bias state of the heading angular rate in the filter's state vector, and the result is written back to the corresponding position in the state vector. After state superposition, the diagonal elements corresponding to the zero-bias state of the heading angular rate in the covariance matrix are reset to a preset, small initial value. This initial value represents a conservative assessment of the residual uncertainty of the corrected zero-bias estimate. At the same time, the cross-correlation elements of the covariance matrix between this state and other states are set to zero. State superposition allows the filter's estimate of the zero-bias of the heading angular rate to immediately absorb the information accumulated from the current label interval, and covariance reset allows this state to re-participate in subsequent covariance propagation and updates from a lower confidence level.
[0117] Preferably, the known spatial distance between two adjacent RFID tags is used as an external scale benchmark to analyze the accumulated track displacement error within the dead reckoning interval, extracting the correction amount for the key error source, the zero bias of the heading angular rate. This correction mechanism does not require additional sensor hardware or change the vehicle operation procedures, relying entirely on the existing tag mileage information and intermediate pose sequence data from the zero-point reset operation. A zero bias observation and correction is obtained after each tag interval is traversed. As the vehicle's mileage increases, the estimated value of the zero bias of the heading angular rate gradually approaches the true value, and the heading drift rate in subsequent tagless intervals gradually decreases, improving the accuracy maintenance time of dead reckoning under long-distance conditions without absolute correction.
[0118] Furthermore, the covariance of the position along the track is reset to a preset initial value, including:
[0119] S701. Determine the mileage interval between the RFID tag read this time and the RFID tag read last time;
[0120] S702. Determine the preset initial value according to the mileage interval and the pre-calibrated functional relationship.
[0121] In this embodiment, the mileage distance between the RFID tag read this time and the RFID tag read last time is determined. After each zero-point reset operation, the tag identifier and the corresponding absolute track position of the current tag are stored in the tag history buffer. This buffer is maintained in a first-in-first-out manner, retaining at least the tag information of the two most recent successful reads. When a new round of reading events triggers a zero-point reset, the positioning processing unit retrieves the absolute track position corresponding to the tag read last time from the buffer, subtracts the absolute track position of the previous tag from the current tag's absolute track position, and takes the absolute value to obtain the mileage distance between the two tags.
[0122] Specifically, the mileage distance is the actual path length between two tags along the track centerline in physical space. This distance is measured point-by-point along the track surface using a total station or high-precision differential satellite positioning equipment during track construction and written into a tag-mileage mapping table, without relying on onboard sensors for online measurement. When the vehicle first powers on or reads a tag after a long period of inactivity, the buffer only contains current tag information and no previous tag information. At this time, the mileage distance cannot be calculated. The positioning processing unit assigns a predetermined default mileage distance value, which is taken from the typical spacing of tags along the line, such as the average tag spacing between two adjacent stations.
[0123] Furthermore, a preset initial value is determined based on the mileage interval according to a pre-calibrated functional relationship. The preset initial value of the track position covariance reflects the residual uncertainty of the track position estimate after zero-point reset. In the extended Kalman filter, the size of the diagonal elements of the covariance matrix determines the filter's confidence in the state estimate: a smaller covariance value indicates a more confident estimate of the state, and a weaker correction effect from subsequent observations; a larger covariance value indicates a less confident estimate of the state, and a stronger correction effect from subsequent observations. The larger the mileage interval between two labels, the longer the uncorrected dead reckoning time experienced by the vehicle within the interval, the greater the divergence of the track position error, and the greater the unmodeled error remaining in the track position after reset. The preset initial value should be increased as the mileage interval increases, so that the filter maintains a moderate degree of uncertainty in the track position after long-interval reset, preserving the ability for further correction of the track position by subsequent lateral ranging or other information.
[0124] It should be noted that the calibration process for the functional relationship includes: selecting several sets of tag pairs on the test or actual operating route, with the mileage interval of each tag pair covering the range from the minimum to the maximum deployment interval. The vehicle repeatedly passes each set of tag pairs at normal operating speed, and the covariance value of the track position along the track at the instant before reset is recorded each time the extended Kalman filter passes a tag, serving as a reference for the cumulative uncertainty of dead reckoning at that mileage interval. Scattered data is generated by plotting the mileage interval as the x-axis and the track position covariance before reset as the y-axis. Least-squares linear fitting is performed on the scattered data, with the fitting objective being to minimize the sum of squares of the differences between the fitted line and the y-axis of each scatter point; the loss function is the sum of squared residuals. The slope and intercept of the fitted line are stored as parameters of the functional relationship in the configuration file of the positioning processing unit. During runtime, the mileage interval is substituted into the linear function to calculate and output the corresponding preset initial value. When the mileage interval is the default mileage interval value, the function outputs the default preset initial value. The slope of the linear function is positive, ensuring that the preset initial value increases monotonically with the increase of the mileage interval.
[0125] Preferably, by using the monotonically increasing function relationship between the mileage interval and the preset initial value, the degree of confidence of the filter in the track position after each zero-point reset is matched with the actual length of the tag interval. After a long interval reset, a reasonable correction space is retained, and after a short interval reset, the high precision of the tag absolute reference is fully utilized, thus improving the filter consistency of each reset.
[0126] Furthermore, outputting the reset pose includes: synchronizing the reset pose with the system timestamp, writing it into the vehicle positioning data output buffer, and triggering the state variables stored in the dead reckoning integrator to be overwritten by the reset pose.
[0127] In this embodiment, outputting the reset pose involves two coordinated actions: publishing the reset pose to the upper-level system and internally closing the loop of the reset pose to the integration starting point of dead reckoning. The positioning processing unit internally maintains a vehicle positioning data output buffer, which is a contiguous shared memory area. The upper-level control system or data recording unit periodically reads the latest vehicle pose information from this buffer via a bus interface. After completing the zero-point reset and generating the reset pose, the time value in the current system clock register is read as the system timestamp. The resolution of the system timestamp is related to the processor frequency and clock source accuracy of the positioning processing unit, using a microsecond-level count generated by a high-precision timer inside the processor.
[0128] Specifically, the fields of the pose reset structure are combined with the system timestamp to form a positioning data frame. This frame sequentially includes the timestamp, track position, lateral position, vertical position, heading angle, pitch angle, and roll angle. The vertical position, pitch angle, and roll angle are calculated using dead reckoning and synchronously output by the filter. This positioning data frame is written to the current write position of the vehicle positioning data output buffer via memory copying or pointer switching, and the write pointer is updated to complete the external publication. After the upper-level system reads this frame from the buffer, it obtains a positioning result with a clear timestamp and corrected poses for all degrees of freedom.
[0129] Furthermore, the dead reckoning integrator continuously operates within the positioning processing unit as a set of state variables, which includes position vectors, velocity vectors, and attitude quaternions. The position vectors store the vehicle's three-axis coordinates in the navigation coordinate system, the velocity vectors store the corresponding three-axis velocity components, and the attitude quaternions store the rotation transformation from the vehicle coordinate system to the navigation coordinate system. Each dead reckoning cycle uses the current state variables as initial values, adds the motion increments measured by the inertial measurement unit within the current cycle, and recursively derives the state variables for the next cycle.
[0130] After generating the reset pose, if only the reset pose is output without overwriting the integrator's internal state variables, the integrator will still recursively calculate from the pre-reset predicted state in the next cycle, leading to a state divergence between the output and the calculation. This causes the positioning result to instantly jump back to the pre-reset position after the reset. Simultaneously with writing the reset pose to the output buffer, the orbital position component corresponding to the position vector stored in the integrator is overwritten with the orbital position value of the reset pose, the lateral position component is overwritten with the lateral position value of the reset pose, and the heading angle component in the velocity vector and attitude quaternion is overwritten with the latest value output by the filter. The overwriting operation is achieved by directly modifying the values of the corresponding fields in the state variable structure in the integrator's memory, completed after writing to the output buffer and before the start of the next dead reckoning cycle. After overwriting, the integrator's internal state remains consistent with the filter state, and the mapping relationship between the filter state and the integrator state is re-established.
[0131] Preferably, resetting the pose and writing it into the output buffer simultaneously serves as the integration starting point for the next round of dead reckoning, ensuring strict numerical consistency between the output pose and the pose used as the recursion starting point internally. There are no intermediate state differences between the reset pose obtained by the upper-level system and the internal dead reckoning state, avoiding inconsistencies in positioning information caused by interface asynchrony. Overwriting the integrator state variables directly modifies the memory fields, introducing no additional latency and avoiding any intermediate steps outside the sensor data processing chain, thus improving the real-time performance of the closed-loop response.
[0132] Furthermore, in the tagless area after passing the RFID tag, the reset pose is used as the initial pose for the next dead reckoning, including: before receiving the next read event signal, the dead reckoning process in the nonlinear filter is integrated with the reset pose as the starting point, and the angular rate output by the inertial measurement unit is compensated by the updated heading angular rate zero bias before the dead reckoning is performed.
[0133] In this embodiment, after the vehicle passes the current RFID tag and enters the tagless area, during the entire period before receiving the next read event signal, the dead reckoning process of the extended Kalman filter continues to recursively advance from the reset pose as the integration starting point. At the same time, the angular rate output by the inertial measurement unit is compensated by the zero bias of the updated heading angular rate before entering the attitude update.
[0134] Specifically, after zero-point reset, the position, velocity, and attitude-related components in the extended Kalman filter's state vector are updated. The track-side position component in the position vector is reset to the absolute track-side position corresponding to the tag, while the lateral position component retains the value after continuous correction for lateral ranging. The velocity vector and attitude quaternions are synchronously output by the filter and remain consistent with the reset position state. This state vector serves as the initial value for dead reckoning until the next readout event signal arrives. When new inertial measurement unit (IMU) sampled data arrives, the positioning processing unit extracts the position, velocity, and attitude from this state vector as the integration starting point for the current cycle, no longer relying on the pre-reset prediction values.
[0135] One difference from the pre-reset operation phase is that the angular rate input consumed in dead reckoning during this phase has been compensated for with online correction of the zero bias of the heading angular rate. The zero bias state of the heading angular rate maintained in the extended Kalman filter state vector has been superimposed with the correction amount and the covariance reset in step S604. The corrected zero bias estimate reflects the optimal estimate of the gyro heading axis system deviation obtained by combining the odometer difference information of the most recent tag interval.
[0136] Within each dead reckoning cycle, the positioning processing unit acquires differential or single-ended angular rate signals from the inertial measurement unit (IMU). After converting these signals into compensated angular rates, before sending them to the attitude update module, it first reads the currently stored zero-bias value of the heading angular rate in the filter state vector. This zero-bias value is then subtracted from the heading axis component of the angular rate vector to complete the zero-bias compensation of the angular rate. The compensated angular rates are used to construct incremental quaternions and update the attitude quaternions. Angular rate compensation and dead reckoning are executed cyclically in each sampling cycle, generating a continuous first predicted pose. The lateral distance, synchronously acquired by the lateral ranging sensor, continuously corrects the lateral position and heading angle of the first predicted pose.
[0137] Within the tagless zone, the dead reckoning along-track position error accumulates slowly with travel distance. However, after the zero bias of the heading angular rate is corrected, the heading drift rate decreases, and the lateral position remains near the track centerline due to the continuous constraint of lateral ranging. The main sources of along-track position error are limited to the accelerometer zero bias along-track component and the residual error of the scaling factor. The error coupling from lateral to along-track caused by heading drift is suppressed. Resetting the pose as the integration start point ensures that the initial conditions are aligned with the absolute position of the tag. The corrected zero bias of the heading angular rate weakens the angular rate error source in real time during integration. The synergy of these two factors slows down the error divergence rate of the pose output in the tagless zone compared to before the reset. When the vehicle reaches the next tag and triggers a new read event signal, a new round of zero-point reset, zero bias correction, and integration start point update is executed again, and the steps are repeated to form a closed loop.
[0138] like Figure 2As shown, a dead reckoning correction system based on radio frequency identification (RFID) zero-point reset is used to implement a dead reckoning correction method based on RFID zero-point reset, including:
[0139] The dead reckoning module performs dead reckoning on the angular rate and acceleration from the inertial measurement unit to generate the vehicle's first predicted pose, which includes the position along the track, the lateral position, and the heading angle.
[0140] The pose correction module, in the interval where no RFID reader has received a reading event signal for RFID tags along the track, constructs an observation equation for lateral position error and heading angle error based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information, and continuously corrects the first predicted pose to obtain the intermediate pose.
[0141] When the pose reset module receives a read event signal, it resets the track position state of the intermediate pose to the track position corresponding to the read RFID tag, resets the track position covariance to a preset initial value, and keeps the lateral position state, heading angle state and covariance of the intermediate pose unchanged to obtain the reset pose.
[0142] The pose feedback module outputs a reset pose and uses the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag.
[0143] 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 dead reckoning correction method based on radio frequency identification zero-point reset, characterized in that, include: The angular rate and acceleration from the inertial measurement unit are used to perform dead reckoning to generate the first predicted pose of the vehicle, which includes the position along the track, the lateral position, and the heading angle. In the interval where no RFID reader receives a signal indicating that the RFID tag along the track has been read, an observation equation for the lateral position error and heading angle error is constructed based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information. The first predicted pose is then continuously corrected to obtain the intermediate pose. When a read event signal is received, the track position state of the intermediate pose is reset to the track position corresponding to the read RFID tag, the track position covariance is reset to the preset initial value, and the lateral position state, heading angle state and covariance of the intermediate pose are kept unchanged to obtain the reset pose. Output the reset pose, and use the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag.
2. The dead reckoning correction method based on RFID zero-point reset according to claim 1, characterized in that, The dead reckoning of angular rates and accelerations from the inertial measurement unit includes: The differential angular rate signal and differential acceleration signal output by the inertial measurement unit are converted into single-ended angular rate signal and single-ended acceleration signal, respectively. The single-ended angular rate signal and single-ended acceleration signal are compensated based on the pre-calibrated scaling factor and zero bias to obtain the compensated angular rate and acceleration. Dead reckoning is performed on the compensated angular rate and acceleration.
3. The dead reckoning correction method based on radio frequency identification zero-point reset according to claim 1, characterized in that, The observation equations for lateral position error and heading angle error are constructed based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information, including: The difference between the lateral distance and the reference lateral distance determined based on the track geometry information is calculated as the lateral position error; The heading angle error is calculated based on the ratio of the time rate of change of the lateral distance to the along-track velocity in the first predicted pose. Using the lateral position error and heading angle error as observations, an observation matrix is constructed that reflects the mapping relationship between the lateral position state, heading angle state and the observations.
4. The dead reckoning correction method based on RFID zero-point reset according to claim 3, characterized in that, Before constructing the observation equation, the method also includes: identifying abrupt changes in the lateral distance. When the rate of change of the lateral distance exceeds a preset rate of change threshold within a sliding window of a preset duration and continues for a preset duration, the lateral distance within the corresponding time period is marked as a turnout area measurement value and deleted.
5. The dead reckoning correction method based on radio frequency identification zero-point reset according to claim 1, characterized in that, The read event signal includes received signal strength indication information; upon receiving the read event signal, it also includes: Generate a signal strength sequence based on the received signal strength indication information; Identify the peak point in the signal intensity sequence and use the time corresponding to the peak point as the reset time; Based on the reset time, perform time interpolation on the intermediate pose sequence to obtain the aligned reset intermediate pose; The step of resetting the track position state of the intermediate pose to the track position corresponding to the RFID tag being read is performed on the aligned intermediate pose.
6. The dead reckoning correction method based on radio frequency identification zero-point reset according to claim 5, characterized in that, When a read event signal is received, the intermediate pose's track position state is reset to the track position corresponding to the read RFID tag, the track position covariance is reset to a preset initial value, and the lateral position state, heading angle state, and covariance of the intermediate pose remain unchanged to obtain the reset pose. The process then further includes: Obtain the mileage difference between the RFID tag read this time and the RFID tag read last time; Obtain the cumulative displacement along the track between two readings of the intermediate pose sequence; The correction amount for zero deviation of heading angular rate is calculated based on the difference between the mileage difference and the displacement along the track, as well as the time interval between the two readings. The correction is superimposed on the zero-bias state of the heading angular rate of the nonlinear filter, and the covariance of the zero-bias state of the heading angular rate is reset.
7. The dead reckoning correction method based on radio frequency identification zero-point reset according to claim 6, characterized in that, The step of resetting the position covariance along the track to a preset initial value includes: Determine the mileage interval between the RFID tag read this time and the RFID tag read last time; Based on the mileage interval, a preset initial value is determined according to a pre-defined functional relationship.
8. The dead reckoning correction method based on radio frequency identification zero-point reset according to claim 1, characterized in that, The output reset pose includes: synchronizing the reset pose with the system timestamp, writing it into the vehicle positioning data output buffer, and triggering the state variables stored in the dead reckoning integrator to be overwritten by the reset pose.
9. The dead reckoning correction method based on radio frequency identification zero-point reset according to claim 8, characterized in that, The step of using the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag includes: before receiving the next read event signal, using the reset pose as the integration starting point for the dead reckoning process in the nonlinear filter, and compensating the angular rate output by the inertial measurement unit with the updated zero bias of the heading angular rate before performing the dead reckoning.
10. A dead reckoning correction system based on radio frequency identification (RFID) zero-point reset, used to implement the dead reckoning correction method based on RFID zero-point reset as described in any one of claims 1 to 9, characterized in that, include: The dead reckoning module performs dead reckoning on the angular rate and acceleration from the inertial measurement unit to generate the vehicle's first predicted pose, which includes the position along the track, the lateral position, and the heading angle. The pose correction module, in the interval where no RFID reader has received a reading event signal for RFID tags along the track, constructs an observation equation for lateral position error and heading angle error based on the lateral distance collected by the lateral ranging sensor and the preset track geometry information, and continuously corrects the first predicted pose to obtain the intermediate pose. When the pose reset module receives a read event signal, it resets the track position state of the intermediate pose to the track position corresponding to the read RFID tag, resets the track position covariance to a preset initial value, and keeps the lateral position state, heading angle state and covariance of the intermediate pose unchanged to obtain the reset pose. The pose feedback module outputs a reset pose and uses the reset pose as the initial pose for the next dead reckoning within the tagless area after passing the RFID tag.
Citation Information
Patent Citations
Novel navigation method and device, electronic equipment and storage medium
CN118794428A
High-precision positioning data compensation method independent of vehicle interface
CN122015910A
