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

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

Info

Publication number
JP5776505B2
JP5776505B2 JP2011250783A JP2011250783A JP5776505B2 JP 5776505 B2 JP5776505 B2 JP 5776505B2 JP 2011250783 A JP2011250783 A JP 2011250783A JP 2011250783 A JP2011250783 A JP 2011250783A JP 5776505 B2 JP5776505 B2 JP 5776505B2
Authority
JP
Japan
Prior art keywords
posture angle
angle calculation
acceleration
angular velocity
sensors
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.)
Expired - Fee Related
Application number
JP2011250783A
Other languages
English (en)
Other versions
JP2013103319A (ja
Inventor
高橋 太郎
太郎 高橋
晋 及川
晋 及川
雄介 小坂
雄介 小坂
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Original Assignee
Toyota Motor Corp
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp filed Critical Toyota Motor Corp
Priority to JP2011250783A priority Critical patent/JP5776505B2/ja
Publication of JP2013103319A publication Critical patent/JP2013103319A/ja
Application granted granted Critical
Publication of JP5776505B2 publication Critical patent/JP5776505B2/ja
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Description

本発明は、ロボットなどの姿勢角度を高精度に算出できる姿勢角度演算装置、姿勢角度演算方法、及びプログラムに関するものである。
例えば、ヒューマノイドロボットなどの関節を柔軟に制御して不整地路面の歩行などを行う場合、その歩行安定化のために、そのロボットの姿勢角度をより高精度に算出することが求められている。これに対し、光ファイバージャイロを用いて姿勢角度を高精度に算出する3次元航法装置などが知られている(特許文献1参照)。
特開平7−072926号公報
しかしながら、上記特許文献1に係る光ファイバージャイロをヒューマノイドロボットなどに搭載する場合、その姿勢角度を高精度に算出することができるが、一方で、その重量が大きく、コスト増加にも繋がる虞がある。
本発明は、このような問題点を解決するためになされたものであり、低コストかつ高精度に姿勢角度を算出できる姿勢角度演算装置、姿勢角度演算方法、及びプログラムを提供することを主たる目的とする。
上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算装置であって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する姿勢角演算手段を備える、ことを特徴とする姿勢角度演算装置である。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を合成する合成手段を更に備え、前記姿勢角演算手段は、前記合成手段により合成された信号に基づいて、前記姿勢角度を算出してもよい。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を夫々利用する複数の姿勢角演算手段を備えていてもよい。
この一態様において、前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換する複数の変換手段を更に備えていてもよい。
この一態様において、前記合成手段は、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように合成してもよい。
この一態様において、複数の前記姿勢角演算手段により夫々算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算する複数の演算手段と、前記演算手段により夫々算出された複数の演算値を加算する加算手段と、
を更に備えていてもよい。
この一態様において、前記異なる計測域は低速域、中速域又は高速域であってもよい。
この一態様において、前記複数のジャイロセンサは、低速域の角速度を検出する第1ジャイロセンサと、高速域の角速度を検出する第2ジャイロセンサと、を含み、前記複数の加速度センサは、低速域の加速度を検出する第1加速度センサと、高速域の加速度を検出する第2加速度センサと、を含んでいても良い。
この一態様において、前記複数の変換手段は、前記第1ジャイロセンサにより検出された低速域の角速度をアナログ信号からデジタル信号に変換する第1変換手段と、前記第2ジャイロセンサにより検出された高速域の角速度をアナログ信号からデジタル信号に変換する第2変換手段と、前記第1加速度センサにより検出された低速域の加速度をアナログ信号からデジタル信号に変換する第3変換手段と、前記第2加速度センサにより検出された高速域の加速度をアナログ信号からデジタル信号に変換する第4変換手段と、を含んでいても良い。
この一態様において、前記複数の合成手段は、前記第1変換手段により変換された角速度信号と、前記第2変換手段により変換された角速度信号を前記計測域の切替え閾値付近で連続するように合成する第1合成手段と、前記第3変換手段により変換された加速度信号と、前記第4変換手段により変換された加速度信号を前記計測域の切替え閾値付近で連続するように合成する第2合成手段と、を含んでいても良い。
この一態様において、前記複数の姿勢角演算手段は、前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第1姿勢角演算手段と、前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第2姿勢角演算手段と、を含み、前記複数の演算手段は、前記第1姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第1所定係数を乗算する第1演算手段と、前記第2姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第2所定係数を乗算する第2演算手段と、を含み、前記加算手段は、前記第1演算手段により算出された演算結果と前記第2演算手段により算出された演算結果と、を加算して最終的な姿勢角度を算出してもよい。
この一態様において、前記姿勢角度演算装置により算出された姿勢角度に基づいて、歩行の制御を行う制御装置と、を備える、ことを特徴とする二足歩行ロボットであってもよい。
他方、上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算方法であって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出するステップを含む、ことを特徴とする姿勢角度演算方法であってもよい。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を合成するステップを更に含み、前記合成された信号に基づいて、前記姿勢角度を算出してもよい。
この一態様において、前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換するステップを更に含んでいても良い。
この一態様において、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように夫々合成してもよい。
この一態様において、前記算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算するステップと、前記算出された複数の演算値を加算するステップと、を更に含んでいても良い。
また、上記目的を達成するための本発明の一態様は、角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出するプログラムであって、前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。
本発明によれば、低コストかつ高精度に姿勢角度を算出できる姿勢角度演算装置、姿勢角度演算方法、及びプログラムを提供することができる。
本発明の一実施の形態に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。 合成処理後におけるセンサ出力とロボットの挙動(姿勢角)との関係の一例を示す図である。 合成比率とロボットの挙動(姿勢角)との関係の一例を示す図である。 本発明の一実施の形態に係る第1及び第2姿勢角演算部の概略的なシステム構成を示すブロック図である。 本発明の一実施の形態に係る姿勢角度演算装置による処理フローの一例を示すフローチャートである。 本発明の一変形例に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。 本発明の一変形例に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。
以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係る姿勢角度演算装置1は、例えば、二足歩行ロボット、ヒューマノイドロボット等のロボットなどに搭載され、そのロボットの姿勢角度を高精度に算出できるものである。
図1は、本発明の一実施の形態に係る姿勢角度演算装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る姿勢角度演算装置1は、第1ジャイロセンサ2と、第2ジャイロセンサ3と、第1AD変換部4と、第2AD変換部5と、第1加速度センサ6と、第2加速度センサ7と、第3AD変換部8と、第4AD変換部9と、第1合成処理部10と、第2合成処理部11と、第1姿勢角演算部12と、第2姿勢角演算部13と、第1確率モデル演算部14と、第2確率モデル演算部15と、加算処理部16と、を備えている。
なお、姿勢角度演算装置1は、例えば、演算処理等と行うCPU(Central Processing Unit)と、CPUによって実行される演算プログラム、演算処理プログラム等が記憶されたROM(Read Only Memory)と、処理データ等を一時的に記憶するRAM(Random Access Memory)と、を有するマイクロコンピュータを中心にして、ハードウェア構成されている。また、これらCPU、ROM、及びRAMは、データバス等によって相互に接続されている。
第1ジャイロセンサ2は、例えば、低速域の角速度のみを高精度に計測できる低速域用のMEMS(Micro Electro Mechanical Systems)ジャイロセンサであり、計測した角速度を第1AD変換部4に対して出力する。
第1AD(Analog Digital)変換部4は、第1変換手段の一具体例であり、第1ジャイロセンサ2から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第1合成処理部10に対して出力する。
第2ジャイロセンサ3は、例えば、低速域から高速域までの角速度を低精度に計測する高速域用のMEMSジャイロセンサであり、計測した角速度を第2AD変換部5に対して出力する。なお、第1ジャイロセンサ2及び第2ジャイロセンサ3は、同一箇所、同一方向、及び、1つの軸成分(x軸成分、y軸成分、又はz軸成分)をこれら2つのセンサで計測する。これにより、後述の如く、低コストかつ軽量のMEMSジャイロを用いても、例えば、高コストの光ファイバージャイロに近い高精度が実現できる。
第2AD変換部5は、第2変換手段の一具体例であり、第2ジャイロセンサ3から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第1合成処理部10に対して出力する。このように、第1及び第2AD変換部4、5により個別にAD変換処理を行い、AD変換処理後に後述の合成処理を行うことで、量子化誤差を低減でき、さらに、高分解能のAD変換器を必要とせずコスト低減にも繋がる。
第1加速度センサ6は、例えば、低速域の加速度のみを高精度に計測できる低速域用のMEMS加速度センサであり、計測した加速度を第3AD変換部8に対して出力する。
第3AD変換部8は、第3変換手段の一具体例であり、第1加速度センサ6から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第2合成処理部11に対して出力する。
第2加速度センサ7は、例えば、低速域から高速域までの加速度を低精度に計測する高速域用のMEMS加速度センサであり、計測した加速度を第4AD変換部9に対して出力する。
第4AD変換部9は、第4変換手段の一具体例であり、第2加速度センサ7から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第2合成処理部11に対して出力する。
このように、第1及び第2加速度センサ6、7の出力値を第3及び第4AD変換部8、9により個別にAD変換処理を行い、AD変換処理後に後述の合成処理を行うことにより、量子化誤差を低減でき、さらに、高分解能のAD変換器を必要とせずコスト低減にも繋がる。
ここで、第1ジャイロセンサ2から出力される低速域用の角速度データと、第2ジャイロセンサ3から出力される高速域用の角速度データと、の合成処理について考える。例えば、ある閾値を設定し、ジャイロセンサからの角速度がその設定した閾値以上であれば第2ジャイロセンサ3から出力される高速域用の角速度データを用いて、閾値未満であれば第1ジャイロセンサ2から出力される低速域用の角速度データを用いる合成処理が考えられる。
しかしながら、第1ジャイロセンサ2から出力される低速域用の角速度データ、および、第2ジャイロセンサ3から出力される高速域用の角速度データは、夫々、若干の非線形を含んでいる。このため、一般的に行われている傾きや零点の補正、温度補正を行った場合、その誤差が残存し、合成処理を行った場合に、上記閾値付近で不連続となる。この不連続性は、後述の処理に悪影響を及ぼし、最終的に算出される姿勢角度に誤差を生じさせることとなる。
そこで、第1合成処理部10は、第1AD変換部4からのデジタルの角速度データと、第2AD変換部5からのデジタルの角速度データと、に基づいて、合成比率による下記(1)式を用いて、角速度の合成処理を行う(図2)。これにより、計測域の異なるジャイロセンサ及び加速度センサの出力値を合成した場合でも、その計測域の切替え閾値付近で連続させることができ、上記誤差が抑制されるため、最終的な姿勢角度をより高精度に算出することができる。
なお、下記(1)式において、ωsは、第1ジャイロセンサ2の低速域の角速度リミット値(又は第1加速度センサ6の低加速域の加速度リミット値)であり、ωpは、ωsより絶対値が小さい切替え開始角度であり、ωin_lowは、第1AD変換部から出力される角速度データ(又は第3AD変換部8から出力される加速度データ)であり、ωin_highは、第2AD変換部5から出力される角速度データ(第4AD変換部9から出力される加速度データ)であり、ωoutは、第1合成処理部10から出力される合成処理後の角速度(又は第2合成処理部12から出力される合成処理後の加速度)である。
(a)ωin_low≦ωの場合
ωout=ωin_low
(b)ωp≦ωin_low≦ωsの場合 (1)式
ωout=(ωin_lowp)/(ωsp)×ωin_low+(1-(ωin_lowp)/(ωsp))×ωin_high
(c)ωs≦ωin_lowの場合
ωout=ωin_high
また、例えば、図3に示すような1次式の合成比率関数rを予め設定した場合、第1合成処理部10は、第1AD変換部4からのデジタルの角速度データと、第2AD変換部5からのデジタルの角速度データと、に基づいて、下記(2)式を用いて角速度の合成処理を行ってもよい。r、rは、合成比率を示し、r+r=1となるように設定されている。
ωout=ωin_low×r+ωin_high×r (2)式
なお、上記合成比率関数は一例であり、これに限らず、例えば、2次式あるいは3次式の合成比率関数であってもよく、r+r=1となり連続する関数であれば任意の関数を適用できる。
また、例えば、ローパスフィルタ(LPF)とハイパスフィルタ(HPF)を用いた下記(2−1)式を用いて、角速度の合成処理を行っても良い。
ωout=ωin_low×LPF+ωin_high×HPF (2−1)式
ここで、LPF=ω/(s+ω)、HPF=s/(s+ω)が一例として考えられるが、2次或いはそれ以上の次数のフィルタでも良く、全周波数帯域に渡って足して1になるLPF及びHPFを用いればよい。
第1合成処理部10は、第1合成手段の一具体例であり、合成処理した角速度を、第1姿勢角演算部12及び第2姿勢角演算部13に対して出力する。
第2合成処理部11は、第2合成手段の一具体例であり、第3AD変換部8から出力されたデジタルの加速度データと、第4AD変換部9から出力されたデジタルの加速度データと、に基づいて、上記(1)式、(2)式又は(2−1)式を用いて上記第1合成処理部10と同様の合成処理を行う。第2合成処理部11は、合成処理した加速度を、第1姿勢角演算部12及び第2姿勢角演算部13に対して出力する。
ここで、本実施の形態のように、複数のセンサ(第1及び第2ジャイロセンサ2、3、第1及び第2加速度センサ6、7)を切替えて使用する場合、上述の如く、夫々のセンサに応じたパラメータを設定して姿勢角度を演算するのがより正確で好ましい。そこで、以下のように、各センサに適したパラメータを有する第1及び第2姿勢角演算部12、13を用いて姿勢角度を夫々算出する。
第1姿勢角演算部12は、第1姿勢角演算手段の一具体例であり、第1合成処理部10から出力された角速度データと、第2合成処理部11から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出する。第1姿勢角演算部12は、第1積分処理部121と、第1演算処理部122と、第1ハイパスフィルタ部123と、第1ローパスフィルタ部124と、第1加算部125と、を有している(図4)。
第1積分処理部121は、第1合成処理部10から出力された角速度データに対して積分処理を行い、姿勢角度を算出し、第1ハイパスフィルタ部123に対して出力する。
第1演算処理部122は、第2合成処理部11から出力された加速度データから姿勢角度を算出し、第1ローパスフィルタ部124に対して出力する。なお、第1積分処理部121により算出された姿勢角度は、積分処理により累積誤差を含むこととなり、第1及び第2加速度センサ6、7の加速度に基づいた姿勢角度は、その応答速度の遅延を含むこととなる。したがって、後述の如く、第1積分処理部121により算出された姿勢角度に、第1及び第2加速度センサ6、7の加速度に基づいた姿勢角度を加算することで、上記誤差の補正を行いつつ上記遅延を解消している。
第1ハイパスフィルタ部123は、第1積分処理部121により算出された姿勢角度の中から、所定周波数以上の高周波数帯域の姿勢角度を抽出し、第1加算部125に対して出力する。このように、第1及び第2ジャイロセンサ2、3からの角速度に基づいた姿勢角度については、高周波数帯域を重視し、高周波数帯域のデータを抽出する。第1ハイパスフィルタ部123と第1ローパスフィルタ部124は、夫々の出力を加算したときに1になるように設定されている。
一方、第1ローパスフィルタ部124は、第1演算処理部122により算出された姿勢角度の中から、所定周波数以下の低周波数帯域の姿勢角度を抽出し、第1加算部125に対して出力する。このように、第1及び第2加速度センサ6、7からの加速度に基づいた姿勢角度については、低周波数帯域を重視し、低周波数帯域のデータを抽出する。
第1加算部125は、第1ハイパスフィルタ部123から出力された姿勢角度と、第1ローパスフィルタ部124から出力された姿勢角度と、を加算して、暫定的な姿勢角度を算出し、第1確率モデル演算部14に対して出力する。
第2姿勢角演算部13は、第2姿勢角演算手段の一具体例であり、上記第1姿勢角演算部12と略同一の構成を有しており、略同一の処理を行う。第2姿勢角演算部13は、第1合成処理部10から出力された角速度データと、第2合成処理部11から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出する。第2姿勢角演算部13は、第2積分処理部131と、第2演算処理部132と、第2ハイパスフィルタ部133と、第2ローパスフィルタ部134と、第2加算部135と、を有している(図4)。
第2積分処理部131は、第1合成処理部10から出力された角速度データに対して積分処理を行い、姿勢角度を算出し、第2ハイパスフィルタ部133に対して出力する。
第2演算処理部132は、第2合成処理部11から出力された加速度データから姿勢角度を算出し、第2ローパスフィルタ部134に対して出力する。
第2ハイパスフィルタ部133は、第2積分処理部131により算出された姿勢角度の中から、所定周波数以上の高周波数帯域の姿勢角度を抽出し、第2加算部135に対して出力する。一方、第2ローパスフィルタ部134は、第2演算処理部132により算出された姿勢角度の中から、所定周波数以下の低周波数帯域の姿勢角度を抽出し、第2加算部135に対して出力する。
ここで、ハイパスフィルタ部133とローパスフィルタ部134の周波数特性がクロスする周波数である上記所定周波数は、例えば、第1及び第2ジャイロセンサ2、3と、第1及び第2加速度センサ6、7の周波数特性や分解能などに基づいて設定されている。
第2加算部135は、第2ハイパスフィルタ部133から出力された姿勢角度と、第2ローパスフィルタ部134から出力された姿勢角度と、を加算して、暫定的な姿勢角度を算出し、第2確率モデル演算部15に対して出力する。
なお、第1及び第2姿勢角演算部12、13は、カルマンフィルターに基づいた下記(3)式を用いて、姿勢角度を算出してもよい。
ここで、第1及び第2ジャイロセンサ2、3、第1及び第2加速度センサ6、7、ノイズ、及び、算出する姿勢角度を、下記のように状態方程式及び出力方程式として表現する。
=Axk−1+wk−1
=Hx+v
下記式により状態の予測(姿勢角度の推定)を行う。
=Axk−1+Bdk−1
=x+K(z−Hx
下記式により共分散方程式とカルマンゲインの算出を行う。 (3)式
=APk−1+Q
=(I−KH)P
K=P(HP+R)−1
なお、上記(3)式において、xは姿勢角度を含む状態変数の真の値であり、xは姿勢角度を含む状態変数の推定値(上記(3)式により実際に求める値)であり、zは第1及び第2加速度センサ6、7の出力値を用いて求めた姿勢角度(第1及び第2加速度センサ6、7により計測された加速度に基づいて算出された値)であり、vは観測雑音であり、dは第1及び第2ジャイロセンサ2、3により計測される角速度(センサ値)であり、Aは状態遷移行列であり、Bは駆動行列であり、Hは観測行列であり、Kはカルマンゲインである。
また、上記(3)式において、Qはプロセスノイズ共分散行列であり、Rは測定ノイズ共分散行列であり、これらQ及びRは、例えば、第1及び第2ジャイロセンサ2、3と第1及び第2加速度センサ6、7の周波数特性や分解能などに基づいて設定されている。
なお、以上の説明において時間を表わす添え字として、kを用いている。これ以降の説明においては、第1及び第2姿勢角演算部12、13のいずれかを表わす添え字としてiを用いている。また、第1及び第2確率モデル演算部14、15のいずれであるかを表わす添え字としてもiを用いている。
上述の如く、各センサに適したパラメータを有する第1及び第2姿勢角演算器12、13を用いて、夫々算出した姿勢角度を、第1及び第2確率モデル演算部14、15と加算処理部16とを用いて統合する。
第1確率モデル演算部14は、第1演算手段の一具体例であり、第1姿勢角演算部12から出力された姿勢角度に、第1確率モデル係数(第1所定係数の一例)を乗算し、その乗算値を加算処理部16に対して出力する。
第2確率モデル演算部15は、第2演算手段の一具体例であり、第1確率モデル演算部14と同様に、第2姿勢角演算部13から出力された姿勢角度に、第2確率モデル係数(第2所定係数の一例)を乗算し、その乗算値を加算処理部16に対して出力する。なお、上記第1及び第2確率モデル係数は、例えば、上述した合成処理における合成比率を用いて設定することができる。より具体的には、第1確率モデル係数をrとし、第2確率モデル係数をrに設定するようにしてもよい。
一方、第1及び第2姿勢角演算部12、13が上記カルマンフィルターを用いて姿勢角度を算出した場合、確率モデルは正規分布と仮定していることから、下記(4)式を用いて、第1及び第2確率モデル係数を算出することができる。なお、この確率モデルは、推定値と実際値との差が小さいほど確からしいという考え方に基づいたモデルである。このように、確率モデルを用いることで、より高精度に姿勢角度を算出することができる。
Figure 0005776505
さらに、上記(4)式から、本実施の形態において、第1及び第2姿勢角演算部12、13を備えることから、i番目の確率モデル係数p_coef(第1及び第2確率モデル係数:i=1、2)は、下記(5)式により決定される。これにより、第1確率モデル係数と第2確率モデル係数とを加算したとき1となるように、各確率モデル係数が設定される。
Figure 0005776505
またさらに、上述のような閾値を用いた合成比率関数と上記正規分布による確率モデルとを併用した場合、下記(6)式を用いて第1及び第2確率モデル係数を算出することができる。この場合、予め分かっている情報も利用できるため、より高精度に姿勢角度を算出することができる。
Figure 0005776505
また、下記(7)式を用いて合成比率関数のみによって、より少ない計算量によって姿勢角を算出することもできる。さらに、上記段落(0026)に記載のように、LPFとHPFとを合成するようにしてもよい。
Figure 0005776505
加算処理部16は、加算手段の一具体例であり、第1確率モデル演算部12から出力される乗算値と、第2確率モデル演算部13から出力される乗算値と、を加算し最終的な姿勢角度を算出する。
次に、本実施の形態に係る姿勢角度演算装置1による姿勢角度演算方法について、詳細に説明する。図5は、本実施の形態に係る姿勢角度演算装置による処理フローの一例を示すフローチャートである。図5に示す演算処理は、例えば、所定の微小時間毎に繰り返し実行される。
まず、第1ジャイロセンサ2は、低速域の角速度を計測し、計測した角速度を第1AD変換部4に対して出力する。また、第2ジャイロセンサ3は、低速域から高速域の角速度を計測し、計測した角速度を第2AD変換部5に対して出力する(ステップS101)。このように、計測域の異なる複数のジャイロセンサを用いて角速度の計測を行なう。
同時に、第1加速度センサ6は、低速域の加速度を計測し、計測した加速度を第3AD変換部8に対して出力する。また、第2加速度センサ7は、低速域から高速域までの加速度を計測し、計測した加速度を第4AD変換部9に対して出力する(ステップS102)。このように、計測域の異なる複数の加速度センサを用いて加速度の計測を行なう。
次に、第1AD変換部4は、第1ジャイロセンサ2から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第1合成処理部10に対して出力する。第2AD変換部5は、第2ジャイロセンサ3から出力されたアナログの角速度データをデジタルデータに変換し、変換したデジタルの角速度データを第1合成処理部10に対して出力する。第3AD変換部8は、第1加速度センサ6から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第2合成処理部11に対して出力する。第4AD変換部9は、第2加速度センサ7から出力されたアナログの加速度データをデジタルデータに変換し、変換したデジタルの加速度データを第2合成処理部11に対して出力する(ステップS103)。このように、各センサから出力されるセンサ値を個別にAD変換を行なう。
その後、第1合成処理部10は、第1AD変換部4からのデジタルの角速度データと、第2AD変換部5からのデジタルの角速度データと、に基づいて、合成比率による上記(1)式、(2)式又は(2−1)式を用いて、角速度の合成処理を行い、合成処理した角速度を、第1姿勢角演算部12及び第2姿勢角演算部13に対して出力する。第2合成処理部11は、第3AD変換部8から出力されたデジタルの加速度データと、第4AD変換部9から出力されたデジタルの加速度データと、に基づいて、上記(1)式、(2)式又は(2−1)式を用いて合成処理を行い、合成処理した加速度を、第1姿勢角演算部12及び第2姿勢角演算部13に対して出力する(ステップS104)。このように、計測域の異なる複数のセンサから出力されるセンサ値を計測域の切替え閾値付近で連続するように合成処理を行なう。
さらに、第1姿勢角演算部12は、第1合成処理部10から出力された角速度データと、第2合成処理部11から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出し、第1確率モデル演算部14に対して出力する。第2姿勢角演算部13は、第1合成処理部10から出力された角速度データと、第2合成処理部11から出力された加速度データと、に基づいて、暫定的な姿勢角度を算出し、第2確率モデル演算部14に対して出力する(ステップS105)。このように、合成処理後の角加速度及び加速度を用いて、姿勢角度を夫々算出する。
そして、第1確率モデル演算部14は、第1姿勢角演算部12から出力された姿勢角度に、第1確率モデル係数を乗算し、その乗算値を加算処理部16に対して出力する。第2確率モデル演算部15は、第2姿勢角演算部13から出力された姿勢角度に、第2確率モデル係数を乗算し、その乗算値を加算処理部16に対して出力する(ステップS106)。このように、複数の姿勢角演算部により算出された姿勢角度に、確率モデルにより得られた係数を夫々乗算する。
最後に、加算処理部16は、第1確率モデル演算部12から出力される乗算値と、第2確率モデル演算部13から出力される乗算値と、を加算し最終的な姿勢角度を算出する(ステップS107)。このように、各姿勢角度に確率モデルにより得られた係数を夫々乗算した結果を加算し、高精度な姿勢角度を求めることができる。
以上、本実施の形態に係る姿勢角度演算装置1において、計測域の異なる複数のジャイロセンサ及び加速度センサを用いて夫々計測を行い、その計測値を合成し、姿勢角度を算出することで、低コストかつ軽量でありながら高精度な姿勢角度を算出することができる。
また、複数のジャイロセンサ及び加速度センサから出力される信号を個別にAD変換処理を行うことで、量子化誤差を低減でき、より高精度に姿勢角度を算出することができる。
さらに、計測域の異なる複数のジャイロセンサ及び加速度センサの出力値を合成処理した場合でも、その計測域の切替え閾値付近で連続させることで、上記誤差が抑制されるため、最終的な姿勢角度をより高精度に算出することができる。
なお、複数のジャイロセンサ及び加速度センサの出力値を合成処理した後、夫々のセンサに最適なパラメータが設定された複数の姿勢角演算部で姿勢角度を夫々算出し、算出された姿勢角度に確率モデルによって得られる係数を夫々乗算し、その結果を加算して最終的な姿勢角度を算出している。これにより、より高精度に姿勢角度を算出することができる。
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上記一実施の形態において、低速域用の第1ジャイロセンサと、高速域用の第2ジャイロセンサと、低速域用の加速度センサと、高速域用の加速度センサと、を備える構成であるが、これに限らず、例えば、低速域用の第1ジャイロセンサと、高速域用の第2ジャイロセンサと、中速域用の第3ジャイロセンサと、低速域用の加速度センサと、高速域用の加速度センサと、中速域用の第3加速度センサと、を備える構成であってもよく、低中速域用の第5ジャイロセンサ及び低中速域用の第5加速度センサを更に備える構成であってもよく、任意に設定した速度域の組合せが適用可能である。
さらに、上記一実施の形態において、単一のジャイロセンサと第1及び第2加速度センサとからなる構成、第1及び第2ジャイロセンサと単一の加速度センサとからなる構成であってもよく、ジャイロセンサ及び加速度センサのうち少なくとも一方が複数のセンサで構成されていれば、任意の数のセンサの組み合わせが適用可能である。なお、ジャイロセンサ及び加速度センサのうち一方が単一のセンサで構成されている場合、その構成に対応して、合成処理部も単一の構成となってもよい。
またさらに、上記一実施の形態において、複数の姿勢角演算部を有する構成であるが、単一の姿勢角演算部を有する構成であってもよい。(第1及び第2姿勢角演算部12、13のうちいずれか一方のみを有する構成であってもよい)。
上記実施の形態において、第1及び第2合成処理部10、11を有しない構成であってもよい(図6)。この構成に係る姿勢角度演算装置20において、第1及び第3AD変換部4、8は第1姿勢角演算部12に接続され、第2及び第4変換部5、9は第2姿勢角演算部13に接続される。これにより、構成がより簡略され、全体の計算速度を短縮することができる。さらに、本構成において、第1及び第2AD変換部4、5に第1合成処理部10を接続し、第3及び第4AD変換部8、9に第2合成処理部11を接続することで、第1合成処理部10から角速度を出力させ、第2合成処理部11から加速度を出力させるようにしてもよい(図7)。
本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。また、上述の実施の形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、図5に示す処理を、CPUにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体(non-transitory computer readable medium)を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体(tangible storage medium)を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体(例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ)、光磁気記録媒体(例えば光磁気ディスク)、CD−ROM、CD−R、CD−R/W、半導体メモリ(例えば、マスクROM、PROM(Programmable ROM)、EPROM(Erasable PROM)、フラッシュROM、RAM)を含む。
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体(transitory computer readable medium)によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
本発明は、例えば、上述した姿勢角度演算装置1と、姿勢角度演算装置1により高精度に算出された姿勢角度に基づいて歩行の制御を行う制御装置と、を備え、より高精度な歩行を行う二足歩行ロボットなどに適用可能である。
1 姿勢角度演算装置
2 第1ジャイロセンサ
3 第2ジャイロセンサ
4 第1AD変換部
5 第2AD変換部
6 第1加速度センサ
7 第2加速度センサ
8 第3AD変換部
9 第4AD変換部
10 第1合成処理部
11 第2合成処理部
12 第1姿勢角演算部
13 第2姿勢角演算部
14 第1確率モデル演算部
15 第2確率モデル演算部
16 加算処理部

Claims (17)

  1. 角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算装置であって、
    前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
    前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する姿勢角演算手段を備え、
    前記異なる計測域は低速域まで、中速域まで又は高速域まである、ことを特徴とする姿勢角度演算装置。
  2. 請求項1記載の姿勢角度演算装置であって、
    前記計測域の異なる複数のセンサから夫々出力される信号を合成する合成手段を更に備え、
    前記姿勢角演算手段は、前記合成手段により合成された信号に基づいて、前記姿勢角度を算出する、ことを特徴とする姿勢角度演算装置。
  3. 請求項2記載の姿勢角度演算装置であって、
    前記計測域の異なる複数のセンサから夫々出力される信号を夫々利用する複数の姿勢角演算手段を備える、ことを特徴とする姿勢角度演算装置。
  4. 請求項3記載の姿勢角度演算装置であって、
    前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換する複数の変換手段を更に備える、ことを特徴とする姿勢角度演算装置。
  5. 請求項4記載の姿勢角度演算装置であって、
    前記合成手段は、前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように合成する、ことを特徴とする姿勢角度演算装置。
  6. 請求項5記載の姿勢角度演算装置であって、
    複数の前記姿勢角演算手段により夫々算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算する複数の演算手段と、
    前記演算手段により夫々算出された複数の演算値を加算する加算手段と、
    を更に備える、ことを特徴とする姿勢角度演算装置。
  7. 請求項6記載の姿勢角度演算装置であって、
    前記複数のジャイロセンサは、低速域の角速度を検出する第1ジャイロセンサと、高速域の角速度を検出する第2ジャイロセンサと、を含み、
    前記複数の加速度センサは、低速域の加速度を検出する第1加速度センサと、高速域の加速度を検出する第2加速度センサと、を含む、ことを特徴とする姿勢角度演算装置。
  8. 請求項記載の姿勢角度演算装置であって、
    前記複数の変換手段は、
    前記第1ジャイロセンサにより検出された低速域の角速度をアナログ信号からデジタル信号に変換する第1変換手段と、
    前記第2ジャイロセンサにより検出された高速域の角速度をアナログ信号からデジタル信号に変換する第2変換手段と、
    前記第1加速度センサにより検出された低速域の加速度をアナログ信号からデジタル信号に変換する第3変換手段と、
    前記第2加速度センサにより検出された高速域の加速度をアナログ信号からデジタル信号に変換する第4変換手段と、
    を含む、ことを特徴とする姿勢角度演算装置。
  9. 請求項記載の姿勢角度演算装置であって、
    前記複数の合成手段は、
    前記第1変換手段により変換された角速度信号と、前記第2変換手段により変換された角速度信号を前記計測域の切替え閾値付近で連続するように合成する第1合成手段と、
    前記第3変換手段により変換された加速度信号と、前記第4変換手段により変換された加速度信号を前記計測域の切替え閾値付近で連続するように合成する第2合成手段と、
    を含む、ことを特徴とする姿勢角度演算装置。
  10. 請求項記載の姿勢角度演算装置であって、
    前記複数の姿勢角演算手段は、
    前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第1姿勢角演算手段と、
    前記第1合成手段により合成された角速度信号と、前記第2合成手段により合成された加速度信号と、に基づいて姿勢角度を算出する第2姿勢角演算手段と、を含み、
    前記複数の演算手段は、
    前記第1姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第1所定係数を乗算する第1演算手段と、
    前記第2姿勢角演算手段により算出された姿勢角度に、確率モデル又は合成比率関数モデルに基づいた第2所定係数を乗算する第2演算手段と、を含み、
    前記加算手段は、
    前記第1演算手段により算出された演算結果と前記第2演算手段により算出された演算結果と、を加算して最終的な姿勢角度を算出する、ことを特徴とする姿勢角度演算装置。
  11. 請求項1乃至10のうちいずれか1項記載の姿勢角度演算装置と、
    前記姿勢角度演算装置により算出された姿勢角度に基づいて、歩行の制御を行う制御装置と、
    を備える、ことを特徴とする二足歩行ロボット。
  12. 角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出する姿勢角度演算方法であって、
    前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
    前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出するステップを含み、
    前記異なる計測域は低速域まで、中速域まで又は高速域までである、ことを特徴とする姿勢角度演算方法。
  13. 請求項12記載の姿勢角度演算方法であって、
    前記計測域の異なる複数のセンサから夫々出力される信号を合成するステップを更に含み、
    前記合成された信号に基づいて、前記姿勢角度を算出する、ことを特徴とする姿勢角度演算方法。
  14. 請求項13記載の姿勢角度演算方法であって、
    前記複数のセンサから出力される信号を、アナログ信号からデジタル信号に夫々変換するステップを更に含む、ことを特徴とする姿勢角度演算方法。
  15. 請求項14記載の姿勢角度演算方法であって、
    前記計測域の異なる複数のセンサから夫々出力される信号を前記計測域の切替え閾値付近で連続するように夫々合成する、ことを特徴とする姿勢角度演算方法。
  16. 請求項15記載の姿勢角度演算方法であって、
    前記算出された複数の姿勢角度に、確率モデル又は合成比率関数モデルに基づいた所定係数を夫々乗算するステップと、
    前記算出された複数の演算値を加算するステップと、
    を更に含む、ことを特徴とする姿勢角度演算方法。
  17. 角速度を検出するジャイロセンサ及び加速度を検出する加速度センサを備え、前記ジャイロセンサにより検出された角速度と、前記加速度センサにより検出された加速度と、に基づいて姿勢角度を算出するプログラムであって、
    前記ジャイロセンサ及び加速度センサのうち少なくとも一方は、同一箇所及び同一方向を計測する、計測域の異なる複数のセンサで構成され、
    前記計測域の異なる複数のセンサから夫々出力される信号に基づいて、姿勢角度を算出する処理と、をコンピュータに実行させ、
    前記異なる計測域は低速域まで、中速域まで又は高速域までである、ことを特徴とするプログラム。
JP2011250783A 2011-11-16 2011-11-16 姿勢角度演算装置、姿勢角度演算方法、及びプログラム Expired - Fee Related JP5776505B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2011250783A JP5776505B2 (ja) 2011-11-16 2011-11-16 姿勢角度演算装置、姿勢角度演算方法、及びプログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2011250783A JP5776505B2 (ja) 2011-11-16 2011-11-16 姿勢角度演算装置、姿勢角度演算方法、及びプログラム

Publications (2)

Publication Number Publication Date
JP2013103319A JP2013103319A (ja) 2013-05-30
JP5776505B2 true JP5776505B2 (ja) 2015-09-09

Family

ID=48623279

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2011250783A Expired - Fee Related JP5776505B2 (ja) 2011-11-16 2011-11-16 姿勢角度演算装置、姿勢角度演算方法、及びプログラム

Country Status (1)

Country Link
JP (1) JP5776505B2 (ja)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6616679B2 (ja) * 2015-12-16 2019-12-04 日立建機株式会社 建設機械
JP7547865B2 (ja) 2020-05-28 2024-09-10 セイコーエプソン株式会社 姿勢推定装置、センサーモジュール、計測システム、移動体、及び姿勢推定方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0238974A (ja) * 1988-07-29 1990-02-08 Toyota Motor Corp 車両用加速度検出装置
JPH08136569A (ja) * 1994-11-09 1996-05-31 Toyota Motor Corp 検出装置
DE19810213A1 (de) * 1998-03-10 1999-09-23 Bosch Gmbh Robert Verfahren und Vorrichtung zur Erzeugung eines Fahrzustandssignals bei einem Kraftfahrzeug
JP4410051B2 (ja) * 2004-07-22 2010-02-03 本田技研工業株式会社 角速度計測装置および脚式移動ロボット

Also Published As

Publication number Publication date
JP2013103319A (ja) 2013-05-30

Similar Documents

Publication Publication Date Title
CN105717947B (zh) 控制空中装置的方法和有实现此方法的控制器的空中装置
US7822550B2 (en) Environmental characteristic determination
KR100898169B1 (ko) 관성항법시스템의 초기정렬 방법
CN103429986B (zh) 惯性导航划桨算法
WO2013084987A1 (ja) 衛星航法システムを用いた位置出力装置
CN103983278B (zh) 一种测量影响卫星姿态确定系统精度的方法
US10838408B2 (en) Vibration control device, vibration control method, vibration control system, program, and recording medium
US20190308316A1 (en) Robot system for performing learning control by using motor encoder and sensor
JP3726884B2 (ja) 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
JP2015094630A (ja) センサー用ic、センサーデバイス、電子機器及び移動体
JP5776505B2 (ja) 姿勢角度演算装置、姿勢角度演算方法、及びプログラム
KR101564020B1 (ko) 이동체의 전자세 예측 방법 및 이를 이용한 전자세 예측 장치
RU2382988C1 (ru) Бесплатформенная инерциальная система ориентации на "грубых" чувствительных элементах
KR20060081509A (ko) 입력 장치의 자세 측정 방법 및 그 장치
JP7401404B2 (ja) 計測装置
KR20160036922A (ko) 곡률 추정 장치 및 방법
KR100585499B1 (ko) 2축 지자기 센서 및 2축 가속도 센서를 이용한 방위각측정장치 및 측정방법
US10345329B2 (en) Inertial force sensor
JP2015094723A (ja) 温度特性補正装置および温度特性補正方法並びに電子機器および制御回路
TW201416815A (zh) 用於六軸運動姿態感測之方法及其感測系統
CN102135431B (zh) 一种惯性测量单元精度补偿的方法
RU106971U1 (ru) Система автоматического управления беспилотным летательным аппаратом
KR101147133B1 (ko) 각도측정을 위한 상보필터 및 그 방법
KR20150030375A (ko) 자장 센서 바이어스 보정 장치 및 이를 이용한 바이어스 보정 방법

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20140117

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20141006

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20141014

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20141209

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20150609

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20150622

R151 Written notification of patent or utility model registration

Ref document number: 5776505

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151

LAPS Cancellation because of no payment of annual fees