以下に、第1の実施の形態について、図1乃至図8を参照して説明する。なお、実施形態に係る構成要素や、当該要素の説明について、複数の表現を併記することがある。当該構成要素及び説明について、記載されていない他の表現がされることは妨げられない。さらに、複数の表現が記載されない構成要素及び説明について、他の表現がされることは妨げられない。
図1は、本発明の第1の実施形態に係る工具11及び傾き検出装置(三次元姿勢検出システム)12を示す斜視図である。工具11は対象物の一例であり、例えば電動ドライバーのようなネジ締め工具である。なお、工具11はこれに限らず、例えばエアツール、ドリル、及びインパクトレンチのような他の可搬型工具であっても良い。
工具11は、金属製のネジ13を磁気的に吸引して保持する先端部11aを有する。工具11は、電力によって駆動し、先端部11aに保持されたネジ13を回転させる。工具11は、ネジ13を部品14に取り付ける。部品14は、例えば、板金、基板、又は電子部品のような種々の物品である。
工具11は、例えば作業者に手で持って使用される。なお、工具11はこれに限らず、例えば、産業用ロボットのマニピュレータによって保持されて使用されても良い。工具11は、載置台16に置かれ、作業時には作業者によって載置台16から取り上げられる。
図1に示すように、傾き検出装置12は、例えばバンド18によって工具11に固定される傾き検出センサボックス21を有する。バンド18は、長さが調整可能であり、工具11の径にかかわらず工具11に取り付け可能である。なお、傾き検出装置12は、ネジ止め又は接着剤のような他の方法によって工具11に固定されても良く、工具11に含まれても良い。
バンド18は、傾き検出センサボックス21の一部である。しかし、バンド18は、傾き検出センサボックス21と別体の部品であっても良い。この場合、バンド18は、例えば、面ファスナー、ホック、及びレールのような種々の部材によって、傾き検出センサボックス21を着脱可能に保持しても良い。言い換えると、傾き検出センサボックス21は、着脱可能に工具11に取り付けられても良い。
図2は、工具制御ボックス22を示す斜視図である。傾き検出装置12は、工具制御ボックス22をさらに有する。工具制御ボックス22は、第1のケーブル24を介して、工具11に電気的に接続される。工具制御ボックス22は、第2のケーブル25を介して、傾き検出センサボックス21に電気的に接続される。
工具制御ボックス22は、筐体30と、第1の傾き表示灯31と、第2の傾き表示灯32と、第3の傾き表示灯33と、第1の状態表示灯34と、第2の状態表示灯35と、第3の状態表示灯36と、第4の状態表示灯37と、第1のスイッチ41と、第2のスイッチ42と、第3のスイッチ43と、を有する。なお、第1の傾き表示灯31と、第2の傾き表示灯32と、第3の傾き表示灯33と、第1の状態表示灯34と、第2の状態表示灯35と、第3の状態表示灯36と、第4の状態表示灯37と、第1のスイッチ41と、第2のスイッチ42と、第3のスイッチ43とは、傾き検出センサボックス21に設けられても良い。
第1乃至第3の傾き表示灯31〜33は、筐体30の第1の側面30aに設けられる。さらに、第1乃至第3の傾き表示灯31〜33は、第1の側面30aの反対側の面にも設けられる。これにより、作業中における第1乃至第3の傾き表示灯31〜33の視認性が向上する。
第1乃至第4の状態表示灯34〜37と第1乃至第3のスイッチ41〜43とは、筐体30の第2の側面30bに設けられる。第2の側面30bは、第1の側面30aに直交する。第1及び第2のケーブル24,25は、第2の側面30bから延びる。これにより、第1乃至第4の状態表示灯34〜37と第1乃至第3のスイッチ41〜43とを、作業者が作業中に確認できる。なお、第1及び第2のケーブル24,25は、工具制御ボックス22の他の位置から延びても良い。
図3は、工具11及び傾き検出装置12のハードウェア構成について概略的に示すブロック図である。図3に示すように、傾き検出センサボックス21は、三軸角速度センサ(以下、ジャイロセンサと称する)51と、三軸加速度センサ(以下、加速度センサと称する)52と、三軸地磁気センサ(以下、地磁気センサと称する)53と、マイクロコントロールユニット(マイコン。以下、MCUと称する)54と、第1のコネクタ55と、を有する。
MCU54は、制御部54a(例えば、CPU)や、メモリ(例えば、ROM,RAM等)、補助記憶部54b(補助記憶装置、例えば、フラッシュメモリ)等を有している。MCU54は、ROMや補助記憶部54b等の記憶部に記憶された(インストールされた)プログラムを読み出して、プログラムにしたがって各種処理を実行する。補助記憶部54bは、書き換え可能な不揮発性の記憶部である。制御部54a(MCU54)は、演算部、判定部、誤差推定部、補正部、及び出力制御部の一例である。
ジャイロセンサ51は、第1のセンサ及び第1の検出部の一例である。加速度センサ52は、第2のセンサ及び第2の検出部の一例である。地磁気センサ53は、第3のセンサ及び第2の検出部の一例である。なお、第1乃至第3のセンサは、他のセンサであっても良い。
ジャイロセンサ51は、傾き検出センサボックス21が固定された工具11の角速度についての信号を、MCU54に出力する。すなわち、ジャイロセンサ51は、工具11の姿勢の変化を示す値(第1の出力値)を出力する。言い換えると、ジャイロセンサ51は、工具11の姿勢を検出する。
加速度センサ52は、傾き検出センサボックス21が固定された工具11の加速度についての信号を、MCU54に出力する。具体的には、加速度センサ52は、重力加速度及びその変化を検知することで、工具11の加速度を検出する。すなわち、加速度センサ52は、工具11の姿勢の変化を示す値(第2の出力値)を出力する。言い換えると、加速度センサ52は、工具11の姿勢を検出する。
地磁気センサ53は、傾き検出センサボックス21が固定された工具11の、地磁気に対する傾きについての信号を、MCU54に出力する。すなわち、地磁気センサ53は、工具11の姿勢の変化を示す値(第3の出力値)を出力する。言い換えると、地磁気センサ53は、工具11の姿勢を検出する。
工具制御ボックス22は、制御回路61と、電源スイッチ62と、第2のコネクタ63と、第3のコネクタ64と、第4のコネクタ65と、第5のコネクタ66と、第6のコネクタ67と、をさらに有する。制御回路61は、制御部及び出力部の一例である。
制御回路61は、作業者の操作やMCU54の制御に基づき、工具11を制御する。例えば、制御回路61は、工具11の駆動(回転)及び停止や、工具11のトルクの変更を行なう。なお、工具11の制御は、例えば工具11に設けられた電子回路によってされても良い。
第2のコネクタ63は、第2のケーブル25によって、傾き検出センサボックス21の第1のコネクタ55に接続される。第1乃至第3の傾き表示灯31〜33と、第1乃至第4の状態表示灯34〜37と、第1乃至第3のスイッチ41〜43と、制御回路61と、電源スイッチ62とは、第2のコネクタ63を介して、傾き検出センサボックス21のMCU54に電気的に接続される。
第3のコネクタ64は、第1のケーブル24によって工具11に接続される。第4のコネクタ65は、コンセントのような電源P1に、例えば電源ケーブルによって接続される。電源P1は、第4のコネクタ65、第3のコネクタ64、第1のケーブル24を介して、工具11に電力を供給する。工具11は、当該電力によって駆動する。
制御回路61は、第4のコネクタ65と第3のコネクタ64との間を流れる電流を制御することで、工具11を制御する。例えば、制御回路61は、第4のコネクタ65と第3のコネクタ64との間の配線に設けられたインバータ回路を制御する。
第5のコネクタ66は、バッテリのような傾き検出センサ電源P2に、例えばケーブルによって接続される。傾き検出センサ電源P2は、第5のコネクタ66、第2のコネクタ63、及び第2のケーブル25を介して、傾き検出センサボックス21に電力を供給する。なお、傾き検出センサボックス21は、電源P1から電力を供給されても良い。
電源スイッチ62は、第5のコネクタ66と第2のコネクタ63との間の配線に設けられる。電源スイッチ62は、例えば作業者によって操作されることで、傾き検出センサボックス21を作動、又は停止させる。
第6のコネクタ67は、例えばケーブルによって、外部機器Dに接続される。外部機器Dは、例えば、パーソナルコンピュータやディスプレイである。外部機器Dは、例えば、傾き検出センサボックス21のモニタリングに用いられる。
図4は、傾き検出装置12の動作の一例を示す第1のフローチャートである。以下、工具11及び傾き検出装置12の動作の一例について説明する。なお、工具11及び傾き検出装置12の動作は、以下に説明されるものに限られない。
図4に示すように、電源スイッチ62が操作されることにより傾き検出装置12が起動すると、第1の状態表示灯34が点灯するとともに、地磁気センサ53の校正が始まる(ステップS11)。すなわち、地磁気が常に変動するため、MCU54は、地磁気センサ53の補正を実施する。当該補正の実施中、MCU54は、第2の状態表示灯35を点滅させる。
例えば作業者が第1のスイッチ41を操作すると(ステップS12:Yes)、MCU54は、地磁気センサ53の補正を停止する(ステップS13)。言い換えると、第1のスイッチ41が押されるまで、地磁気センサ53の補正が継続する(ステップS12:No)。
制御部54aは、第2の状態表示灯35を例えば3秒間点灯することにより、地磁気センサ53の補正が終了したことを作業者に通知する。なお、第2の状態表示灯35の点灯時間は、例えばプログラム上で変更され得る。
次に、制御部54aは、ジャイロセンサ51、加速度センサ52、及び地磁気センサ53から、それぞれの出力値を読み込む(ステップS14)。ジャイロセンサ51、加速度センサ52、及び地磁気センサ53の座標系はそれぞれ異なる。このため、制御部54aは、当該出力値を同一の座標系へ統一し角度計算を実施するため、座標軸の変換を行なう(ステップS15)。
制御部54aは、座標系の変換を行なった後、ローパスフィルタによって前記出力値の高周波成分をカットしても良い。この場合、下記の式(1)によって、前記出力値(センサ読込値)の高周波成分がカットされる。
出力値=(センサ読込値×α+前回の出力値)/(1+α) ……(1)
すなわち、式(1)は、センサ読込値の重み付けを行なう。
次に、制御部54aは、設定した時間(平滑設定時間)が経過するまで、入力データの平滑化を行なう(ステップS16)。平滑設定時間は、例えば20msecであるが、例えばプログラム上で変更され得る。平滑設定時間が経過すると(ステップS16:Yes)、制御部54aは、カウンタリセットを行い(ステップS17)、平滑化された入力データを次のステップへ出力する。
次に、制御部54aは、ジャイロセンサ51の出力値(センサ値)を、下記の式(2)によって、角速度[rad/sec]へ単位変換する(ステップS18)。
出力値=センサ値×分解能[rad] ……(2)
次に、制御部54aは、フィルタ処理によって、傾き検出装置12が固定された工具11の、より精度の良い傾き角度を算出する(ステップS19,S20)。以下、MCU54のフィルタ処理について説明する。
図5は、MCU54が行なう演算を概略的に示す図である。まず、図5を参照して、MCU54がより精度の良い傾き角度を算出する方法について概略的に説明する。なお、MCU54がより精度の良い傾き角度を算出する方法は、以下に説明されるものに限らない。
図5に示すように、制御部54aは、ジャイロセンサ51の出力値を、オイラー角速度に変換する。これにより、制御部54aは、工具11の回転座標系であるヨー角速度、ロール角速度、及びピッチ角速度を得る。
また、制御部54aは、加速度センサ52の出力値及び地磁気センサ53の出力値を、オイラー角に変換する。これにより、制御部54aは、工具11の回転座標系であるヨー角、ロール角、及びピッチ角を得る。ロール角及びピッチ角は、重力加速度の変化を検出する加速度センサ52の出力値から得られる。ヨー角は、地磁気に対する姿勢の変化を検出する地磁気センサ53の出力値から得られる。本実施形態では、これらの出力値の組み合わせにより、三つの角度(ヨー角、ロール角、及びピッチ角)についての出力値のセットが得られる。
図1に示すように、ロール角は、工具11の回転方向に沿う回転方向である。ヨー角及びピッチ角は、工具11の中心軸に直交する軸周りの回転方向である。ヨー角の回転軸とピッチ角の回転軸とは、互いに直交する。
制御部54aは、加速度センサ52及び地磁気センサ53から得られたヨー角、ロール角、及びピッチ角と、ジャイロセンサ51から得られたヨー角速度、ロール角速度、及びピッチ角速度とに基づいた演算処理(例えばフィルタ処理等)を実行し、より精度の高い検出値を得る。例えば、制御部54aは、ヨー角とヨー角速度とをフィルタリングすることで、より精度の良いヨー角を得る。ロール角及びピッチ角についても同様である。これにより、制御部54aは、工具11の傾き角度をより精度良く検出する。なお、上述のオイラー角及びオイラー角速度への変換は、フィルタ処理の後にされても良い。
図6は、傾き検出装置12を機能的に示すブロック図である。以下、図6を参照して、ヨー角のフィルタ処理の一例について具体的に説明する。なお、フィルタ処理は以下に説明されるものに限らない。さらに、ロール角及びピッチ角についても、同様のフィルタ処理がなされるが、異なるフィルタ処理がなされても良い。
図6に示すように、制御部54a(MCU54、傾き検出装置12)は、角度変換部101と、角速度変換部102と、時間積分部103と、観測値算出部104と、カルマンフィルタ演算部105と、第1のノイズカットフィルタ部106と、オフセットカットフィルタ部107と、第2のノイズカットフィルタ部108と、判定部109とを有する。
角度変換部101と角速度変換部102と、観測値算出部104と、カルマンフィルタ演算部105とは、第1の算出部及び誤差推定部の一例である。第1のノイズカットフィルタ部106と、オフセットカットフィルタ部107と、第2のノイズカットフィルタ部108とは、第2の算出部及び補正部の一例である。
角速度変換部102は、図5に係る上述の説明のように、ジャイロセンサ51の出力値V1をオイラー角速度に変換する。すると、ヨー角速度、ロール角速度、及びピッチ角速度が得られる。時間積分部103は、ヨー角速度を時間積分し、ヨー角y1に変換する。ヨー角y1は、第1の出力パラメータ及び第1の検出値の一例である。
ヨー角y1は、より精度の良いヨー角y1´と、ジャイロセンサノイズμ(k)と、ジャイロセンサ51のオフセット出力の誤差β(k)とを含む。すなわち、ヨー角y1は、以下の式(3)で表される。
y1=y1´+μ(k)+β(k) ……(3)
より精度の良いヨー角y1´は、第1の演算パラメータ及び補正された検出値の一例である。ジャイロセンサノイズμ(k)と、ジャイロセンサ51のオフセット出力の誤差β(k)とは、第1のパラメータ誤差及び第1の検出値の検出誤差の一例である。
角度変換部101は、加速度センサ52の出力値V2と地磁気センサ53の出力値V3とをオイラー角に変換する。すると、ヨー角y2、ロール角、及びピッチ角が得られる。例えば、加速度センサ52の出力値V2からロール角及びピッチ角が算出され、出力値V2と地磁気センサ53の出力値V3とからヨー角y2が算出される。ヨー角y2は、第2の出力パラメータ及び第2の検出値の一例である。
ヨー角y2は、より精度の良いヨー角y2´と、地磁気センサノイズ及び加速度センサノイズが合成されたノイズe2(k)とを含む。ノイズe2(k)は、第2の検出値の検出誤差の一例である。すなわち、ヨー角y2は、以下の式(4)で表される。
y2=y2´+e2(k) ……(4)
より精度の良いヨー角y2´は、第2の演算パラメータ及び補正された検出値の一例である。ノイズe2は、第2のパラメータ誤差の一例である。より精度の良いヨー角y2´は、より精度の良いヨー角y1´とほぼ等しくなる。
観測値算出部104は、ヨー角y1とヨー角y2とを用いて、観測値yを算出する。観測値yは、ヨー角y1とヨー角y2との差である。すなわち、観測値yは、以下の式(5)で表される。
y=y1−y2
=(y1´+μ(k)+β(k))−(y2´+e2(k))
=μ(k)+β(k)−e2(k) ……(5)
式(5)に示すように、より精度の良いヨー角y1´と、より精度の良いヨー角y2´とはほぼ等しいため、観測値yにおいて、ヨー角y1´,y2´は消える。すなわち、観測値yは、ヨー角y1及びヨー角y2に含まれる誤差(ノイズ)成分のみからなる値(誤差の差)である。
カルマンフィルタ演算部105は、観測値yを用いて、ジャイロセンサノイズμ(k)と、オフセット出力の誤差β(k)と、ノイズe2(k)とを算出する。ジャイロセンサノイズμ(k)、オフセット出力の誤差β(k)、及びノイズe2(k)は、誤差値の一例である。
カルマンフィルタ演算部105は、各ノイズ(誤差)μ(k),β(k),e2(k)をカルマンフィルタによって計算する。カルマンフィルタの離散時間状態空間モデルは、一般的に、以下の式(6)及び式(7)で表される。
x(k+1)=Ax(k)+Bv(k) ……(6)
y(k)=Cx(k)+w(k) ……(7)
式(6)及び式(7)において、y(k)はp次元時系列、x(k)はn次元状態ベクトルである。さらに、v(k)は平均値ベクトル0、共分散行列Qのr次元システム雑音ベクトル、w(k)は平均値ベクトル0、共分散行列Rのp次元観測雑音ベクトルであり、互いに独立な正規性白色雑音と仮定される。Aはn×n行列、Bはn×r行列、Cはp×n行列である。
カルマンフィルタ演算部105は、各タイムステップk(=1,2,…,N)について、以下の式(8)乃至式(12)を計算する。
[フィルタリングステップ]
式(8)乃至式(12)において、共分散行列P−(k),P(k)はn×n行列、カルマンゲインG(k)はn×p行列である。
本実施形態では、ジャイロセンサ51から求められたヨー角y1と、加速度センサ52及び地磁気センサ53から求められたヨー角y2とをペアにした状態空間モデルが設定される。上述の観測値yを用い、状態空間モデルは、以下に説明するように設定される。
上述の式(6)は以下の式(13)と設定され、上述の式(7)は以下の式(14)と設定される。
上述の各計算に用いられる変数及び定数は以下のように示される。
計算に用いられるパラメータの初期値は、本実施形態において、以下の式(20)乃至式(23)に示される。
R=0.001 ……(23)
上述の式(13)は、以下の式(24)乃至式(26)のように表すことができる。
μ(k+1)=aμ(k)+(1−a)v1(k) ……(24)
β(k+1)=β(k) ……(25)
e2(k+1)=v2(k) ……(26)
式(24)において、aは、μ(k)とv1(k)との重み付け係数である。v1(k)及びv2(k)は、上記Qによって定められる。a及びQは、例えば実験結果に基づいて設定される。
各タイムステップで計算された上述の式(13)から、ジャイロセンサノイズμ(k)、ジャイロセンサ51のオフセット出力の誤差β(k)、及びノイズe2(k)が算出される。
第1のノイズカットフィルタ部106は、ジャイロセンサ51から得られるヨー角y1から、ジャイロセンサノイズμ(k)を減算する。オフセットカットフィルタ部107は、当該減算結果から、さらにジャイロセンサ51のオフセット出力の誤差β(k)を減算することで、より精度の良いヨー角(演算値)y1´を算出する。
すなわち、演算値y1´は、以下の式(27)に示すように算出される。
y1´=y1−μ(k)−β(k) ……(27)
演算値y1´は、ヨー角y1よりも精度良く、工具11の姿勢を示す。
第2のノイズカットフィルタ部108は、加速度センサ52及び地磁気センサ53から得られるヨー角y2から、ノイズe2を減算することで、より制度の良いヨー角(演算値)y2´を算出する。
すなわち、演算値y2´は、以下の式(28)に示すように算出される。
y2´=y2−e2(k) ……(28)
演算値y2´は、ヨー角y2よりも精度良く、工具11の姿勢を示す。
以上のように、制御部54aは、ジャイロセンサ51、加速度センサ52、及び地磁気センサ53の出力値V1,V2,V3を用いて、各ノイズ(誤差)μ(k),β(k),e(k)を算出する。さらに、制御部54aは、出力値V1,V2,V3から得られるヨー角y1,y2と、各ノイズ(誤差)μ(k),β(k),e(k)とから、より精度の良いヨー角(演算値)y1´,y2´を算出する。
制御部54aは、二つの演算値y1´,y2´のうち、例えば演算値y1´を、工具11の傾き角度として算出する。演算値y1´は、判定部109に出力される。なお、制御部54aは、演算値y2´を、工具11の傾き角度として算出しても良い。二つの演算値y1´,y2´は、互いに殆ど等しい。
なお、カルマンフィルタでは、値が経時的に更新されながらより精度の良い値に近づく。このため、カルマンフィルタによる計算を行なう制御部54aは、精度の良い計算結果を算出するまで多少時間を要する。しかし、工具11が使用される前に地磁気センサ53の校正(ステップS11)がなされることで、ある程度の時間が経過する。このため、工具11が使用されるまでには、制御部54aは精度の良い計算結果を算出する。
図4に示すように、制御部54aは、上述のフィルタ処理(ステップS19)により、より精度の良い傾き角度(ヨー角、ロール角、ピッチ角)を算出する(ステップS20)。次に、制御部54aは、基準設定が行なわれているかを判断する(ステップS21)。
以下、図1に示すように部品14が垂直に配置された場合の基準設定の一例について説明する。なお、基準設定の方法は、以下に説明されるものに限らない。傾き検出装置12は、基準に対する工具11の傾きを検出する。このため、傾き検出装置12は、工具11が使用される前に少なくとも一度基準を設定する。
基準設定がされるとき、工具11は、例えば、部品14の近傍に配置された載置台16に置かれる。載置台16は、上面71と、二つの支持部72と、二つの脚73と、水準器74とを有する。
上面71は、載置される工具11に面する平面である。二つの支持部72は、それぞれ上面71から突出する。支持部72は、工具11の中心軸(回転軸)が上面71と平行になるように、工具11を支持できる。工具11は、二つの支持部72に載せられる。
二つの脚73は、載置台16に載置される工具11の中心軸方向に沿って並べられる。脚73は、上面71の反対側に位置する載置台16の底面からそれぞれ突出する。脚73はアジャスタを有する。このため、載置台16の前記底面から突出する脚73の長さは、例えば無段階調節される。
水準器74は、上面71に設けられる。水準器74は、載置台16の上面71と、載置台16に載置された工具11の中心軸とが、水平であるかを示す。なお、水準器74に限らず、例えばセンサによって上面71の傾きが調べられても良い。
二つの脚73の長さをそれぞれ調整することで、工具11の高さが調整されるとともに、上面71と載置台16に載置された工具11の中心軸とが水平にされる。すなわち、部品14のネジ穴14aの中心軸に直交するとともに鉛直方向に延びる第1の軸A1に沿う方向において、工具11の位置決めがされる。
さらに、例えばレーザ変位計によって部品14との間の距離を測定しながら、載置台16が回転させられる。これにより、部品14のネジ穴14aの中心軸に直交するとともに水平方向に延びる第2の軸A2に沿う方向において、工具11の位置決めがされる。
第1の軸A1に沿う方向における位置決めと、第2の軸A2に沿う方向における位置決めとがされることで、工具11の中心軸は、部品14のネジ穴14aの中心軸とほぼ一致する。制御部54aは、この状態を、ヨー角、ロール角、及びピッチ角の絶対基準(傾きゼロ)として、補助記憶部54b(記憶部)に記憶する。
図7は、使用時の工具11及び傾き検出装置12を示す斜視図である。上記基準設定によって、図7に示すような絶対基準S1が定められる。さらに、補助記憶部54bは、他の基準S2を記憶する。
他の基準S2は、例えば、水平に配置される他の部品19のネジ穴19aと一致する、又は平行な軸である。すなわち、他の基準S2は、絶対基準S1に対して、ピッチ方向に90°傾く軸である。補助記憶部54bは、絶対基準S1が基準設定によって定められると、当該絶対基準S1を用いて他の基準S2を算出する。このように、補助記憶部54bは、複数の基準を記憶する。
本実施形態では、使用時の工具11又は傾き検出装置12の姿勢(角度)の基準(使用基準、使用時の基準姿勢)の一つが、位置合わせのための工具11又は傾き検出装置12の姿勢の基準(絶対基準、位置合わせ用の基準姿勢)としても使われる。しかし、これらはそれぞれ別の姿勢(角度)であっても良い。
図4に示すように、傾き検出装置12が起動してから基準設定が一度もされていないとき(ステップS21:No)、制御部54aは基準設定待機状態に移行する(ステップS22)。当該基準設定待機状態において、制御部54aは基準設定を行なうとともに、第3の状態表示灯36を点灯させる。
制御部54aは、前記基準設定待機状態にある場合、現在の姿勢(傾き角度)を絶対基準として記憶(更新)し続ける。作業者が第2のスイッチ42を押すと(ステップS23:Yes)、基準設定待機が解除される(ステップS24)。制御部54aは、基準設定待機が解除された時点における絶対基準を、絶対基準として設定する。制御部54aは、基準設定を終了するとともに、第3の状態表示灯36を消灯する。
傾き検出装置12の起動後に基準設定が既にされていた場合(ステップS21:Yes)であっても、工具11が第1設定時間(例えば5秒間)静止している場合(ステップS25:Yes)、工具11が載置台16に載置されていると仮定される。このため、制御部54aは基準設定待機状態に移行する(ステップS22)。当該第1設定時間は、プログラム上で変更され得る。
工具11が第1設定時間静止していない場合(ステップS25:No)であっても、前回の基準設定から第2設定時間(例えば10分間)が経過した場合(ステップS26:Yes)、制御部54aは基準設定待機状態に移行する(ステップS22)。当該第2設定時間は、プログラム上で変更され得る。
上述のように、一定時間(第1設定時間又は第2設定時間)が経過することで、制御部54aは基準設定待機状態に移行する。これにより、例えば、工具11の使用によって生じる熱を原因とする傾き検出センサボックス21の取り付け位置のずれによる、傾き検出精度の悪化が抑制される。
前回の基準設定から第2設定時間が経過していない場合(ステップS26:No)、異常状態設定がされているかが判断される(ステップS27)。後述するように、異常状態設定がされている場合、工具11は駆動を制限される。
図8は、傾き検出装置12の動作の一例を示す第2のフローチャートである。図8に示すように、異常状態設定がされていない場合(ステップS27:No)、工具11の傾きが所定範囲内であるかが判断される(ステップS28)。
図6に示すように、判定部109に、工具11の傾き角度としての演算値y1´又はy2´が入力される。判定部109は、当該演算値y1´,y2´と、上述の基準設定により設定された絶対基準(傾きゼロ)とを比較する。判定部109は、例えば、工具11の傾きが絶対基準に対し閾値(例えば±10°)の範囲にあるかを判定する。図7は、絶対基準S1に対する閾値の範囲Sc1を模式的に示す。なお、当該閾値はプログラム上で変更され得る。
さらに、判定部109は、他の基準S2に対する工具11の傾き角度が閾値(例えば±10°)の範囲にあるかを判定する。図7は、他の基準S2に対する閾値の範囲Sc2を模式的に示す。範囲Sc1,Sc2は、所定範囲の一例である。
図8に示すように、絶対基準S1に対する工具11の傾きが前記閾値を超え、且つ他の基準S2に対する工具11の傾きが閾値を超えている場合(ステップS28:No)、制御部54aは、工具11が駆動中かを判断する(ステップS29)。工具11が駆動中(回転中)である場合(ステップS29:Yes)、制御部54aは異常状態設定を行なう(ステップS30)。制御部54aは、工具11の駆動中に閾値を越える傾きを検出したことを示す第4の状態表示灯37を点灯させる。しかし、工具11の駆動(回転)は続行する。
工具11の駆動が終了(ネジ13のネジ締めが完了)すると(ステップS31:Yes)、制御部54aは、制御回路61に工具11を強制停止させる(ステップS32)。すなわち、工具11は、作業者に操作されたとしても駆動しなくなる。制御部54aは、工具11のモータのトルクアップの信号により工具11の駆動の終了を判断する。
作業者は、工具11が強制停止されるとともに、第4の状態表示灯37が点灯することで、作業者は、ネジ締め作業中に工具11が傾いたことを認識できる。このため、作業者は、ネジ13のネジ締めが適切に行なわれたか、ネジ13の座面の状態を目視で確認する。なお、ネジ締め作業中に工具11が傾いたとしても、ネジ13にネジ締め不良が生じる可能性は低い。
工具11の強制停止は、工具11の傾きが閾値内になることで解除される。すなわち、判定部109が、工具11の傾きが閾値内であると判断した場合(ステップS28:Yes)、制御部54aは、工具11が強制停止中か否かを判断する(ステップS33)。
工具11が強制停止中の場合(ステップS33:Yes)、制御部54aは、工具11の強制停止を解除する(ステップS34)。これにより、作業者の操作により、工具11が駆動できる。なお、工具11が強制停止中でない場合(ステップS33:No)、工具11が駆動可能な状態が続く。
一方、工具11の傾きが閾値を超え(ステップS28:No)、工具11が駆動中でない場合(ステップS29:No)、制御部54aは工具11を即座に強制停止させる(ステップS32)。この場合、制御部54aは、異常状態設定を行なわない。このように、制御部54aは、工具11の傾きが所定範囲の中にある場合と外にある場合とで、制御回路61の出力態様が異なるよう制御する。
上述の動作中、制御部54aは、工具11の傾きが閾値を超えた場合、第1の傾き表示灯31を点灯する。第1の傾き表示灯31は、例えば赤色に発光する。制御部54aは、工具11の傾きが閾値内であり、且つ所定値(例えば±5°)の範囲を超えている場合、第2の傾き表示灯32を点灯する。第2の傾き表示灯32は、例えば黄色に発光する。制御部54aは、工具11の傾きが前記所定値の範囲内である場合、第3の傾き表示灯33を点灯する。第3の傾き表示灯33は、例えば緑色に発光する。これにより、作業者は、工具11の傾きの大きさを認識でき、作業品質が向上する。
図4に示すように、異常状態設定がされている場合(ステップS27:Yes)、第3のスイッチ43が操作されたかが判断される(ステップS35)。作業者は工具11を駆動する前に、第3のスイッチ43を操作する。第3のスイッチ43が操作されると(ステップS35:Yes)、制御部54aは異常状態設定を解除し、第4の状態表示灯37を消灯する(ステップS36)。
第1の実施の形態に係る傾き検出装置12は、三種類のセンサ51〜53から出力された出力値V1〜V3を用いて、工具11の姿勢を示す演算値y1´,y2´を算出する。これにより、三種類のセンサ51〜53がそれぞれ有する特徴に応じ、工具11の姿勢を精度良く示す演算値y1´,y2´を得ることができる。従って、工具11の傾きをより精度良く検知することができる。
三種類のセンサ51〜53の出力値V1〜V3を用いて算出された各ノイズ(誤差)μ(k),β(k),e2(k)と、三種類のセンサ51〜53から出力された出力値V1〜V3と、を用いて、工具11の姿勢を示す演算値y1´,y2´が算出される。これにより、三種類のセンサ51〜53がそれぞれ有する特徴によって出力値V1〜V3に含まれる誤差成分を除去し、工具11の姿勢を精度良く示す演算値y1´,y2´を得ることができる。従って、工具11の傾きをより精度良く検知することができる。
ジャイロセンサ51は、例えば、センサの内部構造のばらつきや量子化誤差によって、センサが停止しているときにも角速度を出力してしまう場合がある。加速度センサ52は、重力加速度が作用しない水平方向(ヨー方向)における傾き角度の検知についての精度が低い。地磁気センサ53は、例えば、センサ近傍の磁性体による磁束密度の変動や、時間経過による地磁気の変化によって、角度の検知について精度が低下する場合がある。ジャイロセンサ51、加速度センサ52、及び地磁気センサ53が併せて用いられることで、それぞれのデメリットが補われ、工具11の傾きをより精度良く検知することができる。
工具11が使用される前に、地磁気センサ53の校正がなされる。すなわち、工具11は、傾き検出装置12が起動してから一定の時間が経過するまで使用されない。言い換えると、制御部54aは、傾き検出装置12の起動からの経過時間が所定時間を越えていない状態では、所期の姿勢であることを示す出力を行わない。これにより、カルマンフィルタを用いたMCU54の出力結果(例えば、ヨー角y1´,y2´)の精度が良くなってから、工具11を使用できる。
MCU54によって実行され、上述の動作を行なうプログラムは、角度変換部101、角速度変換部102、時間積分部103、カルマンフィルタ演算部105、第1のノイズカットフィルタ部106、オフセットカットフィルタ部107、第2のノイズカットフィルタ108部、及び判定部109を含む構成となっている。制御部54aが、例えば前記ROMから当該プログラムを読み出して実行することにより上記各部が補助記憶部54bにロードされて生成される。
なお、上記プログラムはこれに限らず、例えば、HDD(Hard Disk Drive)、CD、FD、若しくはDVDのような情報記録媒体から読み取られても良く、又はインターネット等のネットワークを介して提供されても良い。
以下に、第2の実施の形態について、図9を参照して説明する。なお、以下の実施形態の説明において、既に説明された構成要素と同様の機能を持つ構成要素は、当該既述の構成要素と同じ符号が付され、さらに説明が省略される場合がある。また、同じ符号が付された複数の構成要素は、全ての機能及び性質が共通するとは限らず、各実施形態に応じた異なる機能及び性質を有していても良い。
図9は、第2の実施の形態に係る傾き検出装置12のハードウェア構成を概略的に示すブロック図である。図9に示すように、第2の実施形態の傾き検出装置12は、工具11に取り付けられるが、工具11に対して独立した装置である。すなわち、傾き検出装置12と工具11との間には、電気通信及び電力供給が行われない。
第2の実施形態において、傾き検出装置12は、バッテリ81を有する。バッテリ81は、傾き検出装置12に内蔵され、傾き検出装置12に電力を供給する。なお、傾き検出装置12は、外部から電力を供給されても良い。
傾き検出装置12は、第1の実施形態において工具制御ボックス22に設けられた第1乃至第3の傾き表示灯31〜33と、第1乃至第4の状態表示灯34〜37と、第1乃至第3のスイッチ41〜43と、第6のコネクタ67とを有する。
傾き検出装置12は、第1の実施形態と同様に、工具11の傾きを検出する。制御部54aは、上述の絶対基準(傾きゼロ)に対する工具11の傾き角度に応じて、第1乃至第3の傾き表示灯31〜33のいずれか一つを点灯する。
なお、工具11の傾き角度を表示する部分は、第1乃至第3の傾き表示灯31〜33に限らない。例えば、傾き検出装置12は、工具11の傾き角度を作業者に通知する液晶画面、ブザー、又はスピーカのような他の部品を有しても良い。
傾き検出装置12は、工具11に対して独立する。このため、傾き検出装置12は、工具11の傾きが閾値を超えたとしても、工具11を強制停止しない。ただし、作業者は、第1乃至第3の傾き表示灯31〜33によって工具11の傾きを認識できる。なお、傾き検出装置12は、例えば無線制御により、工具11の傾きが閾値を超えたときに工具11を強制停止しても良い。
第2の実施形態の傾き検出装置12によれば、第1の実施形態と同様に、工具11の傾きをより精度良く検知することができる。さらに、傾き検出装置12は、工具11から独立し、工具11との間の電気的接続が不要である。このため、傾き検出装置12は、どのような工具11にも取り付けられ、当該工具11の傾きを検知することができる。
上述の本発明の実施形態は、発明の範囲を限定するものではなく、発明の範囲に含まれる一例に過ぎない。本発明のある実施形態は、上述の実施形態に対して、例えば、具体的な用途、構造、形状、作用、及び効果の少なくとも一部について、発明の要旨を逸脱しない範囲において変更、省略、及び追加がされたものであっても良い。
例えば、上述の実施形態において、工具11が対象物の一例であったが、対象物はこれに限られず、種々の工具や、装置、機器、物体等であってもよい。例えば、産業用ロボットのマニピュレータのような種々の物体が、対象物の例であっても良い。
また、演算部が演算値を算出する方法は、カルマンフィルタを用いた計算に限らない。例えば、演算部は、デジタルフィルタのような他の手段を用いて、より精度の良い演算値を算出しても良い。