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

Device and method for attitude estimation of moving body using inertial sensor, magnetic sensor, and speed meter Download PDF

Info

Publication number
JP2011227017A
JP2011227017A JP2010099462A JP2010099462A JP2011227017A JP 2011227017 A JP2011227017 A JP 2011227017A JP 2010099462 A JP2010099462 A JP 2010099462A JP 2010099462 A JP2010099462 A JP 2010099462A JP 2011227017 A JP2011227017 A JP 2011227017A
Authority
JP
Japan
Prior art keywords
error
posture
moving body
state estimation
estimation filter
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
JP2010099462A
Other languages
Japanese (ja)
Other versions
JP5569681B2 (en
Inventor
Ryo Hashimoto
良 橋本
Masaru Naruoka
優 成岡
Takeshi Tsuchiya
武司 土屋
Hironobu Iwami
浩伸 石見
Kazuyoshi Ishida
和義 石田
Tomoyasu Ichikawa
智康 市川
Yoshinao Oganeku
義直 大兼久
Katsuhisa Fujita
克久 藤田
Shigeki Fujita
茂樹 藤田
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Tokyo NUC
Tokyo Aircraft Instrument Co Ltd
Original Assignee
University of Tokyo NUC
Tokyo Aircraft Instrument Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Tokyo NUC, Tokyo Aircraft Instrument Co Ltd filed Critical University of Tokyo NUC
Priority to JP2010099462A priority Critical patent/JP5569681B2/en
Publication of JP2011227017A publication Critical patent/JP2011227017A/en
Application granted granted Critical
Publication of JP5569681B2 publication Critical patent/JP5569681B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

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.

移動体、例えば航空機の姿勢を正確に推定し、推定した結果を計器表示したり、操縦に利用することにより安全な操縦の手助けとなる。
航空機などの航行時の位置,速度,姿勢および方位を計測するものとして慣性航行装置(IMU)があり、これは非常に高い精度で、基準となる絶対姿勢角からの相対的な姿勢角変化を計算することができる。
移動体の姿勢を推定する方法としてジャイロなどのセンサを用い、誤差修正を推定するカルマンフィルタを取り入れたものが種々提案されている。
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.

特許文献1の姿勢計測方法,姿勢制御装置,方位計及びコンピュータプログラムは、従来よりも短い時間で物体の姿勢または真北を含む方位を計測することができることを目的としたものである。具体的な構成におけるカルマンフィルタの時間更新は姿勢計測装置の姿勢をクオータニオンを用いて表示した状態変数の推定値ハットおよび誤差共分散は時間に対して不変であるとして前ステップの観測更新で得られた値と等しくする。また、観測更新ではジャイロおよび加速度センサを用いて互いに直交する各3軸回りの角速度及び各3軸方向の加速度を測定し、測定した角速度及び加速度の値を観測値として状態変数の推定値ハット及び誤差共分散を計算する。誤差共分散が十分小さくなるまでカルマンフィルタの処理を繰り返し、姿勢を示す状態変数を得るものである。   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.

特許文献2の慣性航法装置,慣性航法装置の初期化方法及び記録媒体は、ある程度の動作環境においてもイニシャルアライメントの高精度・高速化を図り、航空体の運用上の制約を小さくすることを目的とするものである。具体的な構成はイニシャルアライメント処理のファインアライメントにおいてカルマンフィルタの処理を行う処理手段と、この処理手段による処理の際、カルマンフィルタの収束状況を監視し、カルマンフィルタの収束速度が過度であるか否かを判断する監視手段と、この監視手段で収束速度が過度であると判断された場合、カルマンフィルタで推定された方位誤差を基にカルマンフィルタの更新処理をする更新手段とを具備し、処理手段は更新手段による更新後、再度カルマンフィルタの処理を行うものである。   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.

特許文献3の方位姿勢基準装置は、ストラップダウン型の方位姿勢基準装置においてアライメントを迅速化し、インフライトでも実施可能にすることを目的とするものである。座標変換マトリックス(CTM)の誤差修正において、姿勢角誤差修正と方位角誤差修正を分離し、姿勢角誤差修正を行ってから方位角誤差修正を行う。まず姿勢角誤差の推定値が所定の角度で得られるようになったとき、姿勢誤差の推定値によって座標変換マトリックスの誤差修正をし、それが終了した後に、方位角の設定値によって座標変換マトリックスを誤差修正するものである。方位姿勢誤差演算部としてカルマンフィルタが使用されている。   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.

特許文献4の移動体制御装置及び移動体制御方法は、軽量、小型でかつ安価なセンサ等を利用して複合航法システムを実現することを目的とするものである。具体的には慣性航法データと測位データとに基づいて状態推定フィルタ演算を行い、移動体の位置、姿勢、速度の情報を出力する。このとき慣性航法データについての状態推定フィルタ演算における、移動体の位置、姿勢、速度の各誤差を演算するにあたり、位置、姿勢、速度の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として表示された誤差情報を用いて、位置、姿勢、速度の各誤差を演算し、当該誤差情報を乗じることで位置、姿勢、速度の情報が補正され、移動体の制御に供される移動体制御装置である。   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.

特開2006−38650号公報JP 2006-38650 A 特開2001−264106号公報JP 2001-264106 A 特開平11−148827号公報Japanese Patent Laid-Open No. 11-148827 特開2007−276507号公報JP 2007-276507 A

特許文献1は姿勢計測においてカルマンフィルタを用い誤差を十分小さくなるように処理するものであるが、センサとして3軸それぞれの方向の加速度を測定する加速度センサと3軸回りの角速度を測定するジャイロのみを入力手段として用いているものであり、磁気センサ,速度計から得られるデータも考慮に入れたものではない。
また、特許文献2は慣性航法装置としてカルマンフィルタを用いているものの、方位や姿勢角の誤差を推定し、推定した誤差を除去する構成ではなく、またその処理をするためのセンサ部はジャイロセンサおよびアクセロメータのみである。
特許文献3はカルマンフィルタにより方位角誤差修正を分離し姿勢角誤差修正を行ってから方位角誤差修正を行うものであり、センサとしてXジャイロ,YジャイロおよびZジャイロならびにX加速度計,Y加速度計およびZ加速度計からの角速度および加速度のみのデータを得るものである。また特許文献4はセンサとして加速度センサ,ジャイロセンサおよびGPS信号受信部の各データを用い、微小変化単位四元数として表示される誤差情報の演算には磁気センサや速度計の観測値を用いるものではない。
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.

これに対処するために重力加速度以外の外力に由来する加速度成分を除去する方法が考えられる。
〔1〕ピトー管によるX,Y,Z方向外力の推定方法
ピトー管により得られた速度に角速度を乗じることによりY,Z軸について旋回加速度が得られ、さらにピトー管により得られた速度の時間変化からX軸に関する加速度が得られる。これらを加速度計により得られた値から引くことにより(1)式のように重力加速度を得ることができる。ただし、aは測定された加速度,gは重力加速度,ωは角速度である。vx はX軸に関する速度である。

Figure 2011227017
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.
Figure 2011227017

カルマンフィルタに入力される加速度は(1)式の修正を受けるが、観測方程式は従来のカルマンフィルタと変わることがない。
この方式は旋回加速度と前後方向の加速度以外の外力を除去できない欠点がある。また、風速が一定でない場合、本来存在しない加速度が入力されてしまうことになる。例えば、航空機が向かい風で飛行しているときに風速が強まりつつあるとすると抗力が増えるため機体は減速するが、ピトー管の値だけ見ると加速しているように見えるため計算された重力加速度が実際とは異なる向きに現れてしまう。
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〕カルマンフィルタを用いたピトー管によらない外力の推定方法
カルマンフィルタが推定する状態量と外力を追加する。加速度計により得られた値から推定された外力を引くと(2)式のように重力加速度が得られる。ピトー管によらないため風速の変化などの影響を受けることはない。

Figure 2011227017
[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.
Figure 2011227017

観測方程式はつぎによって求められる。
機体固定座標で測定された重力加速度ベクトルgb と現在の推定値から得られると予想される重力加速度ベクトルgb'との差と姿勢推定値のずれqe との関係を示す観測方程式は

Figure 2011227017
となる。これに外力の推定を加えるとa=g+fe から(4)式のようになる。
Figure 2011227017
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
Figure 2011227017
It becomes. When the estimation of external force is added to this, the equation (4) is obtained from a = g + fe .
Figure 2011227017

つぎに共分散の時間変化については以下の通りである。
e は航空機の運動に関する操作量であるから、時間変化を定式化するのは不可能であるためfe の値は観測による修正以外では変化させないことにする。

Figure 2011227017
共分散行列は、現在の推定値の不確かさを表す量であるから、Sのfe に関する対角成分に適当な数値を入力すれば、fe の不確かさを適宜上昇させることができ、加速度が観測されたときに修正されるようにすることができる。 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.
Figure 2011227017
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.

e の過去の推定値を推定に利用し続けていくため、急な旋回やスロットルの急激な操作などの外力が激しい変化に弱いと考えられる。また、共分散の時間変化を決めるパラメータであるSの大きさも、外力が激しく変化すると予想される場合には大きく、変化が少ないと予想される場合には小さく設定する必要があるが、これを定量的に決定する方法を考える必要が生ずる。 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〕ピトー管とカルマンフィルタを組み合わせた外力の推定方法
先に示したピトー管を利用しない外力の推定にピトー管を使った旋回加速度の推定を組み合わせることが考えられる。
操作によって大きく変化すると考えられる旋回加速度を推定値から除外するので急な外力の変化にも強く、推定される外力の大きさは小さくなり共分散の時間変化を決めるパラメータの決定はピトー管を用いない場合に比べて精度を必要としなくなる。

Figure 2011227017
[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.
Figure 2011227017

観測方程式と共分散の時間変化については先に示したピトー管を利用しない外力の推定方法と同じである。
旋回による加速度以外の外力が急激に変化したときに対応することができず、共分散の時間変化を決めるパラメータであるSの大きさを決める方法を定量的に決める方法も必要となる。
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.

前記目的を達成するために本発明の請求項1の姿勢推定装置は、ジャイロまたは加速度計を含む慣性センサ,速度計および磁気センサの観測値を利用する手段と、前記観測値に基づいて状態推定フィルタ演算を行い、移動体の姿勢の情報を出力する状態推定フィルタ演算手段と、前記観測値についての前記状態推定フィルタ演算における、移動体の姿勢の各誤差を演算する誤差演算手段とを備え、前記誤差演算手段が、前記状態推定フィルタ演算手段が出力する姿勢の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として計算された誤差情報を用いて、前記姿勢の各誤差を演算し、前記状態推定フィルタ演算手段が出力する、移動体の姿勢の各情報が、前記誤差演算手段によって演算された誤差情報を乗じることで補正されることを特徴とする。
本発明の請求項2の姿勢推定方法は、ジャイロまたは加速度計を含む慣性センサ,速度計および磁気センサの観測値に基づいて状態推定フィルタ演算を行い、移動体の姿勢の情報を出力する状態推定フィルタ演算工程と、前記観測値についての前記状態推定フィルタ演算における、移動体の姿勢の各誤差を演算する誤差演算工程とからなり、前記誤差演算工程において、状態推定フィルタ演算手段が出力する姿勢の各々を表す状態変数に係り、それよりも要素数の少ない状態変数で特定される微小変化単位四元数として計算された誤差情報を用いて、前記姿勢の各誤差を演算し、前記状態推定フィルタ演算工程が出力する、移動体の姿勢の各情報が、誤差演算手段によって演算された誤差情報を乗じることで補正されることを特徴とする。
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.

上記構成によれば、MEMSセンサなど、軽量,小型、かつ安価なセンサを用いて精度の高い移動体の姿勢を実現すべき誤差を推定する姿勢推定装置および姿勢推定方法を実現でき、該姿勢推定装置,方法から移動体の姿勢や位置などを精度良く表示する姿勢表示装置、例えば、航空機の計器表示器などを提供することができる。   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. ω,X,Y,Zの定義付けを説明するための図である。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.

以下、図面を参照して本発明の実施の形態を詳しく説明する。
図1は、本発明による慣性センサ,加速度計,磁気センサおよび速度計を用いた移動体の姿勢推定装置の概略図であり、航空機に適用した場合である。
機体にジャイロ装置11,加速度計12,速度計13および磁気センサ14が搭載される。ジャイロ装置はMEMS(Micro Electro Mechanical Systems) ジャイロであり、予め振動させている微小振動板が回転により受けるコリオリ力により回転角速度ωを求めるものである。
加速度計12もMEMSセンサであり、航空機の移動により加速度aを出力する。
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.

また、速度計13はMEMSセンサで構成されており、対気速度を出力する。また磁気センサ14は磁気ベクトルを与える。この磁気センサもMEMSセンサで構成することができる。演算器15では加速度a,速度V,ωz ωy の入力を受けて重力加速度gのみを演算して出力するが、実際は外力fe が完全に除かれていない。
カルマンフィルタ19はジャイロ装置11から入力する角速度ωを積分して回転角を算出する。算出された回転角に対し演算器17ではfe を含む重力加速度g,磁気ベクトルおよび積分器16からの回転角により姿勢18を表示するクオータニオンqの推定値を得ることができる。カルマンフィルタ19内では各センサからの観測値や推定した値を使用して所定の演算をし共分散行列の形で誤差の推定が行われる。
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.

図2は本発明による慣性センサ,磁気センサおよび速度計を用いた移動体の姿勢推定装置の構成をブロックで示した図で、図1と同様、航空機に適用したものである。
これはフィルタ演算部22で演算して得た外力fe 21が出力され、外力fe が演算器24に入力され、外力fe も取り除かれ重力加速度g23がフィルタ演算部22に入力される。他の部分の構成は図1と同じ構成である。
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.

つぎに本発明による姿勢推定装置の機能を実現するため、カルマンフィルタ,姿勢角,姿勢角誤差,観測するベクトル,観測方程式に用いる数式を説明する。
なお、数式説明にあたり、座標系,クオータニオン変数および微小な回転を表すクオータニオン変数について定義付けを行う。
〔1〕座標系X,Y,Z軸およびω
3軸は図3に示すように表されX軸は航空機の進行方向であり、ωはX,Y,Z軸それぞれの周りの角速度である。X軸に対しω0 (ωx ),Y軸に対しω1 (ωy ),Z軸に対しω2 (ωz )と表示される。
〔2〕クオータニオン変数
クオータニオン変数はqで表され、qは

Figure 2011227017
の4つの変数であり、q0 ,q1 ,q2 ,q3 の4つの変数で姿勢を表す。
〔3〕微小な回転を表すクオータニオン変数
微小な回転を表すクオータニオン変数はqe で表され、qe
Figure 2011227017
の4つの変数であり、1,qe1,qe2,qe3で表される。1つ目の成分は「1」という定数となる。すなわち、微妙な回転を表す姿勢誤差である。
(1)カルマンフィルタについて
本発明による姿勢推定装置に用いられるシステムはつぎのように表されるとする。
Figure 2011227017
ただし、xは推定した状態量,uは入力,wは外乱である。また、推定値がどの程度信頼できるものであるかを示す共分散行列Pも合わせて考える。
○Time Update について
本発明による姿勢推定装置の処理の流れとして時間経過に対し状態量,共分散行列が係わる式はつぎのようになる。
Figure 2011227017
○Measurement Updateについて
本発明による姿勢推定装置の各センサの観測値入力に対し状態量,カルマンゲイン,共分散行列,行列(変数H)などが係わる式はつぎのようになる。
Figure 2011227017
だだし、vを観測雑音として
Figure 2011227017
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. Ω 0x ) for the X axis, ω 1y ) for the Y axis, and ω 2z ) for the Z axis.
[2] Quartanion variable The quarteranion variable is represented by q,
Figure 2011227017
The posture is represented by the four variables q 0 , q 1 , q 2 , and q 3 .
[3] quaternion variables representing quaternion variable small rotation representing a minute rotation is represented by q e, q e is
Figure 2011227017
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.
Figure 2011227017
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.
Figure 2011227017
○ 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.
Figure 2011227017
However, v is the observation noise
Figure 2011227017

