Joint positioning method, device and storage medium of UWB and imu

Abstract

The application relates to the field of joint positioning, and discloses a UWB and IMU joint positioning method, equipment and a storage medium. The method comprises the following steps: receiving a ranging signal of a UWB base station, and reading angular velocity and specific force values of an IMU; performing screening and optimization processing on the ranging signal according to a preset UWB signal screening algorithm to generate an optimized ranging signal; constructing a high-dimensional state vector; performing navigation propagation processing on the high-dimensional state vector based on the angular velocity and the specific force values to obtain a predicted state vector, and obtaining a predicted covariance matrix according to a preset state propagation algorithm; performing residual calculation on the optimized ranging signal and the predicted state vector to obtain an observation residual and an observation Jacobian matrix; and performing Kalman observation update processing on the predicted state vector and the predicted covariance matrix to obtain an observation state vector and an observation covariance matrix. In the embodiment of the application, high-precision position and attitude information with anti-drift can be synchronously output without an additional sensor.

Description

Technical Field

[0001] This invention relates to the field of joint positioning, and more particularly to a method, apparatus and storage medium for joint positioning of UWB and IMU. Background Technology

[0002] Ultra-wideband (UWB) technology, with its high-precision ranging capabilities, has become one of the mainstream solutions for high-precision indoor positioning. Inertial measurement units (IMUs) can provide high-frequency, continuous information about vehicle motion, but they do not rely on external signals, and their errors accumulate over time. Therefore, combining UWB with an IMU, utilizing the absolute accuracy of UWB to correct the accumulated drift of the IMU, has become an inevitable choice for improving the robustness and continuity of indoor positioning systems.

[0003] Currently, the widely adopted UWB / IMU fusion solutions suffer from limitations due to their loosely coupled models. Most existing technologies employ a "loosely coupled" approach, where the UWB module independently calculates a position coordinate, which is then filtered and fused with the position or velocity calculated by the IMU in the position domain. This approach has two fundamental drawbacks: First, the independent UWB calculation process itself loses original ranging information and introduces additional linearization errors; second, when the UWB signal is affected by non-line-of-sight propagation or multipath effects, resulting in erroneous positions, the loosely coupled model struggles to effectively identify and isolate these errors at the data level, causing the erroneous information to directly contaminate the entire filtering state, leading to jumps or divergences in the positioning results. Therefore, to address the significant errors in existing UWB and IMU fusion positioning solutions, a new technology is needed to solve this problem. Summary of the Invention

[0004] The main objective of this invention is to solve the technical problem of large errors in the existing UWB and IMU fusion positioning schemes.

[0005] The first aspect of the present invention provides a joint localization method using UWB and IMU, the joint localization method including: Receive ranging signals from UWB base stations and read the angular velocity and specific force values ​​of the IMU; The ranging signal is filtered and optimized according to a preset UWB signal filtering algorithm to generate an optimized ranging signal. Construct a high-dimensional state vector X=[p T v T q T b a T b g T s a T ,δd] TAnd the initial covariance matrix, where p is the target vehicle localization, v is the target vehicle velocity, q is the target vehicle attitude quaternion, b a For zero bias of the accelerometer, b g For zero bias of the gyroscope, s a δd represents the scaling factor error of the accelerometer, and δd represents the UWB time delay deviation. Based on the angular velocity and the specific force value, the high-dimensional state vector is subjected to navigation propagation processing to obtain the predicted state vector, and the initial covariance matrix is ​​subjected to variance propagation processing according to the preset state propagation algorithm to obtain the predicted covariance matrix. According to the preset residual analysis algorithm, residual calculation is performed on the optimized ranging signal and the predicted state vector to obtain the observation residual and the observation Jacobian matrix. Based on the observation residuals and the observation Jacobian matrix, Kalman observation update processing is performed on the predicted state vector and the predicted covariance matrix to obtain the observation state vector and the observation covariance matrix, wherein the observation state vector includes: joint positioning of the target and the carrier.

[0006] Optionally, in a first implementation of the first aspect of the present invention, the step of performing navigation propagation processing on the high-dimensional state vector based on the angular velocity and the specific force value to obtain the predicted state vector includes: According to the preset compensation algorithm, the angular velocity and the specific force value are compensated to obtain the compensated angular velocity and the compensated specific force value. Based on the preset inertial differential equation, the compensated angular velocity, and the compensated specific force value, inertial calculations are performed on the high-dimensional state vector to generate a predicted state vector.

[0007] Optionally, in a second implementation of the first aspect of the present invention, the step of performing compensation calculations on the angular velocity and the specific force value according to a preset compensation algorithm to obtain the compensated angular velocity and the compensated specific force value includes:

[0008] Among them, w m b is the angular velocity. g For gyroscope zero bias, w is the compensated angular velocity, I is the identity matrix, and diag(s) a ) represents the scaling factor error s a The diagonal matrix formed, f m b is the ratio of force. a The accelerometer is zero biased, and f is the compensation specific force value.

[0009] Optionally, in a third implementation of the first aspect of the present invention, the step of performing variance propagation processing on the initial covariance matrix according to a preset state propagation algorithm to obtain the predicted covariance matrix includes: Based on the state changes of the predicted state vector and the high-dimensional state vector, a state transition Jacobian matrix is ​​generated. Based on the preset variance propagation equation and the state transition Jacobian matrix, the initial covariance matrix is ​​subjected to variance propagation processing to obtain the predicted covariance matrix.

[0010] Optionally, in a fourth implementation of the first aspect of the present invention, the step of performing residual calculation on the optimized ranging signal and the predicted state vector according to a preset residual analysis algorithm to obtain the observation residual and the observation Jacobian matrix includes: Based on a preset observation function, the predicted state vector is subjected to prediction observation calculation to obtain the predicted measurement value; The difference between the optimized ranging signal and the predicted measurement value is calculated to obtain the observation residual; Calculate the partial derivative of the observation function with respect to the predicted state vector to generate the observation Jacobian matrix.

[0011] Optionally, in a fifth implementation of the first aspect of the present invention, the step of performing Kalman observation update processing on the predicted state vector and the predicted covariance matrix based on the observation residuals and the observation Jacobian matrix to obtain the observation state vector and the observation covariance matrix includes: The Kalman gain matrix is ​​calculated based on the observed Jacobian matrix and the predicted covariance matrix. Based on the Kalman gain matrix and the observation residual, the predicted state vector is updated to obtain the observed state vector. Based on the Kalman gain matrix and the observation Jacobian matrix, the prediction covariance matrix is ​​updated to obtain the observation covariance matrix.

[0012] Optionally, in a sixth implementation of the first aspect of the present invention, the step of calculating the Kalman gain matrix based on the observed Jacobian matrix and the predicted covariance matrix includes:

[0013] Among them, P k|k-1 To predict the covariance matrix, H k Let R be the observation Jacobian matrix at time k. k Let be the observation noise covariance matrix at time k, and let diag() be a diagonal matrix. j 2 Let K be the ranging noise variance of the j-th base station. k Let be the Kalman gain matrix at time k.

[0014] Optionally, in a seventh implementation of the first aspect of the present invention, the step of performing state update processing on the predicted state vector based on the Kalman gain matrix and the observation residual to obtain the observed state vector includes:

[0015] Where, x k|k Let x be the observed state vector. k|k-1 For the predicted state vector, y k Let K be the observation residual at time k. k The Kalman gain matrix at time k. A second aspect of the present invention provides a UWB and IMU joint positioning device, comprising: a memory and at least one processor, wherein the memory stores instructions, and the memory and the at least one processor are interconnected via a line; the at least one processor invokes the instructions in the memory to cause the UWB and IMU joint positioning device to perform the above-described UWB and IMU joint positioning method.

