JP2015169582A - 姿勢検出装置 - Google Patents

Abstract

【課題】対象物の傾きをより精度良く検出できる姿勢検出装置を提供する。
【解決手段】姿勢検出装置１２は、第１のセンサ５１と、第２のセンサ５２と、第３のセンサ５３と、演算部５４と、を備える。第１のセンサ５１は、対象物の姿勢を示す第１の出力値Ｖ１を出力する。第２のセンサ５２は、対象物の姿勢を示すとともに第１の出力値Ｖ１とは異なる第２の出力値Ｖ２を出力する。第３のセンサ５３は、対象物の姿勢を示すとともに第１の出力値Ｖ１及び第２の出力値Ｖ２とは異なる第３の出力値Ｖ３を出力する。演算部５４は、第１の出力値Ｖ１と、第２の出力値Ｖ２と、第３の出力値Ｖ３と、を用いて、対象物の姿勢を示す演算値を算出する。
【選択図】図６

Description

本発明は、姿勢検出装置に関する。

例えば、物品の製造、組立、及び搬送のような種々の作業において、角度（姿勢、傾き）についてより高い精度が求められることがある。例えば、ネジ締め作業では、作業者には、ネジ締め工具をネジの中心軸とネジ穴の中心軸とが一致するような角度で持つことが求められる。従来、これを容易にする、センサによってネジ締め工具の傾きを検知する技術が知られている。

特開２０１２−１１５９４１号公報

傾きをより精度良く検出することができる装置が得られれば、有意義である。すなわち、本発明の解決すべき課題の一つは、対象物の傾きをより精度良く検出できる姿勢検出装置を提供することである。

一つの実施の形態に係る姿勢検出装置は、第１のセンサと、第２のセンサと、第３のセンサと、演算部と、を備える。前記第１のセンサは、対象物の姿勢を示す第１の出力値を出力する。前記第２のセンサは、前記対象物の姿勢を示すとともに前記第１の出力値とは異なる第２の出力値を出力する。前記第３のセンサは、前記対象物の姿勢を示すとともに前記第１の出力値及び前記第２の出力値とは異なる第３の出力値を出力する。前記演算部は、前記第１の出力値と、前記第２の出力値と、前記第３の出力値と、を用いて、前記対象物の姿勢を示す演算値を算出する。

本発明によれば、例えば、より精度良く対象物の傾きを検出できる。

図１は、第１の実施の形態に係る工具及び傾き検出装置を示す斜視図である。 図２は、第１の実施形態の工具制御ボックスを示す斜視図である。 図３は、第１の実施形態の工具及び傾き検出装置のハードウェア構成を示すブロック図である。 図４は、第１の実施形態の傾き検出装置の動作の一例を示す第１のフローチャートである。 図５は、第１の実施形態のＭＣＵが行なう演算を概略的に示す図である。 図６は、第１の実施形態の傾き検出装置を機能的に示すブロック図である。 図７は、第１の実施形態の使用時の工具及び傾き検出装置を示す斜視図である。 図８は、第１の実施形態の傾き検出装置の動作の一例を示す第２のフローチャートである。 図９は、第２の実施の形態に係る検出装置のハードウェア構成を示すブロック図である。

以下に、第１の実施の形態について、図１乃至図８を参照して説明する。なお、実施形態に係る構成要素や、当該要素の説明について、複数の表現を併記することがある。当該構成要素及び説明について、記載されていない他の表現がされることは妨げられない。さらに、複数の表現が記載されない構成要素及び説明について、他の表現がされることは妨げられない。

図１は、本発明の第１の実施形態に係る工具１１及び傾き検出装置（三次元姿勢検出システム）１２を示す斜視図である。工具１１は対象物の一例であり、例えば電動ドライバーのようなネジ締め工具である。なお、工具１１はこれに限らず、例えばエアツール、ドリル、及びインパクトレンチのような他の可搬型工具であっても良い。

工具１１は、金属製のネジ１３を磁気的に吸引して保持する先端部１１ａを有する。工具１１は、電力によって駆動し、先端部１１ａに保持されたネジ１３を回転させる。工具１１は、ネジ１３を部品１４に取り付ける。部品１４は、例えば、板金、基板、又は電子部品のような種々の物品である。

工具１１は、例えば作業者に手で持って使用される。なお、工具１１はこれに限らず、例えば、産業用ロボットのマニピュレータによって保持されて使用されても良い。工具１１は、載置台１６に置かれ、作業時には作業者によって載置台１６から取り上げられる。

図１に示すように、傾き検出装置１２は、例えばバンド１８によって工具１１に固定される傾き検出センサボックス２１を有する。バンド１８は、長さが調整可能であり、工具１１の径にかかわらず工具１１に取り付け可能である。なお、傾き検出装置１２は、ネジ止め又は接着剤のような他の方法によって工具１１に固定されても良く、工具１１に含まれても良い。

バンド１８は、傾き検出センサボックス２１の一部である。しかし、バンド１８は、傾き検出センサボックス２１と別体の部品であっても良い。この場合、バンド１８は、例えば、面ファスナー、ホック、及びレールのような種々の部材によって、傾き検出センサボックス２１を着脱可能に保持しても良い。言い換えると、傾き検出センサボックス２１は、着脱可能に工具１１に取り付けられても良い。

図２は、工具制御ボックス２２を示す斜視図である。傾き検出装置１２は、工具制御ボックス２２をさらに有する。工具制御ボックス２２は、第１のケーブル２４を介して、工具１１に電気的に接続される。工具制御ボックス２２は、第２のケーブル２５を介して、傾き検出センサボックス２１に電気的に接続される。

工具制御ボックス２２は、筐体３０と、第１の傾き表示灯３１と、第２の傾き表示灯３２と、第３の傾き表示灯３３と、第１の状態表示灯３４と、第２の状態表示灯３５と、第３の状態表示灯３６と、第４の状態表示灯３７と、第１のスイッチ４１と、第２のスイッチ４２と、第３のスイッチ４３と、を有する。なお、第１の傾き表示灯３１と、第２の傾き表示灯３２と、第３の傾き表示灯３３と、第１の状態表示灯３４と、第２の状態表示灯３５と、第３の状態表示灯３６と、第４の状態表示灯３７と、第１のスイッチ４１と、第２のスイッチ４２と、第３のスイッチ４３とは、傾き検出センサボックス２１に設けられても良い。

