CN104655132A - Method for estimating body elastic deformation angle on basis of accelerometer - Google Patents

Abstract

The invention relates to a method for estimating a body elastic deformation angle on the basis of an accelerometer, which is used for measuring a body elastic deformation angle of a to-be-detected point (sub-node) on an airplane relative to a known point (main node). The method comprises the steps of respectively orthogonally installing three gyroscopes and three accelerometers on the known point, and orthogonally installing three accelerometers on the to-be-detected point; then establishing a system state equation including an installation error angle, the elastic deformation angle, an accelerometer normal value and a random bias, adopting the difference between the accelerometer measurement values of the main node and the sub-node as measurement, and establishing a nonlinear system measurement equation of the system; and finally estimating the body elastic deformation angle at a sub-node at each sampling moment by adopting a nonlinear filter method, namely an Unscented Kalman filtering estimation method.

Description

A kind of body elastic deformation angular estimation method based on accelerometer

Technical field

The present invention relates to a kind of body elastic deformation angular estimation method based on accelerometer, also can be used for the measurement at the elastic deformation such as naval vessel, vehicle angle.

Background technology

Elastic deformation angular measurement is one of gordian technique of carrier-borne, Airborne Inertial Network Capture each point high-precision motion parameter.In inertance network, usually comprise a host node and multiple child node.The existence of elastic deformation makes the local pose information of each Nodes on carrier and the attitude information of host node have larger difference.If do not measured elastic deformation and compensating, this species diversity will have a strong impact on the precision of each child node place kinematic parameter.

The method of current measurement deflection deformation mainly contains the optical measuring method based on optical sensor and the inertia measurement method based on Inertial Measurement Unit.Wherein optical measuring method requires light beam transmitting-receiving place necessary " intervisibility ", settles more complicated, and existence is subject to weather effect, can not realizes the deficiency of all weather operations etc.Based on the inertia measurement method of Inertial Measurement Unit, require (to be called Inertial Measurement Unit at host node and all orthogonal installation in multiple even each child node places three gyroscopes and three accelerometers, Inertial MeasurementUnit, IMU), the volume of deformation measuring system, weight and cost is considerably increased.Wherein, for the deformation measurement method installing Inertial Measurement Unit at host node and parton Nodes, the different layouts of its Inertial Measurement Unit are very large on the impact of deformation measurement precision.And certain applications are proposed very harsh requirement to the volume of deformation measuring system, weight and cost.Such as, in the airborne earth observation application of multitask remote sensing load, typical application load is distributed SAR system (the Synthetic Aperture Radar based on array technique, SAR), each antenna is distributed in wing both sides, and the SAR antenna on one-sided wing just reaches tens.For obtaining the kinematic parameter at all antenna places and then carrying out imaging moving compensation, just need the elastic deformation of measuring wing.And the space at SAR antenna place and bearing capacity very limited, be therefore difficult to application based on the deformation measurement method of Inertial Measurement Unit.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art, proposes a kind of body elastic deformation angular estimation method based on accelerometer.

Technical solution of the present invention is: a kind of body elastic deformation angular estimation method based on accelerometer.Its concrete steps are as follows:

(1) the orthogonal installation of tested point three accelerometers aboard, orthogonal installation three gyroscopes and three accelerometers respectively at known point place, tested point and known point are designated as child node and host node respectively;

(2) system state equation comprising fix error angle, elastic deformation angle, accelerometer constant value and random bias is set up;

(3) using the difference of the acceleration measuring value of main and sub node as measurement, set up the nonlinear system measurement equation of system;

(4) Unscented Kalman Filter Estimation is adopted to go out t _kthe body elastic deformation angle at moment child node place, k=1,2 ..., N, constantly repeats this step, until main and sub node acceleration counts end.

In described step (2), system state equation comprises the mathematical model of child node fixed installation error angle, body elastic deformation angle and main and sub node acceleration meter constant value and random bias, and concrete establishment step is:

1) child node fixed installation error angle mathematical model is set up

The definition of coherent reference coordinate system comprises: note i is geocentric inertial coordinate system; Carrier coordinate system initial point is carrier center of gravity, and along carrier transverse axis to the right, y-axis is before carrier Y for x-axis, z-axis along carrier vertical pivot upwards, this coordinate system is fixed on carrier, is commonly referred to right front upper carrier coordinate system, represents the carrier coordinate system of host node and child node with a and b respectively;

Child node fixed installation error angle mathematical model is:

$\stackrel{\·}{\mathrm{\ρ}}=0$

Wherein ρ=[ρ _xρ _yρ _z] ^tfor the fixed installation error angle of the relative host node of child node, ρ _x, ρ _yand ρ _zbe respectively the fix error angle of child node carrier system x-axis, y-axis and z-axis;

2) body elastic deformation angle, child node place mathematical model is set up

The differential equation of child node place body elastic deformation angle θ:

${\stackrel{\·\·}{\mathrm{\θ}}}_j+2{\mathrm{\β}}_j{\stackrel{\·}{\mathrm{\θ}}}_j+{{\mathrm{\β}}_j}^2{\mathrm{\θ}}_j={\mathrm{\η}}_j,j=x,y,z$

Wherein θ=[θ _xθ _yθ _z] ^t, θ _jfor the elastic deformation angle on child node carrier system jth axle, β _j=2.146/ τ _j, τ _jfor second order Markov process correlation time; η _jfor zero-mean white noise, its variance meet:

