CN104501838B - SINS Initial Alignment Method - Google Patents
SINS Initial Alignment Method Download PDFInfo
- Publication number
- CN104501838B CN104501838B CN201510027700.4A CN201510027700A CN104501838B CN 104501838 B CN104501838 B CN 104501838B CN 201510027700 A CN201510027700 A CN 201510027700A CN 104501838 B CN104501838 B CN 104501838B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- carrier
- moment
- coordinate
- transition matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- 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 present invention relates to a kind of SINS Initial Alignment Method, described method includes obtaining the transition matrix that inertial coordinate is tied to space flight coordinate system according to the positional information of carrier itself, acceleration and angular speedib0Coordinate is tied to the transition matrix of inertial coodinate systemAnd carrier coordinate system is to ib0The transition matrix of coordinate systemThe transition matrix of space flight coordinate system is tied to according to inertial coordinateib0Coordinate is tied to the transition matrix of inertial coodinate systemAnd carrier coordinate system is to ib0The transition matrix of coordinate systemCarrier coordinate system is obtained to the transition matrix of space flight coordinate systemAccording to the transition matrix of carrier coordinate system to space flight coordinate systemRealize the coarse alignment of carrier;Navigation error angle is tried to achieve after coarse alignment, and realizes according to navigation error angle the fine alignment of carrier.Using the SINS Initial Alignment Method of this kind of structure, the initial alignment precision of SINS, reduction Initial Alignment Error, application are improved relatively broad.
Description
Technical field
The present invention relates to communication technical field, more particularly to Mechanical course, a kind of SINS is specifically referred to initial
Alignment methods.
Background technology
With MEMS (Micro-Electro-Mechanical-System) sensor, navigation and the development of control technology
And further increasing of the country to agriculture support dynamics, precision agriculture is being quickly turned to a kind of trend, and in agricultural machinery
Aid in during Driving control, posture (including the angle of pitch, roll angle and navigation angle), speed and the positional information of car body can be real
When reflect motion and the positional information of car body, these information can be that high-precision integrated navigation and control algolithm provide important
Data input.
Strap-down inertial (Strapdown Inertial Navigation System, SINS) have independent navigation,
The features such as good confidentiality, strong antijamming capability, navigational parameter enrich and precision is high in the short time is widely used, but is due to used
Property sensor constant error presence so that navigation error accumulate over time long-time navigation accuracy it is poor, it is necessary to other miss
The stable navigation system auxiliary of difference, such as high-precision GPS-RTK.Inertial navigation system is according to the car body acceleration measured, warp
Cross integral operation and try to achieve speed and position.For that purpose it is necessary to know initial velocity and position.In addition, using geographic coordinate system as space flight
In the inertial coordinate system of coordinate system, physical platform and mathematical platform are all the benchmark for measuring acceleration, and platform must be accurate
Geographic coordinate system really is directed at and tracks, to avoid platform error from causing acceleration analysis error.The precision being initially aligned is direct
It is related to the operating accuracy of navigation system, is also one of important key technology.
In addition, initial alignment on moving base method generally comprises the following steps in the prior art:
The first step, does not consider measurement error, and carrier (agricultural machinery) is static during coarse alignment, acceleration transducer measurement ratio
PowerIt is projection gs of the acceleration of gravity vector g in carrier coordinate systemb, and direction contrast, i.e.,Gyroscope is passed
Sensor measurement is rotational-angular velocity of the earth vector ωieProjection in carrier coordinate systemDue to being directed at the geographical position in place
Put, it is known that so vector g and ωieProjection g in space flight coordinate systemnWithIt is also known.Attitude matrixCan by g with
And ωieTransformation relation between carrier coordinate system and space flight coordinate system is tried to achieve, i.e.,
Wherein,For projection of the rotational-angular velocity of the earth in space flight coordinate system.
Second step, if gnWithIt is not parallel, for direct solutionIt can be increased by constructing new vector equation
The number of equation,
3rd step, by formula (11), (12) difference transposition,
4th step,
Formula (14) is exactly the calculation formula of conventional analytic formula coarse alignment method.Due to there is interference and noise, acceleration
Meter and gyro to measure value gbWithThere is certain error, so the method that is averaged of measured value is usually taken in a period of time to subtract
Small interference and the influence of noise.If there is the interference of rocking of higher magnitude, above-mentioned coarse alignment method in carrier (i.e. agricultural machinery)
There may be very big error.Such as in low latitudes, it is assumed that carrier coordinate system is overlapped with space flight coordinate system when coarse alignment starts,
I.e. true navigation angle ψ=0 °, it is 60s to take the coarse alignment time, if occurred when coarse alignment closes to an end around carrier oxbAxle is rotated
15 ' rock interference, thenWithWherein,Carrier oxbAxle rotates 15 '
Rock the lower disturbance angle velocity produced of interference,Projection of the acceleration of gravity in carrier coordinate system, carries it into formula
(14) it can ask and obtain coarse alignment result ψ=315 °, it is clear that this result is wrong.
The content of the invention
The purpose of the present invention is that the shortcoming for overcoming above-mentioned prior art can improve SINS there is provided one kind
The relatively broad SINS Initial Alignment Method of initial alignment precision, reduction Initial Alignment Error, application.
To achieve these goals, SINS Initial Alignment Method of the invention has following composition:
The SINS Initial Alignment Method, it is mainly characterized by, and described method comprises the following steps:
(1) carrier obtains the positional information of itself, and acceleration transducer obtains the acceleration of carrier, and gyro sensor is obtained
Take the angular speed of carrier;
(2) inertial coordinate is obtained according to the positional information of carrier itself, acceleration and angular speed and is tied to space flight coordinate system
Transition matrixCoordinate is tied to the transition matrix of inertial coodinate systemAnd carrier coordinate system is to ib0Coordinate system
Transition matrixWherein, described ib0Coordinate system is is in coarse alignment start timet0Moment, carrier coordinate system was in inertial space
In solidify out into ib0Coordinate system, i.e. ib0The origin of coordinate system existst0The center of gravity at moment, and do not moved with the movement of carrier, Axle respectively witht0The coordinate overlapping of axles of the same name of moment carrier coordinate system, and remained pointing in inertial space
It is constant;Wherein, it is describedt0Moment is coarse alignment start time, and k is time scale;
(3) transition matrix of space flight coordinate system is tied to according to inertial coordinateib0Coordinate is tied to the conversion of inertial coodinate system
MatrixAnd carrier coordinate system is to ib0The transition matrix of coordinate systemAnd arrived according to below equation acquisition carrier coordinate system
The transition matrix of space flight coordinate system
Wherein, k is time scale;
(4) according to the transition matrix of carrier coordinate system to space flight coordinate systemRealize the coarse alignment of carrier.
Further, described step (2) comprises the following steps:
(2.1) transition matrix that inertial coordinate is tied to space flight coordinate system is obtained according to the positional information of carrier itself
(2.2) carrier coordinate system is obtained to i according to the acceleration of carrier itselfb0The transition matrix of coordinate system
(2.3) i is obtained according to the positional information of carrier itself, acceleration and angular speedb0Coordinate is tied to inertial coodinate system
Transition matrix
Further, described step (2.1) comprises the following steps:
(2.1.1) obtains the spin velocity of the earth according to the positional information of carrier itself;
(2.1.2) obtains terrestrial coordinates according to the positional information and rotational-angular velocity of the earth of carrier itself and is tied to space flight seat
Mark the transition matrix of systemAnd inertial coordinate is tied to the transition matrix of terrestrial coordinate systemWherein, described terrestrial coordinate system
To the transition matrix of space flight coordinate systemDescribed inertial coordinate is tied to ground
The transition matrix of spherical coordinate systemWherein, λ is the longitude of t carrier, and L is t
The latitude of carrier, ωieFor t rotational-angular velocity of the earth, t is time scale;
(2.1.3) is tied to the transition matrix of space flight coordinate system according to described terrestrial coordinatesAnd inertial coordinate is tied to
The transition matrix of terrestrial coordinate systemAnd the transition matrix that inertial coordinate is tied to space flight coordinate system is obtained according to below equation:
Wherein, λ is the longitude of t carrier, and L is the latitude of t carrier, ωieFor t rotational-angular velocity of the earth, t
For time scale.
Yet further, described step (2.2) comprises the following steps:
(2.2.1) obtains carrier coordinate system to i according to equivalent rotating vector list sample algorithmb0Coordinate system is from the k moment to k+1
Moment converts quaternary number:
Wherein, k moment carrier coordinate system is to ib0The transition matrix of coordinate systemForib0It is corresponding in coordinate system to become
Changing quaternary number isK+1 moment carrier coordinate system is to ib0The transition matrix of coordinate systemForib0It is right in coordinate system
The conversion quaternary number answered isΔθkFor the output at k moment to k+1 moment gyro sensors
Angle increment;
(2.2.2) carries out quaternary number more new Algorithm according to formula (3):
Wherein, whereinQuaternary number is simplest supercomplex, q=q0+q1i+q2j+
q3K, wherein q0、q1、q2、q3For real number, i^2=j^2=k^2=ijk=-1;Wherein,For ib0The k+1 moment in coordinate system
Conversion quaternary number;It is carrier coordinate system relative to ib0Coordinate system converts quaternary number from k moment to the k+1 moment;
(2.2.3) is arrived according to described conversion quaternary number and carrier coordinate systemib0The transition matrix of coordinate systemObtain
Carrier coordinate system is to ib0Transition matrixWherein:
Wherein, quaternary number is simplest supercomplex, q=q0+q1i+q2j+q3K, wherein q0、q1、q2、q3For real number, i^2
=j^2=k^2=ijk=-1.
Yet further, described step (2.3) comprises the following steps:
(2.3.1) is obtained in i according to the list sample algorithm of speedb0The speed of coordinate system projection:
Wherein,For k+1 moment ib0Specific force is integrated in coordinate system
Speed, Δ vkThe specific force speed increment exported for k moment to k+1 moment acceleration transducer, Δ θkFor the k moment to k+1 moment tops
The angle increment of spiral shell instrument sensor output,For carrier coordinate system to ib0The transition matrix of coordinate system, r time scales;
(2.3.2) according to acceleration of gravity during carrier stationary inertial coodinate system projection giExported with acceleration transducer
In ib0The projection of coordinate systemBetween conversion, i.e. below equation (7) obtains
Wherein,It is that an inertial coordinate tried to achieve by formula (7) is tied to ib0The transformation matrix of coordinate system;For carrier
Coordinate is to ib0The transition matrix of coordinate system;The transition matrix of inertial coodinate system is tied to for space flight coordinate;For the k+1 moment
The speed that specific force is integrated into r moment inertial coodinate systems;For the defeated of the specific force in carrier coordinate system, i.e. acceleration transducer
Go out value;λ is the longitude of t carrier, and L is the latitude of t carrier, ωieFor t rotational-angular velocity of the earth, t carves for the time
Degree;G is acceleration of gravity;R, k are respectively two moment, k >=r, and are hadgnIt is acceleration of gravity in space flight coordinate system
In projection;
(2.3.3) is obtained according to formula (7), (8)
Wherein,Respectively l moment and m (l<M) output of moment acceleration transducer is in ib0The throwing of coordinate system
Shadow;t0、tu、tv、tdAt the time of respectively different, wherein t0<tu≤tv<td, t0For coarse alignment start time, tdFor coarse alignment knot
The beam moment;For tdMoment speed is projected in inertial coodinate system, by the acceleration of gravity in inertial coodinate system in tvTo td
Moment integration gained;For tuMoment speed is projected in inertial coodinate system, is existed by the acceleration of gravity in inertial coodinate system
t0To tuMoment integration gained;Respectively tu、tdMoment, speed was in ib0Projected in coordinate system, respectively by ib0Sit
Acceleration transducer in mark system is exported in ib0Coordinate system is projected in t0To tuMoment and tvTo tdMoment integration gained,The respectively projection of m moment and l moment acceleration of gravity in inertial coodinate system, l<M, when l and m very close to when, two
Person is parallel.
Further, described navigational coordinate system is northeast day coordinate system, and described step (4) also includes following step afterwards
Suddenly:
(5) the course error angle of the carrier described in is:
Wherein, P0Coordinate be (x0, y0), P1Coordinate be (x1, y1), the baseline determined by them and northeast day coordinate
Angle in system between north orientation is:
Wherein,For dead-reckoning position, (x1, y1) is known location,Coordinate for~δ ψ are
Course error angle;
(6) fine alignment is carried out to described carrier according to described course error angle;
(7) system judges whether initial alignment terminates;
(8) if initial alignment terminates, terminate and exit;Otherwise step (1) is continued.
The SINS Initial Alignment Method in the invention is employed, compared with prior art, with following beneficial
Effect:
(1) coarse alignment method that the present invention is realized can reduce car body relative to traditional coarse alignment method and occur by a relatively large margin
Rock mushing error, and the problem of calculate mistake at low latitudes navigation angle;
(2) present invention fully takes into account earth rotation speed in initially alignment alignment procedures and geographical position adds to gravity
The influence of speed, is modified using corresponding calculating, so as to obtain higher initial alignment precision;
(3) the quaternary number that the present invention is solved by equivalent rotating vector method, can eliminate rotation noncommutativity error, from
And improve the initial alignment precision of inertial navigation.
Brief description of the drawings
Fig. 1 is the step flow chart of the SINS Initial Alignment Method of the present invention.
The geometrical principle figure of fine alignment during Fig. 2 is initially aligned for the SINS of the present invention.
Embodiment
In order to more clearly describe the technology contents of the present invention, carried out with reference to specific embodiment further
Description.
The alphabetical k in SINS Initial Alignment Method in the present invention has following two implications:The first, k
It is the sampled value or result of calculation for having the variable meaning of this following table to the mark after time discretization for the discrete instants, than
Such as a (k), i.e., it is first time sampled point or result of calculation as k=1, k represents time scale under this kind of implication;Second,
K is the imaginary unit changed in quaternary number, i.e., only in q=q0+q1i+q2j+q3When in k, k is imaginary unit, described below
In, each k appearance all provides k implication (unaccounted k represents time scale), to prevent those skilled in the art due to right
K understanding deviation and influence the understanding of the present invention.
Initial alignment on moving base includes two stages:Coarse alignment and fine alignment stage, base of the fine alignment stage in coarse alignment
On plinth, typically using velocity error as observed quantity, the misalignment of coarse alignment is estimated by certain algorithm.The present invention is with the beginning of tradition
A kind of new SINS Initial Alignment Method is proposed based on beginning alignment methods.
Refer to shown in Fig. 1, at the beginning of SINS Initial Alignment Method of the present invention, especially agricultural machinery inertial navigation
Beginning alignment methods, comprise the following steps:
The narration of the first step, for convenience algorithm, defines a kind of new coordinate system here --- carrier inertial coodinate system (ib0
Coordinate system):Described ib0Coordinate system is is in coarse alignment start timet0Solidification of the moment carrier coordinate system in inertial space
As ib0Coordinate system, i.e. ib0The origin of coordinate system existst0The center of gravity at moment, and do not moved with the movement of carrier, oxib0、oyib0、
ozib0Axle respectively witht0The coordinate overlapping of axles of the same name of moment carrier coordinate system, and remain pointing in inertial space it is constant,t0Moment
It is poised for battle start time to be thick.
Transition matrix of the carrier coordinate system to space flight coordinate systemLower Matrix Formula is can be used to represent:
In formula,The transition matrix of space flight coordinate system is tied to for k moment inertial coordinates, can be by carrier (i.e. agricultural machinery) institute
Determined in the time k of a geographical position and coarse alignment,For k moment carrier coordinate system to ib0The transition matrix of coordinate system, profit
The carrier coordinate system exported with gyro sensor is with respect to ib0The angular movement information of coordinate system, is updated by SINS Attitude and calculated
Method can be in the hope of the transition matrix;For ib0Coordinate is tied to the transition matrix of inertial coodinate system, and the transition matrix is a constant value
Battle array, transformational relation that can be between acceleration of gravity and acceleration transducer output is tried to achieve.
Second step, solves the transition matrix that inertial coordinate is tied to space flight coordinate system
Transition matrixThe transition matrix of space flight coordinate system can be tied to by terrestrial coordinatesThe earth is tied to inertial coordinate to sit
Mark the transition matrix of system(i.e. terrestrial coordinate system is relative to the angle ω that inertial coodinate system is turned overieT) try to achieve, i.e.,
Wherein, λ is the longitude of t carrier, and L is the latitude of t carrier, ωieFor t rotational-angular velocity of the earth, t
For time scale.
3rd step, seeks carrier coordinate system to ib0The transition matrix of coordinate system
Transition matrixIt can be sampled and exported by gyro sensor, tried to achieve using SINS Attitude more new algorithm.It is false
If k moment and k+1 moment carrier coordinate system are to ib0The transition matrix of coordinate systemRespectivelyWithIts is corresponding
Converting quaternary number is respectivelyWithObviously haveWithAssume again that k moment and k+1 moment gyroscopes
The angle increment of sensor output is Δ θk, then carrier coordinate system can be obtained relative to i using equivalent rotating vector list sample algorithmb0Sit
Mark system converts quaternary number from k moment to the k+1 moment and is:
So as to carry out quaternary number more new Algorithm
Assuming that(q herein3K in k is imaginary unit), quaternary number is simplest onlaps
Number, q=q0+q1i+q2j+q3K (q herein3K in k is imaginary unit), wherein q0、q1、q2、q3For real number, i^2=j^2=k^2
=ijk=-1 (k is imaginary unit herein);Wherein,For ib0The conversion quaternary number at k+1 moment in coordinate system;For
ib0The conversion quaternary number at k+1 moment to k moment in coordinate system;Then the relation between conversion quaternary number and transition matrix, can be asked
Obtain transition matrix
Quaternary number is simplest supercomplex, q=q0+q1i+q2j+q3K (q herein3K in k is imaginary unit), wherein
q0、q1、q2、q3For real number, i^2=j^2=k^2=ijk=-1 (k is imaginary unit herein), transition matrixMore new explanation
Calculate cause coarse alignment be provided with tracking carrier angular movement function, that is to say, that acquisition be coarse alignment finish time posture square
Battle array.
4th step, solves carrier coordinate system and arrivesib0The transition matrix of coordinate system
Transition matrixIt is a constant value matrix, when carrier (i.e. agricultural machinery) is static, the matrix can be by acceleration of gravity
In the projection g of inertial coodinate systemiWith being exported with accelerometer in ib0The projection of systemBetween conversion, i.e.,
Two are taken in formula (7) not in the same time, l moment and m moment (l<M),
Due to the presence of the random disturbances such as inertia device noise, formula (9) is integrated near l moment and m moment, to drop
The influence of low interference.If the angle increment of k moment and the output of k+1 moment gyro sensor is Δ θk, acceleration transducer output
Specific force speed increment be Δ vk, can be tried to achieve in i using the list sample algorithm of speedb0The speed more new algorithm of coordinate system projection,
WhereinWherein r<=k,
Formula (6) is substituted into formula (7), integrated from r moment and k moment:
Wherein,It is that an inertial coordinate tried to achieve by formula (7) is tied to ib0The transformation matrix of coordinate system;For carrier
Coordinate is to ib0The transition matrix of coordinate system;The transition matrix of inertial coodinate system is tied to for space flight coordinate;For the k+1 moment
The speed that specific force is integrated in inertial coodinate system;For the output valve of the specific force in carrier coordinate system, i.e. acceleration transducer;λ is
The longitude of t carrier, L is the latitude of t carrier, ωieFor t rotational-angular velocity of the earth, t is t;G is gravity
Acceleration;R, k are respectively two moment, k >=r, and are hadgnFor projection of the acceleration of gravity in space flight coordinate system;
Wherein,Respectively l moment and m (l<M) output of moment acceleration transducer is in ib0The throwing of coordinate system
Shadow;t0、tu、tv、tdAt the time of respectively different, wherein t0<tu≤tv<td, t0For coarse alignment start time, tdFor coarse alignment knot
The beam moment;For tdMoment speed is projected in inertial coodinate system, by the acceleration of gravity in inertial coodinate system in tvTo td
Moment integration gained;For tuMoment speed is projected in inertial coodinate system, is existed by the acceleration of gravity in inertial coodinate system
t0To tuMoment integration gained;Respectively tu、tdMoment, speed was in ib0Projected in coordinate system, respectively by ib0Coordinate
Acceleration transducer in system is exported in ib0Coordinate system is projected in t0To tuMoment and tvTo tdMoment integration gained,
The respectively projection of m moment and l moment acceleration of gravity in inertial coodinate system, l<M, when l and m very close to when, the two is parallel.
5th step, will solve transition matrix of the carrier coordinate system to space flight coordinate system
The coarse alignment process of the above-mentioned SINS for the present invention, now carries out following explanation with regard to fine alignment process, needs
It should be noted that the premise for realizing following fine alignment is by above fine alignment process.
6th step, geometry fine alignment alignment general principle.Described navigational coordinate system is northeast day coordinate system, on the ground
Two location points can be used to carry out navigation angular measurement, can when two location points are at a distance of several kilometers and little height change
So that they are regarded as in same level, refer to shown in Fig. 2, there is point P in plane0(x0, y0) and P1(x1, y1), P0's
Coordinate is (x0, y0), P1Coordinate be in (x1, y1), the then baseline determined by them and northeast day coordinate system between north orientation
Angle is:
It is not that angle of accurately navigating directly is asked for by known 2 points of position in fine alignment is carried out using dead reckoning,
But utilize dead-reckoning positionError between known location P1 (x1, y1) tries to achieve course error angle δ ψ, then
Navigation angle is modified.If ignoring other errors, initial heading error angle is only considered, then due to the shadow at course error angle
Ring, carrier (i.e. agricultural machinery) will appear from site error after travelling a segment distance.Assuming that carrier (i.e. agricultural machinery) is along linear rows
Sail and initial heading error angle is small angle, then can calculate initial heading error angle is approximately:
Wherein,For dead-reckoning position, (x1, y1) is known location,Coordinate for~δ ψ are
Sign is determined on a case-by-case basis in course error angle, above formula.Because course error angle is in carrier (i.e. agricultural machinery) P0P1Traveling
During be basically unchanged, so correcting P in real time using the error angle1Navigation angle at point, rather than P during initial coarse alignment0At point
Navigation angle, just complete navigation angle fine alignment process.
The SINS Initial Alignment Method in the invention is employed, compared with prior art, with following beneficial
Effect:
(1) coarse alignment method that the present invention is realized can reduce car body relative to traditional coarse alignment method and occur by a relatively large margin
Rock mushing error, and the problem of calculate mistake at low latitudes navigation angle;
(2) present invention fully takes into account earth rotation speed in initially alignment alignment procedures and geographical position adds to gravity
The influence of speed, is modified using corresponding calculating, so as to obtain higher initial alignment precision;
(3) the quaternary number that the present invention is solved by equivalent rotating vector method, can eliminate rotation noncommutativity error, from
And improve the initial alignment precision of inertial navigation.
In this description, the present invention is described with reference to its specific embodiment.But it is clear that can still make
Various modifications and alterations are without departing from the spirit and scope of the present invention.Therefore, specification and drawings are considered as illustrative
And it is nonrestrictive.
Claims (2)
1. a kind of SINS Initial Alignment Method, it is characterised in that described method comprises the following steps:
(1) carrier obtains the positional information of itself, and acceleration transducer obtains the acceleration of carrier, and gyro sensor, which is obtained, to be carried
The angular speed of body;
(2) inertial coordinate is obtained according to the positional information of carrier itself, acceleration and angular speed and is tied to turning for space flight coordinate system
Change matrixib0Coordinate is tied to the transition matrix of inertial coodinate systemAnd carrier coordinate system is to ib0The conversion square of coordinate system
Battle arrayWherein, described ib0It in coarse alignment start time is t that coordinate system, which is,0Moment, carrier coordinate system was in inertial space
Solidify out into ib0Coordinate system, i.e. ib0The origin of coordinate system is in t0The center of gravity at moment, and do not moved with the movement of carrier, Axle respectively with t0The coordinate overlapping of axles of the same name of moment carrier coordinate system, and remained pointing in inertial space
It is constant;Wherein, described t0Moment is coarse alignment start time, and k is time scale;
(3) transition matrix of space flight coordinate system is tied to according to inertial coordinateib0Coordinate is tied to the transition matrix of inertial coodinate systemAnd carrier coordinate system is to ib0The transition matrix of coordinate systemAnd carrier coordinate system is obtained to space flight according to below equation
The transition matrix of coordinate system
Wherein, k is time scale;
(4) according to the transition matrix of carrier coordinate system to space flight coordinate systemRealize the coarse alignment of carrier;
Described step (2) comprises the following steps:
(2.1) transition matrix that inertial coordinate is tied to space flight coordinate system is obtained according to the positional information of carrier itself
(2.2) carrier coordinate system is obtained to i according to the angular speed of carrier itselfb0The transition matrix of coordinate system
(2.3) i is obtained according to the positional information of carrier itself, acceleration and angular speedb0Coordinate is tied to turning for inertial coodinate system
Change matrix
Described step (2.1) comprises the following steps:
(2.1.1) obtains the spin velocity of the earth according to the positional information of carrier itself;
(2.1.2) obtains terrestrial coordinates according to the positional information and rotational-angular velocity of the earth of carrier itself and is tied to space flight coordinate system
Transition matrixAnd inertial coordinate is tied to the transition matrix of terrestrial coordinate systemWherein, described terrestrial coordinates is tied to boat
The transition matrix of its coordinate systemDescribed inertial coordinate is tied to earth seat
Mark the transition matrix of systemWherein, λ is the longitude of t carrier, and L is t carrier
Latitude, ωieFor t rotational-angular velocity of the earth, t is time scale;
(2.1.3) is tied to the transition matrix of space flight coordinate system according to described terrestrial coordinatesAnd inertial coordinate is tied to earth seat
Mark the transition matrix of systemAnd the transition matrix that inertial coordinate is tied to space flight coordinate system is obtained according to below equation:
Wherein, λ is the longitude of t carrier, and L is the latitude of t carrier, ωieFor t rotational-angular velocity of the earth, when t is
Between scale;
Described step (2.2) comprises the following steps:
(2.2.1) obtains carrier coordinate system to i according to equivalent rotating vector list sample algorithmb0Coordinate system is from the k moment to the k+1 moment
Converting quaternary number is:
Wherein, k moment carrier coordinate system is to ib0The transition matrix of coordinate systemForib0Corresponding conversion four in coordinate system
First number isK+1 moment carrier coordinate system is to ib0The transition matrix of coordinate systemForib0It is corresponding in coordinate system to become
Changing quaternary number is ΔθkIncrease for the angle at k moment to the output of k+1 moment gyro sensors
Amount;
(2.2.2) carries out quaternary number more new Algorithm according to formula (3):
Wherein, whereinQuaternary number is simplest supercomplex, q=q0+q1i+q2j+q3K, its
Middle q0、q1、q2、q3For real number, i^2=j^2=k^2=ijk=-1;Wherein,For ib0The conversion at k+1 moment in coordinate system
Quaternary number;It is carrier coordinate system relative to ib0Coordinate system converts quaternary number from k moment to the k+1 moment;
(2.2.3) is according to described conversion quaternary number and carrier coordinate system to ib0The transition matrix of coordinate systemObtain carrier
Coordinate is tied to ib0Transition matrixWherein:
Wherein, quaternary number is simplest supercomplex, q=q0+q1i+q2j+q3K, wherein q0、q1、q2、q3For real number, i^2=j^2
=k^2=ijk=-1;
Described step (2.3) comprises the following steps:
(2.3.1) is obtained in i according to the list sample algorithm of speedb0The speed of coordinate system projection:
Wherein, For k+1 moment ib0The speed that specific force is integrated in coordinate system, Δ
vkThe specific force speed increment exported for k moment to k+1 moment acceleration transducer, Δ θkPassed for k moment to k+1 moment gyroscope
The angle increment of sensor output,For carrier coordinate system to ib0The transition matrix of coordinate system, r time scales;
(2.3.2) according to acceleration of gravity during carrier stationary inertial coodinate system projection giWith acceleration transducer output in ib0
The projection of coordinate systemBetween conversion, i.e. below equation (7) obtains
Wherein,It is that an inertial coordinate tried to achieve by formula (7) is tied to ib0The transformation matrix of coordinate system;For carrier coordinate
To ib0The transition matrix of coordinate system;The transition matrix of inertial coodinate system is tied to for space flight coordinate;For k+1 moment inertia
The speed that specific force is integrated in coordinate system;For the output valve of the specific force in carrier coordinate system, i.e. acceleration transducer;λ is t
The longitude of carrier, L is the latitude of t carrier, ωieFor t rotational-angular velocity of the earth, t is time scale;G adds for gravity
Speed;R, k are respectively two moment, k >=r, and are hadgnFor projection of the acceleration of gravity in space flight coordinate system;
(2.3.3) is obtained according to formula (7), (8)
Wherein,Respectively the output of l moment and m moment acceleration transducers is in ib0The projection of coordinate system;t0、tu、
tv、tdAt the time of respectively different, wherein t0<tu≤tv<td, t0For coarse alignment start time, tdFor coarse alignment finish time;For tdMoment speed is projected in inertial coodinate system, by the acceleration of gravity in inertial coodinate system in tvTo tdMoment integrates
Gained;For tuMoment speed is projected in inertial coodinate system, by the acceleration of gravity in inertial coodinate system in t0To tuWhen
Carve integration gained;Respectively tu、tdMoment, speed was in ib0Projected in coordinate system, respectively by ib0In coordinate system
Acceleration transducer is exported in ib0Coordinate system is projected in t0To tuMoment and tvTo tdMoment integration gained,Respectively m
The projection of moment and l moment acceleration of gravity in inertial coodinate system, l<M, when l and m very close to when, the two is parallel.
2. SINS Initial Alignment Method according to claim 1, it is characterised in that described space flight coordinate system
It is further comprising the steps of after described step (4) for northeast day coordinate system:
(5) the course error angle of the carrier described in is:
Wherein, P0Coordinate be (x0, y0), P1Coordinate be (x1, y1), the baseline determined by them and northeast day coordinate system
Angle between north orientation is:
Wherein,For dead-reckoning position, P1(x1, y1) is known location,Coordinate beδ ψ miss for course
Declinate;
(6) fine alignment is carried out to described carrier according to described course error angle;
(7) system judges whether initial alignment terminates;
(8) if initial alignment terminates, terminate and exit;Otherwise step (1) is continued.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510027700.4A CN104501838B (en) | 2015-01-20 | 2015-01-20 | SINS Initial Alignment Method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510027700.4A CN104501838B (en) | 2015-01-20 | 2015-01-20 | SINS Initial Alignment Method |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104501838A CN104501838A (en) | 2015-04-08 |
CN104501838B true CN104501838B (en) | 2017-08-29 |
Family
ID=52943264
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510027700.4A Active CN104501838B (en) | 2015-01-20 | 2015-01-20 | SINS Initial Alignment Method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104501838B (en) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105043418B (en) * | 2015-08-04 | 2017-12-22 | 北京航天控制仪器研究所 | A kind of quick initial coarse alignment method of inertial navigation system suitable for boat-carrying communication in moving |
CN105180937B (en) * | 2015-10-15 | 2018-01-02 | 常熟理工学院 | A kind of MEMS IMU Initial Alignment Methods |
FR3043469B1 (en) * | 2015-11-10 | 2019-10-18 | Safran Electronics & Defense | METHOD FOR DETECTING PARASITE MOVEMENTS DURING STATIC ALIGNMENT OF AN INERTIAL POWER PLANT, AND DETECTION DEVICE THEREOF |
CN106814383B (en) * | 2017-01-16 | 2018-01-12 | 立得空间信息技术股份有限公司 | A kind of high-precision POS rapid alignment methods being applied under various modes |
CN107741240B (en) * | 2017-10-11 | 2020-11-24 | 成都国卫通信技术有限公司 | Adaptive initial alignment method of combined inertial navigation system suitable for communication-in-moving |
CN107941242A (en) * | 2017-11-13 | 2018-04-20 | 东南大学 | A kind of initial coarse alignment method of integrated navigation based on inertial system |
CN111102991A (en) * | 2019-11-28 | 2020-05-05 | 湖南率为控制科技有限公司 | Initial alignment method based on track matching |
CN111044038B (en) * | 2019-12-05 | 2023-04-07 | 河北汉光重工有限责任公司 | Strapdown inertial navigation heading transformation method based on coordinate transformation |
CN111174813B (en) * | 2020-01-21 | 2023-07-21 | 河海大学 | AUV moving base alignment method and system based on outer product compensation |
CN111811543B (en) * | 2020-08-31 | 2020-12-11 | 蓝箭航天空间科技股份有限公司 | Initial alignment method for distributed navigation system of recovery type spacecraft |
CN112611394B (en) * | 2020-12-16 | 2022-08-16 | 西北工业大学 | Aircraft attitude alignment method and system under emission coordinate system |
CN113108781B (en) * | 2021-04-01 | 2022-04-08 | 东南大学 | Improved coarse alignment method applied to unmanned ship during advancing |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102706366A (en) * | 2012-06-19 | 2012-10-03 | 北京航空航天大学 | SINS (strapdown inertial navigation system) initial alignment method based on earth rotation angular rate constraint |
CN102721417A (en) * | 2011-12-23 | 2012-10-10 | 北京理工大学 | Method for error suppression of inertial concretionary coarse alignment of strapdown inertial navigation system |
CN103017787A (en) * | 2012-07-03 | 2013-04-03 | 哈尔滨工程大学 | Initial alignment method suitable for rocking base |
CN103900608A (en) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | Low-precision inertial navigation initial alignment method based on quaternion CKF |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7512493B2 (en) * | 2006-05-31 | 2009-03-31 | Honeywell International Inc. | High speed gyrocompass alignment via multiple Kalman filter based hypothesis testing |
-
2015
- 2015-01-20 CN CN201510027700.4A patent/CN104501838B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102721417A (en) * | 2011-12-23 | 2012-10-10 | 北京理工大学 | Method for error suppression of inertial concretionary coarse alignment of strapdown inertial navigation system |
CN102706366A (en) * | 2012-06-19 | 2012-10-03 | 北京航空航天大学 | SINS (strapdown inertial navigation system) initial alignment method based on earth rotation angular rate constraint |
CN103017787A (en) * | 2012-07-03 | 2013-04-03 | 哈尔滨工程大学 | Initial alignment method suitable for rocking base |
CN103900608A (en) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | Low-precision inertial navigation initial alignment method based on quaternion CKF |
Non-Patent Citations (4)
Title |
---|
一种单子样旋转矢量姿态算法;杨胜 等;《宇航学报》;20100331;第31卷(第3期);第780-785页 * |
摇摆基座上基于信息的捷联惯导粗对准研究;秦永元 等;《西北工业大学学报》;20051031;第23卷(第5期);第681-684页 * |
高动态环境下SINS姿态更新的改进等效旋转矢量算法;杜继永 等;《空间科学学报》;20131231;第33卷(第1期);第85-91页 * |
高动态环境下捷联惯导的优化算法研究;聂水茹;《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》;20040315;第I136-472页 * |
Also Published As
Publication number | Publication date |
---|---|
CN104501838A (en) | 2015-04-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104501838B (en) | SINS Initial Alignment Method | |
CN106595652B (en) | Alignment methods between the backtracking formula of vehicle kinematics constraint auxiliary is advanced | |
CN105203129B (en) | A kind of inertial nevigation apparatus Initial Alignment Method | |
CN103900565B (en) | A kind of inertial navigation system attitude acquisition method based on differential GPS | |
CN103090867B (en) | Error restraining method for fiber-optic gyroscope strapdown inertial navigation system rotating relative to geocentric inertial system | |
US10309786B2 (en) | Navigational and location determination system | |
CN105823481A (en) | GNSS-INS vehicle attitude determination method based on single antenna | |
CN105698822B (en) | Initial Alignment Method between autonomous type inertial navigation based on reversed Attitude Tracking is advanced | |
CN103090866B (en) | Method for restraining speed errors of single-shaft rotation optical fiber gyro strapdown inertial navigation system | |
CN101718560B (en) | Strapdown system error inhibition method based on uniaxial four-position rotation and stop scheme | |
CN102829781B (en) | Implementation method of rotation type strapdown optical-fiber compass | |
CN103076026B (en) | A kind of method determining Doppler log range rate error in SINS | |
CN104697526A (en) | Strapdown inertial navitation system and control method for agricultural machines | |
CN102679978B (en) | Initial alignment method of static base of rotary type strap-down inertial navigation system | |
CN108051866A (en) | Gravimetric Method based on strap down inertial navigation/GPS combination subsidiary level angular movement isolation | |
CN105628025B (en) | A kind of constant speed offset frequency/machine laser gyroscope shaking inertial navigation system air navigation aid | |
CN101963512A (en) | Initial alignment method for marine rotary fiber-optic gyroscope strapdown inertial navigation system | |
CN108007477A (en) | A kind of inertia pedestrian's Positioning System Error suppressing method based on forward and reverse filtering | |
CN108195400A (en) | The moving alignment method of strapdown micro electro mechanical inertia navigation system | |
CN106123917B (en) | Consider the Strapdown Inertial Navigation System compass alignment methods of outer lever arm effect | |
CN108871378A (en) | Lever arm and the outer online dynamic calibrating method of lever arm error in two sets of Rotating Inertial Navigation Systems of one kind | |
CN103090865A (en) | Method for restraining attitude errors of modulation type strapdown inertial navigation system | |
CN107576327A (en) | Varistructure integrated navigation system design method based on Observable degree analysis of Beidou double | |
CN105910623B (en) | The method for carrying out the correction of course using magnetometer assisted GNSS/MINS tight integration systems | |
CN102706349A (en) | Carrier gesture determining method based on optical fiber strap-down compass technology |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C53 | Correction of patent for invention or patent application | ||
CB02 | Change of applicant information |
Address after: 201702 Shanghai city Qingpu District Xujing town Gaojing Road No. 599 China Beidou Industrial Park C Building 2 floor Applicant after: Shanghai Hua Ce airmanship limited company Address before: 201702 Shanghai city Qingpu District Xujing town Gaojing Road No. 599 China Beidou Industrial Park C Building 2 floor Applicant before: CHC Technology Co., Ltd. |
|
COR | Change of bibliographic data |
Free format text: CORRECT: APPLICANT; FROM: SHANGHAI HUACE NAVIGATION TECHNOLOGY CO., LTD. TO: CHC TECHNOLOGY CO., LTD. |
|
GR01 | Patent grant | ||
GR01 | Patent grant |