JP5776505B2

JP5776505B2 - 姿勢角度演算装置、姿勢角度演算方法、及びプログラム

Info

Publication number: JP5776505B2
Application number: JP2011250783A
Authority: JP
Inventors: 高橋　太郎; 太郎高橋; 晋及川; 雄介小坂
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2011-11-16
Filing date: 2011-11-16
Publication date: 2015-09-09
Anticipated expiration: 2031-11-16
Also published as: JP2013103319A

Description

本発明は、ロボットなどの姿勢角度を高精度に算出できる姿勢角度演算装置、姿勢角度演算方法、及びプログラムに関するものである。

例えば、ヒューマノイドロボットなどの関節を柔軟に制御して不整地路面の歩行などを行う場合、その歩行安定化のために、そのロボットの姿勢角度をより高精度に算出することが求められている。これに対し、光ファイバージャイロを用いて姿勢角度を高精度に算出する３次元航法装置などが知られている（特許文献１参照）。

特開平７−０７２９２６号公報

しかしながら、上記特許文献１に係る光ファイバージャイロをヒューマノイドロボットなどに搭載する場合、その姿勢角度を高精度に算出することができるが、一方で、その重量が大きく、コスト増加にも繋がる虞がある。

本発明は、このような問題点を解決するためになされたものであり、低コストかつ高精度に姿勢角度を算出できる姿勢角度演算装置、姿勢角度演算方法、及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算装置であって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する姿勢角演算手段を備える、ことを特徴とする姿勢角度演算装置である。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を合成する合成手段を更に備え、前記姿勢角演算手段は、前記合成手段により合成された信号に基づいて、前記姿勢角度を算出してもよい。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を夫々利用する複数の姿勢角演算手段を備えていてもよい。
この一態様において、前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換する複数の変換手段を更に備えていてもよい。
この一態様において、前記合成手段は、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように合成してもよい。
この一態様において、複数の前記姿勢角演算手段により夫々算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算する複数の演算手段と、前記演算手段により夫々算出された複数の演算値を加算する加算手段と、
を更に備えていてもよい。
この一態様において、前記異なる計測域は低速域、中速域又は高速域であってもよい。
この一態様において、前記複数のジャイロセンサは、低速域の角速度を検出する第１ジャイロセンサと、高速域の角速度を検出する第２ジャイロセンサと、を含み、前記複数の加速度センサは、低速域の加速度を検出する第１加速度センサと、高速域の加速度を検出する第２加速度センサと、を含んでいても良い。
この一態様において、前記複数の変換手段は、前記第１ジャイロセンサにより検出された低速域の角速度をアナログ信号からデジタル信号に変換する第１変換手段と、前記第２ジャイロセンサにより検出された高速域の角速度をアナログ信号からデジタル信号に変換する第２変換手段と、前記第１加速度センサにより検出された低速域の加速度をアナログ信号からデジタル信号に変換する第３変換手段と、前記第２加速度センサにより検出された高速域の加速度をアナログ信号からデジタル信号に変換する第４変換手段と、を含んでいても良い。
この一態様において、前記複数の合成手段は、前記第１変換手段により変換された角速度信号と、前記第２変換手段により変換された角速度信号を前記計測域の切替え閾値付近で連続するように合成する第１合成手段と、前記第３変換手段により変換された加速度信号と、前記第４変換手段により変換された加速度信号を前記計測域の切替え閾値付近で連続するように合成する第２合成手段と、を含んでいても良い。
この一態様において、前記複数の姿勢角演算手段は、前記第１合成手段により合成された角速度信号と、前記第２合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第１姿勢角演算手段と、前記第１合成手段により合成された角速度信号と、前記第２合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第２姿勢角演算手段と、を含み、前記複数の演算手段は、前記第１姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第１所定係数を乗算する第１演算手段と、前記第２姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第２所定係数を乗算する第２演算手段と、を含み、前記加算手段は、前記第１演算手段により算出された演算結果と前記第２演算手段により算出された演算結果と、を加算して最終的な姿勢角度を算出してもよい。
この一態様において、前記姿勢角度演算装置により算出された姿勢角度に基づいて、歩行の制御を行う制御装置と、を備える、ことを特徴とする二足歩行ロボットであってもよい。
他方、上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算方法であって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出するステップを含む、ことを特徴とする姿勢角度演算方法であってもよい。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を合成するステップを更に含み、前記合成された信号に基づいて、前記姿勢角度を算出してもよい。
この一態様において、前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換するステップを更に含んでいても良い。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように夫々合成してもよい。
この一態様において、前記算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算するステップと、前記算出された複数の演算値を加算するステップと、を更に含んでいても良い。
また、上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出するプログラムであって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。

本発明によれば、低コストかつ高精度に姿勢角度を算出できる姿勢角度演算装置、姿勢角度演算方法、及びプログラムを提供することができる。

本発明の一実施の形態に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。合成処理後におけるセンサ出力とロボットの挙動（姿勢角）との関係の一例を示す図である。合成比率とロボットの挙動（姿勢角）との関係の一例を示す図である。本発明の一実施の形態に係る第１及び第２姿勢角演算部の概略的なシステム構成を示すブロック図である。本発明の一実施の形態に係る姿勢角度演算装置による処理フローの一例を示すフローチャートである。本発明の一変形例に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。本発明の一変形例に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。

以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係る姿勢角度演算装置１は、例えば、二足歩行ロボット、ヒューマノイドロボット等のロボットなどに搭載され、そのロボットの姿勢角度を高精度に算出できるものである。

図１は、本発明の一実施の形態に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る姿勢角度演算装置１は、第１ジャイロセンサ２と、第２ジャイロセンサ３と、第１ＡＤ変換部４と、第２ＡＤ変換部５と、第１加速度センサ６と、第２加速度センサ７と、第３ＡＤ変換部８と、第４ＡＤ変換部９と、第１合成処理部１０と、第２合成処理部１１と、第１姿勢角演算部１２と、第２姿勢角演算部１３と、第１確率モデル演算部１４と、第２確率モデル演算部１５と、加算処理部１６と、を備えている。

なお、姿勢角度演算装置１は、例えば、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される演算プログラム、演算処理プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバス等によって相互に接続されている。

第１ジャイロセンサ２は、例えば、低速域の角速度のみを高精度に計測できる低速域用のＭＥＭＳ（Micro Electro Mechanical Systems）ジャイロセンサであり、計測した角速度を第１ＡＤ変換部４に対して出力する。

第１ＡＤ（Analog Digital）変換部４は、第１変換手段の一具体例であり、第１ジャイロセンサ２から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第１合成処理部１０に対して出力する。

第２ジャイロセンサ３は、例えば、低速域から高速域までの角速度を低精度に計測する高速域用のＭＥＭＳジャイロセンサであり、計測した角速度を第２ＡＤ変換部５に対して出力する。なお、第１ジャイロセンサ２及び第２ジャイロセンサ３は、同一箇所、同一方向、及び、１つの軸成分（ｘ軸成分、ｙ軸成分、又はｚ軸成分）をこれら２つのセンサで計測する。これにより、後述の如く、低コストかつ軽量のＭＥＭＳジャイロを用いても、例えば、高コストの光ファイバージャイロに近い高精度が実現できる。

第２ＡＤ変換部５は、第２変換手段の一具体例であり、第２ジャイロセンサ３から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第１合成処理部１０に対して出力する。このように、第１及び第２ＡＤ変換部４、５により個別にＡＤ変換処理を行い、ＡＤ変換処理後に後述の合成処理を行うことで、量子化誤差を低減でき、さらに、高分解能のＡＤ変換器を必要とせずコスト低減にも繋がる。

第１加速度センサ６は、例えば、低速域の加速度のみを高精度に計測できる低速域用のＭＥＭＳ加速度センサであり、計測した加速度を第３ＡＤ変換部８に対して出力する。

第３ＡＤ変換部８は、第３変換手段の一具体例であり、第１加速度センサ６から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第２合成処理部１１に対して出力する。

第２加速度センサ７は、例えば、低速域から高速域までの加速度を低精度に計測する高速域用のＭＥＭＳ加速度センサであり、計測した加速度を第４ＡＤ変換部９に対して出力する。

第４ＡＤ変換部９は、第４変換手段の一具体例であり、第２加速度センサ７から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第２合成処理部１１に対して出力する。

このように、第１及び第２加速度センサ６、７の出力値を第３及び第４ＡＤ変換部８、９により個別にＡＤ変換処理を行い、ＡＤ変換処理後に後述の合成処理を行うことにより、量子化誤差を低減でき、さらに、高分解能のＡＤ変換器を必要とせずコスト低減にも繋がる。

ここで、第１ジャイロセンサ２から出力される低速域用の角速度データと、第２ジャイロセンサ３から出力される高速域用の角速度データと、の合成処理について考える。例えば、ある閾値を設定し、ジャイロセンサからの角速度がその設定した閾値以上であれば第２ジャイロセンサ３から出力される高速域用の角速度データを用いて、閾値未満であれば第１ジャイロセンサ２から出力される低速域用の角速度データを用いる合成処理が考えられる。

