JP5046133B2 - ３次元曲面認識方法及びマニピュレーションシステム - Google Patents

Description

本発明は、３次元曲面を持つ物体認識方法およびその情報を用いて対象物を拾い上げるマニピュレーションシステムに関するものである。

モデルベースの３次元物体の認識では、多くの場合、シーンを観測して得られた３次元データに対してエッジや法線などの特徴量を計算し、モデルの特徴量との照合を行うことで３次元空間内のモデルの位置と姿勢を特定する。

対象物体に際立った特徴（エッジなど）が存在する場合には照合は速やかに行うことが可能であるが、特徴点が乏しい場合、あるいは特徴点が姿勢や他の物体による遮蔽によって観測できなくなるような場合には照合は困難となる。

曲面のみが観測される状態では、背景との境界線が唯一の特徴線となる。しかし、輪郭線は一般的に照明によるシェイディングが発生し、輝度が緩やかに暗くなる現象があり、画像から正確な輪郭線を抽出する処理にも困難が伴う。さらに、特に部品が山積み状態の場合、輪郭線はしばしば隣接する別部品によって遮蔽されるため正確な特徴にはならない。また、輪郭線が正確に抽出できた場合においても、複数の視点から観測される輪郭線は異なる位置の輪郭線である（自己遮蔽）ため、三角測量法によって三次元を復元しようとすると正しく曲面上の曲線にはならないという問題もある。

このような特徴点が少ない物体は工業部品にも多くあり、エルボー（配管を９０度等に曲げるための配管継ぎ手）もその一つである、偶然に開口部が観測されていれば明らかな特徴となり姿勢の特定には有利であるが、開口部が観測されない姿勢も存在し、通常の方法では認識が困難である。

特許文献１、特許文献２などの輪郭線を利用した曲面物体の認識手法も存在するが、背景との切り出しが簡単な場合に限られるため山積みの対象物からのロボットによるマニピュレーションなどの目的には利用できない。

画像中の輪郭線を特徴線として用いる以外に、レンジファインダやステレオ相関法などによってシーンの３次元点群データを取得し、一方モデルとして３次元点群データを用意しておき、点群データ同士を直接もっとも近い位置姿勢に近づけるICPという手法もある（非特許文献３参照）。しかし、この方法を採用する際には初期状態をある程度真値に近い状態で始める必要があり、姿勢がまったく未知の状態からは困難であり、局所的な誤った解になる可能性もある。多数の初期状態から開始すれば正しい解がもとまる可能性は高くなるが、大変に時間がかかる。

生産現場において自動化が難しい作業には上記の理由で個々の部品の位置姿勢を自動的に認識する方法がないか困難な場合が多くある。位置姿勢さえ正確に認識できればこのような作業が自動化されうる。現在人手で作業している部品の中には、重量の大きな部品もあり、扱いに注意が必要であったり事故によって作業員が負傷する危険性が高かったりする場合もある。そのため、生産性向上や作業員の安全性向上のためには自動認識及びマニピュレーションシステムが求められている。

特許2652045号公報 特許2920513号公報

A グレイ「Mathematica 曲線と曲面の微分幾何」、トッパン、1996 吉見、富田「最大最小曲率線ネットによる曲面の表現」,電子情報通信学会論文誌. D-II, 情報・システム, II-情報処理 J77-D-2(1) pp.61-68 1994 Chen, Y. Medioni, G., "Object modeling by registration of multiple range images", Proc. 1991 IEEE Intl. Conf. on Robotics and Automation, pp. 2724-2729, 1991

本発明は、係る問題点を解決して、特徴の少ない曲面物体の３次元点データから位置姿勢を特定することと、その位置姿勢情報をもとに部品を正確に把持するマニピュレーションシステムを構成することを目的としている。

本発明の観測されたシーンにおける３次元曲面の３次元点群データからモデルの位置姿勢を同定する認識方法は、それぞれの３次元点に対して計算された主ベクトル群の単位球上の空間的な分布と、モデルから計算された主ベクトル群の単位球上の空間的な分布とを照合してデータ中のモデルの姿勢を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することでモデルの位置を特定する。

また、本発明の不特定の位置姿勢に置かれた対象物を拾い上げるマニピュレーションシステムは、観測されたシーンにおける３次元曲面の３次元点群データから計算された主ベクトル群の単位球上の空間的な分布と、モデルから計算された主ベクトル群の単位球上の空間的な分布とを照合してデータ中のモデルの姿勢を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することでモデルの位置を特定し、その特定された位置姿勢情報を用いて対象物を拾い上げる。

本発明の認識方法およびマニピュレーションシステムによって、曲面のみが観測されるような特定の対象物体の位置姿勢を認識し、それをマニピュレータによって作業することが可能になる。

主ベクトルの説明図である。 曲面の特徴が単位球上の主ベクトル分布に影響することを示す図である。 軸が平面上に乗る半径一定の管状曲面が分解可能であることを示す図である。 エルボーの図である。 本発明の実施方法を示した説明図である。（実施例１） 本発明の実施方法を示した説明図である。（実施例２） 単位球上の主ベクトル分布から平面A_Dを求める図である。 曲面から断面の円の中心の軌跡S_Dを求める図である。 本発明の実施方法を示した説明図である。（実施例３） エルボーの開口部の位置と方向を示す図である。 ３次元データから計算される平面A_D上の主ベクトル分布とその分布領域を示す図である。 平面A_D上の主ベクトル分布領域とエルボー姿勢の関係を示す図である。

以下、例示に基づき、本発明を説明する。本発明は、主ベクトルを利用してデータ中のモデルの姿勢を特定し、特定された姿勢をもとに位置を特定する。図１は、主ベクトルの説明図である。曲面上の点における主ベクトル p_max およびp_minは、その点における法線 n を含む断面における曲率において、断面を回転させたときに曲率が最大および最小となる方向（主曲率方向）に断面に含まれる接ベクトルを示す（非特許文献１）。数学的に、曲率が最大および最小になる方向は互いに直交することが知られており、したがって、この二つの接ベクトルと法線は局所的な直交系をなし、その向きは曲面の位置姿勢にのみ依存し、観測方向に依存しない。したがって、点の同定がされればこの直交系を計算することで物体の姿勢は特定できる。例外を除きほとんどの曲面上の点には主ベクトルが存在する。

通常は姿勢の同定以前に点の同定はされていない。したがって、個々の点の主ベクトル直交系によって物体全体の姿勢の同定はできないが、曲面全体の単位球上の主ベクトル分布の特徴が存在すればそれを利用して観測されたデータから計算された単位球上の主ベクトルの分布とモデルから計算された単位球上の主ベクトルの分布を照合することで姿勢の特定が可能になる。図２は、曲面の特徴が単位球上の主ベクトル分布に影響することを示す図である。たとえば、図２（Ａ）に示す円柱のような簡単な図形の例では、円柱の最大曲率方向の主ベクトルは単位球の極方向となり、最小曲率方向の主ベクトルは赤道にある。また、図２（Ｂ）に示す円錐であれば、最大曲率方向の主ベクトルは極付近の二つの円弧であり、最小曲率方向の主ベクトルは赤道となる。

ただし、従来は計測された三次元データから主ベクトルを計算することは困難であった。それは微分を用いて定義されていた従来の計算方法が誤差を含んだデータに対して弱かったためである。本発明者らはISL法によってその問題を克服したため、主ベクトルを認識に利用することが可能となった（非特許文献２）。なお、ここで使っている曲率の値は、符号を伴っており、凸の形状の場合にはマイナス、平坦ならばゼロ、凹の形状であればプラスとなる。

図３は、中心軸が平面上に乗る半径一定の管状曲面が分解可能であることを示す図である。単位球上の主ベクトル分布が特徴的な実例として、中心軸が平面上にある外径が一定の管状の曲面がある。図３に示す中心軸は、表示を明確にするために、管状曲面とは分離して表示している。この曲面は中心軸を円弧と直線に分割することで、円柱とトーラスの分割に置き換えられる。円柱もトーラスも曲率が最大となる方向の主ベクトルは常に軸の存在する平面内にあるため、これを接続したもとの曲面全体の主ベクトルもその平面にある。これを利用すると、観測されたデータの単位球上の主ベクトル分布から中心軸の存在する平面の法線を特定することが可能となる。具体的には、曲面上の３次元点群データのすべての点に対して主ベクトルを計算し、単位球上にある主ベクトル分布から平面を計算する。このことにより、３自由度ある３次元空間中の姿勢パラメータのうち２つが特定され、残りは二次元内の回転の１自由度のみとなる。この場合、残りの一自由度の回転パラメータは二次元の平面内の回転であるので、主ベクトルの角度範囲、法線分布その他の角度を決定できる特徴量を利用して特定できる。一自由度の回転パラメータ決定は、３自由度の場合に比べて簡単に計算でき、また安定している。

