UWB and IMU indoor positioning fusion method based on RTS smoothing

By combining IMU and UWB data in indoor positioning, using extended Kalman filtering and RTS smoothing algorithms, the problem of degradation of positioning accuracy in complex environments is solved, and higher positioning accuracy and reliability are achieved.

CN118921622APending Publication Date: 2024-11-08CHANGCHUN UNIV OF SCI & TECH
View PDF 0 Cites 2 Cited by

Patent Information

Application Number
CN202411124442.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-16
Publication Date
2024-11-08

Smart Images

  • Figure CN118921622A_ABST
    Figure CN118921622A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of indoor positioning, and particularly discloses a UWB and IMU indoor positioning fusion method based on RTS smoothing, and the method comprises the following steps: S1, building an IMU measurement model, and obtaining the first positioning estimation data of a moving object through the data of acceleration and angular velocity; s2, establishing a UWB measurement model, and calculating position coordinates by using a multilateral positioning algorithm to obtain second positioning estimation data of the moving object; s3, calculating a difference value obtained by the two kinds of data, and performing data processing by using an extended Kalman filtering algorithm to obtain a group of new and fused data; s4, processing the fused data of the data in the S3 by using an RTS smoothing algorithm; and S5, performing subtraction on the first positioning estimation data and the data obtained after smoothing in the step S4 to obtain final positioning estimation data. The method is simple in algorithm process and high in practicability, the advantages of the two sensors are fully utilized, a high-precision positioning result is provided, and the method is suitable for a UWB and IMU fusion positioning system.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of indoor positioning technology, and in particular to a UWB and IMU indoor positioning fusion method based on RTS smoothing. Background Art

[0002] With the advancement of science and technology, there are many types of indoor positioning technologies, including infrared positioning, ultrasonic positioning, Wi-Fi positioning, UWB positioning, IMU positioning, etc. Each positioning technology has its own advantages and disadvantages and is suitable for different scenarios and needs. UWB (Ultra-Wideband) positioning technology has attracted much attention for its high precision, high transmission rate and low power consumption. It uses broadband signal transmission and ranging technology to provide extremely high positioning accuracy. However, UWB positioning technology is affected by the environment, such as multipath effects and signal attenuation, which may cause large position deviations when the positioning algorithm estimates the tag coordinates.

[0003] IMU positioning technology provides an environment-independent positioning solution by measuring the acceleration and angular velocity of the carrier. It does not rely on external signal sources and can be positioned in complex environments. However, due to problems such as noise and drift in the sensor itself, as well as accumulated errors in the integration process, IMU positioning technology may cause the positioning accuracy to gradually decrease during long-term use.

[0004] Relying solely on a single positioning system for positioning has certain limitations when facing complex indoor environments. A single algorithm may not be able to solve problems such as multipath effects, signal attenuation, and obstacle occlusion in indoor environments, resulting in reduced positioning accuracy. Therefore, we propose a UWB and IMU indoor positioning fusion method based on RTS smoothing to solve the above problems. Summary of the invention

[0005] 1. Technical issues to be resolved

[0006] In view of the deficiencies in the prior art, the present invention provides a UWB and IMU indoor positioning fusion method based on RTS smoothing, which solves the problems raised in the above background technology.

[0007] (II) Technical solution

[0008] In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions:

[0009] A UWB and IMU indoor positioning fusion method based on RTS smoothing includes the following steps:

[0010] S1: Establish an IMU measurement model, use the accelerometer and gyroscope in the IMU sensor to measure the acceleration and angular velocity data of the tag, and obtain the first positioning estimation data of the moving object through the measured acceleration and angular velocity;

[0011] S2: Establish a UWB measurement model, use four UWB base stations, place them in four corners, use the multilateration algorithm to solve the position coordinates, and obtain the second positioning estimation data of the moving object;

[0012] S3: Calculate the difference between the data obtained by S1 and the data obtained by S2, and use the extended Kalman filter algorithm to process the data to obtain a new set of fused data;

[0013] S4: Use the RTS smoothing algorithm to process the fused data of S3;

[0014] S5: Subtract the first positioning estimation data from the data obtained after smoothing in S4 to obtain final positioning estimation data.

[0015] Furthermore, the IMU measurement model established in S1 to obtain the estimated value of the position includes: when the carrier moves, the gyroscope of the inertial navigation system provides the rotation rate information of the carrier by measuring the angular velocity of the carrier around three axes. By integrating the angular velocity output by the gyroscope, the attitude information of the carrier, that is, the posture and orientation of the carrier in space, can be obtained.

[0016] The accelerometer measures the acceleration in the carrier coordinate system, including linear acceleration and gravity acceleration. By integrating the acceleration once, we can get the carrier's velocity information, that is, the carrier's movement rate in the coordinate system. Further integrating the velocity twice, we can get the carrier's position information, that is, the carrier's movement distance in the coordinate system.

[0017] Furthermore, the acceleration and angular velocity data in S1 are provided in the form of a carrier coordinate system, which is converted into information of a navigation coordinate system through a formula, namely, a coordinate estimate (x, y); the estimated value of the position obtained by the IMU measurement model is expressed as:

[0018]

[0019]

[0020] Among them, a is the acceleration measured by the accelerometer, ω is the angular velocity measured by the gyroscope, t is the movement time; V0, x0, y0 are the velocity and coordinate values ​​at the previous moment.

[0021] Furthermore, the formula of the multilateration algorithm in S2 is as follows: The coordinates of the original positioning target at each moment are obtained as follows:

[0022]

[0023] (x i ,y i ) represents the coordinates of the base station, where i = 1, 2, 3, 4. di represents the distance between the positioning target and the base station.

[0024] Substituting the coordinates of each base station and the distance between the base station and the target point into the above formula, we can list the following equations:

[0025]

[0026] Next, we can obtain the linearized equation by subtracting the fourth equation from the first three equations of the system:

[0027] AX=B

[0028] in,

[0029] Solving the above equation using the least squares method yields:

[0030] X=(A T A) -1 A T b

[0031] The target coordinates (x UWB ,y UWB ).

[0032] Furthermore, the data update processing step in S3 is as follows: taking the difference between the first positioning estimation data and the second positioning estimation data as an observation value, and recording the difference at each moment;

[0033] The state equation and observation equation of the system are expressed as follows:

[0034]

[0035] Among them, w k is the system noise, F is the state transfer matrix; n k is the measurement noise and H is the observation matrix.

[0036] Let the observation vector Y k1 =x IMU-UWB and Y k1 =y IMU-UWB Substitute them into the following formulas to update the data; then we can get the fusion algorithm, where x IMU-UWB The horizontal coordinate, y, represents the difference between the first location estimate data and the second location estimate data. IMU-UWB a ordinate representing a difference between the first location estimate data and the second location estimate data;

[0037] Perform state prediction and observation prediction:

[0038]

[0039] Linearize the state equation and observation equation respectively, and calculate the state transfer matrix and observation matrix:

[0040]

[0041] Make covariance matrix predictions:

[0042] P(k+1∣k)=Φ(k+1∣k)P(k∣k)Φ T (k+1|k)+Q(k+1)

[0043] Calculate the Kalman filter gain:

[0044] K(k+1)=P(k+1|k)H T (k+1)[H(k+1)P(k+1|k)H T (k+1)+R(k+1)] -1

[0045] To make a status update:

[0046]

[0047] Update the covariance:

[0048] P(k+1)=[IK(k+1)H(k+1)]P(k+1|k).

[0049] Furthermore, in S4, the fused data is processed using the RTS smoothing algorithm for the data in S3; the formula is as follows:

[0050]

[0051] k=N-1,N-2,…,2,1,0

[0052] Where: is the RTS smoothing value, K s,k It represents the filter gain, P s,k represents the covariance matrix.

[0053] (III) Beneficial effects

[0054] Compared with the prior art, the present invention provides a UWB and IMU indoor positioning fusion method based on RTS smoothing, which has the following beneficial effects:

[0055] The present invention utilizes the complementary characteristics of UWB and IMU positioning systems to overcome the limitations of a single positioning system in a complex environment. IMU positioning data is used to correct the environmental error of UWB positioning in a short period of time. Since IMU positioning data is not affected by the environment, it can provide relatively stable and continuous position information. Secondly, UWB positioning data is used to constrain the cumulative error of IMU positioning data during long-term use. UWB positioning technology has high precision and low cumulative error characteristics, and can be used as reference data to correct the drift and error of IMU positioning data. The extended Kalman filter algorithm is used to fuse IMU and UWB positioning information to reduce the error caused by iteration, and overcome the problem of excessive long-term positioning error in complex indoor environments. The RTS smoothing algorithm can provide more accurate state estimation by using past and future observation data. Compared with the filtering algorithm that only uses current observation data, the RTS smoothing algorithm can use more information, including past observations and future observations, so as to more accurately estimate the state of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is a flow chart of the present invention;

