Imu / uwb integrated navigation method and system based on nonlinear error definition

Abstract

The application belongs to the technical field of integrated navigation, and discloses an IMU / UWB integrated navigation method and system based on nonlinear error definition, wherein based on the dynamic equation of IMU, virtual input and virtual input deviation are introduced to construct an extended IMU dynamic model; a nonlinear error of the extended IMU dynamic model is proposed, an error dynamic model is constructed based on the error definition form; based on the UWB observation equation, an observation model for the nonlinear error variable is constructed; virtual observation variables for the virtual deviation are introduced to construct a virtual observation equation to correct the error state. The method proposed in the application can reduce the dependence of the error dynamic model on state estimation, still obtain stable and reliable integrated navigation results when the initial state error is large, avoid the time-consuming initial alignment process in the actual IMU / UWB integrated navigation, and further reduce the application complexity and hardware cost of the navigation system, so that the application has a wide practical application prospect.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of integrated navigation, and particularly relates to an IMU / UWB integrated navigation method and system based on nonlinear error definition. BACKGROUND

[0002] Precise position, attitude and velocity feedback is the premise and basis for mobile robots to autonomously complete the intended task. Inertial measurement units (IMUs) are most widely used in robot navigation and positioning due to their high sampling frequency, stable output and good autonomy. However, current micro-electromechanical IMUs have the problems of fast error accumulation and inability to be used for long-time navigation and positioning. UWB can calculate accurate positions through precise external ranging information, but cannot obtain robot velocity and attitude information. IMU / UWB integrated navigation can overcome the shortcomings of IMU and UWB, and obtain stable position, velocity and attitude estimation. However, current IMU-based integrated navigation methods require navigation state initialization, i.e., position, velocity and attitude initialization. Since current inertial integrated navigation error dynamics models are related to state estimation values, the accuracy of initialization will affect the accuracy of error dynamics models, and further affect the performance of integrated navigation, especially the attitude, whose initialization accuracy has a great influence on the performance of integrated navigation. However, attitude initialization is relatively difficult and cannot be directly realized by external UWB sensors. It is usually necessary to let the robot stand still for a period of time to obtain the initial roll angle and pitch angle, and to obtain the initial heading angle through sensitive geomagnetic field or specific maneuvering motion of the robot, that is, attitude initialization itself is a relatively tedious and time-consuming process, and may increase the complexity and hardware cost of integrated navigation. Therefore, there is an urgent need to design a new IMU / UWB integrated navigation method to decouple the navigation error dynamics equation and the navigation state estimation value.

[0003] Through the above analysis, the existing problems and defects of the prior art are as follows:

[0004] (1) Current micro-electromechanical IMUs have the problems of fast error accumulation and inability to be used for long-time navigation and positioning, and UWB cannot directly obtain the velocity and attitude information of the robot.

[0005] (2) Current IMU-based integrated navigation systems require accurate navigation state initialization, and system initial alignment and carrier position and velocity initialization are tedious and time-consuming, and attitude initialization is relatively difficult and has large error, and cannot be directly realized by external UWB sensors. SUMMARY

[0006] To overcome the problems in the related art, the embodiments of the present application provide an IMU / UWB integrated navigation method and system based on nonlinear error definition, and the technical solution is as follows:

[0007] The application is implemented based on an IMU / UWB integrated navigation method based on nonlinear error definition, and includes the following steps:

[0008] S1, based on the IMU dynamics equation, a virtual input and a virtual input deviation are introduced to construct an extended IMU dynamics model;

[0009] S2, a nonlinear error for the extended IMU dynamics model is proposed, and an error dynamics model is constructed based on the error definition form;

[0010] S3, based on the UWB observation equation, an observation model for the nonlinear error variable is constructed;

[0011] S4, a virtual observation variable for the virtual deviation is introduced to construct a virtual observation equation;

[0012] S5, the standard Kalman filter recursion is used to obtain the posteriori estimation of the error variable;

[0013] S6, the navigation state and the deviation state are corrected by the posteriori estimation value of the error variable to obtain the IMU / UWB integrated navigation solution.

[0014] In step S1, the extended IMU dynamics model is constructed, including:

[0015] The robot carries an IMU and a UWB tag, the IMU detects the three-axis acceleration and angular velocity of the robot itself, and N UWB base stations are fixed and placed in the positioning area to detect the distance between the UWB tag carried on the robot;

[0016] The geodetic coordinate system is defined , the origin is selected as an arbitrary point in the positioning area, and the three directions of the north-east-sky are axis, the carrier coordinate system is defined, the origin is the center of the robot IMU, the direction of the axis is consistent with the IMU coordinate axis; the IMU mechanical arrangement equation is as follows:

[0017] ;

[0018] ;

[0019] ;

[0020] ;

[0021] ;

[0022] In the formula, is the rotation matrix from the carrier coordinate system to the geodetic coordinate system, is the carrier angular velocity observation obtained by the gyroscope, is the gyroscope bias, is the gyroscope observation white noise, is the gravitational acceleration, is the carrier acceleration observation obtained by the accelerometer, is the accelerometer bias, is the accelerometer observation white noise, is the carrier velocity, is the gyroscope bias noise, is the accelerometer bias noise, is the carrier position, variable denotes the derivative with respect to time; denotes the skew-symmetric matrix of a three-dimensional vector is defined as follows:

[0023] ;

[0024] The virtual input and the virtual input bias , the mechanical arrangement equation of the IMU is extended as follows:

[0025] ;

[0026] ;

[0027] ;

[0028] ;

[0029] ;

[0030] ;

