CN105352502B - A kind of attitude acquisition method of micro- inertia attitude heading reference system - Google Patents
A kind of attitude acquisition method of micro- inertia attitude heading reference system Download PDFInfo
- Publication number
- CN105352502B CN105352502B CN201510843867.8A CN201510843867A CN105352502B CN 105352502 B CN105352502 B CN 105352502B CN 201510843867 A CN201510843867 A CN 201510843867A CN 105352502 B CN105352502 B CN 105352502B
- Authority
- CN
- China
- Prior art keywords
- attitude
- carrier
- angle
- gyro
- matrix
- 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
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
- G01C21/165—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 combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
The invention discloses a kind of attitude acquisition method of micro- inertia attitude heading reference system, comprise the steps：(1) the carrier acceleration information and three-dimensional magnetic field strength information that are gathered according to sensor obtain the attitude angle initial value of carrier；And the system posture matrix at current time is obtained according to the attitude angle initial value；(2) gyroscopic drift error compensation amount is obtained according to the system posture matrix, and the data exported according to gyroscopic drift regulating error model to gyro compensate；(3) data exported according to the gyro after compensation are updated to the system posture matrix at next moment, and the attitude angle of carrier is obtained according to the attitude matrix after renewal.The attitude heading reference system attitude algorithm accuracy that the present invention is solved caused by MEMS gyro precision is low, drift error is big is relatively low, it is impossible to the problem of meeting engineering application；MEMS gyro drift can be suppressed, improve attitude algorithm accuracy；With higher engineering application value and promotional value.
Description
Technical field
The invention belongs to field of navigation technology, is obtained more particularly, to a kind of posture of micro- inertia attitude heading reference system
Method.
Background technology
In inertial navigation system, posture is to reflect an important parameter of carrier movement, and usual attitude algorithm is basis
The data of gyro and the speed of carrier, positional information calculate attitude of carrier matrix in real time, realize navigational coordinate system to geographical coordinate
System follows and the resolving of attitude of carrier (angle of pitch, roll angle and course angle) in real time.And attitude accuracy to the speed of carrier,
Position calculating, recognition and tracking, movement locus, performance evaluation etc. have a major impact.But due to the precision of gyro and accelerometer
Limited, and increase over time error and can constantly accumulate increase, especially inexpensive MEMS gyro drift error is more
Greatly, make the attitude algorithm accuracy of system more and more lower, it is impossible to meet engineering technology requirement.
The content of the invention
The defects of for prior art, it is an object of the invention to provide a kind of posture of micro- inertia attitude heading reference system to obtain
Take method, it is intended to solve in the prior art because MEMS gyro precision is low, drift error causes greatly attitude heading reference system posture solution
Calculate the relatively low technical problem of precision.
The invention provides a kind of attitude acquisition method of micro- inertia attitude heading reference system, comprise the steps：
(1) at the beginning of the carrier acceleration information and three-dimensional magnetic field strength information gathered according to sensor obtains the attitude angle of carrier
Value；And the system posture matrix at current time is obtained according to the attitude angle initial value；
(2) gyroscopic drift error compensation amount is obtained according to the system posture matrix, and according to gyroscopic drift regulating error
The data that model exports to gyro compensate；
(3) data exported according to the gyro after compensation are updated to the system posture matrix at next moment, and root
The attitude angle of carrier is obtained according to the attitude matrix after renewal.
Further, step (1) specifically includes：
(1.1) the carrier acceleration information under carrier coordinate system is obtainedWith three-dimensional magnetic field intensity
Information
(1.2) angle of pitch initial value and roll angle initial value of carrier are calculated according to the carrier acceleration information；And according to institute
State the course angle initial value that three-dimensional magnetic field strength information calculates carrier；
(1.3) according to the angle of pitch initial value of carrier, roll angle initial value and course angle initial value and formula is combined
Obtain the attitude matrix at carrier current time
Wherein, hx hy hzRespectively X-axis, Y-axis, the magnetic field intensity of Z axis, ax ay azRespectively carrier is in X-axis, Y-axis, Z
The acceleration of axle；The angle of pitch of carrierThe roll angle of carrierG is carrier in geographical coordinate
The acceleration of gravity of system, the course angle of carrierHny=hx*cosγ+hy*sinθ*sinγ-hz*cosθ*
sinγ；Hnx=hy*cosθ+hz*sinγ；Navigational coordinate system is northeast day geographic coordinate system,Middle n represents navigational coordinate system, b
Represent carrier coordinate system.
Further, in step (2), gyroscopic drift regulating error model is ω (t)=ωgyro(t)+ωC(t)；
Wherein, under carrier coordinate system gyro angular velocity information The output information of respectively three gyros, ωC(t) it is gyroscopic drift error compensation amount, ω (t) is gyro benefit
Later output is repaid,
Because gyro error can increase over time continuous accumulation, the attitude information for causing to obtain contains very big accumulation
Error.Accelerometer measures information is the observed quantity of the angle of pitch, roll angle, and magnetometer information is the observed quantity of course angle, we
The information that method measures information using accelerometer and magnetometer measures compensates to gyro error, and the accumulation for eliminating gyro misses
Difference, improve posture and obtain precision.
Further, step (3) specifically includes：
(3.1) gyro angle increment antisymmetric matrix is obtained according to the gyro output data ω (t) after compensation
(3.2) according to formulaTo the attitude matrix at next momentMiddle quaternary number is carried out
Renewal；
(3.3) attitude matrix according to the quaternary number after renewal to next momentIt is updated；
(3.4) attitude angle of next moment carrier is calculated according to the attitude matrix after renewal；
Wherein, ωx ωy ωzThe output data of three gyros after respectively compensating；Δ t is system communication cycle；qkFor
The quaternary number at next moment, qk-1For the quaternary number at current time, M is the antisymmetric matrix of gyro angle increment；After renewal is
System attitude matrix
q0,q1,q2,q3For quaternary number；The attitude angle of next moment carrier includes pitching angle theta=arcsin (T32), roll
AngleCourse angle
In embodiments of the present invention, the attitude information of carrier can be used for system control and navigation attitude reference.
The attitude acquisition method of micro- inertia attitude heading reference system provided by the invention, with gyro, accelerometer and magnetometer
For system pedestal sensor, and utilize drift of the information of accelerometer and magnetometer to gyro to carry out effective compensation, reduce top
The drift error of spiral shell, attitude matrix is updated according to the gyro output data after compensation, so as to improve attitude heading reference system
Attitude algorithm accuracy, have larger contribution to the attitude algorithm in inertial navigation field, the present invention is simple ingenious, meets actual demand
Progress is notable and practical, being capable of large-scale promotion use.
Brief description of the drawings
Fig. 1 is a kind of attitude acquisition method implementation process of micro- inertia attitude heading reference system provided in an embodiment of the present invention
Figure.
Embodiment
In order to make the purpose , technical scheme and advantage of the present invention be clearer, it is right below in conjunction with drawings and Examples
The present invention is further elaborated.It should be appreciated that the specific embodiments described herein are merely illustrative of the present invention, and
It is not used in the restriction present invention.
The attitude acquisition method of micro- inertia attitude heading reference system provided by the invention, solve by MEMS gyro precision it is low,
Attitude heading reference system attitude algorithm accuracy caused by drift error is big is relatively low, it is impossible to the problem of meeting engineering application.
As shown in figure 1, the attitude acquisition method of micro- inertia attitude heading reference system provided in an embodiment of the present invention, including it is following
Several steps：
Step 1：System posture matrix initialization process；
Step 2：Accelerometer data and magnetometer data are read, and error compensation is carried out to gyro output data；
Step 3：System posture matrix is updated according to the gyro output data after compensation and according to the appearance after renewal
State matrix asks for posture.
As one embodiment of the present of invention, according to the accelerometer and magnetometer information computing system of reading in step 1
Initial attitude matrix, realized especially by following steps：
(1a) selection northeast day geographic coordinate system obtains carrier acceleration information as navigational coordinate system and from accelerometerWherein ax ay azRespectively carrier X-axis, Y-axis, Z axis acceleration；And obtained from magnetometer
Take three-dimensional magnetic field strength informationWherein hx hy hzRespectively X-axis, Y-axis, the magnetic field intensity of Z axis；Its
In, three directions of northeast day geographic coordinate system correspond to tri- reference axis of XYZ of carrier respectively, and maturation is due up in navigation field
Definition, no longer specific definition.
(1b) is according to formulaThe angle of pitch angle of calculating carrier and roll angle, wherein θ,
γ is respectively the angle of pitch and roll angle of carrier, and g is acceleration of gravity of the carrier in geographic coordinate system；
(1c) is according to formulaCalculate the course angle of carrier；And Hny=hx*cosγ+hy*sinθ*sin
γ-hz* cos θ * sin γ, Hnx=hy*cosθ+hz* sin γ, whereinThe respectively course angle of carrier；
(1d) is according to formula
The attitude matrix of the initial time of carrier is asked for, whereinFor the attitude matrix of carrier initial time, θ, γ,
The respectively angle of pitch of carrier, roll angle and course angle.
As an alternative embodiment of the invention, accelerometer data and magnetometer data are read in step 2, and to gyro
Output data carries out error compensation, is realized especially by following steps：
(2a) obtains the angular velocity information of carrier under carrier coordinate system from gyro
WhereinThe output information of respectively three gyros；
(2b) establishes gyroscopic drift regulating error model ω (t)=ωgyro(t)+ωC(t)；Wherein ω (t) compensates for gyro
Later output, ωgyro(t) it is the output before gyro compensation, ωC(t) it is gyroscopic drift error compensation amount, and
Wherein
As one embodiment of the present of invention, step 3 and step 4 are according to the gyro output data after compensation to posture
Matrix is updated；Posture is asked for according to the attitude matrix after renewal.Realized especially by following steps：
(3a) asks for gyro angle increment antisymmetric matrix according to the gyro output data ω (t) after compensation, make ω (t)=
[ωx ωy ωz]T, wherein ωx ωy ωzThe output information of three gyros after respectively compensating；And gyro angle increment antisymmetry
MatrixΔ t is system communication cycle；
(3b) is according to formulaQuaternary number is updated, wherein, qkFor the quaternary at k moment
Number, qk-1For the quaternary number at k-1 moment, M is the antisymmetric matrix of gyro angle increment；
(3c) is according to formula
To being
System attitude matrix is updated, q0,q1,q2,q3For quaternary number；
(3d) is according to θ=arcsin (T32)；The attitude angle of carrier is calculated, wherein
θ、r、The angle of pitch, roll angle and the course angle of carrier are represented respectively.
It can be obtained by by above-mentioned formula and later gyro output data compensated by accelerometer and magnetometer, utilize
Gyro output data after compensation is updated to quaternary number and attitude matrix, is asked by the attitude matrix after renewal according to formula
Solve the real-time attitude information of attitude heading reference system.
The attitude acquisition method of a kind of micro- inertia attitude heading reference system that the present invention is realized, with gyro, accelerometer and
Magnetic strength is calculated as system sensor, and carries out effective compensation with drift of the information of accelerometer and magnetometer to gyro, reduces top
The drift error of spiral shell, so as to improve the attitude algorithm accuracy of attitude heading reference system, have to the attitude algorithm in inertial navigation field compared with
Big contribution, the present invention is simple ingenious, and it is notable and practical to meet actual demand progress, can large-scale promotion use.
As it will be easily appreciated by one skilled in the art that the foregoing is merely illustrative of the preferred embodiments of the present invention, not to
The limitation present invention, all any modification, equivalent and improvement made within the spirit and principles of the invention etc., all should be included
Within protection scope of the present invention.
Claims (3)
1. a kind of attitude acquisition method of micro- inertia attitude heading reference system, it is characterised in that comprise the steps：
(1) the carrier acceleration information and three-dimensional magnetic field strength information that are gathered according to sensor obtain the attitude angle initial value of carrier；
And the system posture matrix at current time is obtained according to the attitude angle initial value；
(2) gyroscopic drift error compensation amount is obtained according to the system posture matrix, and according to gyroscopic drift regulating error model
The data of gyro output are compensated；
(3) system posture matrix at next moment is updated according to the gyro output data after compensation, and according to renewal
Attitude matrix afterwards obtains the attitude angle of carrier；
Step (1) specifically includes：
(1.1) the carrier acceleration information under carrier coordinate system is obtainedWith three-dimensional magnetic field strength information
(1.2) angle of pitch initial value and roll angle initial value of carrier are calculated according to the carrier acceleration information；And according to described three
Tie up the course angle initial value that magnetic field intensity information calculates carrier；
(1.3) according to the angle of pitch initial value of carrier, roll angle initial value and course angle initial value and formula is combined
Obtain the attitude matrix at carrier current time
Wherein, hx hy hzRespectively X-axis, Y-axis, the magnetic field intensity of Z axis, ax ay azRespectively carrier is in X-axis, Y-axis, Z axis
Acceleration；The angle of pitch of carrierThe roll angle of carrierG is carrier in geographic coordinate system
Acceleration of gravity, the course angle of carrierHny=hx*cosγ+hy*sinθ*sinγ-hz*cosθ*sin
γ；Hnx=hy*cosθ+hz*sinγ；Navigational coordinate system is northeast day geographic coordinate system,Middle n represents navigational coordinate system, b tables
Show carrier coordinate system.
2. attitude acquisition method as claimed in claim 1, it is characterised in that in step (2), gyroscopic drift regulating error model
For ω (t)=ωgyro(t)+ωC(t)；
Wherein, under carrier coordinate system gyro angular velocity information
The output information of respectively three gyros, ωC(t) it is gyroscopic drift error compensation amount, it is later defeated that ω (t) is that gyro compensates
Go out,
3. attitude acquisition method as claimed in claim 2, it is characterised in that step (3) specifically includes：
(3.1) gyro angle increment antisymmetric matrix is obtained according to the gyro output data ω (t) after compensation
(3.2) according to formulaTo the attitude matrix at next momentMiddle quaternary number is updated；
(3.3) attitude matrix according to the quaternary number after renewal to next momentIt is updated；
(3.4) attitude angle of next moment carrier is calculated according to the attitude matrix after renewal；
Wherein, ωxωyωzThe output data of three gyros after respectively compensating；Δ t is system communication cycle；qkFor it is next when
The quaternary number at quarter, qk-1For the quaternary number at current time, M is the antisymmetric matrix of gyro angle increment；Posture square after renewal
Battle array
q0,q1,q2,q3For quaternary number；The attitude angle of next moment carrier includes pitching angle theta=arcsin (T of carrier32), it is horizontal
Roll angleCourse angle
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510843867.8A CN105352502B (en) | 2015-11-26 | 2015-11-26 | A kind of attitude acquisition method of micro- inertia attitude heading reference system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510843867.8A CN105352502B (en) | 2015-11-26 | 2015-11-26 | A kind of attitude acquisition method of micro- inertia attitude heading reference system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105352502A CN105352502A (en) | 2016-02-24 |
| CN105352502B true CN105352502B (en) | 2017-12-12 |
Family
ID=55328512
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510843867.8A Active CN105352502B (en) | 2015-11-26 | 2015-11-26 | A kind of attitude acquisition method of micro- inertia attitude heading reference system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105352502B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106092098B (en) * | 2016-08-25 | 2019-01-04 | 湖北三江航天红峰控制有限公司 | A kind of carrier navigation attitude measurement method based on gyro and inclinator |
| CN106441301A (en) * | 2016-09-19 | 2017-02-22 | 北京机械设备研究所 | Air vehicle launching initial parameter acquiring method and system |
| CN108534744A (en) * | 2018-01-30 | 2018-09-14 | 歌尔科技有限公司 | A kind of attitude angle acquisition methods, device and handle |
| CN110231031A (en) * | 2018-03-05 | 2019-09-13 | 高德信息技术有限公司 | A kind of attitude angle determines method, apparatus and system |
| CN110530396A (en) * | 2019-07-31 | 2019-12-03 | 苏州浪潮智能科技有限公司 | A kind of processing method and equipment of gyro sensor error |
| CN110377056B (en) * | 2019-08-19 | 2022-09-20 | 深圳市道通智能航空技术股份有限公司 | Unmanned aerial vehicle course angle initial value selection method and unmanned aerial vehicle |
| CN118243096B (en) * | 2024-05-28 | 2024-08-06 | 火丰科技(深圳)有限公司 | Inertial orientation method of course axis gyro |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1740746B (en) * | 2005-05-23 | 2010-08-04 | 清华大学 | Micro-miniature dynamic carrier attitude measuring device and its measuring method |
| CN101290229A (en) * | 2008-06-13 | 2008-10-22 | 哈尔滨工程大学 | Inertial/Geomagnetism Combination Method for Silicon Micro-Airborne Attitude System |
| FI124697B (en) * | 2012-04-04 | 2014-12-15 | Jc Inertial Oy | Positioning of vehicles |
| CN102879793B (en) * | 2012-09-28 | 2014-07-23 | 北京信息科技大学 | Super-miniature GPS (global positioning system), INS (inertial navigation system), magnetometer and barometer integrated navigation system |
| US8915116B2 (en) * | 2013-01-23 | 2014-12-23 | Freescale Semiconductor, Inc. | Systems and method for gyroscope calibration |
| CN103776451B (en) * | 2014-03-04 | 2016-11-09 | 哈尔滨工业大学 | A kind of high-precision three-dimensional attitude inertial measurement system based on MEMS and measuring method |
| CN104121905B (en) * | 2014-07-28 | 2017-02-22 | 东南大学 | Course angle obtaining method based on inertial sensor |
| CN104567931B (en) * | 2015-01-14 | 2017-04-05 | 华侨大学 | A kind of heading effect error cancelling method of indoor inertial navigation positioning |
-
2015
- 2015-11-26 CN CN201510843867.8A patent/CN105352502B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN105352502A (en) | 2016-02-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105352502B (en) | A kind of attitude acquisition method of micro- inertia attitude heading reference system | |
| CN106289327B (en) | A kind of difference mobile robot mileage meter calibration method | |
| CN104635251B (en) | A kind of INS/GPS integrated positionings determine appearance new method | |
| CN104501838B (en) | SINS Initial Alignment Method | |
| CN103743413B (en) | Heeling condition modulated is sought northern instrument alignment error On-line Estimation and is sought northern error compensating method | |
| CN105910606B (en) | A kind of adjustment in direction method based on angular speed difference | |
| CN107328410A (en) | Method and automobile computer for positioning automatic driving vehicle | |
| CN103557866B (en) | A kind of based on the virtual gyroscope of geomagnetic technology and the method for algorithm | |
| CN103914068A (en) | Service robot autonomous navigation method based on raster maps | |
| CN109141410B (en) | Multi-sensor fusion positioning method for AGV (automatic guided vehicle) combined navigation | |
| CN103439727B (en) | A kind of measuring method of ground coordinate | |
| CN104748751A (en) | Calculating method of attitude matrix and positioning navigation method based on attitude matrix | |
| CN103245360A (en) | Autocollimation method of carrier aircraft rotating type strapdown inertial navigation system under shaking base | |
| CN108195400A (en) | The moving alignment method of strapdown micro electro mechanical inertia navigation system | |
| CN110672871B (en) | Moving target speed and direction measuring method based on combined inertial navigation information and photoelectric turret video tracking | |
| CN102589546A (en) | Optical-fiber strap-down inertial measurement unit reciprocating-type two-position north finding method for inhibiting slope error influence of devices | |
| CN105841698A (en) | AUV rudder angle precise real-time measurement system without zero setting | |
| CN109752000A (en) | A method for initial alignment of MEMS dual-axis rotational modulation strapdown compass | |
| CN102706349A (en) | Carrier gesture determining method based on optical fiber strap-down compass technology | |
| CN113108781B (en) | Improved coarse alignment method applied to unmanned ship during advancing | |
| CN104121930B (en) | A kind of compensation method based on the MEMS gyro drift error adding table coupling | |
| CN106643726B (en) | Unified inertial navigation resolving method | |
| CN112179340A (en) | Double-shaft rotation modulation method for redundancy configuration inertia measurement unit | |
| CN105737842A (en) | Vehicle-mounted autonomous navigation method based on rotary modulation and virtual odometer | |
| CN106092098B (en) | A kind of carrier navigation attitude measurement method based on gyro and inclinator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |