JP6399591B2 - ロボットアーム機構及びステッピングモータ制御装置 - Google Patents

Description

本発明の実施形態はロボットアーム機構及びステッピングモータ制御装置に関する。

ステッピングモータは、例えばサーボモータとしてロボット等に広く使用されている。ステッピングモータは、過負荷がかかったり、急な速度変化が発生すると脱調する。脱調は、ステッピングモータに入力したパルス信号と実際のモータ回転との間の同期が失われる現象である。脱調が発生すると、ステッピングモータは目標の位置まで回転されない。そのため、まず脱調の発生を検出する必要がある。脱調を検出する方法には、ステッピングモータにエンコーダを設置し、ステッピングモータをフィードバック制御する方法がある。この方法によると、エンコーダの出力に基づいて、ステッピングモータの現在位置を特定することができるため、目標位置と現在位置との間の不一致を脱調として検出することができる。一般的に、ステッピングモータの脱調が検出されると、直後にステッピングモータを停止する制御が行われる。しかしながら、例えば、脱調の要因がすぐに除去できるような場合においては、脱調が発生しても、ステッピングモータを停止させずに、そのまま制御を継続させた方が良い場合がある。

目的は、脱調検出後、ステッピングモータを好適に動作させることのできる制御を提供することにある。

本実施形態に係るロボットアーム機構は、関節部を有するロボットアームと、前記関節部を作動させる動力を発生するステッピングモータと、前記ステッピングモータを、目標角度に従って駆動するモータドライバと、前記ステッピングモータのドライブシャフトが所定の角度を回転する毎にエンコーダパルスを出力するエンコーダと、前記エンコーダパルスに基づいて特定された前記ステッピングモータの現在角度と前記目標角度とに基づいて、前記ステッピングモータの脱調を検出する脱調検出部と、を具備し、前記モータドライバは、前記脱調が検出されてから所定の猶予時間が経過するまでに、前記脱調が回復されなかったとき、前記猶予時間の経過の時点で前記ステッピングモータの駆動を停止する。

図１は、本実施形態に係るロボットアーム機構の外観斜視図である。 図２は、図１のロボットアーム機構の内部構造を示す斜視図である。 図３は、図１のロボットアーム機構の内部構造を断面方向から見た図である。 図４は、本実施形態に係るロボットアーム機構の構成の一例を示すブロック構成図である。 図５は、本実施形態に係るロボットアーム機構の駆動部の動作を説明するためのフローチャートである。 図６は、本実施形態の変形例１に係るロボットアーム機構の駆動部の動作を説明するためのフローチャートである。 図７は、本実施形態の変形例２に係るロボットアーム機構の駆動部の動作を説明するためのフローチャートである。

以下、図面を参照しながら本実施形態に係るロボットアーム機構を説明する。以下の説明において、略同一の機能及び構成を有する構成要素については、同一符号を付し、重複説明は必要な場合にのみ行う。

図１は、本実施形態に係るロボットアーム機構の外観斜視図である。図２、図３は図１のロボットアーム機構の内部構造を示している。ロボットアーム機構は、略円筒形状の基部１と基部１に接続するアーム部２とを有する。アーム部２の先端にはエンドエフェクタと呼ばれる手先効果器３が取り付けられる。図１では手先効果器３として対象物を把持可能なハンド部を図示している。手先効果器３としてはハンド部に限定されず、他のツール、またはカメラ、ディスプレイであってもよい。アーム部２の先端には任意の種類の手先効果器３に交換することができるアダプタが設けられていてもよい。

アーム部２は、複数、ここでは６つの関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を有する。複数の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６は基部１から順番に配設される。一般的に、第１、第２、第３軸ＲＡ１，ＲＡ２，ＲＡ３は根元３軸と呼ばれ、第４、第５、第６軸ＲＡ４，ＲＡ５，ＲＡ６はハンド部３の姿勢を変化させる手首３軸と呼ばれる。根元３軸を構成する関節部Ｊ１，Ｊ２，Ｊ３の少なくとも一つは直動関節である。ここでは第３関節部Ｊ３が直動関節、特に伸縮距離の比較的長い関節部として構成される。第１関節部Ｊ１は台座面に対して例えば垂直に支持される第１回転軸ＲＡ１を中心としたねじり関節である。第２関節部Ｊ２は第１回転軸ＲＡ１に対して垂直に配置される第２回転軸ＲＡ２を中心とした曲げ関節である。第３関節部Ｊ３は、第２回転軸ＲＡ２に対して垂直に配置される第３軸（移動軸）ＲＡ３を中心として直線的に伸縮する関節である。第４関節部Ｊ４は、第３移動軸ＲＡ３に一致する第４回転軸ＲＡ４を中心としたねじり関節であり、第５関節部Ｊ５は第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を中心とした曲げ関節である。第６関節部Ｊ６は第４回転軸ＲＡ４に対して直交し、第５回転軸ＲＡ５に対して垂直に配置される第６回転軸ＲＡ６を中心とした曲げ関節である。

