An IMU and UWB combined positioning method based on extended Kalman filtering

Abstract

The application relates to the field of intelligent positioning, and proposes an IMU and UWB combined positioning method based on an extended Kalman filter. IMU and UWB data are collected, a combined positioning model is established, preliminary ranging is carried out according to the combined positioning model, consistency checking is carried out, independent detection and gross error discrimination of UWB measurement are carried out, the new information is used to suppress the geometrically inconsistent measurement values, when the detection exceeds the threshold, the UWB data at the moment is not used, only the IMU prediction result is reserved, when the detection passes, the square of the normalized new information of each channel is taken as a statistical index, and is mapped to a basic probability distribution function in a DS evidence theory, then the multi-channel measurement evidence is fused through a DS evidence combination rule, the fusion result is converted into a measurement noise covariance adaptive adjustment, the UWB measurement value is filtered and updated in the extended Kalman filter algorithm, and the positioning precision and stability of the IMU and UWB combined positioning in a complex indoor environment are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of intelligent positioning, and in particular to an IMU and UWB combined positioning method based on extended Kalman filtering. BACKGROUND

[0002] In recent years, the rapid development of indoor automatic transport vehicles, indoor delivery robots and underground autonomous parking technology has made the demand for indoor centimeter-level positioning increasingly urgent. However, in the complex environment of factory workshops, underground workshops and coal mine deep wells, indoor signals based on radio are easily blocked by obstacles and produce strong reflection, scattering and attenuation during propagation, resulting in significant degradation of positioning accuracy and failing to meet the demand for indoor positioning.

[0003] In the prior art, a single radio device sensor is easily affected by environmental factors, while an inertial unit can independently calculate the position without relying on any external information. The complementarity of ultra-wideband (UWB) and inertial measurement unit (IMU) can compensate for each other's defects, and the combined positioning method of the two sensors has attracted attention. However, the existing ultra-wideband (UWB) and inertial measurement unit (IMU) combined positioning method has two problems. On the one hand, some research mainly uses neural networks or traditional adaptive filtering algorithms to compensate for observation errors. However, the neural network-based method has weak adaptability to different environments and is expensive, and the conventional adaptive algorithm has limited correction ability for positioning errors in complex indoor environments. On the other hand, in actual complex indoor environments, the uncertainty of the external environment will cause the observation data to contain unpredictable gross errors, which will cause conflicts between the UWB observation position and the IMU predicted position. Whether the fusion strategy can adaptively perceive environmental changes and effectively handle observation conflicts has become the key to realizing high-precision positioning. However, the existing UWB and inertial measurement unit combined positioning algorithm is complex in structure, requires high computing power, and relies on empirical thresholds or specific environmental geometric features for hard decision, which makes it difficult to achieve stable and high-precision positioning in low-cost chips and complex indoor environments.

[0004] Therefore, how to design an IMU and UWB combined positioning method to avoid the interference of complex environments and improve positioning accuracy and stability has become a problem to be solved. SUMMARY

[0005] Based on this, the application provides an IMU and UWB combined positioning method based on an extended Kalman filter, IMU and UWB data are collected, a combined positioning model is established, preliminary ranging is performed according to the combined positioning model, and consistency checking is performed, UWB measurement is autonomously detected and gross error is identified, geometrically inconsistent measurement values are suppressed by using innovation, when the detection exceeds the threshold, the UWB data at the moment is not used, only the IMU prediction result is reserved, when the detection passes, the square of the normalized innovation of each channel is used as a statistical index, and the statistical index is mapped to a basic probability assignment function in DS evidence theory, then the multi-channel measurement evidence is fused by using a DS evidence combination rule, the fusion result is converted into a measurement noise covariance adaptive adjustment, UWB measurement values are filtered and updated in the extended Kalman filter algorithm, and the application improves the positioning precision and stability of the IMU and UWB combined positioning in a complex indoor environment.

[0006] The application provides an IMU and UWB combined positioning method based on an extended Kalman filter, which comprises the following steps:

[0007] IMU and UWB data are collected, and a combined positioning model is established, the combined positioning model is a discrete state space model, the combined positioning model comprises a system state vector, a control input vector and a UWB measurement vector;

[0008] Preliminary ranging is performed according to the combined positioning model, and consistency checking is performed, so as to obtain a preliminary ranging value, the preliminary ranging comprises prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency checking is based on an adjacent difference matrix, and the consistency checking is used for screening out ranging values with non-line-of-sight or gross error;

[0009] Filtering and updating are performed based on an extended Kalman filter algorithm, so as to obtain a combined positioning result, the filtering and updating are based on multi-base station evidence fusion and adaptive weight.

