CN110426033A - Time synchronization algorithm based on loose coupling IMU array navigation system - Google Patents
Time synchronization algorithm based on loose coupling IMU array navigation system Download PDFInfo
- Publication number
- CN110426033A CN110426033A CN201910694121.3A CN201910694121A CN110426033A CN 110426033 A CN110426033 A CN 110426033A CN 201910694121 A CN201910694121 A CN 201910694121A CN 110426033 A CN110426033 A CN 110426033A
- Authority
- CN
- China
- Prior art keywords
- error
- inertial sensor
- navigation system
- inertial
- time synchronization
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 28
- 230000008878 coupling Effects 0.000 title abstract description 4
- 238000010168 coupling process Methods 0.000 title abstract description 4
- 238000005859 coupling reaction Methods 0.000 title abstract description 4
- 238000000034 method Methods 0.000 claims abstract description 30
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 230000004044 response Effects 0.000 claims abstract description 11
- 239000013598 vector Substances 0.000 claims description 34
- 239000011159 matrix material Substances 0.000 claims description 17
- 238000005259 measurement Methods 0.000 claims description 15
- 238000005070 sampling Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000003111 delayed effect Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 3
- 239000004576 sand Substances 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 abstract description 4
- 230000002123 temporal effect Effects 0.000 abstract 1
- 238000004088 simulation Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 9
- 238000012937 correction Methods 0.000 description 3
- 230000001934 delay Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012935 Averaging Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000003139 buffering effect Effects 0.000 description 1
- 230000001364 causal effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000013213 extrapolation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of Time synchronization algorithms based on loose coupling IMU array navigation system, this method initially sets up the error model of IMU array navigation system, the system equation containing time synchronization error is established on the basis of the model, and the estimator in state-space model as state is introduced into using time synchronization error as state variable；Then temporal displacement is carried out to time synchronization error using score filtering wave by prolonging time device, and using the amplitude and phase response of Lagrange's interpolation interpolation fraction filtering wave by prolonging time device, obtains the displacement of inertial sensor data, guarantee the time synchronization of inertial sensor；Error compensation is carried out to the inertial sensor data after delay, and carries out navigation solution in the navigation equation that the inertial sensor data after time shift and error compensation is input to inertial navigation system, obtains actual navigational parameter.This method reduces the operand of navigation system without increasing additional hardware.
Description
Technical Field
The invention relates to a time synchronization algorithm based on a loosely-coupled IMU array navigation system.
Background
Navigation systems use fusion algorithms that typically assume perfect time synchronization of Inertial Measurement Units (IMUs) within an inertial sensor array when using the inertial sensor array or other sensors for data fusion, but in actual use this is difficult to achieve. There are many factors that cause the inertial measurement unit array clock to be out of synchronization, for example: measurement acquisition time, preprocessing time, communication transmission time, buffering time, computer scheduling time, manufacturing process, environmental factor influence, and the like.
At present, three methods are available for solving the problem of time synchronization error: hardware implementations, software implementations, and combinations of software and hardware implementations. In processing GPS/INS integrated navigation, researchers solve the time synchronization problem of SINS/GNSS by a method of detecting 1PPS (pulses Per second) pulse through hardware. There are also time synchronization schemes based on high precision timers, but these methods require additional hardware. The software solution adopts a synchronous extrapolation algorithm, and the method has large calculation amount. The combined hardware and software approach utilizes polynomial fitting, which also requires a large number of data points to be calculated.
Disclosure of Invention
The time synchronization algorithm overcomes the defects of a traditional time synchronization error solution, takes a time synchronization error as a state to be estimated, extends the state to a Kalman filter state vector to realize online estimation, further feeds the estimated time synchronization error back to a navigation system resolving loop, completes the time synchronization of data in a fractional delay filter, does not need to increase extra hardware, and reduces the operation amount of the navigation system.
In order to solve the technical problem, the time synchronization algorithm based on the loosely-coupled IMU array navigation system comprises the following steps:
step one, setting time synchronization error as T in an IMU array navigation systemdThe sampling period of the inertial sensor is TsAnd the offset and drift rate between clocks in the system are compensated, and only the initial offset between the clocks is unknown, namely the time synchronization error is considered as a constant;
establishing an error equation of the IMU array navigation system, selecting error quantities among all navigation subsystems as state quantities, establishing indirect filtering based on a Kalman filter, wherein the input of the Kalman filter is a difference value of measurement output of two inertial navigation systems on the same navigation parameter, and estimating each error quantity through iterative filtering calculation;
setting state vector x composed of errors in inertial navigation systemkComprises the following steps:
wherein, δ mkComposed of position error delta s, velocity error delta v and attitude angle error delta psi of inertial navigation systemkThe error of the inertial sensor is composed of a specific force error delta f of an accelerometer and a gyroscope deviation delta w;
the error state space expression of the inertial navigation system is:
xk+1＝φkxk+Gkek (2)
wherein phi iskBeing a state transition matrix, GkIs a process noise gain matrix, ekFor the inertial sensor to measure noise, the covariance matrix isE {. denotes a desirability function;
the position estimation of a single inertial sensor in the inertial array is obtained by means of the calculation of an inertial navigation system, and a position vector z is obtained by utilizing the difference of the position estimation of any two inertial sensors in the IMU arraykComprises the following steps:
zk＝Hkxk (3)
wherein HkIn order to measure the matrix of the measurements,
Hk＝[Ii 0i,j-i] (4)
wherein i and j are respectively position vectors zkAnd the state vector xkThe dimension of (a);
step (ii) ofEstablishing a system equation of the IMU array navigation system containing time synchronization error, wherein the time synchronization error T exists between the first inertial sensor and the second inertial sensor in the IMU arraydIf the trajectory equation of the carrier is s (t), the position estimation value of the second inertial sensor at the sampling instant k is:
wherein, wkFor the position estimation error of the second inertial sensor, the covariance isDue to TsPhase contrast TdIs a small quantity, at t ═ kTsNearby pair s (kT)s-Td) The second order taylor expansion is performed as:
wherein s isk、vk、akRespectively representing the position, velocity and acceleration of the carrier at the sampling instant k,
substituting formula (6) for formula (5) to obtain:
based on equation (7), the true position vector is fed back to the closed loop systemCan be described as:
wherein,for the position estimate of the first inertial sensor at the sampling instant kA value;
definition ofWherein xf,kFor the introduced auxiliary variable, substituting it into equation (8) yields the position vector equation for the synchronization error system:
definition of d in formula (9)kFor the position error offset vector:
comparing the equations (9) and (3), it can be seen that the position vector of the synchronization error system includes not only the position error Hkxf,kAnd position estimation error w of the second inertial sensorkAnd a position offset term dkThe position offset term contains a time synchronization error TdAnd the dynamics of the inertial navigation system;
step four, establishing a time synchronization model of the IMU array in the IMU array navigation system, and calculating the time synchronization error TdIntroduced as state variables into the state space model, timing synchronization error TdIs estimated asIn the resolving process of the inertial navigation systemA shift operation for controlling inertial sensor data;
setting an expanded state vector xd,kComprises the following steps:
wherein,representing the estimated delay error at time k, the spatial expression of the inertial navigation system is:
xd,k+1＝φd,kxd,k+Gd,kek (12)
wherein,
the position vector equation is given by the difference of the estimated positions of the first inertial sensor and the second inertial sensor and is expressed in the form of:
wherein,delaying data for a first inertial sensorCalculating the position of the second;
in thatIs aligned with zd,kPerforming a second order taylor expansion to obtain:
measurement matrix Hd,kIs defined as:
Hd,k＝[Hk vk] (15)
the position vector equation is then expressed as:
zd,k＝Hd,kxd,k+wk+d'k (16)
wherein,
step five, adopting a fraction delay filter to correct the synchronous error TdIs estimated asThe time shift is performed and the ideal frequency response of the fractional delay filter is given by a linear phase all-pass filter, whose expression is:
H(ej2πv)＝ej2πvDv-normalized frequence (17)
where j is an imaginary unit, v is a normalized frequency, D is a shift factor, for the time required for shifting, take
Interpolating the amplitude value and the phase response of the fractional delay filter by adopting a Lagrange interpolation method, wherein the filtering coefficient expression of the fractional delay filter is as follows:
where N is the order of the filter,and D is (N-1)/2, (N +1)/2]To obtain a delay time;
and step six, performing error compensation on the delayed inertial sensor data, and inputting the inertial sensor data subjected to time shift and error compensation into a navigation equation of an inertial navigation system for navigation solution to obtain actual navigation parameters.
Further, in step five, when the delay D is larger than (N +1)/2, adding unit delay before the fractional delay filter, and making D be [ (N-1)/2, (N +1)/2 ].
The time synchronization algorithm based on the loosely-coupled IMU array navigation system adopts the technical scheme, namely the method firstly establishes an error model of the IMU array navigation system, establishes a system equation containing time synchronization errors on the basis of the model, and introduces the time synchronization errors as state variables into a state space model to be used as state estimators; then, a fractional delay filter is adopted to perform time displacement on the time synchronization error, and the amplitude value and the phase response of the fractional delay filter are interpolated by adopting a Lagrange interpolation method to obtain the displacement of the data of the inertial sensor and ensure the time synchronization of the inertial sensor; and performing error compensation on the delayed inertial sensor data, and inputting the inertial sensor data subjected to time shift and error compensation into a navigation equation of an inertial navigation system for navigation solution to obtain actual navigation parameters. The method does not need to add extra hardware, and reduces the operation amount of the navigation system.
Drawings
The invention is described in further detail below with reference to the following figures and embodiments:
FIG. 1 is a schematic block diagram of a time synchronization algorithm based on a loosely coupled IMU array navigation system of the present invention;
FIG. 2 is a schematic diagram of the amplitude of a fractional delay filter interpolated by a Lagrange interpolation method in the present method;
FIG. 3 is a schematic diagram of a phase response of a fractional delay filter interpolated by a Lagrange interpolation method in the present method;
FIG. 4 is a schematic diagram of the gyroscope output after the inertial sensor array has been calibrated in a simulation experiment;
FIG. 5 is a schematic diagram of accelerometer output after inertial sensor array calibration in a simulation experiment;
FIG. 6 is a graph illustrating simulation results for four inertial sensors with time delays;
FIG. 7 is a schematic representation of the results of an inertial sensor simulation with time delay using the present algorithm;
FIG. 8 is a schematic diagram of inertial sensor motion state estimation before and after application of the present algorithm;
FIG. 9 is a schematic diagram of a mean value of motion state position estimates obtained whether to consider a time synchronization problem in an IMU array;
fig. 10 and fig. 11 are schematic diagrams of simulation results of inertial sensors with different delay times, respectively.
Detailed Description
The time synchronization algorithm based on the loosely-coupled IMU array navigation system comprises the following steps:
step one, setting time synchronization error as T in an IMU array navigation systemdThe sampling period of the inertial sensor is TsAnd the offset and drift rate between clocks in the system are compensated, and only the initial offset between the clocks is unknown, namely the time synchronization error is considered as a constant;
establishing an error equation of the IMU array navigation system, selecting error quantities among all navigation subsystems as state quantities, establishing indirect filtering based on a Kalman filter, wherein the input of the Kalman filter is a difference value of measurement output of two inertial navigation systems on the same navigation parameter, and estimating each error quantity through iterative filtering calculation;
setting state vector x composed of errors in inertial navigation systemkComprises the following steps:
wherein, δ mkPosition error δ s, velocity error δ v and attitude angle error δ from inertial navigation systemψComposition, δ bkThe error of the inertial sensor is composed of a specific force error delta f of an accelerometer and a gyroscope deviation delta w;
the error state space expression of the inertial navigation system is:
xk+1＝φkxk+Gkek (2)
wherein phi iskBeing a state transition matrix, GkIs a process noise gain matrix, ekFor the inertial sensor to measure noise, the covariance matrix isE {. denotes a desirability function;
the position estimation of a single inertial sensor in the inertial array is obtained by means of the calculation of an inertial navigation system, and a position vector z is obtained by utilizing the difference of the position estimation of any two inertial sensors in the IMU arraykComprises the following steps:
zk＝Hkxk (3)
wherein HkIn order to measure the matrix of the measurements,
Hk＝[Ii 0i,j-i] (4)
wherein i and j are respectively position vectors zkAnd the state vector xkThe dimension of (a);
establishing a system equation of the IMU array navigation system containing time synchronization error, wherein the time synchronization error T exists between the first inertial sensor and the second inertial sensor in the IMU arraydIf the trajectory equation of the carrier is s (t), the position estimation value of the second inertial sensor at the sampling instant k is:
wherein, wkFor the position estimation error of the second inertial sensor, the covariance isDue to TsPhase contrast TdIs a small quantity, at t ═ kTsNearby pair s (kT)s-Td) The second order taylor expansion is performed as:
wherein s isk、vk、akRespectively representing the position, velocity and acceleration of the carrier at the sampling instant k,
substituting formula (6) for formula (5) to obtain:
based on equation (7), the true position vector is fed back to the closed loop systemCan be described as:
wherein,the position estimation value of the first inertial sensor at the sampling moment instant k is obtained;
definition ofWherein xf,kFor the introduced auxiliary variable, substituting it into equation (8) yields the position vector equation for the synchronization error system:
definition of d in formula (9)kFor the position error offset vector:
comparing the equations (9) and (3), it can be seen that the position vector of the synchronization error system includes not only the position error Hkxf,kAnd position estimation error w of the second inertial sensorkAnd a position offset term dkThe position offset term contains a time synchronization error TdAnd the dynamics of the inertial navigation system;
step four, establishing a time synchronization model of the IMU array in the IMU array navigation system, and calculating the time synchronization error TdIntroduced as state variables into the state spaceIn the model, the time synchronization error TdIs estimated asIn the resolving process of the inertial navigation systemA shift operation for controlling inertial sensor data;
setting an expanded state vector xd,kComprises the following steps:
wherein,representing the estimated delay error at time k, the spatial expression of the inertial navigation system is:
xd,k+1＝φd,kxd,k+Gd,kek (12)
wherein,
the position vector equation is given by the difference of the estimated positions of the first inertial sensor and the second inertial sensor and is expressed in the form of:
wherein,delaying data for a first inertial sensorCalculating the position of the second;
in thatIs aligned with zd,kPerforming a second order taylor expansion to obtain:
measurement matrix Hd,kIs defined as:
Hd,k＝[Hk vk] (15)
the position vector equation is then expressed as:
zd,k＝Hd,kxd,k+wk+d'k (16)
wherein,
step five and step four assume that the first inertial sensor data is processed in timeSecond shift, which cannot be realized due to the limitation of hardware timestamp of inertial navigation system, therefore, a fractional delay filter is adopted to correct synchronization error TdIs estimated asThe time shift is performed and the ideal frequency response of the fractional delay filter is given by a linear phase all-pass filter, whose expression is:
H(ej2πv)＝ej2πvDv-normalized frequence (17)
where j is an imaginary unit, v is a normalized frequency, D is a shift factor, for the time required for shifting, take
Because the fractional delay filter is a non-causal filter with infinite impulse response and cannot be realized in practical application, an ideal fractional delay filter needs to be approximately processed, and a lagrange interpolation method is a common technology for designing the fractional delay filter, the amplitude and the phase response of the fractional delay filter are interpolated by the lagrange interpolation method, wherein the filter coefficient expression of the fractional delay filter is as follows:
where N is the order of the filter,and D is (N-1)/2, (N +1)/2]To obtain a delay time;
and step six, performing error compensation on the delayed inertial sensor data, and inputting the inertial sensor data subjected to time shift and error compensation into a navigation equation of an inertial navigation system for navigation solution to obtain actual navigation parameters.
Further, in step five, when the delay D is larger than (N +1)/2, adding unit delay before the fractional delay filter, and making D be [ (N-1)/2, (N +1)/2 ].
In order to achieve ideal time synchronization of each inertial sensor in an IMU array navigation system, a closed-loop loosely-coupled feedback filtering framework constructed by the method is shown in FIG. 1, firstly, a time synchronization error is contained in a state space model of a Kalman filtering system as a state variable, then the time synchronization error estimated by the Kalman filtering system is fed back to a fractional delay filter, then the data of the inertial sensor is subjected to time shift by the fractional delay filter, so that error compensation is carried out on the delayed data of the inertial sensor, and finally the data of the inertial sensor subjected to time shift and compensation is input into a navigation equation to carry out navigation solution to obtain an estimated navigation state.
In FIG. 1, the IMU1 and IMU2 are the first inertial sensorAnd a second inertial sensor for detecting the position of the object,representing the estimated navigational state of the navigation system,is indicative of the estimated error of the sensor,andrespectively representing the estimate of the sensor error and the estimate of the navigation state, HkA measurement matrix is represented that represents the measurement matrix,andrespectively, the position estimates, Z, of IMU1 and IMU2 at time kd,kRepresenting the difference between the IMU1 and IMU2 position estimates at time K, i.e., the observed quantity in a Kalman Filter System, KkRepresenting the gain in the kalman filter, is,an estimation error representing a time synchronization error estimated by the extended kalman filter system,for the error of the inertial sensor estimated by the extended kalman filter,representing position error estimated by extended Kalman Filter SystemError in velocityAttitude angle error delta psikThe navigation error estimate of the composition is,the measurement data representing the IMU1,representing the data after a fractional delay,data representing error compensated IMU1, function f (-) representing a navigation equation, d (-) representing a fractional delay filter function, and g (-) representing inertial sensor error compensation.
Fig. 2 and 3 respectively show the amplitude and phase response delay of the lagrange interpolation fractional delay filter with the fractional delay filter order N of 3, and since the inertial sensor has a low-pass filtering characteristic, it is a better choice to use the low-pass characteristic of the lagrange interpolation as the approximation of the ideal fractional delay filter of the inertial sensor data.
The method is subjected to simulation experiment and result analysis, and in the experimental stage, the inertial sensor array is firstly calibrated, and the calibrated result is given. And then, simulating the motion condition of the inertial array with the time synchronization error by using an MATLAB simulation tool, estimating the time synchronization error in the inertial array by using the algorithm, and providing a navigation result after the time error compensation. In order to further explain the effectiveness of the algorithm, the delay time of each inertial sensor is deliberately increased in an MATLAB simulation tool, and the simulation result shows that the algorithm can still well estimate the delayed time.
In order to average out random errors and unmodeled errors during the calibration phase of the inertial sensor array, a calibration support with 20 planes is used, which provides 20 uniform directions during the calibration process, thereby largely averaging random errors and unmodeled errors in the inertial sensor array. In the correction process, each face of the polyhedron is kept still for 5 seconds to collect data, and finally, the Gaussian Newton method is used for correctingAnd solving the correction parameters. The outputs of the gyroscope and the accelerometer after the inertial sensor array is corrected are shown in fig. 4 and 5, the data is acquired after the inertial sensor array is corrected and the inertial sensor array is still and the front side faces upwards, the output data is symmetrical about the X axis because the inertial sensors are embedded in the front and back sides of the embedded plate of the inertial sensor array, and as can be seen from the figure, the output error of the gyroscope after the correction is 0-2deg/s, and the output of the accelerometer in the Z axis under the static condition is 9.9m/s2The output of the X and Y axes is close to 0m/s2。
The IMU array containing the four inertial sensors is then subjected to simulation estimation of the motion state, assuming different time delays between the four inertial sensors during the simulation. Specifically, the first inertial sensor IMU1 is assumed to delay zero seconds, then the second inertial sensor IMU2 is assumed to have a 0.01 second delay, the third inertial sensor IMU3 is assumed to have a 0.015 second delay, and the fourth inertial sensor IMU4 is assumed to have a 0.02 second delay, taking the first inertial sensor as the time reference for the IMU array. The simulation results are shown in fig. 6, which shows the estimation of the actual motion by each inertial sensor, and it is easy to see that the estimated value deviates far from the real trajectory with the increase of time. Further, fig. 7 shows the time delay estimation of the second inertial sensor, the third inertial sensor and the fourth inertial sensor by using the algorithm, and as can be seen from fig. 7, the algorithm not only has good convergence characteristics, but also has the estimation accuracy reaching the millisecond level.
Then, the closed-loop loose coupling feedback system proposed by the algorithm is adopted to respectively estimate the position of the motion state of the closed-loop navigation system formed by the IMU2 and the IMU3 and the IMU4 and the IMU1, and the motion state estimation before and after the algorithm is used is shown in FIG. 8, wherein the IMU12, the IMU13 and the IMU14 respectively represent the position estimation of the motion state of the closed-loop loose coupling feedback system formed by the IMU2 and the IMU3 and the IMU4 and the IMU 1. Line 1 in fig. 9 is the mean value result of the motion state position estimation obtained by the algorithm in the IMU array, and line 2 is the mean value of the motion state position estimation in the IMU array without considering the time synchronization problem, as can be seen from fig. 9, the time synchronization error has a large influence on the navigation performance of the system, and the algorithm reduces the reduction of the navigation accuracy caused by the time asynchronism of the inertial sensor to a certain extent.
In order to test the estimation of the algorithm on the time synchronization error in the IMU array, the numerical value of the time synchronization error between different inertial sensors is intentionally increased in a simulation experiment, 0.02s and 0.03s of delay is performed on IMU2, 0.025s and 0.05s of delay is performed on IMU3, and 0.03s and 0.09s of delay is performed on IMU4, so that simulation result schematic diagrams of fig. 10 and 11 are respectively obtained, and it can be seen from fig. 10 and 11 that the algorithm not only has a good convergence characteristic on the estimation of the time delay, but also can achieve a good estimation effect on the estimated delay, and also achieves an estimation accuracy range of millisecond level.
The algorithm researches the problem of time synchronization error in an IMU array navigation system, selects an indirect filtering method taking the error quantity of a navigation subsystem in an inertial array integrated navigation system as a state quantity, provides a space expression and an observation equation taking the error as a Kalman filtering state variable, provides that the time synchronization error is taken as the state variable and contained in a Kalman filter, and estimates the time synchronization error through the Kalman filter; on the basis of obtaining the time synchronization error, the time shift is carried out on the inertial sensor data by adopting a fractional delay filter, and the time synchronization of the data acquired by different inertial sensors is ensured by adopting a Lagrange interpolation method. Through simulation experiments, the algorithm can well estimate the time synchronization errors of different inertial sensors and can perform data time synchronization, so that the navigation precision of the IMU array navigation system is improved.
Claims (2)
1. A time synchronization algorithm based on a loosely coupled IMU array navigation system is characterized by comprising the following steps:
step one, setting time synchronization error as T in an IMU array navigation systemdThe sampling period of the inertial sensor is TsAnd the offset and drift rates between clocks in the system have been compensated, only the initial between clocksThe offset is unknown, namely the time synchronization error is considered as a constant;
establishing an error equation of the IMU array navigation system, selecting error quantities among all navigation subsystems as state quantities, establishing indirect filtering based on a Kalman filter, wherein the input of the Kalman filter is a difference value of measurement output of two inertial navigation systems on the same navigation parameter, and estimating each error quantity through iterative filtering calculation;
setting state vector x composed of errors in inertial navigation systemkComprises the following steps:
wherein, δ mkComposed of position error delta s, velocity error delta v and attitude angle error delta psi of inertial navigation systemkThe error of the inertial sensor is composed of a specific force error delta f of an accelerometer and a gyroscope deviation delta w;
the error state space expression of the inertial navigation system is:
xk+1＝φkxk+Gkek (2)
wherein phi iskBeing a state transition matrix, GkIs a process noise gain matrix, ekFor the inertial sensor to measure noise, the covariance matrix isE {. denotes a desirability function;
the position estimation of a single inertial sensor in the inertial array is obtained by means of the calculation of an inertial navigation system, and a position vector z is obtained by utilizing the difference of the position estimation of any two inertial sensors in the IMU arraykComprises the following steps:
zk＝Hkxk (3)
wherein HkIn order to measure the matrix of the measurements,
Hk＝[Ii 0i,j-i] (4)
wherein i and j are each a bitLocation vector zkAnd the state vector xkThe dimension of (a);
establishing a system equation of the IMU array navigation system containing time synchronization error, wherein the time synchronization error T exists between the first inertial sensor and the second inertial sensor in the IMU arraydIf the trajectory equation of the carrier is s (t), the position estimation value of the second inertial sensor at the sampling instant k is:
wherein, wkFor the position estimation error of the second inertial sensor, the covariance isDue to TsPhase contrast TdIs a small quantity, at t ═ kTsNearby pair s (kT)s-Td) The second order taylor expansion is performed as:
wherein s isk、vk、akRespectively representing the position, velocity and acceleration of the carrier at the sampling instant k,
substituting formula (6) for formula (5) to obtain:
based on equation (7), the true position vector is fed back to the closed loop systemCan be described as:
wherein,the position estimation value of the first inertial sensor at the sampling moment instant k is obtained;
definition ofWherein xf,kFor the introduced auxiliary variable, substituting it into equation (8) yields the position vector equation for the synchronization error system:
definition of d in formula (9)kFor the position error offset vector:
comparing the equations (9) and (3), it can be seen that the position vector of the synchronization error system includes not only the position error Hkxf,kAnd position estimation error w of the second inertial sensorkAnd a position offset term dkThe position offset term contains a time synchronization error TdAnd the dynamics of the inertial navigation system;
step four, establishing a time synchronization model of the IMU array in the IMU array navigation system, and calculating the time synchronization error TdIntroduced as state variables into the state space model, timing synchronization error TdIs estimated asIn the resolving process of the inertial navigation systemA shift operation for controlling inertial sensor data;
setting an expanded state vector xd,kComprises the following steps:
wherein,representing the estimated delay error at time k, the spatial expression of the inertial navigation system is:
xd,k+1＝φd,kxd,k+Gd,kek (12)
wherein,
the position vector equation is given by the difference of the estimated positions of the first inertial sensor and the second inertial sensor and is expressed in the form of:
wherein,delaying data for a first inertial sensorCalculating the position of the second;
in thatIs aligned with zd,kPerforming a second order taylor expansion to obtain:
measurement matrix Hd,kIs defined as:
Hd,k＝[Hk vk] (15)
the position vector equation is then expressed as:
zd,k＝Hd,kxd,k+wk+d'k (16)
wherein,
step five, adopting a fraction delay filter to correct the synchronous error TdIs estimated asThe time shift is performed and the ideal frequency response of the fractional delay filter is given by a linear phase all-pass filter, whose expression is:
H(ej2πv)＝ej2πvDv-normalized frequence (17)
where j is an imaginary unit, v is a normalized frequency, D is a shift factor, for the time required for shifting, take
Interpolating the amplitude value and the phase response of the fractional delay filter by adopting a Lagrange interpolation method, wherein the filtering coefficient expression of the fractional delay filter is as follows:
where N is the order of the filter,and D is (N-1)/2, (N +1)/2]To obtain a delay time;
and step six, performing error compensation on the delayed inertial sensor data, and inputting the inertial sensor data subjected to time shift and error compensation into a navigation equation of an inertial navigation system for navigation solution to obtain actual navigation parameters.
2. The time synchronization algorithm based on a loosely-coupled IMU array navigation system of claim 1, wherein: and step five, when the delay D is larger than (N +1)/2, adding unit delay in front of the fractional delay filter to make D be equal to [ (N-1)/2, (N +1)/2 ].
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910694121.3A CN110426033A (en) | 2019-07-30 | 2019-07-30 | Time synchronization algorithm based on loose coupling IMU array navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910694121.3A CN110426033A (en) | 2019-07-30 | 2019-07-30 | Time synchronization algorithm based on loose coupling IMU array navigation system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110426033A true CN110426033A (en) | 2019-11-08 |
Family
ID=68413137
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910694121.3A Pending CN110426033A (en) | 2019-07-30 | 2019-07-30 | Time synchronization algorithm based on loose coupling IMU array navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110426033A (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111578939A (en) * | 2020-03-23 | 2020-08-25 | 济南大学 | Robot tight combination navigation method and system considering random variation of sampling period |
| CN111780758A (en) * | 2020-07-08 | 2020-10-16 | 中国人民解放军海军工程大学 | Gravity stabilization platform attitude determination method based on dual-mode calculation and application |
| CN112729290A (en) * | 2020-12-23 | 2021-04-30 | 重庆华渝电气集团有限公司 | Navigation attitude data synchronization error compensation method of inertial navigation equipment |
| CN113325920A (en) * | 2020-02-28 | 2021-08-31 | 阿里巴巴集团控股有限公司 | Data processing method, traveling data fusion method, device and storage medium |
| WO2021189373A1 (en) * | 2020-03-26 | 2021-09-30 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Time determination of an inertial navigation system in autonomous driving systems |
| CN115096304A (en) * | 2022-08-26 | 2022-09-23 | 中国船舶重工集团公司第七0七研究所 | Delay error correction method, device, electronic equipment and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110025562A1 (en) * | 2009-08-03 | 2011-02-03 | Xsens Technologies, B.V. | Tightly Coupled UWB/IMU Pose Estimation System and Method |
| US20110238307A1 (en) * | 2010-03-26 | 2011-09-29 | Mark Lockwood Psiaki | Vehicle navigation using non-gps leo signals and on-board sensors |
| CN103135083A (en) * | 2011-11-24 | 2013-06-05 | 西安电子科技大学 | Electromagnetic vector sensor array amplitude and phase error self-correcting method based on array rotation |
| CN103235328A (en) * | 2013-04-19 | 2013-08-07 | 黎湧 | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method |
| CN104330082A (en) * | 2014-10-22 | 2015-02-04 | 哈尔滨工程大学 | Real-time data synchronization method for MEMS (Micro-Electromechanical System)/GNSS (Global Navigation Satellite System) combined navigation system |
| CN109099913A (en) * | 2018-10-10 | 2018-12-28 | 格物感知（深圳）科技有限公司 | A kind of wearable navigation device and method based on MEMS inertia device |
-
2019
- 2019-07-30 CN CN201910694121.3A patent/CN110426033A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110025562A1 (en) * | 2009-08-03 | 2011-02-03 | Xsens Technologies, B.V. | Tightly Coupled UWB/IMU Pose Estimation System and Method |
| US20110238307A1 (en) * | 2010-03-26 | 2011-09-29 | Mark Lockwood Psiaki | Vehicle navigation using non-gps leo signals and on-board sensors |
| CN103135083A (en) * | 2011-11-24 | 2013-06-05 | 西安电子科技大学 | Electromagnetic vector sensor array amplitude and phase error self-correcting method based on array rotation |
| CN103235328A (en) * | 2013-04-19 | 2013-08-07 | 黎湧 | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method |
| CN104330082A (en) * | 2014-10-22 | 2015-02-04 | 哈尔滨工程大学 | Real-time data synchronization method for MEMS (Micro-Electromechanical System)/GNSS (Global Navigation Satellite System) combined navigation system |
| CN109099913A (en) * | 2018-10-10 | 2018-12-28 | 格物感知（深圳）科技有限公司 | A kind of wearable navigation device and method based on MEMS inertia device |
Non-Patent Citations (1)
| Title |
|---|
| ISAAC SKOG: "Time_Synchronization_Errors_in_Loosely_Coupled_GPS-Aided_Inertial_Navigation_Systems", 《IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS》 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113325920A (en) * | 2020-02-28 | 2021-08-31 | 阿里巴巴集团控股有限公司 | Data processing method, traveling data fusion method, device and storage medium |
| CN111578939A (en) * | 2020-03-23 | 2020-08-25 | 济南大学 | Robot tight combination navigation method and system considering random variation of sampling period |
| WO2021189373A1 (en) * | 2020-03-26 | 2021-09-30 | Baidu.Com Times Technology (Beijing) Co., Ltd. | Time determination of an inertial navigation system in autonomous driving systems |
| CN113728211A (en) * | 2020-03-26 | 2021-11-30 | 百度时代网络技术（北京）有限公司 | Time determination for inertial navigation system in autonomous driving system |
| US11662745B2 (en) | 2020-03-26 | 2023-05-30 | Baidu Usa Llc | Time determination of an inertial navigation system in autonomous driving systems |
| CN111780758A (en) * | 2020-07-08 | 2020-10-16 | 中国人民解放军海军工程大学 | Gravity stabilization platform attitude determination method based on dual-mode calculation and application |
| CN112729290A (en) * | 2020-12-23 | 2021-04-30 | 重庆华渝电气集团有限公司 | Navigation attitude data synchronization error compensation method of inertial navigation equipment |
| CN115096304A (en) * | 2022-08-26 | 2022-09-23 | 中国船舶重工集团公司第七0七研究所 | Delay error correction method, device, electronic equipment and storage medium |
| CN115096304B (en) * | 2022-08-26 | 2022-11-22 | 中国船舶重工集团公司第七0七研究所 | Delay error correction method, device, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110426033A (en) | Time synchronization algorithm based on loose coupling IMU array navigation system | |
| CN107643534B (en) | A kind of dual rate kalman filter method based on GNSS/INS deep integrated navigation | |
| CN112013836B (en) | Attitude heading reference system algorithm based on improved adaptive Kalman filtering | |
| CN110146075B (en) | SINS/DVL combined positioning method of gain compensation adaptive filtering | |
| CN103235328B (en) | GNSS (global navigation satellite system) and MEMS (micro-electromechanical systems) integrated navigation method | |
| CN105300384B (en) | A kind of interactive filtering method determined for the attitude of satellite | |
| US11181379B2 (en) | System and method for enhancing non-inertial tracking system with inertial constraints | |
| CN108981696B (en) | Sins random misalignment angle non-singular rapid transfer alignment method | |
| JP6905841B2 (en) | Navigation system and methods for error correction | |
| CN107479076B (en) | Combined filtering initial alignment method under movable base | |
| CN109507706B (en) | GPS signal loss prediction positioning method | |
| CN110567461A (en) | Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope | |
| CN110632634B (en) | Optimal data fusion method suitable for ballistic missile INS/CNS/GNSS combined navigation system | |
| CN107036598A (en) | Dual quaterion inertia/celestial combined navigation method based on gyro error amendment | |
| CN112874529B (en) | Vehicle mass center slip angle estimation method and system based on event trigger state estimation | |
| CN108562290A (en) | Filtering method, device, computer equipment and the storage medium of navigation data | |
| Wenk et al. | Posture from motion | |
| CN103901459B (en) | The filtering method of Measurement delay in a kind of MEMS/GPS integrated navigation system | |
| CN110736459B (en) | Angular deformation measurement error evaluation method for inertial quantity matching alignment | |
| JP2010096647A (en) | Navigation apparatus and estimation method | |
| CN110345942B (en) | Interpolation three subsampled cone error compensation algorithm based on angular rate input | |
| CN113091754A (en) | Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method | |
| CN110160530B (en) | Spacecraft attitude filtering method based on quaternion | |
| KR20120054347A (en) | Method for estimating a doppler centroid frequency for forming a sar(synthetic aperture radar) image, and a computer-readable media writing a program to implement the same method | |
| JP4343581B2 (en) | Moving body posture detection device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20191108 |