図面 (/)

この項目の情報は公開日時点(2012年10月18日)のものです。
また、この項目は機械的に抽出しているため、正しく解析できていない場合があります

図面 (20)

課題

異なる複数のセンサの各出力に対し、各センサの線形動特性逆伝達関数を用いて補償すると共に、この逆伝達関数に対応したフィルタを施し、最終的に姿勢変換を行うようにして、姿勢推定を適切且つ高精度に行える姿勢推定装置を提供する。

解決手段

角速度センサ10と傾斜計20の各逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似線形特性として伝達関数を同定し、その逆伝達関数を用いて特性を補償すると共に、逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実な演算処理で姿勢推定が実行でき、複数のセンサを用いる相補フィルタの手法で精度のよい姿勢推定を行える。

概要

背景

歩行ロボットなど、運動の空間領域も周波数領域も広範囲な移動体の制御においては、信頼性の高い移動体の姿勢推定、すなわち、移動体の三次元における傾斜状態推定が必要である。
姿勢推定には、通常、角速度センサ傾斜計加速度計などのいわゆる慣性センサが用いられる。こうした慣性センサは、その種類ごとに信頼できる周波数領域がそれぞれ異なっている。

例えば、角速度センサは、動的な角度変化を角速度として検出でき、この角速度の出力を積分することで、姿勢を推定できるが、積分による定常誤差の拡大が問題となるなど、低い周波数領域での信頼性が下がっている。一方、傾斜計は、準静的な状態で傾斜角を検出できるが、動的な状態では、動きの影響による検出までの時間遅れが無視できなくなるなど、高い周波数領域での信頼性が下がるという問題を有している。

また、加速度計は、静的な状態では重力加速度から重力方向を検出可能であるものの、運動中は移動体自体の加速度の影響を受け、正確な検出が行えないという問題がある。
こうしたことから、一種類のセンサのみによる姿勢推定は難しく、異なる種類のセンサを複数組み合わせて用いることが必要になる。

このような異種センサ複合的に利用する技術として、カルマンフィルタを用いた方法が多くとられている。ただし、カルマンフィルタでは、時間領域において、誤差の含まれるセンサ出力時間的変動傾向からセンサ出力に対する補正量を取得して、移動体の姿勢を高い信頼性で推定可能とするフィルタ設計がなされるものの、このフィルタ設計に係る各パラメータを設定するのが困難である。

この他、周波数領域で複数のセンサ出力を重ね合せる相補フィルタも提案されている。これは、各々のセンサの周波数特性に基づいて、信頼性の高い信号を抽出し、重ね合せるものであり、各センサの信頼できる周波数領域がある程度分かっていれば周波数領域設計は比較的容易である。

例えば、傾斜計のような低い周波数領域が信頼できるセンサにはローパスフィルタを適用し、逆に角速度センサのような高い周波数領域が信頼できるセンサにハイパスフィルタを適用して、フィルタ出力を組み合わせることで推定精度を向上させることができる。

こうした相補フィルタを、移動体の姿勢推定に利用する手法は、従来から種々提案されており、その一例として、特開平5−196475号公報や特開平5−223587号公報に開示されるものがある。

概要

異なる複数のセンサの各出力に対し、各センサの線形動特性逆伝達関数を用いて補償すると共に、この逆伝達関数に対応したフィルタを施し、最終的に姿勢変換を行うようにして、姿勢推定を適切且つ高精度に行える姿勢推定装置を提供する。角速度センサ10と傾斜計20の各逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似線形特性として伝達関数を同定し、その逆伝達関数を用いて特性を補償すると共に、逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大をりつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実な演算処理で姿勢推定が実行でき、複数のセンサを用いる相補フィルタの手法で精度のよい姿勢推定を行える。

目的

本発明は前記課題を解消するためになされたもので、異なる複数のセンサの各出力に対し、各センサの線形動特性を同定した伝達関数に対する逆伝達関数を用いて補償すると共に、この逆伝達関数に対応して設計されたフィルタを施した上で、最終的に姿勢変換を行い姿勢を推定するようにして、相補フィルタにおける姿勢推定を適切且つ高精度に行える姿勢推定装置を提供する

効果

実績

技術文献被引用数
0件
牽制数
0件

この技術が所属する分野

ライセンス契約や譲渡などの可能性がある特許掲載中! 開放特許随時追加・更新中 詳しくはこちら

請求項1

動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相フィルタ部が、前記角速度センサの線形近似された動特性伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域フィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えることを特徴とする姿勢推定装置。

請求項2

前記請求項1に記載の姿勢推定装置において、前記角速度センサの伝達関数が、下式、ただし、D1(s)=1+a11s+a12s2+・・・+a1nsnD2(s)=1+a21s+a22s2+・・・+a2nsnD3(s)=1+a31s+a32s2+・・・+a3nsn(n:任意の自然数)で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、ただし、D(s)=1+a1s+a2s2+・・・+amsm(m:任意の自然数)で表されることを特徴とする姿勢推定装置。

請求項3

