A Time Synchronization Estimation Method for a Multi-Source Integrated Navigation System

Abstract

The present invention discloses a time synchronization estimation method for a multi-source integrated navigation system. Aiming at the problem of delay errors caused by the inconsistency of time for signal acquisition, signal processing, etc. among multi-source sensors, the time synchronization estimation method augments multiple delay error state variables on the original traditional Kalman filter for fusion estimation, and takes the delay error estimation value obtained after one fusion estimation as the input for the next filtering and enters the next filtering, thereby realizing the delay compensation between sensors. The present invention can solve the problem of the decrease in positioning accuracy caused by the delay existing among multiple sensors, and at the same time estimate the delay values among multiple sensors, realizing the delay error estimation and compensation between sensors in the case where multiple sensors are used as measurements for assisted navigation, and improving the positioning accuracy of the vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous navigation and guidance methods, and particularly relates to the technical field of time synchronization estimation methods for multi-source integrated navigation systems. Background Art

[0002] The rapid development of the combined mode of Inertial Navigation System (INS) / Global Navigation Satellite System (GNSS) provides complete navigation information such as position, speed, and attitude for land vehicles, and has also become the most common combined navigation mode for vehicle navigation. However, since the signals of satellite navigation systems are extremely vulnerable to occlusion by factors such as leaves and tunnels, and inertial devices accumulate errors over time, the applicable range of the INS / GNSS combined mode is relatively small. Therefore, to expand the applicable range of the INS / GNSS combined mode, it is necessary to introduce other multiple sensors to provide auxiliary information, such as wheel speed odometers, lidar, etc., to achieve the fusion of multi-source information measurements regarding navigation information.

[0003] However, when multi-source sensors perform navigation information fusion, problems such as time delays between sensors often occur. As a result, when fusing information, it is very likely that the auxiliary information transmitted by the sensors for fusion is not the current moment's information, leading to a decrease in the positioning accuracy of positioning based on the fused information. To solve this problem, it is necessary to develop a method that can accurately estimate the time delays between multi-source sensors while obtaining multiple measurement information based on multi-source sensors, so as to achieve accurate and effective fusion of multi-source information. Summary of the Invention

[0004] Aiming at the defects of the prior art, the purpose of the present invention is to provide a delay estimation method for multi-source measurement information fusion, which can simultaneously estimate the relatively accurate delay errors between inertial sensors and other sensors used in the navigation process of the INS / GNSS combined mode, and based on this, accurate fusion of each navigation information can be achieved.

[0005] The technical solution of the present invention is as follows:

[0006] A time synchronization estimation method for a multi-source integrated navigation system, which includes:

[0007] S1: Collect multi-source navigation information of an object to be navigated at the current moment and mark the obtained information with flag bits. Among them, the multi-source navigation information includes the output information of an inertial sensor installed on the object to be navigated and the sensor information of other sensors serving as auxiliary navigation measurement sensors, and the flag bits include an attitude flag bit, a speed flag bit, and a position flag bit;

[0008] S2; Based on the output information of the inertial sensor collected and labeled at the current moment, perform strapdown inertial navigation solution to obtain the current moment navigation data of the object to be navigated calculated by the inertial sensor with flag bit annotation, including its current attitude angle, current speed, and current position;

[0009] S3: Augment the delay error between the inertial sensor and other sensors into the state variables of the Kalman filtering method, and obtain the predicted values of the state observation variables and the state covariance matrix through discretization processing to form an augmented and extended Kalman filter. The state variables include attitude state variables, speed state variables, and position state variables;

[0010] S4: Using the current inertial sensor data as a reference, collect and label the other sensor information obtained at the current moment as measurement data and input it into the augmented and extended Kalman filter. Under time matching, fuse and solve the two, and obtain the state variable estimation value and estimation mean square error of the filter under different flag types through the obtained fusion data, the predicted values of the state observation variables and the state covariance matrix, and different types of measurement equations corresponding to different flag bits;

[0011] S5: Correct the positioning information obtained by the current moment fusion solution through the state variable estimation value and estimation mean square error of the filter under different flag types, and obtain the delay estimation value at the current moment output by the filter. Compensate for the delay during the next filtering according to this delay estimation value to achieve cyclic solution; wherein, the positioning information includes position, speed, and attitude angle.

[0012] According to some specific embodiments of the present invention, in S1, the other sensor information includes one or more of lidar information, odometer information, and satellite navigation sensor information.

[0013] According to some specific embodiments of the present invention, the inertial sensor selects an inertial measurement unit IMU.

[0014] According to some specific embodiments of the present invention, S2 further includes:

[0015] S21: According to the output information of the inertial sensor at the current moment, obtain the current attitude angle of the object to be navigated through the quaternion solution algorithm;

[0016] S22: According to the output information of the inertial sensor at the current moment, obtain the current speed of the object to be navigated;

[0017] S23: According to the output information of the inertial sensor at the current moment, obtain the current position of the object to be navigated.

[0018] According to some specific embodiments of the present invention, in S21, the current posture angle is obtained according to the following calculation model:

[0019]

[0020]

[0021] Q(k)=[λ0,λ1,λ2,λ3]

[0022]

[0023]

[0024] Among them, θ is the pitch angle of the navigation object at the current time, i.e., time k, γ is the roll angle at the current time, ψ is the heading angle at the current time, T ij i,j=1,2,3 is the transformation matrix updated to the current moment The element in the i-th row and j-th column, λ0, λ1, λ2, λ3 are the quaternions at time k, Q(k) is the quaternion at time k, k-1 is the sampling time before the current time, Q(k-1) is the quaternion at time k-1, T is the matrix transpose, Δt is the discrete sampling period, ω(k) is the intermediate variable for solution, where, Represents the column vector composed of the axial components of the carrier coordinate system's angular velocity relative to the geographic coordinate system at the current moment,

[0025] for Components in the x, y, and z axes.

[0026] According to some specific embodiments of the present invention, in S22, the current speed is obtained by the following calculation model:

[0027]

[0028]

[0029]

