JP2004053530A

JP2004053530A - 移動体の高精度姿勢検出方法及びその装置

Info

Publication number: JP2004053530A
Application number: JP2002214401A
Authority: JP
Inventors: Kenji Kaneko; 金子　健二; Fumio Kanehiro; 金広　文男; Kazuhito Yokoi; 横井　一仁; Hideji Kajita; 梶田　秀司; Seiji Fujiwara; 藤原　清司; Hirohisa Hirukawa; 比留川　博久
Original assignee: National Institute of Advanced Industrial Science and Technology AIST
Current assignee: National Institute of Advanced Industrial Science and Technology AIST
Priority date: 2002-07-23
Filing date: 2002-07-23
Publication date: 2004-02-19
Also published as: KR20050045992A; EP1541965A4; WO2004010081A1; EP1541965A1; AU2003252225A1; US20060138988A1

Abstract

【課題】移動体における回転角速度や加速度等の姿勢情報を精度良く検出する高精度姿勢検出方法及びその装置を提供する。
【解決手段】移動体に、その姿勢を検出するレートジャイロ２ａと、該レートジャイロとは反転した出力が得られるもう一つのレートジャイロ２ｂとを一対とする高精度姿勢検出機構１を備え、上記両レートジャイロにおける出力変動を相互に相殺する演算処理を行うことにより、上記移動体の姿勢情報を高精度に検出する。
【選択図】　　　　図１

Description

