A high-precision inertial dynamic posture in-situ calibration method based on relative navigation

Abstract

The invention discloses a high-precision in-situ calibration method for inertial dynamic posture based on relative navigation, which relates to the field of navigation technology and includes an inertial dynamic posture reference datum, optical-based relative navigation, and an in-situ transfer calibration method. The inertial dynamic posture reference datum includes a high-precision optical gyroscope, an accelerometer, and a GNSS. The high-precision optical gyroscope is used to provide high-precision position information, the accelerometer is used to provide speed information, and the GNSS is used to provide attitude and time reference information. The optical-based relative navigation includes optical-based deformation measurement and sub-IMU posture solution. The optical-based relative navigation Kalman filtering method is used to provide sub-IMU posture information. The invention utilizes a high-precision dynamic posture reference system and, with the help of optical-based relative navigation and the in-situ transfer calibration method, achieves high-precision calibration of the zero bias and installation error of the inertial posture measurement system without disassembling the system.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a high-precision inertial dynamic posture in-situ calibration method based on relative navigation. Background Art

[0002] An inertial dynamic position and attitude measurement system (IMU) consists of an inertial measurement unit (IMU) and a global positioning system (GNSS). The IMU is a core component of inertial measurement systems (IMS) such as INS (Inertial Navigation System), GPS, and POS. GNSS provides position, velocity, time, and high-precision pulse per second (PPS) information. IMS provides temporal and spatial reference information for autonomous vehicles and is key to achieving high-precision positioning and navigation. my country has made some progress in the development of IMS, but due to cost and size considerations, autonomous vehicles are typically equipped with MEMS IMS. To ensure the accuracy of MEMS IMS, these systems must be regularly disassembled and calibrated in a laboratory. Furthermore, traditional transfer calibration methods rely solely on mathematical models and fail to reflect actual deformation information. There is an urgent need to develop an in-situ IMS calibration method based on optical relative navigation to achieve high-precision, in-situ, online calibration of MEMS IMS systems.

[0003] An inertial dynamic pose in-situ calibration method based on optical relative navigation is the core of in-situ online calibration of MEMS inertial pose measurement systems. This method receives raw information from a high-precision inertial pose reference system, GNSS, and optical modules, and performs tasks such as data preprocessing, initial calibration, strapdown solution, combined filtering, transfer calibration, and time synchronization. Existing transfer calibration methods fail to consider the effects of transfer calibration deformation when establishing measurement equations, seriously hindering the real-time and accurate estimation of intrinsic parameters such as installation error and zero bias of MEMS inertial pose measurement systems. Summary of the Invention

[0004] In response to the above technical problems, the present invention provides a high-precision in-situ calibration method for inertial dynamic posture based on relative navigation, including an inertial dynamic posture reference benchmark, optical-based relative navigation, and an in-situ transfer calibration method. The inertial dynamic posture reference benchmark includes a high-precision optical gyroscope, an accelerometer, and a GNSS. The high-precision optical gyroscope is used to provide high-precision position information, the accelerometer is used to provide velocity information, and the GNSS is used to provide attitude and time reference information; the optical-based relative navigation includes optical-based deformation measurement and sub-IMU posture solution, and the optical-based relative navigation Kalman filtering method is used to provide sub-IMU posture information; the in-situ transfer calibration method includes an observability adjustment error correction factor algorithm and an observability-based adaptive feedback correction method.

[0005] Furthermore, the optical-based relative navigation Kalman filtering method includes the following steps:

[0006] S1, estimate the baseline length error between the main / sub IMU, the sub-IMU position, velocity, attitude and inertial device error, and feedback correct the estimated error to obtain high-precision sub-IMU real-time navigation results and the relative spatial relationship between the main / sub IMU;

[0007] S2. The state variable X in the Kalman filter model has a total of 45 dimensions, including the misalignment angles of east, north, and celestial directions. Velocity error δV in the east, north and celestial directions E , δV N , δV U , latitude, longitude, and altitude errors δL, δλ, δH, and gyroscope constant drift errors ε in the x, y, and z axes x , ε y , ε z , x, y, z axis accelerometer constant drift error x, y, z axis arm length error Δr x , Δr y , Δr z ;Installation error;

[0008] S3. The quantity measurement Z in the Kalman filter model is the velocity error and position error compensated by a multi-stage lever arm, which consists of a rigid lever arm between the GPS and the main IMU and a flexible lever arm between the main and sub-IMUs.

[0009] Furthermore, the observability adjustment error correction factor algorithm includes the following steps:

[0010] S1. Establish system state equation and measurement equation;

[0011] S2. Establishing a position measurement information correction equation;

[0012] S3. Correct the attitude measurement information.

[0013] Furthermore, the system state equation is: Where X is the system state vector, F is the system state matrix, and W is the system noise matrix;

[0014]

[0015]

[0016] in, They are heading angle error, pitch angle error, and roll angle error; δv E ,δv N ,δv U They are the eastward velocity error, northward velocity error, and celestial velocity error; δL, δλ, and δh are the latitude error, longitude error, and altitude error respectively; ε gx , ε gy , ε gz is the gyro constant drift; ε mx , ε my , ε mz is the gyro first-order Markov process drift; δK gx , δK gy , δK gz is the gyro scale factor error; δW yz , δW zy , δW xz , δW zx , δW xy , δW yx is the gyro installation error; is the addition of a random constant bias; is the drift of the additive first-order Markov process; δK ax , δK ay , δK az is the error of the scale factor; δA yz , δA zy , δA xz , δA zx , δA xy , δA yx is the installation error; θ x ,θ y ,θ z is the flexible deformation angle; is the flexible deformation angular rate.

[0017] Furthermore, the measurement equation is: Z=HX+v, where H is the measurement matrix and v is the measurement noise;

[0018]

[0019]

[0020]

[0021] Where, is the attitude matrix of the inertial pose reference system.

[0022] Furthermore, the position measurement information correction equation is:

[0023]

[0024] Where, L mc ,λ mc 、h mc They are the latitude, longitude and altitude information of the main system respectively; L sc ,λ sc and h sc are the latitude, longitude and altitude information before transmission to the subsystem, R m is the meridian radius of the Earth, R n is the main curvature radius of the Maoyou circle;

[0025]

[0026] Where r is the flexible arm between the inertial pose reference system and the MEMS inertial pose measurement system; r0 is the initial fixed arm between the master and the slave; and Δr is the time-varying arm error between the master and the slave caused by the flexible deformation.

[0027] Furthermore, the steps for correcting the attitude measurement information are as follows:

[0028] S1. Determine the attitude error angle μ, μ=[μ x , μ y , μ z ] T ; The attitude error angle μ includes the fixed installation error angle ρ and the flexible time-varying error angle σ; ρ=[ρ x ,ρ y ,ρ z ] T ,σ=[σ x ,σ y ,σ z ] T The fixed installation error angle ρ is obtained by visual measurement after the system is installed to complete the calibration of the inertial reference system, and the flexible deformation angle σ is measured using an optical sensor.

[0029] The attitude relationship between S2, fixed installation error angle ρ and flexible time-varying error angle σ is:

[0030]

[0031] Where, is the attitude matrix of the sub-IMU, is the attitude error caused by the sub-IMU misalignment angle, Represents the attitude matrix of the main IMU, is the error matrix caused by the attitude error angle;

[0032] S3. When the attitude error angle between the master and subsystems is small:

[0033]

[0034] Where μ = ρ + σ, φx and μx are skew-symmetric matrices composed of misalignment angle φ and error angle μ respectively;

[0035] S4. Expand the formula in S3 to omit the second-order small quantity and take the approximate value:

[0036]

[0037] Let δψ′=ψ-ψ m , δθ′=θ-θ m , δγ′=γ-γ m , thus obtaining:

[0038]

[0039]

[0040]

[0041] Where, is the value of the i-th row and j-th column of the main POS posture matrix, φ E 、φ N 、φ U The misalignment angles for east, north and celestial directions;

[0042] S5. Expand both sides of the above equation according to Taylor series and ignore small quantities above the second order to obtain the corrected attitude measurement formula:

[0043]

[0044]

[0045]

[0046]

[0047] Furthermore, the observability-based adaptive feedback correction method includes:

[0048] S1. Use the instantaneous observability model to establish the observability factor;

[0049] S2, adjust the state variables according to the factor;

[0050] S3. The observability of state variables is improved, filtering estimation and feedback compensation are performed.

