JP2012198057A6 - Posture estimation device - Google Patents
Posture estimation device Download PDFInfo
- Publication number
- JP2012198057A6 JP2012198057A6 JP2011061146A JP2011061146A JP2012198057A6 JP 2012198057 A6 JP2012198057 A6 JP 2012198057A6 JP 2011061146 A JP2011061146 A JP 2011061146A JP 2011061146 A JP2011061146 A JP 2011061146A JP 2012198057 A6 JP2012198057 A6 JP 2012198057A6
- Authority
- JP
- Japan
- Prior art keywords
- transfer function
- filter
- output
- posture
- inclinometer
- 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.)
- Withdrawn
Links
- 238000012546 transfer Methods 0.000 claims abstract description 127
- 230000000295 complement effect Effects 0.000 claims abstract description 53
- 238000006243 chemical reaction Methods 0.000 claims abstract description 34
- 238000005259 measurement Methods 0.000 claims description 9
- 238000000034 method Methods 0.000 abstract description 12
- 238000012545 processing Methods 0.000 abstract description 5
- 230000000052 comparative effect Effects 0.000 description 62
- 230000010354 integration Effects 0.000 description 15
- 230000014509 gene expression Effects 0.000 description 9
- 239000011159 matrix material Substances 0.000 description 6
- 238000013461 design Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 4
- 244000089409 Erythrina poeppigiana Species 0.000 description 2
- 235000009776 Rathbunia alamosensis Nutrition 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000011144 upstream manufacturing Methods 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 239000013598 vector Substances 0.000 description 1
Abstract
【課題】 異なる複数のセンサの各出力に対し、各センサの線形動特性の逆伝達関数を用いて補償すると共に、この逆伝達関数に対応したフィルタを施し、最終的に姿勢変換を行うようにして、姿勢推定を適切且つ高精度に行える姿勢推定装置を提供する。
【解決手段】 角速度センサ１０と傾斜計２０の各逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似線形特性として伝達関数を同定し、その逆伝達関数を用いて特性を補償すると共に、逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実な演算処理で姿勢推定が実行でき、複数のセンサを用いる相補フィルタの手法で精度のよい姿勢推定を行える。
【選択図】 図１PROBLEM TO BE SOLVED: To compensate each output of a plurality of different sensors using an inverse transfer function of the linear dynamic characteristic of each sensor, and to apply a filter corresponding to the inverse transfer function to finally perform posture conversion. Thus, a posture estimation device that can perform posture estimation appropriately and with high accuracy is provided.
Each inverse model of an angular velocity sensor 10 and an inclinometer 20 is divided into a dynamic characteristic compensation part and an attitude conversion part, and a transfer function is identified as an approximate linear characteristic for the sensor dynamic characteristic, and the inverse transfer function is used. In addition to compensating the characteristics, the filter is designed in accordance with the inverse transfer function, and the filter is applied before the attitude conversion calculation to the output signal, so the effective frequency is compensated by the characteristic compensation using the inverse transfer function. While expanding the area, the product of the inverse transfer function and the transfer function of the filter can be a proper transfer function, posture estimation can be performed with reliable calculation processing, and a complementary filter method using multiple sensors. Accurate posture estimation can be performed.
[Selection] Figure 1
Description
本発明は、複数の慣性センサ出力から相補フィルタを用いて移動体の姿勢を推定する姿勢推定装置に関する。 The present invention relates to a posture estimation device that estimates the posture of a moving body using complementary filters from a plurality of inertial sensor outputs.
歩行ロボットなど、運動の空間領域も周波数領域も広範囲な移動体の制御においては、信頼性の高い移動体の姿勢推定、すなわち、移動体の三次元における傾斜状態の推定が必要である。
姿勢推定には、通常、角速度センサや傾斜計、加速度計などのいわゆる慣性センサが用いられる。こうした慣性センサは、その種類ごとに信頼できる周波数領域がそれぞれ異なっている。
In the control of a moving body having a wide spatial range and frequency domain of motion, such as a walking robot, it is necessary to estimate the posture of the moving body with high reliability, that is, to estimate the three-dimensional inclination state of the moving body.
For posture estimation, so-called inertial sensors such as angular velocity sensors, inclinometers, and accelerometers are usually used. Such inertial sensors have different reliable frequency regions for each type.
例えば、角速度センサは、動的な角度変化を角速度として検出でき、この角速度の出力を積分することで、姿勢を推定できるが、積分による定常誤差の拡大が問題となるなど、低い周波数領域での信頼性が下がっている。一方、傾斜計は、準静的な状態で傾斜角を検出できるが、動的な状態では、動きの影響による検出までの時間遅れが無視できなくなるなど、高い周波数領域での信頼性が下がるという問題を有している。 For example, the angular velocity sensor can detect a dynamic angular change as an angular velocity, and can estimate the posture by integrating the output of this angular velocity, but there is a problem of expansion of steady error due to integration. Reliability is down. On the other hand, the inclinometer can detect the tilt angle in a quasi-static state, but in the dynamic state, the time delay until detection due to the influence of motion cannot be ignored, and the reliability in the high frequency range will be reduced. Have a problem.
また、加速度計は、静的な状態では重力加速度から重力方向を検出可能であるものの、運動中は移動体自体の加速度の影響を受け、正確な検出が行えないという問題がある。
こうしたことから、一種類のセンサのみによる姿勢推定は難しく、異なる種類のセンサを複数組み合わせて用いることが必要になる。
In addition, although the accelerometer can detect the direction of gravity from the gravitational acceleration in a static state, it has a problem that it cannot be accurately detected during movement due to the influence of the acceleration of the moving body itself.
For this reason, posture estimation using only one type of sensor is difficult, and it is necessary to use a combination of a plurality of different types of sensors.
このような異種センサを複合的に利用する技術として、カルマンフィルタを用いた方法が多くとられている。ただし、カルマンフィルタでは、時間領域において、誤差の含まれるセンサ出力の時間的変動傾向からセンサ出力に対する補正量を取得して、移動体の姿勢を高い信頼性で推定可能とするフィルタ設計がなされるものの、このフィルタ設計に係る各パラメータを設定するのが困難である。 As a technique for using such heterogeneous sensors in combination, a method using a Kalman filter is often employed. However, the Kalman filter is designed to obtain a correction amount for the sensor output from the temporal fluctuation tendency of the sensor output including an error in the time domain so that the posture of the moving body can be estimated with high reliability. Therefore, it is difficult to set each parameter related to the filter design.
この他、周波数領域で複数のセンサ出力を重ね合せる相補フィルタも提案されている。これは、各々のセンサの周波数特性に基づいて、信頼性の高い信号を抽出し、重ね合せるものであり、各センサの信頼できる周波数領域がある程度分かっていれば周波数領域設計は比較的容易である。 In addition, a complementary filter that superimposes a plurality of sensor outputs in the frequency domain has also been proposed. This is to extract and superimpose highly reliable signals based on the frequency characteristics of each sensor, and the frequency domain design is relatively easy if the reliable frequency domain of each sensor is known to some extent. .
例えば、傾斜計のような低い周波数領域が信頼できるセンサにはローパスフィルタを適用し、逆に角速度センサのような高い周波数領域が信頼できるセンサにハイパスフィルタを適用して、フィルタ出力を組み合わせることで推定精度を向上させることができる。 For example, a low-pass filter is applied to a sensor with a reliable low frequency range, such as an inclinometer, and a high-pass filter is applied to a sensor with a reliable high frequency range, such as an angular velocity sensor, to combine the filter outputs. The estimation accuracy can be improved.
こうした相補フィルタを、移動体の姿勢推定に利用する手法は、従来から種々提案されており、その一例として、特開平５−１９６４７５号公報や特開平５−２２３５８７号公報に開示されるものがある。 Various methods for using such a complementary filter for estimating the posture of a moving body have been proposed, and examples thereof include those disclosed in Japanese Patent Laid-Open Nos. 5-196475 and 5-223585. .
従来における移動体の姿勢推定に係る相補フィルタは、前記各特許文献に示されるような構成となっているが、姿勢推定に用いるそれぞれのセンサの有効周波数領域は相補的であるとは限らないため、センサ出力を相補フィルタにそのまま入力して用いる場合、いずれのセンサにおいても情報の信頼性が低い周波数領域が存在すると、その周波数領域において推定精度が低下してしまうという問題があった。
例えば、角速度センサと傾斜計を姿勢推定のためのセンサとして用いる場合、傾斜計の有効な周波数領域が低いため、角速度センサとの組み合わせでは推定精度の点で問題になる。
Conventional complementary filters related to posture estimation of a moving object are configured as shown in the above-mentioned patent documents, but the effective frequency ranges of the sensors used for posture estimation are not necessarily complementary. When the sensor output is directly input to the complementary filter and used, if there is a frequency region where the reliability of information is low in any sensor, there is a problem that the estimation accuracy decreases in that frequency region.
For example, when an angular velocity sensor and an inclinometer are used as posture estimation sensors, since the effective frequency range of the inclinometer is low, there is a problem in terms of estimation accuracy in combination with the angular velocity sensor.
こうした点から、相補フィルタにおいて、センサの動特性を補償し、有効周波数領域を拡大することが考えられる。例えば、相補フィルタに各センサの逆モデルを導入して、各センサの動特性を補償するという考え方があり、センサ出力を真のセンサ入力に近付けることができると共に、センサの有効周波数領域の拡大も図れると考えられている。しかしながら、センサ特性は非線形であり、逆モデルの伝達関数は不安定で非プロパーになりやすく、特に角速度センサの場合、その逆モデルは複雑な積分要素を含むものとなるなど、演算処理による姿勢推定を行うことは極めて困難であるという課題を有していた。 From this point, it is conceivable to compensate the dynamic characteristics of the sensor and expand the effective frequency region in the complementary filter. For example, there is a concept of introducing an inverse model of each sensor into a complementary filter to compensate for the dynamic characteristics of each sensor, and the sensor output can be brought close to the true sensor input, and the effective frequency range of the sensor can be expanded. It is thought that it can plan. However, the sensor characteristics are non-linear, and the transfer function of the inverse model is unstable and easily non-proper, especially in the case of angular velocity sensors, the inverse model includes complex integration elements, etc. There was a problem that it was extremely difficult to perform.
本発明は前記課題を解消するためになされたもので、異なる複数のセンサの各出力に対し、各センサの線形動特性を同定した伝達関数に対する逆伝達関数を用いて補償すると共に、この逆伝達関数に対応して設計されたフィルタを施した上で、最終的に姿勢変換を行い姿勢を推定するようにして、相補フィルタにおける姿勢推定を適切且つ高精度に行える姿勢推定装置を提供することを目的とする。 The present invention has been made to solve the above-mentioned problems. For each output of a plurality of different sensors, the linear dynamic characteristics of each sensor are compensated by using an inverse transfer function with respect to the identified transfer function, and the inverse transfer is performed. To provide a posture estimation device capable of performing posture estimation in a complementary filter appropriately and with high accuracy by applying a filter designed in accordance with a function and finally performing posture conversion to estimate posture. Objective.
本発明に係る姿勢推定装置は、移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相補フィルタ部が、前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えるものである。 An attitude estimation apparatus according to the present invention complements an angular velocity sensor that detects a dynamic angle change of a moving body, an inclinometer that detects an inclination angle of the moving body, an output of the angular velocity sensor, and an output of the inclinometer. Using a complementary filter unit that obtains a posture estimation output of a moving body, wherein the complementary filter unit identifies in advance a transfer function of a dynamic characteristic approximated to the linearity of the angular velocity sensor, and transmits the transfer A first compensation unit that compensates dynamic characteristics of the output of the angular velocity sensor based on an inverse transfer function obtained from the function, and a filter order that is greater than the order of the inverse transfer function in the first compensation unit, The first filter, which is a high-pass filter that filters the output from the first frequency band based on the complementarity condition, and the output of the first filter, The first posture conversion unit for converting to the present Euler angle and the transfer function of the dynamic characteristic approximated to the linearity of the inclinometer are identified in advance, and based on the inverse transfer function obtained from the transfer function, A second compensation unit that compensates the dynamic characteristics of the output, and a filter order that is greater than the order of the inverse transfer function in the second compensation unit, and the second compensation unit outputs a second based on a complementarity condition to the output from the second compensation unit A second filter that is a low-pass filter that performs filtering in the frequency band, a second attitude converter that converts the output of the second filter into an Euler angle that expresses the attitude of the moving object, and an output of the first attitude converter And an adder that complementarily adds and synthesizes the output of the second posture conversion unit to obtain an estimated value of the posture represented by the Euler angle.
このように本発明によれば、角速度センサの出力と傾斜計の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることにより、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実に演算処理が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に積分演算子が含まれる場合でも、ハイパスフィルタである第一フィルタにおける微分演算子と相殺されることとなり、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。 As described above, according to the present invention, when performing posture estimation using the complementary filter into which the inverse model is introduced from the output of the angular velocity sensor and the output of the inclinometer, the sensor inverse model is converted into the dynamic characteristic compensation portion, the posture conversion portion, and the like. For the sensor dynamic characteristics, the transfer function is identified as a linear characteristic by approximation, the characteristics are compensated using the inverse transfer function for this transfer function, and a filter is designed corresponding to the inverse transfer function of each sensor. In addition, by applying the filter before the attitude conversion calculation to the output signal, the product of the inverse transfer function and the transfer function of the filter can be expanded by expanding the effective frequency region by performing characteristic compensation using the inverse transfer function. Can be a proper transfer function, which can be reliably processed, and can finally perform posture estimation through conversion and complementary addition. Using perform an accurate posture estimation with the required frequency domain. In particular, even when the integration function is included in the inverse transfer function of the angular velocity sensor, it is canceled out with the differential operator in the first filter, which is a high-pass filter, and as a result, the calculation can be performed without integration. With respect to the output of the angular velocity sensor, the steady-state error due to integration does not occur, and effective estimation can be performed without being affected by drift in the sensor, and the estimation accuracy can be improved.
また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、 In addition, the posture estimation apparatus according to the present invention has the transfer function of the angular velocity sensor as
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、
However, D 1 (s) = 1 + a 11 s + a 12 s 2 + ··· + a 1n s n
D 2 (s) = 1 + a 21 s + a 22 s 2 + ··· + a 2n s n
D 3 (s) = 1 + a 31 s + a 32 s 2 + ··· + a 3n s n (n: arbitrary natural number)
It is assumed that the inclinometer has two axes that are not vertical axes as measurement axes, and the transfer function of the inclinometer is expressed by the following equation:
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2＋・・・＋ａmｓm （ｍ：任意の自然数）
で表されるものである。
However, D (s) = 1 + a 1 s + a 2 s 2 + ··· + a m s m (m: arbitrary natural number)
It is represented by
このように本発明によれば、角速度センサの動特性の伝達関数を、その逆伝達関数と共に安定なものとし、且つ、傾斜計の動特性の伝達関数を、その逆伝達関数と共に安定なものとして、各センサの逆伝達関数とフィルタの伝達関数との積を安定且つプロパーな伝達関数とすることにより、センサ出力の補償とフィルタリングに係る演算処理が無理なく速やかに実行できることとなり、相補フィルタによる姿勢推定を高い精度で適切に進められる。 As described above, according to the present invention, the transfer function of the dynamic characteristic of the angular velocity sensor is stable together with the inverse transfer function, and the transfer function of the inclinometer dynamic characteristic is stable together with the inverse transfer function. By making the product of the inverse transfer function of each sensor and the transfer function of the filter a stable and proper transfer function, it is possible to perform the computation processing related to sensor output compensation and filtering reasonably and quickly, and the attitude of the complementary filter The estimation can be appropriately advanced with high accuracy.
また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、 In addition, the posture estimation apparatus according to the present invention has the transfer function of the angular velocity sensor as
で表され、前記傾斜計の伝達関数が、下式、 The transfer function of the inclinometer is expressed by the following equation:
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表されるものである。
However, D (s) = 1 + a 1 s + a 2 s 2
It is represented by
このように本発明によれば、角速度センサの動特性の伝達関数における各要素の分母多項式を一次式とすると共に、傾斜計の動特性の伝達関数における分母多項式を二次式として、姿勢推定に対応した適切な次数とすることにより、各センサの動特性の近似を十分高い精度としつつ、伝達関数及びその逆伝達関数を簡略なものとして、センサ出力の補償とフィルタリングに係る演算処理を高速化でき、相補フィルタによる姿勢推定を、十分な精度を確保しながらより高速に実行でき、推定値を姿勢制御等に適切に活用可能となる。 Thus, according to the present invention, the denominator polynomial of each element in the transfer function of the dynamic characteristics of the angular velocity sensor is a linear expression, and the denominator polynomial in the transfer function of the inclinometer dynamic characteristics is a quadratic expression for posture estimation. By making the corresponding order appropriate, approximation of the dynamic characteristics of each sensor is sufficiently accurate, and the transfer function and its inverse transfer function are simplified to speed up the calculation processing related to sensor output compensation and filtering. In addition, posture estimation using a complementary filter can be executed at higher speed while ensuring sufficient accuracy, and the estimated value can be appropriately used for posture control and the like.
以下、本発明の一実施形態に係る姿勢推定装置を前記図１及び図２に基づいて説明する。
前記各図において、本実施形態に係る姿勢推定装置１は、移動体の動的な角度変化を検出する角速度センサ１０と、移動体の傾斜角を検出する傾斜計２０と、角速度センサ１０の出力と傾斜計２０の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部３０とを備える構成である。
Hereinafter, an attitude estimation apparatus according to an embodiment of the present invention will be described with reference to FIGS. 1 and 2.
In each of the drawings, the posture estimation apparatus 1 according to the present embodiment includes an angular velocity sensor 10 that detects a dynamic angle change of a moving body, an inclinometer 20 that detects an inclination angle of the moving body, and an output of the angular velocity sensor 10. And the output of the inclinometer 20 are used in a complementary manner, and a complementary filter unit 30 for obtaining a posture estimation output of the moving body is provided.
前記角速度センサ１０は、レートジャイロ等の公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、角速度センサ１０の伝達関数をあらかじめ同定しておく必要がある。 The angular velocity sensor 10 is a known sensor such as a rate gyro and will not be described in detail. In designing the complementary filter, it is necessary to identify the transfer function of the angular velocity sensor 10 in advance.
この角速度センサ１０の動特性に係る伝達関数の同定は、公知の手法、例えば、座標系の各軸とセンサ出力の組合せごとに周波数応答を測定する、すなわち、角速度センサに対する姿勢変動の正弦波入力に基づいたセンサ出力波形を、センサを適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。 The transfer function relating to the dynamic characteristics of the angular velocity sensor 10 is identified by a known method, for example, by measuring the frequency response for each combination of each axis of the coordinate system and the sensor output, that is, by inputting the sine wave of the posture fluctuation to the angular velocity sensor The sensor output waveform based on is obtained for a plurality of representative frequencies in the frequency domain to which the sensor is applied, the phase lag and gain are identified from each obtained output waveform, and each value is plotted in the frequency domain. The transfer function is identified by approximation by the least square method from the distribution of the phase lag and gain value at each frequency.
前記傾斜計２０は、加速度計や液面傾斜計などの公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、傾斜計２０についても、その伝達関数をあらかじめ同定しておく必要がある。 The inclinometer 20 is a known sensor such as an accelerometer or a liquid level inclinometer, and will not be described in detail. In designing the complementary filter, it is necessary to identify the transfer function of the inclinometer 20 in advance.
この傾斜計２０の動特性に係る伝達関数の同定は、公知の手法、例えば、傾斜計に対する姿勢変動の正弦波入力に基づいた傾斜計出力波形を、傾斜計を適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。 The transfer function relating to the dynamic characteristics of the inclinometer 20 is identified by a known method, for example, an inclinometer output waveform based on a sine wave input of attitude change to the inclinometer, and a plurality of representatives in a frequency domain to which the inclinometer is applied. The phase lag and gain are identified from each obtained output waveform, each value is plotted in the frequency domain, and the distribution of the phase lag and gain value at each frequency is further calculated by the least square method. This is done in the process of identifying the transfer function by approximation.
前記相補フィルタ部３０は、周波数領域で複数のセンサ出力を相補的に組合わせて姿勢推定を実行するものである。 The complementary filter unit 30 performs posture estimation by complementarily combining a plurality of sensor outputs in the frequency domain.
この相補フィルタ部３０は、詳細には、前記角速度センサ１０の動特性の逆伝達関数に基づいて、角速度センサ１０の動特性を補償する第一補償部３１と、この第一補償部３１からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングする第一フィルタ３２と、この第一フィルタ３２の出力をオイラー角へ変換する第一姿勢変換部３３と、前記傾斜計２０の動特性の逆伝達関数に基づいて、傾斜計２０の動特性を補償する第二補償部３４と、この第二補償部３４からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングする第二フィルタ３５と、この第二フィルタ３５の出力をオイラー角へ変換する第二姿勢変換部３６と、前記第一姿勢変換部３３の出力と第二姿勢変換部３６の出力を加算する加算器３７とを備える構成である。 Specifically, the complementary filter unit 30 includes a first compensation unit 31 that compensates for the dynamic characteristic of the angular velocity sensor 10 based on the inverse transfer function of the dynamic characteristic of the angular velocity sensor 10, and The first filter 32 that filters the output in the first frequency band based on the complementarity condition, the first attitude converting unit 33 that converts the output of the first filter 32 to the Euler angle, and the dynamic characteristics of the inclinometer 20 Based on the inverse transfer function, a second compensation unit 34 that compensates the dynamic characteristics of the inclinometer 20 and a second filter 35 that filters the output from the second compensation unit 34 in a second frequency band based on the complementarity condition. A second attitude conversion unit 36 that converts the output of the second filter 35 into Euler angles, and an adder 37 that adds the output of the first attitude conversion unit 33 and the output of the second attitude conversion unit 36. Structure It is.
前記第一補償部３１は、あらかじめ角速度センサ１０の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、角速度センサ１０の動特性を補償し、角速度センサ１０の有効周波数領域を拡大するものである。 The first compensation unit 31 compensates for the dynamic characteristic of the angular velocity sensor 10 based on the inverse transfer function obtained by identifying the transfer function of the dynamic characteristic approximated linearly of the angular velocity sensor 10 in advance. This extends the effective frequency region.
前記第一フィルタ３２は、第一補償部３１における逆伝達関数の次数より大きいフィルタ次数とされて、第一補償部３１からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングするハイパスフィルタである。この第一フィルタ３２のフィルタ特性は、第一補償部３１での補償後の角速度センサ出力が信頼できる周波数領域に合わせて設計される。 The first filter 32 has a higher filter order than the order of the inverse transfer function in the first compensation unit 31, and filters the output from the first compensation unit 31 in the first frequency band based on the complementarity condition. It is. The filter characteristic of the first filter 32 is designed in accordance with a frequency region in which the angular velocity sensor output after compensation by the first compensation unit 31 is reliable.
前記第一姿勢変換部３３は、第一フィルタ３２の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器３７へ出力するものである。 The first posture conversion unit 33 receives the output of the first filter 32, converts it into an estimated value of Euler angle that represents the posture of the moving body, and outputs it to the adder 37.
前記第二補償部３４は、あらかじめ傾斜計２０の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、傾斜計２０の動特性を補償し、傾斜計２０の有効周波数領域を拡大するものである。 The second compensator 34 compensates for the dynamic characteristics of the inclinometer 20 based on the inverse transfer function obtained by identifying the transfer function of the dynamic characteristics approximated to the linearity of the inclinometer 20 in advance. This extends the effective frequency region.
前記第二フィルタ３５は、第二補償部３４における逆伝達関数の次数より大きいフィルタ次数とされて、第二補償部３４からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングするローパスフィルタである。この第二フィルタ３５のフィルタ特性は、第二補償部３４での補償後の傾斜計出力が信頼できる周波数領域に合わせて設計されるが、前記第一フィルタ３２の伝達関数をＦ1（ｓ）、第二フィルタ３５の伝達関数をＦ2（ｓ）とすると、相補性条件から、Ｆ1（ｓ）＋Ｆ2（ｓ）＝１となるよう設計される必要がある。 The second filter 35 has a filter order larger than the order of the inverse transfer function in the second compensation unit 34, and filters the output from the second compensation unit 34 in a second frequency band based on the complementarity condition. It is. The filter characteristics of the second filter 35 are designed in accordance with a frequency region in which the inclinometer output after compensation by the second compensation unit 34 is reliable. The transfer function of the first filter 32 is F 1 (s). If the transfer function of the second filter 35 is F 2 (s), it is necessary to design F 1 (s) + F 2 (s) = 1 from the complementarity condition.
前記第二姿勢変換部３６は、第二フィルタ３５の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器３７へ出力するものである。 The second posture conversion unit 36 receives the output of the second filter 35, converts it into an estimated value of Euler angle representing the posture of the moving body, and outputs it to the adder 37.
ここで、前記第一姿勢変換部３３と第二姿勢変換部３６で利用するオイラー角による姿勢表現について説明する。 Here, posture expression by Euler angles used in the first posture conversion unit 33 and the second posture conversion unit 36 will be described.
オイラー角で表される、センサユニットが取付けられた移動体の姿勢ηは、鉛直軸以外の軸まわりの回転角β、γ（ｙ軸まわりβ回転、ｘ軸まわりγ回転）を用いて、
η＝［β，γ］T
と表され、また、回転行列を用いると、姿勢wＲbは、
The posture η of the moving body to which the sensor unit is attached, expressed in Euler angles, is calculated using rotation angles β and γ around axes other than the vertical axis (β rotation around the y axis and γ rotation around the x axis)
η = [β, γ] T
And using a rotation matrix, the posture w R b is
と表される。
この場合、センサユニット座標系における角速度センサで出力される角速度bωは、ワールド座標系における角速度wωを用いて、下式（１）のように表せる。
It is expressed.
In this case, the angular velocity b ω output from the angular velocity sensor in the sensor unit coordinate system can be expressed by the following equation (1) using the angular velocity w ω in the world coordinate system.
式（１）より、角速度bωは、オイラー角の微分値 From equation (1), the angular velocity b ω is the differential value of the Euler angle.
を用いて表すと、下式（２）のようになる。 Is expressed by the following formula (2).
続いて、傾斜計の出力とオイラー角による姿勢wＲbの関係を球面三角法より導出する。はじめに、ｙ軸を測定軸として持つ傾斜計の出力θ1を、図２における、ｘ軸とｙ軸がなす平面とwＲbｘとwＲbｚがなす平面の交線ＡＯとwＲbｘがなす角度∠ＡＯＣ＝θ1と仮定する。このとき、２つの球面三角形△ＡＢＣと△ＡＤＥを考える。ここで、^ｘ、^ｙ、^ｚはワールド座標系におけるｘ軸、ｙ軸、ｚ軸を表す単位ベクトルとする。球面三角形の角度Ａ、Ｂ、Ｃと、それぞれの頂点と中心を結ぶ線分がなす角度ａ、ｂ、ｃの間に次の関係が成り立つ。
ｓｉｎＡ／ｓｉｎａ＝ｓｉｎＢ／ｓｉｎｂ＝ｓｉｎＣ／ｓｉｎｃ （３）
式（３）から球面三角形△ＡＤＥにおいて次の関係式（４）が導かれる。
Subsequently, the relationship between the output of the inclinometer and the posture w R b based on the Euler angle is derived from the spherical trigonometry. First , the output θ 1 of the inclinometer having the y-axis as a measurement axis is obtained by using the intersection lines AO and w R b between the plane formed by the x-axis and the y-axis and the plane formed by w R b x and w R b z in FIG. assume angle ∠AOC = theta 1 which x is formed. At this time, two spherical triangles ΔABC and ΔADE are considered. Here, ^ x, ^ y, and ^ z are unit vectors representing the x-axis, y-axis, and z-axis in the world coordinate system. The following relationship is established between the angles A, B, and C of the spherical triangle and the angles a, b, and c formed by the line segments connecting the vertices and the centers.
sinA / sina = sinB / sinb = sinC / sinc (3)
The following relational expression (4) is derived from the expression (3) in the spherical triangle ΔADE.
同様に、球面三角形△ＡＢＣから、下式（５）が導かれる。 Similarly, the following expression (5) is derived from the spherical triangle ΔABC.
球面三角形においても対頂角は等しいので、式（４）と式（５）を整理すると次の関係式（６）を導くことができる。 Since the vertical angle is the same in the spherical triangle, the following relational expression (6) can be derived by arranging the expressions (4) and (5).
同様に、ｘ軸を測定軸に持つ傾斜計の出力θ2では、次の関係式（７）が成り立つ。 Similarly, the following relational expression (7) is established at the output θ 2 of the inclinometer having the x axis as the measurement axis.
これらから、入力を、求めたい軸まわりの回転角に変換するための関数Ｈ（・）は、それぞれ以下のように表される。
角速度センサ出力側については、入力Ｙ1＝［Ｙ11 Ｙ12］Tに対して、
From these, the function H (•) for converting the input into the rotation angle around the axis to be obtained is expressed as follows.
For the angular velocity sensor output side, for input Y 1 = [Y 11 Y 12 ] T ,
となる。また、傾斜計出力側については、入力Ｙ2＝［Ｙ21 Ｙ22］Tに対して、 It becomes. On the inclinometer output side, for input Y 2 = [Y 21 Y 22 ] T ,
となる。 It becomes.
相補フィルタは、それぞれのセンサ出力を相補的に重み付けすることで推定を行うフィルタであり、線形な相補フィルタは、推定値Ｙ（ｓ）、ｉ番目のセンサの測定値Ｘi（ｓ）、ｉ番目のセンサ出力にかけるフィルタＦi（ｓ）を用いて、下式（１０）で表される。 The complementary filter is a filter that performs estimation by complementary weighting of each sensor output, and the linear complementary filter is an estimated value Y (s), an i-th sensor measurement value X i (s), i. Using the filter F i (s) applied to the second sensor output, it is expressed by the following equation (10).
これより、前記相補フィルタ部３０の、センサの逆モデルを用いて設計される相補フィルタで、姿勢の推定値ηestを表した場合、センサ出力を逆伝達関数に通して推定した入力を、求めたい軸まわりの回転角に変換するための関数Ｈ（・）を用いることで、 As a result, when the estimated value η est of the posture is expressed by a complementary filter designed using an inverse model of the sensor of the complementary filter unit 30, an input obtained by estimating the sensor output through the inverse transfer function is obtained. By using the function H (•) to convert the rotation angle around the desired axis,
となる。ここで、 It becomes. here,
は同定したセンサの逆伝達関数である。また、相補フィルタ部３０は、図１に示すようなブロック線図で表すこともできる。 Is the inverse transfer function of the identified sensor. The complementary filter unit 30 can also be represented by a block diagram as shown in FIG.
なお、第一フィルタ３２を、第一姿勢変換部３３より前段に配置し、また、第二フィルタ３５を、第二姿勢変換部３６より前段に配置しているが、その根拠について説明する。 The first filter 32 is arranged upstream of the first attitude converter 33 and the second filter 35 is arranged upstream of the second attitude converter 36. The basis for this will be described.
姿勢をオイラー角で表す場合、従来手法では、角速度センサの出力をオイラー角の微分値に変換し、それらを積分して、オイラー角の推定値を求めた後に、フィルタをかけて出力する。また、傾斜計の出力もオイラー角に変換した後に、フィルタをかけて出力していた。逆モデルを導入した相補フィルタで姿勢推定を行う場合、姿勢の推定値ηestは、 When the posture is expressed by Euler angles, in the conventional method, the output of the angular velocity sensor is converted into a differential value of Euler angles, integrated, and an estimated value of Euler angles is obtained, and then filtered and output. In addition, the output of the inclinometer was output after being filtered after being converted to Euler angles. When performing posture estimation using a complementary filter with an inverse model, the estimated posture value η est is
と表される。 It is expressed.
角速度センサの場合、出力の動特性を補償した角速度センサ入力ωを、式（２）における行列Ｐの擬似逆行列Ｐ+を用いて、下式（１３）によりオイラー角の微分値へ変換する。 In the case of the angular velocity sensor, the angular velocity sensor input ω compensated for the output dynamic characteristic is converted into a differential value of the Euler angle by the following equation (13) using the pseudo inverse matrix P + of the matrix P in the equation (2).
また、傾斜計では、式（６）、（７）の関係を用いて、動特性を補償したセンサ入力θ＝［θ1 θ2］Tを、下式（１４）によりオイラー角へ変換する。 Further, in the inclinometer, the sensor input θ = [θ 1 θ 2 ] T compensated for the dynamic characteristics is converted into the Euler angle by the following equation (14) using the relationship of the equations (6) and (7).
角速度センサ出力側では、入力を微分値に変換し、これを積分して、オイラー角の推定値を求めた後にフィルタをかけるようになっているが、前記式（１３）より、変換は行列の乗算であることから、フィルタと変換部の順序を入換えることができる。変換前にフィルタを施すようにした場合、角速度センサの逆伝達関数が積分演算子１／ｓを含んでいるのに対応して、フィルタが微分演算子ｓを含むように設計されていれば、積分演算子１／ｓと微分演算子ｓが相殺されることとなり、ドリフトの影響などを回避することが可能となる。 On the output side of the angular velocity sensor, the input is converted to a differential value, and this is integrated to obtain an estimated value of the Euler angle, and then the filter is applied. Since it is multiplication, the order of the filter and the conversion unit can be exchanged. When the filter is applied before the conversion, if the inverse transfer function of the angular velocity sensor includes the integral operator 1 / s and the filter is designed to include the differential operator s, The integral operator 1 / s and the differential operator s are canceled out, and the influence of drift and the like can be avoided.
一方、傾斜計出力側では、入力をオイラー角へ変換した後にフィルタをかけるようになっているが、この変換後にフィルタを施す場合の伝達関数の積 On the other hand, on the inclinometer output side, the input is converted to Euler angles and then the filter is applied. However, the product of the transfer function when the filter is applied after this conversion is applied.
と、入力にフィルタを施した後にオイラー角へ変換する場合の伝達関数の積 And the product of the transfer function when converting to Euler angles after filtering the input
とは、そのテイラー展開を求めると、２次のオーダまで一致することとなり、フィルタと変換部の順序を入換えても、精度の点で十分許容できる。 Means that when the Taylor expansion is obtained, it matches up to the second order, and even if the order of the filter and the conversion unit is changed, it is sufficiently acceptable in terms of accuracy.
これらにより、姿勢変換の前に周波数フィルタを施しても、推定値に大きな違いは生じないといえ、相補フィルタ部３０において第一フィルタ３２を第一姿勢変換部３３より前段に配置し、また、第二フィルタ３５を、第二姿勢変換部３６より前段に配置した構成を採用している。 As a result, even if the frequency filter is applied before the posture conversion, it can be said that there is no significant difference in the estimated value. In the complementary filter unit 30, the first filter 32 is arranged before the first posture conversion unit 33, A configuration in which the second filter 35 is arranged in front of the second posture conversion unit 36 is employed.
さらに、角速度センサ１０と傾斜計２０の、それぞれの動特性の伝達関数同定、並びにフィルタ設計についてより詳細に説明する。
各センサの伝達関数Ｇi（ｓ）は、直交する３軸それぞれの軸まわりの回転に対するセンサの周波数応答を元に、各要素が安定且つプロパーとなる行列として同定する。本発明では、異なる軸間の干渉、すなわち各軸まわりの回転における伝達関数は近似的に線形分離可能であると仮定している。また、角速度センサには微分器があり、傾斜計には遅れ要素があると仮定して、軸とセンサ出力の組合せごとに周波数応答を測定し、線形の近似により伝達関数の同定を行う。
Furthermore, the transfer function identification of each dynamic characteristic of the angular velocity sensor 10 and the inclinometer 20 and the filter design will be described in more detail.
The transfer function G i (s) of each sensor is identified as a matrix in which each element is stable and proper on the basis of the frequency response of the sensor with respect to rotation around each of three orthogonal axes. In the present invention, it is assumed that interference between different axes, that is, transfer functions in rotation around each axis, can be approximately linearly separated. Further, assuming that the angular velocity sensor has a differentiator and the inclinometer has a delay element, the frequency response is measured for each combination of the axis and the sensor output, and the transfer function is identified by linear approximation.
角速度センサの動特性の伝達関数は、 The transfer function of the dynamic characteristics of the angular velocity sensor is
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表される。伝達関数の各要素が一次式の場合、特に、
However, D 1 (s) = 1 + a 11 s + a 12 s 2 + ··· + a 1n s n
D 2 (s) = 1 + a 21 s + a 22 s 2 + ··· + a 2n s n
D 3 (s) = 1 + a 31 s + a 32 s 2 + ··· + a 3n s n (n: arbitrary natural number)
It is represented by If each element of the transfer function is linear,
で表される。 It is represented by
一方、傾斜計の動特性の伝達関数は、 On the other hand, the transfer function of inclinometer dynamics is
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表される。なお、伝達関数の各要素の次数は、取扱う事象の複雑さにより決るものであり、姿勢推定の場合、角速度センサのものは一次、傾斜計のものは二次として問題はない。
However, D (s) = 1 + a 1 s + a 2 s 2
It is represented by The order of each element of the transfer function is determined by the complexity of the event to be handled, and in the case of posture estimation, there is no problem with the angular velocity sensor being primary and the inclinometer being secondary.
また、フィルタについては、逆伝達関数の次数よりフィルタ次数が大きくなるように設計する。具体的には、例えば、２軸傾斜計と１軸角速度センサを用いる場合、２軸傾斜計の逆伝達関数行列Ｇ-1 2の各要素が２次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数Ｆ2（ｓ）Ｇ-1 2（ｓ）をプロパーとするために、フィルタの伝達関数Ｆiを２次ローパスフィルタとして設計する。また、角速度センサの逆伝達関数Ｇ-1 1（ｓ）には積分演算子（１／ｓ）が含まれるものの、ハイパスフィルタＦ1（ｓ）に微分演算子（ｓ）を含むようにすれば、積分演算子（１／ｓ）と微分演算子（ｓ）が相殺されるため、結果的に積分演算を伴わない。 The filter is designed so that the filter order is larger than the order of the inverse transfer function. Specifically, for example, when using a 2-axis inclinometer and a 1-axis angular velocity sensor, each element of the inverse transfer function matrix G -1 2 of the 2-axis inclinometer becomes a secondary advance element, In order to make the transfer function F 2 (s) G −1 2 (s), which is the product of the transfer functions of the second filter, proper, the filter transfer function F i is designed as a second-order low-pass filter. Further, the inverse transfer function G −1 1 (s) of the angular velocity sensor includes the integral operator (1 / s), but the high pass filter F 1 (s) includes the differential operator (s). Since the integral operator (1 / s) and the differential operator (s) are canceled out, no integration operation is consequently performed.
次に、本実施形態に係る姿勢推定装置による姿勢推定の過程について説明する。
角速度センサ１０から角速度bωが出力され、傾斜計２０から各軸についての傾斜角θ＝［θ1 θ2］Tが出力されると、角速度センサ出力側については、まず第一補償部３１で出力の動特性が補償され、次いで第一フィルタ３２でハイパスフィルタが施され、出力Ｙ1＝［Ｙ11 Ｙ12］Tが得られる。これが第一姿勢変換部３３に入力され、この入力Ｙ1＝［Ｙ11 Ｙ12］Tを前記式（８）によりオイラー角に変換する。
Next, a process of posture estimation by the posture estimation apparatus according to the present embodiment will be described.
When the angular velocity b ω is output from the angular velocity sensor 10 and the inclination angle θ = [θ 1 θ 2 ] T for each axis is output from the inclinometer 20, the first compensation unit 31 first performs the angular velocity sensor output side. The dynamic characteristics of the output are compensated, and then a high-pass filter is applied by the first filter 32 to obtain an output Y 1 = [Y 11 Y 12 ] T. This is input to the first posture conversion unit 33, and this input Y 1 = [Y 11 Y 12 ] T is converted into Euler angles by the above equation (8).
また、一方、傾斜計出力側では、第二補償部３４で出力の動特性が補償され、次いで第二フィルタ３５でローパスフィルタが施され、出力Ｙ2＝［Ｙ21 Ｙ22］Tが得られる。これが第二姿勢変換部３６に入力され、この入力Ｙ2＝［Ｙ21 Ｙ22］Tを前記式（９）によりオイラー角に変換する。 On the other hand, on the inclinometer output side, the dynamic characteristics of the output are compensated by the second compensation unit 34, and then the low-pass filter is applied by the second filter 35, and the output Y 2 = [Y 21 Y 22 ] T is obtained. . This is input to the second attitude conversion unit 36, and this input Y 2 = [Y 21 Y 22 ] T is converted into Euler angles by the above equation (9).
第一姿勢変換部３３で変換された出力と第二姿勢変換部３６で変換された出力が加算器３７で相補的に合成されると、姿勢の推定値ηest＝［βest γest］Tが求められる。 When the output converted by the first posture converting unit 33 and the output converted by the second posture converting unit 36 are complementarily synthesized by the adder 37, the estimated value of the posture η est = [β est γ est ] T Is required.
このように、本実施形態に係る姿勢推定装置は、角速度センサ１０の出力と傾斜計２０の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積について確実に数値演算が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に含まれる積分演算子が、第一フィルタ３２における微分演算子と相殺されて、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。 As described above, the posture estimation apparatus according to the present embodiment uses the inverse model of the sensor as a dynamic characteristic when performing posture estimation from the output of the angular velocity sensor 10 and the output of the inclinometer 20 using the complementary filter into which the inverse model is introduced. The transfer function is divided into a compensation part and a posture conversion part, and the transfer function is identified as a linear characteristic by approximation for the sensor dynamic characteristic, and the characteristic is compensated using the inverse transfer function for this transfer function. Correspondingly, the filter is designed, and the filter is applied before the attitude conversion calculation to the output signal. Therefore, the inverse transfer function is achieved while expanding the effective frequency region with the characteristic compensation using the inverse transfer function. Can be numerically calculated with respect to the product of the filter and the transfer function of the filter, and finally, posture estimation can be performed appropriately through conversion and complementary addition, which is necessary using multiple sensors. It allows an accurate posture estimation in frequency domain. In particular, the integral operator included in the inverse transfer function of the angular velocity sensor is canceled with the differential operator in the first filter 32, and as a result, the calculation can be performed without integration. The steady-state error does not increase due to integration, and effective estimation can be performed without being affected by drift in the sensor, thereby improving the estimation accuracy.
本発明に係る姿勢推定装置を用いて、角速度センサと傾斜計の出力から姿勢推定を行い、得られた結果について、比較例としての逆伝達関数を用いない相補フィルタを適用した場合等の姿勢推定結果と比較評価した。 Using the posture estimation device according to the present invention, posture estimation is performed from the outputs of the angular velocity sensor and the inclinometer, and the obtained result is a posture estimation when a complementary filter not using an inverse transfer function is applied as a comparative example. The results were compared and evaluated.
姿勢推定の対象には２軸実験機を用い、ＳＳＳＪ製１軸角速度センサＣＲＳ０７−１１Ｓ、ＵＳＤｉｇｉｔａｌ製２軸傾斜計Ｘ３Ｑをそれぞれ取付けた。２軸実験機の１番目の関節はｙ軸周りに角度β、２番目の関節はｘ軸周りに角度γ、それぞれ回転する関節であるとする。 A biaxial experimental machine was used as an object for posture estimation, and a SSSJ 1-axis angular velocity sensor CRS07-11S and a USDigital 2-axis inclinometer X3Q were respectively attached. It is assumed that the first joint of the two-axis experimental machine is an angle β around the y axis, and the second joint is an angle γ around the x axis.
測定に先立ち、角速度センサ、傾斜計についてセンサ動特性（伝達関数）の同定を行った。このセンサ動特性の同定では、異なる軸間の干渉は近似的に線形分離できると仮定し、軸とセンサ出力の組合せごとに正弦波を入力して周波数応答を測定し、最小二乗法により伝達関数を同定する。 Prior to the measurement, sensor dynamic characteristics (transfer function) were identified for the angular velocity sensor and the inclinometer. In this sensor dynamic characteristic identification, it is assumed that the interference between different axes can be approximately linearly separated, a sine wave is input for each combination of axis and sensor output, the frequency response is measured, and the transfer function is calculated by the least square method. Is identified.
実際の周波数応答の測定から、それぞれのセンサの伝達関数を、角速度センサは下式（１５）、傾斜計は下式（１６）、（１７）のように同定した。 From the actual frequency response measurement, the transfer function of each sensor was identified by the following equation (15) for the angular velocity sensor and the following equations (16) and (17) for the inclinometer.
この角速度センサと傾斜計の各伝達関数を用いて、２軸相補フィルタを設計した。実施例１として、この相補フィルタを用いると、姿勢の推定値ηestは下式（１８）で表される。 A biaxial complementary filter was designed using the transfer functions of the angular velocity sensor and the inclinometer. As Example 1, when this complementary filter is used, the estimated value η est of the posture is expressed by the following equation (18).
ここで、Ｘ1は角速度センサの出力、Ｘ2は２軸傾斜角センサの出力である。また、前記式（８）、式（９）で示したＨ1、Ｈ2をオイラー角の推定に用いている。なお、式（８）、（９）に用いている推定値は、実際は離散時間で使用するために、１ステップ前の推定値を用いている。 Here, X 1 is the output of the angular velocity sensor, and X 2 is the output of the biaxial tilt angle sensor. Further, H 1 and H 2 shown in the equations (8) and (9) are used for the estimation of Euler angles. Note that the estimated values used in the equations (8) and (9) are actually used in discrete time, and therefore the estimated values one step before are used.
この相補フィルタでは、２軸傾斜計の逆伝達関数行列Ｇ-1 2の各要素が２次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数Ｆ2(s)Ｇ-1 2 (s)をプロパーとするために、第二フィルタの伝達関数Ｆi（ｓ）を２次ローパスフィルタとして設計した。また、角速度センサの逆伝達関数Ｇ-1 1 (s) には積分器が含まれるが、ハイパスフィルタである第一フィルタＦ1(s) の持つ微分器と相殺されるため、結果的に積分演算を伴わないこととなる。 In this complementary filter, each element of the inverse transfer function matrix G −1 2 of the two-axis inclinometer is a secondary advance element, and therefore, a transfer function F 2 (the product of the inverse transfer function and the transfer function of the second filter) s) The transfer function F i (s) of the second filter is designed as a second-order low-pass filter in order to make G −1 2 (s) proper. Further, the inverse transfer function G −1 1 (s) of the angular velocity sensor includes an integrator, but it is canceled out with the differentiator of the first filter F 1 (s) which is a high-pass filter. No operation is involved.
また、比較例１として、逆伝達関数を用いない２軸相補フィルタを設計し、姿勢推定に用いるようにした。さらに、比較例２として、角速度センサのみを用いて姿勢推定を行うようにした。 Further, as Comparative Example 1, a two-axis complementary filter that does not use an inverse transfer function is designed and used for posture estimation. Further, as Comparative Example 2, posture estimation is performed using only an angular velocity sensor.
この本発明に係る姿勢推定装置を用いる場合（実施例１）と各比較例の場合について、実験機の２軸に正弦波入力をβ＝５７．３ｓｉｎｔ［ｄｅｇ］、γ＝２５．７ｓｉｎ３ｔ［ｄｅｇ］で与え、姿勢推定を行った。なお、いずれの場合も、第一フィルタ、第二フィルタの伝達関数Ｆiは同じものを用いている。 In the case of using the posture estimation apparatus according to the present invention (Example 1) and the case of each comparative example, the sine wave input is input to two axes of the experimental machine by β = 57.3 sin [deg] and γ = 25.7 sin 3t [deg]. ] And estimated the posture. In either case, the same transfer function F i is used for the first filter and the second filter.
実施例１における逆伝達関数を用いて設計した２軸相補フィルタと、逆伝達関数を用いない比較例１の相補フィルタでのβ、γの推定結果を、真値と共に図３に示す。この図３中、実線は実施例１による推定値、濃灰色破線は比較例１による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、角速度センサのみを用いた比較例２の推定結果を、真値と共に図４に示す。この図４中、実線は比較例２による推定値、薄灰色点線は真の入力値をそれぞれ示している。 FIG. 3 shows the estimation results of β and γ with the two-axis complementary filter designed using the inverse transfer function in Example 1 and the complementary filter of Comparative Example 1 that does not use the inverse transfer function together with true values. In FIG. 3, a solid line indicates an estimated value according to the first embodiment, a dark gray broken line indicates an estimated value according to the comparative example 1, and a light gray dotted line indicates a true input value. Moreover, the estimation result of the comparative example 2 using only an angular velocity sensor is shown in FIG. 4 with a true value. In FIG. 4, the solid line indicates the estimated value according to Comparative Example 2, and the light gray dotted line indicates the true input value.
図３に示すように、実施例１の逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例１の場合に比べ、遅れが補償され、比較的よい推定が行えていることがわかる。また、図４に示すように、角速度センサのみを用いた比較例２の推定結果によれば、位相を補償することはできているものの、ドリフトが発生しており、時間経過とともに真値からの誤差が増大している。これに対し、逆伝達関数を用いた相補フィルタは、角速度センサのみの推定で発生していたドリフトの補償も行えている。これらから、本発明の姿勢推定装置における相補フィルタは２軸の推定において有効であるといえる。 As shown in FIG. 3, the complementary filter using the inverse transfer function of Example 1 is compensated for delay and can perform a relatively good estimation as compared with Comparative Example 1 that does not use the inverse transfer function. Recognize. Further, as shown in FIG. 4, according to the estimation result of Comparative Example 2 using only the angular velocity sensor, although the phase can be compensated for, a drift has occurred, and from the true value as time passes. The error is increasing. On the other hand, the complementary filter using the inverse transfer function can also compensate for the drift generated by the estimation of only the angular velocity sensor. From these, it can be said that the complementary filter in the posture estimation apparatus of the present invention is effective in biaxial estimation.
次に、実験機の複雑な動きに対して有効に推定が行われるかどうかについて評価した。
複雑な動きとして、実験機に対し、１軸目の入力ｑ1(t) 、２軸目の入力ｑ2(t) をそれぞれ与え、姿勢推定を行った。ｑ1(t) を下式（１９）、ｑ2(t)を下式（２０）に示す。また、サンプリング周期１［ｋＨｚ］、サンプリング時間２０［ｓ］としている。ｑ1(t)、ｑ2(t)において、パラメータａ1i、ｂ1i、ａ2i、ｂ2iは、振幅の合計が４０［ｄｅｇ］となるように、ランダムに入力した。また、測定周波数ｆ1i、ｆ2iをばらけさせるためにｉ番目の整数部分はｉ−１として決定し、小数部分は０．００から０．９９［Ｈｚ］までをランダムに入力している（表１参照）。
Next, it was evaluated whether the estimation was effectively performed for the complex movement of the experimental machine.
As complicated movements, the first axis input q 1 (t) and the second axis input q 2 (t) were given to the experimental machine, respectively, and posture estimation was performed. q 1 (t) is represented by the following formula (19), and q 2 (t) is represented by the following formula (20). The sampling period is 1 [kHz] and the sampling time is 20 [s]. In q 1 (t) and q 2 (t), the parameters a 1i , b 1i , a 2i , and b 2i are randomly input so that the total amplitude is 40 [deg]. Further, in order to vary the measurement frequencies f 1i and f 2i , the i-th integer part is determined as i−1, and the decimal part is randomly input from 0.00 to 0.99 [Hz] ( (See Table 1).
前記同様、姿勢推定の対象には２軸実験機を用い、ＳＳＳＪ製１軸角速度センサＣＲＳ０７−１１Ｓ、ＵＳＤｉｇｉｔａｌ製２軸傾斜計Ｘ３Ｍ−２をそれぞれ取付けた。２軸実験機の１番目の関節はｙ軸周りに角度β、２番目の関節はｘ軸周りに角度γ、それぞれ回転する関節であるとする。 Similarly to the above, a biaxial experimental machine was used as an object for posture estimation, and a SSSJ uniaxial angular velocity sensor CRS07-11S and a USDigital biaxial inclinometer X3M-2 were attached. It is assumed that the first joint of the two-axis experimental machine is an angle β around the y axis, and the second joint is an angle γ around the x axis.
測定に先立ち、角速度センサ、傾斜計についてセンサ動特性（伝達関数）の同定を行った。正弦波入力により、各軸について、角速度センサは図５ないし図９、傾斜計は図１０、図１１のような周波数応答が得られた。なお、前記図５ないし図１１の各図中で、灰色の十字の点は周波数応答の測定値、連続する線は最小二乗法による近似で求めた伝達関数を、それぞれ示している。 Prior to the measurement, sensor dynamic characteristics (transfer function) were identified for the angular velocity sensor and the inclinometer. With the sine wave input, the frequency response shown in FIGS. 5 to 9 for the angular velocity sensor and FIGS. 10 and 11 for the inclinometer was obtained for each axis. In each of FIGS. 5 to 11, the gray cross points indicate the measured values of the frequency response, and the continuous lines indicate the transfer functions obtained by approximation using the least square method.
これらより、それぞれのセンサの伝達関数を、角速度センサは下式（２１）、傾斜計は下式（２２）のように同定した。 From these, the transfer function of each sensor was identified by the following equation (21) for the angular velocity sensor and the following equation (22) for the inclinometer.
この角速度センサと傾斜計の各伝達関数を用いて、２軸相補フィルタを設計した。フィルタの遮断周波数は、傾斜計の周波数の半分になるように設定した。実施例２として、この相補フィルタを用いると、姿勢の推定値ηestは下式（２３）で表される。 A biaxial complementary filter was designed using the transfer functions of the angular velocity sensor and the inclinometer. The cutoff frequency of the filter was set to be half the frequency of the inclinometer. As Example 2, when this complementary filter is used, the estimated value η est of the posture is expressed by the following equation (23).
また、比較例３として、逆伝達関数を用いない２軸相補フィルタを設計し、姿勢推定に用いるようにした。加えて、比較例４として、傾斜計の出力信号をそのまま用いて、また、比較例５として、傾斜計の出力信号をローパスフィルタにかけたものを用いて、また、比較例６として、角速度センサの出力信号を球面積分したものを用いて、また、比較例７として、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いて、それぞれ姿勢を求めるようにした。なお、比較例５のローパスフィルタ、比較例７のハイパスフィルタは下式（２４）、（２５）に示されるものとなっている。比較例５のローパスフィルタの遮断周波数は５［Ｈｚ］程度の値とし、また、比較例７のハイパスフィルタの遮断周波数は相補フィルタの場合と同様の値とする。 As Comparative Example 3, a two-axis complementary filter that does not use an inverse transfer function was designed and used for posture estimation. In addition, the output signal of the inclinometer is used as it is as Comparative Example 4, the output signal of the inclinometer is subjected to a low-pass filter as Comparative Example 5, and the angular velocity sensor of Comparative Example 6 is used as the comparative example 6. The output signals were obtained by spherical integration, and as Comparative Example 7, the angular velocity sensor output signal was subjected to spherical integration and further subjected to a high-pass filter to obtain the posture. The low-pass filter of Comparative Example 5 and the high-pass filter of Comparative Example 7 are represented by the following equations (24) and (25). The cutoff frequency of the low-pass filter of Comparative Example 5 is set to a value of about 5 [Hz], and the cutoff frequency of the high-pass filter of Comparative Example 7 is set to the same value as that of the complementary filter.
前記ｑ1(t) 、ｑ2(t) の各入力を与える複雑な動きに対して、本発明に係る姿勢推定装置を用いる場合（実施例２）と各比較例の場合で、姿勢推定を行った結果を示す。
実施例２における逆伝達関数を用いて設計した２軸相補フィルタと、逆伝達関数を用いない比較例３の相補フィルタの推定結果を、真値と共に図１２に示す。この図１２中、実線は実施例２による推定値、濃灰色破線は比較例３による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、推定結果における５秒から１０秒の範囲の拡大図を図１３に、各結果の推定誤差を図１４にそれぞれ示す。図１３中の各線が示すものは図１２の場合と同様である。一方、図１４中、実線は実施例２の場合の誤差角度、破線は比較例３の場合の誤差角度をそれぞれ示している。
For complex movements that give q 1 (t) and q 2 (t) inputs, posture estimation is performed in the case of using the posture estimation device according to the present invention (Example 2) and in the case of each comparative example. The results are shown.
FIG. 12 shows estimation results of the biaxial complementary filter designed using the inverse transfer function in Example 2 and the complementary filter of Comparative Example 3 that does not use the inverse transfer function together with true values. In FIG. 12, the solid line indicates the estimated value according to the second embodiment, the dark gray broken line indicates the estimated value according to the comparative example 3, and the light gray dotted line indicates the true input value. Further, FIG. 13 shows an enlarged view of the estimation result in the range from 5 seconds to 10 seconds, and FIG. 14 shows an estimation error of each result. What each line in FIG. 13 indicates is the same as in FIG. On the other hand, in FIG. 14, the solid line indicates the error angle in the second embodiment, and the broken line indicates the error angle in the third comparative example.
さらに、傾斜計の出力信号をそのまま用いた比較例４と、傾斜計の出力信号をローパスフィルタにかけたものを用いた比較例５の各処理結果を、真値と共に図１５に示すと共に、各結果の誤差を図１６に示す。図１５中、濃灰色破線は比較例４による出力値、実線は比較例５による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図１６中、破線は比較例４の場合の誤差角度、実線は比較例５の場合の誤差角度をそれぞれ示している。 Furthermore, each processing result of the comparative example 4 using the output signal of the inclinometer as it is and the comparative example 5 using the output signal of the inclinometer applied to the low-pass filter is shown in FIG. This error is shown in FIG. In FIG. 15, the dark gray broken line indicates the output value according to Comparative Example 4, the solid line indicates the output value according to Comparative Example 5, and the light gray dotted line indicates the true input value. In FIG. 16, the broken line indicates the error angle in the case of the comparative example 4, and the solid line indicates the error angle in the case of the comparative example 5.
また、角速度センサの出力信号を球面積分したものを用いた比較例６と、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いた比較例７の各処理結果を、真値と共に図１７に示すと共に、各結果の誤差を図１８に示す。図１７中、濃灰色破線は比較例６による出力値、実線は比較例７による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図１８中、破線は比較例６の場合の誤差角度、実線は比較例７の場合の誤差角度をそれぞれ示している。 Also, the processing results of Comparative Example 6 using the spherical integration of the output signal of the angular velocity sensor and Comparative Example 7 using the spherical integration of the output signal of the angular velocity sensor and applying a high-pass filter are true. FIG. 17 shows the values together with the values, and FIG. 18 shows the error of each result. In FIG. 17, the dark gray broken line indicates the output value according to Comparative Example 6, the solid line indicates the output value according to Comparative Example 7, and the light gray dotted line indicates the true input value. In FIG. 18, the broken line indicates the error angle in the case of Comparative Example 6, and the solid line indicates the error angle in the case of Comparative Example 7.
図１２に示すように、逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例３の場合に比べ、真値に近く、より精度の高い推定が行えていることがわかる。また、図１５に示すように、比較例４の傾斜計のみを用いた結果や、比較例５の傾斜計出力信号をローパスフィルタにかけた結果によれば、遅れが発生しており、ローパスフィルタによってさらに遅れが生じ、精度も低下していることがわかる。 As shown in FIG. 12, the complementary filter using the inverse transfer function is close to the true value compared to the case of Comparative Example 3 that does not use the inverse transfer function, and it can be seen that more accurate estimation can be performed. Further, as shown in FIG. 15, according to the result using only the inclinometer of Comparative Example 4 and the result of applying the inclinometer output signal of Comparative Example 5 to the low-pass filter, there is a delay. It can also be seen that there is a delay and the accuracy is also reduced.
また、図１７に示すように、比較例６の角速度センサ出力信号を球面積分した場合の結果によれば、球面積分により早い時間ではよい精度が得られているものの、時間の経過と共に真値からの誤差が大きくなっている。加えて、比較例７の球面積分し更にハイパスフィルタにかけた場合の結果によれば、ドリフトの補償は行えているものの、低周波数成分を除去していることで、精度が大きく悪化している。 Further, as shown in FIG. 17, according to the result of the spherical integration of the angular velocity sensor output signal of Comparative Example 6, a good accuracy can be obtained at an early time by the spherical integration, but it is true as time passes. The error from the value is large. In addition, according to the result of the spherical integration of Comparative Example 7 and further applied to the high-pass filter, although the drift can be compensated, the accuracy is greatly deteriorated by removing the low frequency component. .
ここで、前記ｑ1(t) 、ｑ2(t) の各入力に対する前記実施例２、及び比較例３ないし７の各結果における最大誤差を比較したものを図１９にそれぞれ示す。この図１９中では、（ａ）比較例６、（ｂ）比較例７、（ｃ）比較例４、（ｄ）比較例５、（ｅ）比較例３、（ｆ）実施例２、におけるそれぞれの最大誤差角度を示している。これらによれば、実施例２における誤差は、各比較例のそれに比べて小さくなっている。 Here, FIG. 19 shows a comparison of the maximum errors in the results of Example 2 and Comparative Examples 3 to 7 with respect to the inputs of q 1 (t) and q 2 (t). In FIG. 19, (a) Comparative Example 6, (b) Comparative Example 7, (c) Comparative Example 4, (d) Comparative Example 5, (e) Comparative Example 3, and (f) Example 2 respectively. The maximum error angle is shown. According to these, the error in Example 2 is smaller than that in each comparative example.
これらの結果から、本発明の姿勢推定装置における相補フィルタは、複雑な動きに対しても有効に推定が行えることがわかる。 From these results, it can be seen that the complementary filter in the posture estimation apparatus of the present invention can effectively estimate even a complex motion.
１ 姿勢推定装置
１０ 角速度センサ
２０ 傾斜計
３０ 相補フィルタ部
３１ 第一補償部
３２ 第一フィルタ
３３ 第一姿勢変換部
３４ 第二補償部
３５ 第二フィルタ
３６ 第二姿勢変換部
３７ 加算器
DESCRIPTION OF SYMBOLS 1 Posture estimation apparatus 10 Angular velocity sensor 20 Inclinometer 30 Complementary filter part 31 1st compensation part 32 1st filter 33 1st attitude | position conversion part 34 2nd compensation part 35 2nd filter 36 2nd attitude | position conversion part 37 Adder
Claims (3)
前記相補フィルタ部が、
前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、
当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、
当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、
前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、
当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、
当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、
前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えることを
特徴とする姿勢推定装置。 An angular velocity sensor that detects a dynamic angle change of the moving body, an inclinometer that detects the inclination angle of the moving body, and an output of the posture estimation of the moving body that complementarily uses the output of the angular velocity sensor and the output of the inclinometer. In a posture estimation device comprising a complementary filter unit for obtaining
The complementary filter section is
A first compensation unit that preliminarily identifies a transfer function of a linearly approximated dynamic characteristic of the angular velocity sensor and compensates the dynamic characteristic of the output of the angular velocity sensor based on an inverse transfer function obtained from the transfer function;
The first filter is a high-pass filter that has a filter order higher than the order of the inverse transfer function in the first compensation unit, and filters the output from the first compensation unit in the first frequency band based on the complementarity condition When,
A first posture conversion unit that converts the output of the first filter into an Euler angle representing the posture of the moving body;
A second compensation unit for preliminarily identifying a linearly approximated dynamic characteristic transfer function of the inclinometer and compensating the dynamic characteristic for the output of the inclinometer based on an inverse transfer function obtained from the transfer function;
A second filter that is a low-pass filter that has a filter order higher than the order of the inverse transfer function in the second compensation unit and filters the output from the second compensation unit in a second frequency band based on a complementarity condition. When,
A second attitude conversion unit that converts the output of the second filter into an Euler angle representing the attitude of the moving body;
A posture estimation device comprising: an adder that complementarily adds and synthesizes the output of the first posture conversion unit and the output of the second posture conversion unit to obtain an estimated value of the posture represented by the Euler angle.
前記角速度センサの伝達関数が、下式、
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表され、
前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、
当該傾斜計の伝達関数が、下式、
で表されることを
特徴とする姿勢推定装置。 In the posture estimation apparatus according to claim 1,
The transfer function of the angular velocity sensor is expressed by the following equation:
D 2 (s) = 1 + a 21 s + a 22 s 2 + ··· + a 2n s n
D 3 (s) = 1 + a 31 s + a 32 s 2 + ··· + a 3n s n (n: arbitrary natural number)
Represented by
The inclinometer is assumed to have two axes that are not vertical axes as measurement axes,
The transfer function of the inclinometer is
A posture estimation device characterized by the following.
前記角速度センサの伝達関数が、下式、
前記傾斜計の伝達関数が、下式、
で表されることを
特徴とする姿勢推定装置。 In the posture estimation apparatus according to claim 2,
The transfer function of the angular velocity sensor is expressed by the following equation:
The transfer function of the inclinometer is as follows:
A posture estimation device characterized by the following.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011061146A JP2012198057A (en) | 2011-03-18 | 2011-03-18 | Attitude estimation apparatus |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011061146A JP2012198057A (en) | 2011-03-18 | 2011-03-18 | Attitude estimation apparatus |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2012198057A JP2012198057A (en) | 2012-10-18 |
| JP2012198057A6 true JP2012198057A6 (en) | 2013-02-21 |
Family
ID=47180444
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011061146A Withdrawn JP2012198057A (en) | 2011-03-18 | 2011-03-18 | Attitude estimation apparatus |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2012198057A (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE102015211258A1 (en) * | 2015-06-18 | 2016-12-22 | Robert Bosch Gmbh | Apparatus and method for evaluating a signal from a rotary encoder |
| CN110542417B (en) * | 2019-09-05 | 2022-12-13 | 武汉理工大学 | Gyroscope linear measurement method and system based on static and dynamic inclinometer correction |
| JP6994706B1 (en) | 2021-09-10 | 2022-01-14 | 株式会社テクノコア | Position measuring device for moving objects |
-
2011
- 2011-03-18 JP JP2011061146A patent/JP2012198057A/en not_active Withdrawn
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108225308B (en) | Quaternion-based attitude calculation method for extended Kalman filtering algorithm | |
| Du et al. | Online serial manipulator calibration based on multisensory process via extended Kalman and particle filters | |
| CN107607113B (en) | Method for measuring inclination angles of two-axis attitude | |
| US9937620B2 (en) | Robot system having function to calculate position and orientation of sensor | |
| JP4876204B2 (en) | Small attitude sensor | |
| Cantelli et al. | A joint-angle estimation method for industrial manipulators using inertial sensors | |
| JP6922641B2 (en) | Angular velocity derivation device and angular velocity derivation method | |
| JP2011185669A (en) | Device, system, method and program for calculation of attitude information | |
| CN107727114B (en) | Acceleration calibration method and system based on gyroscope, service terminal and memory | |
| CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
| US20220252399A1 (en) | Composite sensor and angular velocity correction method | |
| JP3726884B2 (en) | Attitude estimation apparatus and method using inertial measurement apparatus, and program | |
| CN108871323B (en) | High-precision navigation method of low-cost inertial sensor in locomotive environment | |
| CN107860382B (en) | Method for measuring attitude by applying AHRS under geomagnetic anomaly condition | |
| Munoz-Barron et al. | Sensor fusion for joint kinematic estimation in serial robots using encoder, accelerometer and gyroscope | |
| KR101741076B1 (en) | Apparatus for compensating time delay of inertial navigation system and method thereof | |
| KR101584080B1 (en) | Acceleration signal processing method of 3-dimention rotation motion sensor | |
| JP2012198057A6 (en) | Posture estimation device | |
| JP2012198057A (en) | Attitude estimation apparatus | |
| JP3081518B2 (en) | Robot rigidity identification method and device | |
| KR20170092359A (en) | System for detecting 3-axis position information using 3-dimention rotation motion sensor | |
| CN116641697A (en) | Method and system for dynamically measuring attitude while drilling | |
| CN117091592A (en) | Gesture resolving method, gesture resolving device, and computer storage medium | |
| El Hadri et al. | Attitude estimation with gyros-bias compensation using low-cost sensors | |
| JP5962842B2 (en) | Space stabilization device, space stabilization method, and space stabilization program |