CN107990910B - Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering - Google Patents

Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering Download PDF

Info

Publication number
CN107990910B
CN107990910B CN201711078693.6A CN201711078693A CN107990910B CN 107990910 B CN107990910 B CN 107990910B CN 201711078693 A CN201711078693 A CN 201711078693A CN 107990910 B CN107990910 B CN 107990910B
Authority
CN
China
Prior art keywords
inertial navigation
coordinate system
sub
navigation
main
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
CN201711078693.6A
Other languages
Chinese (zh)
Other versions
CN107990910A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201711078693.6A priority Critical patent/CN107990910B/en
Publication of CN107990910A publication Critical patent/CN107990910A/en
Application granted granted Critical
Publication of CN107990910B publication Critical patent/CN107990910B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention discloses a ship large azimuth misalignment angle transfer alignment method based on cubature Kalman filtering. Firstly, converting the specific force output of a sub inertial navigation accelerometer into a navigation coordinate system, and carrying out filtering processing on the navigation coordinate system by using a Butterworth digital low-pass filter; secondly, respectively carrying out inertial navigation resolving on the main inertial navigation system and the sub inertial navigation system, transmitting speed, attitude and angular speed information of the main inertial navigation system to a navigation computer of the sub inertial navigation system, and measuring by using a speed error, an attitude error and an angular speed error structure quantity between the main inertial navigation system and the sub inertial navigation system; then, establishing a state equation and a measurement equation under the condition of a large azimuth misalignment angle by adopting a matching mode of velocity plus attitude plus angular velocity; and finally, carrying out volume Kalman filtering solution by using the established state equation and measurement equation, estimating an installation error angle between the sub inertial navigation system and the main inertial navigation system, and finishing transfer alignment. The invention solves the problem of fast high-precision alignment of ships under the condition of large azimuth misalignment angle and large rod arm error.

