JP5743495B2

JP5743495B2 - ロボット制御装置

Info

Publication number: JP5743495B2
Application number: JP2010248187A
Authority: JP
Inventors: 根岸　真人; 真人根岸
Original assignee: Canon Inc
Current assignee: Canon Inc
Priority date: 2010-11-05
Filing date: 2010-11-05
Publication date: 2015-07-01
Anticipated expiration: 2030-11-05
Also published as: JP2012096338A; CN102528809A; US20120116582A1; EP2450157B1; EP2450157A3; EP2450157A2; US8744625B2; CN102528809B

Description

本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部を制御するロボット制御装置に関する。

多軸の多関節ロボット本体においては、各関節の角度を同期させて変えながら動作させることが必要である。この同期がずれると、多関節ロボット本体の手先位置の軌道がずれることになるからである。したがって、ロボット制御装置は、多関節ロボット本体の各関節を駆動する複数の駆動部を同期制御している（特許文献１参照）。なお、多関節ロボット本体の目標位置姿勢から各関節角度を計算するのを逆問題、反対に各関節の角度から多関節ロボット本体の位置姿勢を計算するのを順問題と呼び、このような問題を扱う分野を機構学と呼んでいる。

この従来のロボット制御装置は、性質の異なる２つのコンピュータを組み合わせて構成されている。一方のコンピュータは汎用コンピュータであり、他方のコンピュータはリアルタイムコンピュータである。そして、汎用コンピュータは、リアルタイムコンピュータに多関節ロボット本体の目標位置姿勢を示すコマンドを出力する。リアルタイムコンピュータは、逆運動学に基づき、入力したコマンドから、各駆動部の関節角度を示す指令値を時系列に並べた動作指令データを計算することができる。

各駆動部を予定時刻に確実に動作させるには、計算した指令値を多関節ロボット本体の各駆動部に一定の時間間隔（例えば５ｍｓ）で同期して出力する必要がある。

特表２００４−５２４１７１号公報

しかしながら、従来のロボット制御装置では、多関節ロボット本体に人間のように器用な動作をさせることはできなかった。具体的に説明すると、人間のような器用な動作には非常に高度で複雑なアルゴリズムが隠されており、ロボット制御系は高度で複雑なアルゴリズムに基づく計算ができなければならない。そして、多関節ロボット本体の目標位置姿勢が単純な場合でも、多関節ロボット本体の各関節角度は複雑な軌道を描く。つまり、逆運動学に基づく計算処理は、順運動学に基づく計算処理よりも複雑で時間を要する。

したがって、そのような複雑な軌道計算を行う場合、計算の所要時間は同期時間（指令値を出力する時間間隔）よりも長くなってしまうことがある。また計算条件によってもこの所要時間が変わることが多い。つまり軌道計算の所要時間が不定である。この所要時間は例えば１ｓといった、同期時間（例えば５ｍｓ）とは比べられないほど長い時間が想定される。

ところが、特許文献１に記載されているロボット制御装置では、リアルタイムコンピュータにより、複雑な軌道計算と同期動作とを並行して実行する必要がある。したがって、リアルタイムコンピュータは、同期時間内に軌道計算を完了しなければならないが、軌道計算の所要時間が不定であるので、同期時間内に軌道計算が完了しない場合がある。このような場合、リアルタイムコンピュータは、軌道計算を全て完了させてから同期動作を行う必要があり、軌道計算が終了するまで多関節ロボット本体の動作を停止させておかなければならない。これでは、多関節ロボット本体は滑らかな動作を行うことができず、人間のような器用な動作を実現することはできない。

そこで、本発明は、軌道計算が複雑で計算の所要時間が不定であっても、複雑な軌道計算と、同期動作とを並行して実行することが可能なロボット制御装置を提供することを目的とするものである。

本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、共有メモリと、前記共有メモリにアクセス可能に接続された第１の処理部及び第２の処理部と、を備え、前記共有メモリには、前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、前記動作記述部に記述されたコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、を含む命令が予め複数格納され、前記第１の処理部は、前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たしている命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、を有し、前記第２の処理部は、前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たしている命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とする。
また、本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、共有メモリと、前記共有メモリにアクセス可能に接続された第１の処理部及び第２の処理部と、を備え、前記共有メモリには、前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、前記動作記述部に記述されたコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、少なくとも計算開始待ち状態、計算中状態、計算完了状態及び動作中状態のうちのいずれかの状態となる状態変数と、を含む命令が予め複数格納され、前記第１の処理部は、前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たし、且つ状態変数が計算開始待ち状態である命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、前記共有メモリに格納された命令のうち、前記計算開始条件判定部によりコマンドが取得された命令の状態変数を計算中状態に変更する第１の状態変数変更部と、前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、前記共有メモリに格納された命令のうち、前記計算部による計算が完了した動作指令データに対応する命令の状態変数を計算完了状態に変更する第２の状態変数変更部と、を有し、前記第２の処理部は、前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たし、且つ状態変数が計算完了状態である命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、前記共有メモリに格納された命令のうち、前記動作開始条件判定部により取得された動作指令データに対応する命令の状態変数を動作中状態に変更する第３の状態変数変更部と、前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とする。

本発明によれば、第１の処理部に軌道計算処理を実施させて計算結果である動作指令データを共有メモリに格納させているので、第２の処理部は、複雑な軌道計算処理を行う必要がない。そして、第２の処理部は、複雑な軌道計算処理を行わないので、第１の処理部により既に計算された動作指令データの指令値を各駆動部に同期して出力する同期処理を、多関節ロボット本体の動作を停止させることなく実現することができる。したがって、多関節ロボット本体に人間のような器用な動作を行わせることができる。

本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。本発明の第１実施形態に係るロボット装置の概略構成を示すブロック図である。本発明の第１実施形態に係るロボット制御装置の各部の動作の流れを示す説明図である。本発明の第１実施形態に係る共有メモリに格納される各命令の構成を示す説明図である。多関節ロボット本体の各関節に出力する動作指令データの説明図であり、（ａ）は各関節に出力する動作指令データを時系列に並べた場合のタイムチャート、（ｂ）は（ａ）にて円で囲った部分の指令値の列を示す拡大図である。３つの命令を使った動作例を示すタイムチャート図である。本発明の第２実施形態に係るロボット制御装置の動作の流れを示す説明図である。本発明の第２実施形態に係るロボット制御装置の共有メモリに格納される各命令の構成を示す説明図である。本発明の第３実施形態に係るロボット制御装置の第１の処理部の動作の流れを示す説明図である。本発明の第３実施形態に係るロボット制御装置の第１の処理部の動作の流れを示す説明図である。本発明の第４実施形態に係るロボット制御装置により生成される動作指令データの例を示す図である。比較例としてのロボット制御装置により生成される動作指令データの例を示す図である。本発明の第５実施形態に係るロボット制御装置の動作の流れを示す説明図である。本発明の第４実施形態に係るロボット制御装置により生成される、オフセット値を加算した動作指令データの一例を示す図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置の概略構成を示す説明図である。ロボット装置５０は、多関節ロボット本体１００と、多関節ロボット本体１００を制御するロボット制御装置２００と、を備えている。

多関節ロボット本体１００は、複数のリンク１０１〜１０５と、先端リンク１０５に接続されたエンドエフェクタとしてのハンド１０６と、を有しており、リンク１０１〜１０５及びハンド１０６が各関節Ｊ１〜Ｊ６で揺動又は回動可能に連結されている。本実施形態では、多関節ロボット本体１００は、自由度が６の６軸多関節ロボットである。

