(57)【特許請求の範囲】
【請求項１】
関節部を有するロボットアーム機構と、
前記関節部を作動させる動力を発生するステッピングモータと、
前記ステッピングモータを駆動するモータドライバと、
前記ロボットアーム機構の着目点が現在位置である移動開始位置から最終目標位置まで移動する軌道を計算する軌道計算部と、
前記軌道計算部で計算された軌道に応じた指令値を前記モータドライバに出力する指令値出力部と、
前記ステッピングモータの脱調を検知する脱調検知部と、
前記脱調が検知されたとき、前記脱調に起因してシフトした前記着目点の位置から前記最終目標位置までの軌道を再度計算させ、前記再度計算された軌道に従って前記着目点を移動させるために前記軌道計算部と前記指令値出力部とを制御するシステム制御部とを具備することを特徴とするロボット装置。
【請求項２】
前記軌道計算部により、前記脱調に起因してシフトした前記着目点の位置から、前記移動開始位置と前記最終目標位置までの当初の軌道に戻り前記最終目標位置に至る第１軌道と、前記脱調に起因してシフトした前記着目点の位置から前記最終目標位置までの直線的な第２軌道とが再度計算されることを特徴とする請求項１記載のロボット装置。
【請求項３】
前記システム制御部は、前記当初の軌道により予定されているタスクタイムを維持するために必要とされる前記着目点の移動速度に基づいて前記第１、第２軌道の一方を選択することを特徴とする請求項２記載のロボット装置。
【請求項４】
前記システム制御部は、前記第１軌道で予定される前記着目点の移動速度が所定の上限値未満であるとき前記第１軌道を優先的に選択し、前記第１軌道で予定される前記着目点の移動速度が所定の上限値以上であるとき前記第２軌道を選択することを特徴とする請求項３記載のロボット装置。
【請求項５】
前記システム制御部は、前記脱調が検知された時点から所定の待ち時間の経過後に前記第１軌道又は前記第２軌道に従って前記着目点の移動を再開させるために前記指令値出力部を制御することを特徴とする請求項４記載のロボット装置。
【請求項６】
前記システム制御部は、前記脱調が検知された後であってユーザによる移動再開指示を待って前記第１軌道又は前記第２軌道に従って前記着目点の移動を再開させるために前記指令値出力部を制御することを特徴とする請求項４記載のロボット装置。
【請求項７】
前記軌道計算部により、前記脱調に起因してシフトした前記着目点の位置から、前記移動開始位置から前記最終目標位置までの当初の軌道から前記脱調により外れた経路を辿って前記当初の軌道に戻り前記最終目標位置に至る第１軌道と、前記脱調に起因してシフトした前記着目点の位置から前記当初の軌道から前記脱調により外れた経路と異なる経路を辿って前記当初の軌道に戻り前記最終目標位置に至る第２軌道と、前記脱調に起因してシフトした前記着目点の位置から前記最終目標位置までの直線的な第３軌道とが再度計算されることを特徴とする請求項１記載のロボット装置。
【請求項８】
関節部を有するロボットアーム機構と、
前記関節部を作動させる動力を発生するステッピングモータと、
前記ステッピングモータを駆動するモータドライバと、
前記ロボットアーム機構の着目点が現在位置である移動開始位置から最終目標位置まで移動する軌道を計算する軌道計算部と、
前記軌道計算部で計算された軌道に応じた指令値を前記モータドライバに出力する指令値出力部と、
前記ステッピングモータの脱調を検知する脱調検知部と、
前記脱調が検知されたとき、前記脱調に起因してシフトした前記着目点の位置から前記移動開始位置に戻る軌道を計算させ、前記計算された軌道に従って前記着目点を前記移動開始位置に戻すために前記軌道計算部と前記指令値出力部とを制御するシステム制御部とを具備することを特徴とするロボット装置。
【請求項９】
関節部を有するロボットアーム機構と、
前記関節部を作動させる動力を発生するステッピングモータと、
前記ステッピングモータを駆動するモータドライバと、
前記ロボットアーム機構の着目点が現在位置である移動開始位置から最終目標位置まで移動する軌道を計算する軌道計算部と、
前記軌道計算部で計算された軌道に応じた指令値を前記モータドライバに出力する指令値出力部と、
前記ステッピングモータの脱調を検知する脱調検知部と、
前記脱調が検知されたとき、前記軌道から前記脱調に起因してシフトした前記着目点のシフト距離が所定の距離以下であって、且つ前記脱調が検知された時点から前記脱調が回復する時点までの遅延時間が所定の時間以下であるとき、前記脱調に起因してシフトした前記着目点の位置から前記最終目標位置までの軌道に沿って前記着目点を移動させ、一方、前記シフト距離が前記所定の距離以下でなく、又は前記遅延時間が前記所定の時間以下でないとき、前記脱調に起因してシフトした前記着目点の位置から前記移動開始位置に前記着目点を戻すために前記軌道計算部と前記指令値出力部とを制御するシステム制御部とを具備することを特徴とするロボット装置。
【請求項１０】
ロボットアーム機構に装備された関節部を作動させる動力を発生するステッピングモータを駆動するモータドライバを制御するステッピングモータ制御装置において、
前記ロボットアーム機構の着目点が現在位置から最終目標位置まで移動する軌道を計算する軌道計算部と、
前記軌道計算部で計算された軌道に応じた指令値を前記モータドライバに出力する指令値出力部と、
前記ステッピングモータの脱調を検知する脱調検知部において、前記脱調が検知されたとき、前記脱調に起因してシフトした前記着目点の位置から前記最終目標位置までの軌道を再度計算させ、前記再度計算された軌道に従って前記着目点を移動させるために前記軌道計算部と前記指令値出力部とを制御するシステム制御部とを具備することを特徴とするステッピングモータ制御装置。
【請求項１１】
ロボットアーム機構に装備された関節部を作動させる動力を発生するステッピングモータを駆動するモータドライバを制御するステッピングモータ制御装置において、
前記ロボットアーム機構の着目点が現在位置である移動開始位置から最終目標位置まで移動する軌道を計算する軌道計算部と、
前記軌道計算部で計算された軌道に応じた指令値を前記モータドライバに出力する指令値出力部と、
前記ステッピングモータの脱調を検知する脱調検知部において、前記脱調が検知されたとき、前記脱調に起因してシフトした前記着目点の位置から前記移動開始位置に戻る軌道を計算させ、前記計算された軌道に従って前記着目点を前記移動開始位置に戻すために前記軌道計算部と前記指令値出力部とを制御するシステム制御部とを具備することを特徴とするステッピングモータ制御装置。
【請求項１２】
ロボットアーム機構に装備された関節部を作動させる動力を発生するステッピングモータを駆動するモータドライバを制御するステッピングモータ制御装置において、
前記ロボットアーム機構の着目点が現在位置である移動開始位置から最終目標位置まで移動する軌道を計算する軌道計算部と、
前記軌道計算部で計算された軌道に応じた指令値を前記モータドライバに出力する指令値出力部と、
前記ステッピングモータの脱調を検知する脱調検知部において、前記脱調が検知されたとき、前記軌道から前記脱調に起因してシフトした前記着目点のシフト距離が所定の距離以下であって、且つ前記脱調が検知された時点から前記脱調が回復する時点までの遅延時間が所定の時間以下であるとき、前記脱調に起因してシフトした前記着目点の位置から前記最終目標位置までの軌道に沿って前記着目点を移動させ、一方、前記シフト距離が前記所定の距離以下でなく、又は前記遅延時間が前記所定の時間以下でないとき、前記脱調に起因してシフトした前記着目点の位置から前記移動開始位置に前記着目点を戻すために前記軌道計算部と前記指令値出力部とを制御するシステム制御部とを具備することを特徴とするステッピングモータ制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態はロボット装置及びステッピングモータ制御装置に関する。
【背景技術】
【０００２】
ステッピングモータは、回転角がパルス信号の数に比例するので基本的にフィードバック回路の必要性がなく、オープンループ制御が可能であるで、ＡＣモータ、ＤＣモータよりも有利である反面、過負荷がかかったり、パルス周波数が高すぎると同期外れで制御が乱れるいわゆる「脱調」が生じる。脱調が発生すると、ステッピングモータが停止する。また再開しても目標位置には到達させるには基準位置に戻すことが必要とされ、それによるダウンタイムは避けられない。脱調現象はステッピングモータに不可避であり、従ってステッピングモータが脱調を起こさないよう様々な工夫が従来からなされてきたが、その多くが対症的であった。
【発明の概要】
【発明が解決しようとする課題】
【０００３】
目的は、ステッピングモータに脱調が生じることを前提として脱調が生じたときの好適な対処を提供することにある。
【課題を解決するための手段】
【０００４】
本実施形態に係るロボット装置は、関節部を有するロボットアーム機構と、関節部を作動させる動力を発生するステッピングモータと、ステッピングモータを駆動するモータドライバとを、ロボットアーム機構の着目点が現在位置から最終目標位置まで移動する軌道を計算する軌道計算部と、軌道計算部で計算された軌道に応じた指令値をモータドライバに出力する指令値出力部と、ステッピングモータの脱調を検知する脱調検知部とを備える。さらにロボット装置はシステム制御部を備える。システム制御部は、脱調が検知されたとき、脱調に起因してシフトした着目点の位置から最終目標位置までの軌道を再度計算させ、再度計算された軌道に従って着目点を移動させるために軌道計算部と指令値出力部とを制御する。
【図面の簡単な説明】
【０００５】
【図１】図１は、本実施形態に係るロボット装置のロボットアーム機構の外観斜視図である。
【図２】図２は、図１のロボットアーム機構の内部構造を示す側面図である。
【図３】図３は、図１のロボットアーム機構の構成を図記号表現により示す図である。
【図４】図４は、本実施形態に係るロボット装置の構成を示すブロック図である。
【図５】図５は、図４の動作制御装置による脱調時の処理手順を示すフローチャートである。
【図６】図６は、図５の脱調後の複数の軌道を示す図である。
【図７】図７は、図５の工程Ｓ１３の手順を示すフローチャートである。
【発明を実施するための形態】
【０００６】
以下、図面を参照しながら本実施形態に係るロボット装置を説明する。本実施形態に係るロボット装置は動作制御装置を備える。動作制御装置は、ロボット装置に装備されている多関節アーム機構の各関節のモータドライバを制御する単独の装置として機能する。また動作制御装置は、多関節アーム機構を備えるロボット装置に組み込むこともできる。本実施形態に係るロボット装置の動作制御装置は様々な形態の多関節アーム機構に適用可能である。ここでは複数の関節部のうち一が直動伸縮関節の多関節アーム機構を一例として説明する。以下の説明において、略同一の機能及び構成を有する構成要素については、同一符号を付し、重複説明は必要な場合にのみ行う。
【０００７】
図１は、本実施形態に係るボットアーム機構の外観斜視図である。ロボットアーム機構は、略円筒形状の基部１０と基部１０に接続されるアーム部２とアーム部２の先端に取り付けられる手首部４とを有する。手首部４には図示しないアダプタが設けられている。例えば、アダプタは後述の第６回転軸ＲＡ６の回転部に設けられる。手首部４に設けられたアダプタには、用途に応じたロボットハンドが取り付けられる。
【０００８】
ロボットアーム機構は、複数、ここでは６つの関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６を有する。複数の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５，Ｊ６は基部１０から順番に配設される。一般的に、第１、第２、第３関節部Ｊ１，Ｊ２，Ｊ３は根元３軸と呼ばれ、第４、第５、第６関節部Ｊ４，Ｊ５，Ｊ６はロボットハンドの姿勢を変化させる手首３軸と呼ばれる。手首部４は第４、第５、第６関節部Ｊ４，Ｊ５，Ｊ６を有する。根元３軸を構成する関節部Ｊ１，Ｊ２，Ｊ３の少なくとも一つは直動伸縮関節である。ここでは第３関節部Ｊ３が直動伸縮関節部、特に伸縮距離の比較的長い関節部として構成される。アーム部２は直動伸縮関節部Ｊ３（第３関節部Ｊ３）の伸縮部分を表している。
【０００９】
第１関節部Ｊ１は基台面に対して例えば垂直に支持される第１回転軸ＲＡ１を中心としたねじり関節である。第２関節部Ｊ２は第１回転軸ＲＡ１に対して垂直に配置される第２回転軸ＲＡ２を中心とした曲げ関節である。第３関節部Ｊ３は、第２回転軸ＲＡ２に対して垂直に配置される第３軸（移動軸）ＲＡ３を中心として直線的にアーム部２が伸縮する関節である。
【００１０】
第４関節部Ｊ４は、第４回転軸ＲＡ４を中心としたねじり関節である。第４回転軸ＲＡ４は、後述の第７関節部Ｊ７が回転していないとき、つまりアーム部２の全体が直線形状にあるとき、第３移動軸ＲＡ３と略一致する。第５関節部Ｊ５は第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を中心とした曲げ関節である。第６関節部Ｊ６は第４回転軸ＲＡ４に対して直交し、第５回転軸ＲＡ５に対して垂直に配置される第６回転軸ＲＡ６を中心とした曲げ関節である。
【００１１】
基部１０を成すアーム支持体（第１支持体）１１ａは、第１関節部Ｊ１の第１回転軸ＲＡ１を中心に形成される円筒形状の中空構造を有する。第１関節部Ｊ１は図示しない固定台に取り付けられる。第１関節部Ｊ１が回転するとき、アーム部２は第１支持体１１ａの軸回転とともに左右に旋回する。なお、第１支持体１１ａが接地面に固定されていてもよい。その場合、第１支持体１１ａとは独立してアーム部２が旋回する構造に設けられる。第１支持体１１ａの上部には第２支持部１１ｂが接続される。
【００１２】
第２支持部１１ｂは第１支持部１１ａに連続する中空構造を有する。第２支持部１１ｂの一端は第１関節部Ｊ１の回転部に取り付けられる。第２支持部１１ｂの他端は開放され、第３支持部１１ｃが第２関節部Ｊ２の第２回転軸ＲＡ２において回動自在に嵌め込まれる。第３支持部１１ｃは第１支持部１１ａ及び第２支持部に連通する鱗状の外装からなる中空構造を有する。第３支持部１１ｃは、第２関節部Ｊ２の曲げ回転に伴ってその後部が第２支持部１１ｂに収容され、また送出される。ロボットアーム機構の直動伸縮関節部Ｊ３（第３関節部Ｊ３）を構成するアーム部２の後部はその収縮により第１支持部１１ａと第２支持部１１ｂの連続する中空構造の内部に収納される。
【００１３】
第３支持部１１ｃはその後端下部において第２支持部１１ｂの開放端下部に対して第２回転軸ＲＡ２を中心として回動自在に嵌め込まれる。それにより第２回転軸ＲＡ２を中心とした曲げ関節部としての第２関節部Ｊ２が構成される。第２関節部Ｊ２が回動するとき、アーム部２は第２回転軸ＲＡ２を中心に垂直方向に回動、つまり起伏動作をする。
【００１４】
第４関節部Ｊ４は、アーム部２の伸縮方向に沿ったアーム中心軸、つまり第３関節部Ｊ３の第３移動軸ＲＡ３に典型的には接する第４回転軸ＲＡ４を有するねじり関節である。第４関節部Ｊ４が回転すると、手首部４及び手首部４に取り付けられたロボットハンドは第４回転軸ＲＡ４を中心に回転する。第５関節部Ｊ５は、第４関節部Ｊ４の第４回転軸ＲＡ４に対して直交する第５回転軸ＲＡ５を有する曲げ関節部である。第５関節部Ｊ５が回転すると、第５関節部Ｊ５から先端にかけてロボットハンドとともに上下（第５回転軸ＲＡ５を中心に垂直方向）に回動する。第６関節部Ｊ６は、第４関節部Ｊ４の第４回転軸ＲＡ４に直交し、第５関節部Ｊ５の第５回転軸ＲＡ５に垂直な第６回転軸ＲＡ６を有する曲げ関節である。第６関節部Ｊ６が回転すると、ロボットハンドは左右に旋回する。
【００１５】
上記の通り手首部４のアダプタに取り付けられたロボットハンドは、第１、第２、第３関節部Ｊ１．Ｊ２．Ｊ３により任意位置に移動され、第４、第５、第６関節部Ｊ４、Ｊ５、Ｊ６により任意姿勢に配置される。特に第３関節部Ｊ３のアーム部２の伸縮距離の長さは、基部１０の近接位置から遠隔位置までの広範囲の対象にロボットハンドを到達させることを可能にする。第３関節部Ｊ３はそれを構成する直動伸縮機構により実現される直線的な伸縮動作とその伸縮距離の長さとが特徴的である。
【００１６】
（内部構造の説明）  図２  
図２は、図１のロボットアーム機構の内部構造を示す斜視図である。直動伸縮機構はアーム部２と射出部３０とを有する。アーム部２は第１連結コマ列２１と第２連結コマ列２２とを有する。第１連結コマ列２１は複数の第１連結コマ２３からなる。第１連結コマ２３は略平板形に構成される。前後の第１連結コマ２３は、互いの端部箇所においてピンにより屈曲自在に列状に連結される。第１連結コマ列２１は内側や外側に自在に屈曲できる。
【００１７】
第２連結コマ列２２は複数の第２連結コマ２４からなる。第２連結コマ２４は横断面コ字形状の短溝状体に構成される。前後の第２連結コマ２４は、互いの底面端部箇所においてピンにより屈曲自在に列状に連結される。第２連結コマ列２２は内側に屈曲できる。第２連結コマ２４の断面はコ字形状であるので、第２連結コマ列２２は、隣り合う第２連結コマ２４の側板同士が衝突して、外側には屈曲しない。なお、第１、第２連結コマ２３、２４の第２回転軸ＲＡ２に向いた面を内面、その反対側の面を外面というものとする。第１連結コマ列２１のうち先頭の第１連結コマ２３と、第２連結コマ列２２のうち先頭の第２連結コマ２４とは結合コマ２７により接続される。例えば、結合コマ２７は第２連結コマ２４と第１連結コマ２３とを合成した形状を有している。
【００１８】
射出部３０は、複数の上部ローラ３１と複数の下部ローラ３２とが角筒形状のフレーム３５に支持されてなる。例えば、複数の上部ローラ３１は第１連結コマ２３の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。同様に、複数の下部ローラ３２は第２連結コマ２４の長さと略等価な間隔を隔ててアーム中心軸に沿って配列される。射出部３０の後方には、ガイドローラ４０とドライブギア５０とが第１連結コマ列２１を挟んで対向するように設けられる。ドライブギア５０は図示しない減速器を介してステッピングモータ３３０に接続される。第１連結コマ２３の内面には連結方向に沿ってリニアギアが形成されている。複数の第１連結コマ２３が直線状に整列されたときに互いのリニアギアは直線状につながって、長いリニアギアを構成する。ドライブギア５０は、直線状のリニアギアにかみ合わされる。直線状につながったリニアギアはドライブギア５０とともにラックアンドピニオン機構を構成する。
【００１９】
（接合構造の説明）  
アーム伸長時、モータ５５が駆動し、ドライブギア５０が順回転すると、第１連結コマ列２１はガイドローラ４０により、アーム中心軸と平行な姿勢となって、上部ローラ３１と下部ローラ３２との間に誘導される。第１連結コマ列２１の移動に伴い、第２連結コマ列２２は射出部３０の後方に配置された図示しないガイドレールにより射出部３０の上部ローラ３１と下部ローラ３２との間に誘導される。上部ローラ３１と下部ローラ３２との間に誘導された第１、第２連結コマ列２１，２２は互いに押圧される。これにより、第１、第２連結コマ列２１，２２による柱状体が構成される。射出部３０は、第１、第２連結コマ列２１，２２を接合して柱状体を構成するとともに、その柱状体を上下左右に支持する。第１、第２連結コマ列２１、２２の接合による柱状体が射出部３０により堅持されることで、第１、第２連結コマ列２１，２２の接合状態が保持される。第１、第２連結コマ列２１、２２の接合状態が維持されているとき、第１、第２連結コマ列２１，２２の屈曲は互いに拘束される。それにより第１、第２連結コマ列２１、２２は、一定の剛性を備えた柱状体を構成する。柱状体とは、第２連結コマ列２２に第１連結コマ列２１が接合されてなる柱状の棒体を言う。この柱状体は第２連結コマ２４が第１連結コマ２３とともに全体として様々な断面形状の筒状体に構成される。筒状体とは上下左右が天板、底板及び両側板で囲まれ、前端部と後端部とが開放された形状として定義される。第１、第２連結コマ列２１、２２の接合による柱状体は、結合コマ２７が始端となって、第３移動軸ＲＡ３に沿って直線的に第３支持部１１ｃの開口から外に向かって送り出される。
【００２０】
アーム収縮時、モータ５５が駆動し、ドライブギア５０が逆回転されると、ドライブギア５０と係合している第１連結コマ列２１が第１支持体１１ａ内に引き戻される。第１連結コマ列の移動に伴って、柱状体が第３支持体１１ｃ内に引き戻される。引き戻された柱状体は射出部３０後方で分離される。例えば、柱状体を構成する第１連結コマ列２１はガイドローラ４０とドライブギア５０とに挟まれ、柱状体を構成する第２連結コマ列２２は重力により下方に引かれ、それにより第２連結コマ列２２と第１連結コマ列２１とは互いに離反される。離反された第１、第２連結コマ列２１，２２はそれぞれ屈曲可能な状態に復帰し、それぞれ同方向の内側に屈曲され、第１支持体１１ａの内部に略平行に格納される。
【００２１】
（図記号表示）図３  
図３は、図１のロボットアーム機構を図記号表現により示す図である。ロボットアーム機構において、根元３軸を構成する第１関節部Ｊ１と第２関節部Ｊ２と第３関節部Ｊ３とにより３つの位置自由度が実現される。また、手首３軸を構成する第４関節部Ｊ４と第５関節部Ｊ５と第６関節部Ｊ６とにより３つの姿勢自由度が実現される。
【００２２】
ロボット座標系Σbは第1関節部Ｊ１の第１回転軸ＲＡ１上の任意位置を原点とした座標系である。ロボット座標系Σｂにおいて、直交３軸（Ｘｂ、Ｙｂ，Ｚｂ）が規定されている。Ｚｂ軸は第１回転軸ＲＡ１に平行な軸である。Ｘｂ軸とＹｂ軸とは互いに直交し、且つＺb軸に直交する軸である。手先座標系Σｈは、手首部４に取り付けられたロボットハンド５の任意位置（手先基準点）を原点とした座標系である。例えば、ロボットハンド５が２指ハンドのとき、手先基準点（以下、単に手先という。）の位置は２指先間中央位置に規定される。手先座標系Σｈにおいて、直交３軸（Ｘｈ、Ｙｈ，Ｚｈ）が規定されている。Ｚｈ軸は第６回転軸ＲＡ６に平行な軸である。Ｘｈ軸とＹｈ軸とは互いに直交し、且つＺｈ軸に直交する軸である。例えば、Ｘｈ軸は、ロボットハンド５の前後方向に平行な軸である。手先姿勢とは、手先座標系Σhのロボット座標系Σｂに対する直交３軸各々周りの回転角（Ｘｈ軸周りの回転角（ヨウ角）α、Ｙｈ軸周りの回転角（ピッチ角）β、Ｚｈ軸周りの回転角（ロール角）γとして与えられる。
【００２３】
第１関節部Ｊ１は、第１支持部１１ａと第２支持部１１ｂとの間に配設されており、回転軸ＲＡ１を中心としたねじり関節として構成されている。回転軸ＲＡ１は第１関節部Ｊ１の固定部が設置される基台の基準面ＢＰに垂直に配置される。
【００２４】
第２関節部Ｊ２は回転軸ＲＡ２を中心とした曲げ関節として構成される。第２関節部Ｊ２の回転軸ＲＡ２は空間座標系上のＸb軸に平行に設けられる。第２関節部Ｊ２の回転軸ＲＡ２は第１関節部Ｊ１の回転軸ＲＡ１に対して垂直な向きに設けられる。さらに第２関節部Ｊ２は、第１関節部Ｊ１に対して、第１回転軸ＲＡ１の方向（Ｚb軸方向）と第１回転軸ＲＡ１に垂直なＹb軸方向との２方向に関してオフセットされる。第２関節部Ｊ２が第１関節部Ｊ１に対して上記２方向にオフセットされるように、第２支持体１１ｂは第１支持体１１ａに取り付けられる。第１関節部Ｊ１に第２関節部Ｊ２を接続する仮想的なアームロッド部分（リンク部分）は、先端が直角に曲がった２つの鈎形状体が組み合わされたクランク形状を有している。この仮想的なアームロッド部分は、中空構造を有する第１、第２支持体１１ａ、１１ｂにより構成される。
【００２５】
第３関節部Ｊ３は移動軸ＲＡ３を中心とした直動伸縮関節として構成される。第３関節部Ｊ３の移動軸ＲＡ３は第２関節部Ｊ２の回転軸ＲＡ２に対して垂直な向きに設けられる。第２関節部Ｊ２の回転角がゼロ度、つまりアーム部２の起伏角がゼロ度であってアーム部２が水平な基準姿勢においては、第３関節部Ｊ３の移動軸ＲＡ３は、第２関節部Ｊ２の回転軸ＲＡ２とともに第１関節部Ｊ１の回転軸ＲＡ１にも垂直な方向に設けられる。空間座標系上では、第３関節部Ｊ３の移動軸ＲＡ３はＸb軸及びＺb軸に対して垂直なＹb軸に平行に設けられる。さらに、第３関節部Ｊ３は、第２関節部Ｊ２に対して、その回転軸ＲＡ２の方向（Ｙb軸方向）と、移動軸ＲＡ３に直交するＺb軸の方向との２方向に関してオフセットされる。第３関節部Ｊ３が第２関節部Ｊ２に対して上記２方向にオフセットされるように、第３支持体１１ｃは第２支持体１１ｂに取り付けられる。第２関節部Ｊ２に第３関節部Ｊ３を接続する仮想的なアームロッド部分（リンク部分）は、先端が垂直に曲がった鈎形状体を有している。この仮想的なアームロッド部分は、第２、第３支持体１１ｂ、１１ｃにより構成される。
【００２６】
第４関節部Ｊ４は回転軸ＲＡ４を中心としたねじり関節として構成される。第４関節部Ｊ４の回転軸ＲＡ４は第３関節部Ｊ３の移動軸ＲＡ３に略一致するよう配置される。  
第５関節部Ｊ５は回転軸ＲＡ５を中心とした曲げ関節として構成される。第５関節部Ｊ５の回転軸ＲＡ５は第３関節部Ｊ３の移動軸ＲＡ３及び第４関節部Ｊ４の回転軸ＲＡ４に略直交するよう配置される。  
第６関節部Ｊ６は回転軸ＲＡ６を中心としたねじり関節として構成される。第６関節部Ｊ６の回転軸ＲＡ６は第４関節部Ｊ４の回転軸ＲＡ４及び第５関節部Ｊ５の回転軸ＲＡ５に略直交するよう配置される。第６関節部Ｊ６は手先効果器としてのロボットハンド５を左右に旋回するために設けられている。なお、第６関節部Ｊ６は、その回転軸ＲＡ６が第４関節部Ｊ４の回転軸ＲＡ４及び第５関節部Ｊ５の回転軸ＲＡ５に略直交する曲げ関節として構成されてもよい。
【００２７】
このように複数の関節部Ｊ１−Ｊ６の根元３軸のうちの一つの曲げ関節部を直動伸縮関節部に換装し、第１関節部Ｊ１に対して第２関節部Ｊ２を２方向にオフセットさせ、第２関節部Ｊ２に対して第３関節部Ｊ３を２方向にオフセットさせることにより、本実施形態に係るロボット装置のロボットアーム機構は、特異点姿勢を構造上解消している。
【００２８】
（ブロック構成図）図４  
図４は、本実施形態に係るロボット装置の構成を示すブロック図である。本実施形態に係るロボット装置のロボットアーム機構の関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５、Ｊ６には、アクチュエータとして、それぞれステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０が設けられている。ステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０には、ドライバユニット２１０、２２０，２３０，２４０，２５０，２６０が電気的に接続されている。典型的には、ドライバユニット２１０、２２０，２３０，２４０，２５０，２６０は、制御対象のステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０に併設される。これらドライバユニット２１０、２２０，２３０，２４０，２５０，２６０は、同一の構成を有し、動作制御装置１００からの制御信号に従って、制御対象のステッピングモータに対して同一の動作をする。ここでは、ドライバユニット２１０のみ説明し、他のドライバユニット２２０，２３０，２４０，２５０，２６０の説明は省略する。
【００２９】
ドライバユニット２１０は、ステッピングモータ３１０の駆動および停止を制御する。ドライバユニット２１０は、制御部２１１と、電源回路２１２と、パルス信号発生部２１３と、ロータリーエンコーダ２１５と、脱調判定部２１６と、カウンタ２１７とを有する。制御部２１１は、動作制御装置１００から入力された制御信号に従って、ドライバユニット２１０を統括して制御する。
【００３０】
具体的には、ドライバユニット２１０内の制御部２１１には、動作制御装置１００からステッピングモータ３１０の回転角の単位時間△ｔ経過後の目標位置（後述の最終目標位置とは区別される）に関する位置指令信号が入力される。制御信号は△ｔ後の関節変数（関節角度、Ｊ３では伸縮距離）として与えられる。制御部２１１は、ステッピングモータ３１０の回転角の現在位置と目標位置とに基づいてパルス数Ｎｐを決定する。具体的には、制御部２１１は、ステッピングモータ３１０の回転角の現在位置と目標位置とに基づいて回転角度差を計算し、現在位置と目標位置との回転角度差をステッピングモータ３１０の例えば０．７２度のステップ角で除算することによりパルス信号のパルス数Ｎｐを決定する。また、制御部２１１は、決定したパルス数と単位時間△ｔとに基づいてパルス周波数ｆｐを決定する。具体的には、制御部２１１は、単位時間△ｔにパルス数Ｎｐを乗算し、その逆数を計算することによりパルス周波数ｆｐを決定する。制御部２１１は、パルス信号発生部２１３に対して、パルス数に関するパルス数指令信号とパルス周波数に関するパルス周波数指令信号とを出力する。パルス信号発生部２１３は、ステッピングモータ３１０の各相に、パルス数指令信号で指令されたパルス数のパルス信号をパルス周波数指令信号で指令された周波数、つまり当該周波数の逆数の周期で出力する。パルス信号発生部２１３から出力されたパルス信号は後述の脱調判定部２１６にも取り込まれる。
【００３１】
制御部２１１には、動作制御装置１００からステッピングモータ３１０の駆動電流値を表す駆動電流指令信号が入力される。制御部２１１は、電源回路２１２に対して、駆動電流指令信号に応じた制御信号を出力する。電源回路２１２は、図示しない外部の電源供給部から供給された電力を用いて、指定された駆動電流値を有する駆動電流を発生する。発生された駆動電流はステッピングモータ３１０に供給される。ステッピングモータ３１０は、電源回路２１２から供給された駆動電流により駆動され、パルス信号発生部２１３から出力されたパルス信号に従って、回転される。
【００３２】
ロータリーエンコーダ２１５は、ステッピングモータ３１０のドライブシャフトに接続され、例えば０．１８度の一定の回転角ごとにパルス信号（エンコーダパルス）を出力する。カウンタ２１７は、ロータリーエンコーダ２１５から出力されたエンコーダパルスの数を回転方向に応じて加減算することによりカウント数を計算する。このカウント数はステッピングモータ３１０のドライブシャフトの基準位置（原点）でリセットされる。リセット回数とカウント数により関節部の関節角度（関節変数）が求められる。
【００３３】
脱調判定部２１６は、パルス信号発生部２１３から出力されたパルス信号に対するエンコーダパルスのカウント数を比較することによりステッピングモータ３１０が脱調を生じているか、脱調を生じていないかを判定する。脱調判定部２１６は、パルス信号に同期してエンコーダパルスのカウント／リセットを繰り返す。パルス信号の一周期で回転するステップ角は固定である。ステップ角に対応するエンコーダパルスの数は固定である。エンコーダパルスのカウント数がステップ角に対応する数に一致していれば、パルス信号に対してステッピングモータ３１０の回転が同期している、つまり脱調は生じていない。一方、エンコーダパルスのカウント数がステップ角に対応する数に一致していなければ、パルス信号に対してステッピングモータ３１０の回転が同期していない、つまり脱調が生じている。ステッピングモータ３１０が脱調を生じているとき、脱調判定部２１６は、脱調検知信号を発生する。
【００３４】
動作制御装置１００は、システム制御部１０１と、操作部インターフェース１０２と、指令値出力部１０３と、現在位置・姿勢計算部１０４と、軌道計算部１０５と、指令値計算部１０６と、ダイナミクス計算部１０７と、駆動電流決定部１０８と、ドライバユニットインターフェース１０９とを有する。動作制御装置１００には、ドライバユニット２１０から、カウンタ２１７により計数されたカウント数（基準位置からの回転角を表す）とリセット回数（回転回数を表す）に関するデータと脱調判定部２１６から出力された脱調検知信号とがドライバユニットインターフェース１０９を介して入力される。本実施形態に係るロボット装置の動作制御装置１００は、独立した装置として、ステッピングモータを使用する他の装置、機構に対して適用することができる。
【００３５】
（システム制御部）
システム制御部１０１は、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）と半導体メモリ等を有し、動作制御装置１００を統括して制御する。システム制御部１０１には、制御／データバス１１０を介して各部が接続されている。
【００３６】
（操作部）
動作制御装置１００には、操作部インターフェース１０２を介して操作部５０が接続されている。操作部５０は、手首部４又はロボットハンド（手先効果器）の着目点の位置の変更、姿勢の変更および変更時間をユーザが入力するための入力インターフェースとして機能する。動作制御装置１００は、例えば２指ハンドの手先を着目点（制御点）として移動・姿勢変更等の計算処理を実行する。例えば、操作部５０は、ロボットハンドを移動させる最終目標位置と移動時間とを指定するためのジョイスティック等を備える。例えば、ジョイスティックが操作された方向、ジョイスティックが傾けられた角度、ジョイスティックの操作加速度に基づいて、ロボットハンドの最終目標位置と移動時間とが入力される。なお、操作部５０を構成するこれらの入力デバイスは、他のデバイス、例えば、マウス、キーボード、トラックボールおよびタッチパネル等で代替が可能である。
【００３７】
（指令値出力部）  
指令値出力部１０３は、システム制御部１０１の制御に従って、後述の指令値計算部１０６で計算された関節部Ｊ１−Ｊ６ごとの指令値（△ｔ後の関節角度（関節変数））をドライバユニット２１０、２２０，２３０，２４０，２５０，２６０各々に対して出力する。具体的には、指令値出力部１０３は、後述の指令値計算部１０６で計算された関節部Ｊ１−Ｊ６ごとの関節変数に応じた位置指令信号をドライバユニット２１０、２２０，２３０，２４０，２５０，２６０に対して出力する。また、指令値出力部１０３は、後述の駆動電流決定部１０８で決定された関節部Ｊ１−Ｊ６各々を駆動する駆動電流値に応じた駆動電流指令信号をドライバユニット２１０、２２０，２３０，２４０，２５０，２６０に対して出力する。
【００３８】
（現在位置・姿勢計算部）  
現在位置・姿勢計算部１０４は、関節部Ｊ１，Ｊ２，Ｊ３，Ｊ４，Ｊ５、Ｊ６各々の関節変数に基づいて、アーム構造のリンクパラメータに応じて既定された同次変換行列に従って順運動学によりロボット座標系から見た手先着目点の位置と姿勢とを計算する。なお関節変数とは、関節部Ｊ１，Ｊ２，Ｊ４，Ｊ５、Ｊ６では基準位置からの正負の回転角度であり、関節部Ｊ３であれば最も収縮した状態からの伸張距離（直動変位）である。
【００３９】
現在位置・姿勢計算部１０４は、ドライバユニット２１０，２２０，２３０，２４０，２５０，２６０各々のカウンタ２１７により計数されたカウント数に基づいて、関節部Ｊ１−Ｊ６に関する関節変数ベクトル−θを計算する。関節変数ベクトル−θは回転関節部Ｊ１，Ｊ２，Ｊ４、Ｊ５，Ｊ６の関節角度θＪ１，θＪ２，θＪ４、θＪ５、θＪ６と直動伸縮関節部Ｊ３の伸縮長ＬＪ３とで与えられる関節変数の組（θＪ１，θＪ２，ＬＪ３、θＪ４、θＪ５、θＪ６）である。例えば、現在位置・姿勢計算部１０４は、関節部Ｊ１のステッピングモータ３１０の始動前までの累積カウント数を保持し、始動中に計数されたカウント数を累積カウント数に加算することにより、累積カウント数を更新する。現在位置・姿勢計算部１０４は、累積カウント数に１カウントに対応するステップ角度を乗算することにより、ステッピングモータ３１０の基準位置からの回転角度を計算し、関節部Ｊ１の関節変数を特定する。現在位置・姿勢計算部１０４は、他の関節部Ｊ２−Ｊ６の関節変数を同様の手法で特定する。
【００４０】
現在位置・姿勢計算部１０４は、同次変換行列Ｋ（パラメータ（θＪ１，θＪ２，ＬＪ３、θＪ４、θＪ５、θＪ６））により、ロボット座標系Σｂから見た手先着目点の位置（ｘ、ｙ、ｚ）と手先姿勢（α、β、γ）とを計算する。同次変換行列Ｋは、手先座標系Σｈとロボット座標系Σbとの関係を定義する行列式である。同次変換行列Ｋは、ロボットアーム機構を構成するリンク間の関係（リンク長とリンクのねじれ角）と関節部の軸間の関係（リンク間距離とリンクの間角度）とで決まる。例えば、現在位置・姿勢計算部１０４は、現在のロボットアーム機構の関節変数ベクトル−θ０（θ０−Ｊ１，θ０−Ｊ２，Ｌ０−Ｊ３、θ０−Ｊ４、θ０−Ｊ５、θ０−Ｊ６）を同次変換行列Ｋに代入することにより、ロボット座標系Σｂから見た手先の現在位置Ｐ0（ｘ０、ｙ０、ｚ０）と手先姿勢φ０（α０、β０、γ０）とを計算する。
【００４１】
（軌道計算部）  
軌道計算部１０５は、手先の現在位置・姿勢と手先の最終目標位置・姿勢とに基づいて、その間を結ぶ単位時間△ｔ（制御周期、例えば１０ｍｓ）毎の手先の目標位置の点列を計算する。手先の現在位置・姿勢は、現在位置・姿勢計算部１０４による計算処理から得られる。手先の最終目標位置・姿勢と移動時間とは、例えば操作部５０を介してユーザにより入力される。軌道計算部１０５は、予めプリセットされている、手先の現在位置と現在姿勢、手先の最終目標位置と最終目標姿勢とをパラメータとする関数に、各パラメータを代入することにより手先の軌道（以下、手先軌道という）を計算し、その手先軌道上に単位時間△ｔ毎の目標位置を計算する。軌道計算方法としては任意の方法が採用される。単位時間△ｔは制御周期として固定値であり、ユーザにより、例えば１０ｍｓに設定されている。軌道計算部１０５は、手先の現在位置から最終目標位置への移動に要する移動時間幅Ｔと単位時間△ｔとに基づいて、目標位置の数ｍ（＝Ｔ／△ｔ）を決定し、手先軌道上に単位時間△ｔ毎の目標位置（ｐ１、ｐ２・・・ｐｍ（ｍ＝Ｔ／△ｔ））を計算する。ここでは、目標位置は、手先の位置と姿勢との両方を与えるパラメータとする。
【００４２】
詳細は後述するが、軌道計算部１０５は、脱調が検知されたとき、システム制御部１０１の制御のもと５種類の軌道を再度計算することができる。第１−第３軌道は、脱調に起因して本来の軌道からシフトしたハンドの着目点の位置（移動再開位置）から、異なる経路を通って最終目標位置に至る軌道である。第４、第５軌道は、移動再開位置から、異なる経路を通って移動開始時の位置に戻る軌道である。脱調検知時には、これら５つの軌道から一の軌道が選択され、その軌道に沿って移動制御される。
【００４３】
なお、軌道計算部１０５には、軌道計算方法は任意であり、また予め複数の関数がプリセットされていてもよい。例えば、軌道計算部１０５は、システム制御部１０１の制御に従って、手先軌道の計算に用いる関数を変更する。例えば、軌道計算部１０５は、通常の手先軌道の計算に用いられる関数と異なる関数を用いて脱調検知後の手先軌道を再度計算する。例えば、脱調検知後に再度計算された手先軌道は、脱調検知後の位置Ｐ0´と最終目標位置Ｐ１とを結ぶ最短経路と等価である。また、脱調検知後に再度計算された手先軌道は、脱出検知後の位置から脱調検知時の位置に向かって、手先が所定距離引き戻される軌道を有する。
【００４４】
（指令値計算部）  
指令値計算部１０６は、計算された軌道上に△ｔの周期で配列された目標位置にそれぞれ対応する複数の関節変数ベクトルを計算する。なお、関節変数ベクトルとは関節部Ｊ１−Ｊ６の６つの関節変数、つまり回転関節部Ｊ１、Ｊ２、Ｊ４−Ｊ６の回転角と直動伸縮関節部Ｊ３のアーム伸縮長との６変数をいう。指令値計算部１０６の計算処理については後述する。
【００４５】
（ダイナミクス計算部）  
ダイナミクス計算部１０７は、手先を目標位置から△ｔ後の次の目標位置に移動させるために必要とされる関節部Ｊ１−Ｊ６にそれぞれ設けられたステッピングモータ３１０〜３６０各々のトルク（駆動トルク）を計算する。またダイナミクス計算部１０７は、アーム部２を静止させるために各関節部に必要とされるトルク（静止トルク）を計算する。各関節部に必要とされる静止トルクはその関節部より前方の部分の重心質量、その関節部の回転軸から重心までの距離等により計算される当該関節部に係る負荷トルクに逆向きで等価なトルク、つまり自重による負荷トルクと釣り合うトルクである。
【００４６】
ステッピングモータ３１０〜３６０各々の必要トルクは、関節部Ｊ１−Ｊ６に関するダイナミクスモデルデータを用い、ロボットアーム機構の関節部Ｊ１−Ｊ６を連結するリンク各々の重心位置、重心質量、リンク長、慣性テンソル、関節部の関節変数（回転角位置または直動変位）、関節角速度、関節角加速度等から計算される。なお、ダイナミクスモデルは、モータの動特性に関するモデルを指す。
【００４７】
（駆動電流決定部）  
駆動電流決定部１０８は、ダイナミクス計算部１０７で計算されたトルクを発生するためにステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０各々の駆動電流値を決定する。駆動電流決定部１０８は、ステッピングモータ３１０，３２０，３３０，３４０，３５０，３６０各々の、トルクの大きさと駆動電流値とを対応付けた対応表のデータを保持する。駆動電流決定部１０８は、対応表を参照し、ダイナミクス計算部１０７で計算された関節部Ｊ１−Ｊ６各々のトルクの大きさにそれぞれ対応する駆動電流値を決定する。
【００４８】
（脱調時の対処手順）図５，図６  
以下、本実施形態に係るロボット装置の動作制御装置１００による脱調時の対処手順について図５を参照して説明し、その補足説明を図６を参照して行う。図５は、図４の動作制御装置１００による脱調時の処理手順を説明するためのフローチャートである。図６は、第１−第５軌道（１）−（５）を示す図である。動作制御装置１００の各部は、システム制御部１０１の制御に従って、図５のフローチャートに沿って動作される。
【００４９】
（工程Ｓ１１）最終目標位置等の入力  
手先移動に際してはまず操作部インターフェース１０２を介して、手先着目点の最終目標位置が入力される。工程Ｓ１１により、図６に示すように、最終目標位置Ｐ１が入力される。また脱調発生後にオペレータによる手動指示を待って移動再開させるか否か（手動再開のオン／オフ）、手動再開のオフ設定されたときにはさらに脱調発生後移動再開までの待機時間ｔwが設定される。なお、待機時間ｔwは０以上に設定可能である。待機時間ｔwが０時間に設定されていることは、当然にして即座に移動再開を許可していることを意味する。また脱調発生後に移動開始時の位置（移動開始位置）Ｐ0に戻るとき、その帰還経路の異なる軌道（４）、（５）のいずれかをオペレータが手動で選択するか否かが設定される。
【００５０】
（工程Ｓ１２）手先軌道の計算処理  
手先着目点の現在位置（移動開始位置）から、工程Ｓ１１で入力された最終目標位置までの手先軌道が、軌道計算部１０５により計算される。軌道計算部１０５により、手先着目点の現在位置から最終目標位置までの手先軌道上に単位時間△ｔ毎の複数の目標位置が設定される。図６の例では現在位置Ｐ0と最終目標位置Ｐ１とを結ぶ手先軌道が計算され、その手先軌道上に単位時間△ｔ毎の複数の目標位置（ｐ１、ｐ２、・・・ｐｍ−１、ｐｍ（＝Ｐ１））が設定される。手先着目点の現在位置Ｐ0は、現在位置・姿勢計算部１０４による計算処理から得られる。具体的には、現在位置・姿勢計算部１０４により、ドライバユニット２１０，２２０，２３０，２４０，２５０，２６０各々のカウンタ２１７により計数されたカウント数に基づいて、現在の関節変数ベクトル−θ０（θ０−Ｊ１、θ０−Ｊ２、Ｌ０−Ｊ３、θ０−Ｊ４、θ０−Ｊ５、θ０−Ｊ６）が計算され、その関節変数ベクトル−θ０に基づいて、順運動学により手先の現在位置Ｐ0が計算される。
【００５１】
（工程Ｓ１３）関節部Ｊ１−Ｊ６の指令値（関節変数ベクトル）の計算処理  
工程Ｓ１２で計算された複数の目標位置にそれぞれ対応する各関節の角度、伸張距離を表す複数の関節変数ベクトルが、指令値計算部１０６により計算される。工程Ｓ１３により、図６に示す、複数の目標位置（ｐ１、ｐ２、・・・ｐｍ−１、ｐｍ（＝Ｐ１））にそれぞれ対応する複数の関節変数ベクトル（−θ１、−θ２、・・・−θｍ）が計算される。指令値計算部１０６による計算処理手順の詳細は後述する。
【００５２】
（工程Ｓ１４）指令値の出力処理
工程Ｓ１３で計算された関節変数ベクトルに対応する位置指令信号と、駆動電流決定部１０８により決定された駆動電流値に対応する駆動電流指令信号とが、指令値出力部１０３から、ドライバユニット２１０、２２０，２３０，２４０，２５０，２６０に対して、所定の制御周期△ｔ（例えば、１０ｍｓ）で順次出力される。工程Ｓ１４により、手先が現在位置Ｐ0から制御周期△ｔ毎に、目標位置ｐ１、ｐ２、・・・ｐｍ−１を順次通って、最終目標位置Ｐ１まで移動される。
【００５３】
（工程Ｓ１５）手先が最終目標位置に到達したかの判定処理  
システム制御部１０１により、手先が最終目標位置に到達したかが判定される。具体的には、現在位置・姿勢計算部１０４により、制御周期△ｔ毎に計算される手先の現在位置が、図６に示す最終目標位置Ｐ１に対して比較される。手先の現在位置が最終目標位置に一致したとき、手先が最終目標位置に到達したと判定される。このとき、動作制御装置１００による一連の制御が完了される。一方、手先の現在位置が手先の最終目標位置に不一致のとき、処理が工程Ｓ１６に移行される。
【００５４】
（工程Ｓ１６）脱調発生の有無の判定処理  
システム制御部１０１により、脱調発生の有無が判定される。具体的には、脱調判定部２１６から動作制御装置１００に脱調検知信号が入力されたとき、処理が工程Ｓ１７に移行される。図６では、脱調検知時点の手先の位置をＰsoとし、何らかの外因、例えば、作業者がロボットアーム機構のアーム部２に触れてしまい、手先が位置Ｐ0´に移動されてしまったものとする。一方、脱調判定部２１６から脱調検知信号が出力されなければ、手先の現在位置が手先の最終目標位置まで到達するまで、位置指令信号と駆動電流指令信号とが、所定の制御周期△ｔで順次出力される。
【００５５】
（工程Ｓ１７）脱調発生時の手先軌道の再度の計算処理  
工程Ｓ１７では、システム制御部１０１は、脱調検知後に手先移動を再開したときにその到達位置として当初の最終目標位置Ｐ１を維持するのか、又は当初の最終目標位置Ｐ１から移動開始時の位置Ｐ0に変更するのかを、脱調によりシフトした距離△ｄ、つまり当初の軌道上の脱調検知時点の手先の位置Ｐsoと脱調発生後にシフトした手先位置Ｐ0´との距離（シフト距離）△ｄと、脱調検知から脱調解消して移動再開できるまでの時間までの経過時間（ｎ・△t1）とに基づいて、決定する。脱調によるシフト距離△ｄが所定の閾値ＴＨｄ未満であり、且つ脱調検知から移動再開までの経過時間（ｎ・△t1）が所定の閾値ＴＨｔ未満であるとの条件を満たすか否かが判定される。以下、この経過時間（ｎ・△t1）を脱調によるダウンタイムという。なお、脱調検知後、動作制御部１００はドライバーユニット２１０を制御して移動再開のための指令値を繰り返し出力し、移動再開をリトライする。この繰り返し周期（リトライ周期）を△t1、繰り返し回数（リトライ回数）をｎとする。パルス信号発生部２１３からのパルス信号数とエンコードパルス数とが整合、つまりパルス信号と回転とが同期した状態は脱調解消した状態であり、その状態で移動再開が可能になる。この状態に達成するまで移動再開のリトライ制御が繰り返される。
【００５６】
工程Ｓ１７では、脱調に起因して手先位置が本来の軌道から大幅に外れることなく（シフト距離△ｄが比較的短い）、さらに脱調発生してそれが解消するまでに長時間を経過していない（ダウンタイムｎ・△t1が比較的短い）ときには、移動再開して当初の最終目標位置を目指し、一方、脱調に起因して手先位置が本来の軌道から大幅に外れてしまい、（シフト距離△ｄが比較的長い）及び／又は脱調発生してそれが解消するまでに長時間を経過している（ダウンタイムｎ・△t1が比較的長い）ときには、当初の最終目標位置を解除して、移動開始位置に戻り次のタスクを待機するものである。
【００５７】
なお、シフト距離△ｄの長短を判定するためにシフト距離△ｄに比較される閾値ＴＨｄ、ダウンタイム（ｎ・△t1）の長短を判定するためにダウンタイム（ｎ・△t1）に比較される閾値ＴＨｔは、システム制御部１０１により動的に変動される。例えば脱調検知時の位置Ｐsoが最終目標位置Ｐ1より移動開始位置Ｐ0に近いときには比較的小さい値に設定され、脱調検知時の位置Ｐsoが移動開始位置Ｐ0より最終目標位置Ｐ1に近いときには比較的大きな値に設定される。
【００５８】
工程Ｓ１７で“ＹＥＳ”のとき、つまり移動再開して当初の最終目標位置を目指すと判断されたとき、工程Ｓ１８において、システム制御部１０１はオペレータによる移動再開指示が入力されるまで移動再開を待機する。またはオペレータによる移動再開の手動指示が工程Ｓ１１でオフに設定されているときには、脱調検知時点から工程Ｓ１１で設定された待機時間ｔwの経過を待機する。これらは移動再開時の安全性確保のために設けられている。
【００５９】
工程Ｓ１８でオペレータにより移動再開指示が入力されたとき、又は待機時間ｔwが経過しているとき、工程Ｓ１９−Ｓ２１により選択された軌道に従って指令値出力がなされ、移動が再開される。実際には、軌道再計算工程Ｓ１９及び軌道選択工程Ｓ２０は工程Ｓ１８と平行して実行される。工程Ｓ１９では、図６に示す３種類の軌道（１）−（３）が軌道計算部１０５により計算される。
【００６０】
第１軌道（１）は、脱調に起因してシフトしたハンドの着目点の位置（移動再開時の位置）Ｐ0’から、移動開始位置Ｐ0から最終目標位置Ｐ1に至る当初の軌道から脱調に起因して外れた経路を辿って当初の軌道上の脱調検知位置Ｐsoに戻り、当初の軌道に沿って最終目標位置に至る軌道である。
【００６１】
第２軌道（２）は、脱調に起因してシフトしたハンドの着目点の位置（移動再開時の位置）Ｐ0’から、脱調検知時の位置Ｐsoと最終目標位置Ｐ1との間の位置に移動し、当初の軌道に沿って最終目標位置に至る軌道である。第３軌道（３）は、脱調に起因してシフトした着目点の位置Ｐ0’から最終目標位置Ｐ1までの直線的な最短の軌道である。
【００６２】
次に工程Ｓ２０において、これら３種類の軌道（１）−（３）から一の軌道が軌道計算部１０５又はシステム制御部１０１により選択される。ここでは安全性の観点から第１軌道（１）が第１位の優先順位、第２軌道（２）が第２位の優先順位、第３軌道（３）が第３位の優先順位にそれぞれ設定される。選択条件としては、当初の移動開始位置Ｐ0から最終目標位置Ｐ1までの移動に本来的に設定されているタスクタイムが、手先着目点の移動速度に対して既定されている速度上限値未満の範囲内で維持され得るが否かが与えられる。最も軌道長の長い、最も安全性が高いと想定される第１軌道（１）が当該選択条件を満たすのであれば、第１軌道（１）が選択される。第１軌道（１）が当該選択条件を満たさず、次に軌道長の長い、次に安全性が高いと想定される第２軌道（２）が当該選択条件を満たすのであれば、第２軌道（２）が選択される。第１、第２軌道（１）、（２）ともに当該選択条件を満たさず、最終目標位置Ｐ1まで最短経路である第３軌道（３）が当該選択条件を満たすときには、第３軌道（３）が選択される。なお、第３軌道（３）が当該選択条件を満たさないときの動作は任意に設定され、典型的には、その移動再開位置Ｐ0’で留まるようにしても良いし、又は第４軌道（４）を選択して、移動開始位置Ｐ0にも戻って次のタスクを待機するようにしてもよい。
【００６３】
工程２１では、工程Ｓ２０で選択された軌道上の複数の目標位置にそれぞれ対応する複数の関節変数ベクトルを表す指令値が指令値計算部１０６により計算され、所定の制御周期△ｔの周期で順番に駆動電流決定部１０８により決定された各位置の駆動電流値を表す指令信号とともに指令値出力部１０３から、ドライバユニット２１０、２２０，２３０，２４０，２５０，２６０に対して出力される。
【００６４】
工程Ｓ１７で“ＮＯ”のとき、つまり移動再開して、帰還軌道（４）、（５）のいずれかを辿って元の移動開始位置Ｐ0に戻ると判断されたとき、工程Ｓ２２において、システム制御部１０１はオペレータによる移動再開指示が入力されるまで移動再開を待機し、又は脱調検知時点から工程Ｓ１１で設定された待機時間ｔwの経過を待機する。
【００６５】
工程Ｓ２２でオペレータにより移動再開指示が入力されたとき、又は待機時間ｔwが経過しているとき、工程Ｓ２３において、工程Ｓ１１で設定された「移動開始位置Ｐ0に戻る帰還軌道（４）、（５）の手動選択機能」がオフに設定されているとき、工程Ｓ２４において、軌道計算部１０５により、脱調に起因してシフトしたハンドの着目点の位置（移動再開時の位置）Ｐ0’から、元の移動開始位置Ｐ0に戻る帰還軌道（４）が計算される。
【００６６】
帰還軌道（４）は、移動再開時の位置Ｐ0’から、移動開始位置Ｐ0から最終目標位置Ｐ1に至る当初の軌道から脱調に起因して外れた経路を辿って脱調検知位置Ｐsoに戻り当初の軌道を遡って元の移動開始位置Ｐ0に至る軌道である。一方、第５軌道（５）は、脱調に起因してシフトした着目点の位置（移動再開位置）Ｐ0’から元の移動開始位置Ｐ0までの直線的な最短の帰還軌道である。帰還軌道（４）は、直前に通ってきた道程であるので、新規な第５軌道（５）よりも軌道上に障害物等が介在する可能性が低く、すなわち安全性が高いといえる。そのためにオペレータが手動で軌道選択をしないときには帰還軌道（４）が帰還軌道（５）に優先的に選択されるものとしている。
【００６７】
工程２５では、工程Ｓ２４で計算された軌道（４）上の複数の目標位置にそれぞれ対応する複数の関節変数ベクトルを表す指令値が指令値計算部１０６により計算され、所定の制御周期△ｔの周期で順番に駆動電流決定部１０８により決定された各位置の駆動電流値を表す指令信号とともに指令値出力部１０３から、ドライバユニット２１０、２２０，２３０，２４０，２５０，２６０に対して出力される。
【００６８】
工程Ｓ２３において、工程Ｓ１１で設定された「移動開始位置Ｐ0に戻る帰還軌道（４）、（５）の手動選択機能」がオンに設定されているとき、工程Ｓ２６において、システム制御部１０１により操作部５０のディスプレイに帰還軌道（４）と第５軌道（５）といずれかの選択をオペレータに促すための画面が表示される。オペレータが帰還軌道（４）を選択したときは、工程Ｓ２４に移行する。オペレータが帰還軌道（５）を選択したときは、工程Ｓ２８において軌道計算部１０５により、脱調に起因してシフトしたハンドの着目点の位置（移動再開時の位置）Ｐ0’から、元の移動開始位置Ｐ0に直線的に最短で戻る帰還軌道（５）が計算され、工程Ｓ２５に移行する。
【００６９】
上記工程Ｓ１４−Ｓ２８は工程Ｓ１５を介してハンド５の手先着目点が最終目標位置Ｐ1に到達し、又は移動開始位置Ｐ0に帰還するまで繰り返される。
【００７０】
このようにロボットアームのアクチュエータにステッピングモータを採用することはその脱調により動作が停止してしまうためにその採用が難しかったが、上述の通り脱調が発生したときにはその時の位置から最終目標位置までの軌道が再度計算され、その再度計算された軌道に沿った手先移動を再開することができ、またタスクタイムが維持できないときには元の移動開始位置に帰還する手先移動を再開して次のタスク指令を待機することができる。これら作用効果はロボットアームのアクチュエータにステッピングモータを採用することを促進することができる。
【００７１】
本実施形態の脱調時に移動を再開させる制御を採用することは、ステッピングモータ固有の特性である脱調を、安全性向上のために活用できるという新規な着想を現実的に実用化することを意味している。従来ではアームに作業員が接触した時、その接触を加速度センサ等で検知して動作を停止する等の安全機構が必要とされていた。しかし、ステッピングモータを採用したロボットアームに作業員が接触した時、その負荷により脱調が発生し、動作停止する。従って従来同様の安全性が確保され得る。
【００７２】
（指令値計算部の処理の説明）  図７  
次に、工程Ｓ２１、Ｓ２４の指令値計算部１０６による指令値計算処理について図７を参照して説明する。
【００７３】
（工程Ｓ１３１）初期化（ｎ＝０）  
ここでは説明の便宜上、変数ｎを用いる。まずこの変数ｎがゼロ値に初期化される。  
（工程Ｓ１３２）ヤコビアン逆行列Ｊ−１（−θｎ）の計算  
本実施形態のロボット装置のロボットアーム機構はその構造上特異点が存在せず、従ってヤコビアン逆行列が常に存在する。ヤコビアン逆行列は、手先速度（手先位置・姿勢の微小変化）を関節角速度（関節角度・伸縮長の微小変化）に変換する行列である。ヤコビアン逆行列は、手先の位置・手先姿勢を表すベクトルの関節変数による編微分で与えられる。指令値計算部１０６は、工程Ｓ１２で現在位置・姿勢計算部１０４により計算された現在の関節変数ベクトル−θｎ（θｎ−Ｊ１、θｎ−Ｊ２、Ｌｎ−Ｊ３、θｎ−Ｊ４、θｎ−Ｊ５、θｎ−Ｊ６）により、アーム構造のリンクパラメータによるヤコビアン逆行列Ｊ−１（−θｎ）を計算する。
【００７４】
（工程Ｓ１３３）手先速度−ｐｎ＋１の計算  
現在の手先位置（現在の目標位置）ｐｎ、次の手先位置（単位時間△ｔ後の次の目標位置）ｐｎ＋１、単位時間△ｔに基づいて、手先速度・ｐｎ＋１が計算される。  
（工程Ｓ１３４）関節角速度−・θｎ＋１の計算  
工程Ｓ１３３で計算された手先速度−ｐｎ＋１がヤコビアン逆行列Ｊ−１（−θｎ）により、関節角速度−・θｎ＋１に変換される。
【００７５】
（工程Ｓ１３５）目標の関節変数ベクトル−θｎ＋１の計算  
繰り返し回数が前の工程Ｓ１３５で計算された（変数ｎ＝０の場合は工程Ｓ１１で計算された）関節変数ベクトル−θｎ、工程Ｓ１３４で計算された関節角速度−・θｎ＋１、単位時間△ｔに基づいて、次の関節変数ベクトル−θｎ＋１が計算される。関節角速度−・θｎ＋１に単位時間△ｔを乗算することにより、単位時間△ｔの間の各関節の変位量が計算される。移動直前の関節変数ベクトル−θｎに関節の変位量を加算することにより、単位時間△ｔ経過後の関節変数ベクトル−θｎ＋１が計算される。
【００７６】
（工程Ｓ１３６）処理継続の判定  
変数ｎが繰り返し回数（ｍ−１）のとき、指令値計算部１０６による計算処理は終了される。一方、変数（ｎ）が繰り返し回数（ｍ−１）ではない場合、工程Ｓ１３７に処理が移行される。
【００７７】
（工程Ｓ１３７）変数の繰り上げｎ←ｎ＋１  
変数ｎを（ｎ＋１）に繰り上げ、工程Ｓ１４２に処理が戻る。
【００７８】
以上説明した指令値計算部１０６による計算処理により、手先の目標位置（ｐ１、ｐ２、・・・ｐｍ）にそれぞれ対応する関節変数ベクトル（−θ１、−θ２、・・・−θｍ）が計算される。
【００７９】
（効果）  
以上説明したように、本実施形態に係るロボット装置において、ロボットアーム機構を構成する関節部Ｊ１−Ｊ６にはそれぞれステッピングモータ３１０〜３６０が備えられる。これらステッピングモータ３１０〜３６０の動作は、動作制御装置１００により制御される。ステッピングモータは、ステッピングモータに対して、ステッピングモータのトルクよりも大きな負荷が加わると、脱調が発生してしまうため、一般的に産業用ロボットには用いられない。しかしながら、本実施形態に係るロボット装置は、構造上特異点がなく、そのため、アームが予期せぬタイミングで特異点を回避するための大きな旋回移動等をしないことから、例えば、作業者と協働して作業を行うコ・ロボット装置として使用することができる。すなわち、本実施形態に係るロボット装置は、作業者に近接して配置することができる。そのため、関節部のアクチュエータとしてステッピングモータをあえて使用することで、作業者がロボット装置のアームに接触した場合であっても、ステッピングモータの必要トルク以上の負荷がステッピングモータに加われば、脱調してしまうため、アームが作業者に対してステッピングモータの必要トルク以上の大きな力で危害を加えることはない。そのため、関節部のアクチュエータにステッピングモータを用いることで、アームが作業者に接触することによる作業者の怪我等のリスクを他のモータ、例えばＡＣモータ等を使用する場合に比べて低くすることができる。
【００８０】
このように、本実施形態に係るロボット装置の想定配置環境において、作業者がアームに接触し、ステッピングモータが脱調してしまうことは予め想定される。このような場合、脱調後の位置が予定していた手先軌道上から外れてしまう場合がある。しかしながら、本実施形態のロボット装置の動作制御装置１００の軌道計算部１０５は、ステッピングモータ３１０〜３６０に脱調が発生したときに、脱調検知後の手先の現在位置から手先の最終目標位置までの手先軌道を再度計算することができる。したがって、脱調が発生した場合でも、手先の移動制御が継続され、手先を脱調検知後の位置から再び最終目標位置まで移動させることができる。これにより、脱調が発生した場合でも、手先の移動制御が中断されることなく、また、手先を一旦基準位置に戻すことが不要となるため、ダウンタイムを短くすることができる。したがって、本実施形態に係るロボット装置は、ステッピングモータに脱調が生じることを前提として脱調が生じたときの好適な対処を提供することができる。
【００８１】
本実施形態ではロボット装置を例としてステッピングモータ制御について説明したが、他の作動対象、例えばコンベア装置のライン移動のアクチュエータにステッピングモータを採用し、そのステッピングモータ制御にも適用できる。作動対象としてのラインが現在状態（現在速度）から目標状態（目標速度）まで変化（速度変化）する過程を計算し、その過程に従って指令値を出力する。その過程でステッピングモータが脱調を生じたとき、脱調を生じた時点のラインの速度から目標速度までの過程を再度計算し、再度計算された過程に従ってラインの速度を段階的に変化させる。そのような適用も本実施形態のコンセプトの範囲内である。
【００８２】
本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。
【符号の説明】
【００８３】
５０…操作部、１００…動作制御装置、１０１…システム制御部、１０２…操作部Ｉ／Ｆ、１０３…指令値出力部、１０４…現在位置・姿勢計算部、１０５…軌道計算部、１０６…指令値計算部、１０７…ダイナミクス計算部、１０８…駆動電流決定部、１０９…ドライバユニットＩ／Ｆ、１１０…制御／データバス、２１０〜２６０…ドライバユニット、３１０…ステッピングモータ、２１１…制御部、２１２…電源回路、２１３…パルス信号発生部、２１５…エンコーダ、２１６…脱調判定部、２１７…カウンタ
【図１】
0006659238.tif 000002
【図２】
0006659238.tif 000003
【図３】
0006659238.tif 000004
【図４】
0006659238.tif 000005
【図５】
0006659238.tif 000006
【図６】
0006659238.tif 000007
【図７】
0006659238.tif 000008