JP2018167333A - ロボット制御装置、ロボットおよびロボットシステム - Google Patents

Abstract

【課題】障害物を回避するロボットの提供。【解決手段】ロボットのアームを制御するロボット制御装置であって、前記アームの先端部の目標位置を取得する目標位置取得部と、前記先端部が前記目標位置に到達する前の位置である第１位置と前記目標位置との間に障害物が存在する場合、前記第１位置と前記目標位置を結ぶ直線に対して凸状の軌跡で前記先端部を移動させるアーム制御部と、を備えるロボット制御装置が構成される。【選択図】図１

Description

本発明は、ロボット制御装置、ロボットおよびロボットシステムに関する。

ロボットの動作範囲に障害物が存在する場合に当該障害物を回避する技術が知られている。例えば、特許文献１においては、ロボットアームの軌道を計画する際にノードとエッジとによって軌道を表現し、障害物に干渉する場合には追加ノードを生成し、既存のノードを削除する構成が開示されている。

特開２０１２−１９０４０５号公報

上述の従来技術においては、障害物に干渉する場合に削除されるノードが存在する。しかし、ノードが教示された位置である場合、そのノードを削除できない場合がある。従って、目標位置に向けて移動する過程で障害物に干渉し得る場合に従来技術を適用できない場合がある。

上記課題の少なくとも一つを解決するために、本発明のロボット制御装置は、ロボットのアームを制御するロボット制御装置であって、アームの先端部の目標位置を取得する目標位置取得部と、先端部が目標位置に到達する前の位置である第１位置と目標位置との間に障害物が存在する場合、第１位置と目標位置を結ぶ直線に対して凸状の軌跡で先端部を移動させるアーム制御部と、を備える。

この構成によれば、第１位置と目標位置との間に障害物が存在する場合、アームの先端部が凸状の軌跡で移動し、障害物を回避することができる。

さらに、凸状の軌跡は、第１位置と目標位置とを通るなめらかな曲線であってもよい。この構成によれば、アームの先端部が凸状の軌跡を移動している過程において、アームの位置が急変する可能性を低減することができる。

さらに、凸状の軌跡は、第１位置と目標位置とを結ぶ直線に垂直な方向に凸であり、当該直線を含む平面上に存在する構成であっても良い。この構成によれば、簡単に凸状の軌跡を定義することができる。

さらに、凸状の軌跡は、第１位置と目標位置とを通る二次曲線である構成であっても良い。この構成によれば、簡単に凸状の軌跡を定義することができる。

ロボットシステムの斜視図である。 ロボット制御装置の機能ブロック図である。 障害物回避処理のフローチャートである。 障害物回避処理のフローチャートである。 第１位置と目標位置とを示す図である。 凸状の軌跡を示す図である。

以下、本発明の実施形態について添付図面を参照しながら以下の順に説明する。なお、各図において対応する構成要素には同一の符号が付され、重複する説明は省略される。
（１）ロボットシステムの構成：
（２）ロボットの制御：
（３）障害物回避処理：
（４）他の実施形態：

（１）ロボットシステムの構成：
図１は本発明の一実施形態にかかるロボット制御装置で制御されるロボットを含むロボットシステムを示す斜視図である。本発明の一実施例としてのロボットシステムは、図１に示すように、ロボット１を備えている。ロボット１はエンドエフェクターを備える６軸ロボットである。また、本実施形態においては、作業台を視野に含むカメラ２０が設置されており、ロボット１の作業範囲を撮影することができる。

ロボット１は、ロボット制御装置４０によって制御される。ロボット制御装置４０はケーブルによりロボット１、カメラ２０と通信可能に接続される。なお、ロボット制御装置４０の構成要素がロボット１に備えられていても良い。また、ロボット制御装置４０は複数の装置によって構成されても良い。また、ロボット制御装置４０は、図示しない教示装置をケーブル、または無線通信によって接続可能である。教示装置は、専用のコンピューターであってもよいし、ロボット１を教示するためのプログラムがインストールされた汎用のコンピューターであってもよい。さらに、ロボット制御装置４０と教示装置とは、一体に構成されていてもよい。

ロボット１は、アームに各種のエンドエフェクターを装着して使用される単腕ロボットである。図１に示すように、ロボット１は、基台Ｔと、６個のアーム部材Ａ１〜Ａ６と、６個の関節Ｊ１〜Ｊ６を備える。基台Ｔは作業台に固定されている。基台Ｔと６個のアーム部材Ａ１〜Ａ６は関節Ｊ１〜Ｊ６によって連結される。関節Ｊ１〜Ｊ６とエンドエフェクターは可動部であり、これらの可動部が動作することによってロボット１は各種の作業を行うことができる。

本実施形態において、関節Ｊ２、Ｊ３、Ｊ５は曲げ関節であり、関節Ｊ１、Ｊ４、Ｊ６はねじり関節である。アームＡのうち最も先端側のアーム部材Ａ６には、力覚センサーＰとエンドエフェクターとが装着される。ロボット１は、６つの関節を駆動させることによって、可動範囲内においてエンドエフェクターを任意の位置に配置し、任意の姿勢（角度）とすることができる。

ロボット１が備えるエンドエフェクターはグリッパー２３であり、対象物Ｗを把持することができる。本実施形態においては、ロボット１が備えるエンドエフェクターに対して相対的に固定された位置がツールセンターポイント（ＴＣＰ）として定義される。ＴＣＰの位置はエンドエフェクターの基準の位置となり、ＴＣＰが原点となり、エンドエフェクターに対して相対的に固定された３次元直交座標系であるＴＣＰ座標系が定義される。

