A spatiotemporal registration method, apparatus, system, and storage medium

Abstract

This invention provides a spatiotemporal registration method, apparatus, system, and storage medium, belonging to the field of navigation and positioning. The method includes: calculating a priori error state estimate by estimating the error data and sampling time; calculating the priori error state covariance by setting the error value and the priori error state estimate; constructing a measurement using sampling time, Bluetooth sensor coordinates, Bluetooth sensor velocity, inertial navigation system velocity, inertial navigation system coordinates, and accelerometer measurements; and calculating a posterior error state estimate by estimating the priori error state estimate and the priori error state covariance based on the measurement. This invention simplifies the prediction and correction stages of the extended Kalman filter, greatly reduces the impact of IMU zero bias on positioning performance, achieves real-time spatiotemporal registration, and meets the requirements of good real-time performance, strong practicality, and ease of implementation, thus satisfying current needs.

Description

Technical Field

[0001] This invention relates primarily to the field of navigation and positioning technology, specifically to a spatiotemporal registration method, apparatus, system, and storage medium. Background Technology

[0002] Currently, indoor positioning technologies and systems researched both domestically and internationally are mainly divided into three categories: device-specific positioning methods, WiFi-based positioning methods, and motion sensor-based positioning methods. Representative positioning technologies include infrared, ultrasonic, Bluetooth (BLE), Wireless Local Area Networks (WLAN), and Ultra-Wideband (UWB), some of which are already commercially available. However, due to the complexity and diversity of indoor environments, different indoor positioning technologies also have different drawbacks and limitations. Relying on a single positioning method is unlikely to achieve highly reliable and accurate positioning. Combining multiple navigation technologies can achieve high-precision indoor navigation and positioning.

[0003] Bluetooth devices have attracted widespread attention from researchers due to their high accuracy, low power consumption, and small size. The main reasons for using Bluetooth technology for indoor positioning are: 1) short-range wireless communication meets general indoor application scenarios; 2) low cost and low power consumption facilitate the construction of a low-cost positioning sensor network; 3) a large number of devices with Bluetooth modules, such as mobile phones, PDAs, and other portable devices, are emerging, meeting the application needs of ubiquitous computing environments. However, Bluetooth signals are easily affected by the environment and deployment location, leading to larger positioning errors in certain areas and significant bounce phenomena in real-time navigation. INS, as an autonomous navigation system that does not rely on external information or radiate energy, offers high short-term accuracy, compensating for the accuracy degradation caused by Bluetooth signal weakening or disappearance due to environmental interference. Simultaneously, utilizing Bluetooth's high-precision location information can suppress the INS navigation error accumulated over time, forming a complementary performance and achieving seamless and continuous indoor positioning.

[0004] However, when different types of navigation systems fuse information, the track data received by the combined navigation system is often asynchronous due to differences in sensor startup time, transmission delay in scanning cycles, and sampling frequencies. This asynchrony in measurement information between systems can lead to incorrect system state estimation during filter correction, thus reducing the positioning performance of the navigation system. Secondly, the impact of time synchronization errors is related to the dynamic range of the user equipment; the higher the dynamic range, the greater the impact of time synchronization errors. Currently, commonly used time registration methods include Taylor expansion correction, least squares method, interpolation and extrapolation, and the maximum entropy criterion method. All of these methods rely on preserving the data output of the high-sampling-rate system. When another low-sampling-rate system provides measurement output, interpolation is performed from the previously preserved data to find data at the same valid moment for information fusion. However, this significantly increases hardware costs and does not meet market demands. Therefore, there is an urgent need for a method that is real-time, practical, and easy to implement to compensate for lever arm effects and time synchronization errors, achieving high-precision indoor positioning and navigation.

[0005] Timing asynchrony error refers to the discrepancy between the measurement data obtained by different sensors observing the same target in a real-world multi-sensor system due to various factors such as different tasks performed, varying performance characteristics, and different environments. Existing lightweight indoor integrated navigation solutions suffer from this timing asynchrony, making it difficult to solve the real-time time registration problem in complex situations and thus hindering the improvement of positioning accuracy.

[0006] like Figure 2 As shown, the "lever arm effect" was initially discovered in the study of inertial navigation systems (INS). It refers to the fact that, theoretically, the inertial measurement unit (IMU) should be installed at the center of mass of the carrier, and its outer, inner, and azimuth axes should align with the longitudinal, transverse, and normal axes of the carrier system in their normal positions. In reality, the IMU's mounting base deviates from the carrier's center of mass by a certain distance. Due to the presence of tangential and centripetal accelerations, this causes measurement errors in the accelerometer. The accelerometer measurement errors caused by the IMU's mounting base deviating from the center of mass are commonly referred to as the installation deviation lever arm effect or external lever arm effect. Accelerometer measurement errors caused by the non-zero dimensions of the inertial measurement unit itself are called the internal lever arm effect or internal lever arm effect. Generally, the internal lever arm effect is considered when studying internal problems of the INS system, while only the external lever arm effect is considered in integrated navigation systems.

[0007] Currently, researchers mainly estimate and compensate for external lever errors by incorporating accelerometer errors and gyroscope drift within the inertial navigation system (INS) into the state variables and then using Kalman filtering to model these errors. However, under dynamic conditions, the heading angle output by the INS is a large misalignment angle with strong nonlinearity, which Kalman filtering cannot adequately address. Furthermore, current methods that simultaneously consider time asynchrony errors and lever errors are highly complex, costly in hardware, and difficult to implement. Summary of the Invention

