An indoor positioning method and system for non-line-of-sight error compensation based on UWB and IMU

Abstract

The present invention discloses an indoor positioning method for non-line-of-sight error compensation based on UWB and IMU, which overcomes the problem of non-line-of-sight error existing in indoor positioning using UWB and IMU in the prior art, and includes the following steps: dividing an empty indoor storage environment into M grids, and collecting data information in each grid; in an actual indoor storage environment, collecting data information for any logistics path of an AGV cart at each moment; judging the consistency between the empty indoor storage environment and the actual indoor storage environment; according to the judgment result, calculating the final position pos of the target to be measured at the kth moment in different cases k 。 An indoor positioning system for non-line-of-sight error compensation based on UWB and IMU is also provided. It alleviates the influence of non-line-of-sight on errors, improves the positioning accuracy, and achieves the effect of flexible handling and prominent advantages in actual situations.

Description

Technical Field

[0001] The present invention relates to the technical field of indoor positioning and navigation, and particularly relates to an indoor positioning method and system for non-line-of-sight error compensation based on UWB and IMU. Background Art

[0002] For an indoor logistics system to accurately control the position of an Automated Guided Vehicle (AGV) cart, how to mitigate the impact of a non-line-of-sight environment on the positioning information of the actual indoor warehousing environment is the key to ensuring the stable operation of intelligent logistics. Currently, for the positioning technology of AGV cart targets in indoor warehouses, there are infrared rays, laser navigation positioning, ultrasonic guidance, visual sensing, magnetic nail positioning, etc. These technologies have good performance, but are greatly affected by complex indoor environmental factors and cannot well meet the requirements of the positioning and sensing system in a complex environment, having disadvantages such as low positioning accuracy, weak adaptability, and high cost. Compared with traditional positioning technologies, Ultra-Wideband (UWB) technology has become the most widely used wireless indoor positioning technology currently due to its advantages of high multipath resolution, strong penetration, low power consumption, and easy integration.

[0003] An Inertial Navigation Measurement Unit (IMU) automatically performs integral operations on the acceleration of the target carrier to obtain instantaneous velocity and instantaneous position information. As an autonomous navigation system, inertial navigation technology does not depend on the external environment and is not easily interfered with.

[0004] Because in the actual indoor warehousing environment, there are many obstacles such as cargo containers and component shelves, which cause non-line-of-sight errors to UWB, resulting in large positioning errors; the position information of IMU positioning technology is obtained through integration, and the error increases with time accumulation, making it difficult to provide positioning tasks alone for a long time. The current mainstream method is to fuse the two for positioning, realizing the complementary advantages of the two sensors, mitigating the errors caused by non-line-of-sight effects, and achieving precise positioning of the AGV cart target.

[0005] For example, on October 25, 2019, the China Patent Office published an invention named an indoor positioning and navigation system based on the fusion of IMU and UWB, with the publication number CN110375730A. The indoor positioning and navigation system based on the fusion of IMU and UWB of this invention includes an IMU sensor, an IMU position calculation unit, a UWB sensor, a UWB position calculation unit, and a fusion position calculation unit. Using a fusion positioning algorithm, IMU and UWB are combined with each other. The data obtained by IMU is used as the prior information of the Kalman filter, and the data obtained by UWB is used as the observation information of the Kalman filter. By utilizing their respective advantages, the positioning and navigation accuracy of the system can be effectively improved, and high-precision indoor positioning and navigation of the target can be achieved using a small number of observation stations, realizing its application in high-precision indoor positioning and navigation scenario requirements.

[0006] However, this positioning method results in poor real-time performance of the system and slow feedback speed of positioning information. Moreover, when it is recognized that the AGV cart is in a non-line-of-sight situation, the UWB positioning data will be discarded due to large errors, and only the position data calculated by the IMU is used as the positioning information. This will cause the loss of UWB positioning data in the non-line-of-sight situation, and thus the positioning data of a single UWB sensor in the non-line-of-sight situation cannot be compared. Therefore, this method cannot be used as a reliable method for mitigating non-line-of-sight errors. Summary of the Invention

[0007] The object of the present invention is to overcome the problem of non-line-of-sight errors in indoor positioning using UWB and IMU in the prior art, and provide an indoor positioning method and system for compensating non-line-of-sight errors based on UWB and IMU, which comprehensively utilizes the data information obtained by the sensors, and takes different methods according to the line-of-sight conditions in different situations to compensate for the positioning errors caused by non-line-of-sight of the target to be measured, thereby mitigating the influence of non-line-of-sight on errors, improving the positioning accuracy, and achieving the effect of flexible handling and prominent advantages in actual situations.

[0008] To achieve the above object, the present invention adopts the following technical solutions: an indoor positioning method and system for compensating non-line-of-sight errors based on UWB and IMU, which includes the following steps:

[0009] S1: Divide the empty indoor warehousing environment into M grids, and collect data information in each grid;

[0010] S2: In the actual indoor warehousing environment, collect data information for each moment of any logistics path of the AGV cart;

[0011] S3: Judge the consistency between the empty indoor warehousing environment and the actual indoor warehousing environment;

[0012] S4: According to the judgment result, calculate the final position pos of the target to be measured at time k in different cases k .

[0013] The present invention divides the grid in the empty indoor warehousing environment to obtain discretized data information, calibrates the data at each moment with the actual indoor warehousing environment, obtains the line-of-sight conditions in different situations according to the variable value for weighing the consistency degree of the data information and the vector value representing different information, and considers integrating the IMU into the positioning according to the line-of-sight conditions. It comprehensively utilizes the data information obtained by the sensors, takes different methods to position the target to be measured according to the line-of-sight conditions in different situations, thereby mitigating the influence of non-line-of-sight on errors, improving the positioning accuracy, and achieving the effect of flexible handling and prominent advantages in actual situations.

[0014] Preferably, the specific steps of the step S1 are:

[0015] S1.1: Divide the actual empty indoor storage environment into M grids under the condition of meeting the positioning accuracy.

[0016] S1.2: Collect and store data information of the AGV cart with positioning tags in each grid.

[0017] S1.2.1: The data information collected in each grid is represented as a group, and a group of data information includes the ranging values of each base station, the signal strength values received by each base station, and the coordinate values obtained by UWB positioning calculation.

[0018] S1.2.2: Define the ranging values of the base stations for the signal strength information received by the r-th group of grids as the vector p’ r , d’ r and the coordinate value pos' r , where {r|1 ≤ r ≤ M, r ∈ N +}, can be expressed as:

[0019] p r ′ = [p′ 1,r , p′ 2,r , …, p′ n,r T

[0020] d′ r = [d′ 1,r , d′ 2,r , …, d′ n,r T

[0021] In the formula, n represents the number of base stations;

[0022] S1.2.3: In the M groups of grids, respectively set the received signal strength set as P' M , the ranging set as D' M and the coordinate set as Pos' M , which are respectively expressed as:

[0023] P' M = [p’1 p'2...p' r ...p' M

[0024] D' M = [d’1 d'2...d r '...d' M

[0025] Pos' M = [pos’1,pos'2,...pos' r ,...pos'​​​​M T

[0026] P' M 、D' M 、Pos' M respectively store the data information of M groups of grid tags.

[0027] In an empty indoor storage environment, this environment belongs to the line-of-sight condition without obstacles such as shelves and containers being moved in.

[0028] Preferably, the step S2 further includes:

[0029] The data information collected at each moment includes the ranging value of each base station, the signal strength value received by each base station, and the coordinate value obtained by UWB positioning solution; the data information is respectively expressed as:

[0030] p k =[p 1,k , p 2,k , …, p n,k T

[0031] d r =[d 1,k , d 2,k , …, d n,k T

[0032] where p n,k , d n,k respectively represent the data information of the signal strength value and the ranging value obtained by each base station at the k-th moment, p k , d k respectively represent a vector of the signal strength vector obtained by each base station at the k-th moment and the distance vector from the tag, is the tag coordinate value obtained by UWB positioning solution at the k-th moment.

[0033] When obstacles such as shelves and containers are moved in, this is the actual indoor environment at this time.

[0034] Preferably, in the step S3, judge the consistency between the empty indoor storage environment and the actual indoor storage environment:

[0035] By defining the source signal strength variable source ranging variable δ r 、source position variable α r to evaluate the consistency degree of the tag UWB information between the actual storage environment and the empty environment, where:

[0036]

[0037] δ​​​r = ||d k - d’ r ||

[0038] α r = ‖pos k - pos’ r ||;

[0039] Define a new vector p’ s , where {s | 1 ≤ s ≤ M, s ∈ N +}, satisfying:

[0040]

[0041] Obtain the corresponding vector d’ s 、pos' s and three variable values δ s 、α s ;

[0042] Define a new vector d’ l , {l | 1 ≤ l ≤ M, l ∈ N +}, satisfying:

[0043]

[0044] Obtain the corresponding vector p’ l 、pos’ l and three variable values δ l 、 α l ;

[0045] Define a new vector pos’ z , {z | 1 ≤ z ≤ M, z ∈ N +}, satisfying:

[0046]

[0047] Obtain the corresponding vector p' z 、d' z and three variable values α z 、δ z 、

[0048] Preferably, in step S3, it is divided into three cases according to the degree of consistency to distinguish the line-of-sight and occlusion situations in the actual storage environment:

[0049] A1: The base station is in a full line-of-sight situation, define the grid position pos’ v ;

[0050] A2: The base station is incompletely blocked, and the grid position pos’ is defined. v ;

[0051] A3: The base station is severely blocked, and the ranging value d of the base station with non-line-of-sight error at this moment is output. k 。

[0052] If the line-of-sight and occlusion situations are Situation 1 or Situation 2, the average value measurement algorithm is used to estimate the target position; if the line-of-sight and occlusion situations are Situation 3, the UWB and IMU are data-fused based on the Extended Kalman Filter algorithm EKF to obtain the final position of the target at this moment.

[0053] Preferably, in step S4, the final position pos of the target to be measured at time k is calculated according to the judgment result. k :

[0054] S4.1: For the situations of A1 and A2:

[0055] Calculate the grid position coordinates:

[0056]

[0057] The position coordinates solved by the Chan-Taylor positioning algorithm at the k-th moment are The final position pos of the target to be measured at this k-th moment can be obtained by averaging the two position coordinates. k Specifically:

[0058]

[0059] Among them, pos' v =(pos’ v,x , pos’ v,y ), is the position coordinate obtained by Chan-Taylor positioning solution based on UWB;

[0060] S4.2: For the situation of A3:

[0061] S4.2.1: Obtain the position of the navigation coordinate system at the k-th moment obtained by the IMU:

[0062] B1: The IMU obtains the motion parameters of the AGV vehicle including acceleration and angular velocity, and the position and velocity of the AGV vehicle are obtained by double integration according to the IMU positioning solution algorithm.

[0063] B2: The rotation matrix is obtained through coordinate system transformation The acceleration a b on the carrier coordinate system is transformed to obtain the acceleration a n in the navigation coordinate n system. Specifically:

[0064]

[0065] wherein:

[0066]

[0067] represent the accelerations in the horizontal and vertical directions in the carrier coordinate b system;

[0068] B3: When the sampling interval ΔT is short, the carrier target is approximately a uniformly accelerated linear motion. Using Δv n to represent the velocity change of the system in the navigation coordinate n system, we can obtain:

[0069]

[0070] wherein, respectively represent the velocity changes of the system in the horizontal and vertical directions in the navigation coordinate system, represent the accelerations of the system in the horizontal and vertical directions in the navigation coordinate n system;

[0071] B4: Let the velocity at time k - 1 in the navigation coordinate n system be the velocity at time k can be expressed as:

[0072]

[0073] wherein, represent the velocities in the horizontal and vertical directions at time k in the navigation coordinate n system; represent the velocities in the horizontal and vertical directions at time k - 1 in the navigation coordinate n system;

[0074] B5: Let Δpos n be the displacement change in the navigation coordinate n system, specifically:

[0075]

[0076] wherein, respectively represent the displacement changes in the horizontal and vertical directions in the navigation coordinate n system;

[0077] B6: Calculate the actual position of the target to be measured at time k - 1:

[0078] pos k-1 =(pos k-1,x ,pos k-1,y )

[0079]

[0080] Obtain the position of the target to be measured in the navigation coordinate system at time k obtained by the IMU:

[0081]

[0082] S4.2.2: Use the extended Kalman filter, i.e., the EKF algorithm, to fuse and filter the IMU data and the ranging values of UWB with non-line-of-sight errors to obtain the final position pos of the target to be measured at time k k 。

[0083] The acceleration a in the vehicle coordinate system b Refers to the acceleration during the actual movement of the trolley. When in a severely occluded situation, if only the IMU is used for positioning, with the passage of time, there will be an accumulated error, and the positioning accuracy will be relatively low. Therefore, the EKF (extended Kalman filter) algorithm is used to fuse and filter the IMU data and the ranging values of UWB with non-line-of-sight errors.

[0084] Preferably, the step S4.2.2 is specifically expressed as:

[0085] C1: Take the data obtained by the accelerometer of the IMU at time k-1 in the navigation coordinate n system, i.e., the UWB positioning coordinate system As the input of the system, take the ranging value of the base station obtained by the UWB at time k as the observation vector Z k =[d 1,k d 2,k …d n,k T , take the speed and position of the target AGV trolley at time k-1 as the state vector X k-1 =[pos k-1,x pos k-1,y v k-1,x v k-1,y T , according to the EKF principle, establish the system model as follows:

[0086] X k =FX k-1 +Bu k-1 +Gw k-1

[0087] Z k =h[X k +v k

[0088] Among them, F represents the state transition matrix of the system, B is the control input matrix, G represents the noise driving matrix, w k-1 =[w k-x1 ,w k-y ​​​T represents a process noise matrix with a mean of zero and a variance of h[X k = [d 1,k d 2,k …d n,k T represents the non - linear observation function related to the ranging value at time k of the system, v k = [v 1,k v 2,k …v n,k T represents a ranging distance observation noise matrix with a mean of zero and a variance of ;

[0089] C2: Establish the observation equation of the system:

[0090]

[0091] State equation: Z k - Z k-1 = ΔZ k = H k X k +Δv k ;

[0092] C3: Initialize the state mean U(0) = E[X(0)], the state covariance matrix P(0) = var[X(0)], and perform EKF iteration:

[0093] Predict the state:

[0094] Predict the state covariance matrix: P k / k-1 = FP k-1|k-1 F T +GQG T

[0095] where the covariance matrix P k / k-1 of the predicted state is obtained by multiplying the state transition matrix F by the error covariance matrix P k-1 / k-1 at the previous time step and then multiplying by the transpose of the state transition matrix F, i.e., F T , and adding the process noise driving matrix G multiplied by the process noise matrix Q and then multiplied by the transpose of the process noise driving matrix G, i.e., G T .

[0096] Calculate the Kalman filter gain matrix:

[0097] Update the state:

[0098] Update the state covariance matrix: P k|k = [I n ​​-KH k P k|k-1

[0099] Among them, I n is an n×n matrix. The above five steps are used as one calculation cycle of the EKF. Based on the EKF, the UWB and IMU are fused and positioned to obtain the final position pos of the target to be measured at the k-th moment k .

[0100] Taking four base stations as an example, the specific process of establishing the state equation and the observation equation is as follows:

[0101] In the actual situation, when the sampling interval time is ΔT, the system state equation is established as:

[0102]

[0103] Among them, pos k =(pos k,x , pos k,y ), pos k,x , pos k,y respectively represent the horizontal and vertical position coordinate values at the k-th moment, v k,x , v k,y respectively represent the horizontal and vertical velocity values at the k-th moment;

[0104] Convert the equations obtained in C2 into matrix form, then the state equation of the system is:

[0105]

[0106] Among them, the state transition matrix, control input matrix, and noise drive matrix of the system are respectively:

[0107]

[0108] Because the positions of the base stations are known and fixed, let the position coordinates of the base stations be: (x i , y i ), i = 1, 2,... n, where the position of the tag located by the UWB at the k-th moment is The observation equation of the system is:

[0109]

[0110] Among them, the ranging value from the tag to each base station is:

[0111]

[0112] Perform linearization processing. After the first-order Taylor series expansion of the nonlinear function h(·), the Jacobian matrix H k is:

[0113]

[0114] Wherein:

[0115]

[0116] The state equation of the linearized system is obtained as: Z k -Z k-1 = ΔZ k = H k X k + Δv k ;

[0117] Wherein, Δv k = v k - v k-1 .

[0118] Preferably, the signal strength value received by each base station is obtained by the received signal strength algorithm, specifically:

[0119]

[0120] Where p is the signal strength power value of the base station receiving the tag; P CIR is the signal strength power value of the channel impulse response, and N and λ are experimental constant parameters; N refers to the preamble accumulation count value in the register, i.e., PAC.

[0121] Where λ is 115.72 when the PRF (average pulse repetition frequency) is 16 MHz, or 121.74 when the PRF (average pulse repetition frequency) is 64 MHz. In the storage environment of the present invention, the PRF of 16 MHz is preferably adopted, so λ is 115.72.

[0122] Preferably, the coordinate value of the UWB positioning solution is obtained by constructing a nonlinear equation set from the hyperbolic model and then using the Chan-Taylor fusion positioning algorithm, specifically:

[0123] According to: It is obtained that:

[0124] Wherein, R i represents the distance from each base station to the tag position (x, y), (x i , y i ) is the coordinate of base station i, (x, y) is the coordinate of the tag of the target to be measured, and there are n base stations participating in the positioning algorithm. When n > 2, the number of unknowns in the equation set is less than the number of equations in the equation set, which is an overdetermined equation set, and the equation is a nonlinear equation;

[0125] Thus, we obtain:

[0126] G a Z a = h

[0127] Where:

[0128]

[0129] Perform the first LS (Least Squares) estimation to obtain

[0130]

[0131] Where:

[0132] ψ = 4BQB

[0133] B = diag(R1, R2,..., R n )

[0134] e = diag(e1, e2,..., e n )

[0135] Q = E[ee T

[0136] In the formula, e i is the error quantity corresponding to R i .

[0137] Perform the second LS (Least Squares) estimation to obtain:

[0138] Z' a G' a = h'

[0139] Where:

[0140]

[0141] Then the second LS estimation

[0142]

[0143] Where:

[0144]

[0145] Estimate the position of the target label to be measured:

[0146]

[0147] ​Using this position estimate as the initial coordinate value of the Taylor algorithm, performing a Taylor expansion at this position, and ignoring terms of the second order and higher orders, an expression for the error vector can be obtained:

[0148] φ = h d -G d δ d

[0149] Where:

[0150]

[0151] (x i , y i ) represents the position coordinates of the base station, i = 1, 2......n; R i represents the distance from each base station to the initial position (x, y) of the tag, and R i,1 represents the difference between the distance from the tag to be measured to the i-th base station and the distance from the tag to the first base station;

[0152] The WLS (weighted least squares solution) of the expression is:

[0153]

[0154] Where Q is the covariance matrix of the measurement error, and the next recursive initial value is changed to:

[0155] x' = x + Δx

[0156] y' = y + Δy

[0157] Continuously perform recursive iterative calculations according to the above steps until Δx and Δy satisfy the pre-set threshold ε, |Δx| + |Δy| < ε, and the iteration ends to obtain the positioning solution value at the k-th moment

[0158] The Chan-Taylor fusion positioning algorithm uses the Chan algorithm to provide an initial position with relative accuracy for the Taylor series method, and then based on this, Taylor series expansion is carried out to achieve the positioning of the target tag. The Chan algorithm is a non-recursive algorithm that does not require an initial value and can obtain the final result after only two iterations. This algorithm has a high positioning accuracy when the signal is transmitted in the line-of-sight condition and the TDOA measurement value is relatively accurate. However, when the signal is transmitted in the non-line-of-sight condition and the channel performance is poor, the positioning accuracy is low. The Taylor series expansion method is a recursive algorithm that requires an initial estimated position. By continuously recursing to improve the estimated position, it gradually approaches the true value. This algorithm is applicable to various channel environments, but has a large computational complexity and high requirements for the initial estimated position. When the initial estimated position is relatively close to the actual position, the obtained positioning result is more accurate. If the estimated value of the initial position has a large deviation, it directly affects the positioning accuracy of this algorithm. In the indoor environment of a warehouse, there are many obstacles. Although the Chan algorithm is only limited to high positioning accuracy when the signal is transmitted in the line-of-sight, it has great practical significance for determining the initial position of the tag. Although the positioning effect decreases when the channel performance is not excellent, the effect can still reflect the actual situation and can be used as the initial value of the Taylor series method. Using the Chan algorithm to provide a relatively accurate initial value for the Taylor algorithm and based on this, performing Taylor expansion to optimize the defects of the two algorithms has a better effect on the positioning of the AGV cart in the warehouse environment.

[0159] An indoor positioning system based on UWB and IMU for non-line-of-sight error compensation, including: several base stations located in the same indoor, a positioning tag and an IMU module set on the positioning target AGV cart, and also including an upper computer server. The upper computer server includes a data input module for parsing and preprocessing the original data uploaded by the tag and the base station, an algorithm processing module, a positioning result real-time display module for displaying the calculation result, and a data storage module. The upper computer server includes a data input module for parsing and preprocessing the original data uploaded by the tag and the base station, an algorithm processing module, a positioning result real-time display module for displaying the calculation result, and a data storage module. The data input module is connected to the algorithm processing module, and the algorithm processing module is connected to the display result calculation module.

[0160] The data input module includes a data interface and a data parsing module; the positioning result real-time display module includes a display interface that can be optimized; the algorithm processing module includes a Chan-Taylor fusion positioning algorithm, an IMU positioning solution algorithm, and a non-line-of-sight error compensation algorithm; the data storage module includes a data recording and a data storage module.

[0161] Therefore, the present invention has the following beneficial effects: It comprehensively utilizes the data information obtained by the sensors, adopts different methods to locate the target to be measured according to the line-of-sight conditions in different situations, thereby alleviating the influence of non-line-of-sight on the error, improving the positioning accuracy, and achieving the effect of flexible handling and prominent advantages in actual situations. BRIEF DESCRIPTION OF THE DRAWINGS

[0162] Figure 1 is the specific operation flowchart of the method of the present invention;

[0163] Figure 2 is the structural schematic diagram of the system of the present invention;

[0164] Figure 3 is the overall framework structure diagram of the host computer server of the present invention;

[0165] Figure 4 is the flowchart for mitigating the non-line-of-sight error between UWB and IMU of the present invention;

[0166] Figure 5 is the three-point coincidence grid diagram of the completely line-of-sight situation of the present invention;

[0167] Figure 6 is the two-point coincidence grid diagram of the incompletely occluded situation of the present invention;

[0168] Figure 7 is the three-point dispersion grid diagram of the severely occluded situation of the present invention;

[0169] In the figure: 1. Host computer server; 2. Data input module; 3. Algorithm processing module; 4. Real-time display module of positioning result; 5. Data storage module; 6. Data interface; 7. Data parsing module; 8. Optimizable display interface; 9. Chan-Taylor fusion positioning algorithm; 10. IMU positioning calculation algorithm; 11. Non-line-of-sight error compensation algorithm; 12. Data record; 13. Data storage module. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0170] The present invention will be further described in detail below in conjunction with the drawings and the specific embodiments:

[0171] As Figure 1 shown in the embodiment, a method for indoor positioning with non-line-of-sight error compensation based on UWB and IMU can be seen. Its operation process is as follows: Step 1, divide the empty indoor storage environment into M grids, and collect data information in each grid; Step 2, in the actual indoor storage environment, collect data information for each moment of any logistics path of the AGV cart; Step 3, judge the consistency between the empty indoor storage environment and the actual indoor storage environment; Step 4, compensate for the non-line-of-sight error according to the judgment result, and calculate the final position pos of the target to be measured at the kth momentk 。

[0172] The indoor positioning method and system for AGV non-line-of-sight error compensation based on UWB and IMU provided by the present invention discretize the data information obtained by grid division in an empty indoor warehousing environment, calibrate the discrete data information and the actual indoor warehousing environment at each moment, obtain the line-of-sight conditions in different situations according to the variable value for weighing the consistency degree of data information and the vector value representing different information, and consider integrating the IMU into the positioning according to the line-of-sight conditions. This method makes more comprehensive use of the data information obtained by the sensors, adopts different methods to position the target to be measured according to the line-of-sight conditions in different situations, thereby alleviating the influence of non-line-of-sight on the error, improving the positioning accuracy, and achieving the effect of flexible processing and prominent advantages in actual situations.

[0173] Such as Figure 2 In the shown embodiment, an indoor positioning system for non-line-of-sight error compensation based on UWB and IMU can be seen, including: 4 base stations (base station 0, base station 1, base station 2, and base station 3) located in the same indoor and a positioning tag and an IMU module arranged on the positioning target AGV trolley. Such as Figure 2 As shown in the embodiment, there are 3 direct paths and 1 non-direct path. The direct path refers to the line-of-sight situation where there is no obstacle in the signal transmission process between the positioning base station and the positioning tag, and the non-direct path refers to the non-line-of-sight situation where there is an obstacle in the signal transmission process between the positioning base station and the positioning tag.

[0174] Such as Figure 3 In the shown embodiment, it is the overall framework structure diagram of the upper computer server. The upper computer server 1 includes a data input module 2, a positioning result real-time display module 4, an algorithm processing module 3, and a data storage module 5. The data input module includes a data interface 6 and a data parsing module 7; the positioning result real-time display module includes an optimizable display interface 8; the algorithm processing module includes a Chan-Taylor fusion positioning algorithm 9, an IMU positioning solution algorithm 10, and a non-line-of-sight error compensation algorithm 11; the data storage module includes a data record 12 and a data storage module 13.

[0175] The implementation process of the functions of each module is introduced in detail as follows:

[0176] Data input module: Its main function is to parse and preprocess the original data uploaded by the tag and the base station. The data packets to be processed are mainly heartbeat packets with time intervals and positioning data information packets. When the heartbeat packet is received, it can be selected for processing to display some status information of the base station, or it can be directly discarded.

[0177] Algorithm Processing Module: This core includes the Chan-Taylor positioning algorithm based on TDOA, the IMU positioning calculation algorithm, etc. The input values of this algorithm include the incoming measurement data and environment-related configuration data. The measurement data refers to the ranging values of the four base stations to the tag, the signal reception strength, and the acceleration data and angular velocity data measured by the IMU. The environmental configuration refers to the coordinate position of the current base station in the custom coordinate system in this warehouse environment, and the unique identification number of the base station matches the base station number in the measurement data. The algorithm processing module determines the performance of the entire system.

[0178] Real-time Display Module for Positioning Results: The positioning estimation value of the current position of the tag to be positioned output by the algorithm module is sent to the display module via UDP. In this embodiment, two-dimensional display can be achieved by calling the Matlab plotting interface in Java. The principle is to output the Java package file of the custom function in Matlab and import it into the current Java project. In the same way as ordinary Java functions, directly calling the custom function interface can achieve the purpose of Matlab display. However, this is not the only display method. This method can intuitively display the positioning error range and plays a key role in evaluating the performance of the algorithm module.

[0179] Data Storage Module: During the positioning test process, it is necessary to record the information data at each moment of the intermediate process. Recording data has the following advantages: it is convenient to analyze the reasons for error generation to give corresponding error compensation; it is convenient to evaluate the stability and positioning accuracy of the algorithm module. The entire data recording can control the output method of data information: console, file, graphical user interface components, etc., and the data is stored in a compressed txt file.

[0180] Next, continue to further illustrate the technical solution and technical effect of the present invention through specific examples. The following examples are explanations of the present invention and the present invention is not limited to the following examples.

[0181] In this embodiment, an example of an indoor environment containing 4 base stations (Base Station 0, Base Station 1, Base Station 2, and Base Station 3) is taken.

[0182] The first step: Divide the empty indoor warehouse environment into M grids, and collect data information in each grid.

[0183] Combined with Figure 4UWB and IMU Non-line-of-sight Error Mitigation Flowchart. First, in an empty indoor warehousing environment, this environment belongs to the line-of-sight condition without obstacles such as shelves and cabinets being moved in. According to the actual situation of this environment and the condition of meeting the positioning accuracy, M grids are divided, and the AGV cart with positioning tags collects and stores data information in each grid, and the data information collected in each grid is represented as a group. Among them, a group of data information includes the ranging values of four base stations, the signal strength values received by the four base stations, and the coordinate values obtained by UWB positioning solution. The received signal strength value is obtained by the received signal strength algorithm, and the position positioning coordinate is obtained by the algorithm processing module constructing a non-linear equation set according to the hyperbola model and then using the Chan-Taylor fusion positioning algorithm.

[0184] Define the four signal strength information and the ranging values of the four base stations received by the r-th group of grids as vectors p' r , d r ' and the coordinate value pos' r , where {r|1 ≤ r ≤ M, r ∈ N +}, can be expressed as:

[0185] p' r = [p’ 1,r , p' 2,r , p' 3,r , p' 4,r T

[0186] d r ' = [d’ 1,r , d' 2,r , d’ 3,r , d' 4,r T

[0187] The signal strength sets received by the M groups of grids are respectively set as P' M , the ranging sets are D' M and the coordinate sets are Pos' M , which are respectively expressed as:

[0188] P' M = [p’1 p'2...p' r ...p' M

[0189] D' M = [d’1 d'2...d’ r ...d' M

[0190] Pos' M = [pos’1, pos'2,...pos' r ​​​​,...pos' M T

[0191] P' M 、D' M 、Pos' M respectively store the data information of M groups of grid labels.

[0192] Step 2: In the actual indoor warehousing environment, collect data information for any logistics path of the AGV vehicle at each moment

[0193] Combined with Figure 4 the UWB and IMU non-line-of-sight error mitigation flowchart. When moving into obstacles such as shelves and cabinets, it is the actual indoor environment at this time. The system collects data information for any logistics path of the AGV vehicle at each moment, that is, a group of data information is collected at the same time interval. Among them, the data information collected at each moment includes the ranging values of four base stations, the signal strength values received by the four base stations, and the coordinate values obtained by UWB positioning solution; the received signal strength value is obtained by the received signal strength algorithm, and the position positioning coordinate is obtained by the algorithm processing module according to the hyperbolic model to construct a non-linear equation system and then using the Chan-Taylor fusion positioning algorithm.

[0194] The data information is respectively expressed as:

[0195] p k =[p 1,k ,p 2,k ,p 3,k ,p 4,k T

[0196] d k =[d 1,k ,d 2,k ,d 3,k ,d 4,k T

[0197] Among them, p j,k 、d j,k (j = 1, 2, 3, 4) respectively represent the data information of the signal strength value and the ranging value obtained by each base station at the k-th moment, p k 、d k respectively represent a vector as the signal strength vector and the distance vector from the label obtained by each base station at the k-th moment, is the label coordinate value obtained by UWB positioning solution at the k-th moment.

[0198] Step 3: Judge the consistency between the empty indoor warehousing environment and the actual indoor warehousing environment

[0199] The present invention respectively defines the source signal strength variable​​​ Source ranging variable δ r , source position variable α r to evaluate the consistency degree of the tag UWB information between the actual warehousing environment and the open environment.

[0200] The present invention is specifically implemented in the following manner:

[0201]

[0202] δ r = ||d k - d’ r ||

[0203] α r = ‖pos k - pos’ r ||.

[0204] Define a new vector p’ s , where {s|1 ≤ s ≤ M, s ∈ N +}}, satisfying:

[0205]

[0206] Correspondingly, obtain the corresponding vector d’ s , pos' s and three variable values δ s , α s .

[0207] Similarly, define a new vector d l ', {l|1 ≤ l ≤ M, l ∈ N +}}, satisfying:

[0208]

[0209] Correspondingly, obtain the corresponding vector p’ l , pos’ l and three variable values δ l , α l .

[0210] Similarly, define a new vector pos’ z , {z|1 ≤ z ≤ M, z ∈ N +}}, satisfying:

[0211]

[0212] Correspondingly, obtain the corresponding vector p' z , d' z and three variable values αz , δ z ,

[0213] Combined with Figure 4 UWB and IMU non-line-of-sight error mitigation flowchart. The degree of consistency between the warehousing environment and the open environment obtained through the above three variables can be divided into the following three situations to distinguish the line-of-sight and occlusion situations in the actual warehousing environment:

[0214] Situation 1: Combined with Figure 5 The three-point coincidence grid diagram, the three grid positions coincide. At this time, it can be determined that the four base stations are in a complete line-of-sight situation, and the grid position pos' v is defined. Among them, pos' v is the closest to pos' z and is obtained through the Chan-Taylor positioning algorithm.

[0215] Situation 2: Combined with Figure 6 The two-point coincidence grid diagram. At this time, it can be determined that the four base stations are in an incomplete occlusion situation. Select the data information where the two grid positions coincide, and define the grid position pos' v .

[0216] Situation 3: Combined with Figure 7 The three-point dispersed grid diagram, indicating that the tag to be measured is in a non-line-of-sight state during the transmission with at least two base stations, belonging to a serious occlusion situation. Output the ranging value d k with non-line-of-sight error at this moment for the four base stations.

[0217] Fourth step: Compensate for the non-line-of-sight error according to the judgment result, and calculate the final position pos k

[0218] For the above-mentioned Situation 1 and Situation 2, the grid position coordinates pos' are obtained through Equation v .

[0219] At the same time, the position coordinates solved by the Chan-Taylor positioning algorithm at the kth moment are The final position pos k of the target to be measured at the kth moment can be obtained by averaging the two position coordinates, specifically:

[0220]

[0221] Among them, pos'v = (pos'v,x, pos'v,y), The position coordinates obtained by solving the Chan-Taylor positioning according to UWB.

[0222] In the third scenario described above, combined with Figure 4 the UWB and IMU non-line-of-sight error mitigation flowchart, in an actual indoor warehousing environment, the IMU is fixed on the AGV cart, and motion parameters such as acceleration and angular velocity can be measured.

[0223] The algorithm processing module obtains the position and velocity of the AGV cart by double integration according to the IMU positioning algorithm. The rotation matrix is obtained through coordinate transformation The acceleration a b on the carrier coordinate system is converted to obtain the acceleration a n in the navigation coordinate n system, specifically:

[0224]

[0225] where

[0226]

[0227] In the formula, represents the acceleration in the horizontal and vertical directions in the carrier coordinate b system.

[0228] When the sampling interval ΔT is short, the carrier target is approximately a uniformly accelerated linear motion. Therefore, Δv n is used to represent the velocity change of the system in the navigation coordinate n system, and we can get

[0229]

[0230] where respectively represent the velocity changes of the system in the horizontal and vertical directions in the navigation coordinate system, represents the acceleration of the system in the horizontal and vertical directions in the navigation coordinate n system.

[0231] Suppose the velocity at time k - 1 in the navigation coordinate n system is The velocity at time k can be expressed as:

[0232]

[0233] where represent the velocities in the horizontal and vertical directions in the navigation coordinate n system at time k; represent the velocities in the horizontal and vertical directions in the navigation coordinate n system at time k - 1.

[0234] Furthermore, let Δpos n be the displacement change in the navigation coordinate n system, specifically:

[0235]

[0236] Among them, respectively represent the displacement changes in the horizontal and vertical directions in the navigation coordinate n system.

[0237] Furthermore, pos k-1 represents the actual position obtained by the algorithm of the present invention at the (k - 1)th moment. Among them, pos k-1 =(pos k-1,x , pos k-1,y ), Specifically:

[0238]

[0239] Among them, is the position of the navigation coordinate system at the kth moment obtained by the IMU.

[0240] Specifically, in the third case, when in a severe occlusion situation, if only the IMU is used for positioning, with the passage of time, there will be an accumulated error, and the positioning accuracy will be relatively low. Therefore, the EKF (Extended Kalman Filter) algorithm is used to fuse and filter the IMU data and the ranging values of the UWB with non-line-of-sight errors.

[0241] In practical applications, the data obtained by the accelerometer of the IMU at the (k - 1)th moment in the navigation coordinate n system, that is, the UWB positioning coordinate system, is used as the input of the system, and the ranging values of the four base stations obtained by the UWB at the kth moment are used as the observation vector Z k =[d 1,k d 2,k d 3,k d 4,k T , and the speed and position of the target AGV vehicle at the (k - 1)th moment are used as the state vector X k-1 =[pos k-1,x pos k-1,y v k-1,x v k-1,y T , and according to the EKF principle, the system model is established as follows:

[0242] X k =FX k-1 +Bu k-1 +Gw k-1

[0243] Z k =h[X k +v k

[0244] ​​Among them, F represents the state transition matrix of the system, B is the control input matrix, G represents the noise driving matrix, and w k-1 =[w k-1,x w k-1,y T represents the process noise matrix with a mean of zero and a variance of The nonlinear observation function of the system related to the ranging value at time k is h[X k =[d 1,k d 2,k d 3,k d 4,k T and v k =[v 1,k v 2,k v 3,k v 4,k T represents the ranging distance observation noise matrix with a mean of zero and a variance of .

[0245] In actual situations, when the sampling interval time is ΔT, the system state equation is established as follows:

[0246]

[0247] Among them, pos k =(pos k,x , pos k,y ), pos k,x and pos k,y represent the horizontal and vertical position coordinate values at time k, respectively, and v k,x and v k,y represent the horizontal and vertical velocity values at time k, respectively.

[0248] Converting the above equations into matrix form, the state equation of the system is:

[0249]

[0250] Among them, the state transition matrix, control input matrix, and noise driving matrix of the system are:

[0251]

[0252] Since the positions of the four base stations are known and fixed, let the position coordinates of the four base stations be (x i , y i ), i = 1, 2, 3, 4, where the position of the tag located by UWB at time k is The observation equation of the system is:

[0253] ​​​

[0254] Among them, the ranging values from the tag to the four base stations are:

[0255]

[0256] The above equation is non - linear, so it needs to be linearized. After performing a first - order Taylor series expansion on the non - linear function h(·), the Jacobian matrix H can be obtained k as:

[0257]

[0258] Where:

[0259]

[0260] At this time, the state equation of the linearized system can be obtained as:

[0261] Z k -Z k-1 =ΔZ k =H k X k +Δv k

[0262] Among them, Δv k =v k -v k-1 . After determining the state equation and observation equation of the system, according to the EKF (Extended Kalman Filter) algorithm process, first initialize the state mean U(0)=E[X(0)], and the state covariance matrix P(0)=var[X(0)]. I n is an n×n matrix, and the EKF iteration process is given by the following formula:

[0263] 1. Predict the state:

[0264]

[0265] 2. Predict the state covariance matrix:

[0266] P k / k-1 =FP k-1|k-1 F T +GQG T

[0267] 3. Calculate the Kalman filter gain matrix:

[0268]

[0269] 4. Update the state:

[0270]

[0271] 5. Update the state covariance matrix:

[0272] P k|k = [I n - KH k P k|k-1

[0273] The above 5 steps are one calculation cycle of EKF. Based on the above fusion, when ensuring the synchronization of data acquired by UWB and IMU, in the case of serious occlusion when there are more than two non-line-of-sight base stations, at this time, the system fuses UWB and IMU based on EKF to obtain the final position pos of the target to be measured at this k moment. k .

[0274] In the embodiment, the specific method for obtaining the signal strength value received by the base station using the received signal strength algorithm is:

[0275]

[0276] where p is the signal strength power value of the label received by the base station; P CIR is the signal strength power value of the channel impulse response, including information such as the path and attenuation of the channel; N and λ are experimental constant parameters; N refers to the PAC in the register, that is, the preamble accumulation count value. Among them, λ is 115.72 when the PRF (average pulse repetition frequency) is 16 MHz, or λ is 121.74 when the PRF (average pulse repetition frequency) is 64 MHz. In the warehouse environment of this embodiment, the PRF of 16 MHz is adopted, so λ is 115.72.

[0277] The coordinate values obtained by UWB positioning calculation are obtained by constructing a non - linear equation set using the hyperbola model and then adopting the Chan - Taylor fusion positioning algorithm: The non - linear equation set containing the position coordinate information of the target node is constructed using the hyperbola model. The Chan - Taylor fusion positioning algorithm uses the Chan algorithm to provide the initial position with relative accuracy for the Taylor series method, and then based on this, the Taylor series expansion is carried out to achieve the positioning of the target tag. The Chan algorithm is a non - recursive algorithm. It does not require an initial value and can obtain the final result only through two iterations. This algorithm has a high positioning accuracy when the signal is transmitted in the line - of - sight condition and the TDOA measurement value is relatively accurate; however, when the signal is transmitted in the non - line - of - sight condition and the channel performance is poor, the positioning accuracy is low. The Taylor series expansion method is a recursive algorithm that requires an initial estimated position. It improves the estimated position through continuous recursion and gradually approaches the true value. This algorithm is applicable to various channel environments, but has a large amount of calculation and high requirements for the initial estimated position. When the initial estimated position is relatively close to the actual position, the obtained positioning result is more accurate. If the deviation of the estimated value of the initial position is large, it directly affects the positioning accuracy of this algorithm. In the indoor environment of the warehouse, there are many obstacles. Although the Chan algorithm is only limited to high positioning accuracy when the signal is transmitted in the line - of - sight, it has great practical significance for determining the initial position of the tag. Although the positioning effect decreases when the channel performance is not excellent, the effect can still reflect the actual situation and can be used as the initial value of the Taylor series method.

[0278] Using the Chan algorithm to provide a relatively accurate initial value for the Taylor algorithm, and based on this, the Taylor expansion is carried out to optimize the defects of the two algorithms, which has a better effect on the positioning of the AGV car in the warehouse environment. Specifically:

[0279]

[0280] Among them, R i represents the distance from each base station to the tag position (x, y), (x i , y i ) is the coordinate of base station i, (x, y) is the coordinate of the target tag to be measured, and there are n base stations participating in the positioning algorithm. Through the above formula, the following expression can be obtained:

[0281]

[0282] Among them, When n > 2, the number of unknowns in the equation set is less than the number of equations in the equation set, which is an over - determined equation set, and the equation is a non - linear equation. According to the above expression, the following expression can be obtained:

[0283] G a Za = h

[0284] Where:

[0285]

[0286] Perform the first LS (Least Squares) estimation to obtain

[0287]

[0288] Where:

[0289] ψ = 4BQB

[0290] B = diag(R1, R2,..., R n )

[0291] e = diag(e1, e2,..., e n )

[0292] Q = E[ee T

[0293] Where e i is the error quantity corresponding to R i . In the first LS, the prerequisite is to assume that the three unknowns x, y, and K in the Z a matrix are independent of each other. The second LS constructs a new system of equations using the internal relationships of the three unknowns and considers the obtained in the first time with a certain error to obtain the expression:

[0294] Z' a G' a = h'

[0295] Where:

[0296]

[0297] Obtain the second LS estimation

[0298]

[0299] Where:

[0300]

[0301] Among them, ψ’ represents the error mean of the second LS, represents the covariance value of the first LS (Least Squares) estimated value of, represents the coordinate values of the horizontal x and vertical y of the first LS (Least Squares) estimated value. ​

[0302] Position estimation of the target tag to be measured:

[0303]

[0304] Taking this position estimation as the initial coordinate value of the Taylor algorithm, performing Taylor expansion at this position, and ignoring the terms of the second order and above, the expression of the error vector can be obtained:

[0305] φ = h d -G d δ d

[0306] Where:

[0307]

[0308] Among them, (x i , y i ) represents the position coordinates of the base station, i = 1, 2......n; R i represents the distance from each base station to the initial position (x, y) of the tag, and R i,1 represents the difference between the distance from the tag to be measured to the i-th base station and the distance from the tag to the first base station. Then the WLS (weighted least squares solution) of the expression is:

[0309]

[0310] Among them, Q is the covariance matrix of the measurement error, and the next recursive initial value is changed to:

[0311] x’ = x + Δx

[0312] y' = y + Δy

[0313] Keep recursively iterating and calculating according to the above steps until Δx and Δy satisfy the pre-set threshold value ε, |Δx| + |Δy| < ε, and the iteration ends to obtain the positioning solution value pos k,uwb .

[0314] The present invention combines UWB with an IMU sensor to mitigate the non-line-of-sight error of a positioning target. Under the condition of meeting the positioning accuracy, the grid division is carried out in an empty indoor warehousing environment, and the positioning data information of the grid and the data information at each moment in the actual indoor warehousing environment are calibrated. Based on different indexes of the source signal strength variable, the source ranging value variable, and the source position variable, the consistency degree between the information obtained at the current moment in the actual warehousing environment and the corresponding grid in the empty environment is evaluated. At the same time, through the above source signal variable, source ranging value variable, and source position variable, according to the overlapping degree of the corresponding grid positions of these three indexes, three situations of full line of sight, incomplete line of sight, and serious occlusion are analyzed. In the case of serious occlusion, the IMU and UWB are used for cooperative positioning of the target to be measured, and the data of the two are fused based on the EKF (Extended Kalman Filter Algorithm) to obtain the final position of the target at this moment. In the case of full line of sight and incomplete line of sight, the average value measurement algorithm is used to estimate the target position. The data information obtained by the sensor is fully utilized, and different methods are adopted to position the target to be measured according to the line-of-sight conditions in different situations, thereby mitigating the influence of non-line-of-sight on the error, improving the positioning accuracy, and achieving the effect of flexible processing and prominent advantages in actual situations.

[0315] The present invention can also achieve the effect of mitigating the non-line-of-sight error in positioning by using UWB and other sensors, such as Simultaneous Localization and Mapping (SLAM), for cooperative positioning of an AGV cart.

[0316] The embodiments described above are only a preferred solution of the present invention, and do not impose any form of limitation on the present invention. There are other variations and modifications without exceeding the technical solutions described in the claims.

Claims

1. An indoor positioning method for non-line-of-sight error compensation based on UWB and IMU, characterized in that, It includes the following steps: S1: Divide the empty indoor storage environment into M grids, and collect data information in each grid. The data information includes the ranging value of the base station, the received signal strength value, and the coordinate value obtained by UWB positioning solution; S2: In the actual indoor storage environment, collect data information for any logistics path of the AGV cart at each moment; S3: Define source signal strength variables, source ranging variables, and source position variables to judge the consistency between the empty indoor storage environment and the actual indoor storage environment; S4: According to the judgment result, calculate the final position pos of the target to be measured at time k in different cases k : If it is a complete line-of-sight situation and incomplete occlusion, calculate the final position pos according to the average value of the grid position and the position coordinates calculated by the Chan-Taylor positioning algorithm at time k k ; If it is complete occlusion, according to the motion parameters of the AGV vehicle, combine the IMU positioning solution algorithm to obtain the position and speed of the AGV vehicle; convert the acceleration in the carrier coordinate system to the acceleration in the navigation coordinate system; when the sampling interval is short, the carrier target is approximately a uniformly accelerated linear motion, and according to the acceleration in the navigation coordinate system, the position and speed of the target to be measured at time k-1, obtain the position of the target to be measured in the navigation coordinate system of the IMU at time k; use the EKF algorithm to fuse and filter the IMU data and the ranging value of the UWB with non-line-of-sight error to obtain the final position pos k .

2. The indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 1, wherein, The specific steps of step S1 are as follows: S1.1: According to the actual empty indoor storage environment, divide it into M grids under the condition of meeting the positioning accuracy; S1.2: Collect and store data information of the AGV cart with a positioning tag in each grid; S1.2.1: The data information collected in each grid is represented as a group. A group of data information includes the ranging value of each base station, the signal strength value received by each base station, and the coordinate value obtained by UWB positioning solution; S1.2.2: Define the ranging values of the base stations for the signal strength information received by the r-th group of grids as vectors p' r , d r ' and the coordinate values pos' r , where {r|1 ≤ r ≤ M, r ∈ N+}, which can be expressed as: p r ′ = [p1′ ,r , p′ 2,r , …, p′ n,r T ​ d′ r = [d′ 1,r , d′ 2,r , …, d′ n,r T ​ In the formula, n represents the number of base stations; S1.2.3: In the M groups of grids, respectively set the received signal strength set as P' M , the ranging set as D' M and the coordinate set as Pos' M , which are respectively expressed as: P' M = [p1' p'2... p' r ... p' M ​ D' M = [d1'd'2...d r '...d' M ​ Pos' M = [pos1', pos'2,... pos' r ,... pos' M T ​ P' M , D' M , Pos' M respectively store the data information of M groups of grid labels.

3. The indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 2, wherein Step S2 further includes: The data information collected at each moment includes the ranging value of each base station, the signal strength value received by each base station, and the coordinate value obtained by UWB positioning solution; the data information is respectively represented as: p k = [p 1,k , p 2,k , …, p n,k T ​ d r = [d 1,k , d 2,k , …, d n,k T ​ Among them, p n,k , d n,k respectively represent the data information of the signal strength value and the ranging value obtained by each base station at time k. p k , d k respectively represent a vector of the signal strength vector obtained by each base station at time k and the distance vector from the tag. is the tag coordinate value obtained by UWB positioning calculation at time k.

4. An indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 1, characterized in that In step S3, judge the consistency between the empty indoor storage environment and the actual indoor storage environment: By defining the source signal strength variable source ranging variable δ r , source position variable α r to evaluate the consistency degree of the tag UWB information between the actual warehousing environment and the open environment, where: δ r = ||d k - d r '|| α r = ||pos k - pos r '||; Define a new vector p′ s , where {s | 1 ≤ s ≤ M, s ∈ N +}}, satisfying: Obtain the corresponding vector d s ', pos' s and three variable values δ s , α s ; Define a new vector d l ', {l | 1 ≤ l ≤ M, l ∈ N +} and satisfy: Obtain the corresponding vector p l ', pos l ', and the three variable values δ l , α l ; Define a new vector pos' z , {z | 1 ≤ z ≤ M, z ∈ N +}, satisfying: Obtain the corresponding vector p' z , d' z and three variable values α z , δ z , 5. A method for indoor positioning with non-line-of-sight error compensation based on UWB and IMU according to claim 1 or 4, characterized in that, In step S3, according to the degree of consistency, it is divided into three cases to distinguish the line-of-sight and occlusion cases in the actual storage environment: A1: The base station is in a full line-of-sight scenario, and the grid position pos' is defined v ; A2: The base station is partially blocked, defining the grid position pos' v ; A3: The base station is completely blocked, and the ranging value d with non-line-of-sight error of the base station at this moment is output k .

6. An indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 5, characterized in that, In the step S4 described above, calculate the final position pos of the target to be measured at the moment k according to the judgment result k : S4.1: For the cases of A1 and A2: Calculate the grid position coordinates: The position coordinates calculated according to the Chan-Taylor positioning algorithm at the k-th moment are The final position pos of the target to be measured at the k-th moment can be obtained by averaging the two position coordinates k , specifically: where pos' v =(pos' v,x , pos' v,y ), is the position coordinate obtained by Chan-Taylor positioning solution based on UWB; S4.2: For the case of A3: S4.2.1: Obtain the position of the navigation coordinate system at the kth moment obtained by the IMU: B1: The IMU obtains the motion parameters of the AGV cart including acceleration and angular velocity, and the position and velocity of the AGV cart are obtained by quadratic integration according to the IMU positioning solution algorithm; B2: Obtain the rotation matrix through coordinate system transformation The acceleration a in the vehicle coordinate system b is transformed to obtain the acceleration a in the navigation coordinate system n, specifically as follows: n That is: Among them: represent the accelerations in the horizontal and vertical directions in the carrier coordinate b-system; B3: When the sampling interval ΔT is short, the carrier target is approximately in a uniformly accelerated linear motion. Using Δv n to represent the velocity change of the system in the navigation coordinate n-system, we can obtain: Among them, respectively represent the velocity changes of the system in the horizontal and vertical directions in the navigation coordinate system, represent the accelerations of the system in the horizontal and vertical directions in the navigation coordinate system n; B4: Let the velocity at time k-1 in the navigation coordinate n-system be the velocity at time k which can be expressed as: Among them, represents the horizontal and vertical velocities in the navigation coordinate n - system at time k; represents the horizontal and vertical velocities in the navigation coordinate n - system at time k - 1; B5: Let Δpos n be the displacement change in the navigation coordinate n system, specifically: wherein, respectively represent the displacement changes in the horizontal and vertical directions in the navigation coordinate n system; B6: Calculate the actual position of the target to be measured at the (k - 1)th moment: pos k-1 =(pos k-1,x , pos k-1,y ) Obtain the position of the navigation coordinate system of the target to be measured at the kth moment obtained by the IMU: S4.2.2: The extended Kalman filter, i.e., the EKF algorithm, is used to fuse and filter the IMU data and the ranging values of UWB with non-line-of-sight errors to obtain the final position pos of the target to be measured at time k k 。 7. An indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 6, characterized in that The specific expression of step S4.2.2 is: C1: Use the data obtained by the accelerometer of the IMU at the (k - 1)th moment in the navigation coordinate system n, i.e., the UWB positioning coordinate system as the input of the system, and use the ranging values of the base stations obtained by UWB at the kth moment as the observation vector Z k = [d 1,k d 2,k … d n,k T , and use the speed and position of the target AGV cart at the (k - 1)th moment as the state vector X k-1 = [pos k-1,x pos k-1,y v k-1,x v k-1,y T , according to the EKF principle, establish the system model as follows:​​ X k = FX k-1 + Bu k-1 + Gw k-1 Z k = h[X k + v k where, F represents the state transition matrix of the system, B is the control input matrix, G represents the noise driving matrix, w k-1 = [w k-1,x w k-1,y T represents the process noise matrix with zero mean and variance , h[X k = [d 1,k d 2,k …d n,k T represents the non - linear observation function related to the ranging value of the system at time k, v k = [v 1,k v 2,k …v n,k T represents the ranging distance observation noise matrix with zero mean and variance ;​​​ C2: Establish the observation equation of the system: Equation of state: Z k -Z k-1 = ΔZ k = H k X k + Δv k ; C3: Initialize the state mean U(0)=E[X(0)], the state covariance matrix P(0)=var[X(0)], and perform EKF iteration: Prediction status: Predicted state covariance matrix: P k / k-1 = FP k-1|k-1 F T + GQG T Where P k / k-1 is the predicted state covariance matrix, and GQG T is the predicted noise covariance matrix; Calculate the Kalman filter gain matrix: Update status: Update state covariance matrix: P k|k = [I n - KH k P k|k-1 Among them, I n is an n×n matrix. The above five steps are used as one calculation cycle of the EKF. Based on the EKF, the UWB and IMU are fused for positioning to obtain the final position pos of the target to be measured at the kth moment k .

8. An indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 2 or 3, characterized in that, The signal strength value received by each base station is obtained by the received signal strength algorithm, and the specific performance is: where p is the signal strength power value of the base station receiving the tag; P CIR is the signal strength power value of the channel impulse response, and N and λ are experimental constant parameters; N refers to the preamble accumulation count value in the register, i.e., PAC.

9. An indoor positioning method for non-line-of-sight error compensation based on UWB and IMU according to claim 2 or 3, characterized in that, The coordinate values obtained by UWB positioning calculation are obtained by constructing a non-linear equation set from the hyperbola model and then using the Chan-Taylor fusion positioning algorithm, specifically as follows: According to: Get: Among them, R i represents the distance from each base station to the tag position (x, y), (x i , y i ) is the coordinate of base station i, (x, y) is the coordinate of the target tag to be measured, and there are n base stations participating in the positioning algorithm in total. When n > 2, the number of unknowns in the equation set is less than the number of equations in the equation set, which is an overdetermined equation set, and the equation is a non-linear equation; thus obtained: G a Z a = h Among them: Perform the first LS (Least Squares) estimation to obtain Among them: ψ = 4BQB B = diag(R1, R2,..., R n ) e = diag(e1, e2,..., e n ) Q = E[ee T ​ where, e i is the error amount corresponding to R i ; Perform the second LS (least squares method) estimation to obtain: Z' a G' a = h' Among them: Then the second LS estimate Among them: Estimate the position of the target tag to be measured: Take this position estimate as the initial coordinate value of the Taylor algorithm, perform Taylor expansion at this position, and ignore the terms of the second order and above, then the expression of the error vector can be obtained: φ = h d -G d δ d Among them (x i , y i ) represents the location coordinates of the base station, where i = 1, 2......n; R i represents the distance from each base station to the initial position (x, y) of the tag, and R i,1 represents the difference between the distance from the tag to be measured to the i-th base station and the distance from the tag to the first base station; The WLS (weighted least squares solution) of the expression is: Among them, Q is the covariance matrix of the measurement value error, and the next recursive initial value is changed to: x' = x + Δx y' = y + Δy Keep recursively iterating and calculating according to the above steps until Δx and Δy satisfy the pre-set threshold value ε, i.e., |Δx| + |Δy| < ε. Then the iteration ends and the positioning solution value at the k-th moment is obtained.

Images