力覚センサーＰは、６軸の力検出器である。力覚センサーＰは、力覚センサー上の点を原点とした３次元直交座標系であるセンサー座標系において互いに直交する３個の検出軸と平行な力の大きさと、当該３個の検出軸まわりのトルクの大きさとを検出する。なお、本実施例では６軸ロボットを例にしているが、ロボットの態様は種々の態様であっても良いし、ロボット１の態様が異なっていてもよい。また、関節Ｊ６以外の関節Ｊ１〜Ｊ５のいずれか１つ以上に力検出器としての力覚センサーを備えても良い。

ロボット１が設置された空間を規定する座標系をロボット座標系というとき、ロボット座標系は、水平面上において互いに直交するｘ軸とｙ軸と、鉛直上向きを正方向とするｚ軸とによって規定される３次元の直交座標系である（図１参照）。ｚ軸における負の方向は概ね重力方向と一致する。またｘ軸周りの回転角をＲｘで表し、ｙ軸周りの回転角をＲｙで表し、ｚ軸周りの回転角をＲｚで表す。ｘ，ｙ，ｚ方向の位置により３次元空間における任意の位置を表現でき、Ｒｘ，Ｒｙ，Ｒｚ方向の回転角により３次元空間における任意の姿勢を表現できる。以下、位置と表記した場合、姿勢も意味し得ることとする。また、力と表記した場合、トルクも意味し得ることとする。

なお、本実施形態においてはロボット１に作用する力を制御する力制御と、ロボット１の特定の部位を目標位置に配置する位置制御とが実行可能である。力制御においては、任意の点に作用する当該作用力が目標力になるように制御される。各種の部位に作用する力は、３次元直交座標系である力制御座標系において定義される。目標力（トルクを含む）は、力制御座標系で表現された力の作用点を起点としたベクトルで表現可能であり、目標力ベクトルの起点は力制御座標系の原点であり、作用力の方向は力制御座標系の１軸方向と一致している。

位置制御においては、制御対象となる点が目標位置に移動するように制御される。制御対象となる点の位置は、ロボット座標系において定義される。本実施形態において各種の座標系の関係は予め定義されており、各種の座標系での座標値は互いに変換可能である。すなわち、ＴＣＰ座標系、センサー座標系、ロボット座標系、力制御座標系における位置やベクトルは互いに変換可能である。ここでは簡単のため、ロボット制御装置４０がＴＣＰの位置およびＴＣＰに作用する作用力をロボット座標系で制御する説明をするが、ロボット１の位置やロボット１に作用する力は、各種の座標系で定義でき、互いに変換可能であるため、位置や力がどの座標系で定義され、制御されても良い。むろん、ここで述べた座標系以外にも他の座標系（例えば対象物に固定されたオブジェクト座標系等）が定義され、変換可能であっても良い。

（２）ロボットの制御：
ロボット１は、教示を行うことにより各種作業が可能となる汎用ロボットであり、図２に示すようにアクチュエーターとしてのモーターＭ１〜Ｍ６と、センサーとしてのエンコーダーＥ１〜Ｅ６とを備える。アームを制御することはモーターＭ１〜Ｍ６を制御することを意味する。モーターＭ１〜Ｍ６とエンコーダーＥ１〜Ｅ６とは、関節Ｊ１〜Ｊ６のそれぞれに対応して備えられており、エンコーダーＥ１〜Ｅ６はモーターＭ１〜Ｍ６の回転角度を検出する。

ロボット制御装置４０は、コンピューター等のハードウェア資源と記憶部４１に記憶された各種のソフトウェア資源を備え、プログラムを実行可能である。本実施形態においてロボット制御装置４０は、目標位置取得部４２、アーム制御部４３として機能する。なお、ハードウェア資源は、ＣＰＵ，ＲＡＭ，ＲＯＭ等からなる構成であっても良いし、ＡＳＩＣ等によって構成されても良く、種々の構成を採用可能である。

本実施形態において、アーム制御部４３は、位置制御部４３ａ、力制御部４３ｂ、サーボ４３ｃを備えている。また、アーム制御部４３においては、モーターＭ１〜Ｍ６の回転角度の組み合わせと、ロボット座標系におけるＴＣＰの位置との対応関係Ｕ１が図示しない記憶媒体に記憶され、座標系の対応関係Ｕ２が定義され、図示しない記憶媒体に記憶されている。従って、アーム制御部４３は、対応関係Ｕ２に基づいて、任意の座標系におけるベクトルを他の座標系におけるベクトルに変換することができる。例えば、アーム制御部４３は、力覚センサーＰの出力に基づいてセンサー座標系でのロボット１への作用力を取得し、ロボット座標系におけるＴＣＰの位置に作用する力に変換することができる。また、アーム制御部４３は、力制御座標系で表現された目標力をロボット座標系におけるＴＣＰの位置における目標力に変換することができる。むろん、対応関係Ｕ１，Ｕ２は記憶部４１に記憶されていても良い。

アーム制御部４３は、アームを駆動することによって、ロボット１とともに移動する各種の部位の位置や各種の部位に作用する力を制御することができ、位置の制御は主に位置制御部４３ａ、力の制御は主に力制御部４３ｂによって実行される。サーボ４３ｃは、サーボ制御を実行することが可能であり、エンコーダーＥ１〜Ｅ６の出力が示すモーターＭ１〜Ｍ６の回転角度Ｄ_aと、制御目標である目標角度Ｄtとを一致させるフィードバック制御を実行する。すなわち、サーボ４３ｃは、回転角度Ｄ_aと目標角度Ｄtとの偏差、当該偏差の積分、当該偏差の微分にサーボゲインＫpp，Ｋpi，Ｋpdを作用させたＰＩＤ制御を実行することができる。