$Q_{{\mathrm{\η}}_j}=4{{\mathrm{\β}}_j}^3{{\mathrm{\σ}}_j}^2$

Wherein σ _j ²for elastic deformation angle θ _jvariance, β _jwith for describing the parameter of the second order Markov process of elastic deformation angle θ;

3) main and sub node acceleration meter constant value and random bias mathematical model is set up

The mathematical model that main and sub node acceleration meter constant value is biased meets the following differential equation:

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{D}}}_{\mathrm{am}}=0\\ {\stackrel{\·}{\stackrel{\‾}{D}}}_{\mathrm{bm}}=0\end{array}\right.,m=x,y,z$

Wherein ${\stackrel{\‾}{D}}_a={\left[\begin{array}{ccc}{\stackrel{\‾}{D}}_{\mathrm{ax}}& {\stackrel{\‾}{D}}_{\mathrm{ay}}& {\stackrel{\‾}{D}}_{\mathrm{az}}\end{array}\right]}^T$ For host node accelerometer bias, for component on host node carrier system m axle; ${\stackrel{\‾}{D}}_b={\left[\begin{array}{ccc}{\stackrel{\‾}{D}}_{\mathrm{bx}}& {\stackrel{\‾}{D}}_{\mathrm{by}}& {\stackrel{\‾}{D}}_{\mathrm{bz}}\end{array}\right]}^T$ For child node accelerometer bias, for component on child node carrier system m axle;

Main and sub node acceleration meter random bias is represented by one order Markovian process:

$\left\{\begin{array}{c}{\stackrel{\·}{D}}_{\mathrm{ai}}^{\′}+{\mathrm{\μ}}_{\mathrm{ai}}{D}_{\mathrm{ai}}^{\′}={\mathrm{\γ}}_{\mathrm{ai}}\\ {\stackrel{\·}{D}}_{\mathrm{bi}}^{\′}+{\mathrm{\μ}}_{\mathrm{bi}}{D}_{\mathrm{bi}}^{\′}={\mathrm{\γ}}_{\mathrm{bi}}\end{array}\right.,i=x,y,z$

Wherein D ' _a=[D ' _axd ' _ayd ' _az] ^tfor host node accelerometer random bias, D ' _aifor D ' _acomponent on host node carrier system i axle; Wherein D ' _b=[D ' _bxd ' _byd ' _bz] ^tfor child node accelerometer random bias, D ' _bifor D ' _bcomponent on child node carrier system i axle; μ _aiand μ _bifor one order Markovian process parameter, γ _aiand γ _bifor white noise;

4) system state equation is set up

System state equation is:

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

Wherein state variable X=[X ₁x ₂] ^t, X ₁be deformation angle variable between 9 dimension main and sub node, X ₂be 12 dimension accelerometer error variablees; System noise W=[η _xη _yη _zγ _axγ _ayγ _azγ _bxγ _byγ _bz] ^t, state-transition matrix F and noise transition matrix G can fixedly mount error angle, body elastic deformation angle and main and sub node acceleration meter constant value by the child node of above-mentioned foundation and random bias mathematical model is determined; X ₁and X ₂expression formula be:

$X_1={\left[\begin{array}{ccccccccc}{\mathrm{\ρ}}_x& {\mathrm{\ρ}}_y& {\mathrm{\ρ}}_z& {\mathrm{\θ}}_x& {\mathrm{\θ}}_y& {\mathrm{\θ}}_z& {\stackrel{\·}{\mathrm{\θ}}}_x& {\stackrel{\·}{\mathrm{\θ}}}_y& {\stackrel{\·}{\mathrm{\θ}}}_z\end{array}\right]}^T$

$X_2={\left[\begin{array}{cccccccccccc}{\stackrel{\‾}{D}}_{\mathrm{ax}}& {\stackrel{\‾}{D}}_{\mathrm{ay}}& {\stackrel{\‾}{D}}_{\mathrm{az}}& {\stackrel{\‾}{D}}_{\mathrm{bx}}& {\stackrel{\‾}{D}}_{\mathrm{by}}& {\stackrel{\‾}{D}}_{\mathrm{bz}}& D_{\mathrm{ax}}^{\′}& D_{\mathrm{ay}}^{\′}& D_{\mathrm{az}}^{\′}& D_{\mathrm{bx}}^{\′}& D_{\mathrm{by}}^{\′}& D_{\mathrm{bz}}^{\′}\end{array}\right]}^T$

State-transition matrix F and noise transition matrix G is respectively:

$F=\left[\begin{array}{ccccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& A_1& A_2& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& B_1& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\≥3}& 0_{3\×3}& 0_{3\×3}& B_2\end{array}\right]$

$A_1=\left[\begin{array}{ccc}{-\mathrm{\β}}_x^2& 0& 0\\ 0& {-\mathrm{\β}}_y^2& 0\\ 0& 0& {-\mathrm{\β}}_z^2\end{array}\right],$ $A_2=\left[\begin{array}{ccc}{-2\mathrm{\β}}_x& 0& 0\\ 0& {-2\mathrm{\β}}_y& 0\\ 0& 0& {-2\mathrm{\β}}_z\end{array}\right]$

