Automatic alignment method for arbitrarily installed vehicle-mounted strapdown inertial navigation system

Abstract

The invention discloses an automatic alignment method for an arbitrarily-mounted vehicle-mounted strapdown inertial navigation system, which comprises the following steps of: 1, detecting the static, straight and turning motion states of a vehicle based on data of an accelerometer and a gyroscope and speed information calculated by satellite navigation; and step 2, if the vehicle is in a static state, executing static alignment. Calculating a horizontal attitude angle, and estimating zero offset of the gyroscope and the accelerometer; and step 3, if the vehicle is in a moving state, executing dynamic alignment. And calculating an initial course angle, executing GNSS / INS loose combination filtering to carry out attitude angle convergence, and calculating an accurate installation angle. According to the automatic alignment method disclosed by the invention, the misalignment angle between the inertial device and the vehicle system can be automatically estimated, and accurate alignment of the strapdown inertial navigation system can be realized.

Description

Technical Field

[0001] The invention relates to the technical field of navigation and positioning, in particular to an automatic alignment method for an arbitrarily installed vehicle-mounted strapdown inertial navigation system. Background Art

[0002] With the development of intelligent navigation technology, the application of vehicle-mounted strapdown inertial navigation systems in the fields of autonomous driving and high-precision vehicle navigation has become more and more extensive. However, the strapdown inertial navigation system has high requirements for the accuracy and stability of initial alignment, especially in various complex environments and unknown installation angles. It is necessary to solve the problems of insufficient alignment accuracy and sensitivity to installation angles of the inertial navigation system.

[0003] Traditional strapdown inertial navigation initial alignment methods usually rely on the predetermined installation direction of the vehicle or manual adjustment to ensure alignment accuracy, but this increases the installation complexity and operating costs of the system and is difficult to meet the requirements of modern vehicle-mounted systems for installation flexibility and accuracy. Especially during the dynamic startup and operation of the vehicle, the sensors in the inertial navigation system are susceptible to body vibration and environmental disturbances, resulting in a decrease in alignment accuracy. In addition, the traditional alignment method has a slow convergence speed and is difficult to provide a reliable attitude and heading reference when the vehicle is started quickly, limiting the application scope of the strapdown inertial navigation system in dynamic environments. The current multi-source combined alignment method cannot automatically estimate the misalignment angle between the inertial navigation device and the vehicle coordinate system, and still relies on precise manual installation or prior calibration. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide an automatic alignment method for an arbitrarily installed vehicle-mounted strapdown inertial navigation system, which can automatically estimate the installation angle between the inertial device and the vehicle system and realize the precise alignment of the strapdown inertial navigation system, thereby reducing the restrictions on equipment installation and improving the alignment accuracy and automation level.

[0005] In order to solve the above technical problems, the present invention provides an automatic alignment method for an arbitrarily installed vehicle-mounted strapdown inertial navigation system, comprising the following steps:

[0006] Step S1, detecting the vehicle's motion states such as stationary, straight-moving and turning by calculating speed information using accelerometer, gyroscope data and satellite navigation;

[0007] Step S2: If the vehicle is detected to be stationary, static alignment is performed. The horizontal attitude angle is calculated and the gyroscope and accelerometer zero bias are estimated;

[0008] Step S3: If the vehicle is detected to be in motion, dynamic alignment is performed, the initial heading angle is calculated, the GNSS / INS loose combination filter converges the attitude angle, and the installation angle is calculated.

[0009] Preferably, the vehicle motion state detection method described in step S1 is to utilize the output information of inertial sensors (accelerometers and gyroscopes) to determine the vehicle's motion state, including stationary, straight-moving or turning, based on the changing relationship between the vehicle motion state and the inertial sensor data, and in combination with the vehicle speed information calculated by the satellite navigation system.

[0010] Preferably, the calculation formula of the horizontal attitude angle described in step S2 is:

[0011]

[0012] In the formula, are the acceleration output after smoothing in the current inertial navigation measurement system (m system, right front upper coordinate system); θ x is the roll angle in the m system, θ y is the pitch angle in the m system.

[0013] Preferably, the gyroscope zero bias estimation method in step S2 takes into account that when the vehicle is stationary, the angular velocity of the carrier system and the navigation system relative to the earth system is theoretically 0, so the angular velocity of the carrier system (b system, right front upper coordinate system) relative to the navigation system (n system, northeast celestial coordinate system) is calculated as follows: Used as gyro bias compensation.

[0014] Preferably, the estimation of the accelerometer zero bias in step S2 adopts static combined filtering, and its filtering observation model is:

[0015]

[0016] Where z(t) is the observation vector; Represents the three-dimensional velocity vector output by the INS system; ε v represents the speed measurement white noise; Represents the real-time output of the gyroscope; ε g represents the gyro measurement white noise; is the combined navigation state vector, δr n =[δE δN δU] represents the error in the east, north and sky directions respectively, δv n =[δv E δv N δv U ] represent the velocity errors in the east, north and celestial directions respectively, φ=[φ E φ N φ U ] represent the pitch angle, roll angle and heading angle errors respectively, Indicates the zero bias of the gyroscope in the x, y, and z directions, Indicates the zero bias of the accelerometer in the x, y, and z directions.

[0017] Preferably, the calculation formula of the initial heading angle described in step S3 is:

[0018]

[0019] Where ψ is the true heading of the current vehicle, v GE is the eastward velocity calculated by satellite navigation, v GN It is the north velocity calculated by satellite navigation.

[0020] Preferably, the GNSS / INS loose combined filtering is performed in step S3 to perform attitude angle convergence, and the filtering observation model is:

[0021]

[0022] In the formula, represents the three-dimensional position vector output by the INS system, represents the three-dimensional position vector output by the GNSS system, ε r represents the position measurement white noise; represents the three-dimensional velocity vector output by the INS system, represents the three-dimensional velocity vector output by the GNSS system, ε v represents the speed measurement white noise.

[0023] Preferably, the calculation formula of the installation angle in step S3 is:

[0024]

[0025] In the formula, is the installation matrix, i.e., the transformation matrix between the inertial navigation measurement system (m system) and the vehicle carrier coordinate system (b system); θ x ,θ y ,θ z They are the installation angles in the x, y and z directions, and the unit is rad.

[0026] The beneficial effects of the present invention are as follows: automatic alignment in static and dynamic states is achieved, and by automatically estimating the installation angle between the inertial device and the vehicle system, complicated manual installation steps are eliminated, greatly improving the practicality and convenience of the system. In the static state, gravity is used to assist in initial attitude calculation and zero bias estimation; in the dynamic state, high-precision alignment is achieved through vehicle motion information and external navigation data, which significantly improves the overall alignment accuracy and effectively reduces the impact of initial errors on subsequent navigation. The present invention can adapt to a variety of installation environments and meet the needs of vehicle-mounted applications. At the same time, it simplifies the deployment process, reduces time and cost, enhances the stability and reliability of the system in complex environments, and further enhances the application value of vehicle-mounted navigation systems in fields such as autonomous driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a schematic diagram of the method flow of the present invention.

[0028] Figure 2 This is a diagram showing the installation angle results for implementing the present invention.

[0029] Figure 3 A posture output diagram is provided for implementing the present invention. DETAILED DESCRIPTION

[0030] like Figure 1 As shown, a method for automatically aligning an arbitrarily installed vehicle-mounted strapdown inertial navigation system comprises the following steps:

[0031] Step S1: Detect the vehicle's motion states such as stationary, straight-moving and turning through the accelerometer, gyroscope data and the speed calculated by satellite navigation;

[0032] Specifically, an embodiment of the vehicle stationary motion state detection is to perform detection based on the raw data output of the gyroscope and the accelerometer, and the detection model is:

[0033]

[0034] In the formula, T(f k )T(ω k ) is the test statistic of acceleration and gyroscope modulus at epoch k, N is the number of samples in the time window; λ A , G are the test thresholds of acceleration and angular velocity modulus respectively, and 2 to 5 times the statistical mean of the original data of the accelerometer and gyroscope 2 to 5 minutes before the vehicle starts is taken as the test threshold. If the above formulas are satisfied at the same time, it means that the current vehicle is in a stationary state, otherwise it is in a moving state.

[0035] Specifically, an embodiment of the vehicle turning motion state detection is to comprehensively judge the vehicle motion state according to the horizontal speed calculated by the satellite navigation output and the output change of the gyroscope z-axis, and its detection model is:

[0036]

[0037] In the formula, is the heading change rate, ω zi is the gyroscope z-axis output of the i-th epoch, Δt is the IMU acquisition period, and T is the integrated navigation period; is the horizontal speed output by the current satellite guidance, v GE , v GN They are the east and north speeds output by the satellite navigation system. If the above formula is met, it means that the vehicle is currently turning, otherwise it is in a straight state.

[0038] Step S2: If the vehicle is detected to be stationary, perform static alignment, calculate the horizontal attitude angle, and estimate the gyroscope and accelerometer zero bias;

[0039] Specifically, the horizontal attitude angle is calculated by smoothing the output data of the accelerometer maintained at a stationary state for more than 1 second, and the calculation formula is:

[0040]

[0041] In the formula, are the acceleration output after smoothing in the current inertial navigation measurement system (m system, right front upper coordinate system); θ x is the roll angle in the m system, θ y is the pitch angle in the m system.

[0042] Specifically, considering that the angular velocity of the load system and the navigation system relative to the earth system is theoretically 0 when the vehicle is stationary, the gyroscope zero bias estimation method is to output the angular velocity of the load system (b system, right front upper coordinate system) relative to the navigation system (n system, northeast celestial coordinate system), that is, As gyro bias compensation. The reasoning formula is as follows:

[0043]

[0044] In the formula, is the current angular rate output, From the current posture matrix And the satellite navigation position and speed are obtained Calculated. are the Earth's rotation angular rate and the navigation system's rotation angular velocity relative to the Earth's system in the navigation system, are the angular velocities of the carrier system and the navigation system relative to the earth system under the carrier system.

[0045] Specifically, the accelerometer zero bias is estimated after the horizontal attitude angle and gyroscope zero bias are estimated. Static combined filtering is used for estimation, and zero velocity and zero angular velocity are used as observations. The filtering observation model is:

[0046]

[0047] Where z(t) is the observation vector; Represents the three-dimensional velocity vector output by the INS system; ε v represents the speed measurement white noise; Represents the real-time output of the gyroscope; ε g represents the gyro measurement white noise; is the combined navigation state vector, δr n=[δE δN δU] represents the error in the east, north and sky directions respectively, δv n =[δv E δv N δv U ] represent the velocity errors in the east, north and celestial directions respectively, φ=[φ E φ N φ U ] represent the pitch angle, roll angle and heading angle errors respectively, Indicates the zero bias of the gyroscope in the x, y, and z directions, Indicates the zero bias of the accelerometer in the x, y, and z directions.

[0048] Step S3: If the vehicle is detected to be in motion, dynamic alignment is performed, the initial heading angle is calculated, the GNSS / INS loose combined filtering is performed to converge the attitude angle, and the installation angle is calculated.

[0049] Specifically, the initial heading angle is calculated using the eastward speed and northward speed calculated by satellite navigation when the vehicle's horizontal speed is detected to be greater than 2 m / s. The calculation formula is:

[0050]

[0051] Where ψ is the true heading of the current vehicle, v GE is the eastward velocity calculated by satellite navigation, v GN It is the north velocity calculated by satellite navigation.

[0052] Specifically, the GNSS / INS loose combined filtering is performed to converge the attitude angle after the initial heading angle calculation is completed and the attitude angle is in the turning state. The filtering model is:

[0053]

[0054] In the formula, represents the three-dimensional position vector output by the INS system, represents the three-dimensional position vector output by the GNSS system, ε r represents the position measurement white noise; represents the three-dimensional velocity vector output by the INS system, represents the three-dimensional velocity vector output by the GNSS system, ε v represents the speed measurement white noise.

[0055] Specifically, whether the attitude angle has completed convergence is determined based on the number of consecutive turns. If loose combination filtering is completed under 20 consecutive turns, the attitude angle is considered to have completed convergence.

[0056] Specifically, the installation angle is calculated based on the installation matrix, and the calculation formula is:

[0057]

[0058] In the formula, is the installation matrix, i.e., the transformation matrix between the inertial navigation measurement system (m system) and the vehicle carrier coordinate system (b system); θ x ,θ y ,θ z They are the installation angles in the x, y and z directions, and the unit is rad.

[0059] Specifically, the calculation formula of the installation matrix is:

[0060]

[0061] In the formula, is the rotation matrix between the inertial navigation measurement system (m system) and the inertial navigation measurement system (m1 system) after horizontal rough alignment, which can be calculated according to the attitude angle [θ x θ y 0] calculated; is the rotation matrix between the inertial navigation measurement system (m1 system) and the carrier coordinate system (b system) after horizontal alignment, which can be calculated according to the attitude angle [0 θ p θ y ] is calculated, where θ p is the pitch angle, θ y When the vehicle is detected to be in a straight-moving state with a speed greater than 10 m / s, the pitch angle and heading angle are calculated using the filtering results. The calculation formula is:

[0062]

[0063] In the formula, v E ,v N , v U is the velocity component in the filtered northeast celestial coordinate system.

[0064] The embodiment of the present invention provides a set of test results. The IMU sensor frequency used in this test data is 100 Hz, and the performance parameters are shown in Table 1.

[0065] Table 1 IMU performance parameters

[0066]

[0067] The low-precision IMU is installed arbitrarily in a sports car to collect data, and Matlab software is used to perform arbitrary installation angle calculation and loose combined navigation solution. Figure 2 , Figure 3They are respectively real-time installation angle output and attitude angle output. In the figure, stages I, II, III, and IV correspond to static horizontal alignment, dynamic heading coarse alignment, dynamic combination convergence, and installation angle calculation links, respectively. After the end of stage IV, the system smoothly transitions to the traditional loose combination navigation solution stage. Table 2 shows the comparison between the installation angle results calculated by the present invention and the actual results. The significance of the test results lies in that the present invention can not only accurately estimate any installation angle of a low-precision IMU, but also maintain high efficiency and high precision in the process of calculating the installation angle, which is crucial to improving the reliability and accuracy of the navigation system. In addition, the present invention has strong adaptability and is compatible with IMUs of different accuracy levels, so that it can accurately estimate the installation angle in various environments.

[0068] Table 2 Comparison of installation angle results

[0069]

Claims

1. A method for automatically aligning an arbitrarily installed vehicle-mounted strapdown inertial navigation system, characterized in that: The steps include: Step S1, detecting the vehicle's motion states such as stationary, straight-moving and turning by calculating speed information using accelerometer, gyroscope data and satellite navigation; Step S2: If the vehicle is detected to be stationary, static alignment is performed. The horizontal attitude angle is calculated and the gyroscope and accelerometer zero bias are estimated; Step S3: If the vehicle is detected to be in motion, dynamic alignment is performed, the initial heading angle is calculated, the GNSS / INS loose combination filter converges the attitude angle, and the installation angle is calculated.

2. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily according to claim 1, characterized in that: The vehicle motion state detection method described in step S1 is: using the output information of the inertial sensor (accelerometer and gyroscope), according to the changing relationship between the vehicle motion state and the inertial sensor data, and combining the vehicle speed information calculated by the satellite navigation system, to judge the vehicle's motion state, including stationary, straight or turning.

3. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily according to claim 1, characterized in that: The calculation formula of the horizontal attitude angle described in step S2 is: In the formula, are the acceleration output after smoothing in the current inertial navigation measurement system (m system, right front upper coordinate system); θ x is the roll angle, θ y is the pitch angle.

4. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily according to claim 1, characterized in that: Considering that when the vehicle is stationary, the angular velocity of the carrier system and the navigation system relative to the earth system is theoretically 0, the method for estimating the gyroscope zero bias in step S2 is to output the angular velocity of the carrier system (b system, right front upper coordinate system) relative to the navigation system (n system, northeast sky coordinate system), that is, Used as gyro bias compensation.

5. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily according to claim 1, characterized in that: The estimation of the accelerometer zero bias in step S2 is performed by using a static combined filter, and its filter observation model is: Where Z(t) is the observation vector; Represents the three-dimensional velocity vector output by the INS system; ε v represents the speed measurement white noise; Represents the real-time output of the gyroscope; ε g represents the gyro measurement white noise; is the combined navigation state vector, δr n =[δE δN δU] represents the error in the east, north and sky directions respectively, δv n =[δv E δv N δv U ] represent the velocity errors in the east, north and celestial directions respectively, φ=[φ E φ N φ U ] represent the pitch angle, roll angle and heading angle errors respectively, Indicates the zero bias of the gyroscope in the x, y, and z directions, Indicates the zero bias of the accelerometer in the x, y, and z directions.

6. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily in a vehicle according to claim 1, characterized in that: The calculation formula of the initial heading angle described in step S3 is: Where ψ is the true heading of the current vehicle, v GE is the eastward velocity calculated by satellite navigation, v GN It is the north velocity calculated by satellite navigation.

7. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily in a vehicle according to claim 1, characterized in that: The GNSS / INS loose combined filtering described in step S3 is performed to converge the attitude angle, and the filtering observation model is: In the formula, represents the three-dimensional position vector output by the INS system, represents the three-dimensional position vector output by the GNSS system, ε r represents the position measurement white noise; represents the three-dimensional velocity vector output by the INS system, represents the three-dimensional velocity vector output by the GNSS system, ε v represents the speed measurement white noise.

8. The method for automatically aligning a strapdown inertial navigation system installed arbitrarily in a vehicle according to claim 1, characterized in that: The calculation formula of the installation angle in step S3 is: In the formula, is the installation matrix, i.e., the transformation matrix between the inertial navigation measurement system (m system) and the vehicle carrier coordinate system (b system); θ x ,θ y ,θ x They are the installation angles in the x, y and z directions, and the unit is rad.

Images