前記請求項2に記載の姿勢推定装置において、前記角速度センサの伝達関数が、下式、で表され、前記傾斜計の伝達関数が、下式、ただし、D(s)=1+a1s+a2s2で表されることを特徴とする姿勢推定装置。

技術分野

0001

本発明は、複数の慣性センサ出力から相補フィルタを用いて移動体姿勢推定する姿勢推定装置に関する。

背景技術

0002

歩行ロボットなど、運動の空間領域も周波数領域も広範囲な移動体の制御においては、信頼性の高い移動体の姿勢推定、すなわち、移動体の三次元における傾斜状態の推定が必要である。
姿勢推定には、通常、角速度センサ傾斜計加速度計などのいわゆる慣性センサが用いられる。こうした慣性センサは、その種類ごとに信頼できる周波数領域がそれぞれ異なっている。

0003

例えば、角速度センサは、動的な角度変化を角速度として検出でき、この角速度の出力を積分することで、姿勢を推定できるが、積分による定常誤差の拡大が問題となるなど、低い周波数領域での信頼性が下がっている。一方、傾斜計は、準静的な状態で傾斜角を検出できるが、動的な状態では、動きの影響による検出までの時間遅れが無視できなくなるなど、高い周波数領域での信頼性が下がるという問題を有している。

0004

また、加速度計は、静的な状態では重力加速度から重力方向を検出可能であるものの、運動中は移動体自体の加速度の影響を受け、正確な検出が行えないという問題がある。
こうしたことから、一種類のセンサのみによる姿勢推定は難しく、異なる種類のセンサを複数組み合わせて用いることが必要になる。

0005

このような異種センサ複合的に利用する技術として、カルマンフィルタを用いた方法が多くとられている。ただし、カルマンフィルタでは、時間領域において、誤差の含まれるセンサ出力時間的変動傾向からセンサ出力に対する補正量を取得して、移動体の姿勢を高い信頼性で推定可能とするフィルタ設計がなされるものの、このフィルタ設計に係る各パラメータを設定するのが困難である。

0006

この他、周波数領域で複数のセンサ出力を重ね合せる相補フィルタも提案されている。これは、各々のセンサの周波数特性に基づいて、信頼性の高い信号を抽出し、重ね合せるものであり、各センサの信頼できる周波数領域がある程度分かっていれば周波数領域設計は比較的容易である。

0007

例えば、傾斜計のような低い周波数領域が信頼できるセンサにはローパスフィルタを適用し、逆に角速度センサのような高い周波数領域が信頼できるセンサにハイパスフィルタを適用して、フィルタ出力を組み合わせることで推定精度を向上させることができる。

0008

こうした相補フィルタを、移動体の姿勢推定に利用する手法は、従来から種々提案されており、その一例として、特開平5−196475号公報や特開平5−223587号公報に開示されるものがある。

先行技術

0009

特開平5−196475号公報
特開平5−223587号公報

発明が解決しようとする課題

0010

従来における移動体の姿勢推定に係る相補フィルタは、前記各特許文献に示されるような構成となっているが、姿勢推定に用いるそれぞれのセンサの有効周波数領域は相補的であるとは限らないため、センサ出力を相補フィルタにそのまま入力して用いる場合、いずれのセンサにおいても情報の信頼性が低い周波数領域が存在すると、その周波数領域において推定精度が低下してしまうという問題があった。
例えば、角速度センサと傾斜計を姿勢推定のためのセンサとして用いる場合、傾斜計の有効な周波数領域が低いため、角速度センサとの組み合わせでは推定精度の点で問題になる。

0011

こうした点から、相補フィルタにおいて、センサの動特性補償し、有効周波数領域を拡大することが考えられる。例えば、相補フィルタに各センサの逆モデルを導入して、各センサの動特性を補償するという考え方があり、センサ出力を真のセンサ入力に近付けることができると共に、センサの有効周波数領域の拡大も図れると考えられている。しかしながら、センサ特性非線形であり、逆モデルの伝達関数は不安定で非プロパーになりやすく、特に角速度センサの場合、その逆モデルは複雑な積分要素を含むものとなるなど、演算処理による姿勢推定を行うことは極めて困難であるという課題を有していた。

0012

本発明は前記課題を解消するためになされたもので、異なる複数のセンサの各出力に対し、各センサの線形動特性を同定した伝達関数に対する逆伝達関数を用いて補償すると共に、この逆伝達関数に対応して設計されたフィルタを施した上で、最終的に姿勢変換を行い姿勢を推定するようにして、相補フィルタにおける姿勢推定を適切且つ高精度に行える姿勢推定装置を提供することを目的とする。

課題を解決するための手段

0013