第１関節部Ｊ１のねじり回転によりアーム部２がハンド部３とともに旋回する。図３に示すように、ステッピングモータ３１ａは第１関節部Ｊ１の近傍に設けられる。ステッピングモータ３１ａが回転するとき、ステッピングモータ３１ａのドライブシャフトの動力は、図示しないギア等を介して第１関節部Ｊ１に伝達される。それにより、第１関節部Ｊ１が回転され、アーム部２がハンド部３とともに旋回する。

第２関節部Ｊ２の曲げ回転によりアーム部２がハンド部３とともに第２関節部Ｊ２の第２回転軸ＲＡ２を中心に起伏動をする。図３に示すように、ステッピングモータ３２ａは第２関節部Ｊ２の近傍に設けられる。ステッピングモータ３２ａが回転するとき、ステッピングモータ３２ａのドライブシャフトの動力は、例えば、直接第２関節部Ｊ２に伝達される。それにより、アーム部２がハンド部３とともに第２関節部Ｊ２の第２回転軸ＲＡ２を中心に起伏動をする。基部１を成すアーム支持体（第１支持体）１１ａは、第１関節部Ｊ１の第１回転軸ＲＡ１を中心に形成される円筒形状の中空構造を有する。第１関節部Ｊ１は図示しない固定台に取り付けられる。第１関節部Ｊ１が回転するとき、第１支持体１１ａはアーム部２の旋回とともに軸回転する。なお、第１支持体１１ａが接地面に固定されていてもよい。その場合、第１支持体１１ａとは独立してアーム部２が旋回する構造に設けられる。第１支持体１１ａの上部には第２支持部１１ｂが接続される。

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

第１関節部Ｊ１は円環形状の固定部と回転部とからなり、固定部において台座に固定される。回転部には第１支持部１１ａと第２支持部１１ｂとが取り付けられる。第１関節部Ｊ１が回転するとき、第１、第２、第３支持体１１ａ、１１ｂ、１１ｃが第１回転軸ＲＡ１を中心としてアーム部２とハンド部３と共に旋回する。

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

上記の通り関節部としての第３関節部Ｊ３はアーム部２の主要構成物を構成する。アーム部２の先端に上述のハンド部３が設けられる。第１乃至第６関節部Ｊ１−Ｊ６の回転、曲げ、伸縮によりハンド部３の2指ハンド１６を任意の位置・姿勢に配置することが可能である。特に第３関節部Ｊ３の直動伸縮距離の長さは、基部１の近接位置から遠隔位置までの広範囲の対象にハンド部３で作用することを可能にする。

第３関節部Ｊ３はそれを構成する直動伸縮アーム機構により実現される直動伸縮距離の長さが特徴的である。直動伸縮距離の長さは、図２、図３に示す構造により達成される。直動伸縮アーム機構は第１連結コマ列２１と第２連結コマ列２０とを有する。アーム部２が水平に配置される基準姿勢では、第１連結コマ列２１は第２連結コマ列２０の下部に位置し、第２連結コマ列２０は第１連結コマ列２１の上部に位置する。

第１連結コマ列２１は、同一の断面コ字形状を有し、ピンにより背面箇所において列状に連結される複数の第１連結コマ２３からなる。第１連結コマ２３の断面形状及びピンによる連結位置により第１連結コマ列２１はその背面方向ＢＤに屈曲可能であるが逆に表面方向ＦＤには屈曲不可な性質を備える。第２連結コマ列２０は、第１連結コマ２３と略等価な幅を有する略平板形状を有し、背面方向と表面方向とともに屈曲可能な状態でピンにより列状に連結される複数の第２連結コマ２２からなる。第１連結コマ列２１は第２連結コマ列２０と先端部おいて結合コマ２６により結合される。結合コマ２６は、第１連結コマ２３と第２連結コマ２２とが一体的になった形状を有している。結合コマ２６が始端となって、第３支持部１１ｃから第２連結コマ列２０が第１連結コマ列２１とともに送り出されるときには、第１連結コマ列２１と第２連結コマ列２０とは互いに接合される。第１連結コマ列２１と第２連結コマ列２０とは先端部おいて結合コマ２６により結合され、それぞれ後部において第３支持体１１ｃの内部で堅持され引き抜き防止されることにより接合状態が保持される。第１連結コマ列２１と第２連結コマ列２０とが接合状態が保持されたとき、第１連結コマ列２１と第２連結コマ列２０の屈曲は制限され、それにより第１連結コマ列２１と第２連結コマ列２０とにより一定の剛性を備えた柱状体が構成される。第１連結コマ列２１と第２連結コマ列２０とが互いに離反するとき、屈曲制限が解除され、それぞれが屈曲可能状態に復帰する。第１連結コマ列２１と第２連結コマ列２０とは第３支持体１１ｃの開口付近で接合され、送り出される。第１連結コマ列２１と第２連結コマ列２０とは第３支持体１１ｃの内部で離反され、それぞれが屈曲可能状態となる。第１連結コマ列２１と第２連結コマ列２０とは個々に屈曲され、第１支持体１１ａの内部に別対として収容される。

