CN104614555A - Gauss-Newton-based tri-axis accelerometer automatically calibrating method - Google Patents

Abstract

The invention discloses a tri-axis accelerometer automatically calibrating method based on Gauss-Newton optimal iteration. The tri-axis accelerometer automatically calibrating method takes the zero shift errors, the proportional errors and the non-orthogonal errors of a tri-axis accelerometer into consideration and comprises the following steps of, firstly, establishing a calibration model; secondly, simplifying the calibration model; thirdly, statically placing the tri-axis accelerometer in positions in n different directions for n times of measurement, and collecting the output data of the tri-axis accelerometer; fourthly, performing solution through the Newton iteration method; fifthly, through the Jacobian matrix, simplifying the Newton iteration equation, namely, the Gauss-Newton algorithm; sixthly, setting initial calibration parameters and obtaining calibration data and output data calibrated by the tri-axis accelerometer. The tri-axis accelerometer automatically calibrating method based on the Gauss-Newton optimal iteration only needs to collect a plurality of groups of static accelerometer data in different directions, thereby being convenient to operate, saving expensive calibration equipment and control systems and being applicable generalized application.

Description

Based on the three axis accelerometer automatic calibrating method of Gauss-Newton

Technical field

The present invention relates to sensor calibration technique field, be specifically related to a kind of three axis accelerometer automatic calibrating method based on Gauss-Newton of almost cost free, the field with accelerometer of custom requirements can be applied in, such as the field of the Water demand system motions such as somatic sensation television game, virtual augmented reality, robot, unmanned plane.

Background technology

Along with progress and the extensively utilization of micro-electronic mechanical system technique, sensor is little towards weight, and the direction that performance high price is low develops rapidly.This also makes the utilization of sensor expand to field widely.Three axis accelerometer can measure the acceleration of moving object, can analyze the motion of object, obtains position and the direction of object.Accelerometer and gyroscope combine the Inertial Measurement Unit of composition, are the most basic sensors of kinematic system.The accuracy of the output of accelerometer is vital in these kinematic systems.Accelerometer calibration can improve the output accuracy of accelerometer, improves the performance of these kinematic systems.But calibration conventional at present all needs expensive calibration platform and control system.Therefore, a cost is low, and easy to operate calibration steps has important practical significance.

Summary of the invention

For the problems referred to above, the object of the present invention is to provide a kind of three axis accelerometer automatic calibrating method based on Gauss-Newton, it can accurate and effective calibrating accolerometer easily, reduce the impact that accelerometer error brings, have simple to operate, without any need for the advantage of the correcting device of costliness.

For achieving the above object, the technical scheme that the present invention takes is:

A kind of three axis accelerometer automatic calibrating method based on Gauss-Newton, described automatic calibrating method only considers the drift error of three axis accelerometer, proportional error and non-orthogonal errors, and do not consider the error that temperature variation and random drift are brought, it comprises the following steps:

Step 1, set up calibrating patterns:

V＝KNA+B (1)

In formula (1): V is that output valve measured by three axis accelerometer:

V＝[V _xV _yV _z] ^T(2)

K is proportional error matrix:

K＝diag{K _x,K _y,K _z} (3)

N is non-orthogonal errors matrix:

$N=\left(\begin{array}{ccc}{N}_{\mathrm{xx}}& N_{\mathrm{xy}}& N_{\mathrm{xz}}\\ N_{\mathrm{yx}}& N_{\mathrm{yy}}& N_{\mathrm{yz}}\\ N_{\mathrm{zx}}& N_{\mathrm{zy}}& N_{\mathrm{zz}}\end{array}\right)---\left(4\right)$

A is the value that accelerometer ideal exports:

A＝[A _xA _yA _z] ^T(5)

B is drift error matrix:

B＝[B _xB _yB _z] ^T(6)

Step 2, under the impact not considering temperature variation, proportional error matrix K and non-orthogonal errors matrix N are normal matrix, and the two is merged into a matrix, and now, formula (1) can be rewritten into:

A＝M(V-B) (7)

Wherein:

$M={\left(\mathrm{KN}\right)}^{-1}=\left(\begin{array}{ccc}{M}_{\mathrm{xx}}& M_{\mathrm{xy}}& M_{\mathrm{xz}}\\ M_{\mathrm{yx}}& M_{\mathrm{yy}}& M_{\mathrm{yz}}\\ M_{\mathrm{zx}}& M_{\mathrm{zy}}& M_{\mathrm{zz}}\end{array}\right)\≈\left(\begin{array}{ccc}{M}_{\mathrm{xx}}& M_{\mathrm{xy}}& M_{\mathrm{xz}}\\ M_{\mathrm{xy}}& M_{\mathrm{yy}}& M_{\mathrm{yz}}\\ M_{\mathrm{xz}}& M_{\mathrm{yz}}& M_{\mathrm{zz}}\end{array}\right)---\left(8\right)$

Step 3, when three axis accelerometer is static measure is acceleration of gravity constant g=9.8m/s ², so have in the ideal case:

A _x ²+A _y ²+A _z ²＝g ²(9)

In a calibration process, static for the three axis accelerometer n of being placed on (n should be not less than calibration parameter number, and namely n is more than or equal to 9) individual difference towards position carry out n time and measure, gather the output data of three axis accelerometer, the error e that definition kth time is measured _kfor:

e _k＝V _xk ²+V _yk ²+V _zk ²-g ²(10)

In formula (10): 1≤k≤n;

Therefore, the global error function measured for n time is least squares formalism:

$f\left(p\right)=\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{e_k}^2\left(p\right)---\left(11\right)$

In formula (11), p is calibration parameter, p=[p ₁... p ₉]=[M _xxm _xym _xzm _yym _yzm _zzb _xb _yb _z] ^t;

From formula (11), described global error function is the nonlinear equation of calibration parameter, and formula (11) can be write as:

f(p)＝e(p) ^Te(p) (12)

In formula (12), e (p)=[e ₁(p) e ₂(p) ... e _n(p)] ^t;

Step 4, by set up Newton iteration equation adopt Newton iteration method solve formula (12), Newton iteration method has the characteristic of quadratic convergence, only need to provide an initial calibration parameter value can solve formula (12), the Newton iteration equation of formula (12) through type (13) is to solution iteration:

$p^{t+1}=p^t-\mathrm{\α}\frac{\▿f\left(p^t\right)}{{\▿}^{2}f\left(p^t\right)}---\left(13\right)$

In formula (13): p ^tfor the vectorial p of calibration parameter composition is the iterative value of the t time, for the gradient of global error function, the gloomy matrix in sea for global error function, α is ratio of damping;

The end condition of iteration is:

$\max \left\{\right|\frac{p^t-p^{t-1}}{(p^t+p^{t-1})/2}\left|\right\}<\mathrm{\&epsiv;}---\left(14\right)$

In formula (14), ε is threshold value, described threshold epsilon=1.5 × 10 ^-6;

Step 5, simplified (i.e. Gauss-Newton method) formula (13) by Jacobian matrix, described Jacobian matrix is:

$J\left(p\right)={\left[\frac{\&PartialD;e_j}{\&PartialD;p_i}\right]}_{j=1,...,n;i=1,...,9}=\left[\begin{array}{c}\&dtri;e_1{\left(p\right)}^T\\ \&dtri;e_2{\left(p\right)}^T\\ .\\ .\\ .\\ \&dtri;e_n{\left(p\right)}^T\end{array}\right]---\left(15\right)$

Then the gradient of global error equation and Hai Sen matrix adopt Jacobian matrix to be expressed as:

$\&dtri;f\left(p\right)=\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{e}_j\left(p\right)\&dtri;e_j\left(p\right)=J{\left(p\right)}^{T}e\left(p\right)---\left(16\right)$

$\begin{array}{c}{\&dtri;}^{2}f\left(p\right)=\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}\&dtri;e_j\left(p\right)\&dtri;e_j{\left(p\right)}^T+\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{e}_j\left(p\right){\&dtri;}^2{e}_j\left(p\right)\\ =J{\left(p\right)}^{T}J\left(p\right)+\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{e}_j\left(p\right){\&dtri;}^2{e}_j\left(p\right)\\ \&ap;J{\left(p\right)}^{T}J\left(p\right)\end{array}---\left(17\right)$

Wherein extra large gloomy matrix is similar to, and have ignored quadratic term.

Formula (16) and formula (17) are substituted into formula (13) simplify, the iterative equation after being simplified, shown in (18):

$p^{t+1}=p^t-\mathrm{\&alpha;}\frac{J{\left(p\right)}^{T}e\left(p\right)}{J{\left(p\right)}^{T}J\left(p\right)}---\left(18\right)$

