UWB / INS integrated navigation method and system based on quaternion modeling
Through the extremely hybrid correlation entropy combined navigation filtering algorithm based on quaternary modeling, the problem of increasing error in UWB/INS system in non-Gaussian noise environment is solved, high-precision and adaptive navigation performance are achieved, and the system is improved robustness and stability.
Patent Information
- Application Number
- CN202411064251.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-05
- Publication Date
- 2024-12-06
AI Technical Summary
The existing UWB/INS combined navigation system has significantly increased errors when facing non-Gaussian noise environments, and has weak adaptability, making it difficult to maintain optimal performance in complex time-varying environments.
Using a large mixed correlation entropy combined navigation filtering algorithm based on quaternary modeling, information fusion and error state variable estimation are carried out on the output data of INS and UWB systems. Through the state estimation algorithm in the quaternary domain, the navigation output results are corrected. Achieve high-precision combined navigation.
Improves robustness and estimation accuracy in non-Gaussian noise environments, with the ability to adaptively update dominant variance, and improves filtering accuracy and stability.
Smart Images

Figure CN119085703A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of navigation and positioning, and in particular to a UWB / INS combined navigation method and system based on quaternion modeling. Background Art
[0002] Accurate and reliable navigation information is the key supporting technology for smart homes, smart factories, and smart production. In indoor spaces, it is particularly difficult to obtain high-precision navigation information due to the lack of satellite navigation information. The positioning method based on UWB can obtain high-precision positioning information, but the positioning frequency is low and it is difficult to output reliable attitude and speed information; the inertial positioning method based on INS outputs comprehensive information, but there is a problem that navigation information is easily divergent as it accumulates over time. Fusion of INS and UWB information for combined navigation is a very perfect solution, and its core lies in accurate modeling and high-precision state estimation algorithms. Carrier motion is a special equation that satisfies constraints, especially when it comes to attitude calculations, and the use of conventional methods will introduce additional errors. In addition, the sensor is disturbed by noise caused by non-line-of-sight, vibration, etc., which deviates from the Gaussian distribution, resulting in a decrease in the accuracy of the traditional combined navigation algorithm. The present invention further improves the INS / UWB combined navigation performance from the two aspects of accurate modeling and high-precision state estimation algorithms.
[0003] The Chinese patent with publication number CN110375730B discloses an indoor positioning and navigation system based on the fusion of IMU and UWB. The method first uses IMU to obtain the three-axis acceleration and three-axis angular velocity of the unmanned system; then obtains the position information of the unmanned system through UWB; and finally uses the Kalman filter algorithm to fuse the IMU and UWB data. This method does not consider the model under non-Gaussian noise interference, and its effect is not good under complex noise conditions. The Chinese patent with publication number CN113432608A discloses a generalized high-order CKF algorithm based on maximum correlation entropy suitable for INS / CNS combined navigation system. The method first constructs the INS / CNS combined navigation system filter model, then updates the time of the filter algorithm according to the constructed filter model, and finally introduces the maximum correlation entropy criterion and the new judgment criterion to perform measurement update of the filter algorithm. In actual complex time-varying non-Gaussian noise applications, this method will significantly increase the error due to the lack of fixed kernel bandwidth and the lack of the ability to adaptively update the dominant variance. The Chinese patent with publication number CN116164773A discloses a Lie group strapdown inertial navigation error model based on the carrier system quaternion attitude error. The method first derives the carrier system quaternion-based attitude error model, and then proposes and derives the Lie group strapdown inertial navigation error model based on the carrier system quaternion attitude error. The method has weak adaptive ability and cannot effectively adjust its own parameters or structure to maintain optimal performance when facing changes in the external environment. Summary of the invention
[0004] In view of the shortcomings of the currently known UWB / INS integrated navigation, the present invention aims to provide a UWB / INS integrated navigation method and system based on quaternion modeling.
[0005] In order to achieve the above-mentioned purpose of the invention and solve the existing problems, a combined navigation model based on quaternion is constructed, and a combined navigation filtering algorithm based on maximum mixed correlation entropy is used to perform information fusion and error state variable estimation on the output data of INS and UWB systems, and the navigation output result is corrected based on the estimation result to realize high-precision combined navigation. The method includes:
[0006] Step S1, using the accelerometer and gyroscope of the inertial measurement unit IMU of the inertial navigation INS to obtain acceleration information and angular velocity information in real time;
[0007] Step S2, constructing a combined navigation model based on quaternion;
[0008] Step S3, using a combined navigation filtering algorithm based on maximum mixed correlation entropy to perform information fusion and error state variable estimation on the output data of the INS and UWB systems;
[0009] Step S4, using the obtained error estimation result to correct the INS system solution data, output the corrected navigation information such as position, speed, attitude, etc., to achieve high-precision integrated navigation.
[0010] In one embodiment, in step S1, the acceleration information and angular velocity information are acquired in real time using the accelerometer and gyroscope of the inertial measurement unit IMU on the inertial navigation INS;
[0011] In one embodiment, step S2 is to construct a combined navigation model based on quaternions. The implementation steps are as follows:
[0012] S21, construct the process equation based on IMU information:
[0013]
[0014] Where i is the discrete time, x i ∈H n is the n-dimensional quaternion state vector, The mean is 0 and the covariance matrix is Q i-1 process noise.
[0015] S22, construct a measurement equation based on the measurement information between the UWB base station and the UWB tag:
[0016] y i =h(x i )+v i
[0017] In the formula, y i ∈H m is the m-dimensional quaternion measurement vector. i The mean is 0 and the covariance matrix is R i The measurement noise.
[0018] In one embodiment, in step S3, a combined navigation filtering algorithm based on maximum mixed correlation entropy is used to perform information fusion and error state variable estimation on output data of INS and UWB systems;
[0019] S31, calculate the one-step prediction value of the system state according to the process equation and the corresponding covariance matrix P i|i-1 , assuming that the state estimate obtained at the i-1th time moment is Its estimated error covariance is P i-1|i-1 For this discrete system, the time update is the same as that of the Kalman filter, and the prior state estimate and its estimated error covariance P i|i-1 Updated to:
[0020]
[0021] in, P i|i-1 are the state estimation value and error matrix at time i-1, Φ i is a known linear state transfer matrix, which can be obtained by φ(x i ) for x i Find the partial derivative and we get .
[0022] S32, initialize variables: the algorithm is initialized, the initial quaternion state is defined as x0, and the state estimation is The initial quaternion state estimation covariance is P 0∣0 =P0, initialization
[0023] S33, using quaternion Cholesky decomposition to get B p,i|i-1 and B r,i ;
[0024]
[0025] S34, loop steps S35-S39 N times, there are x i =f(x i ), x i It can be solved using the following quaternion fixed point iteration algorithm in, represents the state estimate at fixed-point iteration t.
[0026] S35, calculate the measurement update of cycle t+1
[0027]
[0028] in, represents its prior state estimate, (·) H represents the Hermitian operator, H i is the measurement matrix, which can be obtained by h(x i ) for x i By taking partial derivatives, we can get the Kalman filter gain and estimation error covariance of the t-time cycle respectively:
[0029]
[0030] Where σ1 and σ2 are Gaussian functions and The kernel bandwidth is, e(i) = q1(i) - q2(i), for Draw N samples.
[0031]
[0032] S36, the measurement noise matrix after t cycles is:
[0033] in,(·) -1 Indicates the inverse,
[0034] S37, posterior covariance matrix after t+1 cycles It can be expressed as:
[0035]
[0036] S38, loop S39-S41 N times, and use variational Bayes to improve the posterior update.
[0037] S39, calculate and update the state variable x i :
[0038] In the formula, q l+1 (·) is the approximate posterior probability density of the l+1th cycle, and the mean of the posterior state variables of the l+1th cycle is and its corresponding covariance matrix Calculated according to the following formula:
[0039]
[0040] In the formula, the measurement function Jacobian matrix Hi and the modified Kalman filter gain after l+1 cycles The calculation is as follows:
[0041]
[0042] S40, loop l+1th time to update the measurement matrix vector In the formula, the degree of freedom parameter and the inverse scaling matrix Calculated according to the following formula:
[0043]
[0044] In the formula, the auxiliary function Calculate according to the following formula
[0045]
[0046] S41, calculate the corrected measurement noise matrix
[0047] In the formula, expectation Calculated as:
[0048]
[0049] In one of the embodiments, in step S4, the obtained error estimation result is used to correct the INS system solution data, and the corrected navigation information such as position, speed, attitude, etc. is output.
[0050] A UWB / INS integrated navigation system based on quaternion modeling, comprising: an inertial navigation unit, a pseudorange detection unit, a wireless data transmission unit and a data processing unit;
[0051] An inertial navigation unit, used to measure navigation information; the navigation information includes position, speed and attitude information;
[0052] The pseudorange detection unit is used to obtain pseudorange information; the pseudorange detection unit includes a UWB positioning tag and a UWB reference node; the UWB reference node is pre-placed at a set position, and the inertial navigation device and the UWB positioning tag are respectively fixed on the carrier required for navigation and positioning; the pseudorange information is obtained by measuring the distance between the UWB positioning tag and the UWB reference node;
[0053] A wireless data transmission unit, connected to the inertial navigation device and the pseudorange detection unit, for transmitting the data collected by the inertial navigation device and the pseudorange detection unit to the data processing system through wireless transmission, and transmitting the control command sent by the data processing system to the inertial navigation device;
[0054] The data processing unit selects one of the filters of the quaternion modeling according to the communication quality of the current wireless communication channel to perform data fusion estimation on the collected data, and sends a control command to the inertial navigation device.
[0055] The technical effect of the invention is as follows: compared with the existing quaternion method, the quaternion method of the present invention directly constructs a state estimation algorithm in the quaternion domain, which can avoid frequent conversion between quaternions and real numbers, is more logical and reasonable, reduces the approximate error caused by the conversion, can show high robustness and estimation accuracy in a non-Gaussian noise environment, and has the ability to adaptively update the dominant variance. At the same time, compared with the existing maximum entropy method, the maximum entropy method of this method introduces variational Bayes in its posterior estimation, which can more accurately reflect the dynamic changes of system noise, thereby improving filtering accuracy and stability. BRIEF DESCRIPTION OF THE DRAWINGS
[0056] Figure 1 is a flow chart of the present invention;
[0057] Figure 2 It is a root mean square error curve diagram of the present invention and the quaternion Kalman filter, the maximum correlation entropy quaternion Kalman filter and the mixed entropy Kalman filter x1 under different kernel bandwidths.
[0058] Figure 3 It is a schematic diagram of the actual application scenario of the present invention. DETAILED DESCRIPTION
[0059] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0060] like Figure 1 As shown, a UWB / INS integrated navigation method based on quaternion modeling includes:
[0061] In order to achieve the above-mentioned purpose of the invention and solve the existing problems, a combined navigation model based on quaternion is constructed, and a combined navigation filtering algorithm based on maximum mixed correlation entropy is used to perform information fusion and error state variable estimation on the output data of INS and UWB systems, and the navigation output result is corrected based on the estimation result to realize high-precision combined navigation. The method includes:
[0062] Step S1, using the accelerometer and gyroscope of the inertial measurement unit IMU of the inertial navigation INS to obtain acceleration information and angular velocity information in real time;
[0063] Step S2, constructing a combined navigation model based on quaternion;
[0064] Step S3, using a combined navigation filtering algorithm based on maximum mixed correlation entropy to perform information fusion and error state variable estimation on the output data of the INS and UWB systems;
[0065] Step S4, using the obtained error estimation result to correct the INS system solution data, output the corrected navigation information such as position, speed, attitude, etc., to achieve high-precision integrated navigation.
[0066] Step S1, using the accelerometer and gyroscope of the inertial measurement unit IMU on the inertial navigation INS to obtain acceleration information and angular velocity information in real time;
[0067] Step S2: construct a combined navigation model based on quaternion. The implementation steps are as follows:
[0068] Step S21, constructing a process equation based on IMU information:
[0069]
[0070] Where i is the discrete time, x i ∈H n is the n-dimensional quaternion state vector, The mean is 0 and the covariance matrix is Q i-1 process noise.
[0071] Step S22, constructing a measurement equation based on the measurement information between the UWB base station and the UWB tag:
[0072] y i =h(x i )+v i
[0073] In the formula, y i ∈H m is the m-dimensional quaternion measurement vector. i The mean is 0 and the covariance matrix is R i The measurement noise.
[0074] In step S3, a combined navigation filtering algorithm based on maximum mixed correlation entropy is used to perform information fusion and error state variable estimation on the output data of the INS and UWB systems;
[0075] Step S31, calculate the one-step prediction value of the system state according to the process equation and the corresponding covariance matrix P i|i-1 , assuming that the state estimate obtained at the i-1th time moment is Its estimated error covariance is P i-1|i-1For this discrete system, the time update is the same as that of the Kalman filter, and the prior state estimate and its estimated error covariance P i|i-1 Updated to:
[0076]
[0077] in, P i|i-1 are the state estimation value and error matrix at time i-1, Φ i is a known linear state transfer matrix, which can be obtained by φ(x i ) for x i Find the partial derivative and we get .
[0078] Step S32, initialize variables: the algorithm is initialized, the initial quaternion state is defined as x0, and the state estimation is The initial quaternion state estimation covariance is P 0∣0 =P0, initialization
[0079] Step S33, using quaternion Cholesky decomposition to obtain B p,i|i-1 and B r,i ;
[0080]
[0081] Step S34, loop steps S35-S39 N times, there are x i =f(x i ), x i It can be solved using the following quaternion fixed point iteration algorithm in, represents the state estimate at fixed-point iteration t.
[0082] Step S35, calculate the measurement update for cycle t+1
[0083]
[0084] in, represents its prior state estimate, (·) H represents the Hermitian operator, H i is the measurement matrix, which can be obtained by h(x i ) for x i By taking partial derivatives, we can get the Kalman filter gain and estimation error covariance of the t-time cycle respectively:
[0085]
[0086] Where σ1 and σ2 are Gaussian functions and The kernel bandwidth is, e(i) = q1(i) - q2(i), for Draw N samples.
[0087]
[0088] Step S36, loop t times to measure the noise matrix:
[0089] in,(·) -1 Indicates the inverse,
[0090] Step S37, loop t+1 times the posterior covariance matrix It can be expressed as:
[0091]
[0092] Step S38, loop S39-S41 N times, and use variational Bayes to improve the posterior update.
[0093] Step S39, calculate and update the state variable x i :
[0094] In the formula, q l+1 (·) is the approximate posterior probability density of the l+1th cycle, and the mean of the posterior state variables of the l+1th cycle is and its corresponding covariance matrix Calculated according to the following formula:
[0095]
[0096] In the formula, the measurement function Jacobian matrix H i and the modified Kalman filter gain after l+1 cycles The calculation is as follows:
[0097]
[0098] Step S40, looping for the l+1th time to update the measurement matrix vector In the formula, the degree of freedom parameter and the inverse scaling matrix Calculated according to the following formula:
[0099]
[0100] In the formula, the auxiliary function Calculate according to the following formula
[0101]
[0102] Step S41, calculate the corrected measurement noise matrix
[0103] In the formula, expectation Calculated as:
[0104]
[0105] Step S4, using the obtained error estimation result to correct the INS system solution data, and output the corrected navigation information such as position, speed, attitude, etc.
[0106] The effect of the present invention is further verified as follows:
[0107] The actual application scenario of this embodiment is as follows Figure 2 As shown, the UWB base station measures the distance information between the UWB base station and the tag on the device, and the IMU measurement information is used as the prediction update. Then the method proposed by the present invention is used to process the data to achieve the application under the condition of noise interference.
[0108] Choose the state transition matrix F k And the linearized measurement matrix H k for:
[0109]
[0110]
[0111] H i =
[11]
[0112] Among them, x i =(x1(i-1)x2(i-1)) T ∈H 2 is the system status, w i-1 =(w1(i-1)w2(i-1)) T ∈H 2 is the process noise, In the simulation, the initial prior estimate is set to
[0113] The initial state is set to Correspondingly, the initial error covariance is set to P 0|0 =diag[0.1 2 0.1 2 ].
[0114] In the experiment, the quaternion process noise is still Gaussian noise, but the observation noise is heavy-tailed non-Gaussian noise with mixed Gaussian distribution.
[0115]
[0116] The performance evaluation index is defined by the root mean square error (RMSE) and the average root mean square error (ARMSE), and the formula is:
[0117]
[0118] In the formula, x m (i) and are the estimated and true values, respectively, and M represents the number of Monte Carlo runs. In the simulation, RMSE is calculated by averaging over 100 independent Monte Carlo runs.
[0119] Figure 3 The root mean square error of velocity in this application is compared with that based on Quaternion Kalman Filter (Quaternion Kalman Filter, QKF), Maximum Correntropy Quaternion Kalman Filter (Maximum Correntropy Quaternion Kalman Filter, MCQKF), and Maximum Mixture Correntropy Quaternion Kalman Filter (Maximum Mixture Correntropy Quaternion Kalman Filter, MMCQKF). It is shown that the performance of the method proposed in the present invention is better than that of the existing methods.
[0120] Obviously, those skilled in the art may make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.
[0121] It should be understood that although Figure 1 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.
[0122] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0123] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.
[0124] A UWB / INS integrated navigation system based on quaternion modeling, comprising:
[0125] Inertial navigation unit, pseudorange detection unit, wireless data transmission unit and data processing unit;
[0126] An inertial navigation unit, used to measure navigation information; the navigation information includes position, speed and attitude information;
[0127] The pseudorange detection unit is used to obtain pseudorange information; the pseudorange detection unit includes a UWB positioning tag and a UWB reference node; the UWB reference node is pre-placed at a set position, and the inertial navigation device and the UWB positioning tag are respectively fixed on the carrier required for navigation and positioning; the pseudorange information is obtained by measuring the distance between the UWB positioning tag and the UWB reference node;
[0128] A wireless data transmission unit, connected to the inertial navigation device and the pseudorange detection unit, for transmitting the data collected by the inertial navigation device and the pseudorange detection unit to the data processing system through wireless transmission, and transmitting the control command sent by the data processing system to the inertial navigation device;
[0129] The data processing unit selects one of the filters of the quaternion modeling according to the communication quality of the current wireless communication channel for data fusion estimation of the collected data, and sends a control command to the inertial navigation device.
[0130] Each module in the above-mentioned quaternion-based UWB / INS integrated navigation system can be implemented in whole or in part by software, hardware, and a combination thereof. Each of the above-mentioned modules can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute operations corresponding to each of the above modules.
[0131] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0132] The above-mentioned embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.
Claims
1. A UWB / INS integrated navigation method based on quaternion modeling, characterized in that: The specific steps include: Step S1, using the accelerometer and gyroscope of the inertial measurement unit IMU of the inertial navigation INS to obtain acceleration information and angular velocity information in real time; Step S2, constructing a combined navigation model based on quaternion; Step S3, using a combined navigation filtering algorithm based on maximum mixed correlation entropy to perform information fusion and error state variable estimation on the output data of the INS and UWB systems; Step S4, using the obtained error estimation result to correct the INS system solution data, output the corrected navigation information such as position, speed, attitude, etc., to achieve high-precision integrated navigation.
2. The UWB / INS integrated navigation method based on quaternion modeling according to claim 1, characterized in that: In step S2, a combined navigation model based on quaternion is constructed, and the specific steps are as follows: S21, construct the process equation based on IMU information: Where i is the discrete time, x i ∈H n is the n-dimensional quaternion state vector, The mean is 0 and the covariance matrix is Q i-1 process noise; S22, construct a measurement equation based on the measurement information between the UWB base station and the UWB tag: y i =h(x i )+v i In the formula, y i ∈H m is the m-dimensional quaternion measurement vector, v i The mean is 0 and the covariance matrix is R i The measurement noise.
3. The UWB / INS integrated navigation method based on quaternion modeling according to claim 1, characterized in that: The specific steps of step S3 are: S31, calculate the one-step prediction value of the system state according to the process equation and the corresponding covariance matrix P i|i-1 , assuming that the state estimate obtained at the i-1th time moment is Its estimated error covariance is P i-1|i-1 ; For this discrete system, the time update is the same as the Kalman filter, and the prior state estimate and its estimated error covariance P i|i-1 Updated to: in, P i|i-1 are the state estimation value and error matrix at time i-1, Φ i is a known linear state transfer matrix, which can be obtained by φ(x i ) for x i Taking partial derivatives we get; S32, initialize variables: the algorithm is initialized, the initial quaternion state is defined as x0, and the state estimation is The initial quaternion state estimation covariance is P 0∣0 =P0, initialization S33, using quaternion Cholesky decomposition to get B p,i|i-1 and B r,i ; S34, loop steps S35-S39 N times, there are x i =f(x i ), x i It can be solved using the following quaternion fixed point iteration algorithm in, represents the state estimate at fixed-point iteration t; S35, calculate the measurement update of cycle t+1 in, represents its prior state estimate, (·) H represents the Hermitian operator, H i is the measurement matrix, which can be obtained by h(x i ) for x i By taking partial derivatives, we can get the Kalman filter gain and estimation error covariance of the t-time cycle respectively: Where σ1 and σ2 are Gaussian functions and The kernel bandwidth is, e(i) = q1(i) - q2(i), for Extract N samples; S36, the measurement noise matrix after t cycles is: in,(·) -1 Indicates the inverse, S37, posterior covariance matrix after t+1 cycles It can be expressed as: S38, loop S39-S41 N times, and use variational Bayes to improve the posterior update; S39, calculate and update the state variable x i : In the formula, q l+1 (·) is the approximate posterior probability density of the l+1th cycle, and the mean of the posterior state variables of the l+1th cycle is and its corresponding covariance matrix Calculated according to the following formula: In the formula, the measurement function Jacobian matrix H i and the modified Kalman filter gain after l+1 cycles The calculation is as follows: S40, loop l+1th time to update the measurement matrix vector In the formula, the degree of freedom parameter and the inverse scaling matrix Calculated according to the following formula: In the formula, the auxiliary function Calculate according to the following formula S41, calculate the corrected measurement noise matrix In the formula, expectation Calculated as:
4. A UWB / INS integrated navigation system based on quaternion modeling, wherein the UWB / INS integrated navigation system based on quaternion modeling is constructed by using the UWB / INS integrated navigation method based on quaternion modeling as claimed in any one of claims 1 to 3, characterized in that: The UWB / INS integrated navigation system based on quaternion modeling includes: an inertial navigation unit, a pseudorange detection unit, a wireless data transmission unit and a data processing unit; The inertial navigation unit is used to measure navigation information, and the navigation information includes position, speed and attitude information; The pseudorange detection unit is used to obtain pseudorange information; the pseudorange detection unit includes a UWB positioning tag and a UWB reference node, the UWB reference node is pre-placed at a set position, and the inertial navigation device and the UWB positioning tag are respectively fixed on the carrier required for navigation and positioning; the pseudorange information is obtained by measuring the distance between the UWB positioning tag and the UWB reference node; The wireless data transmission unit is connected to the inertial navigation device and the pseudo-range detection unit, and is used to transmit the data collected by the inertial navigation device and the pseudo-range detection unit to the data processing system through wireless transmission, and transmit the control command sent by the data processing system to the inertial navigation device; The data processing unit selects one of the filters modeled by the quaternion according to the communication quality of the current wireless communication channel to perform data fusion estimation on the collected data, and sends a control command to the inertial navigation device.
Citation Information
Patent Citations
Inertial / GPS (global positioning system) combined navigation method based on dual quaterions
CN106767797A
Compactly combined navigation method of IMU and GPS on basis of MEMS
CN108709552A
INS / GPS tight coupling navigation method and system based on distributed robust filtering
CN115451955A
Multi-sensor fusion pose estimation algorithm
CN115540860A
Inertial navigation attitude calculation method of improved mixed entropy volume Kalman filter
CN117570975A
