JP4410051B2 - 角速度計測装置および脚式移動ロボット - Google Patents

Description

本発明は角速度計測装置と脚式移動ロボットとに関する。

角速度を検出するセンサ（レートジャイロ）としては、振動ジャイロ、ガスレートジャイロなどが公知である。また、例えば特許文献１に見られるように、検出可能な角速度の範囲が比較的狭い高感度ジャイロセンサと、検出可能な角速度の範囲が比較的広い低感度ジャイロセンサとを移動体に搭載し、移動体に発生する角速度の大きさに応じて低感度ジャイロセンサと高感度ジャイロセンサとを選択的に切り替えることで、幅広い範囲での角速度の計測を可能としたものが知られている。
特開２０００−６６７２２号

ところで、例えば２足移動ロボットでは、その姿勢制御を行なうために、レートジャイロなどの角速度計測装置が搭載されている。この場合、従来の２足移動ロボットでは、歩行やおじぎなどの比較的ゆっくりした動作が行なわれていたが、近年では、走行やボールを蹴るなど、ロボットの素早い動きが要求されるようになってきている。そして、このような運動を２足移動ロボットに行なわせるためには、２足移動ロボットの姿勢を所望の姿勢に素早く確実に制御する必要がある。このため、そのロボットに搭載する角速度計測装置には、角速度の変化に対する計測値の応答性が高いこと（角速度の急変に対して計測値も同じように素早く変化すること）と、定常的な角速度に対する計測値の精度が安定して高いこととが要求されるようになってきている。

しかるに、角速度計測装置としての従来の各種のレートジャイロは、それぞれ一長一短があり、上記の要求を十分に満足することができないものとなっていた。この点について説明すると、一般的な振動ジャイロは起歪体、すなわちセンサに付与された角速度によって変形する部分に、電気的変換にも有利な水晶などの硬質の固体を用いている。従って、振動ジャイロは、角速度に対する反応は速いが、温度変化など角速度とは無関係の外乱でも起歪体の形状に変化をもたらす要因が生じればその影響を受けるので、これがドリフトとなって現れる。一方、ガスレートセンサはガス流を通路内に発生させ、これを起歪体として用い、この通路下流に設けられた一対のヒートワイヤに対するガス流の偏向分を角速度として検出している。このため、ガスレートセンサでは、例えば熱膨張等で通路の形状が変化してもガス流の偏向が生じない限り角速度の検出に影響はない。但し、ガスレートセンサのガス流は、起歪体としては固体の追従性より劣るので、高周波の角速度検出には不向きである。

また、前記特許文献１に見られる技術思想を適用して、特性が異なる２つのセンサの検出出力を適宜、選択的に切り替えて使用することも考えられる。しかし、このようにした場合には、センサの検出出力の切り替えの際に、角速度の計測値が不連続となりやすい。また、角速度の急激な変化に対しては、検出出力の切り替えが遅れ、角速度の計測値の応答性を確保することが困難である。そして、このような計測値を、２足移動ロボットの姿勢制御に用いても、２足移動ロボットの素早い動きを適切に制御することは困難である。

なお、応答性が良好で且つ定常的な角速度に対しても安定した検出出力を発生する角速度計として、リングレーザジャイロやファイバーオプティカルジャイロ等が知られているが、これらはいずれも振動ジャイロおよびガスレートジャイロに比べて高価なものであった。

本発明はかかる背景に鑑みてなされたものであり、角速度の計測値の高い応答性と安定性とを有する角速度計測装置を安価に提供することを目的とする。また、姿勢制御を適切に行なうことができる脚式移動ロボットを提供することを目的とする。

本発明の角速度計測装置は、前記の目的を達成するために、角速度の検出出力を発生する第１のセンサと、角速度の検出出力を発生し、該第１のセンサよりも検出出力のドリフト量が少ない第２のセンサと、前記第１のセンサの検出出力が入力されるとともに該検出出力のうちの所定の周波数よりも高い高域周波数成分を出力する高域周波数通過フィルタと、該高域周波数通過フィルタの出力値を逐次記憶保持する記憶手段と、該高域周波数通過フィルタの出力値から前記記憶手段に記憶保持されている所定時間前の出力値を減じる処理を逐次実行する減算処理手段と、該減算処理手段により得られた値を前記第２のセンサの出力値に加える処理を逐次実行する加算処理手段とを備えると共に、前記第１のセンサは前記第２のセンサよりも角速度の変化に対する検出出力の応答遅れが短い出力特性を有しており、該加算処理手段により求められた値を角速度の計測値として得るようにしたことを特徴とする。