[0008] The technical problem to be solved by the present invention is to provide a spatiotemporal registration method, apparatus, system and storage medium to address the shortcomings of the prior art.

[0009] The technical solution of the present invention to solve the above-mentioned technical problems is as follows: A spatiotemporal registration method, comprising the following steps:

[0010] S1: Obtain the current time, import the error data and sampling time, calculate the estimate using the error data and the sampling time, and obtain the prior error state estimate for the next time.

[0011] S2: Import the set error value corresponding to the prior error state estimate of the next time step, and calculate the covariance using the set error value and the prior error state estimate of the next time step to obtain the prior error state covariance of the next time step.

[0012] S3: Obtain the current Bluetooth sensor coordinates and current Bluetooth sensor velocity from the preset Bluetooth sensor; obtain the current inertial navigation velocity, current inertial navigation coordinates, and current accelerometer measurement value from the preset inertial navigation system; and construct the measurement value for the next moment using the sampling time, the current Bluetooth sensor coordinates, the current Bluetooth sensor velocity, the current inertial navigation velocity, the current inertial navigation coordinates, and the current accelerometer measurement value.

[0013] S4: Calculate the estimated value of the prior error state at the next time step and the estimated value of the prior error state covariance at the next time step based on the measurement at the next time step, and obtain the estimated value of the posterior error state at the next time step.

[0014] S5: Analyze the covariance of the set error value and the posterior error state estimate at the next time step, and use the posterior error state estimate at the next time step as the spatiotemporal registration result based on the analysis results.

[0015] Another technical solution of the present invention to solve the above-mentioned technical problems is as follows: A spatiotemporal registration device, comprising:

[0016] The first calculation module is used to obtain the current time, import error data and sampling time, calculate the estimated value using the error data and sampling time, and obtain the prior error state estimate for the next time.

[0017] The second calculation module is used to import the set error value corresponding to the prior error state estimate value at the next time step, and to calculate the covariance using the set error value and the prior error state estimate value at the next time step to obtain the prior error state covariance at the next time step.

[0018] The quantity measurement construction module is used to obtain the current Bluetooth sensor coordinates and current Bluetooth sensor velocity from the pre-set Bluetooth sensor, obtain the current inertial navigation velocity, current inertial navigation coordinates and current accelerometer measurement value from the pre-set inertial navigation system, and construct the quantity measurement for the next moment using the sampling time, the current Bluetooth sensor coordinates, the current Bluetooth sensor velocity, the current inertial navigation velocity, the current inertial navigation coordinates and current accelerometer measurement value;

[0019] The third calculation module is used to calculate the estimated value of the prior error state at the next time moment and the prior error state covariance at the next time moment based on the measurement at the next time moment, so as to obtain the estimated value of the posterior error state at the next time moment.

[0020] The spatiotemporal registration result acquisition module is used to analyze the covariance of the set error value and the posterior error state estimate at the next time step, and to use the posterior error state estimate at the next time step as the spatiotemporal registration result based on the analysis results.

[0021] Based on the above-mentioned spatiotemporal registration method, the present invention also provides a spatiotemporal registration system.

[0022] Another technical solution of the present invention to solve the above-mentioned technical problems is as follows: a spatiotemporal registration system, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the spatiotemporal registration method described above is implemented.

[0023] Based on the above-mentioned spatiotemporal registration method, the present invention also provides a computer-readable storage medium.

[0024] Another technical solution of the present invention to solve the above-mentioned technical problems is as follows: a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the spatiotemporal registration method as described above.

[0025] The beneficial effects of this invention are as follows: A priori error state estimate is calculated using error data and an estimated sampling time; a priori error state covariance is calculated using the set error value and the covariance of the priori error state estimate; a measurement is constructed using sampling time, Bluetooth sensor coordinates, Bluetooth sensor velocity, inertial navigation velocity, inertial navigation coordinates, and accelerometer measurements; a posterior error state estimate is calculated based on the measurement of the priori error state estimate and the estimated value of the priori error state covariance; the covariance of the set error value and the posterior error state estimate is analyzed, and the posterior error state estimate is used as the spatiotemporal registration result based on the analysis results. This simplifies the prediction and correction stages of the extended Kalman filter, greatly reduces the impact of IMU zero bias on positioning performance, and achieves real-time spatiotemporal registration. Furthermore, it meets the requirements of good real-time performance, strong practicality, and ease of implementation, thus satisfying current needs. Attached Figure Description

[0026] Figure 1 A schematic flowchart of a spatiotemporal registration method provided in an embodiment of the present invention;

[0027] Figure 2 A schematic diagram of the lever effect provided for an embodiment of the present invention;

[0028] Figure 3 This is a block diagram of a spatiotemporal registration device provided in an embodiment of the present invention. Detailed Implementation

[0029] The principles and features of the present invention are described below with reference to the accompanying drawings. The examples given are only for explaining the present invention and are not intended to limit the scope of the present invention.

[0030] Figure 1 This is a flowchart illustrating a spatiotemporal registration method provided in an embodiment of the present invention.

[0031] like Figure 1 As shown, a spatiotemporal registration method includes the following steps:

[0032] S1: Obtain the current time, import the error data and sampling time, calculate the estimate using the error data and the sampling time, and obtain the prior error state estimate for the next time.

[0033] S2: Import the set error value corresponding to the prior error state estimate of the next time step, and calculate the covariance using the set error value and the prior error state estimate of the next time step to obtain the prior error state covariance of the next time step.

[0034] S3: Obtain the current Bluetooth sensor coordinates and current Bluetooth sensor velocity from the preset Bluetooth sensor; obtain the current inertial navigation velocity, current inertial navigation coordinates, and current accelerometer measurement value from the preset inertial navigation system; and construct the measurement value for the next moment using the sampling time, the current Bluetooth sensor coordinates, the current Bluetooth sensor velocity, the current inertial navigation velocity, the current inertial navigation coordinates, and the current accelerometer measurement value.

[0035] S4: Calculate the estimated value of the prior error state at the next time step and the estimated value of the prior error state covariance at the next time step based on the measurement at the next time step, and obtain the estimated value of the posterior error state at the next time step.

[0036] S5: Analyze the covariance of the set error value and the posterior error state estimate at the next time step, and use the posterior error state estimate at the next time step as the spatiotemporal registration result based on the analysis results.

[0037] In the above embodiments, a priori error state estimate is calculated using error data and an estimated sampling time. A priori error state covariance is calculated using the covariance of the set error value and the priori error state estimate. Measurements are constructed using sampling time, Bluetooth sensor coordinates, Bluetooth sensor velocity, inertial navigation velocity, inertial navigation coordinates, and accelerometer measurements. A posterior error state estimate is calculated based on the measurements of the priori error state estimate and the priori error state covariance. The covariance of the set error value and the posterior error state estimate is analyzed, and the posterior error state estimate is used as the spatiotemporal registration result based on the analysis results. This simplifies the prediction and correction stages of the extended Kalman filter, significantly reduces the impact of IMU zero bias on positioning performance, and achieves real-time spatiotemporal registration. Furthermore, it meets the requirements of good real-time performance, strong practicality, and ease of implementation, thus satisfying current needs.

[0038] Optionally, as an embodiment of the present invention, the error data includes an error driving matrix, an attitude transformation matrix, an estimated attitude error at the current time, a velocity error at the current time, a position error at the current time, an IMU accelerometer bias at the current time, a gyroscope bias at the current time, an external lever effect error at the current time, and a time synchronization error at the current time.

[0039] In step S1, the process of calculating the prior error state estimate for the next moment using the error data and the sampling time includes:

[0040] The prior error state estimate for the next moment is obtained by calculating the estimated values ​​using the first formula, the sampling time, the error driving matrix, the attitude transformation matrix, the estimated attitude error at the current moment, the velocity error at the current moment, the position error at the current moment, the IMU accelerometer bias at the current moment, the gyroscope bias at the current moment, the external arm effect error at the current moment, and the time synchronization error at the current moment. The first formula is:

[0041]

[0042] in,

[0043] in, Let E be the prior error state estimate at time k+1. k+1 Let be the error state transition matrix at time k+1. Let I be the error state vector at time k, ΔT be the sampling time, R be the attitude transformation matrix, and Ω(-δ) be the error state vector. ω,k ) is a preset quaternion transformation matrix, β1, β2 and F clk All are constants, δp k Let δv be the estimated attitude error at time k. k Let δq be the velocity error at time k. k Let δ be the position error at time k. f,k For the IMU accelerometer to have zero bias at time k, δ ω,k For the gyroscope at time k, δl is zero bias. k Let δx be the error caused by the outer arm effect at time k. clk,k Let be the time synchronization error at time k.

[0044] It should be understood that traditional Kalman filters often do not directly use navigation state variables, but instead model the state error variables. This approach ensures that trajectory estimation and the filtering algorithm are calculated separately. Specifically, the inertial navigation equations recursively output the navigation state, while the filter only estimates the state error when measurements occur and promptly feeds it back into the system state variables. This improves computational efficiency and reduces computational errors. The error state vector is selected as follows:

[0045]

[0046] It includes estimation of attitude error, velocity error, position error, IMU accelerometer bias, and gyroscope bias, where δl k This is the error caused by the outer lever arm effect of the INS, δx clk,k It estimates the time synchronization error between INS and GNSS systems.

[0047] Specifically, the preliminary prior error state estimate is as follows:

[0048]

[0049] This represents the prior error state estimate at time k+1, based on the posterior estimate from the previous iteration. Unreliable estimates were made.

[0050] E k+1 : Represents the error state transition matrix, E x+1 =F, F is The state transition matrix from time k to time k;

[0051] variable This represents the posterior estimate of the previous iteration, with the initial value of 0 set manually for the first iteration.

[0052] In the above embodiments, the prior error state estimate for the next moment is obtained by calculating the estimate using error data and the sampling time. This ensures that the trajectory estimation and filtering algorithm are calculated separately, which helps to improve computational efficiency and reduce computational errors. It simplifies the prediction and correction steps of the extended Kalman filter, greatly reduces the impact of IMU zero bias on positioning performance, and realizes real-time spatiotemporal registration.

[0053] Optionally, as an embodiment of the present invention, in step S2, the process of calculating the covariance using the set error value and the prior error state estimate at the next time moment to obtain the prior error state covariance at the next time moment includes:

[0054] The covariance is calculated using the second equation, the set error value, and the prior error state estimate for the next time step. The second equation is:

[0055]

[0056] in, Let δx be the prior error state covariance at time k+1. k+1 Let $\frac{ ... Let E be the prior error state estimate at time k+1, and let E() be the covariance function.

[0057] Specifically, the error between the prior error state estimate and the true set value (i.e., the prior error state covariance) is as follows:

[0058]

[0059] Represents the prior error state estimate at time k+1. (i.e., the prior error state estimate at time k+1) and the actual setpoint error δx k+1 covariance

[0060] δx k+1 : Represents the set error state true value (constant matrix) (i.e., the set error value).

[0061] This represents the prior error state estimate (the prior error state estimate at time k+1).

[0062] E(): Covariance calculation expression (calculation symbol)

[0063] E k+1 : Represents the error state transition matrix, E k+1 =F

[0064] variable P k The posterior estimated covariance at time k (the initial value is set manually in the first iteration, and the posterior estimated covariance at time k-1 is used in subsequent iterations).

[0065] Q: The covariance of process noise is a fixed matrix that is set manually.

[0066] In the above embodiments, the covariance is calculated by setting the error value and the prior error state estimate at the next moment, and the prior error state covariance at the next moment is obtained. This ensures that the trajectory estimation and filtering algorithm are calculated separately, which is beneficial to improve the computational efficiency and reduce the computational error. It simplifies the prediction and correction links of the extended Kalman filter, greatly reduces the impact of IMU zero bias on positioning performance, and realizes real-time spatiotemporal registration.

[0067] Optionally, as an embodiment of the present invention, in step S3, the process of constructing the measurement for the next moment using the sampling time, the Bluetooth sensor coordinates at the current moment, the Bluetooth sensor velocity at the current moment, the inertial navigation velocity at the current moment, the inertial navigation coordinates at the current moment, and the accelerometer measurement value at the current moment includes:

[0068] The inertial navigation coordinates at the current moment are calculated based on the current inertial navigation velocity, the current accelerometer measurement, and the sampling time to obtain the inertial navigation coordinates at the next moment.

[0069] Based on the sampling time and the accelerometer measurement at the current moment, the inertial navigation velocity at the current moment is calculated to obtain the inertial navigation velocity at the next moment;

[0070] Calculate the difference between the inertial navigation coordinates at the next moment and the Bluetooth sensor coordinates at the current moment to obtain the coordinate difference value at the next moment;

[0071] Calculate the difference between the inertial navigation velocity at the next moment and the Bluetooth sensor velocity at the current moment to obtain the velocity difference value at the next moment;

[0072] The measurement of the next moment is constructed by the difference in coordinates and the difference in velocity at the next moment.

[0073] It should be understood that, based on the current inertial velocity, the current accelerometer measurement, and the sampling time, the inertial coordinates at the current moment are calculated, and the formula for calculating the inertial coordinates at the next moment is as follows:

[0074] np k+1 = n p k + n v k ΔT+ n a k ΔT 2 / 2,

[0075] The target acceleration after lever arm effect error compensation in the navigation coordinate system is: in For accelerometer measurements, position (p) k The hardware can directly output the quantity at time k (i.e., the inertial navigation coordinates at the current time).

[0076] Specifically, based on the sampling time and the accelerometer measurement at the current moment, the inertial navigation velocity at the current moment is calculated, and the calculation formula for the inertial navigation velocity at the next moment is as follows:

[0077] n v k+1 = n v k + n a k ΔT,

[0078] The target acceleration after lever arm effect error compensation in the navigation coordinate system is: in For accelerometer measurements, velocity (v) k The hardware can directly output the quantity at time k (i.e., the inertial navigation velocity at the current time).

[0079] In the above embodiments, the measurement for the next moment is constructed by sampling time, Bluetooth sensor coordinates at the current moment, Bluetooth sensor velocity at the current moment, inertial navigation velocity at the current moment, inertial navigation coordinates at the current moment, and accelerometer measurement value at the current moment. This ensures that trajectory estimation and filtering algorithms are calculated separately, which is beneficial to improving computational efficiency and reducing computational errors. It simplifies the prediction and correction links of the extended Kalman filter, greatly reduces the impact of IMU zero bias on positioning performance, and realizes real-time spatiotemporal registration.

[0080] Optionally, as an embodiment of the present invention, the process of S4 includes:

[0081] The partial derivative of the prior error state estimate for the next time step is calculated based on the measurement at the next time step, and the measurement matrix for the next time step is obtained.

[0082] Import the measurement noise covariance matrix corresponding to the prior error state covariance at the next time step, and calculate the Kalman gain matrix at the next time step using the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix.

[0083] The posterior error state estimate for the next time step is obtained by calculating the estimate using the third equation, the prior error state estimate for the next time step, the Kalman gain matrix for the next time step, the measurement matrix for the next time step, and the measurement values ​​for the next time step. The third equation is as follows:

[0084]

[0085] Where, δx k+1 Let be the posterior error state estimate at time k+1. Let K be the prior error state estimate at time k+1. k+1 Let δz be the Kalman gain matrix at time k+1. k+1 For the measurement at time k+1, H k+1 Let be the measurement matrix at time k+1.

[0086] Specifically, the posterior error state estimate is calculated as follows:

[0087]

[0088] Posterior position error estimation

[0089] Posterior velocity error estimation

[0090] Posterior attitude error estimation

[0091] Posterior accelerometer drift error estimation

[0092] Posterior gyroscope drift error estimation

[0093] Posterior lever arm effect error estimation

[0094] Posterior time asynchrony error estimation

[0095] δx k+1 : Represents the posterior error state estimate at time k+1.

[0096] variable Represents the prior error state estimate.

[0097] K k+1 : Represents the Kalman gain matrix

[0098] variable δz k+1 : Represents the output quantity (i.e., the measurement quantity) of the measurement equation.

[0099] variable H k+1 : represents the measurement matrix, where, Find h k+1 (x k+1 For error state Find the partial derivative.

[0100] In the above embodiments, the prior error state estimate and the prior error state covariance of the next time moment are estimated based on the measurement at the next time moment to obtain the posterior error state estimate at the next time moment. This can greatly reduce the impact of IMU zero bias on positioning performance and realize real-time spatiotemporal registration. At the same time, it can meet the conditions of good real-time performance, strong practicality, and ease of implementation, which meets the current needs.

[0101] Optionally, as an embodiment of the present invention, the process of calculating the Kalman gain matrix at the next time step by using the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix includes:

[0102] The Kalman gain matrix for the next time step is obtained by calculating the Kalman gain using the fourth equation, the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix. The fourth equation is:

[0103]

[0104] Among them, K k+1 H is the Kalman gain matrix at time k+1. k+1 This is the measurement matrix at time k+1. Let R be the prior error state covariance at time k+1. k+1 Let be the measurement noise covariance matrix corresponding to the prior error state covariance at time k+1.

[0105] It should be understood that the Kalman gain is calculated as follows:

[0106]

[0107] K k+1 : Represents the Kalman gain matrix at time k+1

[0108] variable Represents the prior error state covariance

[0109] variable H k+1 : represents the measurement matrix, where, Find h k+1 (x k+1 For error state Find the partial derivative

[0110] R k+1 : Represents the covariance matrix of measurement noise.

[0111] In the above embodiments, the Kalman gain is calculated by using the fourth equation, the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix to obtain the Kalman gain matrix at the next time step. This can greatly reduce the impact of IMU zero bias on positioning performance and achieve real-time spatiotemporal registration. At the same time, it can meet the conditions of good real-time performance, strong practicality, and ease of implementation, which meets the current needs.

[0112] Optionally, as an embodiment of the present invention, the process of S5 includes:

[0113] S51: The covariance is calculated by using the fifth equation, the set error value, and the estimated posterior error state value at the next time step. The fifth equation is:

[0114]

[0115] Among them, P k+1 Let E() be the posterior error state covariance at time k+1, and let δx be the covariance function. k+1 Let δx be the posterior error state estimate at time k+1. k+1 This is the set error value corresponding to the prior error state estimate at time k+1;

[0116] S52: Determine if the next time step is equal to the preset stop time. If yes, execute S53; otherwise, return to S1.

[0117] S53: Plot the covariance of the posterior error state at all times to obtain the covariance curve;

[0118] S54: Determine whether the covariance curve is in a stationary state. If not, import the updated preset stop time and return to S1; if yes, use the posterior error state estimate at the next time moment as the spatiotemporal registration result.

[0119] It should be understood that whether the covariance curve is stationary means that the covariance curve as a whole tends to be stationary.

[0120] Specifically, the covariance of the posterior error state estimate is calculated as follows:

[0121]

[0122] P k+1 : Represents the covariance of the posterior error state estimate at time k+1.

[0123] δx k+1 : Represents the set true value of the error state (constant matrix)

[0124] Represents the posterior error state estimate.

[0125] variable Represents the prior error state covariance

[0126] variable K k+1 : Represents the Kalman gain matrix

[0127] variable H k+1 : Represents the measurement matrix

[0128] I: Represents the identity matrix.

[0129] In the above embodiments, the covariance of the set error value and the posterior error state estimate at the next moment is analyzed, and the posterior error state estimate at the next moment is used as the spatiotemporal registration result based on the analysis results. This can greatly reduce the impact of IMU zero bias on positioning performance and realize real-time spatiotemporal registration. At the same time, it can meet the conditions of good real-time performance, strong practicality, and easy implementation, which meets the current needs.

[0130] Alternatively, as another embodiment of the present invention, the calculation of the measurement of the present invention may also be as follows:

[0131] δz k+1 =h k+1 (x k+1 )

[0132] δz k+1 : Represents the quantity directly output by the sensor at time k+1 (the difference in position and velocity output by different sensors).

[0133] h k+1 : Indicates the relationship between sensor measurements and state variables.

[0134] Alternatively, as another embodiment of the present invention, the steps of the present invention are as follows:

[0135] (1) Establish the motion state equation (considering the errors caused by time asynchrony and lever effect in space) and measurement equation.

[0136] (2) Establish an error state model through the equation of motion and perform prior error state prediction.

[0137] (3) The error value between the prior error state and the actual error is determined by the prior error covariance, wherein the state transition matrix is ​​set and the observation matrix is ​​solved by the first-order linearized measurement equation.

[0138] (4) Further improve the accuracy of the calculated values ​​by calculating the gain of the filter.

[0139] (5) Finally, calculate the required posterior error state value and the error of the posterior error state value (the error can continue to be fed back to step two and step three, and continuously iterate to improve the accuracy of the final result).

[0140] Alternatively, as another embodiment of the present invention, the equation of motion of the present invention is as follows:

[0141] To address the issues of time asynchrony caused by differences in sampling rates and transmission rates among multiple sensors, and accelerometer measurement errors due to lever arm effects, a real-time spatiotemporal registration algorithm based on an extended Kalman filter is proposed. This algorithm uses an extended Kalman filter to filter and predict the sensor observation data to obtain the sensor data at the current registration moment, thus achieving real-time spatiotemporal registration.

[0142] Traditional strapdown inertial navigation systems mostly use 15th-order state variables (raw inputs) to establish the system's equations of motion. In indoor positioning scenarios, since the target is moving at low speeds (less than 100 m / s) and the positioning area is small, such as a few hundred meters, the effects of Earth's rotation and curvature are ignored. This paper considers the error values ​​caused by time asynchrony between multiple sensors and lever arm effects. Therefore, a 19-dimensional state variable (raw input) is used to establish the system's equations of motion. The inertial navigation equations of motion are as follows:

[0143]

[0144] The target acceleration after lever arm effect error compensation in the navigation coordinate system is: in The value is the accelerometer measurement; the rotational angular velocity after error compensation is ω. k =ω k -δ ω,k -w g , where ω k This is a gyroscope measurement. The meanings of the other letters are explained below. n a k ωk Substituting the expression into equation (1), the system of equations (1) can be simplified to matrix form (2):

[0145]

[0146] The motion state quantities (initial input quantities) are respectively

[0147] Position (p) k (The hardware can directly output the quantity at time k)

[0148] velocity (v) k (The hardware can directly output the quantity at time k)

[0149] Attitude Quaternion (q) k (The parameters of the angle directly output by the hardware at time k, which are then converted)

[0150] Accelerometer drift (δ) f,k (preset amount at time k)

[0151] Gyroscope drift (δ) ω,k (preset amount at time k)

[0152] Lever arm effect error (δ) l,k (preset amount at time k)

[0153] Received time state vector (x) clk,k (Preset amount at time k).

[0154] Similarly, the output state quantity at time k+1 is: n p k+1 n v k+1 q k+1 δ f,k+1 δ ω,k+1 δ l,k+1 x clk,k+1 ;

[0155] I is the identity matrix, R is the attitude transformation matrix from the vehicle coordinate system to the navigation coordinate system, ΔT is the sampling time, and β1β2F clk Let Ω(a) be a constant, and B be a quaternion transformation matrix. k Let e ​​be the error driving matrix. k For process noise, the control gain matrix is ​​not considered, i.e. 0

[0156] By simplifying equation (2), the simplified system motion equation is obtained as follows:

[0157] X k+1 =FX k +B k e k(3)

[0158] Among them, X k+1 for The state output at time t; F is The state transition matrix from time k to time k; X k for The state input at time; B k e k Let be the noise error vector at time k.

[0159] Alternatively, as another embodiment of the present invention, the equation of motion of the present invention is as follows:

[0160] To simplify the system, the system measurement equations are expressed as follows:

[0161] Z k+1 =AX k+1 +V k (4)

[0162] Among them, Z k+1 Let A be the system's measurement values, and let X be the measurement matrix. k+1 V is the state output at time k+1 (the output of formula (3)). k For measuring noise.

[0163] Optionally, as another embodiment of the present invention, this invention utilizes the error state quantity modeling of the navigation equation to simplify the prediction and correction stages of the extended Kalman filter. This is because once the error quantity is fed back into the system state, in the first sampling period, assuming the initial value of the prior error state estimate is 0, the task of the time update stage is only to estimate the prior error covariance matrix. In the measurement update stage, the measurement function is zero, which is also based on this assumption that the system state quantity does not have prior error after filtering and correction. Applying this conclusion to the Jacobian matrix, the calculation of the first-order Taylor approximation is also simplified. Therefore, when calculating the posterior error estimate, only the filter gain and vector need to be calculated. Then, in the next sampling period, the posterior error estimate is fed back into the system state. The algorithm repeats this process, setting the prior error estimate to zero each time.

[0164] Optionally, as another embodiment of the present invention, the present invention compensates for errors caused by lever effect and time asynchrony. The position, velocity, attitude, acceleration bias, angular velocity bias, lever effect, and time asynchrony error state vectors are used as the 19-dimensional estimated state vector of the combined Kalman filter. The position and velocity estimated by BLE are used as the measurements of the extended Kalman filter. The difference between these measurements and the position and velocity predicted by the INS navigation equations constitutes the measurement information. This measurement information is used to update the state vector in the extended Kalman filter, and the updated state vector is then used to correct navigation parameters such as INS bias. This significantly reduces the impact of IMU bias on positioning performance during INS navigation updates, ultimately calculating the target position.

[0165] Figure 3 This is a block diagram of a spatiotemporal registration device provided in an embodiment of the present invention.

[0166] Alternatively, as another embodiment of the present invention, such as Figure 3 As shown, a spatiotemporal registration device includes:

[0167] The first calculation module is used to obtain the current time, import error data and sampling time, calculate the estimated value using the error data and sampling time, and obtain the prior error state estimate for the next time.

[0168] The second calculation module is used to import the set error value corresponding to the prior error state estimate value at the next time step, and to calculate the covariance using the set error value and the prior error state estimate value at the next time step to obtain the prior error state covariance at the next time step.

[0169] The quantity measurement construction module is used to obtain the current Bluetooth sensor coordinates and current Bluetooth sensor velocity from the pre-set Bluetooth sensor, obtain the current inertial navigation velocity, current inertial navigation coordinates and current accelerometer measurement value from the pre-set inertial navigation system, and construct the quantity measurement for the next moment using the sampling time, the current Bluetooth sensor coordinates, the current Bluetooth sensor velocity, the current inertial navigation velocity, the current inertial navigation coordinates and current accelerometer measurement value;

[0170] The third calculation module is used to calculate the estimated value of the prior error state at the next time moment and the prior error state covariance at the next time moment based on the measurement at the next time moment, so as to obtain the estimated value of the posterior error state at the next time moment.

[0171] The spatiotemporal registration result acquisition module is used to analyze the covariance of the set error value and the posterior error state estimate at the next time step, and to use the posterior error state estimate at the next time step as the spatiotemporal registration result based on the analysis results.

[0172] Optionally, another embodiment of the present invention provides a spatiotemporal registration system, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the spatiotemporal registration method as described above. This system can be a computer or similar system.

[0173] Optionally, another embodiment of the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the spatiotemporal registration method as described above.

[0174] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus.

[0175] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working process of the above-described apparatus and unit can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0176] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative. For instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed.

[0177] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of the embodiments of the present invention, depending on actual needs.

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

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

[0180] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A spatiotemporal registration method, characterized in that, Includes the following steps: S1: Obtain the current time, import the error data and sampling time, calculate the estimate using the error data and the sampling time, and obtain the prior error state estimate for the next time. S2: Import the set error value corresponding to the prior error state estimate of the next time step, and calculate the covariance using the set error value and the prior error state estimate of the next time step to obtain the prior error state covariance of the next time step. S3: Obtain the current Bluetooth sensor coordinates and current Bluetooth sensor velocity from the preset Bluetooth sensor; obtain the current inertial navigation velocity, current inertial navigation coordinates, and current accelerometer measurement value from the preset inertial navigation system; and construct the measurement value for the next moment using the sampling time, the current Bluetooth sensor coordinates, the current Bluetooth sensor velocity, the current inertial navigation velocity, the current inertial navigation coordinates, and the current accelerometer measurement value. S4: Calculate the prior error state estimate and the prior error state covariance at the next time step based on the measurement at the next time step, and obtain the posterior error state estimate at the next time step; S5: Analyze the covariance between the set error value and the posterior error state estimate at the next moment, and use the posterior error state estimate at the next moment as the spatiotemporal registration result based on the analysis results; The error data includes the error driving matrix, attitude conversion matrix, estimated attitude error at the current moment, velocity error at the current moment, position error at the current moment, IMU accelerometer bias at the current moment, gyroscope bias at the current moment, external lever effect error at the current moment, and time synchronization error at the current moment. In step S1, the process of calculating the prior error state estimate for the next moment using the error data and the sampling time includes: The prior error state estimate for the next moment is obtained by calculating the estimated values ​​using the first formula, the sampling time, the error driving matrix, the attitude transformation matrix, the estimated attitude error at the current moment, the velocity error at the current moment, the position error at the current moment, the IMU accelerometer bias at the current moment, the gyroscope bias at the current moment, the external arm effect error at the current moment, and the time synchronization error at the current moment. The first formula is: ， in, , , in, For the first The prior error state estimate at time t. For the first Error state transition matrix at time t, For the first Error state vector at time step [time]. It is the identity matrix. Sampling time, This is the attitude transformation matrix. For the preset quaternion transformation matrix, , and All are constants. For the first The estimated attitude error at time step. For the first The speed error at any moment, For the first Position error at any given time For the first The IMU accelerometer shows zero bias at a given moment. For the first The gyroscope is at zero bias at any given moment. For the first The error caused by the outer arm effect at any given time. For the first Time synchronization error at any given moment.

2. The spatiotemporal registration method according to claim 1, characterized in that, In step S2, the process of calculating the covariance using the set error value and the prior error state estimate for the next time step to obtain the prior error state covariance includes: The covariance is calculated by using the second equation, the set error value, and the prior error state estimate at the next time step. The prior error state covariance at the next time step is obtained by the second equation: ， in, For the first The prior error state covariance at time 1. For the first The setpoint error value corresponding to the prior error state estimate at time t. For the first The prior error state estimate at time t. Let be the covariance function.

3. The spatiotemporal registration method according to claim 1, characterized in that, In step S3, the process of constructing the measurement for the next moment using the sampling time, the Bluetooth sensor coordinates at the current moment, the Bluetooth sensor velocity at the current moment, the inertial navigation velocity at the current moment, the inertial navigation coordinates at the current moment, and the accelerometer measurement value at the current moment includes: The inertial navigation coordinates at the current moment are calculated based on the current inertial navigation velocity, the current accelerometer measurement, and the sampling time to obtain the inertial navigation coordinates at the next moment. The inertial navigation velocity at the current moment is calculated based on the sampling time and the accelerometer measurement value at the current moment to obtain the inertial navigation velocity at the next moment. Calculate the difference between the inertial navigation coordinates at the next moment and the Bluetooth sensor coordinates at the current moment to obtain the coordinate difference value at the next moment; Calculate the difference between the inertial navigation velocity at the next moment and the Bluetooth sensor velocity at the current moment to obtain the velocity difference value at the next moment; The measurement of the next moment is constructed by the difference in coordinates and the difference in velocity at the next moment.

4. The spatiotemporal registration method according to claim 1, characterized in that, The process in S4 includes: The partial derivative of the prior error state estimate for the next time step is calculated based on the measurement at the next time step, and the measurement matrix for the next time step is obtained. Import the measurement noise covariance matrix corresponding to the prior error state covariance at the next time step, and calculate the Kalman gain matrix at the next time step using the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix. The posterior error state estimate for the next time step is obtained by calculating the estimate using the third equation, the prior error state estimate for the next time step, the Kalman gain matrix for the next time step, the measurement matrix for the next time step, and the measurement values ​​for the next time step. The third equation is as follows: ， in, For the first The posterior error state estimate at time t. For the first The prior error state estimate at time t. For the first The Kalman gain matrix at time t. For the first Measurement of time, For the first Measurement matrix at time.

5. The spatiotemporal registration method according to claim 4, characterized in that, The process of calculating the Kalman gain matrix at the next time step using the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix includes: The Kalman gain matrix for the next time step is obtained by calculating the Kalman gain using the fourth equation, the measurement matrix at the next time step, the prior error state covariance at the next time step, and the measurement noise covariance matrix. The fourth equation is: ， in, For the first The Kalman gain matrix at time t. For the first Measurement matrix at time, For the first The prior error state covariance at time 1. For the first The measurement noise covariance matrix corresponding to the prior error state covariance at time t.

6. The spatiotemporal registration method according to claim 1, characterized in that, The process of S5 includes: S51: The covariance is calculated by using the fifth equation, the set error value, and the posterior error state estimate at the next time step. The fifth equation is: ， in, For the first The posterior error state covariance at time 1. Let covariance function be used. For the first The posterior error state estimate at time t. For the first The set error value corresponding to the prior error state estimate at time t; S52: Determine if the next time step is equal to the preset stop time. If yes, execute S53; otherwise, return to S1. S53: Plot the covariance of the posterior error state at all times to obtain the covariance curve; S54: Determine whether the covariance curve is in a stationary state. If not, import the updated preset stop time and return to S1; if yes, use the posterior error state estimate at the next time moment as the spatiotemporal registration result.

7. A spatiotemporal registration device, characterized in that, include: The first calculation module is used to obtain the current time, import error data and sampling time, calculate the estimated value using the error data and sampling time, and obtain the prior error state estimate for the next time. The second calculation module is used to import the set error value corresponding to the prior error state estimate value at the next time step, and to calculate the covariance using the set error value and the prior error state estimate value at the next time step to obtain the prior error state covariance at the next time step. The quantity measurement construction module is used to obtain the current Bluetooth sensor coordinates and current Bluetooth sensor velocity from the pre-set Bluetooth sensor, obtain the current inertial navigation velocity, current inertial navigation coordinates and current accelerometer measurement value from the pre-set inertial navigation system, and construct the quantity measurement for the next moment using the sampling time, the current Bluetooth sensor coordinates, the current Bluetooth sensor velocity, the current inertial navigation velocity, the current inertial navigation coordinates and current accelerometer measurement value; The third calculation module is used to calculate the estimated value of the prior error state at the next time step and the estimated value of the prior error state covariance at the next time step based on the measurement at the next time step, so as to obtain the estimated value of the posterior error state at the next time step; The spatiotemporal registration result acquisition module is used to analyze the covariance of the set error value and the posterior error state estimate at the next time step, and to use the posterior error state estimate at the next time step as the spatiotemporal registration result based on the analysis results. The error data includes the error driving matrix, attitude conversion matrix, estimated attitude error at the current moment, velocity error at the current moment, position error at the current moment, IMU accelerometer bias at the current moment, gyroscope bias at the current moment, external lever effect error at the current moment, and time synchronization error at the current moment. In the first calculation module, the process of calculating the prior error state estimate for the next moment using the error data and the sampling time includes: The prior error state estimate for the next moment is obtained by calculating the estimated values ​​using the first formula, the sampling time, the error driving matrix, the attitude transformation matrix, the estimated attitude error at the current moment, the velocity error at the current moment, the position error at the current moment, the IMU accelerometer bias at the current moment, the gyroscope bias at the current moment, the external arm effect error at the current moment, and the time synchronization error at the current moment. The first formula is: ， in, , , in, For the first The prior error state estimate at time t. For the first Error state transition matrix at time t, For the first Error state vector at time step [time]. It is the identity matrix. Sampling time, This is the attitude transformation matrix. For the preset quaternion transformation matrix, , and All are constants. For the first The estimated attitude error at time step. For the first The speed error at any moment, For the first Position error at any given time For the first The IMU accelerometer shows zero bias at a given moment. For the first The gyroscope is at zero bias at any given moment. For the first The error caused by the outer arm effect at any given time. For the first Time synchronization error at any given moment.

8. A spatiotemporal registration system, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the spatiotemporal registration method as described in any one of claims 1 to 6.

9. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the spatiotemporal registration method as described in any one of claims 1 to 6.

Images