$B_1=\left[\begin{array}{ccc}{\mathrm{\μ}}_{\mathrm{ax}}& 0& 0\\ 0& {\mathrm{\μ}}_{\mathrm{ay}}& 0\\ 0& 0& {\mathrm{\μ}}_{\mathrm{az}}\end{array}\right],$ $B_2=\left[\begin{array}{ccc}{\mathrm{\μ}}_{\mathrm{bx}}& 0& 0\\ 0& {\mathrm{\μ}}_{\mathrm{by}}& 0\\ 0& 0& {\mathrm{\μ}}_{\mathrm{bz}}\end{array}\right]$

$G=\left[\begin{array}{ccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& D_1& 0_{3\×3}\end{array}\right]$

$D_1=\left[\begin{array}{ccc}{\mathrm{\γ}}_{\mathrm{ax}}& 0& 0\\ 0& {\mathrm{\γ}}_{\mathrm{ay}}& 0\\ 0& 0& {\mathrm{\γ}}_{\mathrm{az}}\end{array}\right],$ $D_2=\left[\begin{array}{ccc}{\mathrm{\γ}}_{\mathrm{bx}}& 0& 0\\ 0& {\mathrm{\γ}}_{\mathrm{by}}& 0\\ 0& 0& {\mathrm{\γ}}_{\mathrm{bz}}\end{array}\right]$

Nonlinear system measurement equation in described step (3), concrete establishment step is:

The accelerometer output valve f of host node _awith child node accelerometer output valve f _brelation can be expressed as:

$f_a+D_a+a_r^a=C_b^a(f_b+D_b)$

Wherein f _a=[f _axf _ayf _az] ^t, f _ax, f _ay, and f _azbe respectively the output valve of host node x-axis, y-axis and z-axis accelerometer; f _b=[f _bxf _byf _bz] ^t, f _bx, f _by, and f _bzbe respectively the output valve of child node x-axis, y-axis and z-axis accelerometer; for being biased of host node accelerometer, comprising constant value and be biased with random bias D ' _atwo parts; for being biased of child node accelerometer, comprising constant value and be biased with random bias D ' _btwo parts; for the projection of main and sub internodal lever arm acceleration under host node carrier system; for child node carrier is tied to the pose transformation matrix of host node carrier system;

Arrangement can obtain

$\mathrm{\Δf}=f_a-f_b=\hat{f}(\mathrm{\ρ}+\mathrm{\θ})+C_b^a{D}_b-D_a-a_r^a$

Wherein $\hat{f}=\left[\begin{array}{ccc}0& f_{\mathrm{bz}}& {-f}_{\mathrm{by}}\\ {-f}_{\mathrm{bz}}& 0& f_{\mathrm{bx}}\\ f_{\mathrm{by}}& {-f}_{\mathrm{bx}}& 0\end{array}\right],$ Lever arm acceleration computing formula be:

$\left\{\begin{array}{c}{a}_r^a=a_1+a_2+a_3+a_4+a_5\\ a_1={2\mathrm{\ω}}_{\mathrm{ia}}^a\×r_0\×\stackrel{\·}{\mathrm{\θ}}+{2\mathrm{\ω}}_{\mathrm{ia}}^a\×(\stackrel{\·}{\mathrm{\θ}}\×r_0)-{\stackrel{\·}{4}}_{\mathrm{\β}}\×r_0\\ a_2=3\stackrel{\·}{\mathrm{\θ}}\×(\stackrel{\·}{\mathrm{\θ}}\×r_0)\\ a_3={-2\mathrm{\θ}}_{{\mathrm{\β}}^2}\×r_0\\ a_4={\mathrm{\ω}}_{\mathrm{ia}}^a\×({\mathrm{\ω}}_{\mathrm{ia}}^a\×r_0)+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ia}}^a\×r_0\\ a_5=2\mathrm{\η}\×r_0\end{array}\right.$

Wherein ${\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{\β}}={\left[\begin{array}{ccc}{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x& {\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y& {\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z\end{array}\right]}^T,$ ${\mathrm{\θ}}_{{\mathrm{\β}}^2}={\left[\begin{array}{ccc}{{\mathrm{\β}}_x}^2{\mathrm{\θ}}_x& {{\mathrm{\β}}_y}^2{\mathrm{\θ}}_y& {{\mathrm{\β}}_z}^2{\mathrm{\θ}}_z\end{array}\right]}^T;$ R ₀for child node is relative to the projection of initial lever arm under host node carrier system of host node; for host node gyroscope output valve, its implication is the projection of rotational angular velocity under host node carrier system of the relative geocentric inertial coordinate system of host node carrier system; form by five, a ₁, a ₂be respectively elastic deformation angular velocity once item and quadratic term, a ₃for the once item of elastic deformation angle θ, a ₄for exporting relevant input item with host node gyroscope, a ₅for with elastic deformation angle second order Markov process noise η=[η _xη _yη _z] ^trelevant noise item;

Nonlinear system measurement equation is designated as:

Z(t)＝h(X,t)+U(t)+V(t)

Wherein measurement amount Z=Δ f=f _a-f _b, input item U is by a ₄determine, system measurements noise V is by a ₅determine, nonlinear function h is by a ₁, a ₂and a ₃determine.

The present invention's advantage is compared with prior art:

Only orthogonal installation three gyroscopes and three accelerometers at host node place, and other child node places only orthogonal installation three accelerometers, and the body elastic deformation angular estimation formula based on accelerometer of having derived.This estimation formulas has than the existing form more succinct based on the deformation measurement method of IMU, Project Realization of being more convenient for.In addition, compare gyroscope due to high-precision accelerometer and there is the significant advantage that quality is light, cost is low, be convenient to installation, therefore instant invention overcomes the deficiency that cost is high, volume is large, quality is heavy, deformation measurement precision is subject to influence of arrangement of the existing deformation measurement method based on IMU, there is more wide application prospect.

Accompanying drawing explanation

Fig. 1 is the system scheme of installation that prior art and the present invention adopt;

Fig. 2 is process flow diagram of the present invention.

Embodiment

As shown in Figure 2, concrete grammar of the present invention is implemented as follows:

1, the orthogonal installation of tested point three accelerometers aboard, orthogonal installation three gyroscopes and three accelerometers respectively at known point place, tested point and known point are designated as child node and host node respectively;

2, the system state equation comprising fix error angle, elastic deformation angle, accelerometer constant value and random bias is set up

(1) child node fixed installation error angle mathematical model is set up

The definition of coherent reference coordinate system comprises: note i is geocentric inertial coordinate system; Carrier coordinate system initial point is carrier center of gravity, and along carrier transverse axis to the right, y-axis is before carrier Y for x-axis, z-axis along carrier vertical pivot upwards, this coordinate system is fixed on carrier, is commonly referred to right front upper carrier coordinate system, represents the carrier coordinate system of host node and child node with a and b respectively.

Child node fixed installation error angle mathematical model is:

$\stackrel{\·}{\mathrm{\ρ}}=0---\left(1\right)$

Wherein ρ=[ρ _xρ _yρ _z] ^tfor the fixed installation error angle of the relative host node of child node, ρ _x, ρ _yand ρ _zbe respectively the fix error angle of child node carrier system x-axis, y-axis and z-axis.

(2) body elastic deformation angle, child node place mathematical model is set up

The differential equation of child node place body elastic deformation angle θ:

${\stackrel{\·\·}{\mathrm{\θ}}}_j+2{\mathrm{\β}}_j{\stackrel{\·}{\mathrm{\θ}}}_j+{{\mathrm{\β}}_j}^2{\mathrm{\θ}}_j={\mathrm{\η}}_j\left(j=x,y,z\right)---\left(2\right)$

Wherein θ=[θ _xθ _yθ _z] ^t, θ _jfor the elastic deformation angle on child node carrier system jth axle, β _j=2.146/ τ _j, τ _jfor second order Markov process correlation time; η _jfor zero-mean white noise, its variance meet:

$Q_{{\mathrm{\η}}_j}=4{{\mathrm{\β}}_j}^3{{\mathrm{\σ}}_j}^2---\left(3\right)$ Wherein σ _j ²for elastic deformation angle θ _jvariance, β _jwith for describing the parameter of the second order Markov process of elastic deformation angle θ.

(3) mathematical model setting up main and sub node acceleration meter constant value and random bias mathematical model main and sub node acceleration meter constant value biased meets the following differential equation:

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{D}}}_{\mathrm{am}}=0\\ {\stackrel{\·}{\stackrel{\‾}{D}}}_{\mathrm{bm}}=0\end{array}\right.,m=x,y,z---\left(4\right)$

Wherein ${\stackrel{\‾}{D}}_a={\left[\begin{array}{ccc}{\stackrel{\‾}{D}}_{\mathrm{ax}}& {\stackrel{\‾}{D}}_{\mathrm{ay}}& {\stackrel{\‾}{D}}_{\mathrm{az}}\end{array}\right]}^T$ For host node accelerometer bias, for component on host node carrier system m axle; ${\stackrel{\‾}{D}}_b={\left[\begin{array}{ccc}{\stackrel{\‾}{D}}_{\mathrm{bx}}& {\stackrel{\‾}{D}}_{\mathrm{by}}& {\stackrel{\‾}{D}}_{\mathrm{bz}}\end{array}\right]}^T$ For child node accelerometer bias, for component on child node carrier system m axle.

Main and sub node acceleration meter random bias is represented by one order Markovian process, namely

$\left\{\begin{array}{c}{\stackrel{\·}{D}}_{\mathrm{ai}}^{\′}+{\mathrm{\μ}}_{\mathrm{ai}}{D}_{\mathrm{ai}}^{\′}={\mathrm{\γ}}_{\mathrm{ai}}\\ {\stackrel{\·}{D}}_{\mathrm{bi}}^{\′}+{\mathrm{\μ}}_{\mathrm{bi}}{D}_{\mathrm{bi}}^{\′}={\mathrm{\γ}}_{\mathrm{bi}}\end{array}\right.,i=x,y,z---\left(5\right)$

Wherein D ' _a=[D ' _axd ' _ayd ' _az] ^tfor host node accelerometer random bias, D ' _aifor D ' _acomponent on host node carrier system i axle; Wherein D ' _b=[D ' _bxd ' _byd ' _bz] ^tfor child node accelerometer random bias, D ' _bifor D ' _bcomponent on child node carrier system i axle; μ _aiand μ _bifor one order Markovian process parameter, γ _aiand γ _bifor white noise.

(4) system state equation is set up

System state equation is:

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