かかる本発明によれば、前記第１のセンサの検出出力を高域周波数通過フィルタに入力するので、該高域周波数通過フィルタの出力値は、第１のセンサの検出出力の低域周波数成分を除去したものになる。そして、前記記憶手段により当該出力値は逐次時系列的に記憶保持されるが、これと並行して前記減算処理手段により、該高域周波数通過フィルタの出力値は、所定時間前の出力値分だけ減算処理される。このため、当該減算処理後に得られる出力値は、所定時間内、すなわち所定時間前から現在の時刻にいたるまで第１のセンサに生じた角速度に対する検出値に相当するものとなる。従って、第１のセンサの起動後から所定時間前までの期間で第１のセンサに生じた検出出力あるいは高域周波数通過フィルタのドリフト分も排除される。

なお、後述するが、上記所定時間はきわめて短い時間（例えば数ミリ秒）であることにより、該所定時間内に生じた検出値のドリフト分は無視できる程度の極めて少ない量である。また、ここでいうドリフトとは、第１のセンサの検出出力が示す角速度の検出値が、時刻に応じて、所定の角速度に対して差異を生じることを指す。角速度を検出するためのセンサとして、例えば振動ジャイロを使用した場合、この振動ジャイロで高精度な測定を行うためには、一般的に、まず高域周波数通過フィルタによって該振動ジャイロの検出出力のうちの低域周波数成分を除去する必要がある。これは温度変化などに起因した機械的影響による振動ジャイロの静止時の検出出力の変動を除去するためである。通常、振動ジャイロは所定の角速度に対して所定量の検出出力を発生するのではなく、静止時の検出出力から角速度に応じた所定量の検出出力を発生する。

このため、振動ジャイロのようなセンサを第１のセンサとして使用した場合、前記高域周波数通過フィルタは所定の角速度に対する検出出力の差異低減に効果を奏する。しかしながら、第１のセンサの検出出力のうち、低域周波数成分が除去されると、低域周波数もしくは一定の角速度に対して、これに対応する検出出力が得られないこととなり、実際の角速度からドリフトを生じる。しかるに、本発明によれば、高域周波数通過フィルタおよび減算処理手段により得られる値（以下、単に減算値ということがある）を前記所定時間内での実際の角速度としてとらえ、前記加算処理手段により該減算値を第２のセンサの検出出力に加えることで角速度の計測値を得る。この場合、第２のセンサは、その検出出力のドリフト量が第１のセンサよりも少ないため、低域周波数または一定の角速度に対しては、ドリフト量が少ない安定した計測値を得ることができる。また、高域周波数の角速度に対しては、第１のセンサの出力特性（外乱に対する出力特性や角速度変化に対する応答特性）を反映させた計測値を得ることができるので、高域周波数の角速度に対する第１のセンサの出力特性を、第２のセンサの出力特性で補うようにして角速度の計測値を得ることができる。

かかる本発明の角速度計測装置では、前記所定時間は、前記第１のセンサの応答遅れと第２のセンサの応答遅れとの差に応じた時間に設定されていることが好適である。

これによれば、第１のセンサおよび第２のセンサの検出出力のドリフト特性と応答性とを相互に補完したものとして、角速度の計測値が得られる。すなわち、実際の角速度の変化に対する計測値の応答性が第１のセンサとほぼ同等になると同時に、実際の角速度がほぼ一定に維持されている状態での計測値の安定性が第２のセンサとほぼ同等になる。よって、角速度の計測値は、第１のセンサのより好ましい応答性と、第２のセンサのより好ましい安定性とを併せもつものとなり、その応答性と安定性との両者を良好なものとすることができる。また、前記所定時間を適切に設定できるので、実際の角速度の変化が比較的急激であるときの、角速度の計測値の応答性を良好に保ちつつ該角速度の計測値の精度を高めることができる。

さらに、本発明の角速度計測装置では、前記第１のセンサは振動ジャイロであり、前記第２のセンサはガスレートジャイロであることが好適である。

すなわち、振動ジャイロの検出出力は、角速度の変化に対する応答性が高く、また、ガスレートジャイロの検出出力は、角速度がほぼ一定であるときの安定性が高い。従って、これらの振動ジャイロおよびガスレートジャイロをそれぞれ第１のセンサ、第２のセンサとして使用することで、本発明の角速度計測装置の応答性と安定性との両者を好適に高めることができる。また、安価な振動ジャイロおよびガスレートジャイロの使用により、総じて高精度な角速度計測装置を安価に提供することができる。

