Anti-disturbance self-calibration method of inertial navigation equipment for navigation
Through the combination of wavelet transformation and Kalman filter, the disturbance speed of inertial navigation equipment is removed, and the calibration accuracy is improved by using particle swarm optimization algorithm, which solves the problem of low calibration accuracy of inertial navigation system in dynamic scenarios, and achieves high-precision inertial navigation system Navigation calibration.
Patent Information
- Application Number
- CN202411305178.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-19
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2044-09-19
AI Technical Summary
The existing inertial navigation systems are difficult to achieve high-precision calibration under mooring, anchoring and heading conditions, especially in the case of large external disturbances in dynamic scenarios, which cannot meet the high-precision requirements of long-term sailing.
Wavelet transformation technology is used to process the speed error of inertial navigation equipment, remove disturbance velocity, and use Kalman filter to estimate the inertial navigation error parameters, and extend the boundary data in combination with particle swarm optimization algorithm to improve calibration accuracy.
It realizes high-precision inertial navigation calibration in dynamic scenarios, improves the calibration accuracy of the inertial navigation system under mooring, anchoring and heading conditions, and meets the high-precision requirements of long-distance flights.
Smart Images

Figure CN119085710A_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the technical field of ship navigation, and in particular to an anti-disturbance self-calibration method for an inertial navigation device for navigation. Background Art
[0002] Calibration technology is a key technology in the field of inertial navigation systems and the basis for navigation solutions of inertial navigation systems. Calibration technology is essentially an error compensation technology, that is, to establish an accurate model of the measurement error of the inertial component and reasonably design experiments to stimulate the error source of the inertial component so that each error parameter can be determined. Finally, the error parameters obtained by calibration are used to perform software compensation on the output of the inertial navigation to reduce the error of the inertial navigation. The calibration result directly affects the accuracy of the inertial navigation.
[0003] The commonly used calibration methods can be divided into discrete calibration methods and system-level calibration methods. The parameters to be calibrated are the zero bias, scale factor error and installation error of the inertial device. Discrete calibration is performed by directly comparing the output of the inertial sensor with the external reference input. When the turntable is leveled and aligned, the rotation of the turntable is equivalent to the external reference input. The inertial navigation is installed on the turntable. The true angular velocity and accelerometer on each axis of the inertial navigation coordinate system are known. The process of rotating the turntable is to construct equations to solve the parameters. The relationship between the inertial sensor output and the turntable input is used to determine the inertial component error coefficient, and then the calibration is completed. The principle of system-level calibration is to use the navigation parameter error as the observed quantity and use the filtering method to identify the error coefficient of the inertial component. This method is based on the principle of navigation error resolution and uses the navigation error to estimate the inertial device error. It does not rely on the turntable accuracy. Therefore, it has also been applied in actual production with good results.
[0004] Self-calibration in dynamic scenes is usually carried out in the field, where various disturbances have a large impact and there is a lack of auxiliary equipment such as high-precision turntables. Discrete calibration is difficult to adapt to such an environment. Therefore, for the problem of dynamic self-calibration, a system-level calibration method is usually used. By designing a reasonable transposition path, the navigation error is used to calculate the device error to avoid dependence on a high-precision turntable. However, the existing system-level calibration method cannot achieve high-precision calibration for working conditions with large disturbances in dynamic scenes, and it is difficult to meet the high-precision calibration requirements of long-duration and high-precision marine inertial navigation under mooring, anchoring and heading conditions.
[0005] Existing technologies usually regard the external disturbance velocity as the observation noise of the Kalman filter, and try to reduce the influence of the disturbance velocity on the inertial navigation calibration by adjusting the observation noise parameters. The disturbance is not effectively suppressed, and in practice, the external disturbance velocity has a fixed frequency and does not obey the Gaussian distribution. It is difficult to meet the assumption that the observation noise is white noise in the Kalman filter, so it will inevitably have a greater impact on the calibration accuracy. Some other existing technologies use some simplified error parameter models and design error models for self-calibration. This method has limited anti-interference ability.
[0006] In summary, there is an urgent need for a method that can meet the high-precision calibration requirements of long-duration and high-precision marine inertial navigation under mooring, anchoring and heading conditions to solve the problems existing in the prior art. Summary of the invention
[0007] The purpose of the present invention is to provide an anti-disturbance self-calibration method for an inertial navigation device for navigation, so as to solve the technical problem that the existing calibration method is difficult to meet the requirements of long-time and high-precision calibration under mooring, anchoring and heading change conditions. The specific technical solution is as follows:
[0008] The present invention provides a method for anti-disturbance self-calibration of an inertial navigation device for navigation, comprising the following steps:
[0009] S1. Rotate the turntable of the marine inertial navigation equipment according to the calibration path to collect raw inertial navigation data;
[0010] S2. Perform inertial solution on the raw inertial navigation data to obtain a solution speed, and subtract the reference speed from the solution speed to obtain a speed error including a disturbance speed;
[0011] S3, removing the disturbance speed from the speed error including the disturbance speed by real-time wavelet transform;
[0012] S4, using the velocity error processed by wavelet transform as the Kalman filter observation quantity, and obtaining the inertial navigation error parameter by using Kalman filtering;
[0013] S5, feeding back the calculated inertial navigation error parameters to the inertial solution process; S6, repeating S2 to S5 until the inertial navigation error parameters estimated by the Kalman filter converge, and completing the calibration of the inertial navigation error parameters.
[0014] A further improvement of the anti-disturbance self-calibration method of the marine inertial navigation device of the present invention is that when the speed error including the disturbance speed is removed by real-time wavelet transform, the method comprises the following steps:
[0015] Through the scale transformation and translation transformation of the wavelet decomposition calculation equation, the velocity error signal is transformed into wavelet coefficients of different scales and positions to obtain wavelet signals of different frequencies. The wavelet decomposition calculation equation is:
[0016]
[0017] Where x(t) is the finite input signal of the velocity error; ψ * () is the complex conjugate of the mother wavelet basis function; * is the complex conjugate; a and b are transformation parameters; x(a,b) is the signal obtained after wavelet transformation; t is time; dt represents the differential of time.
[0018] The anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention is further improved by screening the wavelet signal below the target frequency and reconstructing it to obtain a reconstructed velocity error signal, the expression of which is as follows:
[0019]
[0020] ψ j,k (t) = 2 j / 2 ψ(2 j tk);
[0021] Among them, x r (t) is the reconstructed velocity error signal; ψ j,k (t) is the kth wavelet basis function of the jth layer transformed from the mother wavelet basis function ψ(t); ψ is the mother wavelet basis function; j is the jth layer transformed from the mother wavelet basis function; k is the kth wavelet basis function in this layer.
[0022] A further improvement of the anti-disturbance self-calibration method for an inertial navigation device for navigation of the present invention is that it also includes:
[0023] The wavelet transform is used to eliminate the disturbance speed of the reconstructed speed error signal to obtain the speed error, which includes the following steps:
[0024] Determine the basis function, and choose db4 wavelet as the basis function;
[0025] Determine the number of decomposition layers, and the number of transformation layers K satisfies the following formula:
[0026]
[0027] Among them, f c is the final low-frequency component obtained by wavelet transform; f s is the velocity solution frequency in actual calculation, K is the number of transformation layers;
[0028] Determine the width of the sliding window of the wavelet transform input data window;
[0029] Process the boundary data and extend the input data window to obtain the velocity error.
[0030] A further improvement of the anti-disturbance self-calibration method for an inertial navigation device for navigation of the present invention is that it also includes:
[0031] When extending the input data window, the particle swarm optimization algorithm is used to extend the boundary data, including the following steps:
[0032] The input speed is cached and a fitting function is constructed based on the characteristics of the speed error and the disturbance speed:
[0033]
[0034] Among them, h m 、p n ,q n and t n is the fitting coefficient to be determined; x is the independent variable of the velocity model; V predict is the predicted speed output by the speed model; M is the highest power of x in the fitting function; m is the mth power of the x polynomial in the fitting function and the corresponding mth fitting coefficient; N is the sum of a total of N+1 sinusoidal terms; n is the nth coefficient to be fitted in the sinusoidal function term;
[0035] The particle swarm optimization algorithm is used to fit the cache input speed and solve the fitting coefficient in the fitting function;
[0036] Concatenate input speed and prediction speed to extend data boundaries.
[0037] A further improvement of the anti-disturbance self-calibration method for inertial navigation equipment for navigation of the present invention is that when solving the fitting coefficient in the fitting function, the particle group X composed of S particles is s}, the speed and position of each particle change iteratively; in each iteration, the particle will record two optimal values: one is the optimal solution found by the particle itself; the other is the optimal solution currently found by the entire group; based on these two optimal values, the speed and position of the particle are updated, and the optimal position obtained is the fitting coefficient. The update formula is as follows:
[0038] V i =ω*V i +e1r1(P best -P i )+e2r2(G best -P i );
[0039] P i =P i +V i ;
[0040] Among them, Vi is the velocity of the ith particle; P i is the position of the i-th particle; ω is the inertia weight coefficient; e1 and e2 are learning factors; r1 and r2 are random numbers; the value range is [0,1]; P best is the optimal position of the particle in its own history; G best The optimal position for the entire group.
[0041] A further improvement of the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention is that before obtaining the inertial navigation error parameters by using Kalman filtering, an inertial device error model is determined, and the inertial device error model includes an accelerometer error model, and the accelerometer error model is expressed as follows:
[0042]
[0043] Among them, δa b is the output error of the accelerometer in the carrier coordinate system, a i is the input acceleration of the i-axis accelerometer, (i=x,y,z); K i is the accelerometer scale factor error of the i-axis, (i=x,y,z); M ij is the accelerometer installation error, (i, j = x, y, z and i ≠ j); λ i is the asymmetry error of the scale factor of the i-axis, (i = x, y, z); is the accelerometer zero bias of the i-axis, (i=x,y,z).
[0044] A further improvement of the anti-disturbance self-calibration method of the marine inertial navigation device of the present invention is that the inertial device error model also includes a gyroscope error model, and the gyroscope error model is expressed as follows:
[0045]
[0046] Among them, δω b is the output error of the gyroscope in the carrier coordinate system, ω i is the angular velocity measured by the i-axis gyroscope, (i=x,y,z); S i is the gyro scale factor error of the i-axis, (i=x,y,z); E ij is the gyro installation error, (i, j = x, y, z and i ≠ j); ε i is the gyro zero bias of the i-axis, (i=x,y,z).
[0047] A further improvement of the anti-disturbance self-calibration method of the marine inertial navigation device of the present invention is that after determining the inertial device error model, an inertial navigation error equation is established:
[0048]
[0049] Among them, the superscript n is the navigation coordinate system, b is the carrier coordinate system, e is the earth coordinate system, and i is the inertial coordinate system. is the differential of the position error of the inertial navigation in the navigation coordinate system, is the differential of the velocity error of the inertial navigation in the navigation coordinate system, is the differential of the attitude error of the inertial navigation in the navigation coordinate system, δr n is the position error of the inertial navigation in the navigation coordinate system, δV n is the velocity error of the inertial navigation in the navigation coordinate system, ψ n is the attitude error of the inertial navigation in the navigation coordinate system, is the projection of the rotation angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the projection of the rotation angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the projection of the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the transformation matrix from the carrier coordinate system to the navigation coordinate system, i.e., the attitude matrix, δa b is the output error of the accelerometer in the carrier coordinate system, δω b is the output error of the gyroscope in the carrier coordinate system, f n For comparison, the projection is in the navigation coordinate system.
[0050] A further improvement of the anti-disturbance self-calibration method of the marine inertial navigation device of the present invention is that when the inertial navigation error parameters are obtained by using Kalman filtering, a self-calibration filter is designed:
[0051] Using the standard form of the Kalman filter state equation: u is the noise of the system; the error model of the accelerometer and gyroscope is substituted into the inertial navigation error equation to obtain the matrix form of the calibration model; the state variable of the model is X = [δr n ,δv n ,ψ n ,X g ,X a ] T , namely position error, velocity error, attitude error and device error of gyroscope and accelerometer, the state variables are as follows:
[0052] δr n =[δn N δn E δn D ] T
[0053] δV n =[δV N δVE δV D ] T
[0054] ψ n =[ψ N ψ E ψ D ] T
[0055] X g =[S x E yx S y E zy E xz S z ε x ε y ε z ] T
[0056]
[0057] Among them, δr n is the position error, δn N is the north position error, δn E Easting position error, δn D Earth position error; δV n is the speed error, δV N is the north velocity error, δV E Eastward velocity error, δV D Ground velocity error; ψ n is the attitude error, ψ N is the north attitude error, ψ E is the eastward attitude error, ψ D is the ground attitude error; X g is the device error of the gyroscope, X a is the device error added to the table;
[0058] The coefficient matrix in the model is:
[0059]
[0060] The sub-matrices in F are:
[0061]
[0062] in, is the earth component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the east component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the north component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the earth component of the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the earth component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the east component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the north component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the earth component of the projection of the specific force in the navigation coordinate system, is the east component of the projection of the specific force in the navigation coordinate system, is the north component of the projection of the specific force in the navigation coordinate system;
[0063]
[0064] in, is the transformation matrix from the carrier coordinate system to the navigation coordinate system, i.e., the attitude matrix; c ij for The jth component in the i-th row of the matrix has a value range of i (1, 2, 3) and a value range of j (1, 2, 3).
[0065]
[0066] in, is the north component of the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, ω i is the angular velocity measured by the i-axis gyroscope, (i = x, y, z), c ij for The jth component in the i-th row of the matrix has a value range of i (1, 2, 3) and a value range of j (1, 2, 3).
[0067] The application of the technical solution of the present invention has the following beneficial effects:
[0068] The anti-disturbance self-calibration method of the inertial navigation equipment for navigation of the present invention uses wavelet transform to process the signal, which can realize the separation of signals of different frequencies, and the time delay brought is low, which is suitable for the real-time process of inertial navigation self-calibration. Compared with the low-pass filtering based on Fourier transform, it has variable time-frequency resolution, and can have better frequency resolution for the low-frequency part of the signal, and has better time resolution for the high-frequency part of the signal. The present invention uses wavelet transform to separate the disturbance speed and speed error, eliminate the external disturbance speed, minimize the influence of the reference speed error introduced into the filter observation on the calibration accuracy, and use the speed after eliminating the disturbance as the filter observation. Then establish a Kalman filter to realize the dynamic self-calibration of the dual-axis inertial navigation. Compared with the prior art that usually regards the disturbance speed as the observation noise of the Kalman filter, the calibration accuracy of the present invention is greatly improved. In addition, in view of the boundary signal distortion problem faced after wavelet transform, the present invention uses a particle swarm optimization algorithm to extend the input data window to improve the boundary signal accuracy of wavelet transform. Compared with the commonly used boundary extension methods such as constant extension, symmetric extension, and linear extension, the processing accuracy of the present invention is higher, thereby solving the technical problem that the calibration methods in the prior art cannot achieve the high-precision calibration requirements of long-duration and high-precision marine inertial navigation under mooring, anchoring and heading conditions.
[0069] In addition to the above-described purposes, features and advantages, the present invention has other purposes, features and advantages. The present invention will be further described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS
[0070] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:
[0071] Figure 1 It is a flow chart of the anti-disturbance self-calibration method of the inertial navigation equipment for navigation of the present invention;
[0072] Figure 2 It is a coordinate diagram comparing the errors of different boundary extension methods of the anti-disturbance self-calibration method of the marine inertial navigation device of the present invention;
[0073] Figure 3 It is a comparative coordinate diagram of north velocity error extracted by the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention;
[0074] Figure 4 It is a comparative coordinate diagram of eastward velocity error extracted by the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention;
[0075] Figure 5It is a comparative coordinate diagram of navigation results (latitude error) extracted by the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention;
[0076] Figure 6 It is a comparative coordinate diagram of navigation results (north velocity error) extracted by the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention;
[0077] Figure 7 It is a comparative coordinate diagram of navigation results (longitude error) extracted by the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention;
[0078] Figure 8 It is a comparative coordinate diagram of navigation results (eastward velocity error) extracted by the anti-disturbance self-calibration method of the inertial navigation device for navigation of the present invention. DETAILED DESCRIPTION
[0079] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.
[0080] See also Figures 1 to 8 As shown, a method for anti-disturbance self-calibration of an inertial navigation device for navigation includes the following steps:
[0081] S1. Rotate the turntable of the marine inertial navigation equipment according to the calibration path to collect raw inertial navigation data;
[0082] S2. Perform inertial solution on the raw inertial navigation data to obtain a solution speed, and subtract the reference speed from the solution speed to obtain a speed error including a disturbance speed;
[0083] S3, removing the disturbance speed from the speed error including the disturbance speed by real-time wavelet transform;
[0084] S4, using the velocity error processed by wavelet transform as the Kalman filter observation quantity, and obtaining the inertial navigation error parameter by using Kalman filtering;
[0085] S5, feeding back the calculated inertial navigation error parameters into the inertial solution process;
[0086] S6. Repeat S2 to S5 until the inertial navigation error parameters estimated by the Kalman filter converge, and the calibration of the inertial navigation error parameters is completed.
[0087] Specifically, the calibration path is the calibration path of position 18 in Table 1 as shown in the following table:
[0088] Table 1 18-position rotation path
[0089]
[0090] In the table, in the 18-position rotation path, N represents north, E represents east, D represents earth, U represents sky, W represents west, and S represents south.
[0091] The relevant steps of inertial navigation solution can be found in Chapter 4 of Yan Gongmin's "Strapdown Inertial Navigation Algorithm and Integrated Navigation Principles" (Northwestern Polytechnical University Press).
[0092] The present invention can solve the calibration problem of inertial navigation in dynamic scenes. In the moving state, the self-calibration of inertial navigation is seriously affected by the external disturbance speed, resulting in too low calibration accuracy and unable to meet the requirements of high-precision marine navigation. In order to solve this problem, the present invention proposes an anti-disturbance self-calibration method for laser gyro inertial navigation that can be applied to dynamic scenes. The primary technical problem to be solved is to achieve the separation of the true value and the error value of the velocity observation, that is, the separation of the external disturbance speed and the inertial navigation speed error. The present invention is suitable for the self-calibration of laser gyro inertial navigation systems in dynamic scenes and environments with severe velocity disturbances. Its typical application scenario is the self-calibration of high-precision and long-duration marine inertial navigation under moored, anchored or moving conditions.
[0093] In order to solve the dynamic calibration problem of inertial navigation, the present invention adopts a system-level calibration method. The system-level calibration method is to perform inertial solution on the output of the inertial component, subtract it from the reference to obtain the navigation error, and use the Kalman filtering method to calculate the error parameters and perform compensation.
[0094] The system-level calibration method obtains the relationship between the navigation error result and the inertial device error by linking the inertial device error model with the navigation error equation.
[0095] Therefore, the inertial device error model should be determined first. The model includes scale factor error, installation error, scale factor asymmetry error and zero bias. The inertial device error model includes the accelerometer error model and the gyroscope error model. The accelerometer error model is expressed as follows:
[0096]
[0097] Among them, δa b is the output error of the accelerometer in the carrier coordinate system, a i is the input acceleration of the i-axis accelerometer, (i=x,y,z); K i is the accelerometer scale factor error of the i-axis, (i=x,y,z); M ij is the accelerometer installation error, (i, j = x, y, z and i ≠ j); λ i is the asymmetry error of the scale factor of the i-axis, (i = x, y, z); is the accelerometer zero bias of the i-axis, (i=x,y,z).
[0098] The gyroscope error model is as follows:
[0099]
[0100] Among them, δω b is the output error of the gyroscope in the carrier coordinate system, ω i is the angular velocity measured by the i-axis gyroscope, (i=x,y,z); S i is the gyro scale factor error of the i-axis, (i=x,y,z); E ij is the gyro installation error, (i, j = x, y, z and i ≠ j); ε i is the gyro zero bias of the i-axis, (i=x,y,z).
[0101] In order to uniquely determine the carrier system, in this embodiment, the installation error variables of the three gyroscopes are set to zero, that is, the three axes of the gyroscope system are constrained within the plane of the carrier system.
[0102] Then establish the inertial navigation error equation, estimate the inertial device error through the inertial navigation error, and the relationship between the inertial navigation error and the inertial device error is reflected by the inertial navigation error equation. The inertial navigation error equation is as follows:
[0103]
[0104] Among them, the superscript n is the navigation coordinate system, b is the carrier coordinate system, e is the earth coordinate system, and i is the inertial coordinate system. is the differential of the position error of the inertial navigation in the navigation coordinate system, is the differential of the velocity error of the inertial navigation in the navigation coordinate system, is the differential of the attitude error of the inertial navigation in the navigation coordinate system, δr n is the position error of the inertial navigation in the navigation coordinate system, δV n is the velocity error of the inertial navigation in the navigation coordinate system, ψ n is the attitude error of the inertial navigation in the navigation coordinate system, is the projection of the rotation angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the projection of the rotation angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the projection of the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the transformation matrix from the carrier coordinate system to the navigation coordinate system, i.e., the attitude matrix, δa b is the output error of the accelerometer in the carrier coordinate system, δω b is the output error of the gyroscope in the carrier coordinate system, f n For comparison, the projection is in the navigation coordinate system.
[0105] By substituting the output error model of the accelerometer in the carrier coordinate system and the output error model of the gyroscope in the carrier coordinate system into the above formula, the relationship between the navigation error and the inertial device error can be obtained.
[0106] System-level calibration is to stimulate the inertial device error through external rotation or maneuvering of the carrier, perform inertial navigation solution, compare the solution results with the reference results, and use a specific algorithm to calculate the error parameters. There are mainly two ways to implement the system-level calibration method. The first is the analytical method. The analytical method first designs a specific motion transposition rule, and according to this motion transposition rule, the equation relationship between the navigation error (position error, velocity error, etc.) and the IMU (Inertial Measurement Unit) error parameter to be calibrated is obtained, and then the navigation error during the calibration process is recorded, and finally the least squares method is used to fit and estimate the required error parameter. The second is the filtering method. This method designs a Kalman filter based on the error equation of the strapdown inertial navigation system, selects the required error parameter as the state quantity of the Kalman filter, and measures the navigation error as the quantity of the Kalman filter. At the same time, a specific motion transposition rule is designed, and the motion transposition rule must ensure that the error parameter to be obtained can be fully stimulated. Finally, the required error parameter is estimated by filtering. The system-level fitting method cannot do without the need for calibration equipment and environment, while the system-level filtering method reduces the requirements for turntable accuracy, can establish a complex model to improve system accuracy, and reduce calibration time. Therefore, the present invention adopts the filtering method to achieve system-level calibration.
[0107] The next step is to design a self-calibrating filter. The standard form of the Kalman filter state equation is used: u is the noise of the system; the error model of the accelerometer and gyroscope is substituted into the inertial navigation error equation to obtain the matrix form of the calibration model; the state variable of the model is X = [δr n ,δv n ,ψ n ,X g ,X a ] T , namely position error, velocity error, attitude error and device error of gyroscope and accelerometer, the state variables are as follows:
[0108] δr n =[δn N δn E δn D ] T
[0109] δV n =[δV N δV E δV D ]T
[0110] ψ n =[ψ N ψ E ψ D ] T
[0111] X g =[S x E yx S y E zy E xz S z ε x ε y ε z ] T
[0112]
[0113] Where N is north, E is east, and D is ground; δr n is the position error, δn N is the north position error, δn E Easting position error, δn D Earth position error; δV n is the speed error, δV N is the north velocity error, δV E Eastward velocity error, δV D Ground velocity error; ψ n is the attitude error, ψ N is the north attitude error, ψ E is the eastward attitude error, ψ D is the ground attitude error; X g is the device error of the gyroscope, X a is the device error added to the table; a i (i=x,y,z) is the input acceleration of the i-axis accelerometer, K i (i=x,y,z) is the accelerometer scale factor error of the i-axis, M ij (i, j = x, y, z and i ≠ j) is the accelerometer installation error, λ i (i=x,y,z) is the asymmetric error of the scale factor of the i-axis, is the accelerometer zero bias of the i-axis, ω i (i=x,y,z) is the angular velocity measured by the i-axis gyroscope, S i (i=x,y,z) is the gyro scale factor error of the i-axis, E ij (i, j = x, y, z and i ≠ j) is the gyro installation error, ε i(i=x,y,z) is the gyro zero bias of the i-axis.
[0114] The coefficient matrix in the model is:
[0115]
[0116] The sub-matrices in F are:
[0117]
[0118] in, is the earth component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the east component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the north component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the earth component of the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the earth component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the east component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the north component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the earth component of the projection of the specific force in the navigation coordinate system, is the east component of the projection of the specific force in the navigation coordinate system, is the north component of the projection of the specific force in the navigation coordinate system;
[0119]
[0120] in, is the transformation matrix from the carrier coordinate system to the navigation coordinate system, that is, the attitude matrix. ij for The jth component in the i-th row of the matrix has a value range of i (1, 2, 3) and a value range of j (1, 2, 3).
[0121]
[0122] in, is the north component of the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, ω i is the angular velocity measured by the i-axis gyroscope, (i = x, y, z); c ij for The jth component in the i-th row of the matrix, the value range of i is (1, 2, 3); the value range of j is (1, 2, 3).
[0123] The next step is to design the system's measurement equation z=Hx+v, where z is the measured value of the observed quantity, H is the measurement system parameter, x is the system state, and v is the measurement noise.
[0124] Considering that it is difficult to obtain the absolute attitude of the inertial navigation under field conditions, the speed is selected as the observation quantity, and the convergence speed of the error parameter is faster at this time. After the filter design is completed, the inertial navigation speed error is excited by the rotation of the conversion mechanism, and the instrument error parameter can be identified using the measured speed error. When the carrier is in motion, the reference speed always has an error. In this way, when calculating the filter observation error, directly subtracting the reference speed will introduce the reference speed error into the filter observation, resulting in a reduction in calibration accuracy. Therefore, the present invention specifically proposes an anti-disturbance dynamic self-calibration scheme for the problem of low dynamic self-calibration accuracy. The anti-disturbance dynamic self-calibration method of the present invention is described in detail below, which can effectively reduce the error of the reference speed.
[0125] like Figure 1 As shown in the figure, the overall framework process of dynamic self-calibration is as follows: first, the output data of the inertial sensor is solved by inertial navigation to obtain the speed, the solved speed is subtracted from the reference speed to obtain the speed error, the external disturbance speed is eliminated by wavelet transform, the speed error after eliminating the disturbance is used as the observation quantity of Kalman filter, and then the error parameters are estimated, and the sensor error correction and navigation error correction data are returned to the inertial navigation solution.
[0126] The principle of wavelet transform to eliminate disturbance velocity: By consulting the existing technical literature, it can be known that under the action of waves, sea breeze, etc., ships will produce periodic angular motion and linear motion, and the frequency is generally above 0.05Hz, while the inertial navigation velocity error is composed of the Shura period, Foucault period and earth period. The frequency of these errors is much lower than the frequency of the disturbance motion at sea, so the inertial navigation velocity error and the disturbance motion can be distinguished from the spectrum. The traditional filtering method uses IIR filter (recursive filter) or FIR filter (finite length unit impulse response filter, also known as non-recursive filter) to eliminate the disturbance information. This type of filter can filter out the disturbance motion brought by the outside world, but because the stop band cutoff frequency is too low, phase delay will inevitably be caused while filtering. For real-time systems such as calibration, serious delay will cause the Kalman filter to not converge. Therefore, the present invention uses wavelet transform to eliminate the disturbance information. The time delay brought by wavelet transform as filtering is low, which is suitable for the real-time process of inertial navigation self-calibration. The principle of wavelet transform is to decompose the signal by convolving the signal with a group of wavelet functions to obtain the time-frequency representation of the signal. Compared with the low-pass filtering based on Fourier transform, it has variable time-frequency resolution, which can have better frequency resolution for the low-frequency part of the signal and better time resolution for the high-frequency part of the signal.
[0127] For a velocity error finite input signal, the calculation equation of wavelet decomposition is:
[0128]
[0129] Where x(t) is the finite input signal of the velocity error; ψ * () is the complex conjugate of the mother wavelet basis function; * is the complex conjugate; a and b are transformation parameters; x(a,b) is the signal obtained after wavelet transformation; x is the original signal; t is time; dt is the differential of time.
[0130] Through the above transformation, the signal can be transformed into wavelet coefficients of different scales and positions through the scale transformation and translation transformation of the wavelet function, and a series of wavelets of different frequencies can be obtained. The wavelets below 0.05Hz can be screened out and then reconstructed to obtain the processed signal. The expression is as follows:
[0131]
[0132] ψ j,k (t) = 2 j / 2 ψ(2 j tk);
[0133] Among them, x r (t) is the reconstructed velocity error signal; x(t) is the finite input signal of the velocity error; ψ j,k(t) is the kth wavelet basis function of the jth layer transformed from the mother wavelet basis function ψ(t); ψ is the mother wavelet basis function; j is the jth layer transformed from the mother wavelet basis function; k is the kth wavelet basis function in this layer, and t is time;.
[0134] During the calibration process, the basic principle of extracting speed error is to transform the speed signal into a wavelet signal with multiple frequencies through the above wavelet transform. The frequency of the disturbance speed is higher than 0.05Hz, and the frequency of the speed error is much lower than 0.05Hz. The hard threshold method is used to set the high-frequency wavelet coefficients to zero, and the reconstructed wavelet can obtain the speed error after removing the disturbance speed. The following is the specific process of eliminating the disturbance speed by wavelet transform:
[0135] First, determine the basis function: the basis function is db4 wavelet. The db4 wavelet function has compact support in the time domain, good smoothness, can effectively remove noise in the signal, and the calculation is simple and efficient.
[0136] The second step is to determine the number of decomposition layers: During the wavelet transform process, each transformation will obtain a new low-frequency component that is half of the previous low-frequency component. In order to distinguish the disturbance velocity from the velocity error, the final low-frequency component f c It should be lower than the frequency of the disturbance speed. In actual calculation, the speed solution frequency is f s =250Hz, the frequency of the disturbance speed is above 0.05Hz, the number of transformation layers K satisfies the following formula, and the minimum number of transformation layers is calculated to be K=12.
[0137]
[0138] Among them, f c is the final low-frequency component obtained by wavelet transform; f s is the velocity solution frequency in actual calculation, K is the number of transformation layers;
[0139] The third step is to determine the width of the sliding window: wavelet transform needs to be solved in real time, so a suitable window length needs to be set. After continuous adjustment, the window length is selected as 200s.
[0140] Finally, the boundary data is processed; one problem faced after wavelet transform is the problem of boundary signal distortion, which is due to the insufficient amount of data at the boundary, resulting in a large error at the boundary of the reconstructed signal. The present invention extends the input data window to improve the accuracy of the boundary signal of the wavelet transform. The essence of boundary extension is to increase the observed amount of boundary information to improve the accuracy. Currently, the commonly used boundary extensions include constant extension, symmetric extension, linear extension, etc. These methods are quite effective for regular periodic signal processing, but the processing accuracy is not enough. Therefore, the present invention uses a particle swarm optimization algorithm to extend the boundary data.
[0141] The velocity of the inertial navigation on the sea surface presents the following characteristics: the velocity error oscillates in a long period without sudden changes; the disturbance velocity oscillates in a short period. Based on the above characteristics, the following velocity model is constructed:
[0142]
[0143] Among them, h m 、p n ,q n and t n is the fitting coefficient to be determined; x is the independent variable of the velocity model; V predict is the predicted speed output by the speed model; M is the highest power of x in the fitting function; m is the mth power of the x polynomial in the fitting function and the corresponding mth fitting coefficient; N is the sum of a total of N+1 sinusoidal terms; n is the nth coefficient to be fitted in the sinusoidal function term;
[0144] The polynomial part in the above formula is mainly used to fit the trend term in the window velocity, and the order of the polynomial should not be too high; the trigonometric function part is used to fit the disturbance velocity, which is related to the characteristics of the external disturbance velocity. The commonly used method to solve the coefficients in the above formula is the nonlinear least squares method, but it is easy to fall into the local optimal solution. Therefore, the particle swarm optimization algorithm (PSO algorithm) adopted in the present invention is simple to implement, and it is solved by searching, which has more opportunities to solve the global optimal solution and improves the fitting accuracy. The particle swarm optimization algorithm (PSO algorithm) simulates the foraging process of birds in nature, iteratively selects particles with high fitness, and completes the optimization process. Particle swarm X composed of S particles = {x1, x2,…x s}, the velocity V of each particle i and position P i Iterative changes. In each iteration, the particle will record two optimal values: one is the optimal solution P found by the particle itself best ; The other is the optimal solution G currently found by the entire group best Based on these two optimal values, the speed and position of the particle are updated until the termination condition is met, that is, the maximum number of iterations is reached or the optimal position searched by the particle swarm so far meets the predetermined minimum adaptation threshold. The particle swarm stops updating, and the optimal position found is the fitting coefficient. The update formula is as follows:
[0145] V i =ω*V i +e1r1(P best -P i )+e2r2(G best -P i );
[0146] P i =P i +V i ;
[0147] Among them, V i is the velocity of the ith particle; P i is the position of the i-th particle; ω is the inertia weight coefficient; e1 and e2 are learning factors; r1 and r2 are random numbers; the value range is [0,1]; P best is the optimal position of the particle in its own history; G best The optimal position for the entire group.
[0148] The particle swarm optimization algorithm is used to fit the cached input speed and solve the fitting coefficient in the fitting function; the input speed and the predicted speed are spliced to extend the data boundary.
[0149] The core idea of the boundary extension algorithm based on PSO constructed by the present invention is to predict the future speed based on the current cache speed, and then merge the future speed and the current cache speed to obtain the data of the extended boundary. The boundary extension method based on the PSO algorithm of the present invention is used to solve the problem of boundary signal distortion after wavelet transform. The disturbance information can be eliminated by wavelet transform to obtain the observed value of Kalman filter, and the calibration parameters can be obtained by filtering calculation.
[0150] In view of the problem that the IIR filter and FIR filter in the traditional solution separate the disturbance speed and speed error from the frequency by designing a low-pass filter, which brings obvious time delay, the existing more advanced technology uses a zero-phase shift low-pass filter to process the solution speed. By performing zero-phase filtering on the speed output of a certain type of inertial navigation system in the moored state, it can be seen that the speed error using zero-phase filtering has basically no phase delay, ensuring that the inertial navigation system completes the self-calibration task without relying on external input conditions. In view of the problem that traditional filters will bring time delay, the present invention chooses to use the wavelet transform method to process the speed. The time delay caused by the wavelet transform used as a filter is very low, which is suitable for the real-time process of inertial navigation self-calibration.
[0151] From the theoretical analysis and comparison, it can be seen that the wavelet transform used in the present invention has the following advantages compared with the zero phase shift filter:
[0152] ① Multi-resolution analysis capability: Wavelet transform has the capability of multi-resolution analysis, which can decompose the signal at different scales. This capability makes wavelet transform advantageous in processing non-stationary signals and can better describe the local characteristics of the signal. In contrast, although zero-phase shift filtering can eliminate phase distortion, it may not be as flexible as wavelet transform in multi-resolution analysis.
[0153] ② Fast algorithm: Wavelet transform is a fast algorithm that can complete signal processing in a shorter time. Compared with Fourier transform, wavelet transform is faster in processing non-stationary and nonlinear signals. Although zero-phase shift filtering can reduce the amount of calculation through optimization algorithms in some cases, its overall speed may still be slower than wavelet transform.
[0154] ③ Sparse representation and storage space saving: Wavelet transform is a sparse representation method, that is, only a few coefficients need to be retained. This representation method can greatly reduce storage space and computing time, and can easily perform operations such as compression, noise reduction, and feature extraction. However, zero-phase shift filtering may need to save the entire signal sequence and its filtering results during the processing process, so it may not be as efficient as wavelet transform in terms of storage space.
[0155] ④ Denoising and robustness: Wavelet transform has a certain degree of robustness to noise and interference. In signal processing, noise and interference are inevitable, but wavelet transform can reduce their impact through methods such as filtering and threshold processing, and can better extract the characteristics of the signal. Although zero-phase shift filtering can also suppress noise to a certain extent, its denoising effect may be affected by factors such as filter design and parameter adjustment.
[0156] ⑤ Flexibility in selecting basis: Wavelet transform can use different wavelet basis functions to process different types of signals, such as Haar wavelet (the simplest wavelet basis function), Daubechies wavelet (a wavelet function), etc. This flexibility in selecting basis enables wavelet transform to better adapt to different application scenarios and signal characteristics. In contrast, zero-phase shift filtering may be relatively fixed in terms of filter design and selection, lacking this flexibility.
[0157] ⑥ Applicable to sudden change signals: For sudden change signals (such as edges, spikes, etc.), wavelet transform can maintain a high resolution at the sudden change point, thereby more accurately describing the local characteristics of the signal. In contrast, although zero-phase shift filtering can eliminate phase distortion, it may not provide the same level of detail information as wavelet transform when processing sudden change signals.
[0158] From the perspective of technical effects, the results obtained through respective simulation verifications show that the present invention has higher calibration accuracy than the best prior art to which the second item belongs. The simulation verification results are as follows:
[0159] Table 2 Gyro error calibration simulation results of the present invention
[0160] Error parameters True value Anti-disturbance calibration method <![CDATA[ε x (° / h)]]> 0.01 0.0097 <![CDATA[ε y (° / h)]]> 0.01 0.0096 <![CDATA[ε z (° / h)]]> 0.01 0.0098 <![CDATA[S x (ppm)]]> 100 99.518 <![CDATA[S y (ppm)]]> 100 99.442 <![CDATA[S z (ppm)]]> 100 99.402 <![CDATA[E xz (office)]]> 100 101.724 <![CDATA[E yx (office)]]> 100 99.980 <![CDATA[E zy (office)]]> 100 99.083
[0161] Table 3 Simulation results of gyro error calibration using current technology
[0162] Error parameters True value Calibration method <![CDATA[ε x (° / h)]]> 0.3 0.2866 <![CDATA[ε y (° / h)]]> 0.4 0.3996 <![CDATA[ε z (° / h)]]> 0.5 0.4992 <![CDATA[S x (ppm)]]> 70 81.4253 <![CDATA[S y (ppm)]]> 90 87.5992 <![CDATA[S z (ppm)]]> 90 94.6076 <![CDATA[E xz (office)]]> 145 148.5973 <![CDATA[E yx (office)]]> 242 246.8443 <![CDATA[E zy (office)]]> 242 249.3901
[0163] Table 4 Error calibration results of the present invention
[0164]
[0165]
[0166] Table 5 Simulation results of current technology plus table error calibration
[0167]
[0168] In the table, S i (i=x,y,z) is the gyro scale factor error of the i-axis, E ij (i, j = x, y, z and i ≠ j) is the gyro installation error, ε i (i=x,y,z) is the gyro zero bias of the i-axis. K i (i=x,y,z) is the accelerometer scale factor error of the i-axis, M ij (i, j = x, y, z and i ≠ j) is the accelerometer installation error, λ i (i=x,y,z) is the asymmetric error of the scale factor of the i-axis, is the accelerometer zero bias of the i-axis.
[0169] By comparing the simulation results, it can be seen that the gyroscope zero bias estimation accuracy obtained by the existing technology calibration is better than 0.015° / h, the scale factor estimation accuracy is better than 5ppm, the installation error estimation accuracy is better than 7.2722urad, the accelerometer zero bias estimation accuracy is better than 5ug, the scale factor estimation accuracy is better than 2ppm, and the accelerometer asymmetric error estimation accuracy is better than 2×10 -3 ppm. The gyroscope zero bias estimation accuracy obtained by the present invention is better than 0.003° / h, the scale factor estimation accuracy is better than 0.598ppm, the installation error estimation accuracy is better than 1.724urad, the accelerometer zero bias estimation accuracy is better than 0.708ug, the scale factor estimation accuracy is better than 0.126ppm, and the accelerometer asymmetric error estimation accuracy is better than 0.691ppm. It can be seen from the data results obtained by simulation that the calibration accuracy achieved by the technical solution proposed by the present invention is higher than the prior art.
[0170] Simulation analysis and experimental testing:
[0171] (1) Simulation analysis:
[0172] ①Simulation analysis verifies the effectiveness of the disturbance velocity elimination method based on wavelet transform:
[0173] First, in order to verify the effectiveness of the disturbance velocity elimination method in this paper, the following simulation is carried out: the input signal is composed of sinusoidal signals with frequencies of 0.001Hz and 0.08Hz, with amplitudes of 1m / s, sampling frequency of 250Hz, and signal duration of 200s. Here, the 0.001Hz signal is regarded as an ideal signal, and the 0.08Hz signal is regarded as a disturbance signal. The input signal is processed using the method of the present invention, and compared with the currently commonly used constant extension method, symmetric extension method and linear extension method, and the signal error after wavelet transformation is calculated. In order to compare the changes before and after the extension, only the right side of the input signal is extended during the simulation. By Figure 2 It can be seen that compared with other traditional data extension methods, the extension method of the present invention has the highest accuracy, and the error after wavelet transformation is 1.7%, which is more than 10% higher than other methods. The fitted boundary extension method is then applied to inertial navigation velocity. Figure 2 No extension means no extension, With extension means with extension, Constant extension is constant extension method, Symmetricextension is symmetric extension method, Linearextension is linear extension, PSO extension is particle swarm optimization algorithm extension, the horizontal axis is time in seconds, and the vertical axis is error.
[0174] During the error extraction process, the effectiveness of the wavelet transform method based on the extension of the particle swarm optimization algorithm proposed in the present invention is verified. The simulation process is as follows: a. Generate inertial raw data containing a disturbance velocity of 0.08Hz, and the disturbance velocity amplitude in the carrier coordinate system b is 0.5m / s; b. Perform inertial solution on the raw data to obtain the solution velocity; c. Use the wavelet transform method proposed in the present invention to eliminate the disturbance velocity and obtain the velocity after the disturbance is removed. During the simulation process, the carrier performs periodic linear motion in situ, and the velocity error can be obtained by removing the actual disturbance velocity. The simulation results are shown in the figure. Figure 3 and Figure 4 As shown in the figure, the solved speed is composed of the oscillating disturbance speed and the speed error of the trend change. After being removed by wavelet transform, the eastward disturbance speed is reduced by 97% and the northward disturbance speed is reduced by 95%. The algorithm can extract the speed error more accurately and can solve it in real time. Figure 3 The horizontal axis is time, in seconds; the vertical axis is the north velocity error, in m / s; Velocity is the velocity; Velocity after wavelet transform is the velocity after wavelet transform. Figure 4 The horizontal axis is time, in seconds; the vertical axis is the eastward velocity error, in m / s.
[0175] ② Dynamic base state calibration simulation:
[0176] The dynamic base state calibration simulation is designed to explore the effect of the anti-disturbance calibration method proposed in the present invention on the inertial navigation calibration under the navigation state, and the following simulation conditions are set:
[0177] a. The carrier has periodic angular motion, and the amplitude of the shaking angle is A θs is 3°, and the frequency is 0.08Hz;
[0178] b. The carrier is in the dynamic base state, heading east at a constant speed, with a speed of V cs At the same time, the carrier has periodic linear motion with amplitude A cs The velocity is 0.3m / s, the frequency is 0.08Hz, and the periodic linear motion is used to simulate the disturbance speed of the carrier during navigation;
[0179] c. The inertial navigation error parameters added during the simulation are shown in Table 6, and the calibration path is shown in Table 1;
[0180] d. When performing filtering estimation, the initial value of each error term is 0.
[0181] Table 6 Gyro error calibration results
[0182] Error parameters True value Anti-disturbance calibration method No-disturbance elimination method <![CDATA[ε x (° / h)]]> 0.01 0.0097 0.0341 <![CDATA[ε y (° / h)]]> 0.01 0.0096 0.0328 <![CDATA[ε z (° / h)]]> 0.01 0.0098 -0.0113 <![CDATA[S x (ppm)]]> 100 99.518 105.939 <![CDATA[S y (ppm)]]> 100 99.442 110.561 <![CDATA[S z (ppm)]]> 100 99.402 88.933 <![CDATA[E xz (office)]]> 100 101.724 99.091 <![CDATA[E yx (office)]]> 100 99.980 117.028 <![CDATA[E zy (office)]]> 100 99.083 83.323
[0183] Table 7 Calibration results of table error
[0184]
[0185]
[0186] The simulation lasted 7200 seconds. During the simulation, the inertial navigation system was first aligned for 60 seconds, and then the carrier accelerated eastward to 5 m / s for 100 seconds, maintained a constant speed, and then self-calibrated. The final calibration results are shown in Tables 6 and 7. i (i=x,y,z) is the gyro scale factor error of the i-axis, E ij (i, j = x, y, z and i ≠ j) is the gyro installation error, ε i (i=x,y,z) is the gyro zero bias of the i-axis. K i (i=x,y,z) is the accelerometer scale factor error of the i-axis, M ij (i, j = x, y, z and i ≠ j) is the accelerometer installation error, λ i (i=x,y,z) is the asymmetric error of the scale factor of the i-axis, is the accelerometer zero bias of the i-axis.
[0187] It can be seen from Table 6 and Table 7 that when external disturbance speed exists, the calibration accuracy will be greatly reduced when the disturbance speed is not eliminated. After eliminating the disturbance speed, the calibration accuracy is significantly improved. The algorithm proposed in the present invention can realize the calibration under the inertial navigation moving base. This disturbance speed elimination method is effective and can greatly improve the calibration accuracy.
[0188] (2) Experimental testing
[0189] ① Dynamic base state calibration test
[0190] We conducted a calibration test of the ship's navigation status to verify the performance of the anti-disturbance calibration algorithm of the present invention under the motion base. The dual-axis inertial navigation system was placed on the ship. When the inertial navigation system was aligned, the ship was in a moored state. After the alignment was completed, the self-calibration was performed and the ship sailed away from the port. The test process is as follows:
[0191] a. The inertial navigation system is powered on and the dual-axis inertial navigation turntable is reset to zero;
[0192] b. The inertial navigation system performs coarse alignment for 120 seconds and the ship is moored at the dock;
[0193] c. The inertial navigation system is precisely aligned, which takes 180 seconds, and the ship is moored at the dock;
[0194] d. The inertial navigation system performs self-calibration at 10500s, and the ship is sailing.
[0195] Table 8 INS performance indicators
[0196]
[0197]
[0198] The accuracy of the calibrated inertial navigation device is shown in Table 8. The dual-axis inertial navigation is loaded on the ship and the calibration test is carried out in the navigation state. The total navigation time is 10500s and the maximum navigation speed is 4.6m / s. Before the navigation, the inertial navigation is first aligned and then enters the calibration state. The test conditions during the calibration process are as follows:
[0199] a. The carrier is in a dynamic base state, and the disturbance speed comes from the periodic linear speed of the ship during navigation and the speed caused by the arm error;
[0200] b. The carrier position and reference speed are provided by GPS (GPS position accuracy 5m, speed accuracy 0.1m / s), frequency 1Hz, GPS position is used to correct the position calculated by inertial navigation in real time;
[0201] c. The dual-axis inertial navigation system rotates according to the calibration path table 1;
[0202] d. The initial values of all error terms estimated by the filter are 0.
[0203] In order to make fuller use of the collected raw data, multiple rounds of iterations are used to improve the calibration accuracy. After four iterations, the error parameters are stable. We compare the results of navigation calibration with those of static base calibration, and the results are shown in Tables 9 and 10.
[0204] Table 9 Gyro error calibration results in navigation state
[0205] Error parameters Navigation calibration Static base calibration deviation <![CDATA[ε x (° / h)]]> -0.0329 -0.0363 0.0034 <![CDATA[ε y (° / h)]]> -0.0111 -0.0056 -0.0055 <![CDATA[ε z (° / h)]]> 0.0316 0.02519 0.0065 <![CDATA[S x (ppm)]]> -42.266 -39.234 -3.032 <![CDATA[S y (ppm)]]> -230.573 -230.704 0.131 <![CDATA[S z (ppm)]]> -236.307 -238.321 2.014 <![CDATA[E xz (office)]]> -1378.007 -1384.555 6.548 <![CDATA[E yx (office)]]> 535.288 536.323 -1.035 <![CDATA[E zy (office)]]> 392.172 401.061 -8.889
[0206] Table 10 Navigation status plus table error calibration results
[0207]
[0208]
[0209] Compared with the calibration results under the static base, the gyro bias estimation error under navigation state is 0.007° / h, the gyro scale error error is 3.1ppm, and the gyro installation error is 8.9urad. The table zero bias estimation error is 20.4ug, the table scale error error is 5.3ppm, the table installation error error is 9.4urad, and the table scale asymmetry error is 6ppm, indicating that the proposed calibration scheme can effectively realize the inertial navigation self-calibration under navigation state and realize the effective estimation of all parameters.
[0210] ② Navigation experiment verification
[0211] In order to verify the accuracy of the motion base calibration results, we use the same set of inertial navigation to conduct inertial navigation tests. The calibrated error parameters are written into the program for post-processing, and the post-processing is for the same set of raw data. When performing inertial navigation, the working mode is dual-axis rotation modulation, using a 16-order rotation scheme. The specific test steps are as follows:
[0212] a. Write the inertial navigation error parameters calibrated by different methods under different navigation conditions into the program;
[0213] b. Binding the initial position of the inertial navigation system;
[0214] c. Enter the coarse alignment mode, the coarse alignment time is 240s;
[0215] d. Enter the fine alignment mode, the fine alignment time is 2400s;
[0216] e. Enter the dual-axis rotation modulation mode for inertial navigation, the total time is 10 hours.
[0217] The inertial navigation error parameters calibrated by the method of the present invention in the dynamic base state and the inertial navigation error parameters calibrated by the disturbance suppression method in the dynamic base state are solved and compared. The navigation results are shown in Table 11. Figures 5 to 8 shown.
[0218] Table 11 Navigation results of different calibration algorithms
[0219]
[0220] From Table 11 and Figures 5 to 8 It can be seen that the latitude error corresponding to the inertial navigation error calibrated by the calibration algorithm proposed in the present invention under navigation state is 0.2651n mile, and the longitude error is 0.2852n mile. Compared with the traditional method, the navigation position accuracy and speed accuracy are significantly improved, indicating that the calibration algorithm proposed in the present invention can achieve more accurate calibration under navigation state.
[0221] Figure 5 The horizontal axis is time, with the unit of hour; the vertical axis is latitude error, with the unit of n mile; the blue line in the coordinate axis is the traditional method, and the red line in the coordinate axis is the method of the present invention. Figure 6 The horizontal axis is time, in hours; the vertical axis is north velocity error, in n mile. The blue line in the coordinate axis is the traditional method, and the red line in the coordinate axis is the method of the present invention. Figure 7 The horizontal axis is time, with the unit of hour; the vertical axis is longitude error, with the unit of n mile; the blue line in the coordinate axis is the traditional method, and the red line in the coordinate axis is the method of the present invention. Figure 8 The horizontal axis is time, in hours; the vertical axis is eastward velocity error, in n mile. The blue line in the coordinate axis is the traditional method, and the red line in the coordinate axis is the method of the present invention.
[0222] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.
Claims
1. A method for anti-disturbance self-calibration of inertial navigation equipment for navigation, It is characterized in that The steps include: S1. Rotate the turntable of the marine inertial navigation equipment according to the calibration path to collect raw inertial navigation data; S2. Perform inertial solution on the raw inertial navigation data to obtain a solution speed, and subtract the reference speed from the solution speed to obtain a speed error including a disturbance speed; S3, removing the disturbance speed from the speed error including the disturbance speed by real-time wavelet transform; S4, using the velocity error processed by wavelet transform as the Kalman filter observation quantity, and obtaining the inertial navigation error parameter by using Kalman filtering; S5, feeding back the calculated inertial navigation error parameters into the inertial solution process; S6. Repeat S2 to S5 until the inertial navigation error parameters estimated by the Kalman filter converge, and the calibration of the inertial navigation error parameters is completed.
2. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 1, It is characterized in that When the speed error including the disturbance speed is removed by real-time wavelet transform, the following steps are included: Through the scale transformation and translation transformation of the wavelet decomposition calculation equation, the velocity error signal is transformed into wavelet coefficients of different scales and positions to obtain wavelet signals of different frequencies. The wavelet decomposition calculation equation is: Where x(t) is the finite input signal of the velocity error; ψ * () is the complex conjugate of the mother wavelet basis function; * is the complex conjugate; a and b are transformation parameters; x(a,b) is the signal obtained after wavelet transformation; t is time; dt is the differential of time.
3. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 2, It is characterized in that Also includes: The wavelet signal below the target frequency is filtered and reconstructed to obtain the reconstructed velocity error signal, which is expressed as follows: ψ j,k (t)=2 j / 2 ψ(2 j tk); Among them, x r (t) is the reconstructed velocity error signal; ψ j,k (t) is the kth wavelet basis function of the jth layer transformed from the mother wavelet basis function ψ(t); ψ is the mother wavelet basis function; j is the jth layer transformed from the mother wavelet basis function; k is the kth wavelet basis function in this layer.
4. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 3, It is characterized in that Also includes: The wavelet transform is used to eliminate the disturbance speed of the reconstructed speed error signal to obtain the speed error, which includes the following steps: Determine the basis function, and choose db4 wavelet as the basis function; Determine the number of decomposition layers, and the number of transformation layers K satisfies the following formula: Among them, f c is the final low-frequency component obtained by wavelet transform; f s is the velocity solution frequency in actual calculation, K is the number of transformation layers; Determine the width of the sliding window of the wavelet transform input data window; Process the boundary data and extend the input data window to obtain the velocity error.
5. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 4, It is characterized in that When extending the input data window, the particle swarm optimization algorithm is used to extend the boundary data, including the following steps: The input speed is cached and a fitting function is constructed based on the characteristics of the speed error and the disturbance speed: Among them, h m 、p n ,q n and t n is the fitting coefficient to be determined; x is the independent variable of the velocity model; V predict is the predicted speed output by the speed model; M is the highest power of x in the fitting function; m is the mth power of the x polynomial in the fitting function and the corresponding mth fitting coefficient; N is the sum of a total of N+1 sinusoidal terms; n is the nth coefficient to be fitted in the sinusoidal function term; The particle swarm optimization algorithm is used to fit the cache input speed and solve the fitting coefficient in the fitting function; Concatenate input speed and prediction speed to extend data boundaries.
6. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 5, It is characterized in that When solving the fitting coefficient in the fitting function, the particle swarm composed of multiple particles, the speed and position of each particle changes iteratively; in each iteration, the particle will record two optimal values: one is the optimal solution found by the particle itself; the other is the optimal solution currently found by the entire group; based on these two optimal values, the speed and position of the particle are updated, and the optimal position obtained is the fitting coefficient. The update formula is as follows: V i =ω*V i +e 1 r 1 ( P best -P i )+e 2 r 2 (G best -P i ): P i =P i +V i ; Among them, V i is the velocity of the ith particle; P i is the position of the ith particle; ω is the inertia weight coefficient; e 1 and e 2 is the learning factor; r 1 and r 2 is a random number; its value range is [0,1]; P best is the optimal position of the particle in its own history; G best The optimal position for the entire group.
7. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 1, It is characterized in that Before using Kalman filtering to obtain the inertial navigation error parameters, the inertial device error model is determined. The inertial device error model includes the accelerometer error model. The accelerometer error model is expressed as follows: Among them, δa b is the output error of the accelerometer in the carrier coordinate system, a i is the input acceleration of the i-axis accelerometer, (i=x,y,z); K i is the accelerometer scale factor error of the i-axis, (i=x,y,z); M ij is the accelerometer installation error, (i, j = x, y, z and i ≠ j); λ i is the asymmetric error of the scale factor of the i-axis, (i = x, y, z); i is the accelerometer zero bias of the i-axis, (i=x,y,z).
8. The anti-disturbance self-calibration method for an inertial navigation device for navigation according to claim 7, It is characterized in that The inertial device error model also includes a gyroscope error model, which is expressed as follows: Among them, δω b is the output error of the gyroscope in the carrier coordinate system, ω i is the angular velocity measured by the i-axis gyroscope, (i=x,y,z); S i is the gyro scale factor error of the i-axis, (i=x,y,z); E ij is the gyro installation error, (i, j = x, y, z and i ≠ j); ε i is the gyro zero bias of the i-axis, (i=x,y,z).
9. The anti-disturbance self-calibration method for a marine inertial navigation device according to claim 8, It is characterized in that After determining the inertial device error model, the inertial guidance error equation is established: Among them, the superscript n is the navigation coordinate system, b is the carrier coordinate system, e is the earth coordinate system, and i is the inertial coordinate system. is the differential of the position error of the inertial navigation in the navigation coordinate system, is the differential of the velocity error of the inertial navigation in the navigation coordinate system, is the differential of the attitude error of the inertial navigation in the navigation coordinate system, δr n is the position error of the inertial navigation in the navigation coordinate system, δV n is the velocity error of the inertial navigation in the navigation coordinate system, ψ n is the attitude error of the inertial navigation in the navigation coordinate system, is the projection of the rotation angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the projection of the rotation angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the projection of the rotation angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the transformation matrix from the carrier coordinate system to the navigation coordinate system, i.e., the attitude matrix, δa b is the output error of the accelerometer in the carrier coordinate system, δω b is the output error of the gyroscope in the carrier coordinate system, f n For comparison, the projection is in the navigation coordinate system.
10. The anti-disturbance self-calibration method for a marine inertial navigation device according to claim 9, It is characterized in that When using Kalman filtering to obtain inertial navigation error parameters, a self-calibrated filter is designed: Using the standard form of the Kalman filter state equation: u is the noise of the system; the error model of the accelerometer and gyroscope is substituted into the inertial navigation error equation to obtain the matrix form of the calibration model; the state variable of the model is X = [δr n ,δv n ,ψ n ,X g ,X a ] T , namely position error, velocity error, attitude error and device error of gyroscope and accelerometer, the state variables are as follows: δr n =[δn N δn E δn D ] T δV n =[δV N δV E δV D ] T ψ n =[ψ N ψ E ψ D ] T X g =[S x E yx S y E zy E xz S z e x e y e z ] T X a =[K x M yx M zx M xy K y M zy M xz M yz K z λ x λ y λ z ▽ x ▽ y ▽ z ] T ; Among them, δr n is the position error, δn N is the north position error, δn E Easting position error, δn D Earth position error; δV n is the speed error, δV N is the north velocity error, δV E Eastward velocity error, δV D Ground velocity error; ψ n is the attitude error, ψ N is the north attitude error, ψ E is the eastward attitude error, ψ D is the ground attitude error; X g is the device error of the gyroscope, X a is the device error added to the table; The coefficient matrix in the model is: The sub-matrices in F are: in, is the earth component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the east component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the north component of the projection of the angular velocity of the navigation coordinate system relative to the earth coordinate system in the navigation coordinate system, is the earth component of the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the earth component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the east component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the north component of the projection of the angular velocity of the earth coordinate system relative to the inertial coordinate system in the navigation coordinate system, is the earth component of the projection of the specific force in the navigation coordinate system, is the east component of the projection of the specific force in the navigation coordinate system, is the north component of the projection of the specific force in the navigation coordinate system; in, is the transformation matrix from the carrier coordinate system to the navigation coordinate system, that is, the attitude matrix, c ij for The jth component in the i-th row of the matrix has a value range of i (1, 2, 3) and a value range of j (1, 2, 3). in, is the north component of the projection of the angular velocity of the navigation coordinate system relative to the inertial coordinate system in the navigation coordinate system, ω i is the angular velocity measured by the i-axis gyroscope, (i = x, y, z)c ij for The jth component in the i-th row of the matrix has a value range of i (1, 2, 3) and a value range of j (1, 2, 3).
Citation Information
Patent Citations
MEMS (micro electro mechanical system) gyroscope random error compensation method based on particle swarm wavelet network
CN104101344A
MEMES (micro electro mechanical system) gyroscope random error compensation method on basis of wavelet multi-scale analysis
CN104251712A