[0016] A third aspect of the present invention provides a computer-readable storage medium storing instructions that, when executed on a computer, cause the computer to perform the aforementioned joint positioning method of UWB and IMU.

[0017] In this embodiment of the invention, the UWB ranging signal is first cleaned and filtered to find an optimized ranging signal with high quality and reliability. This optimized ranging signal is used as observation data for subsequent updates to the IMU measurement data. A high-dimensional state vector is constructed, incorporating UWB delay error and accelerometer scaling factor error. The positioning of the high-dimensional state vector is first predicted using the IMU's angular velocity and specific force values, resulting in a predicted state vector and a corresponding prediction covariance matrix. Based on the optimized ranging signal, the predicted state vector is updated using a Kalman update framework. The update amplitude is constrained by the prediction covariance matrix and the observation covariance matrix, generating an observation state vector and observation covariance matrix that fuse IMU and UWB data. The fused positioning of the target vehicle is then obtained from the observation state vector. The compact combination of this scheme is insensitive to individual bad observations, thus the positioning results are stable without drastic changes in dynamic interference scenarios. The accelerometer scaling factor error and UWB circuit delay deviation are jointly estimated as state variables, and high-precision position and attitude information with anti-drift is output synchronously without the need for additional sensors. This solves the technical problem of large errors in the existing UWB and IMU fusion positioning schemes. Attached Figure Description

[0018] Figure 1This is a schematic diagram of an embodiment of the joint localization method of UWB and IMU in this invention; Figure 2 This is a schematic diagram of a specific embodiment of the 104 steps of the joint localization method of UWB and IMU in this invention. Figure 3 This is a schematic diagram of a specific embodiment of step 105 of the joint localization method of UWB and IMU in this invention. Figure 4 This is a schematic diagram of a specific embodiment of the 106 steps of the joint localization method of UWB and IMU in this invention. Figure 5 This is a schematic diagram of an embodiment of a joint positioning device using UWB and IMU in this invention. Detailed Implementation

[0019] This invention provides a method, device, and storage medium for joint positioning using UWB and IMU.

[0020] The embodiments of the present invention will now be described in more detail with reference to the accompanying drawings. While some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present disclosure. It should be understood that the accompanying drawings and embodiments are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0021] In the description of the embodiments disclosed in this invention, the term "comprising" and similar terms should be understood as open-ended inclusion, i.e., "including but not limited to". The term "based on" should be understood as "at least partially based on". The term "one embodiment" or "the embodiment" should be understood as "at least one embodiment". The terms "first", "second", etc., may refer to different or the same objects. Other explicit and implicit definitions may also be included below.

[0022] For ease of understanding, the specific process of the embodiments of the present invention is described below. Please refer to [link / reference]. Figure 1 A schematic diagram of an embodiment of the joint localization method of UWB and IMU in this invention is shown. The joint localization method of UWB and IMU includes: 101. Receive the ranging signal from the UWB base station and read the angular velocity and specific force value of the IMU; In this embodiment, the UWB positioning network of the hardware entity includes at least four UWB fixed base stations with known coordinates and synchronized time. The mobile positioning terminal of the hardware entity is an embedded device integrating a UWB communication tag, an IMU containing a three-axis accelerometer and a three-axis gyroscope, a microprocessor, and a storage unit.

[0023] Calculate the ranging of all base stations, generate the ranging signals of all visible base stations, and read the angular velocity and specific force value of the IMU of the mobile entity fixed terminal. Here, the specific force value is the non-gravitational acceleration experienced by the object in inertial space.

[0024] 102. Based on a preset UWB signal filtering algorithm, the ranging signal is filtered and optimized to generate an optimized ranging signal; In this embodiment, the signal strength (RSSI) or signal-to-noise ratio (SNR) of all visible base station ranging values ​​is calculated, and base stations below a preset threshold are removed. The theoretical distance to each base station is calculated using the current position extrapolated from the previous optimal estimate (IMU short-time prediction), and the residual is calculated by comparing it with the actual ranging value. Cluster analysis (such as DBSCAN) is performed on the residual sequence, and base stations belonging to "outlier clusters" are identified as being severely affected by non-line-of-sight factors and are removed. Among the base stations that pass the above test, 4-5 base stations that minimize the geometrical factor of precision (GDOP) are selected to form the "optimal solution group." This step ensures that, under the premise of reliable data quality, the spatial geometric distribution is also optimal, generating an optimized ranging signal.