第１乃至第３の傾き表示灯３１〜３３は、筐体３０の第１の側面３０ａに設けられる。さらに、第１乃至第３の傾き表示灯３１〜３３は、第１の側面３０ａの反対側の面にも設けられる。これにより、作業中における第１乃至第３の傾き表示灯３１〜３３の視認性が向上する。

第１乃至第４の状態表示灯３４〜３７と第１乃至第３のスイッチ４１〜４３とは、筐体３０の第２の側面３０ｂに設けられる。第２の側面３０ｂは、第１の側面３０ａに直交する。第１及び第２のケーブル２４，２５は、第２の側面３０ｂから延びる。これにより、第１乃至第４の状態表示灯３４〜３７と第１乃至第３のスイッチ４１〜４３とを、作業者が作業中に確認できる。なお、第１及び第２のケーブル２４，２５は、工具制御ボックス２２の他の位置から延びても良い。

図３は、工具１１及び傾き検出装置１２のハードウェア構成について概略的に示すブロック図である。図３に示すように、傾き検出センサボックス２１は、三軸角速度センサ（以下、ジャイロセンサと称する）５１と、三軸加速度センサ（以下、加速度センサと称する）５２と、三軸地磁気センサ（以下、地磁気センサと称する）５３と、マイクロコントロールユニット（マイコン。以下、ＭＣＵと称する）５４と、第１のコネクタ５５と、を有する。

ＭＣＵ５４は、制御部５４ａ（例えば、ＣＰＵ）や、メモリ（例えば、ＲＯＭ，ＲＡＭ等）、補助記憶部５４ｂ（補助記憶装置、例えば、フラッシュメモリ）等を有している。ＭＣＵ５４は、ＲＯＭや補助記憶部５４ｂ等の記憶部に記憶された（インストールされた）プログラムを読み出して、プログラムにしたがって各種処理を実行する。補助記憶部５４ｂは、書き換え可能な不揮発性の記憶部である。制御部５４ａ（ＭＣＵ５４）は、演算部、判定部、誤差推定部、補正部、及び出力制御部の一例である。

ジャイロセンサ５１は、第１のセンサ及び第１の検出部の一例である。加速度センサ５２は、第２のセンサ及び第２の検出部の一例である。地磁気センサ５３は、第３のセンサ及び第２の検出部の一例である。なお、第１乃至第３のセンサは、他のセンサであっても良い。

ジャイロセンサ５１は、傾き検出センサボックス２１が固定された工具１１の角速度についての信号を、ＭＣＵ５４に出力する。すなわち、ジャイロセンサ５１は、工具１１の姿勢の変化を示す値（第１の出力値）を出力する。言い換えると、ジャイロセンサ５１は、工具１１の姿勢を検出する。

加速度センサ５２は、傾き検出センサボックス２１が固定された工具１１の加速度についての信号を、ＭＣＵ５４に出力する。具体的には、加速度センサ５２は、重力加速度及びその変化を検知することで、工具１１の加速度を検出する。すなわち、加速度センサ５２は、工具１１の姿勢の変化を示す値（第２の出力値）を出力する。言い換えると、加速度センサ５２は、工具１１の姿勢を検出する。

地磁気センサ５３は、傾き検出センサボックス２１が固定された工具１１の、地磁気に対する傾きについての信号を、ＭＣＵ５４に出力する。すなわち、地磁気センサ５３は、工具１１の姿勢の変化を示す値（第３の出力値）を出力する。言い換えると、地磁気センサ５３は、工具１１の姿勢を検出する。

工具制御ボックス２２は、制御回路６１と、電源スイッチ６２と、第２のコネクタ６３と、第３のコネクタ６４と、第４のコネクタ６５と、第５のコネクタ６６と、第６のコネクタ６７と、をさらに有する。制御回路６１は、制御部及び出力部の一例である。

制御回路６１は、作業者の操作やＭＣＵ５４の制御に基づき、工具１１を制御する。例えば、制御回路６１は、工具１１の駆動（回転）及び停止や、工具１１のトルクの変更を行なう。なお、工具１１の制御は、例えば工具１１に設けられた電子回路によってされても良い。

第２のコネクタ６３は、第２のケーブル２５によって、傾き検出センサボックス２１の第１のコネクタ５５に接続される。第１乃至第３の傾き表示灯３１〜３３と、第１乃至第４の状態表示灯３４〜３７と、第１乃至第３のスイッチ４１〜４３と、制御回路６１と、電源スイッチ６２とは、第２のコネクタ６３を介して、傾き検出センサボックス２１のＭＣＵ５４に電気的に接続される。

第３のコネクタ６４は、第１のケーブル２４によって工具１１に接続される。第４のコネクタ６５は、コンセントのような電源Ｐ１に、例えば電源ケーブルによって接続される。電源Ｐ１は、第４のコネクタ６５、第３のコネクタ６４、第１のケーブル２４を介して、工具１１に電力を供給する。工具１１は、当該電力によって駆動する。

制御回路６１は、第４のコネクタ６５と第３のコネクタ６４との間を流れる電流を制御することで、工具１１を制御する。例えば、制御回路６１は、第４のコネクタ６５と第３のコネクタ６４との間の配線に設けられたインバータ回路を制御する。

第５のコネクタ６６は、バッテリのような傾き検出センサ電源Ｐ２に、例えばケーブルによって接続される。傾き検出センサ電源Ｐ２は、第５のコネクタ６６、第２のコネクタ６３、及び第２のケーブル２５を介して、傾き検出センサボックス２１に電力を供給する。なお、傾き検出センサボックス２１は、電源Ｐ１から電力を供給されても良い。

電源スイッチ６２は、第５のコネクタ６６と第２のコネクタ６３との間の配線に設けられる。電源スイッチ６２は、例えば作業者によって操作されることで、傾き検出センサボックス２１を作動、又は停止させる。

第６のコネクタ６７は、例えばケーブルによって、外部機器Ｄに接続される。外部機器Ｄは、例えば、パーソナルコンピュータやディスプレイである。外部機器Ｄは、例えば、傾き検出センサボックス２１のモニタリングに用いられる。

