An indoor and outdoor seamless positioning method based on UWB and GNSS

Abstract

The application relates to an indoor and outdoor seamless positioning method based on UWB and GNSS, and belongs to the technical field of positioning and navigation. The UWB positioning technology is applied to a GNSS signal attenuation area, the problem of low positioning accuracy in the area is solved, and the smooth transition and positioning mode switching among different positioning areas are realized through a related algorithm of seamless positioning; the seamless positioning mode combining soft switching and hard switching is adopted; in the aspect of soft switching among systems: a position estimation confidence scheme based on UWB and GNSS systems and a confidence fusion seamless positioning algorithm based on particle filtering are proposed, which are used for realizing the seamless positioning and smooth transition among different positioning areas; in the aspect of hard switching among systems: a positioning mode switching algorithm of a positioning terminal based on the confidence of a joint system and a motion trend is proposed, which is used for realizing the effective and timely switching among the UWB positioning mode, the UWB+GNSS seamless positioning mode and the GNSS positioning mode, and reducing the ping-pong effect.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of positioning and navigation, in particular to an indoor and outdoor seamless positioning method based on UWB and GNSS. BACKGROUND

[0002] With the rapid development of China's economy, indoor and outdoor high-precision seamless positioning has become one of the important hotspots. At present, it is difficult to ensure the reliability of indoor and outdoor seamless positioning by relying on a single positioning technology. The application of Global Navigation Satellite System (GNSS) in indoor or signal shielding environment has many limitations. Ultra Wide Band (UWB) represented by wireless signal has developed rapidly in indoor positioning technology. Due to the high speed, long communication distance and strong penetration ability of UWB, the combination of GNSS technology and UWB positioning technology is an important research direction of seamless positioning at present.

[0003] The traditional positioning mode algorithm has the problems of frequent switching or untimely switching. Since the positioning mode switching command of the positioning terminal is usually issued by the server, frequent switching will cause waste of system resources of the server, and frequent switching or untimely switching will also affect the continuity and stability of the positioning point to a certain extent. SUMMARY

[0004] In order to solve the above technical problems existing in the prior art, the present application provides an indoor and outdoor seamless positioning method based on UWB and GNSS. The method applies UWB positioning technology to the GNSS signal attenuation area, solves the problem of low positioning accuracy in these areas, and realizes smooth transition and positioning mode switching between different positioning areas through the related algorithm of seamless positioning. The method divides the positioning area into single UWB area, single GNSS area and mixed positioning area (UWB+GNSS), divides the positioning mode into UWB positioning mode, UWB+GNSS seamless positioning mode and GNSS positioning mode, and adopts a seamless positioning mode combining soft switching and hard switching. In terms of soft switching between systems: a position estimation confidence scheme based on UWB and GNSS systems and a confidence fusion seamless positioning algorithm based on particle filtering are proposed, which are used to realize seamless positioning and smooth transition between different positioning areas; in terms of hard switching between systems: a positioning mode switching algorithm of the positioning terminal based on joint system confidence and motion trend is proposed, which is used to realize effective and timely switching between UWB positioning mode, UWB+GNSS seamless positioning mode and GNSS positioning mode, and reduce ping-pong effect.

[0005] The technical scheme is as follows:

[0006] A seamless positioning method based on UWB and GNSS, the steps are as follows:

[0007] S1, the positioning area is divided into single UWB area, single GNSS area and mixed positioning area; wherein the single UWB area is defined as the area without GNSS signal and with UWB base station, and the positioning mode is UWB mode; the single GNSS area is defined as the outdoor open area with GNSS signal and without UWB base station, and the positioning mode is GNSS mode; the mixed positioning area is defined as the transition area with GNSS signal attenuation and UWB base station, and the positioning mode is UWB+GNSS seamless positioning mode;

[0008] S2, the geographical coordinates of UWB system and GNSS system are unified by using Gauss projection and relative coordinate conversion;

[0009] S3, GNSS data is added in the positioning information by polynomial interpolation to realize timestamp alignment of UWB system and GNSS system;

[0010] S4, the confidence of UWB system position is estimated;

[0011] S5, the confidence of GNSS system position is estimated;

[0012] S6, the same standardization of UWB / GNSS joint system confidence is performed, and the joint confidence of multiple systems is calculated;

[0013] S7, the confidence fusion seamless positioning algorithm based on particle filtering is performed;

[0014] S8, the positioning mode switching algorithm of positioning terminal based on joint system confidence and motion trend is performed.

[0015] Further, in step S2, the building and the relative positioning coordinate system are converted by relative coordinate conversion, and the conversion steps are as follows:

[0016] Step 1: establish the conversion diagram of independent plane rectangular coordinate system and Gauss-Kruger rectangular coordinate system, XOY coordinate system is Gauss-Kruger coordinate system; X′O′Y′ coordinate system is the coordinate system of UWB system, and the included angle between the two coordinate systems is α;