次に、本発明の脚式移動ロボットは、
上体から延設された２本の脚体と、
角速度の検出出力を発生する第１のセンサと、角速度の検出出力を発生し、該第１のセンサよりも検出出力のドリフト量が少ない第２のセンサと、前記第１のセンサの検出出力が入力されるとともに該検出出力のうちの所定の周波数よりも高い高域周波数成分を出力する高域周波数通過フィルタと、該高域周波数通過フィルタの出力値を逐次時系列的に記憶保持する記憶手段と、該高域周波数通過フィルタの出力値から前記記憶手段に記憶保持されている所定時間前の出力値を減じる処理を逐次実行する減算処理手段と、該減算処理手段により得られた値を前記第２のセンサの出力値に加える処理を逐次実行する加算処理手段とを備え、該加算処理手段により求められた値を角速度の計測値として得るようにした角速度計測装置と、
少なくとも該角速度計測装置から得られる角速度の計測値を基に、所定の部位の姿勢角を推定する姿勢角推定手段と、
少なくとも該姿勢角推定手段により推定した姿勢角を基に該ロボットの関節駆動指令信号を生成する制御手段とを備えると共に、前記第１のセンサは前記第２のセンサよりも角速度の変化に対する検出出力の応答遅れが短い出力特性を有しており、
該関節駆動指令信号に応じてロボットの関節を駆動する関節アクチュエータを動作させるようにしたことを特徴とすることを特徴とする。

この脚式移動ロボットによれば、少なくとも前記角速度計測装置による角速度の計測値を基に、姿勢角推定手段により、ロボットの所定の部位の姿勢角を推定するので、該所定の部位の運動速度によらずに、該所定の部位の姿勢角を精度よく推定することが可能となる。従って、少なくともその姿勢角を基に、ロボットの関節駆動指令信号を生成して、この関節駆動指令信号に応じてロボットの関節アクチュエータを動作させることによって、ロボットの素早い運動やゆっくりした運動によらずに、ロボットの姿勢を所望の姿勢に適切に制御することが可能となる。

なお、前記角速度計測装置は、前記したように、前記所定時間は、前記第１のセンサの応答遅れと第２のセンサの応答遅れとの差に応じた時間に設定されていることが好適である。さらに、前記第１のセンサは振動ジャイロであり、前記第２のセンサはガスレートジャイロであることが好適である。

かかる本発明の脚式移動ロボットでは、前記ロボットに作用する重力加速度を含む加速度を計測する加速度センサを備え、前記姿勢角推定手段は、前記角速度計測装置から得られる角速度の計測値と加速度センサから得られる加速度の計測値とを基に前記所定の部位の姿勢角を推定することが好適である。

すなわち、前記角速度計測装置から得られる角速度の計測値を積分することで、前記所定の部位の姿勢角を推定することが可能であるが、その積分値には、誤差が累積しやすい。そこで、前記角速度計測装置に加えて加速度センサを備えて、それらの計測値を基に前記所定の部位の姿勢角を推定する。これにより、該姿勢角の推定値の精度を高めることが可能となる。

なお、このように角速度計測装置と加速度センサとを使用する場合、前記姿勢角推定手段は、姿勢角の推定値を例えば次のように求めることが好ましい。すなわち角速度計測装置から得られる角速度の計測値を積分したものから、高域周波数通過フィルタ（ハイパスフィルタ）を介して所定の周波数よりも高い高域周波数成分を抽出する（所定周波数よりも低い低域周波数成分を除去する）と共に、加速度センサから得られる加速度の計測値から、低域周波数通過フィルタ（ローパスフィルタ）を介して所定の周波数よりも低い低域周波数成分を抽出する（所定周波数よりも高い高域周波数成分を除去する）。そして、加速度の計測値から抽出した低域周波数成分を基に前記所定の部位の姿勢角の基本値を求め、この基本値を、角速度の計測値の積分値から抽出した高域周波数成分のロボットの姿勢角に加えることで、姿勢角の推定値を求める。

本発明の角速度計測装置の一実施形態を図１〜図５を参照して説明する。図１は本実施形態の角速度計測装置の構成を示すブロック図、図２は図１に示す振動ジャイロ２と演算処理装置５との間の具体的な回路構成を示す回路図、図３は振動ジャイロの出力特性を示すグラフ、図４はガスレートジャイロの出力特性を示すグラフ、図５は図１の角速度計測装置の出力特性を示すグラフである。

図１を参照して、本実施形態の角速度計測装置１は、第１のセンサとしての振動ジャイロ２と、第２のセンサとしてのガスレートジャイロ３と、振動ジャイロ２の検出出力ωvを入力するハイパスフィルタ４（高域周波数通過フィルタ）と、ハイパスフィルタ４の出力ωv’およびガスレートジャイロ３の検出出力ωgを処理する演算処理装置５とを備えている。

振動ジャイロ２とガスレートジャイロ３とは、それぞれの入力軸（検出する角速度の軸）が同一になるように（同じ角速度を検出するように）設けられている。これらの振動ジャイロ２およびガスレートジャイロ３が検出する角速度は１軸回りでもよいが、２軸あるいは３軸回りの角速度でもよい。２軸あるいは３軸回りの角速度を検出する場合には、その検出出力は、各軸回りの角速度の検出出力の組（ベクトル量）である。なお、振動ジャイロ２およびガスレートジャイロ３は公知のものでよい。

