IMU / UWB integrated navigation method and system based on nonlinear error definition

Abstract

The invention belongs to the technical field of integrated navigation, and discloses an IMU / UWB integrated navigation method and system based on nonlinear error definition, and the method comprises the steps: introducing virtual input and virtual input deviation based on an IMU kinetic equation, and constructing an extended IMU kinetic model; providing a nonlinear error aiming at the extended IMU dynamic model, and constructing an error dynamic model based on an error definition form; based on a UWB observation equation, constructing an observation model for the nonlinear error variable; and introducing a virtual observation variable aiming at the virtual deviation, and constructing a virtual observation equation to correct an error state. According to the method provided by the invention, the dependence of an error dynamic model on state estimation can be reduced, a stable and reliable integrated navigation result is still obtained when an initial state error is relatively large, a time-consuming initial alignment process in actual IMU / UWB integrated navigation is avoided, the application complexity and hardware cost of a navigation system are further reduced, and the navigation efficiency is improved. And the method has a wide practical application prospect.

Description

Technical Field

[0001] The present invention belongs to the technical field of integrated navigation, and in particular relates to an IMU / UWB integrated navigation method and system based on nonlinear error definition. Background Art

[0002] Accurate position, attitude, and velocity feedback are essential for mobile robots to autonomously complete their assigned tasks. Inertial measurement units (IMUs) are widely used in robot navigation and positioning due to their high sampling frequency, stable output, and high autonomy. However, current micro-electromechanical (MEMS) IMUs (Micro-Electromechanical) sensors (IMUs) suffer from rapid error accumulation, making them unsuitable for long-term navigation and positioning. While UWB (Ultra-Wideband) sensors can accurately calculate position using precise external ranging information, they cannot obtain robot velocity and attitude information. Integrated IMU / UWB navigation overcomes the limitations of both IMUs and UWB sensors, providing stable position, velocity, and attitude estimates. However, current IMU-based integrated navigation methods require initialization of the navigation state—that is, position, velocity, and attitude. Because the error dynamics models of current inertial integrated navigation are all dependent on the state estimates, initialization accuracy affects the accuracy of the error dynamics model, which in turn impacts the performance of the integrated navigation, particularly the attitude. Initialization accuracy significantly impacts integrated navigation performance. However, attitude initialization is also relatively difficult and cannot be achieved directly with external UWB sensors. Typically, the robot must remain stationary for a period of time to obtain initial roll and pitch angles, and the initial heading angle is determined by sensing the Earth's magnetic field or performing specific maneuvers. This attitude initialization process is inherently tedious and time-consuming, potentially increasing the complexity and hardware cost of integrated navigation applications. To address this issue, a new IMU / UWB integrated navigation method is urgently needed to decouple the navigation error dynamics equations from the navigation state estimates.

[0003] Through the above analysis, the problems and defects of the existing technology are as follows: (1) The current MEMS IMU has problems such as rapid error accumulation and cannot be used for long-term navigation and positioning, while UWB cannot directly obtain the robot's speed and posture information.

[0004] (2) Currently, all IMU-based integrated navigation systems require precise navigation state initialization. However, the system initial alignment and carrier position and velocity initialization are cumbersome and time-consuming, and the attitude initialization is relatively difficult and has relatively large errors, which cannot be achieved directly through external UWB sensors. Summary of the Invention

[0005] To overcome the problems existing in the related art, the embodiments disclosed in the present invention provide an IMU / UWB integrated navigation method and system based on nonlinear error definition. The technical solution is as follows: The present invention is implemented in this way: the IMU / UWB integrated navigation method based on nonlinear error definition includes the following steps: S1, based on the IMU dynamics equation, introduces virtual input and virtual input deviation to construct an extended IMU dynamics model; S2 proposes a nonlinear error for the extended IMU dynamic model and constructs an error dynamic model based on the error definition form; S3, based on the UWB observation equation, construct an observation model for nonlinear error variables; S4, introduce virtual observation variables for virtual deviations and construct virtual observation equations; S5, using standard Kalman filtering recursively to obtain the posterior estimate of the error variable; S6, correct the navigation state and bias state using the posterior estimated value of the error variable to obtain the IMU / UWB integrated navigation solution.

[0006] In step S1, an extended IMU dynamic model is constructed, including: The robot is equipped with an IMU and a UWB tag. The IMU detects the robot's three-axis acceleration and angular velocity. N UWB base stations are fixedly deployed in the positioning area to detect the distance between the robot and the UWB tag. Define the geodetic coordinate system , the origin is selected at any point in the positioning area, and the three directions of northeast and sky are Axis, defining the carrier coordinate system , making the origin 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: ; ; ; ; ; Where, is the rotation matrix from the carrier coordinate system to the earth coordinate system, is the carrier angular velocity observation value obtained by the gyroscope, is the gyroscope bias, is the gyroscope observation white noise, is the acceleration due to gravity, is the carrier acceleration observation value obtained by the accelerometer, is the accelerometer bias, is the accelerometer observation white noise, is the carrier speed, is the gyroscope bias noise, is the accelerometer bias noise, is the carrier position, variable band represents the derivative with respect to time; Represents a three-dimensional vector The skew-symmetric matrix is ​​defined as follows: ; Introducing virtual input Deviation from virtual input , the mechanical arrangement equation of IMU is expanded as follows: ; ; ; ; ; ; Where, is a virtual input, is the virtual input deviation, is the virtual input deviation noise; During the filtering process, the virtual input remains constant. , to ensure that the extended IMU mechanical arrangement equation is the same as the original equation.