さらに、サーボ４３ｃは、当該サーボゲインＫpp，Ｋpi，Ｋpdが作用した出力と、回転角度Ｄ_aの微分との偏差、当該偏差の積分、当該偏差の微分にサーボゲインＫvp，Ｋvi，Ｋvdを作用させたＰＩＤ制御を実行することができる。当該サーボ４３ｃによる制御は、モーターＭ１〜Ｍ６のそれぞれに対して実行可能である。従って、各サーボゲインはロボット１が備える６軸のそれぞれについて実行可能である。

記憶部４１には、ロボット１を制御するためのパラメーター４１ａおよびロボットプログラム４１ｂが記憶される。本実施形態において、パラメーター４１ａおよびロボットプログラム４１ｂは、教示によって生成され、記憶部４１に記憶される。なお、ロボットプログラム４１ｂは、主に、ロボット１が実施する作業のシーケンス（工程の順序）を示し、予め定義されたコマンドの組み合わせによって記述される。また、パラメーター４１ａは、主に、各工程を実現するために必要とされる具体的な値であり、各コマンドの引数として記述される。

すなわち、本実施形態において、一連の作業は複数の工程に分けられ、各工程に含まれる位置制御や力制御において参照されるパラメーターがパラメーター４１ａとして記憶部４１に記憶される。位置制御に使用するパラメーター４１ａは、より具体的には、各工程におけるロボット１のＴＣＰの始点としての目標位置および終点としての目標位置を含み、これらの位置は、例えばロボット座標系で定義される。すなわち、ロボット座標系の各軸についての並進位置と回転位置とが定義される。力制御に関するパラメーター４１ａは、例えば、目標力を示すパラメーターであり、目標力ベクトルの起点と、起点からの６軸成分（３軸の並進力、３軸のトルク）を示すパラメーターやインピーダンスパラメーター（後述するｍ，ｄ，ｋ）等である。

本実施形態において目標位置取得部４２は、記憶部４１を参照してパラメーター４１ａおよびロボットプログラム４１ｂを取得する。このように、本実施形態においてはパラメーター４１ａおよびロボットプログラム４１ｂに基づいてＴＣＰの目標位置が取得されるため、目標位置取得部４２は、ＴＣＰの目標位置を取得する機能に相当する。なお、本実施形態においてＴＣＰはアームの先端部に該当するため、目標位置取得部４２はアームの先端部の目標位置を取得する機能である。

本実施形態において、一連の作業は複数の工程に分けられ、各工程を実施するロボットプログラム４１ｂが教示によって生成されるが、ロボット１の作業過程において位置制御部４３ａは、ロボットプログラム４１ｂが示す各工程をさらに微小時間ΔＴ毎の微小工程に細分化する。そして、位置制御部４３ａは、パラメーター４１ａに基づいて微小工程毎の目標位置であるサブ目標位置Ｌtを生成する。すなわち、位置制御部４３ａは、始点としての目標位置と終点としての目標位置との間をＴＣＰが直線移動するようにサブ目標位置Ｌtを生成する。力制御部４３ｂは、パラメーター４１ａに基づいて一連の作業の各工程における目標力ｆLtを取得する。

すなわち、位置制御部４３ａは、パラメーター４１ａを参照し、各工程の始点としての目標位置から終点としての目標位置まで移動する場合（姿勢の場合は姿勢が変化する場合）の微小工程毎のＴＣＰの位置をサブ目標位置Ｌtとして生成する。力制御部４３ｂは、各工程についての力制御パラメーターが示す目標力を参照し、力制御座標系とロボット座標系との対応関係Ｕ２に基づいて当該目標力をロボット座標系における目標力ｆLtに変換する。当該目標力ｆLtは、任意の点に作用する力として変換され得るが、ここでは、後述の作用力がＴＣＰに作用している力として表現されるため、当該作用力と目標力ｆLtとを運動方程式で解析するため、目標力ｆLtがＴＣＰの位置における力に変換されるとして説明を行う。むろん、工程によっては、目標力ｆLtが定義されない場合もあり、この場合、力制御を伴わない位置制御が行われる。

なお、ここでＬの文字は、ロボット座標系を規定する軸の方向（ｘ，ｙ，ｚ，Ｒｘ，Ｒｙ，Ｒｚ）のなかのいずれか１個の方向を表すこととする。また、Ｌは、Ｌ方向の位置も表すこととする。例えば、Ｌ＝ｘの場合、ロボット座標系にて設定されたサブ目標位置のｘ方向成分がＬt＝ｘtと表記され、目標力のｘ方向成分がｆLt＝ｆxtと表記される。

位置制御や力制御を実行するため、アーム制御部４３は、ロボット１の状態を取得することができる。すなわち、アーム制御部４３は、モーターＭ１〜Ｍ６の回転角度Ｄaを取得し、対応関係Ｕ１に基づいて、当該回転角度Ｄaをロボット座標系におけるＴＣＰの位置Ｌ（ｘ，ｙ，ｚ，Ｒｘ，Ｒｙ，Ｒｚ）に変換することができる。またアーム制御部４３は、対応関係Ｕ２を参照し、ＴＣＰの位置Ｌと、力覚センサーＰの検出値および位置とに基づいて、力覚センサーＰに現実に作用している力をＴＣＰに作用している作用力ｆ_Ｌに変換してロボット座標系において特定することができる。

