A method for vehicle-mounted SINS alignment while driving on uneven road surfaces

Abstract

This invention provides a method for onboard SINS alignment in undulating road environments. Specifically, the method comprises the following steps: Step 1: Acquiring IMU output data during vehicle movement; calculating the height difference of the undulating road surface between two adjacent moments based on the acquired IMU data and onboard barometer data; and using this height difference to calculate the vehicle's azimuth velocity. Step 2: Providing the height information calculated by the onboard barometer to a pseudo-satellite receiver for auxiliary positioning and speed measurement, thereby obtaining the vehicle's azimuth velocity through the pseudo-satellite receiver. Step 3: Fusing the azimuth velocities from Steps 1 and 2 to obtain a fused azimuth velocity. Step 4: Performing initial fine alignment of the onboard SINS based on the fused azimuth velocity using an unscented Kalman filter. This invention effectively compensates for errors in the height and azimuth velocity calculated by the pseudo-satellite receiver, resulting in more accurate position and velocity information.

Description

Technical Field

[0001] This invention belongs to the field of navigation control technology, and particularly relates to a method for vehicle-mounted SINS alignment while driving in environments with uneven road surfaces. Background Technology

[0002] In today's information age, the speed of initial alignment of vehicle-mounted SINS (Straightthrough Inertial Navigation System) is closely related to the effectiveness of special vehicles. The environment in which special vehicles operate exhibits unprecedented complexity and variability. Technologies such as laser interference and electromagnetic interference can easily lead to satellite denial environments, while in the field, the uneven terrain adds significant uncertainty to mission execution. To ensure navigation performance, special vehicles need to achieve high-precision in-transit initial alignment on uneven terrain under satellite denial conditions, thereby successfully completing their corresponding functional tasks.

[0003] Pseudo-satellites can provide satellite-like positioning signals within a localized area. Their signal coverage can meet the needs of the vehicle's driving environment, and effective signal transmission can be achieved through reasonable layout and configuration. Vehicle-mounted SINS (Sensitive Insulation System) is autonomous; combining it with pseudo-satellite signals allows for the complementary advantages of both, improving system reliability and adaptability. However, when using pseudo-satellites alone for assisted positioning, an excessively large VDOP (Vertical Accuracy Factor) can lead to low accuracy in the altitude and azimuth velocity calculated by the pseudo-satellite receiver, thus affecting the initial alignment accuracy of the vehicle-mounted SINS. Summary of the Invention

[0004] Purpose of the invention: In order to solve the problems existing in the prior art, the present invention provides a method for vehicle-mounted SINS alignment in the event of uneven road surfaces.

[0005] Technical solution: This invention discloses a method for vehicle-mounted SINS alignment in undulating road conditions, specifically including the following steps:

[0006] Step 1: Collect the output data of the IMU (Inertial Measurement Unit) during vehicle movement, and calculate the road height difference Δh between two adjacent moments using the onboard barometer based on the collected data. p The vehicle's axial velocity v was calculated using an onboard barometer based on the road surface height difference. p,U ;

[0007] Step 2: Provide the altitude information calculated by the vehicle's barometer to the pseudo-satellite receiver for auxiliary positioning and speed measurement, thereby calculating the vehicle's celestial velocity v using the pseudo-satellite receiver. m,U ;

[0008] Step 3: v p,U and v m,UThe fusion yields the fused celestial velocity v. m,U ;

[0009] Step 4: Based on the fused celestial velocity v m,U Unscented Kalman filtering was used to perform initial fine alignment of the vehicle-mounted SINS.

[0010] Furthermore, in step 1, the height difference is calculated using the following formula:

[0011]

[0012] Where R is the gas constant, T is the thermodynamic temperature, g is the gravitational acceleration, p is the atmospheric pressure at the road surface height, and t represents the time.

[0013] The vehicle's onboard barometer calculates the vehicle's axial velocity v using the following formula. p,U :

[0014]

[0015] Where Δt is the time interval between the current time and the previous time.

[0016] Furthermore, it is characterized by:

[0017] Step 2.1: Establish the following distance equation between the receiver and the pseudo-satellite:

[0018]

[0019] Where m represents the receiver, r jm The distance between the receiver and the j-th pseudo-satellite is measured, where j = 1, 2, 3, ..., n, and n represents the total number of pseudo-satellites. m y m (x) represents the receiver's eastward and northward positions in the Earth-centered, Earth-fixed coordinate system. sj ,y sj ,z sj Let be the coordinates of the j-th pseudo-satellite in the geocentric-ground-fixed coordinate system, and h be the coordinates of the j-th pseudo-satellite. v Let x be the vehicle height; solve the equation using the least squares method to obtain (x) m y m );

[0020] Step 2.2: Construct the following Doppler frequency shift equation:

[0021]

[0022] Among them, f oj Let f be the frequency of the signal transmitted by the j-th pseudo-satellite. j Let v be the frequency of the signal received by the receiver from the j-th pseudosatellite.x v y and v z Let v be the velocity components of the receiver in the geocentric Earth-fixed coordinate system along the x, y, and z axes; the Doppler frequency shift equation is calculated using the least squares method to obtain v. x v y and v z ;

[0023] Step 2.3: Based on the calculated v x v y and v z Calculate the receiver's velocity in the local horizontal coordinate system:

[0024]

[0025] Among them, v m,E v m,N and v m,U These represent the receiver's eastward, northward, and upward velocities in the local horizontal coordinate system, respectively; the expression for M is... Among them, L m and λ m These are the latitude and longitude of the receiver's location, respectively.

[0026] Furthermore, step 4 specifically involves: applying a Kalman filter to v p,U and v m,U For fusion, the state equation is:

[0027]

[0028] Where X1(t) is the state vector, and the state variable is v p,U and v m,U The error between them Let X(t) be the first derivative of X(t), A(t) be the state transition matrix, M(t) be the system noise distribution matrix, and N(t) be the system noise vector.

[0029] The measurement equation is:

[0030] Z(t) = v p,U -v m,U

[0031] Where Z(t) is the measurement vector.

[0032] Furthermore, step 4 specifically involves:

[0033] Step 4.1: Construct the nonlinear error model of the onboard SINS during transit:

[0034]

[0035] Where φ is the inertial navigation system's misalignment error matrix in the northeast-northeast coordinate system. C is the first derivative of φ. ω I represents the Euler angle matrix; I denotes the identity matrix. Let be the attitude transfer matrix from the n-frame to the n′-frame, where the n-frame is the navigation coordinate system and the n′-frame is the computational navigation coordinate system. for The measurement quantity, Let δ represent the projection of the rotational angular velocity of the n-frame relative to the i-frame onto the n-frame, where δ denotes the error. for The error, Let ε be the attitude transfer matrix between the b-frame and the n′-frame, where b-frame is the carrier coordinate system, ε is the gyroscope zero bias, and ε' is the attitude transfer matrix between the b-frame and n′-frame. b v is the projection of the gyroscope's zero bias into the b-frame. n Let δv be the velocity of the inertial navigation system. n For speed error, The first derivative of the velocity error. The specific force output by the accelerometer. This is the error in the Earth's rotational angular velocity. This is the Earth's rotational angular velocity. for The measurement quantity, The angular velocity caused by the motion of the carrier. The error in angular velocity caused by the motion of the carrier. for The quantity to be measured; δg n The vector error is the gravitational acceleration; ▽ represents the accelerometer zero bias. b Let L be the projection of the accelerometer zero bias in the b-frame, and let L, λ, and h be the latitude, longitude, and altitude of the inertial navigation system in the northeast-northeast coordinate system, respectively; δL, δλ, and δh are the latitude error, longitude error, and altitude error of the inertial navigation system in the northeast-northeast coordinate system, respectively. The first derivative of δL The first derivative of δλ v is the first derivative of δh; E v N v U These represent the inertial navigation system's eastward, northward, and celestial velocities in the northeast-sky coordinate system, respectively; R M R is the radius of the meridian circle. N The radius of the circle is the area between the east and west.

[0036] Step 4.2: Based on the SINS nonlinear error model in Step 4.1, establish the system state equation and measurement equations for the unscented Kalman filter:

[0037]

[0038] Where X(t) is the state variable, Let Z1(t) be the first derivative of X(t), and Z1(t) be the measurement. X(t) = [φ E φ N φ U δv E δv N δv U δLδλδhε X ε Y ε Z ▽ X ▽ Y ▽ Z ] T , where φ E φ N φ U These represent the components of the inertial navigation misalignment angle error in the east-west, north-north, and celestial directions in the northeast-sky coordinate system, ε. X ε Y ε Z To achieve zero bias of the three-axis gyroscope in three directions within the carrier coordinate system, ▽ X 、▽ Y 、▽ Z Let F(t) be the zero bias of the triaxial acceleration in the three directions of the carrier coordinate system; F(t) be the state one-step transition matrix; G(t) be the system noise distribution matrix; W(t) be the system noise vector; and H be the system noise vector. v (t) is the velocity measurement matrix, H p (t) is the position measurement matrix, V v (t) is the velocity measurement noise vector, V p (t) is the position measurement noise vector.

[0039] Beneficial effects: By introducing an on-board barometer, this invention can effectively compensate for the errors in altitude and celestial velocity calculated by the pseudo-satellite receiver, obtaining more accurate position and velocity information. Furthermore, by using unscented Kalman filtering to process the nonlinear error model of the SINS during vehicle movement, the mean and covariance of the system can be estimated more accurately, thereby achieving high-precision on-the-go initial alignment of the on-board SINS. Attached Figure Description

[0040] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0041] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0042] This invention provides a method for vehicle-mounted SINS alignment in undulating road conditions, the specific process of which is as follows: Figure 1 As shown:

[0043] Step 1: Collect IMU output data during vehicle movement, and simultaneously calculate altitude and yaw speed information using the onboard barometer. p,U .

[0044] Step 1.1: During vehicle movement, use the onboard barometer to calculate the current altitude difference Δh. p The formula is as follows:

[0045]

[0046] Where R is the gas constant, T is the thermodynamic temperature, g is the gravitational acceleration, p is the atmospheric pressure at the road surface height, and t represents the time.

[0047] Step 1.2: The vehicle-mounted barometer uses the altitude difference Δh... p The calculated celestial velocity v p,U The expression is as follows:

[0048]

[0049] Where Δt is the time interval between the current time and the previous time.

[0050] Step 2: Provide the altitude information calculated by the vehicle-mounted barometer to the pseudo-satellite receiver for auxiliary positioning and velocity measurement, and provide the celestial velocity v calculated by the pseudo-satellite receiver. m,U The axial velocity v calculated from the vehicle's barometer p,U To integrate.

[0051] Step 2.1: The pseudo-satellite receiver uses the vehicle-mounted barometer to calculate the altitude difference Δh. p For auxiliary positioning, let the coordinates of the j-th (j=1,2,3,…,n) pseudo-satellite in the geocentric-ground-fixed coordinate system be (x sj ,y sj ,z sj Let n be the total number of pseudo-satellites; then the distance r between receiver m and the j-th pseudo-satellite is measured by receiver m. jm The expression is as follows:

[0052]

[0053] Among them, (x m y m ) represents the receiver's eastward and northward positions in the geocentric coordinate system, h v For vehicle height;

[0054] For multiple pseudo-satellites, the following system of equations can be established:

[0055]

[0056] Solving the equation using the least squares method yields (x m y m ).

[0057] Step 2.2: The pseudo-satellite receiver performs velocity calculation. Assume f... j Let f be the frequency of the signal received by the receiver from the j-th pseudosatellite. oj Let be the frequency of the signal emitted by the j-th pseudo-satellite, and c be the speed of light. Then, according to the Doppler frequency shift principle, we can obtain:

[0058]

[0059] Among them, v x v y and v z Let x be the components of the receiver's velocity in the geocentric coordinate system along the x, y, and z axes.

[0060] For multiple pseudo-satellites, the following system of equations can be established:

[0061]

[0062] Solving the equation using the least squares method yields v x v y and v z .

[0063] The velocity of the receiver in the local horizontal coordinate system can be obtained using the following expression (where "local" refers to the coordinate system of the receiver's current location):

[0064]

[0065] Among them, v m,E v m,N and v m,U These represent the receiver's eastward, northward, and upward velocities in the local horizontal coordinate system, respectively; the expression for M is... Among them, L m and λ m These are the latitude and longitude of the receiver's location, respectively.

[0066] The axial velocity v obtained from the vehicle-mounted barometer p,U The celestial velocity v calculated by the pseudo-satellite receiver m,U Kalman filtering fusion is performed, and the upward velocity error δv is selected. pm,U Let X1(t) be the state variable; then the state equation is as follows:

[0067]

[0068] in, Let X(t) be the first derivative of X(t), A(t) be the state transition matrix, M(t) be the system noise distribution matrix, and N(t) be the system noise vector.

[0069] The axial velocity v obtained by using the vehicle-mounted barometer is selected. p,U The celestial velocity v calculated by the pseudo-satellite receiver m,U If the difference is taken as a measurement, then the measurement equation is as follows:

[0070] Z(t) = v p,U -v m,U =S U (t)X(t)+T U (t)

[0071] Among them, S U (t) is the celestial velocity measurement matrix, T U (t) represents the upward velocity measurement noise vector. After fusion using Kalman filtering, v can be obtained. K,U .

[0072] Step 3: Perform initial alignment using an alignment algorithm based on unscented Kalman filtering.

[0073] Step 3.1: Establish the nonlinear error model of the onboard SINS during travel:

[0074]

[0075] Where φ is the inertial navigation system's misalignment error matrix in the northeast-northeast coordinate system. C is the first derivative of φ. ω I represents the Euler angle matrix; I denotes the identity matrix. Let be the attitude transfer matrix from the n-frame to the n′-frame, where the n-frame is the navigation coordinate system and the n′-frame is the computational navigation coordinate system. for The measurement quantity, Let δ represent the projection of the rotational angular velocity of the n-frame relative to the i-frame onto the n-frame, where δ denotes the error. for The error, Let ε be the attitude transfer matrix between the b-frame and the n′-frame, where b-frame is the carrier coordinate system, ε is the gyroscope zero bias, and ε' is the attitude transfer matrix between the b-frame and n′-frame. b v is the projection of the gyroscope's zero bias into the b-frame. n Let δv be the velocity of the inertial navigation system. n For speed error, The first derivative of the velocity error. The specific force output by the accelerometer. This is the error in the Earth's rotational angular velocity. This is the Earth's rotational angular velocity. for The measurement quantity, The angular velocity caused by the motion of the carrier. The error in angular velocity caused by the motion of the carrier. for The quantity to be measured; δg n The vector error is the gravitational acceleration; ▽ represents the accelerometer zero bias. b Let L be the projection of the accelerometer zero bias in the b-frame, and let L, λ, and h be the latitude, longitude, and altitude of the inertial navigation system in the northeast-northeast coordinate system, respectively; δL, δλ, and δh are the latitude error, longitude error, and altitude error of the inertial navigation system in the northeast-northeast coordinate system, respectively. The first derivative of δL The first derivative of δλ v is the first derivative of δh; E v N v U These represent the inertial navigation system's eastward, northward, and celestial velocities in the northeast-sky coordinate system, respectively; R M R is the radius of the meridian circle. N C is the radius of the circle. ω and The expression is as follows:

[0076]

[0077] Where sφ represents sinφ, and cφ represents cosφ; φ E φ N φ U These represent the components of the inertial navigation misalignment angle error in the east-west, north-north, and sky directions in the northeast-sky coordinate system.

[0078] Step 3.2: Based on the SINS nonlinear error model, the system state equations are established as follows:

[0079]

[0080] Where X(t) is the state variable, Let X(t) be the first derivative of X(t), then X(t) = [φ E φ N φ U δv E δv N δv U δLδλδhε X ε Y ε Z ▽ X▽ Y ▽ Z ] T , ε X ε Y ε Z To achieve zero bias of the three-axis gyroscope in three directions within the carrier coordinate system, ▽ X 、▽ Y 、▽ Z F(t) represents the zero bias of the triaxial acceleration in the three directions of the carrier coordinate system; F(t) is the state one-step transition matrix; G(t) is the system noise distribution matrix; and W(t) is the system noise vector.

[0081] The difference between the velocity and position information measured by the vehicle-mounted barometer and pseudo-satellite receiver and the velocity and position information calculated by the strapdown inertial navigation system is used as a measurement for filtering. The velocity and position measurement equations are as follows:

[0082]

[0083] Among them, Z v (t) represents the velocity measurement, Z p (t) represents the position measurement, v SINS,E v SINS,N and v SINS,U These are the inertial navigation systems calculating the vehicle's eastward, northward, and upward velocities, v. m,E and v m,N These are the eastward and northward speeds of the vehicle calculated by the pseudo-satellite receiver, v. K,U L represents the celestial velocity obtained by fusing the vehicle-mounted barometer and pseudo-satellite receiver using Kalman filtering. SINS and L m1 The latitude information of the vehicle is calculated by the inertial navigation and pseudo-satellite receivers, respectively. SINS and λ m1 The inertial navigation and pseudo-satellite receivers respectively calculate the vehicle's longitude information, h. SINS For the altitude information calculated by the inertial navigation system, Δh p h is the height difference calculated by the vehicle-mounted barometer. v For vehicle height, speed measurement matrix H v (t)=[0 3×3 I 3×3 0 3×9 Position measurement matrix H p (t)=[0 3×6 I 3×3 0 3×6 ], V v (t) is the velocity measurement noise vector, V p (t) represents the position measurement noise vector. Combining the velocity measurement equation and the position measurement equation, the overall measurement equation is:

[0084]

[0085] It should also be noted that the various specific technical features described in the above embodiments can be combined in any suitable manner without contradiction. To avoid unnecessary repetition, the present invention will not describe the various possible combinations separately.

Claims

1. A method for vehicle-mounted SINS alignment during travel in environments with uneven road surfaces, characterized in that: Specifically, the steps include the following: Step 1: Collect IMU output data during vehicle movement. Based on the collected IMU data, use an onboard barometer to calculate the road surface height difference Δh between two adjacent time points. p The vehicle's axial velocity v is calculated using the road surface height difference measured by the onboard barometer. p,U ; Step 2: Provide the altitude information calculated by the vehicle's barometer to the pseudo-satellite receiver for auxiliary positioning and speed measurement, thereby obtaining the vehicle's celestial velocity v through the pseudo-satellite receiver. m,U ; Step 3: Put v p,U and v m,U The fusion yields the fused celestial velocity v. m,U ; Step 4: Based on the fused celestial velocity v m,U Unscented Kalman filtering is used to perform initial fine alignment of the vehicle-mounted SINS; Step 4 specifically involves: applying a Kalman filter to v p,U and v m,U For fusion, the state equation is: Where X1(t) is the state vector, and the state variable is v p,U and v m,U The error between them Let X(t) be the first derivative of X(t), A(t) be the state transition matrix, M(t) be the system noise distribution matrix, and N(t) be the system noise vector. The measurement equation is: Z(t)=v p,U -v m,U Where Z(t) is the measurement vector Step 4 specifically involves: Step 4.1: Construct a nonlinear error model for the onboard SINS during transit; Step 4.2: Based on the SINS nonlinear error model in Step 4.1, establish the system state equation and measurement equation for the unscented Kalman filter.

2. The method for vehicle-mounted SINS alignment in undulating road conditions according to claim 1, characterized in that: In step 1, the height difference is calculated using the following formula: Where R is the gas constant, T is the thermodynamic temperature, g is the gravitational acceleration, p is the atmospheric pressure at the road surface height, and t represents the time. The vehicle's onboard barometer calculates the vehicle's axial velocity v using the following formula. p,U : Where Δt is the time interval between the current time and the previous time.

3. The method for vehicle-mounted SINS alignment in undulating road conditions according to claim 1, characterized in that: Step 2.1: Establish the following distance equation between the receiver and the pseudo-satellite: Where m represents the receiver, r jm The distance between the receiver and the j-th pseudo-satellite is measured, where j = 1, 2, 3, ..., n, and n represents the total number of pseudo-satellites. m y m (x) represents the receiver's eastward and northward positions in the Earth-centered, Earth-fixed coordinate system. sj ,y sj ,z sj Let be the coordinates of the j-th pseudo-satellite in the geocentric-ground-fixed coordinate system, and h be the coordinates of the j-th pseudo-satellite. v Let x be the vehicle height; solve the equation using the least squares method to obtain (x) m y m ); Step 2.2: Construct the following Doppler frequency shift equation: Among them, f oj Let f be the frequency of the signal transmitted by the j-th pseudo-satellite. j Let v be the frequency of the signal received by the receiver from the j-th pseudosatellite. x v y and v z Let v be the components of the receiver's velocity in the geocentric coordinate system along the x, y, and z axes; the Doppler frequency shift equation is calculated using the least squares method to obtain v. x v y and v z ; Step 2.3: Based on the calculated v x v y and v z Calculate the receiver's velocity in the local horizontal coordinate system: Among them, v m,E v m,N and v m,U These represent the receiver's eastward, northward, and upward velocities in the local horizontal coordinate system, respectively; the expression for M is... Among them, L m and λ m These are the latitude and longitude of the receiver's location, respectively.

4. The method for vehicle-mounted SINS alignment in undulating road conditions according to claim 1, characterized in that: The expression for the nonlinear error model of the in-vehicle SINS constructed in step 4.1 is: Where φ is the inertial navigation system's misalignment error matrix in the northeast-northeast coordinate system. C is the first derivative of φ. ω I represents the Euler angle matrix; I denotes the identity matrix. Let be the attitude transfer matrix from the n-frame to the n′-frame, where the n-frame is the navigation coordinate system and the n′-frame is the computational navigation coordinate system. for The measurement quantity, Let δ represent the projection of the rotational angular velocity of the n-frame relative to the i-frame onto the n-frame, where δ denotes the error. for The error, Let ε be the attitude transfer matrix between the b-frame and the n′-frame, where b-frame is the carrier coordinate system, ε is the gyroscope zero bias, and ε' is the attitude transfer matrix between the b-frame and n′-frame. b v is the projection of the gyroscope's zero bias into the b-frame. n Let δv be the velocity of the inertial navigation system. n For speed error, The first derivative of the velocity error. The specific force output by the accelerometer. This is the error in the Earth's rotational angular velocity. The angular velocity of Earth's rotation. for The measurement quantity, The angular velocity caused by the motion of the carrier. The error in angular velocity caused by the motion of the carrier. for The quantity to be measured; δg n This represents the error in the gravitational acceleration vector. To achieve zero bias in the accelerometer, Let L be the projection of the accelerometer zero bias in the b-frame, and let L, λ, and h be the latitude, longitude, and altitude of the inertial navigation system in the northeast-northeast coordinate system, respectively; δL, δλ, and δh are the latitude error, longitude error, and altitude error of the inertial navigation system in the northeast-northeast coordinate system, respectively. The first derivative of δL The first derivative of δλ v is the first derivative of δh; E v N v U These represent the inertial navigation system's eastward, northward, and celestial velocities in the northeast-sky coordinate system, respectively; R M R is the radius of the meridian circle. N The radius of the circle is the area between the east and west. The system state equations and measurement equations for the unscented Kalman filter established in step 4.2 are expressed as follows: Where X(t) is the state variable, Let X(t) be the first derivative of X(t), and Z1(t) be the measurement unit. Where, φ E φ N φ U These represent the components of the inertial navigation misalignment angle error in the east-west, north-north, and celestial directions in the northeast-sky coordinate system, ε. X ε Y ε Z To achieve zero bias of the three-axis gyroscope in three directions within the carrier coordinate system, Let F(t) be the zero bias of the triaxial acceleration in the three directions of the carrier coordinate system; F(t) be the state one-step transition matrix; G(t) be the system noise distribution matrix; W(t) be the system noise vector; and H be the system noise vector. v (t) is the velocity measurement matrix, H p (t) is the position measurement matrix, V v (t) is the velocity measurement noise vector, V p (t) is the position measurement noise vector.

Images