図２に示すように第２連結コマ２２の内側には個々にリニアギア２２ａが形成されている。リニアギア２２ａは第２連結コマ２２が直線状になったときに連結され、連続的なリニアギアを構成する。図３に示すように第２連結コマ２２は第３支持体１１ｃ内でローラＲ１とドライブギア２４ａとの間に挟まれる。リニアギア２２ａはドライブギア２４ａに噛み合わされる。図３に示すように、ステッピングモータ３３ａは第２関節部Ｊ２の近傍に設けられる。ステッピングモータ３３ａが回転するとき、ステッピングモータ３３ａのドライブシャフトの動力は、ドライブギア２４ａに伝達される。ステッピングモータ３３ａによりドライブギア２４ａが順回転することにより第２連結コマ列２０は第１連結コマ列２１とともに第３支持体１１ｃから送り出される。その際、第１連結コマ列２１と第２連結コマ列２０とは第３支持体１１ｃの開口付近に設けられた一対の上下ローラＲ２，Ｒ４に挟まれ、相互に押圧され、接合された状態で第３移動軸ＲＡ３に沿って直線的に送り出される。ステッピングモータ３３ａによりドライブギア２４ａが逆回転することにより第２連結コマ列２０と第１連結コマ列２１とは第３支持体１１ｃの内部であって上下ローラＲ２，Ｒ４の後方において接合状態を解除され、互いに離反される。離反された第２連結コマ列２０と第１連結コマ列２１とはそれぞれ屈曲可能な状態になり、第２、第３支持体１１ｂ、１１ｃの内部に設けられたガイドレールにガイドされて第１回転軸ＲＡ１に沿う方向に屈曲され、第１支持体１１ａの内部に収容される。

ハンド部３は、図１に示すようにアーム部２の先に装備されている。ハンド部３は、第１、第２、第３関節部Ｊ１．Ｊ２．Ｊ３により任意位置に移動され、第４、第５、第６関節部Ｊ４、Ｊ５、Ｊ６により任意姿勢に配置される。ハンド部３は、開閉される２つの指部１６ａ、１６ｂを有している。第４関節部Ｊ４は、アーム部２の伸縮方向に沿ったアーム部２の中心軸、つまり第３関節部Ｊ３の移動軸ＲＡ３に典型的には一致する第４回転軸ＲＡ４を有するねじり関節である。第４関節部Ｊ４が回転すると、第４関節部Ｊ４から先端にかけてハンド部３が第４回転軸ＲＡ４を中心に回転する。図３に示すように、ステッピングモータ３４ａは第４関節部Ｊ４の近傍に設けられる。ステッピングモータ３４ａが回転するとき、ステッピングモータ３４ａのドライブシャフトの動力は、例えば、直接第４関節部Ｊ４に伝達される。それにより、第４関節部Ｊ４から先端にかけてハンド部３が第４関節部Ｊ４の第４回転軸ＲＡ４を中心に回転する。

第５関節部Ｊ５は、第４関節部Ｊ４の移動軸ＲＡ４に対して直交する第５回転軸ＲＡ５を有する曲げ関節部である。第５関節部が回転すると、第５関節部Ｊ５から先端にかけてハンド部１６とともに上下に回動する。図３に示すように、ステッピングモータ３５ａは第５関節部Ｊ５の近傍に設けられる。ステッピングモータ３５ａが回転するとき、ステッピングモータ３５ａのドライブシャフトの動力は、図示しないギア等を介して第５関節部Ｊ５に伝達される。それにより、第５関節部Ｊ５から先端にかけてハンド部１６が上下に回動する。

第６関節部Ｊ６は、第４関節部Ｊ４の第４回転軸ＲＡ４に直交し、第５関節部Ｊ５の第５回転軸ＲＡ５に垂直な第６回転軸ＲＡ６を有する曲げ関節である。第６関節部Ｊ６が回転するとハンド１６が左右に旋回する。図３に示すように、ステッピングモータ３６ａは第６関節部Ｊ６の近傍に設けられる。ステッピングモータ３６ａが回転するとき、ステッピングモータ３６ａのドライブシャフトの動力は、図示しないギア等を介して第６関節部Ｊ６に伝達される。それにより、ハンド部１６が左右に旋回する。

図４は、本実施形態に係るロボットアーム機構の構成の一例を示すブロック構成図である。なお、本実施形態に係るロボットアーム機構には、ステッピングモータ３１ａ、３２ａ、３３ａ、３４ａ、３５ａ、３６ａをそれぞれ有するモータ駆動部３１−３６がそれぞれ関節部Ｊ１−Ｊ６に設けられる。ここでは、モータ駆動部３１のみ説明する。他のモータ駆動部３２−３６の構成および動作は、モータ駆動部３１のそれらと同一であるので、それらの説明は、ここでは省略する。ロボットアーム機構は、システム制御部４１と、電源供給部４２と、モータ制御部４３と、入力部４４と、設定部４５と、モータ駆動部３１とを有する。モータ制御部４３とモータ駆動部３１とはステッピングモータ制御装置を構成する。ステッピングモータ制御装置は、独立した装置として、ステッピングモータを使用する他の装置、機構に対して適用することができる。