[0007] Furthermore, during the filtering process, the filter uses a deterministic dynamic equation, sets the noise value to 0, and calculates the estimated value of each state quantity; ; ; ; ; ; ; In the formula, the variable band Represents the estimated value of the variable.

[0008] In step S2, the nonlinear error is defined as: ; ; ; ; ; ; Where, is the exponential map of the SO(3) group, is the attitude error, is the speed error, is the position error, is the gyroscope bias error, is the accelerometer bias error, is the virtual input deviation error, the superscript Represents the transpose of a matrix or vector.

[0009] In step S2, an error dynamics model is constructed based on the error definition form, including: According to the first-order approximation of the exponential map: ; Where, for dimensional unit matrix; Express the error variable as a first-order approximation: ; ; ; ; ; ; Combining the inertial navigation mechanical arrangement equation and the filtering equation, the attitude error differential equation is obtained as follows: ; The velocity error differential equation is: ; The position error differential equation is: ; The error differential equation for the gyroscope bias is: ; The error differential equation for the accelerometer bias is: ; The error differential equation of the virtual input deviation is: ; The error vector and noise vector are defined as: ; ; Where, is the error vector, is the noise vector; Then the error vector differential equation is obtained as: ; Where, is the system matrix in continuous time form, is the noise matrix; The system matrix in continuous time form for: ; Noise Matrix for: .

[0010] In step S3, an observation model for the nonlinear error variable is constructed, including: The UWB observation 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: , the tag is fixed on the robot, and the relative arm between it and the IMU is , the lever arm value is obtained by pre-calibration; UWB tags and The distance observation equation between base stations is: ; Where, is the distance observation value, is the true value of the distance, is the observation noise, is the two-norm of the vector; The observation equation for the constructed error variable is: ; By Taylor expansion we get: ; Due to the virtual input deviation Always 0, introduce virtual observation: ; According to the virtual observation, the error observation variable is constructed as follows: ; The distance error variables observed by all N base stations and the virtual observation error variables are combined to obtain: ; in, is the observation vector, is the observation noise vector, is the observation matrix; ; Observation Matrix The specific expression is: .

[0011] In step S5, the standard Kalman filter is used to recursively obtain the posterior estimate of the error variable, and the error dynamics equation in continuous time form is discretized and the first-order approximation is used to obtain: ; Where, , subscript Indicates The variable value at that moment, is a discrete time interval; When the IMU obtains angular velocity and acceleration observations, the continuous-time system matrix is ​​constructed based on the observation values. and the discrete-time system matrix , predict according to the following equation; ; ; Where, The error variables are A priori estimate of the time and The posterior estimate of time, They are The prior variance of the moment The posterior variance at time t, is the process noise variance matrix in discrete time form, is the expected value of a random quantity.

[0012] Furthermore, the posterior estimate of the error variable is calculated by the Kalman filter update equation as follows: When UWB ranging observations are obtained, the posterior state estimate and posterior variance are obtained through correction; ; ; ; Where, represents the Kalman gain, represents the observation noise variance matrix.

[0013] In step S6, the navigation state and the deviation state are corrected, including: IMU in When obtaining angular velocity and acceleration observations at any moment, the IMU dynamic equation is discretized to determine the current position, attitude, velocity and deviation terms; ; ; ; ; ; ; Obtained by Kalman filtering After the a posteriori estimation of the error variables at the moment, the position, velocity, attitude and deviation terms solved by the inertial navigation are corrected; ; ; ; ; ; .

[0014] Another object of the present invention is to provide an IMU / UWB integrated navigation system based on nonlinear error definition, which is used to control the IMU / UWB integrated navigation method based on nonlinear error definition. The system includes: The IMU dynamics model construction module introduces virtual input and virtual input deviation based on the IMU dynamics equation to construct an extended IMU dynamics model; Error dynamics model construction module, which is used to propose nonlinear errors for the extended IMU dynamics model and construct the error dynamics model based on the error definition form; The observation model construction module is used to construct an observation model for nonlinear error variables based on the UWB observation equation. At the same time, virtual observation variables for virtual deviations are introduced to construct virtual observation equations to correct the error state. The error state estimation module is used to obtain the posterior estimate of the error variable using standard Kalman filtering recursion; The navigation state correction module is used to correct the navigation state and bias state using the posterior estimated value of the error variable to obtain the IMU / UWB combined navigation solution.