図２は、ロボット装置５０の概略構成を示すブロック図である。各関節Ｊ１〜Ｊ６には、各関節Ｊ１〜Ｊ６を駆動する複数の駆動部としてのアーム用モータＭａ（Ｍａ_１〜Ｍａ_６）が設けられ、ハンド１０６には、ハンド用モータＭｂが設けられている。また、図示は省略するが、多関節ロボット本体１００は、直動アクチュエータやシリンダ等を有している。これらアーム用モータＭａ、ハンド用モータＭｂ、直動アクチュエータ及びシリンダが出力機器２１４である。

また、各関節Ｊ１〜Ｊ６には、各アーム用モータＭａの回転角度を検知するアーム用エンコーダＥａが設けられている。また、ハンド１０６には、ハンド用モータＭｂの回転角度を検知するハンド用エンコーダＥｂが設けられている。ハンド１０６と先端リンク１０５との間には、ハンド１０６に作用する力及びトルクを検知する力覚センサＳａが設けられている。また、図示は省略するが、各モータＭａ，Ｍｂに供給する電流を検知する電流センサ、各関節Ｊ１〜Ｊ６のトルクを検知するトルクセンサが、多関節ロボット本体１００に設けられている。これらエンコーダＥａ，Ｅｂ、力覚センサＳａ、電流センサ及びトルクセンサが、多関節ロボット本体１００の状態を検知するセンサとしての入力機器２１５である。以下、入力機器２１５及び出力機器２１４を総称して、デバイス２０７という。

ロボット制御装置２００は、大別すると、第１の処理部２０１と、第２の処理部２０２と、共有メモリ２０３と、を備えている。第１の処理部２０１及び第２の処理部２０２は、共有メモリ２０３にアクセス可能に接続されている。本実施形態では、ロボット制御装置２００は、１つのデュアルコアＣＰＵを有し、１つのデュアルコアＣＰＵにおける各コアが、第１の処理部２０１及び第２の処理部２０２のそれぞれとして機能するように構成されている。なお、ロボット制御装置２００が、シングルコアＣＰＵを２つ有し、各ＣＰＵが第１の処理部２０１及び第２の処理部２０２のそれぞれとして機能するように構成してもよい。

本第１実施形態では、第１の処理部２０１は、多関節ロボット本体１００を目標位置姿勢に動作させるコマンドを入力し、この入力したコマンドから各モータＭａ_１〜Ｍａ_６に出力する関節角度の指令値を時系列に連ねた動作指令データ（軌道データ）を計算する。そして、計算結果である動作指令データを共有メモリ２０３に格納する。このように、第１の処理部２０１は、逆運動学に基づく計算所要時間の不定な軌道計算処理を行う。

第２の処理部２０２は、共有メモリ２０３に格納された動作指令データを取得して、指令値を各モータＭａ_１〜Ｍａ_６に一定の時間間隔で同期して出力する同期処理を行う。つまり、本第１実施形態では、複雑で計算に要する時間が不定となる軌道計算処理を、第１の処理部２０１で行い、各モータＭａ_１〜Ｍａ_６の同期処理は、第２の処理部２０２が行う。

以下、各処理部２０１，２０２及び共有メモリ２０３について詳細に説明する。第１の処理部２０１は、計算開始条件判定部２１１と、計算部２１２と、を有している。第２の処理部２０２は、動作開始条件判定部２２１と、受信部２２２と、指令値補正部２２３と、送信部２２４と、動作完了条件判定部２２５と、を有している。

図３は、ロボット制御装置２００の各部の動作の流れを示す説明図である。図３に示すように、まず、共有メモリ２０３には、予め複数の命令Ｃ_１，Ｃ_２，Ｃ_３…からなる命令集合Ａが格納されている。つまり、共有メモリ２０３には、多関節ロボット本体１００の関節角度を決めるための命令が予め複数格納されている。命令集合Ａは、例えばテキスト形式のファイルで、ユーザがロボットの動作を記述する目的で準備する。通常は命令を多数組み合わせてロボットの動作を実現する。図４に示すように、各命令Ｃ_ｎ（ｎは正の整数）は、動作記述部ｃ_１、計算開始条件記述部ｃ_２、動作開始条件記述部ｃ_３、及び動作完了条件記述部ｃ_４で構成されている。

まず、共有メモリ２０３に格納されている命令のうち、各モータＭａ_１〜Ｍａ_６に駆動動作を行わせるための命令について説明する。動作記述部ｃ_１には、多関節ロボット本体１００（より具体的には、多関節ロボット本体１００のハンド１０６）を目標位置姿勢に動作させるためのコマンドが記述されている。ここで、コマンドは、「ジョブ」あるいは「Ｉｎｓｔｒｕｃｔｉｏｎ」と同じ意味である。コマンドは、多関節ロボット本体１００のハンド１０６の目標位置姿勢を直行座標系で指定するものである。即ち、ハンド１０６の目標位置姿勢は、直交座標系で（Ｘ，Ｙ，Ｚ，θ_Ｘ，θ_Ｙ，θ_Ｚ）で指定される。そして、例えばハンド１０６を、Ｘ＝１００ｍｍ、Ｙ＝２００ｍｍの座標位置に移動せよ、といったコマンドが動作記述部ｃ_１に記述されている。また例えば、ハンド１０６の現在の位置姿勢から相対的な位置姿勢を指定するコマンドが動作記述部ｃ_１に記述されている。

計算開始条件記述部ｃ_２には、計算部２１２により軌道計算を開始する条件が記述されている。例えば、他の命令に含まれているコマンドを実行するときにそのコマンドによりハンド１０６が到達する最終目標位置が確定したら計算を開始する、といった条件が、計算開始条件記述部ｃ_２に記述されている。また例えば、多関節ロボット本体１００の動作が停止した時に計算を開始する、センサの値が確定した時に計算を開始する、といった条件が、計算開始条件記述部ｃ_２に記述されている。

動作開始条件記述部ｃ_３には、コマンドを実行するための各モータＭａ_１〜Ｍａ_６の動作を開始する条件が記述されている。例えば２本のアームを協調させて作業する場合、同時に動作を開始する必要があるので、例えば動作開始条件は２本分の軌道の計算を待つという条件が動作開始条件記述部ｃ_３に記述されている。

動作完了条件記述部ｃ_４には、各モータＭａ_１〜Ｍａ_６の動作を完了させる条件が記述されている。例えばモータＭａ_１〜Ｍａ_６の軌道が終端に達した場合（多関節ロボット本体１００が目標位置姿勢に達した場合）にモータＭａ_１〜Ｍａ_６の動作を完了させる、といった条件が動作完了条件記述部ｃ_４に記述されている。

なお、軌道計算ではないが、モータＭｂや不図示の直動アクチュエータ、シリンダ等に駆動動作を行わせるための命令についても同様に構成されており、共有メモリ２０３に予め格納されている。

また、共有メモリ２０３には、入力機器（センサ）２１５に測定動作を行わせるための命令も格納されている。以下、入力機器２１５に測定動作を行わせるための命令について説明する。

動作記述部ｃ_１には、入力機器２１５への測定要求が記述される。計算開始条件記述部ｃ_２には、入力機器２１５が測定する前に必要な測定準備を開始する条件が記述される。例えば入力機器２１５がハンド１０６に取り付けられた力覚センサＳａや視覚センサである場合、ハンド１０６が目標位置姿勢になるまで待つといった条件が記述される。

動作開始条件記述部ｃ_３には、入力機器２１５に測定動作を開始させる条件が記述される。入力機器２１５が視覚センサの場合、実際に視覚センサが測定を開始できる条件が記述される。例えば他のロボット装置が測定対象を把持している場合、そのロボット装置が停止するまで待つといった条件が記述される。動作完了条件記述部ｃ_４には、入力機器２１５による測定動作を完了させる条件が記述されている。例えば、測定値が安定したかどうかを判定する条件が記述されている。

次に、第１の処理部２０１について説明する。計算開始条件判定部２１１は、共有メモリ２０３に格納された命令集合Ａにおける複数の命令Ｃ_１，Ｃ_２，Ｃ_３…の計算開始条件記述部ｃ_２を順次参照する。そして、計算開始条件判定部２１１は、計算開始条件を満たしている命令の動作記述部ｃ_１に記述されているコマンドを共有メモリ２０３から１つ取得する。

計算開始条件判定部２１１は、もし、計算開始条件を満たすものが無い場合は、条件を満たすまで待つ。従って最初に起動するときは、複数の命令Ｃ_１，Ｃ_２，Ｃ_３…のうち、少なくとも１つ計算開始条件を満たす命令が必要である。また、図示していないが、外部から命令の中の計算開始条件を変更することによって、起動させることもできる。

計算部２１２は、計算開始条件判定部２１１にて取得されたコマンドを受け取り、そのコマンドから各モータＭａ_１〜Ｍａ_６に出力する関節角度の指令値を時系列に連ねた動作指令データＢを逆運動学モデルに基づいて計算する。そして、計算部２１２は、計算結果である動作指令データＢを共有メモリ２０３に格納する。共有メモリ２０３には、計算部２１２で計算された動作指令データＢが順次格納されるので、複数の動作指令データＢが格納されることとなる。

以下、動作指令データＢについて、図５に示す具体例を参照しながら説明する。図５（ａ）は、各関節に出力する動作指令データＢを時系列に並べた場合のタイムチャートである。図５（ａ）に示す多関節ロボット本体１００を位置姿勢Ｐ１から位置姿勢Ｐ３に位置姿勢Ｐ２を経由して移動させよ、というコマンドを計算開始条件判定部２１１が取得した場合、計算部２１２は、そのコマンドから動作指令データＢを計算する。この動作指令データＢは、各モータＭａ_１〜Ｍａ_６に出力する各関節Ｊ１〜Ｊ６の関節角度を示す指令値を時系列に連ねたものである。

図５（ｂ）は、図５（ａ）にて円で囲った部分の指令値の列を示す拡大図である。図５（ｂ）に示すように、関節Ｊ１におけるモータＭａ_１に対応して、指令値ｄ１_１〜ｄ１_９が計算され、関節Ｊ６におけるモータＭａ_６に対応して、指令値ｄ６_１〜ｄ６_９が計算される。そして、各指令値は、第２の処理部２０２にて、一定の時間間隔δｔ（つまり同期時間δｔ）で各モータＭａ_１〜Ｍａ_６に出力される。以上のように、第１の処理部２０１は、計算が複雑で計算所要時間が不定な軌道計算処理を行う。以上の計算部２１２の処理動作が終了したら、再び計算開始条件判定部２１１による動作を実施する。

なお、計算部２１２は、ハンド用モータＭｂや直動アクチュエータ、シリンダ等の軌道計算を行わないものについても、入力したコマンドに基づいて、指令値からなる動作指令データを計算する。また、入力機器２１５の場合も同様である。モータＭａ_１〜Ｍａ_６の時のように軌道は計算しないが、測定する前の計算を行う。例えば力覚センサＳａの場合、多関節ロボット本体１００（ハンド１０６）が所定の位置にきた時の値を計算して、その計算結果である測定指令データを共有メモリ２０３に格納する。

次に、第２の処理部２０２について説明する。動作開始条件判定部２２１は、共有メモリ２０３に格納された各命令Ｃ_１，Ｃ_２，Ｃ_３…の動作開始条件記述部ｃ_３を参照する。そして、動作開始条件判定部２２１は、共有メモリ２０３に格納された動作指令データＢ…のうち、動作開始条件を満たしている命令に対応する動作指令データＢを共有メモリ２０３から取得する。このとき、動作開始条件を満たしている命令が複数ある場合には、動作開始条件判定部２２１は、その複数の動作指令データを共有メモリ２０３から取得する。なお、動作開始条件判定部２２１は、測定指令データについても動作開始条件を満たしていれば、対応する測定指令データを共有メモリ２０３から取得する。

受信部２２２は、入力機器２１５（センサ）から検知結果である信号を入力する。そして、動作開始条件判定部２２１が測定指令データを取得していた場合には、検知結果を補正する。例えば力覚センサＳａの場合は、実際に測定する時に測定指令値データの指令値を毎回差し引くことによって、重力の影響を軽減する。また、例えば視覚センサの場合は、多関節ロボット本体１００の関節角度から視覚センサが取り付いている３次元的な位置を計算し、実際に測定する時の補正に使用する。

指令値補正部２２３は、動作開始条件判定部２２１にて取得された動作指令データＢの各モータＭａ_１〜Ｍａ_６に同期して出力するための各指令値を、入力機器２１５からの信号を用いて補正をし、送信部２２４に出力する。これにより、例えば入力機器２１５が力覚センサＳａの場合、力フィードバック制御が可能になり、また、入力機器２１５が視覚センサの場合、視覚フィードバック制御が可能になる。

補正量を計算する方法は既知である。例えば文献（ロボット工学ハンドブック、日本ロボット学界編、コロナ社（東京）、２００５年、２８７ページ）などに記述されているインピーダンス制御、あるいはコンプライアンス制御等の力制御が利用可能である。

送信部２２４は、共有メモリ２０３から取得され補正された各指令値を入力し、各指令値を各モータＭａ_１〜Ｍａ_６に一定の時間間隔δｔ、つまり同期時間δｔで同期して出力する。この同期時間δｔはミリ秒、例えば５ミリ秒といった短い時間である。動作完了条件判定部２２５は処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。そして、共有メモリ２０３に、動作結果Ｄが格納される。以上説明した各部２２１〜２２５の動作は、同期時間δｔ毎に繰返し行なわれる。

以上の構成により、例えば命令Ｃ_１，Ｃ_２，Ｃ_３の順に軌道計算処理を行い、命令Ｃ_１，Ｃ_２を同時に実行し、命令Ｃ_２の実行終了後、命令Ｃ_３を実行する場合について説明する。図６に、３つの命令を使った動作例のタイムチャート図を示す。

第１の処理部２０１の計算部２１２は、共有メモリ２０３から取得された命令Ｃ_１のコマンドに基づいて命令Ｃ_１に対応する動作指令データを計算して、共有メモリ２０３に格納する。次いで、計算部２１２は、共有メモリ２０３から取得された命令Ｃ_２のコマンドに基づいて命令Ｃ_２に対応する動作指令データを計算して、共有メモリ２０３に格納する。次いで、計算部２１２は、共有メモリ２０３から取得された命令Ｃ_３のコマンドに基づいて命令Ｃ_３に対応する動作指令データを計算して、共有メモリ２０３に格納する。このように、第１の処理部２０１の計算部２１２は、各コマンドを１つずつ順次計算して共有メモリ２０３に計算結果を格納する。

