3 次元テンプレートを用いた把持姿勢推定に関する研究
指導教授：藤吉 弘亘

TR18017 真野 航輔
1. はじめにバラ積みされた物体を高速かつ高精度に把持姿勢推定を行う Fast Graspability Evaluation(FGE)[1] が提案されている．FGE は，深度センサから取得した深度マップと２次元のハンドテンプレートを畳み込むこことで，深度マップにおける把持位置を推定する．このとき，深度マップに対する把持位置の探索は深度センサの直行平面に対してのみ行う．そのため，ハンドのアプローチ姿勢はカメラの姿勢に依存するため，把持対象に対して適切な把持アプローチ姿勢を求めることができない．そこで，本研究では，FGEを 3 次元に拡張することで，物体に適切な把持位置とアプローチ姿勢を推定する．また，3 次元拡張により膨大となるハンドテンプレートを特異値分解することで計算量を削減する．

する問題がある．そこで，衝突，接触ボクセルテンプレー
トを特異値分解する．分解することで，少数の固有値テンプレートの組み合わせとなり，畳み込み後の行列を近似することで処理時間の増加を抑制できる．
衝突，接触ボクセルテンプレートを特異値分解するために，ボクセルテンプレート群を 2 次元の行列 Mc , Mt で表現する．行列 Mc ∈ RE×F , Mt ∈ RE×F の各列ベクトルはベ(d,θ(α,β,δ) )(d,θ(α,β,δ) )クトル化したテンプレート mc∈ RE , mt∈
ER で構成される．図 2 に示すように，E はハンドテンプ
レートのボクセル数 (= 27000 px)，F はロボットハンドの状態数 (= 216 パラメータ) である．行列 Mc , Mt に対して特異値分解を適用すると，3 つの行列 U, S, V T に分解できる．

2. 従来手法

VTd = 6, θ(x,y,z) = {0, 0, 0}

d = 7, θ(x,y,z) = {0, 15, 90}

unu1 u 2EMSnFGE の概要を図 1 に示す．FGE はロボットのハンドモ
デルからハンドの衝突領域と接触領域をテンプレートとして作成する．ハンドの衝突，接触テンプレートをそれぞれ把持対象となる物体の接触領域とハンドが干渉する物体の衝突領域に畳み込み交点を計算することでハンドが干渉しない領域を求める．ハンドが干渉しない領域にガウシアンフィルタを適応し，値が最大となる位置を把持位置を推定する．

U≈=ハンドの衝突領域

物体の衝突領域

衝突
ボクセルテンプレート=
ハンドの接触領域

物体の接触領域

図 2 : ボクセルテンプレートの特異値分解(x,y,z)=ハンドモデルハンドパラメータ
開き幅 d: 30 mm
回転角度θ：45°=ガウシアン
フィルタ

特異値

F

∩

非衝突領域

graspability

図 1 : Fast Graspability Evaluation の概要

3. 提案手法
提案手法では対象物体に対して適切なアプローチ角度を推定するために，FGE の 3 次元拡張を提案する．FGE で用いる 2 次元のハンドテンプレートを 3 次元に拡張することで，3 次元空間における把持位置と姿勢を同時に推定する．

3.1 FGE の 3 次元拡張
FGE では対象物の 2 値画像とテンプレートを畳み込むことで把持姿勢をする．そのため，FGE では x, y, z, d, θの 5 パラメータの推定となる．5 パラメータではカメラ軸上からアプロ―チする場合のみの推定となるため物体に対して適切なアプローチとは限らない．そこで FGE を 3 次元拡張することで，x, y, z, d, θ(α,β,δ) の 7 パラメータで推定が可能となり，物体に対して適切な把持姿勢が推定できると考えられる．まず，ロボットのエンドエフェクタから衝突ボクセルテンプレート Hc と接触ボクセルテンプレー
ト Ht を作成する．そして，テンプレートは開き幅 d と回転 θ(α,β,δ) のパラメータを持つ，図 3 のステップ 1 に示すように，2 値のボクセルで表現し，パラメータを変化させた時のボクセルテンプレートを作成する．このとき，テンプレートは θα ，θβ ，θδ の順番で回転を行う．次に，対象物体を多視点から撮影した点群から対象物体のボクセルを作成する．

3.2 特異値分解による固有値テンプレートの算出
衝突ボクセルテンプレートと接触ボクセルテンプレート
は , d, θ(α,β,δ) の 4 パラメータを変化させるため膨大な組合せとなる．また，従来手法である FGE はパラメータ毎に畳み込みを行うためパラメータ数により処理時間が増加

図 2 に示すように，行列 Sc は対角成分に特異値 sci を持つ対角行列であり，特異値は上位の要素のみ大きな値を持ち，下位の要素では 0 に近い値となる．ここで，行列 Scの特異値から求めた累積寄与率を用いて，次元削減を行う．
この時の行列を Sc′ とする．uci ∈ RE を行列 Uc の i 番目の列ベクトル，γci ∈ RF を行列 S ′ VcT の i 番目の行ベクトルとすると，パラメータ (d, θ(α,β,δ) ) の衝突ボクセルテ(d,θ(α,β,δ) )ンプレート mc

は式 (1) のように定義できる．同(d,θ(α,β,δ) )様に mtも定義できる．
(d,θ(α,β,δ) )mc=N∑

γci (d, θ(α,β,δ) )uci

(1)

i=1

uci , uti を分解前の形状に戻すことで 3 次元のテンプレート行列とみなすことができる．ここでは “固有値テンプレート”と呼び，重み係数となる γci , γti を “固有関数”と呼ぶ．

