An Indoor Positioning Method Based on Multi-State Constraints for UWB-IMU Integration

Abstract

The present invention proposes a UWB-IMU combined indoor positioning method based on multi-state constraints, which specifically includes the following steps: S1. System state initialization; S2. Perform the state and covariance propagation update of the IMU; S3. Determine whether UWB data is detected. If yes, execute step S4, otherwise execute S7; S4. Perform UWB key frame detection. If it is a key frame, execute S5, otherwise execute S6; S5. Perform UWB state augmentation; S6. Perform UWB multi-state constraint update of the IMU state; S7. If the number of UWB states reaches the window size, perform UWB state marginalization and continue to execute S2. Otherwise, directly execute S2. The present invention provides a UWB-IMU combined indoor positioning method based on multi-state constraints. On the basis of Kalman filtering, the IMU state is constrained by using the UWB states at multiple historical moments, which can better constrain the divergence of the IMU and obtain high-precision positioning results.

Description

Technical Field

[0001] The present invention belongs to the technical field of indoor positioning, and particularly relates to a UWB-IMU combined indoor positioning method based on multi-state constraints. Background Art

[0002] In today's society, people's daily lives are increasingly inseparable from location services, such as common information queries, driving navigation, deformation monitoring, search and rescue, smart homes, etc. In outdoor scenarios, the most widely used location service is provided by the Global Navigation Satellite System (GNSS). In outdoor environments with less shielding, GNSS has the advantages of high efficiency, accuracy, reliability, and maturity. Almost all current smartphones have satellite navigation and positioning functions. Although satellite positioning technology is already very mature, it is difficult to achieve effective positioning relying on satellite positioning in indoor environments with severe satellite signal blockages. UWB positioning technology and inertial navigation positioning technology both have their own unique advantages and insurmountable defects, but they are highly complementary. Inertial navigation has the characteristic of short-term reliability, and this advantage can be used to reduce the impact of NLOS errors in UWB positioning. The advantage of UWB in providing accurate positioning in line-of-sight conditions can be used to correct the cumulative errors of inertial positioning, so as to improve the accuracy, reliability, and stability of the combined positioning system.

[0003] Traditional UWB / INS combined positioning is based on the extended Kalman filter. Its principle is to estimate the system state at the next moment based on the system state at the previous moment and the observed values of the system. This method often only retains the system state at one past moment, and more historical states are discarded. The data utilization rate of this method is not high. In fact, there is still a lot of information in the historical states of past moments. If it can be fully utilized, the estimated value of the state can be made more consistent with the optimal state. In combined positioning methods based on filtering for vision and IMU, the pose states of the camera at different moments observing common feature points are often used to constrain the IMU state to obtain better positioning effects. Inspired by this, the present invention proposes a UWB / IMU combined positioning algorithm based on multi-state constraints. Summary of the Invention

[0004] To solve the above technical problems, the present invention proposes a UWB-IMU combined indoor positioning method based on multi-state constraints, which can more effectively fuse UWB / IMU data. Essentially, based on the Kalman filter, the UWB states at multiple historical moments are used to jointly constrain the IMU to reduce the divergence of IMU errors.

[0005] To achieve the above object, the technical solution adopted by the present invention is:

[0006] A UWB-IMU combined indoor positioning method based on multi-state constraints, characterized in that it specifically includes the following steps:

[0007] S1. System state initialization;

[0008] S2. Perform state and covariance propagation updates of the IMU;

[0009] S3. Determine whether UWB data is detected. If yes, execute step S4; otherwise, execute S7;

[0010] S4. Perform UWB key frame detection. If it is a key frame, execute S5; otherwise, execute S6;

[0011] S5. Perform UWB state augmentation;

[0012] In the step S5, the steps of performing UWB state augmentation are as follows:

[0013] After receiving a UWB data frame, the overall IMU data between the current moment and the previous moment will be subjected to state prediction. After completing the state prediction of all IMU data, state augmentation immediately follows. For the nominal state, directly use the nominal state of the current moment IMU state as the nominal state of the current moment IMU;

[0014] For the error state, the mean value does not need to be augmented because before the observation, the mean values of all error states are zero. The most important step should be the augmentation of the covariance matrix of the error state. State augmentation is divided into two steps: cloning and correction;

