特許 5915322 ロボット装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5915322

(24)【登録日】2016年4月15日

(45)【発行日】2016年5月11日

(54)【発明の名称】ロボット装置

(51)【国際特許分類】

   B25J 19/06 20060101AFI20160422BHJP

【ＦＩ】

   B25J19/06

【請求項の数】2

【全頁数】15

(21)【出願番号】特願2012-77297(P2012-77297)

(22)【出願日】2012年3月29日

(65)【公開番号】特開2013-202772(P2013-202772A)

(43)【公開日】2013年10月7日

【審査請求日】2015年3月9日

(73)【特許権者】

【識別番号】501428545

【氏名又は名称】株式会社デンソーウェーブ

(74)【代理人】

【識別番号】110000567

【氏名又は名称】特許業務法人 サトー国際特許事務所

(72)【発明者】

【氏名】桝山 貴史

(72)【発明者】

【氏名】壁谷 昇吾

(72)【発明者】

【氏名】飯田 慎二

【審査官】 木原 裕二

(56)【参考文献】

【文献】 実開昭６０−１３１３９１（ＪＰ，Ｕ）

【文献】 特開平１１−０５８２７１（ＪＰ，Ａ）

【文献】 特開２００２−２６４０７０（ＪＰ，Ａ）

【文献】 特開２００９−２７６１７３（ＪＰ，Ａ）

【文献】 特開平０８−０５７７３８（ＪＰ，Ａ）

【文献】 特開平１１−２２６８８９（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００ − ２１／０２

(57)【特許請求の範囲】

【請求項1】

ベース上にロボットアームを設けて構成され、前記ベースを据え付け床面に固定してなるロボット本体と、
前記ベースの周りを走行可能に設けられ、前記ベースの周囲３６０度以下の所定範囲内の走行を１周期走行として当該１周期走行を繰り返し行う走行体と、
前記走行体の位置を検出する走行位置検出手段と、
前記走行体に搭載され、探査信号を斜め上方に出射して当該探査信号の出射方向に存する物体の検出および検出した物体までの距離を計測する探査装置と、
前記走行体の前記１周期走行毎に、前記走行位置検出手段により検出された前記走行体の位置と前記探査装置による検出物体までの距離とから前記ベースの周囲に存する物体の位置データを取得する位置データ取得手段と、
前記走行体の今回の前記１周期走行により取得された前記位置データと予め定められた所定の基本位置データとを比較する第１の比較手段と、
前記第１の比較手段により今回の前記１周期走行による前記位置データと前記基本位置データとの間に差があるとされたとき、今回の前記１周期走行による前記位置データと前回の前記１周期走行による前記位置データとを比較する第２の比較手段と、
前記第２の比較手段により今回の前記１周期走行による前記位置データと前回の前記１周期走行による前記位置データとの間に差がないとされたとき、今回または前回の前記１周期走行による前記位置データを前記基本位置データとして置き換える基本位置データ変換手段と、
前記第２の比較手段により今回の前記１周期走行による前記位置データと前回の前記１周期走行による前記位置データとの間に差があるとされたとき、検出物体が移動しているとして当該検出物体との衝突を回避する衝突回避措置を設定する衝突回避措置設定手段と、
前記第１の比較手段が今回の前記１周期走行による前記位置データと前記基本位置データとの間に差なしとしたときには、前記ロボットアームに動作プログラムの通りの動作を実行させ、前記衝突回避措置設定手段が衝突回避措置を設定したときには、当該衝突回避措置を前記ロボット本体に実行させるロボット制御手段と
を具備してなるロボット装置。

【請求項2】

前記衝突回避措置設定手段は、
今回の前記１周期走行による前記位置データと前回の前記１周期走行による前記位置データとの間の差から検出物体の移動速度と移動方向を算出し、算出した移動方向および移動速度でその後も検出物体が移動するとしたとき、当該検出物体が動作プログラム通りに動作する前記ロボットアームと衝突する場合、前記ロボットアームの速度を減速する第１の衝突回避措置を設定し、
前記第１の衝突回避措置では衝突を回避できない場合、前記ロボットアームを停止するまで制動する停止動作を行わせる第２の衝突回避措置を設定し、
前記第２の衝突回避措置では衝突を回避できない場合、前記ロボットアームを停止動作させながら当該ロボットアームの移動方向を次第に検出物体の移動方向と同じ方向となるように変更する第３の衝突回避措置を設定する
ことを特徴とする請求項１記載のロボット装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は物体の侵入を検出してロボットアームとの衝突を回避するようにしたロボット装置に関する。

【背景技術】

【0002】

ロボットと人との衝突を回避するために、ロボットの動作領域を囲む防護柵を設置し、人がロボットの動作領域内に入ることができないようにすることが一般に行われている。
しかし、防護柵を設置すると、例えば、補給部品を搭載したワゴンなどをロボットの動作領域内に搬入するような場合にその補給作業が面倒なものとなる。

【0003】

防護柵を設けることなく、人とロボットとの衝突を回避するために適用可能な技術として、例えば特許文献１がある。この特許文献１は、ロボットの上方にＣＣＤカメラを設置し、このＣＣＤカメラによりロボットの動作領域内への人の侵入が検出されたとき、ロボットを停止させることで人との衝突を回避しようとするものである。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１１−１２５９７５号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

