JP2019166623A - ロボット制御装置、ロボット制御方法、プログラム - Google Patents

Abstract

【課題】ロボットを直線移動させるときに、従来よりも高速に移動させる。【解決手段】本発明の一実施形態は、始点と終点を結ぶ直線のうち、所定の角速度に達するまでの加速区間、所定の角速度を維持する等速区間、および、所定の角速度から減速する減速区間を設定する区間設定部と、加速区間、等速区間、および、減速区間をそれぞれ複数のセグメントに分割するセグメント設定部であって、基準点の各セグメントの移動時間が実質的に同一となるように、前記速区間、等速区間、および、減速区間の各セグメントの距離を設定するセグメント設定部と、加速区間、等速区間、および、減速区間の各セグメントについて角度変化量が最大となる関節の角度変化量に基づいて、各セグメントにおいてＰＴＰ制御で移動させるときの各セグメントの角速度を設定する角速度設定部と、を備えたロボット制御装置である。【選択図】図３

Description

本発明は、ロボット制御装置、ロボット制御方法、プログラムに関する。

従来から、ロボットのアーム先端（手先）の位置を始点（教示点）から終点（教示点）まで移動させる場合の経路制御方式としてＰＴＰ(Point to Point)制御が知られている。また、ロボットのアームの先端の位置を直線経路で移動させる直線補間制御が知られている（特許文献１、２）。

特許文献１には、ロボットの現在位置情報を取得する手段と、ロボットの関節の定格最高速度情報を取得する手段と、上記ロボット現在位置情報と上記関節定格最高速度とに基づいてロボット先端の直線補間動作において取り得る許容限界速度を算出する手段と、を備えるロボットの速度演算装置が記載されている。
特許文献２には、あい前後する２つの移動区間の補間種別が関節補間動作から直線補間動作に移り変わるか、直線補間動作から関節補間動作に移り変わるかを判別し、その２つの移動区間の繋ぎ点もしくはその近傍を通過させる際に速度の結合処理が可能かどうかを判定し、速度の結合処理が可能であると判定されたとき、異種補間動作間の速度を重ね合わせるような挙動をさせるための補間点を関節変数により決定することが記載されている。

特開１９９７−２６５３１３号公報 特開２００４−２５２８１４号公報

ロボットのアーム先端を始点から終点まで移動させるときにはＰＴＰ制御を適用することで移動時間が最速となるが、ＰＴＰ制御では移動経路が保証されないため、ロボットの作業スペースにおいて他の物体や壁等に衝突する可能性がある。
直線補間制御では、ロボットのアーム先端を直線経路で移動させることができ、始点から終点までの経路を保証することができるが、従来は高速に移動させることができなかった。

そこで、本発明は、ロボットを直線移動させるときに、従来よりも高速に移動させることを目的とする。

本願の例示的な第１発明は、複数の関節を含む多関節ロボットの基準点を始点から終点まで直線補間で移動させるロボット制御装置であって、前記基準点が前記始点から加速して所定の角速度に達するまでの加速時間の要求値、および、前記基準点が前記所定の角速度から減速して前記終点に到達するまでの減速時間の要求値に基づいて、前記始点と前記終点を結ぶ直線のうち、加速区間、前記基準点が前記所定の角速度を維持する等速区間、および、減速区間を設定する区間設定部と、前記加速区間、前記等速区間、および、前記減速区間をそれぞれ複数のセグメントに分割するセグメント設定部であって、前記基準点の各セグメントの移動時間が実質的に同一となるように、前記加速区間、前記等速区間、および、前記減速区間の各セグメントの距離を設定するセグメント設定部と、前記加速区間、前記等速区間、および、前記減速区間の各セグメントについて角度変化量が最大となる関節の前記角度変化量に基づいて、前記基準点を各セグメントにおいてＰＴＰ（Point to Point）制御で移動させるときの各セグメントの角速度を設定する角速度設定部と、を備え、前記セグメント設定部は、前記角速度設定部によって設定された各セグメントの角速度に基づく加速時間と前記加速時間の要求値の第１差分値が第１閾値より大きい場合、前記第１差分値が前記第１閾値以下となるように前記加速区間のセグメント数または各セグメントの距離を再設定し、前記角速度設定部によって設定された各セグメントの角速度に基づく減速時間と前記減速時間の要求値との第２差分値が第２閾値より大きい場合、前記第２差分値が前記第２閾値以下となるように前記減速区間のセグメント数または各セグメントの距離を再設定する、ロボット制御装置である。

本発明によれば、ロボットを直線移動させるときに、従来よりも高速に移動させることができる。

図１は、第１の実施形態のロボットシステムの概略構成を示す図である。 図２は、第１の実施形態のロボットシステムの内部構成を示す図である。 図３は、第１の実施形態のロボット制御装置の機能ブロック図である。 図４は、ＰＴＰ制御で移動した場合に時間の経過に対する主関節の角速度の変化を示す図である。 図５は、加速区間および減速区間について説明する図である。 図６は、ＰＴＰ制御による時間の経過に対する角速度変化を示す図である。 図７は、加速区間の算出方法について説明する図である。 図８は、第１の実施形態のロボット制御方法による時間の経過に対する角速度変化を示す図である。 図９は、第１の実施形態のロボット制御装置によって実行されるフローチャートである。 図１０は、ＰＴＰ制御と第１の実施形態の制御における時間の経過に対する制御パルスの数の変化を説明する図である。 図１１は、第２の実施形態のロボット制御方法において、セグメント調整前後の時間の経過に対する角速度変化を示す図である。 図１２は、第１の実施形態のロボット制御装置によって実行されるフローチャートである。