[0015] S6. Perform UWB multi-state constraint update of the IMU state;

[0016] S7. If the number of UWB states reaches the window size, perform UWB state marginalization, and continue to execute S2; otherwise, directly execute S2.

[0017] As a further improvement of the present invention, the two steps of state augmentation in the step S5 are specifically as follows:

[0018] The first step of cloning is as follows:

[0019]

[0020] The second step of correction is as follows:

[0021]

[0022] In the above formula, X iis the state variable of the IMU, which is the traditional 15-dimensional state variable, namely the position error, velocity error, attitude error, accelerometer zero bias error, and gyroscope zero bias error of the IMU, X j is the augmented UWB state variable, that is, the three-dimensional position error state of the UWB, P ii is the covariance between the IMU state variables, P jj is the covariance between the UWB state variables, P ij is the covariance between the IMU and UWB state variables, J ji is the newly added state variable X j Relative to X i The Jacobian matrix of, in the multi-state constrained Kalman filter, the Jacobian of the newly added error UWB state with respect to the current error state is:

[0023]

[0024] Among them, the second term is the Jacobian of the current UWB error state variable with respect to other error UWB state variables, which is a zero matrix, and the first term is the Jacobian of the newly added UWB error state variable with respect to the IMU error state variable, and the result is as follows:

[0025]

[0026] In the formula is the UWB mounting lever arm value, and the system state can be augmented according to the above-derived content.

[0027] As a further improvement of the present invention, in the step S6, the steps of updating the IMU state by UWB multi-state constraint are as follows:

[0028] After the state augmentation, start constructing the observation update to update the error state, that is, execute the last three steps in the Kalman filter. First, construct the observation equation. The observation equation is as follows:

[0029] z i =H i X + n i (5)

[0030] The above formula represents the observation equation corresponding to the i-th UWB state, z i is the corresponding observed value, which is the difference between the UWB ranging value corresponding to the i-th UWB state and the theoretically derived distance between the UWB tag and the fixed base station. H i and n i are the corresponding observation equation and error vector respectively. Combining m observation equations gives:

[0031] z = HX + n (6)

[0032] The following is the general formula for Kalman filtering:

[0033]

[0034] After completing the state update, wait for the next filtering state augmentation and update. When the number of UWB states reaches the window maximum value, the corresponding oldest UWB state needs to be deleted from the window. The deletion operation can be completed by simply deleting the corresponding rows and columns of the covariance matrix for this UWB state.

[0035] The present invention proposes a UWB-IMU combined indoor positioning method based on multi-state constraints, specifically including the following steps:

[0036] S1. Initialize the system state; S2. Update the state and covariance propagation of the IMU; S3. Determine whether UWB data is detected. If yes, execute step S4, otherwise execute S7; S4. Detect UWB key frames. If it is a key frame, execute S5, otherwise execute S6; S5. Perform UWB state augmentation; S6. Update the IMU state with UWB multi-state constraints; S7. If the number of UWB states reaches the window size, perform UWB state marginalization, and continue to execute S2, otherwise directly execute S2.

[0037] The present invention provides a UWB-IMU combined indoor positioning method based on multi-state constraints. Based on Kalman filtering, the IMU state is constrained by using UWB states at multiple historical moments, which can better constrain the divergence of the IMU and obtain high-precision positioning results. Description of the Drawings

[0038] Figure 1 is the system working flowchart;

[0039] Figure 2 is the schematic diagram of state augmentation. Detailed Embodiment

[0040] The following further describes the present invention in detail in conjunction with the drawings and specific embodiments:

[0041] The overall working process of the present invention is as Figure 1 shown. A UWB / IMU combined indoor positioning method based on multi-state constraints includes the following steps:

[0042] S1. Initialize the system state;

[0043] S2. Update the state and covariance propagation of the IMU;

[0044] S3. Determine whether UWB data is detected. If yes, execute step S4, otherwise execute S7;

[0045] S4. Perform UWB key frame detection. If it is a key frame, execute S5; otherwise, execute S6.

[0046] S5. Perform UWB state amplification.

[0047] S6. Perform UWB multi-state constraint update on the IMU state.

[0048] S7. If the number of UWB states reaches the window size, perform UWB state marginalization and continue to execute S2; otherwise, directly execute S2.

[0049] IMU state and covariance propagation update

[0050] In the Kalman filter based on multi-state constraints, the state variables maintained consist of two parts, namely the IMU error state X I = [δθ δb g δv δb a δp I T and the UWB error state X U = [δp U T . During the state prediction process, only the nominal state of the IMU and the IMU error state are predicted. The covariance of the IMU error state and the covariance of the IMU error state relative to the UWB error state will change during the prediction process. The UWB-related states will only change during the observation update.

[0051] First, the motion equation of the error in continuous time is given:

[0052]

[0053] where F is the Jacobian matrix of the motion equation with respect to the error, and G is the Jacobian matrix of the motion equation with respect to the perturbation (noise). F and G are related to the current IMU acceleration, angular velocity, and the rotation at the previous moment. w I is the noise term.

[0054]

[0055]

[0056] After obtaining the continuous-time system derivation formula for the error state, it is discretized. According to the linear system theory, the discretized form of the continuous-time system is:

[0057]

[0058] Regarding the F(t) matrix within a short time interval as a constant matrix F k ​​Process and perform a third-order Taylor expansion on the exact discretization formula to obtain:

[0059]

[0060] The discretized equation of motion is obtained as:

[0061] X k+1 = Φ k+1|k X k+1 + W k

[0062] where

[0063] The main purpose of the variance of the noise term in the equation of motion is to perform the kinematic derivation of the Kalman filter. The mean of the error state can be ignored because the error state is driven by noise, and the mean of the noise term is zero. Under the iteration of the equation of motion, the mean is always zero, while the variance of the error state will be affected by the noise variance:

[0064]

[0065] Update the covariance matrix between the IMU and UWB. The overall covariance matrix is as follows:

[0066]

[0067] where is the covariance between the IMU and UWB states, is the UWB state covariance.

[0068] UWB key frame detection

[0069] When amplifying the UWB state, since the UWB state in the stationary state cannot effectively constrain the IMU state, it is necessary to detect the UWB state to make the UWB state in the window a moving state as much as possible. In the stationary case, the carrier is theoretically only affected by the gravitational acceleration. After coordinate transformation and removing the gravitational acceleration, the magnitude of the acceleration in the geodetic coordinate system should be zero. Therefore, the magnitude of the acceleration measurement ||a k || at a certain moment can be used and combined with a threshold for judgment:

[0070]

[0071] where is the carrier acceleration at time k.

[0072] Similar to the principle of detecting the magnitude of the acceleration measurement, during walking, the angular velocity changes rapidly; while during the foot contact phase, the angular velocity tends to zero. Therefore, the magnitude of the angular velocity measurement ||ωk ||, and make a judgment in combination with a threshold:

[0073]

[0074] In the formula, is the angular velocity of the carrier at time k.

[0075] Using a single criterion method may lead to missed detection and false detection in the stationary state. Therefore, the present invention adopts a comprehensive detection method based on acceleration and angular velocity, and uses the above two detection methods at the same time to detect the zero-speed interval, that is, when both the acceleration and the angular velocity are detected as stationary, the carrier is judged to be stationary.

[0076] System state augmentation

[0077] After receiving a UWB data frame, the overall state prediction of all IMU data between the current time and the previous time will be performed. After completing the state prediction of all IMU data, state augmentation follows immediately. For the nominal state, the nominal state of the current time IMU state is directly used as the nominal state of the current time IMU.

[0078] For the error state, the mean value does not need to be augmented because, before the observation, the mean values of all error states are zero. The most important step should be the augmentation of the covariance matrix of the error state. Figure 2 is a schematic diagram of state augmentation and update. State augmentation is divided into two steps: cloning and correction.

[0079] The first step of cloning is as follows:

[0080]

[0081] The second step of correction is as follows:

[0082]

[0083] where J ji is the Jacobian matrix of the new state variable X j relative to X i In the multi-state constrained Kalman filter, the Jacobian of the new error UWB state with respect to the current error state is:

[0084]

[0085] where the second term is the Jacobian of the current UWB error state variable relative to other UWB error state variables, which is a zero matrix, and the first term is the Jacobian of the new UWB error state variable relative to the IMU error state variable, and the result is as follows:

[0086]

[0087] In the formula is the UWB installation rod arm value, and the system state can be amplified according to the content derived above.

[0088] System state update

[0089] After the state amplification, the observation update is started to update the error state, that is, the last three steps in the Kalman filter are executed. First, the observation equation is constructed. The observation equation is as follows:

[0090] z i = H i X + n i

[0091] The above formula represents the observation equation corresponding to the i-th UWB state. z i is the corresponding observed value, which is the difference between the UWB ranging value corresponding to the i-th UWB state and the theoretically deduced distance between the UWB tag and the fixed base station. H i and n i are the corresponding observation equation and error vector respectively. Combining m observation equations gives:

[0092] z = HX + n

[0093] The following is the general formula of the Kalman filter:

[0094]

[0095] UWB state marginalization

[0096] After the state update is completed, it is possible to wait for the next filtering state amplification and update. When the number of UWB states reaches the window maximum value, the corresponding oldest UWB state needs to be deleted from the window. The deletion operation can be completed by simply deleting the row and column corresponding to this UWB state in the covariance matrix.

[0097] The above is only a preferred embodiment of the present invention, and it is not any other form of limitation to the present invention. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.

Claims

1. A UWB-IMU combined indoor positioning method based on multi-state constraints, characterized in that, it specifically includes the following steps: S1. System state initialization; S2. Perform the state and covariance propagation update of the IMU; S3. Determine whether UWB data is detected. If yes, execute step S4; otherwise, execute S7; S4. Perform UWB key frame detection. If it is a key frame, execute S5; otherwise, execute S6; S5. Perform UWB state augmentation; In the step S5, the steps for performing UWB state augmentation are as follows: After receiving a UWB data frame, the overall IMU data between the current moment and the previous moment will be subjected to state prediction. After completing the state prediction of all IMU data, state augmentation follows immediately. For the nominal state, directly take the nominal state of the current moment IMU state as the nominal state of the current moment IMU; For the error state, the mean does not need to be augmented because before the observation, the means of all error states are zero. The most important step is the covariance matrix augmentation of the error state. State augmentation is divided into two steps: cloning and correction; The two steps of state augmentation in step S5 are specifically as follows: The first step of cloning is as follows: The second step of correction is as follows: In the above formula, X i is the state variable of the IMU, which is a traditional 15-dimensional state variable, namely the position error, velocity error, attitude error, accelerometer zero bias error, and gyroscope zero bias error of the IMU. X j is the augmented UWB state variable, that is, the three-dimensional position error state of the UWB. P ii is the covariance between IMU state variables. P jj is the covariance between UWB state variables. P ij is the covariance between IMU and UWB state variables. J ji is the newly added state variable X j relative to X i Jacobian matrix. In the multi-state constrained Kalman filter, the Jacobian of the newly added error UWB state with respect to the current error state is: where X U is the UWB state, and X I is the IMU state. The superscript T represents the matrix transpose. The second term is the Jacobian of the current UWB error state variable relative to other error UWB state variables, which is a zero matrix. The first term is the Jacobian of the newly added UWB error state variable relative to the IMU error state variable, and the result is as follows: In the formula is the UWB installation rod arm value, and the system state can be amplified according to the content derived above; S6. Perform UWB multi-state constraint update of the IMU state; In step S6, the steps for performing UWB multi-state constraint update of the IMU state are as follows: After state augmentation, start constructing the observation update to update the error state, that is, execute the last three steps in the Kalman filter. First, construct the observation equation. The observation equation is as follows: z i = H i X + n i (5) The above equation represents the observation equation corresponding to the i-th UWB state, z i is the corresponding observation value, which is the difference between the UWB ranging value corresponding to the i-th UWB state and the theoretically derived distance between the UWB tag and the fixed base station, H i and n i are the corresponding observation equation and error vector respectively. Combining m observation equations gives: z = HX + n (6) The following are the general formulas of the Kalman filter: After completing the state update, wait for the next filtering state augmentation and update. When the number of UWB states reaches the window maximum value, the corresponding oldest UWB state needs to be deleted from the window. The deletion operation only needs to delete the corresponding rows and columns of the covariance matrix of this UWB state to complete the state deletion; S7. If the number of UWB states reaches the window size, perform UWB state marginalization, and continue to execute S2; otherwise, directly execute S2.

Images