特許文献１の技術では、ＣＣＤカメラを工場の天井などに設置しなければならず、ロボットを工場の床面に据え付ける作業の他にＣＣＤカメラの設置するための追加作業を必要とする。また、ＣＣＤカメラは工場の天井などの高い場所に取り付ける必要があるため、撮影画像中に人が小さくしか映らず、高度な画像解析が必要となる。

【0006】

本発明は上記の事情に鑑みてなされたもので、その目的は、移動体（人を含む）との衝突を回避でき、しかも、ロボットの設置時に物体の侵入を検出するための装置の設置作業などの追加作業を必要とせず、且つ、簡易な手法で物体の侵入を検出できるロボット装置を提供することにある。

【課題を解決するための手段】

【0007】

請求項１の発明では、物体を検出するための探査装置をロボット本体のベースに設けるので、ロボット本体を工場の床面などに設置する作業の他に移動体を検出する装置の設置のための追加作業を必要としない。
また、物体の侵入を検出するための探査装置は、探査信号を出射し、その出射方向に存在する物体とその物体までの距離を検出するものであり、そして、探査装置はベースの周りを移動する走行体に搭載されているため、走行体の位置と、探査装置から物体までの距離とから容易に物体の位置を取得することができる。

【0008】

前記探査装置は、探査信号を斜め上方に出射するので、人の場合には、上半身に向けて出射されることとなる。検出した物体が移動しているか否かは、物体の位置が前回の計測位置と今回の計測位置との間に差があるか否かによって判定するが、人、特に人の上半身は揺らぎがあるのに対し、例えばワゴンなどの物は設置後、動くことはないので、人と物との区別をすることが容易になる。

【0009】

探査装置の１周期走行によって得られた今回の位置データを基本位置データと比較することで、検出物体が移動しているか否かを判断する。このため、例えば、補給部品を搭載したワゴンを搬入したとき、ワゴンが動かされている間は検出物体は移動しているとされる。ワゴンの設置後は、ワゴンは静止しているので、今回の１周期走行による位置データと前回の１周期走行による位置データとの間に差がなくなる。すると、今回または前回の１周期走行による位置データが前記基本の位置データに置き換えられる。このため、新たにワゴンを設置した場合などのように、探査装置によって位置データを取得する度に、検出物体が移動していると判断されることがなくなり、処理の迅速化を図ることができる。

【0010】

請求項２の発明によれば、検出物体とロボットアームとが衝突する恐れがある場合、まず、ロボットアームを減速させ、減速では衝突回避できない場合には、ロボットアームを制動して停止させる停止動作を行わせ、停止動作では衝突回避できない場合には、ロボットアームを停止動作させながらロボットアームの移動方向を次第に検出物体の移動方向と同じ方向となるように変更するので、検出物体とロボットアームとの衝突をより確実に回避することができる。

【図面の簡単な説明】

【0011】

【図1】本発明の一実施形態を示すロボット本体の側面図

【図2】ベースに取り付けられた走行車を示す側面図

【図3】走行車の概略的な斜視図

【図4】電気的構成を示すブロック図

【図5】計測データの処理内容を示すフローチャート

【図6】ロボット本体の停止中の制御内容を示すフローチャート

【図7】ロボット本体の動作中の制御内容を示すフローチャート

【図8】（ａ）は移動物体とロボットアームの移動方向を示す図、（ｂ）はロボットアームの衝突回避動作を示す図

【図9】（ａ）は速度パターン図、（ｂ）は衝突判定領域を示す図

【発明を実施するための形態】

【0012】

以下、本発明の一実施形態を図面に基づいて説明する。図１には、６軸の垂直多関節型ロボット装置のロボット本体１が示されている。このロボット本体１は、ベース２上にロボットアーム３を設けて構成されている。ロボットアーム３は、ベース２に水平方向に旋回可能に支持されたショルダ部４と、このショルダ部４に上下方向に旋回可能に支持された下アーム５と、この下アーム５に上下方向に旋回可能に支持された第１の上アーム６と、この第１の上アーム６の先端部に捻り回転可能に支持された第２の上アーム７と、この第２の上アーム７に上下方向に旋回可能に支持された手首アーム８と、この手首アーム８に捻り回転可能に支持されたフランジ９とから構成されている。なお、ロボットアーム先端であるフランジ９には、ワークを把持するハンドや、視覚検査のために用いるカメラなどのエンドエフェクタ（図示せず）が取り付けられるようになっている。

【0013】

ショルダ部４、下アーム５、第１の上アーム６、第２の上アーム７、手首アーム８、フランジ９は、ロボットアーム３におけるアーム（リンク）として機能し、各アーム４〜９は、前段のアームに回転可能に支持された回転関節軸（図示せず）に固定連結されている。そして、各アーム４〜９の回転関節軸は、それぞれ図４に示すサーボモータ１０により減速装置１１を介して回転されるようになっている。

【0014】

