IMU and UWB combined positioning method based on extended Kalman filtering

Abstract

The invention relates to the field of intelligent positioning, and provides an IMU (Inertial Measurement Unit) and UWB (Ultra Wideband) combined positioning method based on extended Kalman filtering, which comprises the following steps: acquiring IMU and UWB data, establishing a combined positioning model, carrying out preliminary distance measurement according to the combined positioning model, carrying out consistency check, and carrying out autonomous detection and gross error identification on UWB measurement. According to the method, measured values which are geometrically inconsistent are suppressed by using information, when detection exceeds a threshold, UWB data at the moment are not adopted, only an IMU prediction result is reserved, and when detection passes, standardized information square of each channel is taken as a statistical index and mapped to a basic probability distribution function in a DS evidence theory; according to the method, the UWB measurement value is subjected to UWB measurement, multi-channel measurement evidences are fused through a DS evidence combination rule, a fusion result is converted into measurement noise covariance for adaptive adjustment, filtering updating of the UWB measurement value in an extended Kalman filtering algorithm is achieved, and the positioning precision and stability of IMU and UWB combined positioning in a complex indoor environment are improved.

Description

Technical Field

[0001] This invention relates to the field of intelligent positioning, and in particular to a combined positioning method based on extended Kalman filtering and IMU and UWB. Background Technology

[0002] In recent years, the rapid development of indoor automated guided vehicles, indoor delivery robots, and underground autonomous parking technologies has created an increasingly urgent need for centimeter-level precision positioning in indoor environments. However, in the complex environments of factory workshops, underground workshops, and deep coal mines, indoor radio signals are easily blocked by obstacles during propagation, resulting in strong reflection, scattering, and attenuation, which significantly degrades positioning accuracy and fails to meet indoor positioning requirements.

[0003] In existing technologies, single wireless device sensors are susceptible to environmental factors, while inertial units can autonomously calculate and locate without relying on any external information. Ultra-wideband (UWB) and inertial measurement units (IMUs) complement each other, compensating for each other's shortcomings. Therefore, combined positioning methods using these two sensors have attracted attention. However, existing combined positioning methods using UWB and IMUs have two main problems. First, some studies mainly utilize neural networks or traditional adaptive filtering algorithms to compensate for observation errors. However, neural network-based methods have weak adaptability to different environments and are expensive, while conventional adaptive algorithms are less effective in complex environments. In complex indoor environments, the ability to correct positioning errors is limited. On the other hand, in real-world complex indoor environments, the uncertainty of the external environment can lead to unpredictable gross errors in the observation data, causing conflicts between the UWB observation position and the IMU predicted position. The ability to adaptively sense environmental changes and effectively handle observation conflicts has become the key to achieving high-precision positioning. However, existing UWB and inertial measurement unit combined positioning algorithms are often structurally complex, require high computing power, and rely on empirical thresholds or specific environmental geometric features for hard decisions. This makes it difficult to achieve stable and high-precision positioning in low-cost chips and complex indoor environments.

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

[0005] Based on this, the present invention proposes an IMU and UWB combined positioning method based on extended Kalman filtering. This method collects IMU and UWB data and establishes a combined positioning model. Preliminary ranging is performed based on the combined positioning model, and consistency checks are conducted. UWB measurements are autonomously detected and gross errors are identified. Innovative features are used to suppress geometrically inconsistent measurements. When a detection exceeds a threshold, the UWB data at that moment is not used, and only the IMU prediction result is retained. When a detection passes, the standardized squared innovation of each channel is used as a statistical index and mapped to the basic probability allocation function in the DS evidence theory. Then, multi-channel measurement evidence is fused using DS evidence combination rules, and the fusion result is transformed into adaptive adjustment of the measurement noise covariance. This allows UWB measurements to be filtered and updated using the extended Kalman filtering algorithm. The present invention improves the positioning accuracy and stability of IMU and UWB combined positioning in complex indoor environments.