振動ジャイロ２の検出出力ωvを入力するハイパスフィルタ４は、振動ジャイロ２の検出出力ωvから低域周波成分（直流成分を含む）を除去するためのフィルタであり、該振動ジャイロ２の検出出力ωvのうちの、所定周波数（例えば数Ｈｚ程度）よりも高い高域周波数成分を通過させて出力すると共に、該所定周波数よりも低い低域周波数成分を遮断する。このようなハイパスフィルタ４は、例えば図２に示すように、コンデンサ６および抵抗７からなる回路８により構成される。なお、前記図１では図示を省略したが、ハイパスフィルタ４の出力ωv’は、振動ジャイロ２の検出出力ωvに含まれる高周波ノイズ（数ｋＨｚ付近のノイズ）を除去しつつ出力レベルを調整する信号増幅回路９（図２に示す）を介して演算処理装置５に入力される。

また、ガスレートジャイロ３の検出出力ωgは、出力レベルを調整する図示しない信号増幅回路を介して演算処理装置５に入力される。

ここで、振動ジャイロ２およびガスレートジャイロ３の出力特性をそれぞれ図３および図４を参照して説明しておく。これらの図３および図４には、電動モータなどにより回転駆動されるターンテーブル（図示せず）に各ジャイロ２，３を固定設置して、このターンテーブルの回転角速度を０から、あらかじめ定めた所定の回転角速度（一定値）にステップ状に変化させたときの、各ジャイロ２，３の検出出力が示す角速度（角速度検出値）の経時変化の様子がそれぞれ図３の実線グラフａ、図４の実線グラフｂで示されている。また、各ジャイロ２，３毎に、図示しない回転速度センサにより検出したターンテーブルの回転角速度の経時変化の様子がそれぞれ図３の実線グラフｃ、図４の実線グラフｄで示されている。これの実線グラフｃ，ｄが示すターンテーブルの回転角速度は、各ジャイロ２，３に付与する実際の角速度（実角速度）に相当するものである。

なお、これらの図３および図４では、縦軸の角速度は、ターンテーブルの定常状態の回転角速度の目標値を「１」として、正規化されている。また、各ジャイロ２，３の検出出力は、ターンテーブルの回転軸（鉛直軸）と平行な軸回りの角速度を示すものである。また、図３に示す振動ジャイロ２の検出出力は、より正確には、その検出出力を前記ハイパルフィルタ４に通した後の出力（ハイパスフィルタ４の出力）である。

図３に示されるように、振動ジャイロ２の検出出力（ハイパスフィルタ４の出力）が示す角速度検出値は、ターンテーブルの回転角速度がステップ状に変化したとき、応答遅れをほとんど伴うことなく、ターンテーブルの回転角速度の変化に追従している。このように、振動ジャイロ２は、与えられる実際の角速度の変化に対する応答性が高い。但し、ターンテーブルの回転角速度がほぼ一定値に維持される定常状態では、振動ジャイロ２の検出出力（ハイパスフィルタ４の出力）が示す角速度検出値は、実際の角速度に対してドリフト（ハイパスフィルタ４の特性上、定常入力においては出力が減少する）が発生し、そのドリフト量が時間の経過と共に増加していく。

また、図４に示されるように、ガスレートジャイロ３の検出出力（角速度検出値）は、ターンテーブルの回転角速度がステップ状に変化したとき、振動ジャイロ２よりも応答性が低いために、ターンテーブルの回転角速度の変化に対して応答遅れが発生している。一方、ターンテーブルの回転角速度がほぼ一定値に維持される定常状態では、ガスレートジャイロ３の検出出力（角速度検出値）は、ドリフトを生じることなく、実際の角速度に精度よく安定に維持される。このようにガスレートジャイロ３の検出出力は、与えられる実角速度の変化に対する応答性は低いが、定常状態での精度および安定性は高い。

図１の説明に戻って、前記演算処理装置５は、ハイパスフィルタ４の出力ωv’を逐次時系列的に記憶保持する記憶手段たるメモリ１０と、このメモリ１０に所定時間ｔsdの時間差をおいて記憶保持される２つの出力値の差を逐次演算する減算処理部１１と、この減算処理部１１で求められた値とガスレートジャイロ３の検出出力ωgとを加算する加算処理部１２とを備えている。なお、減算処理部１１および加算処理部１２は、それぞれ本発明の角速度計測装置における減算処理手段、加算処理手段に相当するものである。

メモリ１０は、ハイパスフィルタ４の出力ωv’を所定のサンプリング周期Δｔで逐次サンプリングし、そのサンプリングした出力値ωv’の時系列ωv'(t)，ωv'(t−Δt)，ωv'(t−２Δt)，……を所定時間ｔsdの期間分、記憶保持する。すなわち、現在時刻ｔから所定時間ｔsd前までのハイパスフィルタ４の出力値ωv'(t)，ωv'(t−Δt)，…，ωv'(t−tsd)がメモリ１０に記憶保持される。なお、新たな出力値をメモリ１０に記憶保持する毎に、最も古い出力値がメモリ１０から消去される。