図４は、傾き検出装置１２の動作の一例を示す第１のフローチャートである。以下、工具１１及び傾き検出装置１２の動作の一例について説明する。なお、工具１１及び傾き検出装置１２の動作は、以下に説明されるものに限られない。

図４に示すように、電源スイッチ６２が操作されることにより傾き検出装置１２が起動すると、第１の状態表示灯３４が点灯するとともに、地磁気センサ５３の校正が始まる（ステップＳ１１）。すなわち、地磁気が常に変動するため、ＭＣＵ５４は、地磁気センサ５３の補正を実施する。当該補正の実施中、ＭＣＵ５４は、第２の状態表示灯３５を点滅させる。

例えば作業者が第１のスイッチ４１を操作すると（ステップＳ１２：Ｙｅｓ）、ＭＣＵ５４は、地磁気センサ５３の補正を停止する（ステップＳ１３）。言い換えると、第１のスイッチ４１が押されるまで、地磁気センサ５３の補正が継続する（ステップＳ１２：Ｎｏ）。

制御部５４ａは、第２の状態表示灯３５を例えば３秒間点灯することにより、地磁気センサ５３の補正が終了したことを作業者に通知する。なお、第２の状態表示灯３５の点灯時間は、例えばプログラム上で変更され得る。

次に、制御部５４ａは、ジャイロセンサ５１、加速度センサ５２、及び地磁気センサ５３から、それぞれの出力値を読み込む（ステップＳ１４）。ジャイロセンサ５１、加速度センサ５２、及び地磁気センサ５３の座標系はそれぞれ異なる。このため、制御部５４ａは、当該出力値を同一の座標系へ統一し角度計算を実施するため、座標軸の変換を行なう（ステップＳ１５）。

制御部５４ａは、座標系の変換を行なった後、ローパスフィルタによって前記出力値の高周波成分をカットしても良い。この場合、下記の式（１）によって、前記出力値（センサ読込値）の高周波成分がカットされる。
出力値＝（センサ読込値×α＋前回の出力値）／（１＋α） ……（１）
すなわち、式（１）は、センサ読込値の重み付けを行なう。

次に、制御部５４ａは、設定した時間（平滑設定時間）が経過するまで、入力データの平滑化を行なう（ステップＳ１６）。平滑設定時間は、例えば２０ｍｓｅｃであるが、例えばプログラム上で変更され得る。平滑設定時間が経過すると（ステップＳ１６：Ｙｅｓ）、制御部５４ａは、カウンタリセットを行い（ステップＳ１７）、平滑化された入力データを次のステップへ出力する。

次に、制御部５４ａは、ジャイロセンサ５１の出力値（センサ値）を、下記の式（２）によって、角速度［ｒａｄ／ｓｅｃ］へ単位変換する（ステップＳ１８）。
出力値＝センサ値×分解能［ｒａｄ］ ……（２）

次に、制御部５４ａは、フィルタ処理によって、傾き検出装置１２が固定された工具１１の、より精度の良い傾き角度を算出する（ステップＳ１９，Ｓ２０）。以下、ＭＣＵ５４のフィルタ処理について説明する。

図５は、ＭＣＵ５４が行なう演算を概略的に示す図である。まず、図５を参照して、ＭＣＵ５４がより精度の良い傾き角度を算出する方法について概略的に説明する。なお、ＭＣＵ５４がより精度の良い傾き角度を算出する方法は、以下に説明されるものに限らない。

図５に示すように、制御部５４ａは、ジャイロセンサ５１の出力値を、オイラー角速度に変換する。これにより、制御部５４ａは、工具１１の回転座標系であるヨー角速度、ロール角速度、及びピッチ角速度を得る。

また、制御部５４ａは、加速度センサ５２の出力値及び地磁気センサ５３の出力値を、オイラー角に変換する。これにより、制御部５４ａは、工具１１の回転座標系であるヨー角、ロール角、及びピッチ角を得る。ロール角及びピッチ角は、重力加速度の変化を検出する加速度センサ５２の出力値から得られる。ヨー角は、地磁気に対する姿勢の変化を検出する地磁気センサ５３の出力値から得られる。本実施形態では、これらの出力値の組み合わせにより、三つの角度（ヨー角、ロール角、及びピッチ角）についての出力値のセットが得られる。

図１に示すように、ロール角は、工具１１の回転方向に沿う回転方向である。ヨー角及びピッチ角は、工具１１の中心軸に直交する軸周りの回転方向である。ヨー角の回転軸とピッチ角の回転軸とは、互いに直交する。

制御部５４ａは、加速度センサ５２及び地磁気センサ５３から得られたヨー角、ロール角、及びピッチ角と、ジャイロセンサ５１から得られたヨー角速度、ロール角速度、及びピッチ角速度とに基づいた演算処理（例えばフィルタ処理等）を実行し、より精度の高い検出値を得る。例えば、制御部５４ａは、ヨー角とヨー角速度とをフィルタリングすることで、より精度の良いヨー角を得る。ロール角及びピッチ角についても同様である。これにより、制御部５４ａは、工具１１の傾き角度をより精度良く検出する。なお、上述のオイラー角及びオイラー角速度への変換は、フィルタ処理の後にされても良い。

図６は、傾き検出装置１２を機能的に示すブロック図である。以下、図６を参照して、ヨー角のフィルタ処理の一例について具体的に説明する。なお、フィルタ処理は以下に説明されるものに限らない。さらに、ロール角及びピッチ角についても、同様のフィルタ処理がなされるが、異なるフィルタ処理がなされても良い。

図６に示すように、制御部５４ａ（ＭＣＵ５４、傾き検出装置１２）は、角度変換部１０１と、角速度変換部１０２と、時間積分部１０３と、観測値算出部１０４と、カルマンフィルタ演算部１０５と、第１のノイズカットフィルタ部１０６と、オフセットカットフィルタ部１０７と、第２のノイズカットフィルタ部１０８と、判定部１０９とを有する。

角度変換部１０１と角速度変換部１０２と、観測値算出部１０４と、カルマンフィルタ演算部１０５とは、第１の算出部及び誤差推定部の一例である。第１のノイズカットフィルタ部１０６と、オフセットカットフィルタ部１０７と、第２のノイズカットフィルタ部１０８とは、第２の算出部及び補正部の一例である。

