CN107796388B - Relative attitude measurement method based on inertia technology - Google Patents

Relative attitude measurement method based on inertia technology Download PDF

Info

Publication number
CN107796388B
CN107796388B CN201610803806.3A CN201610803806A CN107796388B CN 107796388 B CN107796388 B CN 107796388B CN 201610803806 A CN201610803806 A CN 201610803806A CN 107796388 B CN107796388 B CN 107796388B
Authority
CN
China
Prior art keywords
matrix
theta
attitude
relative attitude
tied
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
CN201610803806.3A
Other languages
Chinese (zh)
Other versions
CN107796388A (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 CN201610803806.3A priority Critical patent/CN107796388B/en
Publication of CN107796388A publication Critical patent/CN107796388A/en
Application granted granted Critical
Publication of CN107796388B publication Critical patent/CN107796388B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
  • Navigation (AREA)

Abstract

The invention relates to a relative attitude measurement method, in particular to a relative attitude measurement method based on an inertia technology; according to the method, the calculation method is improved, the existing equipment of the ship is fully utilized, the attitude information of the ship inertial navigation equipment is matched with the relative attitude measuring device in the angular velocity, equivalent gyro drift is adopted in a filter model to replace gyro drift of the ship inertial navigation equipment and gyro drift of the relative attitude measuring device so as to reduce the dimension of the filter model, the limit of the calculation speed on the output frequency is weakened, and the complexity and the cost of the measuring device are reduced; by using the relative attitude measurement method based on the inertia technology, the measurement of the relative attitude can be realized in the normal movement process of the ship, no limitation is caused to the movement of the ship, and the movement adaptability is good; the invention does not need to install calibration and is simple and convenient to use; the relative attitude measuring device does not need to be subjected to inertial alignment when being started, has short preparation time and can realize measurement more quickly.

Description

Relative attitude measurement method based on inertia technology
Technical Field
The invention relates to a relative attitude measurement method, in particular to a relative attitude measurement method based on an inertia technology.
Background
For a larger ship, a plurality of devices are generally distributed and installed, wherein the plurality of devices need to use attitude information with accurate installation positions, and the attitude information is generally provided by the ship inertial navigation device directly or indirectly. However, the attitude information given by the ship inertial navigation device is the attitude information of the installation position of the ship inertial navigation device, and is not the attitude information of the installation positions of all the devices, and a certain difference exists between the two attitude information. However, due to the thermal and mechanical influences of ship movement, ship load changes, wind and sunlight, and the like, the deck is deformed. Therefore, the attitude information sent by the ship inertial navigation equipment to each ship-based equipment distributed on the whole ship has certain dynamic deviation with the real attitude information of the position of the ship-based equipment. For some high precision devices, these deviations must be measured by effective means to eliminate their effect.
In the past 60 years, researchers began to study the measurement of the relative attitude caused by the deformation of the deck of a ship, and early methods such as a polarized light energy measurement method, a dual-frequency polarized light method, a grating method, a large steel pipe reference method and the like can only measure the relative attitude under a static condition although the methods have higher measurement accuracy. The following liquid pressure measurement method, photogrammetry method, and the like can be used for dynamic measurement, but the real-time property is limited by the data output frequency of the measurement device.
Disclosure of Invention
Aiming at the prior art, the invention aims to provide a relative attitude measurement method based on an inertia technology, and solves the problem of measurement of the relative attitude of a ship.
In order to achieve the above object, the present invention adopts the following technical solutions.
The invention aims to provide a relative attitude measurement method based on an inertia technology, which comprises the following steps:
defining a coordinate system;
b1and (3) coordinate system: carrier coordinate system ox of ship inertial navigation equipmentb1yb1zb1Front upper right coordinate system, xb1Axis pointing to the right of the vessel, yb1Axis pointing towards the front of the ship, zb1The shaft points to the upper part of the ship;
b2and (3) coordinate system: relative attitude measuring device carrier coordinate system oxb2yb2zb2Front upper right coordinate system, xb2Right, y, of the axis-pointing measuring deviceb2Front of the axis-pointing measuring device, zb2The shaft points to the upper part of the measuring device;
n coordinate system: navigation coordinate system oxnynznNortheast geographic coordinate system, xnThe axis pointing east, ynThe axis pointing north, znThe axis points to the sky;
b1system b2The calculated coordinate systems of the system and the n system are respectively
Figure BDA0001109936980000021
And
Figure BDA0001109936980000022
note that the initial time is t0Establishing an inertial coordinate system i1、i2And n0Respectively consist of b1And b2Coordinate system is at t0The position of the moment solidifies into an inertial frame i1And i2，
Figure BDA0001109936980000023
At t is at0The position of the moment is frozen into an inertial frame n0，i1And i2The calculated coordinate system of the system is respectively
Figure BDA0001109936980000024
And
Figure BDA0001109936980000025
determining an attitude transformation matrix and initializing a Kalman filter;
note the book
Figure BDA0001109936980000026
Is tied to i1The attitude transformation matrix of the system is
Figure BDA0001109936980000027
Figure BDA0001109936980000028
Is tied to i2The attitude transformation matrix of the system is
Figure BDA0001109936980000029
t0Of time of day
Figure BDA00011099369800000210
And
Figure BDA00011099369800000211
an attitude transformation matrix;
recalculating in real time according to attitude information output by the ship inertial navigation equipment
Figure BDA00011099369800000212
Is tied to
Figure BDA00011099369800000213
Attitude transformation matrix of system
Figure BDA00011099369800000214
Figure BDA00011099369800000215
Attitude transformation matrix of time
Figure BDA00011099369800000216
Is that
Figure BDA00011099369800000217
The state vector X of the kalman filter is:
Figure BDA0001109936980000031
in the formula: phi is ═ phix Φy Φz]TIs t0Theta is a relative attitude angle vector caused by dynamic deformation of the ship body and is a unit rad;
Figure BDA0001109936980000032
is the differential of theta0Is t0Value of time thetaiFor i caused by gyro drift1And i2The Euler angle vector of the error of the two inertial systems, unit rad; epsilon0Is equivalent constant gyro drift, unit rad/s; epsilonrEquivalent gyroscope random drift is carried out in unit rad/s;
step two, updating time of the attitude matrix determined in the step one, and updating a Kalman filtering state transition matrix;
considering the relative attitude change caused by the dynamic deformation of the hull as a second-order markov process, the corresponding filter equation can be expressed as:
Figure BDA0001109936980000033
in the formula: j is x, y, z, and represents formula (2) as a system of equations of coordinate system x, y, z; mu.s2jIs a dynamic irregularity coefficient, λjIn order to dominate the frequency of the dynamic deformation,
Figure BDA0001109936980000034
Djas a noise intensity-related parameter, wj(t) is zero-mean unit Gaussian white noise;
dividing the gyro drift of the ship inertial navigation equipment and the relative attitude measuring device into constant gyro drift epsilon01、ε02And random gyro drift epsilonr1、εr2Two parts; wherein:
Figure BDA0001109936980000035
in the formula: epsilon01And ε02Respectively constant gyro drift of the ship inertial navigation equipment and a relative attitude measuring device in unit rad/s;
Figure BDA0001109936980000036
and
Figure BDA0001109936980000037
are respectively epsilon01And ε02The reciprocal of (a);
gyro random drift epsilon of device for approximating ship owner inertial navigation and relative attitude measurement by using first-order Markov modelr1And εr2And then:
Figure BDA0001109936980000038
in the formula: j' is 1x,1y,1z,2x,2y,2z, and expression (4) is an equation set of coordinate system 1x,1y,1z,2x,2y,2 z; mu.s1j′In order to obtain a first order markov coefficient,
Figure BDA0001109936980000041
for gyroscopic random drift epsilonrInverse of (e ∈)r＝εr2-εr1；σj′White noise, w, driven for the first-order Markov processj′(t) is zero-mean unit Gaussian white noise;
due to the presence of the gyro error,
Figure BDA0001109936980000042
and
Figure BDA0001109936980000043
will gradually deviate from the true
Figure BDA0001109936980000044
And
Figure BDA0001109936980000045
i.e. calculating the coordinate system
Figure BDA0001109936980000046
And
Figure BDA0001109936980000047
gradually deviating from the coordinate system b1And b2(ii) a By using
Figure BDA0001109936980000048
To respectively represent
Figure BDA0001109936980000049
Is a combination of b and1is a,
Figure BDA00011099369800000410
Is a combination of b and2an attitude transformation matrix between the systems; defining the corresponding attitude angle vectors as thetaε1And thetaε2；
In fact, can also be regarded as
Figure BDA00011099369800000411
Are disclosed and claimed
Figure BDA00011099369800000412
Is gradually deviated from i1A series of and i2Is a step of; by using
Figure BDA00011099369800000413
To respectively represent
Figure BDA00011099369800000414
Is a reaction of with i1Is a,
Figure BDA00011099369800000415
Is a reaction of with i2The attitude transformation matrix between the systems also represents the calculated deviation caused by the measurement error of the gyroscope, and the corresponding attitude angle vectors are respectively defined as thetai1And thetai2And then:
Figure BDA00011099369800000416
Figure BDA00011099369800000417
Figure BDA00011099369800000418
Figure BDA00011099369800000419
in the formula: thetaε1Is composed of
Figure BDA00011099369800000420
Is off from1Relative attitude angle vector of the system, unit rad; thetaε2Is composed of
Figure BDA00011099369800000421
Is off from2Relative attitude angle vector of the system, unit rad; thetai1Is composed of
Figure BDA00011099369800000422
Is off from i1Relative attitude angle vector of the system, unit rad; thetai2Is composed of
Figure BDA00011099369800000423
Is off from i2Relative attitude angle vector of the system, unit rad;
Figure BDA00011099369800000424
is b is1Is tied to i1The attitude transformation matrix of the system is obtained,
Figure BDA00011099369800000425
is b is2Is tied to i2A posture conversion matrix of the system;
Figure BDA00011099369800000426
and
Figure BDA00011099369800000427
are each thetai1And thetai2The reciprocal of (a); epsilon01And ε02Constant gyro drift in units of rad/s for ship inertial navigation equipment and a relative attitude measuring device; epsilonr1And εr2For ship inertial navigationRandom gyro drift, unit rad/s, of the device and relative attitude measurement means;
defining:
ε0＝ε02-ε01 (9)
εr＝εr2-εr1 (10)
θε＝θε2-θε1 (11)
θi＝θi2-θi1 (12)
in the formula: epsilon0Is equivalent constant gyro drift, unit rad/s; epsilon01And ε02Constant gyro drift, unit rad/s, for ship inertial navigation equipment and a relative attitude measurement device; epsilonrEquivalent gyroscope random drift is carried out in unit rad/s; epsilonr1And εr2Random gyro drift, unit rad/s, of the ship inertial navigation equipment and the relative attitude measurement device; thetaεCalculating a relative attitude angle vector, unit rad, caused by the deviation under the system b; thetaε1Is composed of
Figure BDA0001109936980000051
Is off from1Relative attitude angle vector of the system, unit rad; thetaε2Is composed of
Figure BDA0001109936980000052
Is off from2Relative attitude angle vector of the system, unit rad; thetaiCalculating a relative attitude angle vector, unit rad, for the i system; thetai1Is composed of
Figure BDA0001109936980000053
Is off from i1Relative attitude angle vector of the system, unit rad; thetai2Is composed of
Figure BDA0001109936980000054
Is off from i2Relative attitude angle vector of the system, unit rad;
in summary, the state matrix of the obtained filtering model is:
Figure BDA0001109936980000055
wherein:
Figure BDA0001109936980000056
Figure BDA0001109936980000057
Figure BDA0001109936980000058
in the formula:
Figure BDA0001109936980000059
Figure BDA00011099369800000510
and
Figure BDA00011099369800000511
are each beta2Components in the coordinate system x, y, z; mu.s2Is a dynamic irregularity coefficient, and λ is the dominant frequency of dynamic deformation; mu.s2x、μ2yAnd mu2zAre respectively mu2Components in the coordinate system x, y, z; mu.seFirst order Markov coefficient, μ, for random drift of an equivalent gyroex、μeyAnd muezAre respectively mueComponents in the coordinate system x, y, z; i is3×3Is a unit array;
the state noise matrix of the filtering model is:
Figure BDA0001109936980000061
wherein:
Figure BDA0001109936980000062
Figure BDA0001109936980000063
in the formula:
Figure BDA0001109936980000064
Figure BDA0001109936980000065
and
Figure BDA0001109936980000066
are each beta2Components in the coordinate system x, y, z; mu.s2Is a dynamic irregularity coefficient, and λ is the dominant frequency of dynamic deformation; mu.s2x、μ2yAnd mu2zAre respectively mu2Components in the coordinate system x, y, z; d is a noise intensity related parameter, Dx、DyAnd DzThe components of D in the coordinate system x, y, z, respectively; mu.seFirst order Markov coefficient, μ, for random drift of an equivalent gyroex、μeyAnd muezAre respectively mueComponents in the coordinate system x, y, z; sigmaeWhite noise, σ, driving the first order Markov process for equivalent gyro random driftex、σeyAnd σezAre respectively sigmaeComponents in the coordinate system x, y, z; (ii) a I is3×3Is a unit array;
the filter model state equation is then:
Figure BDA0001109936980000067
in the formula:
Figure BDA0001109936980000071
is the reciprocal of X; f is a state matrix; x is a state quantity; g is a state noise matrix; w is the system noise;
thirdly, calculating an observation value and updating an observation matrix;
the following holds true from the attitude matrix transformation relationship:
Figure BDA0001109936980000072
in the formula:
Figure BDA0001109936980000073
is time t
Figure BDA0001109936980000074
Is tied to i1A posture conversion matrix of the system;
Figure BDA0001109936980000075
is a unit attitude transformation matrix;
Figure BDA0001109936980000076
from t0Obtained by output and reconstruction of main inertial navigation attitude of ship at any moment
Figure BDA0001109936980000077
Is tied to
Figure BDA0001109936980000078
A posture conversion matrix of the system;
Figure BDA0001109936980000079
is composed of
Figure BDA00011099369800000710
Is tied to n0The attitude transformation matrix of the system is obtained by calculating the position information output by the main inertial navigation system and the current elapsed time in real time;
Figure BDA00011099369800000711
is composed of
Figure BDA00011099369800000712
Is tied to
Figure BDA00011099369800000713
The attitude transformation matrix of the system is obtained by reconstructing attitude information output by the ship main inertial navigation in real time;
Figure BDA00011099369800000714
is composed of
Figure BDA00011099369800000715
Is tied to i2The attitude conversion matrix of the system is updated in real time by utilizing the output angular rate of a gyroscope of the relative attitude measuring device;
the relative attitude at the time t is embodied in an attitude transformation matrix
Figure BDA00011099369800000716
Above, the following relationships exist:
Figure BDA00011099369800000717
in the formula:
Figure BDA00011099369800000718
is b is2Is tied to b1A posture conversion matrix of the system;
Figure BDA00011099369800000719
is b is1Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800000720
is i2Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800000721
is b is2Is tied to i2A posture conversion matrix of the system;
considering the error between the calculated coordinate system and the theoretical coordinate system, there are:
Figure BDA00011099369800000722
in the formula:
Figure BDA00011099369800000723
is b is1Is tied to
Figure BDA00011099369800000724
A posture conversion matrix of the system;
Figure BDA00011099369800000725
is b is2Is tied to b1A posture conversion matrix of the system;
Figure BDA00011099369800000726
is composed of
Figure BDA00011099369800000727
Is tied to b2A posture conversion matrix of the system;
Figure BDA00011099369800000728
is composed of
Figure BDA00011099369800000729
Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800000730
is i2Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800000731
is composed of
Figure BDA00011099369800000732
Is tied to i2A posture conversion matrix of the system;
let the relative attitude angle vector at time t be
Figure BDA00011099369800000733
Then t0The relative attitude angle vector of the moment is
Figure BDA00011099369800000734
PostureTransformation matrix
Figure BDA00011099369800000735
And
Figure BDA00011099369800000736
the corresponding attitude angle vectors are respectively thetaε1And thetaε2Then, the above-mentioned attitude angles are all small angles, neglect the second order above the small quantity should:
Figure BDA0001109936980000081
Figure BDA0001109936980000082
Figure BDA0001109936980000083
Figure BDA0001109936980000084
in the formula:
Figure BDA0001109936980000085
is b is2Is tied to b1A posture conversion matrix of the system; i is a unit array;
Figure BDA0001109936980000086
representing a vector
Figure BDA00011099369800000826
An antisymmetric matrix of (a);
Figure BDA0001109936980000087
is i2Is tied to i1A posture conversion matrix of the system; (ii) a
Figure BDA0001109936980000088
To representVector quantity
Figure BDA00011099369800000827
An antisymmetric matrix of (a);
Figure BDA0001109936980000089
is b is1Is tied to
Figure BDA00011099369800000810
A posture conversion matrix of the system; [ theta ] ofε1×]Represents the vector thetaε1An antisymmetric matrix of (a);
Figure BDA00011099369800000811
is composed of
Figure BDA00011099369800000812
Is tied to i2A posture conversion matrix of the system; [ theta ] ofε2×]Represents the vector thetaε2An antisymmetric matrix of (a);
the following equations (28) to (31) are substituted into a relative attitude relationship matrix relational expression (27) in consideration of the error of the calculation coordinate system:
Figure BDA00011099369800000813
finishing to obtain:
Figure BDA00011099369800000814
in the formula: i is a unit array; [ theta ] ofε1×]Represents the vector thetaε1An antisymmetric matrix of (a);
Figure BDA00011099369800000815
representing a vector
Figure BDA00011099369800000828
An antisymmetric matrix of (a); [ theta ] ofε2×]Represents the vector thetaε2An antisymmetric matrix of (a);
Figure BDA00011099369800000816
is composed of
Figure BDA00011099369800000817
Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800000818
representing a vector
Figure BDA00011099369800000829
An antisymmetric matrix of (a);
Figure BDA00011099369800000819
is composed of
Figure BDA00011099369800000820
Is tied to i2A posture conversion matrix of the system;
Figure BDA00011099369800000821
representing a vector
Figure BDA00011099369800000822
An antisymmetric matrix of (a);
if the two directional cosine matrices are equal, the above formula expansion will have 9 equations, and only 3 of the 9 equations are independent due to the constraint characteristic of the attitude euler angle;
if the attitude transformation matrix
Figure BDA00011099369800000823
Or
Figure BDA00011099369800000824
The corresponding Euler angle vector is theta ═ thetax θy θz]TDefining its attitude transformation matrix to be represented by theta, e.g. attitude transformation matrix
Figure BDA00011099369800000825
Expressed as:
Figure RE-GDA0001159008440000091
note the book
Figure BDA0001109936980000092
And
Figure BDA0001109936980000093
respectively as follows:
Figure BDA0001109936980000094
Figure BDA0001109936980000095
3 equations corresponding to the elements (1,2), (3,1) and (3,2) on the non-diagonal line in the formula (33) are selected to establish
Figure BDA0001109936980000096
And
Figure BDA0001109936980000097
the relationship of (1):
Figure BDA0001109936980000098
Figure BDA0001109936980000099
Figure BDA00011099369800000910
order:
Figure BDA00011099369800000911
Figure BDA00011099369800000912
Figure BDA00011099369800000913
then:
Figure RE-GDA0001159008440000101
in the formula:
Figure BDA0001109936980000102
is the relative attitude angle vector at time t,
Figure BDA0001109936980000103
is t0Relative attitude angle vector of time, attitude transformation matrix
Figure BDA0001109936980000104
And
Figure BDA0001109936980000105
the corresponding attitude angle vectors are respectively thetaε1And thetaε2；
The observation matrix is:
Figure BDA0001109936980000106
in the formula: h is an observation matrix;
Figure BDA0001109936980000107
is b is2Is tied to i2A posture conversion matrix of the system;
fourthly, performing Kalman filtering calculation;
discretizing a continuous Kalman filtering state equation and a system equation, and calculating through Kalman filtering to obtain related parameters of the state quantity X to obtain measurement of the relative attitude.
Figure BDA0001109936980000108
Figure BDA0001109936980000109
Figure BDA00011099369800001010
Figure BDA00011099369800001011
In the formula:
Figure BDA00011099369800001012
is epsilon0Inverse of (e ∈)0Is equivalent constant gyro drift, unit rad/s; j is x, y, z, and represents equation (14) as a system of equations of coordinate system x, y, z;
Figure BDA00011099369800001013
is epsilonrjThe reciprocal of (a); epsilonrjEquivalent gyroscope random drift is carried out in unit rad/s; mu.sejFirst order Markov coefficient, σ, for random drift of an equivalent gyroejWhite noise, w, driving the first order Markov process for equivalent gyro random driftej(t) is zero mean unit Gaussian white noise of the random drift of the equivalent gyroscope;
Figure BDA00011099369800001014
is thetaεReciprocal of (a), thetaεCalculating a relative attitude angle vector, unit rad, caused by the deviation under the system b;
Figure BDA00011099369800001015
is b is2Is tied to i2A posture conversion matrix of the system; thetaiComputing the deviation for i system resulting in a relative attitude angleAmount, unit rad;
Figure BDA00011099369800001016
is thetaiThe reciprocal of (a); epsilonrThe unit rad/s is equivalent gyro random drift.
Calculating by adopting a Kalman filtering calculation method after discretization in the fourth step:
and (3) time updating:
X(k+1/k)＝φ(k+1,k)X(k/k) (45)
P(k+1/k)＝φ(k+1,k)P(k/k)φT(k+1,k)+Q(k) (46)
in the formula: x (k +1/k) is a state prediction value of the next filtering period to the current filtering period; x (k/k) is a real-time state estimated value of the current filtering period; phi (k +1, k) is a state transition matrix; p (k +1/k) is a covariance matrix of error estimation of the next filtering period to the current filtering period; p (k/k) is a covariance matrix of error estimation of the current filtering period; phi is aT(k +1, k) is a transposed matrix of the phi (k +1, k) matrix; q (k) is a noise variance matrix;
measurement updating:
K(k+1)＝P(k+1/k)HT(k+1)[H(k+1)P(k+1/k)HT(k+1)+R(k+1)]-1 (47)
Figure BDA0001109936980000111
X(k+1/k+1)＝X(k+1/k)+K(k+1)(Z(k+1)-H(k+1)X(k+1/k)) (49)
in the formula: k (K +1) is a filter gain array of the next filter period; p (k +1/k) is a covariance matrix of error estimation of the next filtering period to the current filtering period; hT(K +1) is a transposed matrix of the matrix K (K + 1); r (k +1) is a measured noise variance matrix of the next filtering period; p (k +1/k +1) is a covariance matrix of error estimation of the next filtering period; x (k +1/k +1) the state estimation value of the next filtering period; z (k +1) next filtering period observation quantity matrix; h (k +1) is a next filtering period measurement matrix; and X (k +1/k) is a state predicted value of the next filtering period to the current filtering period.
The relative attitude measuring device includes: the device comprises a measuring module, an interface module, a control processor, a display, a keyboard and a power supply module;
the control processor is a core component of the relative attitude measuring device and is responsible for calculating and generating relative attitude information according to the obtained data information;
the interface module is used for transmitting the ship navigation information measured by the ship inertial navigation equipment to the control processor, and simultaneously outputting the calculation result of the control processor in the form of data information to the outside and providing the data information to required external equipment;
the measuring module comprises an orthogonal three-axis high-precision gyroscope and is used for measuring the angular velocity information of the installation position of the relative attitude measuring device;
the keyboard and the display provide input and output of information;
the power supply module is connected with an external power supply, is converted into a power supply form required by each component in the relative attitude measuring device, and supplies power to each module of the relative attitude measuring device.
B is ensured as much as possible when the relative attitude measuring device is installed at the position to be measured2Is a combination of b and1are corresponding to each other in the axial direction and have a smaller included angle; and a power supply module and an interface module of the relative attitude measuring device are respectively connected with a power supply cable and a signal cable of the ship inertial navigation equipment.
The technical scheme provided by the embodiment of the invention has the following beneficial effects:
by using the relative attitude measurement method based on the inertia technology, the measurement of the relative attitude can be realized in the normal movement process of the ship, no limitation is caused to the movement of the ship, and the movement adaptability is good.
The invention does not need to install calibration and is simple and convenient to use; the relative attitude measuring device does not need to be subjected to inertial alignment when being started, has short preparation time and can realize measurement more quickly.
More importantly, compared with two sets of inertial navigation equipment or two sets of orthogonal triaxial gyroscopes used for other inertia matching relative attitude measurement devices, the relative attitude measurement method and device provided by the invention only use one set of orthogonal triaxial high-precision gyroscopes, and have the advantages of simple equipment and low cost; through model optimization in the processor calculation method, the calculation amount is reduced, and the limit of the calculation speed on the output frequency is weakened.
Drawings
FIG. 1 is a schematic diagram of a relative attitude measurement apparatus according to an embodiment of the present invention;
FIG. 2 is a schematic view of an installation of a relative attitude measurement device according to an embodiment of the present invention;
Detailed Description
The following describes a relative attitude measurement method based on inertial technology in detail with reference to specific embodiments.
The invention relates to a relative attitude measurement method based on an inertia technology, which comprises the following steps:
a coordinate system;
b1and (3) coordinate system: carrier coordinate system ox of ship inertial navigation equipmentb1yb1zb1Front upper right coordinate system, xb1Axis pointing to the right of the vessel, yb1Axis pointing towards the front of the ship, zb1The shaft points to the upper part of the ship;
b2and (3) coordinate system: relative attitude measuring device carrier coordinate system oxb2yb2zb2Front upper right coordinate system, xb2Right, y, of the axis-pointing measuring deviceb2Front of the axis-pointing measuring device, zb2The shaft points to the upper part of the measuring device;
and (2) an N coordinate system: navigation coordinate system oxnynznNortheast geographic coordinate system, xnThe axis pointing east, ynThe axis pointing north, znThe axis points to the sky;
b1system b2The calculated coordinate systems of the system and the n system are respectively
Figure BDA0001109936980000131
And
Figure BDA0001109936980000132
note that the initial time is t0Establishing inertial coordinatesIs i of1、i2And n0Respectively consist of b1And b2Coordinate system is at t0The position of the moment solidifies into an inertial frame i1And i2，
Figure BDA0001109936980000133
At t is at0The position of the moment is frozen into an inertial frame n0，i1And i2The calculated coordinate system of the system is respectively
Figure BDA0001109936980000134
And
Figure BDA0001109936980000135
as shown in fig. 1, the relative posture measuring apparatus includes: the device comprises a measuring module, an interface module, a control processor, a display, a keyboard and a power supply module;
the control processor is a core component of the relative attitude measuring device and is responsible for calculating and generating relative attitude information according to the obtained data information;
the interface module is used for transmitting the ship navigation information measured by the ship inertial navigation equipment to the control processor, and simultaneously outputting the calculation result of the control processor in the form of data information to the outside and providing the data information to required external equipment;
the measuring module comprises an orthogonal three-axis high-precision gyroscope and is used for measuring the angular velocity information of the installation position of the relative attitude measuring device;
the keyboard and the display provide input and output of information;
the power supply module is connected with an external power supply, is converted into a power supply form required by each component in the relative attitude measuring device and supplies power to each module of the relative attitude measuring device;
mounting the relative attitude measuring device at the position to be measured, and ensuring b as much as possible2Is a combination of b and1are corresponding to each other in the axial direction and have a smaller included angle; and a power supply module and an interface module of the relative attitude measuring device are respectively connected with a power supply cable and a signal cable of the ship inertial navigation equipment.
Determining an attitude transformation matrix and initializing a Kalman filter;
note the book
Figure BDA0001109936980000141
Is tied to i1The attitude transformation matrix of the system is
Figure BDA0001109936980000142
Figure BDA0001109936980000143
Is tied to i2The attitude transformation matrix of the system is
Figure BDA0001109936980000144
Then according to the process of establishing the coordinate system, t0Of time of day
Figure BDA0001109936980000145
And
Figure BDA0001109936980000146
converting a matrix for a unit attitude;
according to the attitude information output by the ship inertial navigation equipment, the attitude information can be recalculated in real time
Figure BDA0001109936980000147
Is tied to
Figure BDA0001109936980000148
Attitude transformation matrix of system
Figure BDA0001109936980000149
t0Attitude transformation matrix of time
Figure BDA00011099369800001410
Is that
Figure BDA00011099369800001411
The state vector X of the kalman filter is:
Figure BDA00011099369800001412
in the formula: phi is ═ phix Φy Φz]TIs t0Theta is a relative attitude angle vector caused by dynamic deformation of the ship body and is a unit rad;
Figure BDA00011099369800001413
is the differential of theta0Is t0Value of time thetaiFor i caused by gyro drift1And i2The Euler angle vector of the error of the two inertial systems, unit rad; epsilon0Is equivalent constant gyro drift, unit rad/s; epsilonrEquivalent gyroscope random drift is carried out in unit rad/s;
step two, updating time of the attitude matrix determined in the step one, and updating a Kalman filtering state transition matrix;
considering the relative attitude change caused by dynamic deformation of the hull as a second order Markov (Markov) process, its corresponding filter equation can be expressed as:
Figure BDA0001109936980000151
in the formula: j is x, y, z, and represents formula (2) as a system of equations of coordinate system x, y, z; mu.s2jIs a dynamic irregularity coefficient, λjIn order to dominate the frequency of the dynamic deformation,
Figure BDA0001109936980000152
Djas a noise intensity-related parameter, wj(t) is zero-mean unit Gaussian white noise;
dividing the gyro drift of the ship inertial navigation equipment and the relative attitude measuring device into constant gyro drift epsilon01、ε02And random gyro drift epsilonr1、εr2Two parts; wherein:
Figure BDA0001109936980000153
in the formula: epsilon01And ε02Respectively constant gyro drift of the ship inertial navigation equipment and a relative attitude measuring device in unit rad/s;
Figure BDA0001109936980000154
and
Figure BDA0001109936980000155
are respectively epsilon01And ε02The reciprocal of (a);
gyro random drift epsilon of device for approximating ship main inertial navigation and relative attitude measurement by using first-order Markov modelr1And εr2And then:
Figure BDA0001109936980000156
in the formula: j' is 1x,1y,1z,2x,2y,2z, and expression (4) is an equation set of coordinate system 1x,1y,1z,2x,2y,2 z; mu.s1j′In order to be a first-order Markov coefficient,
Figure BDA0001109936980000157
for gyroscopic random drift epsilonrInverse of (e ∈)r＝εr2-εr1；σj′White drive noise for the first-order Markov process, wj′(t) is zero-mean unit Gaussian white noise;
due to the presence of the gyro error,
Figure BDA0001109936980000158
and
Figure BDA0001109936980000159
will gradually deviate from the true
Figure BDA00011099369800001510
And
Figure BDA00011099369800001511
i.e. calculating the coordinate system
Figure BDA00011099369800001512
And
Figure BDA00011099369800001513
gradually deviating from the coordinate system b1And b2(ii) a By using
Figure BDA00011099369800001514
To respectively represent
Figure BDA00011099369800001515
Is a combination of b and1is a,
Figure BDA00011099369800001516
Is a combination of b and2an attitude transformation matrix between the systems; defining the corresponding attitude angle vectors as thetaε1And thetaε2；
In fact, can also be regarded as
Figure BDA0001109936980000161
Are disclosed and claimed
Figure BDA0001109936980000162
Is gradually deviated from i1A series of and i2Is a step of; by using
Figure BDA0001109936980000163
To respectively represent
Figure BDA0001109936980000164
Is a reaction of with i1Is a,
Figure BDA0001109936980000165
Is a reaction of with i2The attitude transformation matrix between the systems also represents the calculated deviation caused by the measurement error of the gyroscope, and the corresponding attitude angle vectors are respectively defined as thetai1And thetai2And then:
Figure BDA0001109936980000166
Figure BDA0001109936980000167
Figure BDA0001109936980000168
Figure BDA0001109936980000169
in the formula: thetaε1Is composed of
Figure BDA00011099369800001610
Is off from1Relative attitude angle vector of the system, unit rad; thetaε2Is composed of
Figure BDA00011099369800001611
Is off from2Relative attitude angle vector of the system, unit rad; thetai1Is composed of
Figure BDA00011099369800001612
Is off from i1Relative attitude angle vector of the system, unit rad; thetai2Is composed of
Figure BDA00011099369800001613
Is off from i2Relative attitude angle vector of the system, unit rad;
Figure BDA00011099369800001614
is b is1Is tied to i1The attitude transformation matrix of the system is obtained,
Figure BDA00011099369800001615
is b is2Is tied to i2A posture conversion matrix of the system;
Figure BDA00011099369800001616
and
Figure BDA00011099369800001617
are each thetai1And thetai2The reciprocal of (a); epsilon01And ε02Constant gyro drift in units of rad/s for ship inertial navigation equipment and a relative attitude measuring device; epsilonr1And εr2Random gyro drift, unit rad/s, for ship inertial navigation equipment and relative attitude measurement devices;
defining:
ε0＝ε02-ε01 (9)
εr＝εr2-εr1 (10)
θε＝θε2-θε1 (11)
θi＝θi2-θi1 (12)
in the formula: epsilon0Is equivalent constant gyro drift, unit rad/s; epsilon01And ε02Constant gyro drift, unit rad/s, for ship inertial navigation equipment and a relative attitude measurement device; epsilonrEquivalent gyroscope random drift is carried out in unit rad/s; epsilonr1And εr2Random gyro drift, unit rad/s, of the ship inertial navigation equipment and the relative attitude measurement device; thetaεCalculating a relative attitude angle vector, unit rad, caused by the deviation under the system b; thetaε1Is composed of
Figure BDA0001109936980000171
Is off from1Relative attitude angle vector of the system, unit rad; thetaε2Is composed of
Figure BDA0001109936980000172
Is off from2Relative attitude angle vector of the system, unit rad; thetaiCalculating a relative attitude angle vector, unit rad, for the i system; thetai1Is composed of
Figure BDA0001109936980000173
Is off from i1Relative attitude angle vector of the system, unit rad; thetai2Is composed of
Figure BDA0001109936980000174
Is off from i2Relative attitude angle vector of the system, unit rad;
then:
Figure BDA0001109936980000175
Figure BDA0001109936980000176
Figure BDA0001109936980000177
Figure BDA0001109936980000178
in the formula:
Figure BDA0001109936980000179
is epsilon0Inverse of (e ∈)0Is equivalent constant gyro drift, unit rad/s; j is x, y, z, and represents equation (14) as a system of equations of coordinate system x, y, z;
Figure BDA00011099369800001710
is epsilonrjThe reciprocal of (a); epsilonrjEquivalent gyroscope random drift is carried out in unit rad/s; mu.sejFirst order Markov coefficient, σ, for random drift of an equivalent gyroscopeejWhite drive noise, w, for the first-order Markov process of equivalent gyro random driftej(t) is zero mean unit Gaussian white noise of the random drift of the equivalent gyroscope;
Figure BDA00011099369800001711
is thetaεReciprocal of (a), thetaεCalculating the deviation under b to result in a relative attitude angle vectorUnit rad;
Figure BDA00011099369800001712
is b is2Is tied to i2A posture conversion matrix of the system; thetaiCalculating a relative attitude angle vector, unit rad, for the i system;
Figure BDA00011099369800001713
is thetaiThe reciprocal of (a); epsilonrEquivalent gyroscope random drift is carried out in unit rad/s;
in the formulas (14) to (16), the gyro characteristic parameters of the ship-borne inertial navigation are approximately replaced by the gyro characteristic parameters of the relative attitude measurement device, the precision of the ship-borne inertial navigation gyro is generally high, and the replacement does not influence the estimation of the random drift of the equivalent gyro; by using
Figure BDA00011099369800001714
Approximately replace
Figure BDA00011099369800001715
The substitute error is a second-order small quantity, and the error can be ignored;
in summary, the state matrix of the filter model can be obtained as follows:
Figure BDA0001109936980000181
wherein:
Figure BDA0001109936980000182
Figure BDA0001109936980000183
Figure BDA0001109936980000184
in the formula:
Figure BDA0001109936980000185
Figure BDA0001109936980000186
and
Figure BDA0001109936980000187
are each beta2Components in the coordinate system x, y, z; mu.s2Is a dynamic irregularity coefficient, and λ is the dominant frequency of dynamic deformation; mu.s2x、μ2yAnd mu2zAre respectively mu2Components in the coordinate system x, y, z; mu.seFirst order Markov coefficient, μ, for equivalent gyro random driftex、μeyAnd muezAre respectively mueComponents in the coordinate system x, y, z; i is3×3Is a unit array;
the state noise matrix of the filtering model is:
Figure BDA0001109936980000188
wherein:
Figure RE-GDA0001159008440000191
Figure BDA0001109936980000192
in the formula:
Figure BDA0001109936980000193
Figure BDA0001109936980000194
and
Figure BDA0001109936980000195
are each beta2Components in the coordinate system x, y, z; mu.s2For dynamic irregularity coefficients, λ is the dominant frequency of dynamic deformation；μ2x、μ2yAnd mu2zAre respectively mu2Components in the coordinate system x, y, z; d is a noise intensity related parameter, Dx、DyAnd DzThe components of D in the coordinate system x, y, z, respectively; mu.seFirst order Markov coefficient, μ, for equivalent gyro random driftex、μeyAnd muezAre respectively mueComponents in the coordinate system x, y, z; sigmaeWhite driving noise, σ, of the first-order Markov process for equivalent gyro random driftex、σeyAnd σezAre respectively sigmaeComponents in the coordinate system x, y, z; (ii) a I is3×3Is a unit array;
the filter model state equation is then:
Figure BDA0001109936980000196
in the formula:
Figure BDA0001109936980000197
is the reciprocal of X; f is a state matrix; x is a state quantity; g is a state noise matrix; w is the system noise;
thirdly, calculating an observation value and updating an observation matrix;
the following holds true from the attitude matrix transformation relationship:
Figure BDA0001109936980000198
in the formula:
Figure BDA0001109936980000199
is time t
Figure BDA00011099369800001910
Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800001911
is a unit attitude transformation matrix;
Figure BDA00011099369800001912
can be formed by t0Obtained by output and reconstruction of main inertial navigation attitude of ship at any moment
Figure BDA00011099369800001913
Is tied to
Figure BDA00011099369800001914
A posture conversion matrix of the system;
Figure BDA00011099369800001915
is composed of
Figure BDA00011099369800001916
Is tied to n0The attitude transformation matrix of the system can be obtained by calculating the position information output by the main inertial navigation system and the current elapsed time in real time;
Figure BDA00011099369800001917
is composed of
Figure BDA00011099369800001918
Is tied to
Figure BDA00011099369800001919
The attitude transformation matrix of the system is obtained by reconstructing attitude information output by the ship main inertial navigation in real time;
Figure BDA0001109936980000201
is composed of
Figure BDA0001109936980000202
Is tied to i2The attitude conversion matrix of the system can be updated in real time by utilizing the output angular rate of the gyroscope of the relative attitude measuring device;
the relative attitude at the time t is embodied in an attitude transformation matrix
Figure BDA0001109936980000203
Above, the following relationships exist:
Figure BDA0001109936980000204
in the formula:
Figure BDA0001109936980000205
is b is2Is tied to b1A posture conversion matrix of the system;
Figure BDA0001109936980000206
is b is1Is tied to i1A posture conversion matrix of the system;
Figure BDA0001109936980000207
is i2Is tied to i1A posture conversion matrix of the system;
Figure BDA0001109936980000208
is b is2Is tied to i2A posture conversion matrix of the system;
considering the error between the calculated coordinate system and the theoretical coordinate system, there are:
Figure BDA0001109936980000209
in the formula:
Figure BDA00011099369800002010
is b is1Is tied to
Figure BDA00011099369800002011
A posture conversion matrix of the system;
Figure BDA00011099369800002012
is b is2Is tied to b1A posture conversion matrix of the system;
Figure BDA00011099369800002013
is composed of
Figure BDA00011099369800002014
Is tied to b2A posture conversion matrix of the system;
Figure BDA00011099369800002015
is composed of
Figure BDA00011099369800002016
Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800002017
is i2Is tied to i1A posture conversion matrix of the system;
Figure BDA00011099369800002018
is composed of
Figure BDA00011099369800002019
Is tied to i2A posture conversion matrix of the system;
let the relative attitude angle vector at time t be
Figure BDA00011099369800002020
Then t0The relative attitude angle vector of the moment is
Figure BDA00011099369800002021
Attitude transformation matrix
Figure BDA00011099369800002022
And
Figure BDA00011099369800002023
the corresponding attitude angle vectors are respectively thetaε1And thetaε2Then, the above-mentioned attitude angles are all small angles, neglect the second order above the small quantity should:
Figure BDA00011099369800002024
Figure BDA00011099369800002025
Figure BDA00011099369800002026
Figure BDA00011099369800002027
in the formula:
Figure BDA00011099369800002028
is b is2Is tied to b1A posture conversion matrix of the system; i is a unit array;
Figure BDA00011099369800002029
representing a vector
Figure BDA00011099369800002036
An antisymmetric matrix of (a);
Figure BDA00011099369800002030
is i2Is tied to i1A posture conversion matrix of the system; (ii) a
Figure BDA00011099369800002031
Representing a vector
Figure BDA00011099369800002037
An antisymmetric matrix of (a);
Figure BDA00011099369800002032
is b is1Is tied to
Figure BDA00011099369800002033
A posture conversion matrix of the system; [ theta ] ofε1×]Represents the vector thetaε1An antisymmetric matrix of (a);
Figure BDA00011099369800002034
is composed of
Figure BDA00011099369800002035
Is tied to i2A posture conversion matrix of the system; [ theta ] ofε2×]Represents the vector thetaε2An antisymmetric matrix of (a);
the following equations (28) to (31) are substituted into a relative attitude relationship matrix relational expression (27) in consideration of the error of the calculation coordinate system:
Figure BDA0001109936980000211
finishing to obtain:
Figure BDA0001109936980000212
in the formula: i is a unit array; [ theta ] ofε1×]Represents the vector thetaε1An antisymmetric matrix of (a);
Figure BDA0001109936980000213
representing a vector
Figure BDA00011099369800002119
An antisymmetric matrix of (a); [ theta ] ofε2×]Represents the vector thetaε2An antisymmetric matrix of (a);
Figure BDA0001109936980000214
is composed of
Figure BDA0001109936980000215
Is tied to i1A posture conversion matrix of the system;
Figure BDA0001109936980000216
representing a vector
Figure BDA00011099369800002120
An antisymmetric matrix of (a);
Figure BDA0001109936980000217
is composed of
Figure BDA0001109936980000218
Is tied to i2A posture conversion matrix of the system;
Figure BDA0001109936980000219
representing a vector
Figure BDA00011099369800002110
An antisymmetric matrix of (a);
equation (33) is that the two directional cosine matrices are equal, which means that there will be 9 equations to expand the above equation, but in practice only 3 of the 9 equations are independent due to the constraint characteristic of the attitude euler angle itself;
if the attitude transformation matrix
Figure BDA00011099369800002111
Or
Figure BDA00011099369800002112
The corresponding Euler angle vector is theta ═ thetax θy θz]TDefining its attitude transformation matrix to be represented by theta, e.g. attitude transformation matrix
Figure BDA00011099369800002113
Expressed as:
Figure BDA00011099369800002114
note the book
Figure BDA00011099369800002115
And
Figure BDA00011099369800002116
respectively as follows:
Figure BDA00011099369800002117
Figure BDA00011099369800002118
3 equations corresponding to the elements (1,2), (3,1) and (3,2) on the non-diagonal line in the formula (33) are selected to establish
Figure BDA0001109936980000221
And
Figure BDA0001109936980000222
the relationship of (1):
Figure BDA0001109936980000223
Figure BDA0001109936980000224
Figure BDA0001109936980000225
order:
Figure BDA0001109936980000226
Figure BDA0001109936980000227
Figure BDA0001109936980000228
then:
Figure BDA0001109936980000229
in the formula:
Figure BDA00011099369800002210
phase at time tFor the attitude angle vector,
Figure BDA00011099369800002211
is t0Relative attitude angle vector of time, attitude transformation matrix
Figure BDA00011099369800002212
And
Figure BDA00011099369800002213
the corresponding attitude angle vectors are respectively thetaε1And thetaε2；
The observation matrix is:
Figure BDA00011099369800002214
in the formula: h is an observation matrix;
Figure BDA00011099369800002215
is b is2Is tied to i2A posture conversion matrix of the system;
fourthly, performing Kalman filtering calculation;
firstly, discretizing a continuous Kalman filtering state equation and a system equation, and calculating by adopting the following Kalman filtering calculation method after discretization:
and (3) time updating:
X(k+1/k)＝φ(k+1,k)X(k/k) (45)
P(k+1/k)＝φ(k+1,k)P(k/k)φT(k+1,k)+Q(k) (46)
in the formula: x (k +1/k) is a state prediction value of the next filtering period to the current filtering period; x (k/k) is a real-time state estimated value of the current filtering period; phi (k +1, k) is a state transition matrix; p (k +1/k) is a covariance matrix of error estimation of the next filtering period to the current filtering period; p (k/k) is a covariance matrix of error estimation of the current filtering period; phi is aT(k +1, k) is a transposed matrix of the phi (k +1, k) matrix; q (k) is a noise variance matrix;
measurement updating:
K(k+1)＝P(k+1/k)HT(k+1)[H(k+1)P(k+1/k)HT(k+1)+R(k+1)]-1 (47)
Figure BDA0001109936980000231
X(k+1/k+1)＝X(k+1/k)+K(k+1)(Z(k+1)-H(k+1)X(k+1/k)) (49)
in the formula: k (K +1) is a filter gain array of the next filter period; p (k +1/k) is a covariance matrix of error estimation of the next filtering period to the current filtering period; hT(K +1) is a transposed matrix of the matrix K (K + 1); r (k +1) is a measured noise variance matrix of the next filtering period; p (k +1/k +1) is a covariance matrix of error estimation of the next filtering period; x (k +1/k +1) the state estimation value of the next filtering period; z (k +1) next filtering period observation quantity matrix; h (k +1) is a next filtering period measurement matrix; x (k +1/k) is a state prediction value of the next filtering period to the current filtering period;
and performing Kalman filtering calculation to obtain the relevant parameters of the state quantity X and obtain the measurement of the relative attitude.

