CN106289244A - A kind of attitude determination method of anti-acceleration noise - Google Patents

A kind of attitude determination method of anti-acceleration noise Download PDF

Info

Publication number
CN106289244A
CN106289244A CN201610649983.0A CN201610649983A CN106289244A CN 106289244 A CN106289244 A CN 106289244A CN 201610649983 A CN201610649983 A CN 201610649983A CN 106289244 A CN106289244 A CN 106289244A
Authority
CN
China
Prior art keywords
acceleration
linear acceleration
attitude
carrier
determination method
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201610649983.0A
Other languages
Chinese (zh)
Other versions
CN106289244B (en
Inventor
杭义军
邢丽
贾文峰
吕印新
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiyi Robot (shanghai) Co Ltd
Original Assignee
Jiyi Robot (shanghai) Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiyi Robot (shanghai) Co Ltd filed Critical Jiyi Robot (shanghai) Co Ltd
Priority to CN201610649983.0A priority Critical patent/CN106289244B/en
Publication of CN106289244A publication Critical patent/CN106289244A/en
Application granted granted Critical
Publication of CN106289244B publication Critical patent/CN106289244B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Gyroscopes (AREA)
  • Navigation (AREA)

Abstract

The present invention relates to equipment precision evaluation areas, particularly relate to the attitude determination method of a kind of anti-acceleration noise.The attitude determination method of a kind of anti-acceleration noise of the application design is by setting up linear acceleration model and linear acceleration error equation, the linear acceleration of real-time adaptive equipment is carried out with this, determine the attitude of the anti-acceleration noise of equipment, compared with traditional attitude determination method, the method for proposition self adaptation can estimate carrier linear acceleration the most in real time.Compared with traditional attitude determination method, the attitude determination method of proposition effectively weakens the interference of carrier linear acceleration, improves Attitude estimation precision and stability.

Description

A kind of attitude determination method of anti-acceleration noise
Technical field
The present invention relates to equipment precision evaluation areas, particularly relate to the attitude determination method of a kind of anti-acceleration noise.
Background technology
Small and micro-satellite, The Cloud Terrace or hand-held The Cloud Terrace etc. need to measure and the relevant MEMS navigation application neck of perception attitude Territory, is required for using MEMS gyroscope to measure attitudes vibration, but MEMS gyroscope is during calculating attitude, due to The existence of device error, integration can introduce bigger calculating error, need for this to combine MEMS acceleration during obtaining attitude The attitude angle of gyro integration is modified by meter and MEMS gaussmeter.Three kinds of MEMS sensor of existing comprehensive utilization carry out attitude appearance The method that state determines mainly has complementary filter algorithm, Kalman filtering algorithm and gradient descent method.When only utilizing MEMS gyro When instrument and mems accelerometer, can accurately add by sensitive gravity in the case of radio acceleration mainly by mems accelerometer Speed, thus obtain the attitude that MEMS gyroscope integration obtains by attitude of carrier and be modified, when carrier has linear acceleration, existing Method is had to be mainly the correcting force to the attitude that MEMS gyroscope integration obtains of the attitude by reducing mems accelerometer calculating Degree, reduces the interference of linear acceleration.But the method for this anti-acceleration noise, owing to can not carry out linear acceleration interference Effectively estimating, therefore anti-jamming effectiveness is poor.For this problem, it is proposed that the attitude side of determination of a kind of anti-acceleration noise Method, improves the precision and stability of attitude determination method.
Summary of the invention
It is an object of the invention to provide the attitude determination method of a kind of anti-acceleration noise, in view of the above problems, it makes up Above-mentioned defect also provides the advantage that
The attitude determination method of a kind of anti-acceleration noise, is applied to measure and in the equipment of perception attitude, and it is special In levying, including:
The linear acceleration setting up carrier coordinate system estimates model；
Set up the state equation of the linear acceleration estimation difference comprising described carrier coordinate system；
Set up the measurement equation with acceleration of gravity vector observation error as measurement；
Utilize described linear acceleration estimation difference state equation, described with acceleration of gravity vector observation error for measure The measurement equation of amount and linear acceleration estimate that model real-time adaptive estimates linear acceleration, determine the anti-acceleration of described equipment with this The attitude of degree interference.
Above-mentioned method, wherein, described method also includes:
Linear acceleration under carrier system is changed and is approximately single order markoff process.
Above-mentioned method, wherein, described method also includes:
Choose the linear acceleration estimation difference under attitude of carrier angular estimation error, the inclined estimation difference of gyroscope zero and carrier system As the quantity of state of wave filter, build Kalman filtering state error vector with this.
Above-mentioned method, wherein, described method also includes:
Described measurement is chosen the acceleration of gravity estimated by accelerometer and the gravity acceleration estimated by gyroscope The difference of degree constitutes three-dimensional observation vector.
Above-mentioned method, wherein, described method also includes:
Linear acceleration described in self-adaptative adjustment estimates the algorithm coefficient in model.
In sum, the attitude determination method of a kind of anti-acceleration noise of the open design of the present invention, with traditional attitude The method of determination is compared, and the method for proposition self adaptation can estimate carrier linear acceleration the most in real time.With traditional attitude side of determination Method is compared, and the attitude determination method of proposition effectively weakens the interference of carrier linear acceleration, improves Attitude estimation precision and stablizes Property.
Accompanying drawing explanation
With reference to appended accompanying drawing, more fully to describe embodiments of the invention.But, appended accompanying drawing be merely to illustrate and Illustrate, be not intended that limitation of the scope of the invention.
Fig. 1 is the flow chart of the present invention.
Detailed description of the invention
The present invention is further illustrated with specific embodiment below in conjunction with the accompanying drawings, but not as the limit of the present invention Fixed.
As it is shown in figure 1, the application designs the attitude determination method of a kind of anti-acceleration noise, concrete operating principle is such as Under:
1, establish carrier coordinate system linear acceleration and estimate model；
When carrier carries out that acceleration and deceleration are motor-driven or under vibration, carrier exists linear acceleration, can be by under carrier system Linear acceleration change is approximately single order markoff process, as shown in formula (1), whereinFor tkThe carrier anchor line (string) in moment accelerates Degree,For tk-1The carrier anchor line (string) acceleration in moment, subscript B represents carrier system.τ is single order Markov correlation time.Carrying In the method gone out, caIt is referred to as linear acceleration and estimates the attenuation quotient of model, from model, can be seen that caRelevant with τ.
2, the state equation comprising carrier coordinate system linear acceleration estimation difference is established；
Choose attitude of carrier angular estimation error(Δ γ is roll angle error, and Δ φ is the angle of pitch Error,For course angle error), the inclined estimation difference Δ ε of gyroscope zerob=[Δ εbx Δεby Δεbz] and carrier system under Linear acceleration estimation differenceAs the quantity of state of wave filter, the Kalman filtering state of structure Error vector is Δ X=[Δ θ Δ εb ΔaB]T.Shown in the state equation such as formula (2) set up, wherein Ak+1/k=09×9, state The one-step prediction equation estimated is abbreviated as formula (3), and State Estimation equation is abbreviated as formula (4).WhereinFor error state amount Priori estimates,For the posterior estimate of error state amount, KkFor tkThe Kalman filtering gain in moment, ZkFor measuring Amount, HkFor measurement matrix.
ΔXk=Ak/k-1ΔXk-1+Wk-1 (2)
3, the measurement equation with acceleration of gravity vector observation error as measurement is set up；
In order to solve the State Estimation in formula (4), need to build measurement ZkModel.Measurement is chosen for by accelerating Degree counts the 3-dimensional measurement vector that the difference of the acceleration of gravity estimated and the acceleration of gravity estimated by gyroscope is constituted, and estimates The acceleration of gravity difference of meter is all projected in body carrier coordinate system, shown in the measurement equation of structure such as formula (5).
In formulaThe acceleration of gravity that expression is estimated by accelerometer is in the projection of carrier system, and it calculates process such as formula (6) shown in, whereinFor tkThe accelerometer output in moment,For tk-1Line under moment carrier system accelerates posterior estimate,For tkLinear acceleration priori estimates under moment carrier system.
In formula (5)Being the acceleration of gravity projection in carrier system of gyroscope estimation, it calculates process such as formula (7) Shown in.WhereinFor tkAcceleration of gravity vector under moment navigation system,For tkMoment direction cosines The priori estimates of matrix,For tk-1The direction cosine matrix posterior estimate in moment, subscript N represents navigation system,The priori estimates that gyro zero is to the rear is revised for gyro outputThe attitudes vibration matrix that integration obtains.
The acquisition that throughput is measured, through further deriving, can obtain the relation between measurement and quantity of state such as Shown in formula (8), and formula measures HkBeing represented by formula (9), T is the Kalman filtering cycle.
4, the algorithm coefficient during self-adaptative adjustment linear acceleration estimates model；
When attitude determination method is applied to concrete carrier, when carrier does acceleration and deceleration motion or in vibration environment, The attenuation quotient c in model can be estimated by the linear acceleration shown in self-adaptative adjustment formula (1)aCarry out linear acceleration interference to estimate Meter.caAutomatic adjusument process see shown in formula (10), whereinMean square for the linear acceleration error estimator under plateau Difference,For tkThe linear acceleration error estimate in moment.
5, utilizing linear acceleration estimation difference and linear acceleration to estimate model, real-time adaptive estimates linear acceleration；
The state built according to formula (3)～formula (9) and measurement equation, estimate partially to attitude of carrier angle error Δ θ, gyroscope zero Meter error delta epsilonbAnd the linear acceleration estimation difference Δ a under carrier systemBEstimation procedure such as formula (11) shown in.Qk-1For tk-1Time The state-noise matrix carved, Pk/k-1For tkThe state covariance prior estimate matrix in moment, PkFor tkAfter the state covariance in moment Test estimated matrix, RkFor tkThe measurement noise matrix in moment.
(mainly it is modified estimating to attitude quaternion to attitude angle further based on the state error amount estimated Meter), gyro zero linear acceleration partially and under carrier system is modified estimating.Attitude angle quaternary number and the inclined correction of gyro zero are estimated Shown in process such as formula (12).WhereinFor tkThe attitude quaternion posterior estimate in moment,For tk-1The attitude quaternary in moment Number posterior estimate,For tkThe attitude quaternion priori estimates in moment,For by tkThe attitude that moment is estimated Angle errorSolve the attitude quaternion increment obtained,Zero priori estimates to the rear is revised for gyro outputThe attitude quaternion increment that integration obtains, in formula (7)Can pass throughSolve,Can pass throughSolve.For tkThe gyroscope output in moment,For tkThe inclined priori of gyroscope zero in moment is estimated Evaluation,For tk-1The inclined posterior estimate of gyroscope zero in moment,For tkThe gyroscope zero offset error estimated value in moment.
Shown in linear acceleration correction estimation procedure such as formula (13) under carrier system.In formulaFor tkLine under moment carrier system Acceleration posterior estimate,For tkLinear acceleration priori estimates under moment carrier system,For tkUnder moment carrier system Linear acceleration estimation difference,For tk-1Linear acceleration posterior estimate under moment carrier system, caEstimate for linear acceleration Algorithm coefficient in model.
The attitude determination method of the anti-acceleration noise of the application can apply to unmanned plane, The Cloud Terrace and hand-held The Cloud Terrace, wears Wear formula pedestrian navigation etc..
In sum, the attitude determination method of a kind of anti-acceleration noise of the application design, determine with traditional attitude Method is compared, and the method for proposition self adaptation can estimate carrier linear acceleration the most in real time.With traditional attitude determination method phase Ratio, the attitude determination method of proposition effectively weakens the interference of carrier linear acceleration, improves Attitude estimation precision and stability.
By explanation and accompanying drawing, give the exemplary embodiments of the ad hoc structure of detailed description of the invention, based on present invention essence God, also can make other conversion.Although foregoing invention proposes existing preferred embodiment, but, these contents are not intended as Limitation.
For a person skilled in the art, after reading described above, various changes and modifications are undoubtedly by aobvious and easy See.Therefore, appending claims should regard whole variations and modifications of true intention and the scope containing the present invention as.? In Claims scope, the scope of any and all equivalence and content, be all considered as still belonging to the intent and scope of the invention.

Claims (5)

1. an attitude determination method for anti-acceleration noise, is applied to measure and in the equipment of perception attitude, its feature It is, including:
The linear acceleration setting up carrier coordinate system estimates model；
Set up the state equation of the linear acceleration estimation difference comprising described carrier coordinate system；
Set up the measurement equation with acceleration of gravity vector observation error as measurement；
Utilize the state equation of described linear acceleration estimation difference, described with acceleration of gravity vector observation error as measurement Measurement equation and linear acceleration estimate that model real-time adaptive estimates linear acceleration, do with the anti-acceleration that this determines described equipment The attitude disturbed.
Method the most according to claim 1, it is characterised in that described method also includes:
Linear acceleration under carrier system is changed and is approximately single order markoff process.
Method the most according to claim 1, it is characterised in that described method also includes:
Choose the linear acceleration estimation difference conduct under attitude of carrier angular estimation error, the inclined estimation difference of gyroscope zero and carrier system The quantity of state of wave filter, builds Kalman filtering state error vector with this.
Method the most according to claim 1, it is characterised in that described method also includes:
Described measurement is chosen the acceleration of gravity estimated by accelerometer and the acceleration of gravity estimated by gyroscope Difference constitutes three-dimensional observation vector.
Method the most according to claim 1, it is characterised in that described method also includes:
Linear acceleration described in self-adaptative adjustment estimates the algorithm coefficient in model.
CN201610649983.0A 2016-08-10 2016-08-10 A kind of attitude determination method of anti-acceleration noise Active CN106289244B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610649983.0A CN106289244B (en) 2016-08-10 2016-08-10 A kind of attitude determination method of anti-acceleration noise

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610649983.0A CN106289244B (en) 2016-08-10 2016-08-10 A kind of attitude determination method of anti-acceleration noise

Publications (2)

Publication Number Publication Date
CN106289244A true CN106289244A (en) 2017-01-04
CN106289244B CN106289244B (en) 2019-11-26

Family

ID=57667691

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610649983.0A Active CN106289244B (en) 2016-08-10 2016-08-10 A kind of attitude determination method of anti-acceleration noise

Country Status (1)

Country Link
CN (1) CN106289244B (en)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107014386A (en) * 2017-06-02 2017-08-04 武汉云衡智能科技有限公司 The disturbing acceleration measuring method that a kind of attitude of flight vehicle is resolved
CN108426559A (en) * 2018-02-27 2018-08-21 北京环境特性研究所 A kind of antenna attitude detection device and method
CN108513610A (en) * 2017-04-21 2018-09-07 深圳市大疆灵眸科技有限公司 A kind of holder Attitude estimation method, apparatus and corresponding holder
WO2018191963A1 (en) * 2017-04-21 2018-10-25 深圳市大疆灵眸科技有限公司 Remote control, camera mount, and camera mount control method, device, and system
CN110672099A (en) * 2019-09-09 2020-01-10 武汉元生创新科技有限公司 Course correction method and system for indoor robot navigation
CN113752265A (en) * 2021-10-13 2021-12-07 国网山西省电力公司输电检修分公司 Method, system and device for planning obstacle avoidance path of mechanical arm

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1664506A (en) * 2004-03-05 2005-09-07 清华大学 Carrier attitude measurement method and system
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)
US20120136604A1 (en) * 2010-11-30 2012-05-31 Industrial Technology Research Institute Method and apparatus for 3d attitude estimation
US20140290398A1 (en) * 2010-02-15 2014-10-02 Texas Instruments Incorporated Accelerometer-aided gyroscope
CN104819717A (en) * 2015-05-20 2015-08-05 苏州联芯威电子有限公司 Multi-rotor aircraft attitude detection method based on MEMS inertial sensor group
CN105371846A (en) * 2015-11-13 2016-03-02 广州周立功单片机科技有限公司 Carrier attitude detection method and system

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1664506A (en) * 2004-03-05 2005-09-07 清华大学 Carrier attitude measurement method and system
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)
US20140290398A1 (en) * 2010-02-15 2014-10-02 Texas Instruments Incorporated Accelerometer-aided gyroscope
US20120136604A1 (en) * 2010-11-30 2012-05-31 Industrial Technology Research Institute Method and apparatus for 3d attitude estimation
CN104819717A (en) * 2015-05-20 2015-08-05 苏州联芯威电子有限公司 Multi-rotor aircraft attitude detection method based on MEMS inertial sensor group
CN105371846A (en) * 2015-11-13 2016-03-02 广州周立功单片机科技有限公司 Carrier attitude detection method and system

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
孟唐宇，等，: "姿态解算与外力加速度同步估计算法", 《计算机应用》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108513610A (en) * 2017-04-21 2018-09-07 深圳市大疆灵眸科技有限公司 A kind of holder Attitude estimation method, apparatus and corresponding holder
WO2018191963A1 (en) * 2017-04-21 2018-10-25 深圳市大疆灵眸科技有限公司 Remote control, camera mount, and camera mount control method, device, and system
CN107014386A (en) * 2017-06-02 2017-08-04 武汉云衡智能科技有限公司 The disturbing acceleration measuring method that a kind of attitude of flight vehicle is resolved
CN107014386B (en) * 2017-06-02 2019-08-30 武汉云衡智能科技有限公司 A kind of disturbing acceleration measurement method that attitude of flight vehicle resolves
CN108426559A (en) * 2018-02-27 2018-08-21 北京环境特性研究所 A kind of antenna attitude detection device and method
CN108426559B (en) * 2018-02-27 2020-07-24 北京环境特性研究所 Antenna attitude detection device and method
CN110672099A (en) * 2019-09-09 2020-01-10 武汉元生创新科技有限公司 Course correction method and system for indoor robot navigation
CN113752265A (en) * 2021-10-13 2021-12-07 国网山西省电力公司输电检修分公司 Method, system and device for planning obstacle avoidance path of mechanical arm
CN113752265B (en) * 2021-10-13 2024-01-05 国网山西省电力公司超高压变电分公司 Method, system and device for planning obstacle avoidance path of mechanical arm

