ロボットの干渉回避装置
【課題】ロボットを極力動作させながら、ロボットと人との接触を回避する。
【解決手段】ロボットアームが物体に近付く方向に移動する場合、人とロボットアームとがある距離以内に接近しているときには、ロボットアームを緊急停止させる。そして、人とロボットアームとがある距離を越えて離れているときには、離間距離に応じてロボットアームを減速させ、しかも、ロボットアームが人から遠ざかる方向に移動する場合には、ロボットアームを停止させたり、減速させたりすることなく、そのまま動作プログラムに従った動作を継続させる。
【解決手段】ロボットアームが物体に近付く方向に移動する場合、人とロボットアームとがある距離以内に接近しているときには、ロボットアームを緊急停止させる。そして、人とロボットアームとがある距離を越えて離れているときには、離間距離に応じてロボットアームを減速させ、しかも、ロボットアームが人から遠ざかる方向に移動する場合には、ロボットアームを停止させたり、減速させたりすることなく、そのまま動作プログラムに従った動作を継続させる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの停止を極力避けながらロボットと人との干渉を回避できるようにしたロボットの干渉回避装置に関する。
【背景技術】
【０００２】
人がロボットの危険領域に侵入したとき、ロボットを停止させてロボットと人との接触を回避するようにした装置がある。例えば、特許文献１に開示された装置では、停止エリア（危険領域）の直ぐ外側付近に光線ＬＢを通し、その外側にやや距離をおいて警告エリアを構成する光線ＬＡを通し、光線ＬＡが遮断されると、人が警告エリアの外側から内側へ侵入したとして、ロボットの動作速度を減速させ、更に、光線ＬＢが遮断されると、人が警告エリアを通過して停止エリアへと侵入したとして、ロボットを停止させるようにしている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】実用新案登録第２５８７５８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１の装置では、ロボットの移動方向とは関係なく、とにかく、人が警告エリア内に侵入するとロボットの動作速度を減速させ、停止エリア内に侵入するとロボットを停止させる。このため、ロボットの移動方向によっては、人と接触する恐れがない場合でも、ロボットは減速し、或は停止するため、ロボットによる作業効率が低下する。
【０００５】
本発明は上記の事情に鑑みてなされたもので、その目的は、物体（人を含む）の位置とロボットの移動方向とを考慮して減速や停止の要・不要を判断し、ロボットの停止を極力避けながら、換言すれば、ロボットを極力動作させながら物体との接触を回避できるロボットの干渉回避装置を提供することにある。
【課題を解決するための手段】
【０００６】
本発明では、物体が所定の領域内に侵入したとしても、ロボットアームが物体の侵入位置から遠ざかる方向に移動している場合には、ロボットの動作速度を減速したり、ロボットを停止させたりしない。例えば、ロボットアームと物体との間の距離が短くても、ロボットアームが物体から離れてゆくときには、ロボットアームを減速させたり、停止させたりすることなく、そのまま動作プログラムに従った動作を継続させる。ロボットアームが物体に近付いてゆく場合であっても、所要のアームの先端と物体との離間距離がある距離を越えている場合には、ロボットアームを離間距離に応じて減速動作させ、所要のアームの先端が物体に対し、緊急停止時の制動距離を考慮したある距離以内に接近した場合、ロボットアームを緊急停止させる。このため、ロボットアームの稼働率を高く維持しながら、ロボットアームと物体との接触（干渉）を回避できる。
【図面の簡単な説明】
【０００７】
【図１】本発明の一実施形態を示すロボットの斜視図
【図２】ロボットの電気的構成を示すブロック図
【図３】干渉回避の制御内容を示すフローチャート
【図４】干渉回避の制御内容を補助的に説明するための平面図
【発明を実施するための形態】
【０００８】
以下、本発明の一実施形態を図面に基づいて説明する。図２には、６軸の垂直多関節型ロボット１が示されている。このロボット１のロボットアーム２は、ベース３と、このベース３に第１軸Ｌｃ−１を中心に水平方向に旋回可能に支持されたショルダ部４と、このショルダ部４に第２軸Ｌｃ−２を中心に上下方向に旋回可能に支持された下アーム５と、この下アーム５に第３軸Ｌｃ−３を中心に上下方向に旋回可能に支持された第１の上アーム６と、この第１の上アーム６の先端部に第４軸Ｌｃ−４を中心に捻り回転可能に支持された第２の上アーム７と、この第２の上アーム７に第５軸Ｌｃ−５を中心に上下方向に旋回可能に支持された手首８と、この手首８に第６軸Ｌｃ−６を中心に捻り回転可能に支持されたフランジ９とから構成されている。なお、ロボット先端であるフランジ９には、ワークを把持するハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。
【０００９】
ベース３、ショルダ部４、下アーム５、第１の上アーム６、第２の上アーム７、手首８、フランジ９は、ロボットアーム２におけるアームとして機能し、固定アームであるベース３を除く各アーム４〜９は、前段のアームに回転可能に支持された回転関節軸（図示せず）に固定連結されている。そして、各アーム４〜９の回転関節軸は、それぞれ図２に示すサーボモータ１０により減速装置１１を介して回転されるようになっている。なお、各回転関節軸は、上記第１軸Ｌｃ−１〜第６軸Ｌｃ−６に相当するものである。
【００１０】
ロボットアーム２の動作を制御する制御手段としての制御装置１２は、マイコンからなり、図２に示すように、ＣＰＵ１３、ＲＯＭ１４およびＲＡＭ１５を備えている。この制御装置１２には、サーボモータ１０の駆動回路１６、サーボモータ１０の回転角を検出する回転位置検出回路（回転位置検出手段）１７が接続されている。この回転位置検出回路１７は、サーボモータ１０の回転軸（図示せず）に連結されたロータリエンコーダ（回転センサ）１８から入力される回転検出信号に基づいてサーボモータ１０の回転角を検出し、その回転角情報を制御装置１２と駆動回路１６に与える。なお、図２では、サーボモータ１０は１台しか示されていないが、実際には、各アーム４〜９に１台ずつ設けられているものである。
【００１１】
ここで、ベース３および各アーム４〜９には、３次元の座標が規定されている。このうち、床面に据え付けられるベース３の座標系（図１に座標軸ＸＹＺで示す。）は、不動の座標系で基準座標（ロボット座標）とされる。アーム４〜９の座標は、各アーム４〜９の図示しない回転関節軸の回転中心軸線上に設定されており、アーム４〜９の回転によりロボット座標上での位置と姿勢が変化する。ＲＯＭ１４には、ロボット座標上におけるショルダ部４の座標位置、ショルダ部４の座標上における下アーム５の座標位置、下アーム５の座標上における第１の上アーム６の座標位置、第１の上アーム６の座標上における第２の上アーム７の座標位置、第２の上アーム７の座標上における手首８の座標位置、手首８の座標上におけるフランジ９の座標位置、各アーム４〜９の長さなどの各種のパラメータが記憶されている。なお、以下では、各アーム４〜９の座標のロボット座標上の位置を、単に、各アーム４〜９の位置ということとする。
【００１２】
ロボット先端であるフランジ９の座標の原点は、当該フランジ９の先端面の回転中心に定められている。そして、ＣＰＵ１３は、座標変換の計算機能を有し、ロボット先端の位置（姿勢を含む；以下同じ）が与えられると、当該与えられた位置をロボット先端が取るような各アーム４〜８の回転角を演算（逆変換）できるようになっており、また、各アーム４〜９の回転角が与えられると、各アーム４〜９の位置を演算（順変換）できるようにもなっている。
【００１３】
ロボットアーム２の動作プログラムは図示しないティーチングペンダント（動作プログラム作成手段）により作成されてＲＡＭ１５に記憶されている。ＣＰＵ１３は、このＲＡＭ１５に記憶された動作プログラムに基づいて、ロボット先端の移動軌跡を演算し、そして、ロボット先端が移動開始位置から移動終了位置まで移動する間、当該ロボット先端の速度パターンが台形パターンや三角パターンなどの所定の速度パターンとなるように、サンプリングタイム毎のロボット先端の位置を演算する。なお、サンプリングタイムの周期は、例えば１０ｍｓといったごく短い時間に定められている。サンプリングタイム毎のロボット先端の位置を求めると、ＣＰＵ１３は、各アーム４〜８のサンプリングタイム毎の回転角を求め記憶手段としてのＲＡＭ１５に記憶させる。
【００１４】
ロボットアーム２の動作制御中、ＣＰＵ１３は、サンプリングタイム毎に、回転位置検出回路１７から与えられる各サーボモータ１０の回転角から各アーム４〜９の回転角を演算し、各アーム４〜９の回転角からフランジ９の位置を演算して現在位置を検出する。そして、ロボット先端の現在位置の次の位置における各アーム４〜９の回転角をＲＡＭ１５から取得し、各アーム４〜９の回転角から各サーボモータ１０の回転角を演算して当該各回転角を各サーボモータ１０の目標回転角として駆動回路１６に出力する。なお、各サーボモータ１０の回転角から各アーム４〜９の回転角を求めるには、各サーボモータ１０の回転角を各減速装置１１の減速比で除する。逆に、各アーム４〜９の回転角から各サーボモータ１０の回転角を求めるには、各アーム４〜９の回転角に各減速装置の減速比を乗ずる。
【００１５】
駆動回路１６は、ロボット制御装置１２から与えられた目標回転角と、回転位置検出回路１７からフィードバックされた各サーボモータ１０の回転角とを比較し、その差分に応じた電流を各サーボモータ１０に供給する。これによりサーボモータ１０が目標回転角に回転するように制御される。このような制御がサンプリングタイム毎に行われることによってロボット先端が動作プログラムにより定められた軌跡通りに動作するものである。
【００１６】
さて、ロボットアーム２の上方、例えばロボット座標の垂直のＺ座標軸の延長上に、侵入検出手段としてのＣＣＤカメラ１９が設置されている。このＣＣＤカメラ１９は、ロボットアーム２を含む所定の範囲、具体的にはロボットアーム２を中心、厳密にはロボット座標のＺ座標軸を中心にした所定の範囲であって、ロボットアーム２の動作領域を包含する範囲を撮影領域（検出領域）としている。このＣＣＤカメラ１９の制御部（図示せず）は、所定の周期、例えばロボット１の制御装置（以下、単にロボット制御装置）１２のサンプリングタイムの周期以下の周期で撮影画像を解析し、前回の撮影画像と今回の撮影画像を比較する。
【００１７】
そして、今回の撮影画像で新たに撮影された物があったとき、これを検出領域内に侵入した物体と判断する。また、今回の撮影画像と前回の撮影画像の双方に撮影されている物であっても、位置が異なっている場合には、これを検出領域内に侵入した物体と判断する。ＣＣＤカメラ１９の制御部はロボット制御装置１２に接続され、物体を検出したとき、その物体の撮影画像上における位置をロボット制御装置１２に与えるようになっている。この場合、ＣＣＤカメラ１９による撮影画像上の位置は水平の２次元座標（カメラ座標）上の位置で与えられ、そのカメラ座標上の位置をロボット座標のＸＹ座標上の位置に変換できるように、予めカメラ座標とロボット座標との間でキャリブレーションが取られている。なお、上記ＣＣＤカメラ１９が検出する物体には、ワークを載置した搬送車や作業者などが含まれるが、代表的な物体は人であるから、以下では、物体を人と言い換えて説明する。
【００１８】
ロボット制御装置１２は、ＣＣＤカメラ１９を人検出用のセンサとしてロボットアーム２が人と干渉しないように制御するための干渉回避装置を構成する。このロボット制御装置１２の干渉回避装置としての機能の特徴は、人と干渉（接触）する恐れのある移動部分として監視の対象とする部分をロボット先端（フランジ９の先端）だけとするのではなく、他のアームの先端（関節など）をも含めて監視対象部を複数にしていること、監視対象部と人との距離が短くても、監視対象部の移動方向が人に近付く方向の成分を持っていなければ、その監視対象部は人に接触する恐れがないとして緊急停止などの干渉回避処置はとらないこと、にある。
【００１９】
この実施形態では、監視対象部として、ロボット先端であるフランジ９の先端と、下アーム５の先端の２つの部分を定めている。下アーム５の先端は肘として機能し、フランジ９の先端の向きと逆向きの方向に移動することがあるので、人と接触する恐れの高い部分であるといえる。他の部分、例えばショルダ部４は、水平旋回する部分で位置の変化がなく、また、第１の上アーム６、第２の上アーム７、手首８は、下アーム５の先端やフランジ９の先端の移動を監視することによって人との接触を避け得る部分と考えられるので、監視対象部から外してある。
【００２０】
次に、ロボット制御装置１２による干渉回避を、図３のフローチャートをも参照しながら説明する。なお、図３では、下アーム５をＪ２アームと称し、フランジ９をＪ６アームと称している。
【００２１】
ＣＣＤカメラ１９の制御部は、常時、所定の短時間が経過する度に撮影画像を取得し、前回の撮影画像と今回の撮影画像とを比較して人が検出領域内に侵入したか否かを監視している（ステップＳ１）。そして、人が検出領域に侵入すると、ＣＣＤカメラ１９は、人が検出領域に侵入したこと、および侵入した人の位置を検出し、人が検出領域内に侵入したこと、および人の位置をロボット制御装置１２に通知する（ステップＳ１で「ＹＥＳ」）。
【００２２】
ロボット制御装置１２のＣＰＵ１３は、人が検出領域内に侵入したことの通知を受けると、現在のサンプリングタイムにおける全ての回転関節軸、つまり全てのアーム４〜９の回転角をＲＡＭ１５から取得し（ステップＳ２：アーム回転角取得手段）、座標変換の演算機能により、一方の監視対象部である下アーム５の位置を演算し、この下アーム５の位置（下アーム５の座標の位置）と下アーム５の長さと下アーム５の回転角とから当該下アーム５の先端のロボット座標上の位置を計算する（ステップＳ３：動作検出手段のうちの監視対象部位置検出手段）。なお、下アーム５の先端を第３軸Ｌｃ−３とすれば、下アーム５の先端の位置は第１の上アーム６の位置として求めることができる。
【００２３】
下アーム５の先端の位置を求めると、ＣＰＵ１３は、下アーム５の先端と人との間の距離を計算する（ステップＳ４：離間距離取得手段）。このときの人までの距離は、人の位置がロボット座標のＸＹ座標で表されるので、ＸＹ座標で規定される２次元座標での水平方向の距離となる。次に、ＣＰＵ１３は、下アーム５の先端の移動速度ベクトル（方向と速さ）を計算し（ステップＳ５：動作検出手段の速さ検出手段および移動方向検出手段）、下アーム５の先端の移動速度ベクトルの人方向への成分を計算する（ステップＳ６：人方向成分検出手段）。
【００２４】
ここで、下アーム５の先端の移動速度ベクトル、つまり速さと方向は、今回のサンプリングタイムにおける下アーム５の先端の位置と次回のサンプリングタイムにおける下アーム５の先端の位置との差から移動距離を演算し、その移動距離をサンプリングタイムの時間間隔で除して速さを求める（動作検出手段のうちの速度検出手段）と共に、今回のサンプリングタイムでの下アーム５の先端の位置から次回のサンプリングタイムの先端の位置に向かうベクトルを求め、そのベクトルの方向を移動方向とする（動作検出手段のうちの移動方向検出手段）ものである。
【００２５】
次いで、ＣＰＵ１３は、ステップＳ２で取得した回転角から他方の監視対象部であるフランジ９の先端の位置を計算し（ステップＳ７：動作検出手段のうちの監視対象部位置検出手段）、続いて、フランジ９の先端と人との間の距離を計算し（ステップＳ８：離間距離取得手段）、更にフランジ９の先端の移動速度ベクトルを下アーム５の先端の場合と同様にして計算し（ステップＳ９：動作検出手段の速度検出手段および移動方向検出手段）、次いで、フランジ９の先端の移動速度ベクトルの人方向への成分を計算する（ステップＳ１０：人方向成分検出手段）。このときも、人までの距離は、水平方向の距離となる。
【００２６】
下アーム５の先端およびフランジ９の先端の移動速度ベクトルについて、人方向の成分は次のようにして計算する。つまり、図４において、Ｒは監視対象部である下アーム５の先端またはフランジ９の先端を示し、Ｖは下アーム５の先端またはフランジ９の先端の移動速度ベクトルを示している。この移動速度ベクトルＶの人方向への成分は、監視対象部Ｒと人とを結んでできる直線Ｌに移動速度ベクトルＶの先端から垂線Ｈｖを下ろし、この垂線Ｈｖと直線Ｌとの交点ｍを求める。そして、監視対象部Ｒから交点ｍまでのベクトルＶｍを求めれば、このベクトルＶｍが移動速度ベクトルＶの人方向の成分となる。この人方向の成分ベクトルＶｍの方向が人の方を向いているとき、移動速度ベクトルの人方向成分Ｖｍが人に向かっていることとなる。
【００２７】
上記のようにして監視対象部Ｒである下アーム５の先端およびフランジ９の先端の移動速度ベクトルの人方向線分を求めると、次に、ＣＰＵ１３は、下アーム５の先端およびフランジ９の先端の移動速度ベクトルの人方向成分が人に向かっているか否かを判断し（ステップＳ１１：接近判断手段）、いずれの移動速度ベクトルの人方向成分も人に向かっていない場合（ステップＳ１１で「ＮＯ」、ステップＳ１２で「ＮＯ」）、以上のステップＳ１〜ステップＳ１２の動作を繰り返す。
【００２８】
つまり、監視対象部Ｒを通り、人と監視対象部Ｒとを結ぶ直線Ｌと直交する直線Ｌｈを仮定したとき、移動速度ベクトルＶが直線Ｌｈと重なっていたり、当該直線Ｌｈを境にして人とは反対側に移動速度ベクトルＶが向かっていたりして、監視対象部Ｒが人から離れていくような場合には、人と監視対象部Ｒとの間の距離が短くとも、監視対象部Ｒと人とが接触する恐れはないので、そのままロボットアーム２の動作を継続させるものである。
【００２９】
一方、ＣＰＵ１３は、下アーム５の先端およびフランジ９の先端の移動速度ベクトルの双方共に、人方向成分が人に向かっている場合には、下アーム５の先端およびフランジ９の先端の双方を危険対象部に設定する（ステップＳ１１で「ＹＥＳ」、ステップＳ１３：危険対象部設定手段、ステップＳ１４で「ＹＥＳ」、ステップＳ１５：危険対象部設定手段）。また、下アーム５の先端およびフランジ９の先端のうち、下アーム５の先端の移動速度ベクトルだけ、人方向成分が人に向かっている場合には、下アーム５の先端だけを危険対象部に設定する（ステップＳ１１で「ＹＥＳ」、ステップＳ１３、ステップＳ１４で「ＮＯ」）。下アーム５の先端およびフランジ９の先端のうち、フランジ９の先端の移動速度ベクトルだけ、人方向成分が人に向かっている場合には、フランジ９の先端だけを危険対象部に設定する（ステップＳ１１で「ＮＯ」、ステップＳ１２で「ＹＥＳ」、ステップＳ１５）。
【００３０】
下アーム５の先端およびフランジ９の先端のうちの双方、或はいずれか一方が危険対象部に設定された場合、ＣＰＵ１３は、次のサンプリングタイムにおける危険対象部の移動速度を前述したと同様にして取得して、この移動速度で緊急停止を行ったとき、緊急停止の開始から停止までに移動する距離（制動距離）を計算し（ステップＳ１６：制動距離取得手段）、この制動距離を基にして安全位置Ｓを求める（ステップＳ１６）。なお、緊急停止とは、例えばサーボモータ１０に最も大きな逆トルクを発生させて各アーム４〜９を急速に停止させる動作を言う。緊急停止手段はサーボモータ１０に逆トルクを発生させるものに限らず、機械的ブレーキを作動させる等、他の手段であっても良い。
【００３１】
ここで、上記安全位置Ｓは、現在の危険対象部の位置から速度ベクトル方向に、制動距離Ｂに所定距離ｂを足した距離だけ離れた位置に設定するものとする。そして、ＣＰＵ１３は、図４において、（Ｄ）で示す危険対象部の緊急停止時の安全位置Ｓから、現在の危険対象部（Ｄ）と人とを結ぶ直線Ｌに垂線を下ろし、当該垂線を危険安全境界線Ｉとする（ステップＳ１７）。
【００３２】
次に、ＣＰＵ１３は、危険安全境界線Ｉと直線Ｌとの交点ｎを求めて当該交点ｎと危険対象部（Ｄ）との距離Ｆを演算し、この距離Ｆを人と危険対象部（Ｄ）との間の距離（ステップＳ４で求めた人と下アーム５の先端との距離および／またはステップＳ８で求めた人とフランジ９の先端との距離）と比較して、人が危険安全境界線Ｉよりも危険対象部（Ｄ）側に存在するか否かを判断する（ステップＳ１８：危険判断手段）。
【００３３】
人が危険安全境界線Ｉ上または危険安全境界線Ｉよりも危険対象部（Ｄ）側にあれば（ステップＳ１８で「ＹＥＳ」）、この時点でロボットアーム２を緊急停止させる（ステップＳ１９）。すると、危険対象部（Ｄ）は、図４の場合には、安全位置Ｓの所定距離ｂだけ手前で停止する。また、図４とは異なるが、危険対象部（Ｄ）が人に向かって真っ直ぐ移動している場合の最悪時でも、危険対象部（Ｄ）が人から距離Ｂ＋ｂだけ離れたところで緊急停止が開始されるので、ロボットアーム２は、危険対象部（Ｄ）が人に接触する直前で停止する。
【００３４】
人が危険安全境界線Ｉ上または危険安全境界線Ｉよりも危険対象部（Ｄ）側になければ（ステップＳ１８で「ＮＯ」）、未だ緊急停止動作させなくとも良いが、人が急にロボットアーム２に接近する方向に移動することも考えられなくもないので、このような場合の安全を確保するために、ＣＰＵ１３はロボットアーム２を減速させる（ステップＳ２０）。このときの減速度は、危険対象部（Ｄ）と人との距離に応じたものとする。例えば、危険対象部と危険安全境界線までの距離に比例した減速度で減速させるようにする。このようにすることにより、人に近付けばそれだけ危険対象部（Ｄ）の速度が低くなるので、人との接触をより確実に防止でき、仮に接触したとしても、ごく軽く接するだけとなり、人へ与える衝撃をごく小さなものとすることができる。
【００３５】
ステップＳ２０で減速後、ステップＳ１に戻り以上の動作を繰り返す。そして、一旦減速しても、以後、人との距離が変わらなければ、ロボットアーム２はそれ以上減速することなく、そのときの速度で動作を継続する。また、危険対象部（Ｄ）と人との距離が増してゆけば、それに応じてロボットアーム２の動作速度は上昇する。
【００３６】
このように本実施形態によれば、下アーム５の先端およびフランジ９の先端が人から離れる方向に移動する際には、人と下アーム５の先端および人とフランジ９の先端との間の距離が短くても（緊急停止動作時の制動距離以内であっても）、緊急停止動作をさせることなく、そのまま動作を継続させる。
【００３７】
また、下アーム５の先端またはフランジ９の先端が人に近付く方向に移動する場合であっても、一定距離以内になったら、ロボットアーム２を停止させるのではなく、人と下アーム５の先端または人とフランジ９の先端との距離に応じて減速させるので、ロボットアーム２の動作をできる限り遅くせずに、人と接触することを防止できる。
【００３８】
しかも、ＣＣＤカメラ１９は、前回の撮影画像と今回の撮影画像を比較するだけで、新たに撮影された物、或は位置が異なる物を検出できるので、簡易な画像解析によって人の侵入を容易に検出することができ、高価格化を抑制することができる。
【００３９】
なお、本発明は上記し且つ図面に示す実施例に限定されるものではなく、以下のような拡張或は変形が可能である。
ステップＳ１９におけるロボットアーム２の減速率は、人と危険対象部との距離に比例した減速率としても良いし、段階的に減速率を変えるものであっても良い。
ロボットは垂直多関節型ロボットに限られず、水平多関節型ロボットであっても良い。
ＣＣＤカメラ１９（撮像手段）は、物体の位置を３次元で取得できるものであっても良い。
侵入検出手段はＣＣＤカメラ１９に限られない。
干渉回避装置の制御部分は、ロボット制御装置１２とは別のマイクロコンピュータで構成しても良い。
【符号の説明】
【００４０】
図面中、２はロボットアーム、５は下アーム（監視対象部）９はフランジ（監視対象部）、１０はサーｂモータ、１２は制御装置（干渉回避装置、制御手段、動作検出手段、接近判断手段、離間距離取得手段、制動距離取得手段、危険判断手段）、１９はＣＣＤカメラ（侵入検出手段）を示す。
【技術分野】
【０００１】
本発明は、ロボットの停止を極力避けながらロボットと人との干渉を回避できるようにしたロボットの干渉回避装置に関する。
【背景技術】
【０００２】
人がロボットの危険領域に侵入したとき、ロボットを停止させてロボットと人との接触を回避するようにした装置がある。例えば、特許文献１に開示された装置では、停止エリア（危険領域）の直ぐ外側付近に光線ＬＢを通し、その外側にやや距離をおいて警告エリアを構成する光線ＬＡを通し、光線ＬＡが遮断されると、人が警告エリアの外側から内側へ侵入したとして、ロボットの動作速度を減速させ、更に、光線ＬＢが遮断されると、人が警告エリアを通過して停止エリアへと侵入したとして、ロボットを停止させるようにしている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】実用新案登録第２５８７５８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１の装置では、ロボットの移動方向とは関係なく、とにかく、人が警告エリア内に侵入するとロボットの動作速度を減速させ、停止エリア内に侵入するとロボットを停止させる。このため、ロボットの移動方向によっては、人と接触する恐れがない場合でも、ロボットは減速し、或は停止するため、ロボットによる作業効率が低下する。
【０００５】
本発明は上記の事情に鑑みてなされたもので、その目的は、物体（人を含む）の位置とロボットの移動方向とを考慮して減速や停止の要・不要を判断し、ロボットの停止を極力避けながら、換言すれば、ロボットを極力動作させながら物体との接触を回避できるロボットの干渉回避装置を提供することにある。
【課題を解決するための手段】
【０００６】
本発明では、物体が所定の領域内に侵入したとしても、ロボットアームが物体の侵入位置から遠ざかる方向に移動している場合には、ロボットの動作速度を減速したり、ロボットを停止させたりしない。例えば、ロボットアームと物体との間の距離が短くても、ロボットアームが物体から離れてゆくときには、ロボットアームを減速させたり、停止させたりすることなく、そのまま動作プログラムに従った動作を継続させる。ロボットアームが物体に近付いてゆく場合であっても、所要のアームの先端と物体との離間距離がある距離を越えている場合には、ロボットアームを離間距離に応じて減速動作させ、所要のアームの先端が物体に対し、緊急停止時の制動距離を考慮したある距離以内に接近した場合、ロボットアームを緊急停止させる。このため、ロボットアームの稼働率を高く維持しながら、ロボットアームと物体との接触（干渉）を回避できる。
【図面の簡単な説明】
【０００７】
【図１】本発明の一実施形態を示すロボットの斜視図
【図２】ロボットの電気的構成を示すブロック図
【図３】干渉回避の制御内容を示すフローチャート
【図４】干渉回避の制御内容を補助的に説明するための平面図
【発明を実施するための形態】
【０００８】
以下、本発明の一実施形態を図面に基づいて説明する。図２には、６軸の垂直多関節型ロボット１が示されている。このロボット１のロボットアーム２は、ベース３と、このベース３に第１軸Ｌｃ−１を中心に水平方向に旋回可能に支持されたショルダ部４と、このショルダ部４に第２軸Ｌｃ−２を中心に上下方向に旋回可能に支持された下アーム５と、この下アーム５に第３軸Ｌｃ−３を中心に上下方向に旋回可能に支持された第１の上アーム６と、この第１の上アーム６の先端部に第４軸Ｌｃ−４を中心に捻り回転可能に支持された第２の上アーム７と、この第２の上アーム７に第５軸Ｌｃ−５を中心に上下方向に旋回可能に支持された手首８と、この手首８に第６軸Ｌｃ−６を中心に捻り回転可能に支持されたフランジ９とから構成されている。なお、ロボット先端であるフランジ９には、ワークを把持するハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。
【０００９】
ベース３、ショルダ部４、下アーム５、第１の上アーム６、第２の上アーム７、手首８、フランジ９は、ロボットアーム２におけるアームとして機能し、固定アームであるベース３を除く各アーム４〜９は、前段のアームに回転可能に支持された回転関節軸（図示せず）に固定連結されている。そして、各アーム４〜９の回転関節軸は、それぞれ図２に示すサーボモータ１０により減速装置１１を介して回転されるようになっている。なお、各回転関節軸は、上記第１軸Ｌｃ−１〜第６軸Ｌｃ−６に相当するものである。
【００１０】
ロボットアーム２の動作を制御する制御手段としての制御装置１２は、マイコンからなり、図２に示すように、ＣＰＵ１３、ＲＯＭ１４およびＲＡＭ１５を備えている。この制御装置１２には、サーボモータ１０の駆動回路１６、サーボモータ１０の回転角を検出する回転位置検出回路（回転位置検出手段）１７が接続されている。この回転位置検出回路１７は、サーボモータ１０の回転軸（図示せず）に連結されたロータリエンコーダ（回転センサ）１８から入力される回転検出信号に基づいてサーボモータ１０の回転角を検出し、その回転角情報を制御装置１２と駆動回路１６に与える。なお、図２では、サーボモータ１０は１台しか示されていないが、実際には、各アーム４〜９に１台ずつ設けられているものである。
【００１１】
ここで、ベース３および各アーム４〜９には、３次元の座標が規定されている。このうち、床面に据え付けられるベース３の座標系（図１に座標軸ＸＹＺで示す。）は、不動の座標系で基準座標（ロボット座標）とされる。アーム４〜９の座標は、各アーム４〜９の図示しない回転関節軸の回転中心軸線上に設定されており、アーム４〜９の回転によりロボット座標上での位置と姿勢が変化する。ＲＯＭ１４には、ロボット座標上におけるショルダ部４の座標位置、ショルダ部４の座標上における下アーム５の座標位置、下アーム５の座標上における第１の上アーム６の座標位置、第１の上アーム６の座標上における第２の上アーム７の座標位置、第２の上アーム７の座標上における手首８の座標位置、手首８の座標上におけるフランジ９の座標位置、各アーム４〜９の長さなどの各種のパラメータが記憶されている。なお、以下では、各アーム４〜９の座標のロボット座標上の位置を、単に、各アーム４〜９の位置ということとする。
【００１２】
ロボット先端であるフランジ９の座標の原点は、当該フランジ９の先端面の回転中心に定められている。そして、ＣＰＵ１３は、座標変換の計算機能を有し、ロボット先端の位置（姿勢を含む；以下同じ）が与えられると、当該与えられた位置をロボット先端が取るような各アーム４〜８の回転角を演算（逆変換）できるようになっており、また、各アーム４〜９の回転角が与えられると、各アーム４〜９の位置を演算（順変換）できるようにもなっている。
【００１３】
ロボットアーム２の動作プログラムは図示しないティーチングペンダント（動作プログラム作成手段）により作成されてＲＡＭ１５に記憶されている。ＣＰＵ１３は、このＲＡＭ１５に記憶された動作プログラムに基づいて、ロボット先端の移動軌跡を演算し、そして、ロボット先端が移動開始位置から移動終了位置まで移動する間、当該ロボット先端の速度パターンが台形パターンや三角パターンなどの所定の速度パターンとなるように、サンプリングタイム毎のロボット先端の位置を演算する。なお、サンプリングタイムの周期は、例えば１０ｍｓといったごく短い時間に定められている。サンプリングタイム毎のロボット先端の位置を求めると、ＣＰＵ１３は、各アーム４〜８のサンプリングタイム毎の回転角を求め記憶手段としてのＲＡＭ１５に記憶させる。
【００１４】
ロボットアーム２の動作制御中、ＣＰＵ１３は、サンプリングタイム毎に、回転位置検出回路１７から与えられる各サーボモータ１０の回転角から各アーム４〜９の回転角を演算し、各アーム４〜９の回転角からフランジ９の位置を演算して現在位置を検出する。そして、ロボット先端の現在位置の次の位置における各アーム４〜９の回転角をＲＡＭ１５から取得し、各アーム４〜９の回転角から各サーボモータ１０の回転角を演算して当該各回転角を各サーボモータ１０の目標回転角として駆動回路１６に出力する。なお、各サーボモータ１０の回転角から各アーム４〜９の回転角を求めるには、各サーボモータ１０の回転角を各減速装置１１の減速比で除する。逆に、各アーム４〜９の回転角から各サーボモータ１０の回転角を求めるには、各アーム４〜９の回転角に各減速装置の減速比を乗ずる。
【００１５】
駆動回路１６は、ロボット制御装置１２から与えられた目標回転角と、回転位置検出回路１７からフィードバックされた各サーボモータ１０の回転角とを比較し、その差分に応じた電流を各サーボモータ１０に供給する。これによりサーボモータ１０が目標回転角に回転するように制御される。このような制御がサンプリングタイム毎に行われることによってロボット先端が動作プログラムにより定められた軌跡通りに動作するものである。
【００１６】
さて、ロボットアーム２の上方、例えばロボット座標の垂直のＺ座標軸の延長上に、侵入検出手段としてのＣＣＤカメラ１９が設置されている。このＣＣＤカメラ１９は、ロボットアーム２を含む所定の範囲、具体的にはロボットアーム２を中心、厳密にはロボット座標のＺ座標軸を中心にした所定の範囲であって、ロボットアーム２の動作領域を包含する範囲を撮影領域（検出領域）としている。このＣＣＤカメラ１９の制御部（図示せず）は、所定の周期、例えばロボット１の制御装置（以下、単にロボット制御装置）１２のサンプリングタイムの周期以下の周期で撮影画像を解析し、前回の撮影画像と今回の撮影画像を比較する。
【００１７】
そして、今回の撮影画像で新たに撮影された物があったとき、これを検出領域内に侵入した物体と判断する。また、今回の撮影画像と前回の撮影画像の双方に撮影されている物であっても、位置が異なっている場合には、これを検出領域内に侵入した物体と判断する。ＣＣＤカメラ１９の制御部はロボット制御装置１２に接続され、物体を検出したとき、その物体の撮影画像上における位置をロボット制御装置１２に与えるようになっている。この場合、ＣＣＤカメラ１９による撮影画像上の位置は水平の２次元座標（カメラ座標）上の位置で与えられ、そのカメラ座標上の位置をロボット座標のＸＹ座標上の位置に変換できるように、予めカメラ座標とロボット座標との間でキャリブレーションが取られている。なお、上記ＣＣＤカメラ１９が検出する物体には、ワークを載置した搬送車や作業者などが含まれるが、代表的な物体は人であるから、以下では、物体を人と言い換えて説明する。
【００１８】
ロボット制御装置１２は、ＣＣＤカメラ１９を人検出用のセンサとしてロボットアーム２が人と干渉しないように制御するための干渉回避装置を構成する。このロボット制御装置１２の干渉回避装置としての機能の特徴は、人と干渉（接触）する恐れのある移動部分として監視の対象とする部分をロボット先端（フランジ９の先端）だけとするのではなく、他のアームの先端（関節など）をも含めて監視対象部を複数にしていること、監視対象部と人との距離が短くても、監視対象部の移動方向が人に近付く方向の成分を持っていなければ、その監視対象部は人に接触する恐れがないとして緊急停止などの干渉回避処置はとらないこと、にある。
【００１９】
この実施形態では、監視対象部として、ロボット先端であるフランジ９の先端と、下アーム５の先端の２つの部分を定めている。下アーム５の先端は肘として機能し、フランジ９の先端の向きと逆向きの方向に移動することがあるので、人と接触する恐れの高い部分であるといえる。他の部分、例えばショルダ部４は、水平旋回する部分で位置の変化がなく、また、第１の上アーム６、第２の上アーム７、手首８は、下アーム５の先端やフランジ９の先端の移動を監視することによって人との接触を避け得る部分と考えられるので、監視対象部から外してある。
【００２０】
次に、ロボット制御装置１２による干渉回避を、図３のフローチャートをも参照しながら説明する。なお、図３では、下アーム５をＪ２アームと称し、フランジ９をＪ６アームと称している。
【００２１】
ＣＣＤカメラ１９の制御部は、常時、所定の短時間が経過する度に撮影画像を取得し、前回の撮影画像と今回の撮影画像とを比較して人が検出領域内に侵入したか否かを監視している（ステップＳ１）。そして、人が検出領域に侵入すると、ＣＣＤカメラ１９は、人が検出領域に侵入したこと、および侵入した人の位置を検出し、人が検出領域内に侵入したこと、および人の位置をロボット制御装置１２に通知する（ステップＳ１で「ＹＥＳ」）。
【００２２】
ロボット制御装置１２のＣＰＵ１３は、人が検出領域内に侵入したことの通知を受けると、現在のサンプリングタイムにおける全ての回転関節軸、つまり全てのアーム４〜９の回転角をＲＡＭ１５から取得し（ステップＳ２：アーム回転角取得手段）、座標変換の演算機能により、一方の監視対象部である下アーム５の位置を演算し、この下アーム５の位置（下アーム５の座標の位置）と下アーム５の長さと下アーム５の回転角とから当該下アーム５の先端のロボット座標上の位置を計算する（ステップＳ３：動作検出手段のうちの監視対象部位置検出手段）。なお、下アーム５の先端を第３軸Ｌｃ−３とすれば、下アーム５の先端の位置は第１の上アーム６の位置として求めることができる。
【００２３】
下アーム５の先端の位置を求めると、ＣＰＵ１３は、下アーム５の先端と人との間の距離を計算する（ステップＳ４：離間距離取得手段）。このときの人までの距離は、人の位置がロボット座標のＸＹ座標で表されるので、ＸＹ座標で規定される２次元座標での水平方向の距離となる。次に、ＣＰＵ１３は、下アーム５の先端の移動速度ベクトル（方向と速さ）を計算し（ステップＳ５：動作検出手段の速さ検出手段および移動方向検出手段）、下アーム５の先端の移動速度ベクトルの人方向への成分を計算する（ステップＳ６：人方向成分検出手段）。
【００２４】
ここで、下アーム５の先端の移動速度ベクトル、つまり速さと方向は、今回のサンプリングタイムにおける下アーム５の先端の位置と次回のサンプリングタイムにおける下アーム５の先端の位置との差から移動距離を演算し、その移動距離をサンプリングタイムの時間間隔で除して速さを求める（動作検出手段のうちの速度検出手段）と共に、今回のサンプリングタイムでの下アーム５の先端の位置から次回のサンプリングタイムの先端の位置に向かうベクトルを求め、そのベクトルの方向を移動方向とする（動作検出手段のうちの移動方向検出手段）ものである。
【００２５】
次いで、ＣＰＵ１３は、ステップＳ２で取得した回転角から他方の監視対象部であるフランジ９の先端の位置を計算し（ステップＳ７：動作検出手段のうちの監視対象部位置検出手段）、続いて、フランジ９の先端と人との間の距離を計算し（ステップＳ８：離間距離取得手段）、更にフランジ９の先端の移動速度ベクトルを下アーム５の先端の場合と同様にして計算し（ステップＳ９：動作検出手段の速度検出手段および移動方向検出手段）、次いで、フランジ９の先端の移動速度ベクトルの人方向への成分を計算する（ステップＳ１０：人方向成分検出手段）。このときも、人までの距離は、水平方向の距離となる。
【００２６】
下アーム５の先端およびフランジ９の先端の移動速度ベクトルについて、人方向の成分は次のようにして計算する。つまり、図４において、Ｒは監視対象部である下アーム５の先端またはフランジ９の先端を示し、Ｖは下アーム５の先端またはフランジ９の先端の移動速度ベクトルを示している。この移動速度ベクトルＶの人方向への成分は、監視対象部Ｒと人とを結んでできる直線Ｌに移動速度ベクトルＶの先端から垂線Ｈｖを下ろし、この垂線Ｈｖと直線Ｌとの交点ｍを求める。そして、監視対象部Ｒから交点ｍまでのベクトルＶｍを求めれば、このベクトルＶｍが移動速度ベクトルＶの人方向の成分となる。この人方向の成分ベクトルＶｍの方向が人の方を向いているとき、移動速度ベクトルの人方向成分Ｖｍが人に向かっていることとなる。
【００２７】
上記のようにして監視対象部Ｒである下アーム５の先端およびフランジ９の先端の移動速度ベクトルの人方向線分を求めると、次に、ＣＰＵ１３は、下アーム５の先端およびフランジ９の先端の移動速度ベクトルの人方向成分が人に向かっているか否かを判断し（ステップＳ１１：接近判断手段）、いずれの移動速度ベクトルの人方向成分も人に向かっていない場合（ステップＳ１１で「ＮＯ」、ステップＳ１２で「ＮＯ」）、以上のステップＳ１〜ステップＳ１２の動作を繰り返す。
【００２８】
つまり、監視対象部Ｒを通り、人と監視対象部Ｒとを結ぶ直線Ｌと直交する直線Ｌｈを仮定したとき、移動速度ベクトルＶが直線Ｌｈと重なっていたり、当該直線Ｌｈを境にして人とは反対側に移動速度ベクトルＶが向かっていたりして、監視対象部Ｒが人から離れていくような場合には、人と監視対象部Ｒとの間の距離が短くとも、監視対象部Ｒと人とが接触する恐れはないので、そのままロボットアーム２の動作を継続させるものである。
【００２９】
一方、ＣＰＵ１３は、下アーム５の先端およびフランジ９の先端の移動速度ベクトルの双方共に、人方向成分が人に向かっている場合には、下アーム５の先端およびフランジ９の先端の双方を危険対象部に設定する（ステップＳ１１で「ＹＥＳ」、ステップＳ１３：危険対象部設定手段、ステップＳ１４で「ＹＥＳ」、ステップＳ１５：危険対象部設定手段）。また、下アーム５の先端およびフランジ９の先端のうち、下アーム５の先端の移動速度ベクトルだけ、人方向成分が人に向かっている場合には、下アーム５の先端だけを危険対象部に設定する（ステップＳ１１で「ＹＥＳ」、ステップＳ１３、ステップＳ１４で「ＮＯ」）。下アーム５の先端およびフランジ９の先端のうち、フランジ９の先端の移動速度ベクトルだけ、人方向成分が人に向かっている場合には、フランジ９の先端だけを危険対象部に設定する（ステップＳ１１で「ＮＯ」、ステップＳ１２で「ＹＥＳ」、ステップＳ１５）。
【００３０】
下アーム５の先端およびフランジ９の先端のうちの双方、或はいずれか一方が危険対象部に設定された場合、ＣＰＵ１３は、次のサンプリングタイムにおける危険対象部の移動速度を前述したと同様にして取得して、この移動速度で緊急停止を行ったとき、緊急停止の開始から停止までに移動する距離（制動距離）を計算し（ステップＳ１６：制動距離取得手段）、この制動距離を基にして安全位置Ｓを求める（ステップＳ１６）。なお、緊急停止とは、例えばサーボモータ１０に最も大きな逆トルクを発生させて各アーム４〜９を急速に停止させる動作を言う。緊急停止手段はサーボモータ１０に逆トルクを発生させるものに限らず、機械的ブレーキを作動させる等、他の手段であっても良い。
【００３１】
ここで、上記安全位置Ｓは、現在の危険対象部の位置から速度ベクトル方向に、制動距離Ｂに所定距離ｂを足した距離だけ離れた位置に設定するものとする。そして、ＣＰＵ１３は、図４において、（Ｄ）で示す危険対象部の緊急停止時の安全位置Ｓから、現在の危険対象部（Ｄ）と人とを結ぶ直線Ｌに垂線を下ろし、当該垂線を危険安全境界線Ｉとする（ステップＳ１７）。
【００３２】
次に、ＣＰＵ１３は、危険安全境界線Ｉと直線Ｌとの交点ｎを求めて当該交点ｎと危険対象部（Ｄ）との距離Ｆを演算し、この距離Ｆを人と危険対象部（Ｄ）との間の距離（ステップＳ４で求めた人と下アーム５の先端との距離および／またはステップＳ８で求めた人とフランジ９の先端との距離）と比較して、人が危険安全境界線Ｉよりも危険対象部（Ｄ）側に存在するか否かを判断する（ステップＳ１８：危険判断手段）。
【００３３】
人が危険安全境界線Ｉ上または危険安全境界線Ｉよりも危険対象部（Ｄ）側にあれば（ステップＳ１８で「ＹＥＳ」）、この時点でロボットアーム２を緊急停止させる（ステップＳ１９）。すると、危険対象部（Ｄ）は、図４の場合には、安全位置Ｓの所定距離ｂだけ手前で停止する。また、図４とは異なるが、危険対象部（Ｄ）が人に向かって真っ直ぐ移動している場合の最悪時でも、危険対象部（Ｄ）が人から距離Ｂ＋ｂだけ離れたところで緊急停止が開始されるので、ロボットアーム２は、危険対象部（Ｄ）が人に接触する直前で停止する。
【００３４】
人が危険安全境界線Ｉ上または危険安全境界線Ｉよりも危険対象部（Ｄ）側になければ（ステップＳ１８で「ＮＯ」）、未だ緊急停止動作させなくとも良いが、人が急にロボットアーム２に接近する方向に移動することも考えられなくもないので、このような場合の安全を確保するために、ＣＰＵ１３はロボットアーム２を減速させる（ステップＳ２０）。このときの減速度は、危険対象部（Ｄ）と人との距離に応じたものとする。例えば、危険対象部と危険安全境界線までの距離に比例した減速度で減速させるようにする。このようにすることにより、人に近付けばそれだけ危険対象部（Ｄ）の速度が低くなるので、人との接触をより確実に防止でき、仮に接触したとしても、ごく軽く接するだけとなり、人へ与える衝撃をごく小さなものとすることができる。
【００３５】
ステップＳ２０で減速後、ステップＳ１に戻り以上の動作を繰り返す。そして、一旦減速しても、以後、人との距離が変わらなければ、ロボットアーム２はそれ以上減速することなく、そのときの速度で動作を継続する。また、危険対象部（Ｄ）と人との距離が増してゆけば、それに応じてロボットアーム２の動作速度は上昇する。
【００３６】
このように本実施形態によれば、下アーム５の先端およびフランジ９の先端が人から離れる方向に移動する際には、人と下アーム５の先端および人とフランジ９の先端との間の距離が短くても（緊急停止動作時の制動距離以内であっても）、緊急停止動作をさせることなく、そのまま動作を継続させる。
【００３７】
また、下アーム５の先端またはフランジ９の先端が人に近付く方向に移動する場合であっても、一定距離以内になったら、ロボットアーム２を停止させるのではなく、人と下アーム５の先端または人とフランジ９の先端との距離に応じて減速させるので、ロボットアーム２の動作をできる限り遅くせずに、人と接触することを防止できる。
【００３８】
しかも、ＣＣＤカメラ１９は、前回の撮影画像と今回の撮影画像を比較するだけで、新たに撮影された物、或は位置が異なる物を検出できるので、簡易な画像解析によって人の侵入を容易に検出することができ、高価格化を抑制することができる。
【００３９】
なお、本発明は上記し且つ図面に示す実施例に限定されるものではなく、以下のような拡張或は変形が可能である。
ステップＳ１９におけるロボットアーム２の減速率は、人と危険対象部との距離に比例した減速率としても良いし、段階的に減速率を変えるものであっても良い。
ロボットは垂直多関節型ロボットに限られず、水平多関節型ロボットであっても良い。
ＣＣＤカメラ１９（撮像手段）は、物体の位置を３次元で取得できるものであっても良い。
侵入検出手段はＣＣＤカメラ１９に限られない。
干渉回避装置の制御部分は、ロボット制御装置１２とは別のマイクロコンピュータで構成しても良い。
【符号の説明】
【００４０】
図面中、２はロボットアーム、５は下アーム（監視対象部）９はフランジ（監視対象部）、１０はサーｂモータ、１２は制御装置（干渉回避装置、制御手段、動作検出手段、接近判断手段、離間距離取得手段、制動距離取得手段、危険判断手段）、１９はＣＣＤカメラ（侵入検出手段）を示す。
【特許請求の範囲】
【請求項１】
複数の関節により複数のアームを連結して構成されたロボットアームを備え、前記ロボットアームを予め定められた動作プログラムに従って動作させるロボットの干渉回避装置において、
前記ロボットアームを含む所定の範囲を検出領域とし、当該検出領域内へ侵入した物体の位置を検出する侵入検出手段と、
前記複数のアームのうちから選択された所要のアームの先端を監視対象部として当該監視対象部の位置、移動方向および移動速度を、前記動作プログラムに基づいて検出する動作検出手段と、
前記各監視対象部の移動速度ベクトルが前記物体に向かう方向の速度成分を有しているか否かを判断する接近判断手段と、
前記各監視対象部のうち、前記移動速度ベクトルが前記物体に向かう方向の速度成分を有していると判断された監視対象部を危険対象部として当該危険対象部から前記物体までの距離を検出する離間距離取得手段と、
前記危険対象部の移動速度から当該危険対象部の緊急停止時における制動距離を取得する制動距離取得手段と、
前記危険対象部の現在位置から移動速度ベクトルの方向に前記制動距離を越えた距離だけ離れた安全位置を仮定し、この安全位置から前記危険対象部と前記物体とを結ぶ直線に下ろした垂線を危険安全境界線としたとき、前記物体が前記危険安全境界線よりも前記危険対象部に近い側に存在するか否かを判断する危険判断手段と、
前記危険判断手段の判断結果に応じて前記ロボットアームを制御する制御手段と
を具備し、
前記制御手段は、前記物体が前記危険安全境界線よりも前記危険対象部に近い側に存在すると判断されたとき、前記ロボットアームを緊急停止させ、前記物体が前記危険安全境界線よりも前記危険対象部に近い側に存在しないと判断されたとき、前記ロボットアームを前記危険対象部と前記物体との離間距離に応じて減速させることを特徴とするロボットの干渉回避装置。
【請求項１】
複数の関節により複数のアームを連結して構成されたロボットアームを備え、前記ロボットアームを予め定められた動作プログラムに従って動作させるロボットの干渉回避装置において、
前記ロボットアームを含む所定の範囲を検出領域とし、当該検出領域内へ侵入した物体の位置を検出する侵入検出手段と、
前記複数のアームのうちから選択された所要のアームの先端を監視対象部として当該監視対象部の位置、移動方向および移動速度を、前記動作プログラムに基づいて検出する動作検出手段と、
前記各監視対象部の移動速度ベクトルが前記物体に向かう方向の速度成分を有しているか否かを判断する接近判断手段と、
前記各監視対象部のうち、前記移動速度ベクトルが前記物体に向かう方向の速度成分を有していると判断された監視対象部を危険対象部として当該危険対象部から前記物体までの距離を検出する離間距離取得手段と、
前記危険対象部の移動速度から当該危険対象部の緊急停止時における制動距離を取得する制動距離取得手段と、
前記危険対象部の現在位置から移動速度ベクトルの方向に前記制動距離を越えた距離だけ離れた安全位置を仮定し、この安全位置から前記危険対象部と前記物体とを結ぶ直線に下ろした垂線を危険安全境界線としたとき、前記物体が前記危険安全境界線よりも前記危険対象部に近い側に存在するか否かを判断する危険判断手段と、
前記危険判断手段の判断結果に応じて前記ロボットアームを制御する制御手段と
を具備し、
前記制御手段は、前記物体が前記危険安全境界線よりも前記危険対象部に近い側に存在すると判断されたとき、前記ロボットアームを緊急停止させ、前記物体が前記危険安全境界線よりも前記危険対象部に近い側に存在しないと判断されたとき、前記ロボットアームを前記危険対象部と前記物体との離間距離に応じて減速させることを特徴とするロボットの干渉回避装置。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２０１１−１２５９７５（Ｐ２０１１−１２５９７５Ａ）
【公開日】平成２３年６月３０日（２０１１．６．３０）
【国際特許分類】
【出願番号】特願２００９−２８７９５２（Ｐ２００９−２８７９５２）
【出願日】平成２１年１２月１８日（２００９．１２．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２３年６月３０日（２０１１．６．３０）
【国際特許分類】
【出願日】平成２１年１２月１８日（２００９．１２．１８）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]