【０００１】
【発明の属する技術分野】
本発明は、移動体の高精度姿勢検出方法及びその装置に関するものであり、特に、歩行ロボットの姿勢の安定制御を行うために、この歩行ロボットの本体の姿勢回転角速度や姿勢角等の姿勢情報を高精度に計測を行う場合などに好適に利用できる、移動体の高精度姿勢検出方法及びその装置に関するものである。
【０００２】
【従来の技術】
移動体の姿勢制御は、従来より、特開２００１−９７７２号公報にも示されているように、ロボット本体の一軸の姿勢に対して、一つのレートジャイロや一つの加速度センサを用いて姿勢角を検出し、その信号をフィードバックして、移動体の姿勢制御を行っていた。
【０００３】
ところが、レートジャイロや加速度センサ等の姿勢センサ出力には温度によるドリフトが存在する。そのため、移動体の姿勢制御を精度良く制御できないという問題があった。また、特に長時間に渡り移動体の姿勢制御を行う場合は、ドリフト量が顕著になるため、歩行ロボットにいたっては転倒を起こす可能性があった。
【０００４】
これらの問題を解決する第１の方法として、例えば、温度センサを用いて姿勢センサの出力を補正する方法が考えられる。
しかしながら、この第１の解決方法は、姿勢センサを駆動する電源の出力が安定しているシステムを備えた移動体には有効であるものの、姿勢センサの駆動電源の出力が変動し、その結果、姿勢センサ出力も変動する場合には、やはり移動体の姿勢制御を精度良く制御できないという問題があった。特に、小型軽量で限られた本体スペースに、制御用コンピュータ、センサ、駆動電源等を搭載している歩行ロボットの場合、制御用コンピュータでの演算処理による負荷変動が、駆動電源の出力変動を引き起こし、その結果、姿勢角の検出変動・ドリフトまでも引き起こしている。そのため、温度センサを用いた姿勢センサの出力を補正したところで、やはり、歩行ロボットは転倒を起こす可能性があった。
【０００５】
また、ドリフトの問題を解決する別の方法として、例えば、特開平０７−０２１８２６９号公報に示されているように、２つの差動回路のみならず、同期検波回路、平滑回路、移相回路を用いて、ドリフトを検出し補正する方法もある。
しかしながら、この解決方法は、回路が複雑になり、また、センサ出力をドリフト補償し制御演算処理等を行う姿勢制御装置に制御信号として取り込んだにも拘わらず、姿勢制御装置の駆動電源変動、すなわちアナログ・デジタル変換回路の基準電圧変動により、姿勢制御装置に取り込んだ制御信号として、やはりドリフトが発生する問題があった。
【０００６】
レートジャイロのドリフト問題の別の解決方法としては、例えば、レートジャイロと加速度センサとを併用して、カルマンフィルタ等の姿勢角検出アルゴリズムを構成し、姿勢センサのドリフトを補正しながら姿勢角を検出する方法が考えられる。
【０００７】
しかしながら、レートジャイロのドリフトを解消する役割の加速度センサ自体のドリフトを補正できないという問題があった。更には、カルマンフィルタ等の姿勢角検出アルゴリズムにおいては、静的な（もしくは低周波数領域の）姿勢角の補正が行われているため、移動体に動的な運動を行わせる場合、動的な（もしくは高周波数領域の）姿勢角を精度良く検出できないという問題も存在する。これらの問題により、カルマンフィルタ等の姿勢角検出アルゴリズムにより姿勢角を検出したところで、やはり、歩行ロボットにいたっては転倒を起こす可能性があった。
【０００８】
【発明が解決しようとする課題】
本発明は、このような問題を解決するためになされたものであり、本発明の第１の課題は、移動体における回転角速度や加速度等の姿勢情報を精度良く検出する高精度姿勢検出方法及びその装置を提供することにある。
本発明の第２の課題は、回転角速度センサや加速度センサ等の姿勢検出手段を駆動する電源と、姿勢を演算処理する手段を駆動する電源が共通の場合においても、移動体における回転角速度や加速度等の姿勢情報を精度良く検出することができる高精度姿勢検出方法及びその装置を提供することにある。
【０００９】
【課題を解決するための手段】
上記課題、特に第１の課題を解決するため、本発明によれば、移動体に、その姿勢を検出する姿勢検出手段と、該姿勢検出手段とは反転した出力が得られるもう一つの姿勢検出手段とを一対とする高精度姿勢検出機構を備え、上記両姿勢検出手段における出力変動を相互に相殺する演算処理を行うことにより、上記移動体の姿勢情報を高精度に検出することを特徴とする移動体の高精度姿勢検出方法が提供される。
【００１０】
また、上記課題を解決するため、本発明によれば、移動体の姿勢検出手段と、該姿勢検出手段と出力が反転して出力されるもう一つの姿勢検出手段とを備え、これら姿勢検出手段を一対とする高精度姿勢検出機構を少なくとも一つ形成し、上記高精度姿勢検出機構に両姿勢検出手段における出力変動を相互に相殺する演算処理機能を持たせることより、移動体の姿勢情報を高精度に検出可能にしたことを特徴とする移動体の高精度姿勢検出装置が提供される。
【００１１】
このような本発明の方法および装置によれば、姿勢検出手段の出力変動やドリフトは、高精度姿勢検出機構を形成する一対の姿勢検出手段の演算処理により相互に相殺することができ、温度ドリフト、姿勢検出手段に供給する駆動電源の変動がある場合でも、精度良く移動体の姿勢を検出する方法及びその装置を構成できる。
【００１２】
上記本発明に係る高精度姿勢検出装置の一つの具体的構成態様としては、上記姿勢検出手段がレートジャイロであって、高精度姿勢検出機構を形成する一対のレートジャイロが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号により、移動体の姿勢情報、具体的には、回転角速度、姿勢角または回転角加速度が検出される装置がある。
【００１３】
本発明の他の具体的構成態様としては、上記姿勢検出手段が加速度センサであって、高精度姿勢検出機構を形成する一対の加速度センサが互いに反対向きに移動体に設置されており、これら加速度センサの出力信号の差動信号により、移動体の姿勢情報、具体的には、加速度、並進速度、並進移動量または姿勢角が検出される装置がある。
【００１４】
本発明の他の具体的構成態様としては、上記姿勢検出手段が傾斜センサであって、高精度姿勢検出機構を形成する一対の傾斜センサが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号により、移動体の姿勢情報、具体的には、傾斜角、回転角速度または回転角加速度が検出される装置がある。
【００１５】
本発明の他の具体的構成態様としては、上記姿勢検出手段が一対のレートジャイロと一対の加速度センサとにより形成され、高精度姿勢検出機構を形成する一対のレートジャイロと一対の加速度センサとは、それぞれが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号と加速度センサの出力信号の差動信号とにより、移動体の姿勢情報、具体的には、傾斜角、回転角速度または回転角加速度が検出される装置がある。
【００１６】
本発明の更に他の具体的構成態様としては、上記姿勢検出手段が一対のレートジャイロと一対の傾斜センサとにより形成され、高精度姿勢検出機構を形成する一対のレートジャイロと一対の傾斜センサとは、それぞれが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号と傾斜センサの出力信号の差動信号とにより、移動体の姿勢情報、具体的には、傾斜角、回転角速度または回転角加速度が検出される装置がある。
【００１７】
また、上記課題、特に第２の課題を解決するため、本発明によれば、移動体に、その姿勢検出手段の出力を入力信号として移動体の姿勢を検出する制御演算処理機構を備え、上記姿勢検出手段と制御演算処理機構とを共通の駆動電源により駆動することを特徴とする上記移動体の高精度姿勢検出方法、並びに、少なくとも一対の移動体の姿勢検出手段を備えると共に、これら姿勢検出手段からの出力を入力信号として移動体の姿勢を検出する制御演算処理機構を備え、上記姿勢検出手段と制御演算処理機構とを共通の駆動電源に接続したことを特徴とする上記移動体の高精度姿勢検出装置が提供される。
【００１８】
このような本発明の高精度姿勢検出方法及びその装置によれば、姿勢検出手段の出力変動やドリフトに限らず、制御演算処理機構への取込時の変動ドリフトは制御演算処理機構により相互に相殺することができ、温度ドリフト、姿勢検出手段に供給する駆動電源の変動、制御演算処理機構への取込時の変動ドリフトがある場合でも、精度良く移動体の姿勢を検出することができる。
【００１９】
また、本発明によれば、移動体が歩行ロボットであることを特徴とする上記移動体の高精度姿勢検出装置が提供される。
このような本発明によれば、歩行ロボットの姿勢を高精度に検出することができるため、安定した歩行ロボットの姿勢制御を実現することができる。
【００２０】
【発明の実施の形態】
図１は、本発明に係る第１の高精度姿勢検出装置を備えた歩行ロボット（移動体）の一例を概念的に示している。この歩行ロボットは、胴体３を支える脚リンク４ａ，４ｂの下端に足部５を備え、それらをアクチュエータ６ａ，６ｂ，６ｃを介して相互に連結したものである。このロボットにおける上記胴体３に設けた高精度姿勢検出機構１は、姿勢検出手段としてレートジャイロ２ａ，２ｂを用い、その高精度姿勢検出機構１によって、歩行ロボットの胴体３の回転角速度や姿勢角等の姿勢情報を高精度に検出するもので、上記一対のレートジャイロ２ａ，２ｂが互いに反対向きに移動体に設置され、つまり、一方のレートジャイロ２ａに対して、他方のレートジャイロ２ｂから反転した出力が得られるようにしている。
【００２１】
この場合、レートジャイロ２ａ，２ｂからの出力は、それぞれ、
ω１＝＋ω＋ωｏｆｆｓｅｔ
ω２＝−ω＋ωｏｆｆｓｅｔ
である。ここで、ωは胴体３の実際の回転角速度、ωｏｆｆｓｅｔは温度ドリフトやレートジャイロ２ａ，２ｂに供給する駆動電源の変動による出力変動である。
【００２２】
図２の高精度姿勢検出機構１Ａは、上記高精度姿勢検出機構１の演算処理をレートジャイロ２ａ，２ｂの出力信号の差動により行う場合を説明するためのもので、レートジャイロ２ａ，２ｂを含む高精度姿勢検出機構１Ａを駆動する単一の駆動電源１０を備え、上記レートジャイロ２ａ，２ｂの出力信号を加算要素７において加算（減算）することにより、それらの出力変動を相殺したうえで、乗算要素８Ａにおいて１／２を乗じる演算処理を行うことにより、歩行ロボットの胴体３の回転角速度が高精度に検出されるものである。
【００２３】
この場合、高精度姿勢検出機構１Ａの演算処理によって得られる出力：Ｖｏｕｔは、
Ｖｏｕｔ＝ω
であり、レートジャイロ２ａ，２ｂの温度ドリフトやレートジャイロ２ａ，２ｂに供給する駆動電源１０の変動による出力変動（ωｏｆｆｓｅｔ）を補正し、高精度に歩行ロボットの胴体３の回転角速度を求めることができる。
【００２４】
図３は、上記図２の高精度姿勢検出機構１Ａに代わる高精度姿勢検出機構１Ｂの演算処理として、レートジャイロ２ａ，２ｂの出力信号の差動信号を積分する処理を行う場合を示し、レートジャイロ２ａ，２ｂを含む高精度姿勢検出機構１Ｂを駆動する単一の駆動電源１０を備え、上記レートジャイロ２ａ，２ｂの出力信号を加算要素７において加算（減算）することにより、それらの出力変動を相殺したうえで、積分要素８Ｂにおいて積分の演算処理を行うことにより、歩行ロボットの胴体３の姿勢角が高精度に検出されるようにしている。
【００２５】
この場合、高精度姿勢検出機構１Ｂの演算処理によって得られる出力：Ｖｏｕｔは、
【数１】