Description

Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering
Technical Field
The invention relates to the technical field of strapdown inertial navigation, in particular to a ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering.
Background
An inertial navigation system is an autonomous navigation system based on the principle of inertia. The strapdown inertial navigation system directly and fixedly connects the gyroscope and the accelerometer to the carrier to measure angular motion and linear motion information of the carrier, and calculates speed, position, attitude and heading information of the carrier relative to the earth through integral operation. The initial alignment is a key technology of the strapdown inertial navigation system, the accuracy of the alignment directly affects the accuracy of the navigation system, and the time for completing the alignment directly affects the quick response capability of the system. The transfer alignment is an initial alignment mode for aligning the sub inertial navigation by using the output information of the main inertial navigation, the alignment speed is high, and no limitation is imposed on the maneuvering of the carrier.
Because ships and warships can be influenced by sea waves when sailing at sea, especially under the condition of poor sea conditions, the real system can be more accurately described based on the nonlinear model with a large azimuth misalignment angle. For a large ship, a main inertial navigation system on the ship is often installed at a swinging center of the large ship, the installation position of a sub inertial navigation system on a ship-based device has a long distance with the main inertial navigation system, and when angular motion exists on a carrier, accelerometers of the main inertial navigation system and the sub inertial navigation system can be sensitive to different accelerated speeds, so that a lever arm speed difference exists between the main inertial navigation system and the sub inertial navigation system, which is a lever arm effect phenomenon in transfer alignment. The lever arm effect can severely affect the accuracy and convergence speed of the transfer alignment and must be compensated for.
In the article of error analysis and compensation of the rod-arm effect in transfer alignment (published in journal, journal of the academic journal of instruments, 2013, vol. 34, 03), gaowei et al propose a directly calculated compensation method for a nonlinear system under a condition of a large azimuth misalignment angle, and the azimuth misalignment angle can be converged to 0.381 degrees within 120 s. Huangxiangyuan et al, in the study on nonlinear alignment technology based on simplified CKF/dimension-reduced CKF hybrid filtering (published in journal, bulletin and arrow and guidance bulletin, 2015, vol. 35, phase 01), proposed a nonlinear alignment method based on simplified CKF/dimension-reduced CKF hybrid filtering, which greatly reduces the amount of calculation, achieves a horizontal alignment accuracy of less than 1 'and achieves an orientation alignment accuracy of less than 5'. Xu dao su et al, entitled "improved CKF-based SINS initial alignment method" (proceedings of science and technology university in china (nature science edition), 2016 (vol. 44, vol. 01)), propose an improved CKF method for transfer alignment under large azimuthal misalignment angle conditions, with azimuthal alignment accuracy of 3' or less. The invention designs a speed plus attitude plus angular speed transfer alignment method based on the cubature Kalman filtering, which can be used for the situation that a ship has a large azimuth misalignment angle and a large rod-arm error, and the alignment speed and accuracy are greatly improved compared with the existing method.
Disclosure of Invention
The invention aims to provide a rapid high-precision transfer alignment method which can be applied to the situation that a ship has a large azimuth misalignment angle and a large rod-arm error.
The technical scheme for realizing the purpose of the invention is as follows: a ship large azimuth misalignment angle transfer alignment method based on cubature Kalman filtering comprises the following steps:
the method comprises the following steps: completing the preparation of starting and preheating the sub inertial navigation system;
step two: converting the specific force output of the sub inertial navigation accelerometer into a navigation coordinate system, and performing filtering processing on the navigation coordinate system by using a Butterworth digital low-pass filter to achieve the purpose of eliminating the influence of lever arm effect errors;
step three: the main inertial navigation system and the sub inertial navigation system respectively carry out inertial navigation resolving, and the speed, the attitude and the angular speed information of the main inertial navigation system are transmitted to a navigation computer of the sub inertial navigation system;
step four: under the condition that a ship has a large azimuth misalignment angle, a matching mode of speed plus attitude plus angular speed is adopted, the coordinate systems of the main inertial navigation carrier and the sub inertial navigation carrier are different, the navigation coordinate systems are the same, the speed error, the attitude error and the angular speed error between the main inertial navigation system and the sub inertial navigation system are selected as quantity measurement, and a state equation and a measurement equation of the system are established;
step five: and performing volume Kalman filtering solution by using the established state equation and measurement equation, estimating an installation error angle between the sub inertial navigation system and the main inertial navigation system, and finishing transfer alignment.
In step two, the technical requirements of the butterworth digital low-pass filter are as follows:
passband cut-off frequency of fp0.01Hz, pass band ripple αp2dB, stop band cut-off frequency fs0.15Hz, stop band attenuation of αs＝40dB。
The discrete transfer function of the second order butterworth digital filter is designed as follows:
Figure BDA0001458468720000021
the state equation of the filter is then:
Figure BDA0001458468720000022
the output equation is:
Figure BDA0001458468720000023
where u (n) represents the input to the filter,
Figure BDA0001458468720000024
c＝[0.00166 0.70710]，d＝5.53551e-06。
in step three, the established velocity plus attitude plus angular velocity matching transfer alignment mathematical model is as follows:
ignoring the vertical channel, the selected state variables are:
Figure BDA0001458468720000025
the system state equation is:
Figure BDA0001458468720000031
wherein n is a navigation coordinate system; m is a main inertial navigation carrier coordinate system; s is a sub inertial navigation carrier coordinate system;
Figure BDA0001458468720000032
calculating a carrier coordinate system for the sub inertial navigation; vnThe projection of the speed error on a navigation coordinate system;
Figure BDA0001458468720000033
a direction cosine matrix from the main inertial navigation carrier coordinate system to the sub inertial navigation carrier coordinate system;
Figure BDA0001458468720000034
calculating a direction cosine matrix of the carrier coordinate system from the main inertial navigation carrier coordinate system to the sub inertial navigation;
Figure BDA0001458468720000035
a direction cosine matrix from the main inertial navigation carrier coordinate system to the navigation coordinate system;
Figure BDA0001458468720000036
the projection of the specific force measured by the sub inertial navigation in a carrier coordinate system is obtained;
Figure BDA0001458468720000037
projecting the rotational angular velocity of the earth in a navigation coordinate system;
Figure BDA0001458468720000038
the projection of the angular velocity of n relative to the earth coordinate system in n is obtained;
Figure BDA0001458468720000039
is the installation error angle between the s series and the m series;
Figure BDA00014584687200000310
is composed of
Figure BDA00014584687200000311
A measured misalignment angle between the system and the m system;
Figure BDA00014584687200000312
the method comprises the following steps of (1) projecting the angular velocity of a main inertial navigation relative to a navigation coordinate system in an m system;
Figure BDA00014584687200000313
constant drift for the accelerometer; w is av(ii) randomly drifting the accelerometer;sconstant drift of the gyroscope;
Figure BDA00014584687200000314
the gyro is randomly drifted.
The speed error V between the main inertial navigation and the sub inertial navigation is obtained by adopting a matching mode of adding speed and attitude plus angular speednMeasuring the misalignment angle
Figure BDA00014584687200000315
And error of angular velocity
Figure BDA00014584687200000320
As observed quantities:
Figure BDA00014584687200000316
the measurement equation is as follows:
Z＝h(X)+V
wherein, V is the measurement noise of the system.
The method comprises the following steps of solving a volume rule by using a three-degree-of-freedom sphere-Radial, designing a nonlinear filtering algorithm, namely volume Kalman filtering, by using a group of 2n volume points with equal weights, and specifically comprising the following steps:
for one continuous non-linear system:
Figure BDA00014584687200000317
discretizing the system model by adopting a 4-order Runge Kutta (Runge Kutta) method to obtain a discrete nonlinear system:
Figure BDA00014584687200000318
wherein, XkIs a system state vector; zkIs an observation vector; wkIs a system noise vector, VkFor measuring the noise vector, the two are zero-mean Gaussian white noise sequences and are not correlated with each other, namely, the following requirements are met:
Figure BDA00014584687200000319
wherein Q iskIs a variance matrix of the system noise sequence; rkA variance matrix for the measured noise sequence;kjis the kronecker function.
The specific implementation steps of the volume Kalman filtering are as follows:
a. time updating
Assume state x at time k-1k-1Is known, first for Pk-1Performing Cholesky decomposition:
Figure BDA0001458468720000041
calculating a volume point:
Figure BDA0001458468720000042
calculating the volume point after the transfer of the system state equation:
Figure BDA0001458468720000043
estimating a state prediction value at the k moment:
Figure BDA0001458468720000044
estimating state prediction covariance matrix at k time:
Figure BDA0001458468720000045
b. measurement update
To Pk/k-1Performing Cholesky decomposition:
Figure BDA0001458468720000046
calculating a volume point:
Figure BDA0001458468720000047
calculating the volume point after the transfer of the system measurement equation:
Zi,k/k-1＝h(Xi,k/k-1)i＝1,2…,2n
estimating a measurement predicted value at the k moment:
Figure BDA0001458468720000048
estimating a measurement prediction covariance matrix at the k moment:
Figure BDA0001458468720000049
estimating a one-step prediction cross-correlation covariance matrix at time k:
Figure BDA00014584687200000410
estimating the filter gain at the k moment:
Figure BDA0001458468720000051
and (3) obtaining a state estimation value at the k moment:
Figure BDA0001458468720000052
and (3) solving a state error covariance matrix at the k moment:
Figure BDA0001458468720000053
and matching a state equation and a measurement equation of transfer alignment according to the established velocity and attitude plus angular velocity, performing volume Kalman filtering solution, estimating an installation error angle between the sub inertial navigation system and the main inertial navigation system, and finishing the transfer alignment.
Compared with the prior art, the invention has the beneficial effects that:
the invention considers the system as a nonlinear model under the condition that a ship has a large azimuth misalignment angle, designs a Butterworth digital low-pass filter to filter the output of the sub inertial navigation accelerometer, establishes a filter model by adopting a matching mode of velocity plus attitude plus angular velocity, and carries out volume Kalman filtering solution, thereby effectively eliminating the influence of a lever arm effect and greatly improving the alignment speed and precision of the ship under the condition of the large azimuth misalignment angle and the error of a large lever arm.
Drawings
FIG. 1 is a basic flow diagram of the present invention;
FIG. 2 is a frequency spectrum of lever arm acceleration;
fig. 3 is a mounting error angle estimation error curve obtained by Matlab simulation.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
In order to verify the effectiveness of the method, a Matlab is utilized to simulate a transfer alignment nonlinear model when the ship is subjected to large azimuth misalignment angle based on the volume Kalman filtering.
The naval vessel can receive the stormy waves influence when navigation in the sea, produces the triaxial and sways the motion, and its mathematical model is:
Figure BDA0001458468720000054
in the formula, psi, theta and gamma respectively represent a course angle, a pitch angle and a roll angle; psim，θm，γmIs the amplitude of the swing angle; omegay，ωp，ωrIs the rocking angular frequency; t isi＝2π/ωi(i ═ y, p, r) is the wobble period;
Figure BDA0001458468720000055
is the initial attitude angle; k is the initial heading.
The simulation parameters are set as follows:
amplitude of rocking angle: psim＝5°，θm＝15°，γm＝10°；
The swing period is as follows: t isy＝8s，Tp＝12s，Tr＝6s；
Initial attitude angle:
Figure BDA0001458468720000056
initial course: k is 30 °;
initial latitude
Figure BDA0001458468720000061
Initial longitude λ 126.6705 °;
the error angle is:
Figure BDA0001458468720000062
gyro constant drift ofx＝y＝z＝0.01°The random drift is 0.001 degree/h;
the random constant bias of the accelerometer is 10-4g, random drift of accelerometer is 10-5g；
The ship directly navigates at a constant speed of 5 m/s;
and (3) filtering period: 0.05 s;
lever arm length: r iss＝[8 25 2]T(m)。
Designing a Butterworth digital low-pass filter to process the output of the sub inertial navigation accelerometer, and specifically comprising the following steps of:
the oscillation of the sub inertial navigation system is mainly the oscillation of a Shula period and a terrestrial rotation period, is in a low frequency region, and has a frequency spectrum distribution of f 2 × 10-4Hz or less. From the spectrum of the lever arm effect acceleration, the specifications of the butterworth low-pass filter can be determined:
passband cut-off frequency fp0.01Hz, passband ripple αp2dB stop band cut-off frequency fs0.15Hz, stop band attenuation αs40 dB. The order N of the filter is first determined by the following equation.
Figure BDA0001458468720000063
In the formula (I), the compound is shown in the specification,
Figure BDA0001458468720000064
Figure BDA00014584687200000611
substitution can obtain N ═ 1.80, and N ═ 2.
The 3dB cut-off frequency is:
Figure BDA0001458468720000065
the normalized prototype system function of the second order low pass filter can be obtained from the butterworth normalized low pass filter parameter table:
Figure BDA0001458468720000066
g is to bea(p) denormalization to obtain the system function of the analog low-pass filter:
Figure BDA0001458468720000067
and (3) converting the sampling interval T to 0.05s by using a bilinear transformation method to obtain a system function of the digital low-pass filter:
Figure BDA0001458468720000068
the state equation of the filter is:
Figure BDA0001458468720000069
the output equation is:
Figure BDA00014584687200000610
where u (n) represents the input to the filter,
Figure BDA0001458468720000071
c＝[0.00166 0.70710]，d＝5.53551e-06。
establishing a speed and attitude plus angular speed matching transfer alignment mathematical model, which comprises the following specific steps:
the starting point of the mathematical modeling of velocity plus attitude plus angular velocity matching transfer alignment is that the carrier coordinate systems of the main inertial navigation and the sub inertial navigation are different, and the navigation coordinate systems are the same. Setting a navigation coordinate system of the main inertial navigation system and a navigation coordinate system of the sub inertial navigation system as an n system, and respectively representing carrier coordinate systems of the main inertial navigation system and the sub inertial navigation system by m and s, wherein the to-be-estimated quantity is an installation error angle between the m system and the s system
Figure BDA0001458468720000072
Before alignment, the sub inertial navigation can not obtain a strapdown matrix of a sub inertial navigation carrier coordinate system s relative to a navigation coordinate system n, so that a sub inertial navigation calculation carrier is definedCoordinate system
Figure BDA0001458468720000073
Will be provided with
Figure BDA0001458468720000074
The error angle between m and m is called the measurement misalignment angle and is expressed as
Figure BDA0001458468720000075
According to the specific force equation, the main inertial navigation system and the sub inertial navigation system have the following steps:
Figure BDA0001458468720000076
in the formula (I), the compound is shown in the specification,
Figure BDA0001458468720000077
and
Figure BDA0001458468720000078
respectively is the projection of the specific force measured by the main inertial navigation system and the sub inertial navigation system in respective carrier coordinate systems,
Figure BDA0001458468720000079
and
Figure BDA00014584687200000710
the projection of the speed of the main inertial navigation and the sub inertial navigation in a navigation coordinate system;
Figure BDA00014584687200000711
a direction cosine matrix from the main inertial navigation carrier coordinate system to the navigation coordinate system;
Figure BDA00014584687200000712
a direction cosine matrix from the sub inertial navigation carrier coordinate system to the navigation coordinate system;
Figure BDA00014584687200000713
projecting the rotational angular velocity of the earth in a navigation coordinate system;
Figure BDA00014584687200000714
the projection of the angular velocity of n relative to the earth coordinate system in n is obtained;
Figure BDA00014584687200000715
and
Figure BDA00014584687200000716
the gravity acceleration of the positions of the main inertial navigation unit and the sub inertial navigation unit are respectively.
Subtracting the two equations to obtain:
Figure BDA00014584687200000717
the output specific force relationship of the main inertial navigation system and the sub inertial navigation system is as follows without considering lever arm effect errors:
Figure BDA00014584687200000718
in the formula (I), the compound is shown in the specification,
Figure BDA00014584687200000719
is the sub inertial navigation accelerometer error;
Figure BDA00014584687200000720
the direction cosine matrix from the main inertial navigation carrier coordinate system to the sub inertial navigation carrier coordinate system. Consider that
Figure BDA00014584687200000721
The following can be obtained:
Figure BDA00014584687200000722
the above equation is the velocity error differential equation for velocity plus attitude plus angular velocity matching transfer alignment.
The differential of the measurement misalignment angle is the projection of the angular velocity of s 'in s' with respect to m, i.e.:
Figure BDA00014584687200000723
regardless of flexural deformation, the output angular velocity relationship of the main inertial navigation system and the sub inertial navigation system is as follows:
Figure BDA00014584687200000724
therefore, there are:
Figure BDA00014584687200000725
the main inertial navigation and the sub inertial navigation are fixedly connected on the carrier, so that the installation error angle is considered
Figure BDA00014584687200000726
Is constant, therefore:
Figure BDA0001458468720000081
the two equations are the differential equation of the misalignment angle measured by the velocity plus attitude plus angular velocity matching transfer alignment and the differential equation of the installation error angle.
The main error sources of inertial devices are gyro drift and accelerometer zero offset
Figure BDA0001458468720000082
The gyro drift is mainly from constant driftcRelated driftrRandom white noise drift wgAnd the like. The correlation time of the correlation drift is generally more than 1 hour, the correlation drift can be approximately regarded as constant drift, and is 1-2 orders of magnitude smaller than the constant drift, so that the gyro error model can be simplified as follows:
Figure BDA0001458468720000083
similarly, accelerometer zero offset can also be reduced to a constant drift, i.e.:
Figure BDA0001458468720000084
ignoring the vertical channel, the selected state variables are:
Figure BDA0001458468720000085
the system state equation is:
Figure BDA0001458468720000086
selecting the speed error, the measurement misalignment angle and the angular speed error between the main inertial navigation and the sub inertial navigation as observed quantities, namely:
Figure BDA0001458468720000087
wherein the velocity observed quantity is VnThe attitude observed quantity is
Figure BDA0001458468720000088
The observed amount of angular velocity is
Figure BDA0001458468720000089
The measurement equation is as follows:
Z＝h(X)+V
wherein, V is the measurement noise of the system.
Performing cubature Kalman filtering solution, wherein the algorithm flow is as follows:
a. time updating
Assume state x at time k-1k-1Is known, first for Pk-1Performing Cholesky decomposition:
Figure BDA00014584687200000810
calculating a volume point:
Figure BDA00014584687200000811
calculating the volume point after the transfer of the system state equation:
Figure BDA0001458468720000091
estimating a state prediction value at the k moment:
Figure BDA0001458468720000092
estimating state prediction covariance matrix at k time:
Figure BDA0001458468720000093
b. measurement update
To Pk/k-1Performing Cholesky decomposition:
Figure BDA0001458468720000094
calculating a volume point:
Figure BDA0001458468720000095
calculating the volume point after the transfer of the system measurement equation:
Zi,k/k-1＝h(Xi,k/k-1)i＝1,2…,2n
estimating a measurement predicted value at the k moment:
Figure BDA0001458468720000096
estimating a measurement prediction covariance matrix at the k moment:
Figure BDA0001458468720000097
estimating a one-step prediction cross-correlation covariance matrix at time k:
Figure BDA0001458468720000098
estimating the filter gain at the k moment:
Figure BDA0001458468720000099
and (3) obtaining a state estimation value at the k moment:
Figure BDA00014584687200000910
and (3) solving a state error covariance matrix at the k moment:
Figure BDA00014584687200000911
initial conditions for the volumetric Kalman filter, including state estimation covariance matrix P0System noise variance matrix Q0And measure the variance matrix R of the noise0The settings are as follows:
P0＝diag{(0.1m/s)2,(0.1m/s)2,(0.2°)2,(0.2°)2,(10°)2,(0.2°)2,(0.2°)2,(10°)2,
(1×10-4g0)2,(1×10-4g0)2,(0.01°/h)2,(0.01°/h)2,(0.01°/h)2}
Q0＝diag{(1×10-5g0)2,(1×10-5g0)2,(0.001°/h)2,(0.001°/h)2,(0.001°/h)2}
R0＝diag{(0.1m/s)2,(0.1m/s)2,(0.001°)2,(0.001°)2,(0.001°)2,(0.5°/h)2,(0.5°/h)2,(0.5°/h)2}
and (3) simulation results:
the results of the simulation under the above simulation conditions are shown in table 1 and fig. 3.
TABLE 1 installation error Angle estimation error in the case of Large azimuthal misalignment Angle
Figure BDA0001458468720000101
As can be seen from Table 1 and FIG. 3, the longitudinal, lateral and heading estimation errors can be rapidly reduced to below 2 angular divisions within 1s, 5s to below 0.1 angular divisions, and after 20s the estimation error to below 0.01 angular divisions by using the method of the present invention. In conclusion, the method provided by the invention can effectively eliminate the influence of the lever arm effect and can realize quick high-precision alignment under the condition that a ship has a large azimuth misalignment angle and a large lever arm error.