[0017] Step 2: assuming that the origin O′ on X′O′Y′ has corresponding coordinates (X0, Y0) in Gauss coordinate system, then the coordinates (X A ,Y′ A ) of point A in UWB system coordinate system projected to Gauss coordinate system (X A ,Y A ) can be expressed as:

[0018]

[0019] Step 3: Coordinate conversion between two coordinate systems, through coordinate inversion, the rectangular coordinates (X' A ,Y′ A ) of point A in the UWB coordinate system are obtained as follows:

[0020]

[0021] Further, in step S3, UTC time obtained by the GNSS module is added to the positioning information sent by the positioning terminal, and the UWB and GNSS information obtained at approximately the same epoch in the positioning information is bound; the working frequency of the GNSS module is 1HZ, and the positioning frequency of the UWB system is set to be above 4HZ.

[0022] Further, in step S4,

[0023] the horizontal dilution of precision (HDOP) which measures the pros and cons of the horizontal configuration of the base station, the Cramer-Rao lower bound (CRLB) which measures the lower limit of the variance of the positioning error of a position in the positioning area are taken as the estimated performance indicators, and the iteration difference Δd u of the position observation information and the filter output estimation value is taken as the dynamic performance indicator to construct the system confidence formula;

[0024] The minimum value of HDOP in the positioning area is HDOP min , the minimum value of CRLB is CRLB min , the upper threshold of CRLB is CRLB max , and the upper threshold of HDOP is HDOP max ; the i-th UWB system horizontal dilution of precision confidence and the Cramer-Rao lower bound confidence can be respectively expressed as:

[0025]

[0026]

[0027] In the formula, HDOP i is the HDOP value of the i-th UWB positioning estimation point, in the formula, CRLB i is the CRLB value of the i-th UWB positioning estimation point, and the position of the positioning estimation point is the predicted value of the state quantity of the filter output at the last time at the next time;

[0028] The confidence of the filter iteration difference is defined as P ε , and the confidence of the i-th UWB filter iteration difference can be expressed as:

[0029]

[0030] In the formula, P is the absolute value of the difference between the observation information and the filter output estimation value at time i-1, T u is the performance threshold, T u is 3 times the standard deviation of the position measurement error of the UWB system;

[0031] The i-th UWB system comprehensive confidence expression is:

[0032]

[0033] wherein, are the i-th UWB system positioning point confidence about HDOP, CRLB and filter iteration difference, w H , w C , w ε is a weight factor, and the empirical values are w H = 0.1, w C = 0.15, w ε = 0.75;

[0034] By setting the time window length W UWB , T is the system time window, FPS UWB is the positioning frequency of the UWB system, and the formula is satisfied:

[0035] W UWB = FPS UWB × T (7)

[0036] The confidence of the UWB subsystem can be expressed as:

[0037]

[0038] wherein T N is the total running time of the system.

[0039] Further, in step S5,

[0040] The position dilution of precision PDOP, the signal-to-noise ratio SNR of the positioning satellite, and the starting mode of the GNSS receiver, which measure the quality indicators of the satellite signals of the GNSS system, are taken as the estimated performance indicators, and the iteration difference Δd u between the position observation information and the filter output estimation value is taken as the dynamic performance indicator to construct the system confidence formula;

[0041] First, the confidence P D , P S based on PDOP and SNR is constructed, and the formula is:

[0042]

[0043]

[0044] In the formula, D min D max ,snr min ,snr max Let D be the threshold parameter; min =1, D max =5, P D It is a monotonically decreasing function; snr is the signal-to-noise ratio of the satellite mapped to the integer set [0,9], and the empirical threshold for signal-to-noise ratio is snr. min =1 and snr max =7, P S It is a monotonically decreasing function;

[0045] The formula for establishing the satellite signal quality confidence level using the signal-to-noise ratio and position accuracy factor (PDOP) of multiple satellites in a GNSS system is as follows:

[0046]

[0047] In the formula, P s i Let P be the SNR confidence score of the i-th satellite, n be the number of satellites participating in the positioning calculation, and P be the SNR confidence score of the i-th satellite. A The larger the value, the better the positioning satellite status of the GNSS system, and the higher the reliability of the system.

[0048] The confidence level of different startup methods is determined by setting a time threshold. The confidence level switches depending on the startup method; the confidence level for the cold start method is defined as follows. The confidence level of the warm start method is T i T is the accumulated time after the receiver starts up. C T H T represents the average initial positioning time under cold and hot start conditions, respectively. This was determined through numerous experiments in an open environment. C =30s, T H =2s. The confidence formula for the startup method is:

[0049]

[0050] The confidence level of the filter iteration difference of the i-th GNSS system can be expressed as:

[0051]

[0052] In the formula, T is the absolute value of the difference between the position observation information and the filtered output estimate at time i-1. g T is the performance threshold. g It is equal to three times the standard deviation of the GNSS system position measurement error;

[0053] Let the i-th confidence formula of GNSS system be P ε , P A , P T consist of:

[0054]

[0055] wherein are the confidence of the i-th GNSS system positioning point about filter iteration difference, satellite signal quality, starting mode, respectively, w ε , w A , w T indicate the weight factor thereof, which is determined by analytic hierarchy process, w ε = 0.63, w A = 0.26, w T = 0.11;

[0056] The confidence of GNSS subsystem can be expressed as:

[0057]

[0058] wherein FPS GNSS is the positioning frequency of GNSS system, W GNSS is the window length of GNSS system, and T N is the total running time of the system.

[0059] Further, in step S6,

[0060] the confidence of UWB system and GNSS system is unified under the same standard by using sliding residual, and the sliding residual formula is:

[0061]

[0062] wherein N is the input value sliding window for calculating residual, d i is the i-th positioning estimation point in the sliding window, d' i is the fitting value, and then the joint confidence of GNSS and UWB can be respectively expressed as:

[0063]

[0064] wherein n is the power of residual, the more sharp the discrimination effect of the confidence of two systems is, and the more close the normalized confidence of two systems is to 0 or 1, wherein n = 0.5-1, and the normalized UWB and GNSS confidence is:

[0065]

[0066] Further, in step S6, the multi-system joint confidence calculation step is as follows:

[0067] Step 1: t = 0, initialize the confidence, when the UWB system positioning data is obtained, let If there is no UWB system output, the confidence of the GNSS system is the highest, let

[0068] Step 2: t = N, calculate the UWB subsystem confidence at the current time through the predicted position HDOP, CRLB value and the filter iteration difference at the last time by formula Calculate the confidence of the GNSS subsystem at the current time through the GNSS system PDOP value, the signal-to-noise ratio information of each positioning satellite, the start time information and the filter iteration difference at the last time by formula

[0069] Step 3: add to the sliding window of historical information, calculate the confidence of each subsystem in the time period by formula

[0070] Step 4: according to the UWB, GNSS sliding residual information, calculate the multi-system joint confidence by formula

[0071] Step 5: t = N + 1, return to step 2 to calculate the confidence at the next time.

[0072] Further, in step S7, the steps of the particle filter process are as follows:

[0073] Step 1: generate a particle set and initialize, each particle includes two parameters: the position information x0 = [s x0 ,v x0 ,s y0 ,v y0 ] T and the weight information of the particle

[0074] Step 2: prediction stage, use the uniform motion model to predict the next state of the system;

[0075]

[0076] Step 3: reevaluate the weight of each particle, first take the output of the GNSS and UWB sensor as the position observation value, and calculate the similarity between it and the corresponding position of the particle, the observation probability density function of the GNSS and UWB system at time k can be represented as:

[0077]

[0078] In the formula, d u and dg R is the Euclidean distance between the UWB and GNSS observation points to the particle u and R g are the noise variances of the UWB and GNSS systems; then the observation probability density function is calculated using additive fusion:

[0079]

[0080] where, and are the confidence of the UWB and GNSS system position estimates, respectively;

[0081] Step 4: State estimation using the normalized weights and the state posterior distribution;

[0082] Step 5: Resampling, eliminating particles whose weights do not reach a set threshold value.

[0083] Further, in step S8,

[0084] The maximum threshold radius D U , D G is set at the transition area of the U, G point at the center of the mixed area, the positioning terminal position (X i , Y i ) at time i and the distance D i from the positioning terminal to the U, G point are obtained, and the time threshold values T U , T G , T F of the single UWB area, the single GNSS area and the mixed area are set.

[0085] The switching process of UWB positioning in the initial state is as follows:

[0086] Step 1: Obtain the positioning terminal position (X i , Y i ) at time i and the distance D i from the positioning terminal to the U point.

[0087] Step 2: When the positioning terminal enters the threshold value range, when D i ≤ D i-1 ≤ D U and the residence time T ≥ T U , it indicates that the positioning point has a tendency to enter the mixed positioning area, at this time the positioning mode is switched to the seamless positioning mode, and the last positioning mode flag Last_flag = UWB is recorded.

[0088] Step 3: According to the flag, the next switching operation is selected, if the positioning terminal returns to the UWB single area, when D i ≥ D i-1≥D U and the residence time T≥T F When the conditions are met, seamless positioning to UWB positioning is completed;

[0089] Step 4: When the terminal is far away from the UWB positioning area and should be switched to GNSS positioning, the system confidence at this time is known and The system threshold μ of seamless positioning switching to GNSS positioning is set UWB2GNSS When and the residence time T≥T F The server issues a control command to set the UWB transmission frequency to 1HZ and switch to the GNSS positioning mode.

[0090] The beneficial effects of the present application are:

[0091] The indoor and outdoor seamless positioning method based on UWB and GNSS has the following technical effects:

[0092] (1) The position estimation confidence of the UWB and GNSS system based on the confidence fusion seamless positioning algorithm of particle filtering for mixed positioning area seamless positioning is better in adaptive ability, and the average positioning accuracy is improved compared with the conventional particle filtering algorithm and federal Kalman filtering algorithm.

[0093] (2) The positioning mode switching algorithm of the positioning terminal based on the joint system confidence and motion trend can effectively reduce the number of invalid mode switching of the positioning terminal and the switching process is timely. BRIEF DESCRIPTION OF DRAWINGS

[0094] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the present application will be described in detail below in combination with the drawings and detailed embodiments. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor. Among them:

[0095] Figure 1 is a schematic diagram of a positioning area;

[0096] Figure 2 is a schematic diagram of indoor and outdoor coordinate conversion;

[0097] Figure 3 is a conversion schematic diagram of an independent plane rectangular coordinate system and a Gauss-Krueger rectangular coordinate system;

[0098] Figure 4 is a general flowchart of the seamless positioning algorithm;

[0099] Figure 5This is a diagram illustrating the region switching process.

[0100] Figure 6 This is a flowchart of the positioning mode switching algorithm for a positioning terminal. Detailed Implementation

[0101] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. The following description, in conjunction with the accompanying drawings... Figures 1-6 The indoor and outdoor seamless positioning method based on UWB and GNSS is further explained.

[0102] This invention comprises the following 8 steps:

[0103] 1. Division of the positioning area

[0104] like Figure 1 As shown, this invention divides the positioning area into a single UWB area, a single GNSS area, and a hybrid positioning area (UWB+GNSS). A single UWB area is defined as an area where UWB base stations are deployed but there is no GNSS signal, and the positioning mode is UWB mode. A single GNSS area is defined as an open outdoor area with GNSS signal but no UWB base station deployment, and the positioning mode is GNSS mode. A hybrid positioning area is defined as a transitional area where GNSS signal attenuation occurs and UWB base stations are deployed, and the positioning mode is UWB+GNSS seamless positioning mode.

[0105] 2. Unification of geographic coordinates between UWB and GNSS systems

[0106] GNSS positioning systems commonly use the WGS-84 geodetic coordinate system, while UWB positioning systems typically use an independent coordinate system. Therefore, it is necessary to unify the geographic coordinates of the two systems. This can be achieved by using Gaussian projection and relative coordinate transformation, such as... Figure 2 As shown.

[0107] Relative coordinate transformation takes into account that the building's orientation may differ from the orientation of the relative positioning coordinate system. The transformation steps are as follows:

[0108] Step 1: As Figure 3 The diagram shows the transformation between an independent planar rectangular coordinate system and a Gauss-Kruger rectangular coordinate system. The XOY coordinate system is the Gauss-Kruger coordinate system. The X′O′Y′ coordinate system is the coordinate system of the UWB system, and the angle between the two coordinate systems is α.

[0109] Step 2: Assuming the origin O′ on X′O′Y′ has coordinates (X0, Y0) in the Gaussian coordinate system, then point A (X′O′Y′) in the UWB coordinate system... A ,Y′A ) projected to the Gaussian coordinate system (X A ,Y A ) can be expressed as:

[0110]

[0111] Step 3: The coordinates of the two coordinate systems can be converted to each other, and through coordinate inversion, the rectangular coordinates (X′ A ,Y′ A ) of point A in the UWB coordinate system can be obtained as:

[0112]

[0113] 3. Timestamp alignment between UWB and GNSS systems

[0114] The UTC time obtained by the GNSS module is added to the positioning information sent by the positioning terminal, and the UWB and GNSS information obtained at approximately the same epoch in the positioning information is bound.

[0115] Since the output positioning frequency of the commonly used GNSS module is 1-10HZ, in order to reduce the power consumption of the positioning terminal, the working frequency of the GNSS module is usually 1HZ; in order to have good dynamic effect for indoor positioning points, the positioning frequency of the UWB system is usually set to be above 4HZ. The present application adds GNSS data in the positioning information through polynomial interpolation, so as to realize the timestamp alignment between the UWB and GNSS systems.

[0116] 4. Position estimation confidence of UWB system

[0117] The present application takes the horizontal dilution of precision HDOP which measures the pros and cons of the horizontal configuration of the base station, and the Cramer-Rao lower bound CRLB which measures the lower limit of the variance of the positioning error of a position in the positioning area as the estimated performance index, takes the iterative difference Δd u between the position observation information and the filtered output estimation value as the dynamic performance index to construct the system confidence formula.

