JP7424097B2 - ロボットの制御装置及びロボットの制御方法 - Google Patents

Description

本発明は、ロボットを実際に動作させる際の移動軌跡が、教示点上を通過するように制御する装置及び方法に関する。

ロボットの実際の動作が、予め演算したサンプリングタイム毎の目標位置に達するようにサーボモータを制御するため、間欠的に与えられた教示点を通ってロボット先端を移動させる制御方法に、ＣＰ(continuous path)方式がある。またロボットの動作は、ある動作から次の動作へ移る際における前の動作の完了を判断する方法により、エンド動作とパス動作とに分かれる。

例えば図９に示すように、３つの教示点Ｐ１～Ｐ３があるとすると、ＣＰ方式のエンド動作では、ロボットの先端が教示点に到達してから次の動作へと移る。そのため、Ｐ１→Ｐ２間とＰ２→Ｐ３間とのそれぞれが直線状の軌跡となるように、ロボットの先端を動作させる。それに伴い、Ｐ１→Ｐ２間，Ｐ２→Ｐ３間それぞれにおけるロボットの動作速度が変化するパターンは、何れも加速期間，定速期間，減速期間を組み合わせた台形状になる。

これに対して、ＣＰ方式のパス動作では、ロボットの先端が教示点に到達する前に次の動作へと移る。そのため、Ｐ１→Ｐ２間の減速期間に併せて、Ｐ２→Ｐ３間の加速期間を開始させる。その結果、ロボットの先端は教示点Ｐ２上を通過せず、その内側，つまり図中の右側を通過する動作となり、これらを合成した軌跡は図中に破線で示したものとなる。

特許第３６６６３４１号公報

ここで、ロボットを製品の組み付け作業に適用する場合を想定する。教示点Ｐ２から教示点Ｐ３への移動が組み付け作業における最終のアプローチ動作である際に、ワークの形状や周囲の環境によっては、ロボットの先端が、周囲に障害物が配置されているように比較的狭い空間を移動する場合がある。すると、Ｐ２→Ｐ３間の動作が直線であればロボット先端は問題なく移動できるが、ＣＰ方式のパス動作により教示点Ｐ２の内側を通過する軌跡になると、ロボットと教示点Ｐ２の内側に位置する障害物とが干渉するおそれがある。

本発明は、上記実情に鑑みてなされたものであり、その目的は、ＣＰ方式のパス動作を採用してロボットの先端が全ての教示点を通過しない軌跡で動作する際に、周囲にある障害物との干渉を防止できるロボットの制御装置及びロボットの制御方法を提供することにある。

請求項１記載のロボットの制御装置によれば、制御部は、ロボット先端が、移動を開始する第１教示点から移動を終了する第３教示点に向って連続的に移動するパス動作を行うように教示データを用いてロボットを制御する。教示点設定部は、前記パス動作を行うロボット先端の連続移動軌跡が、第１教示点と第３教示点との間に設定されている第２教示点に重なるように、第３教示点から第２教示点を通過して延長した直線上に第４教示点を設定する。そして、第４教示点の教示データを用いて制御部に、第１教示点から第４教示点に向かってパス動作を行うように制御させる。

当初に設定された第１～第３教示点についてＣＰ方式のパス動作によりロボット先端を移動させると、上述したように、第１教示点から第２教示点に至るまでの減速期間と第２教示点から第３教示点に向けて設定される加速期間とが重複する結果、連続移動軌跡は第２教示点を通過しなくなる。そして、第２教示点と第３教示点とを結ぶ直線Ｐ２－Ｐ３上において、ロボット先端が直線的に移動する距離は、直線Ｐ２－Ｐ３の長さよりも短くなる。

したがって、教示点設定部が、第３教示点から第２教示点を通過して延長した直線上に第４教示点を設定し、その第４教示点の教示データを用いて制御部に、第１教示点から第４教示点に向かってパス動作を行うように制御させれば、ロボット先端は第２教示点に到達するようになり、その第２教示点から第３教示点に向かって直線的に移動する。これにより、第２教示点から第３教示点に向かう経路の周辺に障害物等が存在しており、ロボット先端が比較的狭い空間を移動する際にも、第２，第３教示点間は直線的に移動することで障害物との干渉が生じることを回避できる。