角速度変換部１０２は、図５に係る上述の説明のように、ジャイロセンサ５１の出力値Ｖ１をオイラー角速度に変換する。すると、ヨー角速度、ロール角速度、及びピッチ角速度が得られる。時間積分部１０３は、ヨー角速度を時間積分し、ヨー角ｙ１に変換する。ヨー角ｙ１は、第１の出力パラメータ及び第１の検出値の一例である。

ヨー角ｙ１は、より精度の良いヨー角ｙ１´と、ジャイロセンサノイズμ（ｋ）と、ジャイロセンサ５１のオフセット出力の誤差β（ｋ）とを含む。すなわち、ヨー角ｙ１は、以下の式（３）で表される。
ｙ１＝ｙ１´＋μ（ｋ）＋β（ｋ） ……（３）

より精度の良いヨー角ｙ１´は、第１の演算パラメータ及び補正された検出値の一例である。ジャイロセンサノイズμ（ｋ）と、ジャイロセンサ５１のオフセット出力の誤差β（ｋ）とは、第１のパラメータ誤差及び第１の検出値の検出誤差の一例である。

角度変換部１０１は、加速度センサ５２の出力値Ｖ２と地磁気センサ５３の出力値Ｖ３とをオイラー角に変換する。すると、ヨー角ｙ２、ロール角、及びピッチ角が得られる。例えば、加速度センサ５２の出力値Ｖ２からロール角及びピッチ角が算出され、出力値Ｖ２と地磁気センサ５３の出力値Ｖ３とからヨー角ｙ２が算出される。ヨー角ｙ２は、第２の出力パラメータ及び第２の検出値の一例である。

ヨー角ｙ２は、より精度の良いヨー角ｙ２´と、地磁気センサノイズ及び加速度センサノイズが合成されたノイズｅ２（ｋ）とを含む。ノイズｅ２（ｋ）は、第２の検出値の検出誤差の一例である。すなわち、ヨー角ｙ２は、以下の式（４）で表される。
ｙ２＝ｙ２´＋ｅ２（ｋ） ……（４）

より精度の良いヨー角ｙ２´は、第２の演算パラメータ及び補正された検出値の一例である。ノイズｅ２は、第２のパラメータ誤差の一例である。より精度の良いヨー角ｙ２´は、より精度の良いヨー角ｙ１´とほぼ等しくなる。

観測値算出部１０４は、ヨー角ｙ１とヨー角ｙ２とを用いて、観測値ｙを算出する。観測値ｙは、ヨー角ｙ１とヨー角ｙ２との差である。すなわち、観測値ｙは、以下の式（５）で表される。
ｙ＝ｙ１−ｙ２
＝（ｙ１´＋μ（ｋ）＋β（ｋ））−（ｙ２´＋ｅ２（ｋ））
＝μ（ｋ）＋β（ｋ）−ｅ２（ｋ） ……（５）

式（５）に示すように、より精度の良いヨー角ｙ１´と、より精度の良いヨー角ｙ２´とはほぼ等しいため、観測値ｙにおいて、ヨー角ｙ１´，ｙ２´は消える。すなわち、観測値ｙは、ヨー角ｙ１及びヨー角ｙ２に含まれる誤差（ノイズ）成分のみからなる値（誤差の差）である。

カルマンフィルタ演算部１０５は、観測値ｙを用いて、ジャイロセンサノイズμ（ｋ）と、オフセット出力の誤差β（ｋ）と、ノイズｅ２（ｋ）とを算出する。ジャイロセンサノイズμ（ｋ）、オフセット出力の誤差β（ｋ）、及びノイズｅ２（ｋ）は、誤差値の一例である。

カルマンフィルタ演算部１０５は、各ノイズ（誤差）μ（ｋ），β（ｋ），ｅ２（ｋ）をカルマンフィルタによって計算する。カルマンフィルタの離散時間状態空間モデルは、一般的に、以下の式（６）及び式（７）で表される。
ｘ（ｋ＋１）＝Ａｘ（ｋ）＋Ｂｖ（ｋ） ……（６）
ｙ（ｋ）＝Ｃｘ（ｋ）＋ｗ（ｋ） ……（７）

式（６）及び式（７）において、ｙ（ｋ）はｐ次元時系列、ｘ（ｋ）はｎ次元状態ベクトルである。さらに、ｖ（ｋ）は平均値ベクトル０、共分散行列Ｑのｒ次元システム雑音ベクトル、ｗ（ｋ）は平均値ベクトル０、共分散行列Ｒのｐ次元観測雑音ベクトルであり、互いに独立な正規性白色雑音と仮定される。Ａはｎ×ｎ行列、Ｂはｎ×ｒ行列、Ｃはｐ×ｎ行列である。

カルマンフィルタ演算部１０５は、各タイムステップｋ（＝１,２，…，Ｎ）について、以下の式（８）乃至式（１２）を計算する。

［予測ステップ］

［フィルタリングステップ］

式（８）乃至式（１２）において、共分散行列Ｐ−（ｋ），Ｐ（ｋ）はｎ×ｎ行列、カルマンゲインＧ（ｋ）はｎ×ｐ行列である。

本実施形態では、ジャイロセンサ５１から求められたヨー角ｙ１と、加速度センサ５２及び地磁気センサ５３から求められたヨー角ｙ２とをペアにした状態空間モデルが設定される。上述の観測値ｙを用い、状態空間モデルは、以下に説明するように設定される。

上述の式（６）は以下の式（１３）と設定され、上述の式（７）は以下の式（１４）と設定される。

上述の各計算に用いられる変数及び定数は以下のように示される。

計算に用いられるパラメータの初期値は、本実施形態において、以下の式（２０）乃至式（２３）に示される。

Ｒ＝０．００１ ……（２３）

上述の式（１３）は、以下の式（２４）乃至式（２６）のように表すことができる。
μ（ｋ＋１）＝ａμ（ｋ）＋（１−ａ）ｖ１（ｋ） ……（２４）
β（ｋ＋１）＝β（ｋ） ……（２５）
ｅ２（ｋ＋１）＝ｖ２（ｋ） ……（２６）

式（２４）において、ａは、μ（ｋ）とｖ１（ｋ）との重み付け係数である。ｖ１（ｋ）及びｖ２（ｋ）は、上記Ｑによって定められる。ａ及びＱは、例えば実験結果に基づいて設定される。

各タイムステップで計算された上述の式（１３）から、ジャイロセンサノイズμ（ｋ）、ジャイロセンサ５１のオフセット出力の誤差β（ｋ）、及びノイズｅ２（ｋ）が算出される。

第１のノイズカットフィルタ部１０６は、ジャイロセンサ５１から得られるヨー角ｙ１から、ジャイロセンサノイズμ（ｋ）を減算する。オフセットカットフィルタ部１０７は、当該減算結果から、さらにジャイロセンサ５１のオフセット出力の誤差β（ｋ）を減算することで、より精度の良いヨー角（演算値）ｙ１´を算出する。

すなわち、演算値ｙ１´は、以下の式（２７）に示すように算出される。
ｙ１´＝ｙ１−μ（ｋ）−β（ｋ） ……（２７）
演算値ｙ１´は、ヨー角ｙ１よりも精度良く、工具１１の姿勢を示す。

第２のノイズカットフィルタ部１０８は、加速度センサ５２及び地磁気センサ５３から得られるヨー角ｙ２から、ノイズｅ２を減算することで、より制度の良いヨー角（演算値）ｙ２´を算出する。

すなわち、演算値ｙ２´は、以下の式（２８）に示すように算出される。
ｙ２´＝ｙ２−ｅ２（ｋ） ……（２８）
演算値ｙ２´は、ヨー角ｙ２よりも精度良く、工具１１の姿勢を示す。

以上のように、制御部５４ａは、ジャイロセンサ５１、加速度センサ５２、及び地磁気センサ５３の出力値Ｖ１，Ｖ２，Ｖ３を用いて、各ノイズ（誤差）μ（ｋ），β（ｋ），ｅ（ｋ）を算出する。さらに、制御部５４ａは、出力値Ｖ１，Ｖ２，Ｖ３から得られるヨー角ｙ１，ｙ２と、各ノイズ（誤差）μ（ｋ），β（ｋ），ｅ（ｋ）とから、より精度の良いヨー角（演算値）ｙ１´，ｙ２´を算出する。

制御部５４ａは、二つの演算値ｙ１´，ｙ２´のうち、例えば演算値ｙ１´を、工具１１の傾き角度として算出する。演算値ｙ１´は、判定部１０９に出力される。なお、制御部５４ａは、演算値ｙ２´を、工具１１の傾き角度として算出しても良い。二つの演算値ｙ１´，ｙ２´は、互いに殆ど等しい。

なお、カルマンフィルタでは、値が経時的に更新されながらより精度の良い値に近づく。このため、カルマンフィルタによる計算を行なう制御部５４ａは、精度の良い計算結果を算出するまで多少時間を要する。しかし、工具１１が使用される前に地磁気センサ５３の校正（ステップＳ１１）がなされることで、ある程度の時間が経過する。このため、工具１１が使用されるまでには、制御部５４ａは精度の良い計算結果を算出する。

図４に示すように、制御部５４ａは、上述のフィルタ処理（ステップＳ１９）により、より精度の良い傾き角度（ヨー角、ロール角、ピッチ角）を算出する（ステップＳ２０）。次に、制御部５４ａは、基準設定が行なわれているかを判断する（ステップＳ２１）。

以下、図１に示すように部品１４が垂直に配置された場合の基準設定の一例について説明する。なお、基準設定の方法は、以下に説明されるものに限らない。傾き検出装置１２は、基準に対する工具１１の傾きを検出する。このため、傾き検出装置１２は、工具１１が使用される前に少なくとも一度基準を設定する。

基準設定がされるとき、工具１１は、例えば、部品１４の近傍に配置された載置台１６に置かれる。載置台１６は、上面７１と、二つの支持部７２と、二つの脚７３と、水準器７４とを有する。

上面７１は、載置される工具１１に面する平面である。二つの支持部７２は、それぞれ上面７１から突出する。支持部７２は、工具１１の中心軸（回転軸）が上面７１と平行になるように、工具１１を支持できる。工具１１は、二つの支持部７２に載せられる。

二つの脚７３は、載置台１６に載置される工具１１の中心軸方向に沿って並べられる。脚７３は、上面７１の反対側に位置する載置台１６の底面からそれぞれ突出する。脚７３はアジャスタを有する。このため、載置台１６の前記底面から突出する脚７３の長さは、例えば無段階調節される。

水準器７４は、上面７１に設けられる。水準器７４は、載置台１６の上面７１と、載置台１６に載置された工具１１の中心軸とが、水平であるかを示す。なお、水準器７４に限らず、例えばセンサによって上面７１の傾きが調べられても良い。

二つの脚７３の長さをそれぞれ調整することで、工具１１の高さが調整されるとともに、上面７１と載置台１６に載置された工具１１の中心軸とが水平にされる。すなわち、部品１４のネジ穴１４ａの中心軸に直交するとともに鉛直方向に延びる第１の軸Ａ１に沿う方向において、工具１１の位置決めがされる。

さらに、例えばレーザ変位計によって部品１４との間の距離を測定しながら、載置台１６が回転させられる。これにより、部品１４のネジ穴１４ａの中心軸に直交するとともに水平方向に延びる第２の軸Ａ２に沿う方向において、工具１１の位置決めがされる。

第１の軸Ａ１に沿う方向における位置決めと、第２の軸Ａ２に沿う方向における位置決めとがされることで、工具１１の中心軸は、部品１４のネジ穴１４ａの中心軸とほぼ一致する。制御部５４ａは、この状態を、ヨー角、ロール角、及びピッチ角の絶対基準（傾きゼロ）として、補助記憶部５４ｂ（記憶部）に記憶する。

図７は、使用時の工具１１及び傾き検出装置１２を示す斜視図である。上記基準設定によって、図７に示すような絶対基準Ｓ１が定められる。さらに、補助記憶部５４ｂは、他の基準Ｓ２を記憶する。

他の基準Ｓ２は、例えば、水平に配置される他の部品１９のネジ穴１９ａと一致する、又は平行な軸である。すなわち、他の基準Ｓ２は、絶対基準Ｓ１に対して、ピッチ方向に９０°傾く軸である。補助記憶部５４ｂは、絶対基準Ｓ１が基準設定によって定められると、当該絶対基準Ｓ１を用いて他の基準Ｓ２を算出する。このように、補助記憶部５４ｂは、複数の基準を記憶する。

