CN204255368U - A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod - Google Patents
A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod Download PDFInfo
- Publication number
- CN204255368U CN204255368U CN201420262058.9U CN201420262058U CN204255368U CN 204255368 U CN204255368 U CN 204255368U CN 201420262058 U CN201420262058 U CN 201420262058U CN 204255368 U CN204255368 U CN 204255368U
- Authority
- CN
- China
- Prior art keywords
- inertial
- navigation
- attitude
- matrix
- error
- 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.)
- Expired - Fee Related
Links
Abstract
Be applicable to a SINS/CNS deep integrated navigation system for Marsokhod, it comprises inertial navigation subsystem, celestial navigation subsystem, inertial navigation position quantity measurement information tectonic element, inertial navigation attitude measurement information tectonic element and integrated navigation wave filter, mathematical platform and location matrix are supplied to inertial navigation attitude measurement information tectonic element by inertial navigation subsystem, also latitude and longitude information is supplied to inertial navigation position quantity measurement information tectonic element simultaneously, celestial navigation subsystem sends inertial attitude matrix to integrated navigation wave filter, the inertial attitude Input matrix of the inertial navigation that inertial navigation attitude measurement information tectonic element is constructed is to integrated navigation wave filter, the location vector information that inertial navigation position quantity measurement information tectonic element is constructed is input to integrated navigation wave filter, integrated navigation wave filter will be respectively celestial navigation subsystem and inertial navigation subsystem provides attitude and position estimation error, this system improves astronomical fixation precision, the requirement of independent navigation can be met, have broad application prospects.
Description
One, technical field
The utility model provides a kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod, it relates to a kind of inertial navigation/celestial navigation (that is: SINS/CNS) deep integrated navigation system and its implementation of being applicable to Marsokhod, belongs to integrated navigation technology field.
Two, background technology
Marsokhod is the important component part of mars exploration, and it can replace the mankind to perform detection mission in the unstructured moving grids of complexity, greatly saves detection cost, is the most effective instrument at present Mars being carried out to proximity detection.Marsokhod should have autonomous roaming and detecting function, martian surface automatic running hundreds of rice or even tens kilometers, can realize the simple directly exploration to Mars surface condition by self-contained scientific instrument.Marsokhod will realize independently roaming and completing scientific experiment, must possess high-precision independent navigation ability.The autonomous navigation system of Marsokhod is the basis realizing automatic obstacle avoidance and path planning in detection process, is the safe and effective important guarantee of finishing the work of Marsokhod.
Marsokhod carries out Navigation Control by land station by radio measurement and control usually, but due to rotation and revolution motion and the change etc. in land station geographic position relative to Marsokhod that brings of Mars and the earth, sometimes Marsokhod can be in land station cannot in the region of observing and controlling.Simultaneously because the Mars distance earth is very remote, radio measurement and control all cannot meet the independent navigation requirement of Marsokhod in real-time and reliability.Therefore, Marsokhod utilizes self-contained instrument and equipment to carry out independent navigation has become a major issue urgently to be resolved hurrily.
Existing autonomous navigation method is a lot, has celestial navigation, inertial navigation, earth-magnetic navigation, wheel odometer etc.Due to the Magnetic Field of unknown Mars, earth-magnetic navigation method is inapplicable at martian surface; Considering that areographic geology is soft, easily there is the phenomenons such as skidding in wheel, causes odometer error comparatively large, is not also suitable for Marsokhod independent navigation; And celestial navigation and inertial navigation belong to environment sensitive airmanship together, there is independence strong, good concealment, and these two kinds of navigate modes also has the feature of mutual supplement with each other's advantages.Therefore, astronomy/inertia combined navigation system is the first-selection of Marsokhod independent navigation.At present, the celestial navigation of relative maturity and the array mode of inertial navigation have simple combination pattern, the integrated mode based on gyroscopic drift correction, comprehensive optimum correction mode and dark integrated mode.
In simple combination pattern, inertial navigation (that is: SINS) works alone, the navigation informations such as position, attitude, speed are provided, the reference information that celestial navigation system and SINS provide combines, the position of carrier, attitude information can be obtained, and the position of SINS, course data are corrected.But owing to not correcting inertia device (gyroscope and accelerometer) error in this integrated mode use procedure, therefore, along with the increase of time, the position of this integrated mode, speed and attitude error are all dispersed.
Based on gyroscopic drift correct integrated mode also known as optimum combination pattern, by the high precision inertial attitude information that Large visual angle star sensor exports, the inertial attitude information constructed with SINS combines, by estimating and compensating gyroscopic drift error in SINS, by effectively revising the attitude error of SINS, the object improving navigation accuracy can be reached.But this integrated mode is owing to estimating the error of accelerometer and compensate, and therefore, along with the prolongation of time, its site error is still dispersed.
Comprehensive optimum correction mode must solve high-precision independent horizontal reference problem, and the method for the indirect responsive Horizon of conventional starlight refraction obtains horizontal reference at present, and martian atmosphere unknown-model, therefore this pattern is not suitable for the design of Marsokhod autonomous navigation system.
In dark integrated mode, SINS and celestial navigation system are assisted mutually, star sensor three axle is determined appearance principle and is obtained inertial attitude and assist the exportable high-precision Horizon information of SINS, and SINS provides Horizon information for astronomy, makes celestial navigation system can export high-precision positional information.And the difference position of strapdown inertial navitation system (SINS) and celestial navigation system, attitude exported is as measurement amount, estimates and correct the navigation calculation error of SINS.Junction filter can be estimated and compensating platform angle error and gyroscopic drift, thus improves navigation accuracy.Owing to can not estimate and the error that compensated acceleration meter zero causes partially, navigation information is still dispersed.Traditional dark combination can not meet the navigation accuracy of long running time.
In order to meet, Marsokhod is long-time, the requirement of high-precision independent navigation, and overcomes the restriction of celestial navigation horizontal reference, the utility model proposes a kind of SINS/CNS deep integrated navigation system and its implementation of being applicable to Marsokhod.
Three, summary of the invention
For problems of the prior art, the utility model proposes a kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod, by utilizing high-precision horizon reference, in conjunction with the high precision inertial attitude that Large visual angle star sensor provides, obtain accurate astronomical position vector, the position provided in conjunction with SINS again, attitude information, achieve effective estimation of inertial device error and correct, thus reaching the object increasing substantially navigation accuracy.
A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod of the utility model, comprises inertial navigation subsystem, celestial navigation subsystem, inertial navigation position quantity measurement information tectonic element, inertial navigation attitude measurement information tectonic element and integrated navigation wave filter, relation between them is: mathematical platform and location matrix are supplied to inertial navigation attitude measurement information tectonic element by inertial navigation subsystem, also latitude and longitude information is supplied to inertial navigation position quantity measurement information tectonic element simultaneously, celestial navigation subsystem sends inertial attitude matrix to integrated navigation wave filter, the inertial attitude Input matrix of the inertial navigation that inertial navigation attitude measurement information tectonic element is constructed is to integrated navigation wave filter, the location vector information that inertial navigation position quantity measurement information tectonic element is constructed is input to integrated navigation wave filter, integrated navigation wave filter will be respectively celestial navigation subsystem and inertial navigation subsystem provides attitude and position estimation error.
Described inertial navigation subsystem comprises inertial measurement component and inertial reference calculation unit, to be inertial measurement component be supplied to inertial reference calculation unit by relative to the angular velocity of inertial space and specific force to relation between them, an input as inertial reference calculation unit: inertial measurement component measures Marsokhod relative to the angular velocity of inertial space and specific force, is sent to inertial reference calculation unit by the angular velocity of gained with than force information; The information that inertial reference calculation unit transmits according to the initial position message of carrier and inertial measurement cluster, by position and the attitude matrix of mechanization real-time resolving Marsokhod;
This inertial measurement component is one used group, comprise three accelerometers and three gyroscopes (in existing used group, choose the element met the demands, require that gyro drift is 0.01 °/h, random drift is 0.01 °/h, and accelerometer constant value drift is 50 μ g, random bias is 10 μ g, inertia device data output period is 0.01s), by orthogonal installation, can the responsive angular velocity relative to inertial space and specific force.(h-hour, s-second)
This inertial reference calculation unit is that a specific force and angular velocity information inertial measurement component exported is through a series of calculating and renewal, calculate the real time position of carrier, speed, the algorithm of the navigation informations such as attitude, this algorithm flow chart is as Fig. 3, first the initial position of Marsokhod is inputted, the information such as speed and attitude, through initial alignment, and calculate Mars autorotation speed, position speed, again according to angular velocity and the specific force of Inertial Measurement Unit output, calculate attitude speed, thus renewal attitude matrix, then inertial navigation fundamental equation computing velocity increment is used, and quadratic integral obtains latitude and longitude information, finally calculate attitude angle, circulation like this, until system stalls, real-time resolving can go out the navigation information that inertial navigation provides, for follow-up integrated navigation provides Data support.\
Described celestial navigation subsystem comprises Large visual angle star sensor, many vectors determine appearance module and astronomical position vectorial structure unit, relation between them is: multiple fixed star starlight vectors that Large visual angle star sensor is observed are supplied to many vectors and determine appearance module, the input of appearance module is determined as many vectors, many vectors determine the input as astronomical position vectorial structure unit of inertial attitude matrix that appearance module exported, the mathematics standard benchmark of astronomical position vectorial structure unit combined high precision again, construct astronomical position vector: this Large visual angle star sensor is primarily of CCD device, peripheral sample circuit, signal processing circuit and optical lens four part composition, in existing Large visual angle star sensor, choose on request: its visual field is 20 ° × 20 °, measurement noise is 2 ", data output period is 1s.It can observe three and above fixed star starlight vector at synchronization, and its observation information is sent into many vectors determines appearance module; Many vectors determine the starlight vector information that appearance resume module receives, and obtain the attitude matrix of Marsokhod bodywork reference frame relative to inertial space; Astronomical position vectorial structure unit determines inertial attitude matrix that appearance module obtains and high-precision mathematics standard benchmark in conjunction with many vectors, constructs astronomical position vector;
This Large visual angle star sensor is a kind of conventional star sensor, and it has larger visual field, can observe the starlight vector of many fixed stars simultaneously.Fixed star starlight scioptics project in the battle array of CCD face, according to optical imaging concept and geometric knowledge, the expression of fixed star starlight under star sensor coordinate system can be obtained, and suppose star sensor coordinate system and bodywork reference frame completely the same, therefore, the information that can be obtained by star sensor, is obtained many expressions of fixed star vector under bodywork reference frame, determines appearance module provide information for many vectors.
This many vector determines the survey appearance principle of appearance module according to star sensor, equation S=GA can be obtained, wherein G is the dimension of n the starlight vector composition that Large visual angle star sensor exports is the matrix of n × 3, S is the dimension corresponding to the coordinate composition of n fixed star in inertial space in G is the matrix of n × 3, is solved the inertial attitude matrix A=(G of star sensor by employing least square method
tg)
-1(G
ts).
This astronomical position vectorial structure unit is a computing unit, the relation that it rotates according to attitude quaternion and rigid body, by under body system
axle rotates to and overlaps with the Z axis under geographic coordinate system, obtains vector
owing to there is certain relation of equivalence between attitude quaternion and pose transformation matrix under this change over condition, above-mentioned attitude quaternion can be replaced by high-precision mathematical platform, then, by inertial attitude matrix by vector
project to Mars inertial coordinates system, obtain position vector
thus constructed astronomical position vector.
The positional informations such as the longitude and latitude that described inertial navigation position quantity measurement information tectonic element is determined according to inertial navigation subsystem, determine the position vector of carrier, and it can be used as measurement information to input in integrated navigation wave filter; This inertial navigation position quantity measurement information tectonic element is a computing unit, by the relation of latitude and longitude information and itself and position vector, calculates inertial navigation position quantity measurement information
The navigation information such as position and attitude matrix that described inertial navigation attitude measurement information tectonic element exports according to inertial navigation subsystem, determines the inertial attitude matrix being tied to carrier coordinate system from mars equatorial inertial coordinate; This inertial navigation attitude measurement information tectonic element is also a computing unit, and it calculates inertial navigation strap-down matrix
transposition, location matrix
and the direction cosine matrix of the solid coordinate system m of Mars star is transformed into from Mars inertial coordinates system I
continued product, and using attitude measurement information that this result of calculation constructs as inertial navigation;
Described integrated navigation wave filter is Kalman filter, it is using SINS error equation as state equation, the difference of the inertial attitude matrix that appearance module provides respectively is determined as an observed quantity using inertial navigation attitude measurement information tectonic element and many vectors, simultaneously, the difference of the carrier positions Vector Message provided respectively using inertial navigation position quantity measurement information tectonic element and astronomical position vectorial structure unit, as another observed quantity, obtains the estimated value of Marsokhod position, attitude and inertial measurement cluster error by integrated navigation wave filter;
The SINS/CNS deep integrated navigation system of a kind of Marsokhod of the utility model, its implementation is as described in following steps:
Step one: Large visual angle star sensor assists strapdown inertial navitation system (SINS) to obtain high-precision horizon reference;
Conveniently following discussion, first defines several conventional coordinate system: navigational coordinate system OX
ny
nz
n(sky, northeast), bodywork reference frame OX
by
bz
b(on right front), Mars inertial coordinates system OX
iy
iz
i, the solid coordinate system OX of Mars star
my
mz
m.
Coordinate conversion matrix from bodywork reference frame b to navigational coordinate system n is horizontal reference, the system navigate error caused due to SINS alignment error and the gyroscopic drift of mathematics standard benchmark core component increased and accumulation with the working time, inertia mathematics standard fiducial error vibrates and disperses, its attitude error increases in time, and therefore the SINS attitude matrix of unmodified can not be used as mathematics standard benchmark.According to the relation of attitude matrix and attitude angle, the strap-down matrix of SINS
with desirable transition matrix
between attitude error can by the misaligned angle of the platform (φ
eφ
nφ
u) be expressed as:
Adopting Large visual angle star sensor to assist the method for correction SINS attitude matrix by estimating the misaligned angle of the platform, revising strap-down matrix
thus establish mathematics standard benchmark
and the angle of rotation speed of geographic coordinate system is combined with horizontal reference, make horizontal reference can the surface level of real-time follow-up car body point.Shown in (2):
The information such as the position of being resolved by inertial navigation, attitude, can obtain the direction cosine matrix that Mars inertial coordinate is tied to Marsokhod bodywork reference frame
the inertial attitude direction cosine matrix of appearance module acquisition is determined in conjunction with many vectors
the misaligned angle of the platform and the gyroscopic drift of inertial navigation can be estimated, and carry out correcting to obtain high-precision mathematics standard benchmark.Schematic diagram is as Fig. 4, and detailed process is as follows:
According to Present navigation time t and Mars spin velocity ω
imthe direction cosine matrix being transformed into the solid coordinate system m of Mars star from Mars inertial coordinates system I can be obtained
in conjunction with the strap-down matrix of SINS
and location matrix
have
The specific force exported by inertia device accelerometer and SINS resolve obtained strap-down matrix, by quadratic integral, can calculate the position λ of car body
i, L
i, then according to the relation of location matrix and longitude and latitude, can location matrix be obtained
If the site error that SINS resolves existence is δ λ, δ L, the therefore location matrix of SINS
with real location matrix
relation as follows:
Finally, the transformation matrix by SINS is calculated
the transformation matrix exported with star sensor
combine, by integrated navigation wave filter, obtain the estimated value of misalignment and gyroscopic drift
then by feeding back this estimated value to inertial navigation subsystem, SINS mathematical platform misalignment and gyroscopic drift being revised, high-precision mathematics standard benchmark can be obtained
Step 2: the astronomical position vector based on high-precision horizon reference is determined;
Due to navigational coordinate system OX
ny
nz
nthe Z in (sky, northeast)
naxle instruction local ground vertical line, can by Z
naxle is at the position vector of Mars inertial coordinates system I
as local position vector.And arrange,
represent the direction cosine matrix being transformed into coordinate system b by coordinate system a;
for the projection of unit vector under coordinate system b of the Z axis of coordinate system a.
Z in bodywork reference frame b
bthe coordinate of unit vector under car body system b of axle is
Vector rotation according to hypercomplex number represents: vector
around Y in car body system b
bthe negative direction of axle rotates γ (roll angle), obtains
again around b
1the X of system
b1the negative direction of axle rotates θ (angle of pitch), and quaternionic vector rotates with the relation of direction cosine matrix known, direction cosine matrix
also can the rotation of witness vector.Through direction cosine matrix
by vector
rotate to be the projection of Z axis vector of unit length under car body system of Department of Geography
after, pedal line direction Z in Department of Geography n can be obtained
nthe coordinate of unit vector in car body system b
for:
The appearance of novel Large visual angle star sensor, make to observe the nautical star of more than three and three become possibility simultaneously, under the prerequisite without any need for outside reference information, provide the attitude information in accurate car body relative inertness space, and precision keeps stable in overall process.If star sensor observes three and above nautical star simultaneously, and suppose that the nautical star number be observed is n, the unit starlight vector in inertial space is reference vector G
1, G
2..., G
n, the unit starlight vector under star sensor coordinate system is measurement vector S
1, S
2..., S
n.Two vector matrixs are written as with reference to vector measurement vector:
S=[S
1S
2… S
n] G=[G
1G
2… G
n]
Direction cosine
for body coordinate system is to the conversion of Earth central inertial system, therefore, following relation is met:
And obtain optimum solution by least square method
According to inertial space attitude matrix, the projection of position vector under Mars inertial system can be obtained
for
Due to structure measurement amount use SINS to provide horizontal reference, wherein comprise the misaligned angle of the platform, that is:
Therefore, make the true geographic position of car body for (λ L), then can obtain real Mars inertial coordinates system upper/lower positions vector
position vector under the same coordinate system resolved with celestial navigation system (i.e. CNS)
pass be:
Wherein t
gHAfor the Greenwich hour angle of Mars, in units of radian; λ
c, L
cbe respectively the Marsokhod longitude and latitude that astronomy calculates; α
c, δ
cbe respectively right ascension and the declination of the Marsokhod that celestial navigation system resolves.And the geographic position that SINS resolves is (λ
il
i), then the position vector being converted into Mars inertial coordinates system lower body is:
Wherein α
i, δ
ifor right ascension and the declination of the Marsokhod of inertial reference calculation.Therefore, the geographic position (λ that resolves of SINS
il
i) and the real geographic position of car body (λ L) between relation can be expressed as:
λ
I=λ+δλ
(11)
L
I=L+δL
Using above-mentioned site error as a small amount of, and its expression formula is brought into
in, ignore high-order and can obtain in a small amount:
To sum up, according to high-precision horizontal reference and star sensor measurement information, astronomical position vector can be obtained.
Step 3: state model and the measurement model of setting up integrated navigation system;
The error state of SINS comprises attitude error angle (φ
e, φ
n, φ
u), site error (δ L, δ λ, δ h), velocity error (δ V
e, δ V
n, δ V
u), gyro drift (ε
bx, ε
by, ε
bz), accelerometer bias (▽
bx, ▽
by, ▽
bz).SINS systematic error state equation is
Wherein, X=[φ
e, φ
n, φ
u, δ V
e, δ V
n, δ V
u, δ L, δ λ, δ h, ε
bx, ε
by, ε
bz, ▽
bx, ▽
by, ▽
bz]
tfor state variable; F is system state matrix, W=[ω
gx, ω
gy, ω
gz, ω
dx, ω
dy, ω
dz]
tfor system noise sequence, ω
gi(i=x, y, z), ω
di(i=x, y, z) is respectively gyroscope, accelerometer random white noise,
for strap-down matrix, G is noise matrix.F, F
sbe respectively with G
Wherein, F
nfor corresponding to the system dynamic matrix of 9 parameters (3 attitude errors, 3 velocity errors, 3 site errors) of SINS.
By formula (1) to formula (3), the Mars inertial coordinate that can obtain being gone out by the output construction of inertial reference calculation unit is tied to the direction cosine transformation matrix of bodywork reference frame
and then, the misaligned angle of the platform and site error are regarded as in a small amount, and ignore higher order term, then the inertial attitude matrix obtained by inertial navigation attitude measurement information tectonic element 4
through arranging:
Wherein Collision risk is:
The error of star sensor mainly contains error in measurement and alignment error, wherein error in measurement is rad level, and alignment error is angle classification, but alignment error can carry out critical alignment by demarcation, the attitude accuracy measured due to the star sensor after process alignment error calibration is higher, and its measuring error may be thought of as the white noise of zero-mean, so the transformation matrix that star sensor exports can think real transformation matrix
with the measurement white noise acoustic matrix V of star sensor
3 × 3sum, can represent such as formula (16).
Therefore, by inertial attitude matrix that the output that inertial navigation attitude measuring construction unit 5, many vectors determine appearance module 202 is determined respectively
difference be denoted as attitude measurement amount Z
s, have
And by Z
s (3 × 3)be launched into column vector Z
1 (9 × 1), in conjunction with the state vector X of combined system, can arrange and write out measurement equation,
Z
1=Η
1X+V
1(18)
Wherein, H
1=[H
m0
9 × 3h
n0
9 × 6] be attitude measurement matrix, V
1for the measurement white noise sequence of star sensor.
By the vertical line Vector Message that inertial position measuring construction unit 4, astronomical position vectorial structure unit 203 export
difference measure as position quantity, and bonding state vector X, can obtain measurement equation is:
Z
2=H
2X-V
2(19)
In formula,
h
2=[H
210
3 × 3h
220
3 × 7] be vertical line vector measurement matrix, V
2for CNS vertical line vector error,
Step 4: Information Fusion for Integrated Navigation Application;
The vertical line vector provided by inertial navigation position quantity measurement information tectonic element 3 and astronomical position vectorial structure unit 203
the attitude information that appearance module 202 provides is determined with inertial navigation attitude measurement information tectonic element 4 and many vectors
to do after difference as measurement information respectively, carry out information fusion by integrated navigation wave filter 5, the misaligned angle of the platform can be estimated, gyrostatic drift in inertial measurement component 101, accelerometer bias and inertial navigation subsystem 1 site error, velocity error.
Step 5: correct each sub-system error by information feed back;
According to the relation of attitude matrix and attitude angle, the attitude error of integrated navigation system directly represent the precision of the horizontal reference that Large visual angle star sensor 201 assists inertial navigation subsystem 1 to obtain.Only can estimate due to the dark integrated mode of tradition and correct the gyroscopic drift error relevant with attitudes such as the misaligned angle of the platforms, and it is inclined to fail corrected acceleration meter zero, site error is caused to be dispersed, and dispersing of site error can cause dispersing of attitude error, thus cause the mathematics Horizon provided by SINS to there is larger error, by correcting the site error of accelerometer bias in inertial measurement component and inertial navigation subsystem 1 based on the astronomical position vectorial structure unit 203 of horizontal reference, eliminate the trend that positioning error is dispersed in time, thus inhibit dispersing of attitude error.
Then observed by high precision Large visual angle star sensor 201, and determine by many vectors the inertial attitude information that appearance module 202 exports and assist continuously and revise inertial navigation subsystem 1, the attitude error of inertial navigation subsystem 1 is made to have converged in certain limit, improve its attitude accuracy, thus provide high-precision horizontal reference for celestial navigation subsystem 2; Meanwhile, combined high precision horizontal reference and Large visual angle star sensor 201 observation information, astronomical position vectorial structure unit can obtain more accurate local position vector, thus introduce celestial navigation subsystem 2 position vector measurement information to estimate accelerometer error, the site error of inertial navigation subsystem 1, and carry out feedback compensation, improve the precision of the precision of mathematics standard benchmark and the CNS position vector based on mathematics standard benchmark further.
Finally, by correcting accelerometer bias and the site error of SINS based on the CNS position vector of mathematics standard benchmark, eliminate the trend of determining appearance and positioning error and dispersing in time, system is obtained and determines appearance and positioning precision for a long time, and attitude and site error remain stable.
Wherein, " adopting Large visual angle star sensor to assist the method for correction SINS attitude matrix by estimating the misaligned angle of the platform, revising strap-down matrix described in step one
thus establish mathematics standard benchmark
"; be the multiple starlight vector information utilizing Large visual angle star sensor to provide; and determine appearance resume module by many vectors and obtain high-precision inertial attitude matrix; in conjunction with the inertial navigation inertial attitude information that inertial navigation attitude measurement information tectonic element provides; both make difference can construct attitude measurement amount, carries out information fusion, estimates the misaligned angle of the platform; revise strap-down matrix, finally set up mathematics standard benchmark by integrated navigation wave filter.
Wherein, described in step one " information such as the position of being resolved by inertial navigation, attitude, can obtain the direction cosine matrix that Mars inertial coordinate is tied to Marsokhod bodywork reference frame
the inertial attitude direction cosine matrix of appearance module acquisition is determined in conjunction with many vectors
the misaligned angle of the platform and the gyroscopic drift of inertial navigation can be estimated, and carry out correcting to obtain high-precision mathematics standard benchmark ", the stellar inertial attitude matrix namely utilizing many vectors to determine appearance module to provide
and the inertial navigation inertial attitude matrix that inertial navigation attitude measurement information tectonic element provides
respectively by matrix
with matrix
each element correspondence subtract each other, obtain nine observed quantities, set up the relation of these observed quantities and quantity of state, obtain measurement equation, carry out information fusion by integrated navigation wave filter, the misaligned angle of the platform and gyroscopic drift can be estimated, at strap-down matrix
basis on, through the misaligned angle of the platform correction, high-precision mathematics standard benchmark can be obtained
and inhibit dispersing of attitude error by revising gyroscopic drift.
Wherein, " specific force exported by inertia device accelerometer and SINS resolve obtained strap-down matrix " described in step one, because the specific force of inertia device accelerometer output is under car body system, and the specific force needed in inertial navigation fundamental equation is under navigational coordinate system, therefore car body is needed to be tied to the direction cosine matrix of navigation system, replace with the strap-down matrix that SINS resolves herein, thus can according to the increment of inertial navigation fundamental equation computing velocity.
Wherein, described in step one, " by quadratic integral, the position λ of car body can be calculated
i, L
i"; to the incremental integration of aforementioned obtained speed; and be added with the velocity in a upper moment velocity being this moment, integration is carried out to the velocity in this moment and was added with the positional information in a upper moment, the positional information λ in this moment can be obtained
i, L
i.
Wherein, described in step one, " again according to the relation of location matrix and longitude and latitude, location matrix can be obtained
", so-called location matrix is exactly the direction cosine matrix that the solid coordinate of Mars star is tied between navigational coordinate system; because navigational coordinate system is relevant with the Marsokhod position in each moment; therefore, location matrix is also relevant with longitude and latitude, the relation between them is provided by formula (4).
Wherein, described in step one " by the transformation matrix that SINS is calculated
the transformation matrix exported with star sensor
combine, by integrated navigation wave filter, obtain the estimated value of misalignment and gyroscopic drift
then by feeding back this estimated value to inertial navigation subsystem, SINS mathematical platform misalignment and gyroscopic drift being revised, high-precision mathematics standard benchmark can be obtained
", by the inertial navigation inertial attitude that inertial navigation attitude measurement information tectonic element is exported
the stellar inertial attitude that appearance module provides is determined with many vectors
combine, the element of each correspondence position is poor, difference is as an attitude measurement amount, and set up the relation of each measurement amount and quantity of state, set up measurement equation, information fusion is carried out through Kalman filtering, filter result can estimate the misaligned angle of the platform and gyroscopic drift, then respectively estimated information is fed back to inertial reference calculation unit and inertial measurement component, revise error and the inertial measurement component gyroscopic drift of the strap-down matrix of SINS, have modified the attitude error of strap-down matrix in real time, and fundamentally inhibit the generation of error, therefore high-precision mathematics standard benchmark can be obtained.
Wherein, described in step 2 " through direction cosine matrix
by vector
rotate to be the projection of Z axis vector of unit length under car body system of Department of Geography
after, pedal line direction Z in Department of Geography n can be obtained
nthe coordinate of unit vector in car body system b
", this is described by the rotation of hypercomplex number to rigid body, needs twice rotation: vector
around Y in car body system b
bthe negative direction of axle rotates γ (roll angle), obtains
again around b
1the X of system
b1the negative direction of axle rotates θ (angle of pitch), obtains
requirement can be reached.Horizontal reference direction cosine matrix forwards the projection of a vector to navigation system from car body system at least needs rotation three times, last rotate be around
axle rotates ψ (crab angle).Therefore, for
axle, it is identical for rotating secondary with the result of being rotated three times by horizontal reference direction cosine matrix by hypercomplex number, and they can obtain
when actual computation, because this direction cosine matrix is known, and attitude quaternion is unknown, therefore, is replace attitude quaternion to carry out the rotation of vector with direction cosine matrix.
Wherein, " making the true geographic position of car body for (λ L), then can obtain real Mars inertial coordinates system upper/lower positions vector described in step 2
position vector under the same coordinate system resolved with celestial navigation system and CNS
relation "; in the middle of the process resolving position vector at CNS; owing to introducing certain attitude error; therefore; have difference between the position vector under the position vector that CNS resolves and real Mars inertial coordinates system; this difference can be showed by platform error angle, as shown in formula (9), sets up attitude measurement equation provide foundation for subsequent combination Navigation.
Wherein, " the misaligned angle of the platform and site error regarded as in a small amount, and ignoring higher order term, then the inertial attitude matrix obtained by inertial navigation attitude measurement information tectonic element 4 described in step 3
", inertial navigation attitude measurement information tectonic element, by three direction cosine matrixs are connected multiplied arriving, introduces attitude error when calculating attitude matrix; equally because the navigation information of inertial navigation exists error; when construction location matrix, in turn introduce site error, these errors can respectively by
represent with δ P, and
all belong in a small amount with δ P, through Lian Chenghou, there will be high-order error term, herein this higher order term is ignored.Therefore by after consideration error, three direction cosine matrixs are connected and takes advantage of, just can obtain the inertial attitude matrix of inertial navigation attitude measurement information structure.
Wherein, described in step 3 " by the inertial attitude matrix that the output that inertial navigation attitude measuring construction unit 5, many vectors determine appearance module 202 is determined respectively
difference be denoted as attitude measurement amount Z
s", because formula (15) and (16) show respectively the relation between inertial attitude matrix and quantity of state that celestial navigation and inertial navigation provide, be therefore easy to the relation setting up attitude measurement amount and quantity of state, namely determine attitude measurement equation.
Wherein, described in step 3 " by the vertical line Vector Message that inertial position measuring construction unit 4, astronomical position vectorial structure unit 203 export
difference measure as position quantity, and bonding state vector X, can obtain measurement equation ", state vector X comprises site error, mesa corners error etc., in formula (9) and (12), describe two position vectors
relation between element respectively and in X, can set up position quantity thus and measure the relation with quantity of state, namely set up measurement equation.
Wherein, " information fusion is carried out by integrated navigation wave filter 5 described in step 4, the misaligned angle of the platform can be estimated, gyrostatic drift in inertial measurement component 101, the site error of accelerometer bias and inertial navigation subsystem 1, velocity error ", this integrated navigation wave filter is Kalman filter, through previous section to the foundation of system state equation and measurement equation and correlation filtering parameter, after the setting of filtering initial value, can information fusion be carried out and correlation behavior amount is estimated, when filtering reaches stable, the value of estimated quantity of state will level off to its actual value, for zero inclined and gyrostatic drift and the attitude of feedback compensation inertia device accelerometer, position, velocity error.
Wherein, described in step 5 " by correcting the site error of accelerometer bias in inertial measurement component and inertial navigation subsystem 1 based on the astronomical position vectorial structure unit 203 of horizontal reference, eliminate the trend that positioning error is dispersed in time, thus inhibit dispersing of attitude error ", because the site error of car body and attitude error are interactional, therefore, by introducing astronomical position vector and the position vector provided in conjunction with inertial navigation, can estimate and correct the zero inclined of the site error of car body and inertial measurement component accelerometer, what fundamentally solve site error disperses the position causing attitude error to be dispersed, therefore, improve positioning precision, on the other hand also for suppressing dispersing of attitude error to make certain contribution.
Wherein, " accelerometer bias of SINS and site error is corrected by the CNS position vector based on mathematics standard benchmark described in step 5, eliminate the trend of determining appearance and positioning error and dispersing in time, system is obtained and determines appearance and positioning precision for a long time, and attitude and site error remain stable ", inertial navigation and the astronomical inertial attitude matrix provided can solve to be estimated and corrects attitude error and gyroscopic drift, thus reach high-precision attitude, but due to the crosslinked relation of position and attitude in inertial reference calculation, if site error is dispersed, dispersing of attitude error will be caused, therefore, by based on the CAN position vector of mathematics standard benchmark and the information fusion of inertial navigation position vector, go the accelerometer bias and the site error that correct SINS, on the basis of eliminating the trend that positioning error is dispersed in time, also eliminate and determine appearance error dispersing in time, therefore, position and attitude error remains stable.
A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod of the utility model, its advantage is:
(1) the utility model by utilize the High Accuracy Observation information of Large visual angle star sensor 201 and through too much vector determine appearance module 202 process after the high precision inertial attitude information that exports, inertial navigation subsystem 1 can be assisted, thus obtain high-precision mathematics standard reference information.
(2) the utility model is by utilizing high-precision mathematics standard reference information, structure astronomical position Vector Message, thus obtains high-precision astronomical position information, further increases astronomical fixation precision.
(3) the utility model utilizes Kalman filter to carry out information fusion to the attitude of celestial navigation subsystem 2 and inertial navigation subsystem 1 and location vector information, to the misaligned angle of the platform, inertial measurement component 101 zero partially and carrier positions error carry out optimal estimation, by the feedback compensation to navigation error, improve the positioning precision of mathematics standard benchmark and carrier, the requirement of high-precision independent navigation can be met.
(4) the utility model by mutually the assisting of inertial navigation subsystem and celestial navigation subsystem in the process of integrated navigation, learn from other's strong points to offset one's weaknesses, give full play to inertial navigation subsystem and celestial navigation subsystem advantage separately, had broad application prospects.
Four, accompanying drawing explanation
Fig. 1 is navigational system structural representation described in the utility model;
Fig. 2 is navigational system implementation method process flow diagram described in the utility model;
Fig. 3 is that inertial navigation described in the utility model resolves process flow diagram;
Fig. 4 is the utility model high precision mathematical platform determination schematic diagram;
In Fig. 1:
1-inertial navigation subsystem, 2-celestial navigation subsystem, 3-inertial navigation position quantity measurement information tectonic element
4-inertial navigation attitude measurement information tectonic element, 5-integrated navigation wave filter
101-inertial measurement component, 102-inertial reference calculation unit
201-Large visual angle star sensor, 202-many vectors determine appearance module
203-astronomical position vectorial structure unit
the determined inertial attitude matrix of-celestial navigation subsystem
-high precision mathematics standard benchmark
the determined inertial attitude matrix of-inertial navigation subsystem
λ
il
ithe carrier longitude and latitude of-inertial reference calculation
the determined position vector of-inertial navigation subsystem
the determined position vector of-celestial navigation subsystem
In Fig. 4:
the determined inertial attitude matrix of-celestial navigation subsystem
the attitude matrix of-inertial navigation
the determined inertial attitude matrix of-inertial navigation subsystem
the location matrix of-inertial navigation
-the misaligned angle of the platform error
-high precision mathematics standard benchmark
-compare force information
-angular velocity information
-gyroscopic drift
the projection of-specific force under navigation system
-revised angular velocity information
-attitude speed
-position speed and the projection of earth rate sum under carrier coordinate system
Five, embodiment
See Fig. 1-4, below in conjunction with accompanying drawing, the utility model is described in further detail.
A kind of SINS/CNS deep integrated navigation system and its implementation being applicable to Marsokhod of the utility model.By utilizing the measurement information of Large visual angle star sensor, its attitude matrix of correction of auxiliary strapdown inertial navitation system (SINS), obtains high-precision mathematics standard benchmark
on this basis, high-precision mathematics standard benchmark is utilized
and the inertial attitude information that celestial navigation provides
by determining astronomical position vector
realize astronomical fixation.Again by inertial navigation inertial attitude matrix that inertial navigation attitude measurement information tectonic element and inertial navigation position quantity measurement information tectonic element obtain respectively
with inertial navigation position vector
finally, in conjunction with the error model of strapdown inertial navitation system (SINS), utilize integrated navigation wave filter to estimate and feedback compensation systematic error, thus realize high-precision inertia/celestial combined navigation.
See Fig. 1, a kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod of the utility model, comprises inertial navigation subsystem 1, celestial navigation subsystem 2, inertial navigation position quantity measurement information tectonic element 3, inertial navigation attitude measurement information tectonic element 4 and integrated navigation wave filter 5;
Inertial navigation subsystem 1 comprises inertial measurement component 101 and inertial reference calculation unit 102; Inertial measurement component 101 records Marsokhod relative to the angular velocity of inertial space and specific force, and is outputted to inertial reference calculation unit 102; Inertial reference calculation unit 102 is according to the information of inertial measurement component 101, and the mechanization algorithm in conjunction with inertial navigation calculates the position (λ of Marsokhod in real time
il
i) and attitude information;
Celestial navigation subsystem 2 comprises Large visual angle star sensor 201, many vectors determine appearance module 202 and astronomical position vectorial structure unit 203; Large visual angle star sensor 201 observes the starlight of more than three and three fixed stars obtain starlight vector information simultaneously, and this information is supplied to many vectors determines appearance module 202; Many vectors are determined appearance module 202 and are processed the starlight vector information received, and obtain the attitude matrix of Marsokhod relative to inertial space
astronomical position vectorial structure unit 203 combined high precision mathematics standard benchmark and many vectors determine the inertial attitude that appearance module 202 provides, and obtain astronomical position vector;
The Marsokhod latitude and longitude information that inertial navigation position quantity measurement information tectonic element 3 provides according to inertial reference calculation unit 102, can construct the position vector that inertial navigation provides;
The Marsokhod attitude matrix that inertial navigation attitude measurement information tectonic element 4 provides according to inertial navigation stage unit 102 and location matrix, can construct Marsokhod that inertial navigation the provides inertial attitude matrix relative to inertial space;
Integrated navigation wave filter 5, in conjunction with the error model of strapdown inertial navitation system (SINS), determines the stellar inertial attitude information of appearance module 202 output with many vectors
the inertial navigation inertial attitude information of inertial navigation attitude measurement information tectonic element 4 structure
and the astronomical position vector of astronomical position vectorial structure unit 203 structure
the inertial navigation position vector of inertial navigation position quantity measurement information tectonic element 3 structure
make difference between two as measurement amount, set up measurement equation, carry out integrated navigation information fusion, obtain system error information and compensated by feedback compensation;
See Fig. 2, a kind of implementation method being applicable to the SINS/CNS deep integrated navigation system of Marsokhod of the utility model, specifically comprises the following steps:
Step one: Large visual angle star sensor assists strapdown inertial navitation system (SINS) to obtain high-precision horizon reference;
Conveniently following discussion, first defines several conventional coordinate system: navigational coordinate system OX
ny
nz
n(sky, northeast), bodywork reference frame OX
by
bz
b(on right front), Mars inertial coordinates system OX
iy
iz
i, the solid coordinate system OX of Mars star
my
mz
m.
Coordinate conversion matrix from bodywork reference frame b to navigational coordinate system n is horizontal reference, the system navigate error caused due to SINS alignment error and the gyroscopic drift of mathematics standard benchmark core component increased and accumulation with the working time, inertia mathematics standard fiducial error vibrates and disperses, its attitude error increases in time, and therefore the SINS attitude matrix of unmodified can not be used as mathematics standard benchmark.According to the relation of attitude matrix and attitude angle, the strap-down matrix of SINS
with desirable transition matrix
between attitude error can by the misaligned angle of the platform (φ
eφ
nφ
u) be expressed as:
Adopting Large visual angle star sensor to assist the method for correction SINS attitude matrix by estimating the misaligned angle of the platform, revising strap-down matrix
thus establish mathematics standard benchmark
and the angle of rotation speed of geographic coordinate system is combined with horizontal reference, make horizontal reference can the surface level of real-time follow-up car body point.Shown in (2):
The information such as the position of being resolved by inertial navigation, attitude, can obtain the direction cosine matrix that Mars inertial coordinate is tied to Marsokhod bodywork reference frame
the inertial attitude direction cosine matrix of appearance module acquisition is determined in conjunction with many vectors
the misaligned angle of the platform and the gyroscopic drift of inertial navigation can be estimated, and carry out correcting to obtain high-precision mathematics standard benchmark.Schematic diagram is as Fig. 4, and detailed process is as follows:
According to Present navigation time t and Mars spin velocity ω
imthe direction cosine matrix being transformed into the solid coordinate system m of Mars star from Mars inertial coordinates system I can be obtained
in conjunction with the strap-down matrix of SINS
and location matrix
have
The specific force exported by inertia device accelerometer and SINS resolve obtained strap-down matrix, by quadratic integral, can calculate the position λ of car body
i, L
i, then according to the relation of location matrix and longitude and latitude, can location matrix be obtained
If the site error that SINS resolves existence is δ λ, δ L, the therefore location matrix of SINS
with real location matrix
relation as follows:
Finally, the transformation matrix by SINS is calculated
the transformation matrix exported with star sensor
combine, by integrated navigation wave filter, obtain the estimated value of misalignment and gyroscopic drift
then by feeding back this estimated value to inertial navigation subsystem, SINS mathematical platform misalignment and gyroscopic drift being revised, high-precision mathematics standard benchmark can be obtained
Step 2: the astronomical position vector based on high-precision horizon reference is determined;
Due to navigational coordinate system OX
ny
nz
nthe Z in (sky, northeast)
naxle instruction local ground vertical line, can by Z
naxle is at the position vector of Mars inertial coordinates system I
as local position vector.And arrange,
represent the direction cosine matrix being transformed into coordinate system b by coordinate system a;
for the projection of unit vector under coordinate system b of the Z axis of coordinate system a.
Z in bodywork reference frame b
bthe coordinate of unit vector under car body system b of axle is
Vector rotation according to hypercomplex number represents: vector
around Y in car body system b
bthe negative direction of axle rotates γ (roll angle), obtains
again around b
1the X of system
b1the negative direction of axle rotates θ (angle of pitch), and quaternionic vector rotates with the relation of direction cosine matrix known, direction cosine matrix
also can the rotation of witness vector.Through direction cosine matrix
by vector
rotate to be the projection of Z axis vector of unit length under car body system of Department of Geography
after, pedal line direction Z in Department of Geography n can be obtained
nthe coordinate of unit vector in car body system b
for:
The appearance of novel Large visual angle star sensor, make to observe the nautical star of more than three and three become possibility simultaneously, under the prerequisite without any need for outside reference information, provide the attitude information in accurate car body relative inertness space, and precision keeps stable in overall process.If star sensor observes three and above nautical star simultaneously, and suppose that the nautical star number be observed is n, the unit starlight vector in inertial space is reference vector G
1, G
2..., G
n, the unit starlight vector under star sensor coordinate system is measurement vector S
1, S
2..., S
n.Two vector matrixs are written as with reference to vector measurement vector:
S=[S
1S
2… S
n] G=[G
1G
2… G
n]
Direction cosine
for body coordinate system is to the conversion of Earth central inertial system, therefore, following relation is met:
And obtain optimum solution by least square method
According to inertial space attitude matrix, the projection of position vector under Mars inertial system can be obtained
for
Due to structure measurement amount use SINS to provide horizontal reference, wherein comprise the misaligned angle of the platform, that is:
Therefore, make the true geographic position of car body for (λ L), then can obtain real Mars inertial coordinates system upper/lower positions vector
position vector under the same coordinate system resolved with celestial navigation system (CNS)
pass be:
Wherein t
gHAfor the Greenwich hour angle of Mars, in units of radian; λ
c, L
cbe respectively the Marsokhod longitude and latitude that astronomy calculates; α
c, δ
cbe respectively right ascension and the declination of the Marsokhod that celestial navigation system resolves.And the geographic position that SINS resolves is (λ
il
i), then the position vector being converted into Mars inertial coordinates system lower body is:
Wherein α
i, δ
ifor right ascension and the declination of the Marsokhod of inertial reference calculation.Therefore, the geographic position (λ that resolves of SINS
il
i) and the real geographic position of car body (λ L) between relation can be expressed as:
λ
I=λ+δλ
(11)
L
I=L+δL
Using above-mentioned site error as a small amount of, and its expression formula is brought into
in, ignore high-order and can obtain in a small amount:
To sum up, according to high-precision horizontal reference and star sensor measurement information, astronomical position vector can be obtained.
Step 3: state model and the measurement model of setting up integrated navigation system;
The error state of SINS comprises attitude error angle (φ
e, φ
n, φ
u), site error (δ L, δ λ, δ h), velocity error (δ V
e, δ V
n, δ V
u), gyro drift (ε
bx, ε
by, ε
bz), accelerometer bias (▽
bx, ▽
by, ▽
bz).SINS systematic error state equation is
Wherein, X=[φ
e, φ
n, φ
u, δ V
e, δ V
n, δ V
u, δ L, δ λ, δ h, ε
bx, ε
by, ε
bz, ▽
bx, ▽
by, ▽
bz]
tfor state variable; F is system state matrix, W=[ω
gx, ω
gy, ω
gz, ω
dx, ω
dy, ω
dz]
tfor system noise sequence, ω
gi(i=x, y, z), ω
di(i=x, y, z) is respectively gyroscope, accelerometer random white noise,
for strap-down matrix, G is noise matrix.F, F
sbe respectively with G
Wherein, F
nfor corresponding to the system dynamic matrix of 9 parameters (3 attitude errors, 3 velocity errors, 3 site errors) of SINS.
By formula (1) to formula (3), the Mars inertial coordinate that can obtain being gone out by the output construction of inertial reference calculation unit is tied to the direction cosine transformation matrix of bodywork reference frame
and then, the misaligned angle of the platform and site error are regarded as in a small amount, and ignore higher order term, then the inertial attitude matrix obtained by inertial navigation attitude measurement information tectonic element 4
through arranging:
Wherein Collision risk is:
The error of star sensor mainly contains error in measurement and alignment error, wherein error in measurement is rad level, and alignment error is angle classification, but alignment error can carry out critical alignment by demarcation, the attitude accuracy measured due to the star sensor after process alignment error calibration is higher, and its measuring error may be thought of as the white noise of zero-mean, so the transformation matrix that star sensor exports can think real transformation matrix
with the measurement white noise acoustic matrix V of star sensor
3 × 3sum, can represent such as formula (16).
Therefore, by inertial attitude matrix that the output that inertial navigation attitude measuring construction unit 5, many vectors determine appearance module 202 is determined respectively
difference be denoted as attitude measurement amount Z
s, have
And by Z
s (3 × 3)be launched into column vector Z
1 (9 × 1), in conjunction with the state vector X of combined system, can arrange and write out measurement equation,
Z
1=Η
1X+V
1(18)
Wherein, H
1=[H
m0
9 × 3h
n0
9 × 6] be attitude measurement matrix, V
1for the measurement white noise sequence of star sensor.
By the vertical line Vector Message that inertial position measuring construction unit 4, astronomical position vectorial structure unit 203 export
difference measure as position quantity, and bonding state vector X, can obtain measurement equation is:
Z
2=H
2X-V
2(19)
In formula,
h
2=[H
210
3 × 3h
220
3 × 7] be vertical line vector measurement matrix, V
2for CNS vertical line vector error,
Step 4: Information Fusion for Integrated Navigation Application;
The vertical line vector provided by inertial navigation position quantity measurement information tectonic element 3 and astronomical position vectorial structure unit 203
the attitude information that appearance module 202 provides is determined with inertial navigation attitude measurement information tectonic element 4 and many vectors
to do after difference as measurement information respectively, carry out information fusion by integrated navigation wave filter 5, the misaligned angle of the platform can be estimated, gyrostatic drift in inertial measurement component 101, accelerometer bias and inertial navigation subsystem 1 site error, velocity error.
Step 5: correct each sub-system error by information feed back;
According to the relation of attitude matrix and attitude angle, the attitude error of integrated navigation system directly represent the precision of the horizontal reference that Large visual angle star sensor 201 assists inertial navigation subsystem 1 to obtain.Only can estimate due to the dark integrated mode of tradition and correct the gyroscopic drift error relevant with attitudes such as the misaligned angle of the platforms, and it is inclined to fail corrected acceleration meter zero, site error is caused to be dispersed, and dispersing of site error can cause dispersing of attitude error, thus cause the mathematics Horizon provided by SINS to there is larger error, by correcting the site error of accelerometer bias in inertial measurement component and inertial navigation subsystem 1 based on the astronomical position vectorial structure unit 203 of horizontal reference, eliminate the trend that positioning error is dispersed in time, thus inhibit dispersing of attitude error.
Then observed by high precision Large visual angle star sensor 201, and determine by many vectors the inertial attitude information that appearance module 202 exports and assist continuously and revise inertial navigation subsystem 1, the attitude error of inertial navigation subsystem 1 is made to have converged in certain limit, improve its attitude accuracy, thus provide high-precision horizontal reference for celestial navigation subsystem 2; Meanwhile, combined high precision horizontal reference and Large visual angle star sensor 201 observation information, astronomical position vectorial structure unit can obtain more accurate local position vector, thus introduce celestial navigation subsystem 2 position vector measurement information to estimate accelerometer error, the site error of inertial navigation subsystem 1, and carry out feedback compensation, improve the precision of the precision of mathematics standard benchmark and the CNS position vector based on mathematics standard benchmark further.
Finally, by correcting accelerometer bias and the site error of SINS based on the CNS position vector of mathematics standard benchmark, eliminate the trend of determining appearance and positioning error and dispersing in time, system is obtained and determines appearance and positioning precision for a long time, and attitude and site error remain stable.
Wherein, " adopting Large visual angle star sensor to assist the method for correction SINS attitude matrix by estimating the misaligned angle of the platform, revising strap-down matrix described in step one
thus establish mathematics standard benchmark
"; be the multiple starlight vector information utilizing Large visual angle star sensor to provide; and determine appearance resume module by many vectors and obtain high-precision inertial attitude matrix; in conjunction with the inertial navigation inertial attitude information that inertial navigation attitude measurement information tectonic element provides; both make difference can construct attitude measurement amount, carries out information fusion, estimates the misaligned angle of the platform; revise strap-down matrix, finally set up mathematics standard benchmark by integrated navigation wave filter.
Wherein, described in step one " information such as the position of being resolved by inertial navigation, attitude, can obtain the direction cosine matrix that Mars inertial coordinate is tied to Marsokhod bodywork reference frame
the inertial attitude direction cosine matrix of appearance module acquisition is determined in conjunction with many vectors
the misaligned angle of the platform and the gyroscopic drift of inertial navigation can be estimated, and carry out correcting to obtain high-precision mathematics standard benchmark ", the stellar inertial attitude matrix namely utilizing many vectors to determine appearance module to provide
and the inertial navigation inertial attitude matrix that inertial navigation attitude measurement information tectonic element provides
respectively by matrix
with matrix
each element correspondence subtract each other, obtain nine observed quantities, set up the relation of these observed quantities and quantity of state, obtain measurement equation, carry out information fusion by integrated navigation wave filter, the misaligned angle of the platform and gyroscopic drift can be estimated, at strap-down matrix
basis on, through the misaligned angle of the platform correction, high-precision mathematics standard benchmark can be obtained
and inhibit dispersing of attitude error by revising gyroscopic drift.
Wherein, " specific force exported by inertia device accelerometer and SINS resolve obtained strap-down matrix " described in step one, because the specific force of inertia device accelerometer output is under car body system, and the specific force needed in inertial navigation fundamental equation is under navigational coordinate system, therefore car body is needed to be tied to the direction cosine matrix of navigation system, replace with the strap-down matrix that SINS resolves herein, thus can according to the increment of inertial navigation fundamental equation computing velocity.
Wherein, described in step one, " by quadratic integral, the position λ of car body can be calculated
i, L
i"; to the incremental integration of aforementioned obtained speed; and be added with the velocity in a upper moment velocity being this moment, integration is carried out to the velocity in this moment and was added with the positional information in a upper moment, the positional information λ in this moment can be obtained
i, L
i.
Wherein, described in step one, " again according to the relation of location matrix and longitude and latitude, location matrix can be obtained
", so-called location matrix is exactly the direction cosine matrix that the solid coordinate of Mars star is tied between navigational coordinate system; because navigational coordinate system is relevant with the Marsokhod position in each moment; therefore, location matrix is also relevant with longitude and latitude, the relation between them is provided by formula (4).
Wherein, described in step one " by the transformation matrix that SINS is calculated
the transformation matrix exported with star sensor
combine, by integrated navigation wave filter, obtain the estimated value of misalignment and gyroscopic drift
then by feeding back this estimated value to inertial navigation subsystem, SINS mathematical platform misalignment and gyroscopic drift being revised, high-precision mathematics standard benchmark can be obtained
", by the inertial navigation inertial attitude that inertial navigation attitude measurement information tectonic element is exported
the stellar inertial attitude that appearance module provides is determined with many vectors
combine, the element of each correspondence position is poor, difference is as an attitude measurement amount, and set up the relation of each measurement amount and quantity of state, set up measurement equation, information fusion is carried out through Kalman filtering, filter result can estimate the misaligned angle of the platform and gyroscopic drift, then respectively estimated information is fed back to inertial reference calculation unit and inertial measurement component, revise error and the inertial measurement component gyroscopic drift of the strap-down matrix of SINS, have modified the attitude error of strap-down matrix in real time, and fundamentally inhibit the generation of error, therefore high-precision mathematics standard benchmark can be obtained.
Wherein, described in step 2 " through direction cosine matrix
by vector
rotate to be the projection of Z axis vector of unit length under car body system of Department of Geography
after, pedal line direction Z in Department of Geography n can be obtained
nthe coordinate of unit vector in car body system b
", this is described by the rotation of hypercomplex number to rigid body, needs twice rotation: vector
around Y in car body system b
bthe negative direction of axle rotates γ (roll angle), obtains
again around b
1the X of system
b1the negative direction of axle rotates θ (angle of pitch), obtains
requirement can be reached.Horizontal reference direction cosine matrix forwards the projection of a vector to navigation system from car body system at least needs rotation three times, last rotate be around
axle rotates ψ (crab angle).Therefore, for
axle, it is identical for rotating secondary with the result of being rotated three times by horizontal reference direction cosine matrix by hypercomplex number, and they can obtain
when actual computation, because this direction cosine matrix is known, and attitude quaternion is unknown, therefore, is replace attitude quaternion to carry out the rotation of vector with direction cosine matrix.
Wherein, " making the true geographic position of car body for (λ L), then can obtain real Mars inertial coordinates system upper/lower positions vector described in step 2
position vector under the same coordinate system resolved with CNS (celestial navigation system)
relation "; in the middle of the process resolving position vector at CNS; owing to introducing certain attitude error; therefore; have difference between the position vector under the position vector that CNS resolves and real Mars inertial coordinates system; this difference can be showed by platform error angle, as shown in formula (9), sets up attitude measurement equation provide foundation for subsequent combination Navigation.
Wherein, " the misaligned angle of the platform and site error regarded as in a small amount, and ignoring higher order term, then the inertial attitude matrix obtained by inertial navigation attitude measurement information tectonic element 4 described in step 3
", inertial navigation attitude measurement information tectonic element, by three direction cosine matrixs are connected multiplied arriving, introduces attitude error when calculating attitude matrix; equally because the navigation information of inertial navigation exists error; when construction location matrix, in turn introduce site error, these errors can respectively by
represent with δ P, and
all belong in a small amount with δ P, through Lian Chenghou, there will be high-order error term, herein this higher order term is ignored.Therefore by after consideration error, three direction cosine matrixs are connected and takes advantage of, just can obtain the inertial attitude matrix of inertial navigation attitude measurement information structure.
Wherein, described in step 3 " by the inertial attitude matrix that the output that inertial navigation attitude measuring construction unit 5, many vectors determine appearance module 202 is determined respectively
difference be denoted as attitude measurement amount Z
s", because formula (15) and (16) show respectively the relation between inertial attitude matrix and quantity of state that celestial navigation and inertial navigation provide, be therefore easy to the relation setting up attitude measurement amount and quantity of state, namely determine attitude measurement equation.
Wherein, described in step 3 " by the vertical line Vector Message that inertial position measuring construction unit 4, astronomical position vectorial structure unit 203 export
difference measure as position quantity, and bonding state vector X, can obtain measurement equation ", state vector X comprises site error, mesa corners error etc., in formula (9) and (12), describe two position vectors
relation between element respectively and in X, can set up position quantity thus and measure the relation with quantity of state, namely set up measurement equation.
Wherein, " information fusion is carried out by integrated navigation wave filter 5 described in step 4, the misaligned angle of the platform can be estimated, gyrostatic drift in inertial measurement component 101, the site error of accelerometer bias and inertial navigation subsystem 1, velocity error ", this integrated navigation wave filter is Kalman filter, through previous section to the foundation of system state equation and measurement equation and correlation filtering parameter, after the setting of filtering initial value, can information fusion be carried out and correlation behavior amount is estimated, when filtering reaches stable, the value of estimated quantity of state will level off to its actual value, for zero inclined and gyrostatic drift and the attitude of feedback compensation inertia device accelerometer, position, velocity error.
Wherein, described in step 5 " by correcting the site error of accelerometer bias in inertial measurement component and inertial navigation subsystem 1 based on the astronomical position vectorial structure unit 203 of horizontal reference, eliminate the trend that positioning error is dispersed in time, thus inhibit dispersing of attitude error ", because the site error of car body and attitude error are interactional, therefore, by introducing astronomical position vector and the position vector provided in conjunction with inertial navigation, can estimate and correct the zero inclined of the site error of car body and inertial measurement component accelerometer, what fundamentally solve site error disperses the position causing attitude error to be dispersed, therefore, improve positioning precision, on the other hand also for suppressing dispersing of attitude error to make certain contribution.
Wherein, " accelerometer bias of SINS and site error is corrected by the CNS position vector based on mathematics standard benchmark described in step 5, eliminate the trend of determining appearance and positioning error and dispersing in time, system is obtained and determines appearance and positioning precision for a long time, and attitude and site error remain stable ", inertial navigation and the astronomical inertial attitude matrix provided can solve to be estimated and corrects attitude error and gyroscopic drift, thus reach high-precision attitude, but due to the crosslinked relation of position and attitude in inertial reference calculation, if site error is dispersed, dispersing of attitude error will be caused, therefore, by based on the CAN position vector of mathematics standard benchmark and the information fusion of inertial navigation position vector, go the accelerometer bias and the site error that correct SINS, on the basis of eliminating the trend that positioning error is dispersed in time, also eliminate and determine appearance error dispersing in time, therefore, position and attitude error remains stable.
Claims (1)
1. be applicable to a SINS/CNS deep integrated navigation system for Marsokhod, it is characterized in that: comprise inertial navigation subsystem, celestial navigation subsystem, inertial navigation position quantity measurement information tectonic element, inertial navigation attitude measurement information tectonic element and integrated navigation wave filter, relation between them is: mathematical platform and location matrix are supplied to inertial navigation attitude measurement information tectonic element by inertial navigation subsystem, also latitude and longitude information is supplied to inertial navigation position quantity measurement information tectonic element simultaneously, celestial navigation subsystem sends inertial attitude matrix to integrated navigation wave filter, the inertial attitude Input matrix of the inertial navigation that inertial navigation attitude measurement information tectonic element is constructed is to integrated navigation wave filter, the location vector information that inertial navigation position quantity measurement information tectonic element is constructed is input to integrated navigation wave filter, integrated navigation wave filter will be respectively celestial navigation subsystem and inertial navigation subsystem provides attitude and position estimation error,
Described inertial navigation subsystem comprises inertial measurement component and inertial reference calculation unit, to be inertial measurement component be supplied to inertial reference calculation unit by relative to the angular velocity of inertial space and specific force to relation between them, an input as inertial reference calculation unit: inertial measurement component measures Marsokhod relative to the angular velocity of inertial space and specific force, is sent to inertial reference calculation unit by the angular velocity of gained with than force information; The information that inertial reference calculation unit transmits according to the initial position message of carrier and inertial measurement cluster, by position and the attitude matrix of mechanization real-time resolving Marsokhod;
This inertial measurement component is one used group, comprises three accelerometers and three gyroscopes; In existing used group, choose the element met the demands, require that gyro drift is 0.01 °/h, random drift is 0.01 °/h, accelerometer constant value drift is 50 μ g, and random bias is 10 μ g, and inertia device data output period is 0.01s, by orthogonal installation, can sensitive carrier relative to the angular velocity of inertial space and specific force;
Described celestial navigation subsystem comprises Large visual angle star sensor, many vectors determine appearance module and astronomical position vectorial structure unit, relation between them is: multiple fixed star starlight vectors that Large visual angle star sensor is observed are supplied to many vectors and determine appearance module, the input of appearance module is determined as many vectors, many vectors determine the input as astronomical position vectorial structure unit of inertial attitude matrix that appearance module exported, the mathematics standard benchmark of astronomical position vectorial structure unit combined high precision again, construct astronomical position vector: this Large visual angle star sensor is by CCD device, peripheral sample circuit, signal processing circuit and optical lens four part composition, in existing Large visual angle star sensor, choose on request: its visual field is 20 ° × 20 °, measurement noise is 2 ", data output period is 1s.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201420262058.9U CN204255368U (en) | 2014-05-21 | 2014-05-21 | A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201420262058.9U CN204255368U (en) | 2014-05-21 | 2014-05-21 | A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod |
Publications (1)
Publication Number | Publication Date |
---|---|
CN204255368U true CN204255368U (en) | 2015-04-08 |
Family
ID=52959665
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201420262058.9U Expired - Fee Related CN204255368U (en) | 2014-05-21 | 2014-05-21 | A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN204255368U (en) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104833375A (en) * | 2015-05-19 | 2015-08-12 | 北京控制工程研究所 | IMU (Inertial Measurement Unit) two-position alignment method by virtue of star sensor |
CN106153051A (en) * | 2016-06-29 | 2016-11-23 | 上海航天控制技术研究所 | A kind of spacecraft shading device combined navigation methods |
CN111783271A (en) * | 2020-05-11 | 2020-10-16 | 北京控制工程研究所 | Nonlinear correction method for spacecraft triple control system |
CN113432608A (en) * | 2021-02-03 | 2021-09-24 | 东南大学 | Generalized high-order CKF algorithm based on maximum correlation entropy and suitable for INS/CNS integrated navigation system |
-
2014
- 2014-05-21 CN CN201420262058.9U patent/CN204255368U/en not_active Expired - Fee Related
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104833375A (en) * | 2015-05-19 | 2015-08-12 | 北京控制工程研究所 | IMU (Inertial Measurement Unit) two-position alignment method by virtue of star sensor |
CN104833375B (en) * | 2015-05-19 | 2017-07-28 | 北京控制工程研究所 | A kind of IMU Two position methods by star sensor |
CN106153051A (en) * | 2016-06-29 | 2016-11-23 | 上海航天控制技术研究所 | A kind of spacecraft shading device combined navigation methods |
CN106153051B (en) * | 2016-06-29 | 2019-04-19 | 上海航天控制技术研究所 | A kind of spacecraft shading device combined navigation methods |
CN111783271A (en) * | 2020-05-11 | 2020-10-16 | 北京控制工程研究所 | Nonlinear correction method for spacecraft triple control system |
CN111783271B (en) * | 2020-05-11 | 2023-08-29 | 北京控制工程研究所 | Nonlinear correction method for three-override control system of spacecraft |
CN113432608A (en) * | 2021-02-03 | 2021-09-24 | 东南大学 | Generalized high-order CKF algorithm based on maximum correlation entropy and suitable for INS/CNS integrated navigation system |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103994763B (en) | The SINS/CNS deep integrated navigation system of a kind of Marsokhod and its implementation | |
CN101788296B (en) | SINS/CNS deep integrated navigation system and realization method thereof | |
CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
CN101413800B (en) | Navigating and steady aiming method of navigation / steady aiming integrated system | |
CN103424114B (en) | A kind of full combined method of vision guided navigation/inertial navigation | |
CN111156994B (en) | INS/DR & GNSS loose combination navigation method based on MEMS inertial component | |
CN101949703B (en) | Strapdown inertial/satellite combined navigation filtering method | |
CN102608596B (en) | Information fusion method for airborne inertia/Doppler radar integrated navigation system | |
CN101246012B (en) | Combinated navigation method based on robust dissipation filtering | |
CN103900611B (en) | Method for aligning two composite positions with high accuracy and calibrating error of inertial navigation astronomy | |
CN103389092B (en) | A kind of kite balloon airship attitude measuring and measuring method | |
CN103674034B (en) | Multi-beam test the speed range finding revise robust navigation method | |
CN201955092U (en) | Platform type inertial navigation device based on geomagnetic assistance | |
Konrad et al. | Advanced state estimation for navigation of automated vehicles | |
CN104713555A (en) | Autonomous vehicle navigation method for assisting orientation by applying omnimax neutral point | |
CN103674021A (en) | Integrated navigation system and method based on SINS (Strapdown Inertial Navigation System) and star sensor | |
CN105371844A (en) | Initialization method for inertial navigation system based on inertial / celestial navigation interdependence | |
CN1995916A (en) | Integrated navigation method based on star sensor calibration | |
CN109612460B (en) | Plumb line deviation measuring method based on static correction | |
CN204255368U (en) | A kind of SINS/CNS deep integrated navigation system being applicable to Marsokhod | |
CN103604428A (en) | Star sensor positioning method based on high-precision horizon reference | |
CN114739425A (en) | Coal mining machine positioning calibration system based on RTK-GNSS and total station and application method | |
CN103968844A (en) | Large ellipse maneuverable spacecraft autonomous navigation method based on low-orbit platform tracking measurement | |
CN104501809A (en) | Attitude coupling-based strapdown inertial navigation/star sensor integrated navigation method | |
CN104634348B (en) | Attitude angle computational methods in integrated navigation |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20150408 Termination date: 20170521 |