図４は、エルボーの図である。上記の管状物体の代表例として、円柱が一度だけ屈曲している管状の形状である配管継ぎ手のエルボーが挙げられる。その場合にも同様の処理によって姿勢が決定できる。エルボーはトーラスと二本の円柱が組み合わさった形であるので、中心軸が存在する平面内に最大曲率に対応する主ベクトルが存在する。また、このような物体の場合、管の中心軸がある平面に二次元の座標軸 U,V をとると、主ベクトルは単位円の９０度の扇型の範囲に分布する。扇型の両端はそれぞれの端部の方向を示しているので、この分布範囲からUV平面内の回転軸が決定できる。

以上の方法によって姿勢が特定されたあとで位置を特定する。姿勢に関する回転のパラメータがすでに決定しているので、モデルとデータを同じ姿勢で比較することができ、モデルの形状やデータの観測方向によって信頼性の高い部分の点の３次元座標値が最も一致する移動量を計算する。たとえばエルボーの場合、端面の位置を使うこともできるし、観測光学系に近い部分同士を合致させることもできる。

姿勢および位置を決定したあとで、それらの情報を用いてマニピュレーションを行うことで、さまざまな姿勢に置かれた対象物を自由に拾い上げるマニピュレーションシステムを構成できる。本発明では、対象となる曲面の形状の３次元点群データを用いて対象物の位置姿勢を特定する。

図５に基本的な処理の流れを示す。まず、対象物の三次元モデルから単位球上の主ベクトルの空間的な分布データを作成し、モデルの主ベクトル分布としてモデルの座標系において記録する。これは計測処理に先立ってオフラインで行う。また、法線や曲率の分布も必要であれば計算し、記録する。また、マニピュレータによる把持位置をモデルの座標系で登録する。モデルが複数存在する場合には、それぞれのモデルごとに行う。以上の処理はあらかじめオフラインで準備する。

次に、対象シーンの距離画像を計測する。対象シーンとは、不特定の位置姿勢に置かれた対象物を対象物以外の背景も含めて観測したものである。計測は計測空間内の３次元点群データを高密度高精度に計測する。計測の手段としてはレーザーレンジファインダ、複数のカメラの二次元画像からのステレオ相関法、空間コード化法によるレンジファインダ、接触式の３次元計測装置などが使用可能で、これは対象物の属性や計測環境や計測時間などの制約によって任意のものを選択することができる。

次に、計測された距離画像の各点に対してデータ座標系における法線や主ベクトルや曲率を計算する。法線の計算方法には接平面を計算する方法、距離画像を微分する方法、二次曲面などをあてはめる方法などがある。また、主ベクトルを計算する方法にも微分で計算する方法、二次曲面をあてはめる方法、ISL法で計算する方法がある。

次に、法線、主ベクトル、曲率がそれぞれの領域内で連続になるように距離画像を領域分割(セグメンテーション)する。この処理は、一般的に対象とする曲面以外の物体（複数の対象物や背景など）が存在する場合に必要であるが、あらかじめ認識対象の曲面のみが計測されている場合には必要ない。
次に、領域の集合から一部の認識対象となる領域を抽出する前処理を行う。明らかに対象物体と異なる領域（背景など）や十分な大きさをもたない雑音と思われる領域を排除する。

次に、残った各領域に対してデータ座標系における単位球上の主ベクトル分布を作成し、あらかじめ作成してあったモデル座標系におけるモデルの単位球上の主ベクトル分布と最も一致する姿勢を特定し回転行列を計算する。主ベクトルには符号の不定性があるため、単位球上の主ベクトル分布の評価の際には不定性を考慮した姿勢の評価を行う。モデルが対称図形の場合には複数の姿勢が存在するが、複数の姿勢を特定する必要がなければ一つの回転行列を出力する。観測されるデータの不完全性によって姿勢に曖昧性が存在する場合や観測されない内部の形状によって非対称性がある場合には複数の姿勢候補に対して回転行列を出力する。

次に、得られた回転行列を用いてモデルを回転してデータ座標系に変換するか、あるいはデータを回転してモデル座標系に変換する。
次に、姿勢を合わせたモデルとデータが最も近くなるような移動ベクトルを計算する。
最後に、計算された位置と姿勢に基づき、モデルに登録してある把持点をロボット座標に変換し、マニピュレータにより把持する。

図６に基本的な処理の流れを示す。対象物が、軸が一平面上の曲線と直線であらわされるような半径を同一とする管状物体である場合、まず対象物の３次元モデルから軸の存在する平面A_Mをモデル座標系内において導出する。また、その平面内で軸位置を軌跡S_Mとして記録する。軸位置は複数の線分あるいは曲線が接続した形で記録する。また、マニピュレータによる把持位置をモデルの座標系で登録する。モデルが複数存在する場合には、それぞれのモデルごとに行う。以上の処理はあらかじめオフラインで準備する。

