A spatial deviation processing apparatus, method and storage medium

Abstract

The application provides a space domain deviation processing device, method and storage medium, and belongs to the technical field of radio positioning and navigation. The space domain deviation processing device comprises: an acquisition unit that acquires inertial measurement data and space domain observation data; a modeling unit that constructs an augmented error state vector comprising a carrier navigation error state and a space domain deviation state; a reference construction unit that recursively constructs a nominal pose of the carrier based on the inertial measurement data and updates an error covariance matrix; a residual calculation unit that constructs a residual based on the nominal pose, the space domain deviation state and an observation model; and an updating unit that corrects and updates the space domain deviation state according to the residual and the error covariance matrix. The space domain deviation processing device provided by the application can reduce the influence of the space domain deviation state on the carrier pose state, reduce the navigation drift problem caused by antenna installation deviation or multipath interference, and improve the positioning and pose stability.

Description

Technical Field

[0001] This application relates to the field of radio positioning and navigation technology, and in particular to a spatial deviation processing device, method and storage medium. Background Technology

[0002] In multi-channel ultra-wideband (UWB) positioning systems, to achieve high-precision positioning and attitude determination, in addition to traditional ranging information, the system typically incorporates spatial observations such as phase difference of arrival (PDOA) or angle of arrival (AoA) between multiple receiving channels as auxiliary information. The accuracy of these spatial observations has a significant impact on the final navigation and positioning accuracy. In practical engineering deployments and applications, due to the presence of multipath reflection interference and physical deviations during equipment assembly and installation, systematic biases are often introduced into the spatial observation data.

[0003] In the proposed solution, a static calibration is performed in an ideal anechoic chamber environment before the equipment leaves the factory to eliminate inherent biases. After the equipment is actually deployed and put into operation, for uncalibrated biases caused by multipath interference or installation, the proposed solution treats the biases as equivalent to Gaussian random noise, i.e., by increasing the variance of the observation model (reducing the weight of the observations) to weaken the impact of the biases on the system. Summary of the Invention

[0004] This application provides a spatial deviation processing device, comprising an acquisition unit, a modeling unit, a reference construction unit, a residual calculation unit, and an update unit. The acquisition unit acquires inertial measurement data and spatial observations. The modeling unit, connected to the acquisition unit, constructs an augmented error state vector including the carrier navigation error state and the spatial deviation state. The reference construction unit, connected to the acquisition unit and the modeling unit, recursively derives the nominal pose of the carrier based on the inertial measurement data and updates the error covariance matrix corresponding to the augmented error state vector, ensuring that the covariance corresponding to the carrier navigation error state is less than the covariance corresponding to the spatial deviation state. The residual calculation unit, connected to the acquisition unit, the modeling unit, and the reference construction unit, obtains predicted values ​​of spatial observations using a preset observation model based on the nominal pose and spatial deviation state of the carrier, and constructs residuals based on the spatial observations and the predicted values ​​of the spatial observations. The update unit, connected to the modeling unit, the reference construction unit, and the residual calculation unit, determines the correction amount of the spatial deviation state based on the residuals and the error covariance matrix, and updates the spatial deviation state based on the correction amount.

[0005] This application also provides a method for handling spatial deviations. First, inertial measurement data and spatial observations are acquired, and an augmented error state vector is constructed based on the inertial measurement data and the spatial observations. The augmented error state vector includes a carrier navigation error state and a spatial deviation state. Further, the nominal pose of the carrier is recursively deduced based on the inertial measurement data, and the error covariance matrix corresponding to the augmented error state vector is updated so that the covariance corresponding to the carrier navigation error state is less than the covariance corresponding to the spatial deviation state.

[0006] After constructing the augmented error state vector, based on the nominal pose of the carrier and the spatial bias state, the predicted value corresponding to the spatial observation is determined using a preset observation model, and a residual is constructed based on the difference between the spatial observation and the predicted value. Further, the spatial bias state is updated based on the residual and the error covariance matrix to determine the spatial bias corresponding to the spatial observation.

[0007] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the spatial deviation processing method provided in the embodiments of this application. Attached Figure Description

[0008] Figure 1 This is a structural example of the spatial deviation processing device provided in the embodiments of this application.

[0009] Figure 2 This is a schematic diagram illustrating the decoupling and convergence effect of spatial bias provided in the embodiments of this application.

[0010] Figure 3 This is a schematic diagram showing the effect of the residual calculation unit provided in the embodiments of this application in identifying and eliminating multipath interference by performing a consistency check.

[0011] Figure 4 A flowchart of a spatial deviation processing method provided in an embodiment of this application. Detailed Implementation

[0012] The technical solutions in this application will now be clearly and completely described with reference to the accompanying drawings.

[0013] Figure 1 An example of the structure of the spatial deviation processing device 100 is shown. Figure 1 It includes: a spatial deviation processing device 100, which includes an acquisition unit 120, a modeling unit 130, a benchmark construction unit 140, a residual calculation unit 150, and an update unit 160.

[0014] The airspace deviation processing device 100 is an electronic device or logic system used to identify, remove, and compensate for deviations in airspace observation data in a radio positioning and navigation system. The airspace deviation processing device 100 can be widely used in various radio positioning and navigation scenarios requiring high-precision pose estimation, such as high-precision navigation for UAVs, indoor precision positioning, and autonomous robot movement.

[0015] In some embodiments of this application, inertial measurement units (IMUs) can be used to acquire inertial measurement data characterizing the motion state of the carrier. An IMU typically integrates a three-axis gyroscope and a three-axis accelerometer to collect motion parameters of the carrier in the carrier coordinate system in real time. Specifically, the three-axis gyroscope measures the angular velocity data of the carrier about three orthogonal axes, reflecting the carrier's attitude change information; the three-axis accelerometer measures the specific force data of the carrier in the corresponding axes, reflecting the carrier's velocity and position change information.