システム制御部４１は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）と半導体メモリ等を有する。システム制御部４１は、入力部４４を介して、ロボットアーム機構に入力された指示情報を、一時的に半導体メモリに記憶する。システム制御部４１は、入力された指示情報に基づいてロボットアーム機構の各部を統括して制御する。

電源供給部４２は、ロボットアーム機構に搭載されたバッテリ（図示せず）または外部の商用電源から、モータドライバ３１ｂに供給するための電力を発生する。

モータ制御部４３は、モータドライバ３１ｂを総括して制御する。具体的には、モータ制御部４３は、後述の設定部４５で設定された条件に応じた信号をモータドライバ３１ｂおよび脱調検出部３１ｅのうち少なくとも一方に対して出力する。具体的には、モータ制御部４３は、後述の駆動条件設定部４５２で設定されたパルス条件に関するパルス制御信号をモータドライバ３１ｂに対して出力する。モータ制御部４３は、後述の駆動条件設定部４５２で設定された最大駆動電流値に関する電流制御信号をモータドライバ３１ｂに対して出力する。また、モータ制御部４３は、後述の猶予時間設定部４５１で設定された猶予時間に関するデータをモータドライバ３１ｂに対して出力する。

入力部４４は、ロボットアーム機構に対して、ユーザが指示情報を入力するための入力インターフェースとして機能する。入力部４４は、図示しない操作装置を接続する。例えば、操作装置は、関節部を指定するためのスイッチ、関節部を回転または直動させる動作速度、動作方向および動作量とを指定するためのジョイスティック等を備える。また、操作装置は、後述のパルス条件、駆動電流値および猶予時間等の設定のための入力装置としても使用される。ユーザにより操作装置が操作されることで、各関節部の移動指示情報および各種条件の設定情報が入力される。ロボットアーム機構は、移動指示情報に従って動作する。操作装置を構成するこれらの入力デバイスは、他のデバイス、例えば、マウス、キーボード、トラックボールおよびタッチパネル等で代替が可能である。

設定部４５は、猶予時間設定部４５１と駆動条件設定部４５２とを有する。
駆動条件設定部４５２は、ステッピングモータ３１ａの駆動条件を設定する。なお、駆動条件設定部４５２は、複数のステッピングモータ３１ａ−３６ａ各々の駆動条件を個別に設定することができる。駆動条件には、パルス条件と最大許容電流値等とが含まれる。パルス条件は、モータドライバ３１ｂからステッピングモータ３１ａに入力されるパルスの条件を示し、パルス数とパルス周期等が含まれる。パルス数は、ステッピングモータ３１ａの回転角度に関連する。例えば、駆動条件設定部４５２は、入力部４４を介して入力された関節部Ｊ１の回転量に基づいて、対応するステッピングモータ３１ａの目標角度を設定する。そして、設定した目標角度とステッピングモータ３１ａの基本ステップ角度とに基づいてパルス数を決定する。パルス周期は、ステッピングモータ３１ａの回転速度に関連する。駆動条件設定部４５２は、入力部４４を介して入力された動作速度で関節部Ｊ１が回転するためのパルス周期を設定する。最大許容電流値は、モータドライバ３１ｂがステッピングモータ３１ａを駆動するための駆動電流値として許容する最大値である。駆動条件設定部４５２は、関節部Ｊ１に関するダイナミクスモデルデータを用い、ロボットアーム機構の関節部Ｊ１に連結されるアームの重心位置、質量、アーム長、慣性テンソル、関節部Ｊ１の位置（角度）、速度、加速度等から、関節部Ｊ１に発生するトルクを解析する。なお、ダイナミクスモデルデータは、モータの動特性に関するモデルのデータを指す。そして、解析したトルクに基づいて、最大許容電流値を決定する。このように、予め解析したトルクに基づいて、ステッピングモータ３１ａに供給する駆動電流値の最大値を決定することで、ステッピングモータ３１ａの消費電力および発熱量を抑えることができる。なお、駆動条件設定部４５２は、入力部４４を介して入力されたトルクを発生させるための駆動電流値を設定してもよい。

猶予時間設定部４５１は、入力部４４を介したユーザ指示に従って、猶予時間を設定する。猶予時間は、ステッピングモータ３１ａの脱調が検出されてから、モータドライバ３１ｂがステッピングモータ３１ａの駆動の停止を猶予する時間を指す。したがって、ユーザからステッピングモータ３１ａの停止指示がない限り、脱調が検出されてから猶予時間が経過するまでは、ステッピングモータ３１ａの停止は猶予される。

モータ駆動部３１は、ステッピングモータ３１ａと、モータドライバ３１ｂと、エンコーダ３１ｃと、エンコーダパルスカウンタ３１ｄと、脱調検出部３１ｅとを有する。モータ駆動部３１は、対応する関節部Ｊ１の近傍に設けられる。