Claims (5)

1. A relative attitude measurement method based on an inertial technology is characterized by comprising the following steps:
defining a coordinate system;
b1and (3) coordinate system: carrier coordinate system ox of ship inertial navigation equipmentb1yb1zb1Front upper right coordinate system, xb1Axis pointing to the right of the vessel, yb1Axis pointing towards the front of the ship, zb1The shaft points to the upper part of the ship;
b2and (3) coordinate system: relative attitude measuring device carrier coordinate system oxb2yb2zb2Front upper right coordinate system, xb2Right, y, of the axis-pointing measuring deviceb2Front of the axis-pointing measuring device, zb2The shaft points to the upper part of the measuring device;
n coordinate system: navigation coordinate system oxnynznNortheast geographic coordinate system, xnThe axis pointing east, ynShaft fingerTo the north, znThe axis points to the sky;
b1system b2The calculated coordinate systems of the system and the n system are respectively
Figure FDA0002829410060000011
And
Figure FDA0002829410060000012
note that the initial time is t0Establishing an inertial coordinate system i1、i2And n0Respectively consist of b1And b2Coordinate system is at t0The position of the moment solidifies into an inertial frame i1And i2，
Figure FDA0002829410060000013
At t is at0The position of the moment is frozen into an inertial frame n0，i1And i2The calculated coordinate system of the system is respectively
Figure FDA0002829410060000014
And
Figure FDA0002829410060000015
determining an attitude transformation matrix and initializing a Kalman filter;
note the book
Figure FDA0002829410060000016
Is tied to i1The attitude transformation matrix of the system is
Figure FDA0002829410060000017
Figure FDA0002829410060000018
Is tied to i2The attitude transformation matrix of the system is
Figure FDA0002829410060000019
t0Of time of day
Figure FDA00028294100600000110
And
Figure FDA00028294100600000111
an attitude transformation matrix;
recalculating in real time according to attitude information output by the ship inertial navigation equipment
Figure FDA00028294100600000112
Is tied to
Figure FDA00028294100600000113
Attitude transformation matrix t of system0The attitude transformation matrix at the moment is the state vector X of the Kalman filter and is as follows:
Figure FDA00028294100600000114
in the formula: phi is ═ phix Φy Φz]TIs t0Theta is a relative attitude angle vector caused by dynamic deformation of the ship body and is a unit rad;
Figure FDA00028294100600000115
is the differential of theta0Is t0Value of time thetaiFor i caused by gyro drift1And i2The Euler angle vector of the error of the two inertial systems, unit rad; epsilon0Is equivalent constant gyro drift, unit rad/s; epsilonrEquivalent gyroscope random drift is carried out in unit rad/s;
step two, updating time of the attitude transformation matrix determined in the step one, and updating a Kalman filtering state transition matrix;
considering the relative attitude change caused by the dynamic deformation of the hull as a second-order markov process, the corresponding filter equation can be expressed as:
Figure FDA00028294100600000116
in the formula: j is x, y, z, and represents formula (2) as a system of equations of coordinate system x, y, z; mu.s2jIs a dynamic irregularity coefficient, λjIn order to dominate the frequency of the dynamic deformation,
Figure FDA00028294100600000117
Djas a noise intensity-related parameter, wj(t) is zero-mean unit Gaussian white noise; dividing the gyro drift of the ship inertial navigation equipment and the relative attitude measuring device into constant gyro drift epsilon01、ε02And gyro random drift epsilonr1、εr2Two parts;
wherein:
Figure FDA00028294100600000118
in the formula: epsilon01And ε02Respectively constant gyro drift of the ship inertial navigation equipment and a relative attitude measuring device in unit rad/s;
Figure FDA00028294100600000119
and
Figure FDA00028294100600000120
are respectively epsilon01And ε02The reciprocal of (a);
gyro random drift epsilon of device for approximating ship owner inertial navigation and relative attitude measurement by using first-order Markov modelr1And εr2And then:
Figure FDA0002829410060000021
in the formula: j ═ 1x,1y,1z,2x,2y,2z, and represents a formula (4) ofA set of equations for coordinate systems 1x,1y,1z,2x,2y,2 z; mu.s1j′In order to obtain a first order markov coefficient,
Figure FDA0002829410060000022
for random drift of equivalent gyro epsilonrInverse of (e ∈)r＝εr2-εr1；σj′White noise, w, driven for the first-order Markov processj′(t) is zero-mean unit Gaussian white noise;
due to the presence of the gyro error,
Figure FDA0002829410060000023
and
Figure FDA0002829410060000024
will gradually deviate from the true
Figure FDA0002829410060000025
And
Figure FDA0002829410060000026
i.e. calculating the coordinate system
Figure FDA0002829410060000027
And
Figure FDA0002829410060000028
gradually deviating from the coordinate system b1And b2(ii) a By using
Figure FDA0002829410060000029
To respectively represent
Figure FDA00028294100600000210
Is a combination of b and1is a,
Figure FDA00028294100600000211
Is a combination of b and2an attitude transformation matrix between the systems; corresponding posture thereofThe angular vectors are respectively defined as thetaε1And thetaε2；
Due to the presence of the gyro error,
Figure FDA00028294100600000212
and
Figure FDA00028294100600000213
will gradually deviate from the true
Figure FDA00028294100600000214
And
Figure FDA00028294100600000215
can also be regarded as
Figure FDA00028294100600000216
Are disclosed and claimed
Figure FDA00028294100600000217
Is gradually deviated from i1A series of and i2Is a step of; by using
Figure FDA00028294100600000218
To respectively represent
Figure FDA00028294100600000219
Is a reaction of with i1Is a,
Figure FDA00028294100600000220
Is a reaction of with i2The attitude transformation matrix between the systems also represents the calculated deviation caused by the measurement error of the gyroscope, and the corresponding attitude angle vectors are respectively defined as thetai1And thetai2And then:
Figure FDA00028294100600000221
Figure FDA00028294100600000222
Figure FDA00028294100600000223
Figure FDA00028294100600000224
in the formula: thetaε1Is composed of
Figure FDA00028294100600000225
Is off from1Relative attitude angle vector of the system, unit rad; thetaε2Is composed of
Figure FDA00028294100600000226
Is off from2Relative attitude angle vector of the system, unit rad; thetai1Is composed of
Figure FDA00028294100600000227
Is off from i1Relative attitude angle vector of the system, unit rad; thetai2Is composed of
Figure FDA00028294100600000228
Is off from i2Relative attitude angle vector of the system, unit rad;
Figure FDA00028294100600000229
is b is1Is tied to i1The attitude transformation matrix of the system is obtained,
Figure FDA00028294100600000230
is b is2Is tied to i2A posture conversion matrix of the system;
Figure FDA00028294100600000231
and
Figure FDA00028294100600000232
are each thetai1And thetai2The reciprocal of (a); defining:
ε0＝ε02-ε01 (9)
εr＝εr2-εr1 (10)
θε＝θε2-θε1 (11)
θi＝θi2-θi1 (12)
in the formula: thetaεCalculating a relative attitude angle vector, unit rad, caused by the deviation under the system b; thetaε1Is composed of
Figure FDA0002829410060000031
Is off from1Relative attitude angle vector of the system, unit rad; thetaε2Is composed of
Figure FDA0002829410060000032
Is off from2Relative attitude angle vector of the system, unit rad; thetaiCalculating a relative attitude angle vector, unit rad, for the i system; thetai1Is composed of
Figure FDA0002829410060000033
Is off from i1Relative attitude angle vector of the system, unit rad; thetai2Is composed of
Figure FDA0002829410060000034
Is off from i2Relative attitude angle vector of the system, unit rad;
in summary, the state matrix of the obtained filtering model is:
Figure FDA0002829410060000035
wherein:
Figure FDA0002829410060000036
Figure FDA0002829410060000037
Figure FDA0002829410060000038
in the formula:
Figure FDA0002829410060000039
Figure FDA00028294100600000310
and
Figure FDA00028294100600000311
are each beta2Components in the coordinate system x, y, z; mu.s2Is a dynamic irregularity coefficient, and λ is the dominant frequency of dynamic deformation; mu.s2x、μ2yAnd mu2zAre respectively mu2Components in the coordinate system x, y, z; mu.seFirst order Markov coefficient, μ, for random drift of an equivalent gyroex、μeyAnd muezAre respectively mueComponents in the coordinate system x, y, z; i is3×3Is a unit array;
the state noise matrix of the filtering model is:
Figure FDA0002829410060000041
wherein:
Figure FDA0002829410060000042
Figure FDA0002829410060000043
in the formula: d is a noise intensity related parameter, Dx、DyAnd DzThe components of D in the coordinate system x, y, z, respectively; sigmaeWhite noise, σ, driving the first order Markov process for equivalent gyro random driftex、σeyAnd σezAre respectively sigmaeComponents in the coordinate system x, y, z; the filter model state equation is then:
Figure FDA0002829410060000044
in the formula:
Figure FDA0002829410060000045
is the reciprocal of X; f is a state matrix; x is a state vector; g is a state noise matrix; w is the system noise;
thirdly, calculating an observation value and updating an observation matrix;
the following holds true from the attitude matrix transformation relationship:
Figure FDA0002829410060000046
in the formula:
Figure FDA0002829410060000047
is time t
Figure FDA0002829410060000048
Is tied to i1A posture conversion matrix of the system;
Figure FDA0002829410060000049
is a unit postureA state transformation matrix;
Figure FDA00028294100600000410
from t0Obtained by output and reconstruction of main inertial navigation attitude of ship at any moment
Figure FDA00028294100600000411
Is tied to
Figure FDA00028294100600000412
A posture conversion matrix of the system;
Figure FDA00028294100600000413
is composed of
Figure FDA00028294100600000414
Is tied to n0The attitude transformation matrix of the system is obtained by calculating the position information output by the main inertial navigation system and the current elapsed time in real time;
Figure FDA00028294100600000415
is composed of
Figure FDA00028294100600000416
Is tied to
Figure FDA00028294100600000417
The attitude transformation matrix of the system is obtained by reconstructing attitude information output by the ship main inertial navigation in real time;
Figure FDA0002829410060000051
is composed of
Figure FDA0002829410060000052
Is tied to i2The attitude conversion matrix of the system is updated in real time by utilizing the output angular rate of a gyroscope of the relative attitude measuring device;
the relative attitude at the time t is embodied in an attitude transformation matrix
Figure FDA0002829410060000053
Above, the following relationships exist:
Figure FDA0002829410060000054
in the formula:
Figure FDA0002829410060000055
is b is2Is tied to b1A posture conversion matrix of the system;
Figure FDA0002829410060000056
is b is1Is tied to i1A posture conversion matrix of the system;
Figure FDA0002829410060000057
is i2Is tied to i1A posture conversion matrix of the system;
Figure FDA0002829410060000058
is b is2Is tied to i2A posture conversion matrix of the system;
considering the error between the calculated coordinate system and the theoretical coordinate system, there are:
Figure FDA0002829410060000059
in the formula:
Figure FDA00028294100600000510
is b is1Is tied to
Figure FDA00028294100600000511
A posture conversion matrix of the system;
Figure FDA00028294100600000512
is composed of
Figure FDA00028294100600000513
Is tied to b2A posture conversion matrix of the system;
Figure FDA00028294100600000514
is composed of
Figure FDA00028294100600000515
Is tied to i1A posture conversion matrix of the system;
Figure FDA00028294100600000516
is i2Is tied to i1A posture conversion matrix of the system;
Figure FDA00028294100600000517
is composed of
Figure FDA00028294100600000518
Is tied to i2A posture conversion matrix of the system;
let the relative attitude angle vector at time t be
Figure FDA00028294100600000519
Then t0The relative attitude angle vector of the moment is
Figure FDA00028294100600000520
The attitude transformation matrix
Figure FDA00028294100600000521
And
Figure FDA00028294100600000522
the corresponding attitude angle vectors are respectively thetaε1And thetaε2Then, the above-mentioned attitude angles are all small angles, neglect the second order above the small quantity should:
Figure FDA00028294100600000523
Figure FDA00028294100600000524
Figure FDA00028294100600000525
Figure FDA00028294100600000526
in the formula: i is a unit array;
Figure FDA00028294100600000527
representing a vector
Figure FDA00028294100600000528
An antisymmetric matrix of (a);
Figure FDA00028294100600000529
representing a vector
Figure FDA00028294100600000530
An antisymmetric matrix of (a); [ theta ] ofε1×]Represents the vector thetaε1An antisymmetric matrix of (a); [ theta ] ofε2×]Represents the vector thetaε2An antisymmetric matrix of (a);
the following equations (28) to (31) are substituted into a relative attitude relationship matrix relational expression (27) in consideration of the error of the calculation coordinate system:
Figure FDA00028294100600000531
finishing to obtain:
Figure FDA0002829410060000061
in the formula: i is a unit array;
Figure FDA0002829410060000062
representing a vector
Figure FDA0002829410060000063
An antisymmetric matrix of (a);
if the two directional cosine matrices are equal in the formula (33), 9 equations will exist in the expansion of the formula (33), and only 3 of the 9 equations are independent due to the constraint characteristic of the attitude euler angle;
if the attitude transformation matrix
Figure FDA0002829410060000064
Or
Figure FDA0002829410060000065
The corresponding Euler angle vector is theta ═ thetax θy θz]TDefining its attitude transformation matrix to be represented by theta, e.g. attitude transformation matrix
Figure FDA0002829410060000066
Expressed as:
Figure FDA0002829410060000067
note the book
Figure FDA0002829410060000068
And
Figure FDA0002829410060000069
respectively as follows:
Figure FDA00028294100600000610
Figure FDA00028294100600000611
3 equations corresponding to the elements (1,2), (3,1) and (3,2) on the non-diagonal line in the formula (33) are selected to establish
Figure FDA00028294100600000612
And
Figure FDA00028294100600000613
the relationship of (1):
Figure FDA00028294100600000614
Figure FDA00028294100600000615
Figure FDA0002829410060000071
order:
Figure FDA0002829410060000072
Figure FDA0002829410060000073
Figure FDA0002829410060000074
then:
Figure FDA0002829410060000075
in the formula:
Figure FDA0002829410060000076
is the relative attitude angle vector at time t,
Figure FDA0002829410060000077
is t0The relative attitude angle vector of the moment, the attitude transformation matrix and the corresponding attitude angle vector are respectively thetaε1And thetaε2；
The observation matrix is:
Figure FDA0002829410060000078
in the formula: h is an observation matrix;
Figure FDA0002829410060000079
is b is2Is tied to i2A posture conversion matrix of the system;
fourthly, performing Kalman filtering calculation;
discretizing a continuous Kalman filtering state equation and a system equation, and calculating through Kalman filtering to obtain relevant parameters of a state vector X to obtain measurement of relative attitude.
2. The method of claim 1, wherein the method comprises:
Figure FDA0002829410060000081
Figure FDA0002829410060000082
Figure FDA0002829410060000083
Figure FDA0002829410060000084
in the formula:
Figure FDA0002829410060000085
is epsilon0Inverse of (e ∈)0Is equivalent constant gyro drift, unit rad/s; j is x, y, z, and represents equation (14) as a system of equations of coordinate system x, y, z;
Figure FDA0002829410060000086
is epsilonrjThe reciprocal of (a); epsilonrjFor random drift of equivalent gyro epsilonrThe components on the x, y, z axes of the coordinate system, in units rad/s; mu.sejFirst order Markov coefficient, σ, for random drift of an equivalent gyroejWhite noise, w, driving the first order Markov process for equivalent gyro random driftej(t) is zero mean unit Gaussian white noise of the random drift of the equivalent gyroscope;
Figure FDA0002829410060000087
is thetaεReciprocal of (a), thetaεCalculating a relative attitude angle vector, unit rad, caused by the deviation under the system b;
Figure FDA0002829410060000088
is b is2Is tied to i2A posture conversion matrix of the system; thetaiCalculating a relative attitude angle vector, unit rad, for the i system;
Figure FDA0002829410060000089
is thetaiThe reciprocal of (a); epsilonrThe unit rad/s is equivalent gyro random drift.
3. An inertial-technology-based relative attitude measurement method according to claim 1 or 2, characterized in that:
calculating by adopting a Kalman filtering calculation method after discretization in the fourth step:
and (3) time updating:
Figure FDA00028294100600000810
Figure FDA00028294100600000811
in the formula: x (k +1/k) is a state prediction value of the next filtering period to the current filtering period; x (k/k) is a real-time state estimated value of the current filtering period;
Figure FDA00028294100600000812
is a state transition matrix; p (k +1/k) is a covariance matrix of error estimation of the next filtering period to the current filtering period; p (k/k) is a covariance matrix of error estimation of the current filtering period;
Figure FDA00028294100600000813
is composed of
Figure FDA00028294100600000814
A transposed matrix of the matrix; q (k) is a noise variance matrix;
measurement updating:
K(k+1)＝P(k+1/k)HT(k+1)[H(k+1)P(k+1/k)HT(k+1)+R(k+1)]-1 (47)
P(k+1/k+1)＝[I-K(k+1)H(k+1)]P(k+1/k)[I-K(k+1)H(k+1)]T+K(k+1)R(k+1)K(k+1)T(48)
X(k+1/k+1)＝X(k+1/k)+K(k+1)(Z(k+1)-H(k+1)X(k+1/k)) (49)
in the formula: k (K +1) is a filter gain array of the next filter period; p (k +1/k) is a covariance matrix of error estimation of the next filtering period to the current filtering period; hT(K +1) is a transposed matrix of the matrix K (K + 1); r (k +1) is a measured noise variance matrix of the next filtering period; p (k +1/k +1) is a covariance matrix of error estimation of the next filtering period; x (k +1/k +1) the state estimation value of the next filtering period; z (k +1) next filtering period observation quantity matrix; h (k +1) is a next filtering period measurement matrix; and X (k +1/k) is a state predicted value of the next filtering period to the current filtering period.
4. The method of claim 1, wherein the method comprises:
the relative attitude measuring device includes: the device comprises a measuring module, an interface module, a control processor, a display, a keyboard and a power supply module;
the control processor is a core component of the relative attitude measuring device and is responsible for calculating and generating relative attitude information according to the obtained data information;
the interface module is used for transmitting the ship navigation information measured by the ship inertial navigation equipment to the control processor, and simultaneously outputting the calculation result of the control processor in the form of data information to the outside and providing the data information to required external equipment;
the measuring module comprises an orthogonal three-axis high-precision gyroscope and is used for measuring the angular velocity information of the installation position of the relative attitude measuring device;
the keyboard and the display provide input and output of information;
the power supply module is connected with an external power supply, is converted into a power supply form required by each component in the relative attitude measuring device, and supplies power to each module of the relative attitude measuring device.
5. The method of claim 4, wherein the relative attitude measurement method based on inertial technology comprises: b is ensured as much as possible when the relative attitude measuring device is installed at the position to be measured2Is a combination of b and1are corresponding to each other in the axial direction and have a smaller included angle; and a power supply module and an interface module of the relative attitude measuring device are respectively connected with a power supply cable and a signal cable of the ship inertial navigation equipment.
CN201610803806.3A 2016-09-06 2016-09-06 Relative attitude measurement method based on inertia technology Active CN107796388B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610803806.3A CN107796388B (en) 2016-09-06 2016-09-06 Relative attitude measurement method based on inertia technology

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610803806.3A CN107796388B (en) 2016-09-06 2016-09-06 Relative attitude measurement method based on inertia technology

