JP2017007023A - ロボット装置及びモータ制御装置 - Google Patents

Abstract

【課題】 ロボットアームの接触等の外乱因子による動作異常を網羅的に検知すること。
【解決手段】 本実施形態に係るロボット装置は、多関節アーム機構を備え、複数のリンクと、複数のリンク間を接続する複数の関節部Ｊ１−Ｊ６と、複数の関節部を駆動するための動力を発生する複数のモータ３１０−３６０と、複数の関節部の少なくとも一の関節部のモータのドライブシャフトの回転を少なくとも一の関節部の回転軸に伝達する伝達機構と、少なくとも一の関節部のモータのドライブシャフトの回転を検出する第１エンコーダ２３５と、少なくとも一の関節部の回転軸の回転を検出する第２エンコーダ２３６と、第１エンコーダから出力されるエンコーダパルスと第２エンコーダから出力されるエンコーダパルスとに基づいて動作異常の判定をする判定部１０５とを具備する。
【選択図】 図５

Description

本発明の実施形態はロボット装置及びモータ制御装置に関する。

近年ロボットが作業者と同一空間にいる環境が多くなってきている。介護用ロボットはもちろん産業用ロボットでも作業者と並んで協働して作業を行なう状況が今後拡大していくものと考えられる。その状況で重要なのは安全性であり、作業者等に対するロボットアームの接触、特に衝突に代表される外乱因子に帰する動作異常時の迅速な停止又は退避動作にある。

そのために多くのロボット装置ではアームの先端や側面等の多くの箇所に接触センサを配設することが必要とされる。

しかし接触センサの取り付け位置には制限があり接触の可能性のある箇所全てに取り付けることは現実的でなく、接触に対して網羅的に対処することはできなかった。

目的は、ロボットアームの接触等の外乱因子による動作異常を網羅的に検知することにある。

本実施形態に係るロボット装置は、多関節アーム機構を備え、複数のリンクと、前記複数のリンク間を接続する複数の関節部と、前記複数の関節部を駆動するための動力を発生する複数のモータと、前記複数の関節部の少なくとも一の関節部のモータのドライブシャフトの回転を前記少なくとも一の関節部の回転軸に伝達する伝達機構と、前記少なくとも一の関節部のモータのドライブシャフトの回転を検出する第１エンコーダと、前記少なくとも一の関節部の回転軸の回転を検出する第２エンコーダと、前記第１エンコーダから出力されるエンコーダパルスと前記第２エンコーダから出力されるエンコーダパルスとに基づいて動作異常の判定をする判定部とを具備することを特徴とする。

図１は、本実施形態に係るロボット装置のロボットアーム機構の外観斜視図である。 図２は、図１のロボットアーム機構の内部構造を示す側面図である。 図３は、図１のロボットアーム機構の構成を図記号表現により示す図である。 図４は、図１の直動伸縮関節部Ｊ３の駆動機構の構造を示す斜視図である。 図５は、本実施形態によるロボット装置の構成を示すブロック図である。 図６は、図５の衝突判定部１０５による衝突判定処理の手順を示すフローチャートである。

以下、図面を参照しながら本実施形態に係るロボット装置を説明する。ロボット装置は、ステッピングモータをアクチュエータとする関節部を有するロボットアーム機構を備える。当該ロボット装置としては、垂直多関節アーム機構、特に複数の関節部のうち一が直動伸縮関節を備えた垂直多関節アーム機構を例に説明する。以下の説明において、略同一の機能及び構成を有する構成要素については、同一符号を付し、重複説明は必要な場合にのみ行う。

図１は、本実施形態に係るロボット装置の外観斜視図である。ロボット装置を構成するロボットアーム機構は、略円筒形状の基部１と基部１に接続されるアーム部２とアーム部２の先端に取り付けられる手首部４とを有する。手首部４には図示しないアダプタが設けられている。例えば、アダプタは後述の第６回転軸ＲＡ６の回転部に設けられる。手首部４に設けられたアダプタには、用途に応じたロボットハンドが取り付けられる。

ロボットアーム機構は、複数、ここでは６つの関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を有する。複数の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６は基部１から順番に配設される。一般的に、第１、第２、第３関節部Ｊ１，Ｊ２，Ｊ３は根元３軸と呼ばれ、第４、第５、第６関節部Ｊ４，Ｊ５，Ｊ６はロボットハンドの姿勢を変化させる手首３軸と呼ばれる。手首部４は第４、第５、第６関節部Ｊ４，Ｊ５，Ｊ６を有する。根元３軸を構成する関節部Ｊ１，Ｊ２，Ｊ３の少なくとも一つは直動伸縮関節である。ここでは第３関節部Ｊ３が直動伸縮関節部、特に伸縮距離の比較的長い関節部として構成される。アーム部２は直動伸縮関節部Ｊ３（第３関節部Ｊ３）の伸縮部分を表している。

第１関節部Ｊ１は基台面に対して例えば垂直に支持される第１回転軸ＲＡ１を中心としたねじり関節である。第２関節部Ｊ２は第１回転軸ＲＡ１に対して垂直に配置される第２回転軸ＲＡ２を中心とした曲げ関節である。第３関節部Ｊ３は、第２回転軸ＲＡ２に対して垂直に配置される第３軸（移動軸）ＲＡ３を中心として直線的にアーム部２が伸縮する関節である。

第４関節部Ｊ４は、第４回転軸ＲＡ４を中心としたねじり関節である。第４回転軸ＲＡ４は、後述の第７関節部Ｊ７が回転していないとき、つまりアーム部２の全体が直線形状にあるとき、第３移動軸ＲＡ３と略一致する。第５関節部Ｊ５は第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を中心とした曲げ関節である。第６関節部Ｊ６は第４回転軸ＲＡ４に対して直交し、第５回転軸ＲＡ５に対して垂直に配置される第６回転軸ＲＡ６を中心とした曲げ関節である。

基部１を成すアーム支持体（第１支持体）１１ａは、第１関節部Ｊ１の第１回転軸ＲＡ１を中心に形成される円筒形状の中空構造を有する。第１関節部Ｊ１は図示しない固定台に取り付けられる。第１関節部Ｊ１が回転するとき、アーム部２は第１支持体１１ａの軸回転とともに左右に旋回する。なお、第１支持体１１ａが接地面に固定されていてもよい。その場合、第１支持体１１ａとは独立してアーム部２が旋回する構造に設けられる。第１支持体１１ａの上部には第２支持部１１ｂが接続される。