[0010] In summary, according to the above-mentioned IMU and UWB combined positioning method based on extended Kalman filter, the IMU and UWB data are collected, and a combined positioning model is established, and preliminary ranging is performed according to the combined positioning model and consistency test is performed, the UWB measurement is autonomously detected and gross error is identified, the innovation is used to suppress the geometrically inconsistent measurement values, when the detection exceeds the threshold, the UWB data at this moment is not used, only the IMU prediction result is retained, when the detection passes, the standardized innovation square of each channel is taken as a statistical index, and it is mapped to the basic probability assignment function in the DS evidence theory, and then the DS evidence combination rule is used to fuse the multi-channel measurement evidence, the fusion result is converted into the measurement noise covariance adaptive adjustment, the UWB measurement value is updated in the extended Kalman filter algorithm, and the positioning precision and stability of the IMU and UWB combined positioning in the complex indoor environment are improved. Specifically, the IMU and UWB data are collected, and a combined positioning model is established, the combined positioning model is a discrete state space model, the combined positioning model includes a system state vector, a control input vector and a UWB measurement vector, preliminary ranging is performed according to the combined positioning model and consistency test is performed, to obtain a preliminary ranging value, the preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency test is based on an adjacent difference matrix, and the consistency test is used to filter out the ranging value with non-line-of-sight or gross error, and the filtering update is performed based on the extended Kalman filter algorithm, to obtain a combined positioning result, and the filtering update is based on multi-base station evidence fusion and adaptive weight, and the positioning precision and stability of the IMU and UWB combined positioning in the complex indoor environment are improved.

[0011] Further, the step of collecting IMU and UWB data and establishing a combined positioning model specifically includes:

[0012] Collecting IMU and UWB data;

[0013] Constructing a system state vector, a control input vector and a UWB measurement vector, and the specific algorithm of the system state vector, the control input vector and the UWB measurement vector is as follows:

[0014] ,

[0015] ,

[0016] ,

[0017] Wherein, represents a system state vector, represents a control input vector, represents a UWB measurement vector, , represents a position component in navigation coordinates, , represents a velocity component in navigation coordinates, , represents an acceleration component in navigation coordinates, T represents a transpose, , represents an acceleration input component of the IMU, represents a UWB ranging from the tag to the mth base station;

[0018] discretize the system state vector and the UWB measurement vector to construct a combined positioning model.

[0019] Further, the step of discretizing the system state vector and the UWB measurement vector specifically comprises:

[0020] The specific algorithm for discretizing the system state vector and the UWB measurement vector is as follows:

[0021] ,

[0022] ,

[0023] wherein, represents a system state vector, represents a UWB measurement vector, represents a state transition function, which is used to describe the motion relationship from the state at time k-1 to time k under the action of the control input vector , represents process noise, represents a measurement function, which is used to map the system state vector to the theoretical ranging between each base station and the tag, represents measurement noise.

[0024] Further, the step of performing preliminary ranging according to the combined positioning model and performing consistency checking to obtain a preliminary ranging value specifically comprises:

[0025] performing prior prediction on the system state vector according to the IMU data, wherein the prior prediction obtains a prior state prediction and a covariance of the prior state prediction according to the state transition function;

[0026] The specific algorithm for the prior prediction is as follows:

[0027] ,

[0028] wherein, represents a prior state prediction, represents a state transition function, represents a posterior state estimation at time k-1, represents a control input vector at time k;

[0029] According to the prior state prediction, theoretical measurement values between each base station and the tag are calculated, and the specific algorithm of the theoretical measurement values is as follows:

[0030] ,

[0031] wherein, represents a theoretical measurement value, represents a measurement function;

[0032] The difference between the actual measurement value and the theoretical measurement value is calculated to obtain an innovation vector and an innovation covariance, and the specific algorithm of the innovation vector and the innovation covariance is as follows:

[0033] ,

[0034] ,

[0035] wherein, represents an innovation vector, represents a UWB measurement vector, represents an innovation covariance, represents an observation matrix, T represents a transpose, represents a prior state prediction error covariance matrix at time k, represents a UWB measurement noise covariance matrix;

[0036] An adjacent difference matrix is constructed, and the specific algorithm of the adjacent difference matrix is as follows:

[0037] ,

[0038] wherein, represents an adjacent difference matrix, and m represents the number of base stations;

[0039] Consistency checking is performed according to the adjacent difference matrix, and the specific algorithm of the consistency checking is as follows:

[0040] ,

[0041] ,

[0042] ,

[0043] ,

[0044] ,

[0045] ,

[0046] wherein, denotes the new innovation vector after adjacent difference, denotes the new innovation covariance matrix after adjacent difference, denotes the consistency detection statistic, denotes the false alarm probability, denotes the zero hypothesis that the current measurement has no gross error, denotes the chi-square random variable with degree of freedom r greater than the threshold value , denotes the distribution density function, denotes the test threshold value, denotes the cumulative distribution function;

[0047] When , it is determined that there is a gross error in the current measurement, the UWB data is not used, and only the IMU prediction result is reserved;

[0048] When , the filter is updated based on the extended Kalman filtering algorithm.

[0049] Further, the step of updating the filter based on the extended Kalman filtering algorithm to obtain the combined positioning result specifically comprises:

[0050] An identification framework is constructed, and the identification framework is used to describe the UWB ranging propagation condition, wherein the UWB ranging propagation condition includes line-of-sight and non-line-of-sight;

[0051] The specific algorithm of the identification framework is as follows:

[0052] ,

[0053] ,

[0054] wherein, denotes the identification framework, denotes the UWB ranging propagation condition under line-of-sight, denotes the UWB ranging propagation condition under non-line-of-sight, denotes the power set of the identification framework, denotes the empty set;

[0055] A normalized innovation square is defined, and the specific algorithm of the normalized innovation square is as follows:

[0056] ,