[0031] wherein is the virtual input, is the virtual input bias, is the virtual input bias noise;

[0032] During the filtering process, the virtual input is always kept as to ensure that the extended IMU mechanical arrangement equation is the same as the original equation.

[0033] Further, during the filtering process, the filter adopts the deterministic dynamics equation, sets the noise value to 0, and calculates the estimated value of each state variable;

[0034] ;

[0035] ;

[0036] ;

[0037] ;

[0038] ;

[0039] ;

[0040] where the variable denotes the estimate of the variable.

[0041] In step S2, the definition of the nonlinear error is:

[0042] ;

[0043] ;

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] where is the exponential map of SO(3) group, is the attitude error, is the velocity error, is the position error, is the gyroscope bias error, is the accelerometer bias error, is the virtual input bias error, the superscript denotes the transpose of a matrix or vector.

[0049] In step S2, the error dynamics model is constructed based on the error definition form, including:

[0050] According to the first-order approximation of the exponential map:

[0051] ;

[0052] where is the dimensional identity matrix;

[0053] The error variable is expressed in the form of first-order approximation:

[0054] ;

[0055] ;

[0056] ;

[0057] ;

[0058] ;

[0059] ;

[0060] Combining the strapdown mechanical equation and the filter equation, the attitude error differential equation is obtained as follows:

[0061] ;

[0062] The velocity error differential equation is:

[0063] ;

[0064] The position error differential equation is:

[0065] ;

[0066] The error differential equation of the gyroscope bias is:

[0067] ;

[0068] The error differential equation of the accelerometer bias is:

[0069] ;

[0070] The error differential equation of the virtual input bias is:

[0071] ;

[0072] The error vector and the noise vector are defined as:

[0073] ;

[0074] ;

[0075] In the formula, is the error vector, is the noise vector;

[0076] The error vector differential equation is obtained as follows:

[0077] ;

[0078] In the formula, is the system matrix in continuous time form, is the noise matrix;

[0079] where the system matrix in continuous-time form is:

[0080] ;

[0081] the noise matrix is:

[0082] .

[0083] In step S3, an observation model for the nonlinear error variable is constructed, including:

[0084] The observation of UWB is the distance between the UWB tag and multiple base stations, where the base stations are fixed in the geodetic coordinate system, The coordinates of the base stations in the geodetic coordinate system are respectively: The tag is fixed on the robot, and the relative rod arm between the IMU and the tag is The rod arm value is obtained by pre-calibration;

[0085] The distance observation equation between the UWB tag and the i-th base station is:

[0086] ;

[0087] In the formula, is the distance observation value, is the distance true value, is the observation noise, is the two-norm of the vector;

[0088] The observation equation of the error variable is constructed as:

[0089] ;

[0090] Through Taylor expansion, we get:

[0091] ;

[0092] Since the virtual input bias is always 0, a virtual observation is introduced:

[0093] ;

[0094] According to the virtual observation, the error observation variable is constructed as:

[0095] ;

[0096] ​The distance error variables observed by all N base stations and the virtual observation error variables are integrated to obtain:

[0097]

[0098] wherein, is an observation vector, is an observation noise vector, is an observation matrix;

[0099]

[0100] The specific expression of the observation matrix is as follows:

[0101]

[0102] In step S5, the posteriori estimation of the error variable is obtained by using the standard Kalman filter recursion, the error dynamics equation in continuous time form is discretized, and a first-order approximation is used to obtain:

[0103]

[0104] wherein, , the subscript represents the variable value at time , and is a discrete time interval;

[0105] When the IMU obtains angular velocity and acceleration observations, the continuous time system matrix and the discrete time system matrix are constructed according to the observation values, and the prediction is performed according to the following equation:

[0106]

[0107]

[0108] wherein, are the priori estimation at time and the posteriori estimation at time , respectively, are the priori variance at time and the posteriori variance at time , respectively, is a process noise variance matrix in discrete time form, is an expected value of a random quantity.

[0109] Further, the posteriori estimation of the error variable is calculated by the Kalman filter update equation, as follows:

[0110] ​​​​​​When the UWB ranging observation is obtained, the posterior state estimate and the posterior variance are obtained by correction;

[0111] ;

[0112] ;

[0113] ;

[0114] wherein, denotes the Kalman gain, denotes the observation noise variance matrix.

[0115] In step S6, the navigation state and the bias state are corrected, including:

[0116] The IMU obtains the angular velocity and acceleration observations at time , the position, the attitude, the velocity and the bias term at the current time are determined by discretizing the IMU dynamics equation;

[0117] ;

[0118] ;

[0119] ;

[0120] ;

[0121] ;

[0122] ;

[0123] After obtaining the posterior estimate of the error variable at time , the position, the velocity, the attitude and the bias term of the inertial navigation solution are corrected;

[0124] ;

[0125] ;

[0126] ;

[0127] ;

[0128] ;

[0129] .

[0130] Another object of the present application is to provide an IMU / UWB integrated navigation system based on the nonlinear error definition, which is used for regulating the IMU / UWB integrated navigation method based on the nonlinear error definition, and the system comprises:

[0131] An IMU dynamics model construction module, which is used for constructing an extended IMU dynamics model based on the dynamics equation of the IMU and introducing virtual input and virtual input deviation;

[0132] An error dynamics model construction module, which is used for proposing a nonlinear error for the extended IMU dynamics model and constructing an error dynamics model based on an error definition form;

[0133] An observation model construction module, which is used for constructing an observation model for the nonlinear error variable according to a UWB observation equation, and introducing a virtual observation variable for the virtual deviation to construct a virtual observation equation for correcting the error state;