[0015] In combination with all the above technical solutions, the beneficial effects of the present invention are as follows: First, the present invention constructs an extended IMU dynamic equation by introducing virtual inputs and virtual input biases based on the IMU dynamic equations. This invention innovatively defines a nonlinear error for the extended IMU dynamic equations and derives the error dynamic equations based on this error definition. The resulting position, velocity, and attitude dynamic equations are independent of the current state estimate. The error dynamic equations for the accelerometer, gyroscope, and virtual input biases are only related to the bias estimates and not the navigation state estimate. Therefore, the resulting error dynamic model is unaffected by the navigation state (i.e., position, velocity, and attitude) and initial navigation state errors. Based on the UWB observation equation, the present invention constructs observation equations for the nonlinear error variables. Considering that the virtual input bias should always be zero, a virtual observation variable for the virtual bias is introduced to construct the virtual observation equations. The present invention uses standard Kalman filtering to obtain a posteriori estimates of the error variables. The navigation state is then corrected based on the proposed nonlinear error definition to obtain the IMU / UWB integrated navigation solution. The IMU / UWB integrated navigation method proposed in the present invention has better stability and robustness to initial estimation errors because its dynamic equations of position, velocity and attitude errors are completely independent of the navigation state estimation at the current moment. Under the condition of large initial errors, the method proposed in the present invention can still achieve good convergence and stability, and can avoid the relatively time-consuming initial alignment process required in conventional IMU integrated navigation applications. At the same time, no external sensors are required for precise 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 present invention has good engineering application value and practical application potential.

[0016] Second, the present invention proposes a new method for defining nonlinear errors in inertial navigation, and derives the corresponding error dynamics equations and UWB observation equations. The obtained navigation state error dynamics equation does not depend on any state estimation value, and the deviation error dynamics equation only depends on the deviation estimation value, and does not depend on the navigation state estimation value. Therefore, it can reduce the system's dependence on the accuracy of navigation state initialization, especially on attitude initialization. The method proposed in the present invention can achieve stable combined navigation under the condition of "attitude initialization-free".

[0017] The present invention constructs a new nonlinear navigation state error and bias state error, and derives the inertial navigation error dynamics equation and UWB ranging observation equation for the constructed nonlinear error. The navigation state error part in the error state dynamics equation derived by the present invention is completely independent of the state estimation value, and the bias state error part only depends on the slowly varying and small-amplitude bias state estimation. Therefore, the IMU / UWB combined navigation method model proposed by the present invention is completely independent of the navigation state estimation, and therefore does not require precise initialization of the navigation state. Even when the navigation state initialization error is large, a stable estimation result can still be obtained. This beneficial effect of the present invention can save the IMU from the time-consuming initial fine alignment process, and at the same time, does not require external position and velocity sensors for precise initialization of position and velocity, which can further simplify the hardware cost and operation complexity of the IMU / UWB combined navigation, and has good engineering application value and practical application potential.

[0018] Third, the navigation state error dynamic equations derived from the present invention are completely independent of the state estimate, thus eliminating the need for precise initialization of the robot's position, velocity, and attitude to achieve relatively stable and reliable inertial integrated navigation results. This solution eliminates time-consuming initial fine alignment of the robot and eliminates the need for precise position and velocity initialization, saving both the time and hardware required for initialization and improving the reliability and application flexibility of inertial integrated navigation results.

[0019] When considering inertial bias in current integrated navigation systems based on inertial sensors, the navigation error dynamics equation is correlated with the state estimate, and as a result, the model accuracy is affected by the accuracy of state initialization. This paper proposes a new nonlinear error. The resulting navigation error dynamics equation is completely independent of the navigation state estimate and the bias state estimate. The bias error dynamics equation is only correlated with the slowly varying bias state estimate, maintaining good independence from the estimate. This fills a technological gap in the industry, both domestically and internationally.

[0020] Fourth, constructing an inertial navigation error dynamics equation that is independent of the navigation state estimate can reduce the dependence of the inertial combined navigation system on the initial estimation accuracy, and is therefore a difficult problem that people have long been eager to solve. Since the inertial navigation system does not satisfy the group affine property when considering the inertial bias estimate, the current additive error and the error dynamics derived from the nonlinear error based on Lie group error modeling cannot achieve independence from the state estimate. By defining a new nonlinear error form, the present invention obtains the advantages that the navigation state error dynamics are completely independent of the state estimate, and the deviation state error dynamics only depends on the slowly varying deviation estimate, thus solving the technical problem that people have long been eager to solve but have never been successful.