[0057] in, Represents the normalized squared information. This represents the square of the information vector of the m-th base station. Represents the new information covariance matrix The m-th diagonal element;

[0058] A threshold for distinguishing between line-of-sight and non-line-of-sight distances is set, and the specific algorithm for the threshold is as follows:

[0059] ,

[0060] ,

[0061] in, This indicates the threshold for determining the line of sight. Indicates the non-line-of-sight discrimination threshold. This represents the inverse cumulative distribution function of the chi-square distribution with 1 degree of freedom. and These represent the probability thresholds for line-of-sight and non-line-of-sight purposes, respectively. ;

[0062] The BPA function is constructed using the following algorithm:

[0063] ,

[0064] ,

[0065] ,

[0066] in, This indicates that the m-th base station at time k... Allocation of evidence Indicates the m-th base station pair Allocation of evidence Represents an uncertain set Allocation of evidence This represents the maximum trust level. ;

[0067] Multi-base station evidence fusion is performed, and the multi-base station evidence fusion is based on Demspter synthesis rules;

[0068] Calculate adaptive weights for extended Kalman measurement updates.

[0069] Furthermore, the step of performing multi-base station evidence fusion specifically includes:

[0070] The specific algorithm for evidence fusion between the two base stations is as follows:

[0071] ,

[0072] ,

[0073] According to the two base station evidence fusion, the multi-base station evidence fusion is carried out, and the specific algorithm of the multi-base station evidence fusion is as follows:

[0074] ,

[0075] ,

[0076] Wherein, Indicates the evidence quality distribution of the ith base station and the jth base station at the kth moment to the proposition set Indicates the conflict coefficient between two pieces of evidence, Indicates the value of a proposition set of the ith base station at the kth moment, Indicates the value of a proposition set of the jth base station at the kth moment, Indicates the evidence distribution of the ith base station at the kth moment to the proposition set B, Indicates the evidence distribution of the jth base station at the kth moment to the proposition set C, Indicates the evidence distribution of the kth moment to the proposition set B obtained by the evidence fusion of the M base stations, Indicates the overall conflict coefficient of the M base stations, Indicates the proposition set variable corresponding to the Mth base station, Indicates the target proposition set to be fused, Indicates the number of base stations participating in the fusion, Indicates the base station number, Indicates the evidence distribution of the ith base station at the kth moment to the proposition set B, and it is agreed that =0, and . Further, the step of calculating the adaptive weight for measurement update of the extended Kalman includes: Further, the step of calculating the adaptive weight for measurement update of the extended Kalman includes:

[0077] According to the DS evidence theory, the belief function and the likelihood function of the line-of-sight condition are defined, and the specific algorithm of the belief function and the likelihood function is as follows:

[0078]

[0079] ,

[0080] ,

[0081] Wherein, Indicates the belief function of the line-of-sight condition, Indicates the likelihood function of the line-of-sight condition, ​​The evidence distribution of the proposition at time k after multi-base station fusion is represented as The evidence distribution of the universal set at time k after multi-base station fusion is represented as

[0082] An adaptive weight is calculated, and a specific algorithm of the adaptive weight is as follows:

[0083]

[0084] wherein, the adaptive weight is represented as

[0085] An adaptive factor of the extended Kalman measurement noise is constructed according to the adaptive weight, and a specific algorithm of the adaptive factor is as follows:

[0086]

[0087] wherein, the adaptive factor is represented as the adjustment parameter is represented as >0

[0088] The extended Kalman initial noise is corrected to complete the measurement update of the extended Kalman filter, and a specific algorithm of the measurement update is as follows:

[0089]

[0090]

[0091]

[0092]

[0093] wherein, the Kalman gain is represented as the state posterior estimation value at time k is represented as the state posterior covariance matrix at time k is represented as the updated observation matrix is represented as the correction noise is represented as the initial noise is represented as the UWB measurement vector is represented as the nonlinear measurement function is represented as I, and the unit matrix is represented as I.

[0094] The IMU and UWB combined positioning system based on the extended Kalman filter comprises:

[0095] ​​​​​​A combined positioning model construction module is configured to collect IMU and UWB data and establish a combined positioning model, which is a discrete state space model, and the combined positioning model includes a system state vector, a control input vector and a UWB measurement vector;

[0096] A preliminary prediction verification module is configured to perform preliminary ranging based on the combined positioning model and perform consistency verification to obtain a preliminary ranging value, the preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data, and the consistency verification is based on an adjacent difference matrix, and the consistency verification is used to filter out ranging values with non-line-of-sight or gross errors;

[0097] A filter update module is configured to perform filter update based on an extended Kalman filter algorithm to obtain a combined positioning result, and the filter update is based on multi-base station evidence fusion and adaptive weight.

[0098] The application further provides a storage medium storing one or more programs, and the programs are executed by a processor to implement the IMU and UWB combined positioning method based on the extended Kalman filter.

[0099] The application further provides a computer device including a memory and a processor, wherein:

[0100] The memory is used to store a computer program;

[0101] The processor is used to execute the computer program stored in the memory to implement the IMU and UWB combined positioning method based on the extended Kalman filter. BRIEF DESCRIPTION OF DRAWINGS

[0102] Figure 1 A flowchart of the IMU and UWB combined positioning method based on the extended Kalman filter for the first embodiment of the application;