[0025] 103. Construct a high-dimensional state vector X=[p T v T q T b a T b g T s a T ,δd] T And the initial covariance matrix, where p is the target vehicle localization, v is the target vehicle velocity, q is the target vehicle attitude quaternion, b a For zero bias of the accelerometer, b g For zero bias of the gyroscope, s a δd represents the scaling factor error of the accelerometer, and δd represents the UWB time delay deviation. In this embodiment, a high-dimensional state vector X=[p] is first defined and constructed in the scheme. T v T q T b a T b g T s a T ,δd] T Where p is the target vehicle's localization, v is the target vehicle's velocity, q is the target vehicle's attitude quaternion, and b a For zero bias of the accelerometer, b g For zero bias of the gyroscope, s aδd represents the scaling factor error of the accelerometer, and δd represents the UWB time delay deviation.

[0026] Furthermore, for each value in the high-dimensional state vector, there is a corresponding matrix of initial variance and covariance with quantization uncertainty. An initial covariance matrix P can be directly given. k This allows for constraints on the prediction and observation adjustments of high-dimensional state vectors in subsequent processing.

[0027] 104. Based on the angular velocity and the specific force value, perform navigation propagation processing on the high-dimensional state vector to obtain a predicted state vector, and perform variance propagation processing on the initial covariance matrix according to a preset state propagation algorithm to obtain a predicted covariance matrix. In this embodiment, the angular velocity and specific force values ​​are compensated in real time using the current estimation error parameters to obtain values ​​closer to the true angular velocity and specific force. Using the compensated angular velocity and specific force values, the current attitude, velocity, position, and other information of the carrier are calculated using common differential equations of inertial motion. Based on the updated data, the high-dimensional state vector is modified to generate a predicted state vector.

[0028] For state propagation, updates are performed according to the following pattern: 1. Zero-biased model calculation

[0029]

[0030] in, and It is the zero bias transformation rate, b a For zero bias of the accelerometer, b g This is for zero bias of the gyroscope. τ ba and τ bg It is the correlation time constant, describing the rate of change of zero bias, which is usually determined by the sensor characteristics. ba and w bg It drives white noise, giving zero bias a random walk characteristic.

[0031] 2. Scale factor calculation

[0032] in, It is the rate of change of the scaling factor error, τ sa It is the relevant time constant, w sa It is driving white noise, s a This represents the scaling factor error of the accelerometer.

[0033] 3. Calculation of time delay deviation

[0034] Where δd is the UWB time delay bias. It is the transformation rate of the time delay deviation, w d It drives white noise.

[0035] The state propagation scheme is used to predict error states and covariance, and finally generates a prediction covariance matrix.

[0036] For details, please refer to Figure 2 , Figure 2 This is a schematic diagram of a specific embodiment of step 104 of the joint positioning method of UWB and IMU in this invention. Step 104, "based on the angular velocity and the specific force value, performing navigation propagation processing on the high-dimensional state vector to obtain the predicted state vector," includes the following specific implementation methods: 1041. According to the preset compensation algorithm, the angular velocity and the specific force value are compensated to obtain the compensated angular velocity and the compensated specific force value; 1042. Based on the preset inertial differential equation, the compensated angular velocity, and the compensated specific force value, perform inertial calculation on the high-dimensional state vector to generate a predicted state vector.

[0037] In steps 1041-1042, the angular velocity and specific force are first compensated based on the zero bias of the gyroscope and the zero bias of the accelerometer, so as to obtain the compensated angular velocity and the compensated specific force.

