CN108168545A - A kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient - Google Patents
A kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient Download PDFInfo
- Publication number
- CN108168545A CN108168545A CN201711373938.8A CN201711373938A CN108168545A CN 108168545 A CN108168545 A CN 108168545A CN 201711373938 A CN201711373938 A CN 201711373938A CN 108168545 A CN108168545 A CN 108168545A
- Authority
- CN
- China
- Prior art keywords
- compensation
- quasi
- coefficient
- newton method
- optimization
- 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
- 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/20—Instruments for performing navigational calculations
Abstract
The invention discloses a kind of Compensation for Coning Error algorithms of quasi-Newton method Optimization Compensation coefficient, include the following steps：Using the output information of previous cycle, the multiple repairing weld of gyro output angle rate is carried out using rotating vector law, carries out the acquisition of rotating vector；Second compensation is carried out to periodic term；Error compensation coefficient is solved using the quasi-Newton method of optimization；Complete the attitude algorithm update of inertial navigation.Present invention introduces quasi-Newton methods in optimization for solving penalty coefficient, and residual error precision is more accurate compared with Taylor expansion；By to the improvement on algorithm and resolving tool, deriving general Compensation for Coning Error coefficient solution, the precision of the higher hits of traditional algorithm can be reached in the case of relatively low hits, reduce the calculated load of navigation calculation.User can voluntarily change sample number to meet the needs of under carrier Different Dynamic, it is ensured that the precision of navigation calculation is substantially improved.
Description
Technical field
The present invention relates to a kind of Compensation for Coning Error algorithms of quasi-Newton method Optimization Compensation coefficient, belong to inertial navigation field.
Background technology
It is the navigation of reckoning formula in strapdown inertial navigation principle, gyro and accelerometer are directly connected on carrier, are had
There is small, light-weight, at low cost, easy maintenance advantage.But with the fast development of MEMS, make gyro and accelerometer
Towards miniaturization, cost also declines therewith, this, which allows for measurement accuracy, not to reach requirement, the measurement on inertia device
Error is not easy again accurate, it is necessary to algorithmically be innovated and improved.
The present invention is improved on the basis of traditional rotating vector law, and what sample mode was selected is previous cycle angle increment
Form adds in the compensation to periodic term simultaneously, by the blend of predominance of the two so that is carried simultaneously on calculation accuracy and speed
It rises, and is solved on penalty coefficient is solved with the quasi-Newton method in Optimum Theory, avoided being solved with Taylor expansion and mend
The problem of causing computational accuracy not high due to the limitation of residual error when repaying coefficient.It is noted that quasi-Newton method can be according to reality
Demand voluntarily step-up error precision, it is ensured that the navigation calculation precision obtained with this is substantially improved, and system performance is optimized.
Invention content
To solve the above problems, a kind of Compensation for Coning Error calculation the present invention provides quasi-Newton method Optimization Compensation coefficient
Method.
To achieve the above object, the technical solution taken of the present invention is：
A kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient, using the sampling side of previous cycle angle increment
Optimization is carried out at the same time, and penalty coefficient is asked using quasi-Newton method to the periodic term of coning error and aperiodic item under formula
Solution；Include the following steps：
S1, the output information using previous cycle carry out the multiple repairing weld of gyro output angle rate using rotating vector law,
So that information utilization improves and accelerates renewal rate, the acquisition of rotating vector is carried out；
S2, second compensation is carried out to periodic term；
S3, error compensation coefficient is solved using the quasi-Newton method of optimization；
S4, step S2 and step S3 is integrated to the attitude algorithm update for completing inertial navigation, specially：Traditional algorithm compensates
Coefficient has ignored the influence of periodic term, show that the result of penalty coefficient acts on periodic term simultaneously after being resolved to aperiodic item
On, the present invention obtains the period using secondary resolving is individually carried out to periodic term under the premise of using previous cycle angle increment
The penalty coefficient of item replaces traditional Taylor expansion, the rotating vector after most compensating at last on calculation method with quasi-Newton method
Update quaternary number is converted to be updated posture.
The obtained rotating vector Φ expression formulas of step S1 are：
Wherein, θ be current period angle increment, θiFor the angle increment that ith in a cycle samples, θ1、θ2For the first two week
The angle increment of phase, P, Q are penalty coefficient.
The step S2 specifically comprises the following steps：
As the worst environment of analog carrier angular movement, classical conical motion is usually with following vector representation：
U (t)=[0 acoswt asinwt]T；
Wherein, w is coning motion angular frequency, and a is coning motion semi-cone angle；
The angular velocity vector of description conical motion is expressed as below：
In time interval [t, t+h], Bortz equations are integrated and take approximation, the ideal of rotating vector Φ can be obtained
Value
Wherein,
Under classical conical motion environment,It is expressed as：
Wherein,For rotating vector,Respectively its component on three axis of x, y, z, w are transported for circular cone
Dynamic frequency, t are the time, and h is the posture renewal period, and a is the semi-cone angle of conical motion；
According to the collected rotating vector Φ of step S1 institute, added simultaneously on penalty coefficient to y-axis and z-axis direction
Compensation：
Wherein, K, P, Q are the three-dimensional penalty coefficients to be solved after double optimization.
In said program, after integrated improve is carried out to sample mode and compensation term, using quasi-Newton method BFGS to compensation
Coefficient optimizes on solving, and derives general Compensation for Coning Error coefficient solution, can reach in the case of relatively low hits
The precision of the higher hits of traditional algorithm reduces the calculated load of navigation calculation.
Description of the drawings
Fig. 1 is traditional sampling schematic diagram.
Fig. 2 is the used sample mode schematic diagram using previous cycle angle increment of the embodiment of the present invention.
Fig. 3 is the attitude algorithm system block diagram in the embodiment of the present invention.
Specific embodiment
In order to which objects and advantages of the present invention are more clearly understood, the present invention is carried out with reference to embodiments further
It is described in detail.It should be appreciated that the specific embodiments described herein are merely illustrative of the present invention, it is not used to limit this hair
It is bright.
The rotation of rigid body has noncommutativity, is typically using rotating vector law, using it repeatedly when resolving posture
Sampling, which is realized, makees noncommutativity error effective compensation, traditional sampling mode such as Fig. 1, is to complete the Compensation for Coning Error period
hkAfterwards, the compensation for waiting for N number of sampling period that could carry out next step is needed, the present invention such as Fig. 2, the previous cycle after double optimization
Sampling method, three axis all only need to wait for a sampling period Δ θn+1, you can with Δ θ2、Δθ3....ΔθnNew N increments are formed,
Complete compensation cycle hk+1。
As the worst environment of analog carrier angular movement, classical conical motion is usually with following vector representation：
U (t)=[0 acoswt asinwt]T
Wherein w is coning motion angular frequency, and a is coning motion semi-cone angle.The angular velocity vector of description conical motion is expressed as below：
In time interval [t, t+h], Bortz equations are integrated and take approximation, the ideal of rotating vector Φ can be obtained
Value
Wherein,
Under classical conical motion environment,It is expressed as：
Wherein,For rotating vector,Respectively its component on three axis of x, y, z, w are transported for circular cone
Dynamic frequency, t are the time, and h is the posture renewal period, and a is the semi-cone angle of conical motion；
The rotating vector Φ expression formulas obtained by step 1 sample mode are：
Wherein θ be current period angle increment, θiFor the angle increment that ith in a cycle samples, θ1、θ2For the first two week
The angle increment of phase, P, Q are penalty coefficient.
Add the compensation to y-axis and z-axis direction simultaneously on penalty coefficient, wherein K, P, Q is to carry out double optimization
Three-dimensional penalty coefficient to be solved afterwards.
Error criterion is defined asTo make error minimum conventional method using Taylor expansion, expansion item expansion
To the next item down of unknowm coefficient number, that left is poor for residue, and the present invention is using quasi-Newton method in optimizing so that
In the number for being confined to penalty coefficient, compensation system can not be obtained in residual error according to the precision of actual demand sets itself algorithm
Several numerical value.
The penalty coefficient acquired is applied in posture update cycle after sample number is determined as shown in Figure 3, cycle
Iterations are determined according to system operation time.
The above is only the preferred embodiment of the present invention, it is noted that for the ordinary skill people of the art
For member, without departing from the principle of the present invention, several improvements and modifications can also be made, these improvements and modifications also should
It is considered as protection scope of the present invention.
Claims (3)
1. a kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient, which is characterized in that increased using previous cycle angle
Optimization is carried out at the same time under the sample mode of amount to the periodic term of coning error and aperiodic item, and is to compensation using quasi-Newton method
Number is solved；Include the following steps：
S1, the output information using previous cycle carry out the multiple repairing weld of gyro output angle rate using rotating vector law, carry out
The acquisition of rotating vector；
S2, second compensation is carried out to periodic term；
S3, error compensation coefficient is solved using the quasi-Newton method of optimization；
S4, step S2 and step S3 is integrated to the attitude algorithm update for completing inertial navigation, specifically, using before use
The secondary penalty coefficient for resolving and obtaining periodic term is individually carried out under the premise of one period angle increment to periodic term, on calculation method
Traditional Taylor expansion is replaced with quasi-Newton method, the rotating vector after most compensating at last is converted to update quaternary number and posture is carried out
Update.
2. a kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient as described in claim 1, which is characterized in that
The obtained rotating vector Φ expression formulas of step S1 are：
Wherein, θ be current period angle increment, θiFor the angle increment that ith in a cycle samples, θ1、θ2For the first two period
Angle increment, P, Q are penalty coefficient.
3. a kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient as described in claim 1, which is characterized in that
The step S2 specifically comprises the following steps：
As the worst environment of analog carrier angular movement, classical conical motion is usually with following vector representation：
U (t)=[0 acoswt asinwt]T；
Wherein, w is coning motion angular frequency, and a is coning motion semi-cone angle；
The angular velocity vector of description conical motion is expressed as below：
In time interval [t, t+h], Bortz equations are integrated and take approximation, the ideal value of rotating vector Φ can be obtained
Wherein,
Under classical conical motion environment,It is expressed as：
Wherein,For rotating vector,Respectively its component on three axis of x, y, z, w are conical motion frequency
Rate, t are the time, and h is the posture renewal period, and a is the semi-cone angle of conical motion；
According to the collected rotating vector Φ of step S1 institutes, the benefit to y-axis and z-axis direction is added simultaneously on penalty coefficient
It repays：
Wherein, K, P, Q are the three-dimensional penalty coefficients to be solved after double optimization.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711373938.8A CN108168545B (en) | 2017-12-04 | 2017-12-04 | Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711373938.8A CN108168545B (en) | 2017-12-04 | 2017-12-04 | Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108168545A true CN108168545A (en) | 2018-06-15 |
| CN108168545B CN108168545B (en) | 2021-03-23 |
Family
ID=62522861
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711373938.8A Active CN108168545B (en) | 2017-12-04 | 2017-12-04 | Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108168545B (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110455288A (en) * | 2019-08-06 | 2019-11-15 | 东南大学 | A kind of posture renewal method based on angular speed high-order moment |
| CN112033437A (en) * | 2020-08-07 | 2020-12-04 | 贵州航天控制技术有限公司 | Equivalent rotation vector algorithm-based strapdown inertial navigation system optimization method |
| CN115752512A (en) * | 2022-11-22 | 2023-03-07 | 哈尔滨工程大学 | Inertial base combined navigation three-axis non-coincident angle calibration method and system |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101871775A (en) * | 2009-04-21 | 2010-10-27 | 鸿富锦精密工业（深圳）有限公司 | Programming system of three coordinate measuring machine and method thereof |
| US20120188122A1 (en) * | 2010-08-30 | 2012-07-26 | Javad Gnss, Inc. | Handheld global positioning system device |
| CN103135094A (en) * | 2013-01-31 | 2013-06-05 | 西安电子科技大学 | Signal source positioning method based on BFGS quasi-Newton method |
| CN103630135A (en) * | 2013-11-29 | 2014-03-12 | 东南大学 | Angular rate input attitude algorithm structure and parameter optimization method |
| CN103712623A (en) * | 2014-01-20 | 2014-04-09 | 东南大学 | Optical-fiber gyroscope inertial navigation system attitude optimization method based on angular rate input |
| CN103759731A (en) * | 2014-01-16 | 2014-04-30 | 电子科技大学 | Single-subsample rotating vector posture method under angular rate input condition |
| US20150149837A1 (en) * | 2013-06-06 | 2015-05-28 | Zih Corp. | Method, apparatus, and computer program product for collecting and displaying sporting event data based on real time data for proximity and movement of objects |
-
2017
- 2017-12-04 CN CN201711373938.8A patent/CN108168545B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101871775A (en) * | 2009-04-21 | 2010-10-27 | 鸿富锦精密工业（深圳）有限公司 | Programming system of three coordinate measuring machine and method thereof |
| US20120188122A1 (en) * | 2010-08-30 | 2012-07-26 | Javad Gnss, Inc. | Handheld global positioning system device |
| CN103135094A (en) * | 2013-01-31 | 2013-06-05 | 西安电子科技大学 | Signal source positioning method based on BFGS quasi-Newton method |
| US20150149837A1 (en) * | 2013-06-06 | 2015-05-28 | Zih Corp. | Method, apparatus, and computer program product for collecting and displaying sporting event data based on real time data for proximity and movement of objects |
| CN103630135A (en) * | 2013-11-29 | 2014-03-12 | 东南大学 | Angular rate input attitude algorithm structure and parameter optimization method |
| CN103759731A (en) * | 2014-01-16 | 2014-04-30 | 电子科技大学 | Single-subsample rotating vector posture method under angular rate input condition |
| CN103712623A (en) * | 2014-01-20 | 2014-04-09 | 东南大学 | Optical-fiber gyroscope inertial navigation system attitude optimization method based on angular rate input |
Non-Patent Citations (3)
| Title |
|---|
| RUI JIANG 等: "Accurate Compensation of Attitude Angle Error in a Dual-Axis Rotation Inertial Navigation System", 《SENSORS》 * |
| 刘伟: "高精度激光陀螺捷联惯导系统误差模型参数估计方法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 杨博文 等: "装配位姿视觉检测系统中红外靶点质心定位", 《中国机械工程》 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110455288A (en) * | 2019-08-06 | 2019-11-15 | 东南大学 | A kind of posture renewal method based on angular speed high-order moment |
| CN112033437A (en) * | 2020-08-07 | 2020-12-04 | 贵州航天控制技术有限公司 | Equivalent rotation vector algorithm-based strapdown inertial navigation system optimization method |
| CN115752512A (en) * | 2022-11-22 | 2023-03-07 | 哈尔滨工程大学 | Inertial base combined navigation three-axis non-coincident angle calibration method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108168545B (en) | 2021-03-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107690567B (en) | The method for being used to be tracked the navigation of mobile vehicle equipment using extended Kalman filter | |
| CN107618678B (en) | Attitude control information joint estimation method under satellite attitude angle deviation | |
| CN110376882A (en) | Pre-determined characteristics control method based on finite time extended state observer | |
| Jin et al. | The adaptive Kalman filter based on fuzzy logic for inertial motion capture system | |
| CN108225370B (en) | Data fusion and calculation method of motion attitude sensor | |
| CN110702143B (en) | Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description | |
| CN108762285B (en) | Target attitude collaborative planning method and system for multi-stage composite control of spacecraft | |
| CN108168545A (en) | A kind of Compensation for Coning Error algorithm of quasi-Newton method Optimization Compensation coefficient | |
| CN102224395B (en) | Calibration of gyroscopic systems with vibratory gyroscopes | |
| CN110285815B (en) | Micro-nano satellite multi-source information attitude determination method capable of being applied in whole orbit | |
| CN110851776B (en) | Attitude calculation method for high-dynamic variable-speed carrier | |
| CN103878770A (en) | Space robot visual delay error compensation method based on speed estimation | |
| CN107339987B (en) | Rigid body attitude calculation method based on function iteration integral | |
| CN103759731B (en) | Angular speed initial conditions places an order increment Research on Rotation Vector Attitude method | |
| CN108663936A (en) | Model does not know spacecraft without unwinding Attitude Tracking finite-time control method | |
| CN115202380B (en) | Extraterrestrial celestial body landing track planning method based on piecewise polynomial | |
| Bai et al. | A novel plug-and-play factor graph method for asynchronous absolute/relative measurements fusion in multisensor positioning | |
| CN105136150A (en) | Attitude determination method based on multiple star-sensor measure information fusion | |
| CN110873577B (en) | Underwater rapid-acting base alignment method and device | |
| CN110455288A (en) | A kind of posture renewal method based on angular speed high-order moment | |
| Zhe et al. | Adaptive complementary filtering algorithm for imu based on mems | |
| Xing et al. | Offline calibration for MEMS gyroscope g-sensitivity error coefficients based on the Newton iteration and least square methods | |
| CN104567873B (en) | High precision SINS attitude angle for high dynamic carrier application determines method | |
| CN106292678B (en) | A kind of robot for space pedestal decoupling control method for object run | |
| Zhang et al. | Optimal two-iteration sculling compensation mathematical framework for SINS velocity updating |
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 |