Calibration and alignment method suitable for rotary inertial navigation system under shaking base

Abstract

The invention discloses a calibration and alignment method suitable for a rotary inertial navigation system under a shaking base. The calibration and alignment method comprises the following steps: firstly, designing a new 20-position transposition by utilizing a rotation-stop combination mode; then establishing an IMU error model and a 39-dimensional RINS navigation error model, and then performing coarse alignment by using an optimization-based method to obtain an initial attitude of a carrier; and performing simultaneous calibration and alignment by using an iterative Kalman filtering method based on zero-speed observation, recording and compensating all error parameters after each time of calibration, and taking the state quantity at the moment and the covariance matrix corresponding to the state quantity as initial values of next iteration until the covariance matrix is converged to a threshold value or reaches the number of iterations. Therefore, various errors are compensated in a short time, the high-precision initial attitude is obtained, and the long-term navigation positioning precision of the RINS is ensured.

Description

Technical Field

[0001] The present invention is applicable to an underwater vehicle rapidly entering a high-precision autonomous navigation field in an emergency situation, and is specifically applicable to a calibration and alignment method of a rotating inertial navigation system under a shaking base. Background Art

[0002] Underwater vehicles play an important role in the exploration of deep seabed topography and geomorphology and large-scale marine environmental surveys. In order to ensure that underwater vehicles can successfully complete related tasks, the navigation system they are equipped with is required to have the ability to perform long-term autonomous navigation and positioning underwater. Before entering autonomous navigation, the vehicle needs to calibrate and initially align the inertial navigation system to improve the accuracy of inertial navigation positioning. In order for the vehicle to form the ability to quickly enter the navigation phase, it is necessary to achieve calibration of all error parameters and high-precision initial alignment in a short time.

[0003] At present, the commonly used autonomous navigation equipment for underwater vehicles is the Strapdown Inertial Navigation System (SINS). Although SINS has the advantages of strong autonomy, good concealment, and high update frequency, it needs to be calibrated regularly on a laboratory turntable and error compensation is performed to improve its navigation and positioning accuracy. In order to get rid of the dependence of INS on high-precision turntables, the Rotational Inertial Navigation System (RINS) has received more and more attention. RINS relies on its own rotation mechanism to perform autonomous alignment and autonomous calibration regularly to maintain long-term navigation accuracy. Autonomous alignment technology requires RINS to perform initial alignment without relying on external equipment, so as to obtain the initial attitude for solving the autonomous navigation phase. The accuracy of initial alignment will directly affect the autonomous navigation and positioning accuracy of RINS. Initial alignment technology includes two parts: coarse alignment and fine alignment. The currently commonly used coarse alignment is performed under a static base. In actual situations, the vehicle will sway under the influence of water flow, so the optimization-based coarse alignment (OBA) method is proposed to solve the coarse alignment problem of the moving base. The currently commonly used dual-position based precision alignment method mainly considers the influence of the gyro bias error and accelerometer bias error of RINS. Theoretically, if other errors of RINS are considered, such as scale factor error, installation error and lever arm error, the accuracy of precision alignment will be improved. At the same time, in order to improve the long-term navigation and positioning accuracy of the spacecraft, system-level error calibration technology is required. System-level autonomous calibration is mainly carried out by designing reasonable transposition and using Kalman filtering technology for parameter identification. In order to calibrate as many error parameters of RINS as possible, the system state quantity increases, resulting in a higher dimension of the matrix and a problem of slow filter convergence speed. At present, most calibration methods take 0.5 hours, and some even take 1 hour. Therefore, it is crucial for the high-precision autonomous navigation of the spacecraft to calibrate and compensate as many error parameters as possible in a short time and perform initial alignment at the same time.

[0004] In order to calibrate more errors in a short time and perform alignment at the same time, the present invention designs a new and easy-to-implement 20-position rotation scheme by using a rotation-stop combination method, stimulates the gyro bias, gyro scale factor error, gyro installation error, accelerometer bias, accelerometer scale factor error, accelerometer installation error, accelerometer quadratic error and inner arm error parameters of the RINS, and then realizes real-time error compensation and initial alignment by improving the 39-dimensional Kalman filter, so that the spacecraft can complete the preparation work within 15 minutes and enter the autonomous navigation stage.

[0005] The present application is different from the prior art as follows:

[0006] Technical comparison with patent CN104880182A "A laser gyroscope combined error coefficient separation method based on dual-axis rotation"

[0007] Patent CN104880182A designed 16-position rotation for the dual-axis rotating laser gyroscope, and the calibration time was more than 4000 seconds. We designed 20-position rotation for the three-axis rotating inertial navigation system, which took 810 seconds, or 13.5 minutes.