(2)システムの表現
本発明による姿勢推定装置を移動体に適用したときの姿勢角や姿勢角誤差の表現は、クオータニオンqを用いる。
○姿勢角の表現
座標系は北をX,東をY,下をZとして地面固定座標を、前方をX,右側をY,下をZとして機体固定座標をとる。このときの姿勢はクオータニオンqで表すこととする。
このクオータニオンは地面固定座標から機体固定座標への変数を与える。クオータニオンが座標変換を与えるとは、あるベクトルxに対して

Figure 2011227017
で与えられるベクトルX' が変換されたベクトルになっていることを意味する。(ただしq* は共役クオータニオン)この変換は行列で表すこともでき、例えば地面固定座標をn,機体固定座標をbとして変換行列を
Figure 2011227017
また、qの時間変化は
Figure 2011227017
で表される。ただし、ωは地面固定座標で計測された角速度である。 (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
Figure 2011227017
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.
Figure 2011227017
In addition, q changes over time
Figure 2011227017
It is represented by Here, ω is an angular velocity measured with fixed ground coordinates.

○姿勢角誤差の表現
姿勢を表すクオータニオンには絶対値が1でなければならないという制約があるが、カルマンフィルタにより得られた差分を加算して推定値を更新する方法ではこの制約を守ることができない。誤差として微小な回転を表すクオータニオン

Figure 2011227017
を考え(ただし、qe は3つの要素からなるベクトル)、qの推定値
Figure 2011227017
と誤差との和ではなく積の形で表すことにする。
また、この誤差の時間変化は機体固定座標で計測された角速度の誤差をΔωとすると
Figure 2011227017
誤差同士の積は微小であるため無視すると
Figure 2011227017
を得る。つまり、
Figure 2011227017
○観測するベクトルの表現
観測量として地磁気ベクトルと重力加速度ベクトルを姿勢角の修正に用いるが、加速度計により得られる加速度はそのままでは重力以外の外力も含んだものであるのでこれを除去する。
まず、移動体が横すべりなしで旋回を行っている場合、旋回により生じる加速度は、速度をVとして
Figure 2011227017
と表されるので、まずこれを加速度計により得られた加速度から減ずる。
これにより直進時・定常旋回時ともに重力加速度を正しく得ることができるようになったと考えられるが、それ以外の場合においても姿勢を正しく計算するため、旋回加速度以外の外力をカルマンフィルタにより推定する。
以上をまとめると重力加速度gは加速度計により得られる加速度aに以下の計算を施すことにより得られる。
Figure 2011227017
ただし、fe は推定された旋回加速度以外の外力を示す。 ○ 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
Figure 2011227017
(Where q e is a vector of three elements) and q is estimated
Figure 2011227017
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 Δω
Figure 2011227017
Since the product of errors is very small,
Figure 2011227017
Get. In other words,
Figure 2011227017
○ 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
Figure 2011227017
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.
Figure 2011227017
Here, fe represents an external force other than the estimated turning acceleration.

(3)観測方程式
使用する観測量として地磁気センサにより地磁気ベクトルが、加速度計により重力加速度がそれぞれ機体軸上で得られるとする。
このとき、推定値が機体固定座標から誤差の分だけずれた座標b' への変換を表しているとすると、qe に対する変換行列は、

Figure 2011227017
であるので、
Figure 2011227017
あるベクトル量について地面固定座標で測定した量をVn ,機体固定座標で測定した量Vb と書くことにすると、
Figure 2011227017
となり、機体固定座標で測定されたベクトルVb と、現在の推定値から得られると予想されるベクトルVb'の差を入力とした観測方程式を構成できる。 (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
Figure 2011227017
So
Figure 2011227017
If a certain vector quantity is measured with the ground fixed coordinates as V n , and the quantity measured with the airframe fixed coordinates as V b ,
Figure 2011227017
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.

図4は本発明を航空機に適用した場合のカルマンフィルタによる処理の流れを説明するための図である。以後使用する記号における表現形態を以下のように定義する。
「qハット」という表現はクオータニオンqに推定を示す意味のハットマークを乗せたものと同等とする。クオータニオンだけでなく,P,xなどの記号の後部に「ハット」と表現されている場合も、その記号の推定値とする。
「Pバー」という表現は共分散行列Pに現在の値を示すものと同等とする。共分散行列だけでなく、xなどの記号の後ろに「バー」と表現されている場合も、その記号の現在の値であるとする。
また、数式特定のため(1)式〜(29)式と記載するが、これら数式は〔発明が解決しようとする課題〕の項目に記載した数式ではなく、〔発明を実施するための形態〕の項目に記載した数式である。さらに図4のカルマンフィルタのアルゴリズムの流れの中で各数式を用いるステップはその数式を計算するための演算部という表現にした。
本発明による姿勢推定装置の処理はカルマンフィルタのアルゴリズムを実行するプログラム処理によって実現される。
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.

図4において観測値である加速度計31の加速度aと速度計32の速度V,外力fe およびジャイロ34からの角速度ωの入力が演算部40に送られる。演算部40では(22)式の計算が行われ、重力加速度が算出される。ここで(22)式においてω2 はZ軸回りの角速度であり、ω1 はY軸回りの角速度である。観測値から得る重力加速度は(22)式の左辺のgである。この重力加速度gが演算部41に入力される。また、磁気センサ33から観測した磁気ベクトルが出力される。
演算部41の(25)式におけるvb は機体固定座標で測定されたベクトルであり、vb は3軸の重力加速度と3軸の磁気ベクトルを含んでいる。分解するとG(b) とM(b) が出力される。
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), ω 2 is an angular velocity around the Z axis, and ω 1 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.

