CN102095424A - Attitude measuring method suitable for vehicle fiber AHRS (Attitude and Heading Reference System) - Google Patents

Abstract

The invention belongs to the technical field of inertial navigation and particularly relates to an attitude measuring method suitable for a vehicle fiber AHRS (Attitude and Heading Reference System). In the invention, a low-cost fiber AHRS based on a Kalman filter and a GPS (Global Positioning System) integrated navigation system are adopted to detect and insolate a GPS fault signal and ensure the reliability of a SINS (Ship's Inertial Navigation)/GPS integrated system before the initial alignment of the SINS. After the output of the GPS is effective, the difference values between the coarse angle, the north speed and the south speed output by the SINS with the corresponding information output by a GPS receiver are selected as observation quantities to form a three-dimensional observation equation. In the invention, the calculation speed can be greatly increased to realize the real-time filter calculation. Compared with a combined system using a speed and a position as observation quantities, the navigation accuracy of the vehicle fiber AHRS, particularly the accuracy of an attitude computer is greatly improved.

Description

A kind of attitude measurement method that is fit to vehicle-mounted optical fiber boat appearance system

Technical field

The present invention relates to belong to the inertial navigation technology field, be specifically related to a kind of attitude measurement method that is fit to vehicle-mounted optical fiber boat appearance system.Be particularly useful for high dynamically, the vehicular platform that attitude accuracy is had relatively high expectations.

Background technology

Along with onboard navigation system only benefit of application demand in civil area increases, the onboard combined navigation technology is developed rapidly in recent years.Cost is low, precision has reliably become the basic demand of user to civilian onboard navigation system.The GPS attitude determination system has advantages such as cost is low, power consumption is little, volume is little, good data long-term stability; But the GPS attitude determination system also has the defective of himself, and for example data updating rate is low, short-term stability is poor, restricted by the gps signal quality, and in the zone of serious shielding with when cycle slip takes place, Attitude Calculation will make a mistake etc.Strapdown inertial navitation system (SINS) (SINS) can provide continuous carrier positions, speed, attitude information, but because gyro error constantly accumulation in time, the error that attitude is determined increases thereupon, in order to satisfy the accuracy requirement that attitude is determined, it obviously is inapplicable using the pure-inertial guidance scheme under condition cheaply.

The patent No. is to have adopted following method in 200710144846.2 " carrier posture measuring method that is suitable for optical fibre gyro ": initial position parameters and the initial velocity value of determining carrier by external unit; Fiber optic gyro strapdown inertial navigation system carries out initial alignment, determines the initial attitude of the relative navigation coordinate of carrier system, obtains the initial value of attitude quaternion; Determine the posture renewal cycle; Gather the increment of the carrier of optical fibre gyro, accelerometer output with respect to the angular speed calculation rotating vector of inertial coordinates system; By the relation of rotating vector and hypercomplex number, obtain posture renewal hypercomplex number in the posture renewal cycle; Upgrade attitude quaternion by the attitude quaternion renewal equation; Calculating carrier coordinate system b system is the strapdown matrix T of n system with respect to navigation coordinate; Ask the steps such as attitude angle of the relative navigation coordinate of carrier system.

Since the various measuring methods of attitude of carrier of original optical fibre gyro often with speed, position as observed quantity, even some method adopts GPS speed and attitude information as observed quantity, assessment precision to each parameter is higher, when adopting GPS position and velocity information as reference information, assessment precision to attitude error is greatly improved, but, the assessment precision of position and velocity error is not had clear improvement.Do not introduce behind the GPS attitude information when assessment precision of inertial navigation attitude error obviously introduced attitude information and will get well, but little to the assessment influential effect of position, speed, do not improve significantly.

Summary of the invention

Technical matters to be solved by this invention provides a kind of attitude measurement method that is fit to vehicle-mounted optical fiber boat appearance system, on the vehicular applications platform, the realization high-accuracy posture is measured, for vehicular platform provides high-accuracy posture information, thereby reduction is to the accuracy requirement of optical fiber boat appearance system optical fibre gyro and accelerometer.

For solving the problems of the technologies described above, technical scheme of the present invention is:

1, a kind of attitude measurement method that is fit to vehicle-mounted optical fiber boat appearance system adopts low-cost optical fiber boat appearance system and GPS integrated navigation system based on Kalman filter, it is characterized in that,

Resolve flow process according to system, after GPS output effectively, the attitude that attitude, speed and the SINS that utilizes GPS to export exports, the difference of speed specifically comprise the steps: as observed quantity

At first set up integrated navigation system state equation based on the SINS error equation, and on the basis of error equation, set up the range equation of combined system, come the error in measurement state of the error state propagation characteristic and the system of descriptive system with the first-order linear stochastic differential equation, equation is as follows:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z(t)＝H(t)X(t)+V(t)……………………………………(2)

Choose the SINS navigational parameter error of calculation, fibre optic gyroscope and accelerometer error state variable, totally 12 tie up, that is: as system

$X^T=[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}}]---\left(3\right)$

