CN113790720A - Disturbance-rejection coarse alignment method based on recursive least squares - Google Patents

Disturbance-rejection coarse alignment method based on recursive least squares Download PDF

Info

Publication number
CN113790720A
CN113790720A CN202110936292.XA CN202110936292A CN113790720A CN 113790720 A CN113790720 A CN 113790720A CN 202110936292 A CN202110936292 A CN 202110936292A CN 113790720 A CN113790720 A CN 113790720A
Authority
CN
China
Prior art keywords
coarse alignment
recursive
vector
disturbance
squares
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
CN202110936292.XA
Other languages
Chinese (zh)
Other versions
CN113790720B (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.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
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 Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN202110936292.XA priority Critical patent/CN113790720B/en
Publication of CN113790720A publication Critical patent/CN113790720A/en
Application granted granted Critical
Publication of CN113790720B publication Critical patent/CN113790720B/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
    • 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

Abstract

The invention discloses an anti-disturbance coarse alignment method based on recursive least squares, which is characterized in that a velocity integral vector in the whole coarse alignment process is subjected to quadratic fitting by using the recursive least squares method, and the velocity integral vectors of two time points are extrapolated by using parameters obtained by fitting, so that errors brought by disturbance line motion to coarse alignment are reduced. The method has clear principle and simple and quick calculation, can adapt to dynamic conditions of vertical launching wind disturbance, water surface waves, vehicle running and the like, has wide application prospect, and can obtain good social and economic benefits in the military and civil fields.

Description

Disturbance-rejection coarse alignment method based on recursive least squares
Technical Field
The invention belongs to the field of inertial navigation, and relates to an anti-disturbance coarse alignment method.
Background
The inertial navigation system rough alignment is to find and determine the rotation relation (attitude rotation matrix, quaternion or Euler angle) of an inertial navigation body coordinate system (an accelerometer front upper right coordinate system) relative to a navigation coordinate system (a geographic coordinate system, usually a north heaven and east coordinate system) through an inertial device sensitive earth gravity vector and an earth rotation angular velocity vector.
The conventional disturbance-resistant coarse alignment method can only adapt to a disturbance motion state with only angular motion, such as shaking caused by engine work or people getting on or off a carrier. When the carrier has disturbances such as waves on the water surface, which can cause line motion, large coarse alignment errors can be introduced.
Disclosure of Invention
The invention provides an anti-disturbance coarse alignment method based on recursive least squares, which reduces the influence of linear motion on coarse alignment.
The invention discloses an anti-disturbance coarse alignment method based on recursive least squares, which comprises the following technical scheme:
and performing quadratic fitting on the velocity integral vectors in the whole coarse alignment process by using a recursive least square method, and extrapolating the velocity integral vectors of two time points by using parameters obtained by fitting so as to reduce the influence of linear motion on the coarse alignment.
Further, the disturbance rejection coarse alignment method specifically includes:
inertial body coordinate system i for determining inertial navigation at moment when inertial navigation system receives alignment instructionb0System and earth inertial frame ieIs a step of; aligning a b system of an inertial navigation body coordinate system at the moment, namely an inertial coordinate system obtained by solidifying an upper front right coordinate system of the accelerometer; an earth coordinate system e at the alignment moment is an inertial coordinate system obtained by solidification, the origin of the earth coordinate system e is at the earth center, and Z iseAxial north pole, XeThe axis being in the equatorial plane in the local longitudinal direction, YeAxis and ZeAxis, XeThe shafts are arranged in a right-hand arrangement;
in each navigation period, the gravity acceleration at i is respectively calculated by the following formulab0A series of and ieProjection under system:
Figure BDA0003212984440000021
in the formula
Figure BDA0003212984440000022
Is represented as a series from b to ib0An attitude rotation matrix of the system; f. ofbAn acceleration vector sensed by the accelerometer; g is the acceleration of gravity; v is the speed of the carrier,
Figure BDA0003212984440000023
for the velocity differential term, V is obtained from the reference information, and V is,
Figure BDA0003212984440000024
Taking the value as a 0 vector; omega is the angular velocity of rotation of the earth,
Figure BDA0003212984440000025
the local latitude of the vector;
then to ib0And ieIntegrating gravity acceleration vectors under a coordinate system:
Figure BDA0003212984440000026
in the formula TnavIndicating the navigation period, t the length of time after entering coarse alignment,
Figure BDA0003212984440000027
indicating the acceleration of gravity at the k beat after the coarse alignment command is in ib0Is the projection
Figure BDA0003212984440000028
The integral vector of (1);
in gib0During integration, V is calculated for each navigation periodt ib0And (3) carrying out recursive quadratic fitting:
Figure BDA0003212984440000029
in the formula: xkFor coefficients of recursive least squares quadratic fit, PkIs XkCorresponding covariance matrix, ZkTo observe the vector, Hk+1Is an observation matrix.
When the coarse alignment time comes, calculating the gravity acceleration at i of two time points by the following formulab0The projected integrated vector:
Figure BDA00032129844400000210
in the formula XkFor the fitting result of the last recursive least squares, T1 and T2 are two time points in the course of coarse alignment respectively;
and finally, constructing a speed matrix by using speed integral vectors of two time points according to a common disturbance rejection coarse alignment method, and passing through ieA series of and ib0Speed of two coordinate systemsDegree matrix operation is obtained
Figure BDA0003212984440000031
Further obtain the coarse alignment result
Figure BDA0003212984440000032
Further, the calculated V for each navigation cyclet ib0In the recursive quadratic fitting, the formula (3) XkValue of 0, PkIs XkThe corresponding covariance matrix can be set according to system parameters; zkTo observe the vector, take the value of
Figure BDA0003212984440000033
Hk+1For observing the matrix, take the value [ (T)nav·k)2 Tnav·k 1]Wherein T isnavFor the navigation period, k is the coarse alignment beat number.
Further, the gravity acceleration at two time points is calculated at ib0In the projected integral vector, two time points are respectively half of the coarse alignment duration and the coarse alignment duration.
The invention has the beneficial effects that: according to the disturbance rejection rough alignment method based on the recursive least square, the recursive least square method is used for carrying out quadratic fitting on the velocity integral vectors in the whole rough alignment process, and the velocity integral vectors of two time points are extrapolated by using parameters obtained by fitting, so that errors brought by disturbance line motion to rough alignment are reduced. The method has clear principle and simple and quick calculation, can adapt to dynamic conditions of vertical launching wind disturbance, water surface waves, vehicle running and the like, has wide application prospect, and can obtain good social and economic benefits in the military and civil fields.
Detailed Description
When disturbance motion such as online motion exists in a carrier of an inertial navigation system, disturbance velocity is introduced into a velocity integral vector of a traditional disturbance-resistant coarse alignment algorithm, so that a coarse alignment result generates a large error. Aiming at the problem, the disturbance rejection coarse alignment method based on the recursive least square is invented, the recursive least square method is utilized to carry out quadratic fitting on the velocity integral vector in the whole coarse alignment process, and the velocity integral vectors of two time points are extrapolated by using the parameters obtained by fitting, so that the influence of linear motion on the coarse alignment is reduced.
The invention firstly determines an inertial body coordinate system i of inertial navigation at the moment when an inertial navigation system receives an alignment instructionb0The system (alignment moment inertial navigation body coordinate system b system, namely an inertial coordinate system obtained by solidifying the front upper right coordinate system of the accelerometer) and the earth inertial coordinate system ieThe system (an inertial coordinate system obtained by solidifying an earth coordinate system e at the alignment moment, wherein the origin of the earth coordinate system e is at the earth center, ZeAxial north pole, XeThe axis being in the equatorial plane in the local longitudinal direction, YeAxis and ZeAxis, XeThe shafts are arranged in a right-handed arrangement).
Then, in each navigation period, the gravity acceleration at i is respectively calculated by the following formulab0A series of and ieProjection under the system. In the formula
Figure BDA0003212984440000041
Is represented as a series from b to ib0An attitude rotation matrix of the system (calculated by the sensitive angular velocity of the gyroscope and consistent with the calculation mode in the normal disturbance rejection coarse alignment method); f. ofbAn acceleration vector sensed by the accelerometer; g is the acceleration of gravity; v is the speed of the carrier,
Figure BDA0003212984440000042
for the velocity differential term, V is obtained from reference information (satellite, odometer, etc.), and V, and V are determined if the vehicle is stationary,
Figure BDA0003212984440000043
Taking the value as a 0 vector; omega is the angular velocity of rotation of the earth,
Figure BDA0003212984440000044
the local latitude of the vector.
Figure BDA0003212984440000045
Then to ib0And ieIntegrating gravity acceleration vectors under a coordinate system:
Figure BDA0003212984440000046
in the formula TnavIndicating the navigation period, t the length of time after entering coarse alignment,
Figure BDA0003212984440000047
indicating the acceleration of gravity at the k beat after the coarse alignment command is in ib0Is the projection
Figure BDA0003212984440000048
The integrated vector of (2).
In gib0During integration, to reduce the effect of disturbance line motion, V is calculated for each navigation cyclet ib0And (3) carrying out recursive quadratic fitting:
Figure BDA0003212984440000049
in the formula: xkFor the coefficient of recursive least squares quadratic fit, the initial value can be taken as 0; pkIs XkThe corresponding covariance matrix can be set according to system parameters; zkFor observation vector, take value Vib0k；Hk+1For observing the matrix, take the value [ (T)nav·k)2Tnav·k 1]Wherein T isnavFor the navigation period, k is the coarse alignment beat number.
When the coarse alignment time comes, calculating the gravity acceleration at i of two time points by the following formulab0The projected integrated vector:
Figure BDA0003212984440000051
in the formulaXkFor the fitting result of the last recursive least squares, T1 and T2 are two time points in the coarse alignment process, respectively, and are set according to the system algorithm, and are usually set as the coarse alignment duration and half of the coarse alignment duration.
And finally, constructing a speed matrix by using speed integral vectors of two time points according to a common disturbance rejection coarse alignment method, and passing through ieA series of and ib0Obtained by velocity matrix operation of two coordinate systems
Figure BDA0003212984440000052
Further obtain the coarse alignment result
Figure BDA0003212984440000053
The method has clear principle and simple and convenient calculation, can adapt to dynamic conditions of vertical launching wind disturbance, water surface waves, vehicle running and the like, has wide application prospect, and can obtain good social and economic benefits in the military and civil fields.
The above embodiments are only for explaining and explaining the technical solution of the present invention, but should not be construed as limiting the scope of the claims. It should be clear to those skilled in the art that any simple modification or replacement based on the technical solution of the present invention may be adopted to obtain a new technical solution, which falls within the scope of the present invention.