Publications (2)

Publication Number Publication Date
CN107796388A CN107796388A (en) 2018-03-13
CN107796388B true CN107796388B (en) 2021-03-16

Family

ID=61529726

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610803806.3A Active CN107796388B (en) 2016-09-06 2016-09-06 Relative attitude measurement method based on inertia technology

Country Status (1)

Country Link
CN (1) CN107796388B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110736459B (en) * 2019-10-25 2021-08-17 中国人民解放军国防科技大学 Angular deformation measurement error evaluation method for inertial quantity matching alignment
CN112923924B (en) * 2021-02-01 2023-06-30 杭州电子科技大学 Method and system for monitoring posture and position of anchoring ship
CN113375626B (en) * 2021-05-11 2024-05-03 北京自动化控制设备研究所 Space vector relative parallelism measuring method based on inertial device

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538792A (en) * 2012-02-08 2012-07-04 北京航空航天大学 Filtering method for position attitude system
CN102621565A (en) * 2012-04-17 2012-08-01 北京航空航天大学 Transfer aligning method of airborne distributed POS (Position and Orientation System)
CN103323005A (en) * 2013-03-06 2013-09-25 郭雷 Multi-objective optimization anti-interference filtering method for SINS/GPS/polarized light combined navigation system
CN103487822A (en) * 2013-09-27 2014-01-01 南京理工大学 BD/DNS/IMU autonomous integrated navigation system and method thereof
CN103913181A (en) * 2014-04-24 2014-07-09 北京航空航天大学 Airborne distribution type POS (position and orientation system) transfer alignment method based on parameter identification
CN104567930A (en) * 2014-12-30 2015-04-29 南京理工大学 Transfer alignment method capable of estimating and compensating wing deflection deformation
CN105136166A (en) * 2015-08-17 2015-12-09 南京航空航天大学 Strapdown inertial navigation error model simulation method with specified inertial navigation position precision

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538792A (en) * 2012-02-08 2012-07-04 北京航空航天大学 Filtering method for position attitude system
CN102621565A (en) * 2012-04-17 2012-08-01 北京航空航天大学 Transfer aligning method of airborne distributed POS (Position and Orientation System)
CN103323005A (en) * 2013-03-06 2013-09-25 郭雷 Multi-objective optimization anti-interference filtering method for SINS/GPS/polarized light combined navigation system
CN103487822A (en) * 2013-09-27 2014-01-01 南京理工大学 BD/DNS/IMU autonomous integrated navigation system and method thereof
CN103913181A (en) * 2014-04-24 2014-07-09 北京航空航天大学 Airborne distribution type POS (position and orientation system) transfer alignment method based on parameter identification
CN104567930A (en) * 2014-12-30 2015-04-29 南京理工大学 Transfer alignment method capable of estimating and compensating wing deflection deformation
CN105136166A (en) * 2015-08-17 2015-12-09 南京航空航天大学 Strapdown inertial navigation error model simulation method with specified inertial navigation position precision