[0118] Supposing that the minimum value of HDOP in the positioning area is HDOP min , the minimum value of CRLB is CRLB min , the upper limit threshold of CRLB is CRLB max , and the upper limit threshold of HDOP is HDOP max , the horizontal dilution of precision confidence and the Cramer-Rao lower bound confidence of the i-th UWB system can be respectively expressed as:

[0119]

[0120]

[0121] HDOPi i HDOPi i HDOPi

[0122] The confidence of the filter iteration difference is defined as P ε The confidence of the i-th UWB filter iteration difference can be expressed as:

[0123]

[0124] The absolute value of the difference between the observation information and the filter output estimation value at i-1 time, T u The performance threshold, T u is 3 times the standard deviation of the UWB system position measurement error.

[0125] The confidence factor is composed of the pre-estimation performance index (HDOP, CRLB) and the dynamic performance index (filter iteration difference), and the expression of the i-th UWB system comprehensive confidence is:

[0126]

[0127] The confidence of the i-th UWB system positioning point with respect to HDOP, CRLB and filter iteration difference, respectively, w H , w C , w ε are weight factors, and the empirical values w H = 0.1, w C = 0.15, w ε = 0.75 are set through a large number of experiments.

[0128] The frame loss rate of the UWB positioning system is considered in the confidence formula, and the length of the time window W UWB is set, T is the system time window, and FPS UWB is the positioning frequency of the UWB system, and the formula is satisfied:

[0129] W UWB = FPS UWB × T (7)

[0130] The confidence of the UWB subsystem can be expressed as:

[0131]

[0132] N is the total running time of the system.​​​

[0133] 5. Confidence level of GNSS system position estimation

[0134] This invention uses the position accuracy factor (PDOP), the signal-to-noise ratio (SNR) of the positioning satellite, and the startup mode of the GNSS receiver as predictive performance indicators to measure the quality of GNSS system satellite signals. It also uses the iterative difference Δd between the position observation information and the filtered output estimate. u The system confidence formula is constructed using dynamic performance indicators.

[0135] First, construct a confidence level P based on PDOP and SNR. D P S The formula is:

[0136]

[0137]

[0138] In the formula, D min D max ,snr min ,snr max Let D be the threshold parameter. min =1, D max =5, P D It is a monotonically decreasing function. `snr` is the signal-to-noise ratio of the satellite mapped to the integer set [0,9], and the empirical threshold for the signal-to-noise ratio is `snr`. min =1 and snr max =7, P S It is a monotonically decreasing function.

[0139] The formula for establishing the satellite signal quality confidence level using the signal-to-noise ratio and position accuracy factor (PDOP) of multiple satellites in a GNSS system is as follows:

[0140]

[0141] In the formula, Let P be the SNR confidence score of the i-th satellite, n be the number of satellites participating in the positioning calculation, and P be the SNR confidence score of the i-th satellite. A The larger the value, the better the positioning satellite status of the GNSS system, and the higher the reliability of the system.

[0142] The confidence level of different startup methods is determined by setting a time threshold. The confidence level changes depending on the startup method. The confidence level for the cold start method is defined as follows: The confidence level of the warm start method is T i T is the accumulated time after the receiver starts up. C T HThe mean values of the first positioning time of cold and hot start, respectively, T C = 30 s, T H = 2 s. The confidence formula of the start mode is:

[0143]

[0144] The confidence of the i-th GNSS system filtering iteration difference can be expressed as:

[0145]

[0146] wherein, is the absolute value of the difference between the position observation information and the filtering output estimation value at time i-1, T g is the performance threshold, T g is equal to 3 times the standard deviation of the GNSS system position measurement error.

[0147] The confidence formula is composed of the estimated performance indicators (satellite signal quality, start mode) and the dynamic performance indicators (filtering iteration difference). The i-th confidence formula of the GNSS system is composed of P ε , P A , P T

[0148]

[0149] wherein are the i-th GNSS system positioning point confidences about the filtering iteration difference, satellite signal quality, and start mode, respectively, w ε , w A , w T represent their weight factors, which are determined by the analytic hierarchy process, w ε = 0.63, w A = 0.26, and w T = 0.11.

[0150] The confidence of the GNSS subsystem can be expressed as:

[0151]

[0152] wherein FPS GNSS is the positioning frequency of the GNSS system, W GNSS is the window length of the GNSS system, and T N is the total running time of the system.

[0153] 6. Same standardization method and steps of the confidence of the UWB / GNSS combined system

[0154] ​The confidence of UWB and GNSS system is unified by using sliding residual error under the same standard, and the sliding residual error formula is:

[0155]

[0156] In the formula, N is the input value sliding window for calculating residual error, d i is the ith positioning estimation point in the sliding window, d' i is the fitting value, and the joint confidence of GNSS and UWB can be respectively represented as:

[0157]

[0158] In the formula, n is the power of residual error, the more sharp the discrimination effect of the confidence of the two systems is, and the more close to 0 or 1 the confidence of the two systems is after normalization, n = 0.5-1 in the application, and the normalized UWB and GNSS confidence is:

[0159]

[0160] The multi-system joint confidence calculation steps are as follows:

[0161] Step 1. At time t = 0, initialize the confidence, when the UWB system positioning data is acquired, let If there is no UWB system output result, the confidence of the GNSS system is the highest, and let