ここで、(25)式におけるvb'−vb は(8)式の右辺の括弧内の

Figure 2011227017
と同じになる。
よって
Figure 2011227017
から(8)式は(28)式に書き換えられる。
Figure 2011227017
(28)式において右辺の行列は変数Hである。 Here, v b ′ −v b in the expression (25) is in the parenthesis on the right side of the expression (8).
Figure 2011227017
Will be the same.
Therefore
Figure 2011227017
To (8) can be rewritten as (28).
Figure 2011227017
In the equation (28), the matrix on the right side is the variable H.

(8)式に注目すると、xにバーが付いているものは当初推定値であるが、角速度を入力することにより、バーがハットになり、xハットとなる。角速度が入らなければ、ハットをそのままバーに変えて計算する。
よって推定する状態量は(8)式より

Figure 2011227017
となる。よって、演算部43において
Figure 2011227017
となり、(29)式より推定したクオータニオンの姿勢誤差qe と推定した外力の誤差Δfe が得られる。
なお、演算部42では計算した(25)式におけるvb'において、推定されたクオータニオンqから推定された重力加速度G(b')と推定された磁気ベクトルM(b')とが出力されている。演算部44には観測値から得た重力加速度から分解して得たG(b) と上記推定された重力加速度G(b')が入力され、G(b) −G(b')の演算が行われ、演算部43と演算部51にそれぞれ入力される。
また演算部45では観測した磁気ベクトルから分解して得たM(b) と上記推定された磁気ベクトルM(b')が入力され、M(b) −M(b')の演算が行われ、演算部43と演算部51にそれぞれ入力される。
演算部43で出力された外力の誤差量Δfe は演算部55に加えられ、演算部55でfe が算出され、算出されたfe が演算部40に入力される。演算部40では先に述べたように(22)式の演算が行われ、外力の誤差fe を除去した重力加速度gが得られる。 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).
Figure 2011227017
It becomes. Therefore, in the calculation unit 43
Figure 2011227017
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.

つぎに姿勢を表すクオータニオンqを求めるループ49の動作について説明する。
ジャイロ34から角速度ωが演算部46に入力されており、演算部46では(16)式が計算されqの偏微分値を得る。さらに演算部47ではルンゲ・クッタ法等による微分方程式を解くことによりクオータニオンqを算出する。演算部48にはこのクオータニオンqとクオータニオン誤差qe が入力され、

Figure 2011227017
の演算が行われる。得られる推定したクオータニオンqハットは演算部46の(16)式にフィードバック入力される構成となっている。qハットを演算部16にフィードバックする経路の中間に切替部57(ソフト処理)の機能を有している。切替部57は、磁気センサ,加速度計,速度計で観測した場合は端子a側に接続されるため、上述のようにqハットは演算部46にフィードバック入力される。しかしながら、例えば、センサや加速度計,速度計が故障したり、また、ある地域で観測値を得ることを望まない,観測できない状況などの場合には観測しないので、かかるセンサなどから入力がないため、切替部57は端子bに接続されるようになっている。
したがって、磁気センサ,加速度計,速度計で観測しない場合には、観測値の入力は切替部57の端子b側に切り替えられているためフィードバックは行われない。
すなわち、観測した場合は、クオータニオンqを求めるループは演算部46,47,48により形成され、観測しない場合は演算部46,47のループが形成され、クオータニオンqが繰り返し算出され出力される。
このようにして演算されたクオータニオンqは姿勢58の情報として出力される。また、上述したようにクオータニオンqの推定値は(25)式におけるVb'の演算部42に供給される。演算部42では(25)式におけるVb'の演算が行われ、クオータニオンqの推定された姿勢情報からはこの推定値から得た重力加速度と磁気ベクトルが出力され、演算部44と45に入力される。 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 ,
Figure 2011227017
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.

つぎに共分散行列Pを求めるループを含む演算部54の動作について説明する。
上述したように演算部44でG(b) −G(b')の、演算部45でM(b) −M(b')の演算がそれぞれ行われ演算部51に入力される。演算部51は(25)式と等価の式から導き出されたものであり、(28)式に示す変数Hの行列が算出される。変数Hは演算部52の(9)式と、演算部53の(10)式に入力される。(9)式はカルマンゲインKK+1 を、(10)式は共分散行列PK+1 ハットを求める式である。
(9)式で示すカルマンゲインKK+1 は右辺に共分散行列PK+1 バー,HK+1 およびRK+1 を含む項で形成された式であり、変数Hは演算部51から入力される。また、(10)式で示す共分散行列PK+1 ハットは単位行列I,カルマンゲインKK+1 ,共分散行列Pバー,HK+1 を含む項で形成された式であり、変数Hは演算部51から入力される。
演算部52の(9)式には変数H,RK+1 (最初に設定される定数)およびPK+1 バーが入力され、(9)式で演算されてカルマンゲインKK+1 を得る。
また、演算部53の(10)式には演算部51から変数Hおよび演算部52のカルマンゲインKK+1 が入力され、(10)式で演算されてPK+1 ハットを得る。なお、PK+1 バーは(3)式から得ることができる。(3)式のPK+1 バーはΓ,ΨK ,SK を含む項で形成される式であり、ここではΓは単位行列,ΨK およびSK を固定変数として扱う。
演算部53で演算して出力されるPK+1 ハットは演算部50の(3)式にフィードバックされる。この経路に切替部56(ソフト処理)の機能を有している。切替部56は、上述したように磁気センサ,加速度計,速度計で観測した場合、端子a側に接続されるため、PK+1 ハットは演算部50にフィードバック入力される。また、観測しない場合は、センサなどから入力がないため、切替部56は端子bに接続されるようになっている。
磁気センサなどの出力を観測した場合、演算部50の(3)式にはPK+1 ハットが入力され、さらに(7)式で表されるSK と(5)式で表されるQK が入力され、演算されてPK+1 バーを得る。すなわち、観測した場合は、共分散行列Pを求めるループは演算部50,52,53により形成され、観測しない場合は演算部50のみのループが形成され、共分散行列Pが繰り返し算出され出力される。
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