[0038] Then, by substituting the compensated angular velocity and compensated specific force values ​​into the inertial differential equation, the positioning, attitude, and velocity of the high-dimensional state vector are updated to generate the predicted state vector.

[0039] It should be noted that step 1041 includes the following specific implementation methods:

[0040] Among them, w m b is the angular velocity. g For gyroscope zero bias, w is the compensated angular velocity, I is the identity matrix, and diag(s) a ) represents the scaling factor error s a The diagonal matrix formed, f m b is the ratio of force. a The accelerometer is zero biased, and f is the compensation specific force value.

[0041] Specifically, step 104, "performing variance propagation processing on the initial covariance matrix according to the preset state propagation algorithm to obtain the predicted covariance matrix," includes the following specific implementation methods: 1043. Based on the state changes of the predicted state vector and the high-dimensional state vector, generate a state transition Jacobian matrix; 1044. Based on the preset variance propagation equation and the state transition Jacobian matrix, the initial covariance matrix is ​​subjected to variance propagation processing to obtain the predicted covariance matrix.

[0042] In steps 1043-1044, the covariance matrix of the propagation error state is processed by first generating a discretized state transition Jacobian matrix, calculated as follows:

[0043]

[0044] Where, x k-1 Let x be a high-dimensional state vector. k|k-1 For the predicted state vector, f() is a nonlinear state transition function that describes how to calculate the predicted state value for the next time step given the state at the previous time step and the IMU reading at the current time step. k It is a control input, w k To compensate for angular velocity, f k To compensate for the force ratio, k is the subscript for distinguishing time, F k Let be the state transition Jacobian matrix.

[0045] Finally, substituting the state transition Jacobian matrix into the variance propagation equation, the initial covariance matrix is ​​processed by variance propagation to obtain the predicted covariance matrix, calculated as follows: ; Among them, P k-1 Let F be the initial covariance matrix. k Let P be the state transition Jacobian matrix. k|k-1 To predict the covariance matrix, Q k The noise covariance matrix is ​​derived from the driving white noise in IMU measurement noise and error analysis, which is a preset parameter.

[0046] 105. According to the preset residual analysis algorithm, perform residual calculation on the optimized ranging signal and the predicted state vector to obtain the observation residual and the observation Jacobian matrix; In this embodiment, the predicted value of the optimized ranging signal is calculated using the observation function. The observation residual is obtained by calculating the difference between the actual measured value and the predicted value. Then, the partial derivative of the observation function with respect to the predicted state vector is used to generate the observation Jacobian matrix.

[0047] Please see Figure 3 , Figure 3 This is a schematic diagram of a specific embodiment of step 105 of the joint localization method of UWB and IMU in this invention. Step 105 includes the following specific implementation methods: 1051. Based on the preset observation function, perform prediction observation calculations on the predicted state vector to obtain the predicted measurement value; 1052. Calculate the difference between the optimized ranging signal and the predicted measurement value to obtain the observation residual; 1053. Calculate the partial derivative of the observation function with respect to the predicted state vector to generate the observation Jacobian matrix.

[0048] In steps 1051-1053, the observation residuals must first be calculated, as follows:

[0049] Z here k Z is the actual measured value. k =[Z1, Z2, ..., Z j ] T j is a positive integer, and each Z j The distance is measured by the j-th base station, h() is a linear observation function, and x k|k-1 For the predicted state vector, h(x) k|k-1 ) represents the predicted measurement value, y k To observe the residuals.

[0050] y k This reflects the difference between the actual measured value and the predicted value. If the prediction is completely accurate and noise-free, y k It should be zero. But in reality, due to prediction errors and measurement noise, y k The difference is not zero; this difference is precisely the information used to correct the state.

[0051] Then, the partial derivatives of the observation function with respect to position and time delay are calculated, while the partial derivatives with respect to other states (such as velocity, attitude, and zero bias) are all 0, because these states do not affect the ranging prediction. This sparsity simplifies the calculation. The expression for calculating the observation Jacobian matrix is ​​as follows:

[0052] Where h() is a linear observation function, x k|k-1 To predict the state vector, H k To observe the Jacobian matrix.

[0053] 106. Based on the observation residuals and the observation Jacobian matrix, perform Kalman observation update processing on the predicted state vector and the predicted covariance matrix to obtain the observation state vector and the observation covariance matrix, wherein the observation state vector includes: joint positioning of the target and the carrier.

[0054] In this embodiment, based on the Kalman framework, the observation residuals and observation Jacobian matrix are substituted to perform observation constraint correction and update on the predicted state vector and the predicted covariance matrix, generating the observation state vector and the observation covariance matrix. The observation state vector contains seven parameters corresponding to the high-dimensional state vector, and the positioning parameters are the required joint positioning of the target carrier.

[0055] For details, please refer to Figure 4 , Figure 4 This is a schematic diagram of a specific embodiment of the 106 steps of the joint localization method of UWB and IMU in this invention. The 106 steps include the following specific implementation methods: 1061. Based on the observed Jacobian matrix and the predicted covariance matrix, calculate the Kalman gain matrix; 1062. Based on the Kalman gain matrix and the observation residual, perform state update processing on the predicted state vector to obtain the observed state vector; 1063. Based on the Kalman gain matrix and the observation Jacobian matrix, the prediction covariance matrix is ​​updated to obtain the observation covariance matrix.

[0056] In steps 1061-1063, the Kalman gain matrix needs to be calculated first. The Kalman gain matrix is ​​calculated as follows:

[0057] Among them, P k|k-1 To predict the covariance matrix, H k Let R be the observation Jacobian matrix at time k. k Let be the observation noise covariance matrix at time k, and let diag() be a diagonal matrix. j 2 Let K be the ranging noise variance of the j-th base station. k Let be the Kalman gain matrix at time k. The Kalman gain is the optimal weighting factor that makes the updated state estimate optimal in the sense of minimum mean square error.

[0058] Then, the observation state vector is calculated using the Kalman gain matrix and the observation residuals, as follows:

[0059] Where, x k|k Let x be the observed state vector. k|k-1 For the predicted state vector, y k Let K be the observation residual at time k. kLet be the Kalman gain matrix at time k. The observation residuals are distributed to the state variables in an optimal proportion to correct prediction bias. For example, if the ranging residual of a base station is positive (actually farther than predicted), and the base station is located in a certain direction, then its position will be shifted closer along that direction.

[0060] Finally, the predicted covariance matrix is ​​updated using the following method: ; Among them, P k|k-1 To predict the covariance matrix, P k|k To observe the covariance matrix, K k H is the Kalman gain matrix at time k. k Let P be the observation Jacobian matrix at time k. k|k-1 The predicted covariance matrix can be used to predict and correct the observed state vector again at the next time step, and the fused positioning data of UBW and IMU are output cyclically.

[0061] In this embodiment of the invention, the UWB ranging signal is first cleaned and filtered to find an optimized ranging signal with high quality and reliability. This optimized ranging signal is used as observation data for subsequent updates to the IMU measurement data. A high-dimensional state vector is constructed, incorporating UWB delay error and accelerometer scaling factor error. The positioning of the high-dimensional state vector is first predicted using the IMU's angular velocity and specific force values, resulting in a predicted state vector and a corresponding prediction covariance matrix. Based on the optimized ranging signal, the predicted state vector is updated using a Kalman update framework. The update amplitude is constrained by the prediction covariance matrix and the observation covariance matrix, generating an observation state vector and observation covariance matrix that fuse IMU and UWB data. The fused positioning of the target vehicle is then obtained from the observation state vector. The compact combination of this scheme is insensitive to individual bad observations, thus the positioning results are stable without drastic changes in dynamic interference scenarios. The accelerometer scaling factor error and UWB circuit delay deviation are jointly estimated as state variables. Without the need for additional sensors, it synchronously outputs high-precision position and attitude information with anti-drift, solving the technical problem of large errors in existing UWB and IMU fusion positioning schemes.