Claims (4)

1. An anti-disturbance coarse alignment method based on recursive least squares is characterized in that a recursive least square method is used for carrying out quadratic fitting on velocity integral vectors in the whole coarse alignment process, and the velocity integral vectors of two time points are extrapolated by using parameters obtained by fitting, so that the influence of linear motion on coarse alignment is reduced.
2. The method according to claim 1, wherein the method specifically comprises:
inertial body coordinate for inertial navigation is established when inertial navigation system receives alignment instructionIs i ofb0System and earth inertial frame ieIs a step of; aligning a b system of an inertial navigation body coordinate system at the moment, namely an inertial coordinate system obtained by solidifying an upper front right coordinate system of the accelerometer; an earth coordinate system e at the alignment moment is an inertial coordinate system obtained by solidification, the origin of the earth coordinate system e is at the earth center, and Z iseAxial north pole, XeThe axis being in the equatorial plane in the local longitudinal direction, YeAxis and ZeAxis, XeThe shafts are arranged in a right-hand arrangement;
in each navigation period, the gravity acceleration at i is respectively calculated by the following formulab0A series of and ieProjection under system:
Figure FDA0003212984430000011
in the formula
Figure FDA0003212984430000012
Is represented as a series from b to ib0An attitude rotation matrix of the system; f. ofbAn acceleration vector sensed by the accelerometer; g is the acceleration of gravity; v is the speed of the carrier,
Figure FDA0003212984430000013
for the velocity differential term, V is obtained from the reference information, and V is,
Figure FDA0003212984430000014
Taking the value as a 0 vector; omega is the angular velocity of rotation of the earth,
Figure FDA0003212984430000015
the local latitude of the vector;
then to ib0And ieIntegrating gravity acceleration vectors under a coordinate system:
Figure FDA0003212984430000016
in the formula TnavIndicating the navigation period, t the length of time after entering coarse alignment,
Figure FDA0003212984430000017
indicating the acceleration of gravity at the k beat after the coarse alignment command is in ib0Is the projection
Figure FDA0003212984430000018
The integral vector of (1);
in gib0During integration, V is calculated for each navigation periodt ib0And (3) carrying out recursive quadratic fitting:
Figure FDA0003212984430000021
in the formula: xkFor coefficients of recursive least squares quadratic fit, PkIs XkCorresponding covariance matrix, ZkTo observe the vector, Hk+1Is an observation matrix;
when the coarse alignment time comes, calculating the gravity acceleration at i of two time points by the following formulab0The projected integrated vector:
Figure FDA0003212984430000022
in the formula XkFor the fitting result of the last recursive least squares, T1 and T2 are two time points in the course of coarse alignment respectively;
and finally, constructing a speed matrix by using speed integral vectors of two time points according to a common disturbance rejection coarse alignment method, and passing through ieA series of and ib0Obtained by velocity matrix operation of two coordinate systems
Figure FDA0003212984430000023
Further obtain the coarse alignment result
Figure FDA0003212984430000024
3. The method of claim 2, wherein the calculated V for each navigation cycle is a least squares based disturbance rejection coarse alignment methodt ib0In the recursive quadratic fitting, the formula (3) XkValue of 0, PkIs XkThe corresponding covariance matrix can be set according to system parameters; zkFor observation vector, take value Vib0k；Hk+1For observing the matrix, take the value [ (T)nav·k)2Tnav·k 1]Wherein T isnavFor the navigation period, k is the coarse alignment beat number.
4. The disturbance-rejection coarse alignment method based on recursive least squares according to claim 3, wherein the calculation of the gravitational acceleration at two time points is performed at ib0In the projected integral vector, two time points are respectively half of the coarse alignment duration and the coarse alignment duration.
CN202110936292.XA 2021-08-16 2021-08-16 Anti-disturbance coarse alignment method based on recursive least square Active CN113790720B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110936292.XA CN113790720B (en) 2021-08-16 2021-08-16 Anti-disturbance coarse alignment method based on recursive least square

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110936292.XA CN113790720B (en) 2021-08-16 2021-08-16 Anti-disturbance coarse alignment method based on recursive least square