[0006] This invention proposes a combined IMU and UWB localization method based on extended Kalman filtering, comprising: Collect IMU and UWB data and establish a combined positioning model. The combined positioning model is a discrete state-space model, which includes a system state vector, a control input vector, and a UWB measurement vector. Preliminary ranging is performed based on the combined positioning model, and a consistency check is conducted to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency check is based on the adjacent difference matrix and is used to screen out ranging values ​​that are not line-of-sight or have gross errors. The extended Kalman filter algorithm is used to update the filter and obtain the combined positioning result. The filter update is based on multi-base station evidence fusion and adaptive weights.

[0007] In summary, based on the aforementioned IMU and UWB combined positioning method using extended Kalman filtering, this invention collects IMU and UWB data and establishes a combined positioning model. Preliminary ranging and consistency checks are performed based on this model. UWB measurements are autonomously detected and gross errors are identified. Innovative features are used to suppress geometrically inconsistent measurements. When a detection exceeds a threshold, the UWB data at that moment is not used, and only the IMU prediction result is retained. When a detection passes, the standardized squared innovation of each channel is used as a statistical index and mapped to the basic probability allocation function in the DS evidence theory. Then, multi-channel measurement evidence is fused using DS evidence combination rules, and the fusion result is transformed into adaptive adjustment of the measurement noise covariance. This allows UWB measurements to be filtered and updated using the extended Kalman filtering algorithm. This invention improves the positioning accuracy and stability of IMU and UWB combined positioning in complex indoor environments. Specifically, IMU and UWB data are collected and a combined positioning model is established. The combined positioning model is a discrete state-space model, which includes a system state vector, a control input vector, and a UWB measurement vector. Preliminary ranging is performed based on the combined positioning model, and a consistency check is conducted to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency check is based on the adjacent difference matrix and is used to filter ranging values ​​that are not line-of-sight or have gross errors. The extended Kalman filter algorithm is used for filtering and updating to obtain the combined positioning result. The filtering and updating is based on multi-base station evidence fusion and adaptive weights. This invention improves the positioning accuracy and stability of IMU and UWB combined positioning in complex indoor environments.

[0008] Furthermore, the step of acquiring IMU and UWB data and establishing a combined localization model specifically includes: Collect IMU and UWB data; The system state vector, control input vector, and UWB measurement vector are constructed using the following specific algorithms: , , , in, Represents the system state vector. Represents the control input vector. Represents the UWB measurement vector. , Represents the position components in the navigation coordinates. , Represents the velocity components in the navigation coordinates. , Represents the acceleration components in the navigation coordinates. T Indicates transpose. , This represents the acceleration input component of the IMU. This indicates the UWB ranging from the tag to the m-th base station; The system state vector and UWB measurement vector are discretized to construct a combined positioning model.

[0009] Furthermore, the step of discretizing the system state vector and the UWB measurement vector specifically includes: The specific algorithms for discretizing the system state vector and UWB measurement vector are as follows: , , in, Represents the system state vector. Represents the UWB measurement vector. This represents the state transition function, which describes the state transition function used to describe the state transition in the control input vector. The motion relationship from the state at time k-1 to time k under the action of [the action]. Indicates process noise. This represents a measurement function, which is used to represent the system state vector. The mapping is the theoretical ranging between each base station and the tag. This indicates measurement noise.

[0010] Furthermore, the step of performing preliminary ranging based on the combined positioning model and conducting a consistency check to obtain preliminary ranging values ​​specifically includes: The system state vector is predicted a priori based on IMU data. The priori prediction is obtained by using the state transition function to obtain the prior state prediction and the covariance of the prior state prediction. The specific algorithm for the prior prediction is as follows: , in, Indicates prior state prediction, Represents the state transition function. This represents the posterior state estimate at time k-1. express The control input vector at each time step; Based on the prior state prediction, the theoretical measurement value between each base station and the tag is calculated. The specific algorithm for calculating the theoretical measurement value is as follows: , in, Indicates the theoretical measurement value. Represents a measurement function; The difference between the actual measured value and the theoretical measured value is calculated to obtain the innovation vector and the innovation covariance. The specific algorithms for the innovation vector and the innovation covariance are as follows: , , in, Represents the innovation vector. Represents the UWB measurement vector. Indicates the new information covariance. Represents the observation matrix. T Indicates transpose. Let the prior state prediction error covariance matrix at time k be denoted as . Represents the UWB measurement noise covariance matrix; The specific algorithm for constructing the adjacent difference matrix is ​​as follows: , in, represents the adjacent difference matrix, and m represents the number of base stations; A consistency check is performed based on the adjacent difference matrices. The specific algorithm for the consistency check is as follows: , , , , , , in, This represents the innovation vector after adjacent differences. This represents the new information covariance matrix after adjacent differences. This represents the consistency detection statistic. Indicates the probability of a false alarm. This indicates the null hypothesis that the current measurement has no gross errors. Describes a chi-square random variable with r degrees of freedom. Greater than the threshold The probability, Represents the probability density function. Indicates the test threshold. Represents the cumulative distribution function; when If the error is large, the UWB data will not be used, and only the IMU prediction results will be retained. when At that time, the filter is updated based on the extended Kalman filter algorithm.