[0062] Figure 5This is a schematic diagram of a UWB and IMU joint positioning device 500 provided in an embodiment of the present invention. The UWB and IMU joint positioning device 500 can vary significantly due to different configurations or performance. It may include one or more central processing units (CPUs) 510 and memory 520, and one or more storage media 530 for storing application programs 533 or data 532. The memory 520 and storage media 530 can be temporary or persistent storage. The program stored in the storage media 530 may include one or more modules (not shown in the diagram), each module may include a series of instruction operations on the UWB and IMU joint positioning device 500. Furthermore, the processor 510 may be configured to communicate with the storage media 530 and execute the series of instruction operations in the storage media 530 on the UWB and IMU joint positioning device 500.

[0063] The UWB and IMU-based joint positioning device 500 may also include one or more power supplies 540, one or more wired or wireless network interfaces 550, one or more input / output interfaces 560, and / or one or more operating systems 531, such as Windows Server, Mac OS X, Unix, Linux, Free BSD, etc. Those skilled in the art will understand that... Figure 5 The illustrated UWB and IMU co-location device structure does not constitute a limitation on UWB and IMU-based co-location devices, which may include more or fewer components than illustrated, or combine certain components, or have different component arrangements.

[0064] The present invention also provides a computer-readable storage medium, which can be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium, wherein the computer-readable storage medium stores instructions that, when the instructions are executed on a computer, cause the computer to perform the steps of the joint positioning method of UWB and IMU.

[0065] In the context of this disclosure, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0066] Furthermore, although the operations are described in a specific order, this should be understood as requiring that such operations be performed in the specific order shown or in sequential order, or requiring that all illustrated operations be performed to achieve the desired result. In certain environments, multitasking and parallel processing may be advantageous. Similarly, although several specific implementation details are included in the above discussion, these should not be construed as limiting the scope of this disclosure. Certain features described in the context of individual embodiments may also be implemented in combination in a single implementation. Conversely, various features described in the context of a single implementation may also be implemented individually or in any suitable sub-combination in multiple implementations.

[0067] Although the subject matter has been described using language specific to structural features and / or methodological logic, it should be understood that the subject matter defined in the appended claims is not necessarily limited to the specific features or actions described above. Rather, the specific features and actions described above are merely illustrative examples of implementing the claims.

Claims

1. A joint localization method using UWB and IMU, characterized in that, Including the following steps: Receive ranging signals from UWB base stations and read the angular velocity and specific force values ​​of the IMU; The ranging signal is filtered and optimized according to a preset UWB signal filtering algorithm to generate an optimized ranging signal. Construct a high-dimensional state vector X=[p T v T q T b a T b g T s a T ,δd] T And the initial covariance matrix, where p is the target vehicle localization, v is the target vehicle velocity, q is the target vehicle attitude quaternion, b a For zero bias of the accelerometer, b g For zero bias of the gyroscope, s a δd represents the scaling factor error of the accelerometer, and δd represents the UWB time delay deviation. Based on the angular velocity and the specific force value, the high-dimensional state vector is subjected to navigation propagation processing to obtain the predicted state vector, and the initial covariance matrix is ​​subjected to variance propagation processing according to the preset state propagation algorithm to obtain the predicted covariance matrix. According to the preset residual analysis algorithm, residual calculation is performed on the optimized ranging signal and the predicted state vector to obtain the observation residual and the observation Jacobian matrix. Based on the observation residuals and the observation Jacobian matrix, Kalman observation update processing is performed on the predicted state vector and the predicted covariance matrix to obtain the observation state vector and the observation covariance matrix, wherein the observation state vector includes: joint positioning of the target and the carrier.

