移動体の高精度姿勢検出装置
【課題】 移動体に搭載した慣性センサ出力の中から移動体の姿勢演算に不要な慣性成分を除去して、移動体の姿勢検出を精度良く検出することのできる移動体の高精度姿勢検出装置を提供する。
【解決手段】 移動体の高精度姿勢検出装置は、移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体の動作を生成する動作生成手段と、姿勢演算を行う演算処理手段を備え、演算処理手段により慣性検出手段の出力と動作生成手段の出力に基づき移動体の姿勢を高精度に検出する。
【解決手段】 移動体の高精度姿勢検出装置は、移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体の動作を生成する動作生成手段と、姿勢演算を行う演算処理手段を備え、演算処理手段により慣性検出手段の出力と動作生成手段の出力に基づき移動体の姿勢を高精度に検出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の姿勢を高精度に検出する高精度姿勢検出装置に関するものであり、詳細には、歩行ロボットや車輪型移動ロボットなどの移動ロボットの姿勢の安定制御を行うため、この移動ロボットの本体の加速度や姿勢回転角速度や姿勢角等を高精度に計測を行う場合などに好適に利用できる移動体の高精度姿勢検出装置に関するものである。
【背景技術】
【０００２】
移動体である移動ロボットの姿勢制御は、従来より、例えば、特許文献１に示されているように、ロボット本体の一軸の姿勢に対して、一つのレートジャイロや一つの加速度センサを用いて姿勢角を検出し、その信号をフィードバックして、移動体の姿勢制御を行っていた。
【０００３】
ところが、レートジャイロや加速度センサ等の姿勢センサ出力には温度によるドリフトが存在するため、移動体の姿勢制御を精度良く制御できない問題があった。また、特に長時間に渡り移動体の姿勢制御を行う場合はドリフト量が顕著になるため、移動ロボットでも特に歩行ロボットにいたっては転倒を起こす問題があった。
【０００４】
このような問題を解決する１つ目の方法として、温度センサを用いた姿勢センサの出力を補正する方法が考えられている。
【０００５】
しかしながら、この１つ目の解決方法は、姿勢センサを駆動する電源を安定して供給できるシステムを備えた移動体には有効であるものの、姿勢センサの駆動電源が変動し、その結果、姿勢センサ出力も変動する場合には、やはり移動体の姿勢制御を精度良く制御できない問題があった。特に、小型軽量であり、限られたロボット本体の本体スペースに、制御用コンピュータ、センサ、駆動電源等を搭載する歩行ロボットの場合、制御用コンピュータでの演算処理による負荷変動が、駆動電源の出力変動を引き起こし、その結果、姿勢角の検出変動・ドリフトまでも引き起こしている。そのため、温度センサを用いた姿勢センサの出力を補正したところで、やはり、歩行ロボットにいたっては転倒を起こす問題があった。
【０００６】
また、ドリフトの問題を解決する別の方法として、例えば、特許文献２に示されているように、２つの差動回路のみならず、同期検波回路、平滑回路、移相回路を用いて、ドリフトを検出し補正する方法がある。
【０００７】
しかしながら、この解決方法では、回路が複雑になる問題があった。また、センサ出力をドリフト補償したにも関わらず、姿勢制御装置の駆動電源変動、すなわちアナログ・デジタル変換回路の基準電圧変動により、制御演算処理等を行う姿勢制御装置に取り込んだ際には制御信号として、やはりドリフトが発生する問題があった。
【０００８】
レートジャイロのドリフト問題の別の解決方法として、例えば、レートジャイロと加速度センサと併用し、カルマンフィルタ等の姿勢角検出アルゴリズムを構成し、姿勢センサのドリフトを補正しながら姿勢角を検出する方法が考えられている。
【０００９】
しかしながら、レートジャイロのドリフトを解消する役割の加速度センサ自体のドリフトを補正できない問題があった。更には、カルマンフィルタ等の姿勢角検出アルゴリズムにおいては、静的（もしくは低周波数領域）において姿勢角の補正が行われているため、移動体に動的な運動を行わせる場合、動的な（もしくは高周波数領域の）姿勢角を精度良く検出できない問題も存在する。これらの問題により、カルマンフィルタ等の姿勢角検出アルゴリズムにより姿勢角を検出したところで、やはり、歩行ロボットにいたっては転倒を起こす問題があった。
【００１０】
姿勢検出センサの温度ドリフト、姿勢検出手センサに供給する駆動電源の変動、制御演算処理機構での取り込み時に発生する変動ドリフトなどが存在する場合の解決方法としては、本発明者らが先に提案した特許文献３に示すように、移動体の姿勢検出手段と、この姿勢検出手段に対して出力が反転して出力されるもう一つの姿勢検出手段を備え、姿勢検出手段と制御演算処理機構が共通の駆動電源で駆動されて、制御演算処理機構により移動体の姿勢を高精度に検出する方法がある。
【特許文献１】特開２００１−９７７２号公報
【特許文献２】特開平０７−２１８２６９号公報
【特許文献３】特開２００４−５３５３０号公報
【発明の開示】
【発明が解決しようとする課題】
【００１１】
特許文献３に示されるように、本発明者らは、移動体の高精度姿勢検出方法を提案したが、更に、解決すべき課題として、特許文献３に記載の移動体の高精度姿勢検出方法おいては、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合など、加速度センサ自身に移動体の慣性加速度が作用している場合には、姿勢角を高精度に検出できないという問題が存在していた。また、移動体の慣性加速度の影響を除去するため、姿勢検出演算過程でレートジャイロに重みを付けて姿勢角を算出する方法が考えられるが、自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に移動体が居る場合には、レートジャイロセンサ自身に自転の回転角速度が影響するため、姿勢角を高精度に検出できないという問題が存在していた。
【００１２】
本発明は、このような問題を解決するためになされたものであり、本発明の目的は、移動体に搭載した慣性センサ出力の中から移動体の姿勢演算に不要な慣性成分を除去して、移動体の姿勢を精度良く検出することのできる移動体の高精度姿勢検出装置を提供することにある。
【課題を解決するための手段】
【００１３】
前記目的を達成するため、本発明の第１の解決手段によれば、移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体の動作を生成する動作生成手段と、移動体の姿勢演算を行う演算処理手段を備え、前記演算処理手段により慣性検出手段の出力と動作生成手段の出力に基づき移動体の姿勢を高精度に検出することができる移動体の高精度姿勢検出装置が提供される。
【００１４】
このような本発明の第１の解決手段によれば、移動体に搭載した慣性検出手段の出力の中から移動体の姿勢演算に不要な慣性成分を、移動体の動作を生成する動作生成手段の出力（目標値）を用いて除去することができるため、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、または、移動体が自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に居る場合のいずれの場合においても、精度良く移動体の姿勢を検出することができる。
【００１５】
本発明の第１の解決手段の第１の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも動作の加速度目標値が出力されており、前記演算処理手段は、加速度目標値に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき、移動体の姿勢を高精度に検出することができる。
【００１６】
本発明の第１の解決手段の第２の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転による遠心力が出力されており、前記演算処理手段は、遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出することができる。
【００１７】
本発明の第１の解決手段の第３の具体的方法によれば、前記慣性検出手段には、少なくともレートジャイロが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転速度が出力されており、前記演算処理手段は、自転速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出することができる。
【００１８】
また、本発明の第２の解決手段によれば、移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体が居る環境の情報を検出し記憶する環境情報手段と、移動体の姿勢演算を行う演算処理手段を備え、前記演算処理手段により慣性検出手段の出力と環境情報手段の出力に基づき移動体の姿勢を高精度に検出することができる移動体の高精度姿勢検出装置が提供される。
【００１９】
このような本発明の第２の解決手段によれば、移動体に搭載した慣性検出手段の出力の中から移動体の姿勢演算に不要な慣性成分を、移動体が居る環境の情報を検出し記憶する環境情報手段の出力を用いて除去することができるため、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、または、移動体が自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に居る場合のいずれの場合においても、精度良く移動体の姿勢を検出することができる。
【００２０】
本発明の第２の解決手段の第１の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記環境情報手段には、少なくとも移動体と環境との間に作用する力を検出する力センサと、前記力センサと前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、前記演算処理手段は、力センサの出力から動作を生成する加速度値を演算し、演算した加速度値と前記座標変換手段の出力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出することができる。
【００２１】
本発明の第２の解決手段の第２の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境（床）と前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による遠心力を演算し、演算した遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出することができる。
【００２２】
本発明の第２の解決手段の第３の具体的方法によれば、前記慣性検出手段には、少なくともレートジャイロが備えられ、前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境（床）と前記レートジャイロとの間の座標系の変換を行う座標変換手段が備えられ、前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による回転角速度を演算し、演算した回転角速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出することができる。
【発明の効果】
【００２３】
本発明による移動体の高精度姿勢検出装置によれば、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、加速度センサ自身に移動体の慣性加速度が作用している場合のいずれの場合においても、姿勢角を高精度に検出する装置を得ることができる。また、移動体が自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に移動体が居て、レートジャイロセンサ自身に自転の回転角速度が影響する場合においても、姿勢角を高精度に検出する装置を得ることができる。
【発明を実施するための最良の形態】
【００２４】
図１は、本発明の第１実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ａによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。図１は本発明が対象とする移動体の例として、歩行ロボットを用いて示したものであるが、移動体としては歩行ロボットに限らず、車輪型移動ロボットなどさまざまな移動体であってよい。
【００２５】
図１において、３はロボット胴体、１０は慣性検出手段、２０は動作生成手段である。また、４ａは片脚の大腿リンク、４ｂは片脚の下腿リンク、５は片脚の足裏、６ａは第１の関節モータ、６ｂは第２の関節モータ、６ｃは第３の関節モータであり、これらは、それぞれ図示しない他方の片脚分についても備えられている。図示されないが、各関節の関節角は、エンコーダやポテンショメータなどの関節角センサにより検出される。加えて、同じく図示しない制御装置により、動作生成手段２０により生成される動作を忠実に再現するように各関節のアクチュエータが制御されて、図示する歩行ロボットが移動する。
【００２６】
高精度姿勢検出装置２ａは、慣性検出手段１０及び動作生成手段２０から構成されている。慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）が備え付けられており、ここでの動作生成手段２０からは、重力加速度成分を含まず、生成している動作の加減速成分のみからなる加速度目標値２１ａが出力されている。加速度目標値２１ａは、後述するように、演算処理の簡略化のためには、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていることが好ましい。加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていない場合には、後述するように、演算処理部に座標変換手段を設けて、加速度目標値２１ａの出力座標と目標姿勢状態にある胴体３に設置された加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換を行い、同じ方向成分として演算する。以下、説明の簡略化のため、加速度目標値２１ａは、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されているとする。
【００２７】
図２は、図１の高精度姿勢検出装置２ａの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される加速度目標値２１ａの内、加速度センサ１１ａと同じ方向の成分で、且つ重力成分を含まない加速度目標値「αa_ref1」に基づき、加速度センサ１１ａの出力「αa_sen」を補正し、補正加速度「αa」を算出している。同様にして、動作生成手段２０から出力される加速度目標値２１ａの内、加速度センサ１１ｂと同じ方向の成分であり、且つ重力成分を含まない加速度目標値「αb_ref1」に基づき、加速度センサ１１ｂの出力「αb_sen」を補正し、補正加速度「αb」を算出している。最終的には、この補正した加速度信号の「αa」と「αb」に基づき、移動体の姿勢を高精度に検出する。
【００２８】
この場合、高精度姿勢検出装置２ａの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_ref1
αb＝αb_sen−αb_ref1
であり、移動体自身の移動に伴う加減速成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図２に示す演算処理部の演算処理は、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体自身が急な加減速を行う場合、または旋回移動により遠心加速度が移動体に働く場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００２９】
図３〜図５は、本発明による高精度姿勢検出装置の動作を説明する図である。本発明の第１実施例の具体例として、図３に示すように、移動体の胴体３が水平を維持したまま、加速している状態に関して概略を説明する。
【００３０】
従来においては、例えば、特許文献３の高精度姿勢検出装置においては、姿勢角の演算処理過程において、移動体の移動に伴う加速度を考慮していないため、図３に示すように、胴体３が加速をおこなっている状況においては、図４に示すように、胴体３の姿勢が傾いていると誤認識する場合があった。そのため、この姿勢検出情報を用いて移動体の姿勢制御を行った場合、誤った姿勢に制御されてしまうという問題があった。特に、歩行ロボットにいたっては、転倒を起こすという問題があった。
【００３１】
これに対して、本発明の第１実施例の高精度姿勢検出装置では、姿勢角の演算処理過程において、加速度センサに含まれる移動体の移動に伴う加速度を、動作生成手段２０の出力（加速度目標値）を用いて除去しているため、図３のように、胴体３が加速をおこなっている状況においても、図５に示すように、胴体３の姿勢を高精度に演算することが可能となる。そのため、この姿勢検出情報を用いて移動体の姿勢制御を行った場合、目標姿勢に高精度に制御を行うことが可能であり、歩行ロボットにいたっては、転倒を回避して、安定した歩行の実現が可能となる。
【００３２】
図６は、本発明の第２実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｂによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。第２実施例の高精度姿勢検出装置２ｂは、第１実施例の高精度姿勢検出装置２ａと同様に、慣性検出手段１０と動作生成手段２０から構成されており、慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）が備え付けられている。相違点は、動作生成手段２０からは、生成している動作の加減速成分と重力成分を合算した加速度目標値２１ｂが出力されている共に、既知重力加速度２２と目標姿勢角２３も出力されている点である。これらのデータを用いて、高精度に移動体の姿勢角を検出する。なお、第１実施例の高精度姿勢検出装置２ａと同様に、以下の説明の簡略化のために、加速度目標値２１ｂは、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されているとする。
【００３３】
図７は、図６の高精度姿勢検出装置２ｂの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される加速度目標値２１ｂの内、加速度センサ１１ａと同じ方向の成分で、且つ動作の加減速成分と重力成分を合算した加速度目標値「αa_ref2」と、既知の重力加速度２２と、目標姿勢角２３に基づき、加速度センサ１１ａの出力「αa_sen」を補正し、補正加速度「αa」を算出している。同様にして、動作生成手段２０から出力される加速度目標値２１ｂの内、加速度センサ１１ｂと同じ方向の成分で、且つ動作の加減速成分と重力成分を合算した加速度目標値「αb_ref2」と、既知の重力加速度２２と、目標姿勢角２３に基づき、加速度センサ１１ｂの出力「αb_sen」を補正し、補正加速度「αb」を算出している。最終的には、この補正した加速度信号の補正加速度「αa」と補正加速度「αb」に基づいて、移動体の姿勢を高精度に検出している。
【００３４】
ここでは、既知重力加速度２２と目標姿勢角２３によって、移動体の胴体３の姿勢角が目標姿勢角２３になっている場合の重力成分を算出している。すなわち、加速度センサ１１ａと同じ方向の成分「αa_grav」と加速度センサ１１ｂと同じ方向の成分「αb_grav」とを演算している。そのため、高精度姿勢検出装置２ｂの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_ref2＋αa_grav
αb＝αb_sen−αb_ref2＋αb_grav
であり、移動体自身の移動に伴う加減速成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図７に示す演算処理部の演算処理、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体自身が急な加減速を行う場合や旋回移動により遠心加速度が移動体に働く場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００３５】
図８は、本発明の第３実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｃによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００３６】
前述した第１実施例の高精度姿勢検出装置２ａ、第２実施例の高精度姿勢検出装置２ｂと同様に、第３実施例の高精度姿勢検出装置２ｃは、慣性検出手段１０と動作生成手段２０から構成されており、慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）が備え付けられている。相違点は、動作生成手段２０からは、移動体が居る環境（星）の自転による遠心力２４が出力されている点である。遠心力２４は、後述するように、演算処理部による演算処理の簡略化のため、移動体の居る位置での遠心力であるとともに、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていることが好ましい。移動体の居る位置での遠心力が出力されていない場合には、移動体が居る環境（星）の既知半径と既知自転速度と目標緯度から算出する。また、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていない場合には、座標変換手段を備え、座標変換手段により、目標方位と目標姿勢を使用し、遠心力２４の出力座標と目標姿勢状態にある胴体３に設置された加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換を行う。以下、説明の簡略化のため、遠心力２４は、移動体の居る位置での遠心力であるとともに、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されているとする。
【００３７】
図９は、図８の高精度姿勢検出装置２ｃの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される遠心力２４の内、加速度センサ１１ａと同じ方向の成分「αa_cen1」に基づき、加速度センサ１１ａの出力「αa_sen」を補正し、補正加速度「αa」を算出している。同様にして、動作生成手段２０から出力される遠心力２４の内、加速度センサ１１ｂと同じ方向の成分「αb_cen1」に基づき、加速度センサ１１ｂの出力「αb_sen」を補正し、補正加速度「αb」を算出している。最終的には、この補正した加速度信号の補正加速度「αa」と補正加速度「αb」に基づき、移動体の姿勢を高精度に検出する。
【００３８】
この場合、高精度姿勢検出装置２ｃの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_cen1
αb＝αb_sen−αb_cen1
であり、移動体が居る環境（星）の自転により遠心力成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図９に示す演算処理部の演算処理、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、加速度センサ出力に自転による遠心力成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００３９】
図１０は、本発明の第４実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｄによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００４０】
第４実施例の高精度姿勢検出装置２ｄは、前述した第１〜３実施例の高精度姿勢検出装置（２ａ，２ｂ，２ｃ）と同様に、慣性検出手段１０と動作生成手段２０から構成されている。これらの第１〜３実施例の高精度姿勢検出装置との相違点は、慣性検出手段１０には、レートジャイロ１２ｃ備え付けられており、動作生成手段２０からは、移動体が居る環境（星）の自転速度２５が出力されている点である。自転速度２５は、後述する演算処理部の演算処理の簡略化のため、レートジャイロ１２ｃと同じ方向成分で出力されていることが好ましい。レートジャイロ１２ｃと同じ方向成分で出力されていない場合には、座標変換手段を用いて、目標緯度と目標方位と目標姿勢を使用し、自転速度２５の出力座標と目標姿勢状態にある胴体３に設置されたレートジャイロ１２ｃの出力座標との間の座標変換を行う。以下、説明の簡略化のため、自転速度２５は、レートジャイロ１２ｃと同じ方向成分で出力されているとする。
【００４１】
図１１は、図１０の高精度姿勢検出装置２ｄの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される自転速度２５の内、レートジャイロ１２ｃと同じ方向の成分「ωc_rot1」に基づき、レートジャイロ１２ｃの出力「ωc_sen」を補正し、補正角速度「ωc」を算出している。最終的には、この補正した角速度信号の補正角速度「ωc」に基づき、移動体の姿勢を高精度に検出している。
【００４２】
この場合、高精度姿勢検出装置２ｄの演算処理部の演算処理によって得られる補正した角速度信号の補正角速度「ωc」は、
ωc＝ωc_sen−ωc_rot1
であり、移動体が居る環境（星）の自転による自転速度成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する角速度（ωc）を求めることができる。
そのため、図１１に示す演算処理部の演算処理、具体的には
【数１】
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、レートジャイロ出力に自転による自転速度成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００４３】
図１２は、本発明の第５実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｅによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００４４】
第５実施例の高精度姿勢検出装置２ｅは、慣性検出手段１０と環境情報手段３０から構成されており、慣性検出手段１０には、２つの加速度センサ（１１ａ，１１ｂ）が備え付けられている。また、環境情報手段３０には、移動体と環境との間に作用する力を検出する力センサ３１が備え付けられている。図１２には図示していないが、力センサ３１の出力座標と胴体３に設置された加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換を行う座標変換手段４０が設けられている。各関節の関節角を検出するエンコーダやポテンショメータなどの関節角センサ３２、もしくは、同じく図示していない動作生成手段２０から出力されている各関節の関節角指令から、座標変換を行って、必要な方向成分を取り出すように逐次、演算されている。
【００４５】
図１３は、図１２の高精度姿勢検出装置２ｅの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。第５実施例の高精度姿勢検出装置２ｅの演算処理の内容は、第１実施例の高精度姿勢検出装置２ａや第２実施例の高精度姿勢検出装置２ｂの演算処理に類似している。相違点は、動作生成手段２０から出力されるデータを用いずに、環境情報手段３０からのデータを用いて演算処理を行う点である。以下に、具体的な処理の内容を説明する。
【００４６】
先ず、力センサ３１の出力である水平方向成分「Ｆx_sen」と垂直方向成分「Ｆz_sen」と既知重力加速度２２を用いて、動作を生成する加速度値「αx_mot」を求める。歩行ロボットなど、着地衝撃により垂直方向成分「Ｆz_sen」にノイズ成分が多く現れてしまうような場合には、力センサの垂直方向成分「Ｆz_sen」を用いずに、既知体重値を用いても実用上問題はない。次に、動作を生成する加速度値「αx_mot」を、座標変換手段４０による座標変換によって、加速度センサ（１１ａ，１１ｂ）と同じ方向成分の加速度値（αa_mot，αb_mot）に変換する。そして、この方向成分の加速度値「αa_mot」と加速度値「αb_mot」に基づき、加速度センサ（１１ａ，１１ｂ）の出力（αa_sen，αb_sen）を補正して、補正加速度「αa」と補正加速度「αb」を算出している。最終的には、この補正した加速度信号の補正加速度「αa」と補正加速度「αb」に基づき、移動体の姿勢を高精度に検出している。
【００４７】
この場合、力センサ３１を用いて算出する動作を生成する加速度値「αx_mot」は、胴体３の重力方向を含まず、胴体３の水平方向の加減速を生成する加速度値である。
【数２】
【００４８】
一方、胴体３が移動に伴って水平方向に加減速すると、その加速度値「αx_mot」の成分が、加速度センサ（１１ａ，１１ｂ）の出力に現れる。この影響を算出するため、座標変換手段４０により、力センサ３１の出力座標と加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換「ba_Ｒ_xz」を行い、加速度値「αx_mot」を、加速度センサ（１１ａ，１１ｂ）と同じ方向成分（αa_mot，αb_mot）にそれぞれ変換している。
【数３】
【００４９】
最終的には、高精度姿勢検出装置２ｅの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_mot
αb＝αb_sen−αb_mot
であり、移動体自身の移動に伴う加減速成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図１３に示す演算処理部の演算処理、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体自身が急な加減速を行う場合や旋回移動により遠心加速度が移動体に働く場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００５０】
なお、図１３では説明の簡略化のために、２次元図面により説明を簡略化して説明したが、３次元での高精度な姿勢検出を行う場合には、この２つの加速度センサ（１１ａ，１１ｂ）の両方に直交する更に別の加速度センサ１１ｃが備え付けられ、力センサ３１に関してもｘ方向の出力「Ｆx_sen」とｚ方向の出力「Ｆz_sen」の両方に直交するｙ方向の出力「Ｆy_sen」が得られるものが備え付けられていれば、３次元への拡張は容易である。このような拡張は、本発明の全ての実施例に対して有効であることは言うまでもないが、以下、高精度姿勢検出装置２ｅに関して説明をしておく。
具体的には、力センサ３１を用いて算出する動作を生成する加速度値を、
【数４】
【数５】
により算出し、
【数６】
のように、座標変換を行い、
αa＝αa_sen−αa_mot、
αb＝αb_sen−αb_mot、
αc＝αc_sen−αc_mot
のように、加速度センサを補償した後、
ｃ軸周りの姿勢角＝−ｔａｎ−１（αb／αa）
ｂ軸周りの姿勢角＝＋ｔａｎ−１（αc／αa）
のような計算により、３次元での胴体３の姿勢を高精度に検出することができる。更に、ノイズに対する信頼性を向上したい場合には、補正した加速度センサの加速度値（αa，αb，αc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することができる。
【００５１】
図１４は、本発明の第６実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｆによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００５２】
第６実施例の高精度姿勢検出装置２ｆは、第５実施例の高精度姿勢検出装置２ｅと同様に、慣性検出手段１０と環境情報手段３０から構成されており、慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）と、加速度センサ（１１ａ，１１ｂ）に直交する加速度センサ１１ｃ（図示せず）が備え付けられている。第５実施例の高精度姿勢検出装置２ｅとの相違点は、環境情報手段３０が、ＧＰＳセンサ３３と方位センサ３４から構成されると同時に、環境情報手段３０によって、移動体が居る環境（星）の自転速度２５と半径３５の情報が記憶されている。
【００５３】
環境情報手段３０におけるＧＰＳセンサ３３によって、移動体が居る緯度を検出しており、方位センサ３４により、移動体が向いている方向を検出している。更に、ＧＰＳセンサ３３の出力と、移動体が居る環境（星上：地球上に限らず月や火星などを含む全ての星上）の自転速度２５と半径３５の情報から、移動体の居る位置での遠心力が、逐次、演算されている。加えて、環境（床）と前記加速度センサとの間の座標変換を行う座標変換手段４１が設けられており、必要な座標変換を行う。すなわち、各関節の関節角を検出するエンコーダやポテンショメータなどの関節角センサ３２（図示せず）、同じく動作生成手段２０（図示せず）から出力されている各関節の関節角指令、または、同じく図示していない動作生成手段２０から出力されている胴体３の目標姿勢角と方位センサ３４の出力から遠心力が、逐次、演算されて、また、必要な座標変換が行われる。方位センサ３４は、２台のＧＰＳセンサ３３から構成されても良い。
【００５４】
図１５は、図１４の高精度姿勢検出装置２ｆの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。第６実施例の高精度姿勢検出装置２ｆの演算処理の内容は、高精度姿勢検出装置２ｃの演算処理に類似している。相違点は、動作生成手段２０の代わりに環境情報手段３０を用いている点である。以下に、具体的な処理の内容を説明する。
【００５５】
先ず、ＧＰＳセンサ３３の出力と、この移動体が居る環境（星）の既知情報である自転速度２５と半径３５の情報から、移動体の居る位置での遠心力（αx_cen2，αy_cen2，αz_cen2）を先ず算出する。この遠心力（αx_cen2，αy_cen2，αz_cen2）を、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と、方位センサ３４で検出される方位角から座標変換手段４１による座標変換を行い、加速度センサ（１１ｂ，１１ｃ，１１ａ）と同じ方向成分（αb_cen2，αc_cen2，αa_cen2）に変換する。そして、この方向成分（αb_cen2，αc_cen2，αa_cen2）に基づき、それぞれの加速度センサ（１１ｂ，１１ｃ，１１ａ）の出力（αb_sen，αc_sen，αa_sen）を補正し、補正加速度（αb，αc，αa）を算出している。最終的には、この補正した加速度信号の補正加速度（αb，αc，αa）に基づき、移動体の姿勢を高精度に検出する。
【００５６】
この場合、まず、ＧＰＳセンサ３３の出力「θ_gps」と、この移動体が居る環境（星）の自転速度２５の「ω_rot」と半径３５の「ｒ」の情報から、移動体の居る位置での遠心力を計算する。
【数７】
【００５７】
一方、移動体が自転している環境（星）に居ると、自転による遠心力の成分が、加速度センサ（１１ａ，１１ｂ，１１ｃ）の出力に現れる。この影響を算出するため、座標変換手段４１により、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と方位センサ３４で検出される方位角からの座標変換「bca_Ｒ_xyz」を用いて、遠心力（αx_cen2，αy_cen2，αz_cen2）を、加速度センサ（１１ｂ，１１ｃ，１１ａ）と同じ方向成分（αb_cen2，αc_cen2，αa_cen2）に変換している。
【数８】
【００５８】
最終的には、第６実施例の高精度姿勢検出装置２ｆの演算処理によって得られる補正した加速度信号の補正加速度（αa，αb，αc）は、それぞれ
αa＝αa_sen−αa_cen2
αb＝αb_sen−αb_cen2
αc＝αc_sen−αc_cen2
であり、移動体がいる環境（星）の自転に伴う遠心力を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb，αc）を求めることができる。
そのため、図１５に示す演算処理部の演算処理、具体的には
ｃ軸周りの姿勢角＝−ｔａｎ−１（αb／αa）
ｂ軸周りの姿勢角＝＋ｔａｎ−１（αc／αa）
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、加速度センサ出力に自転による遠心力成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。なお、更にノイズに対する信頼性を向上したい場合には、補正した加速度センサ値（αa，αb，αc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することができる。
【００５９】
図１６は、本発明の第７実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｇによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００６０】
第７実施例の高精度姿勢検出装置２ｇは、第５実施例の高精度姿勢検出装置２ｅおよび第６実施例の高精度姿勢検出装置２ｆと同様に、慣性検出手段１０と環境情報手段３０から構成されている。これらの実施例の高精度姿勢検出装置（２ｅ，２ｆ）との相違点は、慣性検出手段１０に、レートジャイロ１２ｃと、当該レートジャイロ１２ｃに直交するレートジャイロ（１２ａ，１２ｂ；図１７に示す）が備え付けられていると同時に、環境情報手段３０が、ＧＰＳセンサ３３と方位センサ３４から構成されており、環境情報手段３０により、移動体が居る環境（星）の自転速度２５の情報が記憶されている点である。このＧＰＳセンサ３３により、移動体が居る緯度を検出している。また、方位センサ３４によって、移動体が向いている方向を検出している。加えて、座標変換手段４１により、必要な座標変換を行い、図示しないが、各関節の関節角を検出するエンコーダやポテンショメータなどの関節角センサ３２、動作生成手段２０から出力されている各関節の関節角指令、動作生成手段２０から出力されている胴体３の目標姿勢角と方位センサ３４の出力からの座標変換された回転角速度が、逐次、演算されている。方位センサ３４は、２台のＧＰＳセンサ３３から構成しても良い。
【００６１】
図１７は、図１５の高精度姿勢検出装置２ｇの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。この高精度姿勢検出装置２ｇの演算処理内容は、高精度姿勢検出装置２ｄの演算処理に類似しており、相違点は動作生成手段２０の代わりに環境情報手段３０を用いている点である。以下に、具体的な処理の内容を説明する。
【００６２】
先ず、ＧＰＳセンサ３３の出力と、移動体が居る環境（星）の既知情報である自転速度２５の情報から、移動体の居る位置での自転速度の回転角速度（ωx_rot2，ωy_rot2，ωz_rot2）を算出する。この移動体の居る位置での回転角速度（ωx_rot2，ωy_rot2，ωz_rot2）を、座標変換手段４１により座標変換を行い、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と方位センサ３４で検出される方位角からの座標変換によって、レートジャイロ（１２ｂ，１２ｃ，１２ａ）と同じ方向成分（ωb_rot2，ωc_rot2，ωa_rot2）に変換する。そして、この方向成分（ωb_rot2，ωc_rot2，ωa_rot2）に基づき、レートジャイロ（１２ｂ，１２ｃ，１２ａ）の出力（ωb_sen，ωc_sen，ωa_sen）を補正し、補正角速度（ωb，ωc，ωa）を算出している。最終的には、この補正した角速度信号（ωb，ωc，ωa）に基づき、移動体の姿勢を高精度に検出している。
【００６３】
この場合、ＧＰＳセンサ３３の出力「θ_gps」と、この移動体が居る環境（星）の自転速度２５の「ω_rot」の情報から、移動体の居る位置での自転速度成分を計算する。
【数９】
【００６４】
一方、移動体が自転している環境（星）に居ると、この自転速度成分が、レートジャイロ（１２ａ，１２ｂ，１２ｃ）の出力に現れる。この影響を算出するため、座標変換手段４１により、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と方位センサ３４で検出される方位角からの座標変換の「bca_Ｒ_xyz」を用いて、自転速度成分（ωx_ro2t，ωy_rot2，ωz_rot2）を、レートジャイロ（１２ｂ，１２ｃ，１２ａ）と同じ方向成分（ωb_rot2，ωc_rot2，ωa_rot2）に変換している。
【数１０】
【００６５】
最終的には、高精度姿勢検出装置２ｇの演算処理によって得られる補正した角速度信号（ωb，ωc，ωa）は、それぞれ
ωa＝ωa_sen−ωa_rot2
ωb＝ωb_sen−ωb_rot2
ωc＝ωc_sen−ωc_rot2
であり、移動体がいる環境（星）の自転に伴う自転速度成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する角速度ωa，ωb，ωcを求めることができる。
そのため、図１７に示す演算処理部の演算処理、具体的には
【数１１】
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、レートジャイロ出力に自転による自転速度成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。なお、より正確に姿勢角を求めたい場合には、補正した角速度値（ωa，ωb，ωc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢をより正確で高精度に検出することができる。
【００６６】
本発明の高精度姿勢検出装置は、更に、前述した第１実施例〜第７実施例の高精度姿勢検出装置を組み合わせることにより、より高精度な姿勢検出が可能となる。
具体的には、補正した加速度信号（αb，αc，αa）と補正した角速度信号（ωb，ωc，ωa）を次のようにして求める。
αa＝αa_sen−｛Ｗa1・αa_ref1
＋Ｗa2・（αa_ref2−αa_grav）＋（１−Ｗa1−Ｗa2）・αa_mot｝
αb＝αb_sen−｛Ｗb1・αb_ref1
＋Ｗb2・（αb_ref2−αb_grav）＋（１−Ｗb1−Ｗb2）・αb_mot｝
αa＝αa_sen−｛Ｗc1・αc_ref1
＋Ｗc2・（αc_ref2−αc_grav）＋（１−Ｗc1−Ｗc2）・αc_mot｝
ωa＝ωa_sen−｛Ｖa・ωa_rot1＋（１−Ｖa）ωa_rot2｝
ωb＝ωa_sen−｛Ｖb・ωb_rot1＋（１−Ｖb）ωb_rot2｝
ωc＝ωa_sen−｛Ｖc・ωc_rot1＋（１−Ｖc）ωc_rot2｝
但し、Ｗa1，Ｗa2，Ｗb1，Ｗb2，Ｗc1，Ｗc2，Ｖa，Ｖb，Ｖcは、
高精度姿勢検出装置２ａから２ｇを選択して使用する重み係数である。
【００６７】
これにより、加速度センサの出力から、移動体自身の移動に伴う加減速成分と移動体がいる環境（星）の自転に伴う遠心力を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb，αc）を求めることができる。また、レートジャイロセンサの出力から、移動体がいる環境（星）の自転に伴う自転速度成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する角速度（ωa，ωb，ωc）を求めることができる。この補正した加速度信号の補正加速度（αb，αc，αa）と補正した角速度信号の補正角速度（ωb，ωc，ωa）を、既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することが可能である。
【産業上の利用可能性】
【００６８】
以上に詳述したように、本発明による移動体の高精度姿勢検出装置は、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、加速度センサ自身に移動体の慣性加速度が作用している場合のいずれの場合であっても、姿勢角を高精度に検出する高精度姿勢検出装置を得ることができる。また、移動体が自転する環境（星上）に移動体が居て、レートジャイロセンサ自身に自転の回転角速度が影響する場合においても、姿勢角を高精度に検出する装置を得ることができる。このため、移動体である歩行ロボットの姿勢の安定制御を行うため、歩行ロボットの本体の加速度や姿勢回転角速度や姿勢角等を高精度に計測を行う場合などに好適に利用できる
【図面の簡単な説明】
【００６９】
【図１】本発明の第１実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図２】高精度姿勢検出装置２ａの演算処理部の構成を示すブロック図である。
【図３】本発明による高精度姿勢検出装置の動作を説明する第１の図である。
【図４】本発明による高精度姿勢検出装置の動作を説明する第２の図である。
【図５】本発明による高精度姿勢検出装置の動作を説明する第３の図である。
【図６】本発明の第２実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図７】高精度姿勢検出装置２ｂの演算処理部の構成を示すブロック図である。
【図８】本発明の第３実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図９】高精度姿勢検出装置２ｃの演算処理部の構成を示すブロック図である。
【図１０】本発明の第４実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１１】高精度姿勢検出装置２ｄの演算処理部の構成を示すブロック図である。
【図１２】本発明の第５実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１３】高精度姿勢検出装置２ｅの演算処理部の構成を示すブロック図である。
【図１４】本発明の第６実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１５】高精度姿勢検出装置２ｆの演算処理部の構成を示すブロック図である。
【図１６】本発明の第７実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１７】高精度姿勢検出装置２ｇの演算処理部の構成を示すブロック図である。
【符号の説明】
【００７０】
２ａ〜２ｇ 高精度姿勢検出装置
３ ロボット胴体
４ａ 片脚の大腿リンク
４ｂ 片脚の下腿リンク
５ 片脚の足裏
６ａ 第１の関節モータ
６ｂ 第２の関節モータ
６ｃ 第３の関節モータ
１０ 慣性検出手段
１１ａ 加速度センサ
１１ｂ 加速度センサ
１１ｃ 加速度センサ
１２ａ レートジャイロ
１２ｂ レートジャイロ
１２ｃ レートジャイロ
２０ 動作生成手段
２５ 自転速度
３０ 環境情報手段
３２ 関節角センサ
３３ ＧＰＳセンサ
３４ 方位センサ
３５ 半径
４０ 座標変換手段
４１ 座標変換手段
【技術分野】
【０００１】
本発明は、移動体の姿勢を高精度に検出する高精度姿勢検出装置に関するものであり、詳細には、歩行ロボットや車輪型移動ロボットなどの移動ロボットの姿勢の安定制御を行うため、この移動ロボットの本体の加速度や姿勢回転角速度や姿勢角等を高精度に計測を行う場合などに好適に利用できる移動体の高精度姿勢検出装置に関するものである。
【背景技術】
【０００２】
移動体である移動ロボットの姿勢制御は、従来より、例えば、特許文献１に示されているように、ロボット本体の一軸の姿勢に対して、一つのレートジャイロや一つの加速度センサを用いて姿勢角を検出し、その信号をフィードバックして、移動体の姿勢制御を行っていた。
【０００３】
ところが、レートジャイロや加速度センサ等の姿勢センサ出力には温度によるドリフトが存在するため、移動体の姿勢制御を精度良く制御できない問題があった。また、特に長時間に渡り移動体の姿勢制御を行う場合はドリフト量が顕著になるため、移動ロボットでも特に歩行ロボットにいたっては転倒を起こす問題があった。
【０００４】
このような問題を解決する１つ目の方法として、温度センサを用いた姿勢センサの出力を補正する方法が考えられている。
【０００５】
しかしながら、この１つ目の解決方法は、姿勢センサを駆動する電源を安定して供給できるシステムを備えた移動体には有効であるものの、姿勢センサの駆動電源が変動し、その結果、姿勢センサ出力も変動する場合には、やはり移動体の姿勢制御を精度良く制御できない問題があった。特に、小型軽量であり、限られたロボット本体の本体スペースに、制御用コンピュータ、センサ、駆動電源等を搭載する歩行ロボットの場合、制御用コンピュータでの演算処理による負荷変動が、駆動電源の出力変動を引き起こし、その結果、姿勢角の検出変動・ドリフトまでも引き起こしている。そのため、温度センサを用いた姿勢センサの出力を補正したところで、やはり、歩行ロボットにいたっては転倒を起こす問題があった。
【０００６】
また、ドリフトの問題を解決する別の方法として、例えば、特許文献２に示されているように、２つの差動回路のみならず、同期検波回路、平滑回路、移相回路を用いて、ドリフトを検出し補正する方法がある。
【０００７】
しかしながら、この解決方法では、回路が複雑になる問題があった。また、センサ出力をドリフト補償したにも関わらず、姿勢制御装置の駆動電源変動、すなわちアナログ・デジタル変換回路の基準電圧変動により、制御演算処理等を行う姿勢制御装置に取り込んだ際には制御信号として、やはりドリフトが発生する問題があった。
【０００８】
レートジャイロのドリフト問題の別の解決方法として、例えば、レートジャイロと加速度センサと併用し、カルマンフィルタ等の姿勢角検出アルゴリズムを構成し、姿勢センサのドリフトを補正しながら姿勢角を検出する方法が考えられている。
【０００９】
しかしながら、レートジャイロのドリフトを解消する役割の加速度センサ自体のドリフトを補正できない問題があった。更には、カルマンフィルタ等の姿勢角検出アルゴリズムにおいては、静的（もしくは低周波数領域）において姿勢角の補正が行われているため、移動体に動的な運動を行わせる場合、動的な（もしくは高周波数領域の）姿勢角を精度良く検出できない問題も存在する。これらの問題により、カルマンフィルタ等の姿勢角検出アルゴリズムにより姿勢角を検出したところで、やはり、歩行ロボットにいたっては転倒を起こす問題があった。
【００１０】
姿勢検出センサの温度ドリフト、姿勢検出手センサに供給する駆動電源の変動、制御演算処理機構での取り込み時に発生する変動ドリフトなどが存在する場合の解決方法としては、本発明者らが先に提案した特許文献３に示すように、移動体の姿勢検出手段と、この姿勢検出手段に対して出力が反転して出力されるもう一つの姿勢検出手段を備え、姿勢検出手段と制御演算処理機構が共通の駆動電源で駆動されて、制御演算処理機構により移動体の姿勢を高精度に検出する方法がある。
【特許文献１】特開２００１−９７７２号公報
【特許文献２】特開平０７−２１８２６９号公報
【特許文献３】特開２００４−５３５３０号公報
【発明の開示】
【発明が解決しようとする課題】
【００１１】
特許文献３に示されるように、本発明者らは、移動体の高精度姿勢検出方法を提案したが、更に、解決すべき課題として、特許文献３に記載の移動体の高精度姿勢検出方法おいては、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合など、加速度センサ自身に移動体の慣性加速度が作用している場合には、姿勢角を高精度に検出できないという問題が存在していた。また、移動体の慣性加速度の影響を除去するため、姿勢検出演算過程でレートジャイロに重みを付けて姿勢角を算出する方法が考えられるが、自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に移動体が居る場合には、レートジャイロセンサ自身に自転の回転角速度が影響するため、姿勢角を高精度に検出できないという問題が存在していた。
【００１２】
本発明は、このような問題を解決するためになされたものであり、本発明の目的は、移動体に搭載した慣性センサ出力の中から移動体の姿勢演算に不要な慣性成分を除去して、移動体の姿勢を精度良く検出することのできる移動体の高精度姿勢検出装置を提供することにある。
【課題を解決するための手段】
【００１３】
前記目的を達成するため、本発明の第１の解決手段によれば、移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体の動作を生成する動作生成手段と、移動体の姿勢演算を行う演算処理手段を備え、前記演算処理手段により慣性検出手段の出力と動作生成手段の出力に基づき移動体の姿勢を高精度に検出することができる移動体の高精度姿勢検出装置が提供される。
【００１４】
このような本発明の第１の解決手段によれば、移動体に搭載した慣性検出手段の出力の中から移動体の姿勢演算に不要な慣性成分を、移動体の動作を生成する動作生成手段の出力（目標値）を用いて除去することができるため、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、または、移動体が自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に居る場合のいずれの場合においても、精度良く移動体の姿勢を検出することができる。
【００１５】
本発明の第１の解決手段の第１の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも動作の加速度目標値が出力されており、前記演算処理手段は、加速度目標値に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき、移動体の姿勢を高精度に検出することができる。
【００１６】
本発明の第１の解決手段の第２の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転による遠心力が出力されており、前記演算処理手段は、遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出することができる。
【００１７】
本発明の第１の解決手段の第３の具体的方法によれば、前記慣性検出手段には、少なくともレートジャイロが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転速度が出力されており、前記演算処理手段は、自転速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出することができる。
【００１８】
また、本発明の第２の解決手段によれば、移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体が居る環境の情報を検出し記憶する環境情報手段と、移動体の姿勢演算を行う演算処理手段を備え、前記演算処理手段により慣性検出手段の出力と環境情報手段の出力に基づき移動体の姿勢を高精度に検出することができる移動体の高精度姿勢検出装置が提供される。
【００１９】
このような本発明の第２の解決手段によれば、移動体に搭載した慣性検出手段の出力の中から移動体の姿勢演算に不要な慣性成分を、移動体が居る環境の情報を検出し記憶する環境情報手段の出力を用いて除去することができるため、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、または、移動体が自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に居る場合のいずれの場合においても、精度良く移動体の姿勢を検出することができる。
【００２０】
本発明の第２の解決手段の第１の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記環境情報手段には、少なくとも移動体と環境との間に作用する力を検出する力センサと、前記力センサと前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、前記演算処理手段は、力センサの出力から動作を生成する加速度値を演算し、演算した加速度値と前記座標変換手段の出力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出することができる。
【００２１】
本発明の第２の解決手段の第２の具体的方法によれば、前記慣性検出手段には、少なくとも加速度センサが備えられ、前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境（床）と前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による遠心力を演算し、演算した遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出することができる。
【００２２】
本発明の第２の解決手段の第３の具体的方法によれば、前記慣性検出手段には、少なくともレートジャイロが備えられ、前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境（床）と前記レートジャイロとの間の座標系の変換を行う座標変換手段が備えられ、前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による回転角速度を演算し、演算した回転角速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出することができる。
【発明の効果】
【００２３】
本発明による移動体の高精度姿勢検出装置によれば、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、加速度センサ自身に移動体の慣性加速度が作用している場合のいずれの場合においても、姿勢角を高精度に検出する装置を得ることができる。また、移動体が自転する環境（星上：地球上に限らず月や火星などを含む全ての星上）に移動体が居て、レートジャイロセンサ自身に自転の回転角速度が影響する場合においても、姿勢角を高精度に検出する装置を得ることができる。
【発明を実施するための最良の形態】
【００２４】
図１は、本発明の第１実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ａによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。図１は本発明が対象とする移動体の例として、歩行ロボットを用いて示したものであるが、移動体としては歩行ロボットに限らず、車輪型移動ロボットなどさまざまな移動体であってよい。
【００２５】
図１において、３はロボット胴体、１０は慣性検出手段、２０は動作生成手段である。また、４ａは片脚の大腿リンク、４ｂは片脚の下腿リンク、５は片脚の足裏、６ａは第１の関節モータ、６ｂは第２の関節モータ、６ｃは第３の関節モータであり、これらは、それぞれ図示しない他方の片脚分についても備えられている。図示されないが、各関節の関節角は、エンコーダやポテンショメータなどの関節角センサにより検出される。加えて、同じく図示しない制御装置により、動作生成手段２０により生成される動作を忠実に再現するように各関節のアクチュエータが制御されて、図示する歩行ロボットが移動する。
【００２６】
高精度姿勢検出装置２ａは、慣性検出手段１０及び動作生成手段２０から構成されている。慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）が備え付けられており、ここでの動作生成手段２０からは、重力加速度成分を含まず、生成している動作の加減速成分のみからなる加速度目標値２１ａが出力されている。加速度目標値２１ａは、後述するように、演算処理の簡略化のためには、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていることが好ましい。加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていない場合には、後述するように、演算処理部に座標変換手段を設けて、加速度目標値２１ａの出力座標と目標姿勢状態にある胴体３に設置された加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換を行い、同じ方向成分として演算する。以下、説明の簡略化のため、加速度目標値２１ａは、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されているとする。
【００２７】
図２は、図１の高精度姿勢検出装置２ａの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される加速度目標値２１ａの内、加速度センサ１１ａと同じ方向の成分で、且つ重力成分を含まない加速度目標値「αa_ref1」に基づき、加速度センサ１１ａの出力「αa_sen」を補正し、補正加速度「αa」を算出している。同様にして、動作生成手段２０から出力される加速度目標値２１ａの内、加速度センサ１１ｂと同じ方向の成分であり、且つ重力成分を含まない加速度目標値「αb_ref1」に基づき、加速度センサ１１ｂの出力「αb_sen」を補正し、補正加速度「αb」を算出している。最終的には、この補正した加速度信号の「αa」と「αb」に基づき、移動体の姿勢を高精度に検出する。
【００２８】
この場合、高精度姿勢検出装置２ａの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_ref1
αb＝αb_sen−αb_ref1
であり、移動体自身の移動に伴う加減速成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図２に示す演算処理部の演算処理は、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体自身が急な加減速を行う場合、または旋回移動により遠心加速度が移動体に働く場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００２９】
図３〜図５は、本発明による高精度姿勢検出装置の動作を説明する図である。本発明の第１実施例の具体例として、図３に示すように、移動体の胴体３が水平を維持したまま、加速している状態に関して概略を説明する。
【００３０】
従来においては、例えば、特許文献３の高精度姿勢検出装置においては、姿勢角の演算処理過程において、移動体の移動に伴う加速度を考慮していないため、図３に示すように、胴体３が加速をおこなっている状況においては、図４に示すように、胴体３の姿勢が傾いていると誤認識する場合があった。そのため、この姿勢検出情報を用いて移動体の姿勢制御を行った場合、誤った姿勢に制御されてしまうという問題があった。特に、歩行ロボットにいたっては、転倒を起こすという問題があった。
【００３１】
これに対して、本発明の第１実施例の高精度姿勢検出装置では、姿勢角の演算処理過程において、加速度センサに含まれる移動体の移動に伴う加速度を、動作生成手段２０の出力（加速度目標値）を用いて除去しているため、図３のように、胴体３が加速をおこなっている状況においても、図５に示すように、胴体３の姿勢を高精度に演算することが可能となる。そのため、この姿勢検出情報を用いて移動体の姿勢制御を行った場合、目標姿勢に高精度に制御を行うことが可能であり、歩行ロボットにいたっては、転倒を回避して、安定した歩行の実現が可能となる。
【００３２】
図６は、本発明の第２実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｂによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。第２実施例の高精度姿勢検出装置２ｂは、第１実施例の高精度姿勢検出装置２ａと同様に、慣性検出手段１０と動作生成手段２０から構成されており、慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）が備え付けられている。相違点は、動作生成手段２０からは、生成している動作の加減速成分と重力成分を合算した加速度目標値２１ｂが出力されている共に、既知重力加速度２２と目標姿勢角２３も出力されている点である。これらのデータを用いて、高精度に移動体の姿勢角を検出する。なお、第１実施例の高精度姿勢検出装置２ａと同様に、以下の説明の簡略化のために、加速度目標値２１ｂは、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されているとする。
【００３３】
図７は、図６の高精度姿勢検出装置２ｂの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される加速度目標値２１ｂの内、加速度センサ１１ａと同じ方向の成分で、且つ動作の加減速成分と重力成分を合算した加速度目標値「αa_ref2」と、既知の重力加速度２２と、目標姿勢角２３に基づき、加速度センサ１１ａの出力「αa_sen」を補正し、補正加速度「αa」を算出している。同様にして、動作生成手段２０から出力される加速度目標値２１ｂの内、加速度センサ１１ｂと同じ方向の成分で、且つ動作の加減速成分と重力成分を合算した加速度目標値「αb_ref2」と、既知の重力加速度２２と、目標姿勢角２３に基づき、加速度センサ１１ｂの出力「αb_sen」を補正し、補正加速度「αb」を算出している。最終的には、この補正した加速度信号の補正加速度「αa」と補正加速度「αb」に基づいて、移動体の姿勢を高精度に検出している。
【００３４】
ここでは、既知重力加速度２２と目標姿勢角２３によって、移動体の胴体３の姿勢角が目標姿勢角２３になっている場合の重力成分を算出している。すなわち、加速度センサ１１ａと同じ方向の成分「αa_grav」と加速度センサ１１ｂと同じ方向の成分「αb_grav」とを演算している。そのため、高精度姿勢検出装置２ｂの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_ref2＋αa_grav
αb＝αb_sen−αb_ref2＋αb_grav
であり、移動体自身の移動に伴う加減速成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図７に示す演算処理部の演算処理、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体自身が急な加減速を行う場合や旋回移動により遠心加速度が移動体に働く場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００３５】
図８は、本発明の第３実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｃによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００３６】
前述した第１実施例の高精度姿勢検出装置２ａ、第２実施例の高精度姿勢検出装置２ｂと同様に、第３実施例の高精度姿勢検出装置２ｃは、慣性検出手段１０と動作生成手段２０から構成されており、慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）が備え付けられている。相違点は、動作生成手段２０からは、移動体が居る環境（星）の自転による遠心力２４が出力されている点である。遠心力２４は、後述するように、演算処理部による演算処理の簡略化のため、移動体の居る位置での遠心力であるとともに、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていることが好ましい。移動体の居る位置での遠心力が出力されていない場合には、移動体が居る環境（星）の既知半径と既知自転速度と目標緯度から算出する。また、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されていない場合には、座標変換手段を備え、座標変換手段により、目標方位と目標姿勢を使用し、遠心力２４の出力座標と目標姿勢状態にある胴体３に設置された加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換を行う。以下、説明の簡略化のため、遠心力２４は、移動体の居る位置での遠心力であるとともに、加速度センサ（１１ａ，１１ｂ）と同じ方向成分で出力されているとする。
【００３７】
図９は、図８の高精度姿勢検出装置２ｃの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される遠心力２４の内、加速度センサ１１ａと同じ方向の成分「αa_cen1」に基づき、加速度センサ１１ａの出力「αa_sen」を補正し、補正加速度「αa」を算出している。同様にして、動作生成手段２０から出力される遠心力２４の内、加速度センサ１１ｂと同じ方向の成分「αb_cen1」に基づき、加速度センサ１１ｂの出力「αb_sen」を補正し、補正加速度「αb」を算出している。最終的には、この補正した加速度信号の補正加速度「αa」と補正加速度「αb」に基づき、移動体の姿勢を高精度に検出する。
【００３８】
この場合、高精度姿勢検出装置２ｃの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_cen1
αb＝αb_sen−αb_cen1
であり、移動体が居る環境（星）の自転により遠心力成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図９に示す演算処理部の演算処理、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、加速度センサ出力に自転による遠心力成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００３９】
図１０は、本発明の第４実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｄによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００４０】
第４実施例の高精度姿勢検出装置２ｄは、前述した第１〜３実施例の高精度姿勢検出装置（２ａ，２ｂ，２ｃ）と同様に、慣性検出手段１０と動作生成手段２０から構成されている。これらの第１〜３実施例の高精度姿勢検出装置との相違点は、慣性検出手段１０には、レートジャイロ１２ｃ備え付けられており、動作生成手段２０からは、移動体が居る環境（星）の自転速度２５が出力されている点である。自転速度２５は、後述する演算処理部の演算処理の簡略化のため、レートジャイロ１２ｃと同じ方向成分で出力されていることが好ましい。レートジャイロ１２ｃと同じ方向成分で出力されていない場合には、座標変換手段を用いて、目標緯度と目標方位と目標姿勢を使用し、自転速度２５の出力座標と目標姿勢状態にある胴体３に設置されたレートジャイロ１２ｃの出力座標との間の座標変換を行う。以下、説明の簡略化のため、自転速度２５は、レートジャイロ１２ｃと同じ方向成分で出力されているとする。
【００４１】
図１１は、図１０の高精度姿勢検出装置２ｄの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。具体的には、動作生成手段２０から出力される自転速度２５の内、レートジャイロ１２ｃと同じ方向の成分「ωc_rot1」に基づき、レートジャイロ１２ｃの出力「ωc_sen」を補正し、補正角速度「ωc」を算出している。最終的には、この補正した角速度信号の補正角速度「ωc」に基づき、移動体の姿勢を高精度に検出している。
【００４２】
この場合、高精度姿勢検出装置２ｄの演算処理部の演算処理によって得られる補正した角速度信号の補正角速度「ωc」は、
ωc＝ωc_sen−ωc_rot1
であり、移動体が居る環境（星）の自転による自転速度成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する角速度（ωc）を求めることができる。
そのため、図１１に示す演算処理部の演算処理、具体的には
【数１】
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、レートジャイロ出力に自転による自転速度成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００４３】
図１２は、本発明の第５実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｅによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００４４】
第５実施例の高精度姿勢検出装置２ｅは、慣性検出手段１０と環境情報手段３０から構成されており、慣性検出手段１０には、２つの加速度センサ（１１ａ，１１ｂ）が備え付けられている。また、環境情報手段３０には、移動体と環境との間に作用する力を検出する力センサ３１が備え付けられている。図１２には図示していないが、力センサ３１の出力座標と胴体３に設置された加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換を行う座標変換手段４０が設けられている。各関節の関節角を検出するエンコーダやポテンショメータなどの関節角センサ３２、もしくは、同じく図示していない動作生成手段２０から出力されている各関節の関節角指令から、座標変換を行って、必要な方向成分を取り出すように逐次、演算されている。
【００４５】
図１３は、図１２の高精度姿勢検出装置２ｅの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。第５実施例の高精度姿勢検出装置２ｅの演算処理の内容は、第１実施例の高精度姿勢検出装置２ａや第２実施例の高精度姿勢検出装置２ｂの演算処理に類似している。相違点は、動作生成手段２０から出力されるデータを用いずに、環境情報手段３０からのデータを用いて演算処理を行う点である。以下に、具体的な処理の内容を説明する。
【００４６】
先ず、力センサ３１の出力である水平方向成分「Ｆx_sen」と垂直方向成分「Ｆz_sen」と既知重力加速度２２を用いて、動作を生成する加速度値「αx_mot」を求める。歩行ロボットなど、着地衝撃により垂直方向成分「Ｆz_sen」にノイズ成分が多く現れてしまうような場合には、力センサの垂直方向成分「Ｆz_sen」を用いずに、既知体重値を用いても実用上問題はない。次に、動作を生成する加速度値「αx_mot」を、座標変換手段４０による座標変換によって、加速度センサ（１１ａ，１１ｂ）と同じ方向成分の加速度値（αa_mot，αb_mot）に変換する。そして、この方向成分の加速度値「αa_mot」と加速度値「αb_mot」に基づき、加速度センサ（１１ａ，１１ｂ）の出力（αa_sen，αb_sen）を補正して、補正加速度「αa」と補正加速度「αb」を算出している。最終的には、この補正した加速度信号の補正加速度「αa」と補正加速度「αb」に基づき、移動体の姿勢を高精度に検出している。
【００４７】
この場合、力センサ３１を用いて算出する動作を生成する加速度値「αx_mot」は、胴体３の重力方向を含まず、胴体３の水平方向の加減速を生成する加速度値である。
【数２】
【００４８】
一方、胴体３が移動に伴って水平方向に加減速すると、その加速度値「αx_mot」の成分が、加速度センサ（１１ａ，１１ｂ）の出力に現れる。この影響を算出するため、座標変換手段４０により、力センサ３１の出力座標と加速度センサ（１１ａ，１１ｂ）の出力座標との間の座標変換「ba_Ｒ_xz」を行い、加速度値「αx_mot」を、加速度センサ（１１ａ，１１ｂ）と同じ方向成分（αa_mot，αb_mot）にそれぞれ変換している。
【数３】
【００４９】
最終的には、高精度姿勢検出装置２ｅの演算処理によって得られる補正した加速度信号の補正加速度「αa」と補正加速度「αb」は、それぞれ
αa＝αa_sen−αa_mot
αb＝αb_sen−αb_mot
であり、移動体自身の移動に伴う加減速成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb）を求めることができる。
そのため、図１３に示す演算処理部の演算処理、具体的には
姿勢角＝−ｔａｎ−１（αb／αa）
により姿勢角を求める。これにより、移動体自身が急な加減速を行う場合や旋回移動により遠心加速度が移動体に働く場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
【００５０】
なお、図１３では説明の簡略化のために、２次元図面により説明を簡略化して説明したが、３次元での高精度な姿勢検出を行う場合には、この２つの加速度センサ（１１ａ，１１ｂ）の両方に直交する更に別の加速度センサ１１ｃが備え付けられ、力センサ３１に関してもｘ方向の出力「Ｆx_sen」とｚ方向の出力「Ｆz_sen」の両方に直交するｙ方向の出力「Ｆy_sen」が得られるものが備え付けられていれば、３次元への拡張は容易である。このような拡張は、本発明の全ての実施例に対して有効であることは言うまでもないが、以下、高精度姿勢検出装置２ｅに関して説明をしておく。
具体的には、力センサ３１を用いて算出する動作を生成する加速度値を、
【数４】
【数５】
により算出し、
【数６】
のように、座標変換を行い、
αa＝αa_sen−αa_mot、
αb＝αb_sen−αb_mot、
αc＝αc_sen−αc_mot
のように、加速度センサを補償した後、
ｃ軸周りの姿勢角＝−ｔａｎ−１（αb／αa）
ｂ軸周りの姿勢角＝＋ｔａｎ−１（αc／αa）
のような計算により、３次元での胴体３の姿勢を高精度に検出することができる。更に、ノイズに対する信頼性を向上したい場合には、補正した加速度センサの加速度値（αa，αb，αc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することができる。
【００５１】
図１４は、本発明の第６実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｆによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００５２】
第６実施例の高精度姿勢検出装置２ｆは、第５実施例の高精度姿勢検出装置２ｅと同様に、慣性検出手段１０と環境情報手段３０から構成されており、慣性検出手段１０には、加速度センサ（１１ａ，１１ｂ）と、加速度センサ（１１ａ，１１ｂ）に直交する加速度センサ１１ｃ（図示せず）が備え付けられている。第５実施例の高精度姿勢検出装置２ｅとの相違点は、環境情報手段３０が、ＧＰＳセンサ３３と方位センサ３４から構成されると同時に、環境情報手段３０によって、移動体が居る環境（星）の自転速度２５と半径３５の情報が記憶されている。
【００５３】
環境情報手段３０におけるＧＰＳセンサ３３によって、移動体が居る緯度を検出しており、方位センサ３４により、移動体が向いている方向を検出している。更に、ＧＰＳセンサ３３の出力と、移動体が居る環境（星上：地球上に限らず月や火星などを含む全ての星上）の自転速度２５と半径３５の情報から、移動体の居る位置での遠心力が、逐次、演算されている。加えて、環境（床）と前記加速度センサとの間の座標変換を行う座標変換手段４１が設けられており、必要な座標変換を行う。すなわち、各関節の関節角を検出するエンコーダやポテンショメータなどの関節角センサ３２（図示せず）、同じく動作生成手段２０（図示せず）から出力されている各関節の関節角指令、または、同じく図示していない動作生成手段２０から出力されている胴体３の目標姿勢角と方位センサ３４の出力から遠心力が、逐次、演算されて、また、必要な座標変換が行われる。方位センサ３４は、２台のＧＰＳセンサ３３から構成されても良い。
【００５４】
図１５は、図１４の高精度姿勢検出装置２ｆの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。第６実施例の高精度姿勢検出装置２ｆの演算処理の内容は、高精度姿勢検出装置２ｃの演算処理に類似している。相違点は、動作生成手段２０の代わりに環境情報手段３０を用いている点である。以下に、具体的な処理の内容を説明する。
【００５５】
先ず、ＧＰＳセンサ３３の出力と、この移動体が居る環境（星）の既知情報である自転速度２５と半径３５の情報から、移動体の居る位置での遠心力（αx_cen2，αy_cen2，αz_cen2）を先ず算出する。この遠心力（αx_cen2，αy_cen2，αz_cen2）を、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と、方位センサ３４で検出される方位角から座標変換手段４１による座標変換を行い、加速度センサ（１１ｂ，１１ｃ，１１ａ）と同じ方向成分（αb_cen2，αc_cen2，αa_cen2）に変換する。そして、この方向成分（αb_cen2，αc_cen2，αa_cen2）に基づき、それぞれの加速度センサ（１１ｂ，１１ｃ，１１ａ）の出力（αb_sen，αc_sen，αa_sen）を補正し、補正加速度（αb，αc，αa）を算出している。最終的には、この補正した加速度信号の補正加速度（αb，αc，αa）に基づき、移動体の姿勢を高精度に検出する。
【００５６】
この場合、まず、ＧＰＳセンサ３３の出力「θ_gps」と、この移動体が居る環境（星）の自転速度２５の「ω_rot」と半径３５の「ｒ」の情報から、移動体の居る位置での遠心力を計算する。
【数７】
【００５７】
一方、移動体が自転している環境（星）に居ると、自転による遠心力の成分が、加速度センサ（１１ａ，１１ｂ，１１ｃ）の出力に現れる。この影響を算出するため、座標変換手段４１により、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と方位センサ３４で検出される方位角からの座標変換「bca_Ｒ_xyz」を用いて、遠心力（αx_cen2，αy_cen2，αz_cen2）を、加速度センサ（１１ｂ，１１ｃ，１１ａ）と同じ方向成分（αb_cen2，αc_cen2，αa_cen2）に変換している。
【数８】
【００５８】
最終的には、第６実施例の高精度姿勢検出装置２ｆの演算処理によって得られる補正した加速度信号の補正加速度（αa，αb，αc）は、それぞれ
αa＝αa_sen−αa_cen2
αb＝αb_sen−αb_cen2
αc＝αc_sen−αc_cen2
であり、移動体がいる環境（星）の自転に伴う遠心力を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb，αc）を求めることができる。
そのため、図１５に示す演算処理部の演算処理、具体的には
ｃ軸周りの姿勢角＝−ｔａｎ−１（αb／αa）
ｂ軸周りの姿勢角＝＋ｔａｎ−１（αc／αa）
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、加速度センサ出力に自転による遠心力成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。なお、更にノイズに対する信頼性を向上したい場合には、補正した加速度センサ値（αa，αb，αc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することができる。
【００５９】
図１６は、本発明の第７実施例の高精度姿勢検出装置の構成を概略的に示す図であり、高精度姿勢検出装置２ｇによって、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。
【００６０】
第７実施例の高精度姿勢検出装置２ｇは、第５実施例の高精度姿勢検出装置２ｅおよび第６実施例の高精度姿勢検出装置２ｆと同様に、慣性検出手段１０と環境情報手段３０から構成されている。これらの実施例の高精度姿勢検出装置（２ｅ，２ｆ）との相違点は、慣性検出手段１０に、レートジャイロ１２ｃと、当該レートジャイロ１２ｃに直交するレートジャイロ（１２ａ，１２ｂ；図１７に示す）が備え付けられていると同時に、環境情報手段３０が、ＧＰＳセンサ３３と方位センサ３４から構成されており、環境情報手段３０により、移動体が居る環境（星）の自転速度２５の情報が記憶されている点である。このＧＰＳセンサ３３により、移動体が居る緯度を検出している。また、方位センサ３４によって、移動体が向いている方向を検出している。加えて、座標変換手段４１により、必要な座標変換を行い、図示しないが、各関節の関節角を検出するエンコーダやポテンショメータなどの関節角センサ３２、動作生成手段２０から出力されている各関節の関節角指令、動作生成手段２０から出力されている胴体３の目標姿勢角と方位センサ３４の出力からの座標変換された回転角速度が、逐次、演算されている。方位センサ３４は、２台のＧＰＳセンサ３３から構成しても良い。
【００６１】
図１７は、図１５の高精度姿勢検出装置２ｇの演算処理部の構成を示すブロック図であり、歩行ロボットの胴体３の姿勢角を、高精度に検出する実施例の装置の構成を説明する図である。この高精度姿勢検出装置２ｇの演算処理内容は、高精度姿勢検出装置２ｄの演算処理に類似しており、相違点は動作生成手段２０の代わりに環境情報手段３０を用いている点である。以下に、具体的な処理の内容を説明する。
【００６２】
先ず、ＧＰＳセンサ３３の出力と、移動体が居る環境（星）の既知情報である自転速度２５の情報から、移動体の居る位置での自転速度の回転角速度（ωx_rot2，ωy_rot2，ωz_rot2）を算出する。この移動体の居る位置での回転角速度（ωx_rot2，ωy_rot2，ωz_rot2）を、座標変換手段４１により座標変換を行い、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と方位センサ３４で検出される方位角からの座標変換によって、レートジャイロ（１２ｂ，１２ｃ，１２ａ）と同じ方向成分（ωb_rot2，ωc_rot2，ωa_rot2）に変換する。そして、この方向成分（ωb_rot2，ωc_rot2，ωa_rot2）に基づき、レートジャイロ（１２ｂ，１２ｃ，１２ａ）の出力（ωb_sen，ωc_sen，ωa_sen）を補正し、補正角速度（ωb，ωc，ωa）を算出している。最終的には、この補正した角速度信号（ωb，ωc，ωa）に基づき、移動体の姿勢を高精度に検出している。
【００６３】
この場合、ＧＰＳセンサ３３の出力「θ_gps」と、この移動体が居る環境（星）の自転速度２５の「ω_rot」の情報から、移動体の居る位置での自転速度成分を計算する。
【数９】
【００６４】
一方、移動体が自転している環境（星）に居ると、この自転速度成分が、レートジャイロ（１２ａ，１２ｂ，１２ｃ）の出力に現れる。この影響を算出するため、座標変換手段４１により、関節角センサ３２の値から算出される足裏５と胴体３との間の姿勢角と方位センサ３４で検出される方位角からの座標変換の「bca_Ｒ_xyz」を用いて、自転速度成分（ωx_ro2t，ωy_rot2，ωz_rot2）を、レートジャイロ（１２ｂ，１２ｃ，１２ａ）と同じ方向成分（ωb_rot2，ωc_rot2，ωa_rot2）に変換している。
【数１０】
【００６５】
最終的には、高精度姿勢検出装置２ｇの演算処理によって得られる補正した角速度信号（ωb，ωc，ωa）は、それぞれ
ωa＝ωa_sen−ωa_rot2
ωb＝ωb_sen−ωb_rot2
ωc＝ωc_sen−ωc_rot2
であり、移動体がいる環境（星）の自転に伴う自転速度成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する角速度ωa，ωb，ωcを求めることができる。
そのため、図１７に示す演算処理部の演算処理、具体的には
【数１１】
により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、レートジャイロ出力に自転による自転速度成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。なお、より正確に姿勢角を求めたい場合には、補正した角速度値（ωa，ωb，ωc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢をより正確で高精度に検出することができる。
【００６６】
本発明の高精度姿勢検出装置は、更に、前述した第１実施例〜第７実施例の高精度姿勢検出装置を組み合わせることにより、より高精度な姿勢検出が可能となる。
具体的には、補正した加速度信号（αb，αc，αa）と補正した角速度信号（ωb，ωc，ωa）を次のようにして求める。
αa＝αa_sen−｛Ｗa1・αa_ref1
＋Ｗa2・（αa_ref2−αa_grav）＋（１−Ｗa1−Ｗa2）・αa_mot｝
αb＝αb_sen−｛Ｗb1・αb_ref1
＋Ｗb2・（αb_ref2−αb_grav）＋（１−Ｗb1−Ｗb2）・αb_mot｝
αa＝αa_sen−｛Ｗc1・αc_ref1
＋Ｗc2・（αc_ref2−αc_grav）＋（１−Ｗc1−Ｗc2）・αc_mot｝
ωa＝ωa_sen−｛Ｖa・ωa_rot1＋（１−Ｖa）ωa_rot2｝
ωb＝ωa_sen−｛Ｖb・ωb_rot1＋（１−Ｖb）ωb_rot2｝
ωc＝ωa_sen−｛Ｖc・ωc_rot1＋（１−Ｖc）ωc_rot2｝
但し、Ｗa1，Ｗa2，Ｗb1，Ｗb2，Ｗc1，Ｗc2，Ｖa，Ｖb，Ｖcは、
高精度姿勢検出装置２ａから２ｇを選択して使用する重み係数である。
【００６７】
これにより、加速度センサの出力から、移動体自身の移動に伴う加減速成分と移動体がいる環境（星）の自転に伴う遠心力を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する加速度（αa，αb，αc）を求めることができる。また、レートジャイロセンサの出力から、移動体がいる環境（星）の自転に伴う自転速度成分を除去することにより、高精度に歩行ロボットの胴体３の姿勢に起因する角速度（ωa，ωb，ωc）を求めることができる。この補正した加速度信号の補正加速度（αb，αc，αa）と補正した角速度信号の補正角速度（ωb，ωc，ωa）を、既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することが可能である。
【産業上の利用可能性】
【００６８】
以上に詳述したように、本発明による移動体の高精度姿勢検出装置は、移動体自身が急な加減速を行う場合、旋回移動により遠心加速度が移動体に働く場合、加減速する台車に移動体が搭乗している場合、加速度センサ自身に移動体の慣性加速度が作用している場合のいずれの場合であっても、姿勢角を高精度に検出する高精度姿勢検出装置を得ることができる。また、移動体が自転する環境（星上）に移動体が居て、レートジャイロセンサ自身に自転の回転角速度が影響する場合においても、姿勢角を高精度に検出する装置を得ることができる。このため、移動体である歩行ロボットの姿勢の安定制御を行うため、歩行ロボットの本体の加速度や姿勢回転角速度や姿勢角等を高精度に計測を行う場合などに好適に利用できる
【図面の簡単な説明】
【００６９】
【図１】本発明の第１実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図２】高精度姿勢検出装置２ａの演算処理部の構成を示すブロック図である。
【図３】本発明による高精度姿勢検出装置の動作を説明する第１の図である。
【図４】本発明による高精度姿勢検出装置の動作を説明する第２の図である。
【図５】本発明による高精度姿勢検出装置の動作を説明する第３の図である。
【図６】本発明の第２実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図７】高精度姿勢検出装置２ｂの演算処理部の構成を示すブロック図である。
【図８】本発明の第３実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図９】高精度姿勢検出装置２ｃの演算処理部の構成を示すブロック図である。
【図１０】本発明の第４実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１１】高精度姿勢検出装置２ｄの演算処理部の構成を示すブロック図である。
【図１２】本発明の第５実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１３】高精度姿勢検出装置２ｅの演算処理部の構成を示すブロック図である。
【図１４】本発明の第６実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１５】高精度姿勢検出装置２ｆの演算処理部の構成を示すブロック図である。
【図１６】本発明の第７実施例の高精度姿勢検出装置の構成を概略的に示す図である。
【図１７】高精度姿勢検出装置２ｇの演算処理部の構成を示すブロック図である。
【符号の説明】
【００７０】
２ａ〜２ｇ 高精度姿勢検出装置
３ ロボット胴体
４ａ 片脚の大腿リンク
４ｂ 片脚の下腿リンク
５ 片脚の足裏
６ａ 第１の関節モータ
６ｂ 第２の関節モータ
６ｃ 第３の関節モータ
１０ 慣性検出手段
１１ａ 加速度センサ
１１ｂ 加速度センサ
１１ｃ 加速度センサ
１２ａ レートジャイロ
１２ｂ レートジャイロ
１２ｃ レートジャイロ
２０ 動作生成手段
２５ 自転速度
３０ 環境情報手段
３２ 関節角センサ
３３ ＧＰＳセンサ
３４ 方位センサ
３５ 半径
４０ 座標変換手段
４１ 座標変換手段
【特許請求の範囲】
【請求項１】
移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体の動作を生成する動作生成手段と、移動体の姿勢演算を行う演算処理手段を備え、
前記演算処理手段により前記慣性検出手段の出力と前記動作生成手段の出力に基づき演算処理を行い、移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項２】
請求項１に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも動作の加速度目標値が出力されており、
前記演算処理手段は、前記加速度目標値に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項３】
請求項１に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転による遠心力が出力されており、
前記演算処理手段は、遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項４】
請求項１に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくともレートジャイロが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転速度が出力されており、
前記演算処理手段は、自転速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項５】
移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体が居る環境の情報を検出し記憶する環境情報手段と、移動体の姿勢演算を行う演算処理手段を備え、
前記演算処理手段により前記慣性検出手段の出力と環境情報手段の出力に基づき移動体の姿勢を高精度に検出することを特徴とする移動体の高精度姿勢検出装置。
【請求項６】
請求項５に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、
前記環境情報手段には、少なくとも移動体と環境との間に作用する力を検出する力センサと、前記力センサと前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、
前記演算処理手段は、力センサの出力から動作を生成する加速度値を演算し、演算した加速度値と前記座標変換手段の出力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項７】
請求項５に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、
前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境と前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、
前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による遠心力を演算し、演算した遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項８】
請求項５に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくともレートジャイロが備えられ、
前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境と前記レートジャイロとの間の座標系の変換を行う座標変換手段が備えられ、
前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による回転角速度を演算し、演算した回転角速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項９】
請求項１〜８の何れかに記載の移動体の高精度姿勢検出装置の複数の出力値が重み係数を付加して加算されて、移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項１０】
請求項１〜９の何れかに記載の移動体の高精度姿勢検出装置が搭載されている歩行ロボット。
【請求項１】
移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体の動作を生成する動作生成手段と、移動体の姿勢演算を行う演算処理手段を備え、
前記演算処理手段により前記慣性検出手段の出力と前記動作生成手段の出力に基づき演算処理を行い、移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項２】
請求項１に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも動作の加速度目標値が出力されており、
前記演算処理手段は、前記加速度目標値に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項３】
請求項１に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転による遠心力が出力されており、
前記演算処理手段は、遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項４】
請求項１に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくともレートジャイロが備えられ、前記動作生成手段の出力から少なくとも移動体が居る環境の自転速度が出力されており、
前記演算処理手段は、自転速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項５】
移動体に、当該移動体の慣性を検出する慣性検出手段と、当該移動体が居る環境の情報を検出し記憶する環境情報手段と、移動体の姿勢演算を行う演算処理手段を備え、
前記演算処理手段により前記慣性検出手段の出力と環境情報手段の出力に基づき移動体の姿勢を高精度に検出することを特徴とする移動体の高精度姿勢検出装置。
【請求項６】
請求項５に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、
前記環境情報手段には、少なくとも移動体と環境との間に作用する力を検出する力センサと、前記力センサと前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、
前記演算処理手段は、力センサの出力から動作を生成する加速度値を演算し、演算した加速度値と前記座標変換手段の出力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項７】
請求項５に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくとも加速度センサが備えられ、
前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境と前記加速度センサとの間の座標変換を行う座標変換手段が備えられ、
前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による遠心力を演算し、演算した遠心力に基づき慣性検出手段の加速度センサ出力を補正し、補正した加速度センサの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項８】
請求項５に記載の移動体の高精度姿勢検出装置において、
前記慣性検出手段には、少なくともレートジャイロが備えられ、
前記環境情報手段には、少なくとも移動体の緯度を検出するＧＰＳセンサと、方位を検出する方位センサと、環境と前記レートジャイロとの間の座標系の変換を行う座標変換手段が備えられ、
前記演算処理手段は、ＧＰＳセンサの出力、方位センサの出力、座標変換手段の出力により、移動体が居る位置での自転による回転角速度を演算し、演算した回転角速度に基づき慣性検出手段のレートジャイロ出力を補正し、補正したレートジャイロの出力に基づき移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項９】
請求項１〜８の何れかに記載の移動体の高精度姿勢検出装置の複数の出力値が重み係数を付加して加算されて、移動体の姿勢を高精度に検出する
ことを特徴とする移動体の高精度姿勢検出装置。
【請求項１０】
請求項１〜９の何れかに記載の移動体の高精度姿勢検出装置が搭載されている歩行ロボット。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【公開番号】特開２００７−７５９６７（Ｐ２００７−７５９６７Ａ）
【公開日】平成１９年３月２９日（２００７．３．２９）
【国際特許分類】
【出願番号】特願２００５−２６９１４０（Ｐ２００５−２６９１４０）
【出願日】平成１７年９月１５日（２００５．９．１５）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成１６年度、新エネルギー・産業技術総合開発機構、「基盤技術研究促進事業（民間基盤技術研究支援制度）実環境で働く人間型ロボット基盤技術の研究開発」、産業活力再生特別措置法第３０条の適用を受ける特許出願
【出願人】（３０１０２１５３３）独立行政法人産業技術総合研究所 (6,529)
【Ｆターム（参考）】
【公開日】平成１９年３月２９日（２００７．３．２９）
【国際特許分類】
【出願日】平成１７年９月１５日（２００５．９．１５）
【国等の委託研究の成果に係る記載事項】（出願人による申告）平成１６年度、新エネルギー・産業技術総合開発機構、「基盤技術研究促進事業（民間基盤技術研究支援制度）実環境で働く人間型ロボット基盤技術の研究開発」、産業活力再生特別措置法第３０条の適用を受ける特許出願
【出願人】（３０１０２１５３３）独立行政法人産業技術総合研究所 (6,529)
【Ｆターム（参考）】
[ Back to top ]