本実施形態では、使用時の工具１１又は傾き検出装置１２の姿勢（角度）の基準（使用基準、使用時の基準姿勢）の一つが、位置合わせのための工具１１又は傾き検出装置１２の姿勢の基準（絶対基準、位置合わせ用の基準姿勢）としても使われる。しかし、これらはそれぞれ別の姿勢（角度）であっても良い。

図４に示すように、傾き検出装置１２が起動してから基準設定が一度もされていないとき（ステップＳ２１：Ｎｏ）、制御部５４ａは基準設定待機状態に移行する（ステップＳ２２）。当該基準設定待機状態において、制御部５４ａは基準設定を行なうとともに、第３の状態表示灯３６を点灯させる。

制御部５４ａは、前記基準設定待機状態にある場合、現在の姿勢（傾き角度）を絶対基準として記憶（更新）し続ける。作業者が第２のスイッチ４２を押すと（ステップＳ２３：Ｙｅｓ）、基準設定待機が解除される（ステップＳ２４）。制御部５４ａは、基準設定待機が解除された時点における絶対基準を、絶対基準として設定する。制御部５４ａは、基準設定を終了するとともに、第３の状態表示灯３６を消灯する。

傾き検出装置１２の起動後に基準設定が既にされていた場合（ステップＳ２１：Ｙｅｓ）であっても、工具１１が第１設定時間（例えば５秒間）静止している場合（ステップＳ２５：Ｙｅｓ）、工具１１が載置台１６に載置されていると仮定される。このため、制御部５４ａは基準設定待機状態に移行する（ステップＳ２２）。当該第１設定時間は、プログラム上で変更され得る。

工具１１が第１設定時間静止していない場合（ステップＳ２５：Ｎｏ）であっても、前回の基準設定から第２設定時間（例えば１０分間）が経過した場合（ステップＳ２６：Ｙｅｓ）、制御部５４ａは基準設定待機状態に移行する（ステップＳ２２）。当該第２設定時間は、プログラム上で変更され得る。

上述のように、一定時間（第１設定時間又は第２設定時間）が経過することで、制御部５４ａは基準設定待機状態に移行する。これにより、例えば、工具１１の使用によって生じる熱を原因とする傾き検出センサボックス２１の取り付け位置のずれによる、傾き検出精度の悪化が抑制される。

前回の基準設定から第２設定時間が経過していない場合（ステップＳ２６：Ｎｏ）、異常状態設定がされているかが判断される（ステップＳ２７）。後述するように、異常状態設定がされている場合、工具１１は駆動を制限される。

図８は、傾き検出装置１２の動作の一例を示す第２のフローチャートである。図８に示すように、異常状態設定がされていない場合（ステップＳ２７：Ｎｏ）、工具１１の傾きが所定範囲内であるかが判断される（ステップＳ２８）。

図６に示すように、判定部１０９に、工具１１の傾き角度としての演算値ｙ１´又はｙ２´が入力される。判定部１０９は、当該演算値ｙ１´，ｙ２´と、上述の基準設定により設定された絶対基準（傾きゼロ）とを比較する。判定部１０９は、例えば、工具１１の傾きが絶対基準に対し閾値（例えば±１０°）の範囲にあるかを判定する。図７は、絶対基準Ｓ１に対する閾値の範囲Ｓｃ１を模式的に示す。なお、当該閾値はプログラム上で変更され得る。

さらに、判定部１０９は、他の基準Ｓ２に対する工具１１の傾き角度が閾値（例えば±１０°）の範囲にあるかを判定する。図７は、他の基準Ｓ２に対する閾値の範囲Ｓｃ２を模式的に示す。範囲Ｓｃ１，Ｓｃ２は、所定範囲の一例である。

図８に示すように、絶対基準Ｓ１に対する工具１１の傾きが前記閾値を超え、且つ他の基準Ｓ２に対する工具１１の傾きが閾値を超えている場合（ステップＳ２８：Ｎｏ）、制御部５４ａは、工具１１が駆動中かを判断する（ステップＳ２９）。工具１１が駆動中（回転中）である場合（ステップＳ２９：Ｙｅｓ）、制御部５４ａは異常状態設定を行なう（ステップＳ３０）。制御部５４ａは、工具１１の駆動中に閾値を越える傾きを検出したことを示す第４の状態表示灯３７を点灯させる。しかし、工具１１の駆動（回転）は続行する。

工具１１の駆動が終了（ネジ１３のネジ締めが完了）すると（ステップＳ３１：Ｙｅｓ）、制御部５４ａは、制御回路６１に工具１１を強制停止させる（ステップＳ３２）。すなわち、工具１１は、作業者に操作されたとしても駆動しなくなる。制御部５４ａは、工具１１のモータのトルクアップの信号により工具１１の駆動の終了を判断する。

作業者は、工具１１が強制停止されるとともに、第４の状態表示灯３７が点灯することで、作業者は、ネジ締め作業中に工具１１が傾いたことを認識できる。このため、作業者は、ネジ１３のネジ締めが適切に行なわれたか、ネジ１３の座面の状態を目視で確認する。なお、ネジ締め作業中に工具１１が傾いたとしても、ネジ１３にネジ締め不良が生じる可能性は低い。

工具１１の強制停止は、工具１１の傾きが閾値内になることで解除される。すなわち、判定部１０９が、工具１１の傾きが閾値内であると判断した場合（ステップＳ２８：Ｙｅｓ）、制御部５４ａは、工具１１が強制停止中か否かを判断する（ステップＳ３３）。

工具１１が強制停止中の場合（ステップＳ３３：Ｙｅｓ）、制御部５４ａは、工具１１の強制停止を解除する（ステップＳ３４）。これにより、作業者の操作により、工具１１が駆動できる。なお、工具１１が強制停止中でない場合（ステップＳ３３：Ｎｏ）、工具１１が駆動可能な状態が続く。

一方、工具１１の傾きが閾値を超え（ステップＳ２８：Ｎｏ）、工具１１が駆動中でない場合（ステップＳ２９：Ｎｏ）、制御部５４ａは工具１１を即座に強制停止させる（ステップＳ３２）。この場合、制御部５４ａは、異常状態設定を行なわない。このように、制御部５４ａは、工具１１の傾きが所定範囲の中にある場合と外にある場合とで、制御回路６１の出力態様が異なるよう制御する。