[0021] Because the inertial navigation mechanical equations themselves do not satisfy group affinity when considering inertial bias estimates, researchers generally believe that it is difficult to obtain a navigation error dynamic equation that is completely independent of the state estimate using the left-invariant error definition for inertial integrated navigation. This invention overcomes this technical bias and proposes a new nonlinear error method. The resulting navigation state error dynamic equation is completely independent of the navigation state and bias state estimates. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present disclosure and, together with the description, serve to explain the principles of the present disclosure; Figure 1 This is a flow chart of an IMU / UWB integrated navigation method based on nonlinear error definition provided by an embodiment of the present invention; Figure 2 Schematic diagram of a simulated robot motion trajectory and a UWB base station provided by an embodiment of the present invention; Figure 3 is a graph showing the relationship between the position, speed, and posture of a robot as a function of time, provided by an embodiment of the present invention; Figure 4 1 is a diagram comparing the root mean square errors of position estimation using four methods provided by an embodiment of the present invention; Figure 5 1 is a comparison diagram of the root mean square error of speed estimation using four methods provided by an embodiment of the present invention; Figure 6 1 is a comparison diagram of the root mean square error of attitude estimation using four methods provided by an embodiment of the present invention; Figure 7 1. This is a comparison diagram of the root mean square error of gyroscope bias estimation using four methods provided by an embodiment of the present invention; Figure 8 1 is a comparison diagram of the root mean square error of accelerometer bias estimation using four methods provided in an embodiment of the present invention; Figure 9 This is a graph of the heading angle and z-axis gyro bias estimation errors for each Monte Carlo simulation using the LIEKF, TFLIEKF, and PLIEKF methods provided by an embodiment of the present invention; Figure 10 It is a typical trajectory and corresponding UWB base station location map in the MILUV dataset provided by an embodiment of the present invention; Figure 11 is a graph showing the relationship between the trajectory provided by an embodiment of the present invention and the corresponding UWB base station position and posture changes over time; Figure 12 This is a position comparison diagram provided by an embodiment of the present invention and a comparative method; Figure 13This is a comparison diagram of the posture ARMSE provided by the embodiment of the present invention and the comparison method; Figure 14 is a relationship diagram of position changes over time provided by an embodiment of the present invention; Figure 15 is a graph showing the relationship between attitude RMSE and time, provided by an embodiment of the present invention; Figure 16 1 is a graph showing the relationship between heading angle errors and time for several methods provided in embodiments of the present invention. DETAILED DESCRIPTION

[0023] To make the above-mentioned objects, features, and advantages of the present invention more readily apparent, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings. The following description sets forth numerous specific details to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways than those described herein, and those skilled in the art may make similar modifications without departing from the scope of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0024] To address the problem that existing IMU-based integrated navigation requires time-consuming initial carrier attitude alignment and carrier position and velocity initialization, the innovation of the present invention lies in: the present invention proposes a new inertial navigation nonlinear error definition method. Based on this, the error dynamics equation is derived. The obtained navigation error dynamics equation is completely independent of the navigation state and bias state estimates, and the obtained bias error dynamics equation depends only on the slowly varying bias state estimate and is completely independent of the navigation state estimate. Therefore, the error model's dependence on the navigation state estimate can be eliminated, and stable and accurate state estimates can be obtained without the need for initial fine alignment of the IMU and precise position and velocity initialization, thereby enhancing the practical application efficiency and capability of IMU integrated navigation. Furthermore, for the specific application of IMU / UWB integrated navigation, the present invention derives the UWB observation equation based on the designed nonlinear error, introduces virtual observation variables for virtual biases, and constructs the corresponding observation equation. When obtaining UWB observations, a standard Kalman filter can be used to solve the posterior state estimate of the nonlinear error, thereby correcting the IMU navigation state estimate. Thanks to the mutual independence of the proposed nonlinear error dynamics equation and the navigation state estimation value, the IMU / UWB integrated navigation system proposed in the present invention does not require precise position, attitude and velocity initialization, and therefore has broad practical application prospects.

[0025] The method proposed in the present invention can reduce the dependence of the error dynamics model on state estimation, and still obtain stable and reliable combined navigation results when the initial state error is large, avoiding the time-consuming initial alignment process in actual IMU / UWB combined navigation, thereby reducing the application complexity and hardware cost of the navigation system, and has broad practical application prospects.

[0026] Example 1, as Figure 1 As shown, the IMU / UWB integrated navigation method based on nonlinear error definition provided by an embodiment of the present invention includes the following steps: S1, based on the IMU dynamics equation, introduces virtual input and virtual input deviation to construct an extended IMU dynamics model; Assume 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. N UWB base stations are fixedly deployed in the positioning area to detect the distance between the robot and the tag.