[0016] The primary function of an inertial measurement unit (IMU) is to provide a high-frequency, high-confidence short-term motion reference. Leveraging the physical characteristics of inertial sensors—minimal integral drift and extremely high relative measurement accuracy within extremely short time windows (e.g., milliseconds or seconds)—the system can obtain a stable and continuous rigid short-time reference trajectory. This reference trajectory, serving as a physical reference scale, can be aligned with low-frequency, ultra-wideband observation signals in both the time and spatial domains. This provides the necessary mathematical constraints and physical prerequisites for subsequently extracting constant or slowly varying spatial system biases (such as antenna installation errors and multipath interference) from complex radio signals. Furthermore, depending on the specific application requirements, the IMU can also be implemented using micro-electro-mechanical systems (MEMS) sensors to adapt to different types of controlled vehicles, such as drones, mobile robots, or indoor positioning terminals.

[0017] In some embodiments of this application, spatial observations can be acquired through an ultra-wideband (UWB) communication unit. An UWB communication unit typically includes an UWB transceiver chip, a radio frequency (RF) front-end circuit, and an antenna array (such as a dual-antenna or multi-antenna array). In actual operation, the UWB communication unit transmits and receives electromagnetic wave signals with extremely narrow pulse widths (typically on the nanosecond scale) to achieve communication and ranging between the carrier and external known location nodes (such as UWB base stations or anchor points).

[0018] Specifically, the ultra-wideband communication unit can acquire distance information based on the time of flight (ToF) of the signal, and use the phase difference or time difference of arrival of the signal received by the antenna array to calculate spatial observations such as PDOA and / or AoA.

[0019] Because ultra-wideband communication units are affected by antenna installation position deviations, carrier obstruction, and environmental multipath reflections during actual deployment, the raw spatial observations output by ultra-wideband communication units often contain systematic spatial biases.

[0020] The acquisition unit 120 is the data input source for the spatial bias processing device 100, used to collect various types of raw observation data. In physical implementation, the acquisition unit 120 can be a processor peripheral, application-specific integrated circuit, or data acquisition card with a data communication interface (such as Serial Peripheral Interface (SPI), Inter-Integrated Circuit (I2C), Universal Asynchronous Receiver / Transmitter (UART)).

[0021] The acquisition unit 120 is connected to the inertial measurement unit and is used to acquire high-frequency output inertial measurement data. The inertial measurement data may include angular velocity data collected by the gyroscope and specific force data collected by the accelerometer. The acquisition unit 120 can continuously trigger the acquisition of inertial measurement data according to the system's preset sampling frequency (e.g., 100Hz to 1000Hz) to provide a reference for subsequent processing.

[0022] The acquisition unit 120 is connected to the ultra-wideband communication unit and acquires spatial observations by receiving ultra-wideband signals. Spatial observations are measurements reflecting the spatial geometric relationship between the carrier and external reference nodes, including but not limited to phase difference, angle of arrival, time difference of arrival (TDOA), or ranging information. During the acquisition process, the acquisition unit 120 converts the raw physical layer data calculated by the ultra-wideband communication unit into standardized observation vectors defined uniformly by the system.

[0023] In addition, the acquisition unit 120 can also perform time synchronization of multi-source data. Since the sampling frequencies of inertial measurement data and spatial observations may be different and asynchronous, the acquisition unit 120 is configured to mark and align the two types of data received, ensuring that the synchronized inertial measurement data and spatial observations are sent to the modeling unit 130 and the reference construction unit 140 under the same time reference, thereby reducing errors caused by inconsistent time axes.

[0024] The modeling unit 130 is connected to the acquisition unit 120 and is used to construct the mathematical model required by the spatial deviation processing device 100 for state estimation, and to construct an augmented error state vector including the carrier navigation error state and the spatial deviation state.

[0025] Specifically, the augmented error state vector constructed by the modeling unit 130 in this embodiment is based on the error state based on inertial recursion, and additionally extends it with a system bias term that reflects the characteristics of radio observation, thereby integrating multiple error sources with different properties into a unified mathematical framework for processing.