Wherein state variable X=[X ₁x ₂] ^t, X ₁be deformation angle variable between 9 dimension main and sub node, X ₂be 12 dimension accelerometer error variablees; System noise W=[η _xη _yη _zγ _axγ _ayγ _azγ _bxγ _byγ _bz] ^t, state-transition matrix F and noise transition matrix G can fixedly mount error angle, body elastic deformation angle and main and sub node acceleration meter constant value by the child node of above-mentioned foundation and random bias mathematical model is determined; X ₁and X ₂expression formula be:

$\begin{array}{c}{X}_1={\left[\begin{array}{ccccccccc}{\mathrm{\ρ}}_x& {\mathrm{\ρ}}_y& {\mathrm{\ρ}}_z& {\mathrm{\θ}}_x& {\mathrm{\θ}}_y& {\mathrm{\θ}}_z& {\stackrel{\·}{\mathrm{\θ}}}_z& {\stackrel{\·}{\mathrm{\θ}}}_y& {\stackrel{\·}{\mathrm{\θ}}}_z\end{array}\right]}^T\\ X_2={\left[\begin{array}{cccccccccccc}{\stackrel{\‾}{D}}_{\mathrm{ax}}& {\stackrel{\‾}{D}}_{\mathrm{ay}}& {\stackrel{\‾}{D}}_{\mathrm{az}}& {\stackrel{\‾}{D}}_{\mathrm{bx}}& {\stackrel{\‾}{D}}_{\mathrm{by}}& {\stackrel{\‾}{D}}_{\mathrm{bz}}& D_{\mathrm{ax}}^{\′}& D_{\mathrm{ay}}^{\′}& D_{\mathrm{az}}^{\′}& D_{\mathrm{bx}}^{\′}& D_{\mathrm{by}}^{\′}& D_{\mathrm{bz}}^{\′}\end{array}\right]}^T\end{array}---\left(7\right)$

State-transition matrix F and noise transition matrix G is respectively:

$F=\left[\begin{array}{ccccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& A_1& A_2& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& B_1& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\≥3}& 0_{3\×3}& 0_{3\×3}& B_2\end{array}\right]---\left(8\right)$

$A_1=\left[\begin{array}{ccc}{-\mathrm{\β}}_x^2& 0& 0\\ 0& {-\mathrm{\β}}_y^2& 0\\ 0& 0& {-\mathrm{\β}}_z^2\end{array}\right]---\left(9\right)$

$A_2=\left[\begin{array}{ccc}{-2\mathrm{\β}}_x& 0& 0\\ 0& {-2\mathrm{\β}}_y& 0\\ 0& 0& {-2\mathrm{\β}}_z\end{array}\right]---\left(10\right)$

$B_1=\left[\begin{array}{ccc}{\mathrm{\μ}}_{\mathrm{ax}}& 0& 0\\ 0& {\mathrm{\μ}}_{\mathrm{ay}}& 0\\ 0& 0& {\mathrm{\μ}}_{\mathrm{az}}\end{array}\right]---\left(11\right)$

$B_2=\left[\begin{array}{ccc}{\mathrm{\μ}}_{\mathrm{bx}}& 0& 0\\ 0& {\mathrm{\μ}}_{\mathrm{by}}& 0\\ 0& 0& {\mathrm{\μ}}_{\mathrm{bz}}\end{array}\right]---\left(12\right)$

$G=\left[\begin{array}{ccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& D_1& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& D_2\end{array}\right]---\left(13\right)$

$D_1=\left[\begin{array}{ccc}{\mathrm{\γ}}_{\mathrm{ax}}& 0& 0\\ 0& {\mathrm{\γ}}_{\mathrm{ay}}& 0\\ 0& 0& {\mathrm{\γ}}_{\mathrm{az}}\end{array}\right]---\left(14\right)$

$D_2=\left[\begin{array}{ccc}{\mathrm{\γ}}_{\mathrm{bx}}& 0& 0\\ 0& {\mathrm{\γ}}_{\mathrm{by}}& 0\\ 0& 0& {\mathrm{\γ}}_{\mathrm{bz}}\end{array}\right]---\left(15\right)$

2, the nonlinear system measurement equation of system is set up

The accelerometer output valve f of host node _awith child node accelerometer output valve f _brelation can be expressed as:

$f_a+D_a+a_r^a=C_b^a(f_b+D_b)---\left(16\right)$

Wherein f _a=[f _axf _ayf _az] ^t, f _ax, f _ay, and f _azbe respectively the output valve of host node x-axis, y-axis and z-axis accelerometer; f _b=[f _bxf _byf _bz] ^t, f _bx, f _by, and f _bzbe respectively the output valve of child node x-axis, y-axis and z-axis accelerometer; for being biased of host node accelerometer, comprising constant value and be biased with random bias D ' _atwo parts; for being biased of child node accelerometer, comprising constant value and be biased with random bias D ' _btwo parts; for the projection of main and sub internodal lever arm acceleration under host node carrier system; for child node carrier is tied to the pose transformation matrix of host node carrier system.

Arrangement can obtain

$\mathrm{\Δf}=f_a-f_b=\hat{f}(\mathrm{\ρ}+\mathrm{\θ})+C_b^a{D}_b-D_a-a_r^a---\left(17\right)$

Wherein $\hat{f}=\left[\begin{array}{ccc}0& f_{\mathrm{bz}}& {-f}_{\mathrm{by}}\\ {-f}_{\mathrm{bz}}& 0& f_{\mathrm{bx}}\\ f_{\mathrm{by}}& {-f}_{\mathrm{bx}}& 0\end{array}\right],$ Lever arm acceleration computing formula be:

$\left\{\begin{array}{c}{a}_r^a=a_1+a_2+a_3+a_4+a_5\\ a_1={2\mathrm{\ω}}_{\mathrm{ia}}^a\×r_0\×\stackrel{\·}{\mathrm{\θ}}+{2\mathrm{\ω}}_{\mathrm{ia}}^a\×(\stackrel{\·}{\mathrm{\θ}}\×r_0)-{\stackrel{\·}{4}}_{\mathrm{\β}}\×r_0\\ a_2=3\stackrel{\·}{\mathrm{\θ}}\×(\stackrel{\·}{\mathrm{\θ}}\×r_0)\\ a_3={-2\mathrm{\θ}}_{{\mathrm{\β}}^2}\×r_0\\ a_4={\mathrm{\ω}}_{\mathrm{ia}}^a\×({\mathrm{\ω}}_{\mathrm{ia}}^a\×r_0)+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ia}}^a\×r_0\\ a_5=2\mathrm{\η}\×r_0\end{array}\right.---\left(18\right)$

