CN114264304A - High-precision horizontal attitude measurement method and system in complex dynamic environment - Google Patents

High-precision horizontal attitude measurement method and system in complex dynamic environment Download PDF

Info

Publication number
CN114264304A
CN114264304A CN202111587433.8A CN202111587433A CN114264304A CN 114264304 A CN114264304 A CN 114264304A CN 202111587433 A CN202111587433 A CN 202111587433A CN 114264304 A CN114264304 A CN 114264304A
Authority
CN
China
Prior art keywords
error
equation
gyroscope
axis
accelerometer
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.)
Granted
Application number
CN202111587433.8A
Other languages
Chinese (zh)
Other versions
CN114264304B (en
Inventor
吴志洁
曾浩瀚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Original Assignee
Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials filed Critical Hunan Aerospace Institute of Mechanical and Electrical Equipment and Special Materials
Priority to CN202111587433.8A priority Critical patent/CN114264304B/en
Publication of CN114264304A publication Critical patent/CN114264304A/en
Application granted granted Critical
Publication of CN114264304B publication Critical patent/CN114264304B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Navigation (AREA)

Abstract

The invention discloses a high-precision horizontal attitude measurement method and a system in a complex dynamic environment, which comprises the steps of calculating an initial attitude matrix; filtering feedback compensation is carried out on the output increment information of the accelerometer and the gyroscope respectively; constructing a speed updating equation, an attitude updating equation and a position updating equation; constructing a Kalman filtering state equation and a measurement equation; and constructing a Kalman filtering equation, performing partial feedback correction when the continuous rotation accumulated time exceeds the set time, and performing feedback correction of all state quantities when the continuous rotation accumulated time is less than the set time. The invention can adapt to the processes of different rotating speeds, alternate rotation and stop, radar antenna erection and the like, ensures the high-precision horizontal attitude measurement of the radar large disc in the leveling process, the radar antenna erection process and the radar antenna in the complex working state, and improves the anti-interference capability of the system.

Description

High-precision horizontal attitude measurement method and system in complex dynamic environment
Technical Field
The invention belongs to the technical field of inertial navigation, and particularly relates to a high-precision horizontal attitude measurement method and system for a complex dynamic environment.
Background
According to the radar error principle, the out-of-levelness of the large plate is an important component of an error source of a radar shafting and has important influence on the radar angle measurement precision, so that the accurate out-of-levelness of the large plate is obtained on the premise of improving the radar measurement precision. The out-of-level of the large disk refers to the non-parallelism of the plane of the azimuth turntable and the reference plane of the inertial navigation coordinate system, and if the reference plane of the inertial navigation coordinate system is not flat, the error of the out-of-level of the large disk can be increased.
At present, the level of a traditional radar antenna is calibrated by adopting a combined image level gauge in the domestic antenna radar station. The image combination level meter is a level meter with a base measuring surface and a micrometric screw pair for adjusting the base measuring surface, the meter combines image reading by an optical principle, the precision of aiming reading is improved, the working principle is that a prism is used for amplifying a bubble symbol in the level meter to improve the reading precision, and a set of transmission mechanism of a lever and a micromotion screw is used for improving the reading sensitivity. Therefore, when the measured piece is inclined by 0.01 mm/m, the measured piece can be accurately read in the image combiner. The image combination level meter is used by arranging the image combination level meter on the working surface of the tested piece, rotating the dial until the two bubble images are overlapped due to the inclination of the tested surface, and obtaining the reading at the moment, wherein the actual inclination of the tested piece can be calculated by the actual inclination which is scale value multiplied by fulcrum distance multiplied by dial reading.
For example, the technology of calibration and flight calibration of aerospace survey ship control communication equipment (published by national defense industry publishers), which is compiled by sedian, and the application of electronic gradienters in the large dish non-levelness test (value engineering), which is proposed by qiangqi and jiyai, all disclose the use of gradienters for large dish non-levelness measurement. However, the measurement method cannot record the inclination of the large disk when the antenna rotates in real time, and human factor errors are introduced, so that the measurement data accuracy is low, and the measurement accuracy of the non-levelness of the large disk is reduced.
Meanwhile, the rotation state of the vehicle-mounted radar system is complex during working, algorithm errors can be introduced into the same algorithm when the radar system is in a preparation state (leveling and erecting) and a working state (complex dynamic state and needs to be continuously rotated and stopped), and the measurement accuracy of the levelness is greatly reduced.
Disclosure of Invention
The invention aims to provide a high-precision horizontal attitude measurement method and system in a complex dynamic environment, and aims to solve the problem of low measurement precision of the existing measurement method.
The invention solves the technical problems through the following technical scheme: a high-precision horizontal attitude measurement method for a complex dynamic environment comprises the following steps:
acquiring the gravity acceleration information of the geographical position of the strapdown inertial navigation system and the output information of an inertial device, and calculating an initial attitude matrix C according to the gravity acceleration information and the output information of the inertial devicebn0；
Respectively carrying out filtering feedback compensation on the output increment information of the accelerometer and the gyroscope to obtain compensated output increment information of the accelerometer and the gyroscope;
constructing a speed updating equation, an attitude updating equation and a position updating equation according to an error equation of the strapdown inertial navigation system and an error equation of an inertial device by using the compensated output increment information of the accelerometer and the compensated output increment information of the gyroscope;
constructing a Kalman filtering state equation according to an error equation of a strapdown inertial navigation system, an error equation of an inertial device and an error equation of an accelerometer size effect, and constructing a measurement equation according to the speed updating equation;
constructing a Kalman filtering equation according to the Kalman filtering state equation and the measurement equation;
and acquiring continuous rotation accumulated time of the gyroscope, judging whether the continuous rotation accumulated time exceeds set time, if so, performing feedback correction according to the speed error, the attitude error, the position error and the gyroscope zero-offset error estimated by the Kalman filtering equation, and otherwise, performing feedback correction according to the full state quantity estimated by the Kalman filtering equation.
Further, the initial attitude matrix
Figure BDA0003428048900000021
The calculation formula of (2) is as follows:
Figure BDA0003428048900000022
Figure BDA0003428048900000023
Figure BDA0003428048900000024
wherein g is gravity acceleration, omegaieIs the rotational angular velocity of the earth, L is the latitude of the geographic location,
Figure BDA0003428048900000025
respectively outputting increment average values for the accelerometers of the x axis, the y axis and the z axis,
Figure BDA0003428048900000026
the average values of the output increments of the gyroscopes in the x axis, the y axis and the z axis are respectively, n is a navigation coordinate system, b is a carrier coordinate system,
Figure BDA0003428048900000027
the projection of the carrier motion angle of the carrier coordinate system b relative to the geocentric inertial coordinate system i in the carrier coordinate system b is obtained;
Figure BDA0003428048900000028
the projection of the carrier motion angle of the navigation coordinate system n relative to the geocentric inertial coordinate system i in the navigation coordinate system n is obtained; gbThe projection of the gravity acceleration in a carrier coordinate system b is obtained; gnThe projection of the gravity acceleration on a navigation coordinate system n is obtained; r isnThe vector cross multiplication of the outputs of the accelerometer and the gyroscope under the navigation coordinate system n is performed, rbAnd (4) performing cross multiplication on output vectors of the accelerometer and the gyroscope in a carrier coordinate system b.
Further, the specific expression of the filtering feedback compensation is as follows:
the output increment information of the gyroscope after filtering feedback compensation is as follows:
Figure BDA0003428048900000029
the output increment information of the accelerometer after filtering feedback compensation is as follows:
Figure BDA0003428048900000031
wherein,
Figure BDA0003428048900000032
in order to compensate the projection of the gyroscope output increment information in the carrier coordinate system b,
Figure BDA0003428048900000033
projection of the gyroscope output delta information in the carrier coordinate system b, w, before compensationgjA feedback value obtained by zero-offset filtering estimation of the gyroscope; f. ofbFor the projection of the compensated accelerometer output incremental information in the carrier coordinate system b,
Figure BDA0003428048900000034
projection of the accelerometer output increment information in the carrier coordinate system b, f, before compensationgjFeedback value, f, estimated for the accelerometer zero-offset filteringaError of the accelerometer size effect, fa＝[fax,fay,faz]T。
Further, the calculation formula of the accelerometer dimension effect error is as follows:
Figure BDA0003428048900000035
Figure BDA0003428048900000036
Figure BDA0003428048900000037
Figure BDA0003428048900000038
Figure BDA0003428048900000039
Figure BDA00034280489000000310
wherein f isax、fay、fazRespectively x-axis accelerometer dimension effect error, y-axis accelerometer dimension effect error and z-axis accelerometer dimension effect error,
Figure BDA00034280489000000311
in order to compensate the projection of the gyroscope output increment information in the carrier coordinate system b,
Figure BDA00034280489000000312
in order to compensate the projection of the gyroscope angular velocity in the carrier coordinate system b,
Figure BDA00034280489000000313
Figure BDA00034280489000000314
respectively projecting the distances from the accelerometers to the rotation center of the carrier in an x-axis, a y-axis and a z-axis in a carrier coordinate system b; a isbijAnd (i, y, z, j, x, y, z) are respectively the projection of the distances from the accelerometers to the rotation center of the carrier in the x-axis, y-axis and z-axis carrier coordinate systems.
Further, the velocity update equation, the attitude matrix update equation, and the position update equation are respectively:
the velocity update equation:
Figure BDA00034280489000000315
wherein,
Figure BDA00034280489000000316
navigating coordinates for time kIs the speed of the motor at the speed of n,
Figure BDA00034280489000000317
for the velocity in the navigation coordinate system n at time k-1,
Figure BDA00034280489000000318
the accelerometer outputs a projection of the incremental information in the carrier coordinate system b after compensation for time k,
Figure BDA00034280489000000319
for the matrix of the attitude at the time k,
Figure BDA00034280489000000320
is the projection of the carrier motion angle under the navigation coordinate system n,
Figure BDA00034280489000000321
is the projection of the rotational angular velocity of the earth under a navigation coordinate system n, delta t is data sampling time,
Figure BDA00034280489000000322
the projection of the gravity acceleration at the moment k under a navigation coordinate system n;
the attitude matrix updating equation:
Figure BDA0003428048900000041
wherein
Figure BDA0003428048900000042
Comprises the following steps:
Figure BDA0003428048900000043
wherein,
Figure BDA0003428048900000044
is a k-1 time attitude matrix, omegaieIs the rotational angular velocity of the earth, L is the latitude of the geographic location,
Figure BDA0003428048900000045
initially as an identity matrix, i.e.
Figure BDA0003428048900000046
Is a matrix of the units,
Figure BDA0003428048900000047
the update formula of (2) is:
Figure BDA0003428048900000048
Figure BDA0003428048900000049
wherein:
Figure BDA00034280489000000410
Figure BDA00034280489000000411
Figure BDA00034280489000000412
Figure BDA00034280489000000413
wherein,
Figure BDA00034280489000000414
respectively projecting the incremental information output by the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope after the compensation at the moment k in a carrier coordinate system b,
Figure BDA00034280489000000415
Figure BDA00034280489000000416
respectively projecting the incremental information output by the gyroscope of the x axis, the y axis and the z axis after the compensation at the moment of k-1 in a carrier coordinate system b;
position update equation:
Figure BDA00034280489000000417
wherein L isk、λk、hkRespectively, latitude, longitude, altitude, L of the geographic location of the system at time k(k-1)、λ(k-1)、h(k-1)Respectively, the latitude, longitude and altitude of the geographical position of the system at the moment k-1, and R is the radius of the earth.
Further, the error equation of the strapdown inertial navigation system is as follows:
Figure BDA00034280489000000418
Figure BDA00034280489000000419
Figure BDA00034280489000000420
Figure BDA00034280489000000421
Figure BDA00034280489000000422
Figure BDA0003428048900000051
Figure BDA0003428048900000052
Figure BDA0003428048900000053
Figure BDA0003428048900000054
the inertial device error equation is as follows:
Figure BDA0003428048900000055
the accelerometer dimension effect error equation is as follows:
Figure BDA0003428048900000056
wherein, δ vx、δvy、δvzRespectively the speed errors of the system in the east direction, the north direction and the sky direction; phi is ax、φy、φzRespectively a pitch angle error, a roll angle error and an azimuth angle error of the system;
Figure BDA0003428048900000057
respectively is a zero offset error of an east accelerometer, a zero offset error of a north accelerometer and a zero offset error of a sky accelerometer; δ L, δ λ and δ h are respectively a system latitude error, a longitude error and an altitude error; f. ofx、fy、fzRespectively outputting incremental information for the compensated accelerometers; f. ofa＝[fax,fay,faz]T，fax、fay、fazRespectively measuring the size effect errors of the accelerometer on an x axis, a y axis and a z axis; tau isaZero offset correlation time for the accelerometer; epsilon ═ epsilonx,εy,εz]T，εx、εy、εzRespectively an east gyroscope zero bias error, a north gyroscope zero bias error and a sky gyroscope zero bias error; tau isgZero drift correlation time for the gyroscope; omegaieIs groundBall self-rotation angular velocity; rMIs the latitude circle radius of the earth; v. ofx、vy、vzThe system east, north and sky speeds are respectively; rNIs the longitude circle radius of the earth; w is aa、wgWhite noise with zero mean values of an accelerometer and a gyroscope respectively; delta vl＝[δvlx,δvly,δvlz]T，δvlSpeed error due to size effects; cbIs an attitude matrix;
the Kalman filtering state equation is as follows:
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1
wherein, Xk、Xk-1State quantities at time k and time k-1, respectively, phik,k-1For discrete state transition matrices, Γk,k-For system noise-driven arrays, Wk-1A state noise array;
Figure BDA0003428048900000058
W＝[wvx wvy wvz wφx wφy wφz wδL wδλ01×15]T
wherein, wvxIs the x-direction velocity error noise coefficient, wvyIs the y-direction velocity error noise coefficient, wvzIs the z-direction velocity error noise coefficient, wφxIs the x-direction attitude error noise coefficient, wφyIs the y-direction attitude error noise coefficient, wφzIs the z-direction attitude error noise coefficient, wδLAs a latitude error noise coefficient, wδλIs a longitude error noise coefficient;
taking the speed error as an observed quantity, the measurement equation is as follows:
Zk＝HkXk+Vk
wherein Z iskFor Kalman filtering observations, Z is obtained by real-time updating of a velocity update equationk＝[vxk,vyk,vzk]T，HkAs a matrix of observation coefficients, Hk＝[I2×202×21]，VkTo observe the noise array.
Further, the kalman filter equation is:
Figure BDA0003428048900000061
wherein,
Figure BDA0003428048900000062
predicting an estimate for the step;
Figure BDA0003428048900000063
is XkA state estimate of (a); kkA gain matrix for time k; zkIs a kalman filter observation; hkIs an observation coefficient matrix; pk,k-1Estimating a variance matrix for the one-step prediction; pk-1Estimating a variance matrix for the state; rkMeasuring a noise variance matrix; qk-1Is a system noise variance matrix.
Further, when feedback correction is performed according to the full state quantity estimated by the kalman filter equation, a specific correction formula is as follows:
the accelerometer dimension effect error correction formula is as follows:
abxx(k)＝abxx(k-1)+X(15)
abxy(k)＝abxy(k-1)+X(16)
abxz(k)＝abxz(k-1)+X(17)
abyx(k)＝abyx(k-1)+X(18)
abyy(k)＝abyy(k-1)+X(19)
abyz(k)＝abyz(k-1)+X(20)
abzx(k)＝abzx(k-1)+X(21)
abzy(k)＝abzy(k-1)+X(22)
abzz(k)＝abzz(k-1)+X(23)
the zero offset error correction formula of the gyroscope is as follows:
wgjx(k)＝wgjx(k-1)+X(12)
wgjy(k)＝wgjy(k-1)+X(13)
wgjz(k)＝wgjz(k-1)+X(14)
the accelerometer zero offset error correction formula is as follows:
fgjx(k)＝fgjx(k-1)+X(9)
fgjy(k)＝fgjy(k-1)+X(10)
fgjz(k)＝fgjz(k-1)+X(11)
wherein X (i) represents a filtered estimate value, wgjx(k)、wgjy(k)、wgjz(k)Zero-offset filtering estimated value w of x-axis gyroscope, y-axis gyroscope and z-axis gyroscope at k momentgjx(k-1)、wgjy(k-1)、wgjz(k-1)Zero-offset filtering estimated value f of x-axis gyroscope, y-axis gyroscope and z-axis gyroscope at the moment of k-1gjx(k)、fgjy(k)、fgjz(k)Zero-offset filter estimation values f of x-axis, y-axis and z-axis accelerometers at the time kgjx(k-1)、fgjy(k-1)、fgjz(k-1)And the zero offset filtering estimated values of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer at the moment of k-1.
Further, the set time is 2 s.
The invention also provides a high-precision horizontal attitude measurement system for a complex dynamic environment, which comprises:
the acquisition unit is used for acquiring the gravity acceleration information of the geographic position where the strapdown inertial navigation system is located and the output information of the inertial device; a first calculation unit for calculating an initial attitude matrix according to the gravitational acceleration information and the output information of the inertial device
Figure BDA0003428048900000071
The filtering compensation unit is used for respectively carrying out filtering feedback compensation on the output increment information of the accelerometer and the gyroscope to obtain compensated output increment information of the accelerometer and compensated output increment information of the gyroscope;
the first construction unit is used for constructing a speed updating equation, an attitude updating equation and a position updating equation according to an error equation of the strapdown inertial navigation system and an error equation of an inertial device by using the compensated output increment information of the accelerometer and the output increment information of the gyroscope;
the second construction unit is used for constructing a Kalman filtering state equation according to a strapdown inertial navigation system error equation, an inertial device error equation and an accelerometer size effect error equation, and constructing a measurement equation according to the speed updating equation;
the third construction unit is used for constructing a Kalman filtering equation according to the Kalman filtering state equation and the measurement equation;
and the feedback correction unit is used for acquiring the continuous rotation accumulated time of the gyroscope and judging whether the continuous rotation accumulated time exceeds the set time, if so, performing feedback correction according to the speed error, the attitude error, the position error and the gyroscope zero offset error estimated by the Kalman filtering equation, otherwise, performing feedback correction according to the full state quantity estimated by the Kalman filtering equation.
Advantageous effects
Compared with the prior art, the invention has the advantages that:
1. only the gravity acceleration, the accelerometer and the gyroscope output data are utilized to realize the rapid coarse alignment within 10s, and no additional input information is needed;
2. the invention provides a size effect error parameter self-estimation calculation method, which can automatically estimate the distance from the mass center of a product to the rotation center of a radar large disc without providing initial information from the outside;
3. when the size effect error parameters are input externally, the measurement error of the accelerometer caused by the size effect error can be estimated and compensated in real time, so that the measurement accuracy of the accelerometer is improved, and the measurement accuracy of the horizontal attitude is further improved;
4. the invention can be used for judging and controlling according to the continuous rotation accumulated time of the gyroscope, can adapt to the processes of alternative rotation and stop at different rotating speeds, radar antenna erection and the like, ensures the high-precision horizontal attitude measurement of the radar large disc in the leveling process, the radar antenna erection process and the radar antenna in the complex working state, improves the anti-interference capability of the system, and can be suitable for the high-precision horizontal attitude measurement in the complex dynamic environment.
Drawings
In order to more clearly illustrate the technical solution of the present invention, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are only one embodiment of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on the drawings without creative efforts.
FIG. 1 is a flow chart of a high-precision horizontal attitude measurement method for a complex dynamic environment according to an embodiment of the present invention;
fig. 2 is a flow chart of lever arm self-estimation in an embodiment of the present invention.
Detailed Description
The technical solutions in the present invention are clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The technical solution of the present application will be described in detail below with specific examples. The following several specific embodiments may be combined with each other, and details of the same or similar concepts or processes may not be repeated in some embodiments.
As shown in fig. 1, a method for measuring a high-precision horizontal attitude in a complex dynamic environment according to an embodiment of the present invention includes the following steps:
step 1: acquiring the gravity acceleration information of the geographical position of the strapdown inertial navigation system and the output information of the inertial device, and calculating an initial attitude matrix according to the gravity acceleration information and the output information of the inertial device
Figure BDA0003428048900000081
When a strapdown inertial navigation system is initialized, inputting a gravity acceleration g, and calculating an initial attitude matrix by using the gravity acceleration g and the output information of an inertial device in 10s, wherein the specific calculation formula is as follows:
Figure BDA0003428048900000091
Figure BDA0003428048900000092
Figure BDA0003428048900000093
wherein g is gravity acceleration, omegaieIs the rotational angular velocity of the earth, L is the latitude of the geographic location,
Figure BDA0003428048900000094
respectively outputting increment average values for the accelerometers of the x axis, the y axis and the z axis,
Figure BDA0003428048900000095
the average values of the output increments of the gyroscopes in the x axis, the y axis and the z axis are respectively, n is a navigation coordinate system, b is a carrier coordinate system,
Figure BDA0003428048900000096
the projection of the carrier motion angle of the carrier coordinate system b relative to the geocentric inertial coordinate system i in the carrier coordinate system b is obtained;
Figure BDA0003428048900000097
the projection of the carrier motion angle of the navigation coordinate system n relative to the geocentric inertial coordinate system i in the navigation coordinate system n is obtained; gbThe projection of the gravity acceleration in a carrier coordinate system b is obtained; gnThe projection of the gravity acceleration on a navigation coordinate system n is obtained; r isnThe vector cross multiplication of the outputs of the accelerometer and the gyroscope under the navigation coordinate system n is performed, rbAnd (4) performing cross multiplication on output vectors of the accelerometer and the gyroscope in a carrier coordinate system b.
Step 2: and respectively carrying out filtering feedback compensation on the output increment information of the accelerometer and the gyroscope to obtain compensated output increment information of the accelerometer and the gyroscope.
The product cannot be installed at the absolute rotation center of the carrier, so the size effect parameters generally comprise an outer lever arm and an inner lever arm, wherein the outer lever arm is defined as a position vector of the mass center of the product relative to the rotation center of the carrier, and the inner lever arm is defined as a position vector of the mass center of the accelerometer of the x-axis, the y-axis and the z-axis relative to the origin of a b system of a carrier coordinate system. When the external or internal structure of the product is complex and accurate lever arm parameters are not easily obtained, the accurate estimation and compensation are needed (the specific estimation process is shown in fig. 2) to improve the measurement accuracy of the accelerometer, and further improve the measurement accuracy of the horizontal attitude.
In this embodiment, the inner and outer lever arms are not distinguished, and the lever arms from the accelerometers on the x-axis, the y-axis and the z-axis of the product to the rotation center of the carrier on which the product is mounted (for example, the distance from the accelerometer on the product mounted on the radar disk to the rotation center of the radar disk) are set, and the projections are projected under the x-axis, the y-axis and the z-axis of the carrier coordinate system
Figure BDA0003428048900000098
Comprises the following steps:
Figure BDA0003428048900000099
wherein, abij(i, y, z, j, x, y, z) are respectively the projection of the distances from the accelerometer of the x axis, the y axis and the z axis to the rotation center of the carrier under the x axis, the y axis and the z axis of the carrier coordinate system; for example abxxRespectively projection of the distance from the accelerometer on the x axis to the rotation center of the carrier under the x axis of the carrier coordinate system, abyzRespectively, the projection of the distance from the accelerometer on the y axis to the rotation center of the carrier under the z axis of the carrier coordinate system. If no external input initial value is available, then abijThe initial value of (2) is 0, if the initial value is inputted from the outside,then abijIs an input value and is then updated according to equations (19) and (21).
Accelerometer measurement error f caused by size effectax、fay、fazThe formula for calculation (i.e., the size effect error) is:
Figure BDA0003428048900000101
therefore, the output increment information of the gyroscope after filtering feedback compensation is as follows:
Figure BDA0003428048900000102
the output increment information of the accelerometer after filtering feedback compensation is as follows:
Figure BDA0003428048900000103
wherein,
Figure BDA0003428048900000104
in order to compensate the projection of the gyroscope output increment information in the carrier coordinate system b,
Figure BDA0003428048900000105
projection of the gyroscope output delta information in the carrier coordinate system b, w, before compensationgjA feedback value obtained by zero-offset filtering estimation of the gyroscope; f. ofbFor the projection of the compensated accelerometer output incremental information in the carrier coordinate system b,
Figure BDA0003428048900000106
projection of the accelerometer output increment information in the carrier coordinate system b, f, before compensationgjFeedback value, f, estimated for the accelerometer zero-offset filteringaError of the accelerometer size effect, fa＝[fax,fay,faz]T。wgj、fgjHas an initial value of 0.
And step 3: and constructing a speed updating equation, an attitude updating equation and a position updating equation according to the error equation of the strapdown inertial navigation system and the error equation of the inertial device by using the compensated output increment information of the accelerometer and the compensated output increment information of the gyroscope.
The velocity update equation:
Figure BDA0003428048900000107
wherein,
Figure BDA0003428048900000108
for the velocity at time k in the navigation coordinate system n,
Figure BDA0003428048900000109
for the velocity in the navigation coordinate system n at time k-1,
Figure BDA00034280489000001010
the accelerometer outputs a projection of the incremental information in the carrier coordinate system b after compensation for time k,
Figure BDA00034280489000001011
for the matrix of the attitude at the time k,
Figure BDA00034280489000001012
is the projection of the carrier motion angle under the navigation coordinate system n,
Figure BDA00034280489000001013
is the projection of the rotational angular velocity of the earth under a navigation coordinate system n, delta t is data sampling time,
Figure BDA00034280489000001014
is the projection of the gravity acceleration at the moment k in the navigation coordinate system n.
The attitude matrix updating equation:
Figure BDA0003428048900000111
wherein
Figure BDA0003428048900000112
Comprises the following steps:
Figure BDA0003428048900000113
wherein,
Figure BDA0003428048900000114
is a k-1 time attitude matrix, omegaieIs the rotational angular velocity of the earth, L is the latitude of the geographic location,
Figure BDA0003428048900000115
initially as an identity matrix, i.e.
Figure BDA0003428048900000116
Is a matrix of the units,
Figure BDA0003428048900000117
the update formula of (2) is:
Figure BDA0003428048900000118
Figure BDA0003428048900000119
wherein:
Figure BDA00034280489000001110
Figure BDA00034280489000001111
wherein,
Figure BDA00034280489000001112
respectively projecting the incremental information output by the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope after the compensation at the moment k in a carrier coordinate system b,
Figure BDA00034280489000001113
Figure BDA00034280489000001114
and respectively projecting the incremental information output by the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope after the compensation at the moment of k-1 in a carrier coordinate system b.
Position update equation:
Figure BDA00034280489000001115
wherein L isk、λk、hkRespectively, latitude, longitude, altitude, L of the geographic location of the system at time k(k-1)、λ(k-1)、h(k-1)Respectively, the latitude, longitude and altitude of the geographical position of the system at the moment k-1, and R is the radius of the earth.
System speed, position and attitude information can be obtained from equations (6), (7) and (12).
And 4, step 4: and constructing a Kalman filtering state equation according to an error equation of the strapdown inertial navigation system, an error equation of an inertial device and an error equation of an accelerometer size effect, and constructing a measurement equation according to a speed updating equation.
In order to simplify the formula, the superscript n is omitted, and the error equation of the strapdown inertial navigation system is as follows:
Figure BDA0003428048900000121
Figure BDA0003428048900000122
Figure BDA0003428048900000123
Figure BDA0003428048900000124
Figure BDA0003428048900000125
Figure BDA0003428048900000126
Figure BDA0003428048900000127
Figure BDA0003428048900000128
the inertial device error equation is:
Figure BDA0003428048900000129
the accelerometer dimension effect error equation is:
Figure BDA00034280489000001210
wherein, δ vx、δvy、δvzRespectively the speed errors of the system in the east direction, the north direction and the sky direction; phi is ax、φy、φzRespectively a pitch angle error, a roll angle error and an azimuth angle error of the system;
Figure BDA00034280489000001211
respectively is a zero offset error of an east accelerometer, a zero offset error of a north accelerometer and a zero offset error of a sky accelerometer; δ L, δ λ and δ h are respectively a system latitude error, a longitude error and an altitude error; f. ofx、fy、fzRespectively compensated accelerometer outputsOutputting incremental information; f. ofa＝[fax,fay,faz]T，fax、fay、fazRespectively measuring the size effect errors of the accelerometer on an x axis, a y axis and a z axis; tau isaZero offset correlation time for the accelerometer; epsilon ═ epsilonx,εy,εz]T，εx、εy、εzRespectively an east gyroscope zero bias error, a north gyroscope zero bias error and a sky gyroscope zero bias error; tau isgZero drift correlation time for the gyroscope; omegaieThe rotational angular velocity of the earth; rMIs the latitude circle radius of the earth; v. ofx、vy、vzThe system east, north and sky speeds are respectively; rNIs the longitude circle radius of the earth; w is aa、wgWhite noise with zero mean values of an accelerometer and a gyroscope respectively; delta vl＝[δvlx,δvly,δvlz]T，δvlSpeed error due to size effects; cbIs a matrix of poses. East, north and sky are the three axes of the system navigation coordinate system.
Constructing a Kalman filtering state equation according to the formulas (13) to (15), and discretizing to obtain:
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1 (16)
wherein, Xk、Xk-1State quantities at time k and time k-1, respectively, phik,k-1For discrete state transition matrices, Γk,k-1W is a system noise driving matrix (obtained by equations (13) to (15))k-1A state noise array;
Figure BDA0003428048900000131
W＝[wvx wvy wvz wφx wφy wφz wδL wδλ01×15]T
wherein, wvxIs x-direction velocity errorNoise coefficient, wvyIs the y-direction velocity error noise coefficient, wvzIs the z-direction velocity error noise coefficient, wφxIs the x-direction attitude error noise coefficient, wφyIs the y-direction attitude error noise coefficient, wφzIs the z-direction attitude error noise coefficient, wδLAs a latitude error noise coefficient, wδλIs the longitude error noise coefficient. The specific values of these error noise coefficients are generally derived from engineering experience and are generally zero-mean white noise.
Taking the speed error as an observed quantity, the measurement equation is as follows:
Zk＝HkXk+Vk (17)
wherein Z iskFor Kalman filtering observations, Z is obtained by real-time updating of a velocity update equationk＝[vxk,vyk,vzk]T，
HkAs a matrix of observation coefficients, Hk＝[I2×202×21]，VkTo observe the noise array.
And 5: constructing a Kalman filtering equation according to a Kalman filtering state equation and a measurement equation, which specifically comprises the following steps:
Figure BDA0003428048900000132
wherein,
Figure BDA0003428048900000133
predicting an estimate for the step;
Figure BDA0003428048900000134
is XkA state estimate of (a); kkA gain matrix for time k; zkIs a kalman filter observation; hkIs an observation coefficient matrix; pk,k-1Estimating a variance matrix for the one-step prediction; pk-1Estimating a variance matrix for the state; rkMeasuring a noise variance matrix; qk-1Is a system noise variance matrix.
Step 6: and acquiring the continuous rotation accumulated time of the gyroscope, judging whether the continuous rotation accumulated time exceeds the set time, if so, performing feedback correction according to the speed error, the attitude error, the position error and the gyroscope zero-offset error estimated by the Kalman filtering equation, and otherwise, performing feedback correction according to the total state quantity estimated by the Kalman filtering equation.
When the radar vehicle is in a leveling state and the radar antenna is in a vertical state, instantaneous shaking occurs, if a filtering algorithm is adopted, filtering estimation is inaccurate (navigation filtering convergence estimation requires time, and therefore instantaneous shaking cannot be accurately estimated), therefore, the method takes the continuous rotation accumulated time of the gyroscope as a basis, only partial feedback correction is carried out when the continuous rotation accumulated time of the gyroscope is less than set time, namely, the size effect error and the accelerometer zero offset error are not corrected and compensated, and the estimation result at the last moment is still used; and when the continuous rotation accumulated time of the gyroscope is more than or equal to the set time, performing full-state feedback.
In this embodiment, the set time is 2 s.
If partial feedback is carried out, the size effect error estimation value and the accelerometer zero offset error are not corrected, and the method specifically comprises the following steps:
Figure BDA0003428048900000141
Figure BDA0003428048900000142
if the full-state feedback is carried out, the specific correction formula is as follows:
the accelerometer dimension effect error correction formula is as follows:
Figure BDA0003428048900000143
the zero offset error correction formula of the gyroscope is as follows:
Figure BDA0003428048900000144
the accelerometer zero offset error correction formula is as follows:
Figure BDA0003428048900000151
wherein X (i) represents a filtered estimate value, wgjx(k)、wgjy(k)、wgjz(k)Zero-offset filtering estimated value w of x-axis gyroscope, y-axis gyroscope and z-axis gyroscope at k momentgjx(k-1)、wgjy(k-1)、wgjz(k-1)Zero-offset filtering estimated value f of x-axis gyroscope, y-axis gyroscope and z-axis gyroscope at the moment of k-1gjx(k)、fgjy(k)、fgjz(k)Zero-offset filter estimation values f of x-axis, y-axis and z-axis accelerometers at the time kgjx(k-1)、fgjy(k-1)、fgjz(k-1)And the zero offset filtering estimated values of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer at the moment of k-1.
The embodiment of the invention also provides a high-precision horizontal attitude measurement system for a complex dynamic environment, which comprises:
and the acquisition unit is used for acquiring the gravity acceleration information of the geographic position of the strapdown inertial navigation system and the output information of the inertial device.
A first calculation unit for calculating an initial attitude matrix according to the gravitational acceleration information and the output information of the inertial device
Figure BDA0003428048900000152
As shown in formula (1).
And the filtering compensation unit is used for respectively carrying out filtering feedback compensation on the output increment information of the accelerometer and the gyroscope to obtain compensated output increment information of the accelerometer and the compensated output increment information of the gyroscope, wherein the compensated output increment information of the accelerometer and the compensated output increment information of the gyroscope are shown in formulas (2) to (5).
And the first construction unit is used for constructing a speed updating equation, an attitude updating equation and a position updating equation according to the error equation of the strapdown inertial navigation system and the error equation of the inertial device by using the compensated output increment information of the accelerometer and the compensated output increment information of the gyroscope, wherein the equations are shown in equations (6) to (12).
And the second construction unit is used for constructing a Kalman filtering state equation (shown in a formula (16)) according to the error equation of the strapdown inertial navigation system, the error equation of the inertial device and the error equation of the accelerometer size effect, and constructing a measurement equation (shown in a formula (17)) according to the speed updating equation.
And the third construction unit is used for constructing a Kalman filtering equation according to the Kalman filtering state equation and the measurement equation, and the equation is shown as the formula (18).
And the feedback correction unit is used for acquiring the continuous rotation accumulated time of the gyroscope and judging whether the continuous rotation accumulated time exceeds the set time, if so, performing feedback correction according to the speed error, the attitude error, the position error and the gyroscope zero offset error estimated by the Kalman filtering equation, otherwise, performing feedback correction according to the full state quantity estimated by the Kalman filtering equation (shown in formulas (21) to (23)).
The above disclosure is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of changes or modifications within the technical scope of the present invention, and shall be covered by the scope of the present invention.

