CN108168545B - Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method - Google Patents

Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method Download PDF

Info

Publication number
CN108168545B
CN108168545B CN201711373938.8A CN201711373938A CN108168545B CN 108168545 B CN108168545 B CN 108168545B CN 201711373938 A CN201711373938 A CN 201711373938A CN 108168545 B CN108168545 B CN 108168545B
Authority
CN
China
Prior art keywords
quasi
compensation
compensation coefficient
cone
rotation vector
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
Application number
CN201711373938.8A
Other languages
Chinese (zh)
Other versions
CN108168545A (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.)
Northeast Electric Power University
Original Assignee
Northeast Dianli University
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 Northeast Dianli University filed Critical Northeast Dianli University
Priority to CN201711373938.8A priority Critical patent/CN108168545B/en
Publication of CN108168545A publication Critical patent/CN108168545A/en
Application granted granted Critical
Publication of CN108168545B publication Critical patent/CN108168545B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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 cone error compensation algorithm for optimizing compensation coefficients by a quasi-Newton method, which comprises the following steps of: the output information of the previous period is utilized, a rotation vector method is adopted to carry out multiple sampling of the output angular rate of the gyroscope, and the rotation vector is obtained; performing secondary compensation on the periodic term; solving an error compensation coefficient by adopting an optimized quasi-Newton method; and completing the attitude calculation and updating of the strapdown inertial navigation. The invention introduces a quasi-Newton method in optimization for solving the compensation coefficient, and the residual error precision is more accurate than that of a Taylor expansion method; by improving the algorithm and the calculation tool, a general cone error compensation coefficient solution is deduced, the precision of the traditional algorithm with higher sampling number can be achieved under the condition of lower sampling number, and the calculation load of navigation calculation is reduced. The user can modify the number of the sub-samples by self to meet the requirements of the carrier under different dynamic conditions, and the accuracy of navigation calculation is greatly improved.

Description

Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method
Technical Field
The invention relates to a cone error compensation algorithm for optimizing compensation coefficients by a quasi-Newton method, belonging to the field of inertial navigation.
Background
The strapdown inertial navigation is a calculation type navigation on the principle, and the gyroscope and the accelerometer are directly and fixedly connected on the carrier, so that the inertial navigation system has the advantages of small volume, light weight, low cost and simple and convenient maintenance. However, with the rapid development of MEMS, the cost of the gyroscope and the accelerometer is reduced due to the miniaturization, so that the measurement accuracy may not meet the requirement, and the measurement error on the inertial device is not easy to be accurate, which requires some innovation and improvement in the algorithm.
The invention is improved on the basis of the traditional rotation vector method, the sampling mode selects the previous period angle increment mode and simultaneously adds the compensation to the period item, the advantages of the two modes are fused, the calculation precision and the speed are simultaneously improved, the compensation coefficient is solved by the quasi-Newton method in the optimization theory, and the problem of low calculation precision caused by the limitation of residual error when the compensation coefficient is solved by the Taylor expansion method is avoided. It is noted that the quasi-Newton method can automatically set the error precision according to the actual requirement, so that the navigation resolving precision obtained by the method is greatly improved, and the system performance is optimized.
Disclosure of Invention
In order to solve the problems, the invention provides a cone error compensation algorithm for optimizing a compensation coefficient by a quasi-Newton method.
In order to achieve the purpose, the invention adopts the technical scheme that:
a cone error compensation algorithm for optimizing compensation coefficients by a quasi-Newton method optimizes periodic terms and non-periodic terms of cone errors simultaneously in a sampling mode of previous period angle increment, and solves the compensation coefficients by the quasi-Newton method; the method comprises the following steps:
s1, using the output information of the previous period, adopting a rotation vector method to perform multiple sampling of the output angular rate of the gyroscope, so that the information utilization rate is improved, the updating rate is accelerated, and the rotation vector is obtained;
s2, performing secondary compensation on the period term;
s3, solving an error compensation coefficient by adopting an optimized quasi-Newton method;
s4, integrating the step S2 and the step S3 to complete attitude calculation updating of strapdown inertial navigation, specifically: the method comprises the steps of obtaining a compensation coefficient of a periodic item by singly carrying out secondary solution on the periodic item on the premise of adopting a previous periodic angle increment, replacing a traditional Taylor expansion method with a quasi-Newton method on a solution method, and finally converting a compensated rotation vector into an updated quaternion to update the posture.
The expression of the rotation vector Φ obtained in step S1 is:
Figure BSA0000155862920000021
where θ is the current cycle angle increment, θiIs the angular increment of the ith sample in a cycle, theta1、θ2For the angular increment of the first two cycles, P, Q is the compensation factor.
The step S2 specifically includes the following steps:
as the worst environment for simulating angular motion of a carrier, the classical conic motion is usually represented by the following vector:
u(t)＝[0 acoswt asinwt]T；
wherein, w is the angular frequency of cone motion, a is the half cone angle of cone motion;
the angular velocity vector describing the conical motion is expressed as follows:
Figure BSA0000155862920000022
at time interval [ t, t + h]In the method, the Bortz equation is integrated and approximated to obtain the ideal value of the rotation vector phi
Figure BSA0000155862920000023
Figure BSA0000155862920000024
Wherein the content of the first and second substances,
Figure BSA0000155862920000025
under the environment of the classical conical motion,
Figure BSA0000155862920000026
expressed as:
Figure BSA0000155862920000027
wherein the content of the first and second substances,
Figure BSA0000155862920000028
in order to rotate the vector of the vector,
Figure BSA0000155862920000029
the components of the three-dimensional object are respectively on the x axis, the y axis and the z axis, w is the cone motion frequency, t is time, h is the posture updating period, and a is the half cone angle of the cone motion;
according to the rotation vector Φ collected in step S1, the compensation for the y-axis and z-axis directions is added to the compensation coefficient:
Figure BSA00001558629200000210
Figure BSA0000155862920000031
wherein K, P, Q are the compensation coefficients to be solved for the three-dimensional object after the second optimization.
In the scheme, after the sampling mode and the compensation item are integrated and improved, the solution of the compensation coefficient is optimized by using a quasi-Newton method BFGS, a general cone error compensation coefficient solution is deduced, the precision of a traditional algorithm with a high sampling number can be achieved under the condition of a low sampling number, and the calculation load of navigation calculation is reduced.
Drawings
Fig. 1 is a schematic diagram of a conventional sampling manner.
Fig. 2 is a schematic diagram of a sampling method using a previous cycle angle increment according to an embodiment of the present invention.
FIG. 3 is a block diagram of an attitude resolution system in an embodiment of the invention.
Detailed Description
In order that the objects and advantages of the invention will be more clearly understood, the invention is further described in detail below with reference to examples. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The rotation of the rigid body has non-commutative property, a rotation vector method is usually adopted when the attitude is solved, the non-commutative error is effectively compensated by utilizing multiple sampling of the rotation vector method, and the traditional sampling mode is shown in figure 1, namely, after a conical error compensation period h is completedkThen, the next compensation can be carried out only by waiting for N sampling periods, and in the sampling method of the previous period after the second optimization, three axes only need to wait for one sampling period delta theta as shown in figure 2n+1I.e. with Δ θ2、Δθ3....ΔθnForming new N sub-samples to complete the compensation period hk+1。
As the worst environment for simulating angular motion of a carrier, the classical conic motion is usually represented by the following vector:
u(t)＝[0 acoswt asinwt]T
wherein w is the cone motion angular frequency and a is the cone motion half cone angle. The angular velocity vector describing the conical motion is expressed as follows:
Figure BSA0000155862920000032
at time interval [ t, t + h]In the method, the Bortz equation is integrated and approximated to obtain the ideal value of the rotation vector phi
Figure BSA0000155862920000033
Figure BSA0000155862920000034
Wherein the content of the first and second substances,
Figure BSA0000155862920000041
under the environment of the classical conical motion,
Figure BSA0000155862920000042
expressed as:
Figure BSA0000155862920000043
wherein the content of the first and second substances,
Figure BSA0000155862920000044
in order to rotate the vector of the vector,
Figure BSA0000155862920000045
the components of the three-dimensional object are respectively on the x axis, the y axis and the z axis, w is the cone motion frequency, t is time, h is the posture updating period, and a is the half cone angle of the cone motion;
the expression of the rotation vector phi obtained by the sampling mode in the step 1 is as follows:
Figure BSA0000155862920000046
where θ is the current cycle angle increment, θiIs the angular increment of the ith sample in a cycle, theta1、θ2For the angular increment of the first two cycles, P, Q is the compensation factor.
Figure BSA0000155862920000047
And the compensation in the y-axis direction and the z-axis direction is added to the compensation coefficient at the same time, wherein K, P, Q is the compensation coefficient to be solved after the secondary optimization.
The error criterion is defined as
Figure BSA0000155862920000048
In order to minimize the error, the traditional method adopts a Taylor expansion method, the expansion term is expanded to the next term of the unknown coefficient number, and the remaining term is the residue difference, the invention adopts a quasi-Newton method in optimization, so that the residual error is not limited to a compensation systemThe number of the compensation coefficients can be obtained by automatically setting the precision of the algorithm according to actual requirements.
As shown in fig. 3, the calculated compensation coefficient is applied to the system attitude update loop after the subsample number is determined, and the loop iteration number is determined according to the system running time.
The foregoing is only a preferred embodiment of the present invention, and it should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be construed as the protection scope of the present invention.