[0057] Figure 2 System model diagram;

[0058] Figure 3 Comparison chart of trajectory tracking using only UWB and using UWB+IMU;

[0059] Figure 4 Comparison chart of trajectory tracking using only EKF and using EKF+IMU for trajectory tracking. DETAILED DESCRIPTION

[0060] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0061] Example

[0062] like Figure 1-4 As shown, an embodiment of the present invention proposes a UWB and IMU indoor positioning fusion method based on RTS smoothing, comprising the following steps:

[0063] S1: Establish an IMU measurement model, use the accelerometer and gyroscope in the IMU sensor to measure the acceleration and angular velocity data of the tag, and obtain the first positioning estimation data of the moving object through the measured acceleration and angular velocity;

[0064] The calculation formula for the first positioning estimation data is as follows:

[0065]

[0066]

[0067] Among them, a is the acceleration measured by the accelerometer, ω is the angular velocity measured by the gyroscope, t is the movement time; V0, x0, y0 are the velocity and coordinate values ​​at the previous moment;

[0068] S2: Establish a UWB measurement model, use four UWB base stations, place them in four corners, use the multilateration algorithm to solve the position coordinates, and obtain the second positioning estimation data of the moving object. The calculation formula of the second positioning estimation data is as follows:

[0069] like Figure 2 As shown, the coordinates of the four base stations are set to (x1, y1), (x2, y2), (x3, y3), (x4, y4), the coordinates of the moving target are set to (x, y), and the distances between the four base stations and the moving target are d1, d2, d3, d4. According to the formula of the multilateration algorithm, we can get

[0070]

[0071] Combining the above formulas, we can get

[0072]

[0073] After subtracting the fourth equation from the first three equations in the system, we get the linearized equation:

[0074] AX=B

[0075] in,

[0076] Solving the above equation using the least squares method yields:

[0077] X=(A T A) -1 A T b

[0078] The target coordinates (x, y) can be obtained.

[0079] S3: Calculate the difference between the data obtained by S1 and the data obtained by S2, and use the extended Kalman filter algorithm to process the data to obtain a new set of fused data;

[0080] A difference value obtained from the first positioning estimation data and the second positioning estimation data is taken as an observation value, and the difference value at each moment is recorded;

[0081] The state equation and observation equation of the system are expressed as follows:

[0082]

[0083] Among them, w k is the system noise, F is the state transfer matrix; n k is the measurement noise and H is the observation matrix.

[0084] Let the observation vector Y k1 =x IMU-UWB and Y k1 =y IMU-UWB Substitute them into the following formulas to update the data; then we can get the fusion algorithm, where x IMU-UWB The horizontal coordinate, y, represents the difference between the first location estimate data and the second location estimate data. IMU-UWB a ordinate representing a difference between the first location estimate data and the second location estimate data;

[0085] Perform state prediction and observation prediction:

[0086]

[0087] Linearize the state equation and observation equation respectively, and calculate the state transfer matrix and observation matrix:

[0088]

[0089] Make covariance matrix predictions:

[0090] P(k+1∣k)=Φ(k+1∣k)P(k∣k)Φ T (k+1|k)+Q(k+1)

[0091] Calculate the Kalman filter gain:

[0092] K(k+1)=P(k+1|k)H T (k+1)[H(k+1)P(k+1|k)H T (k+1)+R(k+1)] -1

[0093] To make a status update:

[0094]

[0095] Update the covariance:

[0096] P(k+1)=[IK(k+1)H(k+1)]P(k+1|k).

[0097] S4: Use the RTS smoothing algorithm to process the fused data from S3;

[0098] The formula is as follows:

[0099]

[0100] k=N-1,N-2,…,2,1,0

[0101] Where: is the RTS smoothing value, K s,k It represents the filter gain, P s,k represents the covariance matrix.

[0102] S5, using the first positioning estimation data and the data obtained after smoothing in S4 to perform a difference to obtain final positioning estimation data.

[0103] The effect of the present invention is further described below in conjunction with simulation experiments:

[0104] 1. Simulation conditions.

