An Indoor Positioning Method Based on UWB Fusion IMU

Abstract

This invention belongs to the field of indoor positioning technology, specifically relating to an indoor positioning method based on UWB fusion with IMU. The method includes: calculating attitude based on an IMU positioning model to obtain position information; obtaining the optimal horizontal position using an iterative weighted least squares algorithm based on the UWB model; obtaining a height array in three-dimensional spatial coordinates through trigonometric operations, removing the maximum and minimum values ​​from the height array, and averaging the remaining data to obtain stable height coordinate data; acquiring the optimal height data via ultrasound; combining the optimal horizontal position and the height data to obtain position information; and obtaining accurate position information by performing a tight-combined unscented Kalman fusion of the IMU position information and the IMU position information. This invention uses UWB to reduce the problem of IMU integral errors increasing over time and also adds ultrasound to increase the anti-interference capability of UWB, thereby improving positioning accuracy and obtaining a more precise positioning location.

Description

Technical Field

[0001] This invention belongs to the field of indoor positioning technology for intelligent machines, and specifically relates to an indoor positioning method based on UWB fusion IMU. Background Technology

[0002] In today's era of interconnectedness and continuous advancements in intelligent technology, there is a tremendous demand for location-based services (LBS) applications and low-cost, high-precision positioning and navigation technologies. Positioning and navigation are now widely used in daily life. While GPS satellite navigation and positioning technology can meet most positioning needs in open outdoor environments, in confined and complex indoor environments, GPS suffers from poor signal reception due to building obstructions or large obstruction fields, leading to insufficient navigation accuracy. The development of indoor navigation technology effectively addresses these issues.

[0003] Currently, mainstream indoor navigation technologies primarily utilize IMU (Inertial Measurement Unit) navigation and positioning technology. Inertial navigation is an autonomous navigation technology that does not rely on external information or radiate energy to the outside world, offering advantages such as less susceptibility to external interference. However, due to inherent hardware limitations of the IMU device and its navigation and positioning principle, IMUs accumulate errors over long periods of positioning. Without external calibration, the accuracy and reliability of positioning will decrease. Compared to traditional positioning technologies, UWB (Ultra-Wideband) positioning technology offers advantages such as wide pulse communication, strong resistance to multipath effects, and high ranging and positioning accuracy. However, since most current UWB positioning technologies rely primarily on Time Difference of Arrival (TDOA) algorithms for positioning, they inevitably encounter factors such as Non-Line-of-Sight (NLOS) transmission, which can affect positioning accuracy. Therefore, UWB positioning technology alone cannot meet the high-precision positioning requirements of complex indoor environments. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention proposes an indoor positioning method based on UWB fusion IMU, comprising:

[0005] S1: The attitude is calculated based on the gyroscope, accelerometer and magnetometer of the IMU positioning model to obtain the position information;

[0006] S2: Based on the location information of N base stations in the UWB model and the distance information between the base stations and the tags, the horizontal optimal position is obtained using the iterative weighted least squares algorithm;

[0007] S3: The UWB model constructs a three-dimensional space using the coordinates of a tag and any three base stations. It calculates the height array of the Z-axis in the three-dimensional space coordinates using trigonometric operations. The maximum and minimum values ​​in the height array are removed, and the remaining data is averaged to obtain a relatively stable height coordinate data.

[0008] S4: Obtain optimal height data via ultrasound;

[0009] S5: Combine the optimal horizontal position and height data to obtain position information;

[0010] S6: Accurate coordinate position information is obtained by tightly combining the position information obtained from the IMU with the position information obtained from the IMU using unscented Kalman fusion.

[0011] The beneficial effects of this invention are as follows: This invention reduces the problem of IMU integral error increasing over time by using UWB, and also increases the anti-interference capability of UWB by adding ultrasonic waves, thereby improving positioning accuracy and obtaining a more precise positioning position. Attached Figure Description

[0012] Figure 1 This is a general framework diagram of a novel UWB-fused IMU navigation method according to the present invention;

[0013] Figure 2 This is a framework diagram of a specific method for an IMU positioning module according to the present invention;