また、請求項１記載のロボットの制御装置によれば、教示点設定部は、第２教示点を起点として、パス動作の連続移動軌跡が、第２教示点から第３教示点に向かう直線に合流する点までの距離Ｌｐａを計算すると、第４教示点を、第２教示点より距離Ｌｐａだけ延長した直線上に設定する。上述のように、当初に設定された第１～第３教示点についてロボット先端を移動させると、直線Ｐ２－Ｐ３上でロボット先端が直線的に移動する距離は、直線Ｐ２－Ｐ３の長さよりも短くなるので、その短くなる分の距離がＬｐａとして求められる。そして、直線Ｐ２－Ｐ３の長さに距離Ｌｐａの長さを加えた位置に第４教示点を設定し、ロボットにパス動作を行わせれば、ロボット先端の連続移動軌跡が第２教示点を通過するようになる。

更に、請求項１記載のロボットの制御装置によれば、教示点設定部は、制御部が、第１教示点から第４教示点に向かうパス動作を行う際の連続移動軌跡を再計算した結果について距離Ｌｐａを再度計算する。そして、再計算した距離Ｌｐａと先に計算した距離Ｌｐａとの差を求め、求めた差が閾値以上であれば、その差分を再計算した距離Ｌｐａに加算して距離Ｌｐａを更新する。それから、更新した距離Ｌｐａに基づいて第４教示点を再度設定し、制御部にパス動作の制御を再実行させる。

このように構成すれば、誤差要因が存在するため、当初に求めた距離Ｌｐａと第２，第４教示点間の距離との差が閾値以上であれば差分に応じて距離Ｌｐａを更新し、パス動作の制御を再実行させることを繰り返して距離Ｌｐａに含まれる誤差を縮小できる。これにより、第４教示点を精度良く設定することが可能になる。

一実施形態であり、ロボットシステムの構成を示す図 コントローラの構成を示す機能ブロック図 コントローラの処理内容を要旨に係る部分について示すフローチャート 当初の教示点Ｐ１～Ｐ３に対してパス動作を行う際のロボット先端の移動軌跡を示す図 教示点Ｐ２を新たな教示点Ｐ２＿ｎに置き換えてパス動作を行う際のロボット先端の移動軌跡を示す図 本実施形態における処理概念を速度パターンにより説明する図（その１） 同処理概念を速度パターンにより説明する図（その２） 同処理概念を速度パターンにより説明する図（その３） ＣＰ方式のエンド動作及びパス動作を説明する図

以下、一実施形態について説明する。図１に示すように、ロボットシステム１は、多関節型のロボット２、ロボット２を制御するコントローラ３、コントローラ３に接続されたティーチングペンダント４を備えている。このロボットシステム１は、一般的な産業用に用いられている。

ロボット２は、いわゆる６軸の垂直多関節型ロボットとして周知の構成を備えており、ベース５上に、Ｚ方向の軸心を持つ第１軸（Ｊ１）を介してショルダ６が水平方向に回転可能に連結されている。ショルダ６には、Ｙ方向の軸心を持つ第２軸（Ｊ２）を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸（Ｊ３）を介して第一上アーム８が垂直方向に回転可能に連結されている。第一上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸（Ｊ４）を介して第二上アーム９が捻り回転可能に連結されている。第二上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸（Ｊ５）を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸（Ｊ６）を介してフランジ１１が捻り回転可能に連結されている。

ベース５、ショルダ６、下アーム７、第一上アーム８、第二上アーム９、手首１０及びフランジ１１は、ロボット２のアームとして機能し、アームの先端となるフランジ１１には、図示は省略するが、エンドエフェクタとも呼ばれるハンドが取り付けられる。ハンドは、例えば図示しないワークを保持して移送したり、ワークを加工する工具等が取り付けられたりする。ロボット２に設けられている各軸Ｊ１～Ｊ６には、それぞれに対応して駆動源となる、図２に示すモータ１２が設けられている。

コントローラ３は、ロボットの制御装置に相当し、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御部においてコンピュータプログラムを実行することで、ロボット２を制御している。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータ１２に対応して設けられているエンコーダ１７で検知したモータの回転位置に基づいて例えばフィードバック制御によりモータ１２を駆動する。