しかしながら、第１ジャイロセンサ２から出力される低速域用の角速度データ、および、第２ジャイロセンサ３から出力される高速域用の角速度データは、夫々、若干の非線形を含んでいる。このため、一般的に行われている傾きや零点の補正、温度補正を行った場合、その誤差が残存し、合成処理を行った場合に、上記閾値付近で不連続となる。この不連続性は、後述の処理に悪影響を及ぼし、最終的に算出される姿勢角度に誤差を生じさせることとなる。

そこで、第１合成処理部１０は、第１ＡＤ変換部４からのデジタルの角速度データと、第２ＡＤ変換部５からのデジタルの角速度データと、に基づいて、合成比率による下記（１）式を用いて、角速度の合成処理を行う（図２）。これにより、計測域の異なるジャイロセンサ及び加速度センサの出力値を合成した場合でも、その計測域の切替え閾値付近で連続させることができ、上記誤差が抑制されるため、最終的な姿勢角度をより高精度に算出することができる。

なお、下記（１）式において、ω_sは、第１ジャイロセンサ２の低速域の角速度リミット値（又は第１加速度センサ６の低加速域の加速度リミット値）であり、ω_pは、ω_sより絶対値が小さい切替え開始角度であり、ω_{in_low}は、第１ＡＤ変換部から出力される角速度データ（又は第３ＡＤ変換部８から出力される加速度データ）であり、ω_{in_high}は、第２ＡＤ変換部５から出力される角速度データ（第４ＡＤ変換部９から出力される加速度データ）であり、ω_outは、第１合成処理部１０から出力される合成処理後の角速度（又は第２合成処理部１２から出力される合成処理後の加速度）である。
（ａ）ω_{in_low}≦ω_ｐの場合
ω_out＝ω_{in_low}
（ｂ）ω_p≦ω_{in_low}≦ω_sの場合（１）式
ω_out＝(ω_{in_low}-ω_p)/(ω_s-ω_p)×ω_{in_low}+(1-(ω_{in_low}-ω_p)/（ω_s-ω_p）)×ω_{in_high}
（ｃ）ω_s≦ω_{in_low}の場合
ω_out＝ω_{in_high}

また、例えば、図３に示すような１次式の合成比率関数ｒを予め設定した場合、第１合成処理部１０は、第１ＡＤ変換部４からのデジタルの角速度データと、第２ＡＤ変換部５からのデジタルの角速度データと、に基づいて、下記（２）式を用いて角速度の合成処理を行ってもよい。ｒ_１、ｒ_２は、合成比率を示し、ｒ_１＋ｒ_２＝１となるように設定されている。
ω_out＝ω_{in_low}×ｒ_１＋ω_{in_high}×ｒ_２（２）式

なお、上記合成比率関数は一例であり、これに限らず、例えば、２次式あるいは３次式の合成比率関数であってもよく、ｒ_１＋ｒ_２＝１となり連続する関数であれば任意の関数を適用できる。
また、例えば、ローパスフィルタ（ＬＰＦ）とハイパスフィルタ（ＨＰＦ）を用いた下記（２−１）式を用いて、角速度の合成処理を行っても良い。
ω_out＝ω_{in_low}×LPF＋ω_{in_high}×HPF （２−１）式
ここで、LPF＝ω_ｃ／（ｓ＋ω_ｃ）、HPF＝ｓ／（ｓ＋ω_ｃ）が一例として考えられるが、２次或いはそれ以上の次数のフィルタでも良く、全周波数帯域に渡って足して１になるＬＰＦ及びＨＰＦを用いればよい。

第１合成処理部１０は、第１合成手段の一具体例であり、合成処理した角速度を、第１姿勢角演算部１２及び第２姿勢角演算部１３に対して出力する。

第２合成処理部１１は、第２合成手段の一具体例であり、第３ＡＤ変換部８から出力されたデジタルの加速度データと、第４ＡＤ変換部９から出力されたデジタルの加速度データと、に基づいて、上記（１）式、（２）式又は（２−１）式を用いて上記第１合成処理部１０と同様の合成処理を行う。第２合成処理部１１は、合成処理した加速度を、第１姿勢角演算部１２及び第２姿勢角演算部１３に対して出力する。