[0008] Patent CN112561998B uses the Kalman filter method to calibrate the zero term and linearity deviation and installation error. We use the backward-iterative Kalman filter method to calibrate and compensate the gyro zero bias, accelerometer zero bias, gyro installation error, accelerometer installation error, gyro scale factor error, accelerometer installation error, accelerometer quadratic term error and inner lever arm error, and also complete the rough alignment and fine alignment. The entire calibration compensation and initial alignment time is within 15 minutes.

[0009] Technical comparison with patent CN108507592A "A method for calibrating non-orthogonal angles of a dual-axis rotating inertial navigation system"

[0010] Patent CN108507592 uses a two-position method to calibrate non-orthogonal angles in a dual-axis rotation inertial navigation system. We first designed a 20-position indexing, then performed a rough alignment based on optimization under a shaking base, and then used a backward-iterative Kalman filter to perform calibration and fine alignment simultaneously. In addition to calibrating the non-orthogonal angle error (installation error), we also calibrated the zero bias error, scale factor error, accelerometer quadratic error, and inner lever arm error.

[0011] Technical comparison with patent CN103245360A "Self-alignment method for rotating strapdown inertial navigation system of carrier-based aircraft under shaking base"

[0012] Patent CN103245360A uses the gravity acceleration in the inertial system as a reference vector to calculate the rough initial attitude, and then uses a fading adaptive Kalman filter to perform fine alignment in order to isolate the impact of shaking on the initial alignment. We use an optimization-based method for rough alignment, because the optimization method constructs multiple sets of observation equations during the alignment process and makes full use of the observation vectors constructed during the entire alignment process, so it can improve the initial alignment speed of the shaking carrier.

[0013] Patent CN103245360A only solves the self-alignment problem of the rotating strapdown inertial navigation system under the shaking base. However, we have solved the problem of self-calibration and self-alignment in a short time under the shaking base.

[0014] Technical comparison with patent CN113639766B "System-level calibration method for dual-axis rotational inertial navigation system containing non-orthogonal angles"

[0015] Patent CN113639766B constructs a 34-dimensional error model for non-orthogonal angles between the axes of a dual-axis rotating inertial navigation system, and then uses the Kalman filter algorithm to calibrate the system's parameters. We construct a 39-dimensional error model for a three-axis rotating inertial navigation system, and then use the backward-iterative Kalman filter algorithm to calibrate and compensate the system's parameters, while also performing initial alignment.

[0016] Patent CN113639766B designed a rotation scheme for self-calibration based on the observability analysis of the system. This scheme only uses the rotation of angular velocity to calibrate the non-orthogonal angle error, zero bias error and proportional factor error. In addition to calibrating the above errors, our rotation-stop combined 20-position rotation scheme can also calibrate the accelerometer quadratic error and inner lever arm error within 13.5 minutes. Summary of the invention

[0017] In view of the problems of long autonomous calibration time and low initial alignment accuracy of RINS, the present invention proposes a calibration and alignment method suitable for a rotating inertial navigation system under a shaking base, which can not only quickly calibrate and compensate for all errors of RINS, but also perform high-precision initial alignment, thereby improving the rapid navigation capability and long-term navigation positioning accuracy of RINS.

[0018] To achieve the above object, the technical solution adopted by the present invention is:

[0019] A calibration and alignment method for a rotating inertial navigation system under a shaking base comprises the following steps:

[0020] Step 1: Designed a new 20-position inversion:

[0021] RINS contains a three-axis gyroscope and a three-axis accelerometer. In order to fully excite the gyro bias, gyro scale factor error, gyro installation error, accelerometer bias, accelerometer scale factor error, accelerometer installation error, accelerometer quadratic error and inner lever arm error, a static-rotation combination method is used for transposition design;

[0022] Step 2: Establish the IMU error model and the 39-dimensional RINS navigation error model:

[0023] The models of 3 gyro bias errors, 3 accelerometer bias errors, 3 gyro scale factor errors, 6 gyroscope installation errors, 3 accelerometer scale factor errors, 3 accelerometer installation errors, 3 accelerometer quadratic errors and 9 inner lever arm errors are established respectively, and a 39-dimensional state quantity is constructed based on the above errors and attitude errors and velocity errors;

[0024] Step 3: Get the initial pose based on the optimized coarse alignment:

[0025] First, the real-time attitude matrix of the carrier is decomposed into two time-varying matrices and a constant matrix. The observation vectors of the gyroscope and accelerometer at multiple times and not collinear are obtained by transposition. Then, the initial attitude of the carrier is determined by using an optimization algorithm.