ティーチングペンダント４は、例えば概ね略矩形箱状に形成されており、ユーザが所持したまま操作可能な程度の大きさに形成されている。このティーチングペンダント４には、各種キースイッチやタッチパネル等が設けられており、ユーザは、それらキースイッチやタッチパネル等を用いてティーチングを行う。

コントローラ３は、図２に示すように、ＣＰＵを主体とする制御部１３と、記憶部１４と、位置検出部１５とを備えている。制御部１３は、産業用ロボット装置１の全体を統括制御する。制御部１３には、記憶部１４及び位置検出部１５の他、前記ティーチングペンダント４及びモータ１２の駆動回路１６が接続されている。記憶部１４には、各アーム６～１１を駆動するモータ１２についての最大加速度（＋α）、最大減速度（－α）、最高速度Ｖｍａｘ等のパラメータ、ロボット制御用の各種ソフトウエアが予め記憶されている。また、記憶部１４には、実際のロボットの作業に際しては、ティーチングペンダント４により設定される動作プログラムや、実際の動作に先立って生成されるロボット先端の移動軌跡情報などが記憶されている。制御部１３は、教示点設定部に相当する。

位置検出部１５には、ショルダ６やアーム７～９等に対応する各モータ１２の回転軸に連結されたロータリエンコーダ１７が接続されている。位置検出部１５は、ロータリエンコーダ１７から入力される回転検出信号に基づいてモータ１２の回転角度，すなわち軸値を検出し、その回転位置情報を制御部１３に与える。制御部１３は、モータ１２の回転角度情報から対応するアーム等の回転角を演算すると、その回転角を目標角度と比較して差分に応じた電流指令値を駆動回路１６に与える。すると、駆動回路１６は、与えられた電流指令値に応じた電流をモータ１２に供給する。これによりモータ１２の回転が制御され、各アーム６～１１等が目標角度に回転される。

次に、本実施形態の作用について説明する。図３は、コントローラ３の制御部１３が、ロボット２の先端の連続移動軌跡を求める処理を、要旨に関する部分について示すフローチャートである。制御部１３は、先ず、パス動作後のアプローチ動作か否かを判断する（Ｓ１）。「アプローチ動作」とは、図４に示すように、連続移動軌跡における末部の３つの教示点Ｐ１～Ｐ３のうち、教示点Ｐ２からＰ３に至る区間であり、教示点Ｐ３は、ロボット２が組み付け作業等を行う際にその先端が最終的に到達する位置である。また「パス動作」は、同図に破線で示すように、教示点Ｐ１からＰ２に至る間に、ロボット先端が直線的に移動する区間を外れて教示点Ｐ２の内側を通過し、アプローチ動作区間の直線軌跡に繋がるまでの区間である。

パス動作後のアプローチ動作であれば（Ｓ１；ＹＥＳ）、アプローチ動作区間内でパス動作が行われる距離Ｌｐａを計算する（Ｓ２）。パス動作区間開始時のロボット先端の速度をＶｍ，減速度をＤｍ，減速時間をＴｐとすると、減速時間Ｔｐは、
Ｔｐ＝Ｖｍ／Ｄｍ …（１）
で求められる。そして、アプローチ動作におけるロボット先端の加速度をＡａとすると、パス動作距離Ｌｐａは、
Ｌｐａ＝Ａａ×Ｔｐ^２／２ …（２）
で求められる。

次に、パス動作距離Ｌｐａ＿ｎに、ステップＳ２で求めた距離Ｌｐａを代入して初期化すると（Ｓ３）、アプローチ動作の開始点Ｘａ，つまり教示点Ｐ２の座標を、距離Ｌｐａ＿ｎだけアプローチベクトル方向に移動した座標を、新たな開始点Ｘａ＿ｎｅｗとするように計算する（Ｓ４）。図５では、開始点Ｘａ＿ｎｅｗを「Ｐ２＿ｎｅｗ」としている。アプローチ動作のベクトルを（ａｘ，ａｙ，ａｚ）とし、開始点Ｘａ＝（ｘａ，ｙａ，ｚａ）とすると、新たな開始点Ｘａ＿ｎｅｗは、
Ｘａ＿ｎｅｗ＝Ｘａ－Ｌｐａ×（ａｘ，ａｙ，ａｚ） …（３）
で求められる。尚、教示点Ｐ１～Ｐ３は第１～第３教示点に相当し、上記のＰ２＿ｎｅｗは第４教示点に相当する。

