JP4365402B2 - 移動体の移動角度検出装置 - Google Patents

Description

この発明は移動体の移動角度検出装置に関し、より詳しくは移動体に配置されたジャイロセンサなどの移動角度検出器の出力を零点較正する装置に関する。

脚式移動ロボットや車両などの移動体の移動角度あるいは方位角は、通例、振動ジャイロのようなジャイロセンサなどの検出器を利用して検出されるが、この種の検出器は、静止状態にあっても出力にドリフト（オフセット）を生じる不都合がある。

そのため、特許文献１記載技術のように、車両に搭載されたヨーレートセンサ（ジャイロ）からなる検出器が静止状態にあるか否か検知し、静止状態にあると検知される度に静止時点の角速度データに含まれるドリフト成分量を算出して更新自在に保存すると共に、検出器の出力データから保存中のドリフト成分量を逐次除去して補正データを算出することが提案されている。
特許第３１６５０４５号公報

上記した従来技術にあっては、検出器から出力されるヨーレートの変動成分の単位時間（例えば２秒間）の累積値に基づき、検出器、より具体的にはそれが配置された車両が静止状態にあるか否か判定している。しかしながら、検出器の出力の変動成分のみからでは、静止状態を必ずしも十分に精度良く検知できず、従って検出器の出力を十分に精度良く較正することができない。

従ってこの発明の目的は上記した従来技術の不都合を解消し、検出器が配置される移動体の静止状態を精度良く判定することで検出器の出力を精度良く較正するようにした移動体の移動角度検出装置を提供することにある。

上記した課題を解決するために、請求項１にあっては、移動体に配置され、前記移動体の移動に応じた角速度を示す出力を生じる検出器、前記検出器の出力を順次取り込む出力取り込み手段、前記取り込まれた出力群の中のいずれかを零点を示す較正値と仮決めする較正値仮決め手段、前記仮決めされた較正値とそれ以降に取り込まれた出力群との差の積分値を算出する積分値算出手段、前記取り込まれた出力群の変化幅を算出する変化幅算出手段、前記算出された積分値と変化幅がそれぞれ所定の許容範囲内にあるか否か判定する判定手段、前記許容範囲にあると判定されるとき、前記移動体が静止状態にあると判断して前記仮決めされた較正値を前記積分値の平均値で補正する較正値補正手段、前記積分値が前記所定の許容範囲より狭く設定された第２の所定の許容範囲内にあるか否か判定する第２の判定手段、および前記積分値が前記第２の所定の許容範囲内にあると判定されるとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づき、前記移動体の移動角度を検出する移動角度検出手段を備えると共に、前記移動角度検出手段は、前記積分値が前記第２の所定の許容範囲内にないと判定されるとき、前記補正された較正値と順次取り込まれる出力群との差の第２の積分値が前記第２の所定の許容範囲内にあると判定されるまで前記補正された較正値で較正した値に基づいての前記移動体の移動角度の検出を中止する如く構成した。

尚、この明細書で「移動体が静止状態にある」とは、移動体が静止している状態に限られるものではなく、移動体が等速直線運動（等速度運動）あるいは等加速度運動をしている状態を含む、換言すれば回転力が作用しない状態を全て含む意味で使用する。

請求項２に係る移動体の移動角度検出装置にあっては、さらに、前記補正された較正値と前記零点として設定された値の差が絶対値において所定角度未満か否か判定する所定角度判定手段を備えると共に、前記移動角度検出手段は、前記差が所定角度未満と判断されるとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づいて前記移動体の移動角度を検出する如く構成した。

請求項３に係る移動体の移動角度検出装置にあっては、さらに、前記移動体の可動部を駆動するアクチュエータの駆動量を検出する手段を備えると共に、前記移動角度検出手段は、前記駆動量が所定値未満のとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づいて前記移動体の移動角度を検出する如く構成した。

