CN116380052A - Indoor positioning method based on UWB fusion IMU - Google Patents
Indoor positioning method based on UWB fusion IMU Download PDFInfo
- Publication number
- CN116380052A CN116380052A CN202211595905.9A CN202211595905A CN116380052A CN 116380052 A CN116380052 A CN 116380052A CN 202211595905 A CN202211595905 A CN 202211595905A CN 116380052 A CN116380052 A CN 116380052A
- Authority
- CN
- China
- Prior art keywords
- data
- uwb
- imu
- height
- position information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 230000004927 fusion Effects 0.000 title claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 238000012935 Averaging Methods 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 44
- 230000001133 acceleration Effects 0.000 claims description 40
- 238000006243 chemical reaction Methods 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 5
- 230000010354 integration Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 230000009471 action Effects 0.000 claims description 2
- 238000006073 displacement reaction Methods 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000010276 construction Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000012549 training Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/01—Determining conditions which influence positioning, e.g. radio environment, state of motion or energy consumption
- G01S5/012—Identifying whether indoors or outdoors
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
- G01S5/0258—Hybrid positioning by combining or switching between measurements derived from different systems
- G01S5/02585—Hybrid positioning by combining or switching between measurements derived from different systems at least one of the measurements being a non-radio measurement
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
The invention belongs to the technical field of indoor positioning, and particularly relates to an indoor positioning method based on UWB fusion IMU, which comprises the following steps: carrying out gesture calculation according to the IMU positioning model to obtain position information; obtaining a horizontal optimal position by using an iterative weighted least square algorithm according to the UWB model; obtaining a height array in the three-dimensional space coordinate through triangle operation, removing the maximum and minimum values of the height array, and averaging the rest data to obtain stable height coordinate data; acquiring optimal height data through ultrasonic waves; combining the horizontal optimal position and the height data to obtain position information; and obtaining accurate position information by carrying out unscented Kalman fusion on the IMU position information and the IMU position information in a tight combination way. According to the invention, the problem that the IMU integral error is larger and larger along with time is reduced through UWB, and the anti-interference capability of UWB is increased through adding ultrasonic waves, so that the positioning accuracy is improved, and a more accurate positioning position is obtained.
Description
Technical Field
The invention belongs to the technical field of intelligent machine indoor positioning, and particularly relates to an indoor positioning method based on UWB fusion IMU.
Background
In the current age of continuous progress of intelligent technology in the world of interconnection of things. There is a great need for Location Based Services (LBS) applications and for low cost, high precision positioning and navigation techniques. Positioning navigation is now widely used in life applications. Under the outdoor wide environment, the satellite navigation and positioning technology of the GPS can meet most positioning requirements, however, under the indoor narrow and complex environment, the GPS has poor signal receiving caused by the influence of building shielding or large shielding fields, so that the problems of insufficient navigation precision and the like are caused, and the development of the indoor navigation technology just makes up the problems.
The main stream indoor navigation technology mainly applies an IMU navigation positioning technology, and inertial navigation is an autonomous navigation technology which does not depend on external information and does not radiate energy to the outside, and has the advantages of less external interference and the like. However, due to the hardware problem of the IMU device and the navigation positioning principle thereof, accumulated errors can occur in the long-time positioning of the IMU, and if the external equipment is not used for calibrating the positioning of the IMU, the positioning accuracy and the positioning reliability can be reduced. Compared with the traditional positioning technology, the UWB positioning technology adopts the advantages of wide pulse communication technology, strong multipath effect resistance, high ranging positioning precision and the like. However, most UWB positioning technologies mainly rely on time difference of arrival (TDOA) algorithm to realize positioning, which results in unavoidable influence on positioning accuracy due to NLOS (non line of sight transmission), so that it appears that a single UWB positioning technology is actually a high-accuracy positioning that cannot meet indoor complex environments.
Disclosure of Invention
In order to solve the technical problems, the invention provides an indoor positioning method based on UWB fusion IMU, comprising the following steps:
s1: according to the gyroscope, the accelerometer and the magnetometer of the IMU positioning model, carrying out attitude calculation to obtain position information;
s2: obtaining horizontal optimal positions by using an iterative weighted least square algorithm according to N base station position information of the UWB model and distance information between the base station and the tag;
s3: constructing a three-dimensional space by the UWB model through the coordinates of the tag and any three base stations, calculating in a triangular operation mode to obtain a Z-axis height array in the three-dimensional space coordinates, removing the maximum and minimum values of data in the height array, and then averaging the rest data to obtain more stable height coordinate data;
s4: acquiring optimal height data through ultrasonic waves;
s5: combining the horizontal optimal position and the height data to obtain position information;
s6: and carrying out unscented Kalman fusion on the position information obtained by the IMU and the position information obtained by the IMU in a tightly combined mode to obtain accurate coordinate position information.
The invention has the beneficial effects that: according to the invention, the problem that the IMU integral error is larger and larger along with time is reduced through UWB, and the anti-interference capability of UWB is increased through adding ultrasonic waves, so that the positioning accuracy is improved, and a more accurate positioning position is obtained.
Drawings
FIG. 1 is a general frame diagram of a novel UWB fusion IMU navigation method of the present invention;
FIG. 2 is a block diagram of an exemplary method for locating an IMU in accordance with the present invention;
FIG. 3 is a block diagram of an embodiment of the method for ultrasonic fusion UWB in the present invention;
FIG. 4 is a three-dimensional space configuration diagram in accordance with the present invention;
FIG. 5 is a schematic diagram of an iterative weighted least squares process in accordance with the present invention;
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
An indoor positioning method based on UWB fusion IMU, as shown in figure 1, comprises the following steps:
s1: according to the gyroscope, the accelerometer and the magnetometer of the IMU positioning model, carrying out attitude calculation to obtain position information;
s2: obtaining horizontal optimal positions (x, y) by using an iterative weighted least square algorithm according to the position information of N base stations of the UWB model and the distance information between the base stations and the labels;
s3: constructing a three-dimensional space by the UWB model through the coordinates of the tag and any three base stations, calculating in a triangular operation mode to obtain a Z-axis height array in the three-dimensional space coordinates, removing the maximum and minimum values of data in the height array, and then averaging the rest data to obtain more stable height coordinate data;
s4: acquiring optimal height data through ultrasonic waves;
s5: combining the horizontal optimal position and the height data to obtain position information;
s6: and carrying out unscented Kalman fusion on the position information obtained by the IMU and the position information obtained by the IMU in a tightly combined mode to obtain accurate coordinate position information.
According to the gyroscope, the accelerometer and the magnetometer of the IMU positioning model, attitude calculation is performed to obtain position information, as shown in fig. 2, specifically including:
s11: respectively carrying out high-pass filtering on the gyroscope, low-pass filtering on the accelerometer, and carrying out initial calibration and inclination angle compensation on the magnetometer;
s12: carrying out attitude calculation through triaxial acceleration data of the accelerometer and triaxial magnetic force data of the magnetometer in a static state to obtain an initial course angle;
s13: updating quaternions according to the filtered gyroscope data-angular velocity through a fourth-order Longer-Kutta algorithm, and converting the updated quaternions into heading angles in motion through a quaternion conversion equation;
s14: calculating a coordinate conversion matrix from a carrier coordinate system to a navigation coordinate system according to course angle information of an object
Conversion matrix of label->
The actual acceleration data of the object is obtained by converting the acceleration data expression and removing the gravitational acceleration, and the estimated value of the position is obtained by the actual acceleration data of the object according to the acceleration integration.
And carrying out attitude calculation through the triaxial acceleration data of the accelerometer and the triaxial magnetic force data of the magnetometer in a static state to obtain an initial course angle, wherein the initial course angle is expressed as:
wherein, gamma, theta and psi are the roll angle, the pitch angle and the heading angle relative to true north of the attitude angle respectively,
three-axis acceleration data, respectively +.>
Triaxial magnetometer data, respectively magnetometer, < >>
Arctan () represents an arctan function, which is the angle between true north and magnetic north.
Converting the filtered gyroscope data-angular velocity into quaternions through a fourth-order Dragon-Kutta algorithm, and representing the quaternions as follows:
wherein Q (T) is the updated quaternion at the moment, Q (0) is the quaternion at the last moment, w (0) is the angular velocity conversion matrix at the last moment, T is the time difference between the last moment and the moment, and K 1 、K 2 、K 3 、K 4 Respectively the first, second, third and fourth formulas of Dragon-Kutta, w (t) is an angular velocity matrix at the moment t, ψ is a course angle during action, q 0 、q 1 、q 2 、q 3 The first element, the second element, the third element and the fourth element in the quaternions are respectively.
Converting the updated quaternion into a course angle in motion through a quaternion conversion equation, wherein the course angle is expressed as follows:
wherein ψ is the course angle during the motion, q 0 、q 1 、q 2 、q 3 The first element, the second element, the third element and the fourth element in the quaternions are respectively.
Calculating a carrier coordinate system to a guide according to the course angle information of the objectCoordinate transformation matrix of aviation coordinate system
Expressed as:
wherein, gamma, theta and psi are the roll angle, pitch angle and relative north course angle of the attitude angle respectively.
Conversion matrix of the label
By the acceleration data expression conversion, expressed as:
wherein,,
acceleration values of x, y and z axes in the carrier coordinate system respectively, < >>
Acceleration values of x, y and z axes in a navigation coordinate system are respectively obtained.
Removing the gravitational acceleration to obtain actual acceleration data of the object, wherein the actual acceleration data is expressed as:
wherein,,
and g is the gravity angular acceleration, which is the acceleration value of the x, y and z axes under the navigation coordinate system after the gravity acceleration is removed.
The estimated value of the position is obtained through the actual acceleration data of the object according to the acceleration integration, and is expressed as:
wherein v is 0 Sum s 0 The initial velocity and initial displacement are respectively, t is a uniform time, s and v respectively represent the position and velocity at the moment, and d represents a differential sign.
A three-dimensional space is constructed by the coordinates of the tag and any three base stations, as shown in fig. 4 in this embodiment, coordinate data- (height group) of the Z axis can be obtained by performing calculation by means of trigonometry, and height group of the Z axis in the three-dimensional space coordinates is obtained by performing calculation by means of trigonometry:
wherein h is n To correspond to the height of the Nth base station, r n For the distance of the tag to the nth base station, x and y are the optimal positions (x, y) of the tag in the previous step (x n ,y n ) Is the known location information of the nth base station.
As shown in fig. 3, a multi-sensor fusion positioning model is established by combining UWB and ultrasonic waves, the horizontal optimal position is obtained by using an iterative weighted least square algorithm according to the N base station position information of the UWB model and the distance information between the base station and the tag, and then the height data is obtained by the ultrasonic waves, so that UWB position information is obtained.
Each base station is arranged on the same horizontal plane, and the distance construction function of each base station can be constructed according to the coordinates of the base station and the distance from the label to the base station, wherein the distance construction function is used for constructing a matrix form at the rear, so that the least square function at the rear can be obtained
A and b values of (a);
n constructors are used for subtracting the last function from the next function to establish N-1 equation sets, and the N equation sets are converted into a matrix form; constructing a function according to the coordinates of the base stations and the distance from the tag to each base station:
wherein,,
representing a distance function constructed from the coordinates of the base station and the tag, x and y being the optimal positions (x, y) of the tag in the previous step (x n ,y n ) Is the location information of the known nth base station.
According to the N base station position information of the UWB model and the distance information between the base station and the tag, the iterative weighted least square algorithm is used to obtain a horizontal optimal position, which specifically includes, as shown in fig. 5:
s21: estimating an iteration initial value by using a least square method
And find the initial residual e 0 ；
S22: normalized residual error to give u i Setting an initial weight W i ；
S23: using a weighted least squares expression
Replace->
And find a new residual v 1 ；
S24: repeating S22-S23 for iterative calculation until the difference value of the regression coefficients of two adjacent steps
And (3) when the maximum value of the absolute value of the UWB is smaller than the iteration standard error, ending the iteration, and obtaining the optimal position coordinate (x, y) of the UWB on the horizontal plane.
Expression of least squares method:
wherein,,
position information representing the least squares prediction, T representing the transpose of the matrix, A being the matrix form of the training samples, b being the matrix form of the target values;
the normalized residual expression is:
wherein u is i Representing normalized residual error, s representing scale estimation, s typically taking the median of the absolute value of the residual error divided by a constant 0.6755, med calculated as the median, e i Is the residual.
The initialization weight expression is:
wherein u is i To normalize the residual error, W i For the iterative weights to be used,
representing the relative x, which is the position information of the UWB measurement, is derived.
The weighted least squares expression is:
wherein W is a weighting matrix,
position information representing the weighted least squares prediction, T representing a matrixTranspose, a is the matrix form of the training samples, and b is the matrix form of the target values.
Obtaining optimal height data through ultrasonic waves specifically comprises the following steps:
s41: the ultrasonic wave is subjected to mean value filtering treatment, and smoother ultrasonic wave data of the measured height are obtained through the ultrasonic wave subjected to the mean value filtering treatment;
s42: averaging the relatively stable height coordinate data and the ultrasonic data obtained by the UWB model, and taking the difference value of the average value followed by the optimal height data at one moment as a set threshold value;
s43: the data of the ultrasonic wave and the UWB height coordinate data are subjected to difference value to obtain optimal height data;
if the difference value is smaller than the set threshold value, carrying out extended Kalman fusion on the data to obtain UWB optimal height data;
if the difference value is larger than the set threshold value, respectively making difference between the height data obtained by ultrasonic wave at the moment and the height data of UWB at the moment and the optimal height data at the moment, taking the two difference values into coefficient equations with different weights to calculate the optimal height difference data of UWB, and adding the optimal height difference data at the moment to the optimal height data at the moment to obtain the optimal height data at the moment.
The two difference values are brought into coefficient equations with different weights to calculate UWB optimal height difference value data:
wherein Δh at this point in time i For the optimal height data difference, h i-1 For the last moment of optimal height data,
for UWB mean height data at this point, +.>
For this moment, the ultrasonic mean height data, k 1 And k 2 Setting for adaptationWeight coefficient (k) 1 +k 2 ＝1)。
The self-adaptive weight setting formula is as follows:
wherein the method comprises the steps of
For the last UWB mean height data, </u >>
The ultrasonic mean height data is the last moment.
The unscented Kalman fusion is carried out by tightly combining the position information obtained by the IMU and the position information obtained by the IMU;
the system state equation expressed in a matrix form is deduced according to the IMU:
X(k+1)＝FX(k)+GW(k)
W(k)＝[w x (k),w y (k)] T
wherein X (k+1) represents a system state matrix at time k+1, F represents a system state transition matrix, X (k) represents a process noise driving matrix, W (k) represents a process noise matrix, X (k) represents a system state matrix at time k, and W x (k) Noise representing x-axis position coordinates, w y (k) Noise representing the y-axis position coordinates, T representing the matrix transposition, the process noise matrix mean value being 0, variance being
Representing x-axis acceleration variance +.>
Representing the y-axis acceleration variance, diag () represents the diagonal matrix;
the expression of the observation equation is obtained by UWB:
wherein Z (k) represents a matrix of predicted values at time k, h [ X (k) ]]X (k) represents a system state matrix at k moment, V (k) represents a distance observation noise matrix, d 1 (k)、d 2 (k)、d 3 (k) Respectively represent the distance from the tag to the three base stations, v 1 (k)、v 2 (k)、v 3 (k) Interference error values respectively representing distances from the tag to the three base stations;
using UT transformation to obtain a set of system state equation Sigma sampling points:
wherein,,
for the first mean, P (k|k) is the variance, n is the dimension, and λ is the scale factor; the larger the lambda, the farther the Sigma point is from the mean of the states, the smaller the lambda, and the closer the Sigma point is to the mean of the states;
and carrying out one-step prediction on the state according to a system state equation:
wherein,,
FX is a predicted system state value (i) (k|k) represents the system state transition matrix multiplied by the system state value;
and updating the Sigma sampling points by utilizing UT conversion to obtain:
wherein,,
for the second mean, P (k+ 1|k) is the variance, n is the dimension, and λ is the scale factor;
and the system observation equation is predicted in one step to obtain
Wherein,,
is a predicted observed value;
then obtaining the observation vector auto-covariance matrix
And observation vector and state vector cross covariance matrix +.>
Wherein,,
to predict observationsVariance, R is Gaussian distributed white noise interference;
obtaining Kalman filtering gain according to the matrix:
wherein,,
for the observation vector autocovariance matrix and the observation vector, < >>
The state vector cross covariance matrix is adopted, and K is Kalman gain;
finally, updating the system state and the state covariance matrix according to the Kalman filtering gain;
in the present example, the UKF algorithm is used to fuse the last accurate coordinate location information, because the EKF algorithm uses a first order Taylor series expansion to convert the nonlinearity into linearity, and there is a nonlinear processing error. The UKF algorithm adopts a UT conversion form, and a nonlinear system does not need to be subjected to approximate processing, so that the precision is higher.
In the motion process, the problem that the IMU has larger and larger integral error along with time is reduced through UWB, and the anti-interference capability of UWB is improved through adding ultrasonic waves;
thereby promote positioning accuracy, obtain more accurate location.
Although embodiments of the present 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 therein without departing from the principles and spirit of the invention, the scope of which is defined in the appended claims and their equivalents.
Claims (10)
1. An indoor positioning method based on UWB fusion IMU is characterized by comprising the following steps:
s1: according to the gyroscope, the accelerometer and the magnetometer of the IMU positioning model, carrying out attitude calculation to obtain IMU position information;
s2: obtaining horizontal optimal positions by using an iterative weighted least square algorithm according to the position information of N base stations of the UWB model and the distance information between the base stations and the labels;
s3: constructing a three-dimensional space by the UWB model through the coordinates of the tag and any three base stations, calculating in a triangular operation mode to obtain a Z-axis height array in the three-dimensional space coordinates, removing the maximum and minimum values of data in the height array, and then averaging the rest data to obtain more stable height coordinate data;
s4: acquiring optimal height data through ultrasonic waves;
s5: combining the horizontal optimal position and the height data to obtain UWB position information;
s6: and carrying out unscented Kalman fusion on the position information obtained by the IMU and the position information obtained by the IMU in a tightly combined mode to obtain accurate coordinate position information.
2. The indoor positioning method based on UWB fusion IMU according to claim 1, wherein the method is characterized in that the attitude calculation is carried out according to a gyroscope, an accelerometer and a magnetometer of an IMU positioning model to obtain the position information, and specifically comprises the following steps:
s11: respectively carrying out high-pass filtering on the gyroscope, low-pass filtering on the accelerometer, and carrying out initial calibration and inclination angle compensation on the magnetometer;
s12: carrying out attitude calculation through triaxial acceleration data of the accelerometer and triaxial magnetic force data of the magnetometer in a static state to obtain an initial course angle;
s13: updating quaternions according to the filtered gyroscope data-angular velocity through a fourth-order Longer-Kutta algorithm, and converting the updated quaternions into heading angles in motion through a quaternion conversion equation;
s14: calculating a coordinate conversion matrix from a carrier coordinate system to a navigation coordinate system according to course angle information of an object
Conversion matrix of label->
The actual acceleration data of the object is obtained by converting the acceleration data expression and removing the gravitational acceleration, and the estimated value of the position is obtained by the actual acceleration data of the object according to the acceleration integration.
3. The indoor positioning method based on UWB fusion IMU according to claim 2, wherein the initial course angle is obtained by performing gesture calculation on three-axis acceleration data of the accelerometer and three-axis magnetic force data of the magnetometer in a static state, and is expressed as:
wherein, gamma, theta and psi are the roll angle, the pitch angle and the heading angle relative to true north of the attitude angle respectively,
three-axis acceleration data, respectively +.>
Triaxial magnetometer data, respectively magnetometer, < >>
Arctan () represents an arctan function, which is the angle between true north and magnetic north.
4. The indoor positioning method based on UWB fusion IMU according to claim 2, wherein the quaternion is updated according to the filtered gyroscope data-angular velocity through a fourth-order Longge-Kutta algorithm, expressed as:
converting the updated quaternion into a course angle in motion through a quaternion conversion equation, wherein the course angle is expressed as follows:
wherein Q (T) is the updated quaternion at the moment, Q (0) is the quaternion at the last moment, w (0) is the angular velocity conversion matrix at the last moment, T is the time difference between the last moment and the moment, and K 1 、K 2 、K 3 、K 4 Respectively the first, second, third and fourth formulas of Dragon-Kutta, w (t) is an angular velocity matrix at the moment t, ψ is a course angle during action, q 0 、q 1 、q 2 、q 3 The first element, the second element, the third element and the fourth element in the quaternions are respectively.
5. The indoor positioning method based on UWB fusion IMU according to claim 2, wherein the coordinate transformation matrix from the carrier coordinate system to the navigation coordinate system is calculated according to the heading angle information of the object
Expressed as:
wherein, gamma, theta and psi are the roll angle, pitch angle and relative north course angle of the attitude angle respectively.
6. Indoor positioning method based on UWB fusion IMU as recited in claim 2Characterized in that the label is converted into matrix
By the acceleration data expression conversion, expressed as:
removing the gravitational acceleration to obtain actual acceleration data of the object, wherein the actual acceleration data is expressed as:
the estimated value of the position is obtained through the actual acceleration data of the object according to the acceleration integration, and is expressed as:
wherein,,
acceleration values of x, y and z axes in the carrier coordinate system respectively, < >>
Acceleration values of x, y and z axes in a navigation coordinate system respectively, < >>
The acceleration values of x, y and z axes of the gravitational acceleration are respectively removed under the navigation coordinate system, g is the gravitational acceleration, v 0 Sum s 0 Respectively an initial velocity and an initial displacement, t is a uniform time, s and v respectively represent the momentSum velocity, d represents the differential sign.
7. The indoor positioning method based on UWB fusion IMU according to claim 1, wherein the method is characterized in that according to N pieces of base station position information of UWB model and distance information between the base station and the tag, a horizontal optimal position is obtained by utilizing an iterative weighted least square algorithm, and specifically comprises the following steps:
s21: estimating an iteration initial value by using a least square method
And find the initial residual e 0 ；
S22: normalized residual error to give u i Setting an initial weight W i ；
S23: using a weighted least squares expression
Replace->
And find a new residual v 1 ；
S24: repeating S22-S23 for iterative calculation until the difference value of the regression coefficients of two adjacent steps
And (3) when the maximum value of the absolute value of the UWB is smaller than the iteration standard error, ending the iteration, and obtaining the optimal position coordinate (x, y) of the UWB on the horizontal plane.
8. The indoor positioning method based on UWB fusion IMU according to claim 1, wherein the optimal height data is obtained by ultrasonic waves, specifically comprising:
s41: the ultrasonic wave is subjected to mean value filtering treatment, and smoother ultrasonic wave data of the measured height are obtained through the ultrasonic wave subjected to the mean value filtering treatment;
s42: averaging the relatively stable height coordinate data and the ultrasonic data obtained by the UWB model, and taking the difference value of the average value followed by the optimal height data at one moment as a set threshold value;
s43: the data of the ultrasonic wave and the UWB height coordinate data are subjected to difference value to obtain optimal height data;
if the difference value is smaller than the set threshold value, carrying out extended Kalman fusion on the data to obtain UWB optimal height data;
if the difference value is larger than the set threshold value, respectively making difference between the height data obtained by ultrasonic wave at the moment and the height data of UWB at the moment and the optimal height data at the moment, taking the two difference values into coefficient equations with different weights to calculate the optimal height difference data of UWB, and adding the optimal height difference data at the moment to the optimal height data at the moment to obtain the optimal height data at the moment.
9. The indoor positioning method based on the UWB fusion IMU according to claim 1, wherein the UWB optimal height difference data is calculated by introducing two difference values into coefficient equations with different weights:
wherein Δh i For the optimal height data difference, h i-1 For the last moment of optimal height data,
for UWB mean height data at this point, +.>
For this moment, the ultrasonic mean height data, k 1 And k 2 For the two weight coefficients to be set,
for the last moment of UWB mean height data,
the ultrasonic mean height data is the last moment.
10. The indoor positioning method based on UWB fusion IMU according to claim 1, wherein the unscented Kalman fusion of the tight combination of the position information obtained by the IMU and the position information obtained by UWB specifically comprises:
s61: according to the IMU, a system state equation expressed in a matrix form is deduced, and a system observation equation is obtained through UWB;
s62: transforming a group of system state equations by utilizing UT transformation to obtain Sigma sampling points, predicting the state in one step according to the system state equations, updating the Sigma sampling points by utilizing UT transformation, and predicting the system observation equations in one step to obtain
Then find the observation vector autocovariance matrix +.>
And observation vector and state vector cross covariance matrix +.>
S63: and obtaining Kalman filtering gain according to the matrix, and finishing system state and state covariance matrix updating according to the Kalman filtering gain to obtain accurate coordinate position information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211595905.9A CN116380052A (en) | 2022-12-13 | 2022-12-13 | Indoor positioning method based on UWB fusion IMU |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211595905.9A CN116380052A (en) | 2022-12-13 | 2022-12-13 | Indoor positioning method based on UWB fusion IMU |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116380052A true CN116380052A (en) | 2023-07-04 |
Family
ID=86971845
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211595905.9A Pending CN116380052A (en) | 2022-12-13 | 2022-12-13 | Indoor positioning method based on UWB fusion IMU |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116380052A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117119586A (en) * | 2023-08-29 | 2023-11-24 | 长春理工大学 | Indoor positioning fusion algorithm based on UWB and IMU |
-
2022
- 2022-12-13 CN CN202211595905.9A patent/CN116380052A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117119586A (en) * | 2023-08-29 | 2023-11-24 | 长春理工大学 | Indoor positioning fusion algorithm based on UWB and IMU |
| CN117119586B (en) * | 2023-08-29 | 2024-05-24 | 长春理工大学 | Indoor positioning fusion method based on UWB and IMU |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN103196445B (en) | Based on the carrier posture measuring method of the earth magnetism supplementary inertial of matching technique | |
| CN110375730A (en) | The indoor positioning navigation system merged based on IMU and UWB | |
| CN110231029B (en) | Underwater robot multi-sensor fusion data processing method | |
| CN111982102B (en) | BP-EKF-based UWB-IMU positioning method in complex environment | |
| CN110686671B (en) | Indoor 3D real-time positioning method and device based on multi-sensor information fusion | |
| CN112197765B (en) | Method for realizing fine navigation of underwater robot | |
| CN114739397B (en) | Mine environment motion inertia estimation self-adaptive Kalman filtering fusion positioning method | |
| US7711516B2 (en) | Method for estimating movement of a solid | |
| CN107289932A (en) | Single deck tape-recorder Kalman Filtering guider and method based on MEMS sensor and VLC positioning fusions | |
| CN113175926B (en) | Self-adaptive horizontal attitude measurement method based on motion state monitoring | |
| CN113503872B (en) | Low-speed unmanned aerial vehicle positioning method based on fusion of camera and consumption-level IMU | |
| CN108007477A (en) | A kind of inertia pedestrian's Positioning System Error suppressing method based on forward and reverse filtering | |
| CN117289322B (en) | High-precision positioning algorithm based on IMU, GPS and UWB | |
| CN111649747A (en) | IMU-based adaptive EKF attitude measurement improvement method | |
| CN116380052A (en) | Indoor positioning method based on UWB fusion IMU | |
| CN113551669B (en) | Combined navigation positioning method and device based on short base line | |
| KR102095135B1 (en) | Method of positioning indoor and apparatuses performing the same | |
| CN115031723A (en) | UWB positioning accuracy improving method | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN111578939B (en) | Robot tight combination navigation method and system considering random variation of sampling period | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN116482735A (en) | High-precision positioning method for inside and outside of limited space | |
| CN116105726A (en) | Multi-sensor fusion type wall climbing robot elevation positioning method | |
| CN115950447A (en) | High-precision alignment method and system for underwater movable base based on magnetic compass and velocimeter | |
| CN115451946A (en) | Indoor pedestrian positioning method combining MEMS-IMU and Wi-Fi |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |