Vehicle-mounted SINS (Strapdown Inertial Navigation System) advancing alignment method in undulating pavement environment

Abstract

The invention provides a vehicle-mounted SINS (Strapdown Inertial Navigation System) advancing alignment method in a high-low fluctuating road surface environment, which specifically comprises the following steps of: 1, acquiring output data of an IMU (Inertial Measurement Unit) in a vehicle advancing process, calculating a fluctuating road surface height difference between two adjacent moments on the basis of the acquired IMU data in combination with vehicle-mounted barometer data, and calculating the sky-direction speed of a vehicle by adopting the fluctuating road surface height difference; step 2, providing height information calculated by the vehicle-mounted barometer for a pseudo satellite receiver to carry out auxiliary positioning and speed measurement so as to calculate the sky-direction speed of the vehicle through the pseudo satellite receiver; step 3, the sky-direction speed in the step 1 and the sky-direction speed in the step 2 are fused, and the fused sky-direction speed is obtained; and 4, based on the fused sky-direction velocity, performing initial fine alignment on the vehicle-mounted SINS by adopting unscented Kalman filtering. According to the method, the error of the height and the sky-direction speed calculated by the pseudo satellite receiver can be effectively compensated, and more accurate position and speed information can be obtained.

Description

Technical Field

[0001] The invention belongs to the technical field of navigation control, and in particular relates to a method for aligning a vehicle-mounted SINS on the move in an undulating road environment. Background Art

[0002] In the current information age, the speed of the initial alignment of the vehicle-mounted SINS (Strapdown Inertial Navigation System) is closely related to the performance of special vehicles. The environment in which special vehicles are located presents unprecedented complexity and variability. Laser interference, electromagnetic interference and other technological means can easily lead to the formation of a satellite denial environment. In the field environment, the ups and downs of the road surface add great uncertainty to the execution of the task. In order to ensure navigation performance, special vehicles need to complete high-precision on-the-go initial alignment on the ups and downs of the road under satellite denial conditions, and then successfully complete the corresponding functional tasks.

[0003] Pseudolites can provide satellite-like positioning signals in local areas. Their signal coverage can meet the needs of the vehicle driving environment, and effective signal transmission can be achieved through reasonable layout and configuration. The vehicle-mounted SINS itself has autonomy. After combining with pseudolite signals, it can take advantage of the complementary advantages of the two to improve the reliability and adaptability of the system. However, when pseudolites are used alone for auxiliary positioning, the VDOP (satellite vertical dilution of precision) is too large, which can easily lead to low accuracy of the altitude and celestial velocity calculated by the pseudolite receiver, thereby 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 above-mentioned prior art, the present invention provides a method for aligning a vehicle-mounted SINS on the move under an undulating road environment.

[0005] Technical solution: The present invention discloses a method for aligning a vehicle-mounted SINS on the road under an undulating road environment, which specifically includes the following steps:

[0006] Step 1: Collect the output data of IMU (inertial sensor) while the vehicle is moving, and use the on-board barometer to calculate the road height difference Δh between two adjacent moments based on the collected data p , and the vehicle's celestial velocity v is calculated based on the road height difference using the onboard barometer p,U ;

[0007] Step 2: Provide the altitude information calculated by the on-board barometer to the pseudo-satellite receiver for auxiliary positioning and speed measurement, so as to obtain the vehicle's celestial speed v through the pseudo-satellite receiver. m,U ;

[0008] Step 3: Set v p,U and v m,UFusion, get the fusion celestial velocity v m,U ;

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

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

[0011]

[0012] Among them, R is the gas constant, T is the thermodynamic temperature, g is the acceleration of gravity, p is the atmospheric pressure at the height of the road surface, and t represents the time;

[0013] The vehicle barometer calculates the vehicle's celestial velocity v according to the following formula p,U :

[0014]

[0015] Among them, Δt is the time interval between the current moment and the previous moment.

[0016] Furthermore, it is characterized in that:

[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 is the distance between the receiver and the jth pseudo-satellite measured, j = 1, 2, 3, ..., n, n represents the total number of pseudo-satellites, (x m ,y m ) represents the east and north positions of the receiver in the Earth-centered Earth-fixed coordinate system, (x sj ,y sj ,z sj ) is the coordinate of the jth pseudo-satellite in the Earth-centered Earth-fixed coordinate system, h v is the vehicle height; the equation is solved by the least square method to obtain (x m ,y m );

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

[0021]

[0022] Among them, f oj is the frequency of the signal transmitted by the jth pseudo-satellite, f j is the frequency of the signal from the jth pseudo-satellite received by the receiver, vx 、v y and v z is the x-axis, y-axis and z-axis components of the receiver's velocity in the Earth-centered Earth-fixed coordinate system; 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 velocity of the receiver in the local horizontal coordinate system:

[0024]

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

[0026] Furthermore, step 4 is as follows: use Kalman filtering to filter v p,U and v m,U The state equation is:

[0027]

[0028] Among them, X1(t) is the state vector, and the state quantity is v p,U and v m,U The error between is the first-order derivative of X(t), A(t) is the state one-step transfer matrix, M(t) is the system noise allocation matrix, and N(t) is 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, the step 4 is specifically as follows:

[0033] Step 4.1: Construct the nonlinear error model of the vehicle-mounted SINS during movement:

[0034]

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

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

[0037]

[0038] Among them, X(t) is the state quantity, is the first-order derivative of X(t), Z1(t) is the quantity 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 are the components of the inertial navigation misalignment angle error in the northeast celestial coordinate system in the east, north and celestial directions, respectively, X , ε Y , ε Z is the zero bias of the three-axis gyroscope in three directions in the carrier coordinate system, ▽ X ,▽ Y ,▽ Z is the zero bias of the three-axis acceleration in the three directions of the carrier coordinate system; F(t) is the state one-step transfer matrix, G(t) is the system noise distribution matrix, W(t) is the system noise vector, H v (t) is the velocity measurement matrix, H p (t) is the position measurement matrix, V v (t) is the speed measurement noise vector, V p (t) is the position measurement noise vector.

[0039] Beneficial effects: By introducing a vehicle-mounted barometer, the present invention can effectively compensate for the errors of the altitude and celestial velocity calculated by the pseudo-satellite receiver, obtain more accurate position and velocity information, and use unscented Kalman filtering to process the nonlinear error model of SINS during the vehicle's movement, more accurately estimate the mean and covariance of the system, thereby achieving high-precision on-the-go initial alignment of the vehicle-mounted SINS. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 The figure is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0041] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0042] The present invention provides a method for aligning a vehicle-mounted SINS on the road under an undulating road environment. The specific process is as follows: Figure 1 As shown:

[0043] Step 1: Collect the output data of IMU while the vehicle is moving, and calculate the altitude and celestial velocity information v with the onboard barometer p,U .

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

[0045]

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

[0047] Step 1.2: The vehicle barometer is based on the height difference Δh p The calculated celestial velocity v p,U The expression is as follows:

[0048]

[0049] Among them, Δt is the time interval between the current moment and the previous moment.

[0050] Step 2: Provide the altitude information calculated by the on-board barometer to the pseudo-satellite receiver for auxiliary positioning and speed measurement, and use the celestial velocity v calculated by the pseudo-satellite receiver as the m,U and the celestial velocity v calculated by the onboard barometer p,U To merge.

[0051] Step 2.1: The pseudo-satellite receiver uses the on-board barometer to calculate the height difference Δh p To perform auxiliary positioning, let the coordinates of the jth (j=1, 2, 3, ..., n) pseudo-satellite in the Earth-centered Earth-fixed coordinate system be (x sj ,y sj ,z sj ), n is the total number of pseudolites; then the distance r between the receiver m and the jth pseudolites measured is jm The expression is as follows:

[0052]

[0053] Among them, (x m ,y m ) represents the east and north positions of the receiver in the Earth-centered Earth-fixed coordinate system, h v For vehicle height;

[0054] For multiple pseudo-satellites, the equations can be listed as follows:

[0055]

[0056] Solving the equation by the least squares method, we get (x m ,y m ).

[0057] Step 2.2: Pseudo-satellite receiver performs velocity calculation. Assume f j is the frequency of the signal from the jth pseudo-satellite received by the receiver, f oj is the frequency of the signal transmitted by the jth pseudo-satellite, c is the speed of light, then according to the Doppler frequency shift principle, we can get:

[0058]

[0059] Among them, v x 、v y and v z are the x-axis, y-axis and z-axis components of the receiver's velocity in the Earth-centered Earth-fixed coordinate system;

[0060] For multiple pseudo-satellites, the equations can be listed as follows:

[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 according to the following expression (the local refers to the coordinate system where the receiver is currently located):

[0064]

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

[0066] The celestial velocity v calculated by the vehicle-mounted barometer p,U and the celestial velocity v calculated by the pseudo-satellite receiver m,U Perform Kalman filter fusion and select the celestial velocity error δv pm,U is the state quantity X1(t); then the state equation is as follows:

[0067]

[0068] in, is the first-order derivative of X(t), A(t) is the state one-step transfer matrix, M(t) is the system noise allocation matrix, and N(t) is the system noise vector;

[0069] Select the celestial velocity v calculated by the on-board barometer p,U and the celestial velocity v calculated by the pseudo-satellite receiver m,U The difference is taken as the quantity measurement, and 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) is the noise vector of the celestial velocity measurement. After Kalman filter fusion, we can get v K,U .

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

[0073] Step 3.1: Establish the nonlinear error model of the vehicle-mounted SINS during driving:

[0074]

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

[0076]

[0077] Among them, sφ represents sinφ, cφ represents cosφ; φ E ,φ N ,φ U They are respectively the east, north and celestial components of the inertial navigation misalignment angle error in the northeast celestial coordinate system.

[0078] Step 3.2: According to the SINS nonlinear error model, the system state equation is established as follows:

[0079]

[0080] Among them, X(t) is the state quantity, is the first-order derivative of X(t), X(t)=[φ E φ N φ U δv E δv N δv U δLδλδhε X ε Y ε Z ▽ X▽ Y ▽ Z ] T , ε X , ε Y , ε Z is the zero bias of the three-axis gyroscope in three directions in the carrier coordinate system, ▽ X ,▽ Y ,▽ Z is the zero bias of the three-axis acceleration in the three directions of the carrier coordinate system; F(t) is the state one-step transfer matrix, G(t) is the system noise distribution matrix, and W(t) is the system noise vector.

[0081] The difference between the speed and position information measured by the on-board barometer and pseudo-satellite receiver and the speed and position information calculated by the strapdown inertial navigation is used as the measurement for filtering, and the speed and position measurement equations are as follows:

[0082]

[0083] Among them, Z v (t) is the velocity measurement, Z p (t) is the position measurement, v SINS,E 、v SINS,N and v SINS,U are the east, north and sky speeds of the vehicle calculated by the inertial navigation, v m,E and v m,N are the eastward and northward velocities of the vehicle calculated by the pseudo-satellite receiver, v K,U is the celestial velocity after the vehicle-mounted barometer and pseudo-satellite receiver are fused through Kalman filtering, L SINS and L m1 The latitude information of the vehicle is solved by the inertial navigation and pseudo-satellite receivers, λ SINS and λ m1 The longitude information of the vehicle is calculated by the inertial navigation and pseudo-satellite receivers respectively, h SINS is the height information calculated by inertial navigation, Δh p is the height difference calculated by the onboard barometer, h v is the 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 speed measurement noise vector, V p (t) is the position measurement noise vector. Combining the velocity measurement equation with the position measurement equation, the total measurement equation is:

[0084]

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

Claims

1. A method for aligning a vehicle-mounted SINS on the road under an undulating road environment, characterized in that: The specific steps include: Step 1: Collect the output data of the IMU while the vehicle is moving, and use the on-board barometer to calculate the height difference Δh of the undulating road surface between two adjacent moments based on the collected IMU data p , and the road height difference of the vehicle's onboard barometer is used to calculate the vehicle's celestial speed v p,U ; Step 2: Provide the altitude information calculated by the on-board barometer to the pseudo-satellite receiver for auxiliary positioning and speed measurement, so as to obtain the vehicle's celestial speed v through the pseudo-satellite receiver. m,U ; Step 3: Set v p,U and v m,U Fusion, get the fusion celestial velocity v m,U ; Step 4: Based on the fused celestial velocity v m,U , unscented Kalman filter is used to perform initial fine alignment of the vehicle-mounted SINS.

2. The method for aligning a vehicle-mounted SINS on the road under an undulating road environment according to claim 1, characterized in that: In step 1, the height difference is calculated according to the following formula: Among them, R is the gas constant, T is the thermodynamic temperature, g is the acceleration of gravity, p is the atmospheric pressure at the height of the road surface, and t represents the time; The vehicle barometer calculates the vehicle's celestial velocity v according to the following formula p,U : Among them, Δt is the time interval between the current moment and the previous moment.

3. The method for aligning a vehicle-mounted SINS on the road under an undulating road environment 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 is the distance between the receiver and the jth pseudo-satellite measured, j = 1, 2, 3, ..., n, n represents the total number of pseudo-satellites, (x m ,y m ) represents the east and north positions of the receiver in the Earth-centered Earth-fixed coordinate system, (x sj ,y sj ,z sj ) is the coordinate of the jth pseudo-satellite in the Earth-centered Earth-fixed coordinate system, h v is the vehicle height; the equation is solved by the least square method to obtain (x m ,y m ); Step 2.2: Construct the following Doppler frequency shift equation: Among them, f oj is the frequency of the signal transmitted by the jth pseudo-satellite, f j is the frequency of the signal from the jth pseudo-satellite received by the receiver, v x 、v y and v z is the x-axis, y-axis and z-axis components of the receiver's velocity in the Earth-centered Earth-fixed coordinate system; 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 velocity of the receiver in the local horizontal coordinate system: Among them, v m,E 、v m,N and v m,U are the eastward, northward and celestial velocities of the receiver in the local horizontal coordinate system respectively; the expression of M is Among them, L m and λ m are the latitude and longitude of the receiver's location, respectively.

4. The method for aligning a vehicle-mounted SINS on the road under an undulating road environment according to claim 1, characterized in that: Step 4 is as follows: Use Kalman filtering to filter v p,U and v m,U The state equation is: Among them, X1(t) is the state vector, and the state quantity is v p,U and v m,U The error between is the first-order derivative of X(t), A(t) is the state one-step transfer matrix, M(t) is the system noise allocation matrix, and N(t) is the system noise vector; The measurement equation is: Z(t)=v p,U -v m,U Where Z(t) is the measurement vector.

5. The method for aligning a vehicle-mounted SINS on the road under an undulating road environment according to claim 1, characterized in that: The step 4 is specifically as follows: Step 4.1: Construct the nonlinear error model of the vehicle-mounted SINS during movement: Where φ is the misalignment angle error matrix of the inertial navigation in the northeast celestial coordinate system, is the first-order derivative of φ, C ω is the Euler angle matrix; I represents the identity matrix, is the attitude transfer matrix from the n system to the n′ system, the n system is the navigation coordinate system, and the n′ system is the computational navigation coordinate system. for The measurement quantity, is the projection of the rotational angular velocity of the n system relative to the i system in the n system, δ represents the error, for The error, is the attitude transfer matrix between b system and n′ system, b system is the carrier coordinate system, ε is the gyroscope zero bias, ε b is the projection of the gyroscope zero bias in the b system, v n is the velocity of the inertial navigation system, δv n is the speed error, is the first-order derivative of the velocity error, is the specific force output by the accelerometer, is the error of the Earth's rotation angular velocity, is the Earth's rotation angular velocity, for The measurement quantity, is the angular velocity caused by the carrier motion, is the error of angular velocity caused by the motion of the carrier, for The measurement quantity; δg n is the gravity acceleration vector error; is the accelerometer bias, is the projection of the accelerometer zero bias in the b system, L, λ, and h are the latitude, longitude, and altitude of the inertial navigation in the northeast celestial coordinate system, respectively; δL, δλ, and δh are the latitude error, longitude error, and altitude error of the inertial navigation in the northeast celestial coordinate system, respectively; is the first-order derivative of δL, is the first-order derivative of δλ, is the first-order derivative of δh; v E 、v N 、v U are the eastward, northward and celestial velocities of the inertial navigation in the northeast celestial coordinate system; R M is the meridian radius, R N is the radius of the Maoyou circle; Step 4.2: Based on the SINS nonlinear error model in step 4.1, establish the system state equation and measurement equation of the unscented Kalman filter: Among them, X(t) is the state quantity, is the first-order derivative of X(t), Z1(t) is the quantity measurement, Among them, φ E ,φ N ,φ U are the components of the inertial navigation misalignment angle error in the northeast celestial coordinate system in the east, north and celestial directions, respectively, X , ε Y , ε Z is the zero bias of the three-axis gyroscope in three directions in the carrier coordinate system, is the zero bias of the three-axis acceleration in the three directions of the carrier coordinate system; F(t) is the state one-step transfer matrix, G(t) is the system noise distribution matrix, W(t) is the system noise vector, H v (t) is the velocity measurement matrix, H p (t) is the position measurement matrix, V v (t) is the speed measurement noise vector, V p (t) is the position measurement noise vector.

Images