[0030] in, v n (k) Components in the x, y, and z directions, v n (k) is the linear velocity of the machine system relative to the navigation system at time k expressed in the navigation system, v n (k-1) components in the x, y, and z directions, v n (k-1) is the linear velocity of the machine system relative to the navigation system at time k-1 expressed in the navigation system, g is the gravitational acceleration, a n(k) is the representation of the linear acceleration of the aircraft system relative to the navigation system at time k in the navigation system, is a n (k) the components in the x, y, and z directions; is the representation of the acceleration of the aircraft system relative to the navigation system at time k in the aircraft system, is the components on the x, y, and z axes.

[0031] According to some specific embodiments of the present invention, in S23, the current position is obtained through the following calculation model:

[0032]

[0033] where x n (k), y n (k), z n (k) are the position coordinates of the object to be navigated at time k in the three directions of the x, y, and z axes of the navigation system; x n (k - 1), y n (k - 1), z n (k - 1) are the position coordinates of the object to be navigated at time k - 1 in the three directions of the x, y, and z axes of the navigation system.

[0034] According to some specific embodiments of the present invention, S3 further includes:

[0035] S31 Augment the time-delay error between the inertial sensor and other sensors into the state quantity of the Kalman filtering method to obtain an extended Kalman filtering state equation;

[0036] S32 Discretize the coefficients of the extended Kalman filtering state equation, and correspondingly obtain the one-step prediction equation of its state quantity and the predicted value of the one-step prediction mean square error equation through the Kalman filtering method, forming an augmented extended Kalman filter.

[0037] According to some specific embodiments of the present invention, the other sensors include lidar, odometer, and satellite navigation sensors. In S31, the extended Kalman filtering state equation is as follows:

[0038]

[0039]

[0040]

[0041]

[0042]

[0043]

