JP2011227017A - Device and method for attitude estimation of moving body using inertial sensor, magnetic sensor, and speed meter - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a device and a method for attitude estimation of a moving body using a gyroscope, an accelerometer, a magnetic sensor, and a speed meter, in which a set of light-weight compact and inexpensive sensors with a low precision and a Kalman filter are combined to estimate the attitude of the moving body with an enough precision by using a quaternion for calculation of the attitude information and using the quaternion as a correction amount calculated from observed values of the sensors as well to estimate an attitude error of the moving body with a high precision.SOLUTION: An aircraft is provided with a gyro device 11, an accelerometer 12, a magnetic sensor 14 and a speed meter 13. Attitude information of the aircraft is calculated by computing data obtained from these sensors by a computing unit 15 and a Kalman filter 19. In the calculation of the attitude information, quaternion is used. The quaternion as a correction amount calculated from observed values is multiplied with the calculated values so as to obtain an attitude estimation device that can estimate a precise attitude.

Description

  The present invention includes a gyro, an accelerometer, a magnetic sensor, and a speedometer mounted on the moving body, and estimates and removes the posture error of the moving body from these sensors, and the posture estimation apparatus for the moving body with high accuracy. Regarding the method.

By accurately estimating the attitude of a moving object, for example, an aircraft, displaying the estimated result on an instrument or using it for maneuvering can help safe maneuvering.
There is an inertial navigation unit (IMU) that measures the position, speed, attitude, and direction of an aircraft, etc. during navigation. This is an extremely accurate measurement of the relative attitude angle change from the reference absolute attitude angle. Can be calculated.
Various methods using a gyro sensor or the like and incorporating a Kalman filter for estimating error correction have been proposed as methods for estimating the posture of a moving object.

  The posture measurement method, posture control device, azimuth meter, and computer program of Patent Document 1 are intended to be able to measure the posture of an object or a azimuth including true north in a shorter time than before. The time update of the Kalman filter in the specific configuration was obtained by the observation update of the previous step, assuming that the estimated value hat and error covariance of the state variable displayed using the quaternion of the attitude of the attitude measurement device are invariant with time Equal to the value. In the observation update, the gyroscope and the acceleration sensor are used to measure the angular velocities around the three axes orthogonal to each other and the accelerations in the three axial directions, and the estimated values of the state variables and Calculate the error covariance. The Kalman filter process is repeated until the error covariance becomes sufficiently small to obtain a state variable indicating the posture.

  The inertial navigation device, the initialization method of the inertial navigation device, and the recording medium disclosed in Patent Document 2 aim to increase the initial alignment accuracy and speed even in a certain operating environment, and to reduce the operational restrictions of the aircraft. It is what. The specific configuration is processing means for performing Kalman filter processing in fine alignment of initial alignment processing, and monitoring the Kalman filter convergence status during processing by this processing means to determine whether the convergence speed of the Kalman filter is excessive or not. And a updating unit that updates the Kalman filter based on the azimuth error estimated by the Kalman filter if the monitoring unit determines that the convergence speed is excessive. The processing unit is based on the updating unit. After the update, the Kalman filter process is performed again.

  The azimuth / orientation reference apparatus disclosed in Patent Document 3 is intended to speed up alignment in a strap-down azimuth / orientation reference apparatus so that it can be implemented even in-flight. In the coordinate correction matrix (CTM) error correction, the posture angle error correction and the azimuth angle error correction are separated, and the azimuth angle error is corrected after the posture angle error correction. First, when the estimated value of the attitude angle error can be obtained at a predetermined angle, the error of the coordinate transformation matrix is corrected by the estimated value of the attitude error, and after that, the coordinate transformation matrix is determined by the setting value of the azimuth angle. The error is corrected. A Kalman filter is used as the azimuth orientation error calculation unit.

  The moving body control device and the moving body control method disclosed in Patent Document 4 are intended to realize a composite navigation system using a lightweight, small, and inexpensive sensor. Specifically, state estimation filter calculation is performed based on inertial navigation data and positioning data, and information on the position, posture, and speed of the moving body is output. At this time, in calculating the position, orientation, and speed errors of the moving object in the state estimation filter calculation for the inertial navigation data, it is related to the state variables representing each of the position, orientation, and speed, and the number of elements is smaller than that. Using the error information displayed as a small change unit quaternion specified by the state variable, each position, posture, and speed error is calculated, and the error information is multiplied to correct the position, posture, and speed information. And a moving body control device used for controlling the moving body.

JP 2006-38650 A JP 2001-264106 A Japanese Patent Laid-Open No. 11-148827 JP 2007-276507 A