In the formula: φ _E, φ _N, φ _UBe SINS Attitude Calculation error, δ V _EWith δ V _NVelocity error for SINS; δ L and δ λ are the SINS site error; ε _Bx, ε _ByAnd ε _BzBe 3 axial gyroscopic drift errors

Be the accelerometer measures error;

The course angle, north orientation, east orientation speed of choosing SINS output and the difference of GPS receiver output corresponding information constitute 3 and tie up observation equations, that is: as observed quantity

$Z\left(t\right)=\left[\begin{array}{c}{Z}_v\\ Z_{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{H}_v\left(t\right)\\ H_{\mathrm{\ψ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_v\left(t\right)\\ V_{\mathrm{\ψ}}\left(t\right)\end{array}\right]---\left(4\right)$

In the formula, H _V(t) be the measurement matrix of observed quantity when being speed, H _ψ(t) be the measurement matrix of observed quantity when being attitude, its medium velocity measures vector and is:

$Z_v=\left[\begin{array}{c}{v}_{\mathrm{SINS}.N}-v_{\mathrm{GPS}.N}\\ v_{\mathrm{SINSE}}-v_{\mathrm{GPS}.E}\end{array}\right]=H_v{X}_v+V_v---\left(5\right)$

In the formula, v _SINS.N, v _SINS.EBe the north orientation and the east orientation velocity information of SINS output, v _SINS.N=v _N+ δ v _N, v _SINS.E=v _E+ δ v _E, v wherein _N, v _EBe the true velocity of carrier along geographical co-ordinate system north orientation and east orientation; v _GPS.N, v _GPS.EBe the north orientation of GPS receiver output and the velocity information of east orientation, v _GPS.N=v _N-M _N, v _GPS.E=v _E-M _E, M wherein _N, ME is GPS receiver range rate error, is made as white noise, H _vBe the measurement matrix of observed quantity when being speed; H _v=[diag[1 1] 0 _{2 * 10}], V _vBe the measurement noise of GPS receiver output speed: V _v=[M _NM _E] ^T, getting measurement noise is white noise,

Z _ψ＝ψ _SINS.yaw-ψ _GPS.yaw＝H _ψX _ψ+V _ψ……………………………(6)

In the formula, ψ _SINS.yawBe the course angle of SINS output, ψ _SINS.yaw=ψ _Aw+ δ ψ _SINS.yaw, ψ wherein _AwBe the true course angle, δ ψ _SINS.yawBe SINS course angle measuring error; ψ _GPS.yawBe the course angle of two GPS outputs, ψ _GPS.yaw=ψ _Yaw+ δ ψ _GPS.yaw, δ ψ wherein _GPS.yawBe two GPS course angle measuring error.Decide on two GPS receiver measuring accuracy and base length;

H _ψBe the measurement matrix of observed quantity when being attitude, H _ψ=[0 _{1 * 2}10 _{1 * 9}]; V _ψBe the measurement noise of two GPS output course angles, V _ψ=δ ψ _GPS.yay, getting measurement noise is white noise, establishes GPS course measurement white noise variance to be R then _ψ=(HDOP. δ _ψ) ²

State equation and observation equation after discrete are:

X _K＝Φ _k，k-1X _k-1+Γ _k-1W _k-1……………………………….…(8)

Z _K＝H _kK _k+V _k

Kalman filtering algorithm is as follows, next step prediction of state:

State estimation:

Filter gain:

Next step predicts mean square deviation:

Estimate mean square deviation: $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.$

Preferably, before the SINS initial alignment, the GPS fault-signal is detected and isolate, guarantee the reliability of SINS/GPS combined system, concrete grammar is as follows:

A, judge that gps data detects and whether be positioned at SINS evaluated error band;

B, judge whether the GPS receiver exists and lose satellite, be i.e. data new phenomenon more not;

C, judge whether gps data exists sudden change;

D, residual error χ ²Method of inspection.

The invention has the beneficial effects as follows that the present invention can accelerate computing velocity greatly, realize Real-Time Filtering calculating, and compare as the combined system of observed quantity with speed+position, its navigation accuracy especially precision of Attitude Calculation machine improves a lot.

Description of drawings

Below in conjunction with the drawings and specific embodiments technical scheme of the present invention is further described in detail.

Fig. 1 resolves theory diagram for system of the present invention.

Fig. 2 is a SINS navigation calculation theory diagram of the present invention.

Embodiment

According to Fig. 1 system-computed workflow, after system powers on, GPS carries out initialization, comprise base length of output data form that GPS is set, output frequency, two gps antennas etc., GPS powers on after the operate as normal, and double antenna measures GPS double difference phase observation value, obtains high-precision attitude of carrier information, its precision is according to the base length in two skies, and its measuring accuracy can reach 0.05 ° (4m base length).Navigational computer receives attitude, position, the velocity information of GPS output, and fault diagnosis is carried out in the output of GPS, judges that its output information can use.Though GPS has advantages such as global and high precision, also has weak point,, signal low as the data turnover rate easily is blocked and multipath effect etc.These shortcomings cause the signal of GPS output to have following fault characteristic: the output of (1) GPS no datat; (2) GPS has data output, but data are not upgraded; (3) GPS has data output, and there is bigger sudden change in data.For the fault characteristic of above-mentioned GPS, intend taking following measure to detect and isolating.

A) judge whether the gps data detection is positioned at SINS evaluated error band;

B) judging whether the GPS receiver exists loses satellite (data are not upgraded) phenomenon;

C) judge whether gps data exists sudden change;