[0011] Furthermore, the step of updating the filter based on the extended Kalman filter algorithm to obtain the combined localization result specifically includes: A recognition framework is constructed to describe UWB ranging propagation conditions, which include line-of-sight and non-line-of-sight conditions. The specific algorithm of the identification framework is as follows: , , in, Represents the identification framework. This indicates the UWB ranging propagation conditions at line-of-sight. This indicates the UWB ranging propagation conditions under non-line-of-sight conditions. The power set of the identification frame is represented. Represents the empty set; The normalized innovation square is defined, and the specific algorithm for the normalized innovation square is as follows: , 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; 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: , , in, This indicates the threshold for determining the line of sight. This represents 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. ; The BPA function is constructed using the following algorithm: , , , 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. ; Multi-base station evidence fusion is performed, and the multi-base station evidence fusion is based on Demspter synthesis rules; Calculate adaptive weights for extended Kalman measurement updates.

[0012] Furthermore, the step of performing multi-base station evidence fusion specifically includes: The specific algorithm for evidence fusion between the two base stations is as follows: , , Multi-base station evidence fusion is performed based on evidence fusion from two base stations. The specific algorithm for multi-base station evidence fusion is as follows: , , in, This indicates that the i-th base station and the j-th base station at time k represent the set of propositions. Allocation of evidence quality This represents the conflict coefficient between two pieces of evidence. This represents the set of proposition values ​​taken by the i-th base station at time k. This represents the set of proposition values ​​taken by the j-th base station at time k. This represents the evidence allocation for proposition set B at time k at the i-th base station. This represents the evidence allocation of the j-th base station to the proposition set C at time k. This represents the set of propositions obtained by fusing evidence from M base stations at time k. Allocation of evidence This represents the overall collision coefficient of base station M. Let the variable represent the set of propositions corresponding to the Mth base station. This represents the set of target propositions to be merged. This indicates the number of base stations participating in the integration. Indicates the base station number. This indicates that the i-th base station is at time k in the set of propositions. Evidence allocation, agreement =0, and .

[0013] Furthermore, the step of calculating adaptive weights for extended Kalman measurement updates specifically includes: Based on the DS evidence theory, the trust function and likelihood function for the distance condition are defined, and the specific algorithms for the trust function and likelihood function are as follows: , , in, The trust function represents the line-of-sight condition. The likelihood function represents the line-of-sight condition. This indicates that after multi-base station fusion, at time k, the proposition... Allocation of evidence This indicates that after the fusion of multiple base stations, the entire set is viewed at time K. Allocation of evidence; The adaptive weights are calculated using the following algorithm: , in, Indicates adaptive weights; An adaptive factor for the extended Kalman measurement noise is constructed based on the adaptive weights. The specific algorithm for the adaptive factor is as follows: , in, Indicates the adaptive factor. Indicates the adjustment parameter. >0; The initial noise of the extended Kalman filter is corrected to complete the measurement update of the extended Kalman filter. The specific algorithm for the measurement update is as follows: , , , , in, Indicates Kalman gain, This represents the posterior estimate of the state at time k. Let the posterior covariance matrix of the state at time k be denoted as . This represents the updated observation matrix. Indicates noise correction. Indicates the initial noise. Represents the UWB measurement vector. Let I represent the nonlinear measurement function, and let I represent the identity matrix.

[0014] This invention proposes a combined IMU and UWB positioning system based on extended Kalman filtering, comprising: The integrated positioning model construction module is used to collect IMU and UWB data and establish an integrated positioning model. The integrated positioning model is a discrete state-space model, which includes 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 and consistency verification based on the combined positioning model to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency verification is based on the adjacent difference matrix and is used to screen out ranging values ​​that are not line-of-sight or have gross errors. The filtering update module is used to perform filtering updates based on the extended Kalman filter algorithm to obtain combined positioning results. The filtering update is based on multi-base station evidence fusion and adaptive weights.

[0015] The present invention also provides a storage medium that stores one or more programs, which, when executed by a processor, implement the above-described IMU and UWB combined positioning method based on extended Kalman filtering.

[0016] The present invention also provides a computer device, the computer device including a memory and a processor, wherein: The memory is used to store computer programs; When the processor executes the computer program stored in the memory, it implements the IMU and UWB combined localization method based on extended Kalman filtering as described above. Attached Figure Description

[0017] Figure 1 This is a flowchart of the IMU and UWB combined localization method based on extended Kalman filtering proposed in the first embodiment of the present invention; Figure 2 This is a schematic diagram of the structure of the IMU and UWB combined positioning system based on extended Kalman filtering proposed in the second embodiment of the present invention; Figure 3 This is a logical schematic diagram of the IMU and UWB combined localization method based on extended Kalman filtering proposed in the first embodiment of the present invention; Figure 4 This is a comparison diagram of the present invention and the prior art; The following detailed description, in conjunction with the accompanying drawings, will further illustrate the present invention. Detailed Implementation

[0018] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Several embodiments of the invention are illustrated in the drawings. However, the invention can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.

[0019] It should be noted that when a component is said to be "fixed to" another component, it can be directly on the other component or there may be an intervening component. When a component is said to be "connected to" another component, it can be directly connected to the other component or there may be an intervening component. The terms "vertical," "horizontal," "left," "right," and similar expressions used in this document are for illustrative purposes only.

[0020] 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 invention pertains. The terminology used herein in the description of the invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0021] Please see Figure 1 The diagram shows a flowchart of the IMU and UWB combined localization method based on extended Kalman filtering proposed in the first embodiment of the present invention. This IMU and UWB combined localization method based on extended Kalman filtering includes steps S01 to S03, wherein: Step S01: Collect IMU and UWB data and establish a combined localization model; It should be noted that in this embodiment, the combined positioning model is a discrete state-space model, which includes a system state vector, a control input vector, and a UWB measurement vector, and collects IMU and UWB data. The system state vector, control input vector, and UWB measurement vector are constructed using the following specific algorithms: , , , in, Represents the system state vector. Represents the control input vector. Represents the UWB measurement vector. , Represents the position components in the navigation coordinates. , Represents the velocity components in the navigation coordinates. , Represents the acceleration components in the navigation coordinates. T Indicates transpose. , This represents the acceleration input component of the IMU. This indicates the UWB ranging from the tag to the m-th base station; The system state vector and UWB measurement vector are discretized to construct a combined positioning model.

[0022] The specific algorithms for discretizing the system state vector and UWB measurement vector are as follows: , , in, Represents the system state vector. Represents the UWB measurement vector. This represents the state transition function, which describes the state transition function used to describe the state transition in the control input vector. The motion relationship from the state at time k-1 to time k under the action of [the action]. Indicates process noise. This represents a measurement function, which is used to represent the system state vector. The mapping is the theoretical ranging between each base station and the tag. This indicates measurement noise.