すなわち、力覚センサーＰに作用している力は、センサー座標系で定義される。そこで、アーム制御部４３は、ロボット座標系におけるＴＣＰの位置Ｌと対応関係Ｕ２と力覚センサーＰの検出値に基づいて、ロボット座標系においてＴＣＰに作用する作用力ｆ_Ｌを特定する。また、ロボットに作用するトルクは、作用力ｆ_Ｌと、ツール接触点（エンドエフェクターとワークの接触点）から力覚センサーＰまでの距離とから算出することができ、図示されないｆ_Ｌトルク成分として特定される。なお、アーム制御部４３は、作用力ｆ_Ｌに対して重力補償を行う。重力補償とは、作用力ｆ_Ｌから重力成分を除去する処理である。重力補償は、例えば、ＴＣＰの姿勢ごとにＴＣＰに作用する作用力ｆ_Ｌの重力成分を予め調査しておき、作用力ｆ_Ｌから当該重力成分を減算するなどして実現可能である。

ＴＣＰに作用する重力以外の作用力ｆ_Ｌと、ＴＣＰに作用すべき目標力ｆLtとが特定されると、力制御部４３ｂは、対象物等の物体がＴＣＰに存在し、当該ＴＣＰに力が作用し得る状態において、インピーダンス制御による補正量ΔＬ（以後、力由来補正量ΔＬと呼ぶ。）を取得する。すなわち、力制御部４３ｂはパラメーター４１ａを参照して目標力ｆLtとインピーダンスパラメーターｍ，ｄ，ｋを取得し、運動方程式（１）に代入して力由来補正量ΔＬを取得する。なお、当該力由来補正量ΔＬは、ＴＣＰが機械的インピーダンスを受けた場合に、目標力ｆLtと作用力ｆ_Ｌとの力偏差Δｆ_Ｌ（ｔ）を解消するために、ＴＣＰが移動すべき位置Ｌの大きさを意味する。

（１）式の左辺は、ＴＣＰの位置Ｌの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｌの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｌに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆLtから現実の作用力ｆ_Ｌを減算した力偏差Δｆ_Ｌ（ｔ）によって構成される。（１）式における微分とは、時間による微分を意味する。

力由来補正量ΔＬが得られると、アーム制御部４３は、対応関係Ｕ１に基づいて、ロボット座標系を規定する各軸の方向の動作位置を、各モーターＭ１〜Ｍ６の目標の回転角度である目標角度Ｄtに変換する。サーボ４３ｃは、目標角度ＤtからモーターＭ１〜Ｍ６の現実の回転角度であるエンコーダーＥ１〜Ｅ６の出力（回転角度Ｄa）を減算することにより、駆動位置偏差Ｄe（＝Ｄt−Ｄa）を算出する。サーボ４３ｃは、駆動位置偏差ＤeにサーボゲインＫpp，Ｋpi，Ｋpdを乗算した値と、現実の回転角度Ｄaの時間微分値である駆動速度との差である駆動速度偏差に、サーボゲインＫvp，Ｋvi，Ｋvdを乗算した値とを加算することにより、制御量Ｄcを導出する。制御量Ｄcは、モーターＭ１〜Ｍ６のそれぞれについて特定され、各モーターＭ１〜Ｍ６の制御量ＤcでモーターＭ１〜Ｍ６のそれぞれが制御される。アーム制御部４３がモーターＭ１〜Ｍ６を制御する信号は、ＰＷＭ（Pulse Width Modulation）変調された信号である。

以上のように、運動方程式に基づいて目標力ｆLtから制御量Ｄcを導出してモーターＭ１〜Ｍ６を制御するモードを力制御モードというものとする。またアーム制御部４３は、エンドエフェクター等の構成要素が対象物Ｗから力を受けない非接触状態の工程では、力制御を行わず、サブ目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６を制御する。サブ目標位置から線形演算で導出する回転角度でモーターＭ１〜Ｍ６を制御するモードを位置制御モードというものとする。さらに、アーム制御部４３は、サブ目標位置から線形演算で導出する回転角度と目標力を運動方程式に代入して導出する回転角度とを例えば線型結合によって統合し、統合した回転角度でモーターＭ１〜Ｍ６を制御するハイブリッドモードでもロボット１を制御することができる。これらのモードはロボットプログラム４１ｂによって予め決められる。

位置制御モードまたはハイブリッドモードで制御を行う場合、位置制御部４３ａは、微小工程毎のサブ目標位置Ｌtを取得する。微小工程毎のサブ目標位置Ｌtが得られると、アーム制御部４３は、対応関係Ｕ１に基づいて、ロボット座標系を規定する各軸の方向の動作位置を、各モーターＭ１〜Ｍ６の目標の回転角度である目標角度Ｄtに変換する。サーボ４３ｃは、サーボゲインＫpp，Ｋpi，Ｋpd，Ｋvp，Ｋvi，Ｋvdを取得し、目標角度Ｄtに基づいて、制御量Ｄcを導出する。制御量Ｄcは、モーターＭ１〜Ｍ６のそれぞれについて特定され、各モーターＭ１〜Ｍ６の制御量ＤcでモーターＭ１〜Ｍ６のそれぞれが制御される。この結果、各工程において、ＴＣＰは、微小工程毎のサブ目標位置Ｌtを経由し、各工程の始点としての目標位置から終点としての目標位置まで移動する。なお、ハイブリッドモードでは、アーム制御部４３は、微小工程毎のサブ目標位置Ｌtに、力由来補正量ΔＬを加算することにより動作位置（Ｌt＋ΔＬ）を特定し、当該動作位置に基づいて目標角度Ｄtを取得し、制御量Ｄcを取得する。

以上のように、本実施形態においては、ロボット１のＴＣＰを目標位置に移動させることによって各種の作業を行わせることができる。しかし、本実施形態において目標位置間の移動は、直線移動であるため、アームの直線的な移動先に障害物が存在する状態であると、アームと障害物とが干渉してしまう。

そこで、本実施形態においては、干渉を防止するため障害物を監視する構成を備えている。具体的には、本実施形態においてアーム制御部４３は、カメラ２０を制御し、カメラ２０が備える撮像センサーによって撮像された画像を取得することができる。当該画像は、作業対象である対象物Ｗの検出に利用可能であり、本実施形態においては障害物の検出に利用される。

カメラ２０から画像が出力されると、アーム制御部４３は、撮像画像に基づいて物体の形状、大きさおよび位置を検出する。物体の形状、大きさおよび位置の検出は種々の手法で実施されて良く、撮像画像内の像の位置や形状、大きさに基づいて検出されても良いし、既定の形状および大きさの物体のテンプレートに基づいてテンプレートマッチング処理が行われても良いし、測距計等によって検出されても良く、種々の構成が採用可能である。

いずれにしても、物体が検出されると、アーム制御部４３は、物体とアームとが干渉するか否かを判定する。すなわち、物体の形状、大きさおよび位置が特定された状態において、アーム制御部４３は、現在のアームの状態からＴＣＰが次の目標位置に達するまでの間に、アームと物体とがロボット座標系の同一位置（または既定距離以下の位置）に存在する場合、物体とアームとが干渉すると判定される。

なお、当該判定は種々の手法で行われて良く、ＴＣＰが各サブ目標位置に存在する状況でアームが占める領域を、ロボット１のアームの３次元モデルに基づいて特定し、当該領域内に物体が含まれる場合に干渉すると判定される構成等を採用可能である。むろん、他の手法、例えば、物体に接する多角形（立方体等）を想定し、当該立方体と目標位置間を結ぶ直線とが交差する場合に干渉すると判定される構成など、種々の構成を採用可能である。

物体とアームとが干渉すると判定された場合、当該物体はアームから見ると障害物である。この場合、アーム制御部４３は、第１位置と目標位置を結ぶ直線に対して凸状の軌跡でＴＣＰを移動させる。すなわち、アーム制御部４３は、先端部としてのＴＣＰが目標位置に到達する前の位置である第１位置と目標位置との間に障害物が存在する場合に、サブ目標位置を修正する。この際、サブ目標位置が凸上の軌跡となるようにサブ目標位置を修正する。修正が行われるとアーム制御部４３の位置制御部４３ａは、修正後のサブ目標位置Ｌtでサーボ４３ｃを動作させる。この結果、アームの移動先に障害物が存在する場合であっても、アームは当該障害物を回避して目標位置に移動することができる。

（３）障害物回避処理：
次に、障害物回避処理を詳細に説明する。図３、図４は、障害物回避処理のフローチャートである。本実施形態においては、ロボット１による作業中において一定期間（例えば１００ｍｓ）毎に障害物回避処理の実行が開始される。障害物回避処理によって凸状の軌跡が特定された場合、次の目標位置まで障害物回避処理は開始されない。

障害物回避処理が開始されると、アーム制御部４３は、物体情報を取得する（ステップＳ１００）。すなわち、アーム制御部４３は、カメラ２０を制御してカメラ２０の撮像画像を取得し、物体の検出処理を行う。物体が検出された場合、アーム制御部４３は当該物体の形状、大きさおよび位置を示す情報を物体情報として取得する。

次に、アーム制御部４３は、エンコーダーＥ１〜Ｅ６の出力を取得する（ステップＳ１０５）。次に、アーム制御部４３は、アームの先端部の位置を取得する（ステップＳ１１０）。すなわち、アーム制御部４３は、ステップＳ１０５で取得されたエンコーダーの出力に基づいて、アームの位置および姿勢を取得し、アームに連動する先端部（ＴＣＰ）の位置をロボット座標系において取得する。

次に、アーム制御部４３は、次の目標位置を取得する（ステップＳ１１５）。すなわち、目標位置取得部４２は記憶部４１を参照してパラメーター４１ａおよびロボットプログラム４１ｂを取得する。取得されたパラメーター４１ａおよびロボットプログラム４１ｂは位置制御部４３ａに受け渡され、位置制御部４３ａは、現在のＴＣＰの位置の次に到達すべき目標位置を特定する。

次に、アーム制御部４３は、次の目標位置までの移動過程においてアームと物体とが干渉するか否かを判定する（ステップＳ１２０）。すなわち、アーム制御部４３は、サブ目標位置に基づいてアームが占める領域を取得し、ステップＳ１００で取得された物体の形状、大きさおよび位置と比較し、物体がアームと干渉する障害物であるか否かを判定する。ステップＳ１２０において、アームと物体とが干渉すると判定されない場合、アーム制御部４３は、障害物回避処理を終了する。すなわち、アーム制御部４３は、障害物の回避を伴わない通常の制御を続ける。