ここで、本実施の形態のように、複数のセンサ（第１及び第２ジャイロセンサ２、３、第１及び第２加速度センサ６、７）を切替えて使用する場合、上述の如く、夫々のセンサに応じたパラメータを設定して姿勢角度を演算するのがより正確で好ましい。そこで、以下のように、各センサに適したパラメータを有する第１及び第２姿勢角演算部１２、１３を用いて姿勢角度を夫々算出する。

第１姿勢角演算部１２は、第１姿勢角演算手段の一具体例であり、第１合成処理部１０から出力された角速度データと、第２合成処理部１１から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出する。第１姿勢角演算部１２は、第１積分処理部１２１と、第１演算処理部１２２と、第１ハイパスフィルタ部１２３と、第１ローパスフィルタ部１２４と、第１加算部１２５と、を有している（図４）。

第１積分処理部１２１は、第１合成処理部１０から出力された角速度データに対して積分処理を行い、姿勢角度を算出し、第１ハイパスフィルタ部１２３に対して出力する。

第１演算処理部１２２は、第２合成処理部１１から出力された加速度データから姿勢角度を算出し、第１ローパスフィルタ部１２４に対して出力する。なお、第１積分処理部１２１により算出された姿勢角度は、積分処理により累積誤差を含むこととなり、第１及び第２加速度センサ６、７の加速度に基づいた姿勢角度は、その応答速度の遅延を含むこととなる。したがって、後述の如く、第１積分処理部１２１により算出された姿勢角度に、第１及び第２加速度センサ６、７の加速度に基づいた姿勢角度を加算することで、上記誤差の補正を行いつつ上記遅延を解消している。

第１ハイパスフィルタ部１２３は、第１積分処理部１２１により算出された姿勢角度の中から、所定周波数以上の高周波数帯域の姿勢角度を抽出し、第１加算部１２５に対して出力する。このように、第１及び第２ジャイロセンサ２、３からの角速度に基づいた姿勢角度については、高周波数帯域を重視し、高周波数帯域のデータを抽出する。第１ハイパスフィルタ部１２３と第１ローパスフィルタ部１２４は、夫々の出力を加算したときに１になるように設定されている。

一方、第１ローパスフィルタ部１２４は、第１演算処理部１２２により算出された姿勢角度の中から、所定周波数以下の低周波数帯域の姿勢角度を抽出し、第１加算部１２５に対して出力する。このように、第１及び第２加速度センサ６、７からの加速度に基づいた姿勢角度については、低周波数帯域を重視し、低周波数帯域のデータを抽出する。

第１加算部１２５は、第１ハイパスフィルタ部１２３から出力された姿勢角度と、第１ローパスフィルタ部１２４から出力された姿勢角度と、を加算して、暫定的な姿勢角度を算出し、第１確率モデル演算部１４に対して出力する。

第２姿勢角演算部１３は、第２姿勢角演算手段の一具体例であり、上記第１姿勢角演算部１２と略同一の構成を有しており、略同一の処理を行う。第２姿勢角演算部１３は、第１合成処理部１０から出力された角速度データと、第２合成処理部１１から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出する。第２姿勢角演算部１３は、第２積分処理部１３１と、第２演算処理部１３２と、第２ハイパスフィルタ部１３３と、第２ローパスフィルタ部１３４と、第２加算部１３５と、を有している（図４）。

第２積分処理部１３１は、第１合成処理部１０から出力された角速度データに対して積分処理を行い、姿勢角度を算出し、第２ハイパスフィルタ部１３３に対して出力する。

第２演算処理部１３２は、第２合成処理部１１から出力された加速度データから姿勢角度を算出し、第２ローパスフィルタ部１３４に対して出力する。

第２ハイパスフィルタ部１３３は、第２積分処理部１３１により算出された姿勢角度の中から、所定周波数以上の高周波数帯域の姿勢角度を抽出し、第２加算部１３５に対して出力する。一方、第２ローパスフィルタ部１３４は、第２演算処理部１３２により算出された姿勢角度の中から、所定周波数以下の低周波数帯域の姿勢角度を抽出し、第２加算部１３５に対して出力する。

ここで、ハイパスフィルタ部１３３とローパスフィルタ部１３４の周波数特性がクロスする周波数である上記所定周波数は、例えば、第１及び第２ジャイロセンサ２、３と、第１及び第２加速度センサ６、７の周波数特性や分解能などに基づいて設定されている。

第２加算部１３５は、第２ハイパスフィルタ部１３３から出力された姿勢角度と、第２ローパスフィルタ部１３４から出力された姿勢角度と、を加算して、暫定的な姿勢角度を算出し、第２確率モデル演算部１５に対して出力する。

