(57)【要約】  少ない演算量と記憶領域で、対象物の位置及び姿勢の検出ができる物体検出方法、物体検出装置およびこれを搭載したロボットシステムを提供する。  対象物の距離データ（１０１）から対象物の平面部分とエッジを抽出し（１０２）、抽出された複数の平面部分から任意の平面部分を選択してその方程式を算出し（１０３）、算出した平面にエッジ部分を投影してエッジ画像を生成し（１０４）、予めデータベース（１０５）に記憶した基準エッジ画像を順次回転させ（１０６）、各々のエッジ画像間で濃淡値の近いエッジ点を対応させ、その位置の差を位置空間上に投票し（１０７）、投票の最も高い箇所を求めることで（１０８）、対象物の位置姿勢を特定し（１０９）、これを検出対象物の位置と姿勢とする。
2009028489.tif 000002
【特許請求の範囲】
【請求項１】
対象物の三次元空間上の位置及び姿勢を検出する物体検出方法であって、
前記対象物上の各位置からの距離を計測して距離データを得る距離データ取得ステップと、
前記距離データにおける前記対象物の平面部分とエッジ部分を抽出する平面・エッジ抽出ステップと、
抽出された複数の前記平面部分から任意の平面を選択する平面選択ステップと、
前記エッジ部分を前記選択した平面に対して垂直に投影した位置に、前記エッジ部分から前記選択した平面までの距離を濃淡で表したエッジ画像を作成するエッジ画像作成ステップと、
予め既知の位置にある前記対象物で算出してデータベースとして記憶した基準エッジ画像と前記エッジ画像とを比較して前記エッジ画像上の点と同じ濃淡値を持つ前記基準エッジ画像上の点を各々対応付けその位置の差を位置空間に順次投票して最も投票の集まる箇所を求める投票ステップと、
前記投票ステップを前記基準エッジ画像を回転させて順次繰り返し、最も投票の集まった前記基準エッジ画像の角度と前記位置空間上の位置とから前記対象物の位置及び姿勢を求める位置・姿勢算出ステップと、を有している
ことを特徴とする、物体検出方法。
【請求項２】
前記平面・エッジ抽出ステップは、
前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出するステップと、
前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出するステップとを有している
ことを特徴とする、請求項１記載の物体検出方法。
【請求項３】
前記平面選択ステップでは、前記平面・エッジ抽出ステップにおいて抽出された前記平面部分の中から最も領域が大きいものを優先して選択する
ことを特徴とする、請求項１記載の物体検出方法。
【請求項４】
対象物の三次元空間上の位置及び姿勢を検出する物体検出方法であって、
前記対象物上の各位置からの距離を計測して距離データを得る距離データ取得ステップと、
前記距離データにおける前記対象物の平面部分を抽出する平面抽出ステップと、
抽出された複数の前記平面部分から任意の平面を選択する平面選択ステップと、
前記選択された平面に対し、前記距離データを垂直に投影して前記対象物と前記選択された平面との距離を濃淡で表わした距離画像を作成する距離画像作成ステップと、
前記距離画像上の各点に対し局所的に法線方向を算出してその角度を濃淡で表わすことで法線角度画像を作成する法線角度画像作成ステップと、
予め既知の位置にある対象物で算出してデータベースとして記憶した基準距離画像と前記距離画像作成ステップで作成した前記距離画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を位置空間に順次投票する距離画像投票ステップと、
予め既知の位置にある対象物で算出してデータベースとして記憶した基準法線角度画像と前記法線角度画像作成ステップで作成した前記法線角度画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を前記位置空間に順次投票する距離画像投票ステップと、
距離画像投票ステップ及び距離画像投票ステップの結果、前記位置空間において最も投票の集まる箇所を求める投票ステップと、
前記投票ステップを前記基準距離画像及び基準法線角度画像を回転させて順次繰り返し、最も投票の集まった前記基準距離画像の角度と前記位置空間上の位置とから対象物の位置と姿勢とを求める位置・姿勢算出ステップと、を有している
ことを特徴とする、物体検出方法。
【請求項５】
前記平面抽出ステップは、
前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出するステップと、
前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出するステップとを有している
ことを特徴とする、請求項４記載の物体検出方法。
【請求項６】
前記平面選択ステップでは、前記平面抽出ステップにおいて抽出された前記平面部分の中から最も領域が大きいものを優先して選択する
ことを特徴とする、請求項４記載の物体検出方法。
【請求項７】
対象物の三次元元空間上の位置及び姿勢を検出する物体検出装置であって、
前記対象物上の各位置からの距離を計測して距離データを取得する距離センサと、
前記距離センサの計測値に基づいて演算を行なう演算装置と、を有し
前記演算装置は、
前記距離センサにより計測された距離データにおける前記対象物の平面部分とエッジ部分とを抽出する平面・エッジ抽出部と、
抽出された複数の前記平面部分から任意の平面を選択する平面選択部と、
前記平面選択部で算出された前記平面に対して各エッジ部を垂直に投影した位置に前記エッジ部から前記選択した平面までの距離を濃淡で表したエッジ画像を作成するエッジ画像作成部と、
予め既知の位置にある前記対象物に対して算出し、基準となる基準エッジ画像を記憶したエッジ画像データベースと、
前記エッジ画像作成部の作成した前記エッジ画像と前記基準エッジ画像データベースに記憶されているの基準エッジ画像とを比較して前記エッジ画像上の点と同じ濃淡値を持つ前記基準エッジ画像上の点をそれぞれ対応付け、その位置の差を位置空間上に順次投票して最も投票の集まる箇所を求める投票部と、
前記基準エッジ画像を回転させて順次前記投票部の作業を繰り返し、最も投票の集まった前記基準エッジ画像の角度と前記位置空間上の位置とから前記対象物の位置及び姿勢を求める物体検出部を有している
ことを特徴とする、物体検出装置。
【請求項８】
前記平面・エッジ抽出部は、
前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出し、前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出する
ことを特徴とする、請求項７記載の物体検出装置。
【請求項９】
前記平面選択部は、前記平面・エッジ抽出部により抽出された前記平面部分の中から最も領域が大きいものを優先して選択する
ことを特徴とする、請求項７記載の物体検出装置。
【請求項１０】
対象物の三次元空間上の位置及び姿勢を検出する物体検出装置であって、
前記対象物上の各位置からの距離を計測して距離データを取得する距離センサと、
前記距離センサの計測値に基づいて演算を行なう演算装置と、を有し
前記演算装置は、
前記距離データにおける前記対象物の平面部分を抽出する平面抽出部と、
抽出された複数の前記平面部分から任意の平面を選択する平面選択部と、
前記選択された平面に対し、前記距離データを垂直に投影して前記対象物と前記選択された平面との距離を濃淡で表わした距離画像を作成する距離画像作成部と、
前記距離画像上の各点に対し局所的に法線方向を算出してその角度を濃淡で表わすことで法線角度画像を作成する法線角度画像作成部と、
予め既知の位置にある前記対象物に対して算出し、基準となる基準距離画像を記憶した基準画像データベースと、
予め既知の位置にある前記対象物に対して算出し、基準となる基準法線角度画像を記憶した基準法線角度画像データベースと、
前記基準距離画像と前記距離画像作成ステップで作成した前記距離画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を位置空間に順次投票する距離画像投票部と、
前記基準法線角度画像と前記法線画像作成ステップで作成した前記法線角度画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を前記位置空間に順次投票する距離画像投票部と、
前記距離画像投票及び前記距離画像投票の結果、前記位置空間において最も投票の集まる箇所を求める投票部と、
前記投票ステップを前記基準距離画像及び基準法線角度画像を回転させて順次繰り返し、最も投票の集まった前記基準距離画像の角度と前記位置空間上の位置とから対象物の位置と姿勢とを求める位置・姿勢算出部と、を有している
ことを特徴とする、物体検出装置。
【請求項１１】
前記平面抽出部は、
前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出し、前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出する
ことを特徴とする、請求項１０記載の物体検出装置。
【請求項１２】
前記平面選択部は、前記平面抽出部により抽出された前記平面部分の中から最も領域が大きいものを優先して選択する
ことを特徴とする、請求項１０記載の物体検出装置。
【請求項１３】
対象物を把持するロボットシステムであって、
前記対象物を把持して搬送する搬送ロボットと、
請求項７〜１２のいずれか１項に記載の物体検出装置と、
前記物体検出装置の出力に基づいて、前記搬送ロボットの前記対象物に対する移動軌跡を制御するロボット制御装置と、を有している
ことを特徴とする、ロボットシステム。
【請求項１４】
ランダムな姿勢の前記対象物を収納するワークストッカを有し、
前記物体検出装置の前記距離センサが前記ワークストッカの全領域について前記距離データを取得する
ことを特徴とする、請求項１３記載のロボットシステム。
【請求項１５】
ランダムな姿勢の前記対象物を収納するワークストッカを有し、
前記物体検出装置の前記距離センサが前記ワークストッカの全領域の内予め設定された所定の部分領域について前記距離データを取得する
ことを特徴とする、請求項１３記載のロボットシステム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、対象物の三次元的な位置及び姿勢を判別する物体検出方法と物体検出装置およびこれを用いたロボットシステムに関する。
【背景技術】
【０００２】
ロボットを用いて把持作業行なう場合には、対象物の判別と位置及び姿勢を検出する必要がある。従来、物体検出方法としてＣＣＤカメラからの二次元の濃淡画像を利用してパターンマッチングを行うことで対象物を検出する方法が取られていた。
しかし、この方法では対象物が全て同じ姿勢である場合には有効であるが、山積み状態である場合など、対象物がＣＣＤカメラに対しランダムな姿勢の場合には、対象物の位置及び姿勢を検出することが困難であった。
このため、従来は、対象物を検出する前に、パーツフィーダにより対象物を取り出して整列させる工程を別途設ける必要があった。
また、パーツフィーダは一種類の対象物に対して専用のものが必要となるため、対象物が多種類存在する場合にはパーツフィーダの設置に掛かるコスト及び手間が大きくなりロボットの把持作業の自動化の障害となっていた。【０００３】
これを解決するべく任意の姿勢の対象物を検出するために、レンジセンサ（距離センサ）によって物体の三次元形状を計測してパターンマッチングする手法が提案されている。
例えば、距離データ画像上の特徴点を複数検出し、各々の特徴点の位置関係を予め記憶しておいた対象物の形状モデル上での特徴点間の位置関係とを比較して最も一致する特徴点群を選別することで物体検出を行うことができる（特許文献１参照）。【０００４】
また、距離データと二次元の濃淡画像の双方を利用する手法も提案されている。濃淡画像から対象物の存在する領域をおおまかに特定し、濃淡画像と同一の視点から計測された距離データ画像から物体の慣性主軸の三次元空間中における位置及び姿勢を求め、この慣性主軸周りに対象物を仮想的に回転させたときの、予め記憶しておいた対象物の形状モデルとの一致度を検出するというものである（特許文献２参照）。
【特許文献１】特開２００１−１４３０７３号公報（６頁、図１）
【特許文献２】特開平７−２８７７５６号公報（１３頁、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかし、特許文献１に記載の物体検出方法では、３点以上の特徴点を対応比較する必要がある。したがって、膨大な計算量が必要であり、演算処理に時間がかかる。
また、１つのモデルに対しても大きな記憶領域が必要であるという問題も生じていた。【０００６】
また、特許文献２の方法では、対象物が山積みされた（ランダムにな姿勢で積み重ねられた）状態では濃淡画像からの対象物の検出は困難であり、システムがロバスト性に欠けるという問題がある。
しかも、慣性主軸まわりに対象物を仮想的に回転させたときの一致度を調べる方法では、処理時間が掛かるという問題もある。
本発明はこのような問題点に鑑みてなされたものであり、少ない計算量で精度良く対象物の位置及び姿勢の検出ができる物体検出方法、物体検出装置およびこれを搭載したロボットシステムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記問題を解決するため、本発明は、次のようにしたのである。
本願発明（請求項１）は、対象物の三次元空間上の位置及び姿勢を検出する物体検出方法であって、前記対象物上の各位置からの距離を計測して距離データを得る距離データ取得ステップと、前記距離データにおける前記対象物の平面部分とエッジ部分を抽出する平面・エッジ抽出ステップと、抽出された複数の前記平面部分から任意の平面を選択する平面選択ステップと、前記エッジ部分を前記選択した平面に対して垂直に投影した位置に、前記エッジ部分から前記選択した平面までの距離を濃淡で表したエッジ画像を作成するエッジ画像作成ステップと、予め既知の位置にある前記対象物で算出してデータベースとして記憶した基準エッジ画像と前記エッジ画像とを比較して前記エッジ画像上の点と同じ濃淡値を持つ前記基準エッジ画像上の点を各々対応付けその位置の差を位置空間に順次投票して最も投票の集まる箇所を求める投票ステップと、前記投票ステップを前記基準エッジ画像を回転させて順次繰り返し、最も投票の集まった前記基準エッジ画像の角度と前記位置空間上の位置とから前記対象物の位置及び姿勢を求める位置・姿勢算出ステップと、を有していることを特徴としている。
前記平面・エッジ抽出ステップは、前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出するステップと、前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出するステップとを有していることが好ましい（請求項２）。
前記平面選択ステップでは、前記平面・エッジ抽出ステップにおいて抽出された前記平面部分の中から最も領域が大きいものを優先して選択することが好ましい（請求項３）。
また、本願発明（請求項４）は、対象物の三次元空間上の位置及び姿勢を検出する物体検出方法であって、前記対象物上の各位置からの距離を計測して距離データを得る距離データ取得ステップと、前記距離データにおける前記対象物の平面部分を抽出する平面抽出ステップと、抽出された複数の前記平面部分から任意の平面を選択する平面選択ステップと、前記選択された平面に対し、前記距離データを垂直に投影して前記対象物と前記選択された平面との距離を濃淡で表わした距離画像を作成する距離画像作成ステップと、前記距離画像上の各点に対し局所的に法線方向を算出してその角度を濃淡で表わすことで法線角度画像を作成する法線角度画像作成ステップと、予め既知の位置にある対象物で算出してデータベースとして記憶した基準距離画像と前記距離画像作成ステップで作成した前記距離画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を位置空間に順次投票する距離画像投票ステップと、予め既知の位置にある対象物で算出してデータベースとして記憶した基準法線角度画像と前記法線角度画像作成ステップで作成した前記法線角度画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を前記位置空間に順次投票する距離画像投票ステップと、距離画像投票ステップ及び距離画像投票ステップの結果、前記位置空間において最も投票の集まる箇所を求める投票ステップと、前記投票ステップを前記基準距離画像及び基準法線角度画像を回転させて順次繰り返し、最も投票の集まった前記基準距離画像の角度と前記位置空間上の位置とから対象物の位置と姿勢とを求める位置・姿勢算出ステップと、を有していることを特徴としている。
前記平面抽出ステップは、前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出するステップと、前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出するステップとを有していることが好ましい（請求項５）。
前記平面選択ステップでは、前記平面抽出ステップにおいて抽出された前記平面部分の中から最も領域が大きいものを優先して選択することが好ましい（請求項６）。
また、本願発明（請求項７）は、対象物の三次元元空間上の位置及び姿勢を検出する物体検出装置であって、前記対象物上の各位置からの距離を計測して距離データを取得する距離センサと、前記距離センサの計測値に基づいて演算を行なう演算装置と、を有し前記演算装置は、前記距離センサにより計測された距離データにおける前記対象物の平面部分とエッジ部分とを抽出する平面・エッジ抽出部と、抽出された複数の前記平面部分から任意の平面を選択する平面選択部と、前記平面選択部で算出された前記平面に対して各エッジ部を垂直に投影した位置に前記エッジ部から前記選択した平面までの距離を濃淡で表したエッジ画像を作成するエッジ画像作成部と、予め既知の位置にある前記対象物に対して算出し、基準となる基準エッジ画像を記憶したエッジ画像データベースと、前記エッジ画像作成部の作成した前記エッジ画像と前記基準エッジ画像データベースに記憶されているの基準エッジ画像とを比較して前記エッジ画像上の点と同じ濃淡値を持つ前記基準エッジ画像上の点をそれぞれ対応付け、その位置の差を位置空間上に順次投票して最も投票の集まる箇所を求める投票部と、前記基準エッジ画像を回転させて順次前記投票部の作業を繰り返し、最も投票の集まった前記基準エッジ画像の角度と前記位置空間上の位置とから前記対象物の位置及び姿勢を求める物体検出部を有していることを特徴としている。
前記平面・エッジ抽出部は、前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出し、前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出することが好ましい（請求項８）。
前記平面選択部は、前記平面・エッジ抽出部により抽出された前記平面部分の中から最も領域が大きいものを優先して選択することが好ましい（請求項９）。
また、本願発明（請求項１０）は、対象物の三次元空間上の位置及び姿勢を検出する物体検出装置であって、前記対象物上の各位置からの距離を計測して距離データを取得する距離センサと、前記距離センサの計測値に基づいて演算を行なう演算装置と、を有し前記演算装置は、前記距離データにおける前記対象物の平面部分を抽出する平面抽出部と、抽出された複数の前記平面部分から任意の平面を選択する平面選択部と、前記選択された平面に対し、前記距離データを垂直に投影して前記対象物と前記選択された平面との距離を濃淡で表わした距離画像を作成する距離画像作成部と、前記距離画像上の各点に対し局所的に法線方向を算出してその角度を濃淡で表わすことで法線角度画像を作成する法線角度画像作成部と、予め既知の位置にある前記対象物に対して算出し、基準となる基準距離画像を記憶した基準画像データベースと、予め既知の位置にある前記対象物に対して算出し、基準となる基準法線角度画像を記憶した基準法線角度画像データベースと、前記基準距離画像と前記距離画像作成ステップで作成した前記距離画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を位置空間に順次投票する距離画像投票部と、前記基準法線角度画像と前記法線画像作成ステップで作成した前記法線角度画像とを比較して同じ濃淡値を持つ点を各々対応付け、その位置の差を前記位置空間に順次投票する距離画像投票部と、前記距離画像投票及び前記距離画像投票の結果、前記位置空間において最も投票の集まる箇所を求める投票部と、前記投票ステップを前記基準距離画像及び基準法線角度画像を回転させて順次繰り返し、最も投票の集まった前記基準距離画像の角度と前記位置空間上の位置とから対象物の位置と姿勢とを求める位置・姿勢算出部と、を有していることを特徴としている。
前記平面抽出部は、前記距離データ取得ステップで得られた前記距離データに基づいて、平均曲率を算出し、前記平均曲率の絶対値が所定値よりも小さい箇所を集合させて得られた領域を前記平面部分として抽出することが好ましい（請求項１１）。
前記平面選択部は、前記平面抽出部により抽出された前記平面部分の中から最も領域が大きいものを優先して選択することが好ましい（請求項１２）。
また、本願発明（請求項１３）は、対象物を把持するロボットシステムであって、前記対象物を把持して搬送する搬送ロボットと、請求項７〜１２のいずれか１項に記載の物体検出装置と、前記物体検出装置の出力に基づいて、前記搬送ロボットの前記対象物に対する移動軌跡を制御するロボット制御装置と、を有していることを特徴としている。
ランダムな姿勢の前記対象物を収納するワークストッカを有し、前記物体検出装置の前記距離センサが前記ワークストッカの全領域について前記距離データを取得することが好ましい（請求項１４）。
また、ランダムな姿勢の前記対象物を収納するワークストッカを有し、前記物体検出装置の前記距離センサが前記ワークストッカの全領域の内予め設定された所定の部分領域について前記距離データを取得することが好ましい（請求項１５）。
【発明の効果】
【０００８】
本発明によれば、対象物の位置及び姿勢を検出する際に、検出する対象物から得たエッジ画像と基準エッジ画像との平面上での並進と回転成分だけを求めればよく、計算量が少なくてよいため、同じ性能の演算装置を用いてた場合、より早く対象物の位置及び姿勢を検出することができる。
さらには、位置空間上の投票の最も高い箇所を検出するため、距離センサに対して対象物の一部が隠れていたり、外乱等によりが距離データに影響がある場合でも安定して対象物を検出することができる。
また、対象物の位置及び姿勢を検出する際に、出する対象物から得た距離画像及び法線角度画像と基準距離画像及び基準法線角度画像との平面上での並進と回転成分だけを求めればよく、高速にマッチング処理が可能となる。さらには、位置空間上の投票の最も高い箇所を探索する方法を取るため、対象物の一部が隠れていたり、他の対象物が映り込んでいても投票が集まり安定して対象物の検出が可能である。
また、物体検出装置をロボットシステムに適用することで、ロボットシステムが把持対象物の把持動作を行うに際して、この物体検出装置が把持対象物の位置と姿勢とを高速にかつ正確に求めることができるので、作業効率の良いロボットシステムが得られる。
【図面の簡単な説明】
【０００９】
【図１】本発明の第１実施形態にかかる処理手順を示すフローチャートである。
【図２】本発明の第１、２実施形態にかかるロボットシステムの全体構成を示す図である。
【図３】本発明の一実施形態を示す物体検出装置のブロック図である。
【図４】本発明の第１実施形態にかかるエッジ画像の生成手順を示す図である。
【図５】本発明の第１実施形態にかかるエッジ画像の一例を示す図である。
【図６】（Ａ）〜（Ｃ）はいずれも本発明の第１実施形態にかかる位置空間への投票の態様を示す図である。
【図７】本発明の第２実施形態にかかる処理手順を示すフローチャートである。
【図８】本発明の第２実施形態にかかるエッジ画像の生成態様を説明するための模式図である。
【図９】（Ａ）は本発明の第２実施形態にかかる距離画像を示す図、（Ｂ）は本発明の第２実施形態にかかる法線角度画像を示す図である。
【符号の説明】
【００１０】
２ａ〜２ｃ、１２ａ〜１２ｃ  平面部分
３ａ〜３ｃ  エッジ部分
４、１３  平面
５ａ〜５ｃ、１４ａ〜１４ｃ  平面に投影されたエッジ部分
５０  物体検出装置
５１  平面部・エッジ抽出部
５２  平面選択部
５３  濃淡エッジ画像作成部
５４  基準エッジ画像データベース
５５  位置・姿勢算出部
６０  表示部
７０  ロボット制御装置
８１  ロボットアーム
８２  エンドエフェクタ
８３  距離センサ
８４  ストッカ
１００  ロボットシステム
Ｗ  ワーク（対象物）
【発明を実施するための最良の形態】
【００１１】
以下、本発明の方法を実施するための最良の具体的実施例について、図に基づいて説明する。
まず、本発明の一実施形態にかかる物体検出装置を用いたロボットシステムの概略構成について説明する。
[第１実施形態]
図２に示すように、ロボットシステム１００は、物体検出用演算装置（演算装置）５０、ロボット制御装置７０、ワーク搬送ロボット８１、把持装置８２、距離センサ８３及びワークストッカ８４によって構成されている。距離センサ８３、演算装置５０、ロボット制御装置７０及びワーク搬送ロボット８１はそれぞれ通信可能に接続されている。
ワークストッカ８４は、検出対象物（対象物）としてのワークＷが入った箱状の容器である。ワークストッカ８４には多数のワークＷがランダムな姿勢（ワークＷがとる三次元の方向）で山積みに収納されている。
距離センサ８３はワークストッカ８４の上方に配設されており、ワークストッカ８４の全領域において、後述する距離データを検出するように設定されている。
ワーク搬送用ロボット８１は、ここでは垂直多関節型のマニプレータにより構成されており、ワーク搬送用ロボット８１の先端には、把持装置８２が取り付けられている。把持装置８２は、例えば、ワークＷを掴むグリッパ装置やエアや電磁力による吸着装置等、ワークＷを把持（保持）できるものであればどのような装置を適用してもよい。
ロボット制御装置７０は、コンピュータにより構成されており、ロボットシステム１００の各部機器を制御するように構成されている。
演算装置５０の機能構成については後述する。
図３は距離データから対象物の位置、姿勢を検出する物体検出装置のブロック図の１例を示している。以下、図３を用いて物体検出装置のブロックについて説明する。
演算装置５０は、単数又は複数のコンピュータにより構成されており、５１〜５５の機能ブロックを備えて構成される。平面・エッジ抽出部５１は三次元空間でワークＷを計測した距離データからワークＷの平面部分とエッジ部分を抽出する。平面選択部５２は平面・エッジ抽出部５１で抽出された複数の平面部分より任意に１つの平面を選択して算出する。濃淡エッジ画像作成部５３は平面選択部５２で算出された平面に対して各エッジ部分を垂直に投影した位置にその各エッジ部分から前記算出された平面までの距離を濃淡で表す処理をする。基準エッジ画像データベース５４は予め既知の位置にあるワークＷで算出した、距離に比例した濃淡エッジのデータを格納している。ワークＷの位置・姿勢算出部５５は、濃淡エッジ画像作成部５３の作成した濃淡エッジと前記基準エッジ画像データベースのデータとを比較してワークＷの距離を求めて位置と姿勢を算出する処理を行う。具体的には、濃淡エッジ画像上の点と同じ濃淡値を持つ基準エッジ画像上の点を各々対応付け、その位置の差を位置空間に順次投票して最も投票の集まる箇所を求める。
以下、基準エッジ画像を回転させて順次同様に前記投票を繰り返し、最も投票の集まった前記基準エッジ画像の角度と位置空間上の位置とを求めることによりワークＷの位置と姿勢とを求めるようにしている。
このようにして位置・姿勢算出部５５が算出した結果は、表示部６０で表示させて、オペレータに視覚による確認作業等をさせたり、ロボット制御装置７０に送信して次の作業のための情報とするようになっている。【００１２】
図１は、本発明の第１実施形態にかかる物体検出方法の手順を示すフローチャートである。
物体検出を行うためには、予め検出のワークＷとなる物体の情報（基準エッジ画像）を演算装置５０へ記憶させる必要がある。以下、その手順を説明する。
先ず、ステップＳ１０１では、予め設定された所定の位置に所定の姿勢で置かれたワークＷに対して、距離センサ８３を用いて距離データを取得する。
なお、本実施形態においての、距離データとは、平面（ｘ軸、ｙ軸）上の位置情報と、各ピクセルに対応するワークＷと距離センサ８３との距離（ｚ軸）の情報を平面上のピクセル単位で有するものである。
距離データの取得する距離センサ８３は、レーザ光をワークＷに走査して三次元形状を得る装置（三次元スキャナ）、あるいは複数のカメラでワークＷを撮像した各画像間の対応点を求め、三角測量の原理からワークＷの三次元情報を得る装置（複眼ステレオ視）により構成される。なお、検出のワークＷの全てのパートの距離を検出できるものであれば、そのような方式の距離センサを用いてもよい。
次に、ステップＳ１０２では距離データから平面部分とエッジ部分とを抽出する。距離データから得られた形状が平面か否かの判断は周囲の距離データから曲率を求めることで判断が可能である。曲率を求める代表的な例として平均曲率が挙げられ、平面部分の平均曲率Ｈは式（１）によって求めることができる。【００１３】
2009028489.tif 000003
なお、ｆは距離データ（ｘ、ｙ、ｚ）を表わしている。また、ｆｘ、ｆｙ、ｆｘｘ、ｆｙｙ、ｆｘｙは、それぞれ関数ｆに対して添字ｘ、ｙに関しての微分を表している。
平均曲率Ｈが０（Ｈ＝０）のときは、抽出された平面部分は平面（平坦）である。また、平均曲率Ｈが正（Ｈ＞０）のときには、抽出された平面部分が凸曲面状であり、平均曲率Ｈが負（Ｈ＜０）のときには、抽出された平面部分が凹曲面状となる。
ここでは、式（１）で算出された平均曲率Ｈの絶対値がある程度小さい箇所（予め設定された閾値Ｍよりも平均曲率の絶対値が小さい箇所（Ｍ＞｜Ｈ｜））を集合させて（グループ化して）、領域分割を行うことにより平面とみなせる（近似できる）領域を抽出することができる。
また、距離データからエッジ部分を抽出するには、隣接した距離データとの位置の差が大きい箇所を求めることでエッジ部分を抽出することが出来る。
次に、ステップＳ１０３ではステップＳ１０２で抽出された平面部分から任意の平面を選択し、平面方程式を算出する。ステップＳ１０３において平面を選択する際には、平面領域の大きさが大きいものを優先して選択する。
領域の大きさが大きい平面を優先して選択することにより、物体検出の精度をより向上させることができる。この選択により、ワークストッカ８４内のワークＷから１つのワークが選択されることとなる。
平面は一般に式（２）で表すことができる。
ｚ＝ａｘ＋ｂｙ＋ｃ  ・・・・（２）
選択された平面領域の各距離データの座標を
（ｘｉ、ｙｉ、ｚｉ）  ｉ＝１、…、ｎとすると、
式（３）で表すことができる。【００１４】
2009028489.tif 000004
この式をＲ＝ＰＱとおくと、式（４）となり、平面方程式の係数ａ、ｂ、ｃを求めることができる。【００１５】
2009028489.tif 000005
次に、ステップＳ１０４ではステップＳ１０３で算出された平面にエッジ部分を垂直に投影してエッジ画像を生成する。【００１６】
図４にエッジ画像の生成手順を示す。
ステップＳ１０２までの手順によりワークＷの平面部分２ａ〜２ｃとエッジ部分３ａ〜３ｃを抽出する。ステップＳ１０３により平面部分２ａ〜２ｃから最も領域の大きいもの２ｂを選択し、平面４を算出する。エッジ部分３ａ〜３ｃを平面４に垂直に投影する。このときのエッジ部分２ａ、２ｃ上の点から平面に投影されたエッジ部分５ａ、５ｃ上の点までの距離を求め、これを濃淡値に変換してエッジ画像を生成する。【００１７】
図５にエッジ画像の一例を示す。
エッジ部分から平面までの距離に比例させて濃淡値を決定する。エッジ部分３ｂは平面４上にあるため、エッジ画像上では５ｂで表されるように淡い色と黒との中間で表示する。エッジ部分３ａは平面に対しワークＷ側にあるため投影されたエッジ部分５ａは５ｂよりも濃い色で表示する。反対にエッジ部分３ｃは平面に対し、空間側にあるため投影されたエッジ部分５ｃは５ｂよりも淡い色で表示する。ただし、図５の距離画像の例ではエッジ部分でない部分は白で表示している。これらの工程によりエッジ画像が作成される。
ステップＳ１０５では作成されたエッジ画像を基準エッジ画像としてデータベースに登録する。この際、エッジ画像と共に平面４の方程式も記憶させておく必要がある。【００１８】
次に、実際にワークストッカ８４に積まれているワークＷの位置及び姿勢を検出する手順について説明する。ステップＳ１０１〜Ｓ１０４までの処理は、上述した基準エッジ画像の取得手順と同様である。
図１に示すように、ステップＳ１０１では、距離センサ８３によりワークステッカ８４全体に対しての距離データ（実シーン）を取得する（距離データ取得ステップ）。ステップＳ１０２では、距離データから平面部分とエッジ部分を抽出する（平面・エッジ抽出ステップ）。
そして、ステップＳ１０３では、ステップＳ１０２において抽出された平面部分の中から一つの平面部分を選択して選択した平面を表わす式を算出する（平面選択ステップ）。なお、平面部分を選択する際、平面部分の領域の大きいもの、或いは一番上に存在するものを選択するようにする。
このようにすれば、他のワークＷ等の障害物による干渉の少ないワークＷや上の方に積まれたワークＷの平面を選択することができ、物体検出後、ロボットアーム８１により把持する際に把持し易いワークＷを選択することができ、ロボットアーム８１によるワークＷを把持する精度が向上される。
ステップＳ１０４では算出された平面に対し、エッジ部分を垂直に投影しエッジ画像を作成する（エッジ画像作成ステップ）。
次に、ステップＳ１０６では予めモデルとして登録した基準エッジ画像と比較して位置空間へ投票を行う。【００１９】
図６に位置空間への投票の手順を示す。図６（Ａ）は基準エッジ画像を示しており、図６（Ｂ）は実シーンに対して作成されたエッジ画像を示している。
実シーンで作成されたエッジ画像の任意のエッジ上の点Ｐ１（ｘ１、ｙ１）に対して、モデルとして登録された基準エッジ画像のエッジ上の点Ｐ０（ｘ０、ｙ０）を対応付ける。この際各々の点における濃淡値が近いものだけを対応させて、その位置の差を図６（Ｃ）に示す位置空間上（ｘ１−ｘ０、ｙ１−ｙ０）に投票する。この投票を実シーンのエッジ画像のエッジ上の点に対して繰り返し行う（投票ステップ）。
次に、ステップＳ１０７により投票の最も集中していた箇所（ピーク）を各基準画像の角度に対応した位置空間から探索し、その基準画像の角度と投票の集中していた位置とを求める。
ステップＳ１０８では、基準エッジ画像を所定の角度づつ回転させてステップＳ１０６、Ｓ１０７の処理を実行し、その都度別の位置空間へ投票を繰り返す。
ステップＳ１０８の処理は、基準エッジ画像の累積回転量が所定角度（ここでは、３６０度）に達するまで繰り返し実行される。
既知の位置に置かれたワークＷの平面と実作業時でのワークＷの平面との各々の方程式がステップＳ１０３で求められていることから、実作業時のワークＷの平面部分の姿勢は特定できる。また投票の最も集中していた角度と位置を求めることで、平面上でのワークＷの位置及び回転量が分かることから、ワークＷの三次元空間上での位置と姿勢を算出することができる（ステップＳ１０９）なお、ステップＳ１０８、Ｓ１０９が位置・姿勢算出ステップに相当する。【００２０】
本発明の第１実施形態にかかる物体検出装置及びロボットシステムはこのように構成されているので、ワークＷの位置と姿勢を求める求め方として、まず、ワークＷの平面部分とエッジを抽出し、抽出された複数の平面部分より任意に選択して平面を算出し、エッジを平面に対して垂直に投影した位置にエッジから平面までの距離を濃淡で表すことでエッジ画像を作成し、予め既知の位置にあるワークＷで算出してデータベースとして記憶した基準エッジ画像とを比較し、エッジ画像上の点と同じ濃淡値を持つ基準エッジ画像上の点を各々対応付けその位置の差を位置空間に順次投票して最も投票の集まる箇所を求め、同様に基準エッジ画像を回転させて順次投票を繰り返し、最も投票の集まった基準エッジ画像の角度と位置空間上の位置とを求めるようにすることで、検出ワークＷの形状のばらつきや複数の検出ワークＷがランダムに積み重ねられていても、このような状態に対して良好なロバスト性を確保して、より正確に検出ワークＷの位置と姿勢を求めることができる。
さらに、演算装置５０による演算量（計算量）を少なくすることができ、演算装置５０を構成するコンピュータにかかるコストを抑制しても検出対象物の位置及び姿勢の検出を良好に行なうことができる。
また、物体検出装置が対象物の位置と姿勢とを高速にかつ正確に求めることができるので、対象物であるワークＷを把持して搬送するロボットシステムに適用することで、ロボットシステムがワークＷの把持動作を行うに際して、作業効率の良いロボットシステム１とすることができる。
また、距離センサ８３により、ワークストッカ８４の上面の全領域についての距離データを検出して、得られた距離データに基づいてワークＷの位置及び姿勢を検出するので、ワークストッカ８４に収納されている複数のワークＷの中からロボットシステム１により把持するワークＷ一つ選択してワークＷの位置及び姿勢を検出して、ロボットアーム８１のエンドエフェクタ８２をワークＷに対して適切に接近させてワークＷを良好に把持することができる。
[第２実施形態]
次に本発明の第２実施形態について説明する。第２実施形態はワークＷの形状及び物体検出演算装置５０の手順のみが第１実施形態と異なっており、第１実施形態と同様に構成される箇所については説明を省略する。
図７に示すように、ステップＴ１００〜ステップＴ１０２までの手順によりワークＷの平面部１２ａ〜１２ｃを抽出する。なお、ステップＴ１００〜Ｔ１０２の手順については第１実施形態のステップＳ１００〜Ｓ１０２と同様である。
そして、ステップＴ１０３を実行してにより平面部１２ａ〜１２ｃから最も領域の大きい１２ｃを選択し、平面１３を算出する。平面１３の算出手順は第１実施形態のステップＳ１０３と同様である。
そして、図８に示すように、ステップＴ１０４では、距離データを平面１３に垂直に投影し、このときワークＷの各距離データから平面までの距離を求め、これを濃淡値に変換して距離画像を生成する。
図９（Ａ）は本実施形態にかかる距離画像を示す図である。ここでは、距離データの各ピクセルから平面１３までの距離に比例して距離画像上の濃淡値が決定される。
即ち、投影された投影平面部１４ａは平面部１２ａから平面１３までの距離が遠いため淡い色で表示され、投影平面部１４ｃは平面１３までの距離が近いため濃い色で表示されている。
また投影平面部１４ｂは平面１３に対して傾斜しており平面１３までの距離が変化するため、平面１３までの距離に応じて濃い色から淡い色に変化させて表示される。したがって、平面部１４ｂは平面部１４ａから平面部１４Ｃ側にかけて平面部１２ｂの傾斜に応じて徐々に濃い色から淡い色に変化することとなる。
ステップＴ１０５では、ステップＴ１０４で生成された距離画像における微小領域毎に局所的な仮想面を生成してその法線方向を算出し、平面部１３の法線方向となす角度を算出する。そして、この平面部１３の法線方向となす角度に比例させて濃淡値を決定し、法線角度画像を生成する。法線角度を簡単に求めるために距離画像をＸ方向、Ｙ方向に微分を施し、その変化の強度を求めることで法線角度の代用とすることも可能である。
図９（Ｂ）に法線角度画像の一例を示す。投影された平面部１４ａ及び１４ｃは平面１３と平行な面を形成しているため、距離画像は濃淡変化がなく平面１３の法線方向となす角度はほぼ０度（ｄｅｇｒｅｅ）となるため、濃い色で表示されている。投影された平面部１４ｂは平面１３に対して傾斜をもっているため、法線角度画像上では平面部１４ａ、１４ｃよりも傾斜に応じた淡い色で表示される。
ステップＴ１０６では作成された距離画像および法線角度画像がそれぞれ基準距離画像、基準法線角度画像としてデータベースに登録する。この際には平面１３の方程式対応付けてデータベースに記憶させておく。
次に、ワークストッカ８４内のワークＷを検出する手順を説明する。
距離センサ８３がワークストッカ８４内のワークＷに対して上述のステップＴ１０１からＴ１０３までの工程を実行して得た距離データから平面部分を抽出し、複数抽出された平面部分より任意の平面部分を選択して平面を算出する（距離データ取得ステップ、平面抽出ステップ、平面選択ステップ）。
平面部を選択する際、平面部の領域の大きいもの、或いは一番上（即ち、より距離センサ８３側）に存在するものを優先して選択するようにする。これにより、ワークＷ同士の重なりの少ないワークＷや上の方に積まれたワークＷ上の平面を選択することができる物体検出後ロボットなどで把持する場合に有効である。
ステップＴ１０４では算出された平面に対し、距離データを垂直に投影し距離画像を作成する。
ステップＴ１０５では算出された距離画像の各点に対し平面１３の法線方向とのなす角度をそれぞれ求め、これを濃淡値としてプロットした法線角度画像を生成する。
次に、ステップＴ１０７を実行する。ステップＴ１０７では予めデータベースに登録された基準距離画像と、検出対象（即ち、ワークストッカ８３内のワークＷ）に対して算出した距離画像とを比較して位置空間へ投票を行う。
次に、ステップＴ１０８を実行する。ステップＴ１０８では予めデータベースに登録された基準法線角度画像と、検出対象（即ち、ワークストッカ８３内のワークＷ）に対して算出した法線角度画像とを比較して位置空間へ投票を行う。
投票の手順について説明する。距離画像の任意の点Ｐ１（ｘ１、ｙ１）に対して、データベースに登録された基準距離画像のエッジ上の点Ｐ０（ｘ０、ｙ０）とを対応付ける。つまり、距離画像上の各ピクセルの濃淡値と法線角度画像上での濃淡値が各々近いものだけを対応させて、その位置の差を位置空間上（ｘ１−ｘ０、ｙ１−ｙ０）に投票する（プロットする）。この投票を実シーンの濃淡画像の各点に対して繰り返し行う。
ステップＴ１０９では、基準距離画像及び基準法線角度画像を所定の角度毎に回転させる。
そして、ステップＴ１０７、Ｔ１０８の処理をステップＴ１０９を実行する度に実行し、その都度別の位置空間へ投票を繰り返し行なわれる。
なお、ステップＴ１０９の処理は、基準距離画像及び基準法線角度画像の累積回転量が所定角度（ここでは、３６０度）に達するまで繰り返し実行される。
次に、ステップ１１０では、ステップＴ１０７〜Ｔ１０９において、投票の最も集中していた箇所を各基準画像の角度に対応した位置空間から探索し、その位置空間に対応する基準画像の角度と投票の集中していた位置とを求める。
既知の位置に置かれたワークＷの平面と実作業時でのワークＷの平面との各々の方程式がステップＴ１０３で求められていることから、実作業時のワークＷの平面部の姿勢を求めることができる。
また投票の最も集中していた角度と位置を求めることで、平面上でのワークＷの位置及び回転量が分かることから、ワークＷの三次元空間上での位置と姿勢を求める（ステップ１１０）。
以上、本発明の実施形態について説明したが、本発明は上述の実施形態に限定されず、本発明の主旨を逸脱しない範囲において適宜変形して適用可能である。
上述の第１実施形態及び第２実施形態においては、距離センサ８３によりワークストッカ８４の全領域においての距離データを検出していたが、ワークストッカ８４の上面のうちの一部分の部分領域を演算装置５０に予め設定しておき、ワークストッカ８４の部分領域のみ距離データを検出して得られた距離データに基づいて、対象物の位置及び姿勢を検出するように構成してもよい。
このように構成することにより、ワークストッカ８４の全領域についての距離データを用いる場合よりも計算量を低減させることができる。
【図１】
2009028489.tif 000006
【図２】
2009028489.tif 000007
【図３】
2009028489.tif 000008
【図４】
2009028489.tif 000009
【図５】
2009028489.tif 000010
【図６】
2009028489.tif 000011
【図７】
2009028489.tif 000012
【図８】
2009028489.tif 000013
【図９】
2009028489.tif 000014
【国際調査報告】
2009028489.tif 000015
2009028489.tif 000016
2009028489.tif 000017
2009028489.tif 000018