以下、本発明の実施形態に係るロボット制御装置を含むロボットシステムについて説明する。
各実施形態に係るロボットシステムでは、ロボットの基準点として、ロボットのアーム先端、すなわち、エンドエフェクタの取付の基準となる位置を始点から終点まで移動させるときに、直線補間制御で移動させるように制御される。直線補間制御では、始点と終点を結ぶ直線が複数のセグメントに分割され、各セグメントではロボットをＰＴＰ制御で動作させる。このとき、各セグメントでのＰＴＰ動作は一定の角速度とし、各セグメントの角速度を最適に決定することで高速な疑似直線動作を実現する。

以下の説明において、ロボットの基準点とは、ロボットのアプローチ点、目標点、デパーチャ点等のロボットの教示点の基準となるロボットの位置を意味し、例えばロボットの作用点（ＴＣＰ：Tool Center Point）である。
以下の説明において、ロボットを２点間で移動させることは、ロボットの基準点を当該２点間で移動させることを意味する。

（１）第１の実施形態
（１−１）ロボットシステム１の構成
先ず、第１の実施形態のロボットシステム１の構成について、図１および図２を参照して説明する。図１は、本実施形態のロボットシステム１の概略構成を示す図である。図２は、本実施形態ロボットシステム１の内部構成を示す図である。

図１に示すように、ロボットシステム１は、情報処理装置２、ロボット制御装置３、および、ロボットＲを備える。情報処理装置２とロボット制御装置３とは、例えばイーサネット（登録商標）ケーブルＥＣにより通信可能に接続される。
情報処理装置２は、例えば工場のラインに設置されたロボットＲに対して動作を教示するための装置である。情報処理装置２は、オペレータによるティーチングを行うために設けられており、ロボットＲが設置される工場等から離れた位置（例えば、工場から離れたオペレータの作業場所）に配置されていてもよい。

情報処理装置２は、例えばパーソナルコンピュータ装置、タブレット型コンピュータ装置であり、オペレータによるオフラインティーチングあるいはオンラインティーチングを行うために設けられている。所定の教示プログラムを実行することで、ロボットＲの教示点や動作パラメータが決定される。

ロボット制御装置３は、情報処理装置２から送信されるロボットＲの教示点や動作パラメータを基にロボットプログラムを実行することでロボットＲを制御する。ロボットプログラムの実行結果として、ロボット制御装置３は、ロボットＲの各関節を動作させる複数のモータに対する制御パルスをロボットＲに対して送出する。

ロボットＲは、複数の関節を含む多関節ロボットである。ロボットＲは、ロボット制御装置３から受信する制御パルスに基づいて、各関節のモータを駆動し、情報処理装置２により決定された教示点や動作パラメータに従った動作を行う。

図２を参照すると、情報処理装置２は、制御部２１と、ストレージ２２と、入力装置２３と、表示装置２４と、通信インタフェース部２５とを備える。
制御部２１は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、および、ＲＡＭ（Random Access Memory）を含む。ＲＯＭには、教示ソフトウェアが記憶されている。ＣＰＵは、ＲＯＭに記憶される教示ソフトウェアをＲＡＭに展開して実行する。オペレータにより教示ソフトウェアを介して設定されたロボットＲの教示点や動作パラメータは、ロボットプログラムに組み込まれる。
ストレージ２２は、ＨＤＤ（Hard Disk Drive）あるいはＳＳＤ（Solid State Drive）等の大容量記憶装置であり、制御部２１のＣＰＵにより逐次アクセス可能に構成される。ストレージ２２には、ロボットプログラムが格納される。

入力装置２３は、オペレータによる操作入力を受け付けるためのデバイスであり、ポインティングデバイスを含む。
表示装置２４は、教示ソフトウェアの実行結果を表示するためのデバイスであり、表示駆動回路および表示パネルを含む。
通信インタフェース部２５は、ロボット制御装置３との間でイーサネット通信を行うための通信回路を含む。制御部２１は、オペレータによるロボットのシミュレーションの実行要求、またはロボットの実機を動作させる要求に応じて、ロボットＲの教示点や動作パラメータを含むロボットプログラムを、通信インタフェース部２５を介してロボット制御装置３に送信する。

図２に示すように、ロボット制御装置３は、制御部３１と、ストレージ３２と、通信インタフェース部３３とを備える。
制御部３１は、ＣＰＵ、ＲＯＭ、および、ＲＡＭを含む。ＣＰＵは、情報処理装置２から受信してストレージ３２に記憶されるロボットプログラムを、ＲＡＭに展開して実行する。制御部３１のＣＰＵがロボットプログラムを実行することで、後述する各機能が実現される。
制御部３１は、所定時間毎（例えば１ｍｓ毎）にロボットＲの各関節のモータを動作させるための制御パルスを生成して、ロボットＲに供給する。

ストレージ３２は、ＨＤＤあるいはＳＳＤ等の大容量記憶装置であり、制御部３１のＣＰＵにより逐次アクセス可能に構成される。ストレージ３２には、情報処理装置２から受信するロボットプログラムが格納されるとともに、ロボットプログラムの実行記録である実行ログデータが格納される。
通信インタフェース部３３は、情報処理装置２との間でイーサネット通信を行うための通信回路を含む。

図２に示すように、ロボットＲは、モータ駆動回路１０１およびモータ１０２を含む。モータ駆動回路１０１は、ロボット制御装置３から供給される制御パルスに基づいてモータ１０２を駆動するのに必要な駆動電圧を生成する。モータ１０２は、ロボットＲを動作させる関節の数だけ設けられる。
以下では、ロボットＲが６軸の垂直多関節ロボットである場合について説明する。この場合、ロボットＲには、６個の関節の各々を駆動するモータ１０２が設けられ、各モータ１０２に対してモータ駆動回路１０１から駆動電圧が供給される。

（１−２）ロボット制御装置３の機能
次に、図３〜８を参照して、ロボット制御装置３の制御部３１がロボットプログラムを実行して実現される機能について説明する。図３は、本実施形態のロボット制御装置３の機能ブロック図である。
図３に示すように、ロボット制御装置３によって実現される機能には、区間設定部３１１、セグメント設定部３１２、角速度設定部３１３、パルス数設定部３１４、および、パルス生成部３１５がある。以下、各部が備える機能について順に説明する。

以下では、ロボットの基準点が始点Ａから終点Ｂまで擬似的に直線移動させる場合について説明する。始点Ａ、終点Ｂは、以下において適宜、点Ａ，点Ｂという。
上述したように、ロボット制御装置３が情報処理装置２から受信するロボットプログラムには、ロボットＲの教示点や動作パラメータが設定されている。本実施形態では、ロボットプログラムにおいて教示点として始点Ａおよび終点Ｂが設定され、始点Ａから終点Ｂまでの移動に際して、動作パラメータとして加速時間、最大角速度、減速時間が設定されている。

以下の説明では、動作パラメータとして設定されている加速時間、最大角速度、および減速時間をそれぞれ、「加速時間要求値」、「最大角速度要求値」、および「減速時間要求値」という。加速時間要求値は、ロボットの基準点が始点Ａから加速して最大角速度（上記最大角速度要求値；所定の角速度の一例）に達するまでの加速時間の要求値である。減速時間要求値は、ロボットの基準点が最大角速度から減速して終点Ｂに到達するまでの減速時間の要求値である。

以下の説明において、ロボットの６個の関節のうち各関節の中心軸周りの角度変化量が最大となる関節を「主関節」という。６個の関節のうち主関節となる関節は、点Ａから点Ｂまでの直線移動において注目する時間あるいは範囲によって異なる場合がある。

（１−２−１）区間設定
区間設定部３１１は、始点Ａと終点Ｂを結ぶ直線のうち、ロボットの基準点が始点Ａから加速して最大角速度に達するまでの加速区間、ロボットの基準点が最大角速度を維持する等速区間、および、ロボットの基準点が最大角速度から減速して終点Ｂに到達するまでの減速区間を設定する機能を備える。
本実施形態の一例では、区間設定部３１１は、上述した加速時間要求値および減速時間要求値に基づいて、加速区間、等速区間、および減速区間を設定する。以下、図４を参照して、各区間の設定方法について説明する。

本実施形態では、ロボットを直線ＡＢ間において高速で移動させることを目的として、仮にロボットがＡＢ間をＰＴＰ制御で移動した場合の時間の経過に対する主関節の速度変化を模擬する。
図４は、仮にＡＢ間をＰＴＰ制御で移動した場合に、ＡＢ間における時間の経過に対する主関節の角速度の変化を示す図である。図４において、ロボットが点Ａを移動開始してから最大角速度要求値Ｖｍに達するまでの時間が加速時間要求値ｔ１に相当し、ロボットが時刻ｔ２から減速を開始して時刻ｔｆにおいて点Ｂに到達するまでの時間（ｔｆ−ｔ２）が減速時間要求値に相当する。
図４においてｔｆは、ＰＴＰ制御で移動した場合の総移動時間を意味する。

図５に、ロボットがＡＢ間を仮にＰＴＰ制御により移動する場合の経路Ｔ_ｐｔｐと、ロボットがＡＢ間を本実施形態の直線補間制御で移動する場合の経路Ｔ_ｓｌｎとを示す。
図５に示すように、ロボットがＡＢ間をＰＴＰ制御により移動する場合の経路Ｔ_ｐｔｐは、直線状とならず湾曲した経路となる。経路Ｔ_ｐｔｐ上において点Ａから点ｐ１までの区間はＰＴＰ制御における加速区間であり、点ｐ１から点ｐ２までの区間はＰＴＰ制御における等速区間であり、経路Ｔ_ｐｔｐ上において点ｐ２から点Ｂまでの区間はＰＴＰ制御における減速区間である。
点ｐ１，ｐ２は、図４の時刻ｔ１，ｔ２に対応している。つまり、ＰＴＰ制御では、点Ａから点ｐ１に達するまでの時間が加速時間要求値ｔ１となり、点ｐ２から点Ｂに達するまでの時間が減速時間要求値（ｔｆ−ｔ２）となる。

直線補間制御においても、点Ａから点Ｂまでの直線における加速区間および減速区間は、加速時間要求値および減速時間要求値を満たすように設定する必要がある。そこで、点Ａから点Ｂまでを仮にＰＴＰ制御で移動した場合の加速終了位置である点ｐ１と減速開始位置である点ｐ２を直線ＡＢ上に投影することで、直線補間制御における直線上の加速区間、減速区間を設定する。図４において、ＰＴＰ制御の経路Ｔ_ｐｔｐ上の点ｐ１，ｐ２を直線ＡＢ上に投影した点が、それぞれ点ｐ１ｓ，ｐ２ｓである。

本実施形態の直線補間制御による経路Ｔ_ｓｌｎにおいて、点Ａから点ｐ１ｓまでの区間が加速区間として設定され、点ｐ２ｓから点Ｂまでの区間が減速区間として設定される。
ここで、加速区間の距離acc_dstの直線ＡＢ間の距離に対する比率は、ＰＴＰ制御による経路Ｔ_ｐｔｐ上の加速区間の距離（点Ａと点ｐ１の距離）の、経路Ｔ_ｐｔｐの経路長に対する比率と同じとなるように、設定される。同様に、減速区間の距離dacc_dstの直線ＡＢ間の距離に対する比率は、ＰＴＰ制御による経路Ｔ_ｐｔｐ上の減速区間の距離（点ｐ２と点Ｂの距離）の、経路Ｔ_ｐｔｐの経路長に対する比率と同じとなるように、設定される。

例えば、経路Ｔ_ｓｌｎにおける加速区間の直線ＡＢ間の距離Ｔ_ｓｌｎに対する比率を加速区間比率acc_ratioとすると、加速区間比率acc_ratioは、式（１）のようにして算出される。
なお、式（１）において、
t1：加速時間要求値 (msec)
Vm：最大角速度要求値 (radian)
maxdst：主関節の角度変化量
である。主関節の角度変化量maxdstは、図４に示される台形の面積に相当する。

直線ＡＢ間で各関節の角度は線形に変化するため、点ｐ１におけるロボットの各関節の角度ｐ１［ｉ］（i：関節番号１〜６）は、加速区間比率acc_ratioを用いて、以下式（２）に従ってもとめられる。
なお、式（２）において、
A[i]：点Ａにおける関節iの角度 (radian)
B[i]：点Ｂにおける関節iの角度 (radian)
である。

式（２）により各関節の点ｐ１における角度がわかるため、順運動学により点ｐ１の直交座標を算出することができる。さらに、直線ＡＢ上の加速区間の距離acc_dst（図５参照）は、点Ａ、点ｐ１、および、点Ｂの直交座標の値から、以下のようにして算出される。
図６に示すように、点ｐ１と点ｐ１ｓの距離をｈ１とすると、ｈ１は、下記式（３）に従い、点Ａから点ｐ１に向かうベクトル（以下の式（３）の太字のｐ１）と、点Ａから点Ｂに向かうベクトル（式（３）の太字のＡＢ）との外積の大きさを、直線ＡＢ間の距離Ｌにより除算することによって算出される。
さらに、点Ａから点ｐ１に向かうベクトルの大きさをｎ１とすると、式（４）に従って加速区間の距離acc_dstが算出される。

以上、加速区間の距離acc_dstの算出方法について説明したが、減速区間の距離dacc_dst（図５参照）についても点ｐ２に基づいて同様にして算出することができる。
また、直線補間動作による等速区間の距離は、直線ＡＢ間の距離Ｌから加速区間の距離acc_dstと減速区間の距離dacc_dstとを減算することにより得られる。すなわち、等速区間の距離cst_dstは、以下の式（５）から算出される。

（１−２−２）セグメント設定
図３において、セグメント設定部３１２は、区間設定部３１１によって設定された加速区間、等速区間、および、減速区間をそれぞれ複数のセグメントに分割する機能を備える。このとき、セグメント設定部３１２は、ロボットの基準点の各セグメントの移動時間が実質的に同一となるように、加速区間、等速区間、および、減速区間の各セグメントの距離を設定する。
以下、図７を参照して、本実施形態の直線補間制御における加速区間、等速区間、および、減速区間の各セグメントの距離の設定方法について説明する。

各セグメントの距離を設定するに当たっては、ロボットの基準点の軌道の理想的な直線との誤差と、動作時間とを考慮して決定することが好ましい。すなわち、セグメントの距離が大きい場合には、各セグメントにおいてＰＴＰ制御がなされることから各セグメントにおけるロボットの基準点の軌跡が理想的な直線から乖離し、誤差が大きくなりやすい。その一方で、ロボットに対する制御が基準時間（例えば１ｍｓ）ごとに行われるために、セグメントの距離を小さくした場合には各セグメントで丸め誤差が生じ、その丸め誤差がセグメントの数だけ積算されることによって、動作時間が増加することになる。
そこで、加速区間の距離(acc_dst)、減速区間の距離(dacc_dst)、および、等速区間の距離cst_dstをそれぞれ複数のセグメントに分割するときの好ましい例では、1つのセグメントの距離の最大値を２０ｍｍ以下としつつ、各セグメントの距離が２０ｍｍと比較して著しく短い距離とならないように分割する。

（Ａ）加速区間
先ず、加速区間では、各セグメントの移動時間が極力一定となるように、点Ａから点ｐ１ｓに向かって徐々にセグメントの距離が長くなるように以下の設定を行う。
すなわち、加速区間の距離acc_dstが９０ｍｍ以下の場合には、加速区間のセグメント数acc_seg_num を９とし、加速区間の距離acc_dstが９０ｍｍより大きい場合には、加速区間のセグメント数acc_seg_num を以下の式（６）により算出する。

そして、加速区間の点Ａから点ｐ１ｓに向かって各セグメントのセグメント番号をｊ（j=0, 1, 2, …）としたときに、各セグメントの距離acc_seg[j]を以下の式（７）のようにして設定する。

（Ｂ）減速区間
次に、減速区間では、各セグメントの移動時間が極力一定となるように、点ｐ２ｓから点Ｂに向かって徐々にセグメントの距離が短くなるように以下の設定を行う。
すなわち、減速区間の距離dacc_dstが９０ｍｍ以下の場合には、減速区間のセグメント数dacc_seg_num を９とし、減速区間の距離dacc_dstが９０ｍｍより大きい場合には、減速区間のセグメント数dacc_seg_num を以下の式（８）により算出する。

そして、減速区間の点ｐ２ｓから点Ｂに向かって各セグメントのセグメント番号をｊ（j=0, 1, 2, …）としたときに、各セグメントの距離dacc_seg[j]を以下の式（９）のようにして設定する。