なお、第１及び第２姿勢角演算部１２、１３は、カルマンフィルターに基づいた下記（３）式を用いて、姿勢角度を算出してもよい。

ここで、第１及び第２ジャイロセンサ２、３、第１及び第２加速度センサ６、７、ノイズ、及び、算出する姿勢角度を、下記のように状態方程式及び出力方程式として表現する。
ｘ_ｋ＝Ａｘ_ｋ−１＋ｗ_ｋ−１
ｚ_ｋ＝Ｈｘ_ｋ＋ｖ_ｋ
下記式により状態の予測（姿勢角度の推定）を行う。
ｘ_ｋ＝Ａｘ_ｋ−１＋Ｂｄ_ｋ−１
ｘ_ｋ＝ｘ_ｋ＋Ｋ（ｚ_ｋ−Ｈｘ_ｋ）
下記式により共分散方程式とカルマンゲインの算出を行う。（３）式
Ｐ_ｋ＝ＡＰ_ｋ−１Ａ^Ｔ＋Ｑ
Ｐ_ｋ＝（Ｉ−ＫＨ）Ｐ_ｋ
Ｋ＝Ｐ_ｋＨ^Ｔ（ＨＰ_ｋＨ^Ｔ＋Ｒ）⁻¹

なお、上記（３）式において、ｘ_ｋは姿勢角度を含む状態変数の真の値であり、ｘ_ｋは姿勢角度を含む状態変数の推定値（上記（３）式により実際に求める値）であり、ｚ_ｋは第１及び第２加速度センサ６、７の出力値を用いて求めた姿勢角度（第１及び第２加速度センサ６、７により計測された加速度に基づいて算出された値）であり、ｖ_ｋは観測雑音であり、ｄ_ｋは第１及び第２ジャイロセンサ２、３により計測される角速度（センサ値）であり、Ａは状態遷移行列であり、Ｂは駆動行列であり、Ｈは観測行列であり、Ｋはカルマンゲインである。

また、上記（３）式において、Ｑはプロセスノイズ共分散行列であり、Ｒは測定ノイズ共分散行列であり、これらＱ及びＲは、例えば、第１及び第２ジャイロセンサ２、３と第１及び第２加速度センサ６、７の周波数特性や分解能などに基づいて設定されている。
なお、以上の説明において時間を表わす添え字として、ｋを用いている。これ以降の説明においては、第１及び第２姿勢角演算部１２、１３のいずれかを表わす添え字としてｉを用いている。また、第１及び第２確率モデル演算部１４、１５のいずれであるかを表わす添え字としてもｉを用いている。

上述の如く、各センサに適したパラメータを有する第１及び第２姿勢角演算器１２、１３を用いて、夫々算出した姿勢角度を、第１及び第２確率モデル演算部１４、１５と加算処理部１６とを用いて統合する。

第１確率モデル演算部１４は、第１演算手段の一具体例であり、第１姿勢角演算部１２から出力された姿勢角度に、第１確率モデル係数（第１所定係数の一例）を乗算し、その乗算値を加算処理部１６に対して出力する。

第２確率モデル演算部１５は、第２演算手段の一具体例であり、第１確率モデル演算部１４と同様に、第２姿勢角演算部１３から出力された姿勢角度に、第２確率モデル係数（第２所定係数の一例）を乗算し、その乗算値を加算処理部１６に対して出力する。なお、上記第１及び第２確率モデル係数は、例えば、上述した合成処理における合成比率を用いて設定することができる。より具体的には、第１確率モデル係数をｒ_１とし、第２確率モデル係数をｒ_２に設定するようにしてもよい。

一方、第１及び第２姿勢角演算部１２、１３が上記カルマンフィルターを用いて姿勢角度を算出した場合、確率モデルは正規分布と仮定していることから、下記（４）式を用いて、第１及び第２確率モデル係数を算出することができる。なお、この確率モデルは、推定値と実際値との差が小さいほど確からしいという考え方に基づいたモデルである。このように、確率モデルを用いることで、より高精度に姿勢角度を算出することができる。

さらに、上記（４）式から、本実施の形態において、第１及び第２姿勢角演算部１２、１３を備えることから、ｉ番目の確率モデル係数p_coef_ｉ（第１及び第２確率モデル係数：ｉ＝１、２）は、下記（５）式により決定される。これにより、第１確率モデル係数と第２確率モデル係数とを加算したとき１となるように、各確率モデル係数が設定される。