第２支持部１１ｂは第１支持部１１ａに連続する中空構造を有する。第２支持部１１ｂの一端は第１関節部Ｊ１の回転部に取り付けられる。第２支持部１１ｂの他端は開放され、第３支持部１１ｃが第２関節部Ｊ２の第２回転軸ＲＡ２において回動自在に嵌め込まれる。第３支持部１１ｃは第１支持部１１ａ及び第２支持部に連通する鱗状の外装からなる中空構造を有する。第３支持部１１ｃは、第２関節部Ｊ２の曲げ回転に伴ってその後部が第２支持部１１ｂに収容され、また送出される。ロボットアーム機構の直動伸縮関節部Ｊ３（第３関節部Ｊ３）を構成するアーム部２の後部はその収縮により第１支持部１１ａと第２支持部１１ｂの連続する中空構造の内部に収納される。

第３支持部１１ｃはその後端下部において第２支持部１１ｂの開放端下部に対して第２回転軸ＲＡ２を中心として回動自在に嵌め込まれる。それにより第２回転軸ＲＡ２を中心とした曲げ関節部としての第２関節部Ｊ２が構成される。第２関節部Ｊ２が回動するとき、アーム部２は第２回転軸ＲＡ２を中心に垂直方向に回動、つまり起伏動作をする。

第４関節部Ｊ４は、アーム部２の伸縮方向に沿ったアーム中心軸、つまり第３関節部Ｊ３の第３移動軸ＲＡ３に典型的には接する第４回転軸ＲＡ４を有するねじり関節である。第４関節部Ｊ４が回転すると、手首部４及び手首部４に取り付けられたロボットハンドは第４回転軸ＲＡ４を中心に回転する。第５関節部Ｊ５は、第４関節部Ｊ４の第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を有する曲げ関節部である。第５関節部Ｊ５が回転すると、第５関節部Ｊ５から先端にかけてロボットハンドとともに上下（第５回転軸ＲＡ５を中心に垂直方向）に回動する。第６関節部Ｊ６は、第４関節部Ｊ４の第４回転軸ＲＡ４に直交し、第５関節部Ｊ５の第５回転軸ＲＡ５に垂直な第６回転軸ＲＡ６を有する曲げ関節である。第６関節部Ｊ６が回転すると、ロボットハンドは左右に旋回する。

上記の通り手首部４のアダプタに取り付けられたロボットハンドは、第１、第２、第３関節部Ｊ１．Ｊ２．Ｊ３により任意位置に移動され、第４、第５、第６関節部Ｊ４、Ｊ５、Ｊ６により任意姿勢に配置される。特に第３関節部Ｊ３のアーム部２の伸縮距離の長さは、基部１の近接位置から遠隔位置までの広範囲の対象にロボットハンドを到達させることを可能にする。第３関節部Ｊ３はそれを構成する直動伸縮機構により実現される直線的な伸縮動作とその伸縮距離の長さとが特徴的である。

図２は、図１のロボットアーム機構の内部構造を示す斜視図である。直動伸縮機構はアーム部２と射出部３０とを有する。アーム部２は第１連結コマ列２１と第２連結コマ列２２とを有する。第１連結コマ列２１は複数の第１連結コマ２３からなる。第１連結コマ２３は略平板形に構成される。前後の第１連結コマ２３は、互いの端部箇所においてピンにより屈曲自在に列状に連結される。第１連結コマ列２１は内側や外側に自在に屈曲できる。

第２連結コマ列２２は複数の第２連結コマ２４からなる。第２連結コマ２４は横断面コ字形状の短溝状体に構成される。前後の第２連結コマ２４は、互いの底面端部箇所においてピンにより屈曲自在に列状に連結される。第２連結コマ列２２は内側に屈曲できる。第２連結コマ２４の断面はコ字形状であるので、第２連結コマ列２２は、隣り合う第２連結コマ２４の側板同士が衝突して、外側には屈曲しない。なお、第１、第２連結コマ２３、２４の第２回転軸ＲＡ２に向いた面を内面、その反対側の面を外面というものとする。第１連結コマ列２１のうち先頭の第１連結コマ２３と、第２連結コマ列２２のうち先頭の第２連結コマ２４とは結合コマ２７により接続される。例えば、結合コマ２７は第２連結コマ２４と第１連結コマ２３とを合成した形状を有している。

射出部３０は、複数の上部ローラ３１と複数の下部ローラ３２とが角筒形状のフレーム３５に支持されてなる。例えば、複数の上部ローラ３１は第１連結コマ２３の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。同様に、複数の下部ローラ３２は第２連結コマ２４の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。射出部３０の後方には、ガイドローラ４０とドライブギア５０とが第１連結コマ列２１を挟んで対向するように設けられる。ドライブギア５０は図示しない減速器を介してステッピングモータ３３０に接続される。第１連結コマ２３の内面には連結方向に沿ってリニアギアが形成されている。複数の第１連結コマ２３が直線状に整列されたときに互いのリニアギアは直線状につながって、長いリニアギアを構成する。ドライブギア５０は、直線状のリニアギアにかみ合わされる。直線状につながったリニアギアはドライブギア５０とともにラックアンドピニオン機構を構成する。

アーム伸長時、モータ５５が駆動し、ドライブギア５０が順回転すると、第１連結コマ列２１はガイドローラ４０により、アーム中心軸と平行な姿勢となって、上部ローラ３１と下部ローラ３２との間に誘導される。第１連結コマ列２１の移動に伴い、第２連結コマ列２２は射出部３０の後方に配置された図示しないガイドレールにより射出部３０の上部ローラ３１と下部ローラ３２との間に誘導される。上部ローラ３１と下部ローラ３２との間に誘導された第１、第２連結コマ列２１，２２は互いに押圧される。これにより、第１、第２連結コマ列２１，２２による柱状体が構成される。射出部３０は、第１、第２連結コマ列２１，２２を接合して柱状体を構成するとともに、その柱状体を上下左右に支持する。第１、第２連結コマ列２１、２２の接合による柱状体が射出部３０により堅持されることで、第１、第２連結コマ列２１，２２の接合状態が保持される。第１、第２連結コマ列２１、２２の接合状態が維持されているとき、第１、第２連結コマ列２１，２２の屈曲は互いに拘束される。それにより第１、第２連結コマ列２１、２２は、一定の剛性を備えた柱状体を構成する。柱状体とは、第２連結コマ列２２に第１連結コマ列２１が接合されてなる柱状の棒体を言う。この柱状体は第２連結コマ２４が第１連結コマ２３とともに全体として様々な断面形状の筒状体に構成される。筒状体とは上下左右が天板、底板及び両側板で囲まれ、前端部と後端部とが開放された形状として定義される。第１、第２連結コマ列２１、２２の接合による柱状体は、結合コマ２７が始端となって、第３移動軸ＲＡ３に沿って直線的に第３支持部１１ｃの開口から外に向かって送り出される。