D) residual error χ ²Method of inspection;

Utilize said method that the GPS fault-signal is detected and isolate, thereby guarantee the reliability of SINS/GPS combined system.

According to Fig. 1 system-computed workflow, after definite GPS information can be used, the output information of GPS provided information for the SINS initial alignment.According to shown in Figure 1, after finishing, aligning enters SINS navigation calculation module, and SINS navigation calculation module is seen accompanying drawing 2.

As shown in Figure 2, enter SINS and resolve module.SINS resolves module and comprises the calculating of strapdown matrix and instant correction, the hypercomplex number of hypercomplex number

Best normalization, specific force coordinate conversion and speed is revised immediately, the resolving of the instant correction of location matrix, position speed and earth rate, the calculating of attitude angle speed, the calculating of attitude angle, the calculating of position, the calculating of acceleration of gravity, altitude channel are resolved etc.Enter in the SINS navigation calculation module, gather optical fibre gyro and accelerometer output information earlier, and resolve the carrier of optical fibre gyro, accelerometer output with respect to the angular velocity of inertial coordinates system, the increment that apparent acceleration calculates rotating vector, according to the flow process of resolving shown in the accompanying drawing 2, calculate carrier navigate relatively three-dimensional velocity, position, attitude information under being.

As shown in Figure 1, resolve flow process according to system, after GPS output effectively, the attitude that attitude, speed and the SINS that utilizes GPS to export exports, the difference of speed are as observed quantity.At first set up integrated navigation system state equation, and on the basis of error equation, set up the range equation of combined system based on the SINS error equation.Come the error in measurement state of the error state propagation characteristic and the system of descriptive system with the first-order linear stochastic differential equation, equation is as follows:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z(t)＝H(t)X(t)+V(t)……………………………………(2)

Choose the SINS navigational parameter error of calculation, fibre optic gyroscope and accelerometer error state variable, totally 12 tie up, that is: as system

$X^T=[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}}]---\left(3\right)$

In the formula: φ _E, φ _N, φ _UBe SINS Attitude Calculation error, δ V _EWith δ V _NVelocity error for SINS; δ L and δ λ are the SINS site error; ε _Bx, ε _ByAnd ε _BzBe 3 axial gyroscopic drift errors Be the accelerometer measures error.

The course angle, north orientation, east orientation speed of choosing SINS output and the difference of GPS receiver output corresponding information constitute 3 and tie up observation equations, that is: as observed quantity