[0134] An error state estimation module, which is used for recursively obtaining a posteriori estimation of the error variable by using a standard Kalman filter;

[0135] A navigation state correction module, which is used for correcting the navigation state and the deviation state by using the posteriori estimation of the error variable to obtain an IMU / UWB integrated navigation solution.

[0136] In combination with all the technical solutions described above, the present application has the following beneficial effects:

[0137] Firstly, the application introduces virtual input and virtual input deviation based on the dynamic equation of IMU, and constructs the extended dynamic equation of IMU. The application innovatively defines a nonlinear error for the extended dynamic equation of IMU, and derives the error dynamic equation based on the error definition form, so that the obtained position, velocity and attitude dynamic equations are independent of the state estimation at the current time, and the obtained error dynamic equations of accelerometer, gyroscope and virtual input deviation are only related to the deviation estimation and independent of the navigation state estimation, so that the obtained error dynamic model is not affected by the navigation state (i.e. position, velocity and attitude) and the initial navigation state error. The application constructs the observation equation for the nonlinear error variable based on the UWB observation equation; considering that the virtual input deviation should always be 0, the virtual observation variable for the virtual deviation is introduced to construct the virtual observation equation. The application obtains the posteriori estimation of the error variable by using the standard Kalman filter, and then corrects the navigation state based on the proposed nonlinear error definition to obtain the IMU / UWB integrated navigation solution. The proposed IMU / UWB integrated navigation method has better stability and robustness to the initial estimation error, because the position, velocity and attitude error dynamic equations are completely independent of the navigation state estimation at the current time. Under the condition of large initial error, the proposed method can still obtain good convergence and stability, and can avoid the time-consuming initial alignment process required in the conventional IMU integrated navigation application, and does not need external sensors for accurate initialization of position and velocity, thereby reducing the system complexity and hardware cost of IMU / UWB integrated navigation. Therefore, the IMU / UWB integrated navigation method of the application has good engineering application value and practical application potential.

[0138] Secondly, the application proposes a new nonlinear error definition method for inertial navigation, and derives the corresponding error dynamic equation and UWB observation equation. The obtained navigation state error dynamic equation is independent of any state estimation value, and the deviation error dynamic equation is only dependent on the deviation estimation value and independent of the navigation state estimation value, so that the dependence of the system on the initialization accuracy of the navigation state, especially the attitude initialization, can be reduced. The proposed method can realize stable integrated navigation under the condition of "attitude initialization-free".

[0139] The application constructs a new nonlinear navigation state error and bias state error, and deduces the inertial navigation error dynamics equation and UWB ranging observation equation for the constructed nonlinear error.

[0140] Thirdly, the navigation state error dynamics equation obtained by the technical scheme of the application is completely irrelevant to the state estimation value, so that a relatively stable and reliable inertial integrated navigation result can be obtained without accurate robot position, speed and attitude initialization. After the technical scheme is transformed, the time-consuming robot initial fine alignment can be avoided, and without accurate position and speed initialization, the time and hardware facilities required for initialization can be saved, and the result reliability and application flexibility of the inertial integrated navigation are improved.

[0141] At present, when considering inertial bias, the navigation error dynamics equation of the integrated navigation system based on inertial sensors is related to the state estimation value, so that the model accuracy is affected by the state initialization accuracy. The application proposes a new nonlinear error, and the navigation error dynamics equation deduced based on the new nonlinear error is completely irrelevant to the navigation state estimation value and the bias state estimation value, and the bias error dynamics equation is only related to the slowly varying bias state estimation value, so that the independence of the estimation value is good, and the domestic and foreign technical blank is filled.

[0142] Fourthly, constructing an inertial navigation error dynamics equation that does not depend on the navigation state estimation value can reduce the dependence of the inertial integrated navigation system on the initial estimation accuracy, so it is a difficult problem that people have always wanted to solve. Because the inertial navigation system does not satisfy the group affine property when considering the inertial bias estimation, the error dynamics deduced by the current additive error and the nonlinear error based on the Lie group error modeling cannot realize the independence of the state estimation. The application defines a new nonlinear error form, and obtains the advantages that the navigation state error dynamics is completely independent of the state estimation value, and the bias state error dynamics only depends on the slowly varying bias estimation, so that the technical problem that people have always wanted to solve but have always failed to solve is solved.

[0143] Because the inertial navigation mechanical equation itself does not satisfy the group-affinity in the case of considering the inertial bias estimation, researchers generally believe that it is difficult to obtain the navigation error dynamics equation completely independent of the state estimation value in the left-invariant error definition form. The application overcomes this technical prejudice and proposes a new nonlinear error, and the obtained navigation state error dynamics equation is completely independent of the navigation state and bias state estimation. BRIEF DESCRIPTION OF DRAWINGS

[0144] The drawings herein are incorporated into the specification and form a part of the specification, show embodiments consistent with the present disclosure, and together with the specification, serve to explain the principles of the present disclosure;

[0145] Figure 1 is a flow chart of the IMU / UWB integrated navigation method based on the nonlinear error definition provided by the embodiment of the application;

[0146] Figure 2 is a schematic diagram of the simulated robot motion trajectory and the UWB base station provided by the embodiment of the application;

[0147] Figure 3 is a graph of the relationship between the robot position, velocity and attitude with time provided by the embodiment of the application;

[0148] Figure 4 is a comparison graph of the root mean square errors of the position estimations of the four methods provided by the embodiment of the application;

[0149] Figure 5 is a comparison graph of the root mean square errors of the velocity estimations of the four methods provided by the embodiment of the application;