Claims (10)

1. A high-precision horizontal attitude measurement method for a complex dynamic environment is characterized by comprising the following steps:
acquiring the gravity acceleration information of the geographical position of the strapdown inertial navigation system and the output information of an inertial device, and calculating an initial attitude matrix according to the gravity acceleration information and the output information of the inertial device
Figure FDA0003428048890000011
Respectively carrying out filtering feedback compensation on the output increment information of the accelerometer and the gyroscope to obtain compensated output increment information of the accelerometer and the gyroscope;
constructing a speed updating equation, an attitude updating equation and a position updating equation according to an error equation of the strapdown inertial navigation system and an error equation of an inertial device by using the compensated output increment information of the accelerometer and the compensated output increment information of the gyroscope;
constructing a Kalman filtering state equation according to an error equation of a strapdown inertial navigation system, an error equation of an inertial device and an error equation of an accelerometer size effect, and constructing a measurement equation according to the speed updating equation;
constructing a Kalman filtering equation according to the Kalman filtering state equation and the measurement equation;
and acquiring continuous rotation accumulated time of the gyroscope, judging whether the continuous rotation accumulated time exceeds set time, if so, performing feedback correction according to the speed error, the attitude error, the position error and the gyroscope zero-offset error estimated by the Kalman filtering equation, and otherwise, performing feedback correction according to the full state quantity estimated by the Kalman filtering equation.
2. The complex dynamic environment high accuracy horizontal attitude measurement method of claim 1 in which the initial attitude matrix
Figure FDA0003428048890000012
The calculation formula of (2) is as follows:
Figure FDA0003428048890000013
Figure FDA0003428048890000014
Figure FDA0003428048890000015
wherein g is gravity acceleration, omegaieIs the rotational angular velocity of the earth, L is the latitude of the geographic location,
Figure FDA0003428048890000016
respectively outputting increment average values for the accelerometers of the x axis, the y axis and the z axis,
Figure FDA0003428048890000017
the average values of the output increments of the gyroscopes in the x axis, the y axis and the z axis are respectively, n is a navigation coordinate system, b is a carrier coordinate system,
Figure FDA0003428048890000018
the projection of the carrier motion angle of the carrier coordinate system b relative to the geocentric inertial coordinate system i in the carrier coordinate system b is obtained;
Figure FDA0003428048890000019
the projection of the carrier motion angle of the navigation coordinate system n relative to the geocentric inertial coordinate system i in the navigation coordinate system n is obtained; gbThe projection of the gravity acceleration in a carrier coordinate system b is obtained; gnThe projection of the gravity acceleration on a navigation coordinate system n is obtained; r isnThe vector cross multiplication of the outputs of the accelerometer and the gyroscope under the navigation coordinate system n is performed, rbAnd (4) performing cross multiplication on output vectors of the accelerometer and the gyroscope in a carrier coordinate system b.
3. The complex dynamic environment high-precision horizontal attitude measurement method according to claim 1, wherein the specific expression of the filter feedback compensation is as follows:
the output increment information of the gyroscope after filtering feedback compensation is as follows:
Figure FDA0003428048890000021
the output increment information of the accelerometer after filtering feedback compensation is as follows:
Figure FDA0003428048890000022
wherein,
Figure FDA0003428048890000023
in order to compensate the projection of the gyroscope output increment information in the carrier coordinate system b,
Figure FDA0003428048890000024
projection of the gyroscope output delta information in the carrier coordinate system b, w, before compensationgjA feedback value obtained by zero-offset filtering estimation of the gyroscope; f. ofbFor the projection of the compensated accelerometer output incremental information in the carrier coordinate system b,
Figure FDA0003428048890000025
Figure FDA0003428048890000026
projection of the accelerometer output increment information in the carrier coordinate system b, f, before compensationgjFeedback value, f, estimated for the accelerometer zero-offset filteringaError of the accelerometer size effect, fa＝[fax,fay,faz]T。
4. The complex dynamic environment high accuracy horizontal attitude measurement method of claim 3, wherein the accelerometer dimension effect error is calculated by the formula:
Figure FDA0003428048890000027
Figure FDA0003428048890000028
Figure FDA0003428048890000029
Figure FDA00034280488900000210
Figure FDA00034280488900000211
Figure FDA00034280488900000212
wherein f isax、fay、fazRespectively x-axis accelerometer dimension effect error, y-axis accelerometer dimension effect error and z-axis accelerometer dimension effect error,
Figure FDA00034280488900000213
in order to compensate the projection of the gyroscope output increment information in the carrier coordinate system b,
Figure FDA00034280488900000214
in order to compensate the projection of the gyroscope angular velocity in the carrier coordinate system b,
Figure FDA00034280488900000215
Figure FDA00034280488900000216
respectively projecting the distances from the accelerometers to the rotation center of the carrier in an x-axis, a y-axis and a z-axis in a carrier coordinate system b; a isbijAnd (i, y, z, j, x, y, z) are respectively the projection of the distances from the accelerometers to the rotation center of the carrier in the x-axis, y-axis and z-axis carrier coordinate systems.
5. The complex dynamic environment high-precision horizontal attitude measurement method according to claim 1, wherein the velocity update equation, the attitude matrix update equation and the position update equation are respectively:
the velocity update equation:
Figure FDA00034280488900000217
wherein,
Figure FDA0003428048890000031
for the velocity at time k in the navigation coordinate system n,
Figure FDA0003428048890000032
Figure FDA0003428048890000033
for the velocity in the navigation coordinate system n at time k-1,
Figure FDA0003428048890000034
fk bthe accelerometer outputs a projection of the incremental information in the carrier coordinate system b after compensation for time k,
Figure FDA0003428048890000035
Figure FDA0003428048890000036
for the matrix of the attitude at the time k,
Figure FDA0003428048890000037
is the projection of the carrier motion angle under the navigation coordinate system n,
Figure FDA0003428048890000038
is the projection of the rotational angular velocity of the earth under a navigation coordinate system n, delta t is data sampling time,
Figure FDA0003428048890000039
the projection of the gravity acceleration at the moment k under a navigation coordinate system n;
the attitude matrix updating equation:
Figure FDA00034280488900000310
wherein
Figure FDA00034280488900000311
Comprises the following steps:
Figure FDA00034280488900000312
wherein,
Figure FDA00034280488900000313
is a k-1 time attitude matrix, omegaieIs the rotational angular velocity of the earth, L is the latitude of the geographic location,
Figure FDA00034280488900000314
initially as an identity matrix, i.e.
Figure FDA00034280488900000315
Is a matrix of the units,
Figure FDA00034280488900000316
the update formula of (2) is:
Figure FDA00034280488900000317
Figure FDA00034280488900000318
wherein:
Figure FDA00034280488900000319
Figure FDA00034280488900000320
Figure FDA00034280488900000321
Figure FDA00034280488900000322
wherein,
Figure FDA00034280488900000323
Figure FDA00034280488900000324
respectively projecting the incremental information output by the x-axis gyroscope, the y-axis gyroscope and the z-axis gyroscope after the compensation at the moment k in a carrier coordinate system b,
Figure FDA00034280488900000325
Figure FDA00034280488900000326
Figure FDA00034280488900000327
respectively projecting the incremental information output by the gyroscope of the x axis, the y axis and the z axis after the compensation at the moment of k-1 in a carrier coordinate system b;
position update equation:
Figure FDA00034280488900000328
wherein L isk、λk、hkRespectively, latitude, longitude, altitude, L of the geographic location of the system at time k(k-1)、λ(k-1)、h(k-1)Respectively, the latitude, longitude and altitude of the geographical position of the system at the moment k-1, and R is the radius of the earth.
6. The complex dynamic environment high-precision horizontal attitude measurement method according to claim 1, wherein the strapdown inertial navigation system error equation is:
Figure FDA0003428048890000041
Figure FDA0003428048890000042
Figure FDA0003428048890000043
Figure FDA0003428048890000044
Figure FDA0003428048890000045
Figure FDA0003428048890000046
Figure FDA0003428048890000047
Figure FDA0003428048890000048
Figure FDA0003428048890000049
the inertial device error equation is as follows:
Figure FDA00034280488900000410
the accelerometer dimension effect error equation is as follows:
Figure FDA00034280488900000411
wherein, δ vx、δvy、δvzRespectively the speed errors of the system in the east direction, the north direction and the sky direction; phi is ax、φy、φzRespectively a pitch angle error, a roll angle error and an azimuth angle error of the system; v [x,▽y,▽z]T，▽x、▽y、▽zRespectively is a zero offset error of an east accelerometer, a zero offset error of a north accelerometer and a zero offset error of a sky accelerometer; δ L, δ λ and δ h are respectively a system latitude error, a longitude error and an altitude error; f. ofx、fy、fzRespectively outputting incremental information for the compensated accelerometers; f. ofa＝[fax,fay,faz]T，fax、fay、fazRespectively measuring the size effect errors of the accelerometer on an x axis, a y axis and a z axis; tau isaZero offset correlation time for the accelerometer; epsilon ═ epsilonx,εy,εz]T，εx、εy、εzRespectively an east gyroscope zero bias error, a north gyroscope zero bias error and a sky gyroscope zero bias error; tau isgZero drift correlation time for the gyroscope; omegaieThe rotational angular velocity of the earth; rMIs the latitude circle radius of the earth; v. ofx、vy、vzThe system east, north and sky speeds are respectively; rNIs the longitude circle radius of the earth; w is aa、wgWhite noise with zero mean values of an accelerometer and a gyroscope respectively; delta vl＝[δvlx,δvly,δvlz]T，δvlSpeed error due to size effects; cbIs an attitude matrix;
the Kalman filtering state equation is as follows:
Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1
wherein, Xk、Xk-1State quantities at time k and time k-1, respectively, phik,k-1For discrete state transition matrices, Γk,k-1For system noise-driven arrays, Wk-1A state noise array;
X＝[δvx δvy δvz φx φy φz δL δλ ▽x ▽y ▽z εx εy εz abxx abxy abxz abyx abyyabyz abzx abzy abzz]T
W＝[wvx wvy wvz wφx wφy wφz wδL wδλ 01×15]T
wherein, wvxIs the x-direction velocity error noise coefficient, wvyIs the y-direction velocity error noise coefficient, wvzIs the z-direction velocity error noise coefficient, wφxIs the x-direction attitude error noise coefficient, wφyIs the y-direction attitude error noise coefficient, wφzIs the z-direction attitude error noise coefficient, wδLAs a latitude error noise coefficient, wδλIs a longitude error noise coefficient;
taking the speed error as an observed quantity, the measurement equation is as follows:
Zk＝HkXk+Vk
wherein Z iskFor Kalman filtering observations, Z is obtained by real-time updating of a velocity update equationk＝[vxk,vyk,vzk]T，HkAs a matrix of observation coefficients, Hk＝[I2×2 02×21]，VkTo observe the noise array.
7. The complex dynamic environment high-precision horizontal attitude measurement method according to any one of claims 1-6, characterized in that the Kalman filter equation is as follows:
Figure FDA0003428048890000051
wherein,
Figure FDA0003428048890000052
predicting an estimate for the step;
Figure FDA0003428048890000053
is XkA state estimate of (a); kkA gain matrix for time k; zkIs a kalman filter observation; hkIs an observation coefficient matrix; pk,k-1Estimating a variance matrix for the one-step prediction; pk-1Estimating a variance matrix for the state; rkMeasuring a noise variance matrix; qk-1Is a system noise variance matrix.
8. The method for measuring the high-precision horizontal attitude in the complex dynamic environment according to claim 7, wherein when the feedback correction is performed according to the full state quantity estimated by the Kalman filtering equation, the specific correction formula is as follows:
the accelerometer dimension effect error correction formula is as follows:
abxx(k)＝abxx(k-1)+X (15)
abxy(k)＝abxy(k-1)+X (16)
abxz(k)＝abxz(k-1)+X (17)
abyx(k)＝abyx(k-1)+X (18)
abyy(k)＝abyy(k-1)+X (19)
abyz(k)＝abyz(k-1)+X (20)
abzx(k)＝abzx(k-1)+X (21)
abzy(k)＝abzy(k-1)+X (22)
abzz(k)＝abzz(k-1)+X (23)
the zero offset error correction formula of the gyroscope is as follows:
wgjx(k)＝wgjx(k-1)+X (12)
wgjy(k)＝wgjy(k-1)+X (13)
wgjz(k)＝wgjz(k-1)+X (14)
the accelerometer zero offset error correction formula is as follows:
fgjx(k)＝fgjx(k-1)+X (9)
fgjy(k)＝fgjy(k-1)+X (10)
fgjz(k)＝fgjz(k-1)+X (11)
wherein X (i) represents a filtered estimate value, wgjx(k)、wgjy(k)、wgjz(k)Zero-offset filtering estimated value w of x-axis gyroscope, y-axis gyroscope and z-axis gyroscope at k momentgjx(k-1)、wgjy(k-1)、wgjz(k-1)Zero-offset filtering estimated value f of x-axis gyroscope, y-axis gyroscope and z-axis gyroscope at the moment of k-1gjx(k)、fgjy(k)、fgjz(k)Zero-offset filter estimation values f of x-axis, y-axis and z-axis accelerometers at the time kgjx(k-1)、fgjy(k-1)、fgjz(k-1)And the zero offset filtering estimated values of the x-axis accelerometer, the y-axis accelerometer and the z-axis accelerometer at the moment of k-1.
9. The complex dynamic environment high accuracy horizontal attitude measurement method of claim 8, wherein the set time is 2 s.
10. A high-precision horizontal attitude measurement system for a complex dynamic environment is characterized by comprising:
the acquisition unit is used for acquiring the gravity acceleration information of the geographic position where the strapdown inertial navigation system is located and the output information of the inertial device;
a first calculation unit for calculating an initial attitude matrix according to the gravitational acceleration information and the output information of the inertial device
Figure FDA0003428048890000061
The filtering compensation unit is used for respectively carrying out filtering feedback compensation on the output increment information of the accelerometer and the gyroscope to obtain compensated output increment information of the accelerometer and compensated output increment information of the gyroscope;
the first construction unit is used for constructing a speed updating equation, an attitude updating equation and a position updating equation according to an error equation of the strapdown inertial navigation system and an error equation of an inertial device by using the compensated output increment information of the accelerometer and the output increment information of the gyroscope;
the second construction unit is used for constructing a Kalman filtering state equation according to a strapdown inertial navigation system error equation, an inertial device error equation and an accelerometer size effect error equation, and constructing a measurement equation according to the speed updating equation;
the third construction unit is used for constructing a Kalman filtering equation according to the Kalman filtering state equation and the measurement equation;
and the feedback correction unit is used for acquiring the continuous rotation accumulated time of the gyroscope and judging whether the continuous rotation accumulated time exceeds the set time, if so, performing feedback correction according to the speed error, the attitude error, the position error and the gyroscope zero offset error estimated by the Kalman filtering equation, otherwise, performing feedback correction according to the full state quantity estimated by the Kalman filtering equation.
CN202111587433.8A 2021-12-23 2021-12-23 High-precision horizontal attitude measurement method and system for complex dynamic environment Active CN114264304B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111587433.8A CN114264304B (en) 2021-12-23 2021-12-23 High-precision horizontal attitude measurement method and system for complex dynamic environment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111587433.8A CN114264304B (en) 2021-12-23 2021-12-23 High-precision horizontal attitude measurement method and system for complex dynamic environment