図5は水平旋回しているデータについてのロール角,ピッチの推定結果を示すグラフである。
以上の姿勢推定がどの程度有効なものであるかを検証するため、飛行データをもとに水平旋回の姿勢推定についてシミュレーションし、図5に示すような結果が得られた。
本発明により求めた姿勢qを所定の変換式によってロール角およびピッチ角を導いたもので、ロール角およびピッチ角の誤差の範囲は小さく収まっている。
図5において、実線が推定されたピッチ角,点線が実際のピッチ角,一点鎖線が推定されたロール角,二点鎖線が実際のロール角である。ロール角が大きくなると、誤差が大きくなる傾向にあるが、姿勢誤差と外力が有効に推定されているため、精度良く姿勢を指示していることが理解できる。
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 ジャイロ装置(ジャイロ)
12,31 加速度計
13,32 速度計
14,33 磁気センサ
15,17 演算器
16 積分器
18,58 姿勢
19 カルマンフィルタ
40,41,42,43,44,45,46,47,48,50,51,52,53,55 演算部
49 クオータニオンqを求めるループ
54 共分散行列Pを求めるループを含む演算部
56,57 切替部
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.
JP2010099462A 2010-04-23 2010-04-23 POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER Active JP5569681B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2010099462A JP5569681B2 (en) 2010-04-23 2010-04-23 POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2010099462A JP5569681B2 (en) 2010-04-23 2010-04-23 POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER

Publications (2)

Publication Number Publication Date
JP2011227017A true JP2011227017A (en) 2011-11-10
JP5569681B2 JP5569681B2 (en) 2014-08-13

Family

ID=45042509

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2010099462A Active JP5569681B2 (en) 2010-04-23 2010-04-23 POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER

Country Status (1)