図４に示すように、ロボットアーム３の動作を制御するロボット制御手段としての制御装置１２は、マイコンからなり、ＣＰＵ１３、ＲＯＭ１４およびＲＡＭ１５を備えている。この制御装置１２には、サーボモータ１０の駆動回路１６、サーボモータ１０の回転角を検出する回転位置検出回路（回転位置検出手段）１７が接続されている。この回転位置検出回路１７は、サーボモータ１０の回転軸（図示せず）に連結されたロータリエンコーダ（回転センサ）１８から入力される回転検出信号に基づいてサーボモータ１０の回転角を検出し、その回転角情報を制御装置１２と駆動回路１６に与える。なお、図４では、サーボモータ１０は１台しか示されていないが、実際には、各アーム４〜９に１台ずつ設けられているものである。

【0015】

ここで、ベース２および各アーム４〜９には、３次元の座標が規定されている。このうち、床面に据え付けられるベース２の座標系（図１に座標軸ＸＹＺで示す。）は、不動の座標系でロボット座標（基準座標）とされる。アーム４〜９の座標は、各アーム４〜９の図示しない回転関節軸の回転中心軸線上に設定されており、アーム４〜９の回転によりロボット座標上での位置と姿勢が変化する。

【0016】

ＲＯＭ１４には、ロボット座標上におけるショルダ部４の座標位置、ショルダ部４の座標上における下アーム５の座標位置、下アーム５の座標上における第１の上アーム６の座標位置、第１の上アーム６の座標上における第２の上アーム７の座標位置、第２の上アーム７の座標上における手首アーム８の座標位置、手首アーム８の座標上におけるフランジ９の座標位置、各アーム４〜９の長さなどの各種のパラメータが記憶されている。

【0017】

ロボット先端であるフランジ９の座標の原点は、当該フランジ９の先端面の回転中心に定められている。そして、ＣＰＵ１３は、座標変換の計算機能を有し、ロボット先端の位置（姿勢を含む；以下同じ）が与えられると、当該与えられた位置をロボット先端が取るような各アーム４〜９の回転角を演算（逆変換）できるようになっており、また、各アーム４〜９の回転角が与えられると、各アーム４〜９の位置を演算（順変換）できるようにもなっている。

【0018】

ロボットアーム３の動作プログラムは図示しないティーチングペンダント（動作プログラム作成手段）により作成されてＲＡＭ１５に記憶されている。ＣＰＵ１３は、このＲＡＭ１５に記憶された動作プログラムに基づいて、ロボット先端の移動軌跡を演算し、そして、ロボット先端が移動開始位置から移動終了位置まで移動する間、当該ロボット先端の速度パターンが台形パターンや三角パターンなどの所定の速度パターンとなるように、サンプリングタイム毎のロボット先端の位置を演算する。なお、サンプリングタイムの周期は、例えば１０ｍｓといったごく短い時間に定められている。サンプリングタイム毎のロボット先端の位置を求めると、ＣＰＵ１３は、各アーム４〜８のサンプリングタイム毎の回転角を求め記憶手段としてのＲＡＭ１５に記憶させる。

【0019】

ロボットアーム３の動作制御中、ＣＰＵ１３は、サンプリングタイム毎に、回転位置検出回路１７から与えられる各サーボモータ１０の回転角から各アーム４〜９の回転角を演算し、各アーム４〜９の回転角からフランジ９の位置を演算して現在位置を検出する。そして、ロボット先端の現在位置の次の位置における各アーム４〜９の回転角をＲＡＭ１５から取得し、各アーム４〜９の回転角から各サーボモータ１０の回転角を演算して当該各回転角を各サーボモータ１０の目標回転角として駆動回路１６に出力する。

【0020】

駆動回路１６は、ロボット制御装置１２から与えられた目標回転角と、回転位置検出回路１７からフィードバックされた各サーボモータ１０の回転角とを比較し、その差分に応じた電流を各サーボモータ１０に供給する。これによりサーボモータ１０が目標回転角に回転するように制御される。このような制御がサンプリングタイム毎に行われることによってロボット先端が動作プログラムにより定められた軌跡通りに動作するものである。

【0021】

さて、ベース２の上部には、図２に示すように、当該ベース２を取り巻く円形のレール１９が取り付けられており、このレール１９上に走行車（走行体）２０が乗せられている。走行車２０はレール１９上を転がる車輪２１を有し、この車輪２１は走行車２０に搭載された車輪駆動モータ２２により回転駆動されて走行車２０を走行させる。

【0022】

走行車２０の現在位置を検出するために、ベース２には当該ベース２を囲むリング状の多孔板２３が取り付けられている。一方、走行車２０には、多孔板２３を挟むようにして例えば発光ダイオードからなる投光素子２４とフォトダイオードからなる受光素子２５が配設されている。

【0023】

図３に示すように、多孔板２３には、周方向に沿って多数の小孔２３ａが形成されており、投光素子２４が放射する光は小孔２３ａを通して受光素子２５に受光される。そして、走行車２０が移動すると、受光素子２５は投光素子２４が放射する光を間欠的に受けてパルス状の受光信号を出力する。このような投光素子２４と受光素子２５は走行車２０の位置を検出する位置検出器（位置センサ）２６を構成している。

【0024】

また、走行車２０には、探査装置としてのレーザーレーダ２７が搭載されている。レーザーレーダ２７は、レーザー光（探査信号）をパルス状に出射するレーザーと、出射されたレーザー光の反射光を受光する受光器とから構成されている。このレーザーレーダ２７は、レーザー光を図１に示すように所定の仰角γで斜め上向きに出射するように配設されている。なお、レーザー光を図１においてＬで示した。

【0025】

走行車２０とベース２とは、図３に示すフレキシブルケーブル２８によって接続されている。このフレキシブルケーブル２８は、車輪駆動モータ２２、位置検出器２６、レーザーレーダ２７などの走行車２０に搭載された機器への電源供給と、それら搭載機器と制御装置１２との間での信号の送受のためのものである。走行車２０内には、フレキシブルケーブル２８を巻き取る巻取器２９が設けられている。この巻取器２９はモータ駆動式で、走行車２０の位置に応じたフレキシブルケーブル２８の巻取り量が設定されている。これにより、フレキシブルケーブル２８は常時適正な弛みを持って巻き取り巻き戻される。

【0026】

上記多孔板２３、投光素子２４および受光素子２５、制御装置１２は、走行車２０の位置を検出する走行位置検出手段を構成するもので、受光素子２５が出力する受光パルス信号は、制御装置１２に入力される。そして、制御装置１２は、受光素子２５からの受光パルス信号の数をカウントすることで走行車２０の位置を検出する。

【0027】

つまり、走行車２０のレール１９は、ロボット座標のＺ軸を中心とする円形になっている。本実施形態において、Ｚ軸はショルダ部４の回転中心線と一致している。そして、走行車２０の位置は、レール１９上の所定位置を基準位置とし、この基準位置からＺ軸に下ろした垂線と、走行車２０の現在位置からＺ軸に下ろした垂線とのなす角によって表わすものとする。すると、上記のように基準位置からの受光パルス数をカウントすることで、走行車２０が現在、基準位置からベース２の周りを何度回転した位置に居るかを検出することができる。

【0028】

制御装置１２は、走行車２０がベース２の周囲を３６０度以下の所定範囲、この実施形態では３６０度つまり１周すると、車輪駆動モータ２２を逆転させて走行車２０をそれまでとは逆方向に移動させるようになっている。従って、走行車２０は、ベース２の周りを１周ずつ正逆方向に繰り返し移動する。このベース２の周りの３６０度の移動を走行車２０の１周期走行ということとする。

【0029】

ちなみに、走行車２０が正逆方向に移動する場合でも、制御装置１２は、車輪駆動モータ２２の回転方向により基準位置からどちら方向に何度回転した位置に走行車２０が居るかを検出することができるものである。

【0030】

なお、レーザーレーダ２７のレーザー光の出射方向は、真上から見たとき、Ｚ軸とレーザーとを結んだ直線と所定角度、例えば同方向となるように定められている。また、走行車２０の基準位置はロボット座標のＸ軸、Ｙ軸とのなす角が所定角度となるように、例えば真上から見たときＸ軸とのなす角が０度（Ｘ軸と一致）でＹ軸とのなす角が９０度となるように定められている。

【0031】

レーザーレーダ２７によって物体が検出されたとき、レーザーレーダ２７から物体までの距離は、図１に示すように、レーザー光の出射方向に沿った距離Ａで検出されるから、水平方向の距離ＢはＢ＝Ａ・ｃｏｓγで求めることができる。そして、レーザーレーダ２７の移動軌跡の半径ｄは既知であるから、ロボット座標のＺ軸から検出物体までの水平距離ＤはＤ＝（Ｂ＋ｄ）で求めることができる。

【0032】

従って、レーザーレーダ２７により物体が検出されたとき、そのときの走行車２０の位置とレーザーレーダ２７からの距離により、物体の位置をロボット座標のＸＹ座標値で把握することができる。つまり、走行車２０の現在位置が基準位置からθ度のところにある（Ｘ軸とのなす角がθ度）としたとき、ＸＹ座標値は、ｘ＝Ｄ・ｃｏｓθ、ｙ＝Ｄ・ｓｉｎθで求めることができる。

【0033】

次に上記構成の作用を図５〜図７のフローチャートをも参照しながら説明する。ロボット装置の電源が投入され、スタート操作が行われると、まず、制御装置１２は、走行車２０に基準位置から正方向回りの１周期走行と逆方向回りの１周期走行を繰り返し行わせる。この走行車２０の走行中、レーザーレーダ２７はレーザー光を間欠的に放射して物体検出を行う。
この走行車２０の移動中、制御装置１２は、受光素子２５が受光信号を出力した時の時間を、受光信号の一つ一つについて記憶手段としての例えばＲＡＭ１５に記憶する。この受光信号が出力された時間は、少なくとも前回の１周期走行と今回の１周期走行とについて記憶する。

【0034】

走行車２０の１周期走行において、制御装置１２は、レーザーレーダ２７の受光信号から物体の存在を検出すると、そのときの走行車２０の位置と、レーザーレーダ２７から物体までの距離とから、当該検出物体の位置をロボット座標のＸＹ座標上の位置として計算する。そして、制御装置１２は、走行車２０の１周期走行において検出された全ての物体の位置データを含む物体検出結果を１周期位置データとしてＲＡＭ１５に記憶させる（位置データ取得手段）。なお、走行車２０の位置とレーザーレーダ２７からの物体の距離とを記憶し、必要なときにＸＹ座標上の位置として計算するようにしても良い。