[0014] Figure 3 This is a framework diagram of the specific method for ultrasonic fusion UWB in this invention;

[0015] Figure 4 This is a three-dimensional spatial construction diagram of the present invention;

[0016] Figure 5 This is a schematic diagram of the iterative weighted least squares method in this invention; Detailed Implementation

[0017] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] An indoor positioning method based on UWB fusion IMU, such as Figure 1 As shown, it includes:

[0019] S1: The attitude is calculated based on the gyroscope, accelerometer and magnetometer of the IMU positioning model to obtain the position information;

[0020] S2: Based on the location information of N base stations in the UWB model and the distance information between the base stations and the tags, the horizontal optimal position (x,y) is obtained using the iterative weighted least squares algorithm;

[0021] S3: The UWB model constructs a three-dimensional space using the coordinates of a tag and any three base stations. It calculates the height array of the Z-axis in the three-dimensional space coordinates using trigonometric operations. The maximum and minimum values ​​in the height array are removed, and the remaining data is averaged to obtain a relatively stable height coordinate data.

[0022] S4: Obtain optimal height data via ultrasound;

[0023] S5: Combine the optimal horizontal position and height data to obtain position information;

[0024] S6: Accurate coordinate position information is obtained by tightly combining the position information obtained from the IMU with the position information obtained from the IMU using unscented Kalman fusion.

[0025] Attitude calculations are performed using the gyroscope, accelerometer, and magnetometer of the IMU positioning model to obtain position information, such as... Figure 2 As shown, it specifically includes:

[0026] S11: Perform high-pass filtering on the gyroscope, low-pass filtering on the accelerometer, and initial calibration and tilt compensation on the magnetometer, respectively;

[0027] S12: The attitude is calculated by using the triaxial acceleration data from the accelerometer and the triaxial magnetic data from the magnetometer in a stationary state, and the initial heading angle is obtained.

[0028] S13: The quaternion is updated based on the filtered gyroscope data and angular velocity using the fourth-order Runge-Kutta algorithm. The updated quaternion is then converted using the quaternion conversion equation to obtain the heading angle during motion.

[0029] S14: Calculate the coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system based on the object's heading angle information. Transform the matrix into standard form. The actual acceleration data of the object is obtained by converting the acceleration data expression and removing gravitational acceleration. The estimated position is then obtained by integrating the actual acceleration data of the object.

[0030] Attitude calculations are performed using triaxial acceleration data from the accelerometer and triaxial magnetic data from the magnetometer in a stationary state to obtain the initial heading angle, expressed as:

[0031]

[0032] Where γ, θ, and ψ are the roll angle, pitch angle, and heading angle relative to true north, respectively. These are triaxial acceleration data. These are the triaxial magnetometer data for the magnetometer. The angle between true north and magnetic north is denoted by arctan(), which represents the arctangent function.

[0033] The filtered gyroscope data-angular velocity is converted into quaternions using the fourth-order Runge-Kutta algorithm, and is represented as follows:

[0034]

[0035] Where Q(t) is the updated quaternion at this moment, Q(0) is the quaternion at the previous moment, w(0) is the angular velocity transformation matrix at the previous moment, T is the time difference between the previous moment and the present moment, K1, K2, K3, and K4 are the first, second, third, and fourth Runge-Kutta formulas, respectively, w(t) is the angular velocity matrix at time t, Ψ is the heading angle during the action, and q0, q1, q2, and q3 are the first, second, third, and fourth quaternions in the quaternion, respectively.

[0036] The updated quaternion is transformed using a quaternion transformation equation to obtain the heading angle during motion, expressed as:

[0037]

[0038] Where Ψ is the heading angle during the action, and q0, q1, q2, and q3 are the first, second, third, and fourth quaternions, respectively.

[0039] The coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system is calculated based on the object's heading angle information. Represented as:

[0040]

[0041] Where γ, θ, and ψ are the roll angle, pitch angle, and heading angle relative to true north, respectively.

[0042] Transform the matrix into standard form. Through acceleration data expression conversion, it can be expressed as:

[0043]