一方、ステップＳ１２０において、アームと物体とが干渉すると判定された場合、アーム制御部４３は、凸方向の基準となる基準方向を取得する（ステップＳ１３０）。当該基準方向は、繰り返し処理によって凸形状を決定する際の初期の凸方向であり、本実施形態においては、ＴＣＰの現在位置である第１位置と次の目標位置と既定の固定位置とを通る平面に垂直な方向である。

図５は、固定位置が基台Ｔの根元である場合における平面を一点鎖線によって模式的に示す図である。図５においては、目標位置を位置Ｐo、第１位置を位置Ｐ₁、基台Ｔの根元を位置Ｐｂとして示している。また、ＴＣＰが第１位置Ｐ₁に存在する状態におけるロボット１のアームを破線、ＴＣＰが目標位置Ｐoに存在する状態におけるロボット１のアームを実線によって示している。図６は、位置Ｐ₁ＰoＰｂで形成される平面を模式的に示しており、当該平面に垂直な基準方向Ｄｂを破線の矢印によって示している。平面に垂直な方向は２方向あり得るが、いずれか一方が基準方向とされる。

次に、アーム制御部４３は、回転角度増分ｄφと繰り返し回数Ｎ_φを取得する（ステップＳ１３５）。本実施形態においては、第１位置Ｐ₁と目標位置Ｐoを結ぶ直線を軸として凸方向を回転させながら障害物を回避可能な凸状の軌跡を探索する構成となっており、当該回転の１ステップに相当する回転角度増分ｄφは予め決められている。そこで、アーム制御部４３は当該予め決められた回転角度増分ｄφを取得する。繰り返し回数Ｎ_φは当該ステップの最大回数であり、アーム制御部４３は３６０度／ｄφによって繰り返し回数Ｎ_φを取得する。

次に、アーム制御部４３は、凸の大きさの最大値ｈmax、最小値ｈmin、増分ｄｈを取得する（ステップＳ１４０）。本実施形態において、凸状の軌跡は、第１位置Ｐ₁と目標位置Ｐoとを通るなめらかな曲線であり、放物線である。さらに、本実施形態においては、第１位置Ｐ₁と目標位置Ｐoとを結ぶ直線の中点から、凸方向に凸の大きさだけ離れた位置が頂点となる。

本実施形態においては、当該凸の大きさを変化させながら障害物を回避可能な凸状の軌跡を探索する構成となっており、当該変化の１ステップに相当する大きさの増分ｄｈは予め決められている。また、凸の大きさの最大値ｈmax、最小値ｈminも予め決められている。そこで、アーム制御部４３は当該予め決められた最大値ｈmax、最小値ｈmin、増分ｄｈを取得する

次に、アーム制御部４３は、大きさ変化の繰り返し回数Ｎhを取得する（ステップＳ１４５）。すなわち、繰り返し回数Ｎhは当該大きさの変化ステップの最大繰り返し回数であり、アーム制御部４３は（ｈmax−ｈmin）／ｄｈによって繰り返し回数Ｎhを取得する。

次に、アーム制御部４３は、回転のステップ数をカウントする変数ｉを０に初期化し（ステップＳ１５０）、変数ｉが角度変化の上限回数を示す繰り返し回数Ｎ_φより小さいか否かを判定する（ステップＳ１５５）。ステップＳ１５５において、変数ｉが繰り返し回数Ｎ_φより小さいと判定されない場合、アーム制御部４３は、図示しないディスプレイにエラーを表示して（ステップＳ１６０）、障害物回避処理を終了する。すなわち、この場合、障害物の回避が不可能であると見なされ、ロボット１の作業が停止される。この場合、人為的な障害物の除去等の後に再度ロボット１を動作させることが可能になる。

ステップＳ１５５において、変数ｉが繰り返し回数Ｎ_φより小さいと判定された場合、アーム制御部４３は、凸方向の角度を基準方向からｉ・ｄφだけ回転させる（ステップＳ１６５）。すなわち、アーム制御部４３は、ｉ番目のステップにおける回転角をｉ・ｄφによって算出し、基準方向に対して回転させた角度を現在の角度として取得する。図６においては、ｉ・ｄφがφである場合の回転角が例示されている。すなわち、この例においては、基準方向Ｄｂから角度が矢印方向に回転するようにφが増加して凸方向が変化していく。

次に、アーム制御部４３は、凸の大きさ変化のステップ数をカウントする変数ｊを０に初期化し（ステップＳ１７０）、変数ｊが大きさ変化の上限回数を示す繰り返し回数Ｎhより小さいか否かを判定する（ステップＳ１７５）。ステップＳ１７５において、変数ｊが繰り返し回数Ｎhより小さいと判定されない場合、アーム制御部４３は、変数ｉをインクリメントし（ステップＳ１７７）、ステップＳ１５５以降の処理を繰り返す。

一方、ステップＳ１７５において、変数ｊが繰り返し回数Ｎhより小さいと判定された場合、アーム制御部４３は、凸方向の大きさをｈminからｊ・ｄｈだけ増加させる（ステップＳ１８０）。すなわち、アーム制御部４３は、ｊ番目のステップにおける凸の大きさをｊ・ｄｈによって算出し、大きさの最小値ｈminに対して加えた大きさを現在の凸の大きさとして取得する。図６においては、ｉ・ｄｈ＋ｈminがｈである場合の大きさが例示されている。すなわち、この例においては、大きさｈがｈminからｈmaxに変化していく。

