CN110006447A - A kind of any attitude MEMS integrated attitude determination method without being initially aligned - Google Patents
A kind of any attitude MEMS integrated attitude determination method without being initially aligned Download PDFInfo
- Publication number
- CN110006447A CN110006447A CN201910271775.5A CN201910271775A CN110006447A CN 110006447 A CN110006447 A CN 110006447A CN 201910271775 A CN201910271775 A CN 201910271775A CN 110006447 A CN110006447 A CN 110006447A
- Authority
- CN
- China
- Prior art keywords
- attitude
- error
- magnetometer
- state
- mems
- 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
Classifications
-
- 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)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
A kind of any attitude MEMS integrated attitude determination method without being initially aligned of the present invention, steps are as follows: 1) defining modified R odrigues parameter；2) it establishes and is based on modified R odrigues nonlinearity in parameters attitude error state equation；3) the non-linear observational equation of magnetometer is established；4) linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer, obtains the linearisation observation model of linearisation the state error model and magnetometer of state error；5) obtained nonlinear state error equation and observational equation is utilized, in conjunction with linearisation state model and observation model, the optimal estimation of modified R odrigues parameter is obtained using EKF filter method, it is used as posture result for Guidance and control, realizes the MEMS and magnetometer integrated attitude determination of any attitude.
Description
Technical field
The invention belongs to navigational guidance fields, are related to a kind of integrated attitude determination side any attitude MEMS without being initially aligned
Method.
Background technique
It is realized using MEMS inertia device as leading boat device build navigation system by integral calculation and determines appearance positioning, because
And it needs initially to be aligned before transmission.General common initial alignment scheme needs ground to establish datum mark, passes through ground
Electro-optic theodolite carries out light and takes aim at, and obtains initial navigation system initial state information.Need mating corresponding ground installation, production operation
Process, the alignment time is long, and combat readiness is complicated.An important factor for initial alignment time-consuming is influence launch preparation time, and shorten
Launch preparation time is to improve missile weapon system quick-reaction capability, improves the important means of fighting efficiency.
Summary of the invention
Technical problem solved by the present invention is proposing one aiming at the problem that initial alignment is time-consuming to reduce Weapon Combat efficiency
Kind realizes that three-axis attitude is determined by magnetometer, without ground without any attitude MEMS integrated attitude determination technology being initially aligned
Face is equipped with transmitting preparatory condition, while greatly shortening launch preparation time, efficiently solves and is used to group without MEMS in support transmitting
Initial Alignment.
The technical scheme is that a kind of any attitude MEMS integrated attitude determination method without being initially aligned, feature
It is that steps are as follows:
1) modified R odrigues parameter is defined；
2) it establishes and is based on modified R odrigues nonlinearity in parameters attitude error state equation；
3) the non-linear observational equation of magnetometer is established；
4) linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer, obtains state mistake
The linearisation observation model of linearisation the state error model and magnetometer of difference；
5) using obtained nonlinear state error equation and observational equation, in conjunction with linearisation state model and observation mould
Type obtains the optimal estimation of modified R odrigues parameter using EKF filter method, controls as posture result for guidance
System uses, and realizes the MEMS and magnetometer integrated attitude determination of any attitude.
The concrete form of modified R odrigues parameter in the step 1) are as follows:WhereinFor the unit vector of rotary shaft, θ is the angle turned over, subscriptTIndicate to
The transposition of amount or matrix.
The specific shape based on modified R odrigues nonlinearity in parameters attitude error state equation is established in the step 2)
Formula are as follows:
Wherein δ σ=[δ σ1 δσ2 δσ3]TFor the error of modified R odrigues parameter,For MEMS top
Spiral shell measurement error,Angular velocity of rotation for the used group body coordinate system of MEMS relative to reference frame is used to group ontology in MEMS
Projection under system,It is used to the measured value of group output for MEMS, symbol | | | | indicate that vector field homoemorphism, f (δ σ) indicate modified
Rodrigues parameter state error time change rateWith the functional relation of δ σ.
The concrete form of the non-linear observational equation of magnetometer is established in the step 3) are as follows:
Wherein magnetometer observation error To be calculated by Geomagnetic Field Model, attitude quaternion
The ontology quaternary number arrived,For magnetometer survey value, g (δ σ) indicates relevant to state error δ σ in magnetometer survey error
Functional relation.
Linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer in the step 4),
Obtain the concrete form of the inearized model of state error are as follows:
WhereinState error time rate of change f (δ σ) is indicated to the partial derivative of state error, G indicates state
Partial derivative of the error time change rate to MEMS gyro measurement error.
Concrete form of the state error time rate of change f (δ σ) to the partial derivative of state error are as follows:
It is described to indicate state error time rate of change to the concrete form of the partial derivative G of MEMS gyro measurement error are as follows:
I3×3For the unit matrix of 3 × 3 dimensions.
Linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer in the step 4),
Obtain the concrete form of the linearisation observation model of magnetometer are as follows:
WhereinFor magnetometer observation modelTo the partial derivative of state error δ σ,Indicate that g (δ σ) is to the inclined of state error δ σ in magnetometer error model
Derivative,
G (δ σ) is to the partial derivative of state error δ σ in the magnetometer error modelSpecific formula is as follows:
The tool of the optimal estimation of modified R odrigues parameter is obtained in the step 5) using EKF filter method
Body process are as follows: using obtained nonlinear state error equation and observational equation, in conjunction with linearisation state model and observation model,
Using EKF filter method, recursion obtains the optimal estimation of modified R odrigues parameter, as posture result for guidance
Control uses, and realizes the MEMS and magnetometer integrated attitude determination of any attitude.
Compared with the prior art, the advantages of the invention are the MEMS/ magnetometer integrated attitude determination method that this project proposes, is based on
Modified Rodrigues establish state equation and observational equation, obtain error most using EKF filter method
Excellent estimation realizes the integrated attitude determination under any attitude without initial information, efficiently solves initial without group is used in support transmitting
Alignment issues without being equipped with ground aiming facility, while greatly shortening launch preparation time.
Detailed description of the invention
Fig. 1 is the method for the present invention flow chart；
Fig. 2 is MEMS gyro magnetometer integrated attitude determination error schematic diagram.
Specific embodiment
This project proposes one kind without any initial state information, obtains merely with used group with three axis magnetometer
The method of the three-axis attitude information of missile weapon system.Without any priori posture information, attitude error is no longer small
Deviation cannot establish attitude error model with traditional little deviation linear equation.Description nonlinear attitude error generallys use
Quaternion model, quaternary number is there are four variable, and only there are three being independent, thus there are redundancy issues, can make state error
Covariance matrix generation is unusual, while resolving the problem of there is also standardization using quaternary number.Traditional processing method is using four
The vector section of first number is modeled, and the scalar component in quaternary number, i.e. rotation angle size are ignored, i.e. hypothesis quaternary number error is
Low-angle.But used group initial attitude deviation is larger in practice, and above-mentioned model no longer adapts to, and needs to establish arbitrary initial great-attitude angle
The adaptable attitude error model of deviation.
Modified R odrigues parameter is introduced into posture determination by this project, converts three for four parameters of quaternary number
A parameter describes attitude error, retains the vector form and amplitude of quaternary number, while avoiding existing using attitude angle description
Singularity problem, without any initial state information, can be completed three-axis attitude determine.
1, Modified Rodrigues are defined, three amounts is converted by quaternary number four amounts to describe posture, guarantees three
Independence between a parameter, while avoiding error covariance matrix singular problem and On The Standardization existing for traditional quaternion.
Modified Rodrigues:
In formula,For quaternary number；For rotary shaft unit vector, θ is
The angle turned over.Rotation angle is described by tan (θ 4), modified R odrigues parameter can adapt to-π~π range,
It is suitable for the quaternary number of any rotation angle.
Quaternary number deviationWherein qcFor the observation of quaternary number,
Corresponding modified R odrigues error can indicate are as follows:
In formula, δ qvIndicate quaternary number error vector part；Similarly, that quaternary number error can be obtained by formula (2) is as follows:
2, the attitude error state equation based on modified R odrigues parameter is established:
Differentiating to formula (2), to obtain attitude error equations as follows for equation:
The error equation of quaternary number is as follows:
Wherein gyro to measure error Rotation for used group body coordinate system relative to reference frame
Projection of the tarnsition velocity under used group this system,For the measured value of used group output.
Formula (5), (3) are substituted into formula (4):
3, the observation model of magnetometer is established:
Subtract each other the observation model for establishing magnetometer with magnetic intensity vector:
In formula,For the ontology quaternary number being calculated by Geomagnetic Field Model, attitude quaternion,For magnetic strength measurement
Magnitude；
Formula (3) substitution formula (7) can be obtained:
4, line processing is carried out to the observation model of nonlinear attitude error state model and magnetometer:
Formula (6) is linearized, the inearized model of state error is obtained:
Wherein:
Formula (8) is linearized, magnetometer linearisation observation model is obtained:
Wherein
Wherein,Its calculation formula is as follows:
5, kalman filtering estimation is carried out to error model
According to the state equation and observation model of above-mentioned foundation, using non-linear kalman filtering method, to arbitrary initial
Used group+magnetometer the combined system of MEMS is estimated under posture, realizes integrated attitude determination.
The content that description in the present invention is not described in detail belongs to the well-known technique of professional and technical personnel in the field.
According to the state model and observation model of above-mentioned foundation, using nonlinear Kalman filtering mode, to arbitrary initial
Used group/magnetometer integrated attitude determination the technology of MEMS is verified under attitude misalignment, and simulation result is as shown in Figure 2, the results showed that combination
It is correct to determine appearance model, concept feasible.
Claims (10)
1. a kind of any attitude MEMS integrated attitude determination method without being initially aligned, it is characterised in that steps are as follows:
1) modified R odrigues parameter is defined；
2) it establishes and is based on modified R odrigues nonlinearity in parameters attitude error state equation；
3) the non-linear observational equation of magnetometer is established；
4) linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer, obtains state error
Linearize the linearisation observation model of state error model and magnetometer；
5) using obtained nonlinear state error equation and observational equation, in conjunction with linearisation state model and observation model, benefit
The optimal estimation of modified R odrigues parameter is obtained with EKF filter method, is made as posture result for Guidance and control
With realizing the MEMS and magnetometer integrated attitude determination of any attitude.
2. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 1, feature exist
In: the concrete form of modified R odrigues parameter in the step 1) are as follows:WhereinFor the unit vector of rotary shaft, θ is the angle turned over, subscriptTIndicate the transposition of vector or matrix.
3. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 1, feature exist
In: the concrete form based on modified R odrigues nonlinearity in parameters attitude error state equation is established in the step 2) are as follows:
Wherein δ σ=[δ σ1 δσ2 δσ3]TFor the error of modified R odrigues parameter,For MEMS gyro measurement
Error,It is used to organize under this system in MEMS relative to the angular velocity of rotation of reference frame for the used body coordinate system of organizing of MEMS
Projection,It is used to the measured value of group output for MEMS, symbol | | | | indicate that vector field homoemorphism, f (δ σ) indicate modified
Rodrigues parameter state error time change rateWith the functional relation of δ σ.
4. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 3, feature exist
In: the concrete form of the non-linear observational equation of magnetometer is established in the step 3) are as follows:
Wherein magnetometer observation error It is calculated by Geomagnetic Field Model, attitude quaternion
Ontology quaternary number,For magnetometer survey value, g (δ σ) indicates function relevant to state error δ σ in magnetometer survey error
Relationship.
5. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 3, feature exist
In: linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer in the step 4), is obtained
The concrete form of the inearized model of state error are as follows:
WhereinState error time rate of change f (δ σ) is indicated to the partial derivative of state error, G indicates state error
Partial derivative of the time rate of change to MEMS gyro measurement error.
6. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 5, feature exist
In: concrete form of the state error time rate of change f (δ σ) to the partial derivative of state error are as follows:
7. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 5, feature exist
In: it is described to indicate state error time rate of change to the concrete form of the partial derivative G of MEMS gyro measurement error are as follows:
I3×3For the unit matrix of 3 × 3 dimensions.
8. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 3, feature exist
In: linearization process is carried out to the observational equation of nonlinear attitude error state equation and magnetometer in the step 4), is obtained
The concrete form of the linearisation observation model of magnetometer are as follows:
WhereinFor magnetometer observation modelTo the partial derivative of state error δ σ,Indicate that g (δ σ) is to the inclined of state error δ σ in magnetometer error model
Derivative.
9. a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 8, feature exist
In: g (δ σ) is to the partial derivative of state error δ σ in the magnetometer error modelSpecific formula is as follows:
10. -9 any a kind of any attitude MEMS integrated attitude determination method without being initially aligned according to claim 1,
It is characterized in that: obtaining the optimal estimation of modified R odrigues parameter in the step 5) using EKF filter method
Detailed process are as follows: using obtained nonlinear state error equation and observational equation, in conjunction with linearisation state model and observation mould
Type, using EKF filter method, recursion obtains the optimal estimation of modified R odrigues parameter, supplies as posture result
Guidance and control uses, and realizes the MEMS and magnetometer integrated attitude determination of any attitude.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910271775.5A CN110006447B (en) | 2019-04-04 | 2019-04-04 | Random attitude MEMS combination attitude determination method without initial alignment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910271775.5A CN110006447B (en) | 2019-04-04 | 2019-04-04 | Random attitude MEMS combination attitude determination method without initial alignment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110006447A true CN110006447A (en) | 2019-07-12 |
| CN110006447B CN110006447B (en) | 2020-11-10 |
Family
ID=67169987
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910271775.5A Active CN110006447B (en) | 2019-04-04 | 2019-04-04 | Random attitude MEMS combination attitude determination method without initial alignment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110006447B (en) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101782391A (en) * | 2009-06-22 | 2010-07-21 | 北京航空航天大学 | Attitude estimation method of maneuvering acceleration-assisted extended Kalman filter (EKF) attitude and heading reference system (AHRS) |
| CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
| CN102419597A (en) * | 2011-12-05 | 2012-04-18 | 哈尔滨工业大学 | Method for consistently controlling gesture of large-scale formation spacecraft with limited relative gesture |
| CN102538821A (en) * | 2011-12-17 | 2012-07-04 | 东南大学 | Fast and parameter sectional type self-alignment method for strapdown inertial navigation system |
| CN103900613A (en) * | 2014-03-31 | 2014-07-02 | 哈尔滨工程大学 | Micro-electromechanical system (MEMS) error estimation method based on magnetometer and N step interval detection |
| CN103940442A (en) * | 2014-04-03 | 2014-07-23 | 深圳市宇恒互动科技开发有限公司 | Location method and device adopting accelerating convergence algorithm |
| US20140202229A1 (en) * | 2013-01-23 | 2014-07-24 | Michael E. Stanley | Systems and method for gyroscope calibration |
| CN104635251A (en) * | 2013-11-08 | 2015-05-20 | 中国地质大学（北京） | Novel INS (inertial navigation system)/ GPS (global position system) combined position and orientation method |
| CN105074381A (en) * | 2013-01-21 | 2015-11-18 | 可信定位股份有限公司 | Method and apparatus for determination of misalignment between device and pedestrian |
| CN106648088A (en) * | 2016-12-14 | 2017-05-10 | 影动（北京）科技有限公司 | Inertial motion capture pose transient calibration method and inertial motion capture system |
-
2019
- 2019-04-04 CN CN201910271775.5A patent/CN110006447B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101782391A (en) * | 2009-06-22 | 2010-07-21 | 北京航空航天大学 | Attitude estimation method of maneuvering acceleration-assisted extended Kalman filter (EKF) attitude and heading reference system (AHRS) |
| CN101893445A (en) * | 2010-07-09 | 2010-11-24 | 哈尔滨工程大学 | Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition |
| CN102419597A (en) * | 2011-12-05 | 2012-04-18 | 哈尔滨工业大学 | Method for consistently controlling gesture of large-scale formation spacecraft with limited relative gesture |
| CN102538821A (en) * | 2011-12-17 | 2012-07-04 | 东南大学 | Fast and parameter sectional type self-alignment method for strapdown inertial navigation system |
| CN105074381A (en) * | 2013-01-21 | 2015-11-18 | 可信定位股份有限公司 | Method and apparatus for determination of misalignment between device and pedestrian |
| US20140202229A1 (en) * | 2013-01-23 | 2014-07-24 | Michael E. Stanley | Systems and method for gyroscope calibration |
| CN104635251A (en) * | 2013-11-08 | 2015-05-20 | 中国地质大学（北京） | Novel INS (inertial navigation system)/ GPS (global position system) combined position and orientation method |
| CN103900613A (en) * | 2014-03-31 | 2014-07-02 | 哈尔滨工程大学 | Micro-electromechanical system (MEMS) error estimation method based on magnetometer and N step interval detection |
| CN103940442A (en) * | 2014-04-03 | 2014-07-23 | 深圳市宇恒互动科技开发有限公司 | Location method and device adopting accelerating convergence algorithm |
| CN106648088A (en) * | 2016-12-14 | 2017-05-10 | 影动（北京）科技有限公司 | Inertial motion capture pose transient calibration method and inertial motion capture system |
Non-Patent Citations (4)
| Title |
|---|
| PENG WU 等: ""Research on Attitude Updating Algorithm Based on"", 《THE 2010 IEEE INTERNATIONAL CONFERENCE ON INFORMATION AND AUTOMATION》 * |
| SUN HAIBIN 等: ""Composite control method for stabilizing spacecraft"", 《CHINESE JOURNAL OF AERONAUTICS》 * |
| 李鑫 等: ""基于自适应滤波的MEMS姿态确定方法"", 《传感技术学报》 * |
| 陈记争 等: ""基于 Rodrigues 参数的姿态估计算法"", 《航空学报》 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110006447B (en) | 2020-11-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105157705B (en) | A kind of half strapdown radar seeker line of sight rate extracting method | |
| CN106052716B (en) | Gyro error online calibration method based on starlight information auxiliary under inertial system | |
| KR102017404B1 (en) | How to update the full posture angle of agricultural machinery based on 9 axis MEMS sensor | |
| CN103363992B (en) | Based on four rotor wing unmanned aerial vehicle attitude heading reference system calculation methods of Gradient Descent | |
| CN105091744B (en) | The apparatus for detecting position and posture and method of a kind of view-based access control model sensor and laser range finder | |
| CN105180968A (en) | IMU/magnetometer installation misalignment angle online filter calibration method | |
| CN106525034B (en) | A kind of inertial navigation system Transfer Alignment modeling method based on dual quaterion | |
| CN103591949B (en) | The quadrature compensation method of three-axis attitude measuring system nonorthogonality error | |
| CN103822633B (en) | A kind of low cost Attitude estimation method measuring renewal based on second order | |
| CN106052682B (en) | A kind of hybrid inertial navigation system and air navigation aid | |
| CN104567930A (en) | Transfer alignment method capable of estimating and compensating wing deflection deformation | |
| CN103983276B (en) | A kind of three framework four axle inertial platform error calibrating methods based on navigation benchmark system | |
| CN108318038A (en) | A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method | |
| CN105890598B (en) | Quadrotor attitude algorithm method of the conjugate gradient in conjunction with Extended Kalman filter | |
| CN105865490B (en) | A kind of inertially stabilized platform fixed pedestal multiposition is from method of sight | |
| CN107063254B (en) | Gesture resolving method for gyros and geomagnetic combination | |
| CN102937450B (en) | A kind of relative attitude defining method based on gyro to measure information | |
| WO2020114293A1 (en) | Magnetic side roll-based rotary shell muzzle initial parameter measuring method | |
| CN110044385B (en) | Rapid transfer alignment method under condition of large misalignment angle | |
| CN103940425A (en) | Magnetic-inertial combination strapdown measuring method | |
| CN109827571A (en) | A kind of dual acceleration meter calibration method under the conditions of no turntable | |
| CN104897172A (en) | Motion capture system based method for compensating rotary MEMS (micro-electromechanical system) inertial navigation magnetic heading angle errors | |
| CN104236586A (en) | Moving base transfer alignment method based on measurement of misalignment angle | |
| CN105180728A (en) | Front data based rapid air alignment method of rotary guided projectiles | |
| CN105115508A (en) | Post data-based rotary guided projectile quick air alignment method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |