ロボット制御装置
【課題】多関節ロボット本体の制御において、軌道計算が複雑で計算の所要時間が不定であっても、複雑な軌道計算と、同期動作とを並行して実行する必要がある。
【解決手段】ロボット制御装置200は、共有メモリ203と、共有メモリ203にアクセス可能に接続された第1の処理部201及び第2の処理部202と、を備えている。第1の処理部201は、多関節ロボット本体を目標位置姿勢に動作させるコマンドから各アーム用モータに出力する指令値の連なりを示す動作指令データBを計算し、計算結果である動作指令データBを共有メモリ203に格納する軌道計算処理を行う。第2の処理部202は、共有メモリ203に格納された動作指令データBを取得して、指令値を各アーム用モータに一定の時間間隔で同期して出力する同期処理を行う。
【解決手段】ロボット制御装置200は、共有メモリ203と、共有メモリ203にアクセス可能に接続された第1の処理部201及び第2の処理部202と、を備えている。第1の処理部201は、多関節ロボット本体を目標位置姿勢に動作させるコマンドから各アーム用モータに出力する指令値の連なりを示す動作指令データBを計算し、計算結果である動作指令データBを共有メモリ203に格納する軌道計算処理を行う。第2の処理部202は、共有メモリ203に格納された動作指令データBを取得して、指令値を各アーム用モータに一定の時間間隔で同期して出力する同期処理を行う。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部を制御するロボット制御装置に関する。
【背景技術】
【0002】
多軸の多関節ロボット本体においては、各関節の角度を同期させて変えながら動作させることが必要である。この同期がずれると、多関節ロボット本体の手先位置の軌道がずれることになるからである。したがって、ロボット制御装置は、多関節ロボット本体の各関節を駆動する複数の駆動部を同期制御している(特許文献1参照)。なお、多関節ロボット本体の目標位置姿勢から各関節角度を計算するのを逆問題、反対に各関節の角度から多関節ロボット本体の位置姿勢を計算するのを順問題と呼び、このような問題を扱う分野を機構学と呼んでいる。
【0003】
この従来のロボット制御装置は、性質の異なる2つのコンピュータを組み合わせて構成されている。一方のコンピュータは汎用コンピュータであり、他方のコンピュータはリアルタイムコンピュータである。そして、汎用コンピュータは、リアルタイムコンピュータに多関節ロボット本体の目標位置姿勢を示すコマンドを出力する。リアルタイムコンピュータは、逆運動学に基づき、入力したコマンドから、各駆動部の関節角度を示す指令値を時系列に並べた動作指令データを計算することができる。
【0004】
各駆動部を予定時刻に確実に動作させるには、計算した指令値を多関節ロボット本体の各駆動部に一定の時間間隔(例えば5ms)で同期して出力する必要がある。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特表2004−524171号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、従来のロボット制御装置では、多関節ロボット本体に人間のように器用な動作をさせることはできなかった。具体的に説明すると、人間のような器用な動作には非常に高度で複雑なアルゴリズムが隠されており、ロボット制御系は高度で複雑なアルゴリズムに基づく計算ができなければならない。そして、多関節ロボット本体の目標位置姿勢が単純な場合でも、多関節ロボット本体の各関節角度は複雑な軌道を描く。つまり、逆運動学に基づく計算処理は、順運動学に基づく計算処理よりも複雑で時間を要する。
【0007】
したがって、そのような複雑な軌道計算を行う場合、計算の所要時間は同期時間(指令値を出力する時間間隔)よりも長くなってしまうことがある。また計算条件によってもこの所要時間が変わることが多い。つまり軌道計算の所要時間が不定である。この所要時間は例えば1sといった、同期時間(例えば5ms)とは比べられないほど長い時間が想定される。
【0008】
ところが、特許文献1に記載されているロボット制御装置では、リアルタイムコンピュータにより、複雑な軌道計算と同期動作とを並行して実行する必要がある。したがって、リアルタイムコンピュータは、同期時間内に軌道計算を完了しなければならないが、軌道計算の所要時間が不定であるので、同期時間内に軌道計算が完了しない場合がある。このような場合、リアルタイムコンピュータは、軌道計算を全て完了させてから同期動作を行う必要があり、軌道計算が終了するまで多関節ロボット本体の動作を停止させておかなければならない。これでは、多関節ロボット本体は滑らかな動作を行うことができず、人間のような器用な動作を実現することはできない。
【0009】
そこで、本発明は、軌道計算が複雑で計算の所要時間が不定であっても、複雑な軌道計算と、同期動作とを並行して実行することが可能なロボット制御装置を提供することを目的とするものである。
【課題を解決するための手段】
【0010】
本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、共有メモリと、前記共有メモリにアクセス可能に接続された第1の処理部及び第2の処理部と、を備え、前記第1の処理部は、前記多関節ロボット本体を目標位置姿勢に動作させるコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算し、計算結果である動作指令データを前記共有メモリに格納する軌道計算処理を行い、前記第2の処理部は、前記共有メモリに格納された動作指令データを取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する同期処理を行うことを特徴とする。
【発明の効果】
【0011】
本発明によれば、第1の処理部に軌道計算処理を実施させて計算結果である動作指令データを共有メモリに格納させているので、第2の処理部は、複雑な軌道計算処理を行う必要がない。そして、第2の処理部は、複雑な軌道計算処理を行わないので、第1の処理部により既に計算された動作指令データの指令値を各駆動部に同期して出力する同期処理を、多関節ロボット本体の動作を停止させることなく実現することができる。したがって、多関節ロボット本体に人間のような器用な動作を行わせることができる。
【図面の簡単な説明】
【0012】
【図1】本発明の第1実施形態に係るロボット装置の概略構成を示す説明図である。
【図2】本発明の第1実施形態に係るロボット装置の概略構成を示すブロック図である。
【図3】本発明の第1実施形態に係るロボット制御装置の各部の動作の流れを示す説明図である。
【図4】本発明の第1実施形態に係る共有メモリに格納される各命令の構成を示す説明図である。
【図5】多関節ロボット本体の各関節に出力する動作指令データの説明図であり、(a)は各関節に出力する動作指令データを時系列に並べた場合のタイムチャート、(b)は(a)にて円で囲った部分の指令値の列を示す拡大図である。
【図6】3つの命令を使った動作例を示すタイムチャート図である。
【図7】本発明の第2実施形態に係るロボット制御装置の動作の流れを示す説明図である。
【図8】本発明の第2実施形態に係るロボット制御装置の共有メモリに格納される各命令の構成を示す説明図である。
【図9】本発明の第3実施形態に係るロボット制御装置の第1の処理部の動作の流れを示す説明図である。
【図10】本発明の第3実施形態に係るロボット制御装置の第1の処理部の動作の流れを示す説明図である。
【図11】本発明の第4実施形態に係るロボット制御装置により生成される動作指令データの例を示す図である。
【図12】比較例としてのロボット制御装置により生成される動作指令データの例を示す図である。
【図13】本発明の第5実施形態に係るロボット制御装置の動作の流れを示す説明図である。
【図14】本発明の第4実施形態に係るロボット制御装置により生成される、オフセット値を加算した動作指令データの一例を示す図である。
【発明を実施するための形態】
【0013】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【0014】
[第1実施形態]
図1は、本発明の第1実施形態に係るロボット装置の概略構成を示す説明図である。ロボット装置50は、多関節ロボット本体100と、多関節ロボット本体100を制御するロボット制御装置200と、を備えている。
【0015】
多関節ロボット本体100は、複数のリンク101〜105と、先端リンク105に接続されたエンドエフェクタとしてのハンド106と、を有しており、リンク101〜105及びハンド106が各関節J1〜J6で揺動又は回動可能に連結されている。本実施形態では、多関節ロボット本体100は、自由度が6の6軸多関節ロボットである。
【0016】
図2は、ロボット装置50の概略構成を示すブロック図である。各関節J1〜J6には、各関節J1〜J6を駆動する複数の駆動部としてのアーム用モータMa(Ma1〜Ma6)が設けられ、ハンド106には、ハンド用モータMbが設けられている。また、図示は省略するが、多関節ロボット本体100は、直動アクチュエータやシリンダ等を有している。これらアーム用モータMa、ハンド用モータMb、直動アクチュエータ及びシリンダが出力機器214である。
【0017】
また、各関節J1〜J6には、各アーム用モータMaの回転角度を検知するアーム用エンコーダEaが設けられている。また、ハンド106には、ハンド用モータMbの回転角度を検知するハンド用エンコーダEbが設けられている。ハンド106と先端リンク105との間には、ハンド106に作用する力及びトルクを検知する力覚センサSaが設けられている。また、図示は省略するが、各モータMa,Mbに供給する電流を検知する電流センサ、各関節J1〜J6のトルクを検知するトルクセンサが、多関節ロボット本体100に設けられている。これらエンコーダEa,Eb、力覚センサSa、電流センサ及びトルクセンサが、多関節ロボット本体100の状態を検知するセンサとしての入力機器215である。以下、入力機器215及び出力機器214を総称して、デバイス207という。
【0018】
ロボット制御装置200は、大別すると、第1の処理部201と、第2の処理部202と、共有メモリ203と、を備えている。第1の処理部201及び第2の処理部202は、共有メモリ203にアクセス可能に接続されている。本実施形態では、ロボット制御装置200は、1つのデュアルコアCPUを有し、1つのデュアルコアCPUにおける各コアが、第1の処理部201及び第2の処理部202のそれぞれとして機能するように構成されている。なお、ロボット制御装置200が、シングルコアCPUを2つ有し、各CPUが第1の処理部201及び第2の処理部202のそれぞれとして機能するように構成してもよい。
【0019】
本第1実施形態では、第1の処理部201は、多関節ロボット本体100を目標位置姿勢に動作させるコマンドを入力し、この入力したコマンドから各モータMa1〜Ma6に出力する関節角度の指令値を時系列に連ねた動作指令データ(軌道データ)を計算する。そして、計算結果である動作指令データを共有メモリ203に格納する。このように、第1の処理部201は、逆運動学に基づく計算所要時間の不定な軌道計算処理を行う。
【0020】
第2の処理部202は、共有メモリ203に格納された動作指令データを取得して、指令値を各モータMa1〜Ma6に一定の時間間隔で同期して出力する同期処理を行う。つまり、本第1実施形態では、複雑で計算に要する時間が不定となる軌道計算処理を、第1の処理部201で行い、各モータMa1〜Ma6の同期処理は、第2の処理部202が行う。
【0021】
以下、各処理部201,202及び共有メモリ203について詳細に説明する。第1の処理部201は、計算開始条件判定部211と、計算部212と、を有している。第2の処理部202は、動作開始条件判定部221と、受信部222と、指令値補正部223と、送信部224と、動作完了条件判定部225と、を有している。
【0022】
図3は、ロボット制御装置200の各部の動作の流れを示す説明図である。図3に示すように、まず、共有メモリ203には、予め複数の命令C1,C2,C3…からなる命令集合Aが格納されている。つまり、共有メモリ203には、多関節ロボット本体100の関節角度を決めるための命令が予め複数格納されている。命令集合Aは、例えばテキスト形式のファイルで、ユーザがロボットの動作を記述する目的で準備する。通常は命令を多数組み合わせてロボットの動作を実現する。図4に示すように、各命令Cn(nは正の整数)は、動作記述部c1、計算開始条件記述部c2、動作開始条件記述部c3、及び動作完了条件記述部c4で構成されている。
【0023】
まず、共有メモリ203に格納されている命令のうち、各モータMa1〜Ma6に駆動動作を行わせるための命令について説明する。動作記述部c1には、多関節ロボット本体100(より具体的には、多関節ロボット本体100のハンド106)を目標位置姿勢に動作させるためのコマンドが記述されている。ここで、コマンドは、「ジョブ」あるいは「Instruction」と同じ意味である。コマンドは、多関節ロボット本体100のハンド106の目標位置姿勢を直行座標系で指定するものである。即ち、ハンド106の目標位置姿勢は、直交座標系で(X,Y,Z,θX,θY,θZ)で指定される。そして、例えばハンド106を、X=100mm、Y=200mmの座標位置に移動せよ、といったコマンドが動作記述部c1に記述されている。また例えば、ハンド106の現在の位置姿勢から相対的な位置姿勢を指定するコマンドが動作記述部c1に記述されている。
【0024】
計算開始条件記述部c2には、計算部212により軌道計算を開始する条件が記述されている。例えば、他の命令に含まれているコマンドを実行するときにそのコマンドによりハンド106が到達する最終目標位置が確定したら計算を開始する、といった条件が、計算開始条件記述部c2に記述されている。また例えば、多関節ロボット本体100の動作が停止した時に計算を開始する、センサの値が確定した時に計算を開始する、といった条件が、計算開始条件記述部c2に記述されている。
【0025】
動作開始条件記述部c3には、コマンドを実行するための各モータMa1〜Ma6の動作を開始する条件が記述されている。例えば2本のアームを協調させて作業する場合、同時に動作を開始する必要があるので、例えば動作開始条件は2本分の軌道の計算を待つという条件が動作開始条件記述部c3に記述されている。
【0026】
動作完了条件記述部c4には、各モータMa1〜Ma6の動作を完了させる条件が記述されている。例えばモータMa1〜Ma6の軌道が終端に達した場合(多関節ロボット本体100が目標位置姿勢に達した場合)にモータMa1〜Ma6の動作を完了させる、といった条件が動作完了条件記述部c4に記述されている。
【0027】
なお、軌道計算ではないが、モータMbや不図示の直動アクチュエータ、シリンダ等に駆動動作を行わせるための命令についても同様に構成されており、共有メモリ203に予め格納されている。
【0028】
また、共有メモリ203には、入力機器(センサ)215に測定動作を行わせるための命令も格納されている。以下、入力機器215に測定動作を行わせるための命令について説明する。
【0029】
動作記述部c1には、入力機器215への測定要求が記述される。計算開始条件記述部c2には、入力機器215が測定する前に必要な測定準備を開始する条件が記述される。例えば入力機器215がハンド106に取り付けられた力覚センサSaや視覚センサである場合、ハンド106が目標位置姿勢になるまで待つといった条件が記述される。
【0030】
動作開始条件記述部c3には、入力機器215に測定動作を開始させる条件が記述される。入力機器215が視覚センサの場合、実際に視覚センサが測定を開始できる条件が記述される。例えば他のロボット装置が測定対象を把持している場合、そのロボット装置が停止するまで待つといった条件が記述される。動作完了条件記述部c4には、入力機器215による測定動作を完了させる条件が記述されている。例えば、測定値が安定したかどうかを判定する条件が記述されている。
【0031】
次に、第1の処理部201について説明する。計算開始条件判定部211は、共有メモリ203に格納された命令集合Aにおける複数の命令C1,C2,C3…の計算開始条件記述部c2を順次参照する。そして、計算開始条件判定部211は、計算開始条件を満たしている命令の動作記述部c1に記述されているコマンドを共有メモリ203から1つ取得する。
【0032】
計算開始条件判定部211は、もし、計算開始条件を満たすものが無い場合は、条件を満たすまで待つ。従って最初に起動するときは、複数の命令C1,C2,C3…のうち、少なくとも1つ計算開始条件を満たす命令が必要である。また、図示していないが、外部から命令の中の計算開始条件を変更することによって、起動させることもできる。
【0033】
計算部212は、計算開始条件判定部211にて取得されたコマンドを受け取り、そのコマンドから各モータMa1〜Ma6に出力する関節角度の指令値を時系列に連ねた動作指令データBを逆運動学モデルに基づいて計算する。そして、計算部212は、計算結果である動作指令データBを共有メモリ203に格納する。共有メモリ203には、計算部212で計算された動作指令データBが順次格納されるので、複数の動作指令データBが格納されることとなる。
【0034】
以下、動作指令データBについて、図5に示す具体例を参照しながら説明する。図5(a)は、各関節に出力する動作指令データBを時系列に並べた場合のタイムチャートである。図5(a)に示す多関節ロボット本体100を位置姿勢P1から位置姿勢P3に位置姿勢P2を経由して移動させよ、というコマンドを計算開始条件判定部211が取得した場合、計算部212は、そのコマンドから動作指令データBを計算する。この動作指令データBは、各モータMa1〜Ma6に出力する各関節J1〜J6の関節角度を示す指令値を時系列に連ねたものである。
【0035】
図5(b)は、図5(a)にて円で囲った部分の指令値の列を示す拡大図である。図5(b)に示すように、関節J1におけるモータMa1に対応して、指令値d11〜d19が計算され、関節J6におけるモータMa6に対応して、指令値d61〜d69が計算される。そして、各指令値は、第2の処理部202にて、一定の時間間隔δt(つまり同期時間δt)で各モータMa1〜Ma6に出力される。以上のように、第1の処理部201は、計算が複雑で計算所要時間が不定な軌道計算処理を行う。以上の計算部212の処理動作が終了したら、再び計算開始条件判定部211による動作を実施する。
【0036】
なお、計算部212は、ハンド用モータMbや直動アクチュエータ、シリンダ等の軌道計算を行わないものについても、入力したコマンドに基づいて、指令値からなる動作指令データを計算する。また、入力機器215の場合も同様である。モータMa1〜Ma6の時のように軌道は計算しないが、測定する前の計算を行う。例えば力覚センサSaの場合、多関節ロボット本体100(ハンド106)が所定の位置にきた時の値を計算して、その計算結果である測定指令データを共有メモリ203に格納する。
【0037】
次に、第2の処理部202について説明する。動作開始条件判定部221は、共有メモリ203に格納された各命令C1,C2,C3…の動作開始条件記述部c3を参照する。そして、動作開始条件判定部221は、共有メモリ203に格納された動作指令データB…のうち、動作開始条件を満たしている命令に対応する動作指令データBを共有メモリ203から取得する。このとき、動作開始条件を満たしている命令が複数ある場合には、動作開始条件判定部221は、その複数の動作指令データを共有メモリ203から取得する。なお、動作開始条件判定部221は、測定指令データについても動作開始条件を満たしていれば、対応する測定指令データを共有メモリ203から取得する。
【0038】
受信部222は、入力機器215(センサ)から検知結果である信号を入力する。そして、動作開始条件判定部221が測定指令データを取得していた場合には、検知結果を補正する。例えば力覚センサSaの場合は、実際に測定する時に測定指令値データの指令値を毎回差し引くことによって、重力の影響を軽減する。また、例えば視覚センサの場合は、多関節ロボット本体100の関節角度から視覚センサが取り付いている3次元的な位置を計算し、実際に測定する時の補正に使用する。
【0039】
指令値補正部223は、動作開始条件判定部221にて取得された動作指令データBの各モータMa1〜Ma6に同期して出力するための各指令値を、入力機器215からの信号を用いて補正をし、送信部224に出力する。これにより、例えば入力機器215が力覚センサSaの場合、力フィードバック制御が可能になり、また、入力機器215が視覚センサの場合、視覚フィードバック制御が可能になる。
【0040】
補正量を計算する方法は既知である。例えば文献(ロボット工学ハンドブック、日本ロボット学界編、コロナ社(東京)、2005年、287ページ)などに記述されているインピーダンス制御、あるいはコンプライアンス制御等の力制御が利用可能である。
【0041】
送信部224は、共有メモリ203から取得され補正された各指令値を入力し、各指令値を各モータMa1〜Ma6に一定の時間間隔δt、つまり同期時間δtで同期して出力する。この同期時間δtはミリ秒、例えば5ミリ秒といった短い時間である。動作完了条件判定部225は処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。そして、共有メモリ203に、動作結果Dが格納される。以上説明した各部221〜225の動作は、同期時間δt毎に繰返し行なわれる。
【0042】
以上の構成により、例えば命令C1,C2,C3の順に軌道計算処理を行い、命令C1,C2を同時に実行し、命令C2の実行終了後、命令C3を実行する場合について説明する。図6に、3つの命令を使った動作例のタイムチャート図を示す。
【0043】
第1の処理部201の計算部212は、共有メモリ203から取得された命令C1のコマンドに基づいて命令C1に対応する動作指令データを計算して、共有メモリ203に格納する。次いで、計算部212は、共有メモリ203から取得された命令C2のコマンドに基づいて命令C2に対応する動作指令データを計算して、共有メモリ203に格納する。次いで、計算部212は、共有メモリ203から取得された命令C3のコマンドに基づいて命令C3に対応する動作指令データを計算して、共有メモリ203に格納する。このように、第1の処理部201の計算部212は、各コマンドを1つずつ順次計算して共有メモリ203に計算結果を格納する。
【0044】
このとき、第2の処理部202は、例えば第1の処理部201の計算部212が、命令C3のコマンドから動作指令データを演算している途中であっても、第1の処理部201の計算動作とは独立して動作することができる。従って、第2の処理部202の動作開始条件判定部221は、第1の処理部201による命令C3のコマンドに基づく計算終了を待たずに共有メモリ203にアクセスし、命令C1の動作指令データと、命令C2の動作指令データとを取得している。そして、各命令C1,C2の動作を実行している。その後、第2の処理部202は、命令C3の動作を実行している。このように、本第1実施形態では、軌道計算処理と同期処理とを並行して行うことが可能となり、ロボット装置50の動作時間の短縮につながる。特に、ロボット装置50が生産装置に組み込まれる場合には、動作時間の短縮は生産物のコストを下げることができるので非常に重要である。
【0045】
以上、本第1実施形態によれば、第1の処理部201に軌道計算処理を実施させて計算結果である動作指令データBを共有メモリ203に格納させているので、第2の処理部202は、複雑な軌道計算処理を行う必要がない。したがって、たとえ第1の処理部201で計算時間が多くかかっても、第2の処理部202は、逆運動学に基づく軌道演算処理を行わないので、動作開始条件を満たした直後に各モータMa1〜Ma6を動かすことができる。
【0046】
そして、第2の処理部202は、複雑な軌道計算処理を行わず、第1の処理部201により既に計算された動作指令データBの指令値を各モータMa1〜Ma6に同期して出力する同期処理を行うようにしている。したがって、多関節ロボット本体100の動作を停止させることなく実現することができ、多関節ロボット本体100を滑らかに動作させることができるので、多関節ロボット本体100に人間のような器用な動作を行わせることができる。
【0047】
また、本第1実施形態では、第1の処理部201が計算開始条件判定部211を有しているので、計算部212は、適したタイミングで計算を開始することができる。また、第2の処理部202が、動作開始条件判定部221を有しているので、適したタイミングで動作を開始することができる。つまり、軌道計算処理及び同期処理のそれぞれの処理が、命令C1,C2,C3…に従ってタイミングよく動作させることができるので、他の動作機器(例えば把持対象物が載置される移動テーブル)との同期をとることもできる。
【0048】
以上の説明では、1つのロボット制御装置200で1台の多関節ロボット本体100を制御する場合について説明したが、1つのロボット制御装置200で複数台の多関節ロボット本体を制御する場合についても制御動作は同じである。つまり、共有メモリ203には、各多関節ロボット本体に対する命令が予め格納されており、各処理部201,202は、各命令に基づき、軌道計算処理及び同期処理のそれぞれを行う。特に、各多関節ロボット本体を同時に動作させる場合には、例えば図6に示すように、命令C1と命令C2とを同時に出力することができる。したがって、複数台の多関節ロボット本体を、停止させることなく同期して動作させることができる。これにより、複数台の多関節ロボット本体を滑らかに動作させることができ、複数台の多関節ロボット本体に人間のような器用な動作を行わせることができる。
【0049】
[第2実施形態]
次に、本発明の第2実施形態に係るロボット制御装置について説明する。図7は、ロボット制御装置200Aの動作の流れを示す説明図であり、図8は、共有メモリに格納される各命令の構成を示す説明図である。なお、本第2実施形態において、上記第1実施形態と同様の構成については、同一符号を付して説明を省略する。
【0050】
図8に示すように、共有メモリ203に予め格納される各命令Cnは、上記第1実施形態で説明した、動作記述部c1、計算開始条件記述部c2、動作開始条件記述部c3、及び動作完了条件記述部c4の他、更に状態変数c5を有している。本第2実施形態では、状態変数c5は、計算開始待ち状態、計算中状態、計算完了状態、動作中状態及び動作完了状態のうちのいずれかの状態を意味する値となる。最初に起動するときには状態変数c5を計算開始待ち状態にセットしておく。
【0051】
計算開始待ち状態とは、軌道の計算開始を待っている状態である。計算中状態とは、図7に示す計算部212が軌道の計算を開始し、計算中である状態である。計算完了状態とは、計算部212による計算が完了し、共有メモリ203に動作指令データBが保存された状態である。動作中状態とは、第2の処理部202Aが動作を開始し、動作中である状態であり、動作完了状態とは、第2の処理部202Aが動作を完了している状態である。
【0052】
また、図7に示す第1の処理部201Aは、計算開始条件判定部211A及び計算部212の他、第1の状態変数変更部301及び第2の状態変数変更部302を有している。計算開始条件判定部211Aは、共有メモリ203に格納された各命令C1,C2,C3…の計算開始条件記述部c2を参照する。そして、計算開始条件判定部211Aは、計算開始条件を満たし、且つ状態変数c5が計算開始待ち状態である命令を抽出する。そして、計算開始条件判定部211Aは、その命令に含まれる動作記述部c1に記述されているコマンドを共有メモリ203から1つ取得する。
【0053】
ここで、計算開始条件判定部211Aは、もし該当する命令が存在しなければ、計算条件が満たされるまで待つ。従って最初に起動するときは、少なくとも1つの命令が、計算開始条件を満足していなければならない。また、全部の命令が計算開始条件を満足しない場合でも、図示していない外部のプログラムから、この計算開始条件を変更して、起動させることは容易である。例えば、状態変数を開始待ち状態に変更すればよい。
【0054】
次に、第1の状態変数変更部301は、共有メモリ203に格納された命令C1,C2,C3…のうち、計算開始条件判定部211Aによりコマンドが取得された命令の状態変数c5を、計算開始待ち状態から計算中状態に変更する。そして、計算部212は、計算開始条件判定部211Aにより取得されたコマンドから動作指令データBを計算し、この動作指令データBを共有メモリ203に格納する。
【0055】
つまり、計算開始条件判定部211Aは、状態変数c5が計算開始待ち状態以外の状態である命令に含まれるコマンドを取得することはないので、重複計算となるのを回避することができる。
【0056】
第2の状態変数変更部302は、計算部212で計算が完了した場合、共有メモリ203に格納された命令C1,C2,C3…のうち、計算部212による計算が完了した動作指令データに対応する命令の状態変数c5を変更する。具体的には、状態変数c5を、計算中状態から計算完了状態に変更する。以上の第2の状態変数変更部302による変更動作が終了したら、再び計算開始条件判定部211Aによる動作を実施する。
【0057】
次に、第2の処理部202Aについて説明する。第2の処理部202Aは、動作開始条件判定部221A、受信部222、指令値補正部223、送信部224及び動作完了条件判定部225の他、第3の状態変数変更部303及び第4の状態変数変更部304を有している。
【0058】
動作開始条件判定部221Aは、共有メモリ203に格納された各命令C1,C2,C3…の動作開始条件記述部c3を参照する。そして、動作開始条件判定部221Aは、共有メモリ203に格納された動作指令データB…のうち、動作開始条件を満たし、且つ状態変数c5が計算完了状態である命令に対応する動作指令データBを共有メモリ203から取得する。このとき、動作開始条件を満たし、且つ計算完了状態である命令が複数ある場合には、動作開始条件判定部221Aは、その条件を満たした全ての動作指令データBを共有メモリ203から取得する。
【0059】
第3の状態変数変更部303は、共有メモリ203に格納された命令のうち、動作開始条件判定部221Aにより取得された動作指令データBに対応する命令の状態変数c5を計算完了状態から動作中状態に変更する。これにより、動作中の命令を重複して実行するのを回避することができ、制御が安定する。
【0060】
受信部222は、入力機器215(センサ)から検知結果である信号を入力する。そして、動作開始条件判定部221が測定指令データを取得していた場合には、検知結果を補正する。例えば力覚センサSaの場合は、実際に測定する時に測定指令値データの指令値を毎回差し引くことによって、重力の影響を軽減する。また、例えば視覚センサの場合は、多関節ロボット本体100の関節角度から視覚センサが取り付いている3次元的な位置を計算し、実際に測定する時の補正に使用する。
【0061】
指令値補正部223は、動作開始条件判定部221にて取得された動作指令データBの各モータMa1〜Ma6に同期して出力するための各指令値を、入力機器215からの信号を用いて補正をし、送信部224に出力する。これにより、例えば入力機器215が力覚センサSaの場合、力フィードバック制御が可能になり、また、入力機器215が視覚センサの場合、視覚フィードバック制御が可能になる。
【0062】
送信部224は、共有メモリ203から取得され補正された各指令値を入力し、各指令値を各モータMa1〜Ma6に一定の時間間隔δt、つまり同期時間δtで同期して出力する。この同期時間δtはミリ秒、例えば5ミリ秒といった短い時間である。動作完了条件判定部225は処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。そして、共有メモリ203に、動作結果Dが格納される。
【0063】
第4の状態変数変更部304は、共有メモリ203に格納された命令のうち、動作が完了したときの動作指令データBに対応する命令の状態変数c5を動作中状態から動作完了状態に変更する。以上説明した各部221A〜304の動作は、同期時間δt毎に繰返し行なわれる。なお、共有メモリ203に格納された命令が入力機器215に対するものである場合も同様に動作する。
【0064】
以上、本第2実施形態では、第1の処理部201Aに軌道計算処理を実施させて計算結果である動作指令データBを共有メモリ203に格納させているので、第2の処理部202Aは、複雑な軌道計算処理を行う必要がない。したがって、たとえ第1の処理部201Aで計算時間が多くかかっても、第2の処理部202Aは、逆運動学に基づく軌道演算処理を行わないので、動作開始条件を満たした直後に各モータMa1〜Ma6を動かすことができる。
【0065】
そして、第2の処理部202Aは、複雑な軌道計算処理を行わず、第1の処理部201により既に計算された動作指令データBの指令値を各モータMa1〜Ma6に同期して出力する同期処理を行うようにしている。したがって、多関節ロボット本体100の動作を停止させることなく実現することができ、多関節ロボット本体100を滑らかに動作させることができるので、多関節ロボット本体100に人間のような器用な動作を行わせることができる。
【0066】
また、本第2実施形態では、第1の処理部201Aが計算開始条件判定部211Aを有しているので、計算部212は、適したタイミングで計算を開始することができる。また、第2の処理部202Aが、動作開始条件判定部221Aを有しているので、適したタイミングで動作を開始することができる。つまり、軌道計算処理及び同期処理のそれぞれの処理が、命令C1,C2,C3…に従ってタイミングよく動作させることができるので、他の動作機器(例えば把持対象物が載置される移動テーブル)との同期をとることもできる。
【0067】
また、本第2実施形態では、各命令C1,C2,C3…に状態変数c5が含まれているので、状態変数c5を監視することで、第1の処理部201A及び第2の処理部202Aによる制御動作が安定する。
【0068】
以上の説明では、1つのロボット制御装置200Aで1台の多関節ロボット本体100を制御する場合について説明したが、1つのロボット制御装置200Aで複数台の多関節ロボット本体を制御する場合についても制御動作は同じである。つまり、共有メモリ203には、各多関節ロボット本体に対する命令が予め格納されており、各処理部201A,202Aは、各命令に基づき、軌道計算処理及び同期処理のそれぞれを行う。特に、各多関節ロボット本体を同時に動作させる場合には、例えば図6に示すように、命令C1と命令C2とを同時に出力することができる。したがって、複数台の多関節ロボット本体を、停止させることなく同期して動作させることができる。これにより、複数台の多関節ロボット本体を滑らかに動作させることができ、複数台の多関節ロボット本体に人間のような器用な動作を行わせることができる。
【0069】
[第3実施形態]
次に、本発明の第3実施形態に係るロボット制御装置について説明する。図9はロボット制御装置の第1の処理部の各部の動作の流れを示す説明図である。図10はロボット制御装置の第2の処理部の各部の動作の流れを示す説明図である。なお、上記第1,第2実施形態と同様の構成については、同一符号を付して説明を省略する。ここで、上記第1,第2実施形態では、多関節ロボット本体100が1台の場合について説明したが、2台以上であっても制御動作は同じである。本第3実施形態では、2台の多関節ロボット本体100a,100bを対象としているが、制御動作は多関節ロボット本体が1台であっても、3台以上であっても変わらない。
【0070】
本第3実施形態では、ロボット制御装置は、図9に示す第1の処理部201Bと、図10に示す第2の処理部202Bと、を備えている。まず、第1の処理部201Bについて説明する。計算開始条件判定部211Aは、共有メモリ203に格納されている命令集合Aの中から、それぞれの命令C1,C2,C3…の計算開始条件記述部c2を参照し、計算開始条件を満たす命令を選択する(401)。この時、計算開始条件判定部211Aは、状態変数c5が計算開始待ち状態であることを条件に入れて探索する(402)。もし該当する命令が存在しなければ、再び共有メモリ203に格納されている命令集合Aの中から、それぞれの命令C1,C2,C3…の計算開始条件記述部c2を参照し、計算開始条件を満たす命令を選択する。
【0071】
計算開始条件判定部211Aがコマンドを1つ取得した後、第1の状態変数変更部301が状態変数c5を計算開始待ち状態から計算中状態に変更する。
【0072】
計算部212Bは、計算開始条件判定部211Aが取得したコマンドに基づいて動作指令データBを計算する。計算部212Bは、処理選択部403、モータ制御計算部404、センサ制御計算部405、外部機器制御部406、補正部407、及び動作指令データ出力部408を有している。
【0073】
処理選択部403は、コマンドの内容を解釈する。モータ制御計算部404は、多関節ロボット本体100a,100bの関節のアーム用モータMa1〜Ma6に対する軌道を計算する。軌道が決まった後は、加速時、減速時に最大加速度、あるいは最大速度、最大モータトルクを越えないように、軌道を進む速度を計算する。この計算方法についても様々な方法が提案されており、例えば次の文献には軌道を計算する方法が示されている。
J.E.Bobrow, S.Dubowsky, J.S.Gibson:
Time−Optimal Control of Robotic Manipulators Along Specified Paths,
The International Journal of Rbotics Research, Vol.4, No.3(1985), p3−17.
【0074】
このような軌道を計算するためには、逆運動学モデルである第1のメカモデル409が必要である。ここで、第1のメカモデル409とは、多関節ロボット本体100a,100bのシミュレータであり、逆計算部を有している。モータ制御計算部404は、このメカモデル409を用いることにより、多関節ロボット本体100a,100bを動作させたときの各関節にかかるトルクや加速度を計算して軌道計算を行い、動作指令データBを算出する。
【0075】
センサ制御計算部405は、入力機器(センサ)215への測定指令データを計算して出力する。
【0076】
外部機器制御部406は、多関節ロボット本体100a,100bのモータ以外の外部機器、例えばモータで制御されるテーブルや、エアーシリンダの命令に対応する部分である。それぞれのデバイスに対する動作指令データを出力する。
【0077】
次に、補正部407は、算出された動作指令データBを補正する。一般に多関節ロボットは位置再現性よりも位置決め精度が低い。例えば6軸多関節ロボット本体のハンドを一直線上に動かそうとしても、関節と関節の間の長さや、関節の取り付け誤差があるために、うねって動く。こういった原因による運動誤差は再現するので、一度動いた結果をもとに、次に動くときに改善が期待できる。そこで、本第3実施形態では、予め共有メモリ203に保存した学習データ410を加えるものである。学習データ410を求める方法については、公知の技術で可能である。
【0078】
例えば文献(ロボット工学ハンドブック、日本ロボット学界編、コロナ社(東京)、2005年、320ページ目)などに記述されているように、一度動いた軌道と、目標の軌道との誤差をもとに、次の動作指令を補正する。これによって、目標の軌道に近づけることができる。
【0079】
次に、動作指令データ出力部408は、第1のメカモデル409を用いて計算した補正後の動作指令データBを共有メモリ203に出力する。この動作指令データBは、命令集合Aが格納されているメモリ領域とは異なる別のメモリ領域に格納される。このような動作指令データBは命令を処理するごとに生成されるので、複数の動作指令データBが共有メモリ203に格納されることになる。
【0080】
以上説明した動作が完了すると、第1の処理部201Aは、再び計算開始条件判定部211Aにて他の命令の処理を開始する。
【0081】
次に、第2の処理部202Bについて説明する。第2の処理部202Bは、上記第2実施形態と同様に、動作開始条件判定部221A、受信部222、指令値補正部223、送信部224、動作完了条件判定部225、第3の状態変数変更部303及び第4の状態変数変更部304を有している。更に、第2の処理部202Bは、バッファ書き込み部413、全データ書き込み判定部414、バッファ部415、バッファ読み出し部416及びロボット状態監視部417を有している。
【0082】
動作開始条件判定部221Aは、複数有る動作指令データB1,B2,B3…の中から、それぞれの動作指令データに対応する命令C1,C2,C3…の動作開始条件記述部c3を参照し、動作開始条件を満たす全ての動作指令データを選択する(411)。この時、動作開始条件判定部221Aは、状態変数c5が計算完了状態であることを条件に入れて探索する(412)。これにより、動作開始条件判定部221Aは、動作開始条件を満たす全ての動作指令データを取得する。
【0083】
動作開始条件を満たすものが1つも無い場合はバッファ読み出し部416へ進む。動作開始条件を満たすものが1つでも有る場合は、バッファ書き込み部413でバッファ部415に書き込む。
【0084】
このバッファ部415は、リングバッファと呼ばれているものであり、デバイス207の数だけトラックを有している。つまり、バッファ部415は、アーム用モータMa1〜Ma6に対応した数(6つ)のトラックを有している他、それ以外のデバイスに対応した数のトラックを有している。バッファ部415は、書き込み位置と読み出し位置を保持する。
【0085】
バッファ部415は、行列状のメモリ領域であり、行(トラック)は全てのデバイス207の数だけあり、トラック幅と呼ぶ。例えば、モータやセンサが全部で30個あれば30行のトラック幅を持つ。列方向は時間を表しており、1つの列が同期時間δtの1区間を表している。例えば同期時間間隔δtが1msで40秒間を蓄えるとすると、列の数は4万列である。バッファ書き込み部413は、バッファ部415の書き込み位置を基準にして、動作開始条件判定部221Aにより取得された動作指令データの指令値を、各デバイス207に対応するトラックに時系列順に連続して書き込む。
【0086】
また、バッファ読み出し部416は、同時刻に出力する指令値を、バッファ部415の各トラックから読み出す。全てのデバイス207に対する動作指令データの指令値を同時刻に読み出しているのが特徴である。また、読み出した後は、次の読み出し列を1つ、ずらす。もし最終列に達したら最初の列に戻す。つまり、バッファ部415は、トラック幅をもったリボンのようにリング状になっている。またこのバッファ部415は、最初、全体を「動作指令なし」の状態にリセットしておく。
【0087】
図10において、バッファ書き込み部413は、動作開始条件判定部221Aにより取得された動作指令データをバッファ部415の書込み位置から、該当するトラックに書き込む。指令値を書き込む処理が1列分終了したら、書き込んだデータの回数だけ書き込み位置を進める。書き込み位置がバッファ部415の終端に達すると、バッファ部415の先頭に戻す。また、一定の時間(同期時間δt)以内に動作指令データの全ての指令値の書き込み処理が終了しない場合には、残りの指令値は、次のターンでバッファ部415に書き込まれる。書き込み位置は、書き込んだデータ(指令値)の回数だけ進んでいるので連続性は失わない。
【0088】
このバッファ書き込み部413による書き込み処理が完了すると、全データ書込み判定部414で動作指令データの書き込みが全て完了したかどうかを判定する。もし完了していなければバッファ読み出し部416の処理へ進む。ここで完了していない場合とは、前述した書き込みがまだ一部だけであり、動作指令データにおける指令値が残っている場合である。書き込み処理が完了していれば、第3の状態変数変更部303は、状態変数c5を計算完了状態から動作中状態に変更する。
【0089】
バッファ読み出し部416は、バッファ部415の読み出し位置から、動作指令データを読み出す。バッファ部415からの読み出しは、全てのデバイス207に対して同時にデータ(指令値)を取得できる。バッファ読み出し部416は、データを読み出した後は、次の2つを行う。1つ目は、読み出し位置のデータをクリアーし、もとの動作指令なしの状態に戻す。2つ目は、読み出し位置を次の位置に更新する。この時、読み出し位置がバッファ部415の終端に達していた場合は、バッファ部415の先頭に戻す。従って、バッファ書き込み部413が1つも動作指令を書き込んでいないときは動作指令なしの状態が読み出される。
【0090】
このように、共有メモリ203に格納していた動作指令データBを一旦バッファ部415に蓄えるので、共有メモリ203にアクセスする回数を大幅に減らすことができる。その結果、第1の処理部201Bと第2の処理部202Bが両方同時に共有メモリ203にアクセスするという、アクセスの衝突回数が少なくなり、動作速度が向上する。
【0091】
受信部222は入力機器215(センサ)から測定値を入力する。例えばエンコーダの場合はモータの回転角度位置を受信する。具体的には、各受信器222bが各入力機器215からの信号を入力して、各測定値を受信制御部222aに出力する。受信制御部222aは、各測定値の補正を行い、指令値補正部223に出力する。
【0092】
指令値補正部223は、入力した測定値をもとに動作指令データBを補正する。例えば入力機器215が力覚センサの場合、力フィードバック制御が可能になり、入力機器215が視覚センサの場合、視覚フィードバック制御が可能になる。
【0093】
送信部224は、動作指令データBの指令値をデバイス207に送信する。デバイス207がモータの場合は、モータの関節角度を示す指令値を送信し、デバイス207がセンサの場合は信号の入力コマンドを示す指令値を送信する。具体的には、送信制御部224aが所定時刻に各指令値を出力し、各送信器224bが各出力機器214に出力する。各出力機器214は、入力した指令値に基づいて動作する。
【0094】
ロボット状態監視部417は、多関節ロボット本体100a,100bの姿勢を監視するものであり、順運動学モデルである第2のメカモデル418を用いて多関節ロボット本体100a,100b(ハンド)の位置姿勢を計算する。このハンドの位置姿勢の計算結果は、例えば予め設定した移動禁止領域に侵入したときに停止させる場合などの判定に使う。
【0095】
ここで、第1の処理部201Bは、第1のメカモデル409を用い、多関節ロボット本体100a,100bのハンドの位置姿勢から多関節ロボット本体100a,100bの関節角度を示す指令値を計算する。
【0096】
これに対し、第2の処理部202Bは多関節ロボット本体100a,100bの状態を監視するのに、第1のメカモデル409とは別の第2のメカモデル418を用いている。つまり、ロボット状態監視部417は、第2のメカモデル418を用いて、多関節ロボット本体100a,100bの関節角度から、順計算を実行して多関節ロボット本体100a,100bのハンドの位置を計算する。そして、ロボット状態監視部417は、その計算結果を、共有メモリ203において命令集合Aのメモリ領域及び動作指令データBのメモリ領域とは別のメモリ領域に格納する。
【0097】
従って、第1の処理部201Bと第2の処理部202Bとでそれぞれ独立した2つのメカモデル409,418があるので互いに干渉することがない。つまり、第2の処理部202Bが多関節ロボット本体100a,100b(ハンド)の位置姿勢を保持するメモリ内容を更新するが、命令集合Aのメモリ領域及び動作指令データBのメモリ領域とは別のメモリ領域にデータが格納されるので影響し合うことはない。
【0098】
次に、動作完了条件判定部225は、処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。動作の完了の判定方法は、例えば動作指令データの全指令値をバッファ部415から読み出し、デバイス207に送信したときに動作完了と判定する。または、多関節ロボット本体100a,100bのハンドの位置が予め設定した移動完了領域に侵入したときに動作完了と判定する。動作完了の場合は、第4の状態変数変更部304が、状態変数c5を動作中状態から動作完了状態に変更する。以上の第2の処理部202Bの処理を予め定めた同期時間δt毎に繰返し行う。
【0099】
したがって、上記第1,第2実施形態と同様に、第1の処理部201Bに軌道計算処理を実施させて計算結果である動作指令データBを共有メモリ203に格納させているので、第2の処理部202Bは、複雑な軌道計算処理を行う必要がない。したがって、第1の処理部201Bにより既に計算された動作指令データBの指令値を各モータMa1〜Ma6に同期して出力する同期処理を、多関節ロボット本体100a,100bの動作を停止させることなく実現することができる。ゆえに、多関節ロボット本体100a,100bに人間のような器用な動作を行わせることができる。
【0100】
[第4実施形態]
次に、本発明の第4実施形態に係るロボット制御装置について説明する。なお、ロボット制御装置の構成は、上記第1〜第3実施形態と同様である。図11は、ロボット制御装置により生成される動作指令データの例を示す図である。なお、多関節ロボット本体は、多軸であるので、各関節のモータに出力される複数本の指令値列があるが、説明のために1本の指令値列についてのみ図示している。多関節ロボット本体が同じ動作を繰り返す場合、第1回目の動作指令データと第2回目の動作指令データとが同じとなるので、第2回目以降は、動作指令データの計算を省略することが可能である。
【0101】
そこで、本第4実施形態では、計算部212(212B)は、計算開始条件判定部211(211A)が取得したコマンドから計算される動作指令データが、共有メモリ203に格納されている動作指令データBと一致するか否かを判断する。そして、計算部212(212B)は、一致すると判断した場合は、動作指令データの計算を省略する。
【0102】
ここで、共有メモリ203に格納した動作指令データは、一度、動作開始条件判定部221(221A)に読み出されたとしても、消去せずに残しておく。これにより、動作開始条件判定部221(221A)は、計算部212(212B)に計算が省略されたとしても、共有メモリ203に動作指令データが消去されずに残っているので、再び動作指令データを取得することができる。
【0103】
ここで、各命令C1,C2,C3…は、動作指令データが計算済みか否かを示すフラグを有しており、計算部212(212B)は、そのフラグを参照することで、計算済みか否かを判断することができる。このフラグは、計算部212(212B)により計算が終了した後、計算部212(212B)により変更される。
【0104】
以上、本第4実施形態では、第1回目の動作で計算された動作指令データは、第2回目以後、再利用されることとなる。このように、複雑な計算である軌道計算が省略されるので、多関節ロボット本体100をより高速に動作させることができ、第1の処理部201(201A,201B)の計算能力を温存することができる。
【0105】
[第5実施形態]
次に、本発明の第5実施形態に係るロボット制御装置について説明する。上記第4実施形態では、図11に示すように、連続する2つの動作指令データのうち、一方の動作指令データの終端値と、他方の動作指令データの始端値とが一致する場合について説明した。ところが、図12に示すように、連続する2つの動作指令データのうち、一方の動作指令データの終端値と、他方の動作指令データの始端値とが一致せず、差分Δfずれている場合もある。これが一致していないと2回目の動作指令データに基づく多関節ロボット本体の動作が滑らかとはならない。そこで、本第5実施形態では、オフセット値を加算する処理を行う。
【0106】
図13は、本第5実施形態に係るロボット制御装置200Cの各部の動作の流れを示す図である。なお、図13では、上記第2実施形態のロボット制御装置200Aと比較して動作開始条件判定部の動作が異なるものであり、その他の構成は、同一であるので、同一符号を付して説明を省略する。
【0107】
ロボット制御装置200Cの第2の処理部202Cは、上記第2実施形態と異なる動作開始条件判定部221Cを有している。具体的には、動作開始条件判定部221Cは、上記第2実施形態の動作開始条件判定部221Aと同様に動作する機能を有する他、オフセット加算部501を有している。
【0108】
つまり、動作開始条件判定部221Cのオフセット加算部501は、前回、共有メモリ203から取得した動作指令データ中の最後の指令値(終端値)と、今回、共有メモリ203から取得した動作指令データ中の最初の指令値(始端値)との差分Δfを計算する。そして、動作開始条件判定部221Cのオフセット加算部501は、差分Δfを初期値としてゼロまで漸次減少するオフセット値を、今回、共有メモリ203から取得した動作指令データ中の最初の指令値から時系列順に指令値に加算する補正を行う。
【0109】
つまり、オフセット加算部501は、動作指令データを再利用するとき、動作指令データの先頭データと、現在位置との差を計算し、その差を初期値とし、次第にゼロに減少するオフセット値を、動作指令データに加算する処理を行う。
【0110】
図14に、オフセット値を加算した動作指令データの一例を示す。初期値を差分Δfとし、次第にゼロに減少するオフセット値f(t)を動作指令データに加算すれば、滑らかな動作が可能となる。
【0111】
オフセット関数f(t)に必要な要件は次の2つある。それは、初期値が差分Δfに等しいことと、時間の経過とともにゼロに減少することである。例えば、差分をΔf、オフセット値がゼロになるまでの時間をT、再利用する動作指令データの最初の指令値が出力される時刻からの時間をtとすると、次のようなオフセット関数f(t)が有効である。
【0112】
【数1】
【0113】
この直線関数は計算量が少ないので計算機への負担が軽い。
【0114】
ただし、次の関数のほうがより滑らかである。
【0115】
【数2】
【0116】
コサイン関数なので、直線関数の場合よりも両端が滑らかである。したがって、より滑らかに多関節ロボット本体を動作させることができる。
【0117】
以上、上記第1〜第5実施形態に基づいて本発明を説明したが、本発明はこれに限定されるものではない。駆動部はサーボモータやリニアモータの他、ピエゾアクチュエータや空気圧を利用したシリンダなどであってもよい。一方、センサは、モータの回転角度を測定するエンコーダや変位計、カメラ、力を検出するロードセルなど、あらゆる種類のセンサが適用可能である。
【符号の説明】
【0118】
100…多関節ロボット本体、200,200A,200C…ロボット制御装置、201,201A,201B…第1の処理部、202,202A,202B,202C…第2の処理部、203…共有メモリ、211,211A…計算開始条件判定部、212,212B…計算部、215…入力機器(センサ)、221,221A,221C…動作開始条件判定部、222…受信部、223…指令値補正部、224…送信部、301…第1の状態変数変更部、302…第2の状態変数変更部、303…第3の状態変数変更部、413…バッファ書き込み部、415…バッファ部、416…バッファ読み出し部、417…ロボット状態監視部、B…動作指令データ、c1…動作記述部、c2…計算開始条件記述部、c3…動作開始条件記述部、c5…状態変数、Ma1〜Ma6…アーム用モータ(駆動部)
【技術分野】
【0001】
本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部を制御するロボット制御装置に関する。
【背景技術】
【0002】
多軸の多関節ロボット本体においては、各関節の角度を同期させて変えながら動作させることが必要である。この同期がずれると、多関節ロボット本体の手先位置の軌道がずれることになるからである。したがって、ロボット制御装置は、多関節ロボット本体の各関節を駆動する複数の駆動部を同期制御している(特許文献1参照)。なお、多関節ロボット本体の目標位置姿勢から各関節角度を計算するのを逆問題、反対に各関節の角度から多関節ロボット本体の位置姿勢を計算するのを順問題と呼び、このような問題を扱う分野を機構学と呼んでいる。
【0003】
この従来のロボット制御装置は、性質の異なる2つのコンピュータを組み合わせて構成されている。一方のコンピュータは汎用コンピュータであり、他方のコンピュータはリアルタイムコンピュータである。そして、汎用コンピュータは、リアルタイムコンピュータに多関節ロボット本体の目標位置姿勢を示すコマンドを出力する。リアルタイムコンピュータは、逆運動学に基づき、入力したコマンドから、各駆動部の関節角度を示す指令値を時系列に並べた動作指令データを計算することができる。
【0004】
各駆動部を予定時刻に確実に動作させるには、計算した指令値を多関節ロボット本体の各駆動部に一定の時間間隔(例えば5ms)で同期して出力する必要がある。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特表2004−524171号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、従来のロボット制御装置では、多関節ロボット本体に人間のように器用な動作をさせることはできなかった。具体的に説明すると、人間のような器用な動作には非常に高度で複雑なアルゴリズムが隠されており、ロボット制御系は高度で複雑なアルゴリズムに基づく計算ができなければならない。そして、多関節ロボット本体の目標位置姿勢が単純な場合でも、多関節ロボット本体の各関節角度は複雑な軌道を描く。つまり、逆運動学に基づく計算処理は、順運動学に基づく計算処理よりも複雑で時間を要する。
【0007】
したがって、そのような複雑な軌道計算を行う場合、計算の所要時間は同期時間(指令値を出力する時間間隔)よりも長くなってしまうことがある。また計算条件によってもこの所要時間が変わることが多い。つまり軌道計算の所要時間が不定である。この所要時間は例えば1sといった、同期時間(例えば5ms)とは比べられないほど長い時間が想定される。
【0008】
ところが、特許文献1に記載されているロボット制御装置では、リアルタイムコンピュータにより、複雑な軌道計算と同期動作とを並行して実行する必要がある。したがって、リアルタイムコンピュータは、同期時間内に軌道計算を完了しなければならないが、軌道計算の所要時間が不定であるので、同期時間内に軌道計算が完了しない場合がある。このような場合、リアルタイムコンピュータは、軌道計算を全て完了させてから同期動作を行う必要があり、軌道計算が終了するまで多関節ロボット本体の動作を停止させておかなければならない。これでは、多関節ロボット本体は滑らかな動作を行うことができず、人間のような器用な動作を実現することはできない。
【0009】
そこで、本発明は、軌道計算が複雑で計算の所要時間が不定であっても、複雑な軌道計算と、同期動作とを並行して実行することが可能なロボット制御装置を提供することを目的とするものである。
【課題を解決するための手段】
【0010】
本発明は、多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、共有メモリと、前記共有メモリにアクセス可能に接続された第1の処理部及び第2の処理部と、を備え、前記第1の処理部は、前記多関節ロボット本体を目標位置姿勢に動作させるコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算し、計算結果である動作指令データを前記共有メモリに格納する軌道計算処理を行い、前記第2の処理部は、前記共有メモリに格納された動作指令データを取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する同期処理を行うことを特徴とする。
【発明の効果】
【0011】
本発明によれば、第1の処理部に軌道計算処理を実施させて計算結果である動作指令データを共有メモリに格納させているので、第2の処理部は、複雑な軌道計算処理を行う必要がない。そして、第2の処理部は、複雑な軌道計算処理を行わないので、第1の処理部により既に計算された動作指令データの指令値を各駆動部に同期して出力する同期処理を、多関節ロボット本体の動作を停止させることなく実現することができる。したがって、多関節ロボット本体に人間のような器用な動作を行わせることができる。
【図面の簡単な説明】
【0012】
【図1】本発明の第1実施形態に係るロボット装置の概略構成を示す説明図である。
【図2】本発明の第1実施形態に係るロボット装置の概略構成を示すブロック図である。
【図3】本発明の第1実施形態に係るロボット制御装置の各部の動作の流れを示す説明図である。
【図4】本発明の第1実施形態に係る共有メモリに格納される各命令の構成を示す説明図である。
【図5】多関節ロボット本体の各関節に出力する動作指令データの説明図であり、(a)は各関節に出力する動作指令データを時系列に並べた場合のタイムチャート、(b)は(a)にて円で囲った部分の指令値の列を示す拡大図である。
【図6】3つの命令を使った動作例を示すタイムチャート図である。
【図7】本発明の第2実施形態に係るロボット制御装置の動作の流れを示す説明図である。
【図8】本発明の第2実施形態に係るロボット制御装置の共有メモリに格納される各命令の構成を示す説明図である。
【図9】本発明の第3実施形態に係るロボット制御装置の第1の処理部の動作の流れを示す説明図である。
【図10】本発明の第3実施形態に係るロボット制御装置の第1の処理部の動作の流れを示す説明図である。
【図11】本発明の第4実施形態に係るロボット制御装置により生成される動作指令データの例を示す図である。
【図12】比較例としてのロボット制御装置により生成される動作指令データの例を示す図である。
【図13】本発明の第5実施形態に係るロボット制御装置の動作の流れを示す説明図である。
【図14】本発明の第4実施形態に係るロボット制御装置により生成される、オフセット値を加算した動作指令データの一例を示す図である。
【発明を実施するための形態】
【0013】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【0014】
[第1実施形態]
図1は、本発明の第1実施形態に係るロボット装置の概略構成を示す説明図である。ロボット装置50は、多関節ロボット本体100と、多関節ロボット本体100を制御するロボット制御装置200と、を備えている。
【0015】
多関節ロボット本体100は、複数のリンク101〜105と、先端リンク105に接続されたエンドエフェクタとしてのハンド106と、を有しており、リンク101〜105及びハンド106が各関節J1〜J6で揺動又は回動可能に連結されている。本実施形態では、多関節ロボット本体100は、自由度が6の6軸多関節ロボットである。
【0016】
図2は、ロボット装置50の概略構成を示すブロック図である。各関節J1〜J6には、各関節J1〜J6を駆動する複数の駆動部としてのアーム用モータMa(Ma1〜Ma6)が設けられ、ハンド106には、ハンド用モータMbが設けられている。また、図示は省略するが、多関節ロボット本体100は、直動アクチュエータやシリンダ等を有している。これらアーム用モータMa、ハンド用モータMb、直動アクチュエータ及びシリンダが出力機器214である。
【0017】
また、各関節J1〜J6には、各アーム用モータMaの回転角度を検知するアーム用エンコーダEaが設けられている。また、ハンド106には、ハンド用モータMbの回転角度を検知するハンド用エンコーダEbが設けられている。ハンド106と先端リンク105との間には、ハンド106に作用する力及びトルクを検知する力覚センサSaが設けられている。また、図示は省略するが、各モータMa,Mbに供給する電流を検知する電流センサ、各関節J1〜J6のトルクを検知するトルクセンサが、多関節ロボット本体100に設けられている。これらエンコーダEa,Eb、力覚センサSa、電流センサ及びトルクセンサが、多関節ロボット本体100の状態を検知するセンサとしての入力機器215である。以下、入力機器215及び出力機器214を総称して、デバイス207という。
【0018】
ロボット制御装置200は、大別すると、第1の処理部201と、第2の処理部202と、共有メモリ203と、を備えている。第1の処理部201及び第2の処理部202は、共有メモリ203にアクセス可能に接続されている。本実施形態では、ロボット制御装置200は、1つのデュアルコアCPUを有し、1つのデュアルコアCPUにおける各コアが、第1の処理部201及び第2の処理部202のそれぞれとして機能するように構成されている。なお、ロボット制御装置200が、シングルコアCPUを2つ有し、各CPUが第1の処理部201及び第2の処理部202のそれぞれとして機能するように構成してもよい。
【0019】
本第1実施形態では、第1の処理部201は、多関節ロボット本体100を目標位置姿勢に動作させるコマンドを入力し、この入力したコマンドから各モータMa1〜Ma6に出力する関節角度の指令値を時系列に連ねた動作指令データ(軌道データ)を計算する。そして、計算結果である動作指令データを共有メモリ203に格納する。このように、第1の処理部201は、逆運動学に基づく計算所要時間の不定な軌道計算処理を行う。
【0020】
第2の処理部202は、共有メモリ203に格納された動作指令データを取得して、指令値を各モータMa1〜Ma6に一定の時間間隔で同期して出力する同期処理を行う。つまり、本第1実施形態では、複雑で計算に要する時間が不定となる軌道計算処理を、第1の処理部201で行い、各モータMa1〜Ma6の同期処理は、第2の処理部202が行う。
【0021】
以下、各処理部201,202及び共有メモリ203について詳細に説明する。第1の処理部201は、計算開始条件判定部211と、計算部212と、を有している。第2の処理部202は、動作開始条件判定部221と、受信部222と、指令値補正部223と、送信部224と、動作完了条件判定部225と、を有している。
【0022】
図3は、ロボット制御装置200の各部の動作の流れを示す説明図である。図3に示すように、まず、共有メモリ203には、予め複数の命令C1,C2,C3…からなる命令集合Aが格納されている。つまり、共有メモリ203には、多関節ロボット本体100の関節角度を決めるための命令が予め複数格納されている。命令集合Aは、例えばテキスト形式のファイルで、ユーザがロボットの動作を記述する目的で準備する。通常は命令を多数組み合わせてロボットの動作を実現する。図4に示すように、各命令Cn(nは正の整数)は、動作記述部c1、計算開始条件記述部c2、動作開始条件記述部c3、及び動作完了条件記述部c4で構成されている。
【0023】
まず、共有メモリ203に格納されている命令のうち、各モータMa1〜Ma6に駆動動作を行わせるための命令について説明する。動作記述部c1には、多関節ロボット本体100(より具体的には、多関節ロボット本体100のハンド106)を目標位置姿勢に動作させるためのコマンドが記述されている。ここで、コマンドは、「ジョブ」あるいは「Instruction」と同じ意味である。コマンドは、多関節ロボット本体100のハンド106の目標位置姿勢を直行座標系で指定するものである。即ち、ハンド106の目標位置姿勢は、直交座標系で(X,Y,Z,θX,θY,θZ)で指定される。そして、例えばハンド106を、X=100mm、Y=200mmの座標位置に移動せよ、といったコマンドが動作記述部c1に記述されている。また例えば、ハンド106の現在の位置姿勢から相対的な位置姿勢を指定するコマンドが動作記述部c1に記述されている。
【0024】
計算開始条件記述部c2には、計算部212により軌道計算を開始する条件が記述されている。例えば、他の命令に含まれているコマンドを実行するときにそのコマンドによりハンド106が到達する最終目標位置が確定したら計算を開始する、といった条件が、計算開始条件記述部c2に記述されている。また例えば、多関節ロボット本体100の動作が停止した時に計算を開始する、センサの値が確定した時に計算を開始する、といった条件が、計算開始条件記述部c2に記述されている。
【0025】
動作開始条件記述部c3には、コマンドを実行するための各モータMa1〜Ma6の動作を開始する条件が記述されている。例えば2本のアームを協調させて作業する場合、同時に動作を開始する必要があるので、例えば動作開始条件は2本分の軌道の計算を待つという条件が動作開始条件記述部c3に記述されている。
【0026】
動作完了条件記述部c4には、各モータMa1〜Ma6の動作を完了させる条件が記述されている。例えばモータMa1〜Ma6の軌道が終端に達した場合(多関節ロボット本体100が目標位置姿勢に達した場合)にモータMa1〜Ma6の動作を完了させる、といった条件が動作完了条件記述部c4に記述されている。
【0027】
なお、軌道計算ではないが、モータMbや不図示の直動アクチュエータ、シリンダ等に駆動動作を行わせるための命令についても同様に構成されており、共有メモリ203に予め格納されている。
【0028】
また、共有メモリ203には、入力機器(センサ)215に測定動作を行わせるための命令も格納されている。以下、入力機器215に測定動作を行わせるための命令について説明する。
【0029】
動作記述部c1には、入力機器215への測定要求が記述される。計算開始条件記述部c2には、入力機器215が測定する前に必要な測定準備を開始する条件が記述される。例えば入力機器215がハンド106に取り付けられた力覚センサSaや視覚センサである場合、ハンド106が目標位置姿勢になるまで待つといった条件が記述される。
【0030】
動作開始条件記述部c3には、入力機器215に測定動作を開始させる条件が記述される。入力機器215が視覚センサの場合、実際に視覚センサが測定を開始できる条件が記述される。例えば他のロボット装置が測定対象を把持している場合、そのロボット装置が停止するまで待つといった条件が記述される。動作完了条件記述部c4には、入力機器215による測定動作を完了させる条件が記述されている。例えば、測定値が安定したかどうかを判定する条件が記述されている。
【0031】
次に、第1の処理部201について説明する。計算開始条件判定部211は、共有メモリ203に格納された命令集合Aにおける複数の命令C1,C2,C3…の計算開始条件記述部c2を順次参照する。そして、計算開始条件判定部211は、計算開始条件を満たしている命令の動作記述部c1に記述されているコマンドを共有メモリ203から1つ取得する。
【0032】
計算開始条件判定部211は、もし、計算開始条件を満たすものが無い場合は、条件を満たすまで待つ。従って最初に起動するときは、複数の命令C1,C2,C3…のうち、少なくとも1つ計算開始条件を満たす命令が必要である。また、図示していないが、外部から命令の中の計算開始条件を変更することによって、起動させることもできる。
【0033】
計算部212は、計算開始条件判定部211にて取得されたコマンドを受け取り、そのコマンドから各モータMa1〜Ma6に出力する関節角度の指令値を時系列に連ねた動作指令データBを逆運動学モデルに基づいて計算する。そして、計算部212は、計算結果である動作指令データBを共有メモリ203に格納する。共有メモリ203には、計算部212で計算された動作指令データBが順次格納されるので、複数の動作指令データBが格納されることとなる。
【0034】
以下、動作指令データBについて、図5に示す具体例を参照しながら説明する。図5(a)は、各関節に出力する動作指令データBを時系列に並べた場合のタイムチャートである。図5(a)に示す多関節ロボット本体100を位置姿勢P1から位置姿勢P3に位置姿勢P2を経由して移動させよ、というコマンドを計算開始条件判定部211が取得した場合、計算部212は、そのコマンドから動作指令データBを計算する。この動作指令データBは、各モータMa1〜Ma6に出力する各関節J1〜J6の関節角度を示す指令値を時系列に連ねたものである。
【0035】
図5(b)は、図5(a)にて円で囲った部分の指令値の列を示す拡大図である。図5(b)に示すように、関節J1におけるモータMa1に対応して、指令値d11〜d19が計算され、関節J6におけるモータMa6に対応して、指令値d61〜d69が計算される。そして、各指令値は、第2の処理部202にて、一定の時間間隔δt(つまり同期時間δt)で各モータMa1〜Ma6に出力される。以上のように、第1の処理部201は、計算が複雑で計算所要時間が不定な軌道計算処理を行う。以上の計算部212の処理動作が終了したら、再び計算開始条件判定部211による動作を実施する。
【0036】
なお、計算部212は、ハンド用モータMbや直動アクチュエータ、シリンダ等の軌道計算を行わないものについても、入力したコマンドに基づいて、指令値からなる動作指令データを計算する。また、入力機器215の場合も同様である。モータMa1〜Ma6の時のように軌道は計算しないが、測定する前の計算を行う。例えば力覚センサSaの場合、多関節ロボット本体100(ハンド106)が所定の位置にきた時の値を計算して、その計算結果である測定指令データを共有メモリ203に格納する。
【0037】
次に、第2の処理部202について説明する。動作開始条件判定部221は、共有メモリ203に格納された各命令C1,C2,C3…の動作開始条件記述部c3を参照する。そして、動作開始条件判定部221は、共有メモリ203に格納された動作指令データB…のうち、動作開始条件を満たしている命令に対応する動作指令データBを共有メモリ203から取得する。このとき、動作開始条件を満たしている命令が複数ある場合には、動作開始条件判定部221は、その複数の動作指令データを共有メモリ203から取得する。なお、動作開始条件判定部221は、測定指令データについても動作開始条件を満たしていれば、対応する測定指令データを共有メモリ203から取得する。
【0038】
受信部222は、入力機器215(センサ)から検知結果である信号を入力する。そして、動作開始条件判定部221が測定指令データを取得していた場合には、検知結果を補正する。例えば力覚センサSaの場合は、実際に測定する時に測定指令値データの指令値を毎回差し引くことによって、重力の影響を軽減する。また、例えば視覚センサの場合は、多関節ロボット本体100の関節角度から視覚センサが取り付いている3次元的な位置を計算し、実際に測定する時の補正に使用する。
【0039】
指令値補正部223は、動作開始条件判定部221にて取得された動作指令データBの各モータMa1〜Ma6に同期して出力するための各指令値を、入力機器215からの信号を用いて補正をし、送信部224に出力する。これにより、例えば入力機器215が力覚センサSaの場合、力フィードバック制御が可能になり、また、入力機器215が視覚センサの場合、視覚フィードバック制御が可能になる。
【0040】
補正量を計算する方法は既知である。例えば文献(ロボット工学ハンドブック、日本ロボット学界編、コロナ社(東京)、2005年、287ページ)などに記述されているインピーダンス制御、あるいはコンプライアンス制御等の力制御が利用可能である。
【0041】
送信部224は、共有メモリ203から取得され補正された各指令値を入力し、各指令値を各モータMa1〜Ma6に一定の時間間隔δt、つまり同期時間δtで同期して出力する。この同期時間δtはミリ秒、例えば5ミリ秒といった短い時間である。動作完了条件判定部225は処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。そして、共有メモリ203に、動作結果Dが格納される。以上説明した各部221〜225の動作は、同期時間δt毎に繰返し行なわれる。
【0042】
以上の構成により、例えば命令C1,C2,C3の順に軌道計算処理を行い、命令C1,C2を同時に実行し、命令C2の実行終了後、命令C3を実行する場合について説明する。図6に、3つの命令を使った動作例のタイムチャート図を示す。
【0043】
第1の処理部201の計算部212は、共有メモリ203から取得された命令C1のコマンドに基づいて命令C1に対応する動作指令データを計算して、共有メモリ203に格納する。次いで、計算部212は、共有メモリ203から取得された命令C2のコマンドに基づいて命令C2に対応する動作指令データを計算して、共有メモリ203に格納する。次いで、計算部212は、共有メモリ203から取得された命令C3のコマンドに基づいて命令C3に対応する動作指令データを計算して、共有メモリ203に格納する。このように、第1の処理部201の計算部212は、各コマンドを1つずつ順次計算して共有メモリ203に計算結果を格納する。
【0044】
このとき、第2の処理部202は、例えば第1の処理部201の計算部212が、命令C3のコマンドから動作指令データを演算している途中であっても、第1の処理部201の計算動作とは独立して動作することができる。従って、第2の処理部202の動作開始条件判定部221は、第1の処理部201による命令C3のコマンドに基づく計算終了を待たずに共有メモリ203にアクセスし、命令C1の動作指令データと、命令C2の動作指令データとを取得している。そして、各命令C1,C2の動作を実行している。その後、第2の処理部202は、命令C3の動作を実行している。このように、本第1実施形態では、軌道計算処理と同期処理とを並行して行うことが可能となり、ロボット装置50の動作時間の短縮につながる。特に、ロボット装置50が生産装置に組み込まれる場合には、動作時間の短縮は生産物のコストを下げることができるので非常に重要である。
【0045】
以上、本第1実施形態によれば、第1の処理部201に軌道計算処理を実施させて計算結果である動作指令データBを共有メモリ203に格納させているので、第2の処理部202は、複雑な軌道計算処理を行う必要がない。したがって、たとえ第1の処理部201で計算時間が多くかかっても、第2の処理部202は、逆運動学に基づく軌道演算処理を行わないので、動作開始条件を満たした直後に各モータMa1〜Ma6を動かすことができる。
【0046】
そして、第2の処理部202は、複雑な軌道計算処理を行わず、第1の処理部201により既に計算された動作指令データBの指令値を各モータMa1〜Ma6に同期して出力する同期処理を行うようにしている。したがって、多関節ロボット本体100の動作を停止させることなく実現することができ、多関節ロボット本体100を滑らかに動作させることができるので、多関節ロボット本体100に人間のような器用な動作を行わせることができる。
【0047】
また、本第1実施形態では、第1の処理部201が計算開始条件判定部211を有しているので、計算部212は、適したタイミングで計算を開始することができる。また、第2の処理部202が、動作開始条件判定部221を有しているので、適したタイミングで動作を開始することができる。つまり、軌道計算処理及び同期処理のそれぞれの処理が、命令C1,C2,C3…に従ってタイミングよく動作させることができるので、他の動作機器(例えば把持対象物が載置される移動テーブル)との同期をとることもできる。
【0048】
以上の説明では、1つのロボット制御装置200で1台の多関節ロボット本体100を制御する場合について説明したが、1つのロボット制御装置200で複数台の多関節ロボット本体を制御する場合についても制御動作は同じである。つまり、共有メモリ203には、各多関節ロボット本体に対する命令が予め格納されており、各処理部201,202は、各命令に基づき、軌道計算処理及び同期処理のそれぞれを行う。特に、各多関節ロボット本体を同時に動作させる場合には、例えば図6に示すように、命令C1と命令C2とを同時に出力することができる。したがって、複数台の多関節ロボット本体を、停止させることなく同期して動作させることができる。これにより、複数台の多関節ロボット本体を滑らかに動作させることができ、複数台の多関節ロボット本体に人間のような器用な動作を行わせることができる。
【0049】
[第2実施形態]
次に、本発明の第2実施形態に係るロボット制御装置について説明する。図7は、ロボット制御装置200Aの動作の流れを示す説明図であり、図8は、共有メモリに格納される各命令の構成を示す説明図である。なお、本第2実施形態において、上記第1実施形態と同様の構成については、同一符号を付して説明を省略する。
【0050】
図8に示すように、共有メモリ203に予め格納される各命令Cnは、上記第1実施形態で説明した、動作記述部c1、計算開始条件記述部c2、動作開始条件記述部c3、及び動作完了条件記述部c4の他、更に状態変数c5を有している。本第2実施形態では、状態変数c5は、計算開始待ち状態、計算中状態、計算完了状態、動作中状態及び動作完了状態のうちのいずれかの状態を意味する値となる。最初に起動するときには状態変数c5を計算開始待ち状態にセットしておく。
【0051】
計算開始待ち状態とは、軌道の計算開始を待っている状態である。計算中状態とは、図7に示す計算部212が軌道の計算を開始し、計算中である状態である。計算完了状態とは、計算部212による計算が完了し、共有メモリ203に動作指令データBが保存された状態である。動作中状態とは、第2の処理部202Aが動作を開始し、動作中である状態であり、動作完了状態とは、第2の処理部202Aが動作を完了している状態である。
【0052】
また、図7に示す第1の処理部201Aは、計算開始条件判定部211A及び計算部212の他、第1の状態変数変更部301及び第2の状態変数変更部302を有している。計算開始条件判定部211Aは、共有メモリ203に格納された各命令C1,C2,C3…の計算開始条件記述部c2を参照する。そして、計算開始条件判定部211Aは、計算開始条件を満たし、且つ状態変数c5が計算開始待ち状態である命令を抽出する。そして、計算開始条件判定部211Aは、その命令に含まれる動作記述部c1に記述されているコマンドを共有メモリ203から1つ取得する。
【0053】
ここで、計算開始条件判定部211Aは、もし該当する命令が存在しなければ、計算条件が満たされるまで待つ。従って最初に起動するときは、少なくとも1つの命令が、計算開始条件を満足していなければならない。また、全部の命令が計算開始条件を満足しない場合でも、図示していない外部のプログラムから、この計算開始条件を変更して、起動させることは容易である。例えば、状態変数を開始待ち状態に変更すればよい。
【0054】
次に、第1の状態変数変更部301は、共有メモリ203に格納された命令C1,C2,C3…のうち、計算開始条件判定部211Aによりコマンドが取得された命令の状態変数c5を、計算開始待ち状態から計算中状態に変更する。そして、計算部212は、計算開始条件判定部211Aにより取得されたコマンドから動作指令データBを計算し、この動作指令データBを共有メモリ203に格納する。
【0055】
つまり、計算開始条件判定部211Aは、状態変数c5が計算開始待ち状態以外の状態である命令に含まれるコマンドを取得することはないので、重複計算となるのを回避することができる。
【0056】
第2の状態変数変更部302は、計算部212で計算が完了した場合、共有メモリ203に格納された命令C1,C2,C3…のうち、計算部212による計算が完了した動作指令データに対応する命令の状態変数c5を変更する。具体的には、状態変数c5を、計算中状態から計算完了状態に変更する。以上の第2の状態変数変更部302による変更動作が終了したら、再び計算開始条件判定部211Aによる動作を実施する。
【0057】
次に、第2の処理部202Aについて説明する。第2の処理部202Aは、動作開始条件判定部221A、受信部222、指令値補正部223、送信部224及び動作完了条件判定部225の他、第3の状態変数変更部303及び第4の状態変数変更部304を有している。
【0058】
動作開始条件判定部221Aは、共有メモリ203に格納された各命令C1,C2,C3…の動作開始条件記述部c3を参照する。そして、動作開始条件判定部221Aは、共有メモリ203に格納された動作指令データB…のうち、動作開始条件を満たし、且つ状態変数c5が計算完了状態である命令に対応する動作指令データBを共有メモリ203から取得する。このとき、動作開始条件を満たし、且つ計算完了状態である命令が複数ある場合には、動作開始条件判定部221Aは、その条件を満たした全ての動作指令データBを共有メモリ203から取得する。
【0059】
第3の状態変数変更部303は、共有メモリ203に格納された命令のうち、動作開始条件判定部221Aにより取得された動作指令データBに対応する命令の状態変数c5を計算完了状態から動作中状態に変更する。これにより、動作中の命令を重複して実行するのを回避することができ、制御が安定する。
【0060】
受信部222は、入力機器215(センサ)から検知結果である信号を入力する。そして、動作開始条件判定部221が測定指令データを取得していた場合には、検知結果を補正する。例えば力覚センサSaの場合は、実際に測定する時に測定指令値データの指令値を毎回差し引くことによって、重力の影響を軽減する。また、例えば視覚センサの場合は、多関節ロボット本体100の関節角度から視覚センサが取り付いている3次元的な位置を計算し、実際に測定する時の補正に使用する。
【0061】
指令値補正部223は、動作開始条件判定部221にて取得された動作指令データBの各モータMa1〜Ma6に同期して出力するための各指令値を、入力機器215からの信号を用いて補正をし、送信部224に出力する。これにより、例えば入力機器215が力覚センサSaの場合、力フィードバック制御が可能になり、また、入力機器215が視覚センサの場合、視覚フィードバック制御が可能になる。
【0062】
送信部224は、共有メモリ203から取得され補正された各指令値を入力し、各指令値を各モータMa1〜Ma6に一定の時間間隔δt、つまり同期時間δtで同期して出力する。この同期時間δtはミリ秒、例えば5ミリ秒といった短い時間である。動作完了条件判定部225は処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。そして、共有メモリ203に、動作結果Dが格納される。
【0063】
第4の状態変数変更部304は、共有メモリ203に格納された命令のうち、動作が完了したときの動作指令データBに対応する命令の状態変数c5を動作中状態から動作完了状態に変更する。以上説明した各部221A〜304の動作は、同期時間δt毎に繰返し行なわれる。なお、共有メモリ203に格納された命令が入力機器215に対するものである場合も同様に動作する。
【0064】
以上、本第2実施形態では、第1の処理部201Aに軌道計算処理を実施させて計算結果である動作指令データBを共有メモリ203に格納させているので、第2の処理部202Aは、複雑な軌道計算処理を行う必要がない。したがって、たとえ第1の処理部201Aで計算時間が多くかかっても、第2の処理部202Aは、逆運動学に基づく軌道演算処理を行わないので、動作開始条件を満たした直後に各モータMa1〜Ma6を動かすことができる。
【0065】
そして、第2の処理部202Aは、複雑な軌道計算処理を行わず、第1の処理部201により既に計算された動作指令データBの指令値を各モータMa1〜Ma6に同期して出力する同期処理を行うようにしている。したがって、多関節ロボット本体100の動作を停止させることなく実現することができ、多関節ロボット本体100を滑らかに動作させることができるので、多関節ロボット本体100に人間のような器用な動作を行わせることができる。
【0066】
また、本第2実施形態では、第1の処理部201Aが計算開始条件判定部211Aを有しているので、計算部212は、適したタイミングで計算を開始することができる。また、第2の処理部202Aが、動作開始条件判定部221Aを有しているので、適したタイミングで動作を開始することができる。つまり、軌道計算処理及び同期処理のそれぞれの処理が、命令C1,C2,C3…に従ってタイミングよく動作させることができるので、他の動作機器(例えば把持対象物が載置される移動テーブル)との同期をとることもできる。
【0067】
また、本第2実施形態では、各命令C1,C2,C3…に状態変数c5が含まれているので、状態変数c5を監視することで、第1の処理部201A及び第2の処理部202Aによる制御動作が安定する。
【0068】
以上の説明では、1つのロボット制御装置200Aで1台の多関節ロボット本体100を制御する場合について説明したが、1つのロボット制御装置200Aで複数台の多関節ロボット本体を制御する場合についても制御動作は同じである。つまり、共有メモリ203には、各多関節ロボット本体に対する命令が予め格納されており、各処理部201A,202Aは、各命令に基づき、軌道計算処理及び同期処理のそれぞれを行う。特に、各多関節ロボット本体を同時に動作させる場合には、例えば図6に示すように、命令C1と命令C2とを同時に出力することができる。したがって、複数台の多関節ロボット本体を、停止させることなく同期して動作させることができる。これにより、複数台の多関節ロボット本体を滑らかに動作させることができ、複数台の多関節ロボット本体に人間のような器用な動作を行わせることができる。
【0069】
[第3実施形態]
次に、本発明の第3実施形態に係るロボット制御装置について説明する。図9はロボット制御装置の第1の処理部の各部の動作の流れを示す説明図である。図10はロボット制御装置の第2の処理部の各部の動作の流れを示す説明図である。なお、上記第1,第2実施形態と同様の構成については、同一符号を付して説明を省略する。ここで、上記第1,第2実施形態では、多関節ロボット本体100が1台の場合について説明したが、2台以上であっても制御動作は同じである。本第3実施形態では、2台の多関節ロボット本体100a,100bを対象としているが、制御動作は多関節ロボット本体が1台であっても、3台以上であっても変わらない。
【0070】
本第3実施形態では、ロボット制御装置は、図9に示す第1の処理部201Bと、図10に示す第2の処理部202Bと、を備えている。まず、第1の処理部201Bについて説明する。計算開始条件判定部211Aは、共有メモリ203に格納されている命令集合Aの中から、それぞれの命令C1,C2,C3…の計算開始条件記述部c2を参照し、計算開始条件を満たす命令を選択する(401)。この時、計算開始条件判定部211Aは、状態変数c5が計算開始待ち状態であることを条件に入れて探索する(402)。もし該当する命令が存在しなければ、再び共有メモリ203に格納されている命令集合Aの中から、それぞれの命令C1,C2,C3…の計算開始条件記述部c2を参照し、計算開始条件を満たす命令を選択する。
【0071】
計算開始条件判定部211Aがコマンドを1つ取得した後、第1の状態変数変更部301が状態変数c5を計算開始待ち状態から計算中状態に変更する。
【0072】
計算部212Bは、計算開始条件判定部211Aが取得したコマンドに基づいて動作指令データBを計算する。計算部212Bは、処理選択部403、モータ制御計算部404、センサ制御計算部405、外部機器制御部406、補正部407、及び動作指令データ出力部408を有している。
【0073】
処理選択部403は、コマンドの内容を解釈する。モータ制御計算部404は、多関節ロボット本体100a,100bの関節のアーム用モータMa1〜Ma6に対する軌道を計算する。軌道が決まった後は、加速時、減速時に最大加速度、あるいは最大速度、最大モータトルクを越えないように、軌道を進む速度を計算する。この計算方法についても様々な方法が提案されており、例えば次の文献には軌道を計算する方法が示されている。
J.E.Bobrow, S.Dubowsky, J.S.Gibson:
Time−Optimal Control of Robotic Manipulators Along Specified Paths,
The International Journal of Rbotics Research, Vol.4, No.3(1985), p3−17.
【0074】
このような軌道を計算するためには、逆運動学モデルである第1のメカモデル409が必要である。ここで、第1のメカモデル409とは、多関節ロボット本体100a,100bのシミュレータであり、逆計算部を有している。モータ制御計算部404は、このメカモデル409を用いることにより、多関節ロボット本体100a,100bを動作させたときの各関節にかかるトルクや加速度を計算して軌道計算を行い、動作指令データBを算出する。
【0075】
センサ制御計算部405は、入力機器(センサ)215への測定指令データを計算して出力する。
【0076】
外部機器制御部406は、多関節ロボット本体100a,100bのモータ以外の外部機器、例えばモータで制御されるテーブルや、エアーシリンダの命令に対応する部分である。それぞれのデバイスに対する動作指令データを出力する。
【0077】
次に、補正部407は、算出された動作指令データBを補正する。一般に多関節ロボットは位置再現性よりも位置決め精度が低い。例えば6軸多関節ロボット本体のハンドを一直線上に動かそうとしても、関節と関節の間の長さや、関節の取り付け誤差があるために、うねって動く。こういった原因による運動誤差は再現するので、一度動いた結果をもとに、次に動くときに改善が期待できる。そこで、本第3実施形態では、予め共有メモリ203に保存した学習データ410を加えるものである。学習データ410を求める方法については、公知の技術で可能である。
【0078】
例えば文献(ロボット工学ハンドブック、日本ロボット学界編、コロナ社(東京)、2005年、320ページ目)などに記述されているように、一度動いた軌道と、目標の軌道との誤差をもとに、次の動作指令を補正する。これによって、目標の軌道に近づけることができる。
【0079】
次に、動作指令データ出力部408は、第1のメカモデル409を用いて計算した補正後の動作指令データBを共有メモリ203に出力する。この動作指令データBは、命令集合Aが格納されているメモリ領域とは異なる別のメモリ領域に格納される。このような動作指令データBは命令を処理するごとに生成されるので、複数の動作指令データBが共有メモリ203に格納されることになる。
【0080】
以上説明した動作が完了すると、第1の処理部201Aは、再び計算開始条件判定部211Aにて他の命令の処理を開始する。
【0081】
次に、第2の処理部202Bについて説明する。第2の処理部202Bは、上記第2実施形態と同様に、動作開始条件判定部221A、受信部222、指令値補正部223、送信部224、動作完了条件判定部225、第3の状態変数変更部303及び第4の状態変数変更部304を有している。更に、第2の処理部202Bは、バッファ書き込み部413、全データ書き込み判定部414、バッファ部415、バッファ読み出し部416及びロボット状態監視部417を有している。
【0082】
動作開始条件判定部221Aは、複数有る動作指令データB1,B2,B3…の中から、それぞれの動作指令データに対応する命令C1,C2,C3…の動作開始条件記述部c3を参照し、動作開始条件を満たす全ての動作指令データを選択する(411)。この時、動作開始条件判定部221Aは、状態変数c5が計算完了状態であることを条件に入れて探索する(412)。これにより、動作開始条件判定部221Aは、動作開始条件を満たす全ての動作指令データを取得する。
【0083】
動作開始条件を満たすものが1つも無い場合はバッファ読み出し部416へ進む。動作開始条件を満たすものが1つでも有る場合は、バッファ書き込み部413でバッファ部415に書き込む。
【0084】
このバッファ部415は、リングバッファと呼ばれているものであり、デバイス207の数だけトラックを有している。つまり、バッファ部415は、アーム用モータMa1〜Ma6に対応した数(6つ)のトラックを有している他、それ以外のデバイスに対応した数のトラックを有している。バッファ部415は、書き込み位置と読み出し位置を保持する。
【0085】
バッファ部415は、行列状のメモリ領域であり、行(トラック)は全てのデバイス207の数だけあり、トラック幅と呼ぶ。例えば、モータやセンサが全部で30個あれば30行のトラック幅を持つ。列方向は時間を表しており、1つの列が同期時間δtの1区間を表している。例えば同期時間間隔δtが1msで40秒間を蓄えるとすると、列の数は4万列である。バッファ書き込み部413は、バッファ部415の書き込み位置を基準にして、動作開始条件判定部221Aにより取得された動作指令データの指令値を、各デバイス207に対応するトラックに時系列順に連続して書き込む。
【0086】
また、バッファ読み出し部416は、同時刻に出力する指令値を、バッファ部415の各トラックから読み出す。全てのデバイス207に対する動作指令データの指令値を同時刻に読み出しているのが特徴である。また、読み出した後は、次の読み出し列を1つ、ずらす。もし最終列に達したら最初の列に戻す。つまり、バッファ部415は、トラック幅をもったリボンのようにリング状になっている。またこのバッファ部415は、最初、全体を「動作指令なし」の状態にリセットしておく。
【0087】
図10において、バッファ書き込み部413は、動作開始条件判定部221Aにより取得された動作指令データをバッファ部415の書込み位置から、該当するトラックに書き込む。指令値を書き込む処理が1列分終了したら、書き込んだデータの回数だけ書き込み位置を進める。書き込み位置がバッファ部415の終端に達すると、バッファ部415の先頭に戻す。また、一定の時間(同期時間δt)以内に動作指令データの全ての指令値の書き込み処理が終了しない場合には、残りの指令値は、次のターンでバッファ部415に書き込まれる。書き込み位置は、書き込んだデータ(指令値)の回数だけ進んでいるので連続性は失わない。
【0088】
このバッファ書き込み部413による書き込み処理が完了すると、全データ書込み判定部414で動作指令データの書き込みが全て完了したかどうかを判定する。もし完了していなければバッファ読み出し部416の処理へ進む。ここで完了していない場合とは、前述した書き込みがまだ一部だけであり、動作指令データにおける指令値が残っている場合である。書き込み処理が完了していれば、第3の状態変数変更部303は、状態変数c5を計算完了状態から動作中状態に変更する。
【0089】
バッファ読み出し部416は、バッファ部415の読み出し位置から、動作指令データを読み出す。バッファ部415からの読み出しは、全てのデバイス207に対して同時にデータ(指令値)を取得できる。バッファ読み出し部416は、データを読み出した後は、次の2つを行う。1つ目は、読み出し位置のデータをクリアーし、もとの動作指令なしの状態に戻す。2つ目は、読み出し位置を次の位置に更新する。この時、読み出し位置がバッファ部415の終端に達していた場合は、バッファ部415の先頭に戻す。従って、バッファ書き込み部413が1つも動作指令を書き込んでいないときは動作指令なしの状態が読み出される。
【0090】
このように、共有メモリ203に格納していた動作指令データBを一旦バッファ部415に蓄えるので、共有メモリ203にアクセスする回数を大幅に減らすことができる。その結果、第1の処理部201Bと第2の処理部202Bが両方同時に共有メモリ203にアクセスするという、アクセスの衝突回数が少なくなり、動作速度が向上する。
【0091】
受信部222は入力機器215(センサ)から測定値を入力する。例えばエンコーダの場合はモータの回転角度位置を受信する。具体的には、各受信器222bが各入力機器215からの信号を入力して、各測定値を受信制御部222aに出力する。受信制御部222aは、各測定値の補正を行い、指令値補正部223に出力する。
【0092】
指令値補正部223は、入力した測定値をもとに動作指令データBを補正する。例えば入力機器215が力覚センサの場合、力フィードバック制御が可能になり、入力機器215が視覚センサの場合、視覚フィードバック制御が可能になる。
【0093】
送信部224は、動作指令データBの指令値をデバイス207に送信する。デバイス207がモータの場合は、モータの関節角度を示す指令値を送信し、デバイス207がセンサの場合は信号の入力コマンドを示す指令値を送信する。具体的には、送信制御部224aが所定時刻に各指令値を出力し、各送信器224bが各出力機器214に出力する。各出力機器214は、入力した指令値に基づいて動作する。
【0094】
ロボット状態監視部417は、多関節ロボット本体100a,100bの姿勢を監視するものであり、順運動学モデルである第2のメカモデル418を用いて多関節ロボット本体100a,100b(ハンド)の位置姿勢を計算する。このハンドの位置姿勢の計算結果は、例えば予め設定した移動禁止領域に侵入したときに停止させる場合などの判定に使う。
【0095】
ここで、第1の処理部201Bは、第1のメカモデル409を用い、多関節ロボット本体100a,100bのハンドの位置姿勢から多関節ロボット本体100a,100bの関節角度を示す指令値を計算する。
【0096】
これに対し、第2の処理部202Bは多関節ロボット本体100a,100bの状態を監視するのに、第1のメカモデル409とは別の第2のメカモデル418を用いている。つまり、ロボット状態監視部417は、第2のメカモデル418を用いて、多関節ロボット本体100a,100bの関節角度から、順計算を実行して多関節ロボット本体100a,100bのハンドの位置を計算する。そして、ロボット状態監視部417は、その計算結果を、共有メモリ203において命令集合Aのメモリ領域及び動作指令データBのメモリ領域とは別のメモリ領域に格納する。
【0097】
従って、第1の処理部201Bと第2の処理部202Bとでそれぞれ独立した2つのメカモデル409,418があるので互いに干渉することがない。つまり、第2の処理部202Bが多関節ロボット本体100a,100b(ハンド)の位置姿勢を保持するメモリ内容を更新するが、命令集合Aのメモリ領域及び動作指令データBのメモリ領域とは別のメモリ領域にデータが格納されるので影響し合うことはない。
【0098】
次に、動作完了条件判定部225は、処理してきた命令内の動作完了条件をもとに、動作が完了したか判定する。動作の完了の判定方法は、例えば動作指令データの全指令値をバッファ部415から読み出し、デバイス207に送信したときに動作完了と判定する。または、多関節ロボット本体100a,100bのハンドの位置が予め設定した移動完了領域に侵入したときに動作完了と判定する。動作完了の場合は、第4の状態変数変更部304が、状態変数c5を動作中状態から動作完了状態に変更する。以上の第2の処理部202Bの処理を予め定めた同期時間δt毎に繰返し行う。
【0099】
したがって、上記第1,第2実施形態と同様に、第1の処理部201Bに軌道計算処理を実施させて計算結果である動作指令データBを共有メモリ203に格納させているので、第2の処理部202Bは、複雑な軌道計算処理を行う必要がない。したがって、第1の処理部201Bにより既に計算された動作指令データBの指令値を各モータMa1〜Ma6に同期して出力する同期処理を、多関節ロボット本体100a,100bの動作を停止させることなく実現することができる。ゆえに、多関節ロボット本体100a,100bに人間のような器用な動作を行わせることができる。
【0100】
[第4実施形態]
次に、本発明の第4実施形態に係るロボット制御装置について説明する。なお、ロボット制御装置の構成は、上記第1〜第3実施形態と同様である。図11は、ロボット制御装置により生成される動作指令データの例を示す図である。なお、多関節ロボット本体は、多軸であるので、各関節のモータに出力される複数本の指令値列があるが、説明のために1本の指令値列についてのみ図示している。多関節ロボット本体が同じ動作を繰り返す場合、第1回目の動作指令データと第2回目の動作指令データとが同じとなるので、第2回目以降は、動作指令データの計算を省略することが可能である。
【0101】
そこで、本第4実施形態では、計算部212(212B)は、計算開始条件判定部211(211A)が取得したコマンドから計算される動作指令データが、共有メモリ203に格納されている動作指令データBと一致するか否かを判断する。そして、計算部212(212B)は、一致すると判断した場合は、動作指令データの計算を省略する。
【0102】
ここで、共有メモリ203に格納した動作指令データは、一度、動作開始条件判定部221(221A)に読み出されたとしても、消去せずに残しておく。これにより、動作開始条件判定部221(221A)は、計算部212(212B)に計算が省略されたとしても、共有メモリ203に動作指令データが消去されずに残っているので、再び動作指令データを取得することができる。
【0103】
ここで、各命令C1,C2,C3…は、動作指令データが計算済みか否かを示すフラグを有しており、計算部212(212B)は、そのフラグを参照することで、計算済みか否かを判断することができる。このフラグは、計算部212(212B)により計算が終了した後、計算部212(212B)により変更される。
【0104】
以上、本第4実施形態では、第1回目の動作で計算された動作指令データは、第2回目以後、再利用されることとなる。このように、複雑な計算である軌道計算が省略されるので、多関節ロボット本体100をより高速に動作させることができ、第1の処理部201(201A,201B)の計算能力を温存することができる。
【0105】
[第5実施形態]
次に、本発明の第5実施形態に係るロボット制御装置について説明する。上記第4実施形態では、図11に示すように、連続する2つの動作指令データのうち、一方の動作指令データの終端値と、他方の動作指令データの始端値とが一致する場合について説明した。ところが、図12に示すように、連続する2つの動作指令データのうち、一方の動作指令データの終端値と、他方の動作指令データの始端値とが一致せず、差分Δfずれている場合もある。これが一致していないと2回目の動作指令データに基づく多関節ロボット本体の動作が滑らかとはならない。そこで、本第5実施形態では、オフセット値を加算する処理を行う。
【0106】
図13は、本第5実施形態に係るロボット制御装置200Cの各部の動作の流れを示す図である。なお、図13では、上記第2実施形態のロボット制御装置200Aと比較して動作開始条件判定部の動作が異なるものであり、その他の構成は、同一であるので、同一符号を付して説明を省略する。
【0107】
ロボット制御装置200Cの第2の処理部202Cは、上記第2実施形態と異なる動作開始条件判定部221Cを有している。具体的には、動作開始条件判定部221Cは、上記第2実施形態の動作開始条件判定部221Aと同様に動作する機能を有する他、オフセット加算部501を有している。
【0108】
つまり、動作開始条件判定部221Cのオフセット加算部501は、前回、共有メモリ203から取得した動作指令データ中の最後の指令値(終端値)と、今回、共有メモリ203から取得した動作指令データ中の最初の指令値(始端値)との差分Δfを計算する。そして、動作開始条件判定部221Cのオフセット加算部501は、差分Δfを初期値としてゼロまで漸次減少するオフセット値を、今回、共有メモリ203から取得した動作指令データ中の最初の指令値から時系列順に指令値に加算する補正を行う。
【0109】
つまり、オフセット加算部501は、動作指令データを再利用するとき、動作指令データの先頭データと、現在位置との差を計算し、その差を初期値とし、次第にゼロに減少するオフセット値を、動作指令データに加算する処理を行う。
【0110】
図14に、オフセット値を加算した動作指令データの一例を示す。初期値を差分Δfとし、次第にゼロに減少するオフセット値f(t)を動作指令データに加算すれば、滑らかな動作が可能となる。
【0111】
オフセット関数f(t)に必要な要件は次の2つある。それは、初期値が差分Δfに等しいことと、時間の経過とともにゼロに減少することである。例えば、差分をΔf、オフセット値がゼロになるまでの時間をT、再利用する動作指令データの最初の指令値が出力される時刻からの時間をtとすると、次のようなオフセット関数f(t)が有効である。
【0112】
【数1】
【0113】
この直線関数は計算量が少ないので計算機への負担が軽い。
【0114】
ただし、次の関数のほうがより滑らかである。
【0115】
【数2】
【0116】
コサイン関数なので、直線関数の場合よりも両端が滑らかである。したがって、より滑らかに多関節ロボット本体を動作させることができる。
【0117】
以上、上記第1〜第5実施形態に基づいて本発明を説明したが、本発明はこれに限定されるものではない。駆動部はサーボモータやリニアモータの他、ピエゾアクチュエータや空気圧を利用したシリンダなどであってもよい。一方、センサは、モータの回転角度を測定するエンコーダや変位計、カメラ、力を検出するロードセルなど、あらゆる種類のセンサが適用可能である。
【符号の説明】
【0118】
100…多関節ロボット本体、200,200A,200C…ロボット制御装置、201,201A,201B…第1の処理部、202,202A,202B,202C…第2の処理部、203…共有メモリ、211,211A…計算開始条件判定部、212,212B…計算部、215…入力機器(センサ)、221,221A,221C…動作開始条件判定部、222…受信部、223…指令値補正部、224…送信部、301…第1の状態変数変更部、302…第2の状態変数変更部、303…第3の状態変数変更部、413…バッファ書き込み部、415…バッファ部、416…バッファ読み出し部、417…ロボット状態監視部、B…動作指令データ、c1…動作記述部、c2…計算開始条件記述部、c3…動作開始条件記述部、c5…状態変数、Ma1〜Ma6…アーム用モータ(駆動部)
【特許請求の範囲】
【請求項1】
多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、
共有メモリと、前記共有メモリにアクセス可能に接続された第1の処理部及び第2の処理部と、を備え、
前記第1の処理部は、前記多関節ロボット本体を目標位置姿勢に動作させるコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算し、計算結果である動作指令データを前記共有メモリに格納する軌道計算処理を行い、
前記第2の処理部は、前記共有メモリに格納された動作指令データを取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する同期処理を行うことを特徴とするロボット制御装置。
【請求項2】
前記共有メモリには、
前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、
前記動作記述部に記述されたコマンドから動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、
前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、を含む命令が予め複数格納され、
前記第1の処理部は、
前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たしている命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、
前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、を有し、
前記第2の処理部は、
前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たしている命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、
前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とする請求項1に記載のロボット制御装置。
【請求項3】
前記共有メモリには、
前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、
前記動作記述部に記述されたコマンドから動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、
前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、
少なくとも計算開始待ち状態、計算中状態、計算完了状態及び動作中状態のうちのいずれかの状態となる状態変数と、を含む命令が予め複数格納され、
前記第1の処理部は、
前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たし、且つ状態変数が計算開始待ち状態である命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、
前記共有メモリに格納された命令のうち、前記計算開始条件判定部によりコマンドが取得された命令の状態変数を計算中状態に変更する第1の状態変数変更部と、
前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、
前記共有メモリに格納された命令のうち、前記計算部による計算が完了した動作指令データに対応する命令の状態変数を計算完了状態に変更する第2の状態変数変更部と、を有し、
前記第2の処理部は、
前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たし、且つ状態変数が計算完了状態である命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、
前記共有メモリに格納された命令のうち、前記動作開始条件判定部により取得された動作指令データに対応する命令の状態変数を動作中状態に変更する第3の状態変数変更部と、
前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とする請求項1に記載のロボット制御装置。
【請求項4】
前記第2の処理部は、
前記各駆動部に対応した数のトラックを有するバッファ部と、
前記動作開始条件判定部により取得された動作指令データの指令値を、前記各駆動部に対応するトラックに時系列順に連続して書き込むバッファ書き込み部と、
同時刻に出力する指令値を前記バッファ部の各トラックから読み出すバッファ読み出し部と、を有することを特徴とする請求項2又は3に記載のロボット制御装置。
【請求項5】
前記計算部は、前記計算開始条件判定部が取得したコマンドから計算される動作指令データが、前記共有メモリに格納されている動作指令データと一致すると判断した場合は、計算を省略することを特徴とする請求項2乃至4のいずれか1項に記載のロボット制御装置。
【請求項6】
前記動作開始条件判定部は、前回、前記共有メモリから取得した動作指令データ中の最後の指令値と、今回、前記共有メモリから取得した動作指令データ中の最初の指令値との差分を計算し、前記差分を初期値としてゼロまで漸次減少するオフセット値を、今回、前記共有メモリから取得した動作指令データ中の最初の指令値から時系列順に指令値に加算する補正を行うことを特徴とする請求項5に記載のロボット制御装置。
【請求項7】
前記第2の処理部は、前記多関節ロボット本体の姿勢を監視するロボット状態監視部を有し、
前記計算部は、逆運動学モデルを用いて動作指令データを計算し、
前記ロボット状態監視部は、順運動学モデルを用いて前記多関節ロボット本体の位置姿勢を計算することを特徴とする請求項2乃至6のいずれか1項に記載のロボット制御装置。
【請求項8】
前記多関節ロボット本体の状態を検知するセンサを備え、
前記第2の処理部は、
前記センサの検知結果を用いて前記動作開始条件判定部により取得された動作指令データの指令値を補正する指令値補正部を有することを特徴とする請求項2乃至7のいずれか1項に記載のロボット制御装置。
【請求項1】
多関節ロボット本体の各関節を駆動する複数の駆動部の各々に関節角度の指令値を出力して前記各駆動部の駆動動作を制御するロボット制御装置において、
共有メモリと、前記共有メモリにアクセス可能に接続された第1の処理部及び第2の処理部と、を備え、
前記第1の処理部は、前記多関節ロボット本体を目標位置姿勢に動作させるコマンドから前記各駆動部に出力する指令値の連なりを示す動作指令データを計算し、計算結果である動作指令データを前記共有メモリに格納する軌道計算処理を行い、
前記第2の処理部は、前記共有メモリに格納された動作指令データを取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する同期処理を行うことを特徴とするロボット制御装置。
【請求項2】
前記共有メモリには、
前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、
前記動作記述部に記述されたコマンドから動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、
前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、を含む命令が予め複数格納され、
前記第1の処理部は、
前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たしている命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、
前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、を有し、
前記第2の処理部は、
前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たしている命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、
前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とする請求項1に記載のロボット制御装置。
【請求項3】
前記共有メモリには、
前記多関節ロボット本体を目標位置姿勢に動作させるコマンドが記述された動作記述部と、
前記動作記述部に記述されたコマンドから動作指令データを計算する計算開始条件が記述された計算開始条件記述部と、
前記各駆動部を動作させる動作開始条件が記述された動作開始条件記述部と、
少なくとも計算開始待ち状態、計算中状態、計算完了状態及び動作中状態のうちのいずれかの状態となる状態変数と、を含む命令が予め複数格納され、
前記第1の処理部は、
前記共有メモリに格納された各命令の計算開始条件記述部を参照して、計算開始条件を満たし、且つ状態変数が計算開始待ち状態である命令の動作記述部に記述されているコマンドを前記共有メモリから取得する計算開始条件判定部と、
前記共有メモリに格納された命令のうち、前記計算開始条件判定部によりコマンドが取得された命令の状態変数を計算中状態に変更する第1の状態変数変更部と、
前記計算開始条件判定部により取得されたコマンドから動作指令データを計算し、この動作指令データを前記共有メモリに格納する計算部と、
前記共有メモリに格納された命令のうち、前記計算部による計算が完了した動作指令データに対応する命令の状態変数を計算完了状態に変更する第2の状態変数変更部と、を有し、
前記第2の処理部は、
前記共有メモリに格納された各命令の動作開始条件記述部を参照して、前記共有メモリに格納された動作指令データのうち、動作開始条件を満たし、且つ状態変数が計算完了状態である命令に対応する動作指令データを前記共有メモリから取得する動作開始条件判定部と、
前記共有メモリに格納された命令のうち、前記動作開始条件判定部により取得された動作指令データに対応する命令の状態変数を動作中状態に変更する第3の状態変数変更部と、
前記動作開始条件判定部により取得された動作指令データから前記各駆動部に対応する指令値を取得して、指令値を前記各駆動部に一定の時間間隔で同期して出力する送信部と、を有することを特徴とする請求項1に記載のロボット制御装置。
【請求項4】
前記第2の処理部は、
前記各駆動部に対応した数のトラックを有するバッファ部と、
前記動作開始条件判定部により取得された動作指令データの指令値を、前記各駆動部に対応するトラックに時系列順に連続して書き込むバッファ書き込み部と、
同時刻に出力する指令値を前記バッファ部の各トラックから読み出すバッファ読み出し部と、を有することを特徴とする請求項2又は3に記載のロボット制御装置。
【請求項5】
前記計算部は、前記計算開始条件判定部が取得したコマンドから計算される動作指令データが、前記共有メモリに格納されている動作指令データと一致すると判断した場合は、計算を省略することを特徴とする請求項2乃至4のいずれか1項に記載のロボット制御装置。
【請求項6】
前記動作開始条件判定部は、前回、前記共有メモリから取得した動作指令データ中の最後の指令値と、今回、前記共有メモリから取得した動作指令データ中の最初の指令値との差分を計算し、前記差分を初期値としてゼロまで漸次減少するオフセット値を、今回、前記共有メモリから取得した動作指令データ中の最初の指令値から時系列順に指令値に加算する補正を行うことを特徴とする請求項5に記載のロボット制御装置。
【請求項7】
前記第2の処理部は、前記多関節ロボット本体の姿勢を監視するロボット状態監視部を有し、
前記計算部は、逆運動学モデルを用いて動作指令データを計算し、
前記ロボット状態監視部は、順運動学モデルを用いて前記多関節ロボット本体の位置姿勢を計算することを特徴とする請求項2乃至6のいずれか1項に記載のロボット制御装置。
【請求項8】
前記多関節ロボット本体の状態を検知するセンサを備え、
前記第2の処理部は、
前記センサの検知結果を用いて前記動作開始条件判定部により取得された動作指令データの指令値を補正する指令値補正部を有することを特徴とする請求項2乃至7のいずれか1項に記載のロボット制御装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【公開番号】特開2012−96338(P2012−96338A)
【公開日】平成24年5月24日(2012.5.24)
【国際特許分類】
【出願番号】特願2010−248187(P2010−248187)
【出願日】平成22年11月5日(2010.11.5)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
【公開日】平成24年5月24日(2012.5.24)
【国際特許分類】
【出願日】平成22年11月5日(2010.11.5)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
[ Back to top ]