[0023] Step S02: Perform preliminary ranging based on the combined positioning model and conduct a consistency check to obtain preliminary ranging values; It should be noted that in this embodiment, the preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency test is based on the adjacent difference matrix. The consistency test is used to screen ranging values ​​that are not line-of-sight or have gross errors. Prior prediction is performed on the system state vector based on IMU data. The prior prediction obtains the prior state prediction and the covariance of the prior state prediction based on the state transition function. The specific algorithm for the prior prediction is as follows: , in, Indicates prior state prediction, Represents the state transition function. This represents the posterior state estimate at time k-1. express The control input vector at each time step; Based on the prior state prediction, the theoretical measurement value between each base station and the tag is calculated. The specific algorithm for calculating the theoretical measurement value is as follows: , in, Indicates the theoretical measurement value. Represents a measurement function; The difference between the actual measured value and the theoretical measured value is calculated to obtain the innovation vector and the innovation covariance. The specific algorithms for the innovation vector and the innovation covariance are as follows: , , in, Represents the innovation vector. Represents the UWB measurement vector. Indicates the new information covariance. Represents the observation matrix. T Indicates transpose. Let the prior state prediction error covariance matrix at time k be denoted as . Represents the UWB measurement noise covariance matrix; The specific algorithm for constructing the adjacent difference matrix is ​​as follows: , in, represents the adjacent difference matrix, and m represents the number of base stations; A consistency check is performed based on the adjacent difference matrices. The specific algorithm for the consistency check is as follows: , , , , , , in, This represents the innovation vector after adjacent differences. This represents the new information covariance matrix after adjacent differences. This represents the consistency detection statistic. Indicates the probability of a false alarm. This indicates the null hypothesis that the current measurement has no gross errors. Describes a chi-square random variable with r degrees of freedom. Greater than the threshold The probability, Represents the probability density function. Indicates the test threshold. Represents the cumulative distribution function; when If the error is large, the UWB data will not be used, and only the IMU prediction results will be retained. when At that time, the filter is updated based on the extended Kalman filter algorithm.

[0024] Step S03: Perform filter updates based on the extended Kalman filter algorithm to obtain the combined localization results; It should be noted that in this embodiment, the filtering update is based on multi-base station evidence fusion and adaptive weights to construct an identification framework. The identification framework is used to describe the UWB ranging propagation conditions, which include line-of-sight and non-line-of-sight conditions. The specific algorithm of the identification framework is as follows: , , in, Represents the identification framework. This indicates the UWB ranging propagation conditions at line-of-sight. This indicates the UWB ranging propagation conditions under non-line-of-sight conditions. The power set of the identification frame is represented. Represents the empty set; The normalized innovation square is defined, and the specific algorithm for the normalized innovation square is as follows: , 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; 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: , , in, This indicates the threshold for determining the line of sight. This represents 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, respectively, to ensure... Constraints are needed. ; The BPA function is constructed using the following algorithm: , , , 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. In this embodiment, we take =0.9; Multi-base station evidence fusion is performed, and the multi-base station evidence fusion is based on Demspter synthesis rules; Calculate adaptive weights for extended Kalman measurement updates.

[0025] The specific algorithm for evidence fusion between the two base stations is as follows: , , Multi-base station evidence fusion is performed based on evidence fusion from two base stations. The specific algorithm for multi-base station evidence fusion is as follows: , , in, This indicates that the i-th base station and the j-th base station at time k represent the set of propositions. Allocation of evidence quality This represents the conflict coefficient between two pieces of evidence. This represents the set of proposition values ​​taken by the i-th base station at time k. This represents the set of proposition values ​​taken by the j-th base station at time k. This represents the evidence allocation for proposition set B at time k at the i-th base station. This represents the evidence allocation of the j-th base station to the proposition set C at time k. This represents the set of propositions obtained by fusing evidence from M base stations at time k. Allocation of evidence This represents the overall collision coefficient of base station M. Let the variable represent the set of propositions corresponding to the Mth base station. This represents the set of target propositions to be merged. This indicates the number of base stations participating in the integration. Indicates the base station number. This indicates that the i-th base station is at time k in the set of propositions. Evidence allocation, agreement =0, and .