本発明に係る姿勢推定装置は、移動体の動的な角度変化を検出する角速度センサと、移動体の傾斜角を検出する傾斜計と、角速度センサの出力と傾斜計の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部とを備える姿勢推定装置において、前記相補フィルタ部が、前記角速度センサの線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、角速度センサの出力について動特性を補償する第一補償部と、当該第一補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第一補償部からの出力に対し、相補性条件に基づく第一の周波数帯域フィルタリングを施すハイパスフィルタである第一フィルタと、当該第一フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第一姿勢変換部と、前記傾斜計の線形に近似された動特性の伝達関数をあらかじめ同定して、当該伝達関数より求められる逆伝達関数に基づいて、傾斜計の出力について動特性を補償する第二補償部と、当該第二補償部における前記逆伝達関数の次数より大きいフィルタ次数とされ、第二補償部からの出力に対し、相補性条件に基づく第二の周波数帯域でフィルタリングを施すローパスフィルタである第二フィルタと、当該第二フィルタの出力を、移動体の姿勢を表現するオイラー角へ変換する第二姿勢変換部と、前記第一姿勢変換部の出力と第二姿勢変換部の出力を相補的に加算合成し、オイラー角により表される姿勢の推定値を得る加算器とを備えるものである。

0014

このように本発明によれば、角速度センサの出力と傾斜計の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることにより、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積についてはプロパーな伝達関数とすることができ、確実に演算処理が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に積分演算子が含まれる場合でも、ハイパスフィルタである第一フィルタにおける微分演算子と相殺されることとなり、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。

0015

また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式

0016

0017