Claims (2)

1. A ship large azimuth misalignment angle transfer alignment method based on cubature Kalman filtering is characterized by comprising the following steps:
the method comprises the following steps: completing the preparation of starting and preheating the sub inertial navigation system;
step two: converting the specific force output of the sub inertial navigation accelerometer into a navigation coordinate system, and performing filtering processing on the navigation coordinate system by using a Butterworth digital low-pass filter to achieve the purpose of eliminating the influence of lever arm effect errors;
step three: the main inertial navigation system and the sub inertial navigation system respectively carry out inertial navigation resolving, and the speed, the attitude and the angular speed information of the main inertial navigation system are transmitted to a navigation computer of the sub inertial navigation system;
step four: under the condition that a ship has a large azimuth misalignment angle, a matching mode of speed plus attitude plus angular speed is adopted, the coordinate systems of the main inertial navigation carrier and the sub inertial navigation carrier are different, the navigation coordinate systems are the same, the speed error, the attitude error and the angular speed error between the main inertial navigation system and the sub inertial navigation carrier are selected as observed quantities, and a state equation and a measurement equation of the system are established;
setting a navigation coordinate system of the main inertial navigation system and a navigation coordinate system of the sub inertial navigation system as an n system, and respectively representing carrier coordinate systems of the main inertial navigation system and the sub inertial navigation system by m and s, wherein the to-be-estimated quantity is an installation error angle between the m system and the s system
Figure FDA0002204438360000011
Before alignment, the sub inertial navigation system cannot acquire a strapdown matrix of a sub inertial navigation carrier coordinate system s relative to a navigation coordinate system n, so that a sub inertial navigation calculation carrier coordinate system is defined
Figure FDA0002204438360000012
Will be provided with
Figure FDA0002204438360000013
The error angle between m and m is called the measurement misalignment angle and is expressed as
Figure FDA0002204438360000014
According to the specific force equation, the main inertial navigation system and the sub inertial navigation system have the following steps:
Figure FDA0002204438360000015
in the formula (I), the compound is shown in the specification,
Figure FDA0002204438360000016
and
Figure FDA0002204438360000017
respectively is the projection of the specific force measured by the main inertial navigation system and the sub inertial navigation system in respective carrier coordinate systems,
Figure FDA0002204438360000018
and
Figure FDA0002204438360000019
the projection of the speed of the main inertial navigation and the sub inertial navigation in a navigation coordinate system;
Figure FDA00022044383600000110
a direction cosine matrix from the main inertial navigation carrier coordinate system to the navigation coordinate system;
Figure FDA00022044383600000111
a direction cosine matrix from the sub inertial navigation carrier coordinate system to the navigation coordinate system;
Figure FDA00022044383600000112
projecting the rotational angular velocity of the earth in a navigation coordinate system;
Figure FDA00022044383600000113
the projection of the angular velocity of n relative to the earth coordinate system in n is obtained;
Figure FDA00022044383600000114
and
Figure FDA00022044383600000115
respectively the gravity acceleration of the positions of the main inertial navigation unit and the sub inertial navigation unit;
subtracting the two equations to obtain:
Figure FDA00022044383600000116
the output specific force relationship of the main inertial navigation system and the sub inertial navigation system is as follows without considering lever arm effect errors:
Figure FDA00022044383600000117
in the formula (I), the compound is shown in the specification,
Figure FDA00022044383600000118
is the sub inertial navigation accelerometer error;
Figure FDA00022044383600000119
a direction cosine matrix from the main inertial navigation carrier coordinate system to the sub inertial navigation carrier coordinate system; consider that
Figure FDA00022044383600000120
Obtaining:
Figure FDA00022044383600000121
the above equation is a velocity error differential equation of velocity plus attitude plus angular velocity matching transfer alignment;
the differential of the measurement misalignment angle is the projection of the angular velocity of s 'in s' with respect to m, i.e.:
Figure FDA00022044383600000122
regardless of flexural deformation, the output angular velocity relationship of the main inertial navigation system and the sub inertial navigation system is as follows:
Figure FDA0002204438360000021
therefore, there are:
Figure FDA0002204438360000022
the main inertial navigation and the sub inertial navigation are fixedly connected on the carrier, so that the installation error angle is considered
Figure FDA0002204438360000023
Is constant, therefore:
Figure FDA0002204438360000024
the two equations are a differential equation of the misalignment angle measured by the velocity plus attitude plus angular velocity matching transfer alignment and a differential equation of the installation error angle;
the main error sources of inertial devices are gyro drift and accelerometer zero offset
Figure FDA0002204438360000025
The gyro drift is mainly from constant driftcRelated driftrRandom white noise drift wgThe three parts are as follows; correlation time of correlation drift is more than 1 smallIn time, the gyro error model can be approximately regarded as constant drift, and is 1-2 orders of magnitude smaller than the constant drift, so that the gyro error model is simplified as follows:
Figure FDA0002204438360000026
similarly, accelerometer zero offset can also be reduced to a constant drift, i.e.:
Figure FDA0002204438360000027
ignoring the vertical channel, the selected state variables are:
Figure FDA0002204438360000028
the system state equation is:
Figure FDA0002204438360000029
wherein n is a navigation coordinate system; m is a main inertial navigation carrier coordinate system; s is a sub inertial navigation carrier coordinate system;
Figure FDA00022044383600000210
calculating a carrier coordinate system for the sub inertial navigation; vnThe projection of the speed error on a navigation coordinate system;
Figure FDA00022044383600000211
a direction cosine matrix from the main inertial navigation carrier coordinate system to the sub inertial navigation carrier coordinate system;
Figure FDA00022044383600000212
calculating a direction cosine matrix of the carrier coordinate system from the main inertial navigation carrier coordinate system to the sub inertial navigation;
Figure FDA00022044383600000213
is a main inertial navigation carrier seatA direction cosine matrix from the coordinate system to the navigation coordinate system;
Figure FDA00022044383600000214
the projection of the specific force measured by the sub inertial navigation in a carrier coordinate system is obtained;
Figure FDA00022044383600000215
projecting the rotational angular velocity of the earth in a navigation coordinate system;
Figure FDA00022044383600000216
the projection of the angular velocity of n relative to the earth coordinate system in n is obtained;
Figure FDA00022044383600000217
is the installation error angle between the s series and the m series;
Figure FDA00022044383600000218
is composed of
Figure FDA00022044383600000219
A measured misalignment angle between the system and the m system;
Figure FDA00022044383600000220
the method comprises the following steps of (1) projecting the angular velocity of a main inertial navigation relative to a navigation coordinate system in an m system;
Figure FDA00022044383600000221
constant drift for the accelerometer; w is av(ii) randomly drifting the accelerometer;sconstant drift of the gyroscope;
Figure FDA00022044383600000222
randomly drifting the gyroscope;
selecting the speed error, the measurement misalignment angle and the angular speed error between the main inertial navigation and the sub inertial navigation as observed quantities, namely:
Figure FDA0002204438360000031
wherein the velocity observed quantity is VnThe attitude observed quantity is
Figure FDA0002204438360000032
The observed amount of angular velocity is
Figure FDA0002204438360000033
The measurement equation is as follows:
Z＝h(X)+V
wherein, V is the measurement noise of the system;
step five: and performing volume Kalman filtering solution by using the established state equation and measurement equation, estimating an installation error angle between the sub inertial navigation system and the main inertial navigation system, and finishing transfer alignment.
2. The ship large azimuth misalignment angle transfer alignment method based on the cubature Kalman filtering as claimed in claim 1, characterized in that the oscillation of the sub inertial navigation system is mainly the oscillation of the Sula cycle and the earth rotation cycle, and is in the low frequency region, and the frequency spectrum distribution is 2 × 10 ═ f-4Hz or less; from the spectrum of the lever arm effect acceleration, the specifications of the butterworth low-pass filter can be determined:
passband cut-off frequency fp0.01Hz, passband ripple αp2dB stop band cut-off frequency fs0.15Hz, stop band attenuation αs40 dB; firstly, determining the order N of a filter through the following formula;
Figure FDA0002204438360000034
in the formula (I), the compound is shown in the specification,
Figure FDA0002204438360000035
substituting to obtain N-1.80, and taking N-2;
the 3dB cut-off frequency is:
Figure FDA0002204438360000036
the normalized prototype system function of the second order low pass filter can be obtained from the butterworth normalized low pass filter parameter table:
Figure FDA0002204438360000037
g is to bea(p) denormalization to obtain the system function of the analog low-pass filter:
Figure FDA0002204438360000038
and (3) converting the sampling interval T to 0.05s by using a bilinear transformation method to obtain a system function of the digital low-pass filter:
Figure FDA0002204438360000039
the state equation of the filter is:
Figure FDA00022044383600000310
the output equation is:
Figure FDA0002204438360000041
where u (n) represents the input to the filter,
Figure FDA0002204438360000042
c＝[0.00166 0.70710]，d＝5.53551e-06。
CN201711078693.6A 2017-11-06 2017-11-06 Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering Active CN107990910B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711078693.6A CN107990910B (en) 2017-11-06 2017-11-06 Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711078693.6A CN107990910B (en) 2017-11-06 2017-11-06 Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering

Publications (2)

Publication Number Publication Date
CN107990910A CN107990910A (en) 2018-05-04
CN107990910B true CN107990910B (en) 2020-09-11

Family

ID=62030589

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711078693.6A Active CN107990910B (en) 2017-11-06 2017-11-06 Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering

Country Status (1)

Country Link
CN (1) CN107990910B (en)

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109443379B (en) * 2018-09-28 2020-07-21 东南大学 SINS/DV L underwater anti-shaking alignment method of deep-sea submersible vehicle
CN109460075B (en) * 2018-11-01 2021-10-01 湖北航天技术研究院总体设计所 Method and system for fast azimuth alignment
CN109682397B (en) * 2018-12-18 2021-01-29 上海航天控制技术研究所 Ground static alignment method capable of fast convergence without being influenced by historical data
CN109737960A (en) * 2018-12-21 2019-05-10 哈尔滨工业大学 Deformation of hull measurement method based on velocity plus angular rate matching
CN110057383B (en) * 2019-05-05 2023-01-03 哈尔滨工程大学 Lever arm error calibration method of AUV (autonomous Underwater vehicle) push navigation system
CN110044385B (en) * 2019-05-09 2020-12-08 北京壹氢科技有限公司 Rapid transfer alignment method under condition of large misalignment angle
CN110736459B (en) * 2019-10-25 2021-08-17 中国人民解放军国防科技大学 Angular deformation measurement error evaluation method for inertial quantity matching alignment
CN111351481B (en) * 2020-03-13 2023-09-05 南京理工大学 Transmission alignment method based on emission inertial coordinate system
CN113447960B (en) * 2020-03-26 2022-06-24 千寻位置网络有限公司 Error calibration method and system based on GNSS/MEMS combined vehicle navigation system
CN111707292B (en) * 2020-07-18 2022-04-08 东南大学 Fast transfer alignment method of self-adaptive filtering
CN112729279A (en) * 2020-11-17 2021-04-30 武汉第二船舶设计研究所(中国船舶重工集团公司第七一九研究所) CCKF-based underwater unmanned submersible vehicle attitude measurement method
CN112649022B (en) * 2021-01-08 2022-05-27 哈尔滨工业大学 Large misalignment angle transfer alignment method considering flexural deformation and lever arm effect
CN114763994B (en) * 2021-05-06 2024-01-30 苏州精源创智能科技有限公司 Inertial attitude navigation system applied to sweeping robot
CN114061575B (en) * 2021-11-26 2024-06-14 上海机电工程研究所 Missile attitude angle precise alignment method and system under condition of large misalignment angle
CN114136340B (en) * 2021-11-29 2023-06-16 重庆华渝电气集团有限公司 Method for eliminating influence of misalignment angle error on initial alignment
CN114993242B (en) * 2022-06-17 2023-03-31 北京航空航天大学 Array POS installation deviation angle calibration method based on acceleration matching
CN115727875B (en) * 2022-11-29 2023-09-01 哈尔滨理工大学 Singular-free transfer alignment method based on modified Rodrigas parameter
CN117213480B (en) * 2023-09-19 2024-08-27 北京中捷时代航空科技有限公司 Transfer alignment method, system, equipment and storage medium

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105783943A (en) * 2016-04-26 2016-07-20 哈尔滨工程大学 Method for performing transfer alignment on large azimuth misalignment angle of ship in polar region environment based on unscented Kalman filtering
CN105973268B (en) * 2016-05-06 2019-09-27 哈尔滨工程大学 A kind of Transfer Alignment precision quantitative evaluating method based on the installation of cobasis seat