モータドライバ３１ｂは、ステッピングモータ３１ａの駆動および停止を制御する。具体的には、モータドライバ３１ｂは、電源供給部４２から供給された電力を用いて、ステッピングモータ３１ａを駆動するための駆動電流を発生する。このとき、モータドライバ３１ａにより発生される駆動電流は、最大許容電流値以下の電流値を有する。モータドライバ３１ａは、関節部Ｊ１の回転速度と最適な駆動電流との関係を示すデータを保持し、関節部Ｊ１の回転速度に応じた駆動電流を発生する。なお、駆動電流を入力部４４を介してユーザに指示されたトルクに応じて設定されてもよい。

モータドライバ３１ｂは、モータ制御部４３からのパルス制御信号に応じたパルス（以下、ステッピングパルスと称する。）を発生する。ステッピングモータ３１ａは、モータドライバ３１ｂにより発生されたステッピングパルスに従って回転する。ステッピングモータ３１ａが回転されることにより、ステッピングモータ３１ａに直接またはベルトおよびギア等を介して間接的に接続された関節部Ｊ１が駆動される。

モータドライバ３１ｂは、脱調検出部３１ｅからの脱調判定信号に基づいて、脱調の発生および回復のタイミングを特定する。モータドライバ３１ｂは、脱調検出部３１ｅによりステッピングモータ３１ａの脱調が検出されてから猶予時間が経過するまでに、脱調が回復されなかったとき、猶予時間の経過の時点でステッピングパルスの発生と駆動電流との一方または両方を停止する。ここでは、ステッピングパルスの停止を例として説明する。これにより、ステッピングモータ３１ａの駆動が停止される。

モータドライバ３１ｂは、パルス制御信号に基づいて、単位時間あたりの目標角度を特定する。単位時間あたりの目標角度に関するデータは、脱調検出部３１ｅに対して出力される。ここでの単位時間とは、例えば、モータ駆動部３１の制御周期に相当する。なお、モータドライバ３１ｂの制御周期は、モータ制御部４３の制御周期よりも短い。このような場合、モータ駆動部３１の各構成要素は、モータドライバ３１ｂの制御下で動作することで、モータ制御部４３の制御下で動作する場合に比べて、より高速な動作が可能である。

エンコーダ３１ｃは、ステッピングモータ３１ａのドライブシャフトに連結される。エンコーダ３１ｃは、ステッピングモータ３１ａのドライブシャフトに接続されたディスクが所定の角度を回転する毎に、エンコーダパルスを発生する。例えば、１２０個のスリットがディスクの周回方向に等間隔に設けられている場合、エンコーダパルスはステッピングモータ３１ａが３度回転する毎に出力される。エンコーダパルスは、エンコーダパルスカウンタ３１ｄに対して出力される。

エンコーダパルスカウンタ３１ｄは、入力されたエンコーダパルスのパルス数をカウントする。エンコーダパルスのカウント情報は、モータドライバ３１ｂと脱調検出部３１ｅとに対して出力される。

脱調検出部３１ｅは、ステッピングモータ３１ａの脱調の発生を検出する。具体的には、脱調検出部３１ｅは、カウント情報からステッピングモータ３１ａの現在の回転角度（以下、現在角度と称する。）を特定する。また、脱調検出部３１ｅは、モータドライバ３１ｂからの出力に基づいて、ステッピングモータ３１ａの目標角度を特定する。脱調検出部３１ｅは、現在角度と目標角度とが一致していないとき、ステッピングモータ３１ａの脱調を検出する。脱調検出部３１ｅは、脱調の発生の有無を通知する脱調判定信号をモータドライバ３１ｂに対して出力する。

図５は、本実施形態に係るロボットアーム機構の駆動部３１の動作を説明するためのフローチャートである。モータ制御部４３からのパルス制御信号は、各関節部の移動指示が入力される毎にモータドライバ３１ｂに繰り返し入力される。図５に示すフローチャートでは、一パルス制御信号が入力されたときの駆動部３１の動作を説明する。

（ステップＳ１１）目標角度と最大許容電流値の入力
モータ制御部４３からパルス制御信号と電流制御信号とがモータドライバ３１ｂに入力される。

（ステップＳ１２）ステッピングモータ３１ａを駆動
モータドライバ３１ｂにより、パルス制御信号に応じたステッピングパルスと電流制御信号に応じた駆動電流とが発生される。発生された駆動電流によりステッピングモータ３１ａが駆動し、ステッピングパルスによりステッピングモータ３１ａが回転される。

（ステップＳ１３）ステッピングモータ３１ａの脱調の発生を検出
脱調検出部３１ｅにより、ステッピングモータ３１ａの脱調の発生が検出される。脱調の発生が検出されたとき、処理がステップＳ１４に移行される。一方、脱調の発生が検出されない場合は、処理ステップＳ１２に戻る。つまり、脱調が検出されない限り、モータドライバ３１ｂは、通常のステッピングモータ３１ａの制御を行い、ステッピングモータ３１ａの回転角度が目標角度に到達するまで、ステッピングモータ３１ａが回転される。