請求項１にあっては、移動体に配置されて角速度を示す出力を生じる検出器の出力を順次取り込み、取り込まれた出力群の中のいずれかを零点を示す較正値と仮決めし、仮決めされた較正値とそれ以降に取り込まれた出力群との差の積分値を算出すると共に、取り込まれた出力群の変化幅を算出し、算出された積分値と変化幅がそれぞれ所定の許容範囲内にあるか否か判定し、許容範囲にあると判定されるとき、移動体が静止状態にあると判断して仮決めされた較正値を積分値の平均値で補正し、積分値が所定の許容範囲より狭く設定された第２の所定の許容範囲内にあるか否か判定し、積分値が第２の所定の許容範囲内にあると判定されるとき、取り込まれた出力を補正された較正値で較正した値に基づき、移動体の移動角度を検出すると共に、積分値が第２の所定の許容範囲内にないと判定されるとき、補正された較正値と順次取り込まれる出力群との差の第２の積分値が第２の所定の許容範囲内にあると判定されるまで補正された較正値で較正した値に基づいての移動体の移動角度の検出を中止する如く構成したので、仮決めされた較正値と出力群との差の積分値と出力群の変化幅がそれぞれ所定の許容範囲内にあるか否か判定し、許容範囲にあると判定されるときに移動体が静止状態にあると判断することで、検出器の出力のみから移動体の静止状態を精度良く判定することができる。

また、移動体が静止状態にあると判断するとき、仮決めされた較正値を積分値の平均値で補正し、それで取り込まれた出力を較正した値に基づいて移動体の移動角度を検出するようにしたので、較正値を精度良く求めることができ、よって移動体の移動角度を精度良く検出することができる。

また、強制的に静止状態を作らず、一時的に角速度が加わらない間に検出器の出力の積分値を算出してその正しさを判定するため、ユーザの指示を必要とすることなく、検出装置自体で較正を行うことができる。

請求項２に係る移動体の移動角度検出装置にあっては、さらに、補正された較正値と零点として設定された値の差が絶対値において所定角度未満か否か判定し、差が所定角度未満と判断されるとき、取り込まれた出力を補正された較正値で較正した値に基づいて移動体の移動角度を検出する如く構成したので、上記した効果に加え、移動体の移動角度を一層精度良く検出することができる。

請求項３に係る移動体の移動角度検出装置にあっては、さらに、移動体の可動部を駆動するアクチュエータの駆動量が所定値未満のとき、取り込まれた出力を補正された較正値で較正した値に基づいて移動体の移動角度を検出する如く構成したので、上記した効果に加え、移動体あるいは検出器が静止状態にあることを一層確実に判定することができ、よって移動体の移動角度を一層精度良く検出することができる。

以下、添付図面に即してこの発明に係る移動体の移動角度検出装置を実施するための最良の形態について説明する。

図１は、この発明の第１実施例に係る移動角度検出装置が搭載される、移動体としての脚式移動ロボットの正面図であり、図２は図１に示すロボットの側面図である。

図１に示すように、脚式移動ロボット（以下単に「ロボット」という）１０は、複数本、即ち、左右２本の脚部１２Ｌ，１２Ｒ（左側をＬ、右側をＲとする。以下同じ）を備える。脚部１２Ｌ，１２Ｒは、基体（上体）１４の下部に連結される。基体１４の上部には頭部１６が連結されると共に、側方には複数本、即ち、左右２本の腕部２０Ｌ，２０Ｒが連結される。左右の腕部２０Ｌ，２０Ｒの先端には、それぞれハンド（エンドエフェクタ）２２Ｌ，２２Ｒが連結される。

図２に示すように、基体１４の背部には格納部２４が設けられ、その内部には電子制御ユニット（以下「ＥＣＵ」と呼ぶ）２６およびバッテリ（図示せず）などが収容される。

図３は、図１に示すロボット１０をスケルトンで表す説明図である。以下、同図を参照し、ロボット１０の内部構造について関節を中心に説明する。尚、図示のロボット１０は左右対称であるので、以降Ｌ，Ｒの付記を省略する。

左右の脚部１２は、それぞれ大腿リンク３０と下腿リンク３２と足部３４とを備える。大腿リンク３０は、股関節を介して基体１４に連結される。図３では基体１４を基体リンク３６として簡略的に示す。股関節は、Ｚ軸（ヨー軸。具体的には、ロボット１０の高さ方向）回りの回転軸４０と、Ｙ軸（ピッチ軸。具体的には、ロボット１０の左右方向）回りの回転軸４２と、Ｘ軸（ロール軸。具体的には、ロボット１０の前後方向）回りの回転軸４４とから構成され、３自由度を備える。