Patent Document 1 uses a Kalman filter in posture measurement to process the error sufficiently, but only an acceleration sensor that measures acceleration in each of the three axes and a gyro that measures angular velocity around the three axes are used as sensors. It is used as input means and does not take into account data obtained from magnetic sensors and speedometers.
Further, although Patent Document 2 uses a Kalman filter as an inertial navigation device, it is not configured to estimate an error in azimuth and attitude angle and remove the estimated error, and a sensor unit for processing the gyro sensor and Only an accelerometer.
Patent Document 3 separates the azimuth angle error correction by the Kalman filter and corrects the attitude angle error and then corrects the azimuth angle error. As sensors, an X gyro, a Y gyro and a Z gyro, an X accelerometer, a Y accelerometer, Only the angular velocity and acceleration data from the Z accelerometer are obtained. Patent Document 4 uses data of an acceleration sensor, a gyro sensor, and a GPS signal receiving unit as sensors, and uses an observation value of a magnetic sensor or a speedometer for calculation of error information displayed as a minute change unit quaternion. is not.

By the way, the inventor of the present invention uses not only the measured values of angular velocity and acceleration from the gyroscope and accelerometer, but also the values of the azimuth angle, geomagnetic depression angle and velocity of the magnetic sensor and the speedometer when estimating the posture of the moving body, We considered that the output of these sensors could be processed with a Kalman filter to estimate the posture error and estimate the posture of the moving body with good accuracy.
Therefore, in order to estimate the posture accurately with respect to the actual posture, the geomagnetism obtained by the magnetic sensor and the gravitational acceleration obtained by the accelerometer are used as input vectors for posture estimation. In addition, an external force component derived from air or the like is added, which causes an error in posture estimation particularly during turning or acceleration.

In order to cope with this, a method of removing an acceleration component derived from an external force other than gravitational acceleration can be considered.
[1] Method for estimating external force in X, Y, and Z directions using a Pitot tube By multiplying the velocity obtained by the Pitot tube by the angular velocity, the turning acceleration can be obtained for the Y and Z axes, and the speed time obtained by the Pitot tube The acceleration about the X axis is obtained from the change. By subtracting these from the values obtained by the accelerometer, the gravitational acceleration can be obtained as shown in equation (1). Where a is the measured acceleration, g is the gravitational acceleration, and ω is the angular velocity. v _x is the velocity about the X axis.

Although the acceleration input to the Kalman filter is corrected by the equation (1), the observation equation is not different from the conventional Kalman filter.
This method has a drawback that it cannot remove external forces other than turning acceleration and longitudinal acceleration. Further, when the wind speed is not constant, an acceleration that does not exist originally is input. For example, if the wind speed is increasing when the aircraft is flying in a headwind, the aircraft will decelerate because the drag increases, but if you look only at the value of the Pitot tube, it seems that it is accelerating, so the calculated gravitational acceleration is It appears in a different direction from the actual one.

[2] Method for estimating external force not using Pitot tube using Kalman filter The state quantity and external force estimated by the Kalman filter are added. When the external force estimated from the value obtained by the accelerometer is subtracted, the gravitational acceleration is obtained as shown in equation (2). Because it does not depend on the Pitot tube, it is not affected by changes in wind speed.

となる。これに外力の推定を加えるとａ＝ｇ＋ｆ_e から（４）式のようになる。

The observation equation is given by
The observation equation showing the relationship between the difference between the gravitational acceleration vector g ^b measured at the aircraft fixed coordinates and the gravitational acceleration vector g ^{b ′} expected to be obtained from the current estimated value and the deviation q _{e of the} estimated posture value is

It becomes. When the estimation of external force is added to this, the equation (4) is obtained from a = g + _fe .

共分散行列は、現在の推定値の不確かさを表す量であるから、Ｓのｆ_e に関する対角成分に適当な数値を入力すれば、ｆ_e の不確かさを適宜上昇させることができ、加速度が観測されたときに修正されるようにすることができる。 Next, the time variation of covariance is as follows.
Since f _e is an operation amount related to the motion of the aircraft, it is impossible to formulate a change with time, so the value of f _e is not changed except by correction by observation.

The covariance matrix, since a quantity representing the uncertainty of the current estimate, by entering the appropriate value to diagonal elements relating f _e of S, it is possible to increase appropriately the uncertainty of f _e, acceleration Can be modified when observed.