またさらに、上述のような閾値を用いた合成比率関数と上記正規分布による確率モデルとを併用した場合、下記（６）式を用いて第１及び第２確率モデル係数を算出することができる。この場合、予め分かっている情報も利用できるため、より高精度に姿勢角度を算出することができる。

また、下記（７）式を用いて合成比率関数のみによって、より少ない計算量によって姿勢角を算出することもできる。さらに、上記段落（００２６）に記載のように、ＬＰＦとＨＰＦとを合成するようにしてもよい。

加算処理部１６は、加算手段の一具体例であり、第１確率モデル演算部１２から出力される乗算値と、第２確率モデル演算部１３から出力される乗算値と、を加算し最終的な姿勢角度を算出する。

次に、本実施の形態に係る姿勢角度演算装置１による姿勢角度演算方法について、詳細に説明する。図５は、本実施の形態に係る姿勢角度演算装置による処理フローの一例を示すフローチャートである。図５に示す演算処理は、例えば、所定の微小時間毎に繰り返し実行される。

まず、第１ジャイロセンサ２は、低速域の角速度を計測し、計測した角速度を第１ＡＤ変換部４に対して出力する。また、第２ジャイロセンサ３は、低速域から高速域の角速度を計測し、計測した角速度を第２ＡＤ変換部５に対して出力する（ステップＳ１０１）。このように、計測域の異なる複数のジャイロセンサを用いて角速度の計測を行なう。

同時に、第１加速度センサ６は、低速域の加速度を計測し、計測した加速度を第３ＡＤ変換部８に対して出力する。また、第２加速度センサ７は、低速域から高速域までの加速度を計測し、計測した加速度を第４ＡＤ変換部９に対して出力する（ステップＳ１０２）。このように、計測域の異なる複数の加速度センサを用いて加速度の計測を行なう。

次に、第１ＡＤ変換部４は、第１ジャイロセンサ２から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第１合成処理部１０に対して出力する。第２ＡＤ変換部５は、第２ジャイロセンサ３から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第１合成処理部１０に対して出力する。第３ＡＤ変換部８は、第１加速度センサ６から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第２合成処理部１１に対して出力する。第４ＡＤ変換部９は、第２加速度センサ７から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第２合成処理部１１に対して出力する（ステップＳ１０３）。このように、各センサから出力されるセンサ値を個別にＡＤ変換を行なう。

その後、第１合成処理部１０は、第１ＡＤ変換部４からのデジタルの角速度データと、第２ＡＤ変換部５からのデジタルの角速度データと、に基づいて、合成比率による上記（１）式、（２）式又は（２−１）式を用いて、角速度の合成処理を行い、合成処理した角速度を、第１姿勢角演算部１２及び第２姿勢角演算部１３に対して出力する。第２合成処理部１１は、第３ＡＤ変換部８から出力されたデジタルの加速度データと、第４ＡＤ変換部９から出力されたデジタルの加速度データと、に基づいて、上記（１）式、（２）式又は（２−１）式を用いて合成処理を行い、合成処理した加速度を、第１姿勢角演算部１２及び第２姿勢角演算部１３に対して出力する（ステップＳ１０４）。このように、計測域の異なる複数のセンサから出力されるセンサ値を計測域の切替え閾値付近で連続するように合成処理を行なう。

さらに、第１姿勢角演算部１２は、第１合成処理部１０から出力された角速度データと、第２合成処理部１１から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出し、第１確率モデル演算部１４に対して出力する。第２姿勢角演算部１３は、第１合成処理部１０から出力された角速度データと、第２合成処理部１１から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出し、第２確率モデル演算部１４に対して出力する（ステップＳ１０５）。このように、合成処理後の角加速度及び加速度を用いて、姿勢角度を夫々算出する。

そして、第１確率モデル演算部１４は、第１姿勢角演算部１２から出力された姿勢角度に、第１確率モデル係数を乗算し、その乗算値を加算処理部１６に対して出力する。第２確率モデル演算部１５は、第２姿勢角演算部１３から出力された姿勢角度に、第２確率モデル係数を乗算し、その乗算値を加算処理部１６に対して出力する（ステップＳ１０６）。このように、複数の姿勢角演算部により算出された姿勢角度に、確率モデルにより得られた係数を夫々乗算する。

最後に、加算処理部１６は、第１確率モデル演算部１２から出力される乗算値と、第２確率モデル演算部１３から出力される乗算値と、を加算し最終的な姿勢角度を算出する（ステップＳ１０７）。このように、各姿勢角度に確率モデルにより得られた係数を夫々乗算した結果を加算し、高精度な姿勢角度を求めることができる。