減算処理部１１は、メモリ１０に記憶保持したハイパスフィルタ４の現在時刻ｔの出力値ωv'(t)から、メモリ１０に記憶保持されている所定時間ｔsd前の出力値ωv'(t−tsd)を減算する処理を逐次実行する。すなわち、メモリ１０に記憶保持する各時刻のハイパスフィルタ４の出力値ωv'から、その時刻の所定時間ｔsd前にメモリ１０に記憶保持された出力値ωv'を減算する処理が、減算処理部１１により逐次実行される。ここで、所定時間ｔsdは、振動ジャイロ２およびガスレートジャイロ３にステップ状に変化する角速度を与えたときの、各ジャイロ２，３の検出出力の応答遅れの差に応じた時間にあらかじめ設定されている。例えば前記図４を参照して、ガスレートジャイロ３に付与する角速度がその最終定常値の所定割合の値に達した時刻（これは振動ジャイロ２の検出出力が示す角速度が当該所定割合の値に達する時刻にほぼ等しい）から、ガスレートジャイロ３の検出出力が示す角速度検出値が当該所定割合の値に達するまでに要する時間、すなわち、振動ジャイロ２の検出出力に対するガスレートジャイロ３の検出出力の応答遅れ時間（これは図示の例では約１７ｍｓｅｃである）にほぼ等しい時間が前記所定時間ｔsdとして設定されている。このような所定時間ｔsdは、あらかじめ実験やシミュレーションに基づいて定められている。

加算処理部１２は、現在時刻ｔでのハイパスフィルタ４の出力値ωv'(t)に対応して減算処理部１１で求められた値ωv'(t)−ωv'(t−tsd)と、ガスレートジャイロ３の現在時刻ｔの出力値ωg(t)とを加算する処理を逐次実行し、その処理により求めた値を最終的な角速度計測値として逐次出力するものである。

これらの減算処理部１１および加算処理部１２の処理は、より一般化して言えば、各時刻におけるハイパスフィルタ４の出力値ωv'およびガスレートジャイロ３の出力値ωgに対して、出力値ωv'から、当該時刻の所定時間ｔsd前の時刻におけるハイパスフィルタ４の出力値を減じたものを当該時刻におけるガスレートジャイロ３の出力値ωgに加算する処理であるといえる。

次に、本実施形態の角速度計測装置１の作動を説明する。この角速度計測装置１では、振動ジャイロ２の検出出力ωvをハイパスフィルタ４に入力することで、振動ジャイロ２の検出出力ωvから低域周波数成分（直流成分を含む）が除去される。従って、ハイパスフィルタ４の出力ωv’は、振動ジャイロ２の検出出力ωvの変化速度が十分に小さい状態（振動ジャイロ２に付与される実際の角速度が一定となるか、もしくはその変化速度が十分に小さい状態）では、ほぼ「０」の出力値となる。このため、ハイパスフィルタ４の出力ωv’は、振動ジャイロ２の検出出力ωvのドリフトの影響をほとんど受けないものとなる。これはドリフトによる振動ジャイロ２の検出出力ωvの変化は緩やかなもの（変化速度が小さいもの）となるからである。そして、このハイパスフィルタ４の出力値ωv’の時系列ωv'(t)，ωv'(t−Δt)，…，ωv'(t−tsd)がメモリ１０に記憶保持されつつ、前記減算処理部１１の演算が行なわれ、ハイパスフィルタ４の現在の出力値ωv'(t)と、所定時間ｔsd前の出力値ωv'(t−tsd)との差が求められる。このようにして減算処理部１１で求められる差の値ωv'(t)−ωv'(t−tsd)は、振動ジャイロ２の検出出力ωv、ひいてはハイパスフィルタの出力値ωv’が比較的大きな変化速度で変化している状態（すなわち、振動ジャイロ２に付与される実際の角速度が比較的急激に変化している状態）では、所定時間ｔsdの期間における実際の角速度の変化量に精度よく合致するものとなる。これは、前記した如く、振動ジャイロ２の検出出力ωvは、与えられる角速度の変化に対する応答性が高いためである。また、振動ジャイロ２の検出出力ωvの変化速度が定常的に十分に小さい状態では、ωv'(t)−ωv'(t−tsd)は、０、もしくは０に近い値になる。

そして、減算処理部１１より求められた値ωv'(t)−ωv'(t−tsd)が、加算処理部１２により、ガスレートジャイロ３の現在の検出出力ωg(t)に加算される。