[0026] Step 4: Iterative Kalman filtering method based on zero-speed observation:

[0027] The velocity error is used to construct the observation equation of the Kalman filter. When the RINS rotates, the attitude error model and the velocity error model are used to solve the accumulated error. When the RINS is in a stationary period, the accumulated velocity error is subtracted from the matrix as the observation quantity for Kalman filter measurement update. After calibration, all error parameters and state quantities X are recorded. k and the covariance matrix P k ;

[0028] Step 5: Algorithm termination judgment:

[0029] After a calibration, record all errors and the state quantity X at this time. k and the covariance matrix P k , then judge P k Whether it converges to the threshold, if not, enter the next iteration process, by compensating all errors in the RINS original data and converting the X of the previous iteration k and P k As the initial value of this iteration, if P k After converging to the threshold, all calibrated error parameters and initial posture are output, thus completing error calibration compensation and precise alignment simultaneously within 13.5 minutes.

[0030] As a further improvement of the present invention, step 3 performs simultaneous calibration and fine alignment based on the backward-iterative Kalman filter method of zero-speed observation. When the RINS rotates, the attitude error model and the velocity error model are used to solve the accumulated error. When the RINS is in a stationary period, its accumulated velocity error is used as the observation quantity for Kalman filter measurement update. The observation equation is:

[0031] Z=[δv E δv N δvU ] T

[0032] The measurement matrix is ​​H = [0 3×3 Ι 3×3 0 36×36 After calibration, record all error parameters and state quantities X k and the covariance matrix P k .

[0033] As a further improvement of the present invention, in step 5, after a calibration is performed, all errors and the state quantity X at this time are recorded. k and the covariance matrix P k Then determine whether P k Converge to the threshold. If it is not reached, enter the next iteration. Before the iteration, all errors are compensated in the RINS original data. The compensation model is:

[0034]

[0035]

[0036] And the X of the previous iteration k and P k As the initial value of this iteration. k After converging to the threshold, all calibrated error parameters and initial posture are output, thus completing error calibration compensation and precise alignment simultaneously within 13.5 minutes.

[0037] Compared with the prior art, the present invention has the following beneficial effects:

[0038] The present invention provides a method for rapid and simultaneous autonomous calibration and alignment of a rotating inertial navigation system (RINS) under a shaking base. The present invention first designs a new 20-position rotation by combining rotation and stop. Then an IMU error model is established and a 39-dimensional RINS navigation error model is constructed, and the state quantity includes attitude error, velocity error, gyro bias, gyro scale factor error, gyro installation error, accelerometer bias, accelerometer scale factor error, accelerometer installation error, accelerometer quadratic term error and inner rod arm error. Then, a rough alignment is performed using an optimization-based method to obtain the initial attitude of the carrier. Then, an iterative Kalman filtering method based on zero-speed observation is used for simultaneous calibration and alignment, and all error parameters are recorded and compensated after each calibration, and the state quantity at this time and its corresponding covariance matrix are used as the initial value of the next iteration until the covariance matrix converges to a threshold or reaches the number of iterations, thereby not only compensating for various errors in a short time, but also obtaining a high-precision initial attitude, thereby ensuring the long-term navigation and positioning accuracy of RINS. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 Flowchart of the proposed algorithm for simultaneous calibration and alignment in a short time;

[0040] Figure 2 This is the result diagram of accelerometer bias error calibration;

[0041] Figure 3 This is the calibration result diagram of the gyro scale factor error. DETAILED DESCRIPTION

[0042] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0043] like Figure 1 As shown, the present invention proposes a calibration and alignment method for a rotating inertial navigation system under a shaking base, comprising the following steps:

[0044] 1) Designed a new 20-position indexing