上述の動作中、制御部５４ａは、工具１１の傾きが閾値を超えた場合、第１の傾き表示灯３１を点灯する。第１の傾き表示灯３１は、例えば赤色に発光する。制御部５４ａは、工具１１の傾きが閾値内であり、且つ所定値（例えば±５°）の範囲を超えている場合、第２の傾き表示灯３２を点灯する。第２の傾き表示灯３２は、例えば黄色に発光する。制御部５４ａは、工具１１の傾きが前記所定値の範囲内である場合、第３の傾き表示灯３３を点灯する。第３の傾き表示灯３３は、例えば緑色に発光する。これにより、作業者は、工具１１の傾きの大きさを認識でき、作業品質が向上する。

図４に示すように、異常状態設定がされている場合（ステップＳ２７：Ｙｅｓ）、第３のスイッチ４３が操作されたかが判断される（ステップＳ３５）。作業者は工具１１を駆動する前に、第３のスイッチ４３を操作する。第３のスイッチ４３が操作されると（ステップＳ３５：Ｙｅｓ）、制御部５４ａは異常状態設定を解除し、第４の状態表示灯３７を消灯する（ステップＳ３６）。

第１の実施の形態に係る傾き検出装置１２は、三種類のセンサ５１〜５３から出力された出力値Ｖ１〜Ｖ３を用いて、工具１１の姿勢を示す演算値ｙ１´，ｙ２´を算出する。これにより、三種類のセンサ５１〜５３がそれぞれ有する特徴に応じ、工具１１の姿勢を精度良く示す演算値ｙ１´，ｙ２´を得ることができる。従って、工具１１の傾きをより精度良く検知することができる。

三種類のセンサ５１〜５３の出力値Ｖ１〜Ｖ３を用いて算出された各ノイズ（誤差）μ（ｋ），β（ｋ），ｅ２（ｋ）と、三種類のセンサ５１〜５３から出力された出力値Ｖ１〜Ｖ３と、を用いて、工具１１の姿勢を示す演算値ｙ１´，ｙ２´が算出される。これにより、三種類のセンサ５１〜５３がそれぞれ有する特徴によって出力値Ｖ１〜Ｖ３に含まれる誤差成分を除去し、工具１１の姿勢を精度良く示す演算値ｙ１´，ｙ２´を得ることができる。従って、工具１１の傾きをより精度良く検知することができる。

ジャイロセンサ５１は、例えば、センサの内部構造のばらつきや量子化誤差によって、センサが停止しているときにも角速度を出力してしまう場合がある。加速度センサ５２は、重力加速度が作用しない水平方向（ヨー方向）における傾き角度の検知についての精度が低い。地磁気センサ５３は、例えば、センサ近傍の磁性体による磁束密度の変動や、時間経過による地磁気の変化によって、角度の検知について精度が低下する場合がある。ジャイロセンサ５１、加速度センサ５２、及び地磁気センサ５３が併せて用いられることで、それぞれのデメリットが補われ、工具１１の傾きをより精度良く検知することができる。

工具１１が使用される前に、地磁気センサ５３の校正がなされる。すなわち、工具１１は、傾き検出装置１２が起動してから一定の時間が経過するまで使用されない。言い換えると、制御部５４ａは、傾き検出装置１２の起動からの経過時間が所定時間を越えていない状態では、所期の姿勢であることを示す出力を行わない。これにより、カルマンフィルタを用いたＭＣＵ５４の出力結果（例えば、ヨー角ｙ１´，ｙ２´）の精度が良くなってから、工具１１を使用できる。

ＭＣＵ５４によって実行され、上述の動作を行なうプログラムは、角度変換部１０１、角速度変換部１０２、時間積分部１０３、カルマンフィルタ演算部１０５、第１のノイズカットフィルタ部１０６、オフセットカットフィルタ部１０７、第２のノイズカットフィルタ１０８部、及び判定部１０９を含む構成となっている。制御部５４ａが、例えば前記ＲＯＭから当該プログラムを読み出して実行することにより上記各部が補助記憶部５４ｂにロードされて生成される。

なお、上記プログラムはこれに限らず、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＣＤ、ＦＤ、若しくはＤＶＤのような情報記録媒体から読み取られても良く、又はインターネット等のネットワークを介して提供されても良い。

以下に、第２の実施の形態について、図９を参照して説明する。なお、以下の実施形態の説明において、既に説明された構成要素と同様の機能を持つ構成要素は、当該既述の構成要素と同じ符号が付され、さらに説明が省略される場合がある。また、同じ符号が付された複数の構成要素は、全ての機能及び性質が共通するとは限らず、各実施形態に応じた異なる機能及び性質を有していても良い。

図９は、第２の実施の形態に係る傾き検出装置１２のハードウェア構成を概略的に示すブロック図である。図９に示すように、第２の実施形態の傾き検出装置１２は、工具１１に取り付けられるが、工具１１に対して独立した装置である。すなわち、傾き検出装置１２と工具１１との間には、電気通信及び電力供給が行われない。

第２の実施形態において、傾き検出装置１２は、バッテリ８１を有する。バッテリ８１は、傾き検出装置１２に内蔵され、傾き検出装置１２に電力を供給する。なお、傾き検出装置１２は、外部から電力を供給されても良い。

傾き検出装置１２は、第１の実施形態において工具制御ボックス２２に設けられた第１乃至第３の傾き表示灯３１〜３３と、第１乃至第４の状態表示灯３４〜３７と、第１乃至第３のスイッチ４１〜４３と、第６のコネクタ６７とを有する。

傾き検出装置１２は、第１の実施形態と同様に、工具１１の傾きを検出する。制御部５４ａは、上述の絶対基準（傾きゼロ）に対する工具１１の傾き角度に応じて、第１乃至第３の傾き表示灯３１〜３３のいずれか一つを点灯する。

なお、工具１１の傾き角度を表示する部分は、第１乃至第３の傾き表示灯３１〜３３に限らない。例えば、傾き検出装置１２は、工具１１の傾き角度を作業者に通知する液晶画面、ブザー、又はスピーカのような他の部品を有しても良い。