Step 6, initial calibration parameter is set, and this initial calibration parameter is substituted into the iterative equation (18) after simplifying, with formula (14) for condition solves calibration parameter p, wherein, initial calibration parameter p ₀=[1 0010100 0] ^t, after solving calibration parameter p, then the three axis accelerometer substituted into after formula (7) acquisition calibration exports data.

Compared with prior art, beneficial effect of the present invention is: calibration process of the present invention only needs to gather several groups of differences towards static accelerometer data, namely can accurate and effective calibrating accolerometer easily, reduce the impact that accelerometer error brings, have easy and simple to handle, not needing high correcting device and control system, is a kind of accelerometer calibration method that can generally use.

Accompanying drawing explanation

Fig. 1 output effect figure that to be model be before MPU6050 3-axis acceleration meter calibrating;

Fig. 2 adopts the output effect figure after the present invention is based on the three axis accelerometer automatic calibrating method of Gauss-Newton.

Embodiment

Below in conjunction with the drawings and specific embodiments, content of the present invention is described in further details.

Embodiment

With sensor MPU6050, (it comprises three axis accelerometer and gyroscope, here only use three axis accelerometer) data bit Z-axis direction under horizontal stationary place the data instance that a period of time collects, stopped by above-mentioned calibration steps iterative equation t=7 up-to-date style (13) Suo Shi, thus obtain the value of 9 calibration parameters, be respectively: Mxx=0.9870, Mxy=0.1423, Mxz=0.005172, Myy=0.9873, Myz=0.004121, Mzz=0.9922, Bx=-0.1866, By=-0.0515, Bz=-0.0131, then the average that after obtaining calibration, three axis accelerometer exports, below experimental result:

As shown in Figure 1, the average that before calibration, three axis accelerometer exports is respectively :-0.3604 ,-0.2814,9.780.

As shown in Figure 2, the average that after calibration, three axis accelerometer exports is respectively :-0.1205 ,-0.1012,9.799.

In theory, horizontal positioned accelerometer under Z-axis direction, the value of output is: 0,0,9.8, through calibration, can find out with the average of calibration post-acceleration meter output before calibrating, the precision after calibration significantly improves (output after calibration is closer to theoretical value).

By the effect contrast figure before comparison calibration and after calibration, can find out that this calibration steps effectively can improve the precision of accelerometer.

Above-listed detailed description is illustrating for possible embodiments of the present invention, and this embodiment is also not used to limit the scope of the claims of the present invention, and the equivalence that all the present invention of disengaging do is implemented or changed, and all should be contained in the scope of the claims of this case.

Claims (1)

1. based on the three axis accelerometer automatic calibrating method of Gauss-Newton, it is characterized in that, described automatic calibrating method only considers the drift error of three axis accelerometer, proportional error and non-orthogonal errors, and does not consider the error that temperature variation and random drift are brought, and it comprises the following steps:

Step 1, set up calibrating patterns:

V＝KNA+B (1)

In formula (1): V is that output valve measured by three axis accelerometer:

V＝[V _xV _yV _z] ^T(2)

K is proportional error matrix:

K＝diag{K _x,K _y,K _z} (3)

N is non-orthogonal errors matrix:

$N=\left(\begin{array}{ccc}{N}_{\mathrm{xx}}& N_{\mathrm{xy}}& N_{\mathrm{xz}}\\ N_{\mathrm{yx}}& N_{\mathrm{yy}}& N_{\mathrm{yz}}\\ N_{\mathrm{zx}}& N_{\mathrm{zy}}& N_{\mathrm{zz}}\end{array}\right)---\left(4\right)$

A is the value that accelerometer ideal exports:

A＝[A _xA _yA _z] ^T(5)

B is drift error matrix:

B＝[B _xB _yB _z] ^T(6)

Step 2, under the impact not considering temperature variation, proportional error matrix K and non-orthogonal errors matrix N are normal matrix, and the two is merged into a matrix, and now, formula (1) can be rewritten into:

A＝M(V-B) (7)

Wherein:

$M={\left(\mathrm{KN}\right)}^{-1}=\left(\begin{array}{ccc}{M}_{\mathrm{xx}}& M_{\mathrm{xy}}& M_{\mathrm{xz}}\\ M_{\mathrm{yx}}& M_{\mathrm{yy}}& M_{\mathrm{yz}}\\ M_{\mathrm{zx}}& M_{\mathrm{zy}}& M_{\mathrm{zz}}\end{array}\right)\&ap;\left(\begin{array}{ccc}{M}_{\mathrm{xx}}& M_{\mathrm{xy}}& M_{\mathrm{xz}}\\ M_{\mathrm{xy}}& M_{\mathrm{yy}}& M_{\mathrm{yz}}\\ M_{\mathrm{xz}}& M_{\mathrm{yz}}& M_{\mathrm{zz}}\end{array}\right)---\left(8\right)$