（ステップＳ１４）猶予時間内に脱調から回復したかの判定
モータドライバ３１ｂにより、最初に脱調が検出されてからの経過時間が計測される。脱調が検出されてからの経過時間が猶予時間Ｔ１未満のときは、処理がステップＳ１２に戻る。一方、脱調が検出されてから猶予時間Ｔ１が経過するまでに、脱調が回復しないとき、猶予時間Ｔ１になったのを契機に処理がステップＳ１５に移行される。

（ステップＳ１５）ステッピングモータ３１ａを停止
モータドライバ３１ｂにより、ステッピングパルスの発生が停止される。これにより、ステッピングモータ３１ａの駆動が停止される。

本実施形態に係るロボットアーム機構によれば、以下のような効果を得ることができる。本実施形態に係るモータドライバ３１ｂは、ステッピングモータ３１ａの脱調が検出された後に、すぐにステッピングモータ３１ａの駆動を停止させずに、猶予時間Ｔ１が経過するまでは、ステッピングモータ３１ａの駆動の停止を猶予する。そして、脱調が検出されてから猶予時間Ｔ１が経過するまでに脱調が回復した場合は、ステッピングモータ３１ａの駆動の停止は回避される。したがって、すぐに脱調を回復できるような場合において、本実施形態に係るロボットアーム機構は、その脱調が回復されるまでを、設定した猶予時間待つことができる。したがって、本実施形態に係るロボットアーム機構は、脱調検出後であっても、不必要なステッピングモータ３１ａの駆動の停止を少なくすることができる。言い換えると、本実施形態に係るロボットアーム機構は、高い操作性をユーザに提供することができ、これにより、ユーザは、時間効率良くロボットアーム機構を操作することができる。

（変形例１）
変形例１に係るロボットアーム機構と本実施形態に係るロボットアーム機構との間の差異は、変形例１に係るロボットアーム機構の入力部４４に接続されている操作装置が再開スイッチを装備している点にある。再開スイッチは、一般的なメイン電源のＯＮ／ＯＦＦを伴うリセットボタンや再起動ボタンと異なる。一方、再開スイッチは、メイン電源のＯＮ／ＯＦＦを伴うことなく、一旦停止したステッピングパルスの発生および駆動電流の供給を強制的に再開させるためのスイッチである。

図６は、本実施形態の変形例１に係るロボットアーム機構の駆動部３１の動作を説明するためのフローチャートである。図６は、図５の説明と同様に、一パルス制御信号が入力されたときの駆動部３１の動作を説明する。

（ステップＳ２１）目標角度と最大許容電流値の入力
モータ制御部４３からパルス制御信号と電流制御信号とがモータドライバ３１ｂに入力される。

（ステップＳ２２）ステッピングモータ３１ａを駆動
モータドライバ３１ｂにより、パルス制御信号に応じたステッピングパルスと電流制御信号に応じた駆動電流とが発生される。発生された駆動電流によりステッピングモータ３１ａが駆動し、ステッピングパルスによりステッピングモータ３１ａが回転される。

（ステップＳ２３）ステッピングモータ３１ａの脱調の発生を検出
脱調検出部３１ｅにより、ステッピングモータ３１ａの脱調の発生が検出される。脱調の発生が検出されたとき、処理がステップＳ１４に移行される。一方、脱調の発生が検出されない場合は、処理ステップＳ２２に戻る。つまり、脱調が検出されない限り、モータドライバ３１ｂは、通常のステッピングモータ３１ａの制御を行い、ステッピングモータ３１ａの回転角度が目標角度に到達するまで、ステッピングモータ３１ａが回転される。

（ステップＳ２４）猶予時間内に脱調から回復したかの判定
モータドライバ３１ｂにより、最初に脱調が検出されてからの経過時間が計測される。脱調が検出されてからの経過時間が猶予時間Ｔ１未満のときは、処理がステップＳ２２に戻る。一方、脱調が検出されてから猶予時間Ｔ１が経過するまでに、脱調が回復しないとき、猶予時間Ｔ１になったのを契機に処理がステップＳ２５に移行される。

（ステップＳ２５）ステッピングモータ３１ａを停止
モータドライバ３１ｂにより、ステッピングパルスの発生が停止される。これにより、ステッピングモータ３１ａの駆動が停止される。

（ステップＳ２６）再開スイッチの操作の有無の判定
ステッピングモータ３１ａの駆動が停止後、再開スイッチが押された場合、ステップＳ２１から処理が再開される。なお、ステッピングモータ３１ａの駆動停止後の経過時間によらず、メイン電源がＯＦＦにされない限り再開スイッチは常に有効である。ただし、再開スイッチを一時的に無効にするために、再開スイッチの有効／無効とを切り替えるスイッチ等が設けられてもよい。これにより、脱調要因を除去するために、操作者がロボットアーム機構に近づいた際に、第３者が誤って再開スイッチを押し、ロボットアーム機構の動作に操作者が巻き込まれるのを防ぐことができる。