それから、教示点Ｐ２を、開始点Ｘａ＿ｎｅｗ，Ｐ２＿ｎｅｗに置き換えることで生じるパス動作について、パス動作距離Ｌｐａ＿ｎを計算する（Ｓ５）。そして、ステップＳ２で求めた距離Ｌｐａの絶対値と、ステップＳ４で求めた距離Ｌｐａ＿ｎの絶対値との差を求め、その差の絶対値が閾値ｄｅｌｔａ未満か否かを判断する（Ｓ６）。閾値ｄｅｌｔａはゼロ近傍の値である。前記差の絶対値が閾値ｄｅｌｔａ以上であれば（ＮＯ）、パス動作距離Ｌｐａ＿ｎを、当該距離Ｌｐａ＿ｎに前記差の絶対値を加えて更新してから（Ｓ８）ステップＳ４に戻る。

一方、ステップＳ６において、前記差の絶対値が閾値ｄｅｌｔａ未満であれば（ＹＥＳ）、その時点の開始点Ｘａ＿ｎｅｗをアプローチ動作の開始点に設定してロボット２の動作を実行する（Ｓ７）。

上記の処理の概念について、図６から図８に示す速度パターンのグラフを用いて説明する。図６は、図４に示す動作に対応したもので、ロボット先端が教示点Ｐ１からＰ２に向かう際の減速期間の開始に合わせて、教示点Ｐ２からＰ３に向かう際の加速期間を開始させる結果、その期間の合成速度は破線で示すものとなる。そして、この期間にロボット先端が移動する距離のうち、加速による移動距離分だけ、つまり図７に示すハッチングで示す三角形の面積分だけ教示点Ｐ２に到達せずに内回りする移動軌跡となる。

そこで、図８に示すように、教示点Ｐ２においてパス動作が完了するように、つまり減速期間及び加速期間が共に終了するように新たな目標点Ｐ２＿ｎｅｗを設定する。これは、上述した加速による移動距離分を示す三角形の面積に等しい分だけ移動距離を延ばすように、新たな目標点Ｐ２＿ｎｅｗを設定することを意味する。これにより、ロボット先端は、元の教示点Ｐ２に到達してから終了点Ｐ３に向かって直線上を移動するように動作する。

以上のように本実施形態によれば、コントローラ３の制御部１３は、ロボット先端が、移動を開始する教示点Ｐ１から移動を終了する教示点Ｐ３に向って連続的に移動するパス動作を行うように教示データを用いてロボット２を制御する。その際に、パス動作を行うロボット先端の連続移動軌跡が、教示点Ｐ１，Ｐ３間に設定されている教示点Ｐ２に重なるように、教示点Ｐ３から教示点Ｐ２を通過して延長した直線上に新たな教示点Ｐ２＿ｎを設定する。そして、教示点Ｐ２＿ｎの教示データを用い、教示点Ｐ２＿ｎに向かってパス動作を行うように制御する。
これにより、図５に示すように、教示点Ｐ２から教示点Ｐ３に向かう経路の周辺に障害物等が存在しており、ロボット先端が比較的狭い空間を移動する際にも、教示点Ｐ２，Ｐ３間は確実に直線的に移動するので干渉が生じることを回避できる。

この場合、制御部１３は、教示点を起点Ｐ２として、パス動作の連続移動軌跡が、教示点Ｐ２から教示点Ｐ３に向かう直線に合流する点までの距離Ｌｐａを計算すると、教示点Ｐ２＿ｎを、教示点Ｐ２より距離Ｌｐａだけ延長した直線上に設定する。上述のように、当初に設定された教示点Ｐ１～Ｐ３についてロボット先端を移動させる際に、直線Ｐ２－Ｐ３上でロボット先端が直線的に移動する距離はＬｐａだけ短くなる。したがって、直線Ｐ２－Ｐ３の長さに距離Ｌｐａの長さを加えた位置に教示点Ｐ２＿ｎを設定し、ロボット２にパス動作を行わせれば、ロボット先端の連続移動軌跡が当初の教示点Ｐ２を通過するようになる。