であり、レートジャイロ２ａ，２ｂの温度ドリフトやレートジャイロ２ａ，２ｂに供給する駆動電源１０の変動による出力変動（ωｏｆｆｓｅｔ）を補正し、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。
また、図３の積分要素８Ｂの代わりに、図４に示す如く、微分要素８Ｃにおいて微分の演算処理を行うと、高精度姿勢検出機構１Ｃの演算処理によって得られる出力：Ｖｏｕｔは、上式の積分を微分したものとなり、この場合も、レートジャイロ２ａ，２ｂの温度ドリフトやレートジャイロ２ａ，２ｂに供給する駆動電源１０の変動による出力変動（ωｏｆｆｓｅｔ）を補正し、歩行ロボットの胴体３の回転角加速度を高精度に求めることができる。
【００２６】
なお、図１の姿勢検出手段が、傾斜センサであっても、図２の高精度姿勢検出機構１Ａの演算処理と同じ原理で、歩行ロボットの胴体３の姿勢角を、高精度に検出することができる。
また、図１の姿勢検出手段が、傾斜センサであっても、図４の高精度検出機構１Ｃの演算処理と同じ原理により、歩行ロボットの胴体３の回転角速度や回転角加速度を、高精度に検出できることは勿論である。
【００２７】
図５の実施例は、前記図１の歩行ロボットの胴体３に第２の高精度姿勢検出機構１１を設けた場合を示すもので、この第２の高精度姿勢検出機構１１における姿勢検出手段としては、一対の加速度センサ１２ａ，１２ｂを用い、その高精度姿勢検出機構１１によって、歩行ロボットの胴体３の姿勢情報を高精度に検出するようにしている。この高精度姿勢検出機構１１においては、具体的には、上記一対の加速度センサ１２ａ，１２ｂが互いに反対向きに設置され、一方の加速度センサ１２ａに対して他方の加速度センサ１２ｂから反転した出力が得られるようにしている。
なお、この実施例における歩行ロボット自体の構成は、図１の場合と変るところがないので、図中の対応部分に同一の符号を付してその説明を省略する。以下において説明する各実施例についても同様である。
【００２８】
上記実施例の場合、加速度センサ１２ａ，１２ｂからは、それぞれ、
α１＝＋α＋αｏｆｆｓｅｔ
α２＝−α＋αｏｆｆｓｅｔ
なる出力が出る。ここで、αは胴体３の実際の平進加速度、αｏｆｆｓｅｔは、温度ドリフトや加速度センサ１２ａ，１２ｂに供給する駆動電源の変動による出力変動である。
【００２９】
図６の高精度姿勢検出機構１１Ａは、上記高精度姿勢検出機構１１の演算処理を加速度センサ１２ａ，１２ｂの出力信号の差動により行う態様を説明するためのもので、加速度センサ１２ａ，１２ｂを含む高精度姿勢検出機構１１Ａを駆動する単一の駆動電源１０を備え、上記加速度センサ１２ａ，１２ｂの出力信号を加算要素１３において加算（減算）することにより、それらの出力変動を相殺したうえで、乗算要素１４において１／２を乗じる演算処理を行うことにより、歩行ロボットの胴体３の平進加速度が高精度に検出されるようにしている。
【００３０】
ここで、高精度姿勢検出機構１１Ａの演算処理によって得られる出力：Ｖｏｕｔは、
Ｖｏｕｔ＝α
であり、加速度センサ１２ａ，１２ｂの温度ドリフトや加速度センサ１２ａ，１２ｂに供給する駆動電源１０の変動による出力変動を補正し、高精度に歩行ロボットの胴体３の加速度を求めることができる。
【００３１】
図７は、上記図６の高精度姿勢検出機構１１Ａに代わる高精度姿勢検出機構１１Ｂの演算処理として、加速度センサ１２ａ，１２ｂの出力信号の差動信号を積分する処理を行う場合を説明するためのもので、加速度センサ１２ａ，１２ｂを含む高精度姿勢検出機構１１Ｂを駆動する単一の駆動電源１０を備え、上記加速度センサ１２ａ，１２ｂの出力信号を加算要素１３において加算（減算）することにより、それらの出力変動やドリフトを相殺したうえで、積分要素１５において積分の演算処理を行うことにより、歩行ロボットの胴体３の並進移動量が高精度に検出されるようにしている。
【００３２】
ここで、高精度姿勢検出機構１１Ｂの演算処理によって得られる出力：Ｖｏｕｔは、
【数２】