（Ｃ）等速区間
最後に、等速区間では、各セグメントの距離が一定で、かつ２０ｍｍ以下となるように設定する。
具体的には、等速区間の距離cst_dstを２０で除算した値が１．２５より小さい場合には、等速区間のセグメント数cst_seg_numを１とする。等速区間の距離cst_dstを２０で除算した値が１．２５以上である場合には、等速区間のセグメント数cst_seg_numを以下の式（１０）により算出する。各セグメントの距離は、式（１１）に示すとおりである。

なお、上述した各セグメントの距離の設定は一例に過ぎず、適宜変更可能である。例えば、上記式（７）に示したように、加速区間の各セグメントの距離は、隣接するセグメント間の距離が２倍ずつ増加するように設定されているが、その限りではない。隣接するセグメント間の距離の比率は、適宜調整してもよい。減速区間の各セグメントの距離についても同様である。

（１−２−３）各セグメントの角速度設定
角速度設定部３１３は、加速区間、等速区間、および、減速区間の各セグメントについて角度変化量が最大となる関節の角度変化量に基づいて、基準点を各セグメントにおいてＰＴＰ（Point to Point）制御で移動させるときの各セグメントの角速度を設定する機能を備える。

以下、セグメント設定部３１２によって設定された加速区間、等速区間、および、減速区間の各セグメントの主関節の角速度の設定方法について、図８を参照して説明する。図８は、本実施形態のロボット制御方法による時間の経過に対する主関節の角速度変化を示す図であり、各セグメントの主関節の角速度を棒状に表している。

既に述べたように、本実施形態の直線補間制御では、ロボットを直線ＡＢ間において高速で移動させることを目的として、仮にロボットがＡＢ間をＰＴＰ制御で移動した場合の時間の経過に対する主関節の速度変化を模擬する。図８において、台形状の太線は、図４に示したものと同じであり、ロボットをＡＢ間においてＰＴＰ制御で動作させたときの主関節の速度変化を示しており、この速度変化が本実施形態の直線補間動作において模擬される。つまり、本実施形態において各セグメントの主関節の角速度は、ＰＴＰ制御の場合の主関節の角速度の変化と一致するように設定される。
また、各セグメント内の主関節の角速度は一定とする。

本実施形態の直線補間制御では、セグメント毎に主関節（つまり、セグメントにおいて角度変化量が最大となる関節）が特定される。上述したように、各セグメントの距離が設定されて既知であるため、各セグメントの終端位置でのロボットの各関節の角度は逆運動学でもとめられ、それによって角度変化量が最大となる主関節が特定される。

（Ａ）加速区間
加速区間におけるセグメント番号をｊ（j=0, 1, 2, …）としたとき各セグメントの終端位置での時刻ｔは、以下の式（１２）に従って算出される。
なお、式（１２）において、
maxdst_seg[j]：セグメント番号がjのセグメントにおける主関節の角度変化量
jcur：現在のセグメント番号
t1：加速時間要求値 (msec)
Vm：最大角速度要求値 (radian)
である。

（Ｂ）等速区間
等速区間におけるセグメント番号をｊ（j=0, 1, 2, …）としたとき各セグメントの終端位置での時刻ｔは、以下の式（１３）に従って算出される。

（Ｃ）減速区間
減速区間におけるセグメント番号をｊ（j=0, 1, 2, …）としたとき各セグメントの終端位置での時刻ｔは、以下の式（１４）に従って算出される。
なお、式（１４）において、
total_dst：全体（点Ａから点Ｂまで）の主関節の角度変化量
t2：減速時間要求値 (msec)
tf：総移動時間 (msec)
である。total_dstは、図８の太線で示す台形の面積に等しい。

上述したようにして、加速区間、等速区間、および、減速区間における各セグメントの終端位置での時刻ｔを算出した後、各セグメントにおける主関節の角速度は、式（１５）に従って算出される。なお、式（１５）において、ｊ＝０の時にはｔ［ｊ−１］＝０であるとする。

（１−２−４）制御パルスのパルス数の設定
図３において、パルス数設定部３１４は、角速度設定部により設定された各セグメントの角速度に応じた、所定時間ごとのパルス数を設定する機能を有する。パルス生成部３１５は、ロボットの各関節を駆動するモータに対して供給される制御パルスを生成する機能を有する。このとき、制御パルスのパルス数を、パルス数設定部３１４により設定されたパルス数とする。
すなわち、各セグメントの角速度が決定されると、決定された角速度に応じた制御パルスのパルス数が、ロボットに対する制御が基準時間（例えば１ｍｓ）ごとに設定される。パルス数の決定方法は、決定された角速度が得られる制御パルスの数であれば如何なる決定方法でもよい。例えば、制御パルスの数は、決定された角速度を変数とした所定の関数演算により算出してもよいし、所定のルックアップテーブルを参照することにより得るようにしてもよい。

（１−３）本実施形態のロボット制御装置３の処理フロー
次に、本実施形態のロボット制御装置３の処理フローについて、図９を参照して説明する。図９は、本実施形態のロボット制御装置３によって実行されるフローチャートである。
既に述べたように、ロボット制御装置３は情報処理装置２からロボットプログラムを取得し、ロボット制御装置３の制御部３１は、取得したロボットプログラムを実行することによって図９に示す各処理が行われる。ロボットプログラムには、教示点として始点Ａおよび終点Ｂが設定され、始点Ａから終点Ｂまでの移動に際して、動作パラメータとして加速時間、最大角速度、減速時間の要求値が設定されている。
ロボット制御装置３の制御部３１は、上記動作パラメータに基づき、始点Ａから終点Ｂまでロボットの基準点を直線で移動させる直線補間制御を実行する。このとき、直線ＡＢ間の主関節の角速度変化が、仮にＡＢ間をＰＴＰ制御により移動させた場合の主関節の角速度変化と一致するようにして、各セグメントにおける主関節の角速度が決定される。