Since the past estimated value of f _e is continuously used for estimation, it is considered that the external force such as a sudden turn and a sudden operation of the throttle is vulnerable to a drastic change. In addition, the magnitude of S, which is a parameter that determines the temporal change of covariance, needs to be set large when the external force is expected to change drastically and small when the change is expected to be small. It is necessary to consider a method for quantitative determination.

[3] External force estimation method combining a Pitot tube and a Kalman filter It is conceivable to combine the estimation of turning acceleration using a Pitot tube with the above-described estimation of the external force without using the Pitot tube.
Excludes turning acceleration, which is thought to change significantly depending on the operation, from the estimated value, so it is resistant to sudden changes in external force. The estimated external force is small and the Pitot tube is used to determine the parameters that determine the covariance change over time. The accuracy is not required as compared with the case where it is not.

The time variation of the observation equation and covariance is the same as the external force estimation method that does not use the Pitot tube.
There is also a need for a method for quantitatively determining a method for determining the magnitude of S, which is a parameter that determines the temporal change in covariance, because it cannot cope with a sudden change in external force other than acceleration due to turning.

  The present invention has been made in view of the above considerations, and its purpose is light weight, small size, and low cost. By combining a low-accuracy sensor group (gyro, accelerometer, speedometer and magnetic sensor) and a Kalman filter, posture information The quaternion is used in the calculation, and the correction amount calculated from the sensor observation value is also a quaternion, so that the posture error of the moving body can be estimated with high accuracy, and the posture of the moving body can be estimated with sufficient accuracy. An object of the present invention is to provide a posture estimation apparatus and a posture estimation method for a moving body using an inertial sensor, a magnetic sensor, and a speedometer.

In order to achieve the above object, an attitude estimation apparatus according to claim 1 of the present invention includes means for using observation values of an inertial sensor including a gyroscope or an accelerometer, a speedometer, and a magnetic sensor, and state estimation based on the observation values. A state estimation filter calculation unit that performs filter calculation and outputs information on the posture of the moving body; and an error calculation unit that calculates each error of the posture of the moving body in the state estimation filter calculation for the observation value; The error calculation means relates to a state variable representing each posture output by the state estimation filter calculation means, and the error information calculated as a minute change unit quaternion specified by a state variable having a smaller number of elements than that. And calculating each error of the posture, and each information of the posture of the moving body output by the state estimation filter calculating means is calculated by the error calculating means. Characterized in that it is corrected by multiplying the difference information.
The posture estimation method according to claim 2 of the present invention performs state estimation filter calculation based on observation values of an inertial sensor, a speedometer, and a magnetic sensor including a gyroscope or an accelerometer, and outputs state information of the moving body. A filter calculation step and an error calculation step of calculating each error of the posture of the moving body in the state estimation filter calculation for the observation value. In the error calculation step, the posture output by the state estimation filter calculation means Each state error is calculated using error information calculated as a minute change unit quaternion specified by a state variable having a smaller number of elements than the state variable representing each state, and the state estimation filter Each information of the posture of the moving body output by the calculation step is corrected by multiplying by the error information calculated by the error calculation means.

  According to the above configuration, it is possible to realize a posture estimation apparatus and a posture estimation method for estimating an error that should realize a posture of a moving body with high accuracy using a lightweight, small, and inexpensive sensor such as a MEMS sensor. It is possible to provide an attitude display device that accurately displays the attitude and position of a moving object from the apparatus and method, for example, an aircraft instrument display.

It is the schematic of the attitude | position estimation apparatus of the moving body using the inertial sensor, magnetic sensor, and speedometer by this invention, and is a case where it applies to an aircraft. It is the figure which showed the structure of the attitude | position estimation apparatus of the moving body using the inertial sensor, magnetic sensor, and speedometer by this invention with the block. It is a figure for demonstrating definition of (omega), X, Y, Z. It is a figure for demonstrating the flow of the process by a Kalman filter at the time of applying this invention to an aircraft. It is a graph which shows the estimation result of the roll angle and pitch about the data which is turning horizontally about the attitude angle of the aircraft estimated by the present invention.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
FIG. 1 is a schematic diagram of a mobile body posture estimation apparatus using an inertial sensor, an accelerometer, a magnetic sensor, and a speedometer according to the present invention, which is applied to an aircraft.
A gyro device 11, an accelerometer 12, a speedometer 13, and a magnetic sensor 14 are mounted on the airframe. The gyro apparatus is a MEMS (Micro Electro Mechanical Systems) gyro, which obtains a rotational angular velocity ω by a Coriolis force that a micro diaphragm vibrating in advance receives by rotation.
The accelerometer 12 is also a MEMS sensor and outputs an acceleration a as the aircraft moves.

Moreover, the speedometer 13 is comprised with the MEMS sensor, and outputs airspeed. The magnetic sensor 14 provides a magnetic vector. This magnetic sensor can also be composed of a MEMS sensor. The calculator 15 receives the acceleration a, the velocity V, and ω _z ω _y and calculates and outputs only the gravitational acceleration g. However, the external force _fe is not completely removed in practice.
The Kalman filter 19 integrates the angular velocity ω input from the gyro device 11 and calculates the rotation angle. With respect to the calculated rotation angle, the calculator 17 can obtain an estimated value of the quaternion q that displays the posture 18 based on the gravitational acceleration g including _fe , the magnetic vector, and the rotation angle from the integrator 16. In the Kalman filter 19, a predetermined calculation is performed using the observed value or estimated value from each sensor, and the error is estimated in the form of a covariance matrix.

FIG. 2 is a block diagram showing the configuration of a moving body posture estimation apparatus using an inertial sensor, a magnetic sensor, and a speedometer according to the present invention, which is applied to an aircraft as in FIG.
This is because the external force _fe 21 obtained by the computation by the filter computation unit 22 is output, the external force _fe is inputted to the computing unit 24, the external force _fe is also removed, and the gravitational acceleration g23 is inputted to the filter computation unit 22. The other parts are the same as those in FIG.

の４つの変数であり、ｑ₀ ，ｑ₁ ，ｑ₂ ，ｑ₃ の４つの変数で姿勢を表す。
〔３〕微小な回転を表すクオータニオン変数
微小な回転を表すクオータニオン変数はｑ_e で表され、ｑ_e は

の４つの変数であり、１，ｑ_e1，ｑ_e2，ｑ_e3で表される。１つ目の成分は「１」という定数となる。すなわち、微妙な回転を表す姿勢誤差である。
（１）カルマンフィルタについて
本発明による姿勢推定装置に用いられるシステムはつぎのように表されるとする。

ただし、ｘは推定した状態量，ｕは入力，ｗは外乱である。また、推定値がどの程度信頼できるものであるかを示す共分散行列Ｐも合わせて考える。
○Time Update について
本発明による姿勢推定装置の処理の流れとして時間経過に対し状態量，共分散行列が係わる式はつぎのようになる。

○Measurement Updateについて
本発明による姿勢推定装置の各センサの観測値入力に対し状態量，カルマンゲイン，共分散行列，行列（変数Ｈ）などが係わる式はつぎのようになる。

だだし、ｖを観測雑音として

Next, in order to realize the function of the posture estimation apparatus according to the present invention, a Kalman filter, a posture angle, a posture angle error, a vector to be observed, and mathematical formulas used for observation equations will be described.
In the description of the mathematical formulas, the coordinate system, the quaternion variable, and the quaternion variable representing a minute rotation are defined.
[1] Coordinate system X, Y, Z axis and ω
The three axes are represented as shown in FIG. 3, the X axis is the aircraft traveling direction, and ω is the angular velocity around each of the X, Y, and Z axes. Ω ₀ (ω _x ) for the X axis, ω ₁ (ω _y ) for the Y axis, and ω ₂ (ω _z ) for the Z axis.
[2] Quartanion variable The quarteranion variable is represented by q,

The posture is represented by the four variables q ₀ , q ₁ , q ₂ , and q ₃ .
[3] quaternion variables representing quaternion variable small rotation representing a minute rotation is represented by q _e, q _e is

_Are represented by 1, q _e1 , q _e2 , and q _e3 . The first component is a constant “1”. That is, it is an attitude error representing a delicate rotation.
(1) About Kalman filter It is assumed that the system used in the posture estimation apparatus according to the present invention is expressed as follows.

Where x is the estimated state quantity, u is the input, and w is the disturbance. Further, a covariance matrix P indicating how reliable the estimated value is is considered.
○ About Time Update As the processing flow of the posture estimation apparatus according to the present invention, the equation relating the state quantity and the covariance matrix over time is as follows.

○ Measurement Update Expressions relating to the state value, Kalman gain, covariance matrix, matrix (variable H), etc. with respect to the observation value input of each sensor of the posture estimation apparatus according to the present invention are as follows.

However, v is the observation noise

で与えられるベクトルＸ^' が変換されたベクトルになっていることを意味する。（ただしｑ^* は共役クオータニオン）この変換は行列で表すこともでき、例えば地面固定座標をｎ，機体固定座標をｂとして変換行列を

また、ｑの時間変化は