[0103] Figure 2 A structural schematic diagram of the IMU and UWB combined positioning system based on the extended Kalman filter for the second embodiment of the application;

[0104] Figure 3 A logic schematic diagram of the IMU and UWB combined positioning method based on the extended Kalman filter for the first embodiment of the application;

[0105] Figure 4 A comparison effect diagram of the application and the prior art;

[0106] The following specific embodiments will further illustrate the application in combination with the above drawings. DETAILED DESCRIPTION

[0107] For the purpose of promoting an understanding of the application, the application will now be described in greater detail with reference to the figures. Several embodiments of the application are depicted in the drawings. However, the application can be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete. It will fully convey the scope of the application to those skilled in the art, and alternatives should be understood therefrom.

[0108] It is to be understood that where an element is referred to as being "on" another element, it can be directly on the other element or intervening elements can also be present. Where an element is referred to as being "connected", it is understood that, while the element can be directly connected to the other element, intervening elements can be present. As used herein the terms "vertical", "horizontal", "left", "right" and the like are merely used for the purpose of illustration and are not intended to be limiting.

[0109] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used in the description of the application herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.

[0110] Referring to FIG. 1, a flow chart of an IMU and UWB combined positioning method based on an extended Kalman filter according to a first embodiment of the present application is shown. The IMU and UWB combined positioning method based on an extended Kalman filter includes steps S01 to S03, wherein: Figure 1 Step S01: Collecting IMU and UWB data and establishing a combined positioning model;

[0111] It should be noted that in this embodiment, the combined positioning model is a discrete state space model, and the combined positioning model includes a system state vector, a control input vector, and a UWB measurement vector. The IMU and UWB data are collected.

[0112] The system state vector, the control input vector, and the UWB measurement vector are constructed, and the specific algorithm of the system state vector, the control input vector, and the UWB measurement vector is as follows:

[0113]

[0114]

[0115]

[0116]

[0117] wherein, represents the system state vector, represents the control input vector,​​​​ denotes a UWB measurement vector, , denotes a position component in navigation coordinates, , denotes a velocity component in navigation coordinates, , denotes an acceleration component in navigation coordinates, T denotes a transpose, , denotes an acceleration input component of the IMU, denotes a UWB ranging from the tag to the mth base station;

[0118] discretize the system state vector and the UWB measurement vector to construct a combined positioning model.

[0119] The specific algorithm for discretizing the system state vector and the UWB measurement vector is as follows:

[0120] ,

[0121] ,

[0122] wherein, denotes a system state vector, denotes a UWB measurement vector, denotes a state transition function, which is used to describe the motion relationship from the state at time k-1 to time k under the action of the control input vector , denotes process noise, denotes a measurement function, which is used to map the system state vector to the theoretical ranging between each base station and the tag, denotes measurement noise.

[0123] Step S02: preliminary ranging is performed according to the combined positioning model, and consistency check is performed to obtain preliminary ranging values;

[0124] It should be noted that in the embodiment, the preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency check is based on an adjacent difference matrix, and the consistency check is used to filter out ranging values with non-line-of-sight or gross errors. The system state vector is predicted based on the IMU data, and the prior prediction is obtained according to the state transition function and the covariance of the prior state prediction.

[0125] The specific algorithm for the prior prediction is as follows:

[0126] ,

[0127] wherein, represents a prior state prediction, represents a state transition function, represents a posterior state estimation at time k-1, represents a control input vector at time k;

[0128] According to the prior state prediction, theoretical measurement values between each base station and the tag are calculated, and a specific algorithm of the theoretical measurement values is as follows:

[0129] ,

[0130] wherein, represents a theoretical measurement value, represents a measurement function;

[0131] A difference between actual measurement values and the theoretical measurement values is calculated to obtain an innovation vector and an innovation covariance, and a specific algorithm of the innovation vector and the innovation covariance is as follows:

[0132] ,

[0133] ,

[0134] wherein, represents an innovation vector, represents a UWB measurement vector, represents an innovation covariance, represents an observation matrix, T represents a transpose, represents a prior state prediction error covariance matrix at time k, represents a UWB measurement noise covariance matrix;

[0135] A difference matrix is constructed, and a specific algorithm of the difference matrix is as follows:

[0136] ,

[0137] wherein, represents a difference matrix, and m represents a number of base stations;

[0138] Consistency is checked according to the difference matrix, and a specific algorithm of the consistency checking is as follows:

[0139] ,

[0140] ,

[0141] ,

[0142] ,

[0143] ,

[0144] ,

[0145] wherein, denotes the new innovation vector after adjacent difference, denotes the new innovation covariance matrix after adjacent difference, denotes the consistency detection statistic, denotes the false alarm probability, denotes the zero hypothesis that the current measurement has no gross error, denotes the chi-square random variable with degree of freedom r greater than the threshold value , denotes the distribution density function, denotes the test threshold value, denotes the cumulative distribution function;

[0146] When , it is determined that there is a gross error in the current measurement, the UWB data is not used, and only the IMU prediction result is retained.

[0147] When , filtering update is performed based on the extended Kalman filtering algorithm.

[0148] Step S03: filtering update is performed based on the extended Kalman filtering algorithm to obtain a combined positioning result.