膝関節はＹ軸回りの回転軸４６から構成され、１自由度を備える。足関節はＹ軸回りの回転軸４８とＸ軸回りの回転軸５０とから構成され、２自由度を備える。大腿リンク３０と下腿リンク３２は膝関節を介して連結されると共に、下腿リンク３２と足部３４は足関節を介して連結される。脚部１２は、基体１４と脚部１２の適宜位置に配置された１２個の電動モータからなるアクチュエータ（図示せず）によって１２個の回転軸が個別に駆動される。

左右の腕部２０は、それぞれ上腕リンク５２と下腕リンク５４を備える。上腕リンク５２は肩関節を介して基体１４に連結される。上腕リンク５２と下腕リンク５４は肘関節を介して連結されると共に、下腕リンク５４とハンド２２は手首関節を介して連結される。

肩関節はＹ軸回りの回転軸５６とＸ軸回りの回転軸５８とＺ軸回りの回転軸６０とから構成され、３自由度を備える。肘関節はＹ軸回りの回転軸６２から構成され、１自由度を備える。手首関節はＺ軸回りの回転軸６４とＹ軸回りの回転軸６６とＸ軸回りの回転軸６８とから構成され、３自由度を備える。腕部２０も、脚部１２と同様、基体１４と腕部２０の適宜位置に配置された１４個の電動モータからなるアクチュエータ（図示せず）によって１４個の回転軸が個別に駆動される。

頭部１６は、基体１４に、Ｚ軸回りの回転軸７２とＹ軸回りの回転軸７４とからなる首関節を介して連結され、２自由度を備える。回転軸７２，７４も図示しないアクチュエータによって個別に駆動される。

左右の脚部１２には、それぞれ力センサ７６が取り付けられ、床面から脚部１２に作用する床反力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚを示す信号を出力すると共に、左右の腕部２０にも同種の力センサ７８がハンド２２と手首関節の間で取り付けられ、腕部２０に作用する外力の３方向成分Ｆｘ，Ｆｙ，Ｆｚとモーメントの３方向成分Ｍｘ，Ｍｙ，Ｍｚを示す信号を出力する。

基体１４には検出器８０が配置される。検出器８０は、図示の如く、立方体状からなり、その６個の内壁面がＸ−Ｙ平面、Ｙ−Ｚ平面、Ｚ−Ｘ平面と平行となるように基体１４に取着された容器８０ａを備える。容器８０ａの内壁面にはＸ，Ｙ，Ｚ軸方向の加速度を示す出力を生じる３個の加速度センサと、Ｘ，Ｙ，Ｚ軸回りの角速度を示す出力を生じる３個の振動ジャイロからなる、ジャイロセンサがそれぞれ検出軸を対応する方向に一致させられて配置されるが、それについては後述する。

頭部１６には、２個のＣＣＤカメラ８２が設置され、ロボット１０の周囲環境をステレオ視で撮影して得た画像を出力すると共に、マイクロフォン８４ａとスピーカ８４ｂからなる音声入出力装置８４が設けられる。

尚、上記した脚部１２と腕部２０を構成する回転軸４０，４２などのそれぞれにはロータリエンコーダ群（図示せず）が配置され、対応する回転軸４０，４２などの回転角度、即ち、関節角度に応じた信号を出力する。

上記したセンサなどの出力はＥＣＵ２６に入力される。ＥＣＵ２６はマイクロコンピュータからなり、力センサ７６などの出力に基づいて前記した脚部１２のアクチュエータの動作を制御して脚部１２を駆動してロボット１０を移動させると共に、腕部２０のアクチュエータの動作を制御して腕部２０を駆動し、さらに頭部１６のアクチュエータの動作を制御して頭部１６の向きを調整する。

図４は、前記した検出器８０の構成を示すブロック図である。

図４に示される如く、検出器８０は、前記したように、Ｘ，Ｙ，Ｚ軸方向の加速度を示す出力を生じる３個の加速度センサ８０ｂ，８０ｃ，８０ｄと、Ｘ，Ｙ，Ｚ軸回りの角速度を示す出力を生じる３個の振動ジャイロからなるジャイロセンサ８０ｅ，８０ｆ，８０ｇと、それら６個のセンサの出力をＡＤ変換器８０ｈを介して入力するＩＭＵ（Inertial Measurement Unit）用ＣＰＵ８０ｉとからなる。