[0150] Figure 6 is a comparison graph of the root mean square errors of the attitude estimations of the four methods provided by the embodiment of the application;

[0151] Figure 7 is a comparison graph of the root mean square errors of the gyro bias estimations of the four methods provided by the embodiment of the application;

[0152] Figure 8 is a comparison graph of the root mean square errors of the accelerometer bias estimations of the four methods provided by the embodiment of the application;

[0153] Figure 9 is a graph of the yaw angle and z-axis gyro bias estimation error of the LIEKF, TFLIEKF and PLIEKF methods in each Monte Carlo simulation provided by the embodiment of the application;

[0154] Figure 10 is a graph of the typical trajectory and the corresponding UWB base station position in the MILUV data set provided by the embodiment of the application;

[0155] Figure 11 is a trajectory and corresponding UWB base station position and attitude relationship graph changing over time provided by an embodiment of the present application;

[0156] Figure 12 is a position comparison graph of a comparative method provided by an embodiment of the present application;

[0157] Figure 13 is an attitude ARMSE comparison graph of a comparative method provided by an embodiment of the present application;

[0158] Figure 14 is a position relationship graph changing over time provided by an embodiment of the present application;

[0159] Figure 15 is an attitude RMSE relationship graph changing over time provided by an embodiment of the present application;

[0160] Figure 16 is a yaw angle error relationship graph changing over time of several methods provided by an embodiment of the present application. DETAILED DESCRIPTION

[0161] In order to make the above objectives, features and advantages of the present application more apparent, specific embodiments of the present application are described in detail below with reference to the accompanying drawings. In the following description, a large number of specific details are set forth in order to facilitate a full understanding of the present application. However, the present application can be implemented in many different ways other than those described herein, and those skilled in the art can make similar improvements without departing from the scope of the present application, so the present application is not limited to the specific implementation disclosed below.

[0162] The application has the advantages that the method can reduce the dependence of the error dynamics model on the state estimation, stable and reliable combined navigation results can be obtained when the initial state error is large, the time-consuming initial alignment process in the actual IMU / UWB combined navigation is avoided, and the application complexity and hardware cost of the navigation system are reduced, so the method has a wide practical application prospect.

[0163] The method can reduce the dependence of the error dynamics model on the state estimation, stable and reliable combined navigation results can be obtained when the initial state error is large, the time-consuming initial alignment process in the actual IMU / UWB combined navigation is avoided, and the application complexity and hardware cost of the navigation system are reduced, so the method has a wide practical application prospect.

[0164] Embodiment 1, as shown in the figure, the IMU / UWB combined navigation method based on the nonlinear error definition provided by the embodiment of the application comprises the following steps: Figure 1

[0165] S1, based on the IMU dynamics equation, a virtual input and a virtual input bias are introduced, and an extended IMU dynamics model is constructed;

[0166] It is assumed that the robot is equipped with an IMU and a UWB tag, the IMU detects the three-axis acceleration and angular velocity of the carrier itself, and N UWB base stations are fixedly arranged in the positioning area to detect the distance between the tag.

[0167] A geodetic coordinate system is defined as , the origin of which can be selected as any point in the positioning area, and the three directions of north, east and sky are respectively axis, a carrier coordinate system is defined as , the origin of which is the IMU center of the robot, ​The direction of the axis is consistent with the IMU coordinate axis; the IMU mechanical arrangement equation is as follows:

[0168] ;

[0169] ;

[0170] ;

[0171] ;

[0172] ;

[0173] In the formula, is the rotation matrix from the carrier coordinate system to the geodetic coordinate system, is the carrier angular velocity observation obtained by the gyroscope, is the gyroscope bias, is the gyroscope observation white noise, is the gravity acceleration, is the carrier acceleration observation obtained by the accelerometer, is the accelerometer bias, is the accelerometer observation white noise, is the carrier velocity, is the gyroscope bias noise, is the accelerometer bias noise, is the carrier position, and the variable represents the derivative with respect to time; represents the skew-symmetric matrix of the three-dimensional vector , which is defined as follows:

[0174] ;

[0175] The virtual input and the virtual input bias are introduced, and the mechanical arrangement equation of the IMU is extended as follows:

[0176] ;

[0177] ;

[0178] ;

[0179] ;

[0180] ;

[0181] ;

[0182] In the formula, is a virtual input, is a virtual input bias, is a virtual input bias noise;

[0183] In the filtering process, the virtual input is always kept as to ensure that the extended IMU mechanical arrangement equation is the same as the original equation.

[0184] In the filtering process, since the true value of the noise cannot be obtained, the filter uses a deterministic dynamic equation, that is, the noise value is set to 0 to calculate the estimated value of each state quantity, as follows:

[0185] ;

[0186] ;

[0187] ;

[0188] ;

[0189] ;

[0190] ;

[0191] In the formula, the variable with represents the estimated value of the variable.

[0192] S2, a nonlinear error for the extended IMU dynamic model is proposed, and an error dynamic model is constructed based on the error definition form;

[0193] The nonlinear error defined by the application is:

[0194] ;

[0195] ;

[0196] ;

[0197] ;

[0198] ;

[0199] ;

[0200] In the formula, is the exponential mapping of SO (3) group, is the attitude error, is the velocity error, is the position error, for the gyro bias error, for the accelerometer bias error, for the virtual input bias error, superscript denotes the transpose of a matrix or vector.

[0201] According to the first-order approximation:

[0202] ;

[0203] where, is a unit matrix of dimension

[0204] ;

[0205] Express the error variables in the form of the first-order approximation:

[0206] ;

[0207] ;

[0208] ;

[0209] ;

[0210] ;

