CN106197475A - Gyro based on Sequential filter and Magnetic Sensor combined calibrating method - Google Patents

Abstract

The invention provides a kind of gyro based on Sequential filter and Magnetic Sensor combined calibrating method, the present invention fully changes attitude in uniform magnetic field, and the measured value of synchronous acquisition magnetometer and gyro, owing to the data of magnetometer and gyro are provided commonly for the combined calibrating that the magnetometer inner parameter of the present invention, magnetometer external parameter and gyro zero are inclined, therefore demarcate effect good.Can go out magnetometer inside and outside portion parameter with real-time calibration and gyro zero is inclined, and not affected by any acceleration noise, the equipment that therefore need not in implementation process keeps resting state.The present invention can be additionally used in the attitude registration of Magnetic Sensor and Inertial Measurement Unit (including gyro and accelerometer).

Description

Gyro based on Sequential filter and Magnetic Sensor combined calibrating method

Technical field

The present invention relates to sensor technical field, in particular it relates to a kind of gyro based on Sequential filter and Magnetic Sensor Combined calibrating method.

Background technology

Gyro and Magnetic Sensor (the latter has another name called magnetometer, gaussmeter) are frequently used for attitude and determine or scientific measurement field. The angular velocity of gyro sensitive carrier, magnetometer sensitivity environmental magnetic field.When magnetometer is near ferromagnetic material, around magnetometer Magnetic field distorted, it is impossible to correct measurement goes out magnetic field intensity.Magnetic interference can be divided into Hard Magnetic effect and soft magnetism effect two kinds.Firmly Magnetic effect is the additivity magnetic disturbance produced by permanent magnet or electric current, and soft magnetism effect is produced by soft magnetic materials induction, is in background Soft magnetic materials in magnetic field can induce the magnetic field producing self, and the intensity and direction to background magnetic field produces distortion.Except this Outside, because of manufacturing process imperfection, magnetometer there is also constant multiplier, sensitive axes cross-couplings and biases equal error, therefore, Before using magnetometer, it is necessary to above error is carried out calibration, i.e. carry out the internal demarcation of magnetometer.

When magnetometer is used together with gyro, it is necessary to carry out magnetometer extrinsic calibration, i.e. demarcate magnetometer and gyro Between coordinate system misalignment.Soft magnetism effect not only can cause the change of magnetometer inner parameter, also results in magnetometer and top The coordinate system misalignment of spiral shell changes.Therefore, before the use, the internal demarcation of magnetometer is required for carrying out with extrinsic calibration. The magnetometer scaling method that presently, there are or require that magnetometer is internal and demarcate and extrinsic calibration is successively carried out respectively, or require quiet Accelerometer information is gathered under the conditions of state.On the other hand, the zero offset error of low cost gyro (such as MEMS gyro) is relatively big, and often All being varied from during secondary use, if not doing compensation directly use gyro to measure value, the effect of magnetometer extrinsic calibration will be affected.

Summary of the invention

For defect of the prior art, it is an object of the invention to provide a kind of gyro based on Sequential filter and sense with magnetic Device combined calibrating method.

The gyro based on Sequential filter provided according to the present invention and Magnetic Sensor combined calibrating method, including walking as follows Rapid:

Step 1: set up the state-space model of combined calibrating problem；

Step 2: use Sequential filter method to estimate the state of state-space model.

Preferably, in described step 1, the state-space model of combined calibrating problem is as follows:

System equation:

${\stackrel{\·}{C}}_b^i=C_b^i({\mathrm{\ω}}_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

Wherein: the body coordinate system of gyro/accelerometer is expressed as b system, and inertial coodinate system is expressed as i system, i.e. initial time B system；

In formula:Represent the inertial coodinate system attitude matrix relative to body coordinate system,RepresentTime-derivative, Representing the angular velocity vector under the b system of gyro to measure, ε represents gyro zero partially,Represent the time-derivative of ε, n_gRepresent gyro to measure Gaussian noise, n_εRepresenting the Gaussian noise that gyro zero is inclined, matrix S represents magnetometer parameter matrix,The time of representing matrix S Derivative, vector h represents magnetometer zero partially,Represent the time-derivative of h, mⁱIt is vectorial for the lower stationary magnetic field represented of i system,Represent mⁱTime-derivative, n_miRepresent the magnetic field vector error caused because of earth rotation；Wherein: computing a × expression by a three-dimensional to Amount a=[a₁ a₂ a₃]^TConstitute multiplication cross matrix, specifically, a × expansion as follows:

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

Observational equation:

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

In formula: y_mRepresent the measured value of magnetometer,For attitude matrixTransposition, i.e.Subscript T represents The transposition computing of vector, n_mRepresent the Gaussian noise of magnetometer measured value.

Preferably, the state of the state-space model in step 1 includes outside attitude, magnetometer inner parameter, magnetometer Parameter, gyro zero are partially and magnetic field vector；Magnetic field vector mⁱInitial value include: the measured value first of magnetometer；Magnetometer parameter The initial value of matrix S includes: unit matrix or give in advance；The initial value of the inclined ε of gyro zero and the inclined h of magnetometer zero elects zero as；Cause The relation of state ornamental, attitudeInitial value be defined as unit matrix, corresponding initial variance matrix is zero.

Preferably, described step 2 includes: use Sequential filter method to estimate the state of state-space model, described sequence Pass through filtering method to include: EKF, or particle filter method；Specifically, when using EKF, including Following steps:

Step 2.1: definition status error delta x is estimated valueDeduct true value x, i.e.δ represents corresponding states Error, and Attitude estimation valueWith attitude true valueAnd the contextual definition of attitude error ψ is

Then corresponding state error vector representation is δ x ≡ [ψ^T δεT vec^T(δS) δh^T δm^iT]^T, vec () represents will The computing that matrix gets up according to the sequential concatenation of row；ψ^TRepresent the transposition of attitude error ψ, ε^TRepresent turning of gyro zero deflection amount ε Put, h^T、m^iTRepresent h, m respectivelyⁱTransposition；

The approximately linear state-space model of state error is given as follows

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ {\mathrm{\δy}}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

Wherein: noise

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

In formula:Represent the time-derivative of δ x, matrix F, G, H_mRepresent sytem matrix, system input matrix, observation respectively Matrix；I representation unit matrix, the subscript representation unit order of matrix number of I；0 represents null matrix, and the subscript of 0 represents null matrix Ranks number；SymbolRepresenting matrix and the Kronecker product of matrix；

Step 2.2: without loss of generality, it is assumed that when the size of earth-magnetic field vector is 1, the result of EKF is entered The following rescaling of row；Adjustment formula is as follows:

S_rs=| | mⁱ| | S,

mⁱ _rs=mⁱ/||mⁱ||；

In formula: S_rsRepresent the magnetometer matrix after rescaling, mⁱ _rsRepresent the magnetic field vector after rescaling, | | | | Represent modulo operation.

Compared with prior art, the present invention has a following beneficial effect:

In the present invention, magnetometer is fixing with gyro is connected, and fully changes attitude, and synchronous acquisition magnetic force in uniform magnetic field Instrument and the measured value of gyro, owing to the data of magnetometer and gyro are provided commonly for the magnetometer inner parameter of the present invention, magnetometer External parameter and the inclined combined calibrating of gyro zero, therefore to determine effect good for zero standard.Magnetometer inside and outside portion ginseng can be gone out with real-time calibration Number and gyro zero partially, and are not affected by any acceleration noise, and the equipment that therefore need not in implementation process keeps static State.The present invention can be additionally used in the attitude registration of Magnetic Sensor and Inertial Measurement Unit (including gyro and accelerometer).

Detailed description of the invention

Below in conjunction with specific embodiment, the present invention is described in detail.Following example will assist in the technology of this area Personnel are further appreciated by the present invention, but limit the present invention the most in any form.It should be pointed out that, the ordinary skill to this area For personnel, without departing from the inventive concept of the premise, it is also possible to make some changes and improvements.These broadly fall into the present invention Protection domain.

The gyro based on Sequential filter provided according to the present invention and Magnetic Sensor combined calibrating method, including walking as follows Rapid:

Step 1: set up the state-space model of combined calibrating problem；

Step 2: use Sequential filter method to estimate the state of state-space model.

In described step 1, the state-space model of combined calibrating problem is as follows:

System equation:

${\stackrel{\·}{C}}_b^i=C_b^i(h_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

In formula:Represent the inertial coodinate system attitude matrix relative to body coordinate system,RepresentTime-derivative, Representing the angular velocity vector under the b system of gyro to measure, ε represents gyro zero partially,Represent the time-derivative of ε, n_gRepresent gyro to measure Gaussian noise, n_εRepresenting the Gaussian noise that gyro zero is inclined, matrix S represents magnetometer parameter matrix,The time of representing matrix S Derivative, vector h represents magnetometer zero partially,Represent the time-derivative of h, mⁱIt is vectorial for the lower stationary magnetic field represented of i system,Represent mⁱTime-derivative, n_miRepresent the magnetic field vector error caused because of earth rotation；Wherein: computing a × expression by a three-dimensional to Amount a=[a₁ a₂ a₃]^TConstitute multiplication cross matrix, specifically, a × expansion as follows:

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

Observational equation:

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

In formula: ym represents the measured value of magnetometer,For attitude matrixTransposition, i.e.Subscript T represents The transposition computing of vector, n_mRepresent the Gaussian noise of magnetometer measured value.

Specifically, it is not general, it is assumed that the magnetic field intensity of uniform magnetic field is 1, i.e. | | mⁱ| |=1.The shape of model above State includes that attitude, magnetometer inner parameter, magnetometer external parameter, gyro zero are partially and magnetic field vector.Magnetic field vector mⁱInitial Value is chosen as the measured value first of magnetometer, and the initial value of magnetometer parameter matrix S is chosen as unit matrix or gives in advance, top The initial value of spiral shell zero inclined ε and magnetometer parameter matrix h is chosen as zero.Because of the relation of state ornamental, attitudeInitial value true Being set to unit matrix, corresponding initial variance matrix is zero.

Based on model above, Sequential filter method (such as EKF, particle filter etc.) can be used to estimate shape The state of state space model.In order to meet | | mⁱ| | the state constraint of=1, need the result to Sequential filter to carry out following yardstick Adjust: S_rs=| | mⁱ| | S, mⁱ _rs=mⁱ/||mⁱ| |, wherein: S_rsRepresent the magnetometer matrix after rescaling, mⁱ _rsRepresent yardstick Magnetic field vector after adjustment, | | | | represent modulo operation.

Specific implementation process is briefly illustrated below as a example by EKF:

Definition status error delta x is estimated valueDeduct true value x, i.e.And Attitude estimation valueWith attitude true valueAnd the contextual definition of attitude error ψ isThen corresponding state error vector representation is δ x=[ψ^T δεT vec^T(δS) δh^T δm^iT]^T, upper T represents that matrix is risen by the transposition computing of vector, vec () expression according to the sequential concatenation of row The computing come；ψ^TRepresent the transposition of attitude error ψ, ε^TRepresent the transposition of gyro zero deflection amount ε, h^T、m^iTRepresent h, m respectivelyⁱDress Put；

The linear approximation state-space model of state error is given as follows

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ \mathrm{\δ}{y}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

Wherein noiseMatrix

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

In formula: matrix F, G, H_mRepresent sytem matrix, system input matrix and observing matrix respectively；I₃Represent three rank units Matrix；SymbolRepresenting matrix and the Kronecker product of matrix；

The result of EKF is carried out following rescaling: S_rs=| | mⁱ| | S, mⁱ _rs=mⁱ/||mⁱ||。

Above the specific embodiment of the present invention is described.It is to be appreciated that the invention is not limited in above-mentioned Particular implementation, those skilled in the art can make a variety of changes within the scope of the claims or revise, this not shadow Ring the flesh and blood of the present invention.In the case of not conflicting, the feature in embodiments herein and embodiment can any phase Combination mutually.

Claims (4)

1. a gyro based on Sequential filter and Magnetic Sensor combined calibrating method, it is characterised in that comprise the steps:

Step 1: set up the state-space model of combined calibrating problem；

Step 2: use Sequential filter method to estimate the state of state-space model.

Gyro based on Sequential filter the most according to claim 1 and Magnetic Sensor combined calibrating method, it is characterised in that In described step 1, the state-space model of combined calibrating problem is as follows:

System equation:

${\stackrel{\·}{C}}_b^i=C_b^i({\mathrm{\ω}}_{ib}^b-\mathrm{\ϵ}-n_g)\×$

$\stackrel{\·}{\mathrm{\ϵ}}=n_{\mathrm{\ϵ}}$

$\stackrel{\·}{S}=0$

$\stackrel{\·}{h}=0$

${\stackrel{\·}{m}}^i=n_{mi}$

Wherein: the body coordinate system of gyro/accelerometer is expressed as b system, and inertial coodinate system is expressed as the b of i system, i.e. initial time System；

In formula:Represent the inertial coodinate system attitude matrix relative to body coordinate system,RepresentTime-derivative,Represent Angular velocity vector under the b system of gyro to measure, ε represents gyro zero partially,Represent the time-derivative of ε, n_gRepresent the height of gyro to measure This noise, n_εRepresenting the Gaussian noise that gyro zero is inclined, matrix S represents magnetometer parameter matrix,The time-derivative of representing matrix S, Vector h represents magnetometer zero partially,Represent the time-derivative of h, mⁱIt is vectorial for the lower stationary magnetic field represented of i system,Represent mⁱTime Between derivative, n_miRepresent the magnetic field vector error caused because of earth rotation；Wherein: computing a × expression is by three-dimensional vector a= [a₁ a₂ a₃]^TConstitute multiplication cross matrix, specifically, a × expansion as follows:

$a\×=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right];$

Observational equation:

$y_m={\mathrm{SC}}_i^b{m}^i+h+n_m$

In formula: y_mRepresent the measured value of magnetometer,For attitude matrixTransposition, i.e.Subscript T representing matrix or The transposition computing of vector, n_mRepresent the Gaussian noise of magnetometer measured value.

Gyro based on Sequential filter the most according to claim 1 and Magnetic Sensor combined calibrating method, it is characterised in that The state of the state-space model in step 1 include attitude, magnetometer inner parameter, magnetometer external parameter, gyro zero partially and Magnetic field vector；Magnetic field vector mⁱInitial value include: the measured value first of magnetometer；The initial value bag of magnetometer parameter matrix S Include: unit matrix or give in advance；The initial value of the inclined ε of gyro zero and the inclined h of magnetometer zero elects zero as；Pass because of state ornamental System, attitudeInitial value be defined as unit matrix, corresponding initial variance matrix is zero.

Gyro based on Sequential filter the most according to claim 3 and Magnetic Sensor combined calibrating method, it is characterised in that Described step 2 includes: using Sequential filter method to estimate the state of state-space model, described Sequential filter method includes: EKF, or particle filter method；Specifically, when using EKF, comprise the steps:

Step 2.1: definition status error delta x is estimated valueDeduct true value x, i.e.δ represents the error of corresponding states, And Attitude estimation valueWith attitude true valueAnd the contextual definition of attitude error ψ isThen corresponding state Error vector is expressed as δ x ≡ [ψ^T δε^T vec^T(δS) δh^T δm^iT]^T, vec () represents matrix according to the sequential concatenation arranged The computing got up；ψ^TRepresent the transposition of attitude error ψ, ε^TRepresent the transposition of gyro zero deflection amount ε, h^T、m^iTRepresent h, m respectivelyⁱ's Transposition；

The approximately linear state-space model of state error is given as follows

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{x}=F\mathrm{\δ}x+Gw\\ {\mathrm{\δy}}_m=H_m\mathrm{\δ}x+n_m\end{array}\right.$

Wherein: noise

$F=\left[\begin{array}{ccc}{0}_{3\×3}& -C_b^i& 0_{3\×15}\\ 0_{18\×3}& 0_{18\×3}& 0_{18\×15}\end{array}\right],G=\left[\begin{array}{ccc}-C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_3& 0_{3\×3}\\ 0_{12\×3}& 0_{12\×3}& 0_{12\×3}\\ 0_{3\×3}& 0_{3\×3}& I_3\end{array}\right],$

$H_m=\[\begin{array}{ccccc}-{\mathrm{SC}}_i^b\left(m^i\right)\×& 0_{3\×3}& {\left(C_i^b{m}^i\right)}^T\⊗I_3& I_3& {\mathrm{SC}}_i^b\end{array}\]$

In formula:Represent the time-derivative of δ x, matrix F, G, H_mRepresent sytem matrix, system input matrix and observation square respectively Battle array；I representation unit matrix, the subscript representation unit order of matrix number of I；0 represents null matrix, and the subscript of 0 represents the row of null matrix Columns；SymbolRepresenting matrix and the Kronecker product of matrix；

Step 2.2: assuming that when the size of earth-magnetic field vector is 1, the result of EKF is carried out following rescaling； Adjustment formula is as follows:

S_rs=| | mⁱ| | S,

mⁱ _rs=mⁱ/||mⁱ||；

In formula: S_rsRepresent the magnetometer matrix after rescaling, mⁱ _rsRepresent the magnetic field vector after rescaling, | | | | represent Modulo operation.