このとき、第２の処理部２０２は、例えば第１の処理部２０１の計算部２１２が、命令Ｃ_３のコマンドから動作指令データを演算している途中であっても、第１の処理部２０１の計算動作とは独立して動作することができる。従って、第２の処理部２０２の動作開始条件判定部２２１は、第１の処理部２０１による命令Ｃ_３のコマンドに基づく計算終了を待たずに共有メモリ２０３にアクセスし、命令Ｃ_１の動作指令データと、命令Ｃ_２の動作指令データとを取得している。そして、各命令Ｃ_１，Ｃ_２の動作を実行している。その後、第２の処理部２０２は、命令Ｃ_３の動作を実行している。このように、本第１実施形態では、軌道計算処理と同期処理とを並行して行うことが可能となり、ロボット装置５０の動作時間の短縮につながる。特に、ロボット装置５０が生産装置に組み込まれる場合には、動作時間の短縮は生産物のコストを下げることができるので非常に重要である。

以上、本第１実施形態によれば、第１の処理部２０１に軌道計算処理を実施させて計算結果である動作指令データＢを共有メモリ２０３に格納させているので、第２の処理部２０２は、複雑な軌道計算処理を行う必要がない。したがって、たとえ第１の処理部２０１で計算時間が多くかかっても、第２の処理部２０２は、逆運動学に基づく軌道演算処理を行わないので、動作開始条件を満たした直後に各モータＭａ_１〜Ｍａ_６を動かすことができる。

そして、第２の処理部２０２は、複雑な軌道計算処理を行わず、第１の処理部２０１により既に計算された動作指令データＢの指令値を各モータＭａ_１〜Ｍａ_６に同期して出力する同期処理を行うようにしている。したがって、多関節ロボット本体１００の動作を停止させることなく実現することができ、多関節ロボット本体１００を滑らかに動作させることができるので、多関節ロボット本体１００に人間のような器用な動作を行わせることができる。

また、本第１実施形態では、第１の処理部２０１が計算開始条件判定部２１１を有しているので、計算部２１２は、適したタイミングで計算を開始することができる。また、第２の処理部２０２が、動作開始条件判定部２２１を有しているので、適したタイミングで動作を開始することができる。つまり、軌道計算処理及び同期処理のそれぞれの処理が、命令Ｃ_１，Ｃ_２，Ｃ_３…に従ってタイミングよく動作させることができるので、他の動作機器（例えば把持対象物が載置される移動テーブル）との同期をとることもできる。

以上の説明では、１つのロボット制御装置２００で１台の多関節ロボット本体１００を制御する場合について説明したが、１つのロボット制御装置２００で複数台の多関節ロボット本体を制御する場合についても制御動作は同じである。つまり、共有メモリ２０３には、各多関節ロボット本体に対する命令が予め格納されており、各処理部２０１，２０２は、各命令に基づき、軌道計算処理及び同期処理のそれぞれを行う。特に、各多関節ロボット本体を同時に動作させる場合には、例えば図６に示すように、命令Ｃ_１と命令Ｃ_２とを同時に出力することができる。したがって、複数台の多関節ロボット本体を、停止させることなく同期して動作させることができる。これにより、複数台の多関節ロボット本体を滑らかに動作させることができ、複数台の多関節ロボット本体に人間のような器用な動作を行わせることができる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット制御装置について説明する。図７は、ロボット制御装置２００Ａの動作の流れを示す説明図であり、図８は、共有メモリに格納される各命令の構成を示す説明図である。なお、本第２実施形態において、上記第１実施形態と同様の構成については、同一符号を付して説明を省略する。

図８に示すように、共有メモリ２０３に予め格納される各命令Ｃ_ｎは、上記第１実施形態で説明した、動作記述部ｃ_１、計算開始条件記述部ｃ_２、動作開始条件記述部ｃ_３、及び動作完了条件記述部ｃ_４の他、更に状態変数ｃ_５を有している。本第２実施形態では、状態変数ｃ_５は、計算開始待ち状態、計算中状態、計算完了状態、動作中状態及び動作完了状態のうちのいずれかの状態を意味する値となる。最初に起動するときには状態変数ｃ_５を計算開始待ち状態にセットしておく。

計算開始待ち状態とは、軌道の計算開始を待っている状態である。計算中状態とは、図７に示す計算部２１２が軌道の計算を開始し、計算中である状態である。計算完了状態とは、計算部２１２による計算が完了し、共有メモリ２０３に動作指令データＢが保存された状態である。動作中状態とは、第２の処理部２０２Ａが動作を開始し、動作中である状態であり、動作完了状態とは、第２の処理部２０２Ａが動作を完了している状態である。

また、図７に示す第１の処理部２０１Ａは、計算開始条件判定部２１１Ａ及び計算部２１２の他、第１の状態変数変更部３０１及び第２の状態変数変更部３０２を有している。計算開始条件判定部２１１Ａは、共有メモリ２０３に格納された各命令Ｃ_１，Ｃ_２，Ｃ_３…の計算開始条件記述部ｃ_２を参照する。そして、計算開始条件判定部２１１Ａは、計算開始条件を満たし、且つ状態変数ｃ_５が計算開始待ち状態である命令を抽出する。そして、計算開始条件判定部２１１Ａは、その命令に含まれる動作記述部ｃ_１に記述されているコマンドを共有メモリ２０３から１つ取得する。

ここで、計算開始条件判定部２１１Ａは、もし該当する命令が存在しなければ、計算条件が満たされるまで待つ。従って最初に起動するときは、少なくとも１つの命令が、計算開始条件を満足していなければならない。また、全部の命令が計算開始条件を満足しない場合でも、図示していない外部のプログラムから、この計算開始条件を変更して、起動させることは容易である。例えば、状態変数を開始待ち状態に変更すればよい。

次に、第１の状態変数変更部３０１は、共有メモリ２０３に格納された命令Ｃ_１，Ｃ_２，Ｃ_３…のうち、計算開始条件判定部２１１Ａによりコマンドが取得された命令の状態変数ｃ_５を、計算開始待ち状態から計算中状態に変更する。そして、計算部２１２は、計算開始条件判定部２１１Ａにより取得されたコマンドから動作指令データＢを計算し、この動作指令データＢを共有メモリ２０３に格納する。

つまり、計算開始条件判定部２１１Ａは、状態変数ｃ_５が計算開始待ち状態以外の状態である命令に含まれるコマンドを取得することはないので、重複計算となるのを回避することができる。

第２の状態変数変更部３０２は、計算部２１２で計算が完了した場合、共有メモリ２０３に格納された命令Ｃ_１，Ｃ_２，Ｃ_３…のうち、計算部２１２による計算が完了した動作指令データに対応する命令の状態変数ｃ_５を変更する。具体的には、状態変数ｃ_５を、計算中状態から計算完了状態に変更する。以上の第２の状態変数変更部３０２による変更動作が終了したら、再び計算開始条件判定部２１１Ａによる動作を実施する。

次に、第２の処理部２０２Ａについて説明する。第２の処理部２０２Ａは、動作開始条件判定部２２１Ａ、受信部２２２、指令値補正部２２３、送信部２２４及び動作完了条件判定部２２５の他、第３の状態変数変更部３０３及び第４の状態変数変更部３０４を有している。

動作開始条件判定部２２１Ａは、共有メモリ２０３に格納された各命令Ｃ_１，Ｃ_２，Ｃ_３…の動作開始条件記述部ｃ_３を参照する。そして、動作開始条件判定部２２１Ａは、共有メモリ２０３に格納された動作指令データＢ…のうち、動作開始条件を満たし、且つ状態変数ｃ_５が計算完了状態である命令に対応する動作指令データＢを共有メモリ２０３から取得する。このとき、動作開始条件を満たし、且つ計算完了状態である命令が複数ある場合には、動作開始条件判定部２２１Ａは、その条件を満たした全ての動作指令データＢを共有メモリ２０３から取得する。

第３の状態変数変更部３０３は、共有メモリ２０３に格納された命令のうち、動作開始条件判定部２２１Ａにより取得された動作指令データＢに対応する命令の状態変数ｃ_５を計算完了状態から動作中状態に変更する。これにより、動作中の命令を重複して実行するのを回避することができ、制御が安定する。

送信部２２４は、共有メモリ２０３から取得され補正された各指令値を入力し、各指令値を各モータＭａ_１〜Ｍａ_６に一定の時間間隔δｔ、つまり同期時間δｔで同期して出力する。この同期時間δｔはミリ秒、例えば５ミリ秒といった短い時間である。動作完了条件判定部２２５は処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。そして、共有メモリ２０３に、動作結果Ｄが格納される。

第４の状態変数変更部３０４は、共有メモリ２０３に格納された命令のうち、動作が完了したときの動作指令データＢに対応する命令の状態変数ｃ_５を動作中状態から動作完了状態に変更する。以上説明した各部２２１Ａ〜３０４の動作は、同期時間δｔ毎に繰返し行なわれる。なお、共有メモリ２０３に格納された命令が入力機器２１５に対するものである場合も同様に動作する。

以上、本第２実施形態では、第１の処理部２０１Ａに軌道計算処理を実施させて計算結果である動作指令データＢを共有メモリ２０３に格納させているので、第２の処理部２０２Ａは、複雑な軌道計算処理を行う必要がない。したがって、たとえ第１の処理部２０１Ａで計算時間が多くかかっても、第２の処理部２０２Ａは、逆運動学に基づく軌道演算処理を行わないので、動作開始条件を満たした直後に各モータＭａ_１〜Ｍａ_６を動かすことができる。

そして、第２の処理部２０２Ａは、複雑な軌道計算処理を行わず、第１の処理部２０１により既に計算された動作指令データＢの指令値を各モータＭａ_１〜Ｍａ_６に同期して出力する同期処理を行うようにしている。したがって、多関節ロボット本体１００の動作を停止させることなく実現することができ、多関節ロボット本体１００を滑らかに動作させることができるので、多関節ロボット本体１００に人間のような器用な動作を行わせることができる。

また、本第２実施形態では、第１の処理部２０１Ａが計算開始条件判定部２１１Ａを有しているので、計算部２１２は、適したタイミングで計算を開始することができる。また、第２の処理部２０２Ａが、動作開始条件判定部２２１Ａを有しているので、適したタイミングで動作を開始することができる。つまり、軌道計算処理及び同期処理のそれぞれの処理が、命令Ｃ_１，Ｃ_２，Ｃ_３…に従ってタイミングよく動作させることができるので、他の動作機器（例えば把持対象物が載置される移動テーブル）との同期をとることもできる。

また、本第２実施形態では、各命令Ｃ_１，Ｃ_２，Ｃ_３…に状態変数ｃ_５が含まれているので、状態変数ｃ_５を監視することで、第１の処理部２０１Ａ及び第２の処理部２０２Ａによる制御動作が安定する。

以上の説明では、１つのロボット制御装置２００Ａで１台の多関節ロボット本体１００を制御する場合について説明したが、１つのロボット制御装置２００Ａで複数台の多関節ロボット本体を制御する場合についても制御動作は同じである。つまり、共有メモリ２０３には、各多関節ロボット本体に対する命令が予め格納されており、各処理部２０１Ａ，２０２Ａは、各命令に基づき、軌道計算処理及び同期処理のそれぞれを行う。特に、各多関節ロボット本体を同時に動作させる場合には、例えば図６に示すように、命令Ｃ_１と命令Ｃ_２とを同時に出力することができる。したがって、複数台の多関節ロボット本体を、停止させることなく同期して動作させることができる。これにより、複数台の多関節ロボット本体を滑らかに動作させることができ、複数台の多関節ロボット本体に人間のような器用な動作を行わせることができる。

［第３実施形態］
次に、本発明の第３実施形態に係るロボット制御装置について説明する。図９はロボット制御装置の第１の処理部の各部の動作の流れを示す説明図である。図１０はロボット制御装置の第２の処理部の各部の動作の流れを示す説明図である。なお、上記第１，第２実施形態と同様の構成については、同一符号を付して説明を省略する。ここで、上記第１，第２実施形態では、多関節ロボット本体１００が１台の場合について説明したが、２台以上であっても制御動作は同じである。本第３実施形態では、２台の多関節ロボット本体１００ａ，１００ｂを対象としているが、制御動作は多関節ロボット本体が１台であっても、３台以上であっても変わらない。

本第３実施形態では、ロボット制御装置は、図９に示す第１の処理部２０１Ｂと、図１０に示す第２の処理部２０２Ｂと、を備えている。まず、第１の処理部２０１Ｂについて説明する。計算開始条件判定部２１１Ａは、共有メモリ２０３に格納されている命令集合Ａの中から、それぞれの命令Ｃ_１，Ｃ_２，Ｃ_３…の計算開始条件記述部ｃ_２を参照し、計算開始条件を満たす命令を選択する（４０１）。この時、計算開始条件判定部２１１Ａは、状態変数ｃ_５が計算開始待ち状態であることを条件に入れて探索する（４０２）。もし該当する命令が存在しなければ、再び共有メモリ２０３に格納されている命令集合Ａの中から、それぞれの命令Ｃ_１，Ｃ_２，Ｃ_３…の計算開始条件記述部ｃ_２を参照し、計算開始条件を満たす命令を選択する。

計算開始条件判定部２１１Ａがコマンドを１つ取得した後、第１の状態変数変更部３０１が状態変数ｃ_５を計算開始待ち状態から計算中状態に変更する。

計算部２１２Ｂは、計算開始条件判定部２１１Ａが取得したコマンドに基づいて動作指令データＢを計算する。計算部２１２Ｂは、処理選択部４０３、モータ制御計算部４０４、センサ制御計算部４０５、外部機器制御部４０６、補正部４０７、及び動作指令データ出力部４０８を有している。

処理選択部４０３は、コマンドの内容を解釈する。モータ制御計算部４０４は、多関節ロボット本体１００ａ，１００ｂの関節のアーム用モータＭａ_１〜Ｍａ_６に対する軌道を計算する。軌道が決まった後は、加速時、減速時に最大加速度、あるいは最大速度、最大モータトルクを越えないように、軌道を進む速度を計算する。この計算方法についても様々な方法が提案されており、例えば次の文献には軌道を計算する方法が示されている。
Ｊ．Ｅ．Ｂｏｂｒｏｗ，Ｓ．Ｄｕｂｏｗｓｋｙ，Ｊ．Ｓ．Ｇｉｂｓｏｎ：
Ｔｉｍｅ−ＯｐｔｉｍａｌＣｏｎｔｒｏｌｏｆＲｏｂｏｔｉｃＭａｎｉｐｕｌａｔｏｒｓＡｌｏｎｇＳｐｅｃｉｆｉｅｄＰａｔｈｓ，
ＴｈｅＩｎｔｅｒｎａｔｉｏｎａｌＪｏｕｒｎａｌｏｆＲｂｏｔｉｃｓＲｅｓｅａｒｃｈ，Ｖｏｌ．４，Ｎｏ．３（１９８５），ｐ３−１７．

このような軌道を計算するためには、逆運動学モデルである第１のメカモデル４０９が必要である。ここで、第１のメカモデル４０９とは、多関節ロボット本体１００ａ，１００ｂのシミュレータであり、逆計算部を有している。モータ制御計算部４０４は、このメカモデル４０９を用いることにより、多関節ロボット本体１００ａ，１００ｂを動作させたときの各関節にかかるトルクや加速度を計算して軌道計算を行い、動作指令データＢを算出する。

センサ制御計算部４０５は、入力機器（センサ）２１５への測定指令データを計算して出力する。

外部機器制御部４０６は、多関節ロボット本体１００ａ，１００ｂのモータ以外の外部機器、例えばモータで制御されるテーブルや、エアーシリンダの命令に対応する部分である。それぞれのデバイスに対する動作指令データを出力する。

次に、補正部４０７は、算出された動作指令データＢを補正する。一般に多関節ロボットは位置再現性よりも位置決め精度が低い。例えば６軸多関節ロボット本体のハンドを一直線上に動かそうとしても、関節と関節の間の長さや、関節の取り付け誤差があるために、うねって動く。こういった原因による運動誤差は再現するので、一度動いた結果をもとに、次に動くときに改善が期待できる。そこで、本第３実施形態では、予め共有メモリ２０３に保存した学習データ４１０を加えるものである。学習データ４１０を求める方法については、公知の技術で可能である。

例えば文献（ロボット工学ハンドブック、日本ロボット学界編、コロナ社（東京）、２００５年、３２０ページ目）などに記述されているように、一度動いた軌道と、目標の軌道との誤差をもとに、次の動作指令を補正する。これによって、目標の軌道に近づけることができる。

次に、動作指令データ出力部４０８は、第１のメカモデル４０９を用いて計算した補正後の動作指令データＢを共有メモリ２０３に出力する。この動作指令データＢは、命令集合Ａが格納されているメモリ領域とは異なる別のメモリ領域に格納される。このような動作指令データＢは命令を処理するごとに生成されるので、複数の動作指令データＢが共有メモリ２０３に格納されることになる。

以上説明した動作が完了すると、第１の処理部２０１Ａは、再び計算開始条件判定部２１１Ａにて他の命令の処理を開始する。

次に、第２の処理部２０２Ｂについて説明する。第２の処理部２０２Ｂは、上記第２実施形態と同様に、動作開始条件判定部２２１Ａ、受信部２２２、指令値補正部２２３、送信部２２４、動作完了条件判定部２２５、第３の状態変数変更部３０３及び第４の状態変数変更部３０４を有している。更に、第２の処理部２０２Ｂは、バッファ書き込み部４１３、全データ書き込み判定部４１４、バッファ部４１５、バッファ読み出し部４１６及びロボット状態監視部４１７を有している。

動作開始条件判定部２２１Ａは、複数有る動作指令データＢ_１，Ｂ_２，Ｂ_３…の中から、それぞれの動作指令データに対応する命令Ｃ_１，Ｃ_２，Ｃ_３…の動作開始条件記述部ｃ_３を参照し、動作開始条件を満たす全ての動作指令データを選択する（４１１）。この時、動作開始条件判定部２２１Ａは、状態変数ｃ_５が計算完了状態であることを条件に入れて探索する（４１２）。これにより、動作開始条件判定部２２１Ａは、動作開始条件を満たす全ての動作指令データを取得する。

動作開始条件を満たすものが１つも無い場合はバッファ読み出し部４１６へ進む。動作開始条件を満たすものが１つでも有る場合は、バッファ書き込み部４１３でバッファ部４１５に書き込む。

このバッファ部４１５は、リングバッファと呼ばれているものであり、デバイス２０７の数だけトラックを有している。つまり、バッファ部４１５は、アーム用モータＭａ_１〜Ｍａ_６に対応した数（６つ）のトラックを有している他、それ以外のデバイスに対応した数のトラックを有している。バッファ部４１５は、書き込み位置と読み出し位置を保持する。

バッファ部４１５は、行列状のメモリ領域であり、行（トラック）は全てのデバイス２０７の数だけあり、トラック幅と呼ぶ。例えば、モータやセンサが全部で３０個あれば３０行のトラック幅を持つ。列方向は時間を表しており、１つの列が同期時間δｔの１区間を表している。例えば同期時間間隔δｔが１ｍｓで４０秒間を蓄えるとすると、列の数は４万列である。バッファ書き込み部４１３は、バッファ部４１５の書き込み位置を基準にして、動作開始条件判定部２２１Ａにより取得された動作指令データの指令値を、各デバイス２０７に対応するトラックに時系列順に連続して書き込む。

また、バッファ読み出し部４１６は、同時刻に出力する指令値を、バッファ部４１５の各トラックから読み出す。全てのデバイス２０７に対する動作指令データの指令値を同時刻に読み出しているのが特徴である。また、読み出した後は、次の読み出し列を１つ、ずらす。もし最終列に達したら最初の列に戻す。つまり、バッファ部４１５は、トラック幅をもったリボンのようにリング状になっている。またこのバッファ部４１５は、最初、全体を「動作指令なし」の状態にリセットしておく。

図１０において、バッファ書き込み部４１３は、動作開始条件判定部２２１Ａにより取得された動作指令データをバッファ部４１５の書込み位置から、該当するトラックに書き込む。指令値を書き込む処理が１列分終了したら、書き込んだデータの回数だけ書き込み位置を進める。書き込み位置がバッファ部４１５の終端に達すると、バッファ部４１５の先頭に戻す。また、一定の時間（同期時間δｔ）以内に動作指令データの全ての指令値の書き込み処理が終了しない場合には、残りの指令値は、次のターンでバッファ部４１５に書き込まれる。書き込み位置は、書き込んだデータ（指令値）の回数だけ進んでいるので連続性は失わない。

このバッファ書き込み部４１３による書き込み処理が完了すると、全データ書込み判定部４１４で動作指令データの書き込みが全て完了したかどうかを判定する。もし完了していなければバッファ読み出し部４１６の処理へ進む。ここで完了していない場合とは、前述した書き込みがまだ一部だけであり、動作指令データにおける指令値が残っている場合である。書き込み処理が完了していれば、第３の状態変数変更部３０３は、状態変数ｃ５を計算完了状態から動作中状態に変更する。

バッファ読み出し部４１６は、バッファ部４１５の読み出し位置から、動作指令データを読み出す。バッファ部４１５からの読み出しは、全てのデバイス２０７に対して同時にデータ（指令値）を取得できる。バッファ読み出し部４１６は、データを読み出した後は、次の２つを行う。１つ目は、読み出し位置のデータをクリアーし、もとの動作指令なしの状態に戻す。２つ目は、読み出し位置を次の位置に更新する。この時、読み出し位置がバッファ部４１５の終端に達していた場合は、バッファ部４１５の先頭に戻す。従って、バッファ書き込み部４１３が１つも動作指令を書き込んでいないときは動作指令なしの状態が読み出される。

このように、共有メモリ２０３に格納していた動作指令データＢを一旦バッファ部４１５に蓄えるので、共有メモリ２０３にアクセスする回数を大幅に減らすことができる。その結果、第１の処理部２０１Ｂと第２の処理部２０２Ｂが両方同時に共有メモリ２０３にアクセスするという、アクセスの衝突回数が少なくなり、動作速度が向上する。

受信部２２２は入力機器２１５（センサ）から測定値を入力する。例えばエンコーダの場合はモータの回転角度位置を受信する。具体的には、各受信器２２２ｂが各入力機器２１５からの信号を入力して、各測定値を受信制御部２２２ａに出力する。受信制御部２２２ａは、各測定値の補正を行い、指令値補正部２２３に出力する。

指令値補正部２２３は、入力した測定値をもとに動作指令データＢを補正する。例えば入力機器２１５が力覚センサの場合、力フィードバック制御が可能になり、入力機器２１５が視覚センサの場合、視覚フィードバック制御が可能になる。

送信部２２４は、動作指令データＢの指令値をデバイス２０７に送信する。デバイス２０７がモータの場合は、モータの関節角度を示す指令値を送信し、デバイス２０７がセンサの場合は信号の入力コマンドを示す指令値を送信する。具体的には、送信制御部２２４ａが所定時刻に各指令値を出力し、各送信器２２４ｂが各出力機器２１４に出力する。各出力機器２１４は、入力した指令値に基づいて動作する。

ロボット状態監視部４１７は、多関節ロボット本体１００ａ，１００ｂの姿勢を監視するものであり、順運動学モデルである第２のメカモデル４１８を用いて多関節ロボット本体１００ａ，１００ｂ（ハンド）の位置姿勢を計算する。このハンドの位置姿勢の計算結果は、例えば予め設定した移動禁止領域に侵入したときに停止させる場合などの判定に使う。

ここで、第１の処理部２０１Ｂは、第１のメカモデル４０９を用い、多関節ロボット本体１００ａ，１００ｂのハンドの位置姿勢から多関節ロボット本体１００ａ，１００ｂの関節角度を示す指令値を計算する。

これに対し、第２の処理部２０２Ｂは多関節ロボット本体１００ａ，１００ｂの状態を監視するのに、第１のメカモデル４０９とは別の第２のメカモデル４１８を用いている。つまり、ロボット状態監視部４１７は、第２のメカモデル４１８を用いて、多関節ロボット本体１００ａ，１００ｂの関節角度から、順計算を実行して多関節ロボット本体１００ａ，１００ｂのハンドの位置を計算する。そして、ロボット状態監視部４１７は、その計算結果を、共有メモリ２０３において命令集合Ａのメモリ領域及び動作指令データＢのメモリ領域とは別のメモリ領域に格納する。

従って、第１の処理部２０１Ｂと第２の処理部２０２Ｂとでそれぞれ独立した２つのメカモデル４０９，４１８があるので互いに干渉することがない。つまり、第２の処理部２０２Ｂが多関節ロボット本体１００ａ，１００ｂ（ハンド）の位置姿勢を保持するメモリ内容を更新するが、命令集合Ａのメモリ領域及び動作指令データＢのメモリ領域とは別のメモリ領域にデータが格納されるので影響し合うことはない。

次に、動作完了条件判定部２２５は、処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。動作の完了の判定方法は、例えば動作指令データの全指令値をバッファ部４１５から読み出し、デバイス２０７に送信したときに動作完了と判定する。または、多関節ロボット本体１００ａ，１００ｂのハンドの位置が予め設定した移動完了領域に侵入したときに動作完了と判定する。動作完了の場合は、第４の状態変数変更部３０４が、状態変数ｃ_５を動作中状態から動作完了状態に変更する。以上の第２の処理部２０２Ｂの処理を予め定めた同期時間δｔ毎に繰返し行う。

したがって、上記第１，第２実施形態と同様に、第１の処理部２０１Ｂに軌道計算処理を実施させて計算結果である動作指令データＢを共有メモリ２０３に格納させているので、第２の処理部２０２Ｂは、複雑な軌道計算処理を行う必要がない。したがって、第１の処理部２０１Ｂにより既に計算された動作指令データＢの指令値を各モータＭａ_１〜Ｍａ_６に同期して出力する同期処理を、多関節ロボット本体１００ａ，１００ｂの動作を停止させることなく実現することができる。ゆえに、多関節ロボット本体１００ａ，１００ｂに人間のような器用な動作を行わせることができる。

［第４実施形態］
次に、本発明の第４実施形態に係るロボット制御装置について説明する。なお、ロボット制御装置の構成は、上記第１〜第３実施形態と同様である。図１１は、ロボット制御装置により生成される動作指令データの例を示す図である。なお、多関節ロボット本体は、多軸であるので、各関節のモータに出力される複数本の指令値列があるが、説明のために１本の指令値列についてのみ図示している。多関節ロボット本体が同じ動作を繰り返す場合、第１回目の動作指令データと第２回目の動作指令データとが同じとなるので、第２回目以降は、動作指令データの計算を省略することが可能である。

そこで、本第４実施形態では、計算部２１２（２１２Ｂ）は、計算開始条件判定部２１１（２１１Ａ）が取得したコマンドから計算される動作指令データが、共有メモリ２０３に格納されている動作指令データＢと一致するか否かを判断する。そして、計算部２１２（２１２Ｂ）は、一致すると判断した場合は、動作指令データの計算を省略する。

ここで、共有メモリ２０３に格納した動作指令データは、一度、動作開始条件判定部２２１（２２１Ａ）に読み出されたとしても、消去せずに残しておく。これにより、動作開始条件判定部２２１（２２１Ａ）は、計算部２１２（２１２Ｂ）に計算が省略されたとしても、共有メモリ２０３に動作指令データが消去されずに残っているので、再び動作指令データを取得することができる。

ここで、各命令Ｃ_１，Ｃ_２，Ｃ_３…は、動作指令データが計算済みか否かを示すフラグを有しており、計算部２１２（２１２Ｂ）は、そのフラグを参照することで、計算済みか否かを判断することができる。このフラグは、計算部２１２（２１２Ｂ）により計算が終了した後、計算部２１２（２１２Ｂ）により変更される。

以上、本第４実施形態では、第１回目の動作で計算された動作指令データは、第２回目以後、再利用されることとなる。このように、複雑な計算である軌道計算が省略されるので、多関節ロボット本体１００をより高速に動作させることができ、第１の処理部２０１（２０１Ａ，２０１Ｂ）の計算能力を温存することができる。

［第５実施形態］
次に、本発明の第５実施形態に係るロボット制御装置について説明する。上記第４実施形態では、図１１に示すように、連続する２つの動作指令データのうち、一方の動作指令データの終端値と、他方の動作指令データの始端値とが一致する場合について説明した。ところが、図１２に示すように、連続する２つの動作指令データのうち、一方の動作指令データの終端値と、他方の動作指令データの始端値とが一致せず、差分Δｆずれている場合もある。これが一致していないと２回目の動作指令データに基づく多関節ロボット本体の動作が滑らかとはならない。そこで、本第５実施形態では、オフセット値を加算する処理を行う。