であり、加速度センサ１２ａ，１２ｂの温度ドリフトや加速度センサ１２ａ，１２ｂに供給する駆動電源１０の変動による出力変動を補正し、高精度に歩行ロボットの胴体３の並進移動量を求めることができる。
なお、図７の積分要素１５を二重積分ではなく、一重積分にすると、高精度姿勢検出機構１１Ｂの演算処理によって得られる出力：Ｖｏｕｔは、上式の二重積分を一重積分にしたものとなり、加速度センサ１２ａ，１２ｂの温度ドリフトや加速度センサ１２ａ，１２ｂに供給する駆動電源１０の変動による出力変動を補正し、高精度に歩行ロボットの胴体３の並進速度を求めることができる。
【００３３】
図８の実施例は、前記図１の歩行ロボットの胴体３に設ける第３の高精度姿勢検出機構２１として、その姿勢検出手段に一対の加速度センサ２２ａ，２２ｂおよび一対の加速度センサ２２ｃ，２２ｄを用い、各一対の加速度センサを備えた高精度姿勢検出機構２１ａ，２１ｂによって、歩行ロボットの胴体３の姿勢角を高精度に検出できるようにしたものである。
高精度姿勢検出機構２１ａは、一対の加速度センサ２２ａ，２２ｂが互いに反対向きに設置され、また高精度姿勢検出機構２ｂも一対の加速度センサ２２ｃ，２２ｄが互いに反対向きに設置され、両高精度姿勢検出機構２１ａ，２１ｂは互いに直交して設置され、高精度姿勢検出機構２１が形成されている。
【００３４】
図９は、図８の高精度姿勢検出機構２１における加速度センサ出力の演算処理の態様を説明するためのもので、この演算処理により歩行ロボットの胴体３の姿勢角が検出される。
具体的には、この高精度姿勢検出機構２１には、加速度センサ２２ａ〜２２ｄを含む高精度姿勢検出機構２１ａ，２１ｂを駆動する単一の駆動電源１０を備え、加速度センサ２２ａ，２２ｂの出力信号および加速度センサ２２ｃ，２２ｄの出力信号を、それぞれ加算要素２３ａ，２３ｂにおいて加算（減算）することにより、それらの出力変動やドリフトを相殺したうえで、乗算要素２４ａ，２４ｂにおいて１／２を乗じる演算処理を行うことにより、歩行ロボットの胴体３の向きを異にする高精度な加速度信号Ｖｏｕｔ−１およびＶｏｕｔ−２加速度が検出されるよう
【００３５】
この場合、加速度センサ２２ａ，２２ｂの出力信号の差動によって得られる加速度信号Ｖｏｕｔ−１と、加速度センサ２２ｃ，２２ｄの出力信号の差動によって得られる加速度信号Ｖｏｕｔ−２は、それぞれ、
Ｖｏｕｔ−１＝αｚ
Ｖｏｕｔ−２＝αｘ
であり、上記差動により、加速度センサ２２ａ，２２ｂ，２２ｃ，２２ｄの温度ドリフトやそれらの加速度センサに供給する駆動電源１０の出力変動を補正し、高精度に歩行ロボットの胴体３の加速度αｚ，αｘを求めることができる。
【００３６】
また、上記高精度姿勢検出機構２ａ，２ｂは互いに直交して設置されているため、各高精度姿勢検出機構の出力を自乗した総和の平方根は、静的に重力加速度と一致する。そのため、２次元であれば、図９のような演算処理、具体的には、演算要素２５および２６による、
姿勢角（Ｖｏｕｔ）＝ｔａｎ^―１（Ｖｏｕｔ−２／Ｖｏｕｔ−１）
の演算により、高精度に歩行ロボットの胴体３の静的な姿勢角信号を求めることができる。この姿勢角信号が、加速度センサ２２ａ，２２ｂ，２２ｃ，２２ｄの温度ドリフトや加速度センサに供給する駆動電源１０の変動による出力変動を補正されたものであることは勿論である。
【００３７】
図１０の実施例は、歩行ロボットの胴体３に設ける第４の高精度姿勢検出機構３１における姿勢検出手段として、一対のレートジャイロ３２ａ，３２ｂと、二対の加速度センサ３３ａ，３３ｂおよび３３ｃ，３３ｄを用いた場合を示し、上記一対のレートジャイロを備える高精度姿勢検出機構３１ａ、上記加速度センサの各一対を備える高精度姿勢検出機構３１ｂ，３１ｃによって、歩行ロボットの胴体３の姿勢角を高精度に検出するようにしている。
上記高精度姿勢検出機構３１ｂは、一対の加速度センサ３３ａ，３３ｂが互いに反対向きに設置され、高精度姿勢検出機構３１ｃは、一対の加速度センサ３３ｃ，３３ｄが互いに反対向きに設置され、しかも、これらの高精度姿勢検出機構３１ｂ，３１ｃは互いに直交して設置されたものである。一方、高精度姿勢検出機構３１ａは、一対のレートジャイロ３２ａ，３２ｂが互いに反対向きに設置されたものであり、これらの高精度姿勢検出機構３１ａ，３１ｂ，３１ｃによって高精度姿勢検出機構３１が形成されている。
【００３８】
図１１は、図１０の高精度姿勢検出機構３１における演算処理の態様を説明するためのもので、この演算処理により歩行ロボットの胴体３の姿勢角が高精度に検出される。具体的には、加速度センサ３３ａ，３３ｂ，３３ｃ，３３ｄの出力信号を、演算要素３４において、例えば図９により説明したような演算処理を行って、低周波域でのロボット胴体３の姿勢角を演算し、また、レートジャイロ３２ａ，３２ｂの出力信号を、演算要素３６において、例えば図３により説明したような演算処理を行い、高周波域におけるロボット胴体３の姿勢角を演算し、それぞれの姿勢角をローパスフィルタ３５及びハイパスフィルタ３７を通して加算要素３８において合算し、ロボット胴体３の姿勢角を高精度に検出して出力させるようにしている。
【００３９】
また、上記高精度姿勢検出機構３１ａ，３１ｂ，３１ｃにおいて高精度な姿勢センサ出力を演算後、既存のカルマンフィルタを構成しても、歩行ロボットの胴体３の姿勢角を、高精度に検出する装置を形成することができる。
更には、図１０及び図１１の高精度姿勢検出機構３１の処理の中に微分演算要素を加えることにより、歩行ロボットの胴体３の回転角速度、回転角加速度を高精度に検出することができる。
なお、図１０及び図１１の加速度センサ３３ａ〜３３ｄの代わりに、傾斜センサを用いることができ、この場合も、図１１と同様な原理によって歩行ロボットの胴体３の姿勢角を高精度に検出する装置を形成することができる。
【００４０】
図１２の実施例では、歩行ロボットの胴体３に、第５の高精度姿勢検出機構４１における姿勢検出手段として、一対のレートジャイロ４２ａ，４２ｂを設けると共に、制御演算処理機構４３、レートジャイロ４２ａ，４２ｂと上記制御演算処理機構４３を駆動する共通駆動電源４０を備え、これらによって歩行ロボットの胴体３の回転角速度や姿勢角等を、高精度に検出するようにしている。上記一対のレートジャイロ４２ａ，４２ｂは、互いに反対向きに設置されているのは、前記実施例と同様である。
【００４１】
この場合、レートジャイロ４２ａ，４２ｂからは、それぞれ、
ω１＝＋ω＋ωｏｆｆｓｅｔ
ω２＝−ω＋ωｏｆｆｓｅｔ
なる出力がある。ここで、ωは胴体３の実際の回転角速度、ωｏｆｆｓｅｔは温度ドリフトやレートジャイロ４２ａ，４２ｂ及び制御演算処理機構４３に供給する共通駆動電源４０の変動による出力変動である。
【００４２】
図１３は、図１２の高精度姿勢検出機構４１における演算処理の態様を説明するためのもので、この実施例では、レートジャイロ４２ａ，４２ｂの出力信号の差動により、歩行ロボットの胴体３の回転角速度を高精度に検出するようにしている。
具体的には、レートジャイロ４２ａ，４２ｂの出力信号を、制御演算処理機構４３において、それぞれＤ／Ａコンバータ４４ａ，４４ｂを経て加算要素４５において加算（減算）し、演算要素４６において１／（２×ＤＡＧａｉｎ）を乗じる演算処理を行い、歩行ロボット胴体３の回転角速度が高精度に検出される。
【００４３】
上記レートジャイロ４２ａ，４２ｂの出力を、制御演算処理機構４３に取り込んだ際の信号は、それぞれ、
ＤＡω１＝ＤＡＧａｉｎ×（＋ω＋ωｏｆｆｓｅｔ）＋ＤＡｏｆｆｓｅｔ
ＤＡω２＝ＤＡＧａｉｎ×（−ω＋ωｏｆｆｓｅｔ）＋ＤＡｏｆｆｓｅｔ
である。
ここで、ＤＡＧａｉｎは制御演算処理機構４３での取り込みゲイン、ＤＡωｏｆｆｓｅｔは共通駆動電源４０の変動によって発生するＤＡコンバータの基準電圧変動等により制御演算処理機構４３に取り込んだ際に発生する出力変動である。
【００４４】
そして、制御演算処理機構４３の内部演算処理によって得られる出力：ＤＡｏｕｔは、
ＤＡｏｕｔ＝ω
であり、これにより、レートジャイロ４２ａ，４２ｂの温度ドリフトやレートジャイロ４２ａ，４２ｂに供給する駆動電源の変動、さらには制御演算処理機構４３に取り込んだ際に発生する変動ドリフトを補正し、高精度に歩行ロボットの胴体３の回転角速度を求めることができる。
【００４５】
また、図１２の歩行ロボットにおいて、制御演算処理機構４３の内部演算処理を、図３のように積分機能を備えたものとすることができ、これにより、歩行ロボットの胴体３の姿勢角を、高精度に検出する装置を構成することができる。
更には、制御演算処理機構４３の内部演算処理を、図４のように微分機能を備えたものにすれば、歩行ロボットの胴体３の回転角加速度を高精度に検出する装置を構成することができる。
なお、図１２の姿勢検出手段が、加速度センサであっても、あるいは傾斜センサであっても、図１３の制御演算処理機構４３の演算処理と同じ原理で、歩行ロボットの姿勢情報を、高精度に検出する装置を構成することができる。
また、図１２の姿勢検出手段が加速度センサであって、制御演算処理機構４３の内部演算処理が、図７のような積分機能を備えていれば、歩行ロボットの胴体３の並進移動量や並進速度を高精度に検出する装置が構成される。
【００４６】
図１４の実施例は、歩行ロボットの胴体３に設ける第６の高精度姿勢検出機構５１の姿勢検出手段として、一対の加速度センサ５２ａ，５２ｂ及び一対の加速度センサ５２ｃ，５２ｄを用い、上記各一対の加速度センサを備える高精度姿勢検出機構５１ａ，５１ｂによって、歩行ロボットの胴体３の姿勢情報、具体的には、加速度、並進速度、並進移動量または姿勢角を、高精度に検出可能にしたものである。
具体的には、上記高精度姿勢検出機構５１は、歩行ロボットの胴体３の内部に上記一対の加速度センサ５２ａ，５２ｂ及び一対の加速度センサ５２ｃ，５２ｄを有する高精度姿勢検出機構５１ａ，５１ｂを備えると共に、制御演算処理機構５３、及び上記加速度センサと制御演算処理機構５３を駆動する共通駆動電源５０を備え、上記一対の加速度センサ５２ａ，５２ｂが互いに反対向きに設置され、一対の加速度センサ５２ｃ，５２ｄも互いに反対向きに設置され、それらによって構成される高精度姿勢検出機構５１ａ，５１ｂが互いに直交して設置されている。
【００４７】
図１５は、図１４の高精度姿勢検出機構５１における制御演算処理機構５３の演算処理の態様を説明するためのもので、この高精度姿勢検出機構５１においては、加速度センサ５２ａ，５２ｂの出力信号を制御演算処理機構５３に取り込んで、それらの差動によって得られる高精度な加速度信号ＤＡｏｕｔ−１と、加速度センサ５２ｃ，５２ｄの出力信号を制御演算処理機構５３に取り込んで、それらの差動によって得られる高精度な加速度信号ＤＡｏｕｔ−２から、歩行ロボットの胴体３の姿勢角を、高精度に検出するようにしている。
具体的には、制御演算処理機構５３において、加速度センサ５２ａ，５２ｂの出力信号をそれぞれＤ／Ａコンバータ５４ａ，５４ｂを経て加算要素５５ａにおいて加算（減算）し、また、加速度センサ５２ｃ，５２ｄの出力信号をそれぞれＤ／Ａコンバータ５４ｃ，５４ｄを経て加算要素５５ｂにおいて加算（減算）したうえで、演算要素５６ａ及び５６ｂにおいて、それらに１／（２×ＤＡＧａｉｎ）を乗じる演算処理を行い、上記高精度加速度信号ＤＡｏｕｔ−１及びＤＡｏｕｔ−２を得る。
【００４８】
この場合、加速度センサ５２ａ，５２ｂを制御演算処理機構５３に取り込み差動処理して得られた高精度加速度信号ＤＡｏｕｔ−１は、
ＤＡｏｕｔ−１＝αｚ
であり、加速度センサ５２ｃ，５２ｄを制御演算処理機構５３に取り込み差動処理して得られた高精度加速度信号ＤＡｏｕｔ−２は、
ＶＡｏｕｔ−２＝αｘ
である。
【００４９】
従って、加速度センサ５２ａ〜５２ｄの温度ドリフト、それらの加速度センサに供給する駆動電源の変動、さらには加速度センサ５２ａ〜５２ｄと制御演算処理機構５３に共通の共通駆動電源５０の変動によって発生する制御演算処理機構５３内での変動ドリフトを補正し、高精度に歩行ロボットの胴体３の加速度αｚ，αｘを求めることができる。
【００５０】
また、一対の加速度センサ５２ａ，５２ｂと、一対の加速度センサ５２ｃ，５２ｄとは、互いに直交して設置されているため、制御演算処理機構５３内で演算処理された高精度加速度センサ信号ＤＡｏｕｔ−１と高精度加速度センサ信号ＤＡｏｕｔ−２の出力を自乗した総和の平方根は、静的に重力加速度と一致する。そのため、２次元であれば、図１５のような演算処理、具体的には、演算要素５７および５８による、
姿勢角（ＤＡｏｕｔ）＝ｔａｎ^―１（ＤＡｏｕｔ−２／ＤＡｏｕｔ−１）
の演算により、加速度センサ５２ａ〜５２ｄの温度ドリフト、それらの加速度センサ等に供給する共通駆動電源５０の変動によるセンサ出力変動、さらには、上記共通駆動電源５０の変動によって発生する制御演算処理機構５３内での変動ドリフトを補正し、高精度に歩行ロボットの胴体３の静的な姿勢角を求めることができる。
なお、３次元の場合でも、既存の計算方法により、高精度に歩行ロボットの胴体３の静的な姿勢角を求めることができる。
【００５１】
【発明の効果】
以上に詳述したように、本発明によれば、姿勢検出手段に温度ドリフトや姿勢検出手段に供給する駆動電源の変動による出力変動が存在する場合においても、移動体の姿勢検出手段と、該姿勢検出手段とは反転した出力が得られるもう一つの姿勢検出手段とを備え、両姿勢検出手段における出力変動を相互に相殺する演算処理を行うようにしているので、移動体の姿勢情報を高精度に検出することができる。
【００５２】
また、姿勢検出手段に、温度ドリフト、姿勢検出手段に供給する駆動電源の変動、制御演算処理機構での取り込み時に発生する変動ドリフト等が存在する場合においても、移動体の姿勢検出手段と、該姿勢検出手段とは反転した出力が得られるもう一つの姿勢検出手段とを備え、姿勢検出手段と制御演算処理機構が共通の駆動電源で駆動されるようにしているので、制御演算処理機構により移動体の姿勢を高精度に検出することができる。
【図面の簡単な説明】
【図１】本発明の第１の高精度姿勢検出機構を備えた歩行ロボットの一例を概念的に示す構成図である。
【図２】図１の高精度姿勢検出機構の具体的構成を例示するブロック構成図である。
【図３】図１の高精度姿勢検出機構の他の具体的構成例を示すブロック構成図である。
【図４】図１の高精度姿勢検出機構の更に他の具体的構成例を示すブロック構成図である。
【図５】本発明の第２の高精度姿勢検出機構を備えた歩行ロボットの一例を概念的に示す構成図である。
【図６】図５の高精度姿勢検出機構の具体的構成を例示するブロック構成図である。
【図７】図５の高精度姿勢検出機構の他の具体的構成例を示すブロック構成図である。
【図８】本発明の第３の高精度姿勢検出機構を備えた歩行ロボットの一例を概念的に示す構成図である。
【図９】図８の高精度姿勢検出機構の具体的構成を例示するブロック構成図である。
【図１０】本発明の第４の高精度姿勢検出機構を備えた歩行ロボットの一例を概念的に示す構成図である。
【図１１】図１０の高精度姿勢検出機構の具体的構成を例示するブロック構成図である。
【図１２】本発明の第５の高精度姿勢検出機構を備えた歩行ロボットの一例を概念的に示す構成図である。
【図１３】図１２の高精度姿勢検出機構の具体的構成を例示するブロック構成図である。
【図１４】本発明の第６の高精度姿勢検出機構を備えた歩行ロボットの一例を概念的に示す構成図である。
【図１５】図１４の高精度姿勢検出機構の具体的構成を例示するブロック構成図である。
【符号の説明】
１，１Ａ，１Ｂ，１Ｃ，１１，１１Ａ，１１Ｂ，２１，２１ａ，２１ｂ，３１，３１ａ〜３１ｃ，４１，５１，５１ａ，５１ｂ　　　高精度姿勢検出機構
２ａ，２ｂ，３２ａ，３２ｂ，４２ａ，４２ｂ　　レートジャイロ
１２ａ，１２ｂ，２２ａ〜２２ｄ，３３ａ〜３３ｄ，５２ａ〜５２ｄ　加速度センサ

