JP2018027580A - ロボット制御装置 - Google Patents

Abstract

【課題】ランダムに移動するワークにロボットアームを精度よく追従させる。
【解決手段】視覚センサにより撮像される対象物の画像から、対象物における複数の特徴点の位置を所定の周期で検出する特徴点検出部５１と、検出された複数の特徴点の位置に基づいて複数の特徴点の夫々の運動方程式を所定の周期毎に更新し、更新された各運動方程式から算出される複数の特徴点の夫々の位置に基づいて対象物の位置又は姿勢を算出する位置姿勢算出部５２と、算出された対象物の位置又は姿勢に基づいて、対象物に追従するようにロボットアーム４の動作を制御するロボットアーム動作制御部５３と、を備えるロボット制御装置５を提供する。
【選択図】図２

Description

本発明は、ビジュアルサーボによりロボットアームの制御を行うのに好適なロボット制御装置に関する。

従来、ロボットアームを制御する技術の一つとして、カメラ等の視覚センサにより、移動するワーク（対象物）の位置及び姿勢を計測し続け、この計測結果に基づいてロボットアームを対象物に追従させるビジュアルサーボがある（例えば、特許文献１及び特許文献２）。

特許第４２６５０８８号公報 特開２０１１−１４３４９４号公報

しかしながら、上述の特許文献１及び特許文献２に記載のロボット制御装置では、ワークの移動速度が一定であると仮定しているため、ランダムに移動するワークに対しては、ロボットアームを精度よく追従させることができないという問題があった。
また、カメラでワークを検出できない場合には、ワークの一部がカメラの視野に入っているか否かに関わらず、ワークの運動方程式によりワークの位置を計算するため、計算上のワーク位置と実際のワーク位置との間にずれが生じ、追従精度が落ちてしまうという問題があった。

本発明は、上述した事情に鑑みてなされたものであって、ランダムに移動するワークに対してロボットアームを精度よく追従させることのできるロボット制御装置を提供すること目的としている。

上記目的を達成するため、本発明は以下の手段を提供する。
本発明の第一の態様は、視覚センサにより撮像される対象物の画像から、前記対象物における複数の特徴点の位置を所定の周期で検出する特徴点検出部と、前記検出された複数の特徴点の位置に基づいて前記複数の特徴点の夫々の運動方程式を前記所定の周期毎に更新し、更新された各前記運動方程式から算出される前記複数の特徴点の夫々の位置に基づいて前記対象物の位置又は姿勢を算出する位置姿勢算出部と、前記算出された対象物の位置又は姿勢に基づいて、前記対象物に追従するようにロボットアームの動作を制御するロボットアーム動作制御部と、を備えるロボット制御装置である。

上記第一の態様に係るロボット制御装置によれば、特徴点検出部によって、視覚センサにより撮像される対象物の画像から、対象物上の複数の特徴点の位置が所定の周期で検出される。次に、位置姿勢算出部によって、特徴点検出部によって検出された複数の特徴点の位置に基づいて、これら複数の特徴点毎の運動方程式が所定の周期毎に更新される。また、位置姿勢算出部によって、更新された各運動方程式から新たに複数の特徴点の夫々の位置が算出され、算出された複数の特徴点の位置に基づいて対象物の位置又は姿勢が算出される。そして、ロボットアーム動作制御部によって、位置姿勢算出部によって算出された対象物の位置又は姿勢に基づいて、対象物に追従するようにロボットアームが制御される。

このように、所定の周期で各特徴点の運動方程式が更新されるため、オンラインでの対象物の運動方程式の構築が可能となる。結果として、ランダムに移動する対象物に対して高精度にロボットアームを追従させることができる。

上記ロボット制御装置において、前記位置姿勢算出部が、前記対象物における前記複数の特徴点に対し、該複数の特徴点の検出頻度に基づいて重み付けを行う構成であってもよい。

このようにすることで、障害物等によって複数の特徴点のうちの一部が遮蔽され、対象物の位置や姿勢を視覚センサにより直接検出できない場合であっても、運動方程式から算出された各特徴点の位置及び視覚センサで検出可能な各特徴点の位置に、検出頻度に基づく重み付けをして対象物の位置を算出することにより、ロボットアームを精度よく対象物に追従させることができる。

上記ロボット制御装置において、前記ロボットアーム動作制御部が、前記ロボットアームの特異点を回避する特異点回避部を備える構成であってもよい。
この構成によれば、特異点を回避することが可能となるため、ロボットアームにおける特定の関節が異常に高速回転をする等の操作上の不具合を回避することができる。

本発明によれば、ランダムに移動するワークに対してロボットアームを精度よく追従させることができるという効果を奏する。

本発明の一実施形態に係るロボットシステムの概略構成を示す図である。 図１に示されるロボットシステムのブロック図である。 図２に示されるロボット制御装置における処理を示すフローチャートである。 図２に示される位置姿勢算出部における処理を示すフローチャートである。 図２に示されるロボットアーム動作制御部における処理を示すフローチャートである。

以下、本発明に係るロボットシステムの実施形態について、図面を参照して詳細に説明する。なお、本明細書において、英文字記号の上部にハットを付したものを「（英文字記号）^ｈａｔ」と表記し、英文字記号の上部にバーを付したものを「（英文字記号）^ｂａｒ」と表記し、英文字記号の上部にドットを付したものを「（英文字記号）^ｄｏｔ」と表記することとする。

図１に示されるように、本実施形態のロボットシステム１は、カメラ２と、カメラ２を支持する架台３と、ロボットアーム４と、カメラ２及びロボットアーム４が接続されるロボット制御装置５と、を備えている。ロボット制御装置５は、カメラ２から得られる視覚情報に基づいてロボットアーム４がワーク（対象物）７の動きに追従するようにビジュアルサーボによる制御を行うように構成されている。

ワーク７は、ベルトコンベヤや回転テーブル等の搬送装置６の上に載置されており、搬送装置６の移動に合わせてワーク７が移動するようになっている。
なお、本実施形態において、カメラ２は架台３に支持されることとしているが、これに限定されず、ロボットアーム４に搭載されていてもよい。

以下、ロボットシステム１に備えられるロボット制御装置５の構成について、図２を参照して詳しく説明する。なお、ロボット制御装置５は、図示しないプロセッサを備えており、このプロセッサが、図２に示される各機能ブロックにおける処理を実行するように構成されている。

図２に示されるように、ロボット制御装置５は、機能ブロックとして、特徴点検出部５１と、位置姿勢算出部５２と、ロボットアーム動作制御部５３と、を備えている。位置姿勢算出部５２は、機能ブロックとして、運動方程式更新部５２１と、特徴点位置算出部５２２と、重み算出部５２３と、回転・位置行列算出部５２４と、を備えている。ロボットアーム動作制御部５３は、機能ブロックとして、軌道生成部５３１と、特異点回避部５３２と、加算部５３３と、を備えている。

特徴点検出部５１は、カメラ２と、位置姿勢算出部５２に備えられる運動方程式更新部５２１及び重み算出部５２３とに接続されている。運動方程式更新部５２１は、特徴点位置算出部５２２に接続されている。特徴点位置算出部５２２は、重み算出部５２３と回転・位置行列算出部５２４とに接続されている。重み算出部５２３は、回転・位置行列算出部５２４に接続されている。

回転・位置行列算出部５２４は、ロボットアーム動作制御部５３に備えられる軌道生成部５３１に接続されている。軌道生成部５３１は、特異点回避部５３２に接続されている。軌道生成部５３１及び特異点回避部５３２は、加算部５３３に接続されている。加算部５３３は、ロボットアーム４に接続されている。

特徴点検出部５１は、カメラ２によって撮影されたワーク７を含む画像に対し、予め設定された形状によりパターンマッチングを行い、この形状に一致する画像内のワーク７における複数の特徴点の位置ｙ_ｋ（ｔ_ｉ ^ｋ）を検出するように構成されている。ここで、検出された位置ｙ_ｋ（ｔ_ｉ ^ｋ）は、具体的には、ｋ番目の特徴点について、カメラでワークを検出した時刻ｔ_ｉ ^ｋにおけるｘ，ｙ及びｚ方向を含む位置ベクトルを表している。

運動方程式更新部５２１は、特徴点検出部５１によって検出された各特徴点の位置ｙ_ｋ（ｔ_ｉ ^ｋ）を用いることにより、各特徴点の運動方程式を更新するように構成されている。 カメラ２によって撮影された画像から検出される各特徴点の位置は、運動方程式を用いると、式（１）のように表わすことができる。

ここで、ｆ_ｋ［ｙ_ｋ（ｔ_ｉ−１ ^ｋ），ｔ_ｉ ^ｋ］は、各特徴点の運動方程式を示しており、ｖ_ｉ ^ｋはセンサノイズを示している。すなわち、時刻ｔ_ｉ ^ｋにおいて検出される位置ベクトルは、一周期前の時刻ｔ_ｉ−１ ^ｋにおいて検出された位置ｙ_ｋ（ｔ_ｉ−１ ^ｋ）を用いて更新された運動方程式とセンサノイズとに基づいて表される。
なお、各特徴点の運動方程式は、例えば、式（２）及び式（３）により表すことができる。

ここで、ｘ_ｋ（ｔ）は、時刻ｔにおける各特徴点の位置、速度及び加速度を含む９次元ベクトル、ｗ（ｔ）は、特徴点の速度変化の要因である加速度ベクトル、ｖ（ｔ）は、カメラによる検出時のノイズを示している。

運動方程式更新部５２１は、より具体的には、下記の式（４）及び式（５）によりｙ^ｂａｒ _ｋ（ｔ_ｉ ^ｋ）を算出し、これによって各運動方程式を更新するように構成されている。

ここで、ｙ^ｈａｔ _ｋ（ｔ_ｉ ^ｋ）は、時刻ｔ_ｉ ^ｋにおいて、一周期前の時刻ｔ_ｉ−１ ^ｋにおける位置ｙ^ｂａｒ _ｋ（ｔ_ｉ−１ ^ｋ）を用いて更新された運動方程式によって求めた計算値である。また、式（５）におけるｙ^ｂａｒ _ｋ（ｔ_ｉ ^ｋ）は、時刻ｔ_ｉ ^ｋにおけるカメラ２による検出値ｙ_ｋ（ｔ_ｉ ^ｋ）と、式（４）を用いて求めた計算値ｙ^ｈａｔ _ｋ（ｔ_ｉ ^ｋ）との両方を考慮して算出した各特徴点の位置を示している。なお、式（５）において、Ｆ_ｋ（ｔ_ｉ ^ｋ）は、ｙ_ｋ（ｔ_ｉ ^ｋ）とｙ^ｈａｔ _ｋ（ｔ_ｉ ^ｋ）との差分のゲインを示している。

特徴点位置算出部５２２は、上述のようにして求められた時刻ｔ_ｉ ^ｋにおける位置ｙ^ｂａｒ _ｋ（ｔ_ｉ ^ｋ）によって更新された運動方程式を用いて、式（６）のように、現在時刻ｔ_ｎｏｗにおける各特徴点の位置ｙ^ｈａｔ _ｋ（ｔ_ｎｏｗ）を算出するように構成されている。

重み算出部５２３は、各特徴点に対する重みを算出するように構成されている。具体的には、カメラ２により検出される回数（頻度）が多い特徴点ほど重要度が高くなるように、式（７）に示されるように、現在時刻ｔ_ｎｏｗにおける各特徴点の位置の共分散Σ^ｈａｔ _ｋ（ｔ_ｎｏｗ）を各特徴点の重みとして算出するように構成されている。

ここで、ｇ_ｋは、式（８）に示されるように、運動方程式ｆ_ｋに対して共分散を求めた結果である。

また、Σ^ｂａｒ _ｋ（ｔ_ｉ ^ｋ）は、時刻ｔ_ｉ ^ｋにおけるカメラ２による検出値ｙ（ｔ_ｉ ^ｋ）の共分散と、運動方程式によって求めた計算値ｙ^ｈａｔ _ｋ（ｔ_ｉ ^ｋ）の共分散との両方を考慮して算出された共分散である。
なお、検出できなかった特徴点がある場合には、その特徴点の位置は、前回検出されたときの情報を利用して算出される。

回転・位置行列算出部５２４は、重み算出部５２３で算出された共分散Σ^ｈａｔ _ｋ（ｔ_ｎｏｗ）と、特徴点位置算出部５２２で算出された特徴点の位置ｙ^ｈａｔ _ｋ（ｔ_ｎｏｗ）とを用いて、各特徴点に対して重み付けされたワーク７の回転行列及び位置行列を式（９）により算出するように構成されている。

ここで、Ｒ_ｗは、ロボットワールド座標系におけるワーク７の回転行列、Ｔ_ｗは、ロボットワールド座標系におけるワーク７の位置行列、ｓ_ｋは、ワーク７に固定された物体座標系から見た特徴点の位置（ｘ、ｙ及びｚ方向を含むベクトル）を示す。ただし、検出されなかった特徴点がある場合には、その特徴点の位置として、前回検出されたときの結果を利用する。

軌道生成部５３１は、ロボットアーム４の軌道を生成するように構成されている。具体的には、ロボットアーム動作制御部５３で生成される現時点におけるＴＣＰの位置及び姿勢（回転行列Ｒ_ｗ及び位置行列Ｔ_ｗ）と、位置姿勢算出部５２によって算出されたワーク７の位置及び姿勢（回転行列Ｒ_ｒ及び位置行列Ｔ_ｒ）との差分を求め、この差分にヤコビアンの逆数（ヤコビアンの逆行列）を乗じ、各軸角度の差分、つまり、ロボットアーム４の指令速度ｑ^ｄｏｔ＊を算出するように構成されている。