[0045] Since RINS has its own rotation mechanism, its inner frame corresponds to the x-axis, the middle frame corresponds to the y-axis, and the outer frame corresponds to the z-axis. The rotation design is shown in Table 1: 120 seconds of rest, the middle frame rotates 180 degrees forward, 20 seconds of rest, the middle frame rotates 180 degrees forward, 20 seconds of rest, the middle frame reverses 180 degrees, 20 seconds of rest, the middle frame reverses 180 degrees, 20 seconds of rest. The outer frame rotates 90 degrees, 20 seconds of rest, the inner frame rotates 180 degrees forward, 20 seconds of rest, the inner frame rotates 180 degrees forward, 20 seconds of rest, the inner frame reverses 180 degrees, 20 seconds of rest, the inner frame reverses 180 degrees, 20 seconds of rest. The middle frame rotates 90 degrees, 20 seconds of rest, the outer frame rotates 180 degrees forward, 20 seconds of rest, the outer frame rotates 180 degrees forward, 20 seconds of rest, the outer frame reverses 180 degrees, 20 seconds of rest, the outer frame reverses 180 degrees, 20 seconds of rest. The inner frame rotates 90 degrees forward, and then stops for 20 seconds. The middle frame rotates 90 degrees backward, and then stops for 20 seconds. The outer frame rotates 180 degrees forward, and then stops for 20 seconds. The outer frame rotates 180 degrees forward, and then stops for 20 seconds. The outer frame rotates 180 degrees forward, and then stops for 20 seconds. The outer frame rotates 180 degrees backward, and then stops for 20 seconds. The outer frame rotates 180 degrees backward, and then stops for 20 seconds. The total time is 13.5 minutes.

[0046] Table 1. Design of calibration transposition sequence

[0047]

[0048] 2) Establish the IMU error model and the 42-dimensional RINS navigation error model: The measurement model of the gyroscope is:

[0049]

[0050] K gxx ,K gyy ,K gzzIndicates the gyro scale factor error in ppm. K gxy ,K gxz ,K gyx ,K gyz ,K gzx ,K gzy Indicates the installation error in arc seconds. It represents the gyro bias in degrees / hour. Taking the gyro coordinates as the reference coordinate system, the measurement model of the accelerometer is:

[0051]

[0052] K axx ,K ayy ,K azz Indicates the accelerometer scale factor error in ppm. ayx ,K azx ,K azy Represents the accelerometer installation error in arc seconds. Represents the accelerometer zero bias in μg. The model of the accelerometer quadratic error is:

[0053]

[0054] K a2x ,K a2y ,K a2z represents the quadratic error coefficient, where The calculation is done using the following formula:

[0055]

[0056] r x ,r y ,r z represents the inner lever arm parameter, represents the acceleration at the origin, Represents the installation error angle between the acceleration sensitive axis and the frame coordinate system. The 42-dimensional system state quantity is:

[0057]

[0058] where φ E φ N φ U δv E δv N δv U They are attitude error and velocity error respectively.

[0059] The attitude error model is:

[0060]

[0061] The speed error model is:

[0062]

[0063] 2) Obtaining the initial attitude based on optimization rough alignment: In order to adapt to different alignment requirements, the optimization-based method can be applied to both static and moving bases. First, the vector observation model based on attitude determination is expressed in quaternion form:

[0064]

[0065] In the formula, represents quaternion multiplication operation, (·) * Indicates the conjugate operation of the quaternion. Multiply the quaternion on both sides of the above equation get:

[0066]

[0067] Since the reference vector and the observation vector are both three-dimensional vectors, for ease of calculation, two matrices are constructed according to the above formula as follows:

[0068]

[0069] The right side of formula (1.2) is obtained by the matrix [α k ⊙] is converted to:

[0070]

[0071] The left side of formula (1.2) is obtained by the matrix Translates to:

[0072]

[0073] Substituting equations (1.4) and (1.5) into (1.2), the measurement model can be restated as:

[0074]

[0075] Using the least squares constraint method, the following cost function is obtained through formula (1.6):

[0076]

[0077] make Satisfy the minimum cost function The quaternion That is the minimum eigenvalue λ of the K matrix min The corresponding feature vector.

[0078] Discretize the K matrix expression in equation (1.7) and express the iterative solution as follows:

[0079]

[0080] The attitude solution method based on OBA uses all sampled observation vector information when calculating the K matrix. Compared with other methods, this method has a high utilization rate of observation information.

[0081] 3) Backward-iterative Kalman filtering method based on zero-speed observation:

[0082] After obtaining the initial attitude in the previous step, the attitude error model and velocity error model are used to solve the accumulated error when the RINS rotates. When the RINS is in a stationary period, its accumulated velocity error is used as the observation quantity for Kalman filter measurement update. The observation equation is:

[0083] Z=[δv E δv N δv U ] T

[0084] The measurement matrix is ​​H = [0 3×3 Ι 3×3 0 36×36 ], I is the unit matrix. After calibration, all error parameters and state quantities X are recorded. k and the covariance matrix P k .

[0085] 4) Algorithm termination judgment:

[0086] First, after a calibration, record all errors and the state quantity X at this time. k and the covariance matrix P k Then determine whether P k Converge to the threshold, the judgment method is:

[0087] P k <ε||k>N p