[0044] in, These represent the acceleration values ​​along the x, y, and z axes in the carrier coordinate system. These are the acceleration values ​​along the x, y, and z axes in the navigation coordinate system, respectively.

[0045] The actual acceleration data of an object is obtained by removing gravitational acceleration, and is expressed as follows:

[0046]

[0047] in, These are the acceleration values ​​along the x, y, and z axes in the navigation coordinate system after removing gravitational acceleration, where g is the gravitational angular acceleration.

[0048] The estimated position is obtained by integrating the actual acceleration data of the object, and is expressed as:

[0049]

[0050] Where v0 and s0 are the initial velocity and initial displacement, respectively, t is the uniform time, s and v represent the position and velocity at this moment, respectively, and d represents the differential symbol.

[0051] A three-dimensional space is constructed using tags and the coordinates of any three base stations, as shown in this embodiment. Figure 4 As shown, the Z-axis coordinate data (height array) can be obtained by calculating using trigonometric operations. The height array of the Z-axis in 3D space can be obtained by calculating using trigonometric operations:

[0052]

[0053] Among them, h n For the height corresponding to the Nth base station, r n Let x be the distance from the tag to the Nth base station, and let x and y be the optimal position (x, y) of the tag in the previous step. n ,y n ) represents the known location information of the Nth base station.

[0054] like Figure 3 As shown, a multi-sensor fusion positioning model is established by combining UWB and ultrasound. Based on the location information of N base stations in the UWB model and the distance information between the base stations and the tags, the horizontal optimal position is obtained by using an iterative weighted least squares algorithm. Then, the height data is obtained by ultrasound to obtain the UWB location information.

[0055] Assuming each base station is on the same horizontal plane, a distance constructor can be constructed based on the base station's coordinates and the distance from its label to each base station. This distance constructor is used to construct the matrix form later, which is essential for obtaining the values ​​in the subsequent least squares function. The values ​​of A and b;

[0056] By constructing N functions and subtracting the previous function from the next, a system of N-1 equations can be established, which can then be transformed into matrix form. Construct the functions based on the coordinates of the base stations and the distance from the tags to each base station:

[0057]

[0058] in, This represents the distance function constructed based on the base station coordinates and the tag, where x and y are the optimal tag positions (x, y) from the previous step. n ,y n ) represents the known location information of the Nth base station.

[0059] Based on the location information of N base stations in the UWB model and the distance information between the base stations and the tags, the horizontally optimal location is obtained using an iterative weighted least squares algorithm, specifically including, for example... Figure 5 As shown:

[0060] S21: Estimate the initial value of the iteration using the least squares method. And calculate the initial residual e. 0 ;

[0061] S22: Standardized residuals yield u i Set the initial weight W i ;

[0062] S23: Obtained using the weighted least squares expression to replace And calculate the new residual v 1 ;

[0063] S24: Repeat S22-S23 iteratively until the difference between the regression coefficients of two adjacent steps is reached. When the maximum absolute value is less than the standard error of the iteration, the iteration ends and the optimal position coordinates (x, y) of UWB on the horizontal plane are obtained.

[0064] The expression for the least squares method:

[0065]

[0066] in, This represents the location information predicted by the least squares method. T Let A represent the transpose of the matrix, where A is the matrix form of the training samples and b is the matrix form of the target values.

[0067] The standardized residual expression is:

[0068]

[0069] Among them, u i Let represent the standardized residuals, s represent the scaling estimate, and s is generally taken as the median of the absolute values ​​of the residuals divided by a constant 0.6745. med is calculated using the median, and e... i This is the residual.

[0070] The initial weight expression is:

[0071]

[0072] Among them, u i To standardize the residuals, W i For iterative weights, This indicates the derivative with respect to x, where x is the location information measured by UWB.

[0073] The weighted least squares expression is:

[0074]

[0075] Where W is the weighting matrix, This represents the location information predicted by the weighted least squares method. T Let A represent the transpose of the matrix, where A is the matrix form of the training samples and b is the matrix form of the target values.

[0076] Obtaining optimal height data via ultrasound specifically includes:

[0077] S41: Perform mean filtering on the ultrasonic waves to obtain smoother ultrasonic data for measuring height.

[0078] S42: Average the relatively stable height coordinate data obtained from the UWB model with the ultrasonic data, and use the difference between the average value and the optimal height data of the previous moment as the set threshold.

[0079] S43: Calculate the difference between the ultrasonic data and the UWB height coordinate data to obtain the optimal height data;

[0080] If the difference is less than the set threshold, the data will be subjected to extended Kalman fusion to obtain the optimal UWB height data;

[0081] If the difference is greater than the set threshold, the difference between the current height data obtained by ultrasound, the current height data of UWB, and the optimal height data of the previous moment are calculated separately. The two differences are then substituted into the coefficient equation with different weights to calculate the optimal height difference data of UWB. The optimal height data of the current moment is obtained by adding the optimal height difference data of the previous moment to the optimal height data.

[0082] The optimal UWB height difference data is calculated by substituting the two differences into a coefficient equation with different weights:

[0083]

[0084] Wherein, at this moment Δh i h is the optimal height data difference. i-1 This is the optimal height data from the previous moment. This is the mean UWB height data at this moment. For the mean height data of ultrasound at this moment, k1 and k2 are adaptively set weighting coefficients (k1+k2=1).

[0085] The formula for setting adaptive weights is:

[0086]

[0087]

[0088] in This is the average UWB height data from the previous moment. This is the average height data of the ultrasound waves at the previous moment.

[0089] Unscented Kalman fusion is performed by tightly combining the location information obtained from the IMU and the location information obtained from the IMU.

[0090] Based on the IMU, the system state equations are expressed in matrix form:

[0091] X(k+1) = FX(k) + GW(k)

[0092] W(k)=[w x (k),w y (k)] T

[0093] Where X(k+1) represents the system state matrix at time k+1, F represents the system state transition matrix, X(k) represents the process noise driving matrix, G represents the process noise matrix, W(k) is the process noise matrix, and X(k) represents the system state matrix at time k. x (k) represents the noise at the x-axis position coordinate, w y (k) represents the noise of the y-axis position coordinate, T represents the matrix transpose, and the mean of the process noise matrix is ​​0, with a variance of . This represents the variance of acceleration along the x-axis. The y-axis acceleration variance is represented by `diag()`, which represents a diagonal matrix.

[0094] The observation equation expression is obtained from UWB:

[0095]

[0096] Where Z(k) represents the prediction matrix at time k, h[X(k)] is the nonlinear observation function of the true distance, X(k) represents the system state matrix at time k, V(k) is the distance observation noise matrix, d1(k), d2(k), and d3(k) represent the distances from the tag to the three base stations, respectively, and v1(k), v2(k), and v3(k) represent the interference error values ​​of the distances from the tag to the three base stations, respectively.

[0097] Using the UT transform to sample the Sigma points of a set of system state equations, we obtain:

[0098]

[0099] in, Let λ be the first mean, P(k|k) be the variance, n be the dimension, and λ be the scaling factor. The larger λ is, the further the Sigma point is from the mean of the state; the smaller λ is, the closer the Sigma point is to the mean of the state.

[0100] One-step prediction of the state based on the system state equation:

[0101]

[0102] in, For the predicted system state values, FX (i) (k|k) represents the system state transition matrix multiplied by the system state value;

[0103] Then, by using the UT transform to update the Sigma sampling points, we obtain:

[0104]

[0105] in, Let P(k+1|k) be the second mean, P(k+1|k) be the variance, n be the dimension, and λ be the scaling factor.

[0106] Furthermore, a one-step prediction is performed on the system observation equations to obtain...

[0107]

[0108] in, These are the predicted observations;

[0109] Then obtain the autocovariance matrix of the observation vector. The cross-covariance matrix of the observation vector and the state vector

[0110]

[0111]

[0112] in, R represents the variance of the predicted observations, and R is the Gaussian white noise interference.

[0113] The Kalman filter gain is obtained from the above matrix:

[0114]

[0115] in, Given the observation vector autocovariance matrix and the observation vector, Let K be the cross-covariance matrix of the state vectors, and K be the Kalman gain.