[0149] It should be noted that in the embodiment, the filtering update is based on multi-base station evidence fusion and adaptive weight, and an identification framework is constructed, which is used to describe UWB ranging propagation conditions, including line-of-sight and non-line-of-sight.

[0150] The specific algorithm of the identification framework is as follows:

[0151] ,

[0152] ,

[0153] wherein, denotes the identification framework, denotes the UWB ranging propagation condition under line-of-sight, denotes the UWB ranging propagation condition under non-line-of-sight, denotes the power set of the identification framework, denotes the empty set.

[0154] The normalized innovation square is defined, and the specific algorithm is as follows:

[0155] ,

[0156] , The normalized innovation square is denoted as The square of the innovation vector of the mth base station is denoted as The mth diagonal element of the innovation covariance matrix is denoted as

[0157] The discrimination threshold of the line-of-sight and non-line-of-sight is set, and the specific algorithm is as follows:

[0158] ,

[0159] ,

[0160] , The line-of-sight discrimination threshold is denoted as The non-line-of-sight discrimination threshold is denoted as The inverse cumulative distribution function of the chi-square distribution with 1 degree of freedom is denoted as And The probability threshold of the line-of-sight and non-line-of-sight is denoted as , and the constraint is needed to ensure

[0161] The BPA function is constructed, and the specific algorithm is as follows:

[0162] ,

[0163] ,

[0164] ,

[0165] , The evidence distribution of the mth base station at the kth moment to is denoted as The evidence distribution of the mth base station to is denoted as The evidence distribution to the uncertain set is denoted as The maximum trust coefficient is denoted as , and in the embodiment, it is taken as =0.9;

[0166] The multi-base station evidence fusion is performed, and the multi-base station evidence fusion is based on the Demspeter combination rule;

[0167] Adaptive weights are calculated for measurement update of extended Kalman filter.

[0168] The specific algorithm of two base station evidence fusion is as follows:

[0169] ,

[0170] ,

[0171] According to two base station evidence fusion, multi-base station evidence fusion is carried out, and the specific algorithm of the multi-base station evidence fusion is as follows:

[0172] ,

[0173] ,

[0174] wherein, denotes the evidence quality distribution of the ith base station and the jth base station at the kth moment to the proposition set , denotes the conflict coefficient between two pieces of evidence, denotes the value of one proposition set of the ith base station at the kth moment, denotes the value of one proposition set of the jth base station at the kth moment, denotes the evidence distribution of the ith base station at the kth moment to the proposition set B, denotes the evidence distribution of the jth base station at the kth moment to the proposition set C, denotes the evidence distribution of the proposition set at the kth moment obtained by M base station evidence fusion, denotes the overall conflict coefficient of the M base stations, denotes the proposition set variable corresponding to the Mth base station, denotes the target proposition set to be fused, denotes the number of base stations participating in fusion, denotes the base station number, denotes the evidence distribution of the ith base station at the kth moment to the proposition set , and it is agreed that =0, and .

[0175] According to the definition of the DS evidence theory, the trust function and the likelihood function of the sight distance condition are defined, and the specific algorithm of the trust function and the likelihood function is as follows:

[0176] ,

[0177] ,

[0178] wherein, a trust function representing the line-of-sight condition, a likelihood function representing the line-of-sight condition, an evidence assignment of the proposition at time k after multi-base station fusion, an evidence assignment of the universal set at time k after multi-base station fusion;

[0179] calculating an adaptive weight, the specific algorithm of which is as follows:

[0180] ,

[0181] wherein, the adaptive weight represents an adaptive weight,

[0182] constructing an adaptive factor of the extended Kalman measurement noise according to the adaptive weight, the specific algorithm of which is as follows:

[0183] ,

[0184] wherein, the adaptive factor represents an adaptive factor, the adjustment parameter represents an adjustment parameter, > 0;

[0185] correcting the extended Kalman initial noise to complete the measurement update of the extended Kalman filter, the specific algorithm of which is as follows:

[0186] ,

[0187] ,

[0188] ,

[0189] ,

[0190] wherein, the Kalman gain represents a Kalman gain, the state posterior estimation value at time k represents a state posterior estimation value at time k, the state posterior covariance matrix at time k represents a state posterior covariance matrix at time k, the updated observation matrix represents an updated observation matrix, the correction noise represents a correction noise, the initial noise represents an initial noise, the UWB measurement vector represents a UWB measurement vector, the nonlinear measurement function represents a nonlinear measurement function, and I represents a unit matrix;

[0191] The present application is compared with the prior art, and the specific logic flow is described in Figure 3 , and the final comparison result is described inFigure 4 .