ロボット制御装置３の制御部３１は先ず、ロボットプログラムに設定された加速時間要求値、減速時間要求値、および、最大角速度要求値に基づいて、加速区間、等速区間、および、減速区間を設定する（ステップＳ１０）。加速区間、等速区間、および、減速区間の設定は、直線ＡＢ間における加速区間、等速区間、および、減速区間の比率が、仮にＡＢ間をＰＴＰ制御で移動した場合の経路における各区間の比率と同じになるようにして決定される。

次いで、制御部３１は、ステップＳ１０で設定した加速区間、等速区間、および、減速区間の各区間に対して複数のセグメントを設定する（ステップＳ１２）。すなわち、直線ＡＢにおける加速区間、等速区間、および、減速区間の各区間が、それぞれ複数のセグメントに分割され、各セグメントの距離が決定される。このとき、制御部３１は、上述したように、各セグメントの移動時間が実質的に一定となるようにして各セグメントの距離を決定する。加速区間では、点Ａから点Ｂに向けて各セグメントの距離が速度の上昇に応じて長くなるように設定され、減速区間では、点Ｂに向けて各セグメントの距離が速度の減少に応じて短くなるように設定される。

次いで、制御部３１は、各セグメントの主関節の角速度を設定する（ステップＳ１４）。各セグメントの主関節の角速度は、上述したように、仮にＡＢ間をＰＴＰ制御により移動させた場合の主関節の角速度変化と一致するようにして決定される。
以上のステップＳ１０〜Ｓ１４の処理によって、始点Ａから終点Ｂまでロボットの基準点を直線補間で移動させるときの各セグメントにおける主関節の速度が設定されたことになる。ロボット制御装置３の制御部３１は次いで、ステップＳ１４で設定した各セグメントの主関節の角速度が得られるように制御パルスの数を決定し（ステップＳ１６）、当該数の制御パルスを生成して（ステップＳ１８）、ロボットＲへ送出する。

以上説明したように、本実施形態のロボットシステム１では、ロボット制御装置３は、ロボットの基準点を直線補間によって移動させる場合に、ロボットの主関節の角度変化を、仮に当該直線をＰＴＰ制御で移動させた場合の主関節の角度変化と同一となるように制御する。そのため、ロボットを直線移動させるときに、高速に移動させることができる。

図１０に、例示的な２点間においてロボットをＰＴＰ制御で移動させる場合と、同じ２点間においてロボットを本実施形態の直線補間により移動させる場合との、時間の経過に伴って変化する制御パルスの数のシミュレーション結果を示す。図１０では、ロボットの６個の関節のうち角速度が高い関節Ｊ１〜Ｊ３に対する１ｍｓ毎の制御パルスの数が示される。図１０に示すように、ＰＴＰ制御の場合の主関節Ｊ１に対する制御パルスの数と、本実施形態の直線補間の場合の主関節Ｊ１（約５００ｍｓまで）および主関節Ｊ２（約５００ｍｓ以降）に対する制御パルスの数が概ね等しくなっており、２点間の移動時間が同程度となっていることがわかる。すなわち、本実施形態の直線補間制御では、全体をＰＴＰ制御で移動させた場合と同等の移動速度を達成できることが確認された。

（２）第２の実施形態
次に、第２の実施形態について説明する。
第１の実施形態では、加速区間および減速区間は、仮に始点から終点までの全体をＰＴＰ制御で移動した場合の主関節の角度変化量を基に設定される。しかし、加速区間および減速区間では主関節がセグメント単位で特定され、特定された主関節が、全体をＰＴＰ制御で移動させた場合の主関節と一致しない場合が生じうる。そのような場合、全体をＰＴＰ制御で移動させた場合の主関節の移動量に対して、セグメント単位の主関節の移動量をすべてのセグメントで合計した移動量が大きくなり、本実施形態による総移動時間が長くなってしまうことがある。すなわち、上記式（１２）および式（１４）においてΣの項が大きくなり、各セグメントの終端位置での時刻ｔが遅れることから、結果的に総移動時間が長くなってしまうことがある。
そこで、本実施形態では、加速時間要求値および減速時間要求値を満たすように、各セグメントの主関節の角度変化量に基づいて、加速区間および／または減速区間のセグメントを再設定することを特徴とする。

本実施形態のセグメント設定部３１２は、角速度設定部３１３によって設定された各セグメントの角速度に基づく加速時間と加速時間要求値の第１差分値が第１閾値より大きい場合、第１差分値が第１閾値以下となるように加速区間のセグメント数または各セグメントの距離を再設定する。
また、本実施形態のセグメント設定部３１２は、角速度設定部３１３によって設定された各セグメントの角速度に基づく減速時間と減速時間要求値との第２差分値が第２閾値より大きい場合、第２差分値が第２閾値以下となるように減速区間のセグメント数または各セグメントの距離を再設定する。
なお、第１閾値および第２閾値は、それぞれ加速時間要求値および減速時間要求値との乖離度合がシステム上許容可能なレベルに応じて適宜設定可能である。

以下、本実施形態のセグメント設定部３１２によって行われるセグメント数または各セグメントの距離の再設定の例を、図１１を参照して説明する。図１１は、本実施形態のロボット制御方法において、セグメント調整前後の時間の経過に対する主関節の角速度変化を示す図である。
図１１において、セグメント調整前の場合には、加速区間は各セグメントに対して特定された主関節が、全体をＰＴＰ制御で移動させた場合の主関節と一致しないことから、直線補間制御による加速時間が加速時間要求値ｔ１よりも長くなっている。そのため、総移動時間ｔｆが長くなっている。