[0026] Based on the DS evidence theory, the trust function and likelihood function for the distance condition are defined, and the specific algorithms for the trust function and likelihood function are as follows: , , in, The trust function represents the line-of-sight condition. The likelihood function represents the line-of-sight condition. This indicates that after multi-base station fusion, at time k, the proposition... Allocation of evidence This indicates that after the fusion of multiple base stations, the entire set is viewed at time K. Allocation of evidence; The adaptive weights are calculated using the following algorithm: , in, Indicates adaptive weights; An adaptive factor for the extended Kalman measurement noise is constructed based on the adaptive weights. The specific algorithm for the adaptive factor is as follows: , in, Indicates the adaptive factor. Indicates the adjustment parameter. >0; The initial noise of the extended Kalman filter is corrected to complete the measurement update of the extended Kalman filter. The specific algorithm for the measurement update is as follows: , , , , in, Indicates Kalman gain, This represents the posterior estimate of the state at time k. Let the posterior covariance matrix of the state at time k be denoted as . This represents the updated observation matrix. Indicates noise correction. Indicates the initial noise. Represents the UWB measurement vector. I represents the nonlinear measurement function, and I represents the identity matrix. For a comparison of this invention with existing technologies, please refer to the detailed logical flow. Figure 3 Please refer to the final comparison results. Figure 4.

[0027] In summary, based on the aforementioned IMU and UWB combined positioning method using extended Kalman filtering, this invention collects IMU and UWB data and establishes a combined positioning model. Preliminary ranging and consistency checks are performed based on this model. UWB measurements are autonomously detected and gross errors are identified. Innovative features are used to suppress geometrically inconsistent measurements. When a detection exceeds a threshold, the UWB data at that moment is not used, and only the IMU prediction result is retained. When a detection passes, the standardized squared innovation of each channel is used as a statistical index and mapped to the basic probability allocation function in the DS evidence theory. Then, multi-channel measurement evidence is fused using DS evidence combination rules, and the fusion result is transformed into adaptive adjustment of the measurement noise covariance. This allows UWB measurements to be filtered and updated using the extended Kalman filtering algorithm. This invention improves the positioning accuracy and stability of IMU and UWB combined positioning in complex indoor environments. Specifically, IMU and UWB data are collected and a combined positioning model is established. The combined positioning model is a discrete state-space model, which includes a system state vector, a control input vector, and a UWB measurement vector. Preliminary ranging is performed based on the combined positioning model, and a consistency check is conducted to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency check is based on the adjacent difference matrix and is used to filter ranging values ​​that are not line-of-sight or have gross errors. The extended Kalman filter algorithm is used for filtering and updating to obtain the combined positioning result. The filtering and updating is based on multi-base station evidence fusion and adaptive weights. This invention improves the positioning accuracy and stability of IMU and UWB combined positioning in complex indoor environments.

[0028] Please see Figure 2 The figure shows a schematic diagram of the IMU and UWB combined positioning system based on extended Kalman filtering proposed in the second embodiment of the present invention. The system includes: The integrated positioning model construction module 10 is used to collect IMU and UWB data and establish an integrated positioning model. The integrated positioning model is a discrete state-space model, which includes a system state vector, a control input vector, and a UWB measurement vector. The preliminary prediction and verification module 20 is used to perform preliminary ranging and consistency verification based on the combined positioning model to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency verification is based on the adjacent difference matrix and is used to screen out ranging values ​​that are not line-of-sight or have gross errors. The filtering update module 30 is used to perform filtering updates based on the extended Kalman filter algorithm to obtain combined positioning results. The filtering update is based on multi-base station evidence fusion and adaptive weights.

[0029] The present invention also proposes a computer storage medium storing one or more programs, which, when executed by a processor, implement the above-described IMU and UWB combined localization method based on extended Kalman filtering.

[0030] The present invention also proposes a computer device, including a memory and a processor, wherein the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory to implement the above-mentioned IMU and UWB combined positioning method based on extended Kalman filtering.

[0031] Those skilled in the art will understand that the logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can mean any means that can contain stored, communicated, propagated, or transmitted programs for use by, or in conjunction with, an instruction execution system, apparatus, or device.

[0032] More specific examples of computer-readable media (a non-exhaustive list) include: electrical connections (electronic devices) having one or more wires, portable computer disk drives (magnetic devices), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Furthermore, computer-readable media can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in computer memory.

[0033] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0034] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0035] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of this patent should be determined by the appended claims.

Claims

1. A localization method based on a combination of IMU and UWB using extended Kalman filtering, characterized in that, include: Collect IMU and UWB data and establish a combined positioning model. The combined positioning model is a discrete state-space model, which includes a system state vector, a control input vector, and a UWB measurement vector. Preliminary ranging is performed based on the combined positioning model, and a consistency check is conducted to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency check is based on the adjacent difference matrix and is used to screen out ranging values ​​that are not line-of-sight or have gross errors. The extended Kalman filter algorithm is used to update the filter and obtain the combined positioning result. The filter update is based on multi-base station evidence fusion and adaptive weights.