ただし、D1(s)=1+a11s+a12s2+・・・+a1nsn
D2(s)=1+a21s+a22s2+・・・+a2nsn
D3(s)=1+a31s+a32s2+・・・+a3nsn (n:任意の自然数
で表され、前記傾斜計が、鉛直軸でない二つの軸を測定軸とするものとされ、当該傾斜計の伝達関数が、下式、

0018

0019

ただし、D(s)=1+a1s+a2s2+・・・+amsm (m:任意の自然数)
で表されるものである。

0020

このように本発明によれば、角速度センサの動特性の伝達関数を、その逆伝達関数と共に安定なものとし、且つ、傾斜計の動特性の伝達関数を、その逆伝達関数と共に安定なものとして、各センサの逆伝達関数とフィルタの伝達関数との積を安定且つプロパーな伝達関数とすることにより、センサ出力の補償とフィルタリングに係る演算処理が無理なく速やかに実行できることとなり、相補フィルタによる姿勢推定を高い精度で適切に進められる。

0021

また、本発明に係る姿勢推定装置は必要に応じて、前記角速度センサの伝達関数が、下式、

0022

0023

で表され、前記傾斜計の伝達関数が、下式、

0024

0025

ただし、D(s)=1+a1s+a2s2
で表されるものである。

0026

このように本発明によれば、角速度センサの動特性の伝達関数における各要素の分母多項式一次式とすると共に、傾斜計の動特性の伝達関数における分母多項式を二次式として、姿勢推定に対応した適切な次数とすることにより、各センサの動特性の近似を十分高い精度としつつ、伝達関数及びその逆伝達関数を簡略なものとして、センサ出力の補償とフィルタリングに係る演算処理を高速化でき、相補フィルタによる姿勢推定を、十分な精度を確保しながらより高速に実行でき、推定値を姿勢制御等に適切に活用可能となる。

図面の簡単な説明

0027

本発明の一実施形態に係る姿勢推定装置のブロック線図である。
本発明の一実施形態に係る姿勢推定装置で用いるオイラー角による姿勢とワールド座標系との関係の説明図である。
本発明に係る姿勢推定装置の実施例1によるオイラー角β及びγの各推定結果、真の入力値、及び比較例1による推定結果のグラフである。
本発明に係る姿勢推定装置に対する比較例2によるオイラー角β及びγの各推定結果、及び真の入力値のグラフである。
本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅位相周波数応答のグラフ(その1)である。
本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ(その2)である。
本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ(その3)である。
本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ(その4)である。
本発明に係る姿勢推定装置の角速度センサにおける各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ(その5)である。
本発明に係る姿勢推定装置の傾斜計における各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ(その1)である。
本発明に係る姿勢推定装置の傾斜計における各軸についての正弦波入力に対する振幅と位相の周波数応答のグラフ(その2)である。
本発明に係る姿勢推定装置の実施例2による1軸目及び2軸目の各入力の各推定結果、真の入力値、及び比較例3による推定結果のグラフである。
本発明に係る姿勢推定装置の実施例2による1軸目及び2軸目の各入力の各推定結果、真の入力値、及び比較例3による推定結果の、5秒から10秒の範囲を拡大したグラフである。
本発明に係る姿勢推定装置の実施例2による1軸目及び2軸目の各入力の各推定結果、及び比較例3による推定結果の、真の入力値に対する推定誤差のグラフである。
本発明に係る姿勢推定装置に対する比較例4及び比較例5による1軸目及び2軸目の各入力の各推定結果、及び真の入力値のグラフである。
本発明に係る姿勢推定装置に対する比較例4及び比較例5による1軸目及び2軸目の各入力の各推定結果の、真の入力値に対する推定誤差のグラフである。
本発明に係る姿勢推定装置に対する比較例6及び比較例7による1軸目及び2軸目の各入力の各推定結果、及び真の入力値のグラフである。
本発明に係る姿勢推定装置に対する比較例6及び比較例7による1軸目及び2軸目の各入力の各推定結果の、真の入力値に対する推定誤差のグラフである。
本発明に係る姿勢推定装置の実施例2による1軸目及び2軸目の各入力の各推定結果、及び比較例3ないし7による各推定結果の、真の入力値に対する最大誤差の比較説明図である。

0028

以下、本発明の一実施形態に係る姿勢推定装置を前記図1及び図2に基づいて説明する。
前記各図において、本実施形態に係る姿勢推定装置1は、移動体の動的な角度変化を検出する角速度センサ10と、移動体の傾斜角を検出する傾斜計20と、角速度センサ10の出力と傾斜計20の出力とを相補的に用いて、移動体の姿勢推定出力を得る相補フィルタ部30とを備える構成である。

0029

前記角速度センサ10は、レートジャイロ等の公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、角速度センサ10の伝達関数をあらかじめ同定しておく必要がある。

0030

この角速度センサ10の動特性に係る伝達関数の同定は、公知の手法、例えば、座標系の各軸とセンサ出力の組合せごとに周波数応答を測定する、すなわち、角速度センサに対する姿勢変動の正弦波入力に基づいたセンサ出力波形を、センサを適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。

0031

前記傾斜計20は、加速度計や液面傾斜計などの公知のセンサであり、詳細な説明を省略する。相補フィルタ設計にあたっては、傾斜計20についても、その伝達関数をあらかじめ同定しておく必要がある。

0032

この傾斜計20の動特性に係る伝達関数の同定は、公知の手法、例えば、傾斜計に対する姿勢変動の正弦波入力に基づいた傾斜計出力波形を、傾斜計を適用する周波数領域で複数の代表的な周波数について求め、得られた各出力波形から位相遅れ及びゲインを同定して、各値を周波数領域においてプロットし、さらに、各周波数における位相遅れやゲインの値の分布から、最小二乗法による近似で伝達関数を同定する、といった過程で行われる。

0033

前記相補フィルタ部30は、周波数領域で複数のセンサ出力を相補的に組合わせて姿勢推定を実行するものである。

0034

この相補フィルタ部30は、詳細には、前記角速度センサ10の動特性の逆伝達関数に基づいて、角速度センサ10の動特性を補償する第一補償部31と、この第一補償部31からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングする第一フィルタ32と、この第一フィルタ32の出力をオイラー角へ変換する第一姿勢変換部33と、前記傾斜計20の動特性の逆伝達関数に基づいて、傾斜計20の動特性を補償する第二補償部34と、この第二補償部34からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングする第二フィルタ35と、この第二フィルタ35の出力をオイラー角へ変換する第二姿勢変換部36と、前記第一姿勢変換部33の出力と第二姿勢変換部36の出力を加算する加算器37とを備える構成である。

0035

前記第一補償部31は、あらかじめ角速度センサ10の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、角速度センサ10の動特性を補償し、角速度センサ10の有効周波数領域を拡大するものである。

0036

前記第一フィルタ32は、第一補償部31における逆伝達関数の次数より大きいフィルタ次数とされて、第一補償部31からの出力を相補性条件に基づく第一の周波数帯域でフィルタリングするハイパスフィルタである。この第一フィルタ32のフィルタ特性は、第一補償部31での補償後の角速度センサ出力が信頼できる周波数領域に合わせて設計される。

0037

前記第一姿勢変換部33は、第一フィルタ32の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器37へ出力するものである。

0038

前記第二補償部34は、あらかじめ傾斜計20の線形に近似された動特性の伝達関数を同定することで得られる逆伝達関数に基づいて、傾斜計20の動特性を補償し、傾斜計20の有効周波数領域を拡大するものである。

0039

前記第二フィルタ35は、第二補償部34における逆伝達関数の次数より大きいフィルタ次数とされて、第二補償部34からの出力を相補性条件に基づく第二の周波数帯域でフィルタリングするローパスフィルタである。この第二フィルタ35のフィルタ特性は、第二補償部34での補償後の傾斜計出力が信頼できる周波数領域に合わせて設計されるが、前記第一フィルタ32の伝達関数をF1(s)、第二フィルタ35の伝達関数をF2(s)とすると、相補性条件から、F1(s)+F2(s)=1となるよう設計される必要がある。

0040

前記第二姿勢変換部36は、第二フィルタ35の出力を入力され、移動体の姿勢を表現するオイラー角の推定値へ変換して、前記加算器37へ出力するものである。

0041

ここで、前記第一姿勢変換部33と第二姿勢変換部36で利用するオイラー角による姿勢表現について説明する。

0042

オイラー角で表される、センサユニット取付けられた移動体の姿勢ηは、鉛直軸以外の軸まわり回転角β、γ(y軸まわりβ回転、x軸まわりγ回転)を用いて、
η=[β,γ]T
と表され、また、回転行列を用いると、姿勢wRbは、

0043

0044

と表される。
この場合、センサユニット座標系における角速度センサで出力される角速度bωは、ワールド座標系における角速度wωを用いて、下式(1)のように表せる。

0045

0046

式(1)より、角速度bωは、オイラー角の微分値

0047

0048

を用いて表すと、下式(2)のようになる。

0049

0050

続いて、傾斜計の出力とオイラー角による姿勢wRbの関係を球面三角法より導出する。はじめに、y軸を測定軸として持つ傾斜計の出力θ1を、図2における、x軸とy軸がなす平面とwRbxとwRbzがなす平面の交線AOとwRbxがなす角度∠AOC=θ1と仮定する。このとき、2つの球面三角形△ABCと△ADEを考える。ここで、^x、^y、^zはワールド座標系におけるx軸、y軸、z軸を表す単位ベクトルとする。球面三角形の角度A、B、Cと、それぞれの頂点と中心を結ぶ線分がなす角度a、b、cの間に次の関係が成り立つ。
sinA/sina=sinB/sinb=sinC/sinc (3)
式(3)から球面三角形△ADEにおいて次の関係式(4)が導かれる。

0051

0052

同様に、球面三角形△ABCから、下式(5)が導かれる。

0053

0054

球面三角形においても対頂角は等しいので、式(4)と式(5)を整理すると次の関係式(6)を導くことができる。

0055

0056

同様に、x軸を測定軸に持つ傾斜計の出力θ2では、次の関係式(7)が成り立つ。

0057

0058

これらから、入力を、求めたい軸まわりの回転角に変換するための関数H(・)は、それぞれ以下のように表される。
角速度センサ出力側については、入力Y1=[Y11 Y12]Tに対して、

0059

0060

となる。また、傾斜計出力側については、入力Y2=[Y21 Y22]Tに対して、

0061

0062

となる。

0063

相補フィルタは、それぞれのセンサ出力を相補的に重み付けすることで推定を行うフィルタであり、線形な相補フィルタは、推定値Y(s)、i番目のセンサの測定値Xi(s)、i番目のセンサ出力にかけるフィルタFi(s)を用いて、下式(10)で表される。

0064

0065

これより、前記相補フィルタ部30の、センサの逆モデルを用いて設計される相補フィルタで、姿勢の推定値ηestを表した場合、センサ出力を逆伝達関数に通して推定した入力を、求めたい軸まわりの回転角に変換するための関数H(・)を用いることで、

0066

0067

となる。ここで、

0068

0069

は同定したセンサの逆伝達関数である。また、相補フィルタ部30は、図1に示すようなブロック線図で表すこともできる。

0070

なお、第一フィルタ32を、第一姿勢変換部33より前段に配置し、また、第二フィルタ35を、第二姿勢変換部36より前段に配置しているが、その根拠について説明する。

0071

姿勢をオイラー角で表す場合、従来手法では、角速度センサの出力をオイラー角の微分値に変換し、それらを積分して、オイラー角の推定値を求めた後に、フィルタをかけて出力する。また、傾斜計の出力もオイラー角に変換した後に、フィルタをかけて出力していた。逆モデルを導入した相補フィルタで姿勢推定を行う場合、姿勢の推定値ηestは、

0072

0073

と表される。

0074

角速度センサの場合、出力の動特性を補償した角速度センサ入力ωを、式(2)における行列Pの擬似逆行列P+を用いて、下式(13)によりオイラー角の微分値へ変換する。

0075

0076

また、傾斜計では、式(6)、(7)の関係を用いて、動特性を補償したセンサ入力θ=[θ1 θ2]Tを、下式(14)によりオイラー角へ変換する。

0077

0078

角速度センサ出力側では、入力を微分値に変換し、これを積分して、オイラー角の推定値を求めた後にフィルタをかけるようになっているが、前記式(13)より、変換は行列の乗算であることから、フィルタと変換部の順序入換えることができる。変換前にフィルタを施すようにした場合、角速度センサの逆伝達関数が積分演算子1/sを含んでいるのに対応して、フィルタが微分演算子sを含むように設計されていれば、積分演算子1/sと微分演算子sが相殺されることとなり、ドリフトの影響などを回避することが可能となる。

0079

一方、傾斜計出力側では、入力をオイラー角へ変換した後にフィルタをかけるようになっているが、この変換後にフィルタを施す場合の伝達関数の積

0080

0081

と、入力にフィルタを施した後にオイラー角へ変換する場合の伝達関数の積

0082

0083

とは、そのテイラー展開を求めると、2次のオーダまで一致することとなり、フィルタと変換部の順序を入換えても、精度の点で十分許容できる。

0084

これらにより、姿勢変換の前に周波数フィルタを施しても、推定値に大きな違いは生じないといえ、相補フィルタ部30において第一フィルタ32を第一姿勢変換部33より前段に配置し、また、第二フィルタ35を、第二姿勢変換部36より前段に配置した構成を採用している。

0085

さらに、角速度センサ10と傾斜計20の、それぞれの動特性の伝達関数同定、並びにフィルタ設計についてより詳細に説明する。
各センサの伝達関数Gi(s)は、直交する3軸それぞれの軸まわりの回転に対するセンサの周波数応答を元に、各要素が安定且つプロパーとなる行列として同定する。本発明では、異なる軸間の干渉、すなわち各軸まわりの回転における伝達関数は近似的に線形分離可能であると仮定している。また、角速度センサには微分器があり、傾斜計には遅れ要素があると仮定して、軸とセンサ出力の組合せごとに周波数応答を測定し、線形の近似により伝達関数の同定を行う。

0086

角速度センサの動特性の伝達関数は、

0087

0088

ただし、D1(s)=1+a11s+a12s2+・・・+a1nsn
D2(s)=1+a21s+a22s2+・・・+a2nsn
D3(s)=1+a31s+a32s2+・・・+a3nsn (n:任意の自然数)
で表される。伝達関数の各要素が一次式の場合、特に、

0089

0090

で表される。

0091

一方、傾斜計の動特性の伝達関数は、

0092

0093

ただし、D(s)=1+a1s+a2s2
で表される。なお、伝達関数の各要素の次数は、取扱う事象の複雑さにより決るものであり、姿勢推定の場合、角速度センサのものは一次、傾斜計のものは二次として問題はない。

0094

また、フィルタについては、逆伝達関数の次数よりフィルタ次数が大きくなるように設計する。具体的には、例えば、2軸傾斜計と1軸角速度センサを用いる場合、2軸傾斜計の逆伝達関数行列G-12の各要素が2次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数F2(s)G-12(s)をプロパーとするために、フィルタの伝達関数Fiを2次ローパスフィルタとして設計する。また、角速度センサの逆伝達関数G-11(s)には積分演算子(1/s)が含まれるものの、ハイパスフィルタF1(s)に微分演算子(s)を含むようにすれば、積分演算子(1/s)と微分演算子(s)が相殺されるため、結果的に積分演算を伴わない。

0095

次に、本実施形態に係る姿勢推定装置による姿勢推定の過程について説明する。
角速度センサ10から角速度bωが出力され、傾斜計20から各軸についての傾斜角θ=[θ1 θ2]Tが出力されると、角速度センサ出力側については、まず第一補償部31で出力の動特性が補償され、次いで第一フィルタ32でハイパスフィルタが施され、出力Y1=[Y11 Y12]Tが得られる。これが第一姿勢変換部33に入力され、この入力Y1=[Y11 Y12]Tを前記式(8)によりオイラー角に変換する。

0096

また、一方、傾斜計出力側では、第二補償部34で出力の動特性が補償され、次いで第二フィルタ35でローパスフィルタが施され、出力Y2=[Y21 Y22]Tが得られる。これが第二姿勢変換部36に入力され、この入力Y2=[Y21 Y22]Tを前記式(9)によりオイラー角に変換する。

0097

第一姿勢変換部33で変換された出力と第二姿勢変換部36で変換された出力が加算器37で相補的に合成されると、姿勢の推定値ηest=[βest γest]Tが求められる。

0098

このように、本実施形態に係る姿勢推定装置は、角速度センサ10の出力と傾斜計20の出力から、逆モデルを導入した相補フィルタを用いて姿勢推定を行うにあたり、センサの逆モデルを動特性補償部分と姿勢変換部分とに分け、センサ動特性については近似により線形の特性として伝達関数を同定し、この伝達関数に対する逆伝達関数を用いて特性を補償すると共に、各センサの逆伝達関数に対応してフィルタを設計し、且つ出力信号への姿勢変換演算の前にフィルタを施すようにすることから、逆伝達関数を用いた特性補償で、有効周波数領域の拡大を図りつつ、逆伝達関数とフィルタの伝達関数の積について確実に数値演算が可能となり、最終的に変換、相補的な加算を経て適切に姿勢推定が実行でき、複数のセンサを用いて必要な周波数領域で精度のよい姿勢推定を行える。特に、角速度センサの逆伝達関数に含まれる積分演算子が、第一フィルタ32における微分演算子と相殺されて、結果的に積分を伴わずに演算を行うことができ、角速度センサの出力について、積分による定常誤差の拡大が生じず、センサにおけるドリフト等の影響を受けることなく、有効な推定が行え、推定精度を高められる。

0099

本発明に係る姿勢推定装置を用いて、角速度センサと傾斜計の出力から姿勢推定を行い、得られた結果について、比較例としての逆伝達関数を用いない相補フィルタを適用した場合等の姿勢推定結果比較評価した。

0100

姿勢推定の対象には2軸実験機を用い、SSSJ製1軸角速度センサCRS07−11S、USDigital製2軸傾斜計X3Qをそれぞれ取付けた。2軸実験機の1番目の関節はy軸周りに角度β、2番目の関節はx軸周りに角度γ、それぞれ回転する関節であるとする。

0101

測定に先立ち、角速度センサ、傾斜計についてセンサ動特性(伝達関数)の同定を行った。このセンサ動特性の同定では、異なる軸間の干渉は近似的に線形分離できると仮定し、軸とセンサ出力の組合せごとに正弦波を入力して周波数応答を測定し、最小二乗法により伝達関数を同定する。

0102

実際の周波数応答の測定から、それぞれのセンサの伝達関数を、角速度センサは下式(15)、傾斜計は下式(16)、(17)のように同定した。

0103

0104

0105

この角速度センサと傾斜計の各伝達関数を用いて、2軸相補フィルタを設計した。実施例1として、この相補フィルタを用いると、姿勢の推定値ηestは下式(18)で表される。

0106

0107

ここで、X1は角速度センサの出力、X2は2軸傾斜角センサの出力である。また、前記式(8)、式(9)で示したH1、H2をオイラー角の推定に用いている。なお、式(8)、(9)に用いている推定値は、実際は離散時間で使用するために、1ステップ前の推定値を用いている。

0108

この相補フィルタでは、2軸傾斜計の逆伝達関数行列G-12の各要素が2次進み要素となることから、逆伝達関数と第二フィルタの伝達関数の積からなる伝達関数F2(s)G-12 (s)をプロパーとするために、第二フィルタの伝達関数Fi(s)を2次ローパスフィルタとして設計した。また、角速度センサの逆伝達関数G-11 (s) には積分器が含まれるが、ハイパスフィルタである第一フィルタF1(s) の持つ微分器と相殺されるため、結果的に積分演算を伴わないこととなる。

0109

また、比較例1として、逆伝達関数を用いない2軸相補フィルタを設計し、姿勢推定に用いるようにした。さらに、比較例2として、角速度センサのみを用いて姿勢推定を行うようにした。

0110

この本発明に係る姿勢推定装置を用いる場合(実施例1)と各比較例の場合について、実験機の2軸に正弦波入力をβ=57.3sint[deg]、γ=25.7sin3t[deg]で与え、姿勢推定を行った。なお、いずれの場合も、第一フィルタ、第二フィルタの伝達関数Fiは同じものを用いている。

0111

実施例1における逆伝達関数を用いて設計した2軸相補フィルタと、逆伝達関数を用いない比較例1の相補フィルタでのβ、γの推定結果を、真値と共に図3に示す。この図3中、実線は実施例1による推定値、濃灰色破線は比較例1による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、角速度センサのみを用いた比較例2の推定結果を、真値と共に図4に示す。この図4中、実線は比較例2による推定値、薄灰色点線は真の入力値をそれぞれ示している。

0112

図3に示すように、実施例1の逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例1の場合に比べ、遅れが補償され、比較的よい推定が行えていることがわかる。また、図4に示すように、角速度センサのみを用いた比較例2の推定結果によれば、位相を補償することはできているものの、ドリフトが発生しており、時間経過とともに真値からの誤差が増大している。これに対し、逆伝達関数を用いた相補フィルタは、角速度センサのみの推定で発生していたドリフトの補償も行えている。これらから、本発明の姿勢推定装置における相補フィルタは2軸の推定において有効であるといえる。

0113

次に、実験機の複雑な動きに対して有効に推定が行われるかどうかについて評価した。
複雑な動きとして、実験機に対し、1軸目の入力q1(t) 、2軸目の入力q2(t) をそれぞれ与え、姿勢推定を行った。q1(t) を下式(19)、q2(t)を下式(20)に示す。また、サンプリング周期1[kHz]、サンプリング時間20[s]としている。q1(t)、q2(t)において、パラメータa1i、b1i、a2i、b2iは、振幅の合計が40[deg]となるように、ランダムに入力した。また、測定周波数f1i、f2iをばらけさせるためにi番目の整数部分はi−1として決定し、小数部分は0.00から0.99[Hz]までをランダムに入力している(表1参照)。

0114

0115

0116

0117

前記同様、姿勢推定の対象には2軸実験機を用い、SSSJ製1軸角速度センサCRS07−11S、USDigital製2軸傾斜計X3M−2をそれぞれ取付けた。2軸実験機の1番目の関節はy軸周りに角度β、2番目の関節はx軸周りに角度γ、それぞれ回転する関節であるとする。

0118

測定に先立ち、角速度センサ、傾斜計についてセンサ動特性(伝達関数)の同定を行った。正弦波入力により、各軸について、角速度センサは図5ないし図9、傾斜計は図10図11のような周波数応答が得られた。なお、前記図5ないし図11の各図中で、灰色の十字の点は周波数応答の測定値、連続する線は最小二乗法による近似で求めた伝達関数を、それぞれ示している。

0119

これらより、それぞれのセンサの伝達関数を、角速度センサは下式(21)、傾斜計は下式(22)のように同定した。

0120

0121

0122

この角速度センサと傾斜計の各伝達関数を用いて、2軸相補フィルタを設計した。フィルタの遮断周波数は、傾斜計の周波数の半分になるように設定した。実施例2として、この相補フィルタを用いると、姿勢の推定値ηestは下式(23)で表される。

0123

0124

また、比較例3として、逆伝達関数を用いない2軸相補フィルタを設計し、姿勢推定に用いるようにした。加えて、比較例4として、傾斜計の出力信号をそのまま用いて、また、比較例5として、傾斜計の出力信号をローパスフィルタにかけたものを用いて、また、比較例6として、角速度センサの出力信号を球面積分したものを用いて、また、比較例7として、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いて、それぞれ姿勢を求めるようにした。なお、比較例5のローパスフィルタ、比較例7のハイパスフィルタは下式(24)、(25)に示されるものとなっている。比較例5のローパスフィルタの遮断周波数は5[Hz]程度の値とし、また、比較例7のハイパスフィルタの遮断周波数は相補フィルタの場合と同様の値とする。

0125

0126

0127

前記q1(t) 、q2(t) の各入力を与える複雑な動きに対して、本発明に係る姿勢推定装置を用いる場合(実施例2)と各比較例の場合で、姿勢推定を行った結果を示す。
実施例2における逆伝達関数を用いて設計した2軸相補フィルタと、逆伝達関数を用いない比較例3の相補フィルタの推定結果を、真値と共に図12に示す。この図12中、実線は実施例2による推定値、濃灰色破線は比較例3による推定値、薄灰色点線は真の入力値をそれぞれ示している。また、推定結果における5秒から10秒の範囲の拡大図を図13に、各結果の推定誤差を図14にそれぞれ示す。図13中の各線が示すものは図12の場合と同様である。一方、図14中、実線は実施例2の場合の誤差角度、破線は比較例3の場合の誤差角度をそれぞれ示している。

0128

さらに、傾斜計の出力信号をそのまま用いた比較例4と、傾斜計の出力信号をローパスフィルタにかけたものを用いた比較例5の各処理結果を、真値と共に図15に示すと共に、各結果の誤差を図16に示す。図15中、濃灰色破線は比較例4による出力値、実線は比較例5による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図16中、破線は比較例4の場合の誤差角度、実線は比較例5の場合の誤差角度をそれぞれ示している。

0129

また、角速度センサの出力信号を球面積分したものを用いた比較例6と、角速度センサの出力信号を球面積分しさらにハイパスフィルタにかけたものを用いた比較例7の各処理結果を、真値と共に図17に示すと共に、各結果の誤差を図18に示す。図17中、濃灰色破線は比較例6による出力値、実線は比較例7による出力値、薄灰色点線は真の入力値をそれぞれ示している。そして、図18中、破線は比較例6の場合の誤差角度、実線は比較例7の場合の誤差角度をそれぞれ示している。

0130

図12に示すように、逆伝達関数を用いた相補フィルタは、逆伝達関数を用いない比較例3の場合に比べ、真値に近く、より精度の高い推定が行えていることがわかる。また、図15に示すように、比較例4の傾斜計のみを用いた結果や、比較例5の傾斜計出力信号をローパスフィルタにかけた結果によれば、遅れが発生しており、ローパスフィルタによってさらに遅れが生じ、精度も低下していることがわかる。

0131

また、図17に示すように、比較例6の角速度センサ出力信号を球面積分した場合の結果によれば、球面積分により早い時間ではよい精度が得られているものの、時間の経過と共に真値からの誤差が大きくなっている。加えて、比較例7の球面積分し更にハイパスフィルタにかけた場合の結果によれば、ドリフトの補償は行えているものの、低周波数成分を除去していることで、精度が大きく悪化している。

0132

ここで、前記q1(t) 、q2(t) の各入力に対する前記実施例2、及び比較例3ないし7の各結果における最大誤差を比較したものを図19にそれぞれ示す。この図19中では、(a)比較例6、(b)比較例7、(c)比較例4、(d)比較例5、(e)比較例3、(f)実施例2、におけるそれぞれの最大誤差角度を示している。これらによれば、実施例2における誤差は、各比較例のそれに比べて小さくなっている。

実施例

0133

これらの結果から、本発明の姿勢推定装置における相補フィルタは、複雑な動きに対しても有効に推定が行えることがわかる。

0134

1姿勢推定装置
10角速度センサ
20傾斜計
30相補フィルタ部
31 第一補償部
32 第一フィルタ
33 第一姿勢変換部
34 第二補償部
35 第二フィルタ
36 第二姿勢変換部
37 加算器

ページトップへ

この技術を出願した法人

この技術を発明した人物

ページトップへ

関連する挑戦したい社会課題

関連する公募課題

ページトップへ

新着 最近 公開された関連が強い 技術

この 技術と関連性が強い 技術

関連性が強い 技術一覧

この 技術と関連性が強い人物

関連性が強い人物一覧

この 技術と関連する挑戦したい社会課題

関連する挑戦したい社会課題一覧

この 技術と関連する公募課題

関連する公募課題一覧

astavision 新着記事

サイト情報について

本サービスは、国が公開している情報(公開特許公報、特許整理標準化データ等)を元に構成されています。出典元のデータには一部間違いやノイズがあり、情報の正確さについては保証致しかねます。また一時的に、各データの収録範囲や更新周期によって、一部の情報が正しく表示されないことがございます。当サイトの情報を元にした諸問題、不利益等について当方は何ら責任を負いかねることを予めご承知おきのほど宜しくお願い申し上げます。

主たる情報の出典

特許情報…特許整理標準化データ(XML編)、公開特許公報、特許公報、審決公報、Patent Map Guidance System データ