本実施形態の変形例１に係るロボットアーム機構によれば、以下のような効果を得ることができる。変形例１に係るロボットアーム機構は、再開スイッチを有する。モータドライバ３１ｂは、再開スイッチが押されたのを契機に、動作途中で停止したステッピングモータ３１ａの駆動を停止直前の状態から強制的に再開させることができる。ユーザは、ステッピングモータ３１ａが停止してから、脱調要因を除去するための作業を行い、再開スイッチを押すだけで、停止した状態からロボットアーム機構の操作を継続することができる。したがって、本実施形態の変形例１に係るロボットアーム機構は、高いロボット操作性をユーザに提供することができる。

（変形例２）
変形例２に係るロボットアーム機構と本実施形態に係るロボットアーム機構との間の差異は、変形例２に係るロボットアーム機構は、脱調が検出されたときにその要因を特定し、脱調要因に応じてステッピングモータ３１ａの駆動条件を変更することができる点にある。

図７は、本実施形態の変形例２に係るロボットアーム機構の駆動部３１の動作を説明するためのフローチャートである。図７は、図５の説明と同様に、一パルス制御信号が入力されたときの駆動部３１の動作を説明する。

（ステップＳ３１）目標角度と最大許容電流値の入力
モータ制御部４３からパルス制御信号と電流制御信号とがモータドライバ３１ｂに入力される。
（ステップＳ３２）ステッピングモータ３１ａを駆動
モータドライバ３１ｂにより、パルス制御信号に応じたステッピングパルスと電流制御信号に応じた駆動電流とが発生される。発生された駆動電流によりステッピングモータ３１ａが駆動し、ステッピングパルスによりステッピングモータ３１ａが回転される。

（ステップＳ３３）ステッピングモータ３１ａの脱調の発生を検出
脱調検出部３１ｅにより、ステッピングモータ３１ａの脱調の発生が検出される。脱調の発生が検出されたとき、処理がステップＳ３４に移行される。一方、脱調の発生が検出されない場合は、処理ステップＳ３２に戻る。つまり、脱調が検出されない限り、モータドライバ３１ｂは、通常のステッピングモータ３１ａの制御を行い、ステッピングモータ３１ａの回転角度が目標角度に到達するまで、ステッピングモータ３１ａが回転される。

（ステップＳ３４）脱調要因を特定
モータドライバ３１ｂにより、脱調要因が特定される。脱調要因には、内的要因と外的要因とがある。内的要因には、ステッピングモータ３１ａのトルクが不足しているために、モータの回転がステッピングパルスに追従できない現象が含まれる。外的要因には、人または物にアーム部が接触し、予定した動作ができていない現象が含まれる。モータドライバ３１ｂは、図示しない接触センサーからの出力に基づいて脱調要因を特定する。具体的には、モータドライバ３１ｂは、脱調検出部３１ｅにより脱調が検出されたタイミングの接触センサーの出力の変化値が閾値を超えるたとき、当該脱調の要因を外的要因と特定する。また、モータドライバ３１ｂは、脱調検出部３１ｅにより脱調が検出されたタイミングの接触センサーの出力の変化値が閾値未満のとき、当該脱調の要因を内的要因と特定する。

（ステップＳ３５）脱調要因に応じて駆動条件を変更
モータドライバ３１ｂにより、脱調要因に応じて、ステッピングモータ３１ａの駆動電流値が変更される。例えば、脱調要因が外的要因である場合、モータドライバ３１ｂは駆動電流値を下げる。一方、脱調要因が内的要因である場合、モータドライバ３１ｂは駆動電流値を上げる。なお、脱調要因が外的要因である場合、モータドライバ３１ｂは駆動電流値を維持してもよい。また、モータドライバ３１ｂは、駆動電流の変更とともに、猶予時間Ｔ１も変更してもよい。

（ステップＳ３６）猶予時間内に脱調から回復したかの判定
モータドライバ３１ｂにより、最初に脱調が検出されてからの経過時間が計測される。脱調が検出されてからの経過時間が猶予時間Ｔ１未満のときは、処理がステップＳ３２に戻る。一方、脱調が検出されてから猶予時間Ｔ１が経過するまでに、脱調が回復しないとき、猶予時間Ｔ１になったのを契機に処理がステップＳ３７に移行される。なお、猶予時間Ｔ１は、モータドライバ３１ｂにより変更されている場合もある。

（ステップＳ３７）ステッピングモータ３１ａを停止
モータドライバ３１ｂにより、ステッピングパルスの発生が停止される。これにより、ステッピングモータ３１ａの駆動が停止される。

本実施形態の変形例２に係るロボットアーム機構によれば、以下のような効果を得ることができる。変形例２に係るロボットアーム機構のモータドライバ３１ｂは、ステッピングモータ３１ａの脱調が検出された後、脱調要因に応じてステッピングモータ３１ａの駆動条件を変更することができる。例えば、内的要因による脱調は、ステッピングモータ３１ａのトルクが不足することにより発生される。このような場合、モータドライバ３１ｂは、ステッピングモータ３１ａの駆動電流値を上げる制御を行う。これにより、ステッピングモータ３１ａのトルクが大きくなり、内的要因による脱調を回復できる可能性がある。また、脱調要因が外的要因（人や他の干渉物との衝突等）の場合、モータドライバ３１ｂは、駆動電流値を下げる制御を行う。これにより、ステッピングモータ３１ａのトルクが小さくなり、ロボットアーム機構の故障、人の怪我、物の破損等を防ぎ、安全を確保することができる。