ＩＭＵ用ＣＰＵ８０ｉは温度センサ８０ｊとフラッシュＲＯＭ８０ｋを備え、製造時に実測されてフラッシュＲＯＭ８０ｋに格納される補正データに基づき、温度センサ８０ｊの出力から加速度センサ８０ｂ，８０ｃ，８０ｄのゲインとオフセット値、およびジャイロセンサ８０ｅ，８０ｆ，８０ｇのゲインを補正する。ＩＭＵ用ＣＰＵ８０ｉは、ＥＣＵ２６に通信デバイス８０ｌ，２６ａを介して通信自在に接続される。図示は省略するが、ＩＭＵ用ＣＰＵ８０ｉは、これ以外に演算用のＲＡＭも備える。

上記を前提とし、この実施例に係る移動体の移動角度検出装置の動作を説明する。その動作は、ＩＭＵ用ＣＰＵ８０ｉによって実行される。

図５は、そのＩＭＵ用ＣＰＵ８０ｉの動作を示すフロー・チャートである。図示のプログラムは、例えば５００μｓｅｃごとに実行される。

先ず、Ｓ１０において３個のジャイロセンサ８０ｅ，８０ｆ，８０ｇの中、例えば８０ｇのＺ軸回りの角速度（ヨーレート）を示す出力を取り込み（読み込み）、Ｓ１２に進み、別のルーチンで推定されるセンサ出力ゼロ（零）点値（較正値）を読み込み、取り込んだセンサ出力を読み込んだ較正値で加減算してセンサ出力を較正する。

次いでＳ１４に進み、較正されたセンサ出力を積分して移動角度（方位角）を算出する。このようにしてＩＭＵ用ＣＰＵ８０ｉは、ジャイロセンサ８０ｇの出力からＺ軸（鉛直（ヨー）軸）に対するロボット１０の移動角度（方位角）を検出する。ＩＭＵ用ＣＰＵ８０ｉは、上記した処理を残りの２個のジャイロセンサ８０ｅ，８０ｆについても繰り返す。

図６は、図５と平行して実行される、センサ出力ゼロ点値（較正値）の推定作業を示すフロー・チャートである。

以下説明すると、Ｓ１００で３個のジャイロセンサ８０ｅ，８０ｆ，８０ｇの中、例えば８０ｇの出力を取り込み、それを仮ゼロ（零）点値（較正値）とする（較正値と仮決めする）。

次いでＳ１０２に進み、センサ積分値をクリアすると共に、積分カウンタ、センサ最大値、センサ最小値の値をクリアする。これらの値は、前記した演算用のＲＡＭに格納される変数であり、Ｓ１０２の処理はその初期化である。

次いでＳ１０４に進み、センサ出力（ジャイロセンサ８０ｇの出力）を再度取り込み、Ｓ１０６に進み、Ｓ１０４で取り込まれたセンサ出力のローパスフィルタ値を計算する。即ち、センサ出力を、１０から１００Ｈｚ程度の通過周波数特性を備えたローパスフィルタでフィルタリングしたときの値を計算する。これはセンサ出力からノイズを除去するためである。尚、ローパスフィルタ値の算出に代え、数回のセンサ出力の平均値を求めても良い。

次いでＳ１０８に進み、Ｓ１０４で取り込まれたセンサ出力からＳ１００で仮決めした仮ゼロ点値を減算して得た差の積分値を算出する。Ｓ１０８を初めてループするときは、Ｓ１０４で取り込まれたセンサ出力はＳ１００で取り込まれたセンサ出力と同一となって差は零となる。

次いでＳ１１０に進み、Ｓ１０４で取り込まれたセンサ出力の最大値と最小値（変化幅）を更新する。Ｓ１１０の処理はセンサ出力の振れ幅（ノイズ）を計算するための処理であり、Ｓ１０４で取り込んだ値が演算用ＲＡＭに記憶されている最大値と最小値の範囲にあるか否か判断し、範囲外であれば、記憶されている最大値と最小値を書き換える。初めてＳ１１０に進むときは記憶されている値がないことから、Ｓ１０４で取り込まれたセンサ出力を最大値と最小値とする。