次に、実施例１と同様の手法により計測された距離画像の各点に対して実施例１と同様の手法により法線、主ベクトル、曲率を計算する。
次に、実施例１と同様の手法により距離画像を領域分割する。
次に、実施例１と同様の手法により認識対象領域を抽出する。
次に単位球上の曲率が最大となる主ベクトルの分布を計算する。図７は、単位球上の主ベクトル分布から平面A_Dを求める図である。この主ベクトルは平面上に分布するので、平面A_Dをあてはめる。

次に、平面A_Dを平面A_Mに一致させる回転および平行移動の座標変換行列Tを計算する。この段階では、二つの平面の表裏の関係は決定できないので、それぞれの法線が一致するときの行列T₀と法線が逆向きになるT₁を計算する。また、平面内の回転、移動成分は不定であるので仮のパラメータをあてはめる。

次に、計測された各点に対して最大曲率方向を法線とする平面Bを定義する。図８は、曲面から断面の円の中心の軌跡S_Dを求める図である。平面Bは軸と直交する断面であり、円となるので、断面に対して円弧をあてはめてその中心Cをもとめ、平面A_D上に投影する。理想的にはCはA_D上に存在するが、計測誤差や主ベクトル計算時の誤差などで起きるずれを統計的に平均化する。この結果、すべての計測点に対して計算された断面の中心を平面A_Dに投影した軌跡S_Dが得られる。

次に、平面A_D上に作成した軌跡S_Dを行列T₀で変換し、S’_D0とする。S’_D0は平面A_M上に存在するので、平面A_M上で軌跡S’_D0と軌跡S_Mを最もよく一致させる平面内の回転、平行移動行列T’₀を計算する。
同様に、行列T₁を用いて軌跡S_Dを変換したものをS’_D1とし、平面A_M上で軌跡S’_D1と軌跡S_Mを最もよく一致させる平面内の座標変換行列T’₁を計算する。

次に、軌跡S’_D0をT’₀により変換したS”_D0と軌跡S’_D1をT’₁により変換した軌跡SD1’’のうち、軌跡S_Mにより近い結果をもたらすものを選ぶ。
最後に選択された変換行列により、モデルに登録してある把持点をロボット座標に変換し、マニピュレータにより把持する。

図９に基本的な処理の流れを示す。図１０は、エルボーの開口部の位置と方向を示す図である。対象物が、二つの半径の等しい円筒が屈曲部分で接続した形状のエルボーである場合、まず対象物のモデルから軸の存在する平面A_Mを記述する。また、二つの開口部の方向と位置を記述する。

次に、実施例１と同様の手法により計測された距離画像の各点に対して実施例１と同様の手法により法線、主ベクトル、曲率を計算する。
次に、実施例１と同様の手法により距離画像を領域分割する。
次に、領域分割された距離画像中から平面領域を除き、十分に大きな面積を持つ認識対象領域を抽出する。

次に、実施例２と同様に単位球上の曲率が最大となる主ベクトルの分布を計算し、平面A_Dを求め、さらに、その中の角度分布を得る。図１１は、３次元データから計算される平面A_D上の主ベクトル分布とその分布領域を示す図である。エルボーにおいては、単位球上の曲面上の主ベクトル分布は平面内でありかつ扇型の分布を持つため、平面A_D上の主ベクトル分布から扇型分布の両端の方向を表すベクトルを抽出する。主ベクトルには符号についての不定性があるため、それを考慮して一つの主ベクトルpについて、その逆方向の-pも同時に分布に加えることで、図１１のように対称な扇型分布が二つできる。それぞれの扇型をa,bとおき、それぞれの両端のベクトルを{v_a0,v_a1},{v_b0,v_b1}とする。

次に、実施例２と同様に座標変換行列T₀を計算する。エルボーは平面Aに対して対称なので行列は一つでよい。主ベクトルデータを平面A_M上に投影する。扇型分布の両端を表すベクトルも変換する。
次に、扇型の両端ベクトルの角度を調べ、それがエルボーの屈曲角度と等しくなるかどうかを調べる。この角度が等しければ、観測された画像にエルボーの胴体部分全体が写っているとみなし、{v_a0,v_a1}方向がエルボーの両端方向である場合の回転行列T’を計算する。この結果、最大曲率方向の主ベクトル p_max は、断面の方向と垂直であるので、図１１のようにエルボーの二つの可能性な姿勢が特定される。{v_b0,v_b1}方向がエルボーの両端となる回転行列を計算しても、二つの可能な姿勢の位置は変わらない。