Publications (2)

Publication Number Publication Date
CN114264304A true CN114264304A (en) 2022-04-01
CN114264304B CN114264304B (en) 2023-07-14

Family

ID=80829001

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111587433.8A Active CN114264304B (en) 2021-12-23 2021-12-23 High-precision horizontal attitude measurement method and system for complex dynamic environment

Country Status (1)

Country Link
CN (1) CN114264304B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116087900A (en) * 2023-03-10 2023-05-09 中安锐达(北京)电子科技有限公司 Inter-travel detection vehicle-mounted platform for one-dimensional phased array radar

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561292A (en) * 2009-05-12 2009-10-21 北京航空航天大学 Method and device for calibrating size effect error of accelerometer
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN110398257A (en) * 2019-07-17 2019-11-01 哈尔滨工程大学 The quick initial alignment on moving base method of SINS system of GPS auxiliary
CN110440756A (en) * 2019-06-28 2019-11-12 中国船舶重工集团公司第七0七研究所 A kind of inertial navigation system Attitude estimation method
CN112255624A (en) * 2020-09-30 2021-01-22 湖南航天机电设备与特种材料研究所 High-precision horizontal attitude measurement method and system
CN112504275A (en) * 2020-11-16 2021-03-16 哈尔滨工程大学 Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm
US20210348924A1 (en) * 2020-05-11 2021-11-11 Institute Of Geology And Geophysics, Chinese Academy Of Sciences Attitude measurement method

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561292A (en) * 2009-05-12 2009-10-21 北京航空航天大学 Method and device for calibrating size effect error of accelerometer
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN110440756A (en) * 2019-06-28 2019-11-12 中国船舶重工集团公司第七0七研究所 A kind of inertial navigation system Attitude estimation method
CN110398257A (en) * 2019-07-17 2019-11-01 哈尔滨工程大学 The quick initial alignment on moving base method of SINS system of GPS auxiliary
US20210348924A1 (en) * 2020-05-11 2021-11-11 Institute Of Geology And Geophysics, Chinese Academy Of Sciences Attitude measurement method
CN112255624A (en) * 2020-09-30 2021-01-22 湖南航天机电设备与特种材料研究所 High-precision horizontal attitude measurement method and system
CN112504275A (en) * 2020-11-16 2021-03-16 哈尔滨工程大学 Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
GAO PENGYU ET AL.: "An Accelerometers-Size-Effect Self-Calibration Method for Triaxis Rotational Inertial Navigation System", 《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》, vol. 65, no. 2, pages 1655 - 1664 *
周斌;王巍;: "捷联惯导***尺寸效应误差及其补偿算法", 中国惯性技术学报, no. 06, pages 5 - 8 *
常振军等: "基于尺寸效应在线补偿的旋转捷联惯性导航***初始对准", 《兵工学报》, vol. 41, no. 10, pages 2016 - 2022 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116087900A (en) * 2023-03-10 2023-05-09 中安锐达(北京)电子科技有限公司 Inter-travel detection vehicle-mounted platform for one-dimensional phased array radar

Also Published As

Publication number Publication date
CN114264304B (en) 2023-07-14

Similar Documents

Publication Publication Date Title
CN111678538B (en) Dynamic level error compensation method based on speed matching
CN110221332B (en) Dynamic lever arm error estimation and compensation method for vehicle-mounted GNSS/INS integrated navigation
CN110501024B (en) Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN107270893B (en) Lever arm and time asynchronous error estimation and compensation method for real estate measurement
CN111156994B (en) INS/DR & GNSS loose combination navigation method based on MEMS inertial component
CN108051866B (en) Based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation Gravimetric Method
CN110926468B (en) Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment
CN110631574B (en) inertia/odometer/RTK multi-information fusion method
RU2269813C2 (en) Method for calibrating parameters of platform-less inertial measuring module
CN104880189B (en) A kind of antenna for satellite communication in motion low cost tracking anti-interference method
CN111024074B (en) Inertial navigation speed error determination method based on recursive least square parameter identification
CN111006675A (en) Self-calibration method of vehicle-mounted laser inertial navigation system based on high-precision gravity model
CN109612460A (en) One kind being based on static modified deviation of plumb line measurement method
CN114877915B (en) Device and method for calibrating g sensitivity error of laser gyro inertia measurement assembly
CN113340298A (en) Inertial navigation and dual-antenna GNSS external reference calibration method
CN109084755B (en) Accelerometer zero offset estimation method based on gravity apparent velocity and parameter identification
CN112697143B (en) High-precision carrier dynamic attitude measurement method and system
CN114264304B (en) High-precision horizontal attitude measurement method and system for complex dynamic environment
CN111141285B (en) Aviation gravity measuring device
CN113551669A (en) Short baseline-based combined navigation positioning method and device
CN110940357B (en) Inner rod arm calibration method for self-alignment of rotary inertial navigation single shaft
CN112798014A (en) Inertial navigation self-alignment method for compensating vertical line deviation based on gravitational field spherical harmonic model
CN117053802A (en) Method for reducing positioning error of vehicle navigation system based on rotary MEMS IMU
CN116576849A (en) Vehicle fusion positioning method and system based on GMM assistance
CN114674345B (en) Inertial navigation/camera/laser velocimeter online joint calibration method

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
GR01 Patent grant
GR01 Patent grant