Wherein ${\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{\β}}={\left[\begin{array}{ccc}{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x& {\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y& {\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z\end{array}\right]}^T,$ ${\mathrm{\θ}}_{{\mathrm{\β}}^2}={\left[\begin{array}{ccc}{{\mathrm{\β}}_x}^2{\mathrm{\θ}}_x& {{\mathrm{\β}}_y}^2{\mathrm{\θ}}_y& {{\mathrm{\β}}_z}^2{\mathrm{\θ}}_z\end{array}\right]}^T;$ R ₀for child node is relative to the projection of initial lever arm under host node carrier system of host node; for host node gyroscope output valve, its implication is the projection of rotational angular velocity under host node carrier system of the relative geocentric inertial coordinate system of host node carrier system; form by five, a ₁, a ₂be respectively elastic deformation angular velocity once item and quadratic term, a ₃for the once item of elastic deformation angle θ, a ₄for exporting relevant input item with host node gyroscope, a ₅for with elastic deformation angle second order Markov process noise η=[η _xη _yη _z] ^trelevant noise item.

Nonlinear system measurement equation is designated as:

Z (t)=h (X, t)+U (t)+V (t) (19) wherein measurement amount Z=Δ f=f _a-f _b, input item U is by a ₄determine, system measurements noise V is by a ₅determine, nonlinear function h is by a ₁, a ₂and a ₃determine.

3, Unscented Kalman Filter Estimation body elastic deformation angle is adopted

Unscented Kalman Filter Estimation is adopted to go out t _kthe body elastic deformation angle at moment child node place, k=1,2 ..., N, constantly repeats this step, until main and sub node acceleration counts end.

The content be not described in detail in instructions of the present invention belongs to the known prior art of professional and technical personnel in the field.

Claims (3)

1., based on a body elastic deformation angular estimation method for accelerometer, it is characterized in that performing step is as follows:

(1) the orthogonal installation of tested point three accelerometers aboard, orthogonal installation three gyroscopes and three accelerometers respectively at known point place, tested point and known point are designated as child node and host node respectively;

(2) system state equation comprising fix error angle, elastic deformation angle, accelerometer constant value and random bias is set up;

(3) using the difference of the acceleration measuring value of main and sub node as measurement, set up the nonlinear system measurement equation of system;

(4) Unscented Kalman Filter Estimation is adopted to go out t _kthe body elastic deformation angle at moment child node place, k=1,2 ..., N, constantly repeats this step, until main and sub node acceleration counts end.

2. the body elastic deformation angular estimation method based on accelerometer according to claim 1, it is characterized in that: the system state equation in described step (2) comprises the mathematical model of child node fixed installation error angle, body elastic deformation angle and main and sub node acceleration meter constant value and random bias, and concrete establishment step is:

(1) child node fixed installation error angle mathematical model is set up

The definition of coherent reference coordinate system comprises: note i is geocentric inertial coordinate system; Carrier coordinate system initial point is carrier center of gravity, and along carrier transverse axis to the right, y-axis is before carrier Y for x-axis, z-axis along carrier vertical pivot upwards, this coordinate system is fixed on carrier, is commonly referred to right front upper carrier coordinate system, represents the carrier coordinate system of host node and child node with a and b respectively.

Child node fixed installation error angle mathematical model is:

$\stackrel{\·}{\mathrm{\ρ}}=0$

Wherein ρ=[ρ _xρ _yρ _z] ^tfor the fixed installation error angle of the relative host node of child node, ρ _x, ρ _yand ρ _zbe respectively the fix error angle of child node carrier system x-axis, y-axis and z-axis;

(2) body elastic deformation angle, child node place mathematical model is set up

The differential equation of child node place body elastic deformation angle θ:

${\stackrel{\·\·}{\mathrm{\θ}}}_j+2{\mathrm{\β}}_j{\stackrel{\·}{\mathrm{\θ}}}_j+{{\mathrm{\β}}_j}^2{\mathrm{\θ}}_j={\mathrm{\η}}_j,j=x,y,z$

Wherein θ=[θ _xθ _yθ _z] ^t, θ _jfor the elastic deformation angle on child node carrier system jth axle, β _j=2.146/ τ _j, τ _jfor second order Markov process correlation time; η _jfor zero-mean white noise, its variance meet:

$Q_{{\mathrm{\η}}_j}=4{{\mathrm{\β}}_j}^3{{\mathrm{\σ}}_j}^2$

Wherein σ _j ²for elastic deformation angle θ _jvariance, β _jwith for describing the parameter of the second order Markov process of elastic deformation angle θ;

(3) main and sub node acceleration meter constant value and random bias mathematical model is set up

The mathematical model that main and sub node acceleration meter constant value is biased meets the following differential equation:

$\left\{\begin{array}{c}{\stackrel{\·}{\stackrel{\‾}{D}}}_{\mathrm{am}}=0\\ {\stackrel{\·}{\stackrel{\‾}{D}}}_{\mathrm{bm}}=0\end{array}\right.,m=x,y,z$

Wherein ${\stackrel{\‾}{D}}_a={\left[\begin{array}{ccc}{\stackrel{\‾}{D}}_{\mathrm{ax}}& {\stackrel{\‾}{D}}_{\mathrm{ay}}& {\stackrel{\‾}{D}}_{\mathrm{az}}\end{array}\right]}^T$ For host node accelerometer bias, for component on host node carrier system m axle; ${\stackrel{\‾}{D}}_b={\left[\begin{array}{ccc}{\stackrel{\‾}{D}}_{\mathrm{bx}}& {\stackrel{\‾}{D}}_{\mathrm{by}}& {\stackrel{\‾}{D}}_{\mathrm{bz}}\end{array}\right]}^T$ For child node accelerometer bias, for component on child node carrier system m axle;

Main and sub node acceleration meter random bias is represented by one order Markovian process:

$\left\{\begin{array}{c}{\stackrel{\·}{D}}_{\mathrm{ai}}^{\′}+{\mathrm{\μ}}_{\mathrm{ai}}{D}_{\mathrm{ai}}^{\′}={\mathrm{\γ}}_{\mathrm{ai}}\\ {\stackrel{\·}{D}}_{\mathrm{bi}}^{\′}+{\mathrm{\μ}}_{\mathrm{bi}}{D}_{\mathrm{bi}}^{\′}={\mathrm{\γ}}_{\mathrm{bi}}\end{array}\right.,i=x,y,z$

Wherein D ' _a=[D ' _axd ' _ayd ' _az] ^tfor host node accelerometer random bias, D ' _aifor D ' _acomponent on host node carrier system i axle; Wherein D ' _b=[D ' _bxd ' _byd ' _bz] ^tfor child node accelerometer random bias, D ' _bifor D ' _bcomponent on child node carrier system i axle; μ _aiand μ _bifor one order Markovian process parameter, γ _aiand γ _bifor white noise;

(4) system state equation is set up

System state equation is:

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

Wherein state variable X=[X ₁x ₂] ^t, X ₁be deformation angle variable between 9 dimension main and sub node, X ₂be 12 dimension accelerometer error variablees; System noise W=[η _xη _yη _zγ _axγ _ayγ _azγ _bxγ _byγ _bz] ^t, state-transition matrix F and noise transition matrix G can fixedly mount error angle, body elastic deformation angle and main and sub node acceleration meter constant value by the child node of above-mentioned foundation and random bias mathematical model is determined; X ₁and X ₂expression formula be:

$X_1={\left[\begin{array}{ccccccccc}{\mathrm{\ρ}}_x& {\mathrm{\ρ}}_y& {\mathrm{\ρ}}_z& {\mathrm{\θ}}_x& {\mathrm{\θ}}_y& {\mathrm{\θ}}_z& {\stackrel{\·}{\mathrm{\θ}}}_x& {\stackrel{\·}{\mathrm{\θ}}}_y& {\stackrel{\·}{\mathrm{\θ}}}_z\end{array}\right]}^T$

$X_2={\left[\begin{array}{cccccccccccc}{\stackrel{\‾}{D}}_{\mathrm{ax}}& {\stackrel{\‾}{D}}_{\mathrm{ay}}& {\stackrel{\‾}{D}}_{\mathrm{az}}& {\stackrel{\‾}{D}}_{\mathrm{bx}}& {\stackrel{\‾}{D}}_{\mathrm{by}}& {\stackrel{\‾}{D}}_{\mathrm{bz}}& D_{\mathrm{ax}}^{\′}& D_{\mathrm{ay}}^{\′}& D_{\mathrm{az}}^{\′}& D_{\mathrm{bx}}^{\′}& D_{\mathrm{by}}^{\′}& D_{\mathrm{bz}}^{\′}\end{array}\right]}^T$

State-transition matrix F and noise transition matrix G is respectively:

$F=\left[\begin{array}{ccccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& A_1& A_2& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& B_1& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& B_2\end{array}\right]$

$A_1=\left[\begin{array}{ccc}{-\mathrm{\β}}_x^2& 0& 0\\ 0& {-\mathrm{\β}}_y^2& 0\\ 0& 0& {-\mathrm{\β}}_z^2\end{array}\right]{,A}_2=\left[\begin{array}{ccc}{-2\mathrm{\β}}_x& 0& 0\\ 0& {-2\mathrm{\β}}_y& 0\\ 0& 0& {-2\mathrm{\β}}_z\end{array}\right]$

$B_1=\left[\begin{array}{ccc}{\mathrm{\μ}}_{\mathrm{ax}}& 0& 0\\ 0& {\mathrm{\μ}}_{\mathrm{ay}}& 0\\ 0& 0& {\mathrm{\μ}}_{\mathrm{az}}\end{array}\right],B_2=\left[\begin{array}{ccc}{\mathrm{\μ}}_{\mathrm{bx}}& 0& 0\\ 0& {\mathrm{\μ}}_{\mathrm{by}}& 0\\ 0& 0& {\mathrm{\μ}}_{\mathrm{bz}}\end{array}\right]$

$G=\left[\begin{array}{ccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ I_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& D_1& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& D_2\end{array}\right]$

$D_1=\left[\begin{array}{ccc}{\mathrm{\γ}}_{\mathrm{ax}}& 0& 0\\ 0& {\mathrm{\γ}}_{\mathrm{ay}}& 0\\ 0& 0& {\mathrm{\γ}}_{\mathrm{az}}\end{array}\right],D_2=\left[\begin{array}{ccc}{\mathrm{\γ}}_{\mathrm{bx}}& 0& 0\\ 0& {\mathrm{\γ}}_{\mathrm{by}}& 0\\ 0& 0& {\mathrm{\γ}}_{\mathrm{bz}}\end{array}\right]$

3. the body elastic deformation angular estimation method based on accelerometer according to claim 1, it is characterized in that: the nonlinear system measurement equation in described step (3), concrete establishment step is:

The accelerometer output valve f of host node _awith child node accelerometer output valve f _brelation can be expressed as:

$f_a+D_a+a_r^a=C_b^a(f_a+D_b)$

Wherein f _a=[f _axf _ayf _az] ^t, f _ax, f _ay, and f _azbe respectively the output valve of host node x-axis, y-axis and z-axis accelerometer; f _b=[f _bxf _byf _bz] ^t, f _bx, f _by, and f _bzbe respectively the output valve of child node x-axis, y-axis and z-axis accelerometer; for being biased of host node accelerometer, comprising constant value and be biased with random bias D ' _atwo parts; for being biased of child node accelerometer, comprising constant value and be biased with random bias D ' _btwo parts; for the projection of main and sub internodal lever arm acceleration under host node carrier system; for child node carrier is tied to the pose transformation matrix of host node carrier system;

Arrangement can obtain

$\mathrm{\Δf}=f_a-f_b=\hat{f}(\mathrm{\ρ}+\mathrm{\θ})+C_b^a{D}_b-D_a-a_r^a$

Wherein $\hat{f}=\left[\begin{array}{ccc}0& f_{\mathrm{bz}}& {-f}_{\mathrm{by}}\\ {-f}_{\mathrm{bz}}& 0& f_{\mathrm{bx}}\\ f_{\mathrm{by}}& {-f}_{\mathrm{bx}}& 0\end{array}\right],$ Lever arm acceleration computing formula be:

$\left\{\begin{array}{c}{a}_r^a=a_1+a_2+a_3+a_4+a_5\\ a_1={2\mathrm{\ω}}_{\mathrm{ia}}^a\×r_0\×\stackrel{\·}{\mathrm{\θ}}+2{\mathrm{\ω}}_{\mathrm{ia}}^a\×(\stackrel{\·}{\mathrm{\θ}}\×r_0)-4{\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{\β}}\×r_0\\ a_2=3\stackrel{\·}{\mathrm{\θ}}\×(\stackrel{\·}{\mathrm{\θ}}\×r_0)\\ a_3=-2{\mathrm{\θ}}_{{\mathrm{\β}}^2}\×r_0\\ a_4={\mathrm{\ω}}_{\mathrm{ia}}^a\×({\mathrm{\ω}}_{\mathrm{ia}}^a\×r_0)+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ia}}^a\×r_0\\ a_5=2\mathrm{\η}\×r_0\end{array}\right.$

Wherein ${\stackrel{\·}{\mathrm{\θ}}}_{\mathrm{\β}}={\left[\begin{array}{ccc}{\mathrm{\β}}_x{\stackrel{\·}{\mathrm{\θ}}}_x& {\mathrm{\β}}_y{\stackrel{\·}{\mathrm{\θ}}}_y& {\mathrm{\β}}_z{\stackrel{\·}{\mathrm{\θ}}}_z\end{array}\right]}^T,{\mathrm{\θ}}_{{\mathrm{\β}}^2}={\left[\begin{array}{ccc}{{\mathrm{\β}}_x}^2{\mathrm{\θ}}_x& {{\mathrm{\β}}_y}^2{\mathrm{\θ}}_y& {{\mathrm{\β}}_z}^2{\mathrm{\θ}}_z\end{array}\right]}^T;$ R ₀for child node is relative to the projection of initial lever arm under host node carrier system of host node; for host node gyroscope output valve, its implication is the projection of rotational angular velocity under host node carrier system of the relative geocentric inertial coordinate system of host node carrier system; form by five, a ₁, a ₂be respectively elastic deformation angular velocity once item and quadratic term, a ₃for the once item of elastic deformation angle θ, a ₄for exporting relevant input item with host node gyroscope, a ₅for with elastic deformation angle second order Markov process noise η=[η _xη _yη _z] ^trelevant noise item.

Nonlinear system measurement equation is designated as:

Z(t)＝h(X,t)+U(t)+V(t)

Wherein measurement amount Z=Δ f=f _a-f _b, input item U is by a ₄determine, system measurements noise V is by a ₅determine, nonlinear function h is by a ₁, a ₂and a ₃determine.