[0051] Compared with the prior art, the present invention has the following beneficial effects: (1) the present invention realizes in-situ calibration of inertial dynamic posture, which is suitable for online calibration of MEMS inertial navigation system for autonomous driving; (2) the present invention utilizes dynamic posture reference datum and, with the help of optical relative navigation technology, realizes high-precision calibration of zero bias and installation error of MEMS inertial posture measurement system without disassembling the system through transfer calibration estimation method; (3) in view of the problem of in-situ online calibration existing in the actual application of MEMS inertial posture measurement system for autonomous driving, in-situ transfer calibration based on optical relative navigation is adopted to overcome the deficiency of low calibration accuracy of MEMS inertial posture measurement system caused by traditional transfer calibration, and further improves deformation measurement accuracy compared with the transfer calibration method based on deformation mathematical modeling, thereby improving the transfer calibration accuracy of MEMS inertial posture measurement system; (4) adaptive feedback correction based on observability is adopted, and the matching method of measurement parameters "position + posture" is adopted, which makes it easy to realize deformation error correction and has a good integral smoothing effect on flexural motion noise and measurement noise of inertial device. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is a schematic diagram of the overall process of the present invention. DETAILED DESCRIPTION

[0053] The present invention will be further described below with reference to specific embodiments. The illustrative embodiments and descriptions of the present invention are used to explain the present invention but are not intended to limit the present invention.

[0054] Example: Figure 1 A high-precision in-situ calibration method for inertial dynamic posture based on relative navigation is shown, including an inertial dynamic posture reference benchmark, optical-based relative navigation, and an in-situ transfer calibration method. The inertial dynamic posture reference benchmark includes a high-precision optical gyroscope, an accelerometer, and a GNSS. The high-precision optical gyroscope is used to provide high-precision position information, the accelerometer is used to provide velocity information, and the GNSS is used to provide attitude and time reference information; the optical-based relative navigation includes optical-based deformation measurement and sub-IMU posture solution, and the optical-based relative navigation Kalman filtering method is used to provide sub-IMU posture information; the in-situ transfer calibration method includes an observability adjustment error correction factor algorithm and an observability-based adaptive feedback correction method.

[0055] The optical-based relative navigation Kalman filter method includes the following steps:

[0056] S1. Estimate the baseline length error between the master and sub-IMUs, as well as the sub-IMU position, velocity, attitude, and inertial device errors. Feedback and correct the estimated errors to obtain high-precision sub-IMU real-time navigation results and the relative spatial relationship between the master and sub-IMUs.

[0057] S2. The state variable X in the Kalman filter model has a total of 45 dimensions, including the misalignment angles of east, north, and celestial directions. Velocity error δV in the east, north and celestial directions E , δV N , δV u , latitude, longitude, and altitude errors δL, δλ, δH, and gyroscope constant drift errors ε in the x, y, and z axes x , ε y , ε z , x, y, z axis accelerometer constant drift error x, y, z axis arm length error Δr x , Δr y , Δr z ;Installation error;

[0058] S3. The quantity measurement Z in the Kalman filter model is the velocity error and position error compensated by a multi-stage lever arm, which consists of a rigid lever arm between the GPS and the main IMU and a flexible lever arm between the main and sub-IMUs.

[0059] The observability adjustment error correction factor algorithm includes the following steps:

[0060] S1. Establish system state equation and measurement equation;

[0061] S2. Establishing a position measurement information correction equation;

[0062] S3. Correct the attitude measurement information.

[0063] The system state equation is: Where X is the system state vector, F is the system state matrix, and W is the system noise matrix;

[0064]

[0065]

[0066] in, They are heading angle error, pitch angle error, and roll angle error; δv E ,δv N ,δv UThey are the eastward velocity error, northward velocity error, and celestial velocity error; δL, δλ, and δh are the latitude error, longitude error, and altitude error respectively; ε gx , ε gy , ε gz is the gyro constant drift; ε mx , ε my , ε mz is the gyro first-order Markov process drift; δK gx , δK gy , δK gz is the gyro scale factor error; δW yz , δW zy , δW xz , δW zx , δW xy , δW yx is the gyro installation error; is the addition of a random constant bias; is the drift of the additive first-order Markov process; δK ax , δK ay , δK az is the error of the scale factor; δA yz , δA zy , δA xz , δA zx , δA xy , δA yx is the installation error; θ x ,θ y ,θ z is the flexible deformation angle; is the flexible deformation angular rate.

[0067] Due to dynamic deformation, there is a time-varying complex relative motion between the nodes. The relative position and attitude relationship between the main IMU and the sub-IMU changes in real time, resulting in inaccurate position and attitude measurement information in the measurement equation, thereby restricting the transfer alignment accuracy. The deformation displacement measured by the optical sensor and dynamic deformation angle The position and attitude measurement information of the main system can be corrected to obtain a more accurate subsystem transfer alignment measurement Z = [δψ δθ δγ δL δλ δh] T .

[0068] The measurement equation is: Z = HX + v, where H is the measurement matrix and v is the measurement noise;

[0069]

[0070]

[0071]

[0072] Where, is the attitude matrix of the inertial pose reference system.

[0073] The position measurement information correction equation is:

[0074]

[0075] Where, L mc ,λ mc 、h mc They are the latitude, longitude and altitude information of the main system respectively; L sc ,λ sc and h sc are the latitude, longitude and altitude information before transmission to the subsystem, R m is the meridian radius of the Earth, R n is the main curvature radius of the Maoyou circle;

[0076]

[0077] Where r is the flexible arm between the inertial pose reference system and the MEMS inertial pose measurement system; r0 is the initial fixed arm between the master and the slave; and Δr is the time-varying arm error between the master and the slave caused by the flexible deformation.

[0078] The steps to correct the attitude measurement information are:

[0079] S1. Determine the attitude error angle μ, μ=[μ x , μ y , μ z ] T ; The attitude error angle μ includes the fixed installation error angle ρ and the flexible time-varying error angle σ; ρ=[ρ x ,ρ y ,ρ z ] T ,σ=[σ x ,σ y ,σ z ] T The fixed installation error angle ρ is obtained by visual measurement after the system is installed to complete the calibration of the inertial reference system, and the flexible deformation angle σ is measured using an optical sensor.

[0080] The attitude relationship between S2, fixed installation error angle ρ and flexible time-varying error angle σ is:

[0081]

[0082] Where, is the attitude matrix of the sub-IMU, It is the attitude error caused by the misalignment angle of the sub-IMU, which is represented as the attitude matrix of the main IMU, and is the error matrix caused by the attitude error angle;

[0083] S3. When the attitude error angle between the main and sub-systems is a small angle:

[0084]

[0085] In the formula, μ = ρ + σ, and φx and μx are the skew-symmetric matrices composed of the misalignment angle φ and the error angle μ respectively;

[0086] S4. Expand the formula in S3, omit the second-order small quantities, and take the approximation to obtain:

[0087]

[0088] Let δψ′ = ψ - ψ m , δθ′ = θ - θ m , δγ′ = γ - γ m , and thus obtain:

[0089]

[0090] <同

[0091]

[0092] In the formula, is the value of the i-th row and j-th column of the main POS attitude matrix, and φ E , φ N , φ U are the misalignment angles in the east, north, and up directions;

[0093] S5. Expand both sides of the above formula according to the Taylor series and ignore the small quantities above the second order to obtain the corrected attitude measurement formula:

[0094]

[0095]

[0096] [[ID=多62]]

[0097] The adaptive feedback correction method based on observability includes:

[0098] S1. Use the instantaneous observability model to establish the observability factor;

[0099] S2. Adjust the state variables according to this factor;

[0100] S3. Improve the observability of the state variables, perform filtering estimation, and feedback compensation. Note: There seems to be a duplicate tag in the original text, and the translation of <同 is just a guess as it's not clear what "同" means here. Also, there might be some formatting or content issues in the original text that could affect the accuracy of the translation. Please check and correct if possible.

Claims