以上、本実施の形態に係る姿勢角度演算装置１において、計測域の異なる複数のジャイロセンサ及び加速度センサを用いて夫々計測を行い、その計測値を合成し、姿勢角度を算出することで、低コストかつ軽量でありながら高精度な姿勢角度を算出することができる。

また、複数のジャイロセンサ及び加速度センサから出力される信号を個別にＡＤ変換処理を行うことで、量子化誤差を低減でき、より高精度に姿勢角度を算出することができる。

さらに、計測域の異なる複数のジャイロセンサ及び加速度センサの出力値を合成処理した場合でも、その計測域の切替え閾値付近で連続させることで、上記誤差が抑制されるため、最終的な姿勢角度をより高精度に算出することができる。

なお、複数のジャイロセンサ及び加速度センサの出力値を合成処理した後、夫々のセンサに最適なパラメータが設定された複数の姿勢角演算部で姿勢角度を夫々算出し、算出された姿勢角度に確率モデルによって得られる係数を夫々乗算し、その結果を加算して最終的な姿勢角度を算出している。これにより、より高精度に姿勢角度を算出することができる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

例えば、上記一実施の形態において、低速域用の第１ジャイロセンサと、高速域用の第２ジャイロセンサと、低速域用の加速度センサと、高速域用の加速度センサと、を備える構成であるが、これに限らず、例えば、低速域用の第１ジャイロセンサと、高速域用の第２ジャイロセンサと、中速域用の第３ジャイロセンサと、低速域用の加速度センサと、高速域用の加速度センサと、中速域用の第３加速度センサと、を備える構成であってもよく、低中速域用の第５ジャイロセンサ及び低中速域用の第５加速度センサを更に備える構成であってもよく、任意に設定した速度域の組合せが適用可能である。

さらに、上記一実施の形態において、単一のジャイロセンサと第１及び第２加速度センサとからなる構成、第１及び第２ジャイロセンサと単一の加速度センサとからなる構成であってもよく、ジャイロセンサ及び加速度センサのうち少なくとも一方が複数のセンサで構成されていれば、任意の数のセンサの組み合わせが適用可能である。なお、ジャイロセンサ及び加速度センサのうち一方が単一のセンサで構成されている場合、その構成に対応して、合成処理部も単一の構成となってもよい。

またさらに、上記一実施の形態において、複数の姿勢角演算部を有する構成であるが、単一の姿勢角演算部を有する構成であってもよい。（第１及び第２姿勢角演算部１２、１３のうちいずれか一方のみを有する構成であってもよい）。

上記実施の形態において、第１及び第２合成処理部１０、１１を有しない構成であってもよい（図６）。この構成に係る姿勢角度演算装置２０において、第１及び第３ＡＤ変換部４、８は第１姿勢角演算部１２に接続され、第２及び第４変換部５、９は第２姿勢角演算部１３に接続される。これにより、構成がより簡略され、全体の計算速度を短縮することができる。さらに、本構成において、第１及び第２ＡＤ変換部４、５に第１合成処理部１０を接続し、第３及び第４ＡＤ変換部８、９に第２合成処理部１１を接続することで、第１合成処理部１０から角速度を出力させ、第２合成処理部１１から加速度を出力させるようにしてもよい（図７）。

本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、図５に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ）を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

本発明は、例えば、上述した姿勢角度演算装置１と、姿勢角度演算装置１により高精度に算出された姿勢角度に基づいて歩行の制御を行う制御装置と、を備え、より高精度な歩行を行う二足歩行ロボットなどに適用可能である。

１姿勢角度演算装置
２第１ジャイロセンサ
３第２ジャイロセンサ
４第１ＡＤ変換部
５第２ＡＤ変換部
６第１加速度センサ
７第２加速度センサ
８第３ＡＤ変換部
９第４ＡＤ変換部
１０第１合成処理部
１１第２合成処理部
１２第１姿勢角演算部
１３第２姿勢角演算部
１４第１確率モデル演算部
１５第２確率モデル演算部
１６加算処理部

Claims