それに対して本実施形態では、セグメント設定部３１２は、加速区間の各セグメントの距離を短くするか、またはセグメント数を少なくし、それによって加速区間が以下の条件式（１６）を満たすようにする。例えば各セグメントの距離の調整は、上記式（６），（７）の定数を変更することで可能である。
なお、条件式（１６）において、
maxdst_seg[j]：セグメント番号がjのセグメントにおける主関節の角度変化量
jcur：現在のセグメント番号
t1：加速時間要求値 (msec)
Vm：最大角速度要求値 (radian)
である。

セグメント調整前の加速区間の各セグメントの距離では、各セグメントに設定される角速度が図１１のＰＴＰ制御による太線で示す角速度よりも速く、条件式（１６）を満たさない。
そこで、加速区間の各セグメントの距離を短くするか、または加速時間のセグメント数を少なくすることで、条件式（１６）を満たすことが可能となる。その結果、セグメント調整後の加速区間では、図１１に示すように、セグメント調整前と比較して加速が緩やかとなり、加速時間要求値を満たすようになることがわかる。
上記式（１６）の右辺から左辺を引いた値が、第１差分値の一例である。

同様には、セグメント調整前の減速区間では、各セグメントに対して特定された主関節が、全体をＰＴＰ制御で移動させた場合の主関節と必ずしも一致しないことから、加速区間と同様に、本実施形態による総移動時間が長くなってしまうことがある。
そこで、セグメント設定部３１２は、減速区間の各セグメントの距離を短くするか、セグメント数を少なくすることで、以下の条件式（１７）を満たすようにする。例えば各セグメントの距離の調整は、上記式（８），（９）の定数を変更することで可能である。

なお、条件式（１７）において、
maxdst_seg[j]：セグメント番号がjのセグメントにおける主関節の角度変化量
jcur：現在のセグメント番号
t2：減速時間要求値 (msec)
tf：総移動時間 (msec)
Vm：最大角速度要求値 (radian)
である。
上記式（１７）の右辺から左辺を引いた値が、第２差分値の一例である。

本実施形態のロボット制御装置３の処理フローを図１２に示す。図１２は、本実施形態のロボット制御装置３によって実行されるフローチャートである。
図１２に示すフローチャートが図９と異なるのは、ステップＳ１５が追加された点である。
ロボット制御装置３の制御部３１は、各セグメントの主関節の角速度を設定すると（ステップＳ１４）、加速時間要求値および減速時間要求値を満たすか否か（つまり、式（１６）および式（１７）を満たすか否か）判断する（ステップＳ１５）。満たさない場合にはステップＳ１２に戻り、加速区間および／または減速区間のセグメント数または各セグメントの距離を変更する。ステップＳ１２，Ｓ１４の処理は、ステップＳ１５の条件式が満たされるまで行われる。
このとき、ステップＳ１５では、上記式（１６）の右辺から左辺を引いた値が所定の第１閾値以下であり、かつ上記式（１７）の右辺から左辺を引いた値が所定の第２閾値以下であることを収束条件としてもよい。

（３）第３の実施形態
次に、第３の実施形態について説明する。
第１の実施形態のロボット制御装置３において設定される主関節の角速度は、各セグメント内では一定であるため、セグメント間において主関節の角速度が大きく変化する場合がある。しかし、大きな角速度の変化は、ロボットに振動を生じさせることがあり好ましくない。そこで、本実施形態では、ロボットに振動が生じ難くなるように、主関節の角速度の変化を平滑化する。
上記観点から、本実施形態のロボット制御装置３は、パルス数設定部３１４により設定されたパルス数に対してフィルタ処理を行うフィルタ処理部をさらに備える。そして、パルス生成部３１５は、制御パルスのパルス数を、フィルタ処理後のパルス数に設定する。

フィルタ処理の方法は特に限定しないが、例えば所定回数の基準時間（例えば１ｍｓ）に対して設定されたパルス数に基づく移動平均フィルタを利用することができる。移動平均フィルタに使用するパルス数のサンプルの数は問わないが、平滑化の効果を高めるためにはサンプル数が十分に多いことが好ましい一方で、サンプル数が多過ぎると移動時間が長くなる。好適な一例では、サンプル数ｎは１０である。

具体的には、関節番号ｉ（ｉ＝１〜６）の関節に対するｋ番目の制御パルスの数をpls_new[i][k]とすると、パルス数pls_new[i][k]は、式（１８）に従ってもとめられる。
なお、式（１８）において、
pls_new：移動平均フィルタ適用後の制御パルスの数
pls：移動平均フィルタ適用前の制御パルスの数
i：関節番号 (1, 2, 3, 4, 5, 6)
k：動作パルスの生成回数
n：移動平均に使用するサンプル数
である。

以上、本発明のロボット制御装置の複数の実施形態について詳述したが、本発明は上記の各実施形態に限定されない。また、上記の実施形態は、本発明の主旨を逸脱しない範囲において、種々の改良や変更が可能である。例えば、各実施形態について言及した技術的事項は、技術的矛盾が生じない限り、異なる実施形態の間で適宜組み合わせてもよい。

上述した説明により、図３の機能ブロック図に記載された機能のうち少なくとも一部の機能をコンピュータに実現させるためのプログラム、および、当該プログラムが記録されたコンピュータ可読記憶媒体（不揮発性の記憶媒体をも含む。）が開示されていることは当業者に理解される。

１…ロボットシステム、２…情報処理装置、２１…制御部、２２…ストレージ、２３…入力装置、２４…表示装置、２５…通信インタフェース部、３…ロボット制御装置、３１…制御部、３１１…区間設定部、３１２…セグメント設定部、３１３…角速度設定部、３１４…パルス数設定部、３１５…パルス生成部、３２…ストレージ、３３…通信インタフェース部、ＥＣ…イーサネットケーブル、ＷＣ…ケーブル、Ｒ…ロボット、１０１…モータ駆動回路、１０２…モータ