[0116] Finally, the system state and state covariance matrix are updated based on the Kalman filter gain.

[0117] In this example, the UKF algorithm is used to fuse the final accurate coordinate position information. This is because the EKF algorithm uses a first-order Taylor series expansion, converting nonlinearity into linearity, which introduces nonlinearity processing errors. The UKF algorithm, on the other hand, uses a UT transform, eliminating the need for approximation of the nonlinear system, thus resulting in higher accuracy.

[0118] During the process, not only is UWB used to reduce the problem of IMU integral error increasing over time, but ultrasound is also added to increase the anti-interference capability of UWB.

[0119] This improves positioning accuracy and yields a more precise location.

[0120] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. An indoor positioning method based on UWB fusion IMU, characterized in that, include: S1: The attitude is calculated based on the gyroscope, accelerometer and magnetometer of the IMU positioning model to obtain the IMU position information; S2: Based on the location information of N base stations in the UWB model and the distance information between the base stations and the tags, the horizontal optimal position is obtained using the iterative weighted least squares algorithm; S3: The UWB model constructs a three-dimensional space using the coordinates of a tag and any three base stations. It calculates the height array of the Z-axis in the three-dimensional space coordinates using trigonometric operations. The maximum and minimum values ​​in the height array are removed, and the remaining data is averaged to obtain a relatively stable height coordinate data. S4: Obtain optimal height data via ultrasound; S5: Combine the optimal horizontal position and height data to obtain UWB position information; S6: Accurate coordinate position information is obtained by tightly combining the position information obtained from the IMU with the position information obtained from the IMU using unscented Kalman fusion.

2. The indoor positioning method based on UWB fusion IMU according to claim 1, characterized in that, Attitude calculations are performed using the gyroscope, accelerometer, and magnetometer of the IMU positioning model to obtain position information, specifically including: S11: Perform high-pass filtering on the gyroscope, low-pass filtering on the accelerometer, and initial calibration and tilt compensation on the magnetometer, respectively; S12: The attitude is calculated by using the triaxial acceleration data from the accelerometer and the triaxial magnetic data from the magnetometer in a stationary state, and the initial heading angle is obtained. S13: The quaternion is updated based on the filtered gyroscope data and angular velocity using the fourth-order Runge-Kutta algorithm. The updated quaternion is then converted using the quaternion conversion equation to obtain the heading angle during motion. S14: Calculate the coordinate transformation matrix from the vehicle coordinate system to the navigation coordinate system based on the object's heading angle information. Transform the matrix into standard form. The actual acceleration data of the object is obtained by converting the acceleration data expression and removing gravitational acceleration. The estimated position is then obtained by integrating the actual acceleration data of the object.

3. The indoor positioning method based on UWB fusion IMU according to claim 2, characterized in that, Attitude calculations are performed using triaxial acceleration data from the accelerometer and triaxial magnetic data from the magnetometer in a stationary state to obtain the initial heading angle, expressed as: Where γ, θ, and ψ are the roll angle, pitch angle, and heading angle relative to true north, respectively. These are triaxial acceleration data. These are the triaxial magnetometer data for the magnetometer. The angle between true north and magnetic north is denoted by arctan(), which represents the arctangent function.

4. The indoor positioning method based on UWB fusion IMU according to claim 2, characterized in that, The quaternion is updated based on the filtered gyroscope data and angular velocity using the fourth-order Runge-Kutta algorithm, as follows: The updated quaternion is transformed using a quaternion transformation equation to obtain the heading angle during motion, expressed as: Where Q(t) is the updated quaternion at this moment, Q(0) is the quaternion at the previous moment, w(0) is the angular velocity transformation matrix at the previous moment, T is the time difference between the previous moment and the present moment, K1, K2, K3, and K4 are the first, second, third, and fourth Runge-Kutta formulas, respectively, w(t) is the angular velocity matrix at time t, Ψ is the heading angle during the action, and q0, q1, q2, and q3 are the first, second, third, and fourth quaternions in the quaternion, respectively.