[0162] Step 2. At time t = N, the UWB subsystem confidence at the current time is calculated through the HDOP and CRLB values of the predicted position and the filtering iteration difference at the last time according to the formula-. The confidence of the GNSS subsystem at the current time is calculated through the PDOP value of the GNSS system, the signal-to-noise ratio information of each positioning satellite, the start time information and the filtering iteration difference at the last time according to the formula-.

[0163] Step 3. Let be added to the sliding window of historical information, and the confidence of each subsystem in the time period is calculated through the formula-.

[0164] Step 4. According to the UWB and GNSS sliding residual error information, the multi-system joint confidence is calculated through the formula-.

[0165] Step 5. At time t = N+1, return to step 2 to calculate the confidence at the next time.

[0166] 7. Seamless positioning algorithm based on confidence fusion of particle filtering

[0167] AsFigure 4 The position of the positioning terminal and the parameters of each system are calculated by UWB and GNSS systems respectively, the joint confidence is calculated, and then the confidence and the observation value are transmitted into the particle filter after time stamp alignment and unification of geographic coordinates. The filter iteration difference fed back by the particle filter is used to participate in the calculation of the next confidence, and finally the estimated coordinates are output.

[0168] The steps of the particle filter process are as follows:

[0169] Step 1: Generate a particle set and initialize, each particle includes two parameters: the position information x0=[s x0 ,v x0 ,s y0 ,v y0 ] T and the weight information of the particle

[0170] Step 2: In the prediction stage, the uniform motion model is used to predict the next state of the system.

[0171]

[0172] Step 3: Re-evaluate the weight of each particle. First, the outputs of GNSS and UWB sensors are taken as position observations, and the similarity between them and the corresponding position of the particle is calculated. The observation probability density function of GNSS and UWB systems at time k can be represented as:

[0173]

[0174] In the formula, d u and d g are the Euclidean distances from the UWB and GNSS observation points to the particle, R u and R g are the noise variances of the UWB and GNSS systems. Then the additive fusion is used to calculate the observation probability density function:

[0175]

[0176] In the formula, C and C are the confidences of the position estimates of UWB and GNSS systems respectively.

[0177] Step 4: State estimation using normalized weights and state posterior distribution;

[0178] Step 5: Resampling, eliminate particles whose weights do not reach the set threshold value.

[0179] 8. Positioning terminal positioning mode switching algorithm based on joint system confidence and motion trend

[0180] As shown in Figure 5 , the maximum threshold radius D U of the transition area is set at the center of the mixed area import and export position U, G point G , the positioning terminal position (X i , Y i ) at time i and the distance D i from the positioning terminal to the U, G point are obtained, and then the time threshold values T U , T G , T F of the single UWB area, the single GNSS area and the mixed area are set.

[0181] The switching flow chart between the three positioning modes is shown in Figure 6 , and the switching process of UWB positioning in the initial state is as follows:

[0182] Step 1: Obtain the positioning terminal position (X i , Y i ) at time i and the distance D i from the positioning terminal to the U point.

[0183] Step 2: When the positioning terminal enters the threshold value range, when D i ≤D i-1 ≤D U and the residence time T≥T U , it indicates that the positioning point has a tendency to enter the mixed positioning area, at this time the positioning mode is switched to seamless positioning mode (UWB+GNSS), and the last positioning mode flag Last_flag is recorded =UWB.

[0184] Step 3: According to the flag, the next switching operation is selected, if the positioning terminal returns to the UWB single area, when D i ≥D i-1 ≥D U and the residence time T≥T F , the switching from seamless positioning to UWB positioning is completed.

[0185] Step 4: Another case is that the terminal should be switched to GNSS positioning when it is away from the UWB positioning area. The system confidence and at this time are known, and the system threshold μ UWB2GNSS from seamless positioning to GNSS positioning is set. When and the residence time T≥T F , the server issues a control command to set the UWB transmission frequency to 1HZ, and switches to GNSS positioning mode.

[0186] The above merely describes preferred specific embodiments of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art, according to the technical solution and inventive concept of the present application, makes equivalent replacement or change within the technical range disclosed by the present application, should be covered within the protection scope of the present application.

Claims