Also Published As

Publication number Publication date
CN106289244B (en) 2019-11-26

Similar Documents

Publication Publication Date Title
CN106289244A (en) A kind of attitude determination method of anti-acceleration noise
CN110398257B (en) GPS-assisted SINS system quick-acting base initial alignment method
RU2662460C1 (en) Method of upgrading angular position of agricultural machine based on the nine-axis mems sensor
CN107588769B (en) Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method
US7355549B2 (en) Apparatus and method for carrier phase-based relative positioning
CN110553646B (en) Pedestrian navigation method based on inertia, magnetic heading and zero-speed correction
KR100898169B1 (en) Initial alignment method of inertial navigation system
US9037411B2 (en) Systems and methods for landmark selection for navigation
JP4876204B2 (en) Small attitude sensor
Fang et al. An accurate gravity compensation method for high-precision airborne POS
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN110926460A (en) Uwb positioning abnormal value processing method based on IMU
WO2016203744A1 (en) Positioning device
CN101122637A (en) SINS/GPS combined navigation self-adaptive reduced-dimensions filtering method for SAR movement compensation
CN105606093B (en) Inertial navigation method and device based on gravity real-Time Compensation
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
Meiling et al. A loosely coupled MEMS-SINS/GNSS integrated system for land vehicle navigation in urban areas
CN113340298A (en) Inertial navigation and dual-antenna GNSS external reference calibration method
CN104819717B (en) A kind of multi-rotor aerocraft attitude detecting method based on MEMS inertial sensor group
CN111307114B (en) Water surface ship horizontal attitude measurement method based on motion reference unit
CN107179084B (en) Pseudo-range and tabulation combined navigation and drift estimation method for GNSS compatible machine
JP2014240266A (en) Sensor drift amount estimation device and program
CN110007318B (en) Method for judging GPS deception by single unmanned aerial vehicle based on Kalman filtering under wind field interference
RU2548115C1 (en) Platform-free navigation complex with inertial orientation system built around coarse sensors and method of correction of its inertial transducers
KR101257935B1 (en) Device for alignment of inertial navigation system using bias and navigation system thereof

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