[0192] In summary, according to the above-mentioned IMU and UWB combined positioning method based on extended Kalman filter, the IMU and UWB data are collected and a combined positioning model is established, preliminary ranging is performed according to the combined positioning model, and consistency test is performed, the UWB measurement is autonomously detected and gross error is identified, the innovation is used to suppress the geometrically inconsistent measurement values, when the detection exceeds the threshold, the UWB data at this moment is not used, only the IMU prediction result is reserved, when the detection passes, the standardized innovation square of each channel is used as a statistical index, which is mapped to the basic probability assignment function in the DS evidence theory, then the multi-channel measurement evidence is fused through the DS evidence combination rule, and the fusion result is converted into the measurement noise covariance adaptive adjustment, the UWB measurement value is updated in the extended Kalman filter algorithm, and the positioning precision and stability of the IMU and UWB combined positioning in the complex indoor environment are improved. Specifically, the IMU and UWB data are collected and a combined positioning model is established, the combined positioning model is a discrete state space model, the combined positioning model includes a system state vector, a control input vector and a UWB measurement vector, preliminary ranging is performed according to the combined positioning model, and consistency test is performed, to obtain a preliminary ranging value, the preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency test is based on an adjacent difference matrix, and the consistency test is used to exclude ranging values with non-line-of-sight or gross error, and the filtering update is performed based on the extended Kalman filter algorithm, to obtain a combined positioning result, the filtering update is based on multi-base station evidence fusion and adaptive weight, and the positioning precision and stability of the IMU and UWB combined positioning in the complex indoor environment are improved.

[0193] Please refer to Figure 2 , which is a structure schematic diagram of the IMU and UWB combined positioning system based on the extended Kalman filter according to the second embodiment of the present application, and the system comprises:

[0194] A combined positioning model construction module 10 is used to collect IMU and UWB data and establish a combined positioning model, the combined positioning model is a discrete state space model, and the combined positioning model includes a system state vector, a control input vector and a UWB measurement vector;

[0195] A preliminary prediction test module 20 is used to perform preliminary ranging according to the combined positioning model and perform consistency test, to obtain a preliminary ranging value, the preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency test is based on an adjacent difference matrix, and the consistency test is used to exclude ranging values with non-line-of-sight or gross error;

[0196] The filter updating module 30 is configured to perform filter updating based on an extended Kalman filter algorithm to obtain the combined positioning result, and the filter updating is based on multi-base station evidence fusion and adaptive weight.

[0197] The application further provides a computer storage medium, which stores one or more programs, and the programs are executed by a processor to implement the IMU and UWB combined positioning method based on the extended Kalman filter.

[0198] The application further provides a computer device, which comprises a memory and a processor, wherein the memory is configured to store a computer program, and the processor is configured to execute the computer program stored in the memory to implement the IMU and UWB combined positioning method based on the extended Kalman filter.

[0199] Those skilled in the art can understand that the logic and / or steps represented in the flowchart or described herein can be considered as a list of executable instructions for implementing the logic function, which can be specifically implemented in any computer readable medium for use by or in conjunction with an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor or other system that can fetch and execute instructions from the instruction execution system, device or apparatus) or in conjunction with these instruction execution systems, devices or apparatus. For the present specification, the "computer readable medium" can be any device that can contain a program for storing, communicating, propagating or transmitting the program for use by or in conjunction with the instruction execution system, device or apparatus or in conjunction with these instruction execution systems, devices or apparatus.

[0200] More specific examples (a non-exhaustive list) of the computer readable medium include the following: an electrical connection having one or more wires (electrical devices), a portable computer diskette (magnetic devices), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). In addition, the computer readable medium can even be paper or other suitable medium on which the program can be printed, because the program can be electronically obtained, for example, by optical scanning of the paper or other medium, followed by electronic conversion, interpretation or processing, if necessary, in other suitable ways, and then stored in a computer memory.

[0201] It should be understood that aspects of the application can be implemented in hardware, software, firmware or combinations thereof. In the embodiments described above, various steps or methods can be implemented, for example, in software or firmware that is stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, and in another embodiment, any of the following techniques can be used to implement the hardware used to implement the described functions: discrete logic circuitry having logic gates for implementing logic functions upon data signals, application specific integrated circuits having logic gates for implementing the logic functions on data signals, programmable gate arrays (PGA), field programmable gate arrays (FPGA), and the like.

[0202] In the description of the specification, the description of the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" and the like means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In the specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0203] The above-described embodiments only express several implementation manners of the present application, which are described in a more specific and detailed manner, but cannot be understood as a limitation on the scope of the patent of the present application. It should be noted that, for those skilled in the art, several modifications and improvements can be made without departing from the concept of the present application, which are all within the protection scope of the present application. Therefore, the protection scope of the patent of the present application should be subject to the appended claims.

Claims