次に、二つの姿勢から一つの姿勢を特定する。図１２は、平面A_D上の主ベクトル分布領域とエルボー姿勢の関係を示す図である。二つの姿勢は最大曲率が正（凹）の方向と負（凸）の方向が逆であるので、あらかじめ各点について計算してある最大曲率値と法線方向から、姿勢を特定する。もしv_a0,v_a1のなす角度がエルボーの屈曲角度より狭い場合には、遮蔽などの影響でデータが欠落していると考えられるので、角度範囲aの両側をそれぞれ端部とする二つのT’を作成し、それぞれについて可能な二つの姿勢から一つの姿勢を特定し、回転行列の候補を二つ計算する。

次に、端部付近のデータ点に対して円柱をあてはめ、平行移動成分を計算する。
最後に選択された変換行列により、モデルに登録してある把持点をロボット座標に変換し、マニピュレータにより把持する。

Claims (12)

1. 不特定の位置姿勢に置かれた対象物の観測されたシーンにおける３次元曲面の３次元点群データから該対象物の位置姿勢を同定する認識方法において、
   それぞれの３次元点に対して計算された単位球上の主ベクトル群の空間的な分布と、前記対象物のモデルから計算された単位球上の主ベクトル群の空間的な分布とを照合してデータ中の前記対象物の姿勢を特定し、
   さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定する認識方法。
2. 請求項１に記載の認識方法において、単位球上の主ベクトル群の空間的な分布からデータ中の前記対象物の姿勢を特定する処理において姿勢を特定する３自由度のパラメータをすべて特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定する認識方法。
3. 請求項１に記載の認識方法において、単位球上の主ベクトル群の空間的な分布からデータ中の前記対象物の姿勢を特定する処理において姿勢を特定する３自由度のパラメータのうち２自由度を特定し、残る１自由度を後から特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定する認識方法。
4. 請求項１に記載の認識方法において、単位球上の主ベクトル群の空間的な分布からデータ中の前記対象物の姿勢を特定する処理において姿勢を特定する３自由度のパラメータのうち１自由度を特定し、残る２自由度を後から特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定する認識方法。
5. 請求項３に記載の認識方法において、単位球上の曲面上の主ベクトルが平面上に分布することを利用して該平面の法線を特定し、その後該法線を軸とする角度を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定する認識方法。
6. 請求項５に記載の認識方法において、対象物体が配管継ぎ手であるエルボーである場合において、主ベクトルが管の中心軸がある平面と平行な平面内に存在し一定角度範囲の分布範囲を持つことを利用してデータ中の前記対象物の姿勢を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定する認識方法。
7. 不特定の位置姿勢に置かれた対象物を拾い上げるマニピュレーションシステムにおいて、
   不特定の位置姿勢に置かれた対象物の観測されたシーンにおける３次元曲面の３次元点群データから計算された単位球上の主ベクトル群の空間的な分布と、モデルから計算された単位球上の主ベクトル群の空間的な分布とを照合してデータ中の前記対象物の姿勢を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定し、
   その特定された位置姿勢情報を用いて対象物を拾い上げるマニピュレーションシステム。
8. 請求項７に記載のマニピュレーションシステムにおいて、単位球上の主ベクトル群の空間的な分布からデータ中の前記対象物の姿勢を特定する処理において姿勢を特定する３自由度のパラメータをすべて特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定するマニピュレーションシステム。
9. 請求項７に記載のマニピュレーションシステムにおいて、単位球上の主ベクトル群の空間的な分布からデータ中の前記対象物の姿勢を特定する処理において姿勢を特定する３自由度のパラメータのうち２自由度を特定し、残る１自由度を後から特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定するマニピュレーションシステム。
10. 請求項７に記載のマニピュレーションシステムにおいて、単位球上の主ベクトル群の空間的な分布からデータ中のモデルの姿勢を特定する処理において姿勢を特定する３自由度のパラメータのうち１自由度を特定し、残る２自由度を後から特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定するマニピュレーションシステム。
11. 請求項９に記載のマニピュレーションシステムにおいて、単位球上の曲面上の主ベクトルが平面上に分布することを利用して該平面の法線を特定し、その後該法線を軸とする角度を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定するマニピュレーションシステム。
12. 請求項１１に記載のマニピュレーションシステムにおいて、対象物体が配管継ぎ手であるエルボーである場合において、主ベクトルが管の中心軸がある平面と平行な平面内に存在し一定角度範囲の分布範囲を持つことを利用してデータ中のモデルの姿勢を特定し、さらに特定された姿勢においてデータの３次元座標値とモデルの３次元座標値を照合することで前記対象物の位置を特定するマニピュレーションシステム。