1. A high-precision inertial dynamic posture in-situ calibration method based on relative navigation, characterized in that: The invention includes an inertial dynamic posture reference benchmark, optical-based relative navigation, and an in-situ transfer calibration method. The inertial dynamic posture reference benchmark includes a high-precision optical gyroscope, an accelerometer, and a GNSS. The high-precision optical gyroscope is used to provide high-precision position information, the accelerometer is used to provide velocity information, and the GNSS is used to provide attitude and time reference information. The optical-based relative navigation includes optical-based deformation measurement and sub-IMU pose solution, and the optical-based relative navigation Kalman filtering method is used to provide sub-IMU pose information; The in-situ transfer calibration method includes an observability adjustment error correction factor algorithm and an observability-based adaptive feedback correction method; The observability adjustment error correction factor algorithm includes the following steps: S1. Establish system state equation and measurement equation; S2. Establishing a position measurement information correction equation; S3. Correct the attitude measurement information; The position measurement information correction equation is: ； Where, They are the latitude, longitude and altitude information of the main system respectively; Transmit the latitude, longitude and altitude information before alignment to the subsystem respectively. is the meridian radius of the Earth, is the main curvature radius of the Maoyou circle; ； Where r is the flexible arm between the inertial pose reference system and the MEMS inertial pose measurement system; r0 is the initial fixed arm between the main system and the subsystem; and Δr is the time-varying arm error between the main system and the subsystem caused by flexible deformation.

2. A high-precision inertial dynamic posture in-situ calibration method based on relative navigation according to claim 1, characterized in that: The optical-based relative navigation Kalman filtering method comprises the following steps: S1. Estimate the baseline length error between the master and sub-IMUs, as well as the sub-IMU position, velocity, attitude, and inertial device errors. Feedback and correct the estimated errors to obtain high-precision sub-IMU real-time navigation results and the relative spatial relationship between the master and sub-IMUs. S2. The state variable X in the Kalman filter model has a total of 45 dimensions, including the misalignment angles of east, north, and celestial directions. , velocity error in the east, north, and celestial directions , latitude, longitude, and altitude errors Axial gyroscope constant drift error Axial accelerometer constant drift error Axial arm length error ;Installation error; S3. The quantity measurement Z in the Kalman filter model is the velocity error and position error compensated by a multi-stage lever arm, which consists of a rigid lever arm between the GPS and the main IMU and a flexible lever arm between the main and sub-IMUs.

3. The high-precision inertial dynamic posture in-situ calibration method based on relative navigation according to claim 2, characterized in that: The system state equation is: , where X is the system state vector, F is the system state matrix, and W is the system noise matrix; ； in, They are heading angle error, pitch angle error, and roll angle error; They are eastward velocity error, northward velocity error, and celestial velocity error; They are latitude error, longitude error, and altitude error; is the gyro constant drift; is the gyro first-order Markov process drift; is the gyro scale factor error; is the gyro installation error; is the addition of a random constant bias; is the drift of the additive first-order Markov process; is the error of the added scale factor; It is the addition of installation error; is the flexible deformation angle; is the flexible deformation angular rate.

4. A high-precision inertial dynamic posture in-situ calibration method based on relative navigation as claimed in claim 3, characterized in that: The measurement equation is: , where H is the measurement matrix and v is the measurement noise; ； Where, is the attitude matrix of the inertial pose reference system.

5. The high-precision inertial dynamic posture in-situ calibration method based on relative navigation according to claim 4, characterized in that: The steps to correct the attitude measurement information are: S1. Determine the attitude error angle μ, ; The attitude error angle μ includes the fixed installation error angle ρ and the flexible time-varying error angle σ; The fixed installation error angle ρ is obtained by visual measurement after the system is installed to complete the calibration of the inertial reference system, and the flexible deformation angle σ is measured using an optical sensor. The attitude relationship between S2, fixed installation error angle ρ and flexible time-varying error angle σ is: ； Where, is the attitude matrix of the sub-IMU, is the attitude error caused by the sub-IMU misalignment angle, Represents the attitude matrix of the main IMU, is the error matrix caused by the attitude error angle; S3. When the attitude error angle between the master and subsystems is small: ； Where, Misalignment angle and the skew-symmetric matrix composed of the error angle μ; S4. Expand the formula in S3 to omit the second-order small quantity and take the approximate value: ； make , thus obtaining: ； Where, is the value of the i-th row and j-th column of the main POS posture matrix, The misalignment angles for east, north and celestial directions; S5. Expand both sides of the above equation according to Taylor series and ignore small quantities above the second order to obtain the corrected attitude measurement formula: 。 6. The high-precision inertial dynamic posture in-situ calibration method based on relative navigation according to claim 5, characterized in that: The observability-based adaptive feedback correction method includes: S1. Use the instantaneous observability model to establish the observability factor; S2, adjust the state variables according to the factor; S3. The observability of state variables is improved, filtering estimation and feedback compensation are performed.

Images