ｈminやｈmaxは予め決められていれば良く、任意性があるが、ｈminが０ではない値とされることにより、凸状の軌跡の探索における無駄な探索が減る可能性を高めることができる。すなわち、第１位置Ｐ₁と目標位置Ｐoとの間で障害物が干渉し得る場合、第１位置Ｐ₁と目標位置Ｐoとを結ぶ直線から微少量だけ変化させて凸状の軌跡を生成しても、干渉を避けられない場合が多い。そこで、ｈminを０ではない下限値とし、凸の大きさが当該下限値以上になるように設定すれば、凸状の軌跡の探索における無駄な探索が減る可能性を高めることができる。

さらに、ロボット１は、極めて多数の姿勢を取ることができるものの、あらゆる位置にＴＣＰを配置できるほど任意性が高くはない場合も多い。従って、第１位置Ｐ₁と目標位置Ｐoとの間を結ぶ直線から過度に遠い領域には、ロボット１によってＴＣＰを配置できない場合が増加し得る。そこで、ｈmaxを上限値とし、凸の大きさが当該上限値以下になるように設定すれば、凸状の軌跡の探索において、ロボット１で実現できない軌跡が探索される可能性を低減することができる。

次に、アーム制御部４３は、凸方向の角度および大きさに基づいて放物線を取得する（ステップＳ１８５）。すなわち、アーム制御部４３は、ロボット座標系において、第１位置Ｐ₁と目標位置Ｐoとを結ぶ直線を軸として基準方向Ｄｂからφだけ回転した方向に向けて、直線から大きさｈだけはなれた位置を頂点とみなす。そして、アーム制御部４３は、第１位置Ｐ₁と目標位置Ｐoと頂点とを通る放物線を取得する。なお、当該放物線は、第１位置Ｐ₁と目標位置Ｐoとを結ぶ直線に垂直な方向に凸であり、当該直線を含む平面上に存在する二次曲線である。

次に、アーム制御部４３は、凸状の軌跡を実現するアーム関節角度を計算する（ステップＳ１９０）。すなわち、アーム制御部４３は、ステップＳ１８５で取得された放物線を、第１位置Ｐ₁と目標位置Ｐoとの間のＴＣＰの軌道と見なす。そして、アーム制御部４３は、ＴＰＣが微小時間ΔＴ毎に当該軌道上に位置するように微小工程毎の目標位置であるサブ目標位置Ｌtを算出する。なお、ロボット１は、上述のようにあらゆる位置にＴＣＰを配置できるほど任意性が高くはないため、微小工程毎の目標位置であるサブ目標位置Ｌtが算出できず、ステップＳ１９０の計算が失敗する場合もある。

次に、アーム制御部４３は、アーム関節角度の取得が成功したか否かを判定し（ステップＳ１９５）、成功したと判定されない場合には、変数ｊをインクリメントし（ステップＳ２１０）、ステップＳ１７５以降の処理を繰り返す。ステップＳ１９５において、アーム関節角度の取得が成功したと判定された場合、アーム制御部４３は、凸状の軌跡において障害物と干渉するか否かを判定する（ステップＳ２００）。すなわち、アーム制御部４３は、ステップＳ１９０で取得された凸状の軌跡を実現するアーム関節角度のそれぞれに基づいてアームが占める領域を取得し、ステップＳ１００で取得された障害物の形状、大きさおよび位置と比較し、障害物がアームと干渉するか否かを判定する。ステップＳ２００において、アームと障害物とが干渉すると判定された場合、アーム制御部４３は、ステップＳ２１０以後の処理を繰り返す。すなわち、アーム制御部４３は、障害物を回避可能な凸状の軌跡の探索を続ける。

一方、ステップＳ２００において、アームと障害物とが干渉しないと判定された場合、アーム制御部４３は、凸状の軌跡を実現するアーム関節角度でロボットを制御する（ステップＳ２０５）。すなわち、アーム制御部４３は、ステップＳ１９０において計算されたサブ目標位置に基づいてロボット１のアームを位置制御する。

（４）他の実施形態：
以上の実施形態は本発明を実施するための一例であり、他にも種々の実施形態を採用可能である。例えば、ロボット制御装置は、ロボットに内蔵されていても良いし、ロボットの設置場所と異なる場所、例えば外部のサーバ等に備えられていても良い。さらに、ロボット制御装置が複数のロボットを制御するように構成されていても良い。ロボット制御装置が複数の装置に分散して配置されていても良い。例えば、ロボット制御装置の一部がロボットに内蔵され、他の一部がロボットの外部のサーバ等に配置されていても良い。

さらに、上述の実施形態の一部の構成が省略されてもよいし、処理の順序が変動または省略されてもよい。例えば、凸状の軌跡が探索される際に、凸状の軌跡の候補が障害物と干渉するか否か判定された後に当該軌跡を実現するアーム関節角度が計算されても良い。また、凸方向の角度の変化と凸の大きさの変化の順序が逆であっても良い。さらに、増分ｄφ、ｄｈによる回転角や大きさの増加は一様でなくても良い。例えば、最初のステップで基準方向から角度増分ｄφだけ増加させ、次のステップで基準方向から角度増分ｄφだけ減少させる処理が繰り返されても良い。また、最初のステップで基準方向から角度増分ｄφだけ増加させ、次のステップで基準方向＋１８０度の方向から角度増分ｄφだけ増加させる処理が繰り返されても良い。さらに、最初のステップでｈminから大きさの増分ｄｈだけ増加させ、次のステップでｈmaxから大きさの増分ｄｈだけ減少させる処理が繰り返されても良い。