[0027] Define the geodetic coordinate system The origin can be selected at any point in the positioning area, and the three directions of northeast and sky are Axis, defining the carrier coordinate system , whose 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: ; ; ; ; ; Where, is the rotation matrix from the carrier coordinate system to the earth coordinate system, is the carrier angular velocity observation value obtained by the gyroscope, is the gyroscope bias, is the gyroscope observation white noise, is the acceleration due to gravity, is the carrier acceleration observation value obtained by the accelerometer, is the accelerometer bias, is the accelerometer observation white noise, is the carrier speed, is the gyroscope bias noise, is the accelerometer bias noise, is the carrier position, variable band represents the derivative with respect to time; Represents a three-dimensional vector The skew-symmetric matrix is ​​defined as follows: ; Introducing virtual input Deviation from virtual input , the mechanical arrangement equation of IMU is expanded as follows: ; ; ; ; ; ; Where, is a virtual input, is the virtual input deviation, is the virtual input deviation noise; During the filtering process, the virtual input remains constant. , to ensure that the extended IMU mechanical arrangement equation is the same as the original equation.

[0028] During the filtering process, since the true value of the noise cannot be obtained, the filter uses a deterministic dynamic equation, that is, setting the noise value to 0 to calculate the estimated value of each state quantity, as shown below: ; ; ; ; ; ; In the formula, the variable band Represents the estimated value of the variable.

[0029] S2 proposes a nonlinear error for the extended IMU dynamic model and constructs an error dynamic model based on the error definition form; The present invention defines nonlinear error as: ; ; ; ; ; ; Where, is the exponential map of the SO(3) group, is the attitude error, is the speed error, is the position error, is the gyroscope bias error, is the accelerometer bias error, is the virtual input deviation error, the superscript Represents the transpose of a matrix or vector.

[0030] According to the first-order approximation: ; Where, for dimensional unit matrix; ; Express the error variable as a first-order approximation: ; ; ; ; ; ; Combining the inertial navigation mechanical arrangement equation and the filtering equation, the attitude error differential equation is obtained as follows: ; The velocity error differential equation is: ; The position error differential equation is: ; The error differential equation for the gyroscope bias is: ; The error differential equation for the accelerometer bias is: ; The error differential equation of the virtual input deviation is: ; The error vector and noise vector are defined as: ; ; Where, is the error vector, is the noise vector; Then the error vector differential equation is obtained as: ; Where, is the system matrix in continuous time form, is the noise matrix; The system matrix in continuous time form for: ; Noise Matrix for: .

[0031] S3, based on the UWB observation equation, construct an observation model for nonlinear error variables; The UWB observation 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: , the tag is fixed on the robot, and the relative arm between it and the IMU is , the lever arm value is obtained by pre-calibration; UWB tags and The distance observation equation between base stations is: ; Where, is the distance observation value, is the true value of the distance, is the observation noise, is the two-norm of the vector; The observation equation for the constructed error variable is: ; By Taylor expansion we get: ; Due to the virtual input deviation Always 0, introduce virtual observation: ; According to the virtual observation, the error observation variable is constructed as follows: ; The distance error variables observed by all N base stations and the virtual observation error variables are combined to obtain: ; in, is the observation vector, is the observation noise vector, is the observation matrix; ; Observation Matrix The specific expression is: .

[0032] S4, introduce virtual observation variables for virtual deviations and construct virtual observation equations; S5, using standard Kalman filtering recursively to obtain the posterior estimate of the error variable; The standard Kalman filter is used to recursively obtain the posterior estimate of the error variable, and the error dynamics equation in continuous time form is discretized and the first-order approximation is used to obtain: ; Where, , subscript Indicates The variable value at that moment, is a discrete time interval; When the IMU obtains angular velocity and acceleration observations, the continuous-time system matrix is ​​constructed based on the observation values. and the discrete-time system matrix , predict according to the following equation; ; ; Where, The error variables are A priori estimate of the time and The posterior estimate of time, They are The prior variance of the moment The posterior variance at time t, is the process noise variance matrix in discrete time form, is the expected value of a random quantity.

[0033] The posterior estimate of the error variable is calculated using the Kalman filter update equation as follows: When UWB ranging observations are obtained, the posterior state estimate and posterior variance are obtained through correction; ; ; ; Where, represents the Kalman gain, represents the observation noise variance matrix.

[0034] S6, correct the navigation state and bias state using the posterior estimated value of the error variable to obtain the IMU / UWB integrated navigation solution.

[0035] Correct the navigation status and deviation status, including: IMU in When obtaining angular velocity and acceleration observations at any moment, the IMU dynamic equation is discretized to determine the current position, attitude, velocity and deviation terms; ; ; ; ; ; ; Obtained by Kalman filtering After the a posteriori estimation of the error variables at the moment, the position, velocity, attitude and deviation terms solved by the inertial navigation are corrected; ; ; ; ; ; .