2. The IMU and UWB combined localization method based on extended Kalman filtering according to claim 1, characterized in that, The steps of acquiring IMU and UWB data and establishing a combined localization model specifically include: Collect IMU and UWB data; The system state vector, control input vector, and UWB measurement vector are constructed using the following specific algorithms: ， ， ， in, Represents the system state vector. Represents the control input vector. Represents the UWB measurement vector. , Represents the position components in the navigation coordinates. , Represents the velocity components in the navigation coordinates. , Represents the acceleration components in the navigation coordinates. T Indicates transpose. , This represents the acceleration input component of the IMU. This indicates the UWB ranging from the tag to the m-th base station; The system state vector and UWB measurement vector are discretized to construct a combined positioning model.

3. The IMU and UWB combined localization method based on extended Kalman filtering according to claim 2, characterized in that, The step of discretizing the system state vector and the UWB measurement vector specifically includes: The specific algorithms for discretizing the system state vector and UWB measurement vector are as follows: ， ， in, Represents the system state vector. Represents the UWB measurement vector. This represents the state transition function, which describes the state transition function used to describe the state transition in the control input vector. The motion relationship from the state at time k-1 to time k under the action of [the action]. Indicates process noise. This represents a measurement function, which is used to represent the system state vector. The mapping is the theoretical ranging between each base station and the tag. This indicates measurement noise.

4. The IMU and UWB combined localization method based on extended Kalman filtering according to claim 1, characterized in that, The step of performing preliminary ranging based on the combined positioning model and conducting a consistency check to obtain preliminary ranging values ​​specifically includes: The system state vector is predicted a priori based on IMU data. The priori prediction is obtained by using the state transition function to obtain the prior state prediction and the covariance of the prior state prediction. The specific algorithm for the prior prediction is as follows: ， in, Indicates prior state prediction, Represents the state transition function. This represents the posterior state estimate at time k-1. express The control input vector at each time step; Based on the prior state prediction, the theoretical measurement value between each base station and the tag is calculated. The specific algorithm for calculating the theoretical measurement value is as follows: ， in, Indicates the theoretical measurement value. Represents a measurement function; The difference between the actual measured value and the theoretical measured value is calculated to obtain the innovation vector and the innovation covariance. The specific algorithms for the innovation vector and the innovation covariance are as follows: ， ， in, Represents the innovation vector. Represents the UWB measurement vector. Indicates the new information covariance. Represents the observation matrix. T Indicates transpose. Let the prior state prediction error covariance matrix at time k be denoted as . Represents the UWB measurement noise covariance matrix; The specific algorithm for constructing the adjacent difference matrix is ​​as follows: ， in, represents the adjacent difference matrix, and m represents the number of base stations; A consistency check is performed based on the adjacent difference matrices. The specific algorithm for the consistency check is as follows: ， ， ， ， ， ， in, This represents the innovation vector after adjacent differences. This represents the new information covariance matrix after adjacent differences. This represents the consistency detection statistic. Indicates the probability of a false alarm. This indicates the null hypothesis that the current measurement has no gross errors. Describes a chi-square random variable with r degrees of freedom. Greater than the threshold The probability, Represents the probability density function. Indicates the test threshold. Represents the cumulative distribution function; when If the error is large, the UWB data will not be used, and only the IMU prediction results will be retained. when At that time, the filter is updated based on the extended Kalman filter algorithm.

5. The IMU and UWB combined localization method based on extended Kalman filtering according to claim 1, characterized in that, The step of updating the filter based on the extended Kalman filter algorithm to obtain the combined localization result specifically includes: A recognition framework is constructed to describe UWB ranging propagation conditions, which include line-of-sight and non-line-of-sight conditions. The specific algorithm of the identification framework is as follows: ， ， in, Represents the identification framework. This indicates the UWB ranging propagation conditions at line-of-sight. This indicates the UWB ranging propagation conditions under non-line-of-sight conditions. The power set of the identification frame is represented. Represents the empty set; The normalized innovation square is defined, and the specific algorithm for the normalized innovation square is as follows: ， 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; 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: ， ， in, This indicates the threshold for determining the line of sight. This represents 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. ; The BPA function is constructed using the following algorithm: ， ， ， 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. ; Multi-base station evidence fusion is performed, and the multi-base station evidence fusion is based on Demspter synthesis rules; Calculate adaptive weights for extended Kalman measurement updates.