[0105] The hardware platform of the simulation of the present invention is: DW1000 UWB base station, tag (positioning target), IMU, laptop computer with processor i7-8759H, memory 8G, main frequency 2.20GHZ

[0106] Simulation software and hardware environment: Windows 10, MATLABR2021a.

[0107] The data used in the simulation experiment of the present invention was collected from the corridor of the training building of the South Campus of Changchun University of Science and Technology in May 2024. The content of the sample set is the indoor positioning data every second within 120 seconds.

[0108] 2. Analysis of simulation content and results.

[0109] according to Figure 3The results shown compare the effects of using UWB+IMU fusion positioning and UWB positioning alone to track motion trajectories. It can be clearly observed from the figure that the effects of UWB+IMU fusion positioning are better than UWB positioning alone in the three directions of x-axis, y-axis and z-axis. Especially in the z-axis direction, the effect of fusion positioning is more significant. This result shows that by fusing the data of UWB and IMU sensors, the positioning accuracy can be significantly improved. UWB positioning technology has good positioning ability in the horizontal direction, while IMU sensors have good perception ability for vertical movement. By fusing the two, their advantages can be fully utilized to achieve more accurate and reliable positioning results. In the z-axis direction, the effect of using UWB positioning alone is limited due to the relatively weak positioning accuracy of UWB technology in the vertical direction. However, when fused with the IMU sensor, the high-frequency data of the IMU can compensate for the positioning error of UWB in the z-axis direction, thereby significantly improving the positioning accuracy.

[0110] according to Figure 4 As shown in the figure, the comparison results of using EKF and EKF+RTS to track the motion trajectory are shown. These comparative experiments are all carried out under the conditions of UWB+IMU. It can be clearly observed from the figure that the curve using the RTS smoothing algorithm is more in line with the characteristics of the original trajectory. This result strongly proves that the UWB and IMU indoor positioning fusion method based on RTS smoothing proposed in the present invention improves the accuracy of the positioning trajectory. By using the RTS smoothing algorithm, we can make full use of past and future observation data to estimate state variables and provide more accurate positioning results. The advantage of the RTS smoothing algorithm is that it not only provides state estimates, but also provides information on the posterior distribution of state variables. This enables us to better understand the dynamic characteristics of the system and effectively reduce filtering delays. The results show that the positioning trajectory using the RTS smoothing algorithm is more consistent with the original path, indicating that this method can effectively reduce the cumulative effect of positioning errors. This UWB and IMU indoor positioning fusion method based on RTS smoothing has higher positioning accuracy and reliability, and provides strong support for the further development of indoor positioning technology.

[0111] The UWB and IMU indoor positioning fusion method based on RTS smoothing proposed in the present invention has the following advantages: simple algorithm flow, strong practicability and high positioning accuracy, and is suitable for UWB and IMU fusion positioning system. First, the algorithm flow of the method is simple. By using the RTS smoothing algorithm to fuse the data of UWB and IMU, not only the state estimation value can be obtained, but also the posterior distribution information of the state variable can be obtained. This algorithm flow is clear and easy to implement, so that the method has high operability in practical applications. Secondly, the method is practical. By fusing the data of UWB and IMU, the advantages of the two sensors can be fully utilized to provide more accurate and reliable positioning results. UWB positioning technology has good positioning capability in the horizontal direction, while IMU sensors have good perception capabilities for movement in the vertical direction. By fusing the two, their respective limitations can be compensated, thereby improving the overall positioning performance. In addition, the method shows a high level in positioning accuracy. By using the RTS smoothing algorithm, the cumulative effect of positioning errors can be more effectively reduced, and more accurate positioning results can be provided. Especially in the z-axis direction, the fusion algorithm can significantly improve the effect of UWB positioning and improve the overall positioning accuracy.

[0112] In summary, the UWB and IMU indoor positioning fusion method based on RTS smoothing has the advantages of simple algorithm, strong practicality and high positioning accuracy. This innovative method provides a reliable solution for the development of indoor positioning systems and shows broad application prospects in practical applications.