Publications (2)

Publication Number Publication Date
CN113790720A true CN113790720A (en) 2021-12-14
CN113790720B CN113790720B (en) 2023-08-15

Family

ID=79181681

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110936292.XA Active CN113790720B (en) 2021-08-16 2021-08-16 Anti-disturbance coarse alignment method based on recursive least square

Country Status (1)

Country Link
CN (1) CN113790720B (en)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106500733A (en) * 2017-01-09 2017-03-15 北京航空航天大学 The non-orthogonal angle self-calibration of three axle Rotating Inertial Navigation System frameworks of one kind and compensation method
CN106595711A (en) * 2016-12-21 2017-04-26 东南大学 Strapdown inertial navigation system coarse alignment method based on recursive quaternion
CN108088463A (en) * 2016-11-22 2018-05-29 北京自动化控制设备研究所 A kind of inertial alignment method of height sensor auxiliary pseudolite positioning
WO2019188745A1 (en) * 2018-03-28 2019-10-03 パイオニア株式会社 Information processing device, control method, program, and storage medium
CN111024074A (en) * 2019-12-12 2020-04-17 北京遥测技术研究所 Inertial navigation speed error determination method based on recursive least square parameter identification
CN112683267A (en) * 2020-11-30 2021-04-20 电子科技大学 Vehicle-mounted attitude estimation method with GNSS velocity vector assistance

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108088463A (en) * 2016-11-22 2018-05-29 北京自动化控制设备研究所 A kind of inertial alignment method of height sensor auxiliary pseudolite positioning
CN106595711A (en) * 2016-12-21 2017-04-26 东南大学 Strapdown inertial navigation system coarse alignment method based on recursive quaternion
CN106500733A (en) * 2017-01-09 2017-03-15 北京航空航天大学 The non-orthogonal angle self-calibration of three axle Rotating Inertial Navigation System frameworks of one kind and compensation method
WO2019188745A1 (en) * 2018-03-28 2019-10-03 パイオニア株式会社 Information processing device, control method, program, and storage medium
CN111024074A (en) * 2019-12-12 2020-04-17 北京遥测技术研究所 Inertial navigation speed error determination method based on recursive least square parameter identification
CN112683267A (en) * 2020-11-30 2021-04-20 电子科技大学 Vehicle-mounted attitude estimation method with GNSS velocity vector assistance

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
单斌;秦永元;张金亮;王新国;周小刚;: "间接解析粗对准过程中的抗线干扰方法", 弹箭与制导学报, no. 05, pages 76 - 80 *