で表される。ただし、ωは地面固定座標で計測された角速度である。 (2) Representation of system Quartanion q is used to represent the posture angle and posture angle error when the posture estimation apparatus according to the present invention is applied to a moving object.
○ Representation of posture angle The coordinate system takes the fixed coordinates of the body with X as the north, Y as the east, Z as the bottom, Z as the ground fixed coordinates, X as the front, Y as the right, and Z as the bottom. The posture at this time is represented by a quarteranion q.
This quarteranion gives a variable from ground fixed coordinates to aircraft fixed coordinates. A quaternion gives a coordinate transformation for a vector x

This means that the vector X ^′ given by is a transformed vector. (However, q ^* is a conjugate quota) This transformation can also be expressed as a matrix. For example, the ground fixed coordinate is n and the aircraft fixed coordinate is b.

In addition, q changes over time

It is represented by Here, ω is an angular velocity measured with fixed ground coordinates.

を考え（ただし、ｑ_e は３つの要素からなるベクトル）、ｑの推定値

と誤差との和ではなく積の形で表すことにする。
また、この誤差の時間変化は機体固定座標で計測された角速度の誤差をΔωとすると

誤差同士の積は微小であるため無視すると

を得る。つまり、

○観測するベクトルの表現
観測量として地磁気ベクトルと重力加速度ベクトルを姿勢角の修正に用いるが、加速度計により得られる加速度はそのままでは重力以外の外力も含んだものであるのでこれを除去する。
まず、移動体が横すべりなしで旋回を行っている場合、旋回により生じる加速度は、速度をＶとして

と表されるので、まずこれを加速度計により得られた加速度から減ずる。
これにより直進時・定常旋回時ともに重力加速度を正しく得ることができるようになったと考えられるが、それ以外の場合においても姿勢を正しく計算するため、旋回加速度以外の外力をカルマンフィルタにより推定する。
以上をまとめると重力加速度ｇは加速度計により得られる加速度ａに以下の計算を施すことにより得られる。

ただし、ｆ_e は推定された旋回加速度以外の外力を示す。 ○ Expression of posture angle error There is a restriction that the absolute value must be 1 for the quaternion that represents the posture, but this restriction cannot be observed by the method of updating the estimated value by adding the difference obtained by the Kalman filter. . Quartanion that represents a minute rotation as an error

(Where q _e is a vector of three elements) and q is estimated

It is expressed in the form of the product, not the sum of and the error.
In addition, the time variation of this error is, if the error of the angular velocity measured in the aircraft fixed coordinates is Δω

Since the product of errors is very small,

Get. In other words,

○ Representation of observed vector The geomagnetic vector and gravitational acceleration vector are used for correcting the posture angle as observation quantities. However, the acceleration obtained by the accelerometer as it contains external force other than gravity is removed.
First, when the moving body is turning without a side slide, the acceleration generated by the turning is V

First, this is subtracted from the acceleration obtained by the accelerometer.
As a result, it is considered that the gravitational acceleration can be obtained correctly both during straight running and steady turning, but in other cases, the external force other than the turning acceleration is estimated by the Kalman filter in order to correctly calculate the posture.
In summary, the gravitational acceleration g can be obtained by performing the following calculation on the acceleration a obtained by the accelerometer.

Here, _fe represents an external force other than the estimated turning acceleration.

であるので、

あるベクトル量について地面固定座標で測定した量をＶⁿ ，機体固定座標で測定した量Ｖ^b と書くことにすると、

となり、機体固定座標で測定されたベクトルＶ^b と、現在の推定値から得られると予想されるベクトルＶ^b'の差を入力とした観測方程式を構成できる。 (3) Observation equation Assume that the geomagnetic vector is obtained by the geomagnetic sensor and the gravitational acceleration is obtained on the body axis by the accelerometer as the observation quantities to be used.
At this time, assuming that the estimated value represents a conversion to a coordinate b ^′ shifted by an error from the airframe fixed coordinate, a conversion matrix for q _e is

So

If a certain vector quantity is measured with the ground fixed coordinates as V ⁿ , and the quantity measured with the airframe fixed coordinates as V ^b ,