Also Published As

Publication number Publication date
CN107796388A (en) 2018-03-13

Similar Documents

Publication Publication Date Title
CN109556632B (en) INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
CN106289246B (en) A kind of flexible link arm measure method based on position and orientation measurement system
CN108827310B (en) Marine star sensor auxiliary gyroscope online calibration method
CN102608596B (en) Information fusion method for airborne inertia/Doppler radar integrated navigation system
CN101949703B (en) Strapdown inertial/satellite combined navigation filtering method
CN103900565B (en) A kind of inertial navigation system attitude acquisition method based on differential GPS
CN103217159B (en) A kind of SINS/GPS/ polarized light integrated navigation system modeling and initial alignment on moving base method
CN101706284B (en) Method for increasing position precision of optical fiber gyro strap-down inertial navigation system used by ship
Soken et al. UKF-based reconfigurable attitude parameters estimation and magnetometer calibration
CN106940193A (en) A kind of ship self adaptation based on Kalman filter waves scaling method
CN102538792A (en) Filtering method for position attitude system
CN105300404B (en) A kind of naval vessel benchmark inertial navigation system Calibration Method
CN102353378A (en) Adaptive federal filtering method of vector-form information distribution coefficients
CN109708663B (en) Star sensor online calibration method based on aerospace plane SINS assistance
CN112325886B (en) Spacecraft autonomous attitude determination system based on combination of gravity gradiometer and gyroscope
CN107796388B (en) Relative attitude measurement method based on inertia technology
Li et al. Online self-calibration research of single-axis rotational inertial navigation system
CN112146655A (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN103674059A (en) External measured speed information-based horizontal attitude error correction method for SINS (serial inertial navigation system)
Sahawneh et al. Development and calibration of low cost MEMS IMU for UAV applications
CN106840151A (en) Model-free deformation of hull measuring method based on delay compensation
CN106017452A (en) Dual gyro anti-disturbance north-seeking method
CN108303120B (en) Real-time transfer alignment method and device for airborne distributed POS
Vavilova et al. The calibration problem in inertial navigation

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