Claims (1)

1. A cone error compensation algorithm for optimizing a compensation coefficient by a quasi-Newton method is characterized in that a periodic term and a non-periodic term of a cone error are optimized simultaneously in a sampling mode of a previous period angle increment, and the compensation coefficient is solved by the quasi-Newton method; the method comprises the following steps:
s1, using the output information of the previous period, adopting a rotation vector method to perform multiple sampling of the output angular rate of the gyroscope, and acquiring a rotation vector; rotation vector
Figure 748242DEST_PATH_IMAGE001
The expression is as follows:
Figure 399803DEST_PATH_IMAGE002
Figure 22283DEST_PATH_IMAGE003
s2, performing secondary compensation on the period term; the method specifically comprises the following steps:
as the worst environment for simulating the angular motion of the carrier, the classical conic motion is represented by the following vector:
Figure 151913DEST_PATH_IMAGE004
；
wherein the content of the first and second substances,
Figure 435127DEST_PATH_IMAGE005
is the angular frequency of the cone motion and,
Figure 585485DEST_PATH_IMAGE006
is a cone motion half-cone angle;
the angular velocity vector describing the conical motion is expressed as follows:
Figure 586939DEST_PATH_IMAGE007
；
at time intervals
Figure 130047DEST_PATH_IMAGE008
In the method, the Bortz equation is integrated and approximated to obtain a rotation vector
Figure 533347DEST_PATH_IMAGE009
Ideal value of
Figure 120186DEST_PATH_IMAGE010
：
Figure 608936DEST_PATH_IMAGE011
；
Wherein the content of the first and second substances,
Figure 80369DEST_PATH_IMAGE012
；
under the environment of the classical conical motion,
Figure 446497DEST_PATH_IMAGE013
expressed as:
Figure 610762DEST_PATH_IMAGE014
；
Figure 321229DEST_PATH_IMAGE015
according to the rotation vector collected in step S1
Figure 455407DEST_PATH_IMAGE016
And the compensation in the directions of the y axis and the z axis is added to the compensation coefficient at the same time:
Figure 567720DEST_PATH_IMAGE017
Figure 637307DEST_PATH_IMAGE018
；
k, P, Q is a compensation coefficient to be solved after the secondary optimization;
s3, solving an error compensation coefficient by adopting an optimized quasi-Newton method;
s4, integrating the step S2 and the step S3 to complete attitude calculation updating of strapdown inertial navigation, specifically, performing secondary calculation on a periodic item independently on the premise of adopting a previous periodic angle increment to obtain a compensation coefficient of the periodic item, replacing a traditional Taylor expansion method with a quasi-Newton method on the basis of a calculation method, and finally converting a compensated rotation vector into an updated quaternion to update the attitude.
CN201711373938.8A 2017-12-04 2017-12-04 Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method Active CN108168545B (en)

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 CN108168545A (en) 2018-06-15
CN108168545B true 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)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
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 (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101871775B (en) * 2009-04-21 2012-09-19 鸿富锦精密工业（深圳）有限公司 Programming system of three coordinate measuring machine and method thereof
US8125376B1 (en) * 2010-08-30 2012-02-28 Javad Gnss, Inc. Handheld global positioning system device
US10437658B2 (en) * 2013-06-06 2019-10-08 Zebra Technologies Corporation Method, apparatus, and computer program product for collecting and displaying sporting event data based on real time data for proximity and movement of objects

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
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 (2)

* Cited by examiner, † Cited by third party
Title
Accurate Compensation of Attitude Angle Error in a Dual-Axis Rotation Inertial Navigation System;Rui Jiang 等;《Sensors》;20170317;第17卷(第3期);第1-17页 *
装配位姿视觉检测系统中红外靶点质心定位;杨博文 等;《中国机械工程》;20140430;第25卷(第7期);第965-971页 *

Also Published As

Publication number Publication date
CN108168545A (en) 2018-06-15

Similar Documents

Publication Publication Date Title
CN109696183B (en) Calibration method and device of inertia measurement unit
CN108168545B (en) Cone error compensation algorithm for optimizing compensation coefficient by quasi-Newton method
CN110030994B (en) Monocular-based robust visual inertia tight coupling positioning method
CN108225370B (en) Data fusion and calculation method of motion attitude sensor
CN109682377B (en) Attitude estimation method based on dynamic step gradient descent
CN110702143B (en) Rapid initial alignment method for SINS strapdown inertial navigation system moving base based on lie group description
CN107063254B (en) Gesture resolving method for gyros and geomagnetic combination
CN110851776B (en) Attitude calculation method for high-dynamic variable-speed carrier
CN108344413B (en) Underwater glider navigation system and low-precision and high-precision conversion method thereof
CN109724602A (en) A kind of attitude algorithm system and its calculation method based on hardware FPU
CN112665574A (en) Underwater robot attitude acquisition method based on momentum gradient descent method
Hoang et al. Pre-processing technique for compass-less madgwick in heading estimation for industry 4.0
CN110986934A (en) Navigation method and system of integrated double-shaft rotation inertial navigation astronomical integrated navigation system
CN111207773A (en) Attitude unconstrained optimization solving method for bionic polarized light navigation
CN112665570B (en) MEMS gyroscope zero-bias on-orbit simplified engineering calculation method based on star sensor
Guan et al. Sensor fusion of gyroscope and accelerometer for low-cost attitude determination system
CN104048658B (en) A kind of method that use reduces data rate and power consumption based on the equipment that posture generates
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
Jiang et al. An improved attitude compensation algorithm in high dynamic environment
CN112857366B (en) Optical fiber strapdown inertial navigation system attitude calculation method based on compression structure
CN108871312B (en) Combined attitude determination method for gravity gradiometer and star sensor
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN108154001B (en) Dynamic modeling method and system for space film diffraction imaging satellite
CN111207734A (en) EKF-based unmanned aerial vehicle integrated navigation method
CN110455288A (en) A kind of posture renewal method based on angular speed high-order moment

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