[0211] ;

[0212] Combine the INS mechanization equations and the filter equations to obtain the attitude error differential equation as:

[0213] ;

[0214] The velocity error differential equation is:

[0215] ;

[0216] The position error differential equation is:

[0217] ;

[0218] The error differential equation for the gyro bias is:

[0219] ;

[0220] The error differential equation for the accelerometer bias is:

[0221] ;

[0222] The error differential equation for the virtual input bias is:

[0223] ;

[0224] Let the error vector and the noise vector be defined as:

[0225] ;

[0226] ;

[0227] wherein, is the error vector, is the noise vector;

[0228] The error vector differential equation is obtained as:

[0229] ;

[0230] wherein, is the system matrix in continuous time form, is the noise matrix;

[0231] wherein the system matrix in continuous time form is:

[0232] ;

[0233] The noise matrix is:

[0234] .

[0235] S3, based on the UWB observation equation, constructs an observation model for the nonlinear error variable;

[0236] The observation of the UWB is the distance between the UWB tag and multiple base stations, wherein the base stations are fixed in the geodetic coordinate system, The coordinates of the base stations in the geodetic coordinate system are respectively: The tag is fixed on the robot, and the relative rod arm between the IMU and the tag is The rod arm value is obtained through prior calibration;

[0237] The distance observation equation between the UWB tag and the i-th base station is:

[0238] ; wherein,

[0239] is the distance observation value, is the distance true value, is the observation noise, is the two-norm of the vector;

[0240] ​The observation equation of the construction error variable is:

[0241] ;

[0242] Through Taylor expansion, we get:

[0243] ;

[0244] Since the virtual input deviation is always 0, introduce a virtual observation:

[0245] ;

[0246] According to the virtual observation, the error observation variable is constructed as:

[0247] ;

[0248] Integrate all N base station observations of distance error variables and virtual observation error variables to get:

[0249] ;

[0250] Where, is the observation vector, is the observation noise vector, is the observation matrix;

[0251] ;

[0252] The specific expression of the observation matrix is:

[0253] .

[0254] S4, introduce a virtual observation variable for the virtual deviation, and construct a virtual observation equation;

[0255] S5, use the standard Kalman filter recursion to obtain the posteriori estimation of the error variable;

[0256] Use the standard Kalman filter recursion to obtain the posteriori estimation of the error variable, discretize the error dynamics equation in continuous time form, and use the first-order approximation to get:

[0257] ;

[0258] In the formula, , subscript represents the variable value at time, is the discrete time interval;

[0259] When the IMU obtains the angular rate and acceleration observations, the continuous-time system matrix is constructed from the observations and the discrete-time system matrix is predicted according to the following equation;

[0260] ;

[0261] ;

[0262] where are the prior estimate of the error variable at time and the posterior estimate at time , respectively, are the prior variance at time and the posterior variance at time , respectively, is the process noise variance matrix in discrete-time form, is the expected value of the random quantity.

[0263] The posterior estimate of the error variable is calculated by the Kalman filter update equation as follows:

[0264] When the UWB ranging observation is obtained, the posterior state estimate and the posterior variance are obtained by correction;

[0265] ;

[0266] ;

[0267] ;

[0268] where denotes the Kalman gain, denotes the observation noise variance matrix.

[0269] S6, the navigation state and the bias state are corrected by the posterior estimate of the error variable, and the IMU / UWB integrated navigation solution is obtained.

[0270] The navigation state and the bias state are corrected, including:

[0271] When the IMU obtains the angular rate and acceleration observations at time , the IMU dynamics equation is discretized to determine the position, attitude, velocity and bias term at the current time;

[0272] ;

[0273] ;

[0274] ;

[0275] ;

[0276] ;

[0277] ;

[0278] after obtaining the posteriori estimation of the error variable at the moment by Kalman filtering, the position, velocity, attitude and bias term of the inertial navigation solution are corrected;

[0279] ;

[0280] ;

[0281] ;

[0282] ;

[0283] ;

[0284] .

[0285] Embodiment 2, the IMU / UWB integrated navigation system based on the nonlinear error definition provided by the embodiment of the application comprises:

[0286] an IMU dynamics model construction module, which is based on the dynamics equation of the IMU, introduces virtual input and virtual input bias, and constructs an extended IMU dynamics model;

[0287] an error dynamics model construction module, which is used to propose nonlinear errors for the extended IMU dynamics model, and construct an error dynamics model based on the error definition form;

[0288] an observation model construction module, which is used to construct an observation model for the nonlinear error variable according to a UWB observation equation, and introduce a virtual observation variable for the virtual bias to construct a virtual observation equation to correct the error state;

[0289] an error state estimation module, which is used to obtain the posteriori estimation of the error variable recursively by using standard Kalman filtering;

[0290] a navigation state correction module, which is used to correct the navigation state and the bias state by using the posteriori estimation of the error variable, and obtain an IMU / UWB integrated navigation solution.

[0291] To further prove the positive effect of the above-mentioned embodiment, the application based on the above technical solution is used to realize numerical verification through the following experiments. ​

[0292] The navigation performance of the combined navigation method (abbreviated as "PLIEKF") proposed in the present application is demonstrated by numerical simulation. As a comparison, the navigation performance of the existing navigation method based on the definition of additive error (abbreviated as "MEKF"), the navigation method based on the definition of left-invariant error of Lie group (abbreviated as "LIEKF"), and the navigation method based on the definition of left-invariant error of dual coordinate Lie group (abbreviated as "TFLIEKF") are also demonstrated in the present embodiment. The motion trajectory of the simulated robot and the positions of the UWB base stations are shown in FIG. 1. In the figure, the upper left corner diagram represents the three-dimensional trajectory diagram of the simulation, the upper right corner diagram represents the projection diagram of the simulated trajectory in the xz plane, the lower left corner diagram represents the projection diagram of the simulated trajectory in the xy plane, and the lower right corner diagram represents the projection diagram of the simulated trajectory in the yz plane. The corresponding relationships of the robot position, velocity and attitude with time are shown in FIG. 2. Figure 2 Figure 2 Figure 3

[0293] The inertial data is simulated based on the parameters of Bosch BMI088 high-performance IMU, and the UWB ranging data is simulated based on the parameters of DWM1000 UWB module. The IMU and UWB sensor parameters are shown in Table 1.

[0294] Table 1 Simulated sensor noise parameters

[0295]

[0296] For different navigation methods, the filter parameters are set as follows:

[0297] (1) The initial attitude adopts a "calibration-free" scheme, that is, random sampling is performed within its definition domain. The roll angle and the heading angle are randomly sampled between -180° and 180°, and the pitch angle is randomly sampled between -90° and 90°.

[0298] (2) The initial velocity is increased by a Gaussian distribution random noise based on the true value of the velocity, and the standard deviation of the noise is .

[0299] (3) The initial position is increased by a Gaussian distribution random noise based on the true position, and the standard deviation of the noise is .

[0300] (4) The initial value of the IMU bias is set to 0.

[0301] (5) The process noise and observation noise variances of the gyroscope, accelerometer and UWB are set according to Table 1. ​​​​​​​

[0302] (6) The deviation noise spectral densities of the accelerometer and gyroscope are respectively set to and ;

[0303] (7) The virtual input deviation noise spectral density of the method proposed in this invention is The initial value of the virtual input deviation is 0, and the variance matrix of the virtual deviation error is... The virtual observation variance matrix is .

[0304] This experiment compares the statistical performance of different methods by conducting 100 independent Monte Carlo experiments. The root mean square error (RMSE) and average root mean square error (ARMSE) of each variable to be estimated are used as evaluation indicators, defined as follows:

[0305] ;

[0306] in, For the first In the Monte Carlo simulation, the spacecraft... Real value of state at any given moment For the first In the Monte Carlo simulation, the spacecraft State estimate at time 10:00 This represents the total number of Monte Carlo simulations. This is the total simulation duration.

[0307] Figures 4-8 These represent the root mean square errors of the position, velocity, attitude, gyroscope bias, and accelerometer bias estimates for the four positioning methods, respectively. Figure 4 In the middle, from top to bottom, are the RMSE of the position estimates in the x, y, and z directions; in Figure 5 In the middle, from top to bottom, are the RMSE of velocity estimates in the x, y, and z directions; in Figure 6 In the middle, from top to bottom, are the RMSE estimates for roll angle, pitch angle, and bow angle; Figure 7 In the middle, from top to bottom, are the gyroscope bias estimates RMSE in the x, y, and z directions; in Figure 8 In the table below, from top to bottom, the RMSE of the accelerometer bias estimates in the x, y, and z directions are shown; the corresponding root mean square errors are shown in Table 2.

[0308] Table 2. Root Mean Square Error (ARMSE) of Position, Velocity, Attitude, Gyroscope Bias, and Accelerometer Bias Estimation for Four Methods

[0309]

[0310] according to Figures 4-8As can be seen from Table 2, although the initial navigation error set in the present case is large (especially the attitude error, the attitude is initialized by the method of "initial alignment free"), the present application can obtain stable navigation state estimation and inertial sensor bias estimation, while some other comparative methods, including MEKF, LIEKF and TFLIEKF, will appear different degrees of divergence. The position, velocity, attitude, gyroscope bias and accelerometer bias estimation errors obtained by the present method are significantly better than those of other comparative methods. Figure 9 Further, the heading angle and z-axis gyroscope bias estimation errors of LIEKF, TFLIEKF and PLIEKF in each Monte Carlo simulation are shown (similar results are also shown for other navigation states and bias states); in Figure 9 In the middle, the upper three graphs are the heading angle estimation error graphs of LIEKF, TFLIEKF and PLIEKF, and the lower three graphs are the z-axis gyroscope bias estimation error graphs of LIEKF, TFLIEKF and PLIEKF; it can be seen that in 100 simulations, the method proposed in the present application can obtain stable heading angle and z-axis gyroscope bias estimation, which ensures good statistical performance of the method in the present application, while some other comparative methods will appear divergence in a certain number of Monte Carlo simulations, resulting in large statistical errors, which shows that the method proposed in the present application has good estimation stability and can obtain good navigation effect without precise initial alignment of inertial sensors, while some existing comparative methods do not have this advantage.

[0311] The present application further verifies the proposed method based on the MILUV public data set. The MILUV data set uses a UAV (UAV) carrying an IMU, a UWB tag and a laser range finder. The IMU is used to measure the three-axis acceleration and angular velocity of the UAV, the UWB tag is used to measure the distance between the UAV and the base station, and the laser range finder is used to measure the height of the UAV. The motion capture system is used to detect the position and attitude of the UAV.