次いでＳ１１２に進み、Ｓ１０８で算出された積分値が絶対値においてCHECK_SUM未満で、かつＳ１１０で更新された最大値と最小値の差が絶対値においてCHECK_BIT未満か否か判断する。CHECK_SUMは設定許容角度を意味し、積分値の許容する分散値を選択して例えば１０度などと設定する。CHECK_BITは設定許容振れ幅あるいは変化幅を意味し、センサのノイズより大きい値を選択して設定する。

Ｓ１１２の判断は仮ゼロ点値とセンサ出力の差が小さくなり、かつセンサ出力の振れ幅が小さくなったか、即ち、ジャイロセンサ８０ｇが静止状態にある、換言すればそれが配置されるロボット１０が静止状態にあるか否か判断することを意味する。

尚、前記した如く、この明細書で「移動体が静止状態にある」とは、移動体（ロボット１０）が静止している状態に限られるものではなく、移動体が等速直線運動（等速度運動）あるいは等加速度運動をしている状態を含む、換言すれば回転力が作用しない状態を全て含む意味で使用する。

Ｓ１１２で否定されるときはＳ１１４に進み、Ｓ１０６で算出されたフィルタ値を仮ゼロ点値として置き換え、即ち、仮ゼロ点値を更新し、Ｓ１０２からＳ１１０の処理を繰り返す。

他方、Ｓ１１２で肯定されて静止状態にあると判断されるときはＳ１１６に進み、Ｓ１０６の積分値算出回数をカウントする積分カウンタの値を１つインクリメントし、Ｓ１１８に進み、インクリメントされた積分カウンタの値がCHECK_COUNT以上となったか否か判断する。CHECK_COUNTは予定する総積分時間（任意に設定される）をセンサ出力取り込み時間間隔（例えば５００μｓｅｃ）で除算して得た商を意味し、例えば２０とする。

初めてＳ１１８をループするときは当然否定されてＳ１０４に戻り、上記の処理を繰り返す。尚、Ｓ１１２を初めてループするときなども積分値は０で、最大値・最小値も同一なので差は０となり、従ってそれらがCHECK_SUMあるいはCHECK_BIT未満となり、肯定されてＳ１１６に進むが、結果的にＳ１１８で否定されることになる。

他方、処理回数が２０回に達したとき、Ｓ１１８の判断は肯定されてＳ１２０に進み、Ｓ１０８で算出された積分値をCHECK_COUNTで除算し、よって得た商を仮ゼロ点値から減算して仮ゼロ点値を補正する。即ち、Ｓ１０８で算出された積分値をCHECK_COUNTで除算することで、センサ出力のバイアスずれ量を算出して仮ゼロ点値から減算する。

図７は、図６の処理を説明するタイム・チャートである。同図において縦軸はセンサ出力を、横軸は図６の処理を繰り返した回数を示す。

同図から明らかな如く、センサ出力を仮ゼロ点値と仮決めして上記した処理を繰り返した結果、推定されたセンサ出力ゼロ点値は８０回程度までは初期値０付近にあるが、それを超えた辺りから推定に成功している。

図６フロー・チャートの説明に戻ると、次いでＳ１２２に進み、Ｓ１０８で算出された積分値が絶対値においてCHECK_ANGLE（第１の所定角度）未満か否か判断する。CHECK_ANGLEは設定許容ドリフト角度、即ち、ゼロ点誤差の許容量を意味する。尚、しきい値は、CHECK_SUM＞CHECK_ANGLEとする。尚、これらの値はセンサの構造などに基づいて適宜設定される。

Ｓ１２２で否定されるときはＳ１０２に戻ると共に、肯定されるときはＳ１２４に進み、前回ゼロ点値から今回仮ゼロ点値を減算して得た差が絶対値においてCHECK_BIAS（第２の所定値）未満か否か判断する。CHECK_BIASは設定許容ドリフト角度を意味し、ゼロ点変動の起こり得るセンサ出力量を適宜設定して選択する。

Ｓ１２４で否定されるときは前回ゼロ点値から今回仮ゼロ点値の差が設定値以上であることから、静止状態の判定に慎重を期すためにＳ１０２に戻る。このようにＳ１２４の判断はフェイルセーフ的な処理である。

他方、Ｓ１２４で肯定されるときはＳ１２６に進み、Ｓ１２０で補正された仮ゼロ点値をゼロ点値（センサ出力ゼロ点値）と推定し、Ｓ１０２に戻って上記の処理を繰り返す。また、残余のジャイロセンサ８０ｅ，８０ｆの出力についても同様の処理がなされる。

図５フロー・チャートにあっては、Ｓ１２において、図６フロー・チャートのＳ１２６で推定されたセンサ出力ゼロ（零）点値（較正値）を読み込み、取り込んだセンサ出力を読み込んだセンサ出力ゼロ（零）点値（較正値）で加減算してセンサ出力を較正し、Ｓ１４で較正されたセンサ出力を積分して移動角度（方位角）を算出する。

この実施例はロボット１０に配置されて角速度を示す出力を生じるジャイロセンサ８０ｅ，８０ｆ，８０ｇの出力を順次取り込み、取り込まれた出力群の中のいずれかを零点を示す較正値と仮決めし、仮決めされた較正値とそれ以降に取り込まれた出力群との差の積分値を算出すると共に、取り込まれた出力群の変化幅を算出し、算出された積分値と変化幅がそれぞれ所定の許容範囲内にあるか否か判定し、許容範囲にあると判定されるとき、ロボット１０あるいはジャイロセンサ８０ｅ，８０ｆ，８０ｇが静止状態にあると判断して仮決めされた較正値を積分値の平均値で補正し、取り込まれた出力を補正された較正値で較正した値に基づき、ロボット１０の移動角度を検出する如く構成したので、仮決めされた較正値と出力群との差の積分値と出力群の変化幅がそれぞれ所定の許容範囲内にあるか否か判定し、許容範囲にあると判定されるときにロボット１０あるいはジャイロセンサ８０ｅ，８０ｆ，８０ｇが静止状態にあると判断することで、ジャイロセンサ８０ｅ，８０ｆ，８０ｇの出力のみからロボット１０あるいはジャイロセンサ８０ｅ，８０ｆ，８０ｇの静止状態を精度良く判定することができる。

また、ロボット１０あるいはジャイロセンサ８０ｅ，８０ｆ，８０ｇが静止状態にあると判断するとき、仮決めされた較正値を積分値の平均値で補正し、それで取り込まれた出力を較正した値に基づいてロボット１０の移動角度を検出するようにしたので、較正値を精度良く求めることができ、よってロボット１０の移動角度を精度良く検出することができる。また、強制的に静止状態を作らず、一時的に角速度が加わらない間にセンサ出力の積分値を算出してその正しさを判定するため、ユーザの指示を必要とすることなく、検出装置自体で較正を行うことができる。

さらに、積分値が絶対値においてCHECK_ANGLE（第１の所定角度）未満か否か判定すると共に、積分値がCHECK_ANGLE未満と判断されるとき、取り込まれた出力を置換された較正値で較正した値に基づいてロボット１０の移動角度を検出する如く構成したので、上記した効果に加え、ロボット１０の移動角度を一層精度良く検出することができる。

さらに、積分値が第１の所定角度未満ではないと判断されるときは較正値を順次更新すると共に、第１の所定角度未満と判断されるとき、前回ゼロ点値と今回仮ゼロ点値の差が絶対値においてCHECK_BIAS（第２の所定角度）未満か否か判定し、差がCHECK_BIAS未満と判断されるとき、取り込まれた出力を更新された較正値群の中のいずれか、より具体的には最新の値で較正した値に基づいてロボット１０の移動角度を検出するように構成したので、上記した効果に加え、ロボット１０の移動角度を一層精度良く検出することができる。

さらに、センサ出力のローパスフィルタ値を算出して仮ゼロ点値とするように構成したので、ゼロ点値の推定精度を高めることができると共に、推定時間も短縮することができる。

尚、図６に示すアルゴリズムにおいて、CHECK_COUNTを大きな値に設定すれば、積分時間は長くなってゼロ点値の推定精度は向上する反面、ロボット１０あるいはジャイロセンサ８０ｅ，８０ｆ，８０ｇが静止状態にある時間が短いときはＳ１２０に到達するまでに時間がかかる。