傾き検出装置１２は、工具１１に対して独立する。このため、傾き検出装置１２は、工具１１の傾きが閾値を超えたとしても、工具１１を強制停止しない。ただし、作業者は、第１乃至第３の傾き表示灯３１〜３３によって工具１１の傾きを認識できる。なお、傾き検出装置１２は、例えば無線制御により、工具１１の傾きが閾値を超えたときに工具１１を強制停止しても良い。

第２の実施形態の傾き検出装置１２によれば、第１の実施形態と同様に、工具１１の傾きをより精度良く検知することができる。さらに、傾き検出装置１２は、工具１１から独立し、工具１１との間の電気的接続が不要である。このため、傾き検出装置１２は、どのような工具１１にも取り付けられ、当該工具１１の傾きを検知することができる。

上述の本発明の実施形態は、発明の範囲を限定するものではなく、発明の範囲に含まれる一例に過ぎない。本発明のある実施形態は、上述の実施形態に対して、例えば、具体的な用途、構造、形状、作用、及び効果の少なくとも一部について、発明の要旨を逸脱しない範囲において変更、省略、及び追加がされたものであっても良い。

例えば、上述の実施形態において、工具１１が対象物の一例であったが、対象物はこれに限られず、種々の工具や、装置、機器、物体等であってもよい。例えば、産業用ロボットのマニピュレータのような種々の物体が、対象物の例であっても良い。

また、演算部が演算値を算出する方法は、カルマンフィルタを用いた計算に限らない。例えば、演算部は、デジタルフィルタのような他の手段を用いて、より精度の良い演算値を算出しても良い。

１１…工具、１２…傾き検出装置、５１…ジャイロセンサ、５２…加速度センサ、５３…地磁気センサ、５４…ＭＣＵ、６１…制御回路、１０１…角度変換部、１０２…角速度変換部、１０３…時間積分部、１０４…観測値算出部、１０５…カルマンフィルタ演算部、１０６…第１のノイズカットフィルタ部、１０７…オフセットカットフィルタ部、１０８…第２のノイズカットフィルタ部、１０９…判定部、Ｖ１，Ｖ２，Ｖ３…出力値。

Claims (13)

1. 対象物の姿勢を示す第１の出力値を出力する第１のセンサと、
   前記対象物の姿勢を示すとともに前記第１の出力値とは異なる第２の出力値を出力する第２のセンサと、
   前記対象物の姿勢を示すとともに前記第１の出力値及び前記第２の出力値とは異なる第３の出力値を出力する第３のセンサと、
   前記第１の出力値と、前記第２の出力値と、前記第３の出力値と、を用いて、前記対象物の姿勢を示す演算値を算出する演算部と、
   を具備する姿勢検出装置。
2. 前記演算部は、
   前記第１の出力値と、前記第２の出力値と、前記第３の出力値と、を用いて誤差値を算出する第１の算出部と、
   前記誤差値と、前記第１の出力値、前記第２の出力値、及び前記第３の出力値の少なくとも一つと、を用いて、前記演算値を算出する第２の算出部と、
   を有する、請求項１の姿勢検出装置。
3. 前記第１のセンサがジャイロセンサであり、
   前記第２のセンサが加速度センサであり、
   前記第３のセンサが地磁気センサである、
   請求項２の姿勢検出装置。
4. 前記第１の算出部は、前記ジャイロセンサが出力した前記第１の出力値から得られる前記対象物の姿勢を示す第１の出力パラメータと、前記加速度センサが出力した前記第２の出力値及び前記地磁気センサが出力した前記第３の出力値から得られる前記対象物の姿勢を示す第２の出力パラメータと、を用いて、前記誤差値を算出し、
   前記第２の算出部は、前記第１の出力パラメータと前記誤差値に含まれる第１のパラメータ誤差とから算出される第１の演算パラメータと、前記第２の出力パラメータと前記誤差値に含まれる第２のパラメータ誤差とから算出される第２の演算パラメータと、の少なくとも一方を前記演算値として算出する、請求項３の姿勢検出装置。
5. 前記第１の算出部は、カルマンフィルタを用いて前記誤差値を算出する、請求項４の姿勢検出装置。
6. 前記演算値と閾値とを比較する判定部と、
   前記対象物に電気的に接続された制御部と、
   をさらに具備し、
   前記制御部は、前記演算値が前記閾値から外れたときに、電力によって駆動する前記対象物の駆動を停止させる、請求項１乃至５のうちいずれか一つの姿勢検出装置。
7. 姿勢を検出する第１の検出部と、
   姿勢を検出する第２の検出部と、
   前記第１の検出部の検出結果に基づく第１の検出値と、前記第２の検出部の検出結果に基づく第２の検出値と、に基づいて、前記第１の検出値と前記第２の検出値とのそれぞれの検出誤差を推定する誤差推定部と、
   前記第１の検出値及び前記第２の検出値のうち少なくとも一方と、前記検出誤差とに基づいて、姿勢を示す補正された検出値を算出する補正部と、
   を具備する姿勢検出装置。
8. 前記第１の検出部は、ジャイロセンサを有し、
   前記第２の検出部は、地磁気センサ及び加速度センサを有する、
   請求項７の姿勢検出装置。
9. 前記誤差推定部は、カルマンフィルタを用いて前記検出誤差を推定する、請求項８の姿勢検出装置。
10. 前記補正された検出値に基づく出力を実行するよう出力部を制御する出力制御部を備えた、請求項７乃至９のうちいずれか一つの姿勢検出装置。
11. 前記出力制御部は、前記補正された検出値に基づく姿勢が所定範囲の中にある場合と外にある場合とで前記出力部の出力態様が異なるよう制御する、請求項１０の姿勢検出装置。
12. 複数の前記所定範囲に対応したデータを記憶する記憶部をさらに具備し、
    前記出力制御部は、前記補正された検出値に基づく姿勢が複数の所定範囲のいずれかの中にある場合と外にある場合とで前記出力部の出力態様が異なるよう制御する、請求項１１の姿勢検出装置。
13. 前記出力制御部は、姿勢検出装置の起動からの経過時間が所定時間を超えていない状態では、前記出力部が所期の姿勢であることを示す出力を行わないよう制御する請求項１０乃至１２のうちいずれか一つの姿勢検出装置。

Images