Also Published As

Publication number Publication date
CN107990910A (en) 2018-05-04

Similar Documents

Publication Publication Date Title
CN107990910B (en) Ship large azimuth misalignment angle transfer alignment method based on volume Kalman filtering
Chang et al. Initial alignment for a Doppler velocity log-aided strapdown inertial navigation system with limited information
WO2020062791A1 (en) Sins/dvl-based underwater anti-shaking alignment method for deep-sea underwater vehicle
CN107525503B (en) Adaptive cascade Kalman filtering method based on combination of dual-antenna GPS and MIMU
CN104655131B (en) Inertial navigation Initial Alignment Method based on ISTSSRCKF
Chang et al. Strapdown inertial navigation system initial alignment based on group of double direct spatial isometries
CN109000642A (en) A kind of improved strong tracking volume Kalman filtering Combinated navigation method
CN103471616B (en) Initial Alignment Method under a kind of moving base SINS Large azimuth angle condition
CN102486377B (en) Method for acquiring initial course attitude of fiber optic gyro strapdown inertial navigation system
CN105737823B (en) A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF
CN111102993A (en) Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system
CN103344260B (en) Based on the strapdown inertial navitation system (SINS) Initial Alignment of Large Azimuth Misalignment On method of RBCKF
CN108759870B (en) Novel robust generalized high-order cubature Kalman filtering based transfer alignment method
CN105783943A (en) Method for performing transfer alignment on large azimuth misalignment angle of ship in polar region environment based on unscented Kalman filtering
CN108981696B (en) Sins random misalignment angle non-singular rapid transfer alignment method
CN102654406A (en) Initial alignment method for moving bases based on combination of nonlinear prediction filtering and cubature Kalman filtering
CN103076026B (en) A kind of method determining Doppler log range rate error in SINS
CN109945895B (en) Inertial navigation initial alignment method based on fading smooth variable structure filtering
CN106895853B (en) Marine gyro compass alignment method assisted by electromagnetic log
CN103557864A (en) Initial alignment method for micro electro mechanical system (MEMS) strap-down inertial navigation adaptive square-root cubature Kalman filtering (SCKF)
CN105806363A (en) Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter)
JPS5936208B2 (en) Method and device for rapidly aligning an aircraft inertial platform
CN103791918A (en) Polar region moving base alignment method for naval vessel strapdown inertial navigation system
CN108731702B (en) Large misalignment angle transfer alignment method based on Huber method
CN108827288A (en) A kind of dimensionality reduction strapdown inertial navigation system Initial Alignment Method and system based on dual quaterion

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