さらに、凸状の軌跡を探索する際のパラメーターであるｄφ、ｄｈ、ｈmax、ｈminは可変であっても良い。例えば、これらのパラメーターを第１位置と目標位置との距離に応じて変化させても良いし、第１位置と目標位置との間の障害物の大きさや数等に応じて変化させても良い。前者としては、例えば、第１位置と目標位置との距離が長いほどｈmaxやｈminが大きく、または小さくなる構成等が挙げられる。前者としては、例えば、障害物の数が多いほどｈmaxやｈminが大きくなる構成等が挙げられる。

アームは、ロボットの設置位置に対して相対的に移動し、姿勢が変化するように構成されていれば良く、その自由度（可動軸の数等）は任意である。ロボットの態様は、種々の態様であって良く、直交ロボット、水平多関節ロボット、垂直多関節ロボット、双腕ロボット等であって良い。むろん、軸の数やアームの数、エンドエフェクターの態様等は種々の態様を採用可能である。

目標位置取得部は、アームの先端部の目標位置を取得することができればよい。すなわち、ロボットにおいては、アームの先端の特定の部位を目標位置に移動させる位置制御が行われ、目標位置取得部は、当該位置制御における目標位置を取得することができればよい。先端部は、位置制御の目標が設定される部位であり、アーム全体から見て線端側に位置する部位であれば良く、例えば、エンドエフェクターの先端であっても良いし、エンドエフェクターに固定された位置（ＴＣＰ等）であっても良い。

目標位置は、ロボットによる作業の過程でアームの先端部が存在すべき位置や通過すべき位置等であれば良く、少なくとも１個存在する。むろん、位置制御以外の制御、例えば、力制御が併用される場合において先端部が存在すべき位置や通過すべき位置等は目標位置となり得る。

アーム制御部は、第１位置と目標位置との間に障害物が存在する場合、第１位置と目標位置を結ぶ直線に対して凸状の軌跡で先端部を移動させることができればよい。すなわち、アーム制御部は、第１位置と目標位置とを結ぶ直線の軌跡ではなく、当該直線上に存在し得る障害物を避ける凸状の軌跡を設定し、当該軌跡に従ってアームの先端部が移動するようにロボットを制御する。

障害物は、上述の実施形態のようにカメラの撮像画像に基づいて特定されても良いし、他の構成、例えば、作業空間に存在する物体（工場の生産設備の部材等）の設計情報等に基づいてアームの先端部の周囲の物体の位置を特定し、第１位置および目標位置と比較される構成等であっても良い。また、障害物は、アームの先端部やアームと干渉し得る任意の物体である。

第１位置は、先端部が目標位置に到達する前における先端部の位置であればよい。従って、第１位置が目標位置であっても良い。この場合において、２個の目標位置の間に障害物が存在するならば、両目標位置間において先端部が凸状の軌跡を移動する。

凸状の軌跡は、第１位置と目標位置を結ぶ直線に対して凸であれば良く、軌跡に沿って移動する場合に当該直線から一旦遠ざかり、後に近づく軌跡であれば良い。従って、上述の実施形態のような、なめらかな曲線以外の軌跡、例えば、階段状の軌跡であっても良い。

凸状の軌跡がなめらかの曲線である場合、当該曲線は、ロボットのアームが急激な動作を行わないような曲線であることが好ましい。すなわち、変化が大きい曲線の軌跡をアームの先端部でトレースしようとすると、変化が大きい点でアームの位置が急激に変化することがある。そこで、なめらかな曲線、例えば、微分可能な曲線によって凸状の軌跡を構成すれば、アームの位置が急激に変化する可能性を低減することができる。

第１位置と目標位置とを通る二次曲線は上述のような放物線以外にも、種々の曲線を想定可能であり、例えば、楕円や双曲線の一部である構成を採用可能である。

１…ロボット、２０…カメラ、２３…グリッパー、４０…ロボット制御装置、４１…記憶部、４１ａ…パラメーター、４１ｂ…ロボットプログラム、４２…目標位置取得部、４３…アーム制御部、４３ａ…位置制御部、４３ｂ…力制御部、４３ｃ…サーボ

Claims (6)

1. ロボットのアームを制御するロボット制御装置であって、
   前記アームの先端部の目標位置を取得する目標位置取得部と、
   前記先端部が前記目標位置に到達する前の位置である第１位置と前記目標位置との間に障害物が存在する場合、前記第１位置と前記目標位置を結ぶ直線に対して凸状の軌跡で前記先端部を移動させるアーム制御部と、
   を備えるロボット制御装置。
2. 前記凸状の軌跡は、前記第１位置と前記目標位置とを通るなめらかな曲線である、
   請求項１に記載のロボット制御装置。
3. 前記凸状の軌跡は、前記第１位置と前記目標位置とを結ぶ直線に垂直な方向に凸であり、当該直線を含む平面上に存在する、
   請求項１または請求項２のいずれかに記載のロボット制御装置。
4. 前記凸状の軌跡は、前記第１位置と前記目標位置とを通る二次曲線である、
   請求項１〜請求項３のいずれかに記載のロボット制御装置。
5. 請求項１〜請求項４のいずれかに記載されたロボット制御装置によって制御されるロボット。
6. 請求項１〜請求項４のいずれかに記載されたロボット制御装置と、当該ロボット制御装置によって制御される前記ロボットとを備えるロボットシステム。

Images