Step 3, when three axis accelerometer is static measure is acceleration of gravity constant g=9.8m/s ², so have in the ideal case:

A _x ²+A _y ²+A _z ²＝g ²(9)

In a calibration process, static for three axis accelerometer the n of being placed on different towards position carry out n time and measure, gather the output data of three axis accelerometer, the error e of definition kth time measurement _kfor:

e _k＝V _xk ²+V _yk ²+V _zk ²-g ²(10)

In formula (10): 1≤k≤n, n will equal greatly calibration parameter number;

Therefore, the global error function measured for n time is least squares formalism:

$f\left(p\right)=\underset{k=1}{\overset{n}{\mathrm{\&Sigma;}}}{e_k}^2\left(p\right)---\left(11\right)$

In formula (11), p is calibration parameter, p=[p ₁... p ₉]=[M _xxm _xym _xzm _yym _yzm _zzb _xb _yb _z] ^t;

From formula (11), described global error function is the nonlinear equation of calibration parameter, and formula (11) can be write as:

f(p)＝e(p) ^Te(p) (12)

In formula (12), e (p)=[e ₁(p) e ₂(p) ... e _n(p)] ^t;

Step 4, by set up Newton iteration equation adopt Newton iteration method solve formula (12), Newton iteration method has the characteristic of quadratic convergence, only need to provide an initial calibration parameter value can solve formula (12), the Newton iteration equation of formula (12) through type (13) is to solution iteration:

$p^{t+1}=p^t-\mathrm{\&alpha;}\frac{\&dtri;f\left(p^t\right)}{{\&dtri;}^{2}f\left(p^t\right)}---\left(13\right)$

In formula (13): p ^tfor the vectorial p of calibration parameter composition is the iterative value of the t time, for the gradient of global error function, the gloomy matrix in sea for global error function, α is ratio of damping;

The end condition of iteration is:

$\max \left\{\right|\frac{p^t-p^{t-1}}{(p^t+p^{t-1})/2}\left|\right\}<\mathrm{\&epsiv;}---\left(14\right)$

In formula (14), ε is threshold value, described threshold epsilon=1.5 × 10 ^-6;

Step 5, simplified formula (13) by Jacobian matrix, described Jacobian matrix is:

$J\left(p\right)={\left[\frac{{\&PartialD;e}_j}{{\&PartialD;p}_i}\right]}_{j=1,...,n;i=1,...,9}=\left[\begin{array}{c}\&dtri;e_1{\left(p\right)}^T\\ {\&dtri;e}_2{\left(p\right)}^T\\ .\\ .\\ .\\ {\&dtri;e}_n{\left(p\right)}^T\end{array}\right]---\left(15\right)$

Then the gradient of global error equation and Hai Sen matrix adopt Jacobian matrix to be expressed as:

$\&dtri;f\left(p\right)=\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{e}_j\left(p\right){\&dtri;e}_j\left(p\right)=J{\left(p\right)}^{T}e\left(p\right)---\left(16\right)$

$\begin{array}{c}{\&dtri;}^{2}f\left(p\right)=\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\&dtri;e}_j\left(p\right){\&dtri;e}_j{\left(p\right)}^T+\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{e}_j\left(p\right){\&dtri;}^2{e}_j\left(p\right)\\ =J{\left(p\right)}^{T}J\left(p\right)+\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{e}_j\left(p\right){\&dtri;}^2{e}_j\left(p\right)\\ \&ap;J{\left(p\right)}^{T}J\left(p\right)\end{array}---\left(17\right)$

Formula (16) and formula (17) are substituted into formula (13) simplify, the iterative equation after being simplified, shown in (18):

$p^{t+1}=p^t-\mathrm{\&alpha;}\frac{J{\left(p\right)}^{T}e\left(p\right)}{J{\left(p\right)}^{T}J\left(p\right)}---\left(18\right)$

Step 6, initial calibration parameter is set, and this initial calibration parameter is substituted into the iterative equation after simplifying, with (14) for condition solves calibration parameter p, wherein, initial calibration parameter p ₀=[1 0010100 0] ^t.