CN114111770B - 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
- CN114111770B CN114111770B CN202111373775.XA CN202111373775A CN114111770B CN 114111770 B CN114111770 B CN 114111770B CN 202111373775 A CN202111373775 A CN 202111373775A CN 114111770 B CN114111770 B CN 114111770B
- Authority
- CN
- China
- Prior art keywords
- carrier
- angle
- attitude
- accelerometer
- mimu
- 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.)
- Active
Links
- 238000012545 processing Methods 0.000 title claims abstract description 20
- 238000000691 measurement method Methods 0.000 title claims abstract description 17
- 238000003860 storage Methods 0.000 title claims abstract description 17
- 238000000034 method Methods 0.000 claims abstract description 42
- 230000003068 static effect Effects 0.000 claims abstract description 33
- 238000004364 calculation method Methods 0.000 claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 13
- 238000001914 filtration Methods 0.000 claims description 19
- 238000004590 computer program Methods 0.000 claims description 11
- 230000000295 complement effect Effects 0.000 claims description 9
- 230000004927 fusion Effects 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000036544 posture Effects 0.000 description 12
- 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
- 238000012821 model calculation Methods 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
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
Abstract
The invention relates to a horizontal attitude measurement method, a horizontal attitude measurement system, a processing device and a storage medium, which are characterized by comprising the following steps: placing the MIMU on a carrier, and carrying out gesture calculation on gyro output data of the MIMU according to the initial gesture of the carrier to obtain a gesture angle of the carrier; judging the static state of the carrier, and calculating the carrier horizontal attitude angle entering the static state and the carrier noise dynamic observation variance array 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 real-time calculated horizontal attitude angle; according to the noise dynamic observation variance matrix and the misalignment angle of the carrier, determining the gyro zero bias of the MIMU, and obtaining an updated carrier attitude angle; the method and the device for determining the horizontal attitude angle of the carrier can be widely applied to the technical field of inertia by fusing the updated carrier attitude angle with the carrier horizontal attitude angle in a static state to determine the horizontal attitude angle of the carrier.
Description
Technical Field
The present invention relates to the field of inertial technologies, and in particular, to a horizontal attitude measurement method, a horizontal attitude measurement system, a horizontal attitude measurement processing device, and a storage medium.
Background
MIMU (Micro Inertial Measurement Unit ) has become the first choice for unmanned aerial vehicle, unmanned vehicle, micro stabilized platform and human wearable intelligent device due to its small volume, low power consumption, low cost, etc. and is applied to gesture measurement and navigation positioning. At present, the method applied to MIMU attitude measurement mainly utilizes the characteristic that the accuracy and stability of an accelerometer in the MIMU are far higher than the level of a gyroscope device, and fuses the calculated attitude of the gyroscope and the estimated attitude of the accelerometer by methods such as complementary filtering and the like under the consideration of the static or quasi-static state in the carrier motion process, so as to achieve the aim of improving the measurement accuracy of the carrier horizontal attitude.
However, when the fusion is performed by adopting the complementary filtering method at present, the weight setting of different postures only depends on an empirical value, or the accuracy of the horizontal posture estimation of the accelerometer cannot be estimated more accurately based on the first-order linear relation of the amplitude values 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 more accurately performed.
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, capable of dynamically calculating an estimation accuracy of an accelerometer horizontal attitude in real time.
In order to achieve the above purpose, the present invention adopts the following technical scheme: in a first aspect, a horizontal attitude measurement method is provided, including:
placing the MIMU on a carrier, and carrying out gesture calculation on gyro output data of the MIMU according to the initial gesture of the carrier to obtain a gesture angle of the carrier;
according to the output data of the accelerometer in the MIMU, calculating the carrier horizontal attitude angle entering a static state and the noise dynamic observation variance matrix of the carrier in real time;
determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the real-time calculated horizontal attitude angle;
according to the noise dynamic observation variance matrix and the misalignment angle of the carrier, determining the gyro zero bias of the MIMU, and obtaining an updated carrier attitude angle;
and fusing the updated carrier attitude angle and the carrier horizontal attitude angle entering the static state to determine the carrier horizontal attitude angle.
Further, the placing the MIMU on the carrier, and performing gesture calculation on the gyro output data of the MIMU according to the initial gesture of the carrier, to obtain a gesture angle of the carrier, includes:
placing the MIMU on a carrier to obtain the initial zero offset of the MEMS gyroscope;
adopting a quaternion attitude updating method, and according to the initial attitude of the carrier, carrying out attitude calculation on gyro output data of the MIMU to obtain an attitude angle of the carrierComprising a roll angle->Pitch angle->And heading angle->
Further, the calculating, in real time, the carrier horizontal attitude angle entering the static state and the noise dynamic observation variance array of the carrier 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 array 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 includes:
setting the output of the triaxial accelerometer asWherein b is the carrier coordinate system, +.>Andthe output of the accelerometer is respectively the x axis, the y axis and the z axis, the front lower right coordinate projection is taken, and the navigation system n system selects the north east coordinate;
setting a threshold range of accelerometer output amplitude:
wherein g is the value of gravitational acceleration; k is a threshold coefficient;error is contained for the accelerometer output; />Is the output modulus of the triaxial accelerometer.
Further, the determining, in real time, a noise dynamic observation variance matrix according to the horizontal attitude variance output by the accelerometer includes:
according to the noise variance output by the accelerometer, the variance of the horizontal attitude angle is dynamically determined in real time by adopting the following model:
wherein the method comprises the steps of，D(φ N )、D(φ E ) Is the variance of the horizontal attitude angle;noise variance for 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 V is k Noise at time k; v (V) j The noise at the moment j; delta kj As a dirac function, R k Variance D (phi) for heading misalignment angle D ) And a variance D (phi) of the horizontal attitude angle dynamically determined in real time N ) And D (phi) E )：
Further, the determining, in real time, the misalignment angle of the carrier according to the acquired attitude angle and the real-time calculated horizontal attitude angle includes:
determining the Euler angle of a horizontal reference posture according to the posture angle of the carrier and the horizontal posture 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 of the course angle in the acquired carrier attitude angle and the course angle at the static initial moment as the course misalignment angle of the carrier.
Further, 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, including:
determining the gyro zero offset of the MIMU according to the noise dynamic observation variance matrix and the misalignment angle of the carrier by adopting a KF filtering method;
and obtaining an updated carrier attitude angle according to the zero offset of the gyro of the MIMU by adopting a quaternion attitude updating method.
In a second aspect, there is provided a horizontal attitude measurement system comprising:
the attitude calculation module is used for carrying out 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 matrix calculation module is used for calculating the carrier horizontal attitude angle entering a static state and the noise dynamic observation variance matrix 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 dynamically observing a variance array and a misalignment angle according to the noise of the carrier, determining the gyro zero offset of the MIMU 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 and determining the carrier horizontal attitude angle.
In a third aspect, a processing device is provided, comprising computer program instructions for implementing the steps corresponding to the above-mentioned horizontal attitude measurement method when being executed by the processing device.
In a fourth aspect, a computer readable storage medium is provided, on which computer program instructions are stored, wherein the computer program instructions, when executed by a processor, are configured to implement the steps corresponding to the above-mentioned horizontal attitude measurement method.
Due to the adoption of the technical scheme, the invention has the following advantages:
1. the method establishes a real-time dynamic variance estimation model, can accurately evaluate the accuracy of the horizontal attitude estimation by the accelerometer, and can provide accuracy measurement for the horizontal attitude application estimated by the accelerometer.
2. According to the invention, through real-time dynamic variance estimation, KF filtering and complementary filtering are effectively combined to realize two-layer complementation, and the setting of the complementary coefficient is more accurate in real-time weight distribution instead of empirical values or simple linear estimation, so that the estimation accuracy of the horizontal attitude can be further improved and the interference influence caused by environmental vibration can be reduced.
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 parts are designated with like reference numerals throughout the drawings. In the drawings:
FIG. 1 is a schematic flow chart of a method according to an embodiment of the present invention;
fig. 2 is a diagram showing an effect of verifying an accelerometer dynamic variance model according to an embodiment of the invention, wherein fig. 2 (a) shows a model calculation roll angle error, fig. 2 (b) shows a theoretical statistical roll angle error, fig. 2 (c) shows a model calculation pitch angle error, and fig. 2 (d) shows 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 present invention are shown in the drawings, it should be understood that the present invention may 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" are intended to include the plural forms as well, unless the context clearly indicates otherwise. The terms "comprises," "comprising," "includes," "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 an order of performance is explicitly stated. It should also be appreciated 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 can solve the problem of inaccurate attitude fusion weight coefficient setting when the attitude measurement is performed based on MIMU at present, provide a dynamic variance estimation model for estimating the horizontal attitude of an accelerometer accurately, effectively fuse two filtering methods and improve the horizontal attitude measurement accuracy. According to the invention, the real-time dynamic variance estimation model is established, the horizontal attitude estimation is carried out on the accelerometer, the real-time dynamic variance estimation is given, KF filtering and complementary filtering are effectively combined by taking the real-time dynamic variance estimation model as a tie, two layers of complementary processing are realized, and the estimation precision of the horizontal attitude is further improved through real-time accurate weight distribution, so that the method has wide application value in engineering.
Example 1
As shown in fig. 1, the present embodiment provides a horizontal posture measurement method, which includes 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, the gyro output data of the MIMU at initial rest for several seconds (e.g., 3 seconds) is averaged and used as the initial zero bias of the MEMS (microelectromechanical system) gyro.
2) Adopting a quaternion attitude updating method, and according to the initial attitude of the carrier, carrying out attitude calculation on gyro output data of the MIMU to obtain an attitude angle of the carrierComprising a roll angle->Pitch angle->And heading 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 the zero offset instability in the startup preheating process is large, a large error can be generated by only solving the horizontal attitude by the MEMS gyroscope, so that the static state of the carrier is judged by 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 toWherein b is the carrier coordinate system, +.> And->The output of the accelerometer is respectively the x axis, the y axis and the z axis, the front lower right coordinate projection is taken, and the navigation system N is selected as 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 accelerometer output magnitudes:
wherein g is the value of local gravitational acceleration; k is a threshold coefficient, and can be generally 0.5% -2%;containing errors for the output of the accelerometer, there are +.>f is the theoretical output of the accelerometer (f=g in rest or quasi-rest conditions b )；b a Zero bias of accelerometer, and b a ＝[b x b y b z ] T The method comprises the steps of carrying out a first treatment on the surface of the Epsilon is the device output noise and the environmental impact noise, which is generally considered as white noise, and epsilon= [ epsilon ] x b y b z ] T ，ε x 、b y And b z White noise at the output of the x-axis, y-axis, and z-axis accelerometers, respectively.
3.4 According to the output data of the accelerometer in the MIMU and the threshold range of the output amplitude of the accelerometer, judging the static state of the carrier.
Further, when the carrier enters a static state, the simple quaternion gesture calculation enters a gesture 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 according to output data of an accelerometer in the MIMU, the horizontal attitude angle of the carrier, including a roll angle and a pitch angle, is calculated in real time, and specifically, the method comprises the following steps:
4.1 Unitizing the output data of the accelerometer within the MIMU:
wherein () u Representing unitization of vectors;output data of the accelerometer after unitization; /> And->The output of the accelerometer measured value is respectively the x axis, the y axis and the z axis after unitizing.
4.2 According to the output data of the accelerometer after unitization, calculating the horizontal attitude angle of the carrier in real time:
wherein,is the roll angle estimated by the accelerometer; />Is the pitch angle estimated by the accelerometer.
5) According to the output data of the accelerometer in the MIMU, determining a noise dynamic observation variance array in real time, wherein the method specifically comprises the following steps:
5.1 According to the noise variance of the accelerometer output, adopting the following model (6) to dynamically determine the variance of the horizontal attitude angle in real time:
wherein D is%D N )、D(φ E ) Is the variance of the horizontal attitude angle;noise variance for accelerometer output. Verification of the correctness of the model is shown in fig. 2, and the calculation result of the model is matched with the true value by comparing with the theoretical statistical characteristics.
5.2 Variance D (phi) for heading misalignment angle D ) Because of adopting the pseudo observed quantity mode, namely that the observed quantity is a real error increment, the variance D (phi) of the course misalignment angle can be obtained D ) Setting the variance D (phi) to be the horizontal attitude angle N ) Or D (phi) E ) Values of the same magnitude, without affecting the estimation of the horizontal pose accuracy.
5.3 Based on the variance of the horizontal attitude angle dynamically determined in real time, determining a noise dynamic observation variance matrix in real time
Wherein V is k Noise at time k; v (V) j The noise at the moment j; delta kj Is a dirac function, when k is equal to j, it is 1; otherwise, 0; r is R k Variance D (phi) for heading misalignment angle D ) And a variance D (phi) of the horizontal attitude angle dynamically determined in real time N ) And D (phi) E ) The method comprises the following steps:
6) According to the attitude angle of the carrier obtained in 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 comprises the following specific steps:
6.1 According to the attitude angle of the carrier obtained in step 2)And the horizontal attitude angle of the carrier calculated in step 4)>Euler angle for determining horizontal reference posture>
6.2 Using rotation vector calculation method, euler angle according to horizontal reference postureAnd the attitude angle of the carrier obtained in step 2)>Obtaining the horizontal misalignment angle phi of the carrier N And phi E 。
6.3 Taking the difference value of the course angle in the carrier attitude angle obtained in the step 2) and the course angle for judging the static initial moment as the course misalignment angle phi of the carrier D 。
7) And determining the gyro zero offset of the MIMU according to the noise dynamic observation variance matrix and the misalignment angle of the carrier by adopting a KF filtering (Kalman filtering) method.
The state quantity of the conventional KF filtering method is attitude error and gyro zero offset, the observed quantity is misalignment angle, and the state quantity is obtained through the increment error of the accelerometer in the MIMU in the horizontal attitude and heading static state:
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 array; z (t) is the observed quantity of the system; h (t) is a system observation matrix; v (t) is the observed noise, and the variance parameter is set as follows[φ N φ E φ D ] T For misalignment angle, i.e. attitude error angle, phi N 、φ E And phi D Respectively a horizontal misalignment angle and a heading misalignment angle; [ b ] wx b wy b wz ] T Zero offset is output by the triaxial gyroscope; />Projection of output noise of three-axis gyroscope under navigation coordinate system (n system), omega D For projection of the rotation angular velocity of the earth in the D direction omega D ＝-ω ie sinL，ω ie The rotation angular velocity of the earth is represented by L, and the position latitude is represented by L; omega N Is the projection of the rotation angular velocity of the earth in the N direction, omega N ＝ω ie cosL；/>The rotation matrix from b series to n series.
Further, KF filtering is carried out to estimate the zero offset and misalignment angle of the three-axis gyroscope, on one hand, the result of the current gyroscope attitude calculation can be corrected, and meanwhile, the zero offset value of the three-axis gyroscope can be updated in real time for the attitude calculation of the next motion stage, so that the calculation precision can be improved.
8) And obtaining an updated carrier attitude angle according to the zero offset of the gyro of the MIMU by adopting a quaternion attitude updating method.
9) Using complementary filtering method to update the carrier attitude angle and the horizontal attitude angle calculated in step 4) Fusion is carried out, and the horizontal attitude angle of the carrier is determined>
Wherein,the updated carrier attitude angle; and l and m are fusion weight coefficients, and are selected according to the variance of real-time estimation, namely:
wherein P is k (i, i) is the i-th diagonal element of the variance matrix estimated for the state quantity, P k And estimating a variance matrix for the system state in the KF filtering recursive formula.
Example 2
The present embodiment provides a horizontal posture measurement system including:
and the gesture resolving module is used for resolving the gesture of the gyro output data of the MIMU placed on the carrier according to the initial gesture of the carrier to obtain the gesture angle of the carrier.
And the noise dynamic observation variance matrix calculation module is used for calculating the carrier horizontal attitude angle and the carrier noise dynamic observation variance matrix entering the static state 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 and determining the carrier horizontal attitude angle.
Example 3
The present embodiment provides a processing device corresponding to the horizontal posture measurement method provided in the present embodiment 1, and the processing device may be a processing device for a client, for example, a mobile phone, a notebook computer, a tablet computer, a desktop computer, or the like, to perform the method of embodiment 1.
The processing device 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 communication among each other. The memory stores a computer program executable on the processing device, and the processing device executes the horizontal posture measurement method provided in the present embodiment 1 when the processing device executes the computer program.
In some implementations, the memory may be high-speed random access memory (RAM: random Access Memory), and may also include non-volatile memory (non-volatile memory), such as at least one disk memory.
In other implementations, the processor may be a Central Processing Unit (CPU), a Digital Signal Processor (DSP), or other general-purpose processor, which is not limited herein.
Example 4
The present embodiment provides a computer program product corresponding to the horizontal posture measurement method provided in the present embodiment 1, and the computer program product may include a computer readable storage medium having computer readable program instructions for executing the horizontal posture measurement method described in the present embodiment 1.
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 storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any combination of the preceding.
The foregoing embodiments are only for illustrating the present invention, wherein the structures, connection modes, manufacturing processes, etc. of the components may be changed, and all equivalent changes and modifications performed on the basis of the technical solutions of the present invention should not be excluded from the protection scope of the present invention.
Claims (8)
1. A horizontal attitude measurement method, characterized by comprising:
placing the MIMU on a carrier, and carrying out gesture calculation on gyro output data of the MIMU according to the initial gesture of the carrier to obtain a gesture angle of the carrier;
according to the output data of the accelerometer in the MIMU, calculating the carrier horizontal attitude angle entering the static state and the noise dynamic observation variance array of the carrier in real time, comprising:
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;
determining a noise dynamic observation variance array in real time according to the horizontal attitude variance output by the accelerometer;
determining the misalignment angle of the carrier in real time according to the acquired attitude angle and the real-time calculated horizontal attitude angle;
according to the noise dynamic observation variance matrix and the misalignment angle of the carrier, determining the gyro zero bias of the MIMU and obtaining an updated carrier attitude angle, comprising:
determining the gyro zero offset of the MIMU according to the noise dynamic observation variance matrix and the misalignment angle of the carrier by adopting a KF filtering method;
adopting a quaternion attitude updating method, and obtaining an updated carrier attitude angle according to the gyro zero offset of the MIMU;
and adopting a complementary filtering method to fuse the updated carrier attitude angle with the carrier horizontal attitude angle in a static state, and determining the carrier horizontal attitude angle.
2. The method for measuring horizontal attitude as set forth in claim 1, wherein said placing the MIMU on the carrier, performing attitude calculation on gyro output data of the MIMU based on an initial attitude of the carrier, obtaining 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, and according to the initial attitude of the carrier, carrying out attitude calculation on gyro output data of the MIMU to obtain an attitude angle of the carrierComprising a roll angle->Pitch angle->And heading angle->
3. The method of claim 1, wherein the determining the stationary state of the carrier according to the output data of the accelerometer in the MIMU and the threshold range of the output amplitude of the accelerometer comprises:
setting the output of the triaxial accelerometer asWherein the method comprises the steps ofB is the carrier coordinate system,>and->The output of the accelerometer is respectively the x axis, the y axis and the z axis, the front lower right coordinate projection is taken, and the navigation system n system selects the north east coordinate;
setting a threshold range of accelerometer output amplitude:
wherein g is the value of gravitational acceleration; k is a threshold coefficient;error is contained for the accelerometer output; />Is the output modulus of the triaxial accelerometer.
4. A method of measuring horizontal attitude as claimed in claim 3 wherein said determining in real time a noise dynamic observation variance matrix based on the horizontal attitude variance of the accelerometer output comprises:
according to the noise variance output by the accelerometer, the variance of the horizontal attitude angle is dynamically determined in real time by adopting the following model:
wherein D (phi) N )、D(φ E ) Is the variance of the horizontal attitude angle;noise variance for 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 V is k Noise at time k; v (V) j The noise at the moment j; delta kj As a dirac function, R k Variance D (phi) for heading misalignment angle D ) And a variance D (phi) of the horizontal attitude angle dynamically determined in real time N ) And D (phi) E )：
5. A method of measuring a horizontal attitude as set forth in claim 2, wherein said determining in real time the misalignment angle of the carrier based on the acquired attitude angle and the real-time calculated horizontal attitude angle comprises:
determining the Euler angle of a horizontal reference posture according to the posture angle of the carrier and the horizontal posture 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 of the course angle in the acquired carrier attitude angle and the course angle at the static initial moment as the course misalignment angle of the carrier.
6. A horizontal attitude measurement system, comprising:
the attitude calculation module is used for carrying out 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 matrix calculation module is used for calculating the carrier horizontal attitude angle entering a static state and the noise dynamic observation variance matrix of the carrier in real time according to the output data of the accelerometer in the MIMU, and comprises the following components:
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;
determining a noise dynamic observation variance array in real time according to the horizontal attitude variance output by the accelerometer;
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 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 comprises the following steps:
determining the gyro zero offset of the MIMU according to the noise dynamic observation variance matrix and the misalignment angle of the carrier by adopting a KF filtering method;
adopting a quaternion attitude updating method, and obtaining an updated carrier attitude angle according to the gyro zero offset of the MIMU;
and the fusion module is used for fusing the updated carrier attitude angle and the carrier horizontal attitude angle entering the static state by adopting a complementary filtering method and determining the carrier horizontal attitude angle.
7. A processing device comprising computer program instructions, wherein the computer program instructions, when executed by the processing device, are for implementing the steps corresponding to the horizontal attitude measurement method according to any one of claims 1-5.
8. A computer readable storage medium, characterized in that the computer readable storage medium has stored thereon computer program instructions, wherein the computer program instructions, when executed by a processor, are for implementing the steps corresponding to the horizontal pose measurement method according to any of claims 1-5.
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 CN114111770A (en) | 2022-03-01 |
| CN114111770B true 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 (2)
| Title |
|---|
| He, GY ,et al..Improving the Position Accuracy of the ZUPT-Aided Pedestrian Inertial Navigation by Using a Differential Layout MIMU Array.IEEE SENSORS JOURNAL.第23卷(第19期),23420-23430. * |
| 级联卡尔曼滤波在初始对准中的应用;张复建;单斌;杨波;薛亮;腾红磊;;计算机测量与控制;20180919(第09期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114111770A (en) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Michel et al. | On attitude estimation with smartphones | |
| US6711517B2 (en) | Hybrid inertial navigation method and device | |
| WO2022160391A1 (en) | Magnetometer information assisted mems gyroscope calibration method and calibration system | |
| CN110988926B (en) | Method for realizing position accurate fixed point deception migration in loose GNSS/INS combined navigation mode | |
| CN112562077B (en) | Pedestrian indoor positioning method integrating PDR and priori map | |
| CN107764261B (en) | Simulation data generation method and system for distributed POS (point of sale) transfer alignment | |
| CN106153069B (en) | Attitude rectification device and method in autonomous navigation system | |
| JP2012173190A (en) | Positioning system and positioning method | |
| JP7025215B2 (en) | Positioning system and positioning method | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| Amirsadri et al. | Practical considerations in precise calibration of a low-cost MEMS IMU for road-mapping applications | |
| JP3726884B2 (en) | Attitude estimation apparatus and method using inertial measurement apparatus, and program | |
| Li et al. | Low-cost MEMS sensor-based attitude determination system by integration of magnetometers and GPS: A real-data test and performance evaluation | |
| CN114485641A (en) | Attitude calculation method and device based on inertial navigation and satellite navigation azimuth fusion | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| CN117589163A (en) | Multi-sensor combination navigation method and device | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN110645976B (en) | Attitude estimation method of mobile robot and terminal equipment | |
| Ding et al. | A quaternion based error state Kalman filter for attitude estimation using low-cost MEMS MARG sensors | |
| CN114111770B (en) | Horizontal attitude measurement method, system, processing equipment and storage medium | |
| CN110160530B (en) | Spacecraft attitude filtering method based on quaternion | |
| CN109916399B (en) | Carrier attitude estimation method under shadow | |
| 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 |
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 |