CN114111770A - Horizontal attitude measurement method, system, processing equipment and storage medium - Google Patents
Horizontal attitude measurement method, system, processing equipment and storage medium Download PDFInfo
- Publication number
- CN114111770A CN114111770A CN202111373775.XA CN202111373775A CN114111770A CN 114111770 A CN114111770 A CN 114111770A CN 202111373775 A CN202111373775 A CN 202111373775A CN 114111770 A CN114111770 A CN 114111770A
- Authority
- CN
- China
- Prior art keywords
- carrier
- angle
- attitude
- attitude angle
- 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
Links
- 238000012545 processing Methods 0.000 title claims abstract description 19
- 238000000691 measurement method Methods 0.000 title claims abstract description 16
- 238000003860 storage Methods 0.000 title claims abstract description 11
- 238000000034 method Methods 0.000 claims abstract description 38
- 238000004364 calculation method Methods 0.000 claims abstract description 33
- 230000003068 static effect Effects 0.000 claims abstract description 31
- 239000011159 matrix material Substances 0.000 claims description 19
- 238000001914 filtration Methods 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 12
- 230000004927 fusion Effects 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000000295 complement effect Effects 0.000 description 7
- 230000008569 process Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 3
- 238000009826 distribution Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
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
-
- 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
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
Abstract
The invention relates to a horizontal attitude measurement method, a system, a processing device and a storage medium, which are characterized by comprising the following steps: the method comprises the steps of placing an MIMU on a carrier, and carrying out attitude calculation on gyro output data of the MIMU according to the initial attitude of the carrier to obtain an attitude angle of the carrier; judging the static state of the carrier, and calculating the horizontal attitude angle of the carrier entering the static state and the noise dynamic observation variance array of the carrier in real time according to the output data of the accelerometer in the MIMU; determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time; determining gyro zero offset of the MIMU according to the noise dynamic observation variance array and the misalignment angle of the carrier, and obtaining an updated carrier attitude angle; the updated attitude angle of the carrier and the horizontal attitude angle of the carrier entering the static state are fused to determine the horizontal attitude angle of the carrier.
Description
Technical Field
The present invention relates to the field of inertial technology, and in particular, to a horizontal attitude measurement method, system, processing device, and storage medium.
Background
The MIMU (Micro Inertial Measurement Unit) has become a preferred device for a small unmanned aerial vehicle, an unmanned vehicle, a miniature stable platform and a human body wearable intelligent device due to the characteristics of small volume, low power consumption, low cost and the like, and is applied to attitude Measurement and navigation positioning. At present, the method applied to MIMU attitude measurement mainly utilizes the characteristic that the precision and stability of an accelerometer in the MIMU are far higher than the level of a gyro device, and combines the calculation attitude of a gyro and the estimation attitude of the accelerometer through methods such as complementary filtering under the condition of considering the static or quasi-static state in the motion process of a carrier, so as to achieve the purpose of improving the measurement precision of the horizontal attitude of the carrier.
However, when the complementary filtering method is adopted for fusion at present, the accuracy of the horizontal attitude estimation of the accelerometer cannot be estimated more accurately only by relying on empirical values for weight setting of different attitudes or based on the first-order linear relation of the amplitude of the accelerometer. Therefore, a method capable of dynamically calculating the estimation accuracy of the horizontal attitude of the accelerometer in real time is needed, so that the attitude fusion calculation can be performed more accurately.
Disclosure of Invention
In view of the above problems, it is an object of the present invention to provide a horizontal attitude measurement method, system, processing device, and storage medium, which can dynamically calculate the estimation accuracy of the horizontal attitude of an accelerometer in real time.
In order to achieve the purpose, the invention adopts the following technical scheme: in a first aspect, a horizontal attitude measurement method is provided, including:
the method comprises the steps of placing an MIMU on a carrier, and carrying out attitude calculation on gyro output data of the MIMU according to the initial attitude of the carrier to obtain an attitude angle of the carrier;
calculating the horizontal attitude angle of the carrier entering a static state and the noise dynamic observation variance array of the carrier in real time according to the output data of the accelerometer in the MIMU;
determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time;
determining gyro zero offset of the MIMU according to the noise dynamic observation variance array and the misalignment angle of the carrier, and obtaining an updated carrier attitude angle;
and fusing the updated attitude angle of the carrier and the horizontal attitude angle of the carrier in the static state to determine the horizontal attitude angle of the carrier.
Further, place MIMU on the carrier, according to the initial gesture of carrier, carry out the gesture to MIMU's gyro output data and solve, obtain the attitude angle of carrier, include:
placing the MIMU on a carrier to obtain the initial zero offset of the MEMS gyroscope;
adopting a quaternion attitude updating method to perform attitude calculation on gyro output data of the MIMU according to the initial attitude of the carrier to obtain an attitude angle of the carrier
Including roll angle
Pitch angle
And course angle
Further, the real-time calculation of the carrier horizontal attitude angle entering the static state and the carrier noise dynamic observation variance matrix according to the output data of the accelerometer in the MIMU comprises:
judging the static state of the carrier according to the output data of the accelerometer in the MIMU and the output amplitude threshold range of the accelerometer;
when the carrier enters a static state, calculating the horizontal attitude angle of the carrier in real time according to the output data of the accelerometer;
and determining a noise dynamic observation variance matrix in real time according to the horizontal attitude variance output by the accelerometer.
Further, the determining the static state of the carrier according to the output data of the accelerometer in the MIMU and the output amplitude threshold range of the accelerometer comprises:
setting the output of a three-axis accelerometer to
Wherein b is a carrier coordinate system,
and
respectively outputting the accelerometer of an x axis, a y axis and a z axis, taking front-right-lower coordinate projection, and selecting north-east coordinates by a navigation system n;
setting a threshold range of the output amplitude of the accelerometer:
wherein g is the value of gravitational acceleration; k is a threshold coefficient;
containing an error for the output of the accelerometer;
is the output modulus of the triaxial accelerometer.
Further, the determining a noise dynamic observation variance matrix in real time according to the horizontal attitude variance output by the accelerometer includes:
and dynamically determining the variance of the horizontal attitude angle in real time according to the noise variance output by the accelerometer by adopting the following model:
wherein D (phi)N)、D(φE) Is the variance of the horizontal attitude angle;
is the noise variance of the accelerometer output;
determining a noise dynamic observation variance matrix in real time according to the variance of the horizontal attitude angle dynamically determined in real time
Wherein, VkNoise at time k; vjNoise at time j; deltakjIs a dirac function, RkIs the variance D (phi) of the heading misalignment angleD) And the variance D (phi) of the horizontal attitude angle dynamically determined in real timeN) And D (phi)E)：
Further, the determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time includes:
determining an Euler angle of a horizontal reference attitude according to the attitude angle of the carrier and the horizontal attitude angle of the carrier entering a static state;
acquiring a horizontal misalignment angle of the carrier according to the Euler angle of the horizontal reference attitude and the attitude angle of the carrier by adopting a rotation vector calculation method;
and taking the difference value between the obtained course angle in the attitude angle of the carrier and the course angle at the static initial moment as the course misalignment angle of the carrier.
Further, the determining the gyro zero offset of the MIMU according to the noise dynamic observation variance matrix and the misalignment angle of the carrier and obtaining the updated attitude angle of the carrier includes:
determining the gyro zero offset of the MIMU by adopting a KF filtering method according to the noise dynamic observation variance array and the misalignment angle of the carrier;
and obtaining an updated carrier attitude angle by adopting a quaternion attitude updating method according to the gyro zero offset of the MIMU.
In a second aspect, there is provided a horizontal attitude measurement system, comprising:
the attitude calculation module is used for performing attitude calculation on gyro output data of the MIMU placed on the carrier according to the initial attitude of the carrier to obtain an attitude angle of the carrier;
the noise dynamic observation variance array calculation module is used for calculating the horizontal attitude angle of the carrier in a static state and the noise dynamic observation variance array of the carrier in real time according to the output data of the accelerometer in the MIMU;
the misalignment angle calculation module is used for determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time;
the gyro zero-offset calculation module is used for determining gyro zero-offset of the MIMU according to the noise dynamic observation variance array and the misalignment angle of the carrier and obtaining an updated carrier attitude angle;
and the fusion module is used for fusing the updated carrier attitude angle and the carrier horizontal attitude angle entering the static state to determine the horizontal attitude angle of the carrier.
In a third aspect, a processing device is provided, comprising computer program instructions, wherein the computer program instructions, when executed by the processing device, are adapted to implement the corresponding steps of the above-mentioned horizontal attitude measurement method.
In a fourth aspect, a computer-readable storage medium is provided, which has computer program instructions stored thereon, wherein the computer program instructions, when executed by a processor, are configured to implement the corresponding steps of the above-mentioned horizontal attitude measurement method.
Due to the adoption of the technical scheme, the invention has the following advantages:
1. the invention establishes a real-time dynamic variance estimation model, can accurately evaluate the accuracy of horizontal attitude estimation by the accelerometer, and can provide accuracy measurement for the application of the horizontal attitude estimation by the accelerometer.
2. According to the method, KF filtering and complementary filtering are effectively combined through real-time dynamic variance estimation, two-layer complementation is realized, complementary coefficients are set without experience values or simple linear estimation, more accurate real-time weight distribution is realized, the estimation precision of the horizontal attitude can be further improved, and the interference influence caused by environmental vibration is weakened.
In conclusion, the invention can be widely applied to the technical field of inertia.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the invention. Like reference numerals refer to like parts throughout the drawings. In the drawings:
FIG. 1 is a schematic flow chart of a method provided by an embodiment of the present invention;
fig. 2 is a diagram illustrating the verification effect of the accelerometer dynamic variance model according to an embodiment of the present invention, where fig. 2(a) is a model-calculated roll angle error, fig. 2(b) is a theoretical statistical roll angle error, fig. 2(c) is a model-calculated pitch angle error, and fig. 2(d) is a theoretical statistical pitch angle error.
Detailed Description
Exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.
It is to be understood that the terminology used herein is for the purpose of describing particular example embodiments only, and is not intended to be limiting. As used herein, the singular forms "a", "an" and "the" may be intended to include the plural forms as well, unless the context clearly indicates otherwise. The terms "comprises," "comprising," "including," and "having" are inclusive and therefore specify the presence of stated features, steps, operations, elements, and/or components, but do not preclude the presence or addition of one or more other features, steps, operations, elements, components, and/or groups thereof. The method steps, processes, and operations described herein are not to be construed as necessarily requiring their performance in the particular order described or illustrated, unless specifically identified as an order of performance. It should also be understood that additional or alternative steps may be used.
The horizontal attitude measurement method, the system, the processing equipment and the storage medium provided by the embodiment of the invention are based on dynamic variance estimation, can solve the problem that the attitude fusion weight coefficient is not accurately set when attitude measurement is carried out based on MIMU at present, provide a dynamic variance estimation model for accurately estimating the horizontal attitude by the accelerometer, effectively fuse two filtering methods and improve the measurement precision of the horizontal attitude. According to the method, the real-time dynamic variance estimation is given for the horizontal attitude estimation of the accelerometer through the established real-time dynamic variance estimation model, so that the KF filtering and the complementary filtering are effectively combined to realize two-layer complementary processing, the estimation precision of the horizontal attitude is further improved through real-time accurate weight distribution, and the method has wide application value in engineering.
Example 1
As shown in fig. 1, the present embodiment provides a horizontal attitude measurement method, including the following steps:
1) and placing the MIMU on a carrier to obtain the initial zero offset of the MEMS gyroscope.
Specifically, the MIMU is placed on a carrier, and after power-up, gyro output data for the MIMU at an initial rest for several seconds (e.g., 3 seconds) are averaged and taken as the initial zero bias for a MEMS (micro-electro-mechanical system) gyro.
2) Adopting a quaternion attitude updating method to perform attitude calculation on gyro output data of the MIMU according to the initial attitude of the carrier to obtain an attitude angle of the carrier
Including roll angle
Pitch angle
And course angle
3) And judging the static state of the carrier according to the output data of the accelerometer in the MIMU and the output amplitude threshold range of the accelerometer.
Specifically, because the precision of the MEMS gyroscope is poor and is easily affected by temperature, particularly, zero-bias instability during the startup preheating process is large, and a large error is generated only by the MEMS gyroscope to perform horizontal attitude calculation, the static state of the carrier is determined according to the output data of the accelerometer in the MIMU and the output amplitude threshold range of the accelerometer, and the specific process is as follows:
3.1) setting the output of the triaxial accelerometer to
Wherein b is a carrier coordinate system,
and
the outputs of the accelerometers are respectively taken from the x-axis, the y-axis and the z-axis, the front, right and lower coordinate projection is taken, and the navigation system N selects the north-east (N-E-D) coordinate.
3.2) taking the output module value of the triaxial accelerometer:
3.3) setting a threshold range of the output amplitude of the accelerometer:
wherein g is the value of the local gravitational acceleration; k is a threshold coefficient which can be generally 0.5-2%;
for the output of the accelerometer to contain errors, there are
f is the theoretical output of the accelerometer (in a stationary or quasi-stationary state, f-gb)；baIs the zero offset of the accelerometer, and ba＝[bx by bz]T(ii) a ε is the device output noise and the environmental impact noise, generally considered white noise, and ε ═ εx by bz]T，εx、byAnd bzWhite noise at the output of the x-axis, y-axis, and z-axis accelerometers, respectively.
And 3.4) judging the static state of the carrier according to the output data of the accelerometer in the MIMU and the output amplitude threshold range of the accelerometer.
Further, when the carrier enters a static state, at the moment, the simple quaternion attitude calculation enters an attitude fusion estimation stage, and the stage comprises two layers of parallel filtering processing.
4) When the carrier enters a static state, a vector observation method is adopted, and the horizontal attitude angle of the carrier, including a roll angle and a pitch angle, is calculated in real time according to the output data of the accelerometer in the MIMU, and specifically comprises the following steps:
4.1) unitizing the output data of the MIMU internal accelerometer:
wherein (C)uRepresenting the unitization of a vector;
is the output data of the unitized accelerometer;
and
the outputs of the x-axis, y-axis and z-axis after the accelerometer measurement is unitized are respectively.
4.2) calculating the horizontal attitude angle of the carrier in real time according to the output data of the unitized accelerometer:
wherein the content of the first and second substances,
is the roll angle estimated by the accelerometer;
is the pitch angle estimated by the accelerometer.
5) According to the output data of an accelerometer in the MIMU, determining a noise dynamic observation variance matrix in real time, which specifically comprises the following steps:
5.1) dynamically determining the variance of the horizontal attitude angle in real time according to the noise variance output by the accelerometer by adopting the following model (6):
wherein D (D)N)、D(φE) Is the variance of the horizontal attitude angle;
is the variance of the noise output by the accelerometer. The correctness of the model is verified as shown in fig. 2, and the calculated result of the model is matched with the real value by comparing with the theoretical statistical characteristic.
5.2) variance D (φ) for heading misalignment angleD) Because the estimation of (2) adopts a pseudo observation quantity mode, namely the observation quantity is a real error increment, the variance D (phi) of the heading misalignment angle can be obtainedD) The variance D (phi) with the horizontal attitude angle is setN) Or D (phi)E) The same magnitude, without affecting the estimation of the horizontal attitude accuracy.
5.3) determining the variance matrix of the dynamic observation of the noise in real time according to the variance of the dynamically determined horizontal attitude angle in real time
Wherein, VkNoise at time k; vjNoise at time j; deltakjIs a dirac function, is 1 when k equals j; otherwise, it is 0; rkIs the variance D (phi) of the heading misalignment angleD) And the variance D (phi) of the horizontal attitude angle dynamically determined in real timeN) And D (phi)E) Namely:
6) according to the attitude angle of the carrier obtained in the step 2)
And the horizontal attitude angle of the carrier calculated in step 4)
Determining the misalignment angle of the carrier in real time, wherein the misalignment angle comprises a horizontal misalignment angle and a heading misalignment angle, and the method specifically comprises the following steps:
6.1) the attitude angle of the carrier obtained in step 2)
And the horizontal attitude angle of the carrier calculated in step 4)
Determining Euler angles of horizontal reference poses
6.2) Euler angles from horizontal reference attitude by rotation vector calculation
And the attitude angle of the carrier obtained in the step 2)
Obtaining the horizontal misalignment angle phi of the carrierNAnd phiE。
6.3) taking the difference value between the course angle in the carrier attitude angle obtained in the step 2) and the course angle at the static initial moment as the course misalignment angle phi of the carrierD。
7) And determining the gyro zero offset of the MIMU by adopting a KF filtering (Kalman filtering) method according to the noise dynamic observation variance array and the misalignment angle of the carrier.
According to the conventional KF filtering method, the state quantities are attitude errors and gyro zero offset, the observed quantity is a misalignment angle, and the attitude errors and the increment errors of the accelerometer in the MIMU under the static state of the heading are obtained:
wherein, X (t) is a system state variable; f (t) is a state transition matrix; g (t) is a system noise matrix; u (t) is a system noise matrix; z (t) is a system view measurement; h (t) is a system observation matrix; v (t) is observed noise, and the variance parameter is set to
[φN φE φD]TIs a misalignment angle, i.e. an attitude error angle, phiN、φEAnd phiDRespectively a horizontal misalignment angle and a course misalignment angle; [ b ] awx bwy bwz]TZero offset is output of the three-axis gyroscope;
for the projection of the three-axis gyro output noise in the navigation coordinate system (n system) (. omega.)DIs the projection of the angular velocity of the earth in the D direction, omegaD＝-ωiesinL，ωieThe rotational angular velocity of the earth, and L is the position latitude; omegaNIs the projection of the angular velocity of the earth in the N direction, omegaN＝ωiecosL；
Is a rotation matrix from b to n.
Furthermore, KF filtering is carried out to estimate the zero offset and the misalignment angle of the three-axis gyroscope, so that on one hand, the calculation result of the current gyroscope attitude can be corrected, and meanwhile, the zero offset estimation value of the three-axis gyroscope can be updated in real time for calculation of the attitude of the next motion stage, and the calculation precision can be improved.
8) And obtaining an updated carrier attitude angle by adopting a quaternion attitude updating method according to the gyro zero offset of the MIMU.
9) Adopting a complementary filtering method to carry out the calculation on the updated carrier attitude angle and the horizontal attitude angle calculated in the step 4)
Fusing to determine the horizontal attitude angle of the carrier
Wherein the content of the first and second substances,
the updated attitude angle of the carrier; l and m are fusion weight coefficients, and are selected according to the variance of real-time estimation, namely:
wherein, Pk(i, i) is the i-th diagonal element of the variance matrix for state quantity estimation, PkAnd estimating a variance matrix for the system state in the KF filtering recursion formula.
Example 2
The present embodiment provides a horizontal attitude measurement system, including:
and the attitude calculation module is used for performing attitude calculation on the gyro output data of the MIMU placed on the carrier according to the initial attitude of the carrier to obtain the attitude angle of the carrier.
And the noise dynamic observation variance array calculation module is used for calculating the horizontal attitude angle of the carrier in a static state and the noise dynamic observation variance array of the carrier in real time according to the output data of the accelerometer in the MIMU.
And the misalignment angle calculation module is used for determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time.
And the gyro zero-offset calculation module is used for determining the gyro zero-offset of the MIMU according to the noise dynamic observation variance array and the misalignment angle of the carrier and obtaining an updated carrier attitude angle.
And the fusion module is used for fusing the updated carrier attitude angle and the carrier horizontal attitude angle entering the static state to determine the horizontal attitude angle of the carrier.
Example 3
The present embodiment provides a processing device corresponding to the horizontal posture measuring method provided in embodiment 1, where the processing device may be a processing device for a client, such as a mobile phone, a notebook computer, a tablet computer, a desktop computer, etc., to execute the method of embodiment 1.
The processing equipment comprises a processor, a memory, a communication interface and a bus, wherein the processor, the memory and the communication interface are connected through the bus so as to complete mutual communication. The memory stores a computer program that can be executed on the processing device, and the processing device executes the horizontal attitude measurement method provided in embodiment 1 when executing the computer program.
In some implementations, the Memory may be a high-speed Random Access Memory (RAM), and may also include a non-volatile Memory, such as at least one disk Memory.
In other implementations, the processor may be various general-purpose processors such as a Central Processing Unit (CPU), a Digital Signal Processor (DSP), and the like, and is not limited herein.
Example 4
The present embodiment provides a computer program product corresponding to the horizontal attitude measurement method provided in embodiment 1, and the computer program product may include a computer-readable storage medium on which computer-readable program instructions for executing the horizontal attitude measurement method described in embodiment 1 are loaded.
The computer readable storage medium may be a tangible device that retains and stores instructions for use by an instruction execution device. The computer readable storage medium may be, for example, but not limited to, an electronic memory device, a magnetic memory device, an optical memory device, an electromagnetic memory device, a semiconductor memory device, or any combination of the foregoing.
The above embodiments are only used for illustrating the present invention, and the structure, connection mode, manufacturing process, etc. of the components may be changed, and all equivalent changes and modifications performed on the basis of the technical solution of the present invention should not be excluded from the protection scope of the present invention.
Claims (10)
1. A horizontal attitude measurement method, comprising:
the method comprises the steps of placing an MIMU on a carrier, and carrying out attitude calculation on gyro output data of the MIMU according to the initial attitude of the carrier to obtain an attitude angle of the carrier;
calculating the horizontal attitude angle of the carrier entering a static state and the noise dynamic observation variance array of the carrier in real time according to the output data of the accelerometer in the MIMU;
determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time;
determining gyro zero offset of the MIMU according to the noise dynamic observation variance array and the misalignment angle of the carrier, and obtaining an updated carrier attitude angle;
and fusing the updated attitude angle of the carrier and the horizontal attitude angle of the carrier in the static state to determine the horizontal attitude angle of the carrier.
2. The method according to claim 1, wherein the step of placing the MIMU on a carrier and performing attitude solution on gyro output data of the MIMU according to an initial attitude of the carrier to obtain an attitude angle of the carrier comprises:
placing the MIMU on a carrier to obtain the initial zero offset of the MEMS gyroscope;
adopting a quaternion attitude updating method to perform attitude calculation on gyro output data of the MIMU according to the initial attitude of the carrier to obtain an attitude angle of the carrier
Including roll angle
Pitch angle
And course angle
3. The method of claim 1, wherein said calculating in real time the carrier horizontal attitude angle to rest and the carrier noise dynamics observation variance matrix from the MIMU accelerometer output data comprises:
judging the static state of the carrier according to the output data of the accelerometer in the MIMU and the output amplitude threshold range of the accelerometer;
when the carrier enters a static state, calculating the horizontal attitude angle of the carrier in real time according to the output data of the accelerometer;
and determining a noise dynamic observation variance matrix in real time according to the horizontal attitude variance output by the accelerometer.
4. The method according to claim 3, wherein said determining the stationary state of the carrier based on the output data of the accelerometer and the output amplitude threshold range of the accelerometer comprises:
setting the output of a three-axis accelerometer to
Wherein b is a carrier coordinate system,
and
respectively outputting the accelerometer of an x axis, a y axis and a z axis, taking front-right-lower coordinate projection, and selecting north-east coordinates by a navigation system n;
setting a threshold range of the output amplitude of the accelerometer:
wherein g is the value of gravitational acceleration; k is a threshold coefficient;
containing an error for the output of the accelerometer;
is the output modulus of the triaxial accelerometer.
5. The method of claim 3, wherein the determining the noise dynamic observation variance matrix in real time according to the horizontal attitude variance output by the accelerometer comprises:
and dynamically determining the variance of the horizontal attitude angle in real time according to the noise variance output by the accelerometer by adopting the following model:
wherein D (phi)N)、D(φE) Is the variance of the horizontal attitude angle;
is the noise variance of the accelerometer output;
determining a noise dynamic observation variance matrix in real time according to the variance of the horizontal attitude angle dynamically determined in real time
Wherein, VkNoise at time k; vjNoise at time j; deltakjIs a dirac function, RkIs the variance D (phi) of the heading misalignment angleD) And the variance D (phi) of the horizontal attitude angle dynamically determined in real timeN) And D (phi)E)：
6. The method for measuring horizontal attitude of claim 2, wherein the determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time comprises:
determining an Euler angle of a horizontal reference attitude according to the attitude angle of the carrier and the horizontal attitude angle of the carrier entering a static state;
acquiring a horizontal misalignment angle of the carrier according to the Euler angle of the horizontal reference attitude and the attitude angle of the carrier by adopting a rotation vector calculation method;
and taking the difference value between the obtained course angle in the attitude angle of the carrier and the course angle at the static initial moment as the course misalignment angle of the carrier.
7. The method of claim 1, wherein determining the gyro zero offset of the MIMU based on the noise dynamic observation variance matrix and the misalignment angle of the carrier and obtaining the updated attitude angle of the carrier comprises:
determining the gyro zero offset of the MIMU by adopting a KF filtering method according to the noise dynamic observation variance array and the misalignment angle of the carrier;
and obtaining an updated carrier attitude angle by adopting a quaternion attitude updating method according to the gyro zero offset of the MIMU.
8. A horizontal attitude measurement system, comprising:
the attitude calculation module is used for performing attitude calculation on gyro output data of the MIMU placed on the carrier according to the initial attitude of the carrier to obtain an attitude angle of the carrier;
the noise dynamic observation variance array calculation module is used for calculating the horizontal attitude angle of the carrier in a static state and the noise dynamic observation variance array of the carrier in real time according to the output data of the accelerometer in the MIMU;
the misalignment angle calculation module is used for determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the horizontal attitude angle calculated in real time;
the gyro zero-offset calculation module is used for determining gyro zero-offset of the MIMU according to the noise dynamic observation variance array and the misalignment angle of the carrier and obtaining an updated carrier attitude angle;
and the fusion module is used for fusing the updated carrier attitude angle and the carrier horizontal attitude angle entering the static state to determine the horizontal attitude angle of the carrier.
9. A processing device comprising computer program instructions, wherein the computer program instructions, when executed by the processing device, are adapted to implement the steps corresponding to the horizontal attitude measurement method of any one of claims 1-7.
10. A computer readable storage medium having computer program instructions stored thereon, wherein the computer program instructions, when executed by a processor, are for performing the steps corresponding to the horizontal attitude measurement method of any one of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111373775.XA CN114111770B (en) | 2021-11-19 | 2021-11-19 | Horizontal attitude measurement method, system, processing equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111373775.XA CN114111770B (en) | 2021-11-19 | 2021-11-19 | Horizontal attitude measurement method, system, processing equipment and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114111770A true CN114111770A (en) | 2022-03-01 |
| CN114111770B CN114111770B (en) | 2024-04-09 |
Family
ID=80396530
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111373775.XA Active CN114111770B (en) | 2021-11-19 | 2021-11-19 | Horizontal attitude measurement method, system, processing equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114111770B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444467A (en) * | 2017-11-17 | 2018-08-24 | 西北工业大学 | A kind of pedestrian's localization method based on feedback complementary filter and algebraic approximation |
| WO2020253854A1 (en) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | Mobile robot posture angle calculation method |
| CN112629538A (en) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering |
| WO2021090352A1 (en) * | 2019-11-05 | 2021-05-14 | 楽天株式会社 | Control device and control method for controlling flight of aircraft |
| CN113175926A (en) * | 2021-04-21 | 2021-07-27 | 哈尔滨工程大学 | Self-adaptive horizontal attitude measurement method based on motion state monitoring |
-
2021
- 2021-11-19 CN CN202111373775.XA patent/CN114111770B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444467A (en) * | 2017-11-17 | 2018-08-24 | 西北工业大学 | A kind of pedestrian's localization method based on feedback complementary filter and algebraic approximation |
| WO2020253854A1 (en) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | Mobile robot posture angle calculation method |
| WO2021090352A1 (en) * | 2019-11-05 | 2021-05-14 | 楽天株式会社 | Control device and control method for controlling flight of aircraft |
| CN112629538A (en) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering |
| CN113175926A (en) * | 2021-04-21 | 2021-07-27 | 哈尔滨工程大学 | Self-adaptive horizontal attitude measurement method based on motion state monitoring |
Non-Patent Citations (1)
| Title |
|---|
| 张复建;单斌;杨波;薛亮;腾红磊;: "级联卡尔曼滤波在初始对准中的应用", 计算机测量与控制, no. 09, 19 September 2018 (2018-09-19) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114111770B (en) | 2024-04-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
| US20160178657A9 (en) | Systems and methods for sensor calibration | |
| CN109916395B (en) | Gesture autonomous redundant combined navigation algorithm | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN108731664B (en) | Robot state estimation method, device, computer equipment and storage medium | |
| CN107764261B (en) | Simulation data generation method and system for distributed POS (point of sale) transfer alignment | |
| WO2022160391A1 (en) | Magnetometer information assisted mems gyroscope calibration method and calibration system | |
| JP2012173190A (en) | Positioning system and positioning method | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| JP7025215B2 (en) | Positioning system and positioning method | |
| CN112066985B (en) | Initialization method, device, medium and electronic equipment for combined navigation system | |
| CN112562077B (en) | Pedestrian indoor positioning method integrating PDR and priori map | |
| JP2019078560A (en) | Gyro sensor offset correcting device, offset correction program, and pedestrian autonomous navigation device | |
| CN114485641A (en) | Attitude calculation method and device based on inertial navigation and satellite navigation azimuth fusion | |
| Hoang et al. | Measurement optimization for orientation tracking based on no motion no integration technique | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| CN108871319B (en) | Attitude calculation method based on earth gravity field and earth magnetic field sequential correction | |
| CN110645976B (en) | Attitude estimation method of mobile robot and terminal equipment | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| JP2013061309A (en) | Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter | |
| CN106931965B (en) | Method and device for determining terminal posture | |
| CN114111770B (en) | Horizontal attitude measurement method, system, processing equipment and storage medium | |
| JP2006038650A (en) | Posture measuring method, posture controller, azimuth meter and computer program | |
| CN113405567B (en) | Gravity satellite star sensor mounting matrix on-orbit calibration method and system | |
| CN112649001B (en) | Gesture and position resolving method for small unmanned aerial vehicle |
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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Guo Meifeng Inventor after: Yang Haijun Inventor after: Wang Chengbin Inventor after: Li Xin Inventor after: Chen Jianqiang Inventor after: Shi Gengxiu Inventor after: Liang Hongyi Inventor before: Guo Meifeng Inventor before: Yang Haijun Inventor before: Wang Chengbin Inventor before: Li Xin Inventor before: Chen Jianqiang Inventor before: Shi Gengxiu Inventor before: Liang Hongyi |
|
| CB03 | Change of inventor or designer information |