CN113790720B - Anti-disturbance coarse alignment method based on recursive least square - Google Patents
Anti-disturbance coarse alignment method based on recursive least square Download PDFInfo
- Publication number
- CN113790720B CN113790720B CN202110936292.XA CN202110936292A CN113790720B CN 113790720 B CN113790720 B CN 113790720B CN 202110936292 A CN202110936292 A CN 202110936292A CN 113790720 B CN113790720 B CN 113790720B
- Authority
- CN
- China
- Prior art keywords
- vector
- recursive
- alignment
- coarse alignment
- disturbance
- 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
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 square, which utilizes the recursive least square method to carry out secondary fitting on a velocity integral vector in the whole coarse alignment process, and extrapolates velocity integral vectors of two time points by using parameters obtained by fitting, thereby reducing errors caused by disturbance line movement on the coarse alignment. The method has clear principle and simple calculation, can be suitable for dynamic conditions such as vertical emission 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
Technical Field
The invention belongs to the field of inertial navigation, and relates to an anti-disturbance coarse alignment method.
Background
Coarse alignment of an inertial navigation system is to find and determine a rotation relation (attitude rotation matrix, quaternion or Euler angle) of an inertial conductor coordinate system (a right coordinate system on the front of an accelerometer) relative to a navigation coordinate system (a geographic coordinate system, usually a north-east coordinate system) through an inertial device sensitive earth gravity vector and an earth rotation angular velocity vector.
The conventional anti-disturbance coarse alignment method can only adapt to the disturbance motion state with angular motion only, such as engine work or shaking caused by upper and lower carriers of personnel. When there is a disturbance of the carrier in line motion, such as waves on the water surface, a large coarse alignment error will be introduced.
Disclosure of Invention
The invention provides an anti-disturbance coarse alignment method based on recursive least square, which reduces the influence of linear motion on coarse alignment.
The invention discloses an anti-disturbance coarse alignment method based on recursive least square, which comprises the following steps:
and (3) performing secondary fitting on the velocity integral vector 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, thereby reducing the influence of linear motion on the coarse alignment.
Further, the anti-disturbance coarse alignment method specifically includes:
inertial body coordinate system i for establishing inertial navigation at moment when inertial navigation system receives alignment instruction b0 System and earth inertial coordinate system i e Tying; aligning a time inertial navigation body coordinate system b system, namely an inertial coordinate system obtained by solidifying a front upper right coordinate system of the accelerometer; an inertial coordinate system obtained by solidifying an earth coordinate system e system at alignment moment, wherein the origin of the earth coordinate system e system is at the center of the earth, Z e The axis pointing to the north pole, X e With axis in equatorial plane, Y in local longitudinal direction e Axis and Z e Axis, X e The shaft is arranged according to the right-hand system;
at each navigation cycle, the gravity acceleration is calculated at i by b0 Tie sum i e The following projection:
in the middle ofRepresented as from b to i b0 A systematic attitude rotation matrix; f (f) b An acceleration vector perceived for acceleration Ji Min; g is gravity acceleration; v is carrier speed, < >>V is obtained by reference information as a speed derivative, V, & gt if the carrier is in a stationary state>The value is a 0 vector; omega is the rotation angular velocity of the earth, < >>Is the carrier local latitude;
then to i b0 And i e Integrating the gravity acceleration vector under the coordinate system:
t in nav Indicating the navigation period, t indicating the length of time after entering the coarse alignment,indicating that the gravity acceleration of the kth beat after the coarsest alignment instruction is i b0 Is projected (I)>Is a vector of integration of (a);
at g ib0 In the integration process, V is calculated for each navigation period t ib0 And (3) performing recursive quadratic fitting:
wherein: x is X k To recursively least squares quadratic fit coefficients, P k Is X k Corresponding covariance matrix, Z k To observe the vector, H k+1 Is an observation matrix.
When the coarse alignment time is up, the gravitational acceleration at two time points is calculated as i by the following equation b0 Integral vector of the system projection:
wherein X is k For the fitting result of the least square of the last recursion, T1 and T2 are respectively two time points in the rough alignment process;
finally, constructing a velocity matrix by using velocity integration vectors of two time points according to a common anti-disturbance coarse alignment method, and passing through i e Tie sum i b0 Is obtained by the operation of a velocity matrix of two coordinate systemsThereby obtaining coarse alignment result->
Further, the V calculated for each navigation cycle t ib0 In the recursive quadratic fitting, (3) X k Take the value of 0, P k Is X k The corresponding covariance matrix can be set according to system parameters; z is Z k For observing vector, take the value asH k+1 To observe the matrix, the value [ (T) nav ·k) 2 T nav ·k 1]Wherein T is nav For the navigation period, k is the coarse alignment beat number.
Further, the calculation of the gravitational acceleration at two time points is at i b0 In the integral vector of the projection, two time points are respectively the rough alignment duration and half of the rough alignment duration.
The invention has the beneficial effects that: the anti-disturbance coarse alignment method based on the recursive least square provided by the invention utilizes the recursive least square method to carry out secondary fitting on the velocity integral vector in the whole coarse alignment process, and extrapolates the velocity integral vector of two time points by using parameters obtained by fitting, thereby reducing errors caused by disturbance line movement on the coarse alignment. The method has clear principle and simple calculation, can be suitable for dynamic conditions such as vertical emission 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 the inertial navigation system carrier has disturbance motions such as online motions, disturbance speeds can be introduced into speed integration vectors of the traditional disturbance rejection coarse alignment algorithm, so that a coarse alignment result is caused to generate larger errors. Aiming at the problem, the invention provides an anti-disturbance coarse alignment method based on recursive least square, which utilizes the recursive least square method to carry out secondary fitting on a velocity integral vector in the whole coarse alignment process, and extrapolates the velocity integral vector of two time points by using parameters obtained by fitting, thereby reducing the influence of linear motion on the coarse alignment.
The invention firstly establishes an inertial body coordinate system i of inertial navigation at the moment when an inertial navigation system receives an alignment instruction b0 The system (the inertial navigation body coordinate system b system of alignment time, namely the inertial coordinate system obtained by solidifying the right coordinate system on the front of the accelerometer) and the earth inertial coordinate system i e The system (inertial coordinate system obtained by solidifying the earth coordinate system e system at the alignment moment, the origin of the earth coordinate system e system is at the center of the earth, Z e The axis pointing to the north pole, X e With axis in equatorial plane, Y in local longitudinal direction e Shaft and method for producing the sameZ e Axis, X e The shaft is arranged in the right-hand system).
Subsequently, at each navigation cycle, the gravitational acceleration at i is calculated by the following formula, respectively b0 Tie sum i e And (5) the projection is under. In the middle ofRepresented as from b to i b0 The attitude rotation matrix of the system (calculated by gyro sensitive angular velocity and consistent with the calculation mode in the normal disturbance rejection coarse alignment method); f (f) b An acceleration vector perceived for acceleration Ji Min; g is gravity acceleration; v is carrier speed, < >>V is obtained by reference information (satellite, odometer, etc.), V is obtained if the carrier is in a stationary state,The value is a 0 vector; omega is the rotation angular velocity of the earth, < >>Is the carrier local latitude.
Then to i b0 And i e Integrating the gravity acceleration vector under the coordinate system:
t in nav Indicating the navigation period, t indicating the length of time after entering the coarse alignment,indicating that the gravity acceleration of the kth beat after the coarsest alignment instruction is i b0 Is projected (I)>Is included in the integrated vector of (a).
At g ib0 In the integration process, in order to reduce the influence of the disturbance line movement, V is calculated for each navigation period t ib0 And (3) performing recursive quadratic fitting:
wherein: x is X k For the coefficient of recursive least square quadratic fitting, the initial value can be 0; p (P) k Is X k The corresponding covariance matrix can be set according to system parameters; z is Z k For observing vector, take value V ib0k ；H k+1 To observe the matrix, the value [ (T) nav ·k) 2 T nav ·k 1]Wherein T is nav For the navigation period, k is the coarse alignment beat number.
When the coarse alignment time is up, the gravitational acceleration at two time points is calculated as i by the following equation b0 Integral vector of the system projection:
wherein X is k For the final recursive least square fitting result, T1 and T2 are respectively two time points in the rough alignment process, and are set according to a system algorithm, and are commonly set to be the rough alignment duration and half of the rough alignment duration.
Finally, constructing a velocity matrix by using velocity integration vectors of two time points according to a common anti-disturbance coarse alignment method, and passing through i e Tie sum i b0 Is obtained by the operation of a velocity matrix of two coordinate systemsThereby obtaining coarse alignment result->
The method has clear principle and simple calculation, can be suitable for dynamic conditions such as vertical emission 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 limited to the explanation and description of the technical solutions 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 substitution of the technical solution of the present invention results in a new technical solution that falls within the scope of the present invention.
Claims (3)
1. The anti-disturbance coarse alignment method based on recursive least square is characterized in that a recursive least square method is utilized to perform secondary fitting on a speed integral vector in the whole coarse alignment process, and the speed integral vector of two time points is extrapolated by using parameters obtained by fitting, so that the influence of linear motion on the coarse alignment is reduced;
the anti-disturbance coarse alignment method specifically comprises the following steps:
inertial body coordinate system i for establishing inertial navigation at moment when inertial navigation system receives alignment instruction b0 System and earth inertial coordinate system i e Tying; aligning a time inertial navigation body coordinate system b system, namely an inertial coordinate system obtained by solidifying a front upper right coordinate system of the accelerometer; an inertial coordinate system obtained by solidifying an earth coordinate system e system at alignment moment, wherein the origin of the earth coordinate system e system is at the center of the earth, Z e The axis pointing to the north pole, X e With axis in equatorial plane, Y in local longitudinal direction e Axis and Z e Axis, X e The shaft is arranged according to the right-hand system;
at each navigation cycle, the gravity acceleration is calculated at i by b0 Tie sum i e The following projection:
in the middle ofRepresented as from b to i b0 A systematic attitude rotation matrix; f (f) b An acceleration vector perceived for acceleration Ji Min; g is gravity acceleration; v is carrier speed, < >>V is obtained by reference information as a speed derivative, V, & gt if the carrier is in a stationary state>The value is a 0 vector; omega is the rotation angular velocity of the earth, < >>Is the carrier local latitude;
then to i b0 And i e Integrating the gravity acceleration vector under the coordinate system:
t in nav Indicating the navigation period, t indicating the length of time after entering the coarse alignment,indicating that the gravity acceleration of the kth beat after the coarsest alignment instruction is i b0 Is projected (I)>Is a vector of integration of (a);
at g ib0 In the integration process, V is calculated for each navigation period t ib0 And (3) performing recursive quadratic fitting:
wherein: x is X k To recursively least squares quadratic fit coefficients, P k Is X k Corresponding covariance matrix, Z k To observe the vector, H k+1 Is an observation matrix;
when the coarse alignment time is up, the gravitational acceleration at two time points is calculated as i by the following equation b0 Integral vector of the system projection:
wherein X is k For the fitting result of the least square of the last recursion, T1 and T2 are respectively two time points in the rough alignment process;
finally, constructing a velocity matrix by using velocity integration vectors of two time points according to a common anti-disturbance coarse alignment method, and passing through i e Tie sum i b0 Is obtained by the operation of a velocity matrix of two coordinate systemsThereby obtaining coarse alignment result->
2. The coarse disturbance rejection alignment method based on recursive least squares according to claim 1, wherein the calculated V for each navigation cycle t ib0 In the recursive quadratic fitting, (3) X k Take the value of 0, P k Is X k The corresponding covariance matrix can be set according to system parameters; z is Z k For observing vector, take the value asH k+1 To observe the matrix, the value [ (T) nav ·k) 2 T nav ·k 1]Wherein T is nav For the navigation period, k is coarseAligning the beats.
3. The coarse disturbance rejection alignment method based on recursive least squares according to claim 2, wherein the calculated gravitational acceleration at two time points is equal to i b0 In the integral vector of the projection, two time points are respectively the rough alignment duration and half of the rough alignment duration.
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 CN113790720A (en) | 2021-12-14 |
| CN113790720B true 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)
| 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 |
-
2021
- 2021-08-16 CN CN202110936292.XA patent/CN113790720B/en active Active
Patent Citations (6)
| 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)
| Title |
|---|
| 间接解析粗对准过程中的抗线干扰方法;单斌;秦永元;张金亮;王新国;周小刚;;弹箭与制导学报(第05期);76-80 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113790720A (en) | 2021-12-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Li et al. | A fast SINS initial alignment scheme for underwater vehicle applications | |
| CN102353377B (en) | High altitude long endurance unmanned aerial vehicle integrated navigation system and navigating and positioning method thereof | |
| CN103822633B (en) | A kind of low cost Attitude estimation method measuring renewal based on second order | |
| CN109163735B (en) | Forward-forward backtracking initial alignment method for shaking base | |
| CN109709537B (en) | Non-cooperative target position and speed tracking method based on satellite formation | |
| CN107796391A (en) | A kind of strapdown inertial navigation system/visual odometry Combinated navigation method | |
| CN105258698B (en) | A kind of high dynamic spin aerial Combinated navigation method of guided cartridge | |
| CN111366148B (en) | Target positioning method suitable for multiple observations of airborne photoelectric observing and sighting system | |
| CN109596144B (en) | GNSS position-assisted SINS inter-travel initial alignment method | |
| CN110361010A (en) | It is a kind of based on occupy grating map and combine imu method for positioning mobile robot | |
| CN113405563B (en) | Inertial measurement unit alignment method | |
| CN109931955A (en) | Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group | |
| CN111399023B (en) | Inertial basis combined navigation filtering method based on lie group nonlinear state error | |
| CN106840211A (en) | A kind of SINS Initial Alignment of Large Azimuth Misalignment On methods based on KF and STUPF combined filters | |
| CN108680942A (en) | A kind of inertia/multiple antennas GNSS Combinated navigation methods and device | |
| Chen et al. | A novel fusion methodology to bridge GPS outages for land vehicle positioning | |
| CN111238469B (en) | Unmanned aerial vehicle formation relative navigation method based on inertia/data chain | |
| CN110223354A (en) | A kind of Camera Self-Calibration method based on SFM three-dimensional reconstruction | |
| CN109443356A (en) | A kind of the unmanned boat Position And Velocity estimation structure and design method of the noise containing measurement | |
| CN105823463A (en) | Vehicle gesture measuring method and apparatus | |
| CN110221331B (en) | Inertia/satellite combined navigation dynamic filtering method based on state transformation | |
| CN104406592B (en) | A kind of correction of navigation system and attitude angle and backtracking decoupling method for underwater glider | |
| CN112212862A (en) | GPS/INS integrated navigation method for improving particle filtering | |
| CN111307114A (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
| CN113790720B (en) | Anti-disturbance coarse alignment method based on recursive least square |
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 |