また、モータドライバ３１ｂは、駆動電流値の変更と共に、猶予時間を変更する制御を行ってもよい。例えば、脱調要因が外的要因の場合、モータドライバ３１ｂは、猶予時間を短くする制御をすることで、ステッピングモータ３１ａの停止までの猶予時間を短くでき、ロボットアーム機構の故障、人の怪我、物の破損等を防ぐことができる。一方で、猶予時間を長くする制御を行うことで、ステッピングモータ３１ａの脱調からの回復するための時間を長くでき、ステッピングモータ３１ａの不必要に停止を少なくすることができる。つまり、脱調の検出後、モータドライバ３１ａが猶予時間を短くするか、または長くするかは、ロボットアーム機構の使用場面、用途等に応じて異なる。脱調後に変更する駆動条件及びそのパラメータは、ユーザ指示に従って適宜変更が可能である。例えば、ユーザは、ロボットアーム機構の使用用途および場面を選択することで、択一的に変更する駆動条件等が決定されてもよい。

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

１…基部、２…アーム部、３…手先効果器、Ｊ１，Ｊ２，Ｊ４，Ｊ５，Ｊ６…回転関節部、Ｊ３…直動関節部、１１ａ…第１支持体、１１ｂ…第２支持体、１１ｃ…第３支持体、２０…第２連結コマ列、２１…第１連結コマ列、２２…第２連結コマ、２３…第１連結コマ、２６…結合コマ、３１…モータ駆動部、３１ａ…ステッピングモータ、３１ｂ…モータドライバ、３１ｃ…エンコーダ、３１ｄ…エンコーダパルスカウンタ、３１ｅ…脱調検出部、４１…システム制御部、４２…電源供給部、４３…モータ制御部、４４…入力部、４５…設定部、４５１…猶予時間設定部、４５２…駆動条件設定部

Claims (10)

1. 関節部を有するロボットアームと、
   前記関節部を作動させる動力を発生するステッピングモータと、
   前記ステッピングモータを、目標角度に従って駆動するモータドライバと、
   前記ステッピングモータのドライブシャフトが所定の角度を回転する毎にエンコーダパルスを出力するエンコーダと、
   前記エンコーダパルスに基づいて特定された前記ステッピングモータの現在角度と前記目標角度とに基づいて、前記ステッピングモータの脱調を検出する脱調検出部と、を具備し、
   前記モータドライバは、前記脱調が検出されてから所定の猶予時間が経過するまでに、前記脱調が回復されなかったとき、前記猶予時間の経過の時点で前記ステッピングモータの駆動を停止する、ロボットアーム機構。
2. 前記脱調が検出されてから前記所定の猶予時間が経過するまで前記ステッピングモータの停止は猶予される請求項１記載のロボットアーム機構。
3. 前記脱調が検出されてから前記所定の猶予時間が経過するまでに前記脱調が回復されたときには前記ステッピングモータの停止は回避される請求項２記載のロボットアーム機構。
4. 再開操作部と、
   前記ステッピングモータの駆動の停止後に前記再開操作部が操作されたとき、前記ステッピングモータの駆動を再開させるために前記モータドライバを制御する制御部をさらに具備する請求項１記載のロボットアーム機構。
5. 前記モータドライバは、前記脱調が検出されたとき、前記ステッピングモータの駆動電流を変更する請求項１記載のロボットアーム機構。
6. 前記モータドライバは、前記脱調が検出されたとき、前記駆動電流と共に、前記猶予時間を変更する請求項５記載のロボットアーム機構。
7. ステッピングモータと、
   前記ステッピングモータを目標角度に従って駆動するモータドライバと、
   前記ステッピングモータのドライブシャフトが所定の角度を回転する毎にエンコーダパルスを出力するエンコーダと、
   前記エンコーダパルスに基づいて特定された前記ステッピングモータの現在角度と前記目標角度とに基づいて、前記ステッピングモータの脱調を検出する脱調検出部と、を具備し、
   前記モータドライバは、前記脱調が検出されてから所定の猶予時間が経過するまでに、前記脱調が回復されなかったとき、前記所定の猶予時間の経過の時点で前記ステッピングモータの駆動を停止する、ステッピングモータ制御装置。
8. 再開操作部と、
   前記ステッピングモータの駆動の停止後に前記再開操作部が操作されたとき前記ステッピングモータの駆動を再開させるために前記モータドライバを制御する制御部と、をさらに具備する請求項７記載のステッピングモータ制御装置。
9. 前記モータドライバは、前記脱調が検出されたとき、前記ステッピングモータの駆動電流を変更する請求項７記載のステッピングモータ制御装置。
10. 前記モータドライバは、前記脱調が検出されたとき、前記駆動電流値と共に、前記猶予時間を変更する請求項９記載のステッピングモータ制御装置。