[0036] In embodiment 2, an IMU / UWB integrated navigation system based on nonlinear error definition provided by an embodiment of the present invention includes: The IMU dynamics model construction module introduces virtual input and virtual input deviation based on the IMU dynamics equation to construct an extended IMU dynamics model; Error dynamics model construction module, which is used to propose nonlinear errors for the extended IMU dynamics model and construct the error dynamics model based on the error definition form; The observation model construction module is used to construct an observation model for nonlinear error variables based on the UWB observation equation. At the same time, virtual observation variables for virtual deviations are introduced to construct virtual observation equations to correct the error state. The error state estimation module is used to obtain the posterior estimate of the error variable using standard Kalman filtering recursion; The navigation state correction module is used to correct the navigation state and bias state using the posterior estimated value of the error variable to obtain the IMU / UWB combined navigation solution.

[0037] In order to further demonstrate the positive effects of the above embodiment, the present invention conducts the following experiments based on the above technical solution to achieve numerical verification.

[0038] This embodiment demonstrates the navigation performance of the combined navigation method (abbreviated as "PLIEKF") proposed in this invention through numerical simulation. For comparison, this embodiment also demonstrates the existing navigation method based on additive error definition (abbreviated as "MEKF"), Navigation method defined by group left invariant error, i.e., attitude, position and velocity embedding In the group, the navigation performance of the gyroscope and accelerometer using the additive error form (abbreviated as "LIEKF") and the navigation method based on the left invariant error definition of the dual coordinate group (abbreviated as "TFLIEKF") is evaluated. The simulated robot motion trajectory and the position of the UWB base station are shown in Figure 2. Figure 2 As shown in the attached Figure 2 In the figure, the upper left corner shows the simulated three-dimensional trajectory diagram, the upper right corner shows the projection diagram of the simulated trajectory on the xz plane, the lower left corner shows the projection diagram of the simulated trajectory on the xy plane, and the lower right corner shows the projection diagram of the simulated trajectory on the yz plane; the corresponding relationship between the robot position, speed and posture over time is shown in Figure 3 shown.

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

[0040] Table 1 Simulated sensor noise parameters