[0044] W = [ω gx ω gy ω gz ω rx ω ry ω rz ω ax ω ay ω az T

[0045] wherein, is the differential equation of the state quantity, FI is the system transfer matrix, GI is the system noise matrix, W is the white noise random error vector, X is the error state quantity after augmenting the time delay error, F N is the system matrix of 9 basic navigation parameters, F S is the inertial instrument state coefficient matrix of the inertial sensor, F t is the time delay state coefficient matrix; G INS is the inertial instrument error coefficient matrix of the inertial sensor related to the navigation information of the satellite navigation sensor, G t is the time delay error coefficient matrix, F M is the system transfer matrix related to the error state quantity of the inertial instrument error, O 12×9 is a 12×9 zero matrix, O 12×3 is a 12×3 zero matrix, O 3×9 is a 3×9 zero matrix, O 3×3 is a 3×3 zero matrix, O 15×3 is a 15×3 zero matrix; is the platform misalignment angle of the carrier rotating around the x, y, and z axes in the inertial sensor; δV E , δV N , δV U are the velocity estimation errors of the carrier along the northeast-up direction; δL, δλ, δh are the estimation errors of the carrier's position in longitude, latitude, and altitude; ε bx , ε by , ε bz are the zero biases of the gyroscope along the x, y, and z axes in the inertial sensor; ε rx , ε ry , ε rz are the first-order Markov process random noises of the gyroscope along the x, y, and z axes; is the first-order Markov process random noise of the accelerometer along the x, y, and z axes in the inertial sensor; Δt1, Δt2, Δt3 are the time delay values of three different sensors and the inertial sensor respectively; is the transformation matrix from the body coordinate system to the geographic coordinate system, T gx ​, T gy , T gz are the correlation times of the first-order Markov processes of the gyroscope along the x, y, and z axes, T ax , T ay , T az are the correlation times of the first-order Markov processes of the accelerometer along the x, y, and z axes, ω gx , ω gy , ω gz are the white noises of the gyroscope along the x, y, and z axes, ω rx , ω ry , ω rz are the white noises of the first-order Markov processes of the gyroscope along the x, y, and z axes, ω ax , ω ay , ω az are the white noises of the first-order Markov processes of the accelerometer along the x, y, and z axes.

[0046] Among them, F N is the system matrix of the 9 basic navigation parameters of the global satellite navigation system, and the specific forms of its non-zero terms are as follows:

[0047]

[0048] A I (5,1) = f u

[0049] A I (5,3) = -f e

[0050]

[0051]

[0052]

[0053]

[0054] A I (6,1) = -f n

[0055] A I (6,2) = f e

[0056]

[0057]

[0058] AI (4, 2) = -f u A I (6, 7) = -2V e ω ie sinL

[0059] A I (4, 3) = f n

[0060]

[0061]

[0062] A I (9, 6) = 1

[0063] According to some specific embodiments of the present invention, in S32, the predicted values of the one-step prediction equation and the one-step prediction mean square error equation are obtained through the following calculation model:

[0064]

[0065]

[0066] F L = I + F I * T D + F I * F I / 2.0 * T D * T D

[0067] G L = (I + F I / 2.0 * T D + F I * F I / 6.0 * T D * T D ) * G I * T D

[0068] where k is the current time, that is, at the k-th time, is the Kalman filter estimated value of the state quantity X at the (k - 1)-th time, k-1 ; is the predicted value of the state quantity X at the current time obtained through calculation, Φ k is the state transition matrix from the (k - 1)-th time to the k-th time, using the discretized state coefficient matrix F k / k-1 ; P L ; k / k-1 is the predicted value The mean square error matrix, that is, the predicted value of the mean square error equation, Γ k-1 is the noise matrix of the filter at time k-1, and the discretized error noise matrix G is used L , Q k-1 is the system noise matrix at time k-1, and the white noise random error vector W is used; I is the identity matrix, T D is the discrete period.

[0069] According to some specific embodiments of the present invention, the filtering gain equation of the augmented extended Kalman filter is as follows:

[0070]

[0071] where K k is the filtering gain at time k, R k is the measurement noise, and H k is the measurement matrix.

[0072] According to some specific embodiments of the present invention, the S4 further includes:

[0073] S41: Input the other sensor information of the object to be navigated at the current moment collected in S1 into the augmented extended Kalman filter as auxiliary measurement information, trigger its fusion condition, and fuse the navigation information obtained by strapdown inertial navigation with the auxiliary measurement information. The fusion uses different measurement equations according to different type flags of the input measurement data. Among them, the measurement data includes one or more of the current attitude, current speed, and current position of the object to be navigated, and the flags include one or more of the position flag, speed flag, and attitude flag. The measurement equations correspondingly include one or more of the position measurement equation, speed measurement equation, and attitude measurement equation;

[0074] S42: Obtain the state quantity estimation value and the estimated mean square error according to the corresponding measurement equation, as follows:

[0075]

[0076] P k =(I-K k H k )P k / k-1

[0077] where k is the current moment, is the Kalman filter estimation value of the state quantity at time k, is the one-step prediction of X k calculated using the state estimation value at time k-1, Z k is the measurement value at time k; P kis the estimated mean square error at time k, P k / k-1 is the estimated value of the mean square error matrix, and I is the identity matrix.

[0078] According to some specific embodiments of the present invention, in S41,

[0079] when the input measurement data contains a position flag bit, the following position measurement equation is used:

[0080] H P = [O 3×6 diag[R M R N cosL 1] O 3×9 h pΔt

[0081] R P = [diag([ε pe ε pn ε pu )] 2 )

[0082] Where:

[0083]

[0084] Among them, H P is the position measurement matrix, R P is the position measurement noise matrix, O 3×6 is a 3×6 zero matrix, L is the current latitude, R M is the radius of curvature of the meridian of the earth, R N is the radius of curvature of the prime vertical of the earth, ε pe 、ε pn 、ε pu are the ranging errors of the measurement sensor along the northeast-up directions, h pΔt is the delay measurement matrix related to the delay estimated value, veloN e 、veloN n 、veloN u are the eastward, northward, and upward velocities of the object to be navigated calculated at the current moment respectively;

[0085] When the input measurement data contains a velocity flag bit, the following velocity measurement equation is used:

[0086] H V = [O 3×3 diag[1 1 1] O 3×12 h vΔt

[0087] R V ​​= [diag([ε ve ε vn ε vu )] 2 )]

[0088] where:

[0089]

[0090] acceN = F fn - (2ω ie + ω ep ) × v ep - g

[0091] where, H V is the velocity measurement matrix, R V is the velocity measurement noise matrix, O 3×12 is a 3×6 zero matrix, ε ve , ε vn , ε vu are the velocity measurement errors of the measurement sensor along the northeast - north - up directions, h vΔt is the delayed measurement matrix related to the estimated value of the velocity measurement delay, acceN is the acceleration of the object to be navigated calculated at the current moment, acceN e , acceN n , acceN u are the east - ward, north - ward, and up - ward accelerations of the object to be navigated calculated at the current moment respectively, F fn is the projection of the accelerometer output specific force in the geographic coordinate system, ω ie is the angular velocity of the Earth's rotation, ω ep is the rotational speed of the object to be navigated relative to the Earth, v ep represents the motion velocity of the object to be navigated relative to the Earth, g is the Earth's gravitational acceleration;

[0092] When the input measurement data contains the attitude flag bit, use the following attitude measurement equation:

[0093] H A = [H a O 3×3 O 3×15 h aΔt

[0094]

[0095] where:

[0096]

[0097]

[0098] ​

[0099] Among them, H A is the measurement matrix for attitude measurement, O 3×15 is a 3×15 zero matrix, R A is the attitude measurement noise matrix, is the attitude error of the measurement sensor, H a is the attitude measurement matrix, γ, θ, and ψ are the roll, pitch, and heading angles at the current moment respectively, h aΔt is the delay measurement matrix related to the delay estimation value of attitude measurement, [atti_rate] is the attitude angular velocity, atti_rate γ 、atti_rate θ 、atti_rate ψ are the roll, pitch, and heading angle rates of the object to be navigated calculated according to the Euler angle differential equation at the current moment respectively, respectively represent the components of the angular velocity of the vehicle coordinate system relative to the geographic coordinate system along the axes of the vehicle coordinate system.

[0100] According to some specific embodiments of the present invention, the S5 further includes: correcting the position, speed, and attitude of the object to be navigated at the current moment by using the position estimate value, speed estimate value, and attitude estimate value in the state quantity estimate value output by step S4, using the delay value estimate value in the state quantity estimate value output by S4 as the delay estimate value at the current moment, and retaining it until entering the next filtering moment to perform delay compensation on the filtering of the next moment.

[0101] According to some specific embodiments of the present invention, the time synchronization estimation method further includes: performing delay compensation on the multi-source navigation information according to the delay estimate value and then performing fusion.

[0102] The present invention has the following beneficial effects:

[0103] The present invention uses Kalman filtering, expands the dimension by adding delay state quantities, estimates the delay errors existing between multiple sensors, and the estimated value enters the next cycle for delay compensation. It can overcome the delay problems existing between multiple sensors and realize the estimation and compensation of delay errors between sensors in the case of multi-source and multi-measurement.

[0104] The present invention can accurately estimate and compensate the delay errors between multiple sensors and inertial sensors by using the Kalman filtering equation in the case of multiple different measurement information, and improve the positioning accuracy. Brief Description of the Drawings

[0105] Figure 1 is a schematic flow chart of the delay estimation method in the embodiment of the present invention.

[0106] Figure 2 This is a comparison chart of the simulation trajectory and the trajectory before and after time delay estimation and compensation in the embodiments of the present invention.

[0107] Figure 3 This is a result chart of the time delay value estimation of the inertial sensor / odometer in the embodiments of the present invention.

[0108] Figure 4 This is a result chart of the time delay value estimation of the inertial sensor / lidar in the embodiments of the present invention.

[0109] Figure 5 This is a result chart of the time delay value estimation of the inertial sensor / GNSS in the embodiments of the present invention.

[0110] Figure 6 This is a comparison chart of the position error before and after time delay estimation and compensation in the embodiments of the present invention. Specific Embodiments

[0111] The present invention will be described in detail below in conjunction with embodiments and the accompanying drawings. However, it should be understood that the embodiments and the accompanying drawings are only used for exemplary description of the present invention, and do not constitute any limitation to the protection scope of the present invention. All reasonable transformations and combinations within the scope of the inventive concept of the present invention fall within the protection scope of the present invention.

[0112] Referring to the attached Figure 1 , according to the technical solution of the present invention, some specific embodiments of a time synchronization estimation method for a multi-source integrated navigation system include the following steps:

[0113] S1: Collect multi-source navigation information of the object to be navigated, such as a vehicle, at the current moment and mark the obtained information with flag bits. The multi-source navigation information includes the output information of an inertial sensor installed on the object to be navigated and the sensor information of other sensors used as auxiliary navigation measurement sensors. The flag bits include one or more of an attitude flag bit, a speed flag bit, and a position flag bit.

[0114] Further, in some specific embodiments, the sensor information includes one or more of lidar information, odometer information, and satellite navigation sensor information.

[0115] Further, in some specific embodiments, the inertial sensor selects an inertial measurement unit (IMU).

[0116] S2: Perform strapdown inertial navigation solution based on the output information of the inertial sensor at the current moment to obtain the current positioning data of the object to be navigated, including its current attitude angle, current speed, and current position.

[0117] In some specific embodiments, S2 may further include the following sub-steps:

[0118] S21: Obtain the current attitude angle of the object to be navigated through the quaternion solution algorithm according to the output information of the inertial sensor at the current moment;

[0119] Where:

[0120] In attitude solution, the update of the quaternion is implemented through the following update model:

[0121]

[0122] Where k is the current moment, Q(k) is the quaternion at moment k; k-1 is the previous sampling moment of the current moment, Q(k-1) is the quaternion at moment k-1; T is the matrix transpose, Δt is the discrete sampling period; ω(k) is an intermediate variable in the solution, and ω(k) is defined as follows:

[0123]

[0124] Where, represents the column vector composed of the components of the angular velocity of the vehicle coordinate system relative to the geographic coordinate system in the axial direction of the vehicle coordinate system at the current moment; is the components in the x, y, and z axes;

[0125] In attitude solution, the transformation matrix at moment k is as follows:

[0126]

[0127] Where λ0, λ1, λ2, λ3 are the quaternions at the current moment, i.e., k moment, that is, Q(k) = [λ0, λ1, λ2, λ3];

[0128] Under the above update model and transformation matrix, the current attitude angle of the object to be navigated at moment k is obtained as follows:

[0129]

[0130] Where θ is the pitch angle of the object to be navigated at the current moment (i.e., k moment), γ is the roll angle of the object to be navigated at the current moment, ψ is the heading angle of the object to be navigated, T ij i, j = 1, 2, 3 are the elements in the i-th row and j-th column of the transformation matrix updated to the current moment.

[0131] S22: Calculate the current speed of the object to be navigated according to the output information of the inertial sensor at the current moment through the following formula;

[0132]

[0133] Where, is v n (k) components in the x, y, and z directions, v n (k) is the representation of the linear velocity of the aircraft body system relative to the navigation system at time k in the navigation system; is v n (k - 1) components in the x, y, and z directions, v n (k - 1) is the representation of the linear velocity of the aircraft body system relative to the navigation system at time k - 1 in the navigation system; g is the acceleration due to gravity; is a n (k) components in the x, y, and z directions, a n (k) is the representation of the linear acceleration of the aircraft body system relative to the navigation system at time k in the navigation system, as follows:

[0134]

[0135] wherein, is the representation of the acceleration of the aircraft body system relative to the navigation system at time k in the aircraft body system, wherein, respectively represent components on the x, y, and z axes.

[0136] S23: Obtain the current position of the object to be navigated according to the output information of the inertial sensor at the current moment, as follows:

[0137]

[0138] wherein, x n (k), y n (k), z n (k) are the position coordinates of the object to be navigated in the three directions of the x, y, and z axes of the navigation system at the current moment, i.e., time k; x n (k - 1), y n (k - 1), z n (k - 1) are the position coordinates of the object to be navigated in the three directions of the navigation system at the previous moment, i.e., time k - 1.

[0139] S3: Augment the time delay error between the inertial sensor and the other sensors, including the global satellite navigation sensor, odometer, and lidar, into the state quantity of the Kalman filtering method, and obtain the predicted values of the state observable quantity and the state covariance matrix through discretization processing to form an augmented and extended Kalman filter.

[0140] In some specific embodiments, S3 may further include the following sub - steps:

[0141] S31: Augment the time-delay error between the inertial sensor and three measurement sensors, including the global satellite navigation sensor, odometer, and lidar, into the state variables of the Kalman filtering method to obtain the augmented Kalman filtering state equation as follows:

[0142]

[0143]

[0144]

[0145]

[0146]

[0147]

[0148] W = [ω gx ω gy ω gz ω rx ω ry ω rz ω ax ω ay ω az T

[0149] where is the differential equation of the augmented Kalman filtering state variables, F I is the system state matrix, G I is the system noise matrix, W is the white noise random error vector, X is the error state variable after augmenting the time-delay error, F S is the inertial instrument state coefficient matrix of the inertial sensor, F t is the time-delay state coefficient matrix; G INS is the inertial instrument error coefficient matrix of the inertial sensor related to the navigation information of the global satellite navigation sensor, G t is the time-delay error coefficient matrix, F M is the inertial instrument error coefficient matrix, O 12×9 is a 12×9 zero matrix, O 12×3 is a 12×3 zero matrix, O 3×9 is a 3×9 zero matrix, O 3×3 is a 3×9 zero matrix, O 15×3 is a 15×3 zero matrix; are the components of the inertial navigation platform error angle in the east, north, and up directions; δV E , δV N , δV U ​The velocity estimation error of the carrier in the northeast celestial direction; δL, δλ, δh are the estimation errors of the carrier's position in longitude, latitude, and altitude; ε bx , ε by , ε bz are the zero biases of the gyroscopes in the inertial sensor along the x, y, and z axes; ε rx , ε ry , ε rz are the first-order Markov process random noises of the gyroscopes along the x, y, and z axes; is the first-order Markov process random noise of the accelerometers in the inertial sensor along the x, y, and z axes; Δt1, Δt2, and Δt3 are the delay values of the satellite navigation sensor, odometer, lidar, and inertial sensor, respectively, which are used as measurement sensors; is the transformation matrix from the body coordinate system to the geographic coordinate system, T gx , T gy , T gz are the correlation times of the first-order Markov processes of the gyroscopes along the x, y, and z axes, T ax , T ay , T az are the correlation times of the first-order Markov processes of the accelerometers along the x, y, and z axes, ω gx , ω gy , ω gz are the white noises of the gyroscopes along the x, y, and z axes, ω rx , ω ry , ω rz are the white noises of the first-order Markov processes of the gyroscopes along the x, y, and z axes, ω ax , ω ay , ω az are the white noises of the first-order Markov processes of the accelerometers along the x, y, and z axes.

[0150] Among them, F N is the system matrix of the 9 basic navigation parameters of the global satellite navigation system, and the specific forms of its non-zero terms are as follows:

[0151]

[0152] A I (5,1) = f u

[0153] A I (5,3) = -f e

[0154]

[0155]

[0156]

[0157]

[0158] A I (6,1) = -f n

[0159] A I (6,2) = f e

[0160]

[0161]

[0162] A I (4,2) = -f u A I (6,7) = -2V e ω ie sinL

[0163] A I (4,3) = f n

[0164]

[0165]

[0166] A I (9,6) = 1

[0167] S32: Discretize the coefficients of the augmented Kalman filter state equation, and use the Kalman filter method to obtain the one-step prediction equation of its state variables and the predicted value of the one-step prediction mean square error equation, forming an augmented and extended Kalman filter:

[0168] Among them, the coefficient discretization process includes obtaining the following discretized state coefficient matrix and discretized error noise matrix:

[0169] F L = I + F I * T D + F I * F I / 2.0 * T D * T D

[0170] G L = (I + F I / 2.0 * TD +F I *F I / 6.0*T D *T D )*G I *T D

[0171] Among them, F L is the discretized state coefficient matrix, I is the identity matrix, T D is the discrete period; G L is the discretized error noise matrix.

[0172] Furthermore, the predicted value of the one-step prediction equation of the state quantity is obtained by the following formula:

[0173]

[0174] Among them, k is the current moment, that is, the k-th moment, is the Kalman filter estimated value of the state quantity X k-1 at the (k - 1)-th moment, is the predicted value of the state quantity X at the current moment calculated through k Φ k / k-1 is the state transition matrix from the (k - 1)-th moment to the k-th moment, that is, the discretized state coefficient matrix F L .

[0175] Furthermore, the predicted value of the one-step prediction mean square error equation of the state quantity is obtained by the following formula:

[0176]

[0177] Among them, P k / k-1 is the mean square error matrix of the predicted value , that is, the predicted value of the mean square error equation, Γ k-1 is the noise matrix of the filter at the (k - 1)-th moment, that is, the discretized error noise matrix G L , Q k-1 is the system noise matrix at the (k - 1)-th moment, that is, the white noise random error vector W.

[0178] Furthermore, the filtering gain equation of the augmented Kalman filter is as follows:

[0179]

[0180] Among them, K k is the filtering gain of the system at the k-th moment, R k is the measurement noise, H k is the measurement matrix.

[0181] S4: Input the current positioning information of the other sensors except the inertial sensor as measurement information into the augmented Kalman filter. When the measurement information enters the filter, fuse and calculate the positioning information of the current inertial sensor and the currently input measurement information. Obtain the state quantity estimation value and the estimated mean square error of the filter through the obtained fusion data, the predicted values of the state observable quantity and the state covariance matrix, and the measurement equations corresponding to different types of measurement data.

[0182] In some specific embodiments, S4 may further include the following sub-steps:

[0183] Specifically, it includes the following sub-steps:

[0184] S41: Input the information of the other sensors of the object to be navigated at the current moment collected in step S1 except the inertial sensor as auxiliary measurement information into the augmented Kalman filter, trigger its fusion condition, and fuse the navigation information obtained by strapdown inertial navigation solution with the auxiliary measurement information. The fusion uses different measurement equations according to the different type flags of the input measurement data. Among them, the measurement information can form a position-velocity combination, a velocity-attitude combination, and a full-information combination, namely a position-velocity-attitude combination.

[0185] Furthermore, the position measurement equation, the velocity measurement equation, and the attitude measurement equation are respectively constructed as follows:

[0186] When the input measurement data contains a position flag, use the following position measurement equation:

[0187] H P =[O 3×6 diag[R M R N cosL 1] O 3×9 h pΔt

[0188] R P =[diag([ε pe ε pn ε pu )] 2 )]

[0189] Among them, H p is the position measurement matrix, R p is the position two-side noise matrix, O 3×6 is a 3×6 zero matrix, L is the current latitude, R M is the meridian earth curvature radius, R N is the prime vertical earth curvature radius, ε pe 、ε pn 、ε​pu The ranging error of the satellite navigation sensor for inputting position information in the northeast - up direction, h pΔt is the delay measurement matrix related to the delay estimation value, as follows:

[0190]

[0191] where, veloN e 、veloN n 、veloN u are respectively the east - direction, north - direction, and up - direction velocities of the object to be navigated calculated by the navigation system through recursive solution based on the velocity fused at the previous moment and the inertial sensor information at the current moment. The solution is implemented through and in S21.

[0192] When the measured data input contains a velocity flag bit, the following velocity measurement equation is used:

[0193] H V =[O 3×3 diag[1 1 1] O 3×12 h vΔt

[0194] R V =[diag([ε ve ε vn ε vu 2 )]

[0195] where, H V is the velocity measurement matrix, O 3×12 is a 3×12 zero matrix, ε ve 、ε vn 、ε vu are the velocity measurement errors of the odometer in the northeast - up direction, h vΔt is the delay measurement matrix related to the velocity measurement delay estimation value, as follows:

[0196]

[0197] where, acceN e 、acceN n 、acceN u are respectively the east - direction, north - direction, and up - direction accelerations of the object to be navigated calculated at the current moment. The calculation method is as follows:

[0198] acceN=F fn -(2ω ie +ω ep )×v ep -g ​​

[0199] wherein, acceN is the acceleration in the navigation system at the current moment, and F fn is the projection of the accelerometer output specific force in the geographic system, ω ie is the angular velocity of the Earth's rotation, ω ep is the rotational speed of the object to be navigated relative to the Earth, v ep represents the motion speed of the object to be navigated relative to the Earth, and g is the acceleration due to gravity of the Earth.

[0200] When the measured data input contains an attitude flag bit, the following attitude measurement equation is used:

[0201] H A =[H a O 3×3 O 3×15 h aΔt

[0202]

[0203] wherein, H A is the attitude measurement matrix, O 3×15 is a 3×15 zero matrix, R A is the attitude measurement noise matrix, is the attitude measurement error of the lidar, H a is the attitude measurement matrix, as follows:

[0204]

[0205] wherein, γ, θ, and ψ are the roll, pitch, and heading angles at the current moment,

[0206] h aΔt is the delay measurement matrix related to the estimated value of the attitude measurement delay, as follows:

[0207]

[0208] wherein, atti_rate is the attitude angular velocity at the current moment, which can be calculated from the output information of the strapdown inertial navigation system in step S1 ; atti_rate γ 、atti_rate θ 、atti_rate ψ are the roll, pitch, and heading angle rates of the object to be navigated calculated from the Euler angle differential equation at the current moment, respectively, and they satisfy:

[0209]

[0210] wherein, γ, θ, and ψ are the roll, pitch, and heading angles at the current moment, ​They respectively represent the components of the angular velocity of the vehicle coordinate system relative to the geographic coordinate system along the axes of the vehicle coordinate system.

[0211] S42 Obtain the state quantity estimate and the estimated mean square error according to the corresponding measurement equation as follows:

[0212]

[0213] Where k is the current time, is the Kalman filter estimate of the state quantity at time k, is the one-step prediction of X calculated using the state estimate at time k-1, Z k k is the measurement value at time k.

[0214] P k =(I-K k H k )P k / k-1

[0215] Where P k is the estimated mean square error at time k, P k / k-1 is the mean square error matrix of the estimate , and I is the identity matrix.

[0216] S5: Correct the navigation quantity through the state quantity estimate and the estimated mean square error of the obtained filter, and obtain the delay estimate value at the current time output by the filter. Compensate for the delay during the next filtering according to the estimated value of the delay to achieve cyclic calculation.

[0217] Furthermore, it may include the following steps:

[0218] Correct the position, speed, and attitude at the current time through the state quantity estimate output by step S4, use the state quantity corresponding to the delay estimate output by S4 as the delay estimate value at the current time, retain it until entering the next filtering time, and compensate for the delay during the filtering of the next time.

[0219] According to the above specific implementation manners, the following simulation test embodiments are carried out:

[0220] In this simulation test, in addition to the inertial sensor and the global satellite navigation sensor, lidar information and odometer information are also incorporated. The delay parameter values of each sensor and the navigation system are set as follows:

[0221] Inertial sensor / odometer: 0.1 s.

[0222] Inertial sensor / lidar: 0.1 s.

[0223] Inertial sensor / global satellite navigation sensor: 0.2 s. ​

[0224] Comparison of the simulated trajectory obtained by testing with the fused trajectory without time delay estimation and compensation and the fused trajectory obtained by performing time delay estimation and compensation according to the method of the present invention is as follows Figure 2 shown. The comparison between the time delay estimation values and the true time delay values of the inertial sensor / odometer, inertial sensor / radar, and inertial sensor / satellite navigation sensor obtained according to the method of the present invention is respectively as shown in Appendix Figure 3 、 4 、5. The comparison between the position error after obtaining the time delay estimation value according to the method of the present invention and compensating through the time delay estimation value and the position error before time delay estimation and compensation is as shown in Appendix Figure 6 shown. It can be seen that after direct fusion without compensation, the longitude error is 0.5260 m, the latitude error is 0.4555 m, and the altitude error is 0.0747 m. After time delay estimation and compensation, the longitude error is reduced to 0.0596 m, the latitude error is reduced to 0.0813 m, and the altitude error is reduced to 0.0510 m, significantly improving the positioning accuracy.

[0225] The above specific description further details the object of the invention, the technical solution, and the beneficial effects. It should be understood that the above is only a specific embodiment of the present invention and is not used to limit the protection scope of the present invention. Any modification or equivalent replacement of the technical solution of the present invention without departing from the spirit and scope of the technical solution of the present invention shall be covered by the protection scope of the present invention.

Claims

1. A time synchronization estimation method for a multi-source integrated navigation system, characterized in that It includes: S1: Collect multi-source navigation information of the object to be navigated at the current moment and mark the obtained information with flag bits. Among them, the multi-source navigation information includes the output information of the inertial sensor installed on the object to be navigated and the sensor information of other sensors serving as auxiliary navigation measurement sensors. The flag bits include attitude flag bits, speed flag bits, and position flag bits; S2: According to the output information of the inertial sensor at the current moment collected and marked, perform strapdown inertial navigation solution to obtain the navigation data of the object to be navigated calculated by the inertial sensor at the current moment with flag bit markings, including its current attitude angle, current speed, and current position; S3: Augment the delay error between the inertial sensor and other sensors into the state variables of the Kalman filtering method, and obtain the predicted values of the state observation variables and the state covariance matrix through discretization processing to form an augmented Kalman filter. The state variables include attitude state variables, speed state variables, and position state variables; S4: Using the current inertial sensor data as a reference, collect and mark the other sensor information at the current moment obtained as measurement data and input it into the augmented Kalman filter. Under time matching, fuse and solve the two. Through the obtained fusion data, the predicted values of the state observation variables and the state covariance matrix, and different types of measurement equations corresponding to different flag bits, obtain the state variable estimation values and estimation mean square errors of the filter under different flag types; S5: Correct the positioning information obtained by the fusion solution at the current moment through the state variable estimation values and estimation mean square errors of the filter under different flag types, and obtain the delay estimation value at the current moment output by the filter. Compensate for the delay during the next filtering according to this delay estimation value to achieve cyclic solution; among them, the positioning information includes position, speed, and attitude angle.

2. The time synchronization estimation method according to claim 1, characterized in that In S1, the other sensor information includes one or more of lidar information, odometer information, and satellite navigation sensor information; and / or, the inertial sensor selects an inertial measurement unit IMU.

3. The time synchronization estimation method according to claim 1, characterized in that S2 further includes: S21: According to the output information of the inertial sensor at the current moment, obtain the current attitude angle of the object to be navigated through the quaternion solution algorithm; S22: According to the output information of the inertial sensor at the current moment, obtain the current speed of the object to be navigated; S23: According to the output information of the inertial sensor at the current moment, obtain the current position of the object to be navigated.

4. The time synchronization estimation method according to claim 3, wherein Among them, In S21, the current attitude angle is obtained according to the following calculation model: Q(k) = [λ0, λ1, λ2, λ3] where, θ is the pitch angle of the object to be navigated at the current moment, i.e., the k-th moment, γ is the roll angle at the current moment, ψ is the heading angle at the current moment, T ij i, j = 1, 2, 3 are the elements of the transformation matrix updated to the current moment in the i-th row and j-th column of, λ0, λ1, λ2, λ3 are the quaternions at the k-th moment, Q(k) is the quaternion at the k-th moment, k - 1 is the previous sampling moment of the current moment, Q(k - 1) is the quaternion at the k - 1 moment, T is the matrix transpose, Δt is the discrete sampling period, ω(k) is the intermediate variable for calculation, is the column vector composed of the components of the angular velocity of the vehicle coordinate system relative to the geographic coordinate system at the current moment in the axial directions of the vehicle coordinate system, is the components in the x, y, and z axial directions; and / or, in S22, the current speed is obtained according to the following calculation model: Among them, is the component of v n (k) in the x, y, and z directions, and v n (k) is the representation of the linear velocity of the aircraft system relative to the navigation system at time k in the navigation system, is the component of v n (k - 1) in the x, y, and z directions, and v n (k - 1) is the representation of the linear velocity of the aircraft system relative to the navigation system at time k - 1 in the navigation system, g is the acceleration due to gravity, and a n (k) is the representation of the linear acceleration of the aircraft system relative to the navigation system at time k in the navigation system, is the component of a n (k) in the x, y, and z directions, is the representation of the acceleration of the aircraft system relative to the navigation system at time k in the aircraft system, is the components on the x, y, and z axes; and / or, in S23, the current position is obtained according to the following calculation model: Among them, x n (k), y n (k), z n (k) are the position coordinates of the object to be navigated at the k-th moment in the three directions of the x, y, and z axes in the navigation system; x n (k - 1), y n (k - 1), z n (k - 1) are the position coordinates of the object to be navigated at the (k - 1)-th moment in the three directions of the x, y, and z axes in the navigation system.

5. The time synchronization estimation method according to claim 1, wherein S3 further includes: S31 Augment the time delay error between the inertial sensor and other sensors into the state variables of the Kalman filtering method to obtain the augmented Kalman filter state equation; S32 discretizes the coefficients of the dimension-expanded Kalman filter state equation, and correspondingly obtains the predicted values of the one-step prediction equation and the one-step prediction mean square error equation of its state variables through the Kalman filtering method, forming an augmented and dimension-expanded Kalman filter.

6. The time synchronization estimation method according to claim 5, characterized in that The other sensors include lidar, odometer and satellite navigation sensors. In S31, the dimension-expanded Kalman filter state equation is as follows: W = [ω gx ω gy ω gz ω rx ω ry ω rz ω ax ω ay ω az T ​ Among them, is the differential equation of the state quantity, F I is the system transition matrix, G I is the system noise matrix, W is the white noise random error vector, X is the error state quantity after augmenting the time delay error, F N is the system matrix of the 9 basic navigation parameters of the global satellite navigation system, F S is the inertial instrument state coefficient matrix of the inertial sensor, F t is the time delay state coefficient matrix; G INS is the inertial instrument error coefficient matrix of the inertial sensor related to the navigation information of the satellite navigation sensor, G t is the time delay error coefficient matrix, FM is the system transition matrix related to the error state quantity of the inertial instrument error, O 12×9 is a 12×9 zero matrix, O 12×3 is a 12×3 zero matrix, O 3×9 is a 3×9 zero matrix, O 3×3 is a 3×3 zero matrix, O 15×3 is a 15×3 zero matrix; is the platform misalignment angle of the carrier rotating around the x, y, and z directions in the inertial sensor; δV E , δV N , δV U are the velocity estimation errors of the carrier along the northeast-up directions; δL, δλ, δh are the estimation errors of the carrier's position in longitude, latitude, and altitude; ε bx , ε by , ε bz are the zero biases of the gyroscope along the x, y, and z axes in the inertial sensor; ε rx , ε ry , ε rz are the first-order Markov process random noises of the gyroscope along the x, y, and z axes; is the first-order Markov process random noise of the accelerometer along the x, y, and z axes in the inertial sensor; Δt1, Δt2, Δt3 are the time delay values of three different sensors and the inertial sensor respectively; is the transformation matrix from the body coordinate system to the geographic coordinate system, T gx , T gy , T gz are the correlation times of the first-order Markov process of the gyroscope along the x, y, and z axes, T ax , T ay , T az are the correlation times of the first-order Markov process of the accelerometer along the x, y, and z axes, ω gx , ω gy , ω gz are the white noises of the gyroscope x, y, z axes, ω rx , ω ry , ω rz is the white noise of the first-order Markov process of the x, y, and z axes of the gyroscope, and ω ax , ω ay , ω az is the white noise of the first-order Markov process of the x, y, and z axes of the accelerometer; Among them, F N is the system matrix of nine basic navigation parameters of the global satellite navigation system, and the specific form of its non-zero terms is as follows: A I (5,1) = f u A I (5,3) = -f e A I (6,1) = -f n A I (6,2) = f e A I (4,2) = -f u A I (6,7) = -2V e ω ie sinL A I (4,3) = f n A I (9,6)＝1 where ω ie is the angular velocity of the Earth's rotation and L is the current latitude; And / or, in S32, the predicted values of the one-step prediction equation and the one-step prediction mean square error equation are obtained through the following calculation model: F L = I + F I * T D + F I * F I / 2.0 * T D * T D G L = (I + F I / 2.0 * T D + F I * F I / 6.0 * T D * T D ) * G I * T D Among them, k is the current time, that is, time k, is the state quantity X at time k-1 k-1 The Kalman filter estimate of To pass The current state quantity X calculated k Predicted value, Φ k / k-1 is the state transfer matrix from time k-1 to time k, using the discretized state coefficient matrix F L ;P k / k-1 For the predicted value The mean square error matrix, that is, the predicted value of the mean square error equation, Γ k-1 is the noise matrix of the filter at time k-1, using the discretized error noise matrix G L , Q k-1 is the system noise matrix at time k-1, using the white noise random error vector W; I is the unit matrix, T D is a discrete period; And / or, the filtering gain equation of the augmented and dimension-expanded Kalman filter is as follows: Among them, K k is the filtering gain at time k, R k is the measurement noise, and H k is the measurement matrix.

7. The time synchronization estimation method according to claim 6, wherein S4 further includes: S41: Input the other sensor information of the object to be navigated at the current moment collected by S1 into the augmented and dimension-expanded Kalman filter, trigger its fusion condition, and fuse the navigation information obtained by strapdown inertial navigation with the auxiliary measurement information. According to different type flags of the input measurement data, different measurement equations are used correspondingly. Among them, the measurement data includes one or more of the current attitude, current speed, and current position of the object to be navigated, and the flags include one or more of the position flag, speed flag, and attitude flag. The measurement equations correspondingly include one or more of the position measurement equation, speed measurement equation, and attitude measurement equation; S42: Obtain the state variable estimated value and the estimated mean square error according to the corresponding measurement equation, as follows: P k = (I - K k H k )P k / k-1 where k is the current time, is the Kalman filter estimate of the state variable at time k, is the one-step prediction of X calculated using the state estimate at time k - 1, Z k is the measurement value at time k; P k is the estimated mean square error at time k, P k is the mean square error matrix of the estimate k / k-1 , and I is the identity matrix. ​ 8. The time synchronization estimation method according to claim 7, wherein In S41, When the input measurement data contains a position flag, the following position measurement equation is used: H P = [O 3×6 diag[R M R N cosL 1]O 3×9 h pΔt ​ R P = [diag([ε pe ε pn ε pu )] 2 ) Where: Among them, H P is the position measurement matrix, R P is the position measurement noise matrix, O 3×6 is a 3×6 zero matrix, L is the current latitude, R M is the radius of curvature of the meridian of the earth, R N is the radius of curvature of the prime vertical of the earth, ε pe and ε pn and ε pu are the ranging errors of the measurement sensor along the northeast-up directions, h pΔt is the delay measurement matrix related to the delay estimated value, veloN e and veloN n and veloN u are the eastward, northward, and upward velocities of the object to be navigated calculated at the current moment, respectively; When the input measurement data contains a speed flag, the following speed measurement equation is used: H V = [O 3×3 diag[1 1 1]O 3×12 h vΔt ​ R V = [diag([ε ve ε vn ε vu )] 2 ) Where: acceN = F fn -(2ω ie + ω ep ) × v ep - g Among them, H V is the velocity measurement matrix, R V is the velocity measurement noise matrix, O 3×12 is a 3×6 zero matrix, ε ve , ε vn , ε vu are the velocity measurement errors of the measurement sensor along the northeast - north - up directions, h vΔt is the delay measurement matrix related to the estimated value of the velocity measurement delay, acceN e , acceN n , acceN u are respectively the east - ward, north - ward, and up - ward accelerations of the object to be navigated calculated at the current moment. acceN is the acceleration of the object to be navigated calculated at the current moment, F fn is the projection of the accelerometer output specific force in the geographical coordinate system, ω ie is the angular velocity of the Earth's rotation, ω ep is the rotational speed of the object to be navigated relative to the Earth, v ep represents the motion speed of the object to be navigated relative to the Earth; g is the acceleration due to gravity of the Earth; When the input measurement data contains an attitude flag, the following attitude measurement equation is used: H A = [H a O 3×3 O 3×15 h aΔt ​ Where: Among them, H A is the measurement matrix for attitude measurement, O 3×15 is a 3×15 zero matrix, R A is the attitude measurement noise matrix, is the attitude error of the measurement sensor, H a is the attitude measurement matrix, γ, θ, and ψ are the roll, pitch, and heading angles at the current moment, h aΔt is the delay measurement matrix related to the delay estimation value of attitude measurement, [atti_rate] is the attitude angular velocity, atti_rate γ , atti_rate θ , atti_rate ψ are the roll, pitch, and heading angle rates of the object to be navigated calculated according to the Euler angle differential equation at the current moment, respectively represent the components of the angular velocity of the vehicle coordinate system relative to the geographical coordinate system in the axial direction of the vehicle coordinate system.

9. The time synchronization estimation method according to claim 1, wherein S5 further includes: correcting the current position, speed, and attitude of the object to be navigated through the position estimated value, speed estimated value, and attitude estimated value in the state variable estimated value output by step S4, using the delay estimated value in the state variable estimated value output by S4 as the current moment delay estimated value, and retaining it until the next filtering moment to perform delay compensation on the filtering of the next moment.

10. The time synchronization estimation method according to claim 1, wherein It also includes: delaying and compensating the multi-source navigation information according to the delay estimated value and then fusing it.

Images