ここで、ガスレートジャイロ３の検出出力ωgは、与えられる実際の角速度の変化に対して前記所定時間ｔsdにほぼ等しい時間の応答遅れを生じるものである。また、与えられる角速度の変化速度が比較的大きい状態では、前記した如く、減算処理部１１で求められる差の値ωv'(t)−ωv'(t−tsd)は、所定時間ｔsdの期間における実際の角速度の変化量に精度よく合致するものとなる。このため、実際の角速度の変化速度が比較的大きい状態では、加算処理部１２により求められる値ωg(t)＋（ωv'(t)−ωv'(t−tsd)）は、ガスレートジャイロ３の検出出力ωgをその応答遅れ分を補償するようにして修正したものとなる。

また、与えられる実際の角速度の変化速度が比較的小さい状態では、前記したように、ωv'(t)−ωv'(t−tsd)≒０となるので、加算処理部１２により求められる値ωg(t)＋（ωv'(t)−ωv'(t−tsd)）は、ほぼωg(t)に等しいものとなる。この場合、ガスレートジャイロ３の検出出力ωgは、角速度がほぼ一定となる状態では、精度よく且つ安定に実際の角速度に対応するものとなるので、加算処理部１２により求められる値ωg(t)＋（ωv'(t)−ωv'(t−tsd)）も精度よく且つ安定に実際の角速度に対応するものとなる。

本実施形態の角速度計測装置１は、以上のようにして加算処理部１２により求められた値ωg(t)＋（ωv'(t)−ωv'(t−tsd)）を最終的な角速度計測値として逐次出力する。

かかる角速度計測装置１の出力の例を図５を参照して説明する。図５には、角速度計測装置１の振動ジャイロ２およびガスレートジャイロ３を前記図３および図４の場合と同様にターンテーブルに固定設置して、このターンテーブルの回転速度を０から所定の回転角速度（一定値）にステップ状に変化させたときの、角速度計測装置１の出力（角速度計測値）の経時変化の様子が実線グラフｅで示されている。また、図示しない回転速度センサにより検出したターンテーブルの回転角速度の経時変化の様子が実線グラフｆで示されていると共に、振動ジャイロ２およびガスレートジャイロ３の検出出力（角速度検出値）の経時変化の様子がそれぞれ破線グラフｇ，ｈで示されている。なお、縦軸の角速度は、ターンテーブルの定常状態での角速度の目標値を「１」として、正規化されている。また、角速度計測装置１の出力（角速度計測値）、並びに各ジャイロ２，３の検出出力（角速度検出値）は、ターンテーブルの回転軸（鉛直軸）と平行な軸回りの角速度を示すものである。また、図５中の振動ジャイロ２の検出出力のグラフｇは、より正確には、振動ジャイロ２の検出出力をハイパスフィルタ４に通した後の出力（ハイパスフィルタ４の出力）の変化を示すグラフである。

同図５に示されるように、角速度計測装置１の出力（角速度計測値）は、ターンテーブルの回転角速度がステップ状に変化したとき、振動ジャイロ２の検出出力（ハイパスフィルタ４の出力）と同様に、応答遅れをほとんど伴うことなく、ターンテーブルの回転角速度の変化に追従している。また、ターンテーブルの回転角速度がほぼ一定値に維持される定常状態では、ガスレートジャイロ３の検出出力と同様に、ドリフトを生じることなく、実際の角速度に精度よく安定に維持される。

このように、本実施形態の角速度計測装置１の出力は、高い応答性と安定性とを有し、与えられる実際の角速度の変化状態によらずに、精度よく合致するものとなる。

次に、本発明の脚式移動ロボットの一実施形態を図６および図７を参照して説明する。図６は脚式移動ロボットとしての２足移動ロボットの外観側面図、図７はこの２足移動ロボットの制御システムの概略構成を示すブロック図である。

図６を参照して、本実施形態の２足移動ロボット２１（以下、単にロボット２１という）は、上体２２（基体）の下端部から股関節２３ａを介して脚体２３が延設され、また、上体２２の上部の側部から肩関節２４ａを介して腕体２４が延設されている。脚体２３および腕体２４は、図６ではロボット２１の前方に向かって右側のもののみが図示されているが、それぞれ左右一対づつ備えられている。なお、上体２２は、本発明の２足移動ロボットにおける所定の部位に相当するものである。

各脚体２３は、股関節２３ａのほか、該脚体２３の先端側に向かって順番に膝関節２３ｂおよび足首関節２３ｃを備え、これらの各関節２３ａ〜２３ｃでの１軸もしくは複数軸の回りの回転運動が可能となっている。同様に、各腕体２４は、肩関節２４ａのほか、該腕体２４の先端側に向かって順番に肘関節２４ｂおよび手首関節２４ｃを備え、これらの各関節２４ａ〜２４ｃでの１軸もしくは複数軸回りの回転運動が可能となっている。

なお、図６では図示を省略するが、これらの各脚体２３および各腕体２４の各関節２３ａ〜２３ｃ、２４ａ〜２４ｃには、電動モータなどの関節アクチュエータが設けられている。後に説明する図７で参照符号３０を付して示した「関節アクチュエータ」は、それらの複数の関節アクチュエータを総称的に示したものである。