[0312] The MILUV data set contains multiple UAV motion trajectories, one typical trajectory and the corresponding UWB base station position are shown in Figure 10 The corresponding position and attitude change with time are shown in Figure 11 In the middle, the upper three graphs are the heading angle estimation error graphs of LIEKF, TFLIEKF and PLIEKF, and the lower three graphs are the z-axis gyroscope bias estimation error graphs of LIEKF, TFLIEKF and PLIEKF; it can be seen that in 100 simulations, the method proposed in the present application can obtain stable heading angle and z-axis gyroscope bias estimation, which ensures good statistical performance of the method in the present application, while some other comparative methods will appear divergence in a certain number of Monte Carlo simulations, resulting in large statistical errors, which shows that the method proposed in the present application has good estimation stability and can obtain good navigation effect without precise initial alignment of inertial sensors, while some existing comparative methods do not have this advantage. Figure 11 In the middle, the upper three graphs are the heading angle estimation error graphs of LIEKF, TFLIEKF and PLIEKF, and the lower three graphs are the z-axis gyroscope bias estimation error graphs of LIEKF, TFLIEKF and PLIEKF; it can be seen that in 100 simulations, the method proposed in the present application can obtain stable heading angle and z-axis gyroscope bias estimation, which ensures good statistical performance of the method in the present application, while some other comparative methods will appear divergence in a certain number of Monte Carlo simulations, resulting in large statistical errors, which shows that the method proposed in the present application has good estimation stability and can obtain good navigation effect without precise initial alignment of inertial sensors, while some existing comparative methods do not have this advantage. Figure 10 In the middle, the upper three graphs are the heading angle estimation error graphs of LIEKF, TFLIEKF and PLIEKF, and the lower three graphs are the z-axis gyroscope bias estimation error graphs of LIEKF, TFLIEKF and PLIEKF; it can be seen that in 100 simulations, the method proposed in the present application can obtain stable heading angle and z-axis gyroscope bias estimation, which ensures good statistical performance of the method in the present application, while some other comparative methods will appear divergence in a certain number of Monte Carlo simulations, resulting in large statistical errors, which shows that the method proposed in the present application has good estimation stability and can obtain good navigation effect without precise initial alignment of inertial sensors, while some existing comparative methods do not have this advantage. Figure 10The corresponding trajectory roll angle, pitch angle and heading angle change over time. The experimental ID and sequence number corresponding to each experimental trajectory in the MILUV dataset are shown in the following table.

[0313]

[0314] For each data trajectory, 100 different initial navigation states are randomly sampled, and the position and attitude RMSEs obtained by different trajectories in 100 different initial estimates are verified. The position and attitude ARMSEs of the proposed method and the comparative method are compared as shown in Figure 12 and Figure 13 In Figure 12 , from top to bottom are the ARMSE graphs of the xyz three-direction position estimates, and in Figure 13 , from top to bottom are the ARMSEs of the roll angle, pitch angle and heading angle. For a typical data trajectory (experimental ID: default_1_random3_0), the relationship between the position and attitude RMSEs and time is shown in Figure 14 and Figure 15 In Figure 14 , the upper, middle and lower three graphs are the RMSE graphs of the xyz three-direction position estimates of a single trajectory, and in Figure 15 , the upper, middle and lower three graphs are the RMSE graphs of the roll angle, pitch angle and heading angle estimates of a single trajectory; the relationship between the heading angle error of several methods and time is shown in Figure 16 In Figure 16 , from top to bottom are the heading angle error graphs of the LIKF, TFLIKF and TGLIKF methods. Similar results can be obtained as in the simulation data verification, that is, under the condition of "initialization-free" attitude error, the proposed method can still obtain stable position and attitude estimation results, and its RMSE is lower than that of the existing comparative method in almost all motion trajectories. According to Figure 16 It can be seen that for a typical motion trajectory (experimental ID: default_1_random3_0), the proposed method can obtain stable navigation state estimation results under 100 different initialization conditions, while other comparative methods will diverge to different degrees, affecting their statistical performance and filter stability.

[0315] The above is only a preferred specific embodiment of the present application, but the protection scope of the present application is not limited thereto, and any modification, equivalent replacement and improvement made by those skilled in the art within the technical range disclosed by the present application, as long as it is within the spirit and principles of the present application, should be covered within the protection scope of the present application.

Claims

1. An IMU / UWB integrated navigation method based on nonlinear error definition, characterized in that, The method comprises the following steps: S1, based on the IMU dynamics equation, a virtual input and a virtual input deviation are introduced to construct an extended IMU dynamics model; S2, a nonlinear error for the extended IMU dynamics model is proposed, and an error dynamics model is constructed based on the error definition form; S3, based on the UWB observation equation, an observation model for the nonlinear error variable is constructed; S4, a virtual observation variable for the virtual input deviation is introduced to construct a virtual observation equation; S5, the posterior estimation of the error variable is obtained by recursive standard Kalman filtering; S6, the navigation state and the deviation state are corrected by the posterior estimation value of the error variable to obtain an IMU / UWB integrated navigation solution.

2. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 1, characterized in that, In step S1, the extended IMU dynamics model is constructed, including: The robot carries an IMU and a UWB tag, the IMU detects the three-axis acceleration and angular velocity of the robot itself, and N UWB base stations are fixedly arranged in the positioning area to detect the distance between the UWB tag carried on the robot and the UWB base station; A geodetic coordinate system {G} is defined, the origin is selected as an arbitrary point in the positioning area, and the three directions of northeast are respectively x, y and z axes, a carrier coordinate system {B} is defined, the origin is the center of the robot IMU, and the directions of x, y and z axes are consistent with the IMU coordinate axes; the IMU mechanical arrangement equation is as follows: where R is the rotation matrix from the body coordinate system to the geodetic coordinate system, is the body angular velocity observation obtained from the gyroscope, b g is the gyroscope bias, w g is the gyroscope observation white noise, g is the gravitational acceleration, is the body acceleration observation obtained from the accelerometer, b a is the accelerometer bias, w a is the accelerometer observation white noise, v is the body velocity, w bg is the gyroscope bias noise, w ba is the accelerometer bias noise, p is the body position, the variable with denotes the derivative with respect to time; axdenotes the skew-symmetric matrix of the three-dimensional vector a, defined as follows: Introducing virtual inputs Bias b with virtual inputs v The mechanical pose equation of the IMU is extended as follows: wherein is a virtual input, b v is a virtual input bias, w bv is a virtual input bias noise; During the filtering process, the virtual input is always kept as to ensure that the extended IMU mechanization equations are identical to the original equations.

3. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 2, wherein, In the filtering process, the filter adopts a deterministic dynamics equation, the noise value is set to 0, and the estimation values of various state variables are calculated; In the formula, the variable with denotes the estimated value of the variable.

4. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 3, wherein, In step S2, the definition of the nonlinear error is: where Exp(a) is the exponential map of the SO(3) group, a is the attitude error, dv is the velocity error, dp is the position error, db g is the gyroscope bias error, db a is the accelerometer bias error, db v is the virtual input bias error, and the superscript T denotes the transpose of a matrix or vector.

5. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 4, wherein, In step S2, the error dynamics model is constructed based on the error definition form, including: According to the first-order approximation of the exponential mapping: Exp(a)≈I3+(a×) wherein I n is the n-dimensional identity matrix; The error variable is expressed in the form of first-order approximation: Combined with the inertial navigation mechanical arrangement equation and the filtering equation, the attitude error differential equation is obtained as: The velocity error differential equation is: The position error differential equation is: The error differential equation of the gyroscope bias is: The error differential equation of the accelerometer bias is: The error differential equation of the virtual input deviation is: The error vector and the noise vector are defined as: In the formula, η is the error vector, and w is the noise vector; Then the error vector differential equation is obtained as: In the formula, F is a system matrix in continuous time form, and G is a noise matrix; wherein the system matrix F in continuous time form is: The noise matrix G is:

6. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 5, wherein, In step S3, the observation model for the nonlinear error variable is constructed, including: The observation of UWB is the distance between the UWB tag and multiple base stations, wherein the base stations are fixed in the geodetic coordinate system, and the coordinates of N base stations in the geodetic coordinate system are respectively: b，1 , P b，2 , …P b，N , The tag is fixed on the robot, and the relative rod arm between the tag and the IMU is P t , and the rod arm value is obtained through pre-calibration; The distance observation equation between the UWB tag and the i-th base station is: where is the distance observation, r i is the distance ground truth, ε i is the observation noise, ||·|| is the two-norm of a vector; The observation equation of the error variable is constructed as: Through Taylor expansion, the following is obtained: Due to the virtual input bias b v Always 0, introduce virtual observation: 0 3×1 = b v + ε v According to the virtual observation, the error observation variable is constructed as: The distance error variables observed by all N base stations and the virtual observation error variables are integrated to obtain: z=Hη+ε Wherein, z is an observation vector, ε is an observation noise vector, and H is an observation matrix; The specific expression of the observation matrix H is:

7. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 1, wherein, In step S5, the posterior estimation of the error variable is obtained by recursive standard Kalman filtering, the error dynamics equation in continuous time form is discretized, and the first-order approximation is adopted to obtain: η k = A k-1 η k-1 + B k-1 w k-1 wherein A k-1 ≈I 18 +ΔtF k-1 , B k-1 ≈ΔtG k-1 , the subscript k denotes the value of the variable at time k, and Δt is the discrete time interval. When the IMU obtains angular velocity and acceleration observations, construct the continuous-time system matrices F, G, and the discrete-time system matrices A, B from the observations k k Predict according to the following equations;​ wherein are the prior estimate of the error variable at time k and the posterior estimate at time k-1, respectively, are the prior variance at time k and the posterior variance at time k-1, respectively, is the process noise variance matrix in discrete time form, and E[·] is the expected value of a random quantity.

8. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 7, wherein, The posterior estimation of the error variable is calculated through the Kalman filtering update equation, as follows: When the UWB ranging observation is obtained, the posterior state estimation and the posterior variance are obtained by correction; where K k represents a Kalman gain, represents an observation noise variance matrix.

9. The IMU / UWB integrated navigation method based on nonlinear error definition of claim 1, wherein, In step S6, the navigation state and the bias state are corrected, including: When the IMU obtains the angular velocity and acceleration observation at time k-1, the IMU dynamics equation is discretized to determine the position, attitude, velocity and bias term at the current time; After the posterior estimation of the error variable at time k is obtained by Kalman filtering, the position, velocity, attitude and bias term of the inertial navigation solution are corrected; 10. An IMU / UWB integrated navigation system based on nonlinear error definition, characterized in that, The system is used for regulating and controlling the IMU / UWB integrated navigation method based on the nonlinear error definition according to any one of claims 1-9, and the system comprises: An IMU dynamics model construction module, which is based on the dynamics equation of the IMU, introduces virtual input and virtual input bias, and constructs an extended IMU dynamics model; An error dynamics model construction module, which is used for proposing a nonlinear error for the extended IMU dynamics model, and constructing an error dynamics model based on the error definition form; An observation model construction module, which is used for constructing an observation model for the nonlinear error variable according to the UWB observation equation, and introducing a virtual observation variable for the virtual input bias to construct a virtual observation equation for correcting the error state; An error state estimation module, which is used for recursively obtaining the posterior estimation of the error variable by using the standard Kalman filtering; A navigation state correction module, which is used for correcting the navigation state and the bias state by using the posterior estimation of the error variable to obtain the IMU / UWB integrated navigation solution.

Images