アーム収縮時、モータ５５が駆動し、ドライブギア５０が逆回転されると、ドライブギア５０と係合している第１連結コマ列２１が第１支持体１１ａ内に引き戻される。第１連結コマ列の移動に伴って、柱状体が第３支持体１１ｃ内に引き戻される。引き戻された柱状体は射出部３０後方で分離される。例えば、柱状体を構成する第１連結コマ列２１はガイドローラ４０とドライブギア５０とに挟まれ、柱状体を構成する第２連結コマ列２２は重力により下方に引かれ、それにより第２連結コマ列２２と第１連結コマ列２１とは互いに離反される。離反された第１、第２連結コマ列２１，２２はそれぞれ屈曲可能な状態に復帰する。収納に際しては、射出部３０から、第１支持体１１ａ（基部１）の内部の収納部に第２連結コマ列２２は内側に屈曲されて搬送され、第１連結コマ列２１も第２連結コマ列２２と同じ方向（内側）に屈曲されて搬送される。第１連結コマ列２１は第２連結コマ列２２に略平行な状態で格納される。

図３は、図１のロボットアーム機構を図記号表現により示す図である。ロボットアーム機構において、根元３軸を構成する第１関節部Ｊ１と第２関節部Ｊ２と第３関節部Ｊ３とにより３つの位置自由度が実現される。また、手首３軸を構成する第４関節部Ｊ４と第５関節部Ｊ５と第６関節部Ｊ６とにより３つの姿勢自由度が実現される。

ロボット座標系Σbは第1関節部Ｊ１の第１回転軸ＲＡ１上の任意位置を原点とした座標系である。ロボット座標系Σｂにおいて、直交３軸（Ｘｂ、Ｙｂ，Ｚｂ）が規定されている。Ｚｂ軸は第１回転軸ＲＡ１に平行な軸である。Ｘｂ軸とＹｂ軸とは互いに直交し、且つＺb軸に直交する軸である。手先座標系Σｈは、手首部４に取り付けられたロボットハンド５の任意位置（手先基準点）を原点とした座標系である。例えば、ロボットハンド５が２指ハンドのとき、手先基準点（以下、単に手先という。）の位置は２指先間中央位置に規定される。手先座標系Σｈにおいて、直交３軸（Ｘｈ、Ｙｈ，Ｚｈ）が規定されている。Ｚｈ軸は第６回転軸ＲＡ６に平行な軸である。Ｘｈ軸とＹｈ軸とは互いに直交し、且つＺｈ軸に直交する軸である。例えば、Ｘｈ軸は、ロボットハンド５の前後方向に平行な軸である。手先姿勢とは、手先座標系Σhのロボット座標系Σｂに対する直交３軸各々周りの回転角（Ｘｈ軸周りの回転角（ヨウ角）α、Ｙｈ軸周りの回転角（ピッチ角）β、Ｚｈ軸周りの回転角（ロール角）γとして与えられる。

第１関節部Ｊ１は、第１支持部１１ａと第２支持部１１ｂとの間に配設されており、回転軸ＲＡ１を中心としたねじり関節として構成されている。回転軸ＲＡ１は第１関節部Ｊ１の固定部が設置される基台の基準面ＢＰに垂直に配置される。

第２関節部Ｊ２は回転軸ＲＡ２を中心とした曲げ関節として構成される。第２関節部Ｊ２の回転軸ＲＡ２は空間座標系上のＸb軸に平行に設けられる。第２関節部Ｊ２の回転軸ＲＡ２は第１関節部Ｊ１の回転軸ＲＡ１に対して垂直な向きに設けられる。さらに第２関節部Ｊ２は、第１関節部Ｊ１に対して、第１回転軸ＲＡ１の方向（Ｚb軸方向）と第１回転軸ＲＡ１に垂直なＹb軸方向との２方向に関してオフセットされる。第２関節部Ｊ２が第１関節部Ｊ１に対して上記２方向にオフセットされるように、第２支持体１１ｂは第１支持体１１ａに取り付けられる。第１関節部Ｊ１に第２関節部Ｊ２を接続する仮想的なアームロッド部分（リンク部分）は、先端が直角に曲がった２つの鈎形状体が組み合わされたクランク形状を有している。この仮想的なアームロッド部分は、中空構造を有する第１、第２支持体１１ａ、１１ｂにより構成される。

第３関節部Ｊ３は移動軸ＲＡ３を中心とした直動伸縮関節として構成される。第３関節部Ｊ３の移動軸ＲＡ３は第２関節部Ｊ２の回転軸ＲＡ２に対して垂直な向きに設けられる。第２関節部Ｊ２の回転角がゼロ度、つまりアーム部２の起伏角がゼロ度であってアーム部２が水平な基準姿勢においては、第３関節部Ｊ３の移動軸ＲＡ３は、第２関節部Ｊ２の回転軸ＲＡ２とともに第１関節部Ｊ１の回転軸ＲＡ１にも垂直な方向に設けられる。空間座標系上では、第３関節部Ｊ３の移動軸ＲＡ３はＸb軸及びＺb軸に対して垂直なＹb軸に平行に設けられる。さらに、第３関節部Ｊ３は、第２関節部Ｊ２に対して、その回転軸ＲＡ２の方向（Ｙb軸方向）と、移動軸ＲＡ３に直交するＺb軸の方向との２方向に関してオフセットされる。第３関節部Ｊ３が第２関節部Ｊ２に対して上記２方向にオフセットされるように、第３支持体１１ｃは第２支持体１１ｂに取り付けられる。第２関節部Ｊ２に第３関節部Ｊ３を接続する仮想的なアームロッド部分（リンク部分）は、先端が垂直に曲がった鈎形状体を有している。この仮想的なアームロッド部分は、第２、第３支持体１１ｂ、１１ｃにより構成される。

第４関節部Ｊ４は回転軸ＲＡ４を中心としたねじり関節として構成される。第４関節部Ｊ４の回転軸ＲＡ４は第３関節部Ｊ３の移動軸ＲＡ３に略一致するよう配置される。
第５関節部Ｊ５は回転軸ＲＡ５を中心とした曲げ関節として構成される。第５関節部Ｊ５の回転軸ＲＡ５は第３関節部Ｊ３の移動軸ＲＡ３及び第４関節部Ｊ４の回転軸ＲＡ４に略直交するよう配置される。
第６関節部Ｊ６は回転軸ＲＡ６を中心としたねじり関節として構成される。第６関節部Ｊ６の回転軸ＲＡ６は第４関節部Ｊ４の回転軸ＲＡ４及び第５関節部Ｊ５の回転軸ＲＡ５に略直交するよう配置される。第６関節部Ｊ６は手先効果器としてのロボットハンド５を左右に旋回するために設けられている。なお、第６関節部Ｊ６は、その回転軸ＲＡ６が第４関節部Ｊ４の回転軸ＲＡ４及び第５関節部Ｊ５の回転軸ＲＡ５に略直交する曲げ関節として構成されてもよい。

このように複数の関節部Ｊ１−Ｊ６の根元３軸のうちの一つの曲げ関節部を直動伸縮関節部に換装し、第１関節部Ｊ１に対して第２関節部Ｊ２を２方向にオフセットさせ、第２関節部Ｊ２に対して第３関節部Ｊ３を２方向にオフセットさせることにより、本実施形態に係るロボット装置のロボットアーム機構は、特異点姿勢を構造上解消している。

（駆動機構の説明） 図４
次に関節部Ｊ１−Ｊ６各々の駆動機構について説明する。ここでは第３関節部Ｊ３の駆動機構を例に説明するが、他の関節部Ｊ１−Ｊ２，Ｊ４−Ｊ６各々の駆動機構も同様である。

図４は、図１の第３関節部Ｊ３の駆動機構の構造を示す斜視図である。図４に示すように、第３関節部Ｊ３の駆動機構は、ステッピングモータ３３０を有する。ステッピングモータ３３０のドライブシャフト（駆動軸、入力軸）の一端には、ドライブシャフトエンコーダ２３５が係合される。ドライブシャフトエンコーダ２３５はドライブシャフトが所定の微小角回転するごとにパルス（エンコーダパルス）を出力する。ドライブシャフトの他端には駆動プーリーが直結される。この駆動プーリーと従動プーリーとの間にはベルトが掛け渡される。ベルトとしては平ベルト、Ｖベルト、歯付ベルト等任意の構造が採用され、その材料も合成ゴム、ポリウレタン任意のものが採用される。従動プーリーの径は駆動プーリーの径に対して所定の減速比αを実現するよう設計される。駆動プーリーと従動プーリーとベルトとは、ステッピングモータ３３０の回転をドライブギア３３１に伝達するための伝達機構を構成する。なお、伝達機構は、他の構成、例えばギア比の異なる複数のギアから構成されてもよい。従動プーリーのシャフト（出力軸）にはドライブギア３３１が直結される。従動プーリーのシャフト（出力軸、従動軸）には出力軸エンコーダ２３６が係合される。出力軸エンコーダ２３６は従動プーリーのシャフトが、例えば上記ステップ角を回転するごとにエンコーダパルスを出力する。

ドライブギア３３１は、直動伸縮関節部Ｊ３のアーム部２に形成されたリニアギアとともにラックアンドピニオン機構を構成する。ドライブギア３３１の回転角度に応じた距離（伸縮距離）を第３関節部Ｊ３のアーム部２は送り出される。ドライブギア３３１の回転軸（ドライブギアシャフト、従動軸、出力軸）には、伝達機構を介してステッピングモータ３３０の回転軸（ドライブシャフト、駆動軸）に接続される。伝達機構は、典型的にはステッピングモータ３３０の回転速度を減速し、ステッピングモータ３３０のトルクを増大させるために減速比αの減速機構を構成する。例えば、ドライブギア３３１に減速比２の減速機構が用いられたとき、ドライブギア３３１は、ステッピングモータ３３０の回転角速度の１／２倍の回転角速度で回転し、ステッピングモータ３３０のトルクの２倍のトルクを発生する。

ドライブシャフトエンコーダ（第１エンコーダ）２３５と出力軸エンコーダ（第２エンコーダ）２３６とはそれぞれカウンタ２３７，２３８とともにインクリメンタル型ロータリーエンコーダを構成する。光学式ロータリーエンコーダや磁気式ロータリーエンコーダ等のいずれの方式でもよい。ドライブシャフトエンコーダ２３５は、ドライブシャフトが一定の回転角（分解能、ステップ角）を回転する毎にパルス信号（Ａ相信号とＢ相信号）を２相、例えば９０度の位相差でもって出力する。また、ドライブシャフトエンコーダ２３５は、ドライブシャフトが１回転する毎に原点用のパルス信号（Ｚ相信号）を出力する。出力軸エンコーダ２３６は、ドライブシャフトエンコーダ２３５と典型的には同一機能及び性能を備えたものが採用され、ドライブギアシャフトが上記同一のステップ角（分解能）を回転する毎にパルス信号（Ａ相信号とＢ相信号）を２相、例えば９０度の位相差でもって出力し、またドライブギアシャフトが１回転する毎に原点用のパルス信号（Ｚ相信号）を出力する。

（構成の説明）図５
図５は、本実施形態に係るロボット装置の構成を示すブロック図である。本実施形態に係るロボット装置のロボットアーム機構の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５、Ｊ６には、アクチュエータとして、それぞれステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０が設けられている。ステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０には、ドライバユニット２１０、２２０，２３０，２４０，２５０，２６０が電気的に接続されている。これらドライバユニット２１０、２２０，２３０，２４０，２５０，２６０は、同一の構成を有し、外部の制御装置１００からの制御信号（指令値）に従って、制御対象のステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０を駆動する。ここではドライバユニット２３０の構成及び動作を説明する。他のドライバユニット２１０，２２０，２４０，２５０，２６０の構成及び動作はドライバユニット２３０の構成及び動作と同一であるので説明は省略する。

ステッピングモータ３３０は、ロータの周囲に複数のステータコイルを配置してなる。ステータコイルはスイッチング素子を介して電源回路２３２に接続される。これらスイッチング素子は、パルス信号発生部２３３から供給されるパルス信号によりオンされる。これらスイッチング素子が順番にオンされることで、ステッピングモータ３３０（ロータ）は、所定のステップ角で順次回転する。ステッピングモータ３３０の回転速度は、パルス信号の周波数（パルス周波数）を変化させることで変える事ができる。

ドライバユニット２３０は、ステッピングモータ３３０の駆動および停止を制御する。ドライバユニット２３０は、制御部２３１と、電源回路２３２と、パルス信号発生部２３３と、ドライブシャフトエンコーダ２３５と、出力軸エンコーダ２３６と、カウンタ２３７と、カウンタ２３８とを有する。制御部２３１は、制御装置１００から入力された指令値に従って、ドライバユニット２３０を統括して制御する。

制御部２３１には制御装置１００からステッピングモータ３３０の励磁電流値を表す指令値（電流指令コード）が供給される。制御部２３１では電源回路２３２に対して電流指令コードに従った電流を発生するための制御を実行する。電源回路２３２は、電流可変のＡＣ／ＤＣ変換方式電源回路であり、電流指令コードにより指定された励磁電流値の電流を発生する。発生された励磁電流はステッピングモータ３３０のステータコイルに供給される。