[0026] In the embodiments of this application, the augmented error state vector not only includes the carrier navigation error state (such as position error, velocity error, attitude error, etc., reflecting the deviation of the carrier's motion trajectory), but also explicitly introduces the spatial deviation state.

[0027] Spatial bias state is used to quantitatively characterize the spatial bias state generated during the propagation and reception of ultra-wideband signals in space, such as the physical installation deviation of ultra-wideband antennas, cable delay deviation, and slow-varying multipath interference deviation exhibited under specific environments.

[0028] In the specific modeling process, based on the physical characteristics of the deviations in the actual application scenario, the modeling unit 130 can adopt at least one or a combination of random walk models, first-order Gaussian-Markov processes, constant models, polynomial models, or spatial location-based mapping models. For example, when the system operates in a scenario with drastic changes in ambient temperature, using a first-order Gaussian-Markov process can better describe the correlation of link delay; while when the system focuses on long-term static installation errors, using a constant model can improve the convergence accuracy of parameter estimation.

[0029] The reference construction unit 140 is connected to the acquisition unit 120 and the modeling unit 130, and is used to provide a high-confidence spatiotemporal reference for the spatial deviation processing device 100 using inertial measurement data.

[0030] Specifically, the reference construction unit 140 receives inertial measurement data (such as angular velocity and specific force data) provided by the acquisition unit 120, and uses preset kinematic equations or integration algorithms (such as strapdown inertial navigation algorithm, dead reckoning algorithm, IMU prediction algorithm and at least one of artificial intelligence-based pose estimation model) to recursively deduce the nominal pose of the carrier in real time.

[0031] The nominal pose of the carrier is used to characterize the uncorrected spatial observational predictions of the carrier's position, velocity, and attitude calculated by the inertial measurement unit (IMU). Because the IMU operates within an extremely short time window (…), The nominal pose trajectory of the carrier obtained by this recursion has high measurement accuracy and the integral drift effect has not yet appeared. Therefore, the nominal pose trajectory of the carrier has good smoothness and local accuracy, which constitutes a rigid short-time reference for peeling off deviation.

[0032] The baseline building unit 140 also establishes the error covariance matrix corresponding to the augmented error state vector. The error covariance matrix is ​​a mathematical operator used to quantitatively describe the current estimation accuracy and the correlation between various state variables. The error covariance matrix reflects the confidence level and coupling relationship of each component (including pose and deviation) in the augmented error state vector at each time step.

[0033] The reference construction unit 140 calculates the uncertainty change of the system state over time using a time-domain propagation model. The update process may include: constructing a state transition matrix based on the partial derivatives of the carrier kinematic model, and combining it with a preset process noise matrix to predict and update the covariance of the augmented error state vector over time. In the embodiments of this application, the reference construction unit 140 constructs the error characteristics of the inertial measurement unit so that, during the update process, the growth of the covariance corresponding to the carrier navigation error state is limited to a small range, while the covariance corresponding to the spatial deviation state remains at a large order of magnitude.

[0034] The reference construction unit 140 ensures that the spatial deviation processing device 100 can attribute most of the error components in the residual to the spatial deviation state rather than the motion state of the carrier itself, thereby providing a physical reference for the directional absorption and precise stripping of the spatial deviation.

[0035] The residual calculation unit 150 is connected to the acquisition unit 120, the modeling unit 130 and the benchmark construction unit 140, and is used to calculate the deviation between the predicted value and the actual observed data.

[0036] The residual calculation unit 150 obtains the predicted value of the spatial observation quantity through a preset observation model based on the nominal pose of the carrier obtained by the baseline construction unit 140 and the spatial deviation state of the modeling unit 130.

[0037] The observation model is a mathematical model describing the geometric mapping between the nominal pose of the carrier and the spatial observations, and explicitly superimposing the current spatial bias state. For example, when UWB uses ranging mode, the observation model can be the Euclidean distance formula in three-dimensional space, used to map the nominal pose of the carrier at the current moment to the theoretical geometric distance value that the UWB antenna should receive. When UWB uses AoA or PDOA mode, the observation model corresponds to the corresponding angular projection or interferometric geometry. Furthermore, since the actual spatial observations contain constant or slowly varying biases, the embodiments of this application also superimpose the estimated spatial bias state at the current moment into the observation model to construct a complete and accurate prediction value.

[0038] The residual calculation unit 150 performs subtraction between the spatial observations actually acquired by the acquisition unit 120 and the predicted values ​​to construct residuals. Since the nominal pose of the carrier is constrained by the reference construction unit 140 within a short time window, its estimation error is extremely small. Therefore, the constructed residuals can sensitively reflect the spatial bias state in the ultra-wideband observation signal that has not yet been fully compensated.

[0039] The update unit 160 is connected to the modeling unit 130, the baseline construction unit 140 and the residual calculation unit 150, and is used to correct the spatial deviation state based on the residual and the current system uncertainty state.

[0040] Specifically, update unit 160 is configured to execute the following logic: First, the gain allocation coefficients are determined. The update unit 160, combining the state uncertainty reflected in the error covariance matrix and the preset observation noise level, calculates a gain vector for allocating the residuals. The gain vector characterizes the tendency to determine the source of error in the residuals. Since the benchmark construction unit ensures that the covariance corresponding to the carrier navigation error state is much smaller than the covariance corresponding to the spatial deviation state, the gain vector tends to map the residuals to the more uncertain spatial deviation state.

[0041] Next, the correction amount for the spatial bias state is determined. The update unit 160 fuses the residual from the residual calculation unit 150 with the gain vector (e.g., by weighted product calculation) to obtain the correction amount for the spatial bias state.

[0042] Finally, the update unit 160 performs superposition compensation on the spatial deviation state based on the obtained correction amount of the spatial deviation state. Through this directional correction based on covariance constraints, the update unit 160 can ensure that the spatial deviation state contained in the spatial observation is accurately stripped and absorbed into the spatial deviation state without affecting the nominal pose of the carrier, thereby achieving closed-loop calibration of the spatial deviation.

[0043] Figure 1 The provided spatial deviation processing device 100 achieves explicit modeling of the spatial deviation state by constructing an augmented error state vector that includes the carrier navigation error state and the spatial deviation state. By recursively deriving the nominal pose of the carrier using inertial measurement data and updating the error covariance matrix to ensure that the covariance corresponding to the carrier navigation error state is less than the covariance corresponding to the spatial deviation state, the spatial deviation processing device 100 can establish a high-confidence short-term motion reference. The update unit 160 can orient the observation error to a correction amount for the spatial deviation state based on the residuals, thereby achieving online removal of the spatial deviation state hidden in the spatial observations. Figure 1The provided spatial deviation processing device 100 can reduce the impact of spatial deviation on the carrier's pose state, reduce navigation drift caused by antenna installation deviation or multipath interference, and improve positioning and attitude stability.

[0044] against Figure 1 In some embodiments of the spatial deviation processing apparatus 100 shown in this application, the modeling unit 130 is configured to construct a spatial deviation state through a random walk model to characterize the changing characteristics of the spatial deviation state.

[0045] Specifically, a random walk model is a stochastic process that models the derivatives of state variables as stationary white noise. In this embodiment, the modeling unit 130 uses a random walk model to describe the dynamic characteristics of the spatial deviation state evolving over time.

[0046] The random walk model can be characterized as follows: the change in spatial bias state between adjacent sampling times is driven by a process noise with a mean of zero and a pre-defined covariance.

[0047] Installation deviations, link delays, and multipath interference in the environment of ultra-wideband systems typically exhibit constant or slowly varying characteristics over time. Using a random walk model, modeling unit 130 can accurately match this physical characteristic, transforming spatial interference, which is considered random noise in relevant schemes, into explicit state variables with correlation over time.

[0048] Compared to treating the spatial deviation state as an immutable constant model, the random walk model allows the spatial deviation state to have a certain degree of freedom during the filtering process, so that the update unit 160 can continuously absorb the effective components in the residual, thereby dynamically capturing and correcting the subtle fluctuations of the deviation during the carrier's movement.

[0049] The random walk model provides a mathematical basis for the evolution logic of the spatial deviation state in the error covariance matrix through the preset process noise covariance, ensuring that the spatial deviation state remains relatively stable in a short period of time and converges to the true deviation value in a long period of time, thereby supporting the device to strip away the spatial deviation state online and perform accurate calibration.

[0050] against Figure 1 In some embodiments of the spatial deviation processing apparatus 100 shown in this application, the reference construction unit 140 is configured to obtain the nominal pose of the carrier based on inertial measurement data using a strapdown inertial navigation algorithm. The nominal pose of the carrier includes at least one of position, velocity, and attitude.

[0051] Specifically, the reference construction unit 140 receives inertial measurement data collected by the acquisition unit 120, updates its data using the angular velocity data, and performs an integral recursion of velocity and position in the navigation coordinate system in combination with the specific force data. In this way, the reference construction unit 140 can calculate the nominal pose of the carrier in real time. The nominal pose of the carrier specifically includes at least one of the carrier's position in space, motion velocity, and attitude angles (such as heading angle, pitch angle, and roll angle).

[0052] To enable those skilled in the art to more clearly understand how the reference construction unit 140 establishes the covariance constraint relationship between the carrier navigation error state and the spatial deviation state, the mechanism of the constraint relationship is illustrated below with an example derivation using the strapdown inertial navigation algorithm: In the benchmark building unit 140 within an extremely short time window ( Within this range, the evolution of inertial measurement data follows the strapdown inertial navigation differential equation:

[0053]

[0054]

[0055]

[0056] in, The position error is represented by a 3×1 dimensional vector; The derivative of the position error with respect to time is represented by a 3×1 dimensional vector; The error representing the velocity is a 3×1 dimensional vector; The physical meaning is: the rate of change of position error is equal to the velocity error.

[0057] The derivative of the velocity error with respect to time is represented by a 3×1 dimensional vector; The direction cosine matrix (attitude matrix) represents the distance from the carrier coordinate system b to the navigation coordinate system n. It is a 3×3 matrix. b represents the carrier coordinate system, which is the coordinate system fixed on the carrier (such as a mobile phone, vehicle, or robot). n represents the navigation coordinate system, which is usually chosen as "East-North-Up (ENU)" or "North-East-Down (NED)," which is the reference coordinate system fixed on the earth. This represents the antisymmetric matrix corresponding to the specific force vector measured in the b system; This represents the zero bias error of the accelerometer (true zero bias - nominal zero bias), in units of... ; This represents the random white noise of the accelerometer, measured in units of... ; The physical meaning is that the rate of change of velocity error is caused by three factors: gravity / specific force projection deviation caused by attitude error, constant acceleration error caused by accelerometer zero bias error, and random white noise of accelerometer.

[0058] The derivative of the attitude error with respect to time is represented by a 3×1 dimensional vector, with units of . ; The zero-bias error of the gyroscope (true zero-bias - nominal zero-bias) is represented by a 3×1 dimensional vector, with units of: ; This represents the random white noise of the gyroscope, measured in units of... ; The physical meaning is that the rate of change of attitude error is caused by two factors, including attitude drift caused by gyroscope bias error and random white noise of gyroscope.

[0059] The derivative of the spatial deviation state with respect to time is represented by a 3×1 dimensional vector, with units of . ; This represents the spatial bias state, a 3×1 dimensional vector with units consistent with those of spatial observations. The spatial bias state can include installation bias, slow-varying multipath components, etc.

[0060] The noise driving the spatial bias state change is represented by a 3×1 dimensional vector, with units of spatial bias state per time (e.g., ...). ).

[0061] The physical meaning is that the spatial deviation state hardly changes with time. Because it is driven by extremely small noise, even small changes are very slow.

[0062] The establishment of reference building block 140 describes the characteristics of how the errors in inertial measurement data and the deviations in spatial observations change over time. From... As can be seen from the equation, the attitude error does not have a self-feedback term, and and There is an integral relationship between them. Therefore, inertial measurement data within an extremely short time window ( The spatial bias processing device 100 achieves extremely high accuracy within the time frame while maintaining almost no change in the spatial bias state. Therefore, the spatial bias processing device 100 can complement the short-term high-precision characteristics of spatial observations and inertial measurement data, thus providing a physical benchmark for the directional absorption and precise removal of spatial bias.

[0063] Those skilled in the art should understand that the derivation based on the strapdown inertial navigation algorithm is only intended to illustrate, from a mechanistic perspective, that the increment of the covariance corresponding to the carrier navigation error state is much smaller than the uncertainty of the spatial domain deviation, which is an inherent characteristic of inertial measurement data and does not depend on a specific algorithm form.

[0064] against Figure 1 In some embodiments of the spatial bias processing apparatus 100 shown in this application, the update unit 160 specifically achieves the directional allocation of errors by calculating the joint gain matrix.

[0065] Specifically, the update unit 160 calculates the joint gain matrix by combining the current augmented error state vector and the uncertainty represented by the error covariance matrix.

[0066] The joint gain matrix is ​​a set of adaptive weighting coefficients used to measure the distribution ratio of the residual among different error state terms (i.e., vehicle navigation error state and spatial deviation state). The joint gain matrix is ​​an improved Kalman gain matrix. In this embodiment, since the augmented error state vector includes both the vehicle navigation error state and the spatial deviation state, the joint gain matrix can adaptively adjust the correction weights of the residual on the vehicle navigation error state and the spatial deviation state within the same correction period based on the uncertainty differences reflected in the error covariance matrix. This ensures that while maintaining the stability of the pose reference, the error is accurately absorbed into the deviation state term.

[0067] Subsequently, the update unit 160 determines the correction amount for the spatial deviation state based on the joint gain matrix and the residual output by the residual calculation unit 150. Since the pose covariance in the error covariance matrix is ​​much smaller than the deviation covariance, the joint gain matrix automatically adjusts its internal weights so that the error components in the residual are mainly converted into the correction amount for the spatial deviation state, while the correction for the carrier pose state remains within a very small range.

[0068] By introducing a joint gain matrix, the spatial bias processing device 100 can correct the proportional relationship between observation uncertainty and state prediction uncertainty, thereby ensuring that the update process of spatial bias state has a reliable mathematical basis and further improving the accuracy of spatial bias stripping.

[0069] Furthermore, when determining the correction amount for the spatial deviation state, the update unit 160 specifically performs the following processing steps: First, the update unit 160 constructs a Jacobian matrix based on the nominal pose of the carrier, the spatial bias state, and the preset observation model. The Jacobian matrix is ​​used to characterize the degree of influence of changes in the augmentation error state vector on the predicted values ​​of the spatial observations.

[0070] Subsequently, the update unit 160 calculates the joint gain matrix based on the Jacobian matrix, the error covariance matrix, and the observation noise covariance corresponding to the spatial observations. During this process, because the system satisfies the constraint that the covariance corresponding to the spatial deviation state is greater than the covariance corresponding to the carrier navigation error state, the joint gain matrix exhibits an asymmetric distribution characteristic mathematically. Specifically, because the confidence level corresponding to the carrier navigation error state is high (covariance is extremely small), the component corresponding to pose correction in the joint gain matrix is ​​suppressed, while the component corresponding to deviation correction is strengthened.

[0071] Finally, the update unit 160 determines the correction amount for the spatial bias state based on the calculated joint gain matrix and the residuals output by the residual calculation unit. Through the directional mapping effect of the joint gain matrix, the observation error contained in the residuals is mainly transformed into a compensation value for the spatial bias state, thereby achieving accurate extraction of the spatial bias state in the system.

[0072] As an optional proof method, the following derivation is illustrated with a typical mathematical model. However, those skilled in the art should understand that this does not constitute a limitation on the implementation of the update unit 160.

[0073] Spatial observations can be characterized as:

[0074] in, For spatial observations, that is, the data actually received by the spatial deviation processing device 100, such as the measured distance, PDOA phase difference, and AoA angle; These are the predicted values ​​of spatial observations, calculated using a geometric model h (e.g., triangulation formula) based on the currently estimated pose. This represents the true value of the IMU-related state vector, which can specifically include the carrier's motion state such as position, velocity, and attitude. To observe the noise, in Inside, The mean is 0. The covariance matrix is .

[0075] The physical meaning is: the value of the spatial observation obtained through UWB is equal to the geometric value determined by the pose + fixed system bias + random noise.

[0076] The augmented error state vector can be characterized as:

[0077] in, For the augmented error state vector; This is the transpose of the error of the IMU-related state vector; This is the transpose of the systematic bias of UWB.

[0078] For augmented error state vector Taking the partial derivative, the resulting Jacobian matrix H can be characterized as:

[0079] in, Its physical meaning is: the change of 1 unit in the covariance (position / attitude) corresponding to the carrier navigation error state is the change in the spatial observation value, the magnitude of which is determined by the geometric topology; Its physical meaning is: the change in the spatial observation value when the spatial deviation value changes by 1 unit.

[0080] because and satisfy: ,therefore It is an identity matrix.

[0081] Therefore, the physical meaning of the Jacobian matrix H is: the covariance corresponding to the carrier navigation error state affects the spatial observation value through complex geometric topological relationships (such as triangulation and angular projection), while the spatial deviation state can directly and one-to-one affect the spatial observation value.

[0082] Based on spatial observations, the augmented error state vector, and the Jacobian matrix, the residual can be characterized as:

[0083] in, The estimated value of the spatial deviation state can be characterized as:

[0084] in The nominal pose can include three components: position, velocity, and attitude, and is obtained recursively from the reference building unit 140. The estimated value of the spatial deviation state is obtained recursively from the reference building unit 140.

[0085] Will and Substituting the expression The residual can be further written as:

[0086] Under the linearized approximation The residual can be approximated as:

[0087] It can be seen that the residuals contain both the error of the IMU-related state vector and the system bias of the UWB.

[0088] Furthermore, the new information covariance matrix can be characterized as:

[0089] in, For the new information covariance, For Jacobian matrices, For random noise The covariance matrix; The covariance matrix of the augmented error state vector contains the covariance corresponding to the vehicle navigation error state. Covariance corresponding to the deviation error and the cross-covariance of the two. and .

[0090] Expand We can obtain:

[0091] Furthermore, we have:

[0092] The mathematical mechanism by which the increment of the covariance corresponding to the carrier navigation error state is much smaller than the uncertainty of the spatial domain deviation has been explained in detail in the embodiments of this application. Accordingly, this characteristic can be characterized as follows:

[0093] Therefore, within an extremely short time window ( Within this range, the covariance corresponding to the carrier navigation error state approaches 0, i.e. Meanwhile, the covariance and bias corresponding to the carrier navigation error state are independent of each other in a short period of time, and their correlation approaches 0, i.e., the cross-covariance. and satisfy: , .

[0094] Constraints , and Substitution From the expression, we get:

[0095] The physical meaning is: within an extremely short time window ( Within this range, the uncertainty of the residuals almost entirely stems from spatial bias and observation noise, while remaining largely unrelated to the covariance corresponding to the carrier navigation error state.

[0096] Based on the covariance of new information The derivation results show that the update unit 160 will produce a significant asymmetric allocation effect when calculating the joint gain matrix.

[0097] Specifically, the joint gain matrix can be characterized as:

[0098] in For pose gain, This is the bias gain.

[0099] Furthermore, pose gain satisfy:

[0100] Deviation gain satisfy

[0101] Constraints , and Substituting, we get:

[0102]

[0103] because Much greater than observation noise ,therefore .

[0104] Therefore, under the premise of extremely high accuracy within a short time window of the IMU, the joint gain matrix is ​​forcibly split into 0s and 1s, meaning that the pose error is rarely corrected, and the residual is almost entirely absorbed by the deviation. By modeling the augmented error state vector and "asymmetric collapse" of the joint gain matrix, the decoupling of the vehicle navigation error and the spatial deviation is achieved, thereby improving the positioning accuracy.

[0105] To illustrate the decoupling effect of the spatial deviation processing device 100 in this application embodiment on carrier navigation error and spatial deviation, this application embodiment is described in detail below. Figure 2 A schematic diagram illustrating the decoupling and convergence effects of spatial bias is provided.

[0106] exist Figure 2 In the diagram, the horizontal axis represents time (in seconds), and the vertical axis represents the carrier navigation error / airspace deviation (in meters). Figure 2 It contains three curves: the dashed line represents the true reference for airspace deviation (0.8m preset in this example), the solid line represents the estimated airspace deviation output by the airspace deviation processing device 100, and the dotted line represents the carrier navigation error.

[0107] from Figure 2 The dynamic evolution process shows that in the initial stage, due to the unknown initial spatial bias, the estimated spatial bias (solid line) starts from 0.0 meters. With the continuous input of spatial observations, within 2 to 3 seconds, the estimated spatial bias exhibits a smooth and rapidly rising convergence curve, and then accurately and stably fits near the true reference (dashed line), maintaining only slight and reasonable fluctuations with observation noise in the subsequent running time.

[0108] Meanwhile, the carrier navigation error (dotted line) remained close to the horizontal axis of 0.0m throughout the entire convergence and stable operation process, almost appearing as a straight line. This experimental result directly verifies the effectiveness of the "directional stripping" mechanism in the embodiments of this application.

[0109] The spatial observations are effectively absorbed into the spatial deviation estimate (solid line), causing it to converge rapidly to the true value; while the carrier navigation error (dotted line) is not disturbed or contaminated by the spatial deviation convergence process due to the strict limitation of the inertial reference. Figure 2 This demonstrates that the present application can achieve the technical effect of online stripping of airspace deviation and ensuring decoupling of carrier navigation error from airspace deviation estimation.

[0110] To further improve the robustness of the spatial deviation processing device 100 in various complex dynamic scenarios, especially to solve the problems of estimation divergence and parameter jump that are prone to occur in related schemes in static or low dynamic scenarios, in this embodiment of the application, the spatial deviation processing device 100 further includes an excitation determination unit.

[0111] Specifically, the excitation determination unit is connected to the acquisition unit 120 and the update unit 160. The excitation determination unit is used to monitor and evaluate in real time whether the current motion state of the carrier meets the minimum dynamic conditions required for the observed spatial deviation state. Spatial deviations (such as physical installation deviations of ultra-wideband antennas, phase center deviations, etc.) are usually manifested as fixed or slowly varying parameters relative to the carrier coordinate system. These deviation parameters often require the carrier to generate a certain degree of angular or linear motion in space in order to decouple the observation residuals from the covariance corresponding to the carrier's own carrier navigation error state.

[0112] The excitation determination unit is configured to first acquire the raw inertial measurement data provided by the acquisition unit 120. The inertial measurement data includes high-frequency angular velocity signals and specific force signals that reflect the microscopic motion characteristics of the carrier. The excitation determination unit determines the current motion excitation level by processing the raw inertial measurement data in real time.

[0113] In one possible implementation, the excitation determination unit maintains a sliding time window and calculates statistical characteristics of the angular velocity and specific force data within that window. For example, the excitation determination unit can calculate the triaxial combined variance of the angular velocity or the energy integral of the specific force data after deducting the gravitational component. These statistical characteristics can objectively reflect whether the carrier is in a sufficient state of rotation or acceleration at the current moment. The level of motion excitation can be defined as a comprehensive index that quantifies the intensity of the carrier's motion and the richness of its motion dimensions.

[0114] After obtaining the motion excitation level, the excitation determination unit compares it with a preset threshold in real time. The preset threshold can be experimentally calibrated based on the noise floor of the inertial measurement unit, the resolution of the spatial observations, and the expected convergence accuracy of the system. The preset threshold represents the minimum dynamic condition required to make the spatial deviation state observable. When the excitation determination unit determines that the current motion excitation level is below the preset threshold, it means that the carrier is currently in a stationary, quasi-stationary, or extremely low-dynamic operating state. In this case, the excitation determination unit can send a control signal to the update unit 160 to prohibit the use of the currently calculated correction amount to update the spatial deviation state.

[0115] It should be understood that when the motion excitation level is lower than the preset threshold, although the residual calculation unit 150 can still output the residual and the update unit 160 can also calculate the correction amount for the spatial deviation state, the excitation determination unit ensures that the spatial deviation state can be maintained at its previously converged, high-confidence value by cutting off the update channel, and is not contaminated by the current invalid observations. This enhances the performance of the spatial deviation processing device 100 in scenarios such as drone hovering, robot standby, or vehicle stopping at a red light, and avoids the drift of the spatial deviation state.

[0116] Furthermore, the excitation determination unit can dynamically adjust the preset threshold based on the current uncertainty of the spatial deviation state. For example, in the early stages of system startup, when the spatial deviation state has not yet converged, the threshold can be appropriately lowered to allow for initial attempts to capture the deviation; while when the system enters a stable operating period and the spatial deviation state has converged significantly, the threshold can be increased to allow for further fine-tuning when the motion excitation is sufficient, thereby ensuring operational stability.

[0117] By utilizing inertial measurement data, the excitation determination unit introduces a dynamic-aware adaptive update mechanism for the spatial deviation processing device 100, thereby improving the robustness and reliability of the spatial deviation processing device 100 in complex environments.

[0118] In practical radio positioning and navigation scenarios, ultra-wideband signals are highly susceptible to multipath interference. Multipath interference occurs when electromagnetic waves are reflected, refracted, or diffracted by buildings, walls, the ground, or other obstacles during propagation, resulting in the receiver receiving signals from multiple paths, or directly receiving signals from non-line-of-sight (NLOS) paths. At the observational level, multipath interference manifests as significant jumps or abrupt values ​​in spatial observations that do not conform to physical laws. Blindly adopting interfered observation data for state updates will lead to erroneous shifts in the spatial bias state, thereby contaminating the nominal pose of the carrier and potentially causing the positioning and navigation system to collapse.

[0119] In order to reduce the impact of multipath interference on observation data, in this embodiment of the application, the residual calculation unit 150 is configured to perform a consistency check on the residuals in order to identify and filter abnormal observation data affected by multipath interference.

[0120] Specifically, after receiving the spatial observations acquired by the acquisition unit 120, the residual calculation unit 150 first combines the nominal pose of the carrier recursively obtained by the reference construction unit 140 with the spatial bias state currently maintained by the modeling unit 130, and generates a predicted value of the spatial observation through a preset observation model. This predicted value represents the theoretical value that the spatial bias processing device 100 expects to observe based on the existing, high-confidence motion trajectory and the identified system biases.

[0121] Subsequently, the residual calculation unit 150 compares the actual acquired spatial observations with the predicted values ​​of the spatial observations to construct residuals. Under normal operation and favorable environmental conditions, the residuals mainly contain a small amount of random white noise. However, when the carrier enters an area with severe multipath interference, the propagation path lengthens due to reflected signals, causing abnormal deviations in the spatial observations from the laws of physical motion. At this time, the constructed residual values ​​increase rapidly, exceeding the normal statistical distribution range.

[0122] The residual calculation unit 150 is configured to perform a consistency check. The consistency check uses statistical methods to evaluate whether the currently observed residuals conform to the system's preset probability distribution characteristics. In the embodiments of this application, a high-confidence short-term motion benchmark is established through the benchmark construction unit 140, and the estimation error of the carrier's nominal pose within a short time window is extremely small, meaning that the magnitude of the residuals should be strictly limited. The residual calculation unit 150 sets a threshold for the residuals using a preset consistency check algorithm (e.g., statistical methods such as Mahalanobis distance test or chi-square test). The threshold can be dynamically calculated based on the error covariance matrix and the observation noise covariance, reflecting the tolerance limit for observation errors at the current moment.

[0123] If the residuals exceed the consistency check threshold, the residual calculation unit 150 determines that the current spatial observations are abnormal observation data affected by multipath interference. Since physical-level systematic biases (such as installation biases) are usually constant or slowly changing, their impact on the residuals is stable and can be tracked continuously; however, observational anomalies caused by multipath interference are usually sudden, abrupt, and do not conform to the actual dynamic constraints of the carrier. Therefore, the residual calculation unit 150 can distinguish between random gross errors introduced by the environment and inherent systematic biases of the equipment.

[0124] Figure 3 This diagram illustrates a comparison of the effects of the residual calculation unit 150 in this embodiment of the invention in identifying and eliminating multipath interference through a consistency check. In this diagram, the horizontal axis represents time (in seconds), and the vertical axis represents the estimated spatial deviation (in meters). The black dashed line represents the preset 0.8m true baseline; the dotted line represents the comparison curve without consistency check processing; and the solid line represents the estimated curve after incorporating the consistency check scheme described in this application.

[0125] like Figure 3As shown, in the first 6 seconds of system operation, both curves converged normally to near the true reference value of 0.8m. Between 6 and 8 seconds, a strong multipath interference with an amplitude of approximately 3.5m was introduced. Observing the dotted line, it can be seen that, due to the failure to identify abnormal observation data, update unit 160 incorrectly absorbed the huge residual caused by the interference into the state variable, causing the spatial bias estimate to spike instantaneously, reaching a maximum of approximately 4.18m, resulting in severe estimation distortion. Furthermore, after the interference ended (8 seconds later), the curve required a long adjustment period to slowly return to the reference value, severely impacting the system's real-time performance and accuracy.

[0126] However, the solid line after incorporating the consistency check scheme exhibits extremely strong robustness during interference. When multipath interference occurs, the residual calculation unit 150 compares the actual acquired spatial observations with the predicted values ​​calculated based on the nominal pose of the carrier, identifying that the current residual significantly exceeds the preset consistency check threshold. At this point, the system determines that the observation frame is affected by multipath interference and belongs to abnormal observation data, and actively triggers a filtering mechanism (such as discarding the observation frame or increasing the observation noise covariance).

[0127] Throughout the interference period, the solid line remained consistently at approximately 0.82m without any abrupt changes due to the interference signal, successfully traversing the interference zone. This comparative result demonstrates that the embodiments of this application can effectively identify and eliminate abnormal observation data affected by multipath interference, ensuring the stability of the spatial deviation state under complex electromagnetic environments, thereby avoiding contamination of the navigation system caused by abnormal observations.

[0128] In some embodiments of this application, after identifying abnormal observation data, the residual calculation unit 150 is configured to send a signal to the update unit 160 to remove the identified abnormal observation data and prevent the abnormal observation data from participating in the correction of the airspace deviation state.

[0129] In some embodiments of this application, the residual calculation unit 150 is configured to notify the increase of the observation noise covariance corresponding to the abnormal observation data, thereby mathematically reducing the weight of the abnormal observation data in the fusion update, so that its influence on the state variables approaches zero.

[0130] By performing consistency checks on the residuals to eliminate abnormal observation data, invalid information caused by multipath interference can be reduced, thereby improving the purity of the spatial deviation state estimation, maintaining the continuity and accuracy of the nominal pose of the carrier, and enhancing the environmental adaptability of the high-precision positioning and navigation system.

[0131] Figure 4 A flowchart of a spatial deviation processing method provided in an embodiment of this application is shown, including steps S410 to S450.

[0132] Step S410: Acquire the inertial measurement data output by the inertial measurement unit, and acquire spatial observations through the ultra-wideband communication unit.

[0133] Specifically, inertial measurement data is extracted from the inertial measurement unit by the acquisition unit 120, and radio signals transmitted by an external reference node are received by the ultra-wideband communication unit. Spatial observations are then acquired based on the radio signals. After synchronous processing of the inertial measurement data and spatial observations, raw data input is provided for subsequent estimation.

[0134] Step S420: Construct an augmented error state vector that includes the carrier navigation error state and the spatial deviation state.

[0135] Specifically, an augmented error state vector is constructed, which includes not only the carrier navigation error state (such as position error, velocity error, and attitude error) but also explicitly introduces the spatial deviation state. The spatial deviation state is used to quantitatively describe the spatial deviations generated by the ultra-wideband system in practical applications, such as antenna installation deviations or slowly varying multipath interference. Step S420 transforms the spatial interference into mathematically estimable state variables, thus establishing a model foundation for online deviation removal.

[0136] Step S430: Recursively calculate the nominal pose and update the error covariance matrix.

[0137] Specifically, based on inertial measurement data, the nominal pose of the carrier is calculated using a pre-defined recursive logic (such as an integral algorithm). Because the inertial measurement unit has extremely high relative accuracy within a short time window, the nominal pose accurately reflects the carrier's true motion state. Simultaneously, the error covariance matrix corresponding to the augmented error state vector is predicted and updated. During this process, the spatial deviation processing device 100, through the configuration of noise parameters, limits the covariance corresponding to the carrier's navigation error state to a very small range, while maintaining the covariance corresponding to the spatial deviation state at a relatively large level. This asymmetric covariance constraint environment ensures the rigidity of the pose reference, allowing subsequent observation residuals to act directionally on the deviation term.

[0138] Step S440: Obtain the predicted values ​​of the spatial observations and construct the residuals.

[0139] Specifically, the spatial bias processing device 100 uses the nominal pose of the carrier at the current moment and the spatial bias state estimated at the previous moment, and substitutes them into a preset observation model for mapping to calculate the predicted value of the spatial observation. Then, the actual acquired spatial observation is subtracted from the predicted value to construct the residual. Because the nominal pose of the carrier is strictly constrained by the inertial reference, the constructed residual can sensitively capture the systematic error components in the spatial observation that have not yet been absorbed.

[0140] Step S450: Determine the correction amount for the airspace deviation state and update the airspace deviation state.

[0141] Specifically, the spatial deviation processing device 100 calculates the gain allocation weights for the augmented state based on the residuals and the error covariance matrix under strict constraints of inertial accuracy. According to the gain allocation weights, the spatial deviation processing device 100 maps the observation errors contained in the residuals into corrections for the spatial deviation state. Subsequently, the spatial deviation processing device 100 uses these corrections to update the spatial deviation state maintained in the modeling unit 130. Through step S450, the spatial deviation processing device 100 real-time decouples and compensates for the spatial deviation state, thereby decoupling the carrier pose estimation from the spatial deviation estimation and ensuring the accuracy of the navigation system during long-term operation in complex environments.

[0142] This application also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program. When executed by a processor, the computer program implements the spatial offset processing method in any embodiment of this application.

[0143] It should be noted that the technical solutions described in this application can be combined arbitrarily without conflict.

[0144] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

Claims

1. A spatial deviation processing device, characterized in that, include: The acquisition unit acquires inertial measurement data and spatial observations. A modeling unit, connected to the acquisition unit, is used to construct an augmented error state vector that includes the carrier navigation error state and the spatial deviation state. The baseline construction unit is connected to the acquisition unit and the modeling unit to recursively deduce the nominal pose of the carrier based on the inertial measurement data and update the error covariance matrix corresponding to the augmented error state vector, so that the covariance corresponding to the carrier navigation error state is less than the covariance corresponding to the spatial deviation state. The residual calculation unit is connected to the acquisition unit, the modeling unit and the benchmark construction unit to obtain the predicted value of the spatial observation quantity through a preset observation model based on the nominal pose of the carrier and the spatial deviation state, and to construct the residual based on the spatial observation quantity and the predicted value of the spatial observation quantity. An update unit, connected to the modeling unit, the benchmark construction unit, and the residual calculation unit, determines the correction amount of the spatial deviation state based on the residual and the error covariance matrix, and updates the spatial deviation state based on the correction amount of the spatial deviation state.

2. The spatial deviation processing device according to claim 1, characterized in that, The modeling unit is configured to construct the spatial deviation state through a random walk model to characterize the changing characteristics of the spatial deviation state.

3. The spatial deviation processing device according to claim 1, characterized in that, The reference construction unit is configured to obtain the nominal pose of the carrier using a strapdown inertial navigation algorithm based on the inertial measurement data. The nominal pose of the carrier includes at least one of position, velocity, and attitude.

4. The spatial deviation processing device according to claim 1, characterized in that, The update unit is configured as follows: Calculate the joint gain matrix based on the augmented error state vector and the error covariance matrix; The correction amount for the spatial bias state is determined based on the joint gain matrix.

5. The spatial deviation processing device according to claim 4, characterized in that, The update unit is also configured to: Based on the nominal pose of the carrier, the spatial deviation state, and the preset observation model, construct the Jacobian matrix; The joint gain matrix is ​​calculated based on the Jacobian matrix, the error covariance matrix, and the observation noise covariance corresponding to the spatial observations, wherein the covariance corresponding to the spatial deviation state is greater than the covariance corresponding to the carrier navigation error state. The correction amount for the spatial bias state is determined based on the joint gain matrix and the residual.

6. The spatial deviation processing device according to claim 1, characterized in that, The spatial deviation processing device further includes an excitation determination unit, which is connected to the acquisition unit and the update unit. The excitation determination unit is configured to determine the motion excitation level based on the inertial measurement data; If the motion excitation level is lower than a preset threshold, updating the spatial deviation state using the correction amount is prohibited.

7. The spatial deviation processing device according to claim 1, characterized in that, The residual calculation unit is configured to perform a consistency check on the residuals to identify anomalous observation data affected by multipath interference.

8. The spatial deviation processing device according to claim 7, characterized in that, The residual calculation unit is configured to perform a consistency check on the residuals using a chi-square test. If the residual exceeds a preset threshold, the observation noise covariance corresponding to the spatial observation is increased and / or the current spatial observation is discarded to eliminate the abnormal observation data affected by multipath interference.

9. A method for processing spatial deviation, characterized in that, The method includes: Acquire inertial measurement data and spatial observations; Construct an augmented error state vector that includes the carrier navigation error state and the spatial deviation state; The nominal pose of the carrier is recursively calculated based on the inertial measurement data, and the error covariance matrix corresponding to the augmented error state vector is updated so that the covariance corresponding to the carrier navigation error state is less than the covariance corresponding to the spatial deviation state. Based on the nominal pose and spatial deviation state of the carrier, the predicted value of the spatial observation is obtained through a preset observation model, and a residual is constructed based on the spatial observation and the predicted value of the spatial observation. The correction amount of the spatial deviation state is determined based on the residual and the error covariance matrix, and the spatial deviation state is updated based on the correction amount of the spatial deviation state.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the spatial deviation processing method as described in claim 9.

Images