[0041] For different navigation methods, the filter parameters are set as follows: (1) The initial attitude adopts the “calibration-free” scheme, that is, random sampling is performed within its definition domain, and the roll angle and heading angle are Random sampling is performed between Random sampling is performed between (2) The initial velocity is based on the actual velocity value and adds Gaussian distribution random noise. The standard deviation of the noise is ; (3) The initial position adds Gaussian distributed random noise based on the real position, and the standard deviation of the noise is ; (4) The initial value of IMU bias is set to 0; (5) The variance of the gyroscope, accelerometer and UWB process noise and observation noise is set according to Table 1; (6) The accelerometer and gyroscope bias noise spectral densities are set to and ; (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 .

[0042] This experiment compares the statistical performance of different methods by conducting 100 independent Monte Carlo experiments, using the root mean square error (RMSE) and average root mean square error (ARMSE) of each estimated variable as evaluation indicators, which are defined as follows: ; in, For the In the Monte Carlo simulation, the aircraft The true value of the state at that moment For the In the Monte Carlo simulation, the aircraft The estimated state value at time t, represents the total number of Monte Carlo simulations, is the total simulation time.

[0043] Figure 4-Figure 8 Represents the root mean square error of position, velocity, attitude, gyroscope bias and accelerometer bias estimation of the four positioning methods. Figure 4 In the figure, from top to bottom, the RMSE of the position estimation in the three directions of xyz is shown; Figure 5 In the figure, from top to bottom, the RMSE of velocity estimation in the three directions of xyz are shown; Figure 6 In the figure, from top to bottom are the RMSE of the roll angle, pitch angle and heading angle estimation; Figure 7 In the figure, from top to bottom, the gyroscope bias estimation RMSE in the xyz directions is shown; Figure 8 In the figure, from top to bottom are the RMSE of the accelerometer bias estimation in the three directions of xyz; the corresponding average root mean square error is shown in Table 2.

[0044] Table 2 Average root mean square error (ARMSE) of position, velocity, attitude, gyroscope bias, and accelerometer bias estimation using four methods

[0045] according to Figure 4-Figure 8As can be seen in Table 2, although the initial navigation error set in this case is large (especially the attitude error, which is initialized using the "no initial alignment" method), the present invention can obtain stable navigation state estimation and inertial sensor bias estimation. Other comparison methods, including MEKF, LIEKF, and TFLIEKF, all exhibit varying degrees of divergence. The position, velocity, attitude, gyroscope bias, and accelerometer bias estimation errors obtained by the present method are significantly superior to those of the other comparison methods. Figure 9 The heading angle and z-axis gyro bias estimation errors of the three methods, LIEKF, TFLIEKF and PLIEKF, are further shown for each Monte Carlo simulation (similar results are also shown for other navigation states and bias states); Figure 9 In the figure, the three figures in the upper row are the heading angle estimation error diagrams of the three methods of LIEKF, TFLIEKF, and PLIEKF, and the three figures in the lower row are the z-axis gyroscope bias estimation error diagrams of the three methods of LIEKF, TFLIEKF, and PLIEKF. It can be seen that the method proposed in the present invention can obtain stable heading angle and z-axis gyroscope bias estimation in 100 simulations, which ensures the good statistical performance of the method of the present invention, while some other comparison methods will diverge at a specific number of Monte Carlo simulations, resulting in large statistical errors. This shows that the method proposed in the present invention has good estimation stability and can obtain good navigation effect without precise initial alignment of the inertial sensor, while some existing comparison methods do not have this advantage.

[0046] The proposed method was further validated using the MILUV public dataset. The MILUV dataset uses an unmanned aerial vehicle (UAV) equipped with an IMU, a UWB tag, and a laser rangefinder. The IMU measures the UAV's triaxial acceleration and angular velocity, the UWB tag measures the distance between the UAV and a base station, and the laser rangefinder measures the UAV's altitude. A motion capture system is used to detect the true position and attitude of the UAV.

[0047] The MILUV dataset contains multiple UAV motion trajectories, one of which is a typical trajectory and the corresponding UWB base station position. Figure 10 As shown in (experiment ID: default_1_random3_0), the corresponding relationship between position and posture changes over time is as follows Figure 11 As shown (experiment ID: default_1_random3_0), where Figure 11 The three pictures in the middle and upper rows are Figure 10 The relationship between the corresponding trajectory xyz directions and time changes, the three figures in the bottom row are Figure 10The relationship between the roll angle, pitch angle, and heading angle of the corresponding trajectory in the MILUV dataset and time. The experiment ID and serial number corresponding to each experimental trajectory in the MILUV dataset are shown in the following table.

[0048]

[0049] For each data trajectory, the initialization method similar to that used in numerical simulation is also used to randomly sample 100 different initial navigation states, and the RMSE and ARMSE of the position and attitude obtained from the 100 different initial estimates of different trajectories are verified. Figure 12 and Figure 13 As shown, in Figure 12 In the figure, from top to bottom are the ARMSE diagrams of the position estimation in the three directions of xyz. Figure 13 In the figure, from top to bottom are the ARMSE of roll angle, pitch angle and heading angle. For one of the typical data tracks (experiment ID: default_1_random3_0), the relationship between the position and attitude RMSE over time is as follows: Figure 14 and Figure 15 As shown, in Figure 14 The top, middle and bottom three figures are the RMSE diagrams of the xyz position estimation of a single trajectory. Figure 15 The three figures in the middle, top, middle and bottom are the RMSE diagrams of the roll angle, pitch angle and heading angle estimation of a single trajectory respectively; the relationship between the heading angle error of several methods and time is shown in the figure. Figure 16 As shown, in Figure 16 In the figure, from top to bottom are the heading angle error diagrams of the three methods LIKF, TFLIKF and TGLIKF. Results similar to those verified by simulation data can be obtained, that is, under the conditions of large initial position and velocity errors and "initialization-free" attitude errors, the method proposed in this invention can still obtain stable position and attitude estimation results, and its RMSE is lower than that of the existing comparison methods under almost all motion trajectories. Figure 16 It can be seen that for a typical motion trajectory (experiment ID: default_1_random3_0), the proposed method can obtain stable navigation state estimation results under 100 different initialization conditions, while other comparison methods will have different degrees of divergence, affecting their statistical performance and filter stability.

[0050] The above description is only a preferred specific implementation method of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with this technical field within the technical scope disclosed by the present invention and within the spirit and principles of the present invention should be covered by the scope of protection of the present invention.

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, introduces virtual input and virtual input deviation to construct an extended IMU dynamics model; S2 proposes a nonlinear error for the extended IMU dynamic model and constructs an error dynamic model based on the error definition form; S3, based on the UWB observation equation, construct an observation model for nonlinear error variables; S4, introduce virtual observation variables for virtual deviations and construct virtual observation equations; S5, using standard Kalman filtering recursively to obtain the posterior estimate of the error variable; S6, correct the navigation state and bias state using the posterior estimated value of the error variable to obtain the 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, an extended IMU dynamic model is constructed, including: The robot is equipped with an IMU and a UWB tag. The IMU detects the robot's three-axis acceleration and angular velocity. N UWB base stations are fixedly deployed in the positioning area to detect the distance between the robot and the UWB tag. Define the geodetic coordinate system , the origin is selected at any point in the positioning area, and the three directions of northeast and sky are Axis, defining the carrier coordinate system , making the origin 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: ； ； ； ； ； Where, is the rotation matrix from the carrier coordinate system to the earth coordinate system, is the carrier angular velocity observation value obtained by the gyroscope, is the gyroscope bias, is the gyroscope observation white noise, is the acceleration due to gravity, is the carrier acceleration observation value obtained by the accelerometer, is the accelerometer bias, is the accelerometer observation white noise, is the carrier speed, is the gyroscope bias noise, is the accelerometer bias noise, is the carrier position, variable band represents the derivative with respect to time; Represents a three-dimensional vector The skew-symmetric matrix is ​​defined as follows: ； Introducing virtual input Deviation from virtual input , the mechanical arrangement equation of IMU is expanded as follows: ； ； ； ； ； ； Where, is a virtual input, is the virtual input deviation, is the virtual input deviation noise; During the filtering process, the virtual input remains constant. , to ensure that the extended IMU mechanical arrangement equation is the same as the original equation.

3. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 2, characterized in that: During the filtering process, the filter uses a deterministic dynamic equation, sets the noise value to 0, and calculates the estimated value of each state quantity; ； ； ； ； ； ； In the formula, the variable band Represents the estimated value of the variable.

4. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 3, characterized in that: In step S2, the nonlinear error is defined as: ； ； ； ； ； ； Where, is the exponential map of the SO(3) group, is the attitude error, is the speed error, is the position error, is the gyroscope bias error, is the accelerometer bias error, is the virtual input deviation error, the superscript Represents the transpose of a matrix or vector.

5. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 4, characterized in that: In step S2, an error dynamics model is constructed based on the error definition form, including: According to the first-order approximation of the exponential map: ； Where, for dimensional unit matrix; Express the error variable as a first-order approximation: ； ； ； ； ； ； Combining the inertial navigation mechanical arrangement equation and the filtering equation, the attitude error differential equation is obtained as follows: ； The velocity error differential equation is: ； The position error differential equation is: ； The error differential equation for the gyroscope bias is: ； The error differential equation for the accelerometer bias is: ； The error differential equation of the virtual input deviation is: ； The error vector and noise vector are defined as: ； ； Where, is the error vector, is the noise vector; Then the error vector differential equation is obtained as: ； Where, is the system matrix in continuous time form, is the noise matrix; The system matrix in continuous time form for: ； Noise Matrix for: 。 6. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 5, characterized in that: In step S3, an observation model for the nonlinear error variable is constructed, including: The UWB observation 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: , the tag is fixed on the robot, and the relative arm between it and the IMU is , the lever arm value is obtained by pre-calibration; UWB tags and The distance observation equation between base stations is: ； Where, is the distance observation value, is the true value of the distance, is the observation noise, is the two-norm of the vector; The observation equation for the constructed error variable is: ； By Taylor expansion we get: ； Due to the virtual input deviation Always 0, introduce virtual observation: ； According to the virtual observation, the error observation variable is constructed as follows: ； The distance error variables observed by all N base stations and the virtual observation error variables are combined to obtain: ； in, is the observation vector, is the observation noise vector, is the observation matrix; ； Observation Matrix The specific expression is: 。 7. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 1, characterized in that: In step S5, the standard Kalman filter is used to recursively obtain the posterior estimate of the error variable, and the error dynamics equation in continuous time form is discretized and the first-order approximation is used to obtain: ； Where, , subscript Indicates The variable value at that moment, is a discrete time interval; When the IMU obtains angular velocity and acceleration observations, the continuous-time system matrix is ​​constructed based on the observation values. and the discrete-time system matrix , predict according to the following equation; ； ； Where, The error variables are A priori estimate of the time and The posterior estimate of time, They are The prior variance of the moment The posterior variance at time t, is the process noise variance matrix in discrete time form, is the expected value of a random quantity.

8. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 7, characterized in that: The posterior estimate of the error variable is calculated using the Kalman filter update equation as follows: When UWB ranging observations are obtained, the posterior state estimate and posterior variance are obtained through correction; ； ； ； Where, represents the Kalman gain, represents the observation noise variance matrix.

9. The IMU / UWB integrated navigation method based on nonlinear error definition according to claim 1, characterized in that: In step S6, the navigation state and the deviation state are corrected, including: IMU in When obtaining angular velocity and acceleration observations at any moment, the IMU dynamic equation is discretized to determine the current position, attitude, velocity and deviation terms; ； ； ； ； ； ； Obtained by Kalman filtering After the a posteriori estimation of the error variables at the moment, the position, velocity, attitude and deviation terms solved by the inertial navigation are corrected; ； ； ； ； ； 。 10. An IMU / UWB integrated navigation system based on nonlinear error definition, characterized in that: The system is used to control the IMU / UWB integrated navigation method based on nonlinear error definition according to any one of claims 1 to 9, and the system includes: The IMU dynamics model construction module introduces virtual input and virtual input deviation based on the IMU dynamics equation to construct an extended IMU dynamics model; Error dynamics model construction module, which is used to propose nonlinear errors for the extended IMU dynamics model and construct the error dynamics model based on the error definition form; The observation model construction module is used to construct an observation model for nonlinear error variables based on the UWB observation equation. At the same time, virtual observation variables for virtual deviations are introduced to construct virtual observation equations to correct the error state. The error state estimation module is used to obtain the posterior estimate of the error variable using standard Kalman filtering recursion; The navigation state correction module is used to correct the navigation state and bias state using the posterior estimated value of the error variable to obtain the IMU / UWB combined navigation solution.

Images