【0035】

さて、以上のようにして１周期位置データを取得すると、制御装置１２は、図５のデータ処理ルーチンに入り、今回取得した１周期位置データを予めＲＡＭ１５に記憶された所定の基本位置データと比較する（図５のステップＡ１；第１の比較手段）。基本位置データは、例えば、ロボット本体１の周りに人が居ない状態で取得した１周期位置データである。なお、基本位置データは、電源投入後の最初の１周期位置データとしても良い。

【0036】

１周期位置データと基本位置データとを比較した結果、両データ間に差がないとき（ステップＡ１で「ＮＯ」）、制御装置１２は、移動体検出フラグをオフし（ステップＡ４）、リターンとなる。また、１周期位置データと基本位置データとの間に差があるとき（ステップＡ１で「ＹＥＳ」）、制御装置１２は、レーザーレーダ２７の検出領域内への侵入物体を検出したとし、次に前回の１周期位置データと今回の１周期位置データとを比較する（ステップＡ２；第２の比較手段）。

【0037】

前回の１周期位置データと今回の１周期位置データとを比較した結果、両データ間に差がないとき（ステップＡ２で「ＮＯ」）、制御装置１２は、今回の１周期位置データを基本位置データとして置き換え（ステップＡ５；基本位置データ変換手段）、移動体検出フラグをオフ（ステップＡ６）してリターンとなる。

【0038】

また、前回の１周期位置データと今回の１周期位置データとを比較した結果、両データ間に差があるとき（ステップＡ２で「ＹＥＳ」）、制御装置１２は、検出物体が移動しているとして移動体検出フラグをオン（ステップＡ３）し、リターンとなる。なお、移動していると判断された検出物体を以下では移動体ということとする。

【0039】

以上を具体的に説明する。移動体、例えば、作業者が補給部品を搭載したワゴン（部品補給用ワゴン）を、ロボットアーム３による組み付け作業の結果、空になったワゴンと交換するために、レーザーレーダ２７の検出領域内に搬入してきたとする。すると、今回取得された１周期位置データの中には、今まで存在していなかった部品補給用ワゴンや作業者の位置データが存在することとなるので、基本位置データとも、また前回の１周期位置データとも異なることとなる。このため、ステップＡ１で「ＹＥＳ」、ステップＡ２で「ＹＥＳ」と判断され、ステップＡ３で移動体検出フラグがオンされる。

【0040】

作業者が空になったワゴンに向かって部品補給ワゴンを押して行って部品補給ワゴンを空になったワゴンと交換し、その空になったワゴンを押してレーザーレーダ２７の検出領域の外に出て行くまでは、作業者やワゴンが移動しているので、その間に次々と取得される１周期位置データは基本位置データとも前回取得の１周期位置データとも異なることとなるので、移動体検出フラグはオンされ続ける。

【0041】

作業者が空になったワゴンを押してレーザーレーダ２７の検出領域の外に出てゆくと、移動物体はいなくなる。その直後に新たに取得された１周期位置データは基本位置データと比較される。このとき、部品補給ワゴンが空になったワゴンと同じ位置に置かれた場合には、新たに取得された１周期位置データは基本位置データと一致するので（ステップＡ１で「ＮＯ」）、移動体検出フラグはオフされる（ステップＡ４）。

【0042】

しかしながら、部品補給ワゴンを空になったワゴンと置き換える際、空になったワゴンがあった位置とは多少ずれることが往々にしてある。この場合には、作業者が空になったワゴンを押してレーザーレーダ２７の検出領域の外に出て行った直後に新たに取得された１周期位置データは基本位置データと異なり、前回の１周期位置データ（作業者が空になったワゴンを押してレーザーレーダ２７の検出領域の外に出る直前の位置データであるから作業者、空になったワゴンの位置データがある）とも異なることとなるので、移動体検出フラグはオフされず、オンのままになる（ステップＡ２で「ＹＥＳ」、ステップＡ３）。

【0043】

ところが、次に新たに取得された今回の１周期位置データは基本位置データとは異なる（ステップＡ１で「ＹＥＳ」）が、前回の１周期位置データとは一致することとなるので（ステップＡ２で「ＮＯ」）、それまでの基本位置データがＲＡＭ１５から消去され、今回取得された最新の１周期位置データが新たな基本位置データとしてＲＡＭ１５に記憶され（ステップＡ５；基本位置データ変換手段）、移動体検出フラグがオフされる（ステップＡ６）。そして、次に取得された１周期位置データは新たな基本位置データと一致することとなるので（ステップＡ１で「ＮＯ」）、移動体検出フラグがオフされ続けることとなる。

【0044】

従って、今回の１周期位置データと基本位置データとの比較結果だけで、移動体検出フラグをオンしたりオフしたりする場合とは異なり、ワゴンの位置が少し変化しただけで、実際には物体の侵入がないにもかかわらず、前回の１周期位置データとの差分を比較するという余分の動作を実施するといった不具合の発生を未然に防止できる。このことは、制御装置１２の処理時間が短縮されることを意味し、ひいては、ロボットアーム３の動作についての制御装置１２の処理を余裕をもって実行でき、ロボットのタクトタイム（繰り返し行われる１作業サイクルの所要時間）が長くなることを防止できる。