1. A seamless indoor and outdoor positioning method based on UWB and GNSS, characterized in that, The steps are as follows: S1. The positioning area is divided into single UWB area, single GNSS area, and hybrid positioning area; the single UWB area is defined as an area with UWB base stations deployed but no GNSS signal, and the positioning mode is UWB mode; the single GNSS area is defined as an open outdoor area with GNSS signal but no UWB base station deployed, and the positioning mode is GNSS mode; the hybrid positioning area is defined as a transition area where GNSS signal attenuation occurs and UWB base stations are deployed, and the positioning mode is UWB+GNSS seamless positioning mode. S2. Unify geographic coordinates between UWB and GNSS systems using Gaussian projection and relative coordinate transformation; S3. GNSS data is added to the positioning information through polynomial interpolation to achieve time stamp alignment between the UWB system and the GNSS system; S4. Estimate the confidence level of the location of the UWB system; The horizontal accuracy factor (HDOP), which measures the quality of the base station's horizontal configuration, and the Cramer-Rao lower bound (CRLB), which measures the lower limit of the variance of the positioning error at a certain location within the positioning area, are used as prediction performance indicators. The iterative difference Δd between the location observation information and the filtered output estimate is also used. u The system confidence formula is constructed using dynamic performance indicators; The minimum value of HDOP within the set bit range is HDOP. min The minimum value of CRLB is CRLB. min CRLB upper limit threshold CRLB max HDOP upper limit threshold HDOP max The confidence levels of the horizontal precision factor and the lower bound confidence level of the i-th UWB system can be expressed as follows: HDOP in formula (3) i Let be the HDOP value of the estimated point for the i-th UWB positioning, where CRLB is in equation (4). i Let CRLB be the CRLB value of the i-th UWB positioning estimation point. The position of the positioning estimation point is the predicted value of the state variable of the filter output at the previous time in the next time. Define the confidence level of the filter iteration difference as P. ε Then the confidence level of the difference in the i-th UWB filtering iteration can be expressed as: In equation (5), T is the absolute value of the difference between the observed information and the estimated value of the filtered output at time i-1. u T is the performance threshold. u This is three times the standard deviation of the position measurement error in the UWB system. The expression for the overall confidence level of the i-th UWB system is: In equation (6), Let w represent the confidence scores of the i-th UWB system localization point with respect to HDOP, CRLB, and the filter iteration difference, respectively. H w C w ε Let w be the weighting factor, based on empirical values. H =0.1, w C =0.15, w ε =0.75; By setting the time window length W UWB T represents the system time window, and FPS. UWB The positioning frequency for the UWB system must satisfy the following formula: W UWB =FPS UWB ×T (7) The confidence level of the UWB subsystem can be expressed as: In equation (8), T N Total system uptime; S5. Estimate the confidence level of the GNSS system's location; S6. Perform the same standardization of the UWB / GNSS joint system confidence and calculate the joint confidence of multiple systems; S7. Execute the confidence fusion seamless localization algorithm based on particle filtering; S8. Execute the positioning mode switching algorithm of the positioning terminal based on the joint system confidence and motion trend.

2. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S2, a relative coordinate transformation is performed between the building and the relative positioning coordinate system. The transformation steps are as follows: Step 1: Establish a transformation diagram between the independent Cartesian coordinate system and the Gauss-Kruger Cartesian coordinate system. The XOY coordinate system is the Gauss-Kruger coordinate system; the X′O′Y′ coordinate system is the coordinate system of the UWB system, and the angle between the two coordinate systems is α. Step 2: Assuming the origin O′ on X′O′Y′ has coordinates (X0, Y0) in the Gaussian coordinate system, then point A (X′O′Y′) in the UWB coordinate system... A ,Y A The coordinates (X') projected onto the Gaussian coordinate system A ,Y A This can be represented as: Step 3: Transform the coordinates of the two coordinate systems and perform inverse coordinate calculations to obtain the rectangular coordinates (X′) of point A in the UWB coordinate system. A ,Y A ')for: 。 3. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S3, the UTC time obtained by the GNSS module is added to the positioning information sent by the positioning terminal, and the UWB and GNSS information of approximately the same epoch in the obtained positioning information are bound together; the GNSS module operates at a frequency of 1 Hz, and the UWB system positioning frequency is set above 4 Hz.

4. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S5, The position accuracy factor (PDOP), signal-to-noise ratio (SNR) of positioning satellites, and startup mode of GNSS receivers are used as estimated performance indicators to measure the quality of GNSS satellite signals. The iterative difference Δd between the position observation information and the filtered output estimate is used as the estimated performance index. u The system confidence formula is constructed using dynamic performance indicators; First, construct a confidence level P based on PDOP and SNR. D P S The formula is: In equations (9) and (10), D min D max ,snr min ,snr max Let D be the threshold parameter; min =1, D max =5, P D It is a monotonically decreasing function; snr is the signal-to-noise ratio of the satellite mapped to the integer set [0,9], and the empirical threshold for signal-to-noise ratio is snr. min =1 and snr max =7, P S It is a monotonically decreasing function; The formula for establishing the satellite signal quality confidence level using the signal-to-noise ratio and position accuracy factor (PDOP) of multiple satellites in a GNSS system is as follows: In equation (11), Let P be the SNR confidence score of the i-th satellite, n be the number of satellites participating in the positioning calculation, and P be the SNR confidence score of the i-th satellite. A The larger the value, the better the positioning satellite status of the GNSS system, and the higher the reliability of the system. The confidence level of different startup methods is determined by setting a time threshold. The confidence level switches depending on the startup method; Define the confidence level of the cold start method as: The confidence level of the warm start method is T i T is the accumulated time after the receiver starts up. C T H T represents the average initial positioning time under cold and hot start conditions, respectively. This was determined through numerous experiments in an open environment. C =30s, T H =2s, the confidence formula for the startup method is: The confidence level of the filter iteration difference of the i-th GNSS system can be expressed as: In equation (13), T is the absolute value of the difference between the position observation information and the filtered output estimate at time i-1. g T is the performance threshold. g It is equal to three times the standard deviation of the GNSS system position measurement error; Let the formula for the i-th confidence level of a GNSS system be given by P. ε P A P T composition: In equation (14) Let w represent the confidence levels of the GNSS system positioning point for the i-th iteration with respect to the filter iteration difference, satellite signal quality, and startup mode, respectively. ε w A w T The weighting factor is represented by w, which is determined using the analytic hierarchy process. ε =0.63, w A =0.26, w T =0.11; The confidence level of the GNSS subsystem can be expressed as: In equation (15), FPS GNSS W is the positioning frequency for the GNSS system. GNSS T is the window length of the GNSS system. N This represents the total system runtime.

5. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S6, Using sliding residuals to unify the confidence levels of UWB and GNSS systems under the same standard, the sliding residual formula is as follows: In equation (16), N is the sliding window of input values ​​for calculating the residuals, and d i Let d be the estimated point for the i-th location within the sliding window. i Let ′ be the fitted value. Then the joint confidence of GNSS and UWB can be expressed as follows: In equation (17), n is the power of the residual. The sharper the discrimination effect on the confidence of the two systems, the closer the confidence of the two systems after normalization is to 0 or 1. Where n = 0.5 to 1, the normalized confidence of UWB and GNSS is: 。 6. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S6, the steps for calculating the joint confidence score of multiple systems are as follows: Step 1: At time t=0, initialize the confidence level. When the UWB system positioning data is acquired, let... If no UWB system output is available, the confidence level of the GNSS system is taken as the highest, and let... Step 2: At time t = N, the confidence level of the UWB subsystem at the current time is calculated using formulas (3)-(6) based on the predicted HDOP and CRLB values ​​of the location and the filter iteration difference from the previous time. The confidence level of the GNSS subsystem at the current moment is calculated using formulas (9)-(14) based on the GNSS system PDOP value, the signal-to-noise ratio information of each positioning satellite, the start-up time information, and the filter iteration difference of the previous moment. Step 3: Put The confidence level of each subsystem within a given time period is calculated using formulas (8) and (15) within a sliding window of historical information. Step 4: Based on the UWB and GNSS sliding residual information, calculate the joint confidence level of the multi-system system using formulas (17) and (18). Step 5: At time t = N+1, return to step 2 and calculate the confidence level for the next time step.

7. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S7, the particle filtering process consists of the following steps: Step 1: Generate and initialize a particle set. Each particle includes two parameters: the location information of the positioning terminal. and the particle's weight information Step 2: Prediction phase, using a uniform motion model to predict the next state of the system; Step 3: Re-evaluate the weight of each particle. First, use the outputs of the GNSS and UWB sensors as position observations, and calculate the similarity between their values ​​and the corresponding positions of the particles. The observation probability density functions of the GNSS and UWB systems at time k can be expressed as follows: In equation (20), d u and d g R represents the Euclidean distance from the UWB and GNSS observation points to the particle. u and R g The noise variances of the UWB and GNSS systems are given; then, additive fusion is used to calculate the observation probability density function: In equation (21), and These represent the confidence levels for position estimation using UWB and GNSS systems, respectively. Step 4: Perform state estimation using normalized weights and the posterior distribution of the state; Step 5: Resample and discard particles whose weights have not reached the set threshold.

8. The seamless indoor and outdoor positioning method based on UWB and GNSS as described in claim 1, characterized in that, In step S8, The maximum threshold radius D is set at points U and G, which are the center of the mixed area's import and export locations. U D G The transition region is used to obtain the location of the positioning terminal (X) at time i. i ,Y i And the distance D from the positioning terminal to points U and G. i Then set the time threshold T for single UWB area, single GNSS area, and mixed area. U T G T F ; The initial switching process for UWB positioning is as follows: Step 1: Obtain the location of the positioning terminal at time i as (X i ,Y i And the distance D from the positioning terminal to point U. i ; Step 2: When the positioning terminal enters the threshold range, when D is satisfied... i ≤D i-1 ≤D U And the stay time T≥T U When the location point tends to enter the mixed location area, the location mode is switched to seamless location mode, and the last location mode flag Last_flag = UWB is recorded. Step 3: Select the next switching operation based on the flag bit. If the positioning terminal returns to the UWB single area, when D is satisfied... i ≥D i-1 ≥D U And the stay time T≥T F Under certain conditions, seamless positioning can be switched to UWB positioning. Step 4: When the terminal is far from the UWB positioning area, it should switch to GNSS positioning, given the current system confidence level. and Set the system threshold μ for switching from seamless positioning to GNSS positioning. UWB2GNSS ;when And the stay time T≥T F The server issues a control command to set the UWB transmission frequency to 1Hz and switch to GNSS positioning mode.

Images