$Z\left(t\right)=\left[\begin{array}{c}{Z}_v\\ Z_{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{H}_v\left(t\right)\\ H_{\mathrm{\ψ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_v\left(t\right)\\ V_{\mathrm{\ψ}}\left(t\right)\end{array}\right]---\left(4\right)$

In the formula, H _V(t) be the measurement matrix of observed quantity when being speed, H _ψ(t) be the measurement matrix of observed quantity when being attitude.Its medium velocity measures vector:

$Z_v=\left[\begin{array}{c}{v}_{\mathrm{SINS}.N}-v_{\mathrm{GPS}.N}\\ v_{\mathrm{SINSE}}-v_{\mathrm{GPS}.E}\end{array}\right]=H_v{X}_v+V_v---\left(5\right)$

In the formula, v _SINS.N, v _SINS.EBe the north orientation and the east orientation velocity information of SINS output, v _SINS.N=v _N+ δ v _N, v _SINS.E=v _E+ δ v _E, v wherein _N, v _EBe the true velocity of carrier along geographical co-ordinate system north orientation and east orientation; v _GPS.N, v _GPS.EBe the north orientation of GPS receiver output and the velocity information of east orientation, v _GPS.N=v _N-M _N, v _GPS.E=v _E-M _E, M wherein _N, M _EBe GPS receiver range rate error, be made as white noise.H _vBe the measurement matrix of observed quantity when being speed; H _v=[diag[1 1] 0 _{2 * 10}], V _vBe the measurement noise of GPS receiver output speed: V _v=[M _NM _E] ^T, getting measurement noise is white noise.

Z _ψ＝ψ _SINS.yaw-ψ _GPS.yaw＝H _ψX _ψ+V _ψ……………………………(6)

In the formula, ψ _SINS.yawBe the course angle of SINS output, ψ _SINS.yaw=ψ _Aw+ δ ψ _SINS.yaw, ψ wherein _AwBe the true course angle, δ ψ _SINS.yawBe SINS course angle measuring error; ψ _GPS.yawBe the course angle of two GPS outputs, ψ _GPS.yaw=ψ _Yaw+ δ ψ _GPS.yaw, δ ψ wherein _GPS.yawBe two GPS course angle measuring error.Decide on two GPS receiver measuring accuracy and base length.

H _ψBe the measurement matrix of observed quantity when being attitude, H _ψ=[0 _{1 * 2}10 _{1 * 9}]; V _ψBe the measurement noise of two GPS output course angles, V _ψ=δ ψ _GPS.yaw, getting measurement noise is white noise, establishes GPS course measurement white noise variance to be

R then _ψ=(HDOP. δ _ψ) ²

State equation and observation equation after discrete are:

X _K＝Φ _k，k-1X _k-1+Γ _k-1W _k-1……………………………….…(8)

Z _K＝H _kK _k+V _k

Kalman filtering algorithm is as follows, next step prediction of state:

State estimation:

Filter gain: Next step predicts mean square deviation:

Estimate mean square deviation: $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.$

Attitude matrix can be expressed as the relation of crab angle ψ, pitching angle theta and roll angle γ, can be realized by three conversions of following order by geographic coordinate system to carrier coordinate system: 1. change the ψ degree around the y axle, 2. change the θ degree around the z axle, 3. change the γ degree around the x axle.That is:

Therefore, utilize attitude matrix

In coherent element, just can solve the main value at attitude of carrier angle, promptly

ψ _Main=tg ^-1(T ₃₁/ T ₁₁)

γ _Main=tg ^-1(T ₂₃/ T ₂₂)

Because the variation range of the angle of pitch, roll angle and crab angle is utilized ψ respectively at [90 ° 90 °], [180 ° 180 °] and [180 ° 180 °] _Main, θ _MainAnd γ _MainJudge the true value of ψ, θ and γ.

It should be noted last that, above embodiment is only unrestricted in order to technical scheme of the present invention to be described, although the present invention is had been described in detail with reference to preferred embodiment, those of ordinary skill in the art is to be understood that, can make amendment or be equal to replacement technical scheme of the present invention, and not breaking away from the spirit and scope of technical solution of the present invention, it all should be encompassed in the middle of the claim scope of the present invention.

Claims (2)

1. an attitude measurement method that is fit to vehicle-mounted optical fiber boat appearance system adopts low-cost optical fiber boat appearance system and GPS integrated navigation system based on Kalman filter, it is characterized in that,

Resolve flow process according to system, after GPS output effectively, the attitude that attitude, speed and the SINS that utilizes GPS to export exports, the difference of speed specifically comprise the steps: as observed quantity

At first set up integrated navigation system state equation based on the SINS error equation, and on the basis of error equation, set up the range equation of combined system, come the error in measurement state of the error state propagation characteristic and the system of descriptive system with the first-order linear stochastic differential equation, equation is as follows:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

Z(t)＝H(t)X(t)+V(t)……………………………………(2)

Choose the SINS navigational parameter error of calculation, fibre optic gyroscope and accelerometer error state variable, totally 12 tie up, that is: as system

$X^T=[{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,\mathrm{\δL},\mathrm{\δ\λ},{\mathrm{\ϵ}}_{\mathrm{bx}},{\mathrm{\ϵ}}_{\mathrm{by}},{\mathrm{\ϵ}}_{\mathrm{bz}},{\▿}_{\mathrm{bx}},{\▿}_{\mathrm{by}}]---\left(3\right)$

In the formula: φ _E, φ _N, φ _UBe SINS Attitude Calculation error, δ V _EWith δ V _NVelocity error for SINS; δ L and δ λ are S INS site error; ε _Bx, ε _ByAnd ε _BzBe 3 axial gyroscopic drift errors

Be the accelerometer measures error;

The course angle, north orientation, east orientation speed of choosing SINS output and the difference of GPS receiver output corresponding information constitute 3 and tie up observation equations, that is: as observed quantity

$Z\left(t\right)=\left[\begin{array}{c}{Z}_v\\ Z_{\mathrm{\ψ}}\end{array}\right]=\left[\begin{array}{c}{H}_v\left(t\right)\\ H_{\mathrm{\ψ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_v\left(t\right)\\ V_{\mathrm{\ψ}}\left(t\right)\end{array}\right]---\left(4\right)$

In the formula, H _V(t) be the measurement matrix of observed quantity when being speed, H _ψ(t) be the measurement matrix of observed quantity when being attitude, its medium velocity measures vector and is:

$Z_v=\left[\begin{array}{c}{v}_{\mathrm{SINS}.N}-v_{\mathrm{GPS}.N}\\ v_{\mathrm{SINSE}}-v_{\mathrm{GPS}.E}\end{array}\right]=H_v{X}_v+V_v---\left(5\right)$

In the formula, v _SINS.N, v _SINS.EBe the north orientation and the east orientation velocity information of SINS output, v _SINS.N=v _N+ δ v _N, v _SINS.E=v _E+ δ v _E, v wherein _N, v _EBe the true velocity of carrier along geographical co-ordinate system north orientation and east orientation; v _GPS.N, v _GPS.EBe the north orientation of GPS receiver output and the velocity information of east orientation, v _GPS.N=v _N-M _N, v _GPS.E=v _E-M _E, M wherein _N, M _EBe GPS receiver range rate error, be made as white noise, H _vBe the measurement matrix of observed quantity when being speed; H _v=[diag[1 1] 0 _{2 * 10}], V _vBe the measurement noise of GPS receiver output speed: V _v=[M _NM _E] ^T, getting measurement noise is white noise,

Z _ψ＝ψ _SINS.yaw-ψ _GPS.yaw＝H _ψX _ψ+V _ψ……………………………(6)

In the formula, ψ _SINS.yawBe the course angle of SINS output, ψ _SINS.yaw=ψ _Aw+ δ ψ _SINS.yaw, ψ wherein _AwBe the true course angle, δ ψ _SINS.yawBe SINS course angle measuring error; ψ _GPS.yawBe the course angle of two GPS outputs, ψ _GPS.yaw=ψ _Yaw+ δ ψ _GPS.yaw, δ ψ wherein _GPS.yawBe two GPS course angle measuring error.Decide on two GPS receiver measuring accuracy and base length;

H _ψBe the measurement matrix of observed quantity when being attitude, H _ψ=[0 _{1 * 2}10 _{1 * 9}]; V _ψBe the measurement noise of two GPS output course angles, V _ψ=δ ψ _GPS.yaw, getting measurement noise is white noise, establishes GPS course measurement white noise variance to be

R then _ψ=(HDOP. δ _ψ) ²State equation and observation equation after discrete are:

X _K＝Φ _k，k-1X _k-1+Γ _k-1W _k-1……………………………….…(8)

Z _K＝H _kK _k+V _k

Kalman filtering algorithm is as follows, next step prediction of state:

State estimation:

Filter gain: Next step predicts mean square deviation:

Estimate mean square deviation: $P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T.$

2. according to the described a kind of attitude measurement method that is fit to vehicle-mounted optical fiber boat appearance system of claim 1, it is characterized in that, before the SINS initial alignment, the GPS fault-signal detected and isolates that guarantee the reliability of SINS/GPS combined system, concrete grammar is as follows:

A, judge that gps data detects and whether be positioned at SINS evaluated error band;

B, judge whether the GPS receiver exists and lose satellite, be i.e. data new phenomenon more not;

C, judge whether gps data exists sudden change;

D, residual error χ ²Method of inspection.

Images