また制御部２３１には制御装置１００から次の伸縮距離を表す指令値（位置指令コード）が供給される。次の伸縮長とは、所定の制御周期Δｔ（例えば、１０ｍｓ）後の伸縮距離を指す。なお、伸縮距離とは、アーム部２が最も収縮した状態からの距離を表す。なお、指令値としては、関節部Ｊ１，Ｊ２，Ｊ４，Ｊ５、Ｊ６のドライバユニットの制御部であれば、関節角度として与えられる。関節角度とは、基準位置（原点）からの正負の回転角度を表す。伸縮距離と関節角度とを関節変数と総称する。

制御部２３１は、パルス信号発生部２３３に対して、位置指令コードに応じたパルス制御信号を出力する。具体的には、制御部２３１は、現在の伸縮距離から制御装置１００から入力された所定の制御周期△ｔ後の伸縮距離との距離差を変位するために必要なドライブギア５０の回転角度をステップ長で除算することによりパルス数を決定し、制御周期△ｔをパルス数で除算しその逆数によりパルス周波数を決定する。制御部２３１は、決定したパルス条件（パルス数とパルス周波数）に対応するパルス制御信号をパルス信号発生部２３３に対して出力する。

パルス信号発生部２３３は、制御部２３１から出力されたパルス制御信号で与えられたパルス数のパルスを、ステータコイルと電源回路２３２との間のスイッチング素子に対してパルス周波数で順次切り替えながら供給する。これにより周期Δｔにステッピングモータ３３０は上記距離差を変位するために必要なドライブギア５０の回転角度だけ回転し、それにより指令された伸縮距離までアーム部２は伸縮する。同様に、関節部Ｊ１，Ｊ２，Ｊ４、Ｊ５，Ｊ６に対応するドライバユニット２１０，２２０、２４０，２５０，２６０各々には、次の関節角度を表す位置指令コードが制御装置１００から入力され、これにより関節部Ｊ１，Ｊ２，Ｊ４，Ｊ５，Ｊ６各々は指令された関節角度まで回転される。

制御部２３１には制御装置１００から関節部Ｊ３を現在位置で停止させるための動作停止信号が入力される。ステッピングモータ３３０は、現在位置に対応する相のステータコイルに継続的に電流を供給することによりその位置で停止する。そのために、制御部２３１には、動作停止信号として、例えば関節部Ｊ３の現在の伸縮距離（関節変数）を表すコードが与えられる。同様に、関節部Ｊ１，Ｊ２、Ｊ４、Ｊ５，Ｊ６に対応するドライバユニット２１０、２２０，２４０，２５０，２６０各々には現在の関節角度を表すコードを含む動作停止信号が制御装置１００から入力される。これにより、ロボットアーム機構の動作が停止する。

ドライブシャフトエンコーダ２３５は、ドライブシャフトが一定の回転角（分解能、エンコーダステップ角）を回転する毎にパルスを出力する。実際には順／逆の回転方向を識別するためにＡ相／Ｂ相の２種のパルスが出力される。ドライブシャフトエンコーダ２３５は、ドライブシャフトが１回転する毎に原点用のパルス（Ｚ相パルス）をカウンタ２３７に対して出力する。カウンタ２３７は、ドライブシャフトエンコーダ２３５から出力されたＡ相（又はＢ相）パルスを回転方向に応じて加減算をする。カウンタ２３７は、ドライブシャフトエンコーダ２３５からＺ相パルスが出力されると、カウント値をリセットする。したがって、エンコーダパルスのカウント値は、ドライブシャフトの原点からの回転変位（回転角度）を表す。カウンタ２３７は、ドライブシャフトエンコーダ２３５から出力されたＺ相パルスも計数する。Ｚ相パルスのカウント値は“回転数”を表す。以下、単に「カウント値」というときは、回転角度を表すカウント値と回転数を表すカウント値との総称するものとし、両者を区別する場合には「回転角度を表すカウント値」と「回転数を表すカウント値」を称するものとする。カウンタ２３７からは一定周期△Ｔごとにカウント値が出力される。この周期△Ｔは上記制御周期△ｔと同一でも良いし、相違するものであっても良い。カウンタ２３７から出力されるドライブシャフトの回転に係るカウント値のデータは、制御装置１００のシステム制御部１０１の制御に従って、ドライバユニットインターフェース１０７を介して制御装置１００に取り込まれる。

出力軸エンコーダ２３６は、ドライブシャフトエンコーダ２３５と同様であり、ドライブギア３３１の回転軸（ドライブギアシャフト、出力軸）が一定の回転角を回転する毎にパルス信号（Ａ相／Ｂ相）をカウンタ２３８に対して出力し、ドライブギアシャフトが１回転する毎に原点用のパルス信号（Ｚ相信号）をカウンタ２３８に対して出力する。カウンタ２３８では、出力軸エンコーダ２３６から出力されたＡ相パルス（又はＢ相パルス）を計数し、またＺ相パルスを計数する。ドライブギアシャフトの回転数を表すカウント値を、ドライブギアシャフトの回転数を表すカウント値とともに一定周期△Ｔごとに出力する。

制御装置１００は、システム制御部１０１と、操作部インターフェース１０２と、記憶部１０３と、計算処理部１０４と、衝突判定部１０５と、表示制御部１０６と、ドライバユニットインターフェース１０７と、出力部とを有する。
制御装置１００には、システム制御部１０１の制御に従って、ドライバユニット２１０−２６０各々から、ドライブシャフトの回転角度を表すカウント値と回転数を表すカウント値に関するデータと、ドライブギアシャフトの回転角度を表すカウント値と回転数を表すカウント値とに関するデータとが、ドライバユニットインターフェース１０７を介して所定の制御周期△Ｔ毎（例えば、１０ｍｓ毎）に供給される。

制御装置１００には、操作部インターフェース１０２を介して操作部６０が接続されている。操作部６０は、手首部４又はロボットハンド（手先効果器）の着目点の位置の変更、姿勢の変更および変更時間をオペレータが入力するための入力インターフェースとして機能する。例えば、操作部６０は、ロボットハンドを移動させる最終目標位置と移動時間とを指定するためのジョイスティック等を備える。例えば、ジョイスティックが操作された方向、ジョイスティックが傾けられた角度、ジョイスティックの操作加速度に基づいて、ロボットハンドの最終目標位置と移動時間とが入力される。また、操作部６０は、ロボットアーム機構の動作を停止するための動作停止ボタンを備える。なお、操作部６０を構成するこれらの入力デバイスは、他のデバイス、例えば、マウス、キーボード、トラックボールおよびタッチパネル等で代替が可能である。例えば、ダイレクトティーチング時のペンダントとしの操作部６０は、典型的にはタッチパネル等が採用される。