Claims

移動体に、その姿勢を検出する姿勢検出手段と、該姿勢検出手段とは反転した出力が得られるもう一つの姿勢検出手段とを一対とする高精度姿勢検出機構を備え、上記両姿勢検出手段における出力変動を相互に相殺する演算処理を行うことにより、上記移動体の姿勢情報を高精度に検出することを特徴とする移動体の高精度姿勢検出方法。
移動体に、その姿勢検出手段の出力を入力信号として移動体の姿勢を検出する制御演算処理機構を備え、上記姿勢検出手段と制御演算処理機構とを共通の駆動電源により駆動することを特徴とする請求項１に記載の移動体の高精度姿勢検出方法。
移動体の姿勢検出手段と、該姿勢検出手段と出力が反転して出力されるもう一つの姿勢検出手段とを備え、これら姿勢検出手段を一対とする高精度姿勢検出機構を少なくとも一つ形成し、上記高精度姿勢検出機構に両姿勢検出手段における出力変動を相互に相殺する演算処理機能を持たせることより、移動体の姿勢情報を高精度に検出可能にしたことを特徴とする移動体の高精度姿勢検出装置。
上記姿勢検出手段がレートジャイロであって、高精度姿勢検出機構を形成する一対のレートジャイロが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号により、移動体の姿勢情報を検出することを特徴とする請求項３に記載の移動体の高精度姿勢検出装置。
上記姿勢検出手段が加速度センサであって、高精度姿勢検出機構を形成する一対の加速度センサが互いに反対向きに移動体に設置されており、これら加速度センサの出力信号の差動信号により、移動体の姿勢情報を検出することを特徴とする請求項３に記載の移動体の高精度姿勢検出装置。
上記姿勢検出手段が傾斜センサであって、高精度姿勢検出機構を形成する一対の傾斜センサが互いに反対向きに移動体に設置されており、これら傾斜センサの出力信号の差動信号により、移動体の姿勢情報を検出することを特徴とする請求項３に記載の移動体の高精度姿勢検出装置。
上記姿勢検出手段が一対のレートジャイロと一対の加速度センサとにより形成され、高精度姿勢検出機構を形成する一対のレートジャイロと一対の加速度センサとは、それぞれが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号と加速度センサの出力信号の差動信号とにより、移動体の姿勢情報を検出することを特徴とする請求項３に記載の移動体の高精度姿勢検出装置。
上記姿勢検出手段が一対のレートジャイロと一対の傾斜センサとにより形成され、高精度姿勢検出機構を形成する一対のレートジャイロと一対の傾斜センサとは、それぞれが互いに反対向きに移動体に設置されており、これらレートジャイロの出力信号の差動信号と傾斜センサの出力信号の差動信号とにより、移動体の姿勢情報を検出することを特徴とする請求項３に記載の移動体の高精度姿勢検出装置。
少なくとも一対の移動体の姿勢検出手段を備えると共に、これら姿勢検出手段からの出力を入力信号として移動体の姿勢を検出する制御演算処理機構を備え、上記姿勢検出手段と制御演算処理機構とを共通の駆動電源に接続したことを特徴とする請求項３〜８の何れかに記載の移動体の高精度姿勢検出装置。
移動体が歩行ロボットであることを特徴とする請求項３〜９の何れかに記載の移動体の高精度姿勢検出装置。