1. An IMU and UWB combined positioning method based on extended Kalman filtering, characterized in that, The method comprises the following steps: Collecting IMU and UWB data and establishing a combined positioning model, the combined positioning model being a discrete state space model, the combined positioning model comprising a system state vector, a control input vector and a UWB measurement vector; Performing preliminary ranging according to the combined positioning model and performing consistency checking to obtain preliminary ranging values, the preliminary ranging comprising prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency checking being based on an adjacent difference matrix, and the consistency checking being used to filter out ranging values with non-line-of-sight or gross errors; The step of performing preliminary ranging according to the combined positioning model and performing consistency checking to obtain preliminary ranging values specifically comprises: Performing prior prediction on the system state vector based on IMU data, the prior prediction being used to obtain prior state prediction and covariance of the prior state prediction according to a state transition function; The specific algorithm of the prior prediction is as follows: ， wherein, represents a prior state prediction, represents a state transition function, represents a posterior state estimate at time k-1, represents a control input vector at time k. According to the prior state prediction, calculating the theoretical measurement value between each base station and the tag, the specific algorithm of the theoretical measurement value being as follows: ， wherein represents a theoretical measured value, represents a measured function; Calculating the difference between the actual measurement value and the theoretical measurement value to obtain an innovation vector and an innovation covariance, the specific algorithm of the innovation vector and the innovation covariance being as follows: ， ， wherein, denotes an innovation vector, denotes a UWB measurement vector, denotes an innovation covariance, denotes an observation matrix, T denotes a transpose, denotes a prior state prediction error covariance matrix at time k, denotes a UWB measurement noise covariance matrix; Constructing an adjacent difference matrix, the specific algorithm of the adjacent difference matrix being as follows: ， wherein denotes the adjacent difference matrix, m denotes the number of base stations; Performing consistency checking according to the adjacent difference matrix, the specific algorithm of the consistency checking being as follows: ， ， ， ， ， ， wherein denotes the vector of innovations after the adjacent differencing, denotes the covariance matrix of innovations after the adjacent differencing, denotes the consistency detection statistic, denotes the probability of false alarm, denotes the null hypothesis that the current measurement is free of gross errors, denotes a chi-squared random variable with degrees of freedom r greater than a threshold value , the probability that denotes the density function of the distribution, denotes the test threshold value, denotes the cumulative distribution function; When then it is determined that there is a gross error in the current measurement, the UWB data is not used, and only the IMU prediction result is retained; When filtering update is performed based on an extended Kalman filtering algorithm; Performing filtering update based on an extended Kalman filtering algorithm to obtain a combined positioning result, the filtering update being based on multi-base station evidence fusion and adaptive weight.

2. The EKF-based IMU and UWB combined positioning method according to claim 1, characterized in that, The step of collecting IMU and UWB data and establishing a combined positioning model specifically comprises: Collecting IMU and UWB data; Constructing a system state vector, a control input vector and a UWB measurement vector, the specific algorithm of the system state vector, the control input vector and the UWB measurement vector being as follows: ， ， ， wherein, denotes a system state vector, denotes a control input vector, denotes a UWB measurement vector, , denotes a position component in navigation coordinates, , denotes a velocity component in navigation coordinates, , denotes an acceleration component in navigation coordinates, T denotes a transpose, , denotes an acceleration input component of the IMU, denotes a UWB range from the tag to the m-th base station; Discretizing the system state vector and the UWB measurement vector to construct a combined positioning model.

3. The EKF-based IMU and UWB combined positioning method according to claim 2, characterized in that, The step of discretizing the system state vector and the UWB measurement vector specifically comprises: The specific algorithm of the discretization of the system state vector and the UWB measurement vector is as follows: ， ， wherein, denotes a system state vector, denotes a UWB measurement vector, denotes a state transition function describing the motion relationship from the state at time k-1 to time k under the action of the control input vector denotes a measurement function mapping the system state vector denotes a process noise, denotes a measurement function mapping the system state vector to the theoretical ranging between each base station and tag, denotes a measurement noise.

4. The EKF-based IMU and UWB combined positioning method according to claim 1, characterized in that, The step of performing filtering update based on an extended Kalman filtering algorithm to obtain a combined positioning result specifically comprises: Constructing a recognition framework, the recognition framework being used to describe UWB ranging propagation conditions, the UWB ranging propagation conditions comprising line-of-sight and non-line-of-sight; The specific algorithm of the recognition framework is as follows: ， ， wherein, represents a recognition framework, represents a UWB ranging propagation condition under line of sight, represents a UWB ranging propagation condition under non-line of sight, represents a power set of a recognition framework, represents an empty set; Defining normalized innovation square, the specific algorithm of the normalized innovation square being as follows: ， wherein, denotes the normalized innovation squared, denotes the innovation vector squared of the mth base station, denotes the mth diagonal element of the innovation covariance matrix of the mth base station. Setting a discrimination threshold for line-of-sight and non-line-of-sight, the specific algorithm of the discrimination threshold being as follows: ， ， wherein, denotes a line-of-sight decision threshold, denotes a non-line-of-sight decision threshold, denotes a chi-squared distribution inverse cumulative distribution function of degree 1, and denote probability thresholds for line-of-sight and non-line-of-sight, respectively, ; Constructing a BPA function, the specific algorithm of the BPA function being as follows: ， ， ， wherein, represents the evidence assignment of the mth base station at time k to represents the evidence assignment of the mth base station to represents the evidence assignment to the set of uncertainties represents the maximum trust coefficient, ;​​​ Performing multi-base station evidence fusion, the multi-base station evidence fusion being based on a Demspeter combination rule; Calculating an adaptive weight to perform measurement update of the extended Kalman.

5. The EKF-based IMU and UWB combined positioning method according to claim 4, characterized in that, The step of performing multi-base station evidence fusion specifically comprises: The specific algorithm of two-base station evidence fusion is as follows: ， ， Performing multi-base station evidence fusion according to two-base station evidence fusion, the specific algorithm of the multi-base station evidence fusion being as follows: ， ， wherein, denotes the evidence mass distribution of the ith base station at time k for the proposition set , denotes the conflict coefficient between two pieces of evidence, denotes the value of a proposition set at time k for the ith base station, denotes the value of a proposition set at time k for the jth base station, denotes the evidence distribution of the ith base station at time k for the proposition set B, denotes the evidence distribution of the jth base station at time k for the proposition set C, denotes the evidence distribution of the proposition set at time k obtained by fusing the evidence of M base stations, denotes the overall conflict coefficient of M base stations, denotes the proposition set variable corresponding to the Mth base station, denotes the target proposition set to be fused, denotes the number of base stations participating in fusion, denotes the base station number, denotes the evidence distribution of the ith base station at time k for the proposition set , and it is agreed that = 0, and .