また、制御部１３は、教示点Ｐ１から教示点Ｐ２＿ｎに向かうパス動作を行う際の連続移動軌跡を再計算した結果について距離Ｌｐａを再度計算し、距離Ｌｐａ＿ｎとする。そして、再計算した距離Ｌｐａ＿ｎと先に計算した距離Ｌｐａとの差を求め、求めた差が閾値ｄｅｌｔａ以上であれば、その差分を再計算した距離Ｌｐａ＿ｎに加算して距離Ｌｐａ＿ｎを更新する。それから、更新した距離Ｌｐａ＿ｎに基づいて教示点Ｐ２＿ｎを再度設定し、パス動作の制御を再実行する。このように構成すれば、距離Ｌｐａ＿ｎに含まれる誤差を縮小し、教示点Ｐ２＿ｎを精度良く設定することが可能になる。

図面中、１はロボットシステム、２はロボット、３はコントローラ、４はティーチングペンダント、１３は制御部、１４は記憶部を示す。

Claims (2)

1. 複数の回転動作するアームを備え、各アームをそれぞれの駆動モータにより駆動するロボットの制御装置において、
   ロボット先端を通過させるため教示された３点以上の教示点の位置及び姿勢を、教示テータとして記憶する記憶部と、
   前記ロボット先端が、前記教示点のうち移動を開始する第１教示点から移動を終了する第３教示点に向って連続的に移動するパス動作を行うように教示データを用いて前記ロボットを制御する制御部と、
   前記パス動作を行う前記ロボット先端の連続移動軌跡が、前記第１教示点と前記第３教示点との間に設定されている第２教示点に重なるように、前記第３教示点から前記第２教示点を通過して延長した直線上に第４教示点を設定し、前記第４教示点の教示データを用いて、前記制御部に、前記第１教示点から前記第４教示点に向かって前記パス動作を行うように制御させる教示点設定部とを備え、
   前記教示点設定部は、前記第２教示点を起点として、前記パス動作の連続移動軌跡が、前記第２教示点から前記第３教示点に向かう直線に合流する点までの距離Ｌｐａを計算し、
   前記第４教示点を、前記第２教示点より前記距離Ｌｐａだけ延長した直線上に設定し、
   前記制御部が、前記第１教示点から前記第４教示点に向かうパス動作を行う際の連続移動軌跡を再計算した結果について、前記距離Ｌｐａを再度計算し、
   再計算した距離Ｌｐａと先に計算した距離Ｌｐａとの差を求め、
   前記差が閾値以上であれば、その差分を再計算した距離Ｌｐａに加算して距離Ｌｐａを更新すると、更新した距離Ｌｐａに基づいて第４教示点を再度設定し、前記制御部に前記パス動作の制御を再実行させるロボットの制御装置。
2. 複数の回転動作するアームを備え、各アームをそれぞれの駆動モータにより駆動するロボットを動作させ、ロボット先端を通過させるために教示された３点以上の教示点についての教示データを用いて制御する際に、
   前記教示点のうち移動を開始する第１教示点から移動を終了する第３教示点に向って連続的に移動するパス動作を行前記ロボット先端の連続移動軌跡が、前記第１教示点と前記第３教示点との間に設定されている第２教示点に重なるように、前記第３教示点から前記第２教示点を通過して延長した直線上に第４教示点を設定し、
   前記第４教示点の教示データを用いて、前記第１教示点から前記第４教示点への移動について前記パス動作を行い、
   前記第２教示点を起点として、前記パス動作の軌跡が、前記第２教示点から前記第３教示点に向かう直線に合流する点までの距離Ｌｐａを計算し、
   前記第４教示点を、前記第２教示点より前記距離Ｌｐａだけ延長した直線上に設定し、
   前記第１教示点から前記第４教示点に向かうパス動作を行わせるために連続移動軌跡を再計算した結果について、前記距離Ｌｐａを再度計算し、
   再計算した距離Ｌｐａと、先に計算した距離Ｌｐａとの差を求め、
   前記差が閾値以上であれば、その差分を再計算した距離Ｌｐａに加算して距離Ｌｐａを更新すると、更新した距離Ｌｐａに基づいて第４教示点を再度設定し、前記パス動作を再実行するロボットの制御方法。

Images