(57)【要約】【課題】ランダムに移動するワークにロボットアームを精度よく追従させる。【解決手段】視覚センサにより撮像される対象物の画像から、対象物における複数の特徴点の位置を所定の周期で検出する特徴点検出部５１と、検出された複数の特徴点の位置に基づいて複数の特徴点の夫々の運動方程式を所定の周期毎に更新し、更新された各運動方程式から算出される複数の特徴点の夫々の位置に基づいて対象物の位置又は姿勢を算出する位置姿勢算出部５２と、算出された対象物の位置又は姿勢に基づいて、対象物に追従するようにロボットアーム４の動作を制御するロボットアーム動作制御部５３と、を備えるロボット制御装置５を提供する。【選択図】図２
2018027580.tif 000002
【特許請求の範囲】
【請求項１】
視覚センサにより撮像される対象物の画像から、前記対象物における複数の特徴点の位置を所定の周期で検出する特徴点検出部と、
前記検出された複数の特徴点の位置に基づいて前記複数の特徴点の夫々の運動方程式を前記所定の周期毎に更新し、更新された各前記運動方程式から算出される前記複数の特徴点の夫々の位置に基づいて前記対象物の位置又は姿勢を算出する位置姿勢算出部と、
前記算出された前記対象物の位置又は姿勢に基づいて、前記対象物に追従するようにロボットアームの動作を制御するロボットアーム動作制御部と、
を備えるロボット制御装置。
【請求項２】
前記位置姿勢算出部が、前記対象物における前記複数の特徴点に対し、該複数の特徴点の検出頻度に基づいて重み付けを行う請求項１に記載のロボット制御装置。
【請求項３】
前記ロボットアーム動作制御部が、前記ロボットアームの特異点を回避する特異点回避部を備える請求項１又は２に記載のロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ビジュアルサーボによりロボットアームの制御を行うのに好適なロボット制御装置に関する。
【背景技術】
【０００２】
従来、ロボットアームを制御する技術の一つとして、カメラ等の視覚センサにより、移動するワーク（対象物）の位置及び姿勢を計測し続け、この計測結果に基づいてロボットアームを対象物に追従させるビジュアルサーボがある（例えば、特許文献１及び特許文献２）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第４２６５０８８号公報
【特許文献２】特開２０１１−１４３４９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述の特許文献１及び特許文献２に記載のロボット制御装置では、ワークの移動速度が一定であると仮定しているため、ランダムに移動するワークに対しては、ロボットアームを精度よく追従させることができないという問題があった。
また、カメラでワークを検出できない場合には、ワークの一部がカメラの視野に入っているか否かに関わらず、ワークの運動方程式によりワークの位置を計算するため、計算上のワーク位置と実際のワーク位置との間にずれが生じ、追従精度が落ちてしまうという問題があった。
【０００５】
本発明は、上述した事情に鑑みてなされたものであって、ランダムに移動するワークに対してロボットアームを精度よく追従させることのできるロボット制御装置を提供すること目的としている。
【課題を解決するための手段】
【０００６】
上記目的を達成するため、本発明は以下の手段を提供する。
本発明の第一の態様は、視覚センサにより撮像される対象物の画像から、前記対象物における複数の特徴点の位置を所定の周期で検出する特徴点検出部と、前記検出された複数の特徴点の位置に基づいて前記複数の特徴点の夫々の運動方程式を前記所定の周期毎に更新し、更新された各前記運動方程式から算出される前記複数の特徴点の夫々の位置に基づいて前記対象物の位置又は姿勢を算出する位置姿勢算出部と、前記算出された対象物の位置又は姿勢に基づいて、前記対象物に追従するようにロボットアームの動作を制御するロボットアーム動作制御部と、を備えるロボット制御装置である。
【０００７】
上記第一の態様に係るロボット制御装置によれば、特徴点検出部によって、視覚センサにより撮像される対象物の画像から、対象物上の複数の特徴点の位置が所定の周期で検出される。次に、位置姿勢算出部によって、特徴点検出部によって検出された複数の特徴点の位置に基づいて、これら複数の特徴点毎の運動方程式が所定の周期毎に更新される。また、位置姿勢算出部によって、更新された各運動方程式から新たに複数の特徴点の夫々の位置が算出され、算出された複数の特徴点の位置に基づいて対象物の位置又は姿勢が算出される。そして、ロボットアーム動作制御部によって、位置姿勢算出部によって算出された対象物の位置又は姿勢に基づいて、対象物に追従するようにロボットアームが制御される。
【０００８】
このように、所定の周期で各特徴点の運動方程式が更新されるため、オンラインでの対象物の運動方程式の構築が可能となる。結果として、ランダムに移動する対象物に対して高精度にロボットアームを追従させることができる。
【０００９】
上記ロボット制御装置において、前記位置姿勢算出部が、前記対象物における前記複数の特徴点に対し、該複数の特徴点の検出頻度に基づいて重み付けを行う構成であってもよい。
【００１０】
このようにすることで、障害物等によって複数の特徴点のうちの一部が遮蔽され、対象物の位置や姿勢を視覚センサにより直接検出できない場合であっても、運動方程式から算出された各特徴点の位置及び視覚センサで検出可能な各特徴点の位置に、検出頻度に基づく重み付けをして対象物の位置を算出することにより、ロボットアームを精度よく対象物に追従させることができる。
【００１１】
上記ロボット制御装置において、前記ロボットアーム動作制御部が、前記ロボットアームの特異点を回避する特異点回避部を備える構成であってもよい。
この構成によれば、特異点を回避することが可能となるため、ロボットアームにおける特定の関節が異常に高速回転をする等の操作上の不具合を回避することができる。
【発明の効果】
【００１２】
本発明によれば、ランダムに移動するワークに対してロボットアームを精度よく追従させることができるという効果を奏する。
【図面の簡単な説明】
【００１３】
【図１】本発明の一実施形態に係るロボットシステムの概略構成を示す図である。
【図２】図１に示されるロボットシステムのブロック図である。
【図３】図２に示されるロボット制御装置における処理を示すフローチャートである。
【図４】図２に示される位置姿勢算出部における処理を示すフローチャートである。
【図５】図２に示されるロボットアーム動作制御部における処理を示すフローチャートである。
【発明を実施するための形態】
【００１４】
以下、本発明に係るロボットシステムの実施形態について、図面を参照して詳細に説明する。なお、本明細書において、英文字記号の上部にハットを付したものを「（英文字記号）ｈａｔ」と表記し、英文字記号の上部にバーを付したものを「（英文字記号）ｂａｒ」と表記し、英文字記号の上部にドットを付したものを「（英文字記号）ｄｏｔ」と表記することとする。
【００１５】
図１に示されるように、本実施形態のロボットシステム１は、カメラ２と、カメラ２を支持する架台３と、ロボットアーム４と、カメラ２及びロボットアーム４が接続されるロボット制御装置５と、を備えている。ロボット制御装置５は、カメラ２から得られる視覚情報に基づいてロボットアーム４がワーク（対象物）７の動きに追従するようにビジュアルサーボによる制御を行うように構成されている。
【００１６】
ワーク７は、ベルトコンベヤや回転テーブル等の搬送装置６の上に載置されており、搬送装置６の移動に合わせてワーク７が移動するようになっている。
なお、本実施形態において、カメラ２は架台３に支持されることとしているが、これに限定されず、ロボットアーム４に搭載されていてもよい。
【００１７】
以下、ロボットシステム１に備えられるロボット制御装置５の構成について、図２を参照して詳しく説明する。なお、ロボット制御装置５は、図示しないプロセッサを備えており、このプロセッサが、図２に示される各機能ブロックにおける処理を実行するように構成されている。
【００１８】
図２に示されるように、ロボット制御装置５は、機能ブロックとして、特徴点検出部５１と、位置姿勢算出部５２と、ロボットアーム動作制御部５３と、を備えている。位置姿勢算出部５２は、機能ブロックとして、運動方程式更新部５２１と、特徴点位置算出部５２２と、重み算出部５２３と、回転・位置行列算出部５２４と、を備えている。ロボットアーム動作制御部５３は、機能ブロックとして、軌道生成部５３１と、特異点回避部５３２と、加算部５３３と、を備えている。
【００１９】
特徴点検出部５１は、カメラ２と、位置姿勢算出部５２に備えられる運動方程式更新部５２１及び重み算出部５２３とに接続されている。運動方程式更新部５２１は、特徴点位置算出部５２２に接続されている。特徴点位置算出部５２２は、重み算出部５２３と回転・位置行列算出部５２４とに接続されている。重み算出部５２３は、回転・位置行列算出部５２４に接続されている。
【００２０】
回転・位置行列算出部５２４は、ロボットアーム動作制御部５３に備えられる軌道生成部５３１に接続されている。軌道生成部５３１は、特異点回避部５３２に接続されている。軌道生成部５３１及び特異点回避部５３２は、加算部５３３に接続されている。加算部５３３は、ロボットアーム４に接続されている。
【００２１】
特徴点検出部５１は、カメラ２によって撮影されたワーク７を含む画像に対し、予め設定された形状によりパターンマッチングを行い、この形状に一致する画像内のワーク７における複数の特徴点の位置ｙｋ（ｔｉｋ）を検出するように構成されている。ここで、検出された位置ｙｋ（ｔｉｋ）は、具体的には、ｋ番目の特徴点について、カメラでワークを検出した時刻ｔｉｋにおけるｘ，ｙ及びｚ方向を含む位置ベクトルを表している。
【００２２】
運動方程式更新部５２１は、特徴点検出部５１によって検出された各特徴点の位置ｙｋ（ｔｉｋ）を用いることにより、各特徴点の運動方程式を更新するように構成されている。  カメラ２によって撮影された画像から検出される各特徴点の位置は、運動方程式を用いると、式（１）のように表わすことができる。
【００２３】
【数１】
2018027580.tif 000003
【００２４】
ここで、ｆｋ［ｙｋ（ｔｉ−１ｋ），ｔｉｋ］は、各特徴点の運動方程式を示しており、ｖｉｋはセンサノイズを示している。すなわち、時刻ｔｉｋにおいて検出される位置ベクトルは、一周期前の時刻ｔｉ−１ｋにおいて検出された位置ｙｋ（ｔｉ−１ｋ）を用いて更新された運動方程式とセンサノイズとに基づいて表される。
なお、各特徴点の運動方程式は、例えば、式（２）及び式（３）により表すことができる。
【００２５】
【数２】
2018027580.tif 000004
【００２６】
【数３】
2018027580.tif 000005
ここで、ｘｋ（ｔ）は、時刻ｔにおける各特徴点の位置、速度及び加速度を含む９次元ベクトル、ｗ（ｔ）は、特徴点の速度変化の要因である加速度ベクトル、ｖ（ｔ）は、カメラによる検出時のノイズを示している。
【００２７】
運動方程式更新部５２１は、より具体的には、下記の式（４）及び式（５）によりｙｂａｒｋ（ｔｉｋ）を算出し、これによって各運動方程式を更新するように構成されている。
【００２８】
【数４】
2018027580.tif 000006
【００２９】
【数５】
2018027580.tif 000007
ここで、ｙｈａｔｋ（ｔｉｋ）は、時刻ｔｉｋにおいて、一周期前の時刻ｔｉ−１ｋにおける位置ｙｂａｒｋ（ｔｉ−１ｋ）を用いて更新された運動方程式によって求めた計算値である。また、式（５）におけるｙｂａｒｋ（ｔｉｋ）は、時刻ｔｉｋにおけるカメラ２による検出値ｙｋ（ｔｉｋ）と、式（４）を用いて求めた計算値ｙｈａｔｋ（ｔｉｋ）との両方を考慮して算出した各特徴点の位置を示している。なお、式（５）において、Ｆｋ（ｔｉｋ）は、ｙｋ（ｔｉｋ）とｙｈａｔｋ（ｔｉｋ）との差分のゲインを示している。
【００３０】
特徴点位置算出部５２２は、上述のようにして求められた時刻ｔｉｋにおける位置ｙｂａｒｋ（ｔｉｋ）によって更新された運動方程式を用いて、式（６）のように、現在時刻ｔｎｏｗにおける各特徴点の位置ｙｈａｔｋ（ｔｎｏｗ）を算出するように構成されている。
【００３１】
【数６】
2018027580.tif 000008
【００３２】
重み算出部５２３は、各特徴点に対する重みを算出するように構成されている。具体的には、カメラ２により検出される回数（頻度）が多い特徴点ほど重要度が高くなるように、式（７）に示されるように、現在時刻ｔｎｏｗにおける各特徴点の位置の共分散Σｈａｔｋ（ｔｎｏｗ）を各特徴点の重みとして算出するように構成されている。
【００３３】
【数７】
2018027580.tif 000009
ここで、ｇｋは、式（８）に示されるように、運動方程式ｆｋに対して共分散を求めた結果である。
【００３４】
【数８】
2018027580.tif 000010
【００３５】
また、Σｂａｒｋ（ｔｉｋ）は、時刻ｔｉｋにおけるカメラ２による検出値ｙ（ｔｉｋ）の共分散と、運動方程式によって求めた計算値ｙｈａｔｋ（ｔｉｋ）の共分散との両方を考慮して算出された共分散である。
なお、検出できなかった特徴点がある場合には、その特徴点の位置は、前回検出されたときの情報を利用して算出される。
【００３６】
回転・位置行列算出部５２４は、重み算出部５２３で算出された共分散Σｈａｔｋ（ｔｎｏｗ）と、特徴点位置算出部５２２で算出された特徴点の位置ｙｈａｔｋ（ｔｎｏｗ）とを用いて、各特徴点に対して重み付けされたワーク７の回転行列及び位置行列を式（９）により算出するように構成されている。
【００３７】
【数９】
2018027580.tif 000011
ここで、Ｒｗは、ロボットワールド座標系におけるワーク７の回転行列、Ｔｗは、ロボットワールド座標系におけるワーク７の位置行列、ｓｋは、ワーク７に固定された物体座標系から見た特徴点の位置（ｘ、ｙ及びｚ方向を含むベクトル）を示す。ただし、検出されなかった特徴点がある場合には、その特徴点の位置として、前回検出されたときの結果を利用する。
【００３８】
軌道生成部５３１は、ロボットアーム４の軌道を生成するように構成されている。具体的には、ロボットアーム動作制御部５３で生成される現時点におけるＴＣＰの位置及び姿勢（回転行列Ｒｗ及び位置行列Ｔｗ）と、位置姿勢算出部５２によって算出されたワーク７の位置及び姿勢（回転行列Ｒｒ及び位置行列Ｔｒ）との差分を求め、この差分にヤコビアンの逆数（ヤコビアンの逆行列）を乗じ、各軸角度の差分、つまり、ロボットアーム４の指令速度ｑｄｏｔ＊を算出するように構成されている。
【００３９】
【数１０】
2018027580.tif 000012
ここで、Ｊ＋は、各軸位置と直交位置との間のヤコビアン変換の逆数である。ｈ（ΔＲ，ΔＴ）は、位置及び姿勢の差分ΔＲ，ΔＴに比例ゲインを乗算することを示す関数であり、比例ゲインは、追従すべきワーク７の移動周波数によって調整される。
【００４０】
特異点回避部５３２は、ロボットアーム動作制御部５３で生成された現時点におけるＴＣＰが特異点付近にある場合に、式（１１）により回避動作のための補間速度Δｑｄｏｔを生成するように構成されている。
【００４１】
【数１１】
2018027580.tif 000013
ここで、ｐ（ｑ）は、速度を補正するための関数であり、各軸の補正速度である。各軸の補正速度ｐ（ｑ）は、式（１２）及び式（１３）から求めることができる。
【００４２】
【数１２】
2018027580.tif 000014
【００４３】
【数１３】
2018027580.tif 000015
Ｈ（ｑ）は、ロボット操作可能性（manipulability）を表す式である。式（１３）に示されるように、各軸の補正速度ｐ（ｑ）は、Ｈ（ｑ）の偏微分から計算される。
【００４４】
上述の式（１３）において、λ０は、特異点を回避するための比例ゲインを表している。なお、比例ゲインは、ＴＣＰの位置及び姿勢が特異点付近にない場合は、λ０＝０となり、ＴＣＰの位置及び姿勢が特異点付近にあった場合には、λ０の値が大きくなる。
【００４５】
加算部５３３は、軌道生成部５３１で生成された指令速度ｑｄｏｔ＊と、特異点回避部で算出された補間速度Δｑｄｏｔを加算して、ロボットアーム４の各軸速度コマンドｑｄｏｔを算出するように構成されている。
【００４６】
【数１４】
2018027580.tif 000016
算出された各軸速度コマンドｑｄｏｔにより、ロボットアーム４の制御が行われる。
【００４７】
以下、上記構成を備えるロボット制御装置５の作用について、図３から図５を参照しながら説明する。図３から図５は、ロボット制御装置５に備えられる不図示のプロセッサが所定の周期毎、すなわち、ロボットアーム４の補間周期毎に実行される処理を示している。
【００４８】
まず、カメラ２によって撮像された画像がロボット制御装置５に入力される（図３のステップＳ１）。そして、特徴点検出部５１において、ロボット制御装置５に入力された画像から、ワーク７上における複数の特徴点の位置が検出される（図３のステップＳ２）。次に、位置姿勢算出部５２において、検出されたワーク７上の複数の特徴点の位置に基づいて、ワーク７の位置及び姿勢が算出される（図３のステップＳ３）。その後、ロボットアーム動作制御部５３において、位置姿勢算出部５２において算出されたワーク７の位置及び姿勢に基づいて、ロボットアーム４の各軸に対する速度指令が生成されて（図３のステップＳ４）、ロボットアーム４に対して出力される（図３のステップＳ５）。
【００４９】
次に、図３のステップＳ３で行われるワーク７の位置姿勢算出処理について図４を参照しながら説明する。
まず、運動方程式更新部５２１において、時刻ｔｉｋにおいて特徴点検出部５１で検出された各特徴点の検出位置ｙｋ（ｔｉｋ）と、時刻ｔｉ−１ｋにおいて更新された運動方程式から算出された時刻ｔｉｋにおける各特徴点の算出位置ｙｈａｔｋ（ｔｉｋ）とに基づいて得られる時刻ｔｉｋにおける各特徴点の実際の位置ｙｂａｒｋ（ｔｉｋ）が算出される（式（４）及び式（５）参照）。そして、算出された実際の位置ｙｂａｒｋ（ｔｉｋ）を用いて、運動方程式が更新される（図４のステップＳ２１）。
【００５０】
続いて、特徴点位置算出部５２２において、式（６）に示されるように、更新された運動方程式を用いて、現在時刻ｔｎｏｗにおける各特徴点の位置ｙｈａｔｋ（ｔｎｏｗ）が算出される（図４のステップＳ２２）。
このように、ロボットアーム４の補間周期毎に運動方程式が更新されるため、ワーク７がランダムに移動する場合であっても、精度よくワーク７の現在位置を算出することができる。
【００５１】
次に、重み算出部５２３において、時刻ｔｉｋにおいて特徴点検出部５１で検出された各特徴点の検出位置ｙｋ（ｔｉｋ）の共分散と、時刻ｔｉ−１ｋにおいて更新された運動方程式から算出された時刻ｔｉｋにおける各特徴点の算出位置ｙｈａｔｋ（ｔｉｋ）との共分散の両方に基づいて、式（７）によって、現在時刻ｔｎｏｗにおける各特徴点の位置ｙｈａｔｋ（ｔｎｏｗ）の共分散Σｈａｔｋ（ｔｎｏｗ）が算出される（ステップＳ２３）。
【００５２】
そして、回転・位置行列算出部５２４において、重み算出部５２３で算出された重みとしての共分散Σｈａｔｋ（ｔｎｏｗ）と、特徴点位置算出部５２２で算出された各特徴点の位置ｙｈａｔｋ（ｔｎｏｗ）とを用いて、式（９）により、ワーク７の回転行列Ｒｗ及び位置行列Ｔｗが算出される（図４のステップＳ２４）。
【００５３】
このように、カメラ２により検出される回数が多いほど重要度が高くなるように、各特徴点に対して重み付けを行っているため、例えば、障害物等によりワーク７の一部が遮蔽され、検出できない特徴点があったとしても、ロボットアーム４を精確にワーク７に追従させることができる。
【００５４】
次に、図３のステップＳ４で行われるロボットアーム動作制御処理について図５を参照しながら説明する。
まず、軌道生成部５３１において、ロボットアーム４のＴＣＰの位置及び姿勢（回転行列Ｒｒ及び位置行列Ｔｒ）が算出される（図５のステップＳ３１）。続いて、位置姿勢算出部５２によって算出されたワーク７の回転行列Ｒｗ及び位置行列Ｔｗ、すなわち、ワーク７の位置及び姿勢との差分ΔＲ，ΔＴが算出され（図５のステップＳ３２）。この差分ΔＲ，ΔＴから、式（１０）による逆変換演算によってロボットアーム４の各軸に対する指令速度ｑｄｏｔ＊が算出され、軌道生成が行われる（図５のステップＳ３３）。
【００５５】
次に、特異点回避部５３２において、軌道生成部５３１で算出されたロボットアーム４のＴＣＰの位置及び姿勢が特異点付近にあるか否かのチェックが行われる（図５のステップＳ３４）。ＴＣＰの位置及び姿勢が特異点付近にあった場合にのみ、回避動作を生成し、ＴＣＰの位置及び姿勢が特異点付近にない場合には、回避動作の生成はせずに次の処理を行う（図５のステップＳ３５）。具体的には、ステップＳ３６では、式（１１）から式（１３）により、回避動作の補間速度Δｑｄｏｔが生成される。
【００５６】
最後に、軌道生成部５３１において生成された各軸に対する指令速度ｑｄｏｔ＊と、特異点回避部５３２において生成された補間速度Δｑｄｏｔとが加算されて、ロボットアーム４のＴＣＰの各軸に対する速度指令が算出される（図５のステップＳ３７）。
このようにすることで、特異点を回避することができる。
【００５７】
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこれらの実施形態に限られるものではなく、本発明の要旨を逸脱しない範囲の設計変更等も含まれる。
【符号の説明】
【００５８】
２  カメラ（視覚センサ）
４  ロボットアーム
５  ロボット制御装置
７  ワーク（対象物）
５１  特徴点検出部
５２  位置姿勢算出部
５３  ロボットアーム動作制御部
５２１  運動方程式更新部
５２２  特徴点位置算出部
５２３  重み算出部
５２４  回転・位置行列算出部
５３１  軌道生成部
５３２  特異点回避部
【図１】
2018027580.tif 000017
【図２】
2018027580.tif 000018
【図３】
2018027580.tif 000019
【図４】
2018027580.tif 000020
【図５】
2018027580.tif 000021