Country Link
JP (1) JP5569681B2 (en)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5061264B1 (en) * 2012-03-23 2012-10-31 国立大学法人 千葉大学 Small attitude sensor
CN103954282A (en) * 2014-03-04 2014-07-30 哈尔滨工程大学 Strapdown inertial navigation method based on accelerometer output increment
WO2014133235A1 (en) * 2013-02-26 2014-09-04 (주)아티스 Method for optimizing sensitivity of gyroscope for magnet-gyro guidance device
CN104121907A (en) * 2014-07-30 2014-10-29 杭州电子科技大学 Square root cubature Kalman filter-based aircraft attitude estimation method
JP2015208790A (en) * 2014-04-24 2015-11-24 トヨタ自動車株式会社 Device and method of estimating center of gravity
CN105339256A (en) * 2013-05-13 2016-02-17 稳定解决方案股份有限公司 System and method for monitoring stability of a vessel
US20170059318A1 (en) * 2015-08-26 2017-03-02 Magnachip Semiconductor, Ltd. Method and apparatus of correcting output value of geomagnetic sensor
CN108089434A (en) * 2017-12-11 2018-05-29 北京控制工程研究所 A kind of skin Nano satellite attitude acquisition method based on magnetometer
WO2018135332A1 (en) * 2017-01-18 2018-07-26 ソニー株式会社 Orientation control device, flying object, orientation control method, and program
CN109459005A (en) * 2018-12-20 2019-03-12 合肥优控科技有限公司 A kind of Attitude estimation method
JP2019120587A (en) * 2018-01-05 2019-07-22 ローム株式会社 Positioning system and positioning method
CN110954103A (en) * 2019-12-18 2020-04-03 无锡北微传感科技有限公司 Method and system for estimating dynamic attitude of vehicle body based on MEMS sensor
CN111089606A (en) * 2019-12-20 2020-05-01 湖南航天机电设备与特种材料研究所 Rapid self-calibration method for key parameters of three-self laser inertial measurement unit
CN111220114A (en) * 2020-01-20 2020-06-02 山东大学 Rotating shaft rotating angle inertia measurement system and method for single-shaft rotating carrier
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN111551174A (en) * 2019-12-18 2020-08-18 无锡北微传感科技有限公司 High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN113218391A (en) * 2021-03-23 2021-08-06 合肥工业大学 Attitude calculation method based on EWT algorithm
CN113739794A (en) * 2020-05-28 2021-12-03 精工爱普生株式会社 Attitude estimation device and method, sensor module, measurement system, and moving object
CN113916222A (en) * 2021-09-15 2022-01-11 北京自动化控制设备研究所 Combined navigation method based on variance constraint of Kalman filtering estimation
CN114659488A (en) * 2022-02-08 2022-06-24 东风悦享科技有限公司 High-dynamic vehicle attitude estimation method based on error Kalman filtering

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107168350B (en) * 2017-05-24 2020-04-21 西北工业大学 Calculation method for optimal rotation angular velocity of service spacecraft during fixed-axis rotation
CN111684386A (en) * 2019-05-28 2020-09-18 深圳市大疆创新科技有限公司 Cradle head zero calibration method and cradle head
US11790793B2 (en) 2021-01-08 2023-10-17 Honeywell International Inc. Systems and methods for model based vehicle navigation
US11781836B2 (en) 2021-03-04 2023-10-10 Honeywell International Inc. Systems and methods for model based inertial navigation for a spinning projectile

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0587585A (en) * 1991-09-26 1993-04-06 Tokimec Inc Atitude detector by strapdown system
JP2007276507A (en) * 2006-04-03 2007-10-25 Univ Of Tokyo Mobile element controller and mobile element control method

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0587585A (en) * 1991-09-26 1993-04-06 Tokimec Inc Atitude detector by strapdown system
JP2007276507A (en) * 2006-04-03 2007-10-25 Univ Of Tokyo Mobile element controller and mobile element control method