Next, it can be configured and vector V ^b measured in body-fixed coordinate, the observation equations as input difference vector V ^{b 'which} is expected to be obtained from the current estimate.

FIG. 4 is a diagram for explaining the flow of processing by the Kalman filter when the present invention is applied to an aircraft. The expression form in the symbols used thereafter is defined as follows.
The expression “q-hat” is equivalent to a value obtained by placing a hat mark indicating the estimation on the quarter-onion q. Not only the quaternion but also a symbol “P”, “x”, etc., which is expressed at the back of the symbol, is assumed to be the estimated value of that symbol.
The expression “P bar” is equivalent to the current value in the covariance matrix P. Not only the covariance matrix but also a “bar” after a symbol such as x is assumed to be the current value of that symbol.
Moreover, although it describes with (1) Formula-(29) Formula for numerical formula specification, these numerical formulas are not the numerical formula described in the item of [the subject which an invention is going to solve], but [the form for carrying out the invention] It is the mathematical formula described in the item. Further, the step of using each mathematical expression in the flow of the algorithm of the Kalman filter in FIG. 4 is expressed as an operation unit for calculating the mathematical expression.
The process of the posture estimation apparatus according to the present invention is realized by a program process that executes a Kalman filter algorithm.

In FIG. 4, the input of the acceleration a of the accelerometer 31, the speed V of the speedometer 32, the external force _fe and the angular velocity ω from the gyro 34, which are observed values, is sent to the calculation unit 40. The calculation unit 40 calculates the equation (22) to calculate the gravitational acceleration. In Equation (22), ω ₂ is an angular velocity around the Z axis, and ω ₁ is an angular velocity around the Y axis. The gravitational acceleration obtained from the observed value is g on the left side of equation (22). This gravitational acceleration g is input to the calculation unit 41. Further, the magnetic vector observed from the magnetic sensor 33 is output.
In equation (25) of the calculation unit 41, v ^b is a vector measured with the fixed body coordinates, and v ^b includes a triaxial gravity acceleration and a triaxial magnetic vector. When decomposed, G _(b) and M _(b) are output.

と同じになる。
よって

から（８）式は（２８）式に書き換えられる。

（２８）式において右辺の行列は変数Ｈである。 Here, v ^{b ′} −v ^b in the expression (25) is in the parenthesis on the right side of the expression (8).

Will be the same.
Therefore

To (8) can be rewritten as (28).

In the equation (28), the matrix on the right side is the variable H.

となる。よって、演算部４３において

となり、（２９）式より推定したクオータニオンの姿勢誤差ｑ_e と推定した外力の誤差Δｆ_e が得られる。
なお、演算部４２では計算した（２５）式におけるｖ^b'において、推定されたクオータニオンｑから推定された重力加速度Ｇ_(b')と推定された磁気ベクトルＭ_(b')とが出力されている。演算部４４には観測値から得た重力加速度から分解して得たＧ_(b) と上記推定された重力加速度Ｇ_(b')が入力され、Ｇ_(b) −Ｇ_(b')の演算が行われ、演算部４３と演算部５１にそれぞれ入力される。
また演算部４５では観測した磁気ベクトルから分解して得たＭ_(b) と上記推定された磁気ベクトルＭ_(b')が入力され、Ｍ_(b) −Ｍ_(b')の演算が行われ、演算部４３と演算部５１にそれぞれ入力される。
演算部４３で出力された外力の誤差量Δｆ_e は演算部５５に加えられ、演算部５５でｆ_e が算出され、算出されたｆ_e が演算部４０に入力される。演算部４０では先に述べたように（２２）式の演算が行われ、外力の誤差ｆ_e を除去した重力加速度ｇが得られる。 When attention is paid to the equation (8), the one with a bar in x is an initial estimated value, but by inputting the angular velocity, the bar becomes a hat and becomes an x hat. If the angular velocity does not enter, calculate by changing the hat to a bar.
Therefore, the estimated state quantity is from equation (8).

It becomes. Therefore, in the calculation unit 43

Thus, the posture error q _{e of the} quaternion estimated from the equation (29) and the error Δf _e of the estimated external force are obtained.
Note that the calculation unit 42 outputs the gravitational acceleration G _{(b ′)} estimated from the estimated quaternion q and the estimated magnetic vector M _{(b ′)} in v ^{b ′} in the calculated equation (25). Yes. G _(b) obtained by decomposing from the gravitational acceleration obtained from the observed value and the estimated gravitational acceleration G _{(b ′)} are input to the calculation unit 44, and calculation of G _(b) −G _{(b ′)} is performed. Are input to the calculation unit 43 and the calculation unit 51, respectively.
The M obtained by decomposing the magnetic vector of observing the arithmetic unit 45 _(b) and the estimated magnetic vector M _{(b ')} is _{input, M (b) -M (b} ' _operation) is performed Are input to the calculation unit 43 and the calculation unit 51, respectively.
The error amount Δf _e of the external force output by the calculation unit 43 is added to the calculation unit 55, the _fe is calculated by the calculation unit 55, and the calculated _fe is input to the calculation unit 40. As described above, the calculation unit 40 calculates the equation (22) to obtain the gravitational acceleration g from which the external force error _fe is removed.

の演算が行われる。得られる推定したクオータニオンｑハットは演算部４６の（１６）式にフィードバック入力される構成となっている。ｑハットを演算部１６にフィードバックする経路の中間に切替部５７（ソフト処理）の機能を有している。切替部５７は、磁気センサ，加速度計，速度計で観測した場合は端子ａ側に接続されるため、上述のようにｑハットは演算部４６にフィードバック入力される。しかしながら、例えば、センサや加速度計，速度計が故障したり、また、ある地域で観測値を得ることを望まない，観測できない状況などの場合には観測しないので、かかるセンサなどから入力がないため、切替部５７は端子ｂに接続されるようになっている。
したがって、磁気センサ，加速度計，速度計で観測しない場合には、観測値の入力は切替部５７の端子ｂ側に切り替えられているためフィードバックは行われない。
すなわち、観測した場合は、クオータニオンｑを求めるループは演算部４６，４７，４８により形成され、観測しない場合は演算部４６，４７のループが形成され、クオータニオンｑが繰り返し算出され出力される。
このようにして演算されたクオータニオンｑは姿勢５８の情報として出力される。また、上述したようにクオータニオンｑの推定値は（２５）式におけるＶ^b'の演算部４２に供給される。演算部４２では（２５）式におけるＶ^b'の演算が行われ、クオータニオンｑの推定された姿勢情報からはこの推定値から得た重力加速度と磁気ベクトルが出力され、演算部４４と４５に入力される。 Next, the operation of the loop 49 for obtaining the quaternion q representing the posture will be described.
The angular velocity ω is input from the gyro 34 to the calculation unit 46, and the calculation unit 46 calculates the equation (16) to obtain a partial differential value of q. Further, the computing unit 47 calculates the quota anion q by solving a differential equation by the Runge-Kutta method or the like. The arithmetic unit 48 receives the quarteranion q and the quarteranion error q _e ,

Is calculated. The estimated quarteranion q hat obtained is fed back to the equation (16) of the calculation unit 46. The function of the switching unit 57 (software processing) is provided in the middle of the path for feeding back q hat to the calculation unit 16. Since the switching unit 57 is connected to the terminal a side when observed by a magnetic sensor, an accelerometer, and a speedometer, the q hat is fed back to the calculation unit 46 as described above. However, there is no input from such a sensor because the sensor, accelerometer, and speedometer are broken, or the observation is not desired in a certain area or the observation is not possible. The switching unit 57 is connected to the terminal b.
Therefore, when observation is not performed using a magnetic sensor, an accelerometer, or a speedometer, feedback is not performed because the input of the observed value is switched to the terminal b side of the switching unit 57.
That is, when observed, a loop for obtaining the quaternion q is formed by the arithmetic units 46, 47, 48. When not observing, a loop of the arithmetic units 46, 47 is formed, and the quaternion q is repeatedly calculated and output.
The quarteranion q calculated in this manner is output as posture 58 information. Further, as described above, the estimated value of the quota anion q is supplied to the V ^{b ′} calculator 42 in the equation (25). The computing unit 42 computes V ^{b ′ in} the equation (25), and the gravitational acceleration and magnetic vector obtained from the estimated value are output from the estimated posture information of the quaternion q, and input to the computing units 44 and 45. Is done.

Next, the operation of the calculation unit 54 including a loop for obtaining the covariance matrix P will be described.
As described above, G _(b) −G _{(b ′)} is calculated by the calculation unit 44 and M _(b) −M _{(b ′)} is calculated by the calculation unit 45 and input to the calculation unit 51. The calculation unit 51 is derived from an expression equivalent to the expression (25), and a matrix of the variable H shown in the expression (28) is calculated. The variable H is input to equation (9) of the calculation unit 52 and equation (10) of the calculation unit 53. Equation (9) is an equation for obtaining Kalman gain K _{K + 1} , and Equation (10) is an equation for obtaining a covariance matrix P _{K + 1} hat.
The Kalman gain K _{K + 1} shown in the equation (9) is an expression formed by a term including a covariance matrix P _{K + 1} bar, H _{K + 1} and R _{K + 1} on the right side. It is input from. Further, the covariance matrix P _{K + 1} hat represented by the expression (10) is an expression formed by a term including a unit matrix I, a Kalman gain K _{K + 1} , a covariance matrix P bar, and H _{K + 1} , and a variable H is input from the calculation unit 51.
The variable H, R _{K + 1} (constant initially set) and P _{K + 1} bar are input to the equation (9) of the calculation unit 52, and the Kalman gain K _{K + 1} is calculated by the equation (9). obtain.
Further, the variable H and the Kalman gain K _{K + 1 of the} calculation unit 52 are input from the calculation unit 51 to the equation (10) of the calculation unit 53, and calculated by the equation (10) to obtain P _{K + 1} hat. Note that P _{K + 1} bar can be obtained from equation (3). The P _{K + 1} bar in equation (3) is an expression formed by terms including Γ, Ψ _K , and S _K , where Γ treats the unit matrix, Ψ _K and S _K as fixed variables.
The P _{K + 1} hat calculated and output by the calculation unit 53 is fed back to the expression (3) of the calculation unit 50. This path has the function of the switching unit 56 (software processing). Since the switching unit 56 is connected to the terminal a side when observed with a magnetic sensor, an accelerometer, and a speedometer as described above, the P _{K + 1} hat is fed back to the calculation unit 50. When no observation is made, there is no input from a sensor or the like, so the switching unit 56 is connected to the terminal b.
If observing the output of a magnetic sensor, Q is the (3) of the arithmetic unit 50 is input P _{K + 1} hat, which further (7) represented by S _K and (5) of the formula _K is input and calculated to obtain P _{K + 1} bar. That is, when observed, a loop for obtaining the covariance matrix P is formed by the arithmetic units 50, 52, and 53. When not observed, a loop of only the arithmetic unit 50 is formed, and the covariance matrix P is repeatedly calculated and output. The

FIG. 5 is a graph showing estimation results of the roll angle and pitch for the data turning horizontally.
In order to verify how effective the above posture estimation is, the simulation of the horizontal turning posture estimation was performed based on the flight data, and the results shown in FIG. 5 were obtained.
The posture q obtained by the present invention is derived from the roll angle and pitch angle by a predetermined conversion formula, and the error range of the roll angle and pitch angle is small.
In FIG. 5, the solid line is the estimated pitch angle, the dotted line is the actual pitch angle, the one-dot chain line is the estimated roll angle, and the two-dot chain line is the actual roll angle. As the roll angle increases, the error tends to increase. However, since the posture error and the external force are effectively estimated, it can be understood that the posture is indicated with high accuracy.

  The above embodiment has been described for the case where the posture estimation device is applied to an aircraft as a moving body, but is not limited to an aircraft and can be similarly applied to an automobile, a ship, and the like, and similar effects can be obtained. Obtainable.

  It is mounted on a moving body such as an aircraft, and the posture of the moving body is estimated, and this posture estimation is reflected on an instrument display unit or the like.

11,34 Gyroscope (Gyroscope)
12, 31 Accelerometer 13, 32 Speedometer 14, 33 Magnetic sensor 15, 17 Calculator 16 Integrator 18, 58 Attitude 19 Kalman filter 40, 41, 42, 43, 44, 45, 46, 47, 48, 50, 51 , 52, 53, 55 Arithmetic unit 49 Loop for obtaining the quaternion q 54 Arithmetic unit 56, 57 including a loop for obtaining the covariance matrix P

Claims (2)

Means utilizing the observed values of inertial sensors, speedometers and magnetic sensors including gyroscopes or accelerometers;
State estimation filter calculation means for performing state estimation filter calculation based on the observed value and outputting information on the posture of the moving body;
Error calculation means for calculating each error of the posture of the moving body in the state estimation filter calculation for the observed value;
The error calculation means relates to a state variable representing each posture output by the state estimation filter calculation means, and the error information calculated as a minute change unit quaternion specified by a state variable having a smaller number of elements than that. To calculate each error of the posture,
An inertial sensor, a speedometer, and a magnetic sensor, each of which is corrected by multiplying each piece of information on the posture of the moving body output by the state estimation filter calculating unit by error information calculated by the error calculating unit. Used mobile body posture estimation apparatus. A state estimation filter calculation step of performing state estimation filter calculation based on observation values of an inertial sensor including a gyroscope or an accelerometer, a speedometer, and a magnetic sensor, and outputting information on the posture of the moving body;
An error calculation step of calculating each error of the posture of the moving body in the state estimation filter calculation for the observation value,
In the error calculation step, error information calculated as a minute change unit quaternion specified by a state variable having a smaller number of elements than the state variable representing each of the postures output by the state estimation filter calculation means. To calculate each error of the posture,
Using an inertial sensor, a speedometer, and a magnetic sensor, wherein each piece of information on the posture of the moving object output by the state estimation filter calculation step is corrected by multiplying the error information calculated by the error calculation means. A method for estimating the posture of a moving object.

Images