6. The IMU and UWB combined localization method based on extended Kalman filtering according to claim 5, characterized in that, The steps for performing multi-base station evidence fusion specifically include: The specific algorithm for evidence fusion between the two base stations is as follows: ， ， Multi-base station evidence fusion is performed based on evidence fusion from two base stations. The specific algorithm for multi-base station evidence fusion is as follows: ， ， in, This indicates that the i-th base station and the j-th base station at time k represent the set of propositions. Allocation of evidence quality This represents the conflict coefficient between two pieces of evidence. This represents the set of proposition values ​​taken by the i-th base station at time k. This represents the set of proposition values ​​taken by the j-th base station at time k. This represents the evidence allocation for proposition set B at time k at the i-th base station. This represents the evidence allocation of the j-th base station to the proposition set C at time k. This represents the set of propositions obtained by fusing evidence from M base stations at time k. Allocation of evidence This represents the overall collision coefficient of base station M. Let the variable represent the set of propositions corresponding to the Mth base station. This represents the set of target propositions to be merged. This indicates the number of base stations participating in the integration. Indicates the base station number. This indicates that the i-th base station is at time k in the set of propositions. Evidence allocation, agreement =0, and .

7. The IMU and UWB combined localization method based on extended Kalman filtering according to claim 5, characterized in that, The step of calculating adaptive weights for extended Kalman measurement updates specifically includes: Based on the DS evidence theory, the trust function and likelihood function for the distance condition are defined, and the specific algorithms for the trust function and likelihood function are as follows: ， ， in, The trust function represents the line-of-sight condition. The likelihood function represents the line-of-sight condition. This indicates that after multi-base station fusion, at time k, the proposition... Allocation of evidence This indicates that after the fusion of multiple base stations, the entire set is viewed at time K. Allocation of evidence; The adaptive weights are calculated using the following algorithm: ， in, Indicates adaptive weights; An adaptive factor for the extended Kalman measurement noise is constructed based on the adaptive weights. The specific algorithm for the adaptive factor is as follows: ， in, Indicates the adaptive factor. Indicates the adjustment parameter. >0; The initial noise of the extended Kalman filter is corrected to complete the measurement update of the extended Kalman filter. The specific algorithm for the measurement update is as follows: ， ， ， ， in, Indicates Kalman gain, This represents the posterior estimate of the state at time k. Let the posterior covariance matrix of the state at time k be denoted as . This represents the updated observation matrix. Indicates noise correction. Indicates the initial noise. Represents the UWB measurement vector. Let I represent the nonlinear measurement function, and let I represent the identity matrix.

8. A combined IMU and UWB positioning system based on extended Kalman filtering, characterized in that, include: The integrated positioning model construction module is used to collect IMU and UWB data and establish an integrated positioning model. The integrated positioning model is a discrete state-space model, which includes 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 and consistency verification based on the combined positioning model to obtain preliminary ranging values. The preliminary ranging includes prior prediction based on IMU data and theoretical ranging based on UWB data. The consistency verification is based on the adjacent difference matrix and is used to screen out ranging values ​​that are not line-of-sight or have gross errors. The filtering update module is used to perform filtering updates based on the extended Kalman filter algorithm to obtain combined positioning results. The filtering update is based on multi-base station evidence fusion and adaptive weights.

9. A storage medium, characterized in that, The storage medium stores one or more programs that, when executed by a processor, implement the IMU and UWB combined localization method based on extended Kalman filtering as described in any one of claims 1-7.

10. A computer device, characterized in that, The computer device includes a memory and a processor, wherein: The memory is used to store computer programs; When the processor executes the computer program stored in the memory, it implements the IMU and UWB combined positioning method based on extended Kalman filtering as described in any one of claims 1-7.

Images