[0113] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A UWB and IMU indoor positioning fusion method based on RTS smoothing, Features: The steps include: S1: Establish an IMU measurement model, use the accelerometer and gyroscope in the IMU sensor to measure the acceleration and angular velocity data of the tag, and obtain the first positioning estimation data of the moving object through the measured acceleration and angular velocity; S2: Establish a UWB measurement model, use four UWB base stations, place them in four corners, use the multilateration algorithm to solve the position coordinates, and obtain the second positioning estimation data of the moving object; S3: Calculate the difference between the data obtained by S1 and the data obtained by S2, and use the extended Kalman filter algorithm to process the data to obtain a new set of fused data; S4: Use the RTS smoothing algorithm to process the fused data of S3; S5: Subtract the first positioning estimation data from the data obtained after smoothing in S4 to obtain final positioning estimation data.

2. According to the RTS smoothing based UWB and IMU indoor positioning fusion method of claim 1, Features: The IMU measurement model established in S1 to obtain the estimated value of the position includes: when the carrier moves, the inertial navigation system provides the rotation rate information of the carrier by measuring the angular velocity of the carrier around three axes, and the attitude information of the carrier, that is, the posture and orientation of the carrier in space, can be obtained by integrating the angular velocity output by the gyroscope; The accelerometer measures the acceleration in the carrier coordinate system, including linear acceleration and gravitational acceleration. By integrating the acceleration once, the velocity information of the carrier, that is, the movement rate of the carrier in the coordinate system, can be obtained. By further integrating the velocity twice, the position information of the carrier, that is, the movement distance of the carrier in the coordinate system, can be obtained.

3. According to the RTS smoothing based UWB and IMU indoor positioning fusion method of claim 2, Features: The acceleration and angular velocity data in S1 are provided in the form of a carrier coordinate system, and are converted into information of a navigation coordinate system through a formula, namely, coordinate estimation values ​​(x, y); the estimated value of the position obtained by the IMU measurement model is expressed as: Where a is the acceleration measured by the accelerometer, ω is the angular velocity measured by the gyroscope, and t is the motion time; V 0 ,x 0 ,y 0 It is the speed and coordinate value at the previous moment.

4. According to the RTS smoothing based UWB and IMU indoor positioning fusion method of claim 3, Features: The formula of the multilateration algorithm in S2 is as follows: The coordinates of the original positioning target at each moment are obtained as follows: (x i ,y i ) represents the coordinates of the base station, where i = 1, 2, 3, 4, and di represents the distance between the positioning target and the base station; Substituting the coordinates of each base station and the distance between the base station and the target point into the above formula, we can list the following equations: Next, we can obtain the linearized equation by subtracting the fourth equation from the first three equations of the system: AX=B in, Solving the above equation using the least squares method yields: X=(A T A) -1 A T b The target coordinates (x UWB ,y UWB ).

5. According to the RTS smoothing based UWB and IMU indoor positioning fusion method of claim 4, Features: The data updating processing steps in S3 are as follows: taking the difference between the first positioning estimation data and the second positioning estimation data as an observation value, and recording the difference at each moment; The state equation and observation equation of the system are expressed as follows: Among them, w k is the system noise, F is the state transfer matrix; n k is the measurement noise, H is the observation matrix; Let the observation vector Y k1 =x IMU-UWB and Y k1 =y IMU-UWB Substitute them into the following formulas to update the data; then we can get the fusion algorithm, where x IMU-UWB The horizontal coordinate, y, represents the difference between the first location estimate data and the second location estimate data. IMU-UWB a ordinate representing a difference between the first location estimate data and the second location estimate data; Perform state prediction and observation prediction: Linearize the state equation and observation equation respectively, and calculate the state transfer matrix and observation matrix: Make covariance matrix predictions: P(k+1∣k)=Φ(k+1∣k)P(k∣k)Φ T (k+1∣k)+Q(k+1) Calculate the Kalman filter gain: K(k+1)=P(k+1∣k)H T (k+1)[H(k+1)P(k+1∣k)H T (k+1)+R(k+1)] -1 To make a status update: Update the covariance: P(k+1)=[IK(k+1)H(k+1)]P(k+1∣k).

6. According to the RTS smoothing based UWB and IMU indoor positioning fusion method of claim 5, Features: In S4, the fused data is processed using the RTS smoothing algorithm for the data of S3; the formula is as follows: k=N-1,N-2,…,2,1,0 Where: is the RTS smoothing value, K s,k It represents the filter gain, P s,k represents the covariance matrix.

Citation Information

Cited By

  • Method for positioning unmanned system in closed space under external interference

    CN120176673A

  • Closed space unmanned system positioning method under external interference

    CN120176673B