ここで、Ｊ^＋は、各軸位置と直交位置との間のヤコビアン変換の逆数である。ｈ（ΔＲ，ΔＴ）は、位置及び姿勢の差分ΔＲ，ΔＴに比例ゲインを乗算することを示す関数であり、比例ゲインは、追従すべきワーク７の移動周波数によって調整される。

特異点回避部５３２は、ロボットアーム動作制御部５３で生成された現時点におけるＴＣＰが特異点付近にある場合に、式（１１）により回避動作のための補間速度Δｑ^ｄｏｔを生成するように構成されている。

ここで、ｐ（ｑ）は、速度を補正するための関数であり、各軸の補正速度である。各軸の補正速度ｐ（ｑ）は、式（１２）及び式（１３）から求めることができる。

Ｈ（ｑ）は、ロボット操作可能性（manipulability）を表す式である。式（１３）に示されるように、各軸の補正速度ｐ（ｑ）は、Ｈ（ｑ）の偏微分から計算される。

上述の式（１３）において、λ_０は、特異点を回避するための比例ゲインを表している。なお、比例ゲインは、ＴＣＰの位置及び姿勢が特異点付近にない場合は、λ_０＝０となり、ＴＣＰの位置及び姿勢が特異点付近にあった場合には、λ_０の値が大きくなる。

加算部５３３は、軌道生成部５３１で生成された指令速度ｑ^ｄｏｔ＊と、特異点回避部で算出された補間速度Δｑ^ｄｏｔを加算して、ロボットアーム４の各軸速度コマンドｑ^ｄｏｔを算出するように構成されている。

算出された各軸速度コマンドｑ^ｄｏｔにより、ロボットアーム４の制御が行われる。

以下、上記構成を備えるロボット制御装置５の作用について、図３から図５を参照しながら説明する。図３から図５は、ロボット制御装置５に備えられる不図示のプロセッサが所定の周期毎、すなわち、ロボットアーム４の補間周期毎に実行される処理を示している。

まず、カメラ２によって撮像された画像がロボット制御装置５に入力される（図３のステップＳ１）。そして、特徴点検出部５１において、ロボット制御装置５に入力された画像から、ワーク７上における複数の特徴点の位置が検出される（図３のステップＳ２）。次に、位置姿勢算出部５２において、検出されたワーク７上の複数の特徴点の位置に基づいて、ワーク７の位置及び姿勢が算出される（図３のステップＳ３）。その後、ロボットアーム動作制御部５３において、位置姿勢算出部５２において算出されたワーク７の位置及び姿勢に基づいて、ロボットアーム４の各軸に対する速度指令が生成されて（図３のステップＳ４）、ロボットアーム４に対して出力される（図３のステップＳ５）。

次に、図３のステップＳ３で行われるワーク７の位置姿勢算出処理について図４を参照しながら説明する。
まず、運動方程式更新部５２１において、時刻ｔ_ｉ ^ｋにおいて特徴点検出部５１で検出された各特徴点の検出位置ｙ_ｋ（ｔ_ｉ ^ｋ）と、時刻ｔ_ｉ−１ ^ｋにおいて更新された運動方程式から算出された時刻ｔ_ｉ ^ｋにおける各特徴点の算出位置ｙ^ｈａｔ _ｋ（ｔ_ｉ ^ｋ）とに基づいて得られる時刻ｔ_ｉ ^ｋにおける各特徴点の実際の位置ｙ^ｂａｒ _ｋ（ｔ_ｉ ^ｋ）が算出される（式（４）及び式（５）参照）。そして、算出された実際の位置ｙ^ｂａｒ _ｋ（ｔ_ｉ ^ｋ）を用いて、運動方程式が更新される（図４のステップＳ２１）。

続いて、特徴点位置算出部５２２において、式（６）に示されるように、更新された運動方程式を用いて、現在時刻ｔ_ｎｏｗにおける各特徴点の位置ｙ^ｈａｔ _ｋ（ｔ_ｎｏｗ）が算出される（図４のステップＳ２２）。
このように、ロボットアーム４の補間周期毎に運動方程式が更新されるため、ワーク７がランダムに移動する場合であっても、精度よくワーク７の現在位置を算出することができる。

次に、重み算出部５２３において、時刻ｔ_ｉ ^ｋにおいて特徴点検出部５１で検出された各特徴点の検出位置ｙ_ｋ（ｔ_ｉ ^ｋ）の共分散と、時刻ｔ_ｉ−１ ^ｋにおいて更新された運動方程式から算出された時刻ｔ_ｉ ^ｋにおける各特徴点の算出位置ｙ^ｈａｔ _ｋ（ｔ_ｉ ^ｋ）との共分散の両方に基づいて、式（７）によって、現在時刻ｔ_ｎｏｗにおける各特徴点の位置ｙ^ｈａｔ _ｋ（ｔ_ｎｏｗ）の共分散Σ^ｈａｔ _ｋ（ｔ_ｎｏｗ）が算出される（ステップＳ２３）。

そして、回転・位置行列算出部５２４において、重み算出部５２３で算出された重みとしての共分散Σ^ｈａｔ _ｋ（ｔ_ｎｏｗ）と、特徴点位置算出部５２２で算出された各特徴点の位置ｙ^ｈａｔ _ｋ（ｔ_ｎｏｗ）とを用いて、式（９）により、ワーク７の回転行列Ｒ_ｗ及び位置行列Ｔ_ｗが算出される（図４のステップＳ２４）。

このように、カメラ２により検出される回数が多いほど重要度が高くなるように、各特徴点に対して重み付けを行っているため、例えば、障害物等によりワーク７の一部が遮蔽され、検出できない特徴点があったとしても、ロボットアーム４を精確にワーク７に追従させることができる。

次に、図３のステップＳ４で行われるロボットアーム動作制御処理について図５を参照しながら説明する。
まず、軌道生成部５３１において、ロボットアーム４のＴＣＰの位置及び姿勢（回転行列Ｒ_ｒ及び位置行列Ｔ_ｒ）が算出される（図５のステップＳ３１）。続いて、位置姿勢算出部５２によって算出されたワーク７の回転行列Ｒ_ｗ及び位置行列Ｔ_ｗ、すなわち、ワーク７の位置及び姿勢との差分ΔＲ，ΔＴが算出され（図５のステップＳ３２）。この差分ΔＲ，ΔＴから、式（１０）による逆変換演算によってロボットアーム４の各軸に対する指令速度ｑ^ｄｏｔ＊が算出され、軌道生成が行われる（図５のステップＳ３３）。

次に、特異点回避部５３２において、軌道生成部５３１で算出されたロボットアーム４のＴＣＰの位置及び姿勢が特異点付近にあるか否かのチェックが行われる（図５のステップＳ３４）。ＴＣＰの位置及び姿勢が特異点付近にあった場合にのみ、回避動作を生成し、ＴＣＰの位置及び姿勢が特異点付近にない場合には、回避動作の生成はせずに次の処理を行う（図５のステップＳ３５）。具体的には、ステップＳ３６では、式（１１）から式（１３）により、回避動作の補間速度Δｑ^ｄｏｔが生成される。

最後に、軌道生成部５３１において生成された各軸に対する指令速度ｑ^ｄｏｔ＊と、特異点回避部５３２において生成された補間速度Δｑ^ｄｏｔとが加算されて、ロボットアーム４のＴＣＰの各軸に対する速度指令が算出される（図５のステップＳ３７）。
このようにすることで、特異点を回避することができる。

以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこれらの実施形態に限られるものではなく、本発明の要旨を逸脱しない範囲の設計変更等も含まれる。

２ カメラ（視覚センサ）
４ ロボットアーム
５ ロボット制御装置
７ ワーク（対象物）
５１ 特徴点検出部
５２ 位置姿勢算出部
５３ ロボットアーム動作制御部
５２１ 運動方程式更新部
５２２ 特徴点位置算出部
５２３ 重み算出部
５２４ 回転・位置行列算出部
５３１ 軌道生成部
５３２ 特異点回避部

Claims (3)

1. 視覚センサにより撮像される対象物の画像から、前記対象物における複数の特徴点の位置を所定の周期で検出する特徴点検出部と、
   前記検出された複数の特徴点の位置に基づいて前記複数の特徴点の夫々の運動方程式を前記所定の周期毎に更新し、更新された各前記運動方程式から算出される前記複数の特徴点の夫々の位置に基づいて前記対象物の位置又は姿勢を算出する位置姿勢算出部と、
   前記算出された前記対象物の位置又は姿勢に基づいて、前記対象物に追従するようにロボットアームの動作を制御するロボットアーム動作制御部と、
   を備えるロボット制御装置。
2. 前記位置姿勢算出部が、前記対象物における前記複数の特徴点に対し、該複数の特徴点の検出頻度に基づいて重み付けを行う請求項１に記載のロボット制御装置。
3. 前記ロボットアーム動作制御部が、前記ロボットアームの特異点を回避する特異点回避部を備える請求項１又は２に記載のロボット制御装置。

Images