Also Published As

Publication number Publication date
CN113790720B (en) 2023-08-15

Similar Documents

Publication Publication Date Title
Li et al. A fast SINS initial alignment scheme for underwater vehicle applications
CN105606094B (en) A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems
CN106405670B (en) A kind of gravity anomaly data processing method suitable for strapdown marine gravitometer
CN109540126A (en) A kind of inertia visual combination air navigation aid based on optical flow method
CN107796391A (en) A kind of strapdown inertial navigation system/visual odometry Combinated navigation method
CN109916395B (en) Gesture autonomous redundant combined navigation algorithm
CN109163735B (en) Forward-forward backtracking initial alignment method for shaking base
CN112629538A (en) Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering
CN113405563B (en) Inertial measurement unit alignment method
CN109596144B (en) GNSS position-assisted SINS inter-travel initial alignment method
CN112432642B (en) Gravity beacon and inertial navigation fusion positioning method and system
Fu et al. Autonomous in-motion alignment for land vehicle strapdown inertial navigation system without the aid of external sensors
CN110285838B (en) Inertial navigation equipment alignment method based on gravity vector time difference
CN110736457A (en) combination navigation method based on Beidou, GPS and SINS
CN111722295B (en) Underwater strapdown gravity measurement data processing method
CN111307114B (en) Water surface ship horizontal attitude measurement method based on motion reference unit
CN110007318B (en) Method for judging GPS deception by single unmanned aerial vehicle based on Kalman filtering under wind field interference
CN110221331B (en) Inertia/satellite combined navigation dynamic filtering method based on state transformation
CN111595331A (en) Clock model assisted inertial/satellite/relative ranging information combined navigation method
CN105606093A (en) Inertial navigation method and device based on real-time gravity compensation
CN109506674A (en) A kind of bearing calibration of acceleration and device
CN113790720A (en) Disturbance-rejection coarse alignment method based on recursive least squares
CN114674313B (en) Unmanned distribution vehicle navigation positioning method based on CKF algorithm and integrating GPS/BDS and SINS
CN113932803B (en) Inertial/geomagnetic/satellite integrated navigation system suitable for high-dynamic aircraft
Wang et al. An adaptive cascaded Kalman filter for two-antenna GPS/MEMS-IMU integration

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