[0088] Where ε is the algorithm termination condition judgment threshold, which should be set considering the actual initial alignment environment, inertial navigation type and other factors; k is the number of iterations, N p is the maximum number of iterations.

[0089] If it is not reached, the next iteration is entered. Before the iteration, all errors are compensated in the RINS original data. The compensation model is:

[0090]

[0091] And the X of the previous iterationk and P k As the initial value of this iteration. k After converging to the threshold, all calibrated error parameters and initial posture are output, thus completing error calibration compensation and precise alignment simultaneously within 13.5 minutes.

[0092] Embodiments of the present invention are described in detail below. The embodiments are exemplary and intended to be used to explain the present invention, but should not be construed as limiting the present invention.

[0093] In order to verify the effectiveness of the present invention, the algorithm is simulated and verified using the MATLAB platform. The simulation parameter settings of the rotating inertial navigation system are shown in Table 1.

[0094] Table 1 Simulation condition parameter settings

[0095]

[0096] The calibration results are as follows Figure 2-3 As shown in the figure, the calibration and alignment results are obtained by using the newly designed transposition and three backward-iterative Kalman filtering methods. It can be seen from the figure that the accelerometer bias error and gyro scale factor error converge within 13.5 minutes, thus verifying the effectiveness of this method.

[0097] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A calibration and alignment method for a rotating inertial navigation system under a shaking base, characterized in that: The steps include: Step 1: Designed a new 20-position inversion: RINS contains a three-axis gyroscope and a three-axis accelerometer. In order to fully excite the gyro bias, gyro scale factor error, gyro installation error, accelerometer bias, accelerometer scale factor error, accelerometer installation error, accelerometer quadratic error and inner lever arm error, a static-rotation combination method is used for transposition design; Step 2: Establish the IMU error model and the 39-dimensional RINS navigation error model: The models of 3 gyro bias errors, 3 accelerometer bias errors, 3 gyro scale factor errors, 6 gyroscope installation errors, 3 accelerometer scale factor errors, 3 accelerometer installation errors, 3 accelerometer quadratic errors and 9 inner lever arm errors are established respectively, and a 39-dimensional state quantity is constructed based on the above errors and attitude errors and velocity errors; Step 3: Get the initial pose based on the optimized coarse alignment: Firstly, the real-time attitude matrix of the carrier is decomposed into two time-varying matrices and a constant matrix. The observation vectors of the gyroscope and accelerometer at multiple times and not collinear are obtained by transposition. Then, the initial attitude of the carrier is determined by using the optimization algorithm. Step 4: Iterative Kalman filtering method based on zero-speed observation: The velocity error is used to construct the observation equation of the Kalman filter. When the RINS rotates, the attitude error model and the velocity error model are used to solve the accumulated error. When the RINS is in a stationary period, the accumulated velocity error is subtracted from the matrix as the observation quantity for Kalman filter measurement update. After calibration, all error parameters and state quantities X are recorded. k and the covariance matrix P k ; Step 5: Algorithm termination judgment: After a calibration, record all errors and the state quantity X at this time. k and the covariance matrix P k , then judge P k Whether it converges to the threshold, if not, enter the next iteration process, by compensating all errors in the RINS original data and converting the X of the previous iteration k and P k As the initial value of this iteration, if P k After converging to the threshold, all calibrated error parameters and initial posture are output, thus completing error calibration compensation and precise alignment simultaneously within 13.5 minutes.

2. The calibration and alignment method for a rotating inertial navigation system under a shaking base according to claim 1 is characterized in that: Step 3: The backward-iterative Kalman filter method based on zero-speed observation is used for simultaneous calibration and fine alignment. When the RINS rotates, the attitude error model and the velocity error model are used to solve the accumulated error. When the RINS is in a stationary period, the accumulated velocity error is used as the observation quantity for Kalman filter measurement update. The observation equation is: Z=[δv E δv N δv U ] T The measurement matrix is ​​H = [0 3×3 Ι 3×3 0 36×36 After calibration, record all error parameters and state quantities X k and the covariance matrix P k .

3. The calibration and alignment method for a rotating inertial navigation system under a shaking base according to claim 1, characterized in that: In step 5, after a calibration, all errors and the state quantity X at this time are recorded. k and the covariance matrix P k Then determine whether P k Converge to the threshold. If it is not reached, enter the next iteration. Before the iteration, all errors are compensated in the RINS original data. The compensation model is: And the X of the previous iteration k and P k As the initial value of this iteration. k After converging to the threshold, all calibrated error parameters and initial posture are output, thus completing error calibration compensation and precise alignment simultaneously within 13.5 minutes.

Images