5. The indoor positioning method based on UWB fusion IMU according to claim 2, characterized in that, The coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system is calculated based on the object's heading angle information. Represented as: Where γ, θ, and ψ are the roll angle, pitch angle, and heading angle relative to true north, respectively.

6. The indoor positioning method based on UWB fusion IMU according to claim 2, characterized in that, Transform the matrix into standard form. Through acceleration data expression conversion, it can be expressed as: The actual acceleration data of an object is obtained by removing gravitational acceleration, and is expressed as follows: The estimated position is obtained by integrating the actual acceleration data of the object, and is expressed as: in, These represent the acceleration values ​​along the x, y, and z axes in the carrier coordinate system. These represent the acceleration values ​​along the x, y, and z axes in the navigation coordinate system. , respectively, are the acceleration values ​​of the x, y, and z axes of the navigation coordinate system after removing gravitational acceleration, g is the gravitational acceleration, v0 and s0 are the initial velocity and initial displacement, t is the unified time, s and v represent the position and velocity at this moment, and d represents the differential sign.

7. The indoor positioning method based on UWB fusion IMU according to claim 1, characterized in that, Based on the location information of N base stations in the UWB model and the distance information between the base stations and the tags, the horizontally optimal location is obtained using an iterative weighted least squares algorithm, specifically including: S21: Estimate the initial value of the iteration using the least squares method. And calculate the initial residual e. 0 ; S22: Standardized residuals yield u i Set the initial weight W i ; S23: Obtained using the weighted least squares expression to replace And calculate the new residual v 1 ; S24: Repeat S22-S23 iteratively until the difference between the regression coefficients of two adjacent steps is reached. When the maximum absolute value is less than the standard error of the iteration, the iteration ends and the optimal position coordinates (x, y) of UWB on the horizontal plane are obtained.

8. The indoor positioning method based on UWB fusion IMU according to claim 1, characterized in that, Obtaining optimal height data via ultrasound specifically includes: S41: Perform mean filtering on the ultrasonic waves to obtain smoother ultrasonic data for measuring height. S42: Average the relatively stable height coordinate data obtained from the UWB model with the ultrasonic data, and use the difference between the average value and the optimal height data of the previous moment as the set threshold. S43: Calculate the difference between the ultrasonic data and the UWB height coordinate data to obtain the optimal height data; If the difference is less than the set threshold, the data will be subjected to extended Kalman fusion to obtain the optimal UWB height data; If the difference is greater than the set threshold, the difference between the current height data obtained by ultrasound, the current height data of UWB, and the optimal height data of the previous moment are calculated separately. The two differences are then substituted into the coefficient equation with different weights to calculate the optimal height difference data of UWB. The optimal height data of the current moment is obtained by adding the optimal height difference data of the previous moment to the optimal height data.

9. The indoor positioning method based on UWB fusion IMU according to claim 1, characterized in that, The optimal UWB height difference data is calculated by substituting the two differences into a coefficient equation with different weights: Where, Δh i h is the optimal height data difference. i-1 This is the optimal height data from the previous moment. This is the mean UWB height data at this moment. For the mean height data of the ultrasound at this moment, k1 and k2 are two set weighting coefficients. This is the average UWB height data from the previous moment. This is the average height data of the ultrasound waves at the previous moment.

10. The indoor positioning method based on UWB fusion IMU according to claim 1, characterized in that, Unscented Kalman fusion, which involves a tight combination of location information obtained from IMU and location information obtained from UWB, specifically includes: S61: The system state equations in matrix form are derived from the IMU, and the system observation equations are obtained from the UWB. S62: The Sigma sampling points are obtained by transforming a set of system state equations using the UT transform. A one-step prediction of the state is then performed based on the system state equations. The Sigma sampling points are then updated using the UT transform again, and a one-step prediction of the system observation equations is performed to obtain... Then obtain the autocovariance matrix of the observation vector. The cross-covariance matrix of the observation vector and the state vector S63: Calculate the Kalman filter gain based on the above matrix, and update the system state and state covariance matrix based on the Kalman filter gain to obtain accurate coordinate position information.

Images