6. The EKF-based IMU and UWB combined positioning method according to claim 4, characterized in that, The step of calculating the adaptive weight for measurement update of the extended Kalman filter specifically comprises: The belief function and likelihood function of the line-of-sight condition are defined according to the DS evidence theory, and the specific algorithm of the belief function and likelihood function is as follows: ， ， wherein, a belief function representing the line-of-sight condition, a likelihood function representing the line-of-sight condition, an evidence assignment to the proposition at time k after fusion of multiple base stations, an evidence assignment to the universal set at time k after fusion of multiple base stations; The adaptive weight is calculated, and the specific algorithm of the adaptive weight is as follows: ， wherein denotes an adaptive weight; An adaptive factor of the measurement noise of the extended Kalman filter is constructed according to the adaptive weight, and the specific algorithm of the adaptive factor is as follows: ， wherein denotes an adaptation factor, denotes a regulation parameter, > 0; The initial noise of the extended Kalman filter is corrected to complete the measurement update of the extended Kalman filter, and the specific algorithm of the measurement update is as follows: ， ， ， ， wherein, denotes the Kalman gain, denotes the state posterior estimate at time k, denotes the state posterior covariance matrix at time k, denotes the updated observation matrix, denotes the correction noise, denotes the initial noise, denotes the UWB measurement vector, denotes the nonlinear measurement function, I denotes the identity matrix.

7. An IMU and UWB combined positioning system based on extended Kalman filtering, characterized in that, It comprises: The combined positioning model construction module is used to collect IMU and UWB data and establish a combined positioning model, the combined positioning model is a discrete state space model, and the combined positioning model comprises a system state vector, a control input vector and a UWB measurement vector; The preliminary prediction and verification module is used to perform preliminary ranging according to the combined positioning model and perform consistency verification to obtain a preliminary ranging value, the preliminary ranging comprises prior prediction based on IMU data and theoretical ranging based on UWB data, the consistency verification is based on an adjacent difference matrix, and the consistency verification is used to filter out ranging values with non-line-of-sight or gross errors; The step of performing preliminary ranging according to the combined positioning model and performing consistency verification to obtain a preliminary ranging value specifically comprises: The system state vector is predicted based on IMU data, and the prior prediction obtains a prior state prediction and a covariance of the prior state prediction according to a state transition function; The specific algorithm of the prior prediction is as follows: ， wherein, represents a prior state prediction, represents a state transition function, represents a posterior state estimate at time k-1, represents a control input vector at time k. According to the prior state prediction, the theoretical measurement value between each base station and the tag is calculated, and the specific algorithm of the theoretical measurement value is as follows: ， wherein represents a theoretical measured value, represents a measured function; The difference between the actual measurement value and the theoretical measurement value is calculated to obtain an innovation vector and an innovation covariance, and the specific algorithm of the innovation vector and the innovation covariance is as follows: ， ， wherein, denotes an innovation vector, denotes a UWB measurement vector, denotes an innovation covariance, denotes an observation matrix, T denotes a transpose, denotes a prior state prediction error covariance matrix at time k, denotes a UWB measurement noise covariance matrix; An adjacent difference matrix is constructed, and the specific algorithm of the adjacent difference matrix is as follows: ， wherein denotes the adjacent difference matrix, m denotes the number of base stations; Consistency verification is performed according to the adjacent difference matrix, and the specific algorithm of the consistency verification is as follows: ， ， ， ， ， ， wherein denotes the vector of innovations after the adjacent differencing, denotes the covariance matrix of innovations after the adjacent differencing, denotes the consistency detection statistic, denotes the probability of false alarm, denotes the null hypothesis that the current measurement is free of gross errors, denotes a chi-squared random variable with degrees of freedom r greater than a threshold value , the probability that denotes the density function of the distribution, denotes the test threshold value, denotes the cumulative distribution function; When then it is determined that there is a gross error in the current measurement, the UWB data is not used, and only the IMU prediction result is retained; When filtering update is performed based on an extended Kalman filtering algorithm; The filter update module is used to perform filter update based on the extended Kalman filter algorithm to obtain a combined positioning result, and the filter update is based on multi-base station evidence fusion and adaptive weight.

8. A storage medium, characterized by The storage medium stores one or more programs, which are executed by the processor to implement the extended Kalman filter-based IMU and UWB combined positioning method according to any one of claims 1-6.

9. A computer device, comprising: The computer device comprises a memory and a processor, wherein: The memory is used to store computer programs; The processor is used to execute the computer programs stored on the memory to implement the extended Kalman filter-based IMU and UWB combined positioning method according to any one of claims 1-6. The computer device comprises a memory and a processor, wherein: The memory is used to store computer programs; The processor is used to execute the computer programs stored on the memory to implement the extended Kalman filter-based IMU and UWB combined positioning method according to any one of claims 1-6.

Images