Cited By (31)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5061264B1 (en) * 2012-03-23 2012-10-31 国立大学法人 千葉大学 Small attitude sensor
WO2014133235A1 (en) * 2013-02-26 2014-09-04 (주)아티스 Method for optimizing sensitivity of gyroscope for magnet-gyro guidance device
CN105339256A (en) * 2013-05-13 2016-02-17 稳定解决方案股份有限公司 System and method for monitoring stability of a vessel
CN103954282A (en) * 2014-03-04 2014-07-30 哈尔滨工程大学 Strapdown inertial navigation method based on accelerometer output increment
JP2015208790A (en) * 2014-04-24 2015-11-24 トヨタ自動車株式会社 Device and method of estimating center of gravity
CN104121907A (en) * 2014-07-30 2014-10-29 杭州电子科技大学 Square root cubature Kalman filter-based aircraft attitude estimation method
CN104121907B (en) * 2014-07-30 2017-02-08 杭州电子科技大学 Square root cubature Kalman filter-based aircraft attitude estimation method
US20170059318A1 (en) * 2015-08-26 2017-03-02 Magnachip Semiconductor, Ltd. Method and apparatus of correcting output value of geomagnetic sensor
US10060743B2 (en) * 2015-08-26 2018-08-28 Haechitech Corporation Method and apparatus of correcting output value of geomagnetic sensor
US10962989B2 (en) 2017-01-18 2021-03-30 Sony Corporation Attitude control device, flying object, attitude control method, and program
WO2018135332A1 (en) * 2017-01-18 2018-07-26 ソニー株式会社 Orientation control device, flying object, orientation control method, and program
CN108089434A (en) * 2017-12-11 2018-05-29 北京控制工程研究所 A kind of skin Nano satellite attitude acquisition method based on magnetometer
JP2019120587A (en) * 2018-01-05 2019-07-22 ローム株式会社 Positioning system and positioning method
JP7025215B2 (en) 2018-01-05 2022-02-24 ローム株式会社 Positioning system and positioning method
CN109459005A (en) * 2018-12-20 2019-03-12 合肥优控科技有限公司 A kind of Attitude estimation method
CN109459005B (en) * 2018-12-20 2020-07-10 安徽果力智能科技有限公司 Attitude estimation method
CN110954103A (en) * 2019-12-18 2020-04-03 无锡北微传感科技有限公司 Method and system for estimating dynamic attitude of vehicle body based on MEMS sensor
CN110954103B (en) * 2019-12-18 2022-02-08 无锡北微传感科技有限公司 Method and system for estimating dynamic attitude of vehicle body based on MEMS sensor
CN111551174A (en) * 2019-12-18 2020-08-18 无锡北微传感科技有限公司 High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN111089606A (en) * 2019-12-20 2020-05-01 湖南航天机电设备与特种材料研究所 Rapid self-calibration method for key parameters of three-self laser inertial measurement unit
CN111089606B (en) * 2019-12-20 2023-11-14 湖南航天机电设备与特种材料研究所 Rapid self-calibration method for key parameters of three-self laser inertial measurement unit
CN111220114A (en) * 2020-01-20 2020-06-02 山东大学 Rotating shaft rotating angle inertia measurement system and method for single-shaft rotating carrier
CN111426318A (en) * 2020-04-22 2020-07-17 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN111426318B (en) * 2020-04-22 2024-01-26 中北大学 Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering
CN113739794A (en) * 2020-05-28 2021-12-03 精工爱普生株式会社 Attitude estimation device and method, sensor module, measurement system, and moving object
CN113739794B (en) * 2020-05-28 2024-01-12 精工爱普生株式会社 Posture estimating device and method, sensor module, measuring system, and moving object
CN113218391A (en) * 2021-03-23 2021-08-06 合肥工业大学 Attitude calculation method based on EWT algorithm
CN113916222A (en) * 2021-09-15 2022-01-11 北京自动化控制设备研究所 Combined navigation method based on variance constraint of Kalman filtering estimation
CN113916222B (en) * 2021-09-15 2023-10-13 北京自动化控制设备研究所 Combined navigation method based on Kalman filtering estimation variance constraint
CN114659488A (en) * 2022-02-08 2022-06-24 东风悦享科技有限公司 High-dynamic vehicle attitude estimation method based on error Kalman filtering
CN114659488B (en) * 2022-02-08 2023-06-23 东风悦享科技有限公司 High-dynamic vehicle attitude estimation method based on error Kalman filtering

Also Published As

Publication number Publication date
JP5569681B2 (en) 2014-08-13

Similar Documents

Publication Publication Date Title
JP5569681B2 (en) POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER
JP4782111B2 (en) System and method for estimating position, attitude and / or direction of flight of a vehicle
EP2472225B1 (en) Method and system for initial quaternion and attitude estimation
JP4615287B2 (en) Azimuth and orientation detection device
JP6094026B2 (en) Posture determination method, position calculation method, and posture determination apparatus
US8442703B2 (en) Turning-stabilized estimation of the attitude angles of an aircraft
JP2010032398A (en) Location detecting apparatus and method of navigation system
CN111207745B (en) Inertial measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle
Oh Multisensor fusion for autonomous UAV navigation based on the Unscented Kalman Filter with Sequential Measurement Updates
JP2007232443A (en) Inertia navigation system and its error correction method
JP2012173190A (en) Positioning system and positioning method
JP4447791B2 (en) Aircraft attitude determination apparatus having a gyrometer and an accelerometer
Sokolovic et al. Integration of INS, GPS, magnetometer and barometer for improving accuracy navigation of the vehicle
de Celis et al. Hybridized attitude determination techniques to improve ballistic projectile navigation, guidance and control
CN108627152A (en) A kind of air navigation aid of the miniature drone based on Fusion
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
EP4053504B1 (en) Systems and methods for model based inertial navigation for a spinning projectile
RU2564379C1 (en) Platformless inertial attitude-and-heading reference
JP2007232444A (en) Inertia navigation system and its error correction method
RU2373562C2 (en) Method and device for controlling horizontal orientation of aircraft
RU2487318C1 (en) Platform-free inertial attitude and heading reference system based on sensitive elements of medium accuracy
Lima et al. Performance evaluation of attitude estimation algorithms in the design of an ahrs for fixed wing uavs
Kim et al. Attitude Estimation for Unmanned Aircraft Using a Multiplicative Extended Kalman Filter
Kamali et al. Multiplicative error state Kalman filter vs nonlinear complimentary filter for a high performance aircraft attitude estimation
de Celis et al. Aircraft Attitude Determination Algorithms Employing Gravity Vector Estimations and Velocity Measurements.

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20130314

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A821

Effective date: 20130314

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20140131

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20140204

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20140314

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20140603

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20140610

R150 Certificate of patent or registration of utility model

Ref document number: 5569681

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250