姿勢推定装置
【課題】 異なる複数のセンサの各出力に対し、各センサの線形動特性の逆伝達関数を用いて補償すると共に、この逆伝達関数に対応したフィルタを施し、最終的に姿勢変換を行うようにして、姿勢推定を適切且つ高精度に行える姿勢推定装置を提供する。
【解決手段】 角速度センサ１０と傾斜計２０の各逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似線形特性として伝達関数を同定し、その逆伝達関数を用いて特性を補償すると共に、逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実な演算処理で姿勢推定が実行でき、複数のセンサを用いる相補フィルタの手法で精度のよい姿勢推定を行える。
【解決手段】 角速度センサ１０と傾斜計２０の各逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似線形特性として伝達関数を同定し、その逆伝達関数を用いて特性を補償すると共に、逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実な演算処理で姿勢推定が実行でき、複数のセンサを用いる相補フィルタの手法で精度のよい姿勢推定を行える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の慣性センサ出力から相補フィルタを用いて移動体の姿勢を推定する姿勢推定装置に関する。
【背景技術】
【０００２】
歩行ロボットなど、運動の空間領域も周波数領域も広範囲な移動体の制御においては、信頼性の高い移動体の姿勢推定、すなわち、移動体の三次元における傾斜状態の推定が必要である。
姿勢推定には、通常、角速度センサや傾斜計、加速度計などのいわゆる慣性センサが用いられる。こうした慣性センサは、その種類ごとに信頼できる周波数領域がそれぞれ異なっている。
【０００３】
例えば、角速度センサは、動的な角度変化を角速度として検出でき、この角速度の出力を積分することで、姿勢を推定できるが、積分による定常誤差の拡大が問題となるなど、低い周波数領域での信頼性が下がっている。一方、傾斜計は、準静的な状態で傾斜角を検出できるが、動的な状態では、動きの影響による検出までの時間遅れが無視できなくなるなど、高い周波数領域での信頼性が下がるという問題を有している。
【０００４】
また、加速度計は、静的な状態では重力加速度から重力方向を検出可能であるものの、運動中は移動体自体の加速度の影響を受け、正確な検出が行えないという問題がある。
こうしたことから、一種類のセンサのみによる姿勢推定は難しく、異なる種類のセンサを複数組み合わせて用いることが必要になる。
【０００５】
このような異種センサを複合的に利用する技術として、カルマンフィルタを用いた方法が多くとられている。ただし、カルマンフィルタでは、時間領域において、誤差の含まれるセンサ出力の時間的変動傾向からセンサ出力に対する補正量を取得して、移動体の姿勢を高い信頼性で推定可能とするフィルタ設計がなされるものの、このフィルタ設計に係る各パラメータを設定するのが困難である。
【０００６】
この他、周波数領域で複数のセンサ出力を重ね合せる相補フィルタも提案されている。これは、各々のセンサの周波数特性に基づいて、信頼性の高い信号を抽出し、重ね合せるものであり、各センサの信頼できる周波数領域がある程度分かっていれば周波数領域設計は比較的容易である。
【０００７】
例えば、傾斜計のような低い周波数領域が信頼できるセンサにはローパスフィルタを適用し、逆に角速度センサのような高い周波数領域が信頼できるセンサにハイパスフィルタを適用して、フィルタ出力を組み合わせることで推定精度を向上させることができる。
【０００８】
こうした相補フィルタを、移動体の姿勢推定に利用する手法は、従来から種々提案されており、その一例として、特開平５−１９６４７５号公報や特開平５−２２３５８７号公報に開示されるものがある。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開平５−１９６４７５号公報
【特許文献２】特開平５−２２３５８７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
従来における移動体の姿勢推定に係る相補フィルタは、前記各特許文献に示されるような構成となっているが、姿勢推定に用いるそれぞれのセンサの有効周波数領域は相補的であるとは限らないため、センサ出力を相補フィルタにそのまま入力して用いる場合、いずれのセンサにおいても情報の信頼性が低い周波数領域が存在すると、その周波数領域において推定精度が低下してしまうという問題があった。
例えば、角速度センサと傾斜計を姿勢推定のためのセンサとして用いる場合、傾斜計の有効な周波数領域が低いため、角速度センサとの組み合わせでは推定精度の点で問題になる。
【００１１】
こうした点から、相補フィルタにおいて、センサの動特性を補償し、有効周波数領域を拡大することが考えられる。例えば、相補フィルタに各センサの逆モデルを導入して、各センサの動特性を補償するという考え方があり、センサ出力を真のセンサ入力に近付けることができると共に、センサの有効周波数領域の拡大も図れると考えられている。しかしながら、センサ特性は非線形であり、逆モデルの伝達関数は不安定で非プロパーになりやすく、特に角速度センサの場合、その逆モデルは複雑な積分要素を含むものとなるなど、演算処理による姿勢推定を行うことは極めて困難であるという課題を有していた。
【００１２】
本発明は前記課題を解消するためになされたもので、異なる複数のセンサの各出力に対し、各センサの線形動特性を同定した伝達関数に対する逆伝達関数を用いて補償すると共に、この逆伝達関数に対応して設計されたフィルタを施した上で、最終的に姿勢変換を行い姿勢を推定するようにして、相補フィルタにおける姿勢推定を適切且つ高精度に行える姿勢推定装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本発明に係る姿勢推定装置は、移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相補フィルタ部が、前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えるものである。
【００１４】
このように本発明によれば、角速度センサの出力と傾斜計の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることにより、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実に演算処理が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に積分演算子が含まれる場合でも、ハイパスフィルタである第一フィルタにおける微分演算子と相殺されることとなり、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。
【００１５】
また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、
【００１６】
【数１】
【００１７】
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、
【００１８】
【数２】
【００１９】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2＋・・・＋ａmｓm （ｍ：任意の自然数）
で表されるものである。
【００２０】
このように本発明によれば、角速度センサの動特性の伝達関数を、その逆伝達関数と共に安定なものとし、且つ、傾斜計の動特性の伝達関数を、その逆伝達関数と共に安定なものとして、各センサの逆伝達関数とフィルタの伝達関数との積を安定且つプロパーな伝達関数とすることにより、センサ出力の補償とフィルタリングに係る演算処理が無理なく速やかに実行できることとなり、相補フィルタによる姿勢推定を高い精度で適切に進められる。
【００２１】
また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、
【００２２】
【数３】
【００２３】
で表され、前記傾斜計の伝達関数が、下式、
【００２４】
【数４】
【００２５】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表されるものである。
【００２６】
このように本発明によれば、角速度センサの動特性の伝達関数における各要素の分母多項式を一次式とすると共に、傾斜計の動特性の伝達関数における分母多項式を二次式として、姿勢推定に対応した適切な次数とすることにより、各センサの動特性の近似を十分高い精度としつつ、伝達関数及びその逆伝達関数を簡略なものとして、センサ出力の補償とフィルタリングに係る演算処理を高速化でき、相補フィルタによる姿勢推定を、十分な精度を確保しながらより高速に実行でき、推定値を姿勢制御等に適切に活用可能となる。
【図面の簡単な説明】
【００２７】
【図１】本発明の一実施形態に係る姿勢推定装置のブロック線図である。
【図２】本発明の一実施形態に係る姿勢推定装置で用いるオイラー角による姿勢とワールド座標系との関係の説明図である。
【図３】本発明に係る姿勢推定装置の実施例１によるオイラー角β及びγの各推定結果、真の入力値、及び比較例１による推定結果のグラフである。
【図４】本発明に係る姿勢推定装置に対する比較例２によるオイラー角β及びγの各推定結果、及び真の入力値のグラフである。
【図５】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その１）である。
【図６】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その２）である。
【図７】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その３）である。
【図８】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その４）である。
【図９】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その５）である。
【図１０】本発明に係る姿勢推定装置の傾斜計における各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その１）である。
【図１１】本発明に係る姿勢推定装置の傾斜計における各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その２）である。
【図１２】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、真の入力値、及び比較例３による推定結果のグラフである。
【図１３】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、真の入力値、及び比較例３による推定結果の、５秒から１０秒の範囲を拡大したグラフである。
【図１４】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、及び比較例３による推定結果の、真の入力値に対する推定誤差のグラフである。
【図１５】本発明に係る姿勢推定装置に対する比較例４及び比較例５による１軸目及び２軸目の各入力の各推定結果、及び真の入力値のグラフである。
【図１６】本発明に係る姿勢推定装置に対する比較例４及び比較例５による１軸目及び２軸目の各入力の各推定結果の、真の入力値に対する推定誤差のグラフである。
【図１７】本発明に係る姿勢推定装置に対する比較例６及び比較例７による１軸目及び２軸目の各入力の各推定結果、及び真の入力値のグラフである。
【図１８】本発明に係る姿勢推定装置に対する比較例６及び比較例７による１軸目及び２軸目の各入力の各推定結果の、真の入力値に対する推定誤差のグラフである。
【図１９】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、及び比較例３ないし７による各推定結果の、真の入力値に対する最大誤差の比較説明図である。
【発明を実施するための形態】
【００２８】
以下、本発明の一実施形態に係る姿勢推定装置を前記図１及び図２に基づいて説明する。
前記各図において、本実施形態に係る姿勢推定装置１は、移動体の動的な角度変化を検出する角速度センサ１０と、移動体の傾斜角を検出する傾斜計２０と、角速度センサ１０の出力と傾斜計２０の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部３０とを備える構成である。
【００２９】
前記角速度センサ１０は、レートジャイロ等の公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、角速度センサ１０の伝達関数をあらかじめ同定しておく必要がある。
【００３０】
この角速度センサ１０の動特性に係る伝達関数の同定は、公知の手法、例えば、座標系の各軸とセンサ出力の組合せごとに周波数応答を測定する、すなわち、角速度センサに対する姿勢変動の正弦波入力に基づいたセンサ出力波形を、センサを適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。
【００３１】
前記傾斜計２０は、加速度計や液面傾斜計などの公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、傾斜計２０についても、その伝達関数をあらかじめ同定しておく必要がある。
【００３２】
この傾斜計２０の動特性に係る伝達関数の同定は、公知の手法、例えば、傾斜計に対する姿勢変動の正弦波入力に基づいた傾斜計出力波形を、傾斜計を適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。
【００３３】
前記相補フィルタ部３０は、周波数領域で複数のセンサ出力を相補的に組合わせて姿勢推定を実行するものである。
【００３４】
この相補フィルタ部３０は、詳細には、前記角速度センサ１０の動特性の逆伝達関数に基づいて、角速度センサ１０の動特性を補償する第一補償部３１と、この第一補償部３１からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングする第一フィルタ３２と、この第一フィルタ３２の出力をオイラー角へ変換する第一姿勢変換部３３と、前記傾斜計２０の動特性の逆伝達関数に基づいて、傾斜計２０の動特性を補償する第二補償部３４と、この第二補償部３４からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングする第二フィルタ３５と、この第二フィルタ３５の出力をオイラー角へ変換する第二姿勢変換部３６と、前記第一姿勢変換部３３の出力と第二姿勢変換部３６の出力を加算する加算器３７とを備える構成である。
【００３５】
前記第一補償部３１は、あらかじめ角速度センサ１０の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、角速度センサ１０の動特性を補償し、角速度センサ１０の有効周波数領域を拡大するものである。
【００３６】
前記第一フィルタ３２は、第一補償部３１における逆伝達関数の次数より大きいフィルタ次数とされて、第一補償部３１からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングするハイパスフィルタである。この第一フィルタ３２のフィルタ特性は、第一補償部３１での補償後の角速度センサ出力が信頼できる周波数領域に合わせて設計される。
【００３７】
前記第一姿勢変換部３３は、第一フィルタ３２の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器３７へ出力するものである。
【００３８】
前記第二補償部３４は、あらかじめ傾斜計２０の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、傾斜計２０の動特性を補償し、傾斜計２０の有効周波数領域を拡大するものである。
【００３９】
前記第二フィルタ３５は、第二補償部３４における逆伝達関数の次数より大きいフィルタ次数とされて、第二補償部３４からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングするローパスフィルタである。この第二フィルタ３５のフィルタ特性は、第二補償部３４での補償後の傾斜計出力が信頼できる周波数領域に合わせて設計されるが、前記第一フィルタ３２の伝達関数をＦ1（ｓ）、第二フィルタ３５の伝達関数をＦ2（ｓ）とすると、相補性条件から、Ｆ1（ｓ）＋Ｆ2（ｓ）＝１となるよう設計される必要がある。
【００４０】
前記第二姿勢変換部３６は、第二フィルタ３５の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器３７へ出力するものである。
【００４１】
ここで、前記第一姿勢変換部３３と第二姿勢変換部３６で利用するオイラー角による姿勢表現について説明する。
【００４２】
オイラー角で表される、センサユニットが取付けられた移動体の姿勢ηは、鉛直軸以外の軸まわりの回転角β、γ（ｙ軸まわりβ回転、ｘ軸まわりγ回転）を用いて、
η＝［β，γ］T
と表され、また、回転行列を用いると、姿勢wＲbは、
【００４３】
【数５】
【００４４】
と表される。
この場合、センサユニット座標系における角速度センサで出力される角速度bωは、ワールド座標系における角速度wωを用いて、下式（１）のように表せる。
【００４５】
【数６】
【００４６】
式（１）より、角速度bωは、オイラー角の微分値
【００４７】
【数７】
【００４８】
を用いて表すと、下式（２）のようになる。
【００４９】
【数８】
【００５０】
続いて、傾斜計の出力とオイラー角による姿勢wＲbの関係を球面三角法より導出する。はじめに、ｙ軸を測定軸として持つ傾斜計の出力θ1を、図２における、ｘ軸とｙ軸がなす平面とwＲbｘとwＲbｚがなす平面の交線ＡＯとwＲbｘがなす角度∠ＡＯＣ＝θ1と仮定する。このとき、２つの球面三角形△ＡＢＣと△ＡＤＥを考える。ここで、^ｘ、^ｙ、^ｚはワールド座標系におけるｘ軸、ｙ軸、ｚ軸を表す単位ベクトルとする。球面三角形の角度Ａ、Ｂ、Ｃと、それぞれの頂点と中心を結ぶ線分がなす角度ａ、ｂ、ｃの間に次の関係が成り立つ。
ｓｉｎＡ／ｓｉｎａ＝ｓｉｎＢ／ｓｉｎｂ＝ｓｉｎＣ／ｓｉｎｃ （３）
式（３）から球面三角形△ＡＤＥにおいて次の関係式（４）が導かれる。
【００５１】
【数９】
【００５２】
同様に、球面三角形△ＡＢＣから、下式（５）が導かれる。
【００５３】
【数１０】
【００５４】
球面三角形においても対頂角は等しいので、式（４）と式（５）を整理すると次の関係式（６）を導くことができる。
【００５５】
【数１１】
【００５６】
同様に、ｘ軸を測定軸に持つ傾斜計の出力θ2では、次の関係式（７）が成り立つ。
【００５７】
【数１２】
【００５８】
これらから、入力を、求めたい軸まわりの回転角に変換するための関数Ｈ（・）は、それぞれ以下のように表される。
角速度センサ出力側については、入力Ｙ1＝［Ｙ11 Ｙ12］Tに対して、
【００５９】
【数１３】
【００６０】
となる。また、傾斜計出力側については、入力Ｙ2＝［Ｙ21 Ｙ22］Tに対して、
【００６１】
【数１４】
【００６２】
となる。
【００６３】
相補フィルタは、それぞれのセンサ出力を相補的に重み付けすることで推定を行うフィルタであり、線形な相補フィルタは、推定値Ｙ（ｓ）、ｉ番目のセンサの測定値Ｘi（ｓ）、ｉ番目のセンサ出力にかけるフィルタＦi（ｓ）を用いて、下式（１０）で表される。
【００６４】
【数１５】
【００６５】
これより、前記相補フィルタ部３０の、センサの逆モデルを用いて設計される相補フィルタで、姿勢の推定値ηestを表した場合、センサ出力を逆伝達関数に通して推定した入力を、求めたい軸まわりの回転角に変換するための関数Ｈ（・）を用いることで、
【００６６】
【数１６】
【００６７】
となる。ここで、
【００６８】
【数１７】
【００６９】
は同定したセンサの逆伝達関数である。また、相補フィルタ部３０は、図１に示すようなブロック線図で表すこともできる。
【００７０】
なお、第一フィルタ３２を、第一姿勢変換部３３より前段に配置し、また、第二フィルタ３５を、第二姿勢変換部３６より前段に配置しているが、その根拠について説明する。
【００７１】
姿勢をオイラー角で表す場合、従来手法では、角速度センサの出力をオイラー角の微分値に変換し、それらを積分して、オイラー角の推定値を求めた後に、フィルタをかけて出力する。また、傾斜計の出力もオイラー角に変換した後に、フィルタをかけて出力していた。逆モデルを導入した相補フィルタで姿勢推定を行う場合、姿勢の推定値ηestは、
【００７２】
【数１８】
【００７３】
と表される。
【００７４】
角速度センサの場合、出力の動特性を補償した角速度センサ入力ωを、式（２）における行列Ｐの擬似逆行列Ｐ+を用いて、下式（１３）によりオイラー角の微分値へ変換する。
【００７５】
【数１９】
【００７６】
また、傾斜計では、式（６）、（７）の関係を用いて、動特性を補償したセンサ入力θ＝［θ1 θ2］Tを、下式（１４）によりオイラー角へ変換する。
【００７７】
【数２０】
【００７８】
角速度センサ出力側では、入力を微分値に変換し、これを積分して、オイラー角の推定値を求めた後にフィルタをかけるようになっているが、前記式（１３）より、変換は行列の乗算であることから、フィルタと変換部の順序を入換えることができる。変換前にフィルタを施すようにした場合、角速度センサの逆伝達関数が積分演算子１／ｓを含んでいるのに対応して、フィルタが微分演算子ｓを含むように設計されていれば、積分演算子１／ｓと微分演算子ｓが相殺されることとなり、ドリフトの影響などを回避することが可能となる。
【００７９】
一方、傾斜計出力側では、入力をオイラー角へ変換した後にフィルタをかけるようになっているが、この変換後にフィルタを施す場合の伝達関数の積
【００８０】
【数２１】
【００８１】
と、入力にフィルタを施した後にオイラー角へ変換する場合の伝達関数の積
【００８２】
【数２２】
【００８３】
とは、そのテイラー展開を求めると、２次のオーダまで一致することとなり、フィルタと変換部の順序を入換えても、精度の点で十分許容できる。
【００８４】
これらにより、姿勢変換の前に周波数フィルタを施しても、推定値に大きな違いは生じないといえ、相補フィルタ部３０において第一フィルタ３２を第一姿勢変換部３３より前段に配置し、また、第二フィルタ３５を、第二姿勢変換部３６より前段に配置した構成を採用している。
【００８５】
さらに、角速度センサ１０と傾斜計２０の、それぞれの動特性の伝達関数同定、並びにフィルタ設計についてより詳細に説明する。
各センサの伝達関数Ｇi（ｓ）は、直交する３軸それぞれの軸まわりの回転に対するセンサの周波数応答を元に、各要素が安定且つプロパーとなる行列として同定する。本発明では、異なる軸間の干渉、すなわち各軸まわりの回転における伝達関数は近似的に線形分離可能であると仮定している。また、角速度センサには微分器があり、傾斜計には遅れ要素があると仮定して、軸とセンサ出力の組合せごとに周波数応答を測定し、線形の近似により伝達関数の同定を行う。
【００８６】
角速度センサの動特性の伝達関数は、
【００８７】
【数２３】
【００８８】
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表される。伝達関数の各要素が一次式の場合、特に、
【００８９】
【数２４】
【００９０】
で表される。
【００９１】
一方、傾斜計の動特性の伝達関数は、
【００９２】
【数２５】
【００９３】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表される。なお、伝達関数の各要素の次数は、取扱う事象の複雑さにより決るものであり、姿勢推定の場合、角速度センサのものは一次、傾斜計のものは二次として問題はない。
【００９４】
また、フィルタについては、逆伝達関数の次数よりフィルタ次数が大きくなるように設計する。具体的には、例えば、２軸傾斜計と１軸角速度センサを用いる場合、２軸傾斜計の逆伝達関数行列Ｇ-12の各要素が２次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数Ｆ2（ｓ）Ｇ-12（ｓ）をプロパーとするために、フィルタの伝達関数Ｆiを２次ローパスフィルタとして設計する。また、角速度センサの逆伝達関数Ｇ-11（ｓ）には積分演算子（１／ｓ）が含まれるものの、ハイパスフィルタＦ1（ｓ）に微分演算子（ｓ）を含むようにすれば、積分演算子（１／ｓ）と微分演算子（ｓ）が相殺されるため、結果的に積分演算を伴わない。
【００９５】
次に、本実施形態に係る姿勢推定装置による姿勢推定の過程について説明する。
角速度センサ１０から角速度bωが出力され、傾斜計２０から各軸についての傾斜角θ＝［θ1 θ2］Tが出力されると、角速度センサ出力側については、まず第一補償部３１で出力の動特性が補償され、次いで第一フィルタ３２でハイパスフィルタが施され、出力Ｙ1＝［Ｙ11 Ｙ12］Tが得られる。これが第一姿勢変換部３３に入力され、この入力Ｙ1＝［Ｙ11 Ｙ12］Tを前記式（８）によりオイラー角に変換する。
【００９６】
また、一方、傾斜計出力側では、第二補償部３４で出力の動特性が補償され、次いで第二フィルタ３５でローパスフィルタが施され、出力Ｙ2＝［Ｙ21 Ｙ22］Tが得られる。これが第二姿勢変換部３６に入力され、この入力Ｙ2＝［Ｙ21 Ｙ22］Tを前記式（９）によりオイラー角に変換する。
【００９７】
第一姿勢変換部３３で変換された出力と第二姿勢変換部３６で変換された出力が加算器３７で相補的に合成されると、姿勢の推定値ηest＝［βest γest］Tが求められる。
【００９８】
このように、本実施形態に係る姿勢推定装置は、角速度センサ１０の出力と傾斜計２０の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積について確実に数値演算が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に含まれる積分演算子が、第一フィルタ３２における微分演算子と相殺されて、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。
【実施例】
【００９９】
本発明に係る姿勢推定装置を用いて、角速度センサと傾斜計の出力から姿勢推定を行い、得られた結果について、比較例としての逆伝達関数を用いない相補フィルタを適用した場合等の姿勢推定結果と比較評価した。
【０１００】
姿勢推定の対象には２軸実験機を用い、ＳＳＳＪ製１軸角速度センサＣＲＳ０７−１１Ｓ、ＵＳＤｉｇｉｔａｌ製２軸傾斜計Ｘ３Ｑをそれぞれ取付けた。２軸実験機の１番目の関節はｙ軸周りに角度β、２番目の関節はｘ軸周りに角度γ、それぞれ回転する関節であるとする。
【０１０１】
測定に先立ち、角速度センサ、傾斜計についてセンサ動特性（伝達関数）の同定を行った。このセンサ動特性の同定では、異なる軸間の干渉は近似的に線形分離できると仮定し、軸とセンサ出力の組合せごとに正弦波を入力して周波数応答を測定し、最小二乗法により伝達関数を同定する。
【０１０２】
実際の周波数応答の測定から、それぞれのセンサの伝達関数を、角速度センサは下式（１５）、傾斜計は下式（１６）、（１７）のように同定した。
【０１０３】
【数２６】
【０１０４】
【数２７】
【０１０５】
この角速度センサと傾斜計の各伝達関数を用いて、２軸相補フィルタを設計した。実施例１として、この相補フィルタを用いると、姿勢の推定値ηestは下式（１８）で表される。
【０１０６】
【数２８】
【０１０７】
ここで、Ｘ1は角速度センサの出力、Ｘ2は２軸傾斜角センサの出力である。また、前記式（８）、式（９）で示したＨ1、Ｈ2をオイラー角の推定に用いている。なお、式（８）、（９）に用いている推定値は、実際は離散時間で使用するために、１ステップ前の推定値を用いている。
【０１０８】
この相補フィルタでは、２軸傾斜計の逆伝達関数行列Ｇ-12の各要素が２次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数Ｆ2(s)Ｇ-12 (s)をプロパーとするために、第二フィルタの伝達関数Ｆi（ｓ）を２次ローパスフィルタとして設計した。また、角速度センサの逆伝達関数Ｇ-11 (s) には積分器が含まれるが、ハイパスフィルタである第一フィルタＦ1(s) の持つ微分器と相殺されるため、結果的に積分演算を伴わないこととなる。
【０１０９】
また、比較例１として、逆伝達関数を用いない２軸相補フィルタを設計し、姿勢推定に用いるようにした。さらに、比較例２として、角速度センサのみを用いて姿勢推定を行うようにした。
【０１１０】
この本発明に係る姿勢推定装置を用いる場合（実施例１）と各比較例の場合について、実験機の２軸に正弦波入力をβ＝５７．３ｓｉｎｔ［ｄｅｇ］、γ＝２５．７ｓｉｎ３ｔ［ｄｅｇ］で与え、姿勢推定を行った。なお、いずれの場合も、第一フィルタ、第二フィルタの伝達関数Ｆiは同じものを用いている。
【０１１１】
実施例１における逆伝達関数を用いて設計した２軸相補フィルタと、逆伝達関数を用いない比較例１の相補フィルタでのβ、γの推定結果を、真値と共に図３に示す。この図３中、実線は実施例１による推定値、濃灰色破線は比較例１による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、角速度センサのみを用いた比較例２の推定結果を、真値と共に図４に示す。この図４中、実線は比較例２による推定値、薄灰色点線は真の入力値をそれぞれ示している。
【０１１２】
図３に示すように、実施例１の逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例１の場合に比べ、遅れが補償され、比較的よい推定が行えていることがわかる。また、図４に示すように、角速度センサのみを用いた比較例２の推定結果によれば、位相を補償することはできているものの、ドリフトが発生しており、時間経過とともに真値からの誤差が増大している。これに対し、逆伝達関数を用いた相補フィルタは、角速度センサのみの推定で発生していたドリフトの補償も行えている。これらから、本発明の姿勢推定装置における相補フィルタは２軸の推定において有効であるといえる。
【０１１３】
次に、実験機の複雑な動きに対して有効に推定が行われるかどうかについて評価した。
複雑な動きとして、実験機に対し、１軸目の入力ｑ1(t) 、２軸目の入力ｑ2(t) をそれぞれ与え、姿勢推定を行った。ｑ1(t) を下式（１９）、ｑ2(t)を下式（２０）に示す。また、サンプリング周期１［ｋＨｚ］、サンプリング時間２０［ｓ］としている。ｑ1(t)、ｑ2(t)において、パラメータａ1i、ｂ1i、ａ2i、ｂ2iは、振幅の合計が４０［ｄｅｇ］となるように、ランダムに入力した。また、測定周波数ｆ1i、ｆ2iをばらけさせるためにｉ番目の整数部分はｉ−１として決定し、小数部分は０．００から０．９９［Ｈｚ］までをランダムに入力している（表１参照）。
【０１１４】
【数２９】
【０１１５】
【数３０】
【０１１６】
【表１】
【０１１７】
前記同様、姿勢推定の対象には２軸実験機を用い、ＳＳＳＪ製１軸角速度センサＣＲＳ０７−１１Ｓ、ＵＳＤｉｇｉｔａｌ製２軸傾斜計Ｘ３Ｍ−２をそれぞれ取付けた。２軸実験機の１番目の関節はｙ軸周りに角度β、２番目の関節はｘ軸周りに角度γ、それぞれ回転する関節であるとする。
【０１１８】
測定に先立ち、角速度センサ、傾斜計についてセンサ動特性（伝達関数）の同定を行った。正弦波入力により、各軸について、角速度センサは図５ないし図９、傾斜計は図１０、図１１のような周波数応答が得られた。なお、前記図５ないし図１１の各図中で、灰色の十字の点は周波数応答の測定値、連続する線は最小二乗法による近似で求めた伝達関数を、それぞれ示している。
【０１１９】
これらより、それぞれのセンサの伝達関数を、角速度センサは下式（２１）、傾斜計は下式（２２）のように同定した。
【０１２０】
【数３１】
【０１２１】
【数３２】
【０１２２】
この角速度センサと傾斜計の各伝達関数を用いて、２軸相補フィルタを設計した。フィルタの遮断周波数は、傾斜計の周波数の半分になるように設定した。実施例２として、この相補フィルタを用いると、姿勢の推定値ηestは下式（２３）で表される。
【０１２３】
【数３３】
【０１２４】
また、比較例３として、逆伝達関数を用いない２軸相補フィルタを設計し、姿勢推定に用いるようにした。加えて、比較例４として、傾斜計の出力信号をそのまま用いて、また、比較例５として、傾斜計の出力信号をローパスフィルタにかけたものを用いて、また、比較例６として、角速度センサの出力信号を球面積分したものを用いて、また、比較例７として、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いて、それぞれ姿勢を求めるようにした。なお、比較例５のローパスフィルタ、比較例７のハイパスフィルタは下式（２４）、（２５）に示されるものとなっている。比較例５のローパスフィルタの遮断周波数は５［Ｈｚ］程度の値とし、また、比較例７のハイパスフィルタの遮断周波数は相補フィルタの場合と同様の値とする。
【０１２５】
【数３４】
【０１２６】
【数３５】
【０１２７】
前記ｑ1(t) 、ｑ2(t) の各入力を与える複雑な動きに対して、本発明に係る姿勢推定装置を用いる場合（実施例２）と各比較例の場合で、姿勢推定を行った結果を示す。
実施例２における逆伝達関数を用いて設計した２軸相補フィルタと、逆伝達関数を用いない比較例３の相補フィルタの推定結果を、真値と共に図１２に示す。この図１２中、実線は実施例２による推定値、濃灰色破線は比較例３による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、推定結果における５秒から１０秒の範囲の拡大図を図１３に、各結果の推定誤差を図１４にそれぞれ示す。図１３中の各線が示すものは図１２の場合と同様である。一方、図１４中、実線は実施例２の場合の誤差角度、破線は比較例３の場合の誤差角度をそれぞれ示している。
【０１２８】
さらに、傾斜計の出力信号をそのまま用いた比較例４と、傾斜計の出力信号をローパスフィルタにかけたものを用いた比較例５の各処理結果を、真値と共に図１５に示すと共に、各結果の誤差を図１６に示す。図１５中、濃灰色破線は比較例４による出力値、実線は比較例５による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図１６中、破線は比較例４の場合の誤差角度、実線は比較例５の場合の誤差角度をそれぞれ示している。
【０１２９】
また、角速度センサの出力信号を球面積分したものを用いた比較例６と、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いた比較例７の各処理結果を、真値と共に図１７に示すと共に、各結果の誤差を図１８に示す。図１７中、濃灰色破線は比較例６による出力値、実線は比較例７による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図１８中、破線は比較例６の場合の誤差角度、実線は比較例７の場合の誤差角度をそれぞれ示している。
【０１３０】
図１２に示すように、逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例３の場合に比べ、真値に近く、より精度の高い推定が行えていることがわかる。また、図１５に示すように、比較例４の傾斜計のみを用いた結果や、比較例５の傾斜計出力信号をローパスフィルタにかけた結果によれば、遅れが発生しており、ローパスフィルタによってさらに遅れが生じ、精度も低下していることがわかる。
【０１３１】
また、図１７に示すように、比較例６の角速度センサ出力信号を球面積分した場合の結果によれば、球面積分により早い時間ではよい精度が得られているものの、時間の経過と共に真値からの誤差が大きくなっている。加えて、比較例７の球面積分し更にハイパスフィルタにかけた場合の結果によれば、ドリフトの補償は行えているものの、低周波数成分を除去していることで、精度が大きく悪化している。
【０１３２】
ここで、前記ｑ1(t) 、ｑ2(t) の各入力に対する前記実施例２、及び比較例３ないし７の各結果における最大誤差を比較したものを図１９にそれぞれ示す。この図１９中では、（ａ）比較例６、（ｂ）比較例７、（ｃ）比較例４、（ｄ）比較例５、（ｅ）比較例３、（ｆ）実施例２、におけるそれぞれの最大誤差角度を示している。これらによれば、実施例２における誤差は、各比較例のそれに比べて小さくなっている。
【０１３３】
これらの結果から、本発明の姿勢推定装置における相補フィルタは、複雑な動きに対しても有効に推定が行えることがわかる。
【符号の説明】
【０１３４】
１ 姿勢推定装置
１０ 角速度センサ
２０ 傾斜計
３０ 相補フィルタ部
３１ 第一補償部
３２ 第一フィルタ
３３ 第一姿勢変換部
３４ 第二補償部
３５ 第二フィルタ
３６ 第二姿勢変換部
３７ 加算器
【技術分野】
【０００１】
本発明は、複数の慣性センサ出力から相補フィルタを用いて移動体の姿勢を推定する姿勢推定装置に関する。
【背景技術】
【０００２】
歩行ロボットなど、運動の空間領域も周波数領域も広範囲な移動体の制御においては、信頼性の高い移動体の姿勢推定、すなわち、移動体の三次元における傾斜状態の推定が必要である。
姿勢推定には、通常、角速度センサや傾斜計、加速度計などのいわゆる慣性センサが用いられる。こうした慣性センサは、その種類ごとに信頼できる周波数領域がそれぞれ異なっている。
【０００３】
例えば、角速度センサは、動的な角度変化を角速度として検出でき、この角速度の出力を積分することで、姿勢を推定できるが、積分による定常誤差の拡大が問題となるなど、低い周波数領域での信頼性が下がっている。一方、傾斜計は、準静的な状態で傾斜角を検出できるが、動的な状態では、動きの影響による検出までの時間遅れが無視できなくなるなど、高い周波数領域での信頼性が下がるという問題を有している。
【０００４】
また、加速度計は、静的な状態では重力加速度から重力方向を検出可能であるものの、運動中は移動体自体の加速度の影響を受け、正確な検出が行えないという問題がある。
こうしたことから、一種類のセンサのみによる姿勢推定は難しく、異なる種類のセンサを複数組み合わせて用いることが必要になる。
【０００５】
このような異種センサを複合的に利用する技術として、カルマンフィルタを用いた方法が多くとられている。ただし、カルマンフィルタでは、時間領域において、誤差の含まれるセンサ出力の時間的変動傾向からセンサ出力に対する補正量を取得して、移動体の姿勢を高い信頼性で推定可能とするフィルタ設計がなされるものの、このフィルタ設計に係る各パラメータを設定するのが困難である。
【０００６】
この他、周波数領域で複数のセンサ出力を重ね合せる相補フィルタも提案されている。これは、各々のセンサの周波数特性に基づいて、信頼性の高い信号を抽出し、重ね合せるものであり、各センサの信頼できる周波数領域がある程度分かっていれば周波数領域設計は比較的容易である。
【０００７】
例えば、傾斜計のような低い周波数領域が信頼できるセンサにはローパスフィルタを適用し、逆に角速度センサのような高い周波数領域が信頼できるセンサにハイパスフィルタを適用して、フィルタ出力を組み合わせることで推定精度を向上させることができる。
【０００８】
こうした相補フィルタを、移動体の姿勢推定に利用する手法は、従来から種々提案されており、その一例として、特開平５−１９６４７５号公報や特開平５−２２３５８７号公報に開示されるものがある。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開平５−１９６４７５号公報
【特許文献２】特開平５−２２３５８７号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
従来における移動体の姿勢推定に係る相補フィルタは、前記各特許文献に示されるような構成となっているが、姿勢推定に用いるそれぞれのセンサの有効周波数領域は相補的であるとは限らないため、センサ出力を相補フィルタにそのまま入力して用いる場合、いずれのセンサにおいても情報の信頼性が低い周波数領域が存在すると、その周波数領域において推定精度が低下してしまうという問題があった。
例えば、角速度センサと傾斜計を姿勢推定のためのセンサとして用いる場合、傾斜計の有効な周波数領域が低いため、角速度センサとの組み合わせでは推定精度の点で問題になる。
【００１１】
こうした点から、相補フィルタにおいて、センサの動特性を補償し、有効周波数領域を拡大することが考えられる。例えば、相補フィルタに各センサの逆モデルを導入して、各センサの動特性を補償するという考え方があり、センサ出力を真のセンサ入力に近付けることができると共に、センサの有効周波数領域の拡大も図れると考えられている。しかしながら、センサ特性は非線形であり、逆モデルの伝達関数は不安定で非プロパーになりやすく、特に角速度センサの場合、その逆モデルは複雑な積分要素を含むものとなるなど、演算処理による姿勢推定を行うことは極めて困難であるという課題を有していた。
【００１２】
本発明は前記課題を解消するためになされたもので、異なる複数のセンサの各出力に対し、各センサの線形動特性を同定した伝達関数に対する逆伝達関数を用いて補償すると共に、この逆伝達関数に対応して設計されたフィルタを施した上で、最終的に姿勢変換を行い姿勢を推定するようにして、相補フィルタにおける姿勢推定を適切且つ高精度に行える姿勢推定装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本発明に係る姿勢推定装置は、移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相補フィルタ部が、前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えるものである。
【００１４】
このように本発明によれば、角速度センサの出力と傾斜計の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることにより、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実に演算処理が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に積分演算子が含まれる場合でも、ハイパスフィルタである第一フィルタにおける微分演算子と相殺されることとなり、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。
【００１５】
また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、
【００１６】
【数１】
【００１７】
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、
【００１８】
【数２】
【００１９】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2＋・・・＋ａmｓm （ｍ：任意の自然数）
で表されるものである。
【００２０】
このように本発明によれば、角速度センサの動特性の伝達関数を、その逆伝達関数と共に安定なものとし、且つ、傾斜計の動特性の伝達関数を、その逆伝達関数と共に安定なものとして、各センサの逆伝達関数とフィルタの伝達関数との積を安定且つプロパーな伝達関数とすることにより、センサ出力の補償とフィルタリングに係る演算処理が無理なく速やかに実行できることとなり、相補フィルタによる姿勢推定を高い精度で適切に進められる。
【００２１】
また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、
【００２２】
【数３】
【００２３】
で表され、前記傾斜計の伝達関数が、下式、
【００２４】
【数４】
【００２５】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表されるものである。
【００２６】
このように本発明によれば、角速度センサの動特性の伝達関数における各要素の分母多項式を一次式とすると共に、傾斜計の動特性の伝達関数における分母多項式を二次式として、姿勢推定に対応した適切な次数とすることにより、各センサの動特性の近似を十分高い精度としつつ、伝達関数及びその逆伝達関数を簡略なものとして、センサ出力の補償とフィルタリングに係る演算処理を高速化でき、相補フィルタによる姿勢推定を、十分な精度を確保しながらより高速に実行でき、推定値を姿勢制御等に適切に活用可能となる。
【図面の簡単な説明】
【００２７】
【図１】本発明の一実施形態に係る姿勢推定装置のブロック線図である。
【図２】本発明の一実施形態に係る姿勢推定装置で用いるオイラー角による姿勢とワールド座標系との関係の説明図である。
【図３】本発明に係る姿勢推定装置の実施例１によるオイラー角β及びγの各推定結果、真の入力値、及び比較例１による推定結果のグラフである。
【図４】本発明に係る姿勢推定装置に対する比較例２によるオイラー角β及びγの各推定結果、及び真の入力値のグラフである。
【図５】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その１）である。
【図６】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その２）である。
【図７】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その３）である。
【図８】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その４）である。
【図９】本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その５）である。
【図１０】本発明に係る姿勢推定装置の傾斜計における各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その１）である。
【図１１】本発明に係る姿勢推定装置の傾斜計における各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ（その２）である。
【図１２】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、真の入力値、及び比較例３による推定結果のグラフである。
【図１３】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、真の入力値、及び比較例３による推定結果の、５秒から１０秒の範囲を拡大したグラフである。
【図１４】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、及び比較例３による推定結果の、真の入力値に対する推定誤差のグラフである。
【図１５】本発明に係る姿勢推定装置に対する比較例４及び比較例５による１軸目及び２軸目の各入力の各推定結果、及び真の入力値のグラフである。
【図１６】本発明に係る姿勢推定装置に対する比較例４及び比較例５による１軸目及び２軸目の各入力の各推定結果の、真の入力値に対する推定誤差のグラフである。
【図１７】本発明に係る姿勢推定装置に対する比較例６及び比較例７による１軸目及び２軸目の各入力の各推定結果、及び真の入力値のグラフである。
【図１８】本発明に係る姿勢推定装置に対する比較例６及び比較例７による１軸目及び２軸目の各入力の各推定結果の、真の入力値に対する推定誤差のグラフである。
【図１９】本発明に係る姿勢推定装置の実施例２による１軸目及び２軸目の各入力の各推定結果、及び比較例３ないし７による各推定結果の、真の入力値に対する最大誤差の比較説明図である。
【発明を実施するための形態】
【００２８】
以下、本発明の一実施形態に係る姿勢推定装置を前記図１及び図２に基づいて説明する。
前記各図において、本実施形態に係る姿勢推定装置１は、移動体の動的な角度変化を検出する角速度センサ１０と、移動体の傾斜角を検出する傾斜計２０と、角速度センサ１０の出力と傾斜計２０の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部３０とを備える構成である。
【００２９】
前記角速度センサ１０は、レートジャイロ等の公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、角速度センサ１０の伝達関数をあらかじめ同定しておく必要がある。
【００３０】
この角速度センサ１０の動特性に係る伝達関数の同定は、公知の手法、例えば、座標系の各軸とセンサ出力の組合せごとに周波数応答を測定する、すなわち、角速度センサに対する姿勢変動の正弦波入力に基づいたセンサ出力波形を、センサを適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。
【００３１】
前記傾斜計２０は、加速度計や液面傾斜計などの公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、傾斜計２０についても、その伝達関数をあらかじめ同定しておく必要がある。
【００３２】
この傾斜計２０の動特性に係る伝達関数の同定は、公知の手法、例えば、傾斜計に対する姿勢変動の正弦波入力に基づいた傾斜計出力波形を、傾斜計を適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。
【００３３】
前記相補フィルタ部３０は、周波数領域で複数のセンサ出力を相補的に組合わせて姿勢推定を実行するものである。
【００３４】
この相補フィルタ部３０は、詳細には、前記角速度センサ１０の動特性の逆伝達関数に基づいて、角速度センサ１０の動特性を補償する第一補償部３１と、この第一補償部３１からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングする第一フィルタ３２と、この第一フィルタ３２の出力をオイラー角へ変換する第一姿勢変換部３３と、前記傾斜計２０の動特性の逆伝達関数に基づいて、傾斜計２０の動特性を補償する第二補償部３４と、この第二補償部３４からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングする第二フィルタ３５と、この第二フィルタ３５の出力をオイラー角へ変換する第二姿勢変換部３６と、前記第一姿勢変換部３３の出力と第二姿勢変換部３６の出力を加算する加算器３７とを備える構成である。
【００３５】
前記第一補償部３１は、あらかじめ角速度センサ１０の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、角速度センサ１０の動特性を補償し、角速度センサ１０の有効周波数領域を拡大するものである。
【００３６】
前記第一フィルタ３２は、第一補償部３１における逆伝達関数の次数より大きいフィルタ次数とされて、第一補償部３１からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングするハイパスフィルタである。この第一フィルタ３２のフィルタ特性は、第一補償部３１での補償後の角速度センサ出力が信頼できる周波数領域に合わせて設計される。
【００３７】
前記第一姿勢変換部３３は、第一フィルタ３２の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器３７へ出力するものである。
【００３８】
前記第二補償部３４は、あらかじめ傾斜計２０の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、傾斜計２０の動特性を補償し、傾斜計２０の有効周波数領域を拡大するものである。
【００３９】
前記第二フィルタ３５は、第二補償部３４における逆伝達関数の次数より大きいフィルタ次数とされて、第二補償部３４からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングするローパスフィルタである。この第二フィルタ３５のフィルタ特性は、第二補償部３４での補償後の傾斜計出力が信頼できる周波数領域に合わせて設計されるが、前記第一フィルタ３２の伝達関数をＦ1（ｓ）、第二フィルタ３５の伝達関数をＦ2（ｓ）とすると、相補性条件から、Ｆ1（ｓ）＋Ｆ2（ｓ）＝１となるよう設計される必要がある。
【００４０】
前記第二姿勢変換部３６は、第二フィルタ３５の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器３７へ出力するものである。
【００４１】
ここで、前記第一姿勢変換部３３と第二姿勢変換部３６で利用するオイラー角による姿勢表現について説明する。
【００４２】
オイラー角で表される、センサユニットが取付けられた移動体の姿勢ηは、鉛直軸以外の軸まわりの回転角β、γ（ｙ軸まわりβ回転、ｘ軸まわりγ回転）を用いて、
η＝［β，γ］T
と表され、また、回転行列を用いると、姿勢wＲbは、
【００４３】
【数５】
【００４４】
と表される。
この場合、センサユニット座標系における角速度センサで出力される角速度bωは、ワールド座標系における角速度wωを用いて、下式（１）のように表せる。
【００４５】
【数６】
【００４６】
式（１）より、角速度bωは、オイラー角の微分値
【００４７】
【数７】
【００４８】
を用いて表すと、下式（２）のようになる。
【００４９】
【数８】
【００５０】
続いて、傾斜計の出力とオイラー角による姿勢wＲbの関係を球面三角法より導出する。はじめに、ｙ軸を測定軸として持つ傾斜計の出力θ1を、図２における、ｘ軸とｙ軸がなす平面とwＲbｘとwＲbｚがなす平面の交線ＡＯとwＲbｘがなす角度∠ＡＯＣ＝θ1と仮定する。このとき、２つの球面三角形△ＡＢＣと△ＡＤＥを考える。ここで、^ｘ、^ｙ、^ｚはワールド座標系におけるｘ軸、ｙ軸、ｚ軸を表す単位ベクトルとする。球面三角形の角度Ａ、Ｂ、Ｃと、それぞれの頂点と中心を結ぶ線分がなす角度ａ、ｂ、ｃの間に次の関係が成り立つ。
ｓｉｎＡ／ｓｉｎａ＝ｓｉｎＢ／ｓｉｎｂ＝ｓｉｎＣ／ｓｉｎｃ （３）
式（３）から球面三角形△ＡＤＥにおいて次の関係式（４）が導かれる。
【００５１】
【数９】
【００５２】
同様に、球面三角形△ＡＢＣから、下式（５）が導かれる。
【００５３】
【数１０】
【００５４】
球面三角形においても対頂角は等しいので、式（４）と式（５）を整理すると次の関係式（６）を導くことができる。
【００５５】
【数１１】
【００５６】
同様に、ｘ軸を測定軸に持つ傾斜計の出力θ2では、次の関係式（７）が成り立つ。
【００５７】
【数１２】
【００５８】
これらから、入力を、求めたい軸まわりの回転角に変換するための関数Ｈ（・）は、それぞれ以下のように表される。
角速度センサ出力側については、入力Ｙ1＝［Ｙ11 Ｙ12］Tに対して、
【００５９】
【数１３】
【００６０】
となる。また、傾斜計出力側については、入力Ｙ2＝［Ｙ21 Ｙ22］Tに対して、
【００６１】
【数１４】
【００６２】
となる。
【００６３】
相補フィルタは、それぞれのセンサ出力を相補的に重み付けすることで推定を行うフィルタであり、線形な相補フィルタは、推定値Ｙ（ｓ）、ｉ番目のセンサの測定値Ｘi（ｓ）、ｉ番目のセンサ出力にかけるフィルタＦi（ｓ）を用いて、下式（１０）で表される。
【００６４】
【数１５】
【００６５】
これより、前記相補フィルタ部３０の、センサの逆モデルを用いて設計される相補フィルタで、姿勢の推定値ηestを表した場合、センサ出力を逆伝達関数に通して推定した入力を、求めたい軸まわりの回転角に変換するための関数Ｈ（・）を用いることで、
【００６６】
【数１６】
【００６７】
となる。ここで、
【００６８】
【数１７】
【００６９】
は同定したセンサの逆伝達関数である。また、相補フィルタ部３０は、図１に示すようなブロック線図で表すこともできる。
【００７０】
なお、第一フィルタ３２を、第一姿勢変換部３３より前段に配置し、また、第二フィルタ３５を、第二姿勢変換部３６より前段に配置しているが、その根拠について説明する。
【００７１】
姿勢をオイラー角で表す場合、従来手法では、角速度センサの出力をオイラー角の微分値に変換し、それらを積分して、オイラー角の推定値を求めた後に、フィルタをかけて出力する。また、傾斜計の出力もオイラー角に変換した後に、フィルタをかけて出力していた。逆モデルを導入した相補フィルタで姿勢推定を行う場合、姿勢の推定値ηestは、
【００７２】
【数１８】
【００７３】
と表される。
【００７４】
角速度センサの場合、出力の動特性を補償した角速度センサ入力ωを、式（２）における行列Ｐの擬似逆行列Ｐ+を用いて、下式（１３）によりオイラー角の微分値へ変換する。
【００７５】
【数１９】
【００７６】
また、傾斜計では、式（６）、（７）の関係を用いて、動特性を補償したセンサ入力θ＝［θ1 θ2］Tを、下式（１４）によりオイラー角へ変換する。
【００７７】
【数２０】
【００７８】
角速度センサ出力側では、入力を微分値に変換し、これを積分して、オイラー角の推定値を求めた後にフィルタをかけるようになっているが、前記式（１３）より、変換は行列の乗算であることから、フィルタと変換部の順序を入換えることができる。変換前にフィルタを施すようにした場合、角速度センサの逆伝達関数が積分演算子１／ｓを含んでいるのに対応して、フィルタが微分演算子ｓを含むように設計されていれば、積分演算子１／ｓと微分演算子ｓが相殺されることとなり、ドリフトの影響などを回避することが可能となる。
【００７９】
一方、傾斜計出力側では、入力をオイラー角へ変換した後にフィルタをかけるようになっているが、この変換後にフィルタを施す場合の伝達関数の積
【００８０】
【数２１】
【００８１】
と、入力にフィルタを施した後にオイラー角へ変換する場合の伝達関数の積
【００８２】
【数２２】
【００８３】
とは、そのテイラー展開を求めると、２次のオーダまで一致することとなり、フィルタと変換部の順序を入換えても、精度の点で十分許容できる。
【００８４】
これらにより、姿勢変換の前に周波数フィルタを施しても、推定値に大きな違いは生じないといえ、相補フィルタ部３０において第一フィルタ３２を第一姿勢変換部３３より前段に配置し、また、第二フィルタ３５を、第二姿勢変換部３６より前段に配置した構成を採用している。
【００８５】
さらに、角速度センサ１０と傾斜計２０の、それぞれの動特性の伝達関数同定、並びにフィルタ設計についてより詳細に説明する。
各センサの伝達関数Ｇi（ｓ）は、直交する３軸それぞれの軸まわりの回転に対するセンサの周波数応答を元に、各要素が安定且つプロパーとなる行列として同定する。本発明では、異なる軸間の干渉、すなわち各軸まわりの回転における伝達関数は近似的に線形分離可能であると仮定している。また、角速度センサには微分器があり、傾斜計には遅れ要素があると仮定して、軸とセンサ出力の組合せごとに周波数応答を測定し、線形の近似により伝達関数の同定を行う。
【００８６】
角速度センサの動特性の伝達関数は、
【００８７】
【数２３】
【００８８】
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表される。伝達関数の各要素が一次式の場合、特に、
【００８９】
【数２４】
【００９０】
で表される。
【００９１】
一方、傾斜計の動特性の伝達関数は、
【００９２】
【数２５】
【００９３】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表される。なお、伝達関数の各要素の次数は、取扱う事象の複雑さにより決るものであり、姿勢推定の場合、角速度センサのものは一次、傾斜計のものは二次として問題はない。
【００９４】
また、フィルタについては、逆伝達関数の次数よりフィルタ次数が大きくなるように設計する。具体的には、例えば、２軸傾斜計と１軸角速度センサを用いる場合、２軸傾斜計の逆伝達関数行列Ｇ-12の各要素が２次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数Ｆ2（ｓ）Ｇ-12（ｓ）をプロパーとするために、フィルタの伝達関数Ｆiを２次ローパスフィルタとして設計する。また、角速度センサの逆伝達関数Ｇ-11（ｓ）には積分演算子（１／ｓ）が含まれるものの、ハイパスフィルタＦ1（ｓ）に微分演算子（ｓ）を含むようにすれば、積分演算子（１／ｓ）と微分演算子（ｓ）が相殺されるため、結果的に積分演算を伴わない。
【００９５】
次に、本実施形態に係る姿勢推定装置による姿勢推定の過程について説明する。
角速度センサ１０から角速度bωが出力され、傾斜計２０から各軸についての傾斜角θ＝［θ1 θ2］Tが出力されると、角速度センサ出力側については、まず第一補償部３１で出力の動特性が補償され、次いで第一フィルタ３２でハイパスフィルタが施され、出力Ｙ1＝［Ｙ11 Ｙ12］Tが得られる。これが第一姿勢変換部３３に入力され、この入力Ｙ1＝［Ｙ11 Ｙ12］Tを前記式（８）によりオイラー角に変換する。
【００９６】
また、一方、傾斜計出力側では、第二補償部３４で出力の動特性が補償され、次いで第二フィルタ３５でローパスフィルタが施され、出力Ｙ2＝［Ｙ21 Ｙ22］Tが得られる。これが第二姿勢変換部３６に入力され、この入力Ｙ2＝［Ｙ21 Ｙ22］Tを前記式（９）によりオイラー角に変換する。
【００９７】
第一姿勢変換部３３で変換された出力と第二姿勢変換部３６で変換された出力が加算器３７で相補的に合成されると、姿勢の推定値ηest＝［βest γest］Tが求められる。
【００９８】
このように、本実施形態に係る姿勢推定装置は、角速度センサ１０の出力と傾斜計２０の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積について確実に数値演算が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に含まれる積分演算子が、第一フィルタ３２における微分演算子と相殺されて、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。
【実施例】
【００９９】
本発明に係る姿勢推定装置を用いて、角速度センサと傾斜計の出力から姿勢推定を行い、得られた結果について、比較例としての逆伝達関数を用いない相補フィルタを適用した場合等の姿勢推定結果と比較評価した。
【０１００】
姿勢推定の対象には２軸実験機を用い、ＳＳＳＪ製１軸角速度センサＣＲＳ０７−１１Ｓ、ＵＳＤｉｇｉｔａｌ製２軸傾斜計Ｘ３Ｑをそれぞれ取付けた。２軸実験機の１番目の関節はｙ軸周りに角度β、２番目の関節はｘ軸周りに角度γ、それぞれ回転する関節であるとする。
【０１０１】
測定に先立ち、角速度センサ、傾斜計についてセンサ動特性（伝達関数）の同定を行った。このセンサ動特性の同定では、異なる軸間の干渉は近似的に線形分離できると仮定し、軸とセンサ出力の組合せごとに正弦波を入力して周波数応答を測定し、最小二乗法により伝達関数を同定する。
【０１０２】
実際の周波数応答の測定から、それぞれのセンサの伝達関数を、角速度センサは下式（１５）、傾斜計は下式（１６）、（１７）のように同定した。
【０１０３】
【数２６】
【０１０４】
【数２７】
【０１０５】
この角速度センサと傾斜計の各伝達関数を用いて、２軸相補フィルタを設計した。実施例１として、この相補フィルタを用いると、姿勢の推定値ηestは下式（１８）で表される。
【０１０６】
【数２８】
【０１０７】
ここで、Ｘ1は角速度センサの出力、Ｘ2は２軸傾斜角センサの出力である。また、前記式（８）、式（９）で示したＨ1、Ｈ2をオイラー角の推定に用いている。なお、式（８）、（９）に用いている推定値は、実際は離散時間で使用するために、１ステップ前の推定値を用いている。
【０１０８】
この相補フィルタでは、２軸傾斜計の逆伝達関数行列Ｇ-12の各要素が２次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数Ｆ2(s)Ｇ-12 (s)をプロパーとするために、第二フィルタの伝達関数Ｆi（ｓ）を２次ローパスフィルタとして設計した。また、角速度センサの逆伝達関数Ｇ-11 (s) には積分器が含まれるが、ハイパスフィルタである第一フィルタＦ1(s) の持つ微分器と相殺されるため、結果的に積分演算を伴わないこととなる。
【０１０９】
また、比較例１として、逆伝達関数を用いない２軸相補フィルタを設計し、姿勢推定に用いるようにした。さらに、比較例２として、角速度センサのみを用いて姿勢推定を行うようにした。
【０１１０】
この本発明に係る姿勢推定装置を用いる場合（実施例１）と各比較例の場合について、実験機の２軸に正弦波入力をβ＝５７．３ｓｉｎｔ［ｄｅｇ］、γ＝２５．７ｓｉｎ３ｔ［ｄｅｇ］で与え、姿勢推定を行った。なお、いずれの場合も、第一フィルタ、第二フィルタの伝達関数Ｆiは同じものを用いている。
【０１１１】
実施例１における逆伝達関数を用いて設計した２軸相補フィルタと、逆伝達関数を用いない比較例１の相補フィルタでのβ、γの推定結果を、真値と共に図３に示す。この図３中、実線は実施例１による推定値、濃灰色破線は比較例１による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、角速度センサのみを用いた比較例２の推定結果を、真値と共に図４に示す。この図４中、実線は比較例２による推定値、薄灰色点線は真の入力値をそれぞれ示している。
【０１１２】
図３に示すように、実施例１の逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例１の場合に比べ、遅れが補償され、比較的よい推定が行えていることがわかる。また、図４に示すように、角速度センサのみを用いた比較例２の推定結果によれば、位相を補償することはできているものの、ドリフトが発生しており、時間経過とともに真値からの誤差が増大している。これに対し、逆伝達関数を用いた相補フィルタは、角速度センサのみの推定で発生していたドリフトの補償も行えている。これらから、本発明の姿勢推定装置における相補フィルタは２軸の推定において有効であるといえる。
【０１１３】
次に、実験機の複雑な動きに対して有効に推定が行われるかどうかについて評価した。
複雑な動きとして、実験機に対し、１軸目の入力ｑ1(t) 、２軸目の入力ｑ2(t) をそれぞれ与え、姿勢推定を行った。ｑ1(t) を下式（１９）、ｑ2(t)を下式（２０）に示す。また、サンプリング周期１［ｋＨｚ］、サンプリング時間２０［ｓ］としている。ｑ1(t)、ｑ2(t)において、パラメータａ1i、ｂ1i、ａ2i、ｂ2iは、振幅の合計が４０［ｄｅｇ］となるように、ランダムに入力した。また、測定周波数ｆ1i、ｆ2iをばらけさせるためにｉ番目の整数部分はｉ−１として決定し、小数部分は０．００から０．９９［Ｈｚ］までをランダムに入力している（表１参照）。
【０１１４】
【数２９】
【０１１５】
【数３０】
【０１１６】
【表１】
【０１１７】
前記同様、姿勢推定の対象には２軸実験機を用い、ＳＳＳＪ製１軸角速度センサＣＲＳ０７−１１Ｓ、ＵＳＤｉｇｉｔａｌ製２軸傾斜計Ｘ３Ｍ−２をそれぞれ取付けた。２軸実験機の１番目の関節はｙ軸周りに角度β、２番目の関節はｘ軸周りに角度γ、それぞれ回転する関節であるとする。
【０１１８】
測定に先立ち、角速度センサ、傾斜計についてセンサ動特性（伝達関数）の同定を行った。正弦波入力により、各軸について、角速度センサは図５ないし図９、傾斜計は図１０、図１１のような周波数応答が得られた。なお、前記図５ないし図１１の各図中で、灰色の十字の点は周波数応答の測定値、連続する線は最小二乗法による近似で求めた伝達関数を、それぞれ示している。
【０１１９】
これらより、それぞれのセンサの伝達関数を、角速度センサは下式（２１）、傾斜計は下式（２２）のように同定した。
【０１２０】
【数３１】
【０１２１】
【数３２】
【０１２２】
この角速度センサと傾斜計の各伝達関数を用いて、２軸相補フィルタを設計した。フィルタの遮断周波数は、傾斜計の周波数の半分になるように設定した。実施例２として、この相補フィルタを用いると、姿勢の推定値ηestは下式（２３）で表される。
【０１２３】
【数３３】
【０１２４】
また、比較例３として、逆伝達関数を用いない２軸相補フィルタを設計し、姿勢推定に用いるようにした。加えて、比較例４として、傾斜計の出力信号をそのまま用いて、また、比較例５として、傾斜計の出力信号をローパスフィルタにかけたものを用いて、また、比較例６として、角速度センサの出力信号を球面積分したものを用いて、また、比較例７として、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いて、それぞれ姿勢を求めるようにした。なお、比較例５のローパスフィルタ、比較例７のハイパスフィルタは下式（２４）、（２５）に示されるものとなっている。比較例５のローパスフィルタの遮断周波数は５［Ｈｚ］程度の値とし、また、比較例７のハイパスフィルタの遮断周波数は相補フィルタの場合と同様の値とする。
【０１２５】
【数３４】
【０１２６】
【数３５】
【０１２７】
前記ｑ1(t) 、ｑ2(t) の各入力を与える複雑な動きに対して、本発明に係る姿勢推定装置を用いる場合（実施例２）と各比較例の場合で、姿勢推定を行った結果を示す。
実施例２における逆伝達関数を用いて設計した２軸相補フィルタと、逆伝達関数を用いない比較例３の相補フィルタの推定結果を、真値と共に図１２に示す。この図１２中、実線は実施例２による推定値、濃灰色破線は比較例３による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、推定結果における５秒から１０秒の範囲の拡大図を図１３に、各結果の推定誤差を図１４にそれぞれ示す。図１３中の各線が示すものは図１２の場合と同様である。一方、図１４中、実線は実施例２の場合の誤差角度、破線は比較例３の場合の誤差角度をそれぞれ示している。
【０１２８】
さらに、傾斜計の出力信号をそのまま用いた比較例４と、傾斜計の出力信号をローパスフィルタにかけたものを用いた比較例５の各処理結果を、真値と共に図１５に示すと共に、各結果の誤差を図１６に示す。図１５中、濃灰色破線は比較例４による出力値、実線は比較例５による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図１６中、破線は比較例４の場合の誤差角度、実線は比較例５の場合の誤差角度をそれぞれ示している。
【０１２９】
また、角速度センサの出力信号を球面積分したものを用いた比較例６と、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いた比較例７の各処理結果を、真値と共に図１７に示すと共に、各結果の誤差を図１８に示す。図１７中、濃灰色破線は比較例６による出力値、実線は比較例７による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図１８中、破線は比較例６の場合の誤差角度、実線は比較例７の場合の誤差角度をそれぞれ示している。
【０１３０】
図１２に示すように、逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例３の場合に比べ、真値に近く、より精度の高い推定が行えていることがわかる。また、図１５に示すように、比較例４の傾斜計のみを用いた結果や、比較例５の傾斜計出力信号をローパスフィルタにかけた結果によれば、遅れが発生しており、ローパスフィルタによってさらに遅れが生じ、精度も低下していることがわかる。
【０１３１】
また、図１７に示すように、比較例６の角速度センサ出力信号を球面積分した場合の結果によれば、球面積分により早い時間ではよい精度が得られているものの、時間の経過と共に真値からの誤差が大きくなっている。加えて、比較例７の球面積分し更にハイパスフィルタにかけた場合の結果によれば、ドリフトの補償は行えているものの、低周波数成分を除去していることで、精度が大きく悪化している。
【０１３２】
ここで、前記ｑ1(t) 、ｑ2(t) の各入力に対する前記実施例２、及び比較例３ないし７の各結果における最大誤差を比較したものを図１９にそれぞれ示す。この図１９中では、（ａ）比較例６、（ｂ）比較例７、（ｃ）比較例４、（ｄ）比較例５、（ｅ）比較例３、（ｆ）実施例２、におけるそれぞれの最大誤差角度を示している。これらによれば、実施例２における誤差は、各比較例のそれに比べて小さくなっている。
【０１３３】
これらの結果から、本発明の姿勢推定装置における相補フィルタは、複雑な動きに対しても有効に推定が行えることがわかる。
【符号の説明】
【０１３４】
１ 姿勢推定装置
１０ 角速度センサ
２０ 傾斜計
３０ 相補フィルタ部
３１ 第一補償部
３２ 第一フィルタ
３３ 第一姿勢変換部
３４ 第二補償部
３５ 第二フィルタ
３６ 第二姿勢変換部
３７ 加算器
【特許請求の範囲】
【請求項１】
移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、
前記相補フィルタ部が、
前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、
当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、
当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、
前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、
当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、
当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、
前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えることを
特徴とする姿勢推定装置。
【請求項２】
前記請求項１に記載の姿勢推定装置において、
前記角速度センサの伝達関数が、下式、
【数１】
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表され、
前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、
当該傾斜計の伝達関数が、下式、
【数２】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2＋・・・＋ａmｓm （ｍ：任意の自然数）
で表されることを
特徴とする姿勢推定装置。
【請求項３】
前記請求項２に記載の姿勢推定装置において、
前記角速度センサの伝達関数が、下式、
【数３】
で表され、
前記傾斜計の伝達関数が、下式、
【数４】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表されることを
特徴とする姿勢推定装置。
【請求項１】
移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、
前記相補フィルタ部が、
前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、
当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域でフィルタリングを施すハイパスフィルタである第一フィルタと、
当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、
前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、
当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、
当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、
前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えることを
特徴とする姿勢推定装置。
【請求項２】
前記請求項１に記載の姿勢推定装置において、
前記角速度センサの伝達関数が、下式、
【数１】
ただし、Ｄ1（ｓ）＝１＋ａ11ｓ＋ａ12ｓ2＋・・・＋ａ1nｓn
Ｄ2（ｓ）＝１＋ａ21ｓ＋ａ22ｓ2＋・・・＋ａ2nｓn
Ｄ3（ｓ）＝１＋ａ31ｓ＋ａ32ｓ2＋・・・＋ａ3nｓn （ｎ：任意の自然数）
で表され、
前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、
当該傾斜計の伝達関数が、下式、
【数２】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2＋・・・＋ａmｓm （ｍ：任意の自然数）
で表されることを
特徴とする姿勢推定装置。
【請求項３】
前記請求項２に記載の姿勢推定装置において、
前記角速度センサの伝達関数が、下式、
【数３】
で表され、
前記傾斜計の伝達関数が、下式、
【数４】
ただし、Ｄ（ｓ）＝１＋ａ1ｓ＋ａ2ｓ2
で表されることを
特徴とする姿勢推定装置。
【図１】
【図２】
【図１９】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図２】
【図１９】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【公開番号】特開２０１２−１９８０５７（Ｐ２０１２−１９８０５７Ａ）
【公開日】平成２４年１０月１８日（２０１２．１０．１８）
【国際特許分類】
【出願番号】特願２０１１−６１１４６（Ｐ２０１１−６１１４６）
【出願日】平成２３年３月１８日（２０１１．３．１８）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り 平成２１年５月２６日 日本熱物性学会発行の「第４６回日本伝熱シンポジウム講演論文集」に発表
【出願人】（５０４１４５３４２）国立大学法人九州大学 (960)
【Ｆターム（参考）】
【公開日】平成２４年１０月１８日（２０１２．１０．１８）
【国際特許分類】
【出願日】平成２３年３月１８日（２０１１．３．１８）
【新規性喪失の例外の表示】特許法第３０条第１項適用申請有り 平成２１年５月２６日 日本熱物性学会発行の「第４６回日本伝熱シンポジウム講演論文集」に発表
【出願人】（５０４１４５３４２）国立大学法人九州大学 (960)
【Ｆターム（参考）】
[ Back to top ]