システム制御部１０１は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）と半導体メモリ等を有し、制御装置１００を統括して制御する。システム制御部１０１には、制御／データバス１２０を介して各部が接続されている。

ドライブシャフト（入力軸）の回転角度と回転数に対して、減速比αで換算した上で、ドライブギア３３１のシャフト（出力軸）の回転角度と回転数に差異（回転ずれ、角度差）が生じているとき、衝突等の外乱が生じている恐れがある。しかし、それが伝達機構のベルト伸縮やバックラッシュ等のガタによるものである場合もあり、それを排除するために、ここでは上記「回転ずれ」に対して閾値を設定し、上記「回転ずれ」が閾値を超過しているときに、そのずれが衝突等の外乱に起因するものであると判定する。記憶部１０３はこの閾値θｔｈのデータを記憶する。同様に、記憶部１０３は、関節部Ｊ１，Ｊ２，Ｊ４，Ｊ５、Ｊ６各々に対応する回転ずれの原因が、伝達機構のガタによる場合を排除し、衝突等の外乱によるものであるか否かを判定するための閾値のデータを記憶する。閾値は、それぞれの伝達機構のガタにより設定されているが、操作部６０を介してオペレータ指示に従って任意に変更することもできる。また、記憶部１０３は、ロボット装置に干渉物が衝突したことをオペレータに対して通知するためのメッセージを有する通知画像のデータを記憶する。

計算処理部１０４は、ドライバユニット２１０―２６０各々に対して指令値として与える、制御周期Δｔ経過後の関節変数ベクトルを計算する。なお、関節変数ベクトルとは、関節部Ｊ１−Ｊ６の６つの関節変数、つまり回転関節部Ｊ１、Ｊ２、Ｊ４−Ｊ６の関節角度と直動伸縮関節部Ｊ３の直動変位との６変数をいう。

まず、計算処理部１０４は、制御周期Δｔ経過後の関節変数ベクトルを計算するために、手先着目点の現在位置・姿勢を計算する。具体的には、計算処理部１０４１０４は、ドライバユニット２１０−２６０各々のカウンタにより計数されたドライブシャフトの回転角度と回転数とによりエンコーダ原点からの累積カウント値を計算し、累積カウント値に１カウントに対応するステップ角を乗算することにより、関節部Ｊ１−Ｊ６各々の現在の関節変数を計算する。計算処理部１０４は、同次変換行列Ｋにパラメータとして関節部Ｊ１−Ｊ６各々の現在の関節変数を代入することにより、ロボット座標系Σｂから見た手先着目点の現在位置・姿勢を計算する。同次変換行列Ｋは、手先座標系Σｈとロボット座標系Σbとの関係を定義する行列式である。同次変換行列Ｋは、ロボットアーム機構を構成するリンク間の関係（リンク長とリンクのねじれ角）と関節部の軸間の関係（リンク間距離とリンクの間角度）とで決まる。計算処理部１０４１０４は、制御周期Δｔ毎に上述の計算処理を繰り返し行い、制御周期Δｔ毎の手先着目点の現在位置・姿勢を計算する。

次に、計算処理部１０４は、計算した関節部Ｊ１−Ｊ６各々の現在の関節変数と、操作部５０を介してオペレータにより入力された手先の最終目標位置・姿勢とに基づいて、その間を結ぶ単位時間△ｔ（制御周期、例えば１０ｍｓ）毎の手先着目点の目標位置の点列を計算する。具体的には、計算処理部１０４は、予めプリセットされている、手先着目点の軌道計算式に、パラメータとして手先着目点の現在位置・姿勢と手先着目点の最終目標位置・姿勢を代入することにより、手先着目点の軌道（以下、手先軌道という）を計算し、その手先軌道上に単位時間△ｔ毎の目標位置の点列を計算する。軌道計算方法としては任意の方法が採用される。

最後に、計算処理部１０４は、計算した複数の目標位置にそれぞれ対応する複数の関節変数ベクトルを計算する。具体的には、計算処理部１０４は、手先着目点の現在位置・姿勢と、制御周期Δｔ経過後の次の目標位置・姿勢と、制御周期Δｔとに基づいて、手先速度を計算し、計算した手先速度をヤコビアン逆行列を用いて関節角速度に変換する。ヤコビアン逆行列は、手先着目点の位置・手先姿勢を表すベクトルの関節変数による編微分で与えられ、手先速度（手先位置・姿勢の微小変化）を関節角速度（関節角度・伸縮長の微小変化）に変換する行列である。ヤコビアン逆行列は、現在の関節変数ベクトルとアーム構造のリンクパラメータとにより計算される。計算処理部１０４は、関節角速度に単位時間△ｔを乗算することにより、単位時間△ｔの間の各関節部の変位量を計算し、計算した各関節部の変位量を移動直前(現在)の関節変数ベクトルに加算することにより、単位時間△ｔ経過後の関節変数ベクトルを計算する。

出力部１０８は、システム制御部１０１の制御に従って、計算処理部１０４で計算された関節部Ｊ１−Ｊ６各々の指令値（関節変数）を、ドライバユニット２１０−２６０各々に対して出力する。また、出力部１０８は、システム制御部１０１の制御に従って、動作停止信号をドライバユニット２１０−２６０各々に対して出力する。

衝突判定部１０５は、ドライブシャフトエンコーダ２３５の出力と出力軸エンコーダ２３６の出力に基づいて、実際にはそれらのカウンタ２３７，２３８のカウント値に基づいてロボットアーム部２等に干渉物が衝突する等の外乱因子に起する動作異常が発生したか否かを判定する。ここでは動作異常として干渉物の衝突を例に説明する。衝突判定部１０５は「ロボット装置に干渉物が衝突した」と判定したとき、衝突判定信号を出力する。衝突判定部１０５の詳細は後述する。

システム制御部１０１は、衝突判定部１０５から衝突判定信号が出力されたとき、記憶部１０３から通知画像を読み出し、表示制御部１０６のフレームメモリに書き込む。表示制御部１０６は、フレームメモリに格納された画像のデータを読み出し、表示部７０に表示する。表示部７０としては、典型的に例えばＣＲＴディスプレイや液晶ディスプレイ、有機ＥＬディスプレイ、プラズマディスプレイ等が挙げられる。

