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 PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 238000004364 calculation method Methods 0.000 claims abstract description 74
- 230000036544 posture Effects 0.000 claims 12
- 238000012937 correction Methods 0.000 abstract description 11
- 230000001133 acceleration Effects 0.000 description 53
- 239000011159 matrix material Substances 0.000 description 25
- 239000013598 vector Substances 0.000 description 24
- 230000014509 gene expression Effects 0.000 description 17
- 238000012545 processing Methods 0.000 description 9
- 238000005259 measurement Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 150000001450 anions Chemical class 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 239000002131 composite material Substances 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011423 initialization method Methods 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Images
Landscapes
- Navigation (AREA)
Abstract
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
特許文献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
特許文献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.
特許文献1は姿勢計測においてカルマンフィルタを用い誤差を十分小さくなるように処理するものであるが、センサとして3軸それぞれの方向の加速度を測定する加速度センサと3軸回りの角速度を測定するジャイロのみを入力手段として用いているものであり、磁気センサ,速度計から得られるデータも考慮に入れたものではない。
また、特許文献2は慣性航法装置としてカルマンフィルタを用いているものの、方位や姿勢角の誤差を推定し、推定した誤差を除去する構成ではなく、またその処理をするためのセンサ部はジャイロセンサおよびアクセロメータのみである。
特許文献3はカルマンフィルタにより方位角誤差修正を分離し姿勢角誤差修正を行ってから方位角誤差修正を行うものであり、センサとしてXジャイロ,YジャイロおよびZジャイロならびにX加速度計,Y加速度計およびZ加速度計からの角速度および加速度のみのデータを得るものである。また特許文献4はセンサとして加速度センサ,ジャイロセンサおよびGPS信号受信部の各データを用い、微小変化単位四元数として表示される誤差情報の演算には磁気センサや速度計の観測値を用いるものではない。
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.
ところで、本件発明者は移動体の姿勢推定を行うにあたり、ジャイロおよび加速度計からの角速度や加速度の測定値だけでなく、磁気センサおよび速度計の方位角,地磁気伏角および速度の値も利用し、これらセンサの出力をカルマンフィルタで所定の処理をすることにより、姿勢誤差を推定し精度の良好な移動体の姿勢を推定できると考察した。
そこで実際の姿勢に対し精度良く姿勢推定するためには磁気センサにより得られる地磁気と加速度計により得られる重力加速度を姿勢推定するために入力するベクトルとして用いることを考えた場合、加速度について重力以外にも空気などに由来する外力の成分が加算されてしまい、特に旋回時や加速度時に姿勢推定に誤差が生ずる原因となる。
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軸に関する速度である。
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.
カルマンフィルタに入力される加速度は(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)式のように重力加速度が得られる。ピトー管によらないため風速の変化などの影響を受けることはない。
[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.
観測方程式はつぎによって求められる。
機体固定座標で測定された重力加速度ベクトルgb と現在の推定値から得られると予想される重力加速度ベクトルgb'との差と姿勢推定値のずれqe との関係を示す観測方程式は
となる。これに外力の推定を加えるとa=g+fe から(4)式のようになる。
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 .
つぎに共分散の時間変化については以下の通りである。
fe は航空機の運動に関する操作量であるから、時間変化を定式化するのは不可能であるためfe の値は観測による修正以外では変化させないことにする。
共分散行列は、現在の推定値の不確かさを表す量であるから、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.
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.
fe の過去の推定値を推定に利用し続けていくため、急な旋回やスロットルの急激な操作などの外力が激しい変化に弱いと考えられる。また、共分散の時間変化を決めるパラメータである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〕ピトー管とカルマンフィルタを組み合わせた外力の推定方法
先に示したピトー管を利用しない外力の推定にピトー管を使った旋回加速度の推定を組み合わせることが考えられる。
操作によって大きく変化すると考えられる旋回加速度を推定値から除外するので急な外力の変化にも強く、推定される外力の大きさは小さくなり共分散の時間変化を決めるパラメータの決定はピトー管を用いない場合に比べて精度を必要としなくなる。
[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.
観測方程式と共分散の時間変化については先に示したピトー管を利用しない外力の推定方法と同じである。
旋回による加速度以外の外力が急激に変化したときに対応することができず、共分散の時間変化を決めるパラメータである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
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.
以下、図面を参照して本発明の実施の形態を詳しく説明する。
図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
The
また、速度計13はMEMSセンサで構成されており、対気速度を出力する。また磁気センサ14は磁気ベクトルを与える。この磁気センサもMEMSセンサで構成することができる。演算器15では加速度a,速度V,ωz ωy の入力を受けて重力加速度gのみを演算して出力するが、実際は外力fe が完全に除かれていない。
カルマンフィルタ19はジャイロ装置11から入力する角速度ωを積分して回転角を算出する。算出された回転角に対し演算器17ではfe を含む重力加速度g,磁気ベクトルおよび積分器16からの回転角により姿勢18を表示するクオータニオンqの推定値を得ることができる。カルマンフィルタ19内では各センサからの観測値や推定した値を使用して所定の演算をし共分散行列の形で誤差の推定が行われる。
Moreover, the
The
図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
つぎに本発明による姿勢推定装置の機能を実現するため、カルマンフィルタ,姿勢角,姿勢角誤差,観測するベクトル,観測方程式に用いる数式を説明する。
なお、数式説明にあたり、座標系,クオータニオン変数および微小な回転を表すクオータニオン変数について定義付けを行う。
〔1〕座標系X,Y,Z軸およびω
3軸は図3に示すように表されX軸は航空機の進行方向であり、ωはX,Y,Z軸それぞれの周りの角速度である。X軸に対しω0 (ωx ),Y軸に対しω1 (ωy ),Z軸に対しω2 (ωz )と表示される。
〔2〕クオータニオン変数
クオータニオン変数はqで表され、qは
の4つの変数であり、q0 ,q1 ,q2 ,q3 の4つの変数で姿勢を表す。
〔3〕微小な回転を表すクオータニオン変数
微小な回転を表すクオータニオン変数はqe で表され、qe は
の4つの変数であり、1,qe1,qe2,qe3で表される。1つ目の成分は「1」という定数となる。すなわち、微妙な回転を表す姿勢誤差である。
(1)カルマンフィルタについて
本発明による姿勢推定装置に用いられるシステムはつぎのように表されるとする。
ただし、xは推定した状態量,uは入力,wは外乱である。また、推定値がどの程度信頼できるものであるかを示す共分散行列Pも合わせて考える。
○Time Update について
本発明による姿勢推定装置の処理の流れとして時間経過に対し状態量,共分散行列が係わる式はつぎのようになる。
○Measurement Updateについて
本発明による姿勢推定装置の各センサの観測値入力に対し状態量,カルマンゲイン,共分散行列,行列(変数H)などが係わる式はつぎのようになる。
だだし、vを観測雑音として
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. Ω 0 (ω x ) for the X axis, ω 1 (ω y ) for the Y axis, and ω 2 (ω z ) for the Z axis.
[2] Quartanion variable The quarteranion variable is represented by q,
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
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)システムの表現
本発明による姿勢推定装置を移動体に適用したときの姿勢角や姿勢角誤差の表現は、クオータニオンqを用いる。
○姿勢角の表現
座標系は北をX,東をY,下をZとして地面固定座標を、前方をX,右側をY,下をZとして機体固定座標をとる。このときの姿勢はクオータニオンqで表すこととする。
このクオータニオンは地面固定座標から機体固定座標への変数を与える。クオータニオンが座標変換を与えるとは、あるベクトルxに対して
で与えられるベクトルX' が変換されたベクトルになっていることを意味する。(ただしq* は共役クオータニオン)この変換は行列で表すこともでき、例えば地面固定座標をn,機体固定座標をbとして変換行列を
また、qの時間変化は
で表される。ただし、ωは地面固定座標で計測された角速度である。
(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.
○姿勢角誤差の表現
姿勢を表すクオータニオンには絶対値が1でなければならないという制約があるが、カルマンフィルタにより得られた差分を加算して推定値を更新する方法ではこの制約を守ることができない。誤差として微小な回転を表すクオータニオン
を考え(ただし、qe は3つの要素からなるベクトル)、qの推定値
と誤差との和ではなく積の形で表すことにする。
また、この誤差の時間変化は機体固定座標で計測された角速度の誤差をΔωとすると
誤差同士の積は微小であるため無視すると
を得る。つまり、
○観測するベクトルの表現
観測量として地磁気ベクトルと重力加速度ベクトルを姿勢角の修正に用いるが、加速度計により得られる加速度はそのままでは重力以外の外力も含んだものであるのでこれを除去する。
まず、移動体が横すべりなしで旋回を行っている場合、旋回により生じる加速度は、速度をVとして
と表されるので、まずこれを加速度計により得られた加速度から減ずる。
これにより直進時・定常旋回時ともに重力加速度を正しく得ることができるようになったと考えられるが、それ以外の場合においても姿勢を正しく計算するため、旋回加速度以外の外力をカルマンフィルタにより推定する。
以上をまとめると重力加速度gは加速度計により得られる加速度aに以下の計算を施すことにより得られる。
ただし、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
(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.
(3)観測方程式
使用する観測量として地磁気センサにより地磁気ベクトルが、加速度計により重力加速度がそれぞれ機体軸上で得られるとする。
このとき、推定値が機体固定座標から誤差の分だけずれた座標b' への変換を表しているとすると、qe に対する変換行列は、
であるので、
あるベクトル量について地面固定座標で測定した量をVn ,機体固定座標で測定した量Vb と書くことにすると、
となり、機体固定座標で測定されたベクトル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
So
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 ,
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
In equation (25) of the
ここで、(25)式におけるvb'−vb は(8)式の右辺の括弧内の
と同じになる。
よって
から(8)式は(28)式に書き換えられる。
(28)式において右辺の行列は変数Hである。
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.
(8)式に注目すると、xにバーが付いているものは当初推定値であるが、角速度を入力することにより、バーがハットになり、xハットとなる。角速度が入らなければ、ハットをそのままバーに変えて計算する。
よって推定する状態量は(8)式より
となる。よって、演算部43において
となり、(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).
It becomes. Therefore, in the
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
The M obtained by decomposing the magnetic vector of observing the
The error amount Δf e of the external force output by the
つぎに姿勢を表すクオータニオンqを求めるループ49の動作について説明する。
ジャイロ34から角速度ωが演算部46に入力されており、演算部46では(16)式が計算されqの偏微分値を得る。さらに演算部47ではルンゲ・クッタ法等による微分方程式を解くことによりクオータニオンqを算出する。演算部48にはこのクオータニオンqとクオータニオン誤差qe が入力され、
の演算が行われる。得られる推定したクオータニオン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
The angular velocity ω is input from the
Is calculated. The estimated quarteranion q hat obtained is fed back to the equation (16) of the
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
That is, when observed, a loop for obtaining the quaternion q is formed by the
The quarteranion q calculated in this manner is output as
つぎに共分散行列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
As described above, G (b) −G (b ′) is calculated by the
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
The variable H, R K + 1 (constant initially set) and P K + 1 bar are input to the equation (9) of the
Further, the variable H and the Kalman gain K K + 1 of the calculation unit 52 are input from the
The P K + 1 hat calculated and output by the
If observing the output of a magnetic sensor, Q is the (3) of 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
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.
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)
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 | 合肥优控科技有限公司 | 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)
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)
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 |
-
2010
- 2010-04-23 JP JP2010099462A patent/JP5569681B2/en active Active
Patent Citations (2)
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)
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 | 合肥优控科技有限公司 | 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 | |
Oh | Multisensor fusion for autonomous UAV navigation based on the Unscented Kalman Filter with Sequential Measurement Updates | |
CN111207745B (en) | Inertial measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle | |
JP2007232443A (en) | Inertia navigation system and its error correction method | |
JP2012173190A (en) | Positioning system and positioning method | |
EP4053504B1 (en) | Systems and methods for model based inertial navigation for a spinning projectile | |
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 | |
JP2007232444A (en) | Inertia navigation system and its error correction method | |
RU2564379C1 (en) | Platformless inertial attitude-and-heading reference | |
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 |