図１３は、本第５実施形態に係るロボット制御装置２００Ｃの各部の動作の流れを示す図である。なお、図１３では、上記第２実施形態のロボット制御装置２００Ａと比較して動作開始条件判定部の動作が異なるものであり、その他の構成は、同一であるので、同一符号を付して説明を省略する。

ロボット制御装置２００Ｃの第２の処理部２０２Ｃは、上記第２実施形態と異なる動作開始条件判定部２２１Ｃを有している。具体的には、動作開始条件判定部２２１Ｃは、上記第２実施形態の動作開始条件判定部２２１Ａと同様に動作する機能を有する他、オフセット加算部５０１を有している。

つまり、動作開始条件判定部２２１Ｃのオフセット加算部５０１は、前回、共有メモリ２０３から取得した動作指令データ中の最後の指令値（終端値）と、今回、共有メモリ２０３から取得した動作指令データ中の最初の指令値（始端値）との差分Δｆを計算する。そして、動作開始条件判定部２２１Ｃのオフセット加算部５０１は、差分Δｆを初期値としてゼロまで漸次減少するオフセット値を、今回、共有メモリ２０３から取得した動作指令データ中の最初の指令値から時系列順に指令値に加算する補正を行う。

つまり、オフセット加算部５０１は、動作指令データを再利用するとき、動作指令データの先頭データと、現在位置との差を計算し、その差を初期値とし、次第にゼロに減少するオフセット値を、動作指令データに加算する処理を行う。

図１４に、オフセット値を加算した動作指令データの一例を示す。初期値を差分Δｆとし、次第にゼロに減少するオフセット値ｆ（ｔ）を動作指令データに加算すれば、滑らかな動作が可能となる。

オフセット関数ｆ（ｔ）に必要な要件は次の２つある。それは、初期値が差分Δｆに等しいことと、時間の経過とともにゼロに減少することである。例えば、差分をΔｆ、オフセット値がゼロになるまでの時間をＴ、再利用する動作指令データの最初の指令値が出力される時刻からの時間をｔとすると、次のようなオフセット関数ｆ（ｔ）が有効である。

この直線関数は計算量が少ないので計算機への負担が軽い。

ただし、次の関数のほうがより滑らかである。

コサイン関数なので、直線関数の場合よりも両端が滑らかである。したがって、より滑らかに多関節ロボット本体を動作させることができる。

以上、上記第１〜第５実施形態に基づいて本発明を説明したが、本発明はこれに限定されるものではない。駆動部はサーボモータやリニアモータの他、ピエゾアクチュエータや空気圧を利用したシリンダなどであってもよい。一方、センサは、モータの回転角度を測定するエンコーダや変位計、カメラ、力を検出するロードセルなど、あらゆる種類のセンサが適用可能である。

１００…多関節ロボット本体、２００，２００Ａ，２００Ｃ…ロボット制御装置、２０１，２０１Ａ，２０１Ｂ…第１の処理部、２０２，２０２Ａ，２０２Ｂ，２０２Ｃ…第２の処理部、２０３…共有メモリ、２１１，２１１Ａ…計算開始条件判定部、２１２，２１２Ｂ…計算部、２１５…入力機器（センサ）、２２１，２２１Ａ，２２１Ｃ…動作開始条件判定部、２２２…受信部、２２３…指令値補正部、２２４…送信部、３０１…第１の状態変数変更部、３０２…第２の状態変数変更部、３０３…第３の状態変数変更部、４１３…バッファ書き込み部、４１５…バッファ部、４１６…バッファ読み出し部、４１７…ロボット状態監視部、Ｂ…動作指令データ、ｃ_１…動作記述部、ｃ_２…計算開始条件記述部、ｃ_３…動作開始条件記述部、ｃ_５…状態変数、Ｍａ_１〜Ｍａ_６…アーム用モータ（駆動部）

Claims

多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、
共有メモリと、前記共有メモリにアクセス可能に接続された第１の処理部及び第２の処理部と、を備え、
前記共有メモリには、
前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、
前記動作記述部に記述されたコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、
前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、を含む命令が予め複数格納され、
前記第１の処理部は、
前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たしている命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、
前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、を有し、
前記第２の処理部は、
前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たしている命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、
前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とするロボット制御装置。
多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、
共有メモリと、前記共有メモリにアクセス可能に接続された第１の処理部及び第２の処理部と、を備え、
前記共有メモリには、
前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、
前記動作記述部に記述されたコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、
前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、
少なくとも計算開始待ち状態、計算中状態、計算完了状態及び動作中状態のうちのいずれかの状態となる状態変数と、を含む命令が予め複数格納され、
前記第１の処理部は、
前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たし、且つ状態変数が計算開始待ち状態である命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、
前記共有メモリに格納された命令のうち、前記計算開始条件判定部によりコマンドが取得された命令の状態変数を計算中状態に変更する第１の状態変数変更部と、
前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、
前記共有メモリに格納された命令のうち、前記計算部による計算が完了した動作指令データに対応する命令の状態変数を計算完了状態に変更する第２の状態変数変更部と、を有し、
前記第２の処理部は、
前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たし、且つ状態変数が計算完了状態である命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、
前記共有メモリに格納された命令のうち、前記動作開始条件判定部により取得された動作指令データに対応する命令の状態変数を動作中状態に変更する第３の状態変数変更部と、
前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とするロボット制御装置。
前記第２の処理部は、
前記各駆動部に対応した数のトラックを有するバッファ部と、
前記動作開始条件判定部により取得された動作指令データの指令値を、前記各駆動部に対応するトラックに時系列順に連続して書き込むバッファ書き込み部と、
同時刻に出力する指令値を前記バッファ部の各トラックから読み出すバッファ読み出し部と、を有することを特徴とする請求項１又は２に記載のロボット制御装置。
前記計算部は、前記計算開始条件判定部が取得したコマンドから計算される動作指令データが、前記共有メモリに格納されている動作指令データと一致すると判断した場合は、計算を省略することを特徴とする請求項１乃至３のいずれか１項に記載のロボット制御装置。
前記動作開始条件判定部は、前回、前記共有メモリから取得した動作指令データ中の最後の指令値と、今回、前記共有メモリから取得した動作指令データ中の最初の指令値との差分を計算し、前記差分を初期値としてゼロまで漸次減少するオフセット値を、今回、前記共有メモリから取得した動作指令データ中の最初の指令値から時系列順に指令値に加算する補正を行うことを特徴とする請求項４に記載のロボット制御装置。
前記第２の処理部は、前記多関節ロボット本体の姿勢を監視するロボット状態監視部を有し、
前記計算部は、逆運動学モデルを用いて動作指令データを計算し、
前記ロボット状態監視部は、順運動学モデルを用いて前記多関節ロボット本体の位置姿勢を計算することを特徴とする請求項１乃至５のいずれか１項に記載のロボット制御装置。
前記多関節ロボット本体の状態を検知するセンサを備え、
前記第２の処理部は、
前記センサの検知結果を用いて前記動作開始条件判定部により取得された動作指令データの指令値を補正する指令値補正部を有することを特徴とする請求項１乃至６のいずれか１項に記載のロボット制御装置。