（衝突判定処理） 図６
本実施形態に係るロボット装置は、ロボット装置に干渉物が衝突したか否かを判定するための衝突判定機能を有する。衝突判定機能に係る処理を衝突判定処理という。衝突判定部１０５による衝突判定処理は衝突判定周期（例えば制御周期（１０ｍｓ）に等価）毎に繰り返し行われる。図６は、図５の衝突判定部１０５による衝突判定処理の手順を示すフローチャートである。ここでは、関節部Ｊ３の衝突判定処理について説明する。なお、衝突判定部１０５は、図６と同様の手順で他の関節部Ｊ１，Ｊ２，Ｊ４，Ｊ５，Ｊ６各々の衝突判定処理を行うことができる。

（ステップＳ１１）
システム制御部１０１の制御に従って、ステッピングモータ３３０のドライブシャフト（入力軸）の回転角度と回転数とを表すカウント値がカウンタ２３７から制御装置１００に取り込まれる。同様に、システム制御部１０１の制御に従って、ドライブギア３３１のドライブギアシャフト（出力軸）の回転角度と回転数とを表すカウント値がカウンタ２３８から一定の周期△Ｔで繰り返し制御装置１００に供給される。

（ステップＳ１２）
ステップＳ１１で入力されたドライブシャフトの回転に関するカウント値に基づいて、衝突判定部１０５によりドライブシャフトの回転に関する累計カウント値が順次計算され、記憶される。現在の累計カウント値と、１周期前（△Ｔ）の累計カウント値に対するカウント値との差（変位）Ｎｉｎが衝突判定部１０５により計算される。

同様に、ドライブギアシャフトの回転に関するカウント値に基づいて、衝突判定部１０５によりドライブギアシャフトの回転に関する累計カウント値が順次計算され、記憶される。現在の累計カウント値と、１周期前の累計カウント値との差（変位）Ｎｏｕｔが衝突判定部１０５により計算される。

（ステップＳ１３）
ステップＳ１２で計算されたドライブシャフトの回転に関するカウント値の変位Ｎｉｎにドライブシャフトエンコーダ２３５の分解能（ステップ角）Δθｉｎを乗算することにより、１周期△Ｔの間のステッピングモータ３３０の回転角度変位θｉｎが計算される。同様に、ステップＳ１２で計算されたドライブギアシャフトの回転に関するカウント値の変位Ｎｏｕｔに出力軸エンコーダ２３６の分解能（ステップ角）Δθｏｕｔを乗算することにより、１周期の間のドライブギア３３１の回転角度変位θｏｕｔが計算される。

（ステップＳ１４）
ステップＳ１３で計算された１周期△Ｔの間のステッピングモータ３３０の回転角度の変位θｉｎに対する、ステップＳ１３で計算された１周期△Ｔの間のドライブギア３３１の回転角度の変位θｏｕｔを減速比αにより換算、つまり変位θｏｕｔに減速比αの逆数を乗算した角度の差、つまり入力軸と出力軸との間の「回転ずれ」を計算する。計算した角度差の絶対値が、伝達機構のベルト伸縮やバックラッシュ等のガタに起因する回転ずれを排除するために既定された閾値θｔｈ以下のとき（ステップＳ１４のＮｏ）、衝突判定部１０５は「ロボット装置に干渉物が衝突していない」と判定し、ステップＳ１１に処理が戻る。一方、計算した角度差の絶対値が、閾値θｔｈよりも大きいとき（ステップＳ１４のＹｅｓ）、衝突判定部１０５は「ロボット装置に干渉物が衝突した」と判定し、衝突判定信号を出力する。

なお、上記説明ではドライブシャフトとドライブギアシャフトとの入出力軸間の回転ずれを衝突判定に利用しているが、ステッピングモータではその回転を指示する指令値による回転角度と、実際のドライブシャフトの回転角度とは原則的に等価であることを前提としている。したがって、衝突判定に、その指令値で指令された回転角度と、ステッピングモータ３３０から見て被駆動部としてのドライブギアのシャフトに係合された出力軸エンコーダ２３８のパルスのカウント値から求めた回転角度とを比較して、衝突等の動作異常の判定をするものであっても良い。

（ステップＳ１５）
ステップＳ１４で衝突判定部１０５により衝突判定信号が出力されると、システム制御部１０１は、ロボット装置に干渉物が衝突したことを通知する通知画像のデータを記憶部１０３から読み出し表示制御部１０６のフレームメモリに書き込む。

表示制御部１０６はフレームメモリに格納された通知画像のデータを読み出し、表示部７０に表示する。これにより、表示部７０にはロボット装置に干渉物が衝突したことを通知するメッセージが表示される。

（ステップＳ１６）
ステップＳ１４で衝突判定部１０５により衝突判定信号が出力されると、上記メッセージ出力とともに、システム制御部１０１により、出力部１０８から指令値の出力を停止させ、ロボットアーム機構の動作を停止（緊急停止）させ、ロボット装置に与えられている作業（タスク）を中断し、タスクスケジュール自体をリセットする。

なお、衝突判定部１０５では関節部Ｊ１−Ｊ６の少なくとも一つについて衝突が発生したと判定されたとき、衝突判定信号を出力する。換言すると、衝突判定部１０５は関節部Ｊ１−Ｊ６についての６系統の工程Ｓ１４の比較出力（Ｙｅｓフラグ“１”、Ｎｏフラグ“０”）をＯＲ回路に導入し、その中の一つでも回転ずれが閾値を超過していてフラグ“１”がたっているときに、フラグ“１”を出力し、それにしたがって衝突判定信号を出力する。

つまり、本実施形態では、アーム部２やハンド部に対してどのような方向からどの箇所に作業員等が接触又は衝突したとしても、必ず関節部Ｊ１−Ｊ６の少なくとも一つについて衝突判定がなされることになる。

（ステップＳ１７、Ｓ１８）
衝突発生によりタスクタイム管理は崩れており、タスク動作は例えばラインの搬送機器等の外部機器との連携も必要であることから、アーム動作再開に限定されるものではなく、タスク動作それ自体を再開させる必要がある。また衝突の原因解消も必要とされる。従って工程Ｓ１６による動作停止後、システム制御部１０１はオペレータによるタスク再開の指示を待って、タスクを再開させる。

例えば、工程Ｓ１６による動作停止時点から一定時間経過しても、オペレータによるタスク再開の指示がないとき、アーム動作を終了する。