【0045】

また、レーザーレーダ２７からレーザー光は、所定の仰角γで出射される。これにより、レーザーレーダ２７の検出領域内に侵入してきた人に対して、レーザー光が上半身に照射されるようにしている。人、特に検出領域内に入ってくる人は工場の作業者であるから、定位置に留まっていても、頭や手は絶えず動いている。このため、人が同じ位置に立っていたとしても、上半身が動くことにより、前回の１周期位置データと今回の１周期位置データとは異なるようになり、移動物体検出フラグがオンされ続けるようになる（ステップＡ１で「ＹＥＳ」、ステップＡ２で「ＹＥＳ」、ステップＡ３）。このため、人と物との区別をより正確に行うことができ、人を移動体ではないと判断することを防止できる。

【0046】

以上のような１周期位置データの処理が終了すると、制御装置１２は、図５のデータ処理ルーチンと図６に示す停止中処理ルーチンを交互に繰り返し実行する。図６の停止中処理ルーチンに入ると、制御装置１２は、まず移動体検出フラグがオンされているか否かを判断する（ステップＢ１）。移動体検出フラグがオフであれば（ステップＢ１で「ＮＯ」）、制御装置１２は動作を許可し（ステップＢ７）、ロボットアーム３に動作プログラムに従った動作を開始させる。

【0047】

なお、以下において、ロボットアーム３の位置、動作方向、速度という場合、それは、アーム先端であるフランジ９の位置、動作方向、速度のことを指す。
移動体検出フラグがオンであった場合（ステップＢ１で「ＹＥＳ」）、制御装置１２は、動作プログラムを参照してロボットアーム３の動作開始時の動作方向を検出し、移動体の存在する方向に動作しないなら（ステップＢ２で「ＮＯ」）、動作を許可し（ステップＢ７）、ロボットアーム３に動作プログラムに従った動作を開始させる。

【0048】

ロボットアーム３の動作開始時の動作方向が移動体の存在する方向であるならば（ステップＢ２で「ＹＥＳ」）、次に制御装置１２は、ロボットアーム３の動作プログラムを参照して動作方向と速度を取得すると共に、移動体の動作方向と速度を取得し、ロボットアーム３と移動体とが衝突する位置と動作開始から衝突までの時間を計算する（ステップＢ３）。

【0049】

上記ロボットアーム３の速度は、動作プログラムにより定められた速度パターンから取得する。例えば、ロボットアーム３の速度パターンが図９（ａ）に示すような台形パターンであった場合、加速度α、最高速度Ｖ、減速度−αは動作プログラムに記載してあるので、それを参照して動作開始からｔ時間後の速度を算出する。動作開始からｔ時間の間に移動した距離（位置）はそれまでの速度を積分する、つまりｔ時間までの速度パターンの面積を算出すれば良い。

【0050】

移動体の動作方向と速度は、今回の１周期位置データと前回の１周期位置データを比較して取得する。つまり、移動体の動作方向は、移動体の前回の位置と今回の位置とを結ぶ直線を求め、この直線上を前回の位置から今回の位置に向かう方向に動作するとする。また、移動体の速度は、前回の位置から今回の位置までの距離を求め、この距離を、前回の１周期位置データによって移動体が検出されてから今回の１周期位置データによって移動体が検出されたときまでの時間で除すことによって求めることができる。

【0051】

なお、移動体の前回の位置から今回の位置までの距離は、Ｚ軸から前回の移動体の位置までの距離と、Ｚ軸から今回の移動体の位置までの距離と、Ｚ軸から前回の移動体の位置に引いた直線とＺ軸から今回の移動体の位置に引いた直線とのなす角とから求めることができる。前回の１周期位置データによって移動体が検出されてから今回の１周期位置データによって移動体が検出されたときまでの時間は、今回の移動体の位置で受光素子２５が受光信号を出力した時間から前回の移動体の位置で受光素子２５が受光信号を出力した時間との差で求めることができる。

【0052】

ロボットアーム３と移動体とが衝突する位置と動作開始から衝突までの時間を計算した結果、衝突しないならば（ステップＢ４で「ＮＯ」）、制御装置１２は、ロボットアーム３の動作を許可し、動作を開始させる。
衝突するならば、制御装置１２は、ロボットアーム３の速度を動作プログラムによって定められた速度パターンによらず、それよりも低速度の安全速度、例えばＩＳＯ１０２１８で認められている２５ｍｍ／ｓｅｃ以下でロボットアーム３を動作させるとした場合に移動体と衝突するか否かを判断し、安全速度で衝突回避できるならば（ステップＢ５で「ＹＥＳ」）、ロボットアーム３の速度を安全速度に設定して動作を許可する（ステップＢ６）。

【0053】

安全速度に設定しても衝突を回避できなければ（ステップＢ５で「ＮＯ」）、制御装置１２は、ロボットアーム３の動作を禁止する（ステップＢ８）。
さて、ロボットアーム３の動作が開始されると、制御装置１２は図５のデータ処理ルーチンと図７の動作中処理ルーチンを交互に繰り返し実行する。この図７の動作中処理ルーチンにおいて、制御装置１２は、状況に応じて移動体との衝突を回避するために、種々の衝突回避措置を設定する（衝突回避措置設定手段）。

【0054】

図７のフローチャートに入ると、制御装置１２は、移動体検出フラグがオンされているか否かを判断し（ステップＣ１）、移動体検出フラグがオフであったなら（ステップＣ１で「ＮＯ」）、リターンとなってロボットアーム３の動作を続行させる。
移動体検出フラグがオンであった場合（ステップＣ１で「ＹＥＳ」）、制御装置１２は、前述したと同様にしてロボットアーム３の動作方向と速度を取得すると共に、移動体の移動方向と速度を取得し、ロボットアーム３と移動体とが衝突する位置と衝突までの時間を計算する（ステップＣ２）。

【0055】

計算の結果、ロボットアーム３と移動体とが衝突しないとなった場合（ステップＣ３で「ＮＯ」）、制御装置１２は、ロボットアーム３の動作をそのまま続行する。
ロボットアーム３と移動体とが衝突する場合（ステップＣ３で「ＹＥＳ」）、制御装置１２は、ロボットアーム３を減速させることで、移動体との衝突を回避できるか否かを判断する（ステップＣ４）。

【0056】

例えば、ロボットアーム３の動作方向と移動体の移動方向とが交差し、その交差する位置で両者が衝突する結果が得られた場合、ロボットアーム３の速度を遅くすることで、移動体が交差位置を通過した後にロボットアーム３がその交差位置に到達するようにすれば、両者は衝突しないこととなる。しかし、ロボットアーム３の移動方向と移動体の移動方向とが重なる場合、ロボットアーム３の速度を遅くしても両者は衝突することとなる。

【0057】

ロボットアーム３を減速させると衝突を回避できる場合（ステップＣ４で「ＹＥＳ」）、制御装置１２は、衝突を回避できる速度を演算し、その速度となるまでロボットアーム３を減速する（ステップＣ８；第１の衝突回避措置）。なお、減速はサーボモータ３に逆トルクを発生させてロボットアーム３を制動する（制動手段）ことによって行う。制動手段はサーボモータ３に逆トルクを発生させるものに限らず、機械的ブレーキを作動させるなど他の手段でも良い。

【0058】

ロボットアーム３を減速させても衝突を回避できない場合（ステップＣ５で「ＮＯ」）、制御装置１２は、緊急停止によって回避できるか否かを判断する（ステップＣ５）。緊急停止とは、例えばサーボモータ３に最大の逆トルクを発生させて許容最大減速度でロボットアーム３を急速に停止させる動作をいう。この許容最大減速度は、予め記憶手段、例えばＲＯＭ１４に記憶させてある。

【0059】

緊急停止によって衝突を回避できるか否かは、まず、現在のロボットアーム３の速度を検出し、この速度のときに許容最大減速度で減速させたとき、停止するまでの移動距離（制動距離）ｓを求める。そして、移動物体が制動距離ｓ内に存在していれば衝突すると判断し、制動距離ｓ外であれば衝突しないと判断する。制動距離ｓは、図９（ａ）において、現在速度がＶ、許容最大減速度が−αであるとしたとき、斜線を付した三角形の面積で求めることができる。なお、より確実に衝突を回避するために、現在速度を、実際の速度ではなく、最大許容速度にあると仮定して制動距離ｓを求めるようにしても良い。

【0060】

緊急停止によって衝突を回避できる場合（ステップＣ５で「ＹＥＳ」）、制御装置１２は、ロボットアーム３を緊急停止させる（ステップＣ９）。
緊急停止によっても衝突を回避できない場合（ステップＣ５で「ＮＯ」）、制御装置１２は、衝突回避のために安全な方向へ軌道を変更し（ステップＣ６）、当該安全な方向へ動作させながら緊急停止させる（ステップＣ７）。

【0061】

ここで、安全な方向とは、本実施形態では、移動体の移動方向と同じ方向のことをいう。具体的には、図８（ａ）に示すように、動作プログラムによって示された目標位置Ｑに向かってロボットアーム３が移動する方向を直線Ｌ１で示し、移動体Ｒの移動方向を直線Ｌ２で示したとき、図８（ｂ）に示すように、直線Ｌ１よりも移動体Ｒから離れた任意の位置Ｑ１を回避目標位置に定め、この回避目標位置Ｑ１に到達する軌跡を算出し、この算出した軌跡に沿ってロボットアーム３を緊急停止させながら移動させる。この場合、ロボットアーム３の衝突回避のため移動方向を、より早く移動体Ｒの移動方向に近づけるためには、回避目標位置Ｑ１は、直線Ｌ２を含め当該直線Ｌ２よりもロボットアーム３側に定めることが好ましい。

【0062】