2. The joint positioning method of UWB and IMU according to claim 1, characterized in that, The step of performing navigation propagation processing on the high-dimensional state vector based on the angular velocity and the specific force value to obtain the predicted state vector includes: According to the preset compensation algorithm, the angular velocity and the specific force value are compensated to obtain the compensated angular velocity and the compensated specific force value. Based on the preset inertial differential equation, the compensated angular velocity, and the compensated specific force value, inertial calculations are performed on the high-dimensional state vector to generate a predicted state vector.

3. The joint positioning method of UWB and IMU according to claim 2, characterized in that, The step of calculating the compensation angular velocity and the specific force value according to the preset compensation algorithm to obtain the compensation angular velocity and the compensation specific force value includes: Among them, w m b is the angular velocity. g For gyroscope zero bias, w is the compensated angular velocity, I is the identity matrix, and diag(s) a ) represents the scaling factor error s a The diagonal matrix formed, f m b is the ratio of force. a The accelerometer is zero biased, and f is the compensation specific force value.

4. The joint positioning method of UWB and IMU according to claim 1, characterized in that, The step of performing variance propagation processing on the initial covariance matrix according to a preset state propagation algorithm to obtain the predicted covariance matrix includes: Based on the state changes of the predicted state vector and the high-dimensional state vector, a state transition Jacobian matrix is ​​generated. Based on the preset variance propagation equation and the state transition Jacobian matrix, the initial covariance matrix is ​​subjected to variance propagation processing to obtain the predicted covariance matrix.

5. The joint positioning method of UWB and IMU according to claim 1, characterized in that, The step of calculating the residuals of the optimized ranging signal and the predicted state vector according to the preset residual analysis algorithm to obtain the observation residuals and the observation Jacobian matrix includes: Based on a preset observation function, the predicted state vector is subjected to prediction observation calculation to obtain the predicted measurement value; The difference between the optimized ranging signal and the predicted measurement value is calculated to obtain the observation residual; Calculate the partial derivative of the observation function with respect to the predicted state vector to generate the observation Jacobian matrix.

6. The joint positioning method of UWB and IMU according to claim 1, characterized in that, The step of performing Kalman observation update processing on the predicted state vector and the predicted covariance matrix based on the observation residuals and the observation Jacobian matrix to obtain the observation state vector and the observation covariance matrix includes: The Kalman gain matrix is ​​calculated based on the observed Jacobian matrix and the predicted covariance matrix. Based on the Kalman gain matrix and the observation residual, the predicted state vector is updated to obtain the observed state vector. Based on the Kalman gain matrix and the observation Jacobian matrix, the prediction covariance matrix is ​​updated to obtain the observation covariance matrix.

7. The joint positioning method of UWB and IMU according to claim 6, characterized in that, The step of calculating the Kalman gain matrix based on the observed Jacobian matrix and the predicted covariance matrix includes: Among them, P k|k-1 To predict the covariance matrix, H k Let R be the observation Jacobian matrix at time k. k Let be the observation noise covariance matrix at time k, and let diag() be a diagonal matrix. j 2 Let K be the ranging noise variance of the j-th base station. k Let be the Kalman gain matrix at time k.

8. The joint positioning method of UWB and IMU according to claim 6, characterized in that, The step of performing state update processing on the predicted state vector based on the Kalman gain matrix and the observation residual to obtain the observed state vector includes: Where, x k|k Let x be the observed state vector. k|k-1 For the predicted state vector, y k Let K be the observation residual at time k. k Let be the Kalman gain matrix at time k.

9. A combined UWB and IMU positioning device, characterized in that, The joint positioning device of UWB and IMU includes: a memory and at least one processor, wherein the memory stores instructions and the memory and the at least one processor are interconnected via a line; The at least one processor invokes the instructions in the memory to cause the UWB and IMU joint positioning device to perform the UWB and IMU joint positioning method as described in any one of claims 1-8.

10. A computer-readable storage medium storing a computer program thereon, characterized in that, When the computer program is executed by the processor, it implements the joint positioning method of UWB and IMU as described in any one of claims 1-8.

Images