Claims (5)

1. 複数の関節を含む多関節ロボットの基準点を始点から終点まで直線補間で移動させるロボット制御装置であって、
   前記基準点が前記始点から加速して所定の角速度に達するまでの加速時間の要求値、および、前記基準点が前記所定の角速度から減速して前記終点に到達するまでの減速時間の要求値に基づいて、前記始点と前記終点を結ぶ直線のうち、加速区間、前記基準点が前記所定の角速度を維持する等速区間、および、減速区間を設定する区間設定部と、
   前記加速区間、前記等速区間、および、前記減速区間をそれぞれ複数のセグメントに分割するセグメント設定部であって、前記基準点の各セグメントの移動時間が実質的に同一となるように、前記加速区間、前記等速区間、および、前記減速区間の各セグメントの距離を設定するセグメント設定部と、
   前記加速区間、前記等速区間、および、前記減速区間の各セグメントについて角度変化量が最大となる関節の前記角度変化量に基づいて、前記基準点を各セグメントにおいてＰＴＰ（Point to Point）制御で移動させるときの各セグメントの角速度を設定する角速度設定部と、
   を備え、
   前記セグメント設定部は、
   前記角速度設定部によって設定された各セグメントの角速度に基づく加速時間と前記加速時間の要求値の第１差分値が第１閾値より大きい場合、前記第１差分値が前記第１閾値以下となるように前記加速区間のセグメント数または各セグメントの距離を再設定し、
   前記角速度設定部によって設定された各セグメントの角速度に基づく減速時間と前記減速時間の要求値との第２差分値が第２閾値より大きい場合、前記第２差分値が前記第２閾値以下となるように前記減速区間のセグメント数または各セグメントの距離を再設定する、
   ロボット制御装置。
2. 前記角速度設定部により設定された各セグメントの角速度に応じた、所定時間ごとのパルス数を設定するパルス数設定部と、
   前記パルス数設定部により設定されたパルス数に対してフィルタ処理を行うフィルタ処理部と、
   前記ロボットの各関節を駆動するモータに対して供給される制御パルスを生成するパルス生成部であって、前記制御パルスのパルス数を、前記フィルタ処理後のパルス数に設定する前記パルス生成部と、
   請求項１に記載されたロボット制御装置。
3. 前記フィルタ処理は、所定回数の前記所定時間に対して設定されたパルス数に基づく移動平均フィルタの処理である、
   請求項２に記載されたロボット制御装置。
4. 複数の関節を含む多関節ロボットの基準点を始点から終点まで直線補間で移動させるロボット制御方法であって、
   前記基準点が前記始点から加速して所定の角速度に達するまでの加速時間の要求値、および、前記基準点が前記所定の角速度から減速して前記終点に到達するまでの減速時間の要求値に基づいて、前記始点と前記終点を結ぶ直線のうち、加速区間、前記基準点が前記所定の角速度を維持する等速区間、および、減速区間を設定し、
   前記加速区間、前記等速区間、および、前記減速区間をそれぞれ複数のセグメントに分割し、前記基準点の各セグメントの移動時間が実質的に同一となるように、前記加速区間、前記等速区間、および、前記減速区間の各セグメントの距離を設定し、
   前記加速区間、前記等速区間、および、前記減速区間の各セグメントについて角度変化量が最大となる関節の前記角度変化量に基づいて、前記基準点を各セグメントにおいてＰＴＰ（Point to Point）制御で移動させるときの各セグメントの角速度を設定し、
   前記設定された各セグメントの角速度に基づく加速時間と前記加速時間の要求値の第１差分値が第１閾値より大きい場合、前記第１差分値が前記第１閾値以下となるように前記加速区間のセグメント数または各セグメントの距離を再設定し、
   前記設定された各セグメントの角速度に基づく減速時間と前記減速時間の要求値との第２差分値が第２閾値より大きい場合、前記第２差分値が前記第２閾値以下となるように前記減速区間のセグメント数または各セグメントの距離を再設定する、
   ロボット制御方法。
5. 複数の関節を含む多関節ロボットの基準点を始点から終点まで直線補間で移動させるプログラムであって、
   コンピュータに、
   前記基準点が前記始点から加速して所定の角速度に達するまでの加速時間の要求値、および、前記基準点が前記所定の角速度から減速して前記終点に到達するまでの減速時間の要求値に基づいて、前記始点と前記終点を結ぶ直線のうち、加速区間、前記基準点が前記所定の角速度を維持する等速区間、および、減速区間を設定する手順、
   前記加速区間、前記等速区間、および、前記減速区間をそれぞれ複数のセグメントに分割し、前記基準点の各セグメントの移動時間が実質的に同一となるように、前記加速区間、前記等速区間、および、前記減速区間の各セグメントの距離を設定する手順、
   前記加速区間、前記等速区間、および、前記減速区間の各セグメントについて角度変化量が最大となる関節の前記角度変化量に基づいて、前記基準点を各セグメントにおいてＰＴＰ（Point to Point）制御で移動させるときの各セグメントの角速度を設定する手順、
   前記設定された各セグメントの角速度に基づく加速時間と前記加速時間の要求値の第１差分値が第１閾値より大きい場合、前記第１差分値が前記第１閾値以下となるように前記加速区間のセグメント数または各セグメントの距離を再設定する手順、
   前記設定された各セグメントの角速度に基づく減速時間と前記減速時間の要求値との第２差分値が第２閾値より大きい場合、前記第２差分値が前記第２閾値以下となるように前記減速区間のセグメント数または各セグメントの距離を再設定する手順、
   を実行させるためのプログラム。

Images