このように回避目標位置を定めることにより、ロボットアーム３は、次第に移動体Ｒの移動方向と同じ方向に方向変換しながら緊急停止するようになるので、移動体との衝突を回避できる。
つまり、ロボットアーム３を、移動体から離れる方向に移動させながら緊急停止させることができるので、移動体との衝突を回避することができるのである。
例えば、ロボットアーム３が移動体と衝突するという場合、移動体の移動方向を、前回の１周期位置データでの位置と今回の１周期位置データでの位置とを結んだ直線方向と仮定してのことであるが、実際には、その後、方向転換してロボットアーム３から遠ざかる方向に移動したり、ロボットアーム３に近づく方向に向きを変えて移動してきたりするかも知れない。ロボットアーム３から遠ざかる方向に向きを変えた場合には良いが、図８（ａ）において、直線Ｌ２よりもロボットアーム３側で当該ロボットアーム３に近づく方向に向きを変えて移動してきた場合、ロボットアーム３と移動体とが衝突する恐れを生ずる。

【0063】

これに対し、本実施形態では、ロボットアーム３が回避目標位置Ｑ１に向かって移動体Ｒから遠ざかるように方向変換してゆくので、移動体が図８（ａ）におけるロボットアーム３方向に移動方向を変えたとしても、ロボットアーム３は移動体から離れる方向に移動するので、移動体と衝突することをより確実に回避することができる。

【0064】

以上のように本実施形態によれば、移動体を検出するためのレーザーレーダ２７が走行車２０に搭載され、この走行車２０はベース２に取り付けられたレール１９に移動可能に設けられているので、ロボット本体１を床面に設置すれば、レーザーレーダ２７の設置も同時に終えることができるので、ロボット本体１の設置の他に、レーザーレーダ２７を工場内の所要位置に設置するといった追加作業を必要としない。

【0065】

しかも、レーザーレーダ２７を走行車２０に搭載し、走行車２０を移動させながらレーザーレーダ２７からレーザー光をパルス状に出射することで移動体を検出できるので、工場の天井に取り付けたＣＣＤカメラで検出する場合のように高度な画像解析を必要とせず、移動体の検出を簡易に行うことができる。

【0066】

また、本実施形態では、今回の１周期位置データが基本位置データと異なっていても、前回の１周期位置データと一致していれば、移動他検出フラグをオンすることなく、代わりに、周囲に据え付けられた設備に変化があったとして、今回の１周期位置データを基本位置データに採用して、それまでの基本位置データと置き換えるようにしている（ステップＡ１で「ＹＥＳ」、ステップＡ２で「ＮＯ」、ステップＡ５）。

【0067】

これに対し、今回の１周期位置データと基本位置データと差があったことだけで移動体検出フラグをオンさせる場合には、例えば、部品補給ワゴンを空になったワゴンと置き換えるような場合、部品補給ワゴンの位置が空のワゴンのあった位置に正確に据え置かないと、以後の１周期位置データが全て基本位置データと異なることとなり、その結果、部品補給ワゴンが置かれた後も、移動体検出フラグがオンされ続けることとなる。

【0068】

このような場合、本実施形態では、今回の１周期位置データは基本位置データと異なっても、前回の１周期位置データとは一致するようになる。そして、今回の１周期位置データを基本位置データに置換する。このため、次の１周期位置データは、基本位置データと一致するようになるので、移動体検出フラグはオンされることはなく、いつまでも移動体検出フラグがオンされ続けることはなくなる。

【0069】

また、本実施形態では、ロボットアーム３の停止中に移動体を検出した場合、動作すれば衝突する恐れがあるときは動作を開始させないことは勿論であるが、ロボットアーム３と移動体とが衝突する恐れがなければ、ロボットアーム３の動作を開始し、或いは、安全速度であれば移動体と衝突する恐れがない場合には、安全速度でロボットアーム３の動作を開始させるので、ロボットアーム３の動作禁止を可能な限り防止しながら衝突防止を図ることができる。

【0070】

その上、ロボットアーム３の動作中に移動体を検出した場合、まず減速により衝突を回避し、減速によって衝突回避できないときは、制動をかけて停止させることにより衝突を回避し、制動をかけて停止させることによって衝突回避できないときには、ロボットアーム３を停止動作させながら当該ロボットアーム３の移動方向を移動体の移動方向と同じ方向となるように変更するので、より確実にロボットアーム３が移動体と衝突することを防止できる。

【0071】

なお、本発明は上記し且つ図面に示す実施形態に限定されるものではなく、以下のような拡張或いは変更が可能である。
探査装置はレーザーレーダ２７に限られず、電波（探査信号）を発信しその反射波を測定することにより物体までの距離や方向を測定するレーダであっても良い。
ロボットアームは６軸型のものに限られず、また、水平多関節型のものに限られない。
移動体の駆動源はリニアモータであっても良い。
走行車２０はベース２の周りを往復移動するものに限られず、一方向に移動するものであっても良い。
ロボットアーム３の動作範囲によっては、走行車２０の移動範囲はベース２の周り３６０度未満であっても良い。
ステップＡ５で基本データと置き換える位置データは、今回の１周期位置データに限られず、前回の１周期位置データとしても良い。

【符号の説明】

【0072】

図面中、１はロボット本体、２はベース、３はロボットベース、１２は制御装置（位置データ取得手段、移動位置検出手段、第１の比較手段、第２の比較手段、基本位置データ変換手段、衝突回避措置設定手段、ロボット制御手段）、２３は多孔板（移動位置検出手段）、２４は投光素子、２５は受光素子、２６は位置検出器（移動位置検出手段）、２７はレーザーレーダ（探査装置）を示す。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】