角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算装置であって、
前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する姿勢角演算手段を備え、
前記異なる計測域は低速域まで、中速域まで又は高速域まである、ことを特徴とする姿勢角度演算装置。
請求項１記載の姿勢角度演算装置であって、
前記計測域の異なる複数のセンサから夫々出力される信号を合成する合成手段を更に備え、
前記姿勢角演算手段は、前記合成手段により合成された信号に基づいて、前記姿勢角度を算出する、ことを特徴とする姿勢角度演算装置。
請求項２記載の姿勢角度演算装置であって、
前記計測域の異なる複数のセンサから夫々出力される信号を夫々利用する複数の姿勢角演算手段を備える、ことを特徴とする姿勢角度演算装置。
請求項３記載の姿勢角度演算装置であって、
前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換する複数の変換手段を更に備える、ことを特徴とする姿勢角度演算装置。
請求項４記載の姿勢角度演算装置であって、
前記合成手段は、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように合成する、ことを特徴とする姿勢角度演算装置。
請求項５記載の姿勢角度演算装置であって、
複数の前記姿勢角演算手段により夫々算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算する複数の演算手段と、
前記演算手段により夫々算出された複数の演算値を加算する加算手段と、
を更に備える、ことを特徴とする姿勢角度演算装置。
請求項６記載の姿勢角度演算装置であって、
前記複数のジャイロセンサは、低速域の角速度を検出する第１ジャイロセンサと、高速域の角速度を検出する第２ジャイロセンサと、を含み、
前記複数の加速度センサは、低速域の加速度を検出する第１加速度センサと、高速域の加速度を検出する第２加速度センサと、を含む、ことを特徴とする姿勢角度演算装置。
請求項７記載の姿勢角度演算装置であって、
前記複数の変換手段は、
前記第１ジャイロセンサにより検出された低速域の角速度をアナログ信号からデジタル信号に変換する第１変換手段と、
前記第２ジャイロセンサにより検出された高速域の角速度をアナログ信号からデジタル信号に変換する第２変換手段と、
前記第１加速度センサにより検出された低速域の加速度をアナログ信号からデジタル信号に変換する第３変換手段と、
前記第２加速度センサにより検出された高速域の加速度をアナログ信号からデジタル信号に変換する第４変換手段と、
を含む、ことを特徴とする姿勢角度演算装置。
請求項８記載の姿勢角度演算装置であって、
前記複数の合成手段は、
前記第１変換手段により変換された角速度信号と、前記第２変換手段により変換された角速度信号を前記計測域の切替え閾値付近で連続するように合成する第１合成手段と、
前記第３変換手段により変換された加速度信号と、前記第４変換手段により変換された加速度信号を前記計測域の切替え閾値付近で連続するように合成する第２合成手段と、
を含む、ことを特徴とする姿勢角度演算装置。
請求項９記載の姿勢角度演算装置であって、
前記複数の姿勢角演算手段は、
前記第１合成手段により合成された角速度信号と、前記第２合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第１姿勢角演算手段と、
前記第１合成手段により合成された角速度信号と、前記第２合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第２姿勢角演算手段と、を含み、
前記複数の演算手段は、
前記第１姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第１所定係数を乗算する第１演算手段と、
前記第２姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第２所定係数を乗算する第２演算手段と、を含み、
前記加算手段は、
前記第１演算手段により算出された演算結果と前記第２演算手段により算出された演算結果と、を加算して最終的な姿勢角度を算出する、ことを特徴とする姿勢角度演算装置。
請求項１乃至１０のうちいずれか１項記載の姿勢角度演算装置と、
前記姿勢角度演算装置により算出された姿勢角度に基づいて、歩行の制御を行う制御装置と、
を備える、ことを特徴とする二足歩行ロボット。
角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算方法であって、
前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出するステップを含み、
前記異なる計測域は低速域まで、中速域まで又は高速域までである、ことを特徴とする姿勢角度演算方法。
請求項１２記載の姿勢角度演算方法であって、
前記計測域の異なる複数のセンサから夫々出力される信号を合成するステップを更に含み、
前記合成された信号に基づいて、前記姿勢角度を算出する、ことを特徴とする姿勢角度演算方法。
請求項１３記載の姿勢角度演算方法であって、
前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換するステップを更に含む、ことを特徴とする姿勢角度演算方法。
請求項１４記載の姿勢角度演算方法であって、
前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように夫々合成する、ことを特徴とする姿勢角度演算方法。
請求項１５記載の姿勢角度演算方法であって、
前記算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算するステップと、
前記算出された複数の演算値を加算するステップと、
を更に含む、ことを特徴とする姿勢角度演算方法。
角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出するプログラムであって、
前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する処理と、をコンピュータに実行させ、
前記異なる計測域は低速域まで、中速域まで又は高速域までである、ことを特徴とするプログラム。