ロボット２１の上体２２の背面部には、マイクロコンピュータ（図示せず）を含む制御回路ユニット２６と、前記の実施形態の角速度計測装置１と、加速度センサ２７とが搭載されている。この場合、角速度計測装置１は、ロボット２１の上体２２の上下方向、前後方向、および左右方向の３つの軸回りの角速度を計測し、その３軸回りの角速度計測値の組を制御回路ユニット２６に出力する。同様に、加速度センサ２７は、ロボット２１の上体２２の上下方向、前後方向、および左右方向の３つの軸方向の加速度を計測し、その３軸方向の加速度計測値の組を制御回路ユニット２６に出力する。なお、ロボット２１の直立姿勢状態では、上体２２の上下方向は鉛直方向（重力方向）に一致する。

補足すると、角速度計測装置１の演算処理装置５、あるいは、これとハイパスフィルタ４とは、制御回路ユニット２６に組み込まれていてもよい。この場合、ハイパスフィルタ４の出力およびガスレートジャイロ３の検出出力、あるいは、振動ジャイロ２およびガスレートジャイロ３の検出出力は、制御回路ユニット２６にＡ／Ｄ変換器を介して入力するようにすればよい。

制御回路ユニット２６は、図７に示すように、角速度計測装置１および加速度センサ２７の出力を基に、ロボット２１の上体２２の姿勢角（鉛直方向（重力方向）に対する傾斜角）を推定する姿勢角推定手段２８と、この姿勢角推定手段２８により推定された上体２２の姿勢角などを基に、ロボット２１の各関節２３ａ〜２３ｃ，２４ａ〜２４ｃの関節駆動指令信号を生成し、それを各関節アクチュエータ３０に出力する制御手段２９とを備えている。制御手段２９が生成する関節駆動指令信号は、各関節アクチュエータ３０の回転角度を指示する信号である。

かかる本実施形態のロボット１では、制御回路ユニット２６は、その姿勢角推定手段２８により、ロボット２１の上体２２の姿勢角を推定する処理を逐次実行しながら、制御手段２９により関節駆動指令信号を逐次生成する。この場合、姿勢角推定手段２８は、加速度センサ２７から出力される加速度計測値からローパスフィルタ（図示せず）により所定周波数よりも高い高域周波数成分を除いたもの（これは主に重力による加速度成分に相当するものとなる）から上体２２の姿勢角（鉛直方向に対する傾斜角）の基本値を逐次算出する。そして、姿勢角推定手段２８は、角速度計測装置１から出力される角速度計測値を積分して得られる上体２２の姿勢角（鉛直方向に対する傾斜角）から、ハイパスフィルタ（図示せず）により所定周波数よりも低い低域周波数成分（直流成分を含む）を除いたものを逐次抽出し、これに前記基本値を加えたものを、上体２２の姿勢角の推定値として逐次求める。

補足すると、角速度計測装置１から出力される角速度計測値を積分したものを上体２２の姿勢角の推定値として得るようにしてもよい。但し、角速度計測値の積分値には、角速度計測値の誤差が蓄積されていく。また、加速度センサ２７の加速度計測値から求められる上体２２の姿勢角は、ロボット２１の運動が行なわれている状態では、その運動加速度（慣性力の加速度）の影響によって、誤差が生じやすい。そこで、本実施形態では、加速度センサ２７の加速度計測値と角速度計測装置１の角速度計測値との両者を用いて、上記の如く上体２２の姿勢角の推定値を求めるようにした。

そして、制御手段２９は、例えば次のようにしてロボット２１の姿勢制御を行なう。すなわち、制御手段２９には図示しない目標歩容生成手段からロボット２１の各部の空間的な目標位置および目標姿勢角を規定する目標データ（例えば各脚体２３および各腕体２４の先端部の目標位置および目標姿勢角、上体２２の目標位置および目標姿勢角など）が与えられる。そして、制御手段２９は、その目標データにより定まるロボット２１の上体２２の目標姿勢角と、姿勢角推定手段２８で推定された上体２２の姿勢角とを比較し、その姿勢角の推定値が目標姿勢角に近づくように、前記目標データを修正する。さらに、制御手段２９は、その修正された目標データから、ロボット２１の各関節２３ａ〜２３ｃ，２４ａ〜２４ｃの関節駆動指令信号を生成し、それを各関節アクチュエータ３０に出力する。このとき、各関節アクチュエータ３０は、与えられた関節駆動指令信号に従って動作し、これによりロボット２１の姿勢制御がなされる。

以上のように、本実施形態のロボット２１では、ロボット２１の上体２２の姿勢角を推定するために、前記角速度計測装置１と加速度センサ２７とが用いられる。この場合、特に、角速度計測装置１は、前記した通り応答性および安定性が高いものでのであるので、姿勢角推定手段２８の処理により、上体２２の姿勢角の推定値を精度よく得ることがきる。このため、ロボット２１の姿勢制御を好適に行なうことができる。