そこで、CHECK_COUNTをゼロ点推定精度の下限値に必要な程度の小さい値に設定すると共に、図８に示す如く、２回以上連続でゼロ点推定が成功した場合、推定されたゼロ点値の平均値を求め、それを最終的なゼロ点値としても良い。さらにはゼロ点推定がＮ回連続して成功した場合、最新の数回分の単純平均値を求めるか、より最新の値が反映されるように重み付けを用いて加重平均値を求め、それを最終的なゼロ点値としても良い。

図９は、この発明の第２実施例に係る移動体の移動角度検出装置の動作を示す、図６と部分的に類似するフロー・チャートである。

第１実施例と相違する点に焦点をおいて説明すると、Ｓ１２２まで第１実施例と同様な処理を行ってＳ１２４ａに進み、ロボット１０の膝関節の角度、即ち、膝関節の回転軸４６（可動部）を駆動する電動モータ（アクチュエータ）の駆動量が所定値未満か否か判断する。所定値は、ロボット１０が静止状態にあると判定できるに足る値を選択して設定する。

Ｓ１２４ａで否定されるときはロボット１０が完全に静止状態になく、従ってジャイロセンサ８０ｅ，８０ｆ，８０ｇ自体も完全には静止状態にないと判断されることから、慎重を期すためにＳ１０２に戻る。残余の構成は、第１実施例と異ならない。尚、図６フロー・チャートにおいてＳ１２４ａをＳ１００の前に位置させ、そこで否定されるときは図６フロー・チャートの処理を中止するようにしても良い。

第２実施例に係る移動体の移動角度検出装置にあっては、さらに、ロボット１０の膝関節（可動部）を駆動する電動モータ（アクチュエータ）の駆動量が所定値未満のとき、取り込まれた出力を更新された較正値群の中のいずれかで較正した値に基づいてロボット１０の移動角度を検出する如く構成したので、第１実施例で述べた効果に加え、ロボット１０あるいはジャイロセンサ８０ｅ，８０ｆ，８０ｇが静止状態にあることを一層確実に判定することができ、よってロボット１０の移動角度を一層精度良く検出することができる。

第１、第２実施例は上記の如く、移動体（ロボット１０）に配置され、前記移動体の移動に応じた角速度を示す出力を生じる検出器（ジャイロセンサ８０ｅ，８０ｆ，８０ｇ）、前記検出器の出力を順次取り込む出力取り込み手段（Ｓ１００）、前記取り込まれた出力群の中のいずれかを零点を示す較正値と仮決めする較正値仮決め手段（Ｓ１００）、前記仮決めされた較正値とそれ以降に取り込まれた出力群との差の積分値を算出する積分値算出手段（Ｓ１０８）、前記取り込まれた出力群の変化幅（最大値と最小値）を算出する変化幅算出手段（Ｓ１１０）、前記算出された積分値と変化幅がそれぞれ所定の許容範囲内にあるか否か判定する判定手段（Ｓ１１２）、前記許容範囲にあると判定されるとき、前記移動体が静止状態にあると判断して前記仮決めされた較正値を前記積分値の平均値で補正する較正値補正手段（Ｓ１１６からＳ１２０）、前記積分値が前記所定の許容範囲より狭く設定された第２の所定の許容範囲（CHECK_ANGLE）内にあるか否か判定する第２の判定手段（Ｓ１２２）、および前記積分値が前記第２の所定の許容範囲内にあると判定されるとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づき、前記移動体の移動角度を検出する移動角度検出手段（Ｓ１２２からＳ１２６，Ｓ１０からＳ１４）を備えると共に、前記移動角度検出手段は、前記積分値が前記第２の所定の許容範囲内にないと判定されるとき、前記補正された較正値と順次取り込まれる出力群との差の第２の積分値が前記第２の所定の許容範囲内にあると判定されるまで前記補正された較正値で較正した値に基づいての前記移動体の移動角度の検出を中止する（Ｓ１０２からＳ１２２）如く構成した。

さらに、前記補正された較正値（今回仮ゼロ点値）と前記零点として設定された値（前回ゼロ点値）の差が絶対値において所定角度（CHECK_BIAS）未満か否か判定する所定角度判定手段（Ｓ１２４）を備えると共に、前記移動角度検出手段は、前記差が所定角度未満と判断されるとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づいて前記移動体の移動角度を検出する（Ｓ１２６，Ｓ１０からＳ１４）如く構成した。