3.3 Graspability の効率的な計算
図 3 に提案手法の概要を示す．ステップ 1 では，衝突ボクセルテンプレートを予め特異値分解を行い，固有値テンプレートと固有関数を計算する．ステップ 2 では，対象物のボクセル Ot , Oc と固有値テンプレート uci , uti を，
qci = Oc ⊗ uci , qti = Ot ⊗ uci のようにあらかじめ式 (2)により畳み込める．ステップ 3 では，衝突領域 C ′(d,θ(α,β,δ) )を式 (2) の固有関数値のみ変化させることで任意の開き幅d と回転角度 θ(α,β,δ) における出力を効率的に計算できる．
同様に T ′(d,θ(α,β,δ) ) も計算できる．

C ′(d,θ(α,β,δ) ) =N∑

γci (d, θ(α,β,δ) )qci

(2)

i=1

ステップ 4 では，graspability マップ G(d,θ(α,β,δ) ) を，次式のようにガウシアンフィルタ g を畳み込んで求める．

G(d,θ(α,β,δ) ) = (T ′(d,θ(α,β,δ) ) ∩ C ′(d,θ(α,β,δ) ) ) ⊗ g(3)ステップ 2ステップ 1ステップ 3ステップ 4××

SVD
対象物体ボクセル

衝突
テンプレート行列

衝突
固有値テンプレート衝突
ボクセルテンプレート

衝突領域×
衝突固有関数∩＝ガウシアン
フィルタ＝graspability

非衝突領域××
SVD対象物体ボクセル
接触
テンプレート行列

接触領域

×
接触
固有値テンプレート

接触固有関数接触
ボクセルテンプレート

図 3 : 提案手法の概要
この時，graspability マップの最大値を把持姿勢として推定する．しかし，経路生成や衝突判定で最大値の把持姿勢が使用できない場合が考えられる．そこで，graspabilityマップの値から優先順位をつける．graspability マップは最大値の周囲に高い値を持つ．そのため，graspability マッ
プの x,y 平面に対してグリッド分割を行うことで推定領域における優先順位をつけることができる．

4. 評価実験
本実験では，FGE の 3 次元拡張に対して特異値分解を適応した場合における，累積寄与率を変化させた際の畳み込み時間とテンプレートの誤差を評価する．また，把持姿勢推定にかかる処理時間を評価する．テンプレートのパラメー
タを d ∈ {60, 70}，θα ∈ {0, 30,・・・150}，θβ ∈ {0, 15, 30}，θδ ∈ {0, 30,・・・150} の 216 パラメータとする．テンプレー
トの復元誤差は Mean Absolute Error(MAE) を用いて評価を行う．

4.1 テンプレートにおける最適な累積寄与率の評価
図 4 における衝突ボクセルテンプレートの MAE の結果を示す．図 4 より，累積寄与率が小さくなるにつれ固有値は非線形的に下がることが確認できる．また，累積寄与率50%までほぼ線形的に上昇している．図 5 に衝突ボクセル
テンプレートにおける畳み込み時間を示す．図 5 より，削減率が累積寄与率 60%から変化が微小になるため，累積寄与率は 60%を用いる．

4.2 把持姿勢推定時間の評価
衝突ボクセルテンプレートと接触ボクセルテンプレー
トの累積寄与率を 60%とした際の把持姿勢推定時間を表 1に示す．ここで，FGE を 3 次元拡張した結果を FGE 3Dとし，FGE の 3 次元拡張に特異値分解を導入した手法をFGE 3D SVD とする．FGE 3D に比べ特異値分解を導入することにより，把持姿勢推定結果にかかる処理時間を約66.6%削減した．これにより，特異値分解の導入は処理時間の削減に有効であることを確認した．
表 1 : 把持姿勢推定時間と削減率

FGE 3D推定時間 [sec]

207.4
4.3 把持姿勢推定結果の評価

FGE 3D SVD
69.2

図 6 に把持姿勢推定結果を示す．また，着色した直方体が把持姿勢を示している．図 6(a) 及び (b) より，直方体は把持対象に対して干渉が発生しない領域に対して把持姿勢推定できていることが確認できる．FGE 3D SVD において，FGE 3D より把持姿勢推定結果が多く検出されることが確認できる．これは，特異値分解による復元ノイズが影響していると考えられる．

0.007

250
ᅛ᭷್

ㄗᕪ0.006

2000.004

1000.003

᚟ඖㄗᕪ⣼✚ᅛ᭷್0.005
15090807060504030

5. おわりに本研究では，物体に対して適切なアプローチ角度を高精度に推定する手法を提案した．評価実験より，3 次元拡張により増加する畳み込み回数は特異値分解により大幅に削減することが可能であることが確認できた．今後は，実機による FGE と提案手法の把持成功率の評価及び，把持姿勢推定における探索方法の改善を行う．

20⣼✚ᐤ୚⋡図 4 : 衝突ボクセルテンプレートにおける復元誤差
12014๐ῶ⋡ฎ⌮᫬㛫๐ῶ⋡[%]1080860
640420ฎ⌮᫬㛫[sec]12100

20010090807060

504030(b)FGE 3D0.0010100

図 6 : 把持姿勢推定結果

0.002
500(a)FGE 3D SVD20⣼✚ᐤ୚⋡

図 5 : 衝突ボクセルテンプレートの畳み込み時間と削減率

参考文献[1] Y. Domae, et al., “ Fast graspability evaluation on single
depth maps for bin picking with general grippers”, ICRA,
pp. 1997–2004, 2014.

研究業績[1] K. Mano, et al., “Fast and Precise Detection of Object
Grasping Positions with Eigenvalue Templates”, ICRA,
2019.
(他 学会発表 3 件)