（効果）
以上説明した関節部Ｊ３の衝突判定処理によれば、ロボット装置の関節部Ｊ３よりも先端部分に対して、関節部Ｊ３の第３移動軸ＲＡ３に平行な成分を有する外部トルクが発生するような干渉物の衝突を検知することができる。この衝突判定処理は、関節部Ｊ３に設けられたドライブシャフトエンコーダ２３５の回転角度と出力軸エンコーダ２３６の回転角度との間に所定の角度差が発生したか否かで行う。これは、関節部Ｊ３のアーム部２が伸縮する方向（第３移動軸に平行な方向）に、干渉物の衝突による外部トルクが発生したとき、ステッピングモータ３３０の回転角度とドライブギア３３１（関節部）の実際の回転角度との間に角度差が生じることを利用している。しかしながら、ステッピングモータ３３０とドライブギア３３１との間の伝達機構は、弾性変形等するため、動作開始時や動作停止時に関節部に働く慣性等の、衝突ではない理由によって、ステッピングモータ３３０の回転角度とドライブギア３３１の実際の回転角度との間に角度差が生じる場合がある。このような場合を衝突判定から除外するために、角度差に閾値を設けることで、角度差が閾値を超過したときにのみ、「ロボット装置に干渉物が衝突した」と判定することができる。

なお、関節部Ｊ３の衝突判定処理では、ロボット装置の関節部Ｊ３よりも先端部分に対して、関節部Ｊ３の第３移動軸ＲＡ３に平行な成分を有する外部トルクが発生するような干渉物の衝突した検知することができない。しかしながら、他の関節部Ｊ１、Ｊ２，Ｊ４，Ｊ５，Ｊ６に対しても、関節部Ｊ３と同様の衝突判定処理を行うことで、より網羅的にロボットアーム機構への干渉物の衝突を検知することができる。例えば、関節部Ｊ１の衝突判定処理によれば、ロボット装置の関節部Ｊ１よりも先端部分に対して、関節部Ｊ１の第１回転軸ＲＡ１に直交する成分を有する外部トルクが発生するような干渉物の衝突を検出することができる。また、関節部Ｊ２の衝突判定処理によれば、ロボット装置の関節部Ｊ２よりも先端部分に対して、関節部Ｊ２の第１回転軸ＲＡ２に直交する成分を有する外部トルクが発生するような干渉物の衝突を検出することができる。以上説明した本実施形態に係るロボット装置によれば、ロボットアーム機構の関節部Ｊ１−Ｊ６各々に対する衝突判定処理により、ロボットアーム機構への干渉物の接触を網羅的に検知するができる。つまり、本実施形態では、アーム部２やハンド部に対してどのような方向からどの箇所に作業員等が接触又は衝突したとしても、必ず関節部Ｊ１−Ｊ６の少なくとも一つについて衝突判定がなされることになる。

上記説明では、衝突等の動作異常判定は、ステッピングモータ３３０のモータドライバ２３０から見て外部の制御装置１００の衝突判定部１０５で行なっていたが、ステッピングモータ３３０のモータドライバ２３０の制御部２３１（モータ制御装置）においてカウンタ２３７，２３８の出力を比較して、又は外部の制御装置１００からの指令値と出力軸エンコーダ２３６のカウンタ２３８のカウント値とを比較して衝突判定を行うようにしてもよい。

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

４１…表示部、６０…操作部、１００…制御装置、１０１…システム制御部、１０２…操作部Ｉ／Ｆ、１０３…記憶部、１０４…計算処理部、１０５…衝突判定部、１０６…表示制御部、１０７…ドライバユニットＩ／Ｆ、１０８…出力部、１２０…データ／制御バス、２１０〜２６０…ドライバユニット、２３１…制御部、２３２…電源回路、２３３…パルス信号発生部、２３５…ドライブシャフトエンコーダ、２３５…出力軸エンコーダ、２３６、２３７…カウンタ、３１０〜３６０…ステッピングモータ

Claims (8)

1. 多関節アーム機構を備えるロボット装置において、
   複数のリンクと、
   前記複数のリンク間を接続する複数の関節部と、
   前記複数の関節部を駆動するための動力を発生する複数のモータと、
   前記複数の関節部の少なくとも一の関節部のモータのドライブシャフトの回転を前記少なくとも一の関節部の回転軸に伝達する伝達機構と、
   前記少なくとも一の関節部のモータのドライブシャフトの回転を検出する第１エンコーダと、
   前記少なくとも一の関節部の回転軸の回転を検出する第２エンコーダと、
   前記第１エンコーダから出力されるエンコーダパルスと前記第２エンコーダから出力されるエンコーダパルスとに基づいて動作異常の判定をする判定部とを具備することを特徴とするロボット装置。
2. 前記判定部は、前記第１エンコーダから出力されるエンコーダパルスの数に前記第１エンコーダのステップ角を乗算した回転角度に対する、前記第２エンコーダから出力されるエンコーダパルスの数に前記第２エンコーダのステップ角を乗算した回転角度の前記伝達機構の減速比により換算した角度差が所定の閾値を超過するとき、前記動作異常を判定することを特徴とする請求項１記載のロボット装置。
3. 前記判定部は、前記第１エンコーダから出力されるエンコーダパルスの数と前記第２エンコーダから出力されるエンコーダパルスの数とに基づいて前記動作異常を判定することを特徴とする請求項１記載のロボット装置。
4. 前記判定部により動作異常が判定されたとき、表示又は音あるいは両方により警告を出力する出力部をさらに備えることを特徴とする請求項１記載のロボット装置。
5. 前記判定部により動作異常が判定されたとき、前記複数のモータの駆動を停止する制御部をさらに備えることを特徴とする請求項１記載のロボット装置。
6. リンク間を接続する関節部と、
   前記関節部を駆動するための動力を発生するモータと、
   前記モータのドライブシャフトの回転を前記リンクに接続される前記関節部の回転軸に伝達する伝達機構と、
   前記モータのドライブシャフトの回転を検出する第１エンコーダと、
   前記関節部の回転軸の回転を検出する第２エンコーダと、
   前記第１エンコーダから出力されるエンコーダパルスと前記第２エンコーダから出力されるエンコーダパルスとに基づいて動作異常の判定をする判定部とを具備することを特徴とするアーム機構。
7. 被駆動部を駆動するための動力を発生するモータを制御するモータ制御装置において、
   前記モータのドライブシャフトの回転を検出する第１エンコーダと、
   前記被駆動部の回転を検出する第２エンコーダと、
   前記第１エンコーダから出力されるエンコーダパルスと前記第２エンコーダから出力されるエンコーダパルスとに基づいて動作異常の判定をする判定部とを具備することを特徴とするモータ制御装置。
8. 被駆動部を駆動するための動力を発生するモータを制御するモータ制御装置において、
   外部からの指令値に従って前記モータを駆動する駆動部と、
   前記被駆動部の回転を検出するエンコーダと、
   前記指令値と前記エンコーダから出力されるエンコーダパルスとに基づいて動作異常の判定をする判定部とを具備することを特徴とするモータ制御装置。

Images