さらに、前記移動体の可動部（膝関節の回転軸４６）を駆動するアクチュエータ（電動モータ）の駆動量を検出する駆動量検出手段（ロータリエンコーダ群）を備えると共に、前記移動角度検出手段は、前記駆動量が所定値未満のとき（Ｓ１２４ａ）、前記取り込まれた出力を前記補正された較正値で較正した値に基づいて前記移動体の移動角度を検出する（Ｓ１２６，Ｓ１０からＳ１４）如く構成した。

上記において、検出器の例として振動型のジャイロセンサを示したが、それに限られるものではなく、この発明は光ファイバ、リングレーザ、ガスレートなどであっても良い。

また、移動体の例として脚式移動ロボットを示したが、移動体は車両であっても良い。

この発明の第１実施例に係る移動角度検出装置が登載される、移動体としての脚式移動ロボットの正面図である。 図１に示すロボットの側面図である。 図１に示すロボットをスケルトンで示す説明図である。 図３に示す検出器の構成を示すブロック図である。 第１実施例に係る移動体の移動角度検出装置の動作を示すフロー・チャートである。 第１実施例に係る移動体の移動角度検出装置の動作の中のセンサ出力ゼロ点値推定動作を示すフロー・チャートである。 図６フロー・チャートの処理を説明するタイム・チャートである。 図６フロー・チャートの処理の変形例を説明する説明図である。 この発明の第２実施例に係る移動体の移動角度検出装置の動作を示す、図６と同様の、センサ出力ゼロ点値推定動作を示すフロー・チャートである。

符号の説明

１０ 脚式移動ロボット（ロボット）、１２ 脚部、１４ 基体、２０ 腕部、２６ ＥＣＵ（電子制御ユニット）、４６ 回転軸（可動部）、８０ 検出器、８０ａ 容器、８０ｅ，８０ｆ，８０ｇ ジャイロセンサ（検出器）、８０ｉ ＩＭＵ用ＣＰＵ

Claims (3)

1. 移動体に配置され、前記移動体の移動に応じた角速度を示す出力を生じる検出器、前記検出器の出力を順次取り込む出力取り込み手段、前記取り込まれた出力群の中のいずれかを零点を示す較正値と仮決めする較正値仮決め手段、前記仮決めされた較正値とそれ以降に取り込まれた出力群との差の積分値を算出する積分値算出手段、前記取り込まれた出力群の変化幅を算出する変化幅算出手段、前記算出された積分値と変化幅がそれぞれ所定の許容範囲内にあるか否か判定する判定手段、前記許容範囲にあると判定されるとき、前記移動体が静止状態にあると判断して前記仮決めされた較正値を前記積分値の平均値で補正する較正値補正手段、前記積分値が前記所定の許容範囲より狭く設定された第２の所定の許容範囲内にあるか否か判定する第２の判定手段、および前記積分値が前記第２の所定の許容範囲内にあると判定されるとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づき、前記移動体の移動角度を検出する移動角度検出手段を備えると共に、前記移動角度検出手段は、前記積分値が前記第２の所定の許容範囲内にないと判定されるとき、前記補正された較正値と順次取り込まれる出力群との差の第２の積分値が前記第２の所定の許容範囲内にあると判定されるまで前記補正された較正値で較正した値に基づいての前記移動体の移動角度の検出を中止することを特徴とする移動体の移動角度検出装置。
2. さらに、前記補正された較正値と前記零点として設定された値の差が絶対値において所定角度未満か否か判定する所定角度判定手段を備えると共に、前記移動角度検出手段は、前記差が所定角度未満と判断されるとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づいて前記移動体の移動角度を検出することを特徴とする請求項１記載の移動体の移動角度検出装置。
3. さらに、前記移動体の可動部を駆動するアクチュエータの駆動量を検出する手段を備えると共に、前記移動角度検出手段は、前記駆動量が所定値未満のとき、前記取り込まれた出力を前記補正された較正値で較正した値に基づいて前記移動体の移動角度を検出することを特徴とする請求項１または２記載の移動体の移動角度検出装置。

Images