なお、本実施形態では、ロボット２１の上体２２の姿勢角を推定するようにしたが、ロボット２１の他の部位、例えば各脚体２３の大腿部（股関節２３ａと膝関節２３ｂとの間のリンク）の姿勢角を推定するようにしてもよい。ロボット１の各関節の回転角度は、例えば前記関節駆動指令信号や、適宜のセンサを用いて把握できるので、ロボット２１の１つの部位（リンク）の姿勢角を推定すれば、幾何学的演算によってロボット２１の他の部位の姿勢角も推定できる。

本発明の角速度計測装置の一実施形態の構成を示すブロック図。 図１に示す振動ジャイロと演算処理装置との間の具体的な回路構成を示す回路図。 実施形態で用いる振動ジャイロの出力特性を示すグラフ。 実施形態で用いるガスレートジャイロの出力特性を示すグラフ。 実施形態の角速度計測装置の出力特性を示すグラフ。 本発明の２足移動ロボットの一実施形態の外観側面図。 実施形態のロボットの制御システムの概略構成を示すブロック図。

符号の説明

１…角速度計測装置、２…第１のセンサ（振動ジャイロ）、３…第２のセンサ（ガスレートジャイロ）、４…高域周波数通過フィルタ（ハイパスフィルタ）、１０…記憶手段（メモリ）、１１…減算処理手段（減算処理部）、１２…加算処理手段（加算処理部）、２１…脚式移動ロボット（２足移動ロボット）、２２…所定の部位（上体）、２３…脚体、２３ａ〜２３ｃ，２４ａ〜２４ｃ…関節、２７…加速度センサ、２８…姿勢角推定手段、２９…制御手段、３０…関節アクチュエータ。

Claims (5)

1. 角速度の検出出力を発生する第１のセンサと、角速度の検出出力を発生し、該第１のセンサよりも検出出力のドリフト量が少ない第２のセンサと、前記第１のセンサの検出出力が入力されるとともに該検出出力のうちの所定の周波数よりも高い高域周波数成分を出力する高域周波数通過フィルタと、該高域周波数通過フィルタの出力値を逐次記憶保持する記憶手段と、該高域周波数通過フィルタの出力値から前記記憶手段に記憶保持されている所定時間前の出力値を減じる処理を逐次実行する減算処理手段と、該減算処理手段により得られた値を前記第２のセンサの出力値に加える処理を逐次実行する加算処理手段とを備えると共に、前記第１のセンサは前記第２のセンサよりも角速度の変化に対する検出出力の応答遅れが短い出力特性を有しており、該加算処理手段により求められた値を角速度の計測値として得るようにしたことを特徴とする角速度計測装置。
2. 前記所定時間は、前記第１のセンサの応答遅れと第２のセンサの応答遅れとの差に応じた時間に設定されていることを特徴とする請求項１記載の角速度計測装置。
3. 前記第１のセンサは振動ジャイロであり、前記第２のセンサはガスレートジャイロであることを特徴とする請求項１または２記載の角速度計測装置。
4. 上体から延設された２本の脚体と、
   角速度の検出出力を発生する第１のセンサと、角速度の検出出力を発生し、該第１のセンサよりも検出出力のドリフト量が少ない第２のセンサと、前記第１のセンサの検出出力が入力されるとともに該検出出力のうちの所定の周波数よりも高い高域周波数成分を出力する高域周波数通過フィルタと、該高域周波数通過フィルタの出力値を逐次時系列的に記憶保持する記憶手段と、該高域周波数通過フィルタの出力値から前記記憶手段に記憶保持されている所定時間前の出力値を減じる処理を逐次実行する減算処理手段と、該減算処理手段により得られた値を前記第２のセンサの出力値に加える処理を逐次実行する加算処理手段とを備えると共に、前記第１のセンサは前記第２のセンサよりも角速度の変化に対する検出出力の応答遅れが短い出力特性を有しており、該加算処理手段により求められた値を角速度の計測値として得るようにした角速度計測装置と、
   少なくとも該角速度計測装置から得られる角速度の計測値を基に、所定の部位の姿勢角を推定する姿勢角推定手段と、
   少なくとも該姿勢角推定手段により推定した姿勢角を基に該ロボットの関節駆動指令信号を生成する制御手段とを備え、
   該関節駆動指令信号に応じてロボットの関節を駆動する関節アクチュエータを動作させるようにしたことを特徴とする脚式移動ロボット。
5. 前記ロボットに作用する重力加速度を含む加速度を計測する加速度センサを備え、前記姿勢角推定手段は、前記角速度計測装置から得られる角速度の計測値と加速度センサから得られる加速度の計測値とを基に前記所定の部位の姿勢角を推定することを特徴とする請求項４記載の脚式移動ロボット。

Images