物体操作タスクにおける HMM を用いた障害物回避動作の生成○山内祐輝 (奈良先端科学技術大学院大学), 杉浦孔明, 岩橋直人 (情報通信研究機構),
Sakti Sakriani, 戸田智基, 中村哲 (奈良先端科学技術大学院大学)

Motion Generation and Obstacle Avoidance Using HMMs in Object Manipulation Tasks○ Yuki Yamauchi (NAIST), Komei Sugiura, Naoto Iwahashi (NICT), Sakriani Sakti,
Tomoki Toda, and Satoshi Nakamura (NAIST)
Abstract: It is desirable for a domestic service robot to generate natural trajectories when it is commanded to manipulate
objects such as ”Put the box here.” Although natural trajectories can be generated by imitation learning methods, however
most of them do not consider object collision. In this paper, we present a motion generation method based on hidden
Markov model (HMM) and Rapidly-exploring Random Trees (RRT). Experimental results showed that our method generated natural, collision-free trajectories.

1. はじめに
生活支援ロボットにとって，日用品等のオブジェクトを操作する機能は必要不可欠であるが，各種の日用品や棚に対応する動作を事前にプログラムするコストは非常に大きい．加えて，事前にプログラムされた動作がユーザにとってイメージしにくいものであった場合，安心して動作指示できないという問題もある．本研究Fig.1 左：実験環境，右：カメラ画像の例と動画像から抽出された動作の軌道は，これらの問題を解決するため，模倣学習の枠組みにより動作を学習する手法の構築を目的とする．この2. タスクような学習手法を構築することで，プログラミングス

2·1 参照点に依存した動作キルが必要とされないユーザフレンドリな動作教示方法を実現できる．

本研究では，参照点に依存する動作の概念を，模倣学習の枠組みによりロボットに学習させる問題につい物体操作に模倣学習を適用する手法として，先行研て考える．参照点に依存する動作とは，「A を B に載せ究では，隠れマルコフモデル (HMM) を利用して連続的る」動作や「A を回す」動作などがある．このときのな軌道を生成する手法がある [1]．この手法では，物体動かされるオブジェクトをトラジェクタ，トラジェクタ操作において自然な軌道の生成は可能であるが，障害の基準となるオブジェクトをランドマークと呼ぶこと物を回避できないという問題がある．一方，経路計画法にする [2]．動作には上記の 2 種類のようなランドマーの分野では，Rapidly-exploring Random Trees(RRT) にクを必要とする動作と必要としない動作がある．このより，障害物回避軌道を生成する研究が広く行われてような動作の情報には，通常訓練データ中に明示されいる（例えば，[3]）．ていないため，同じ枠組みで学習させることは簡単ではない．

本論文では，HMM を導入した RRT を用いて軌道を生成する手法について提案する．これにより，障害物を2·2 ハードウェア回避しつつ自然な軌道を生成することができる．本論

実験環境を Fig. 1 の左図に示す．ロボットシステム

文では，人間が実行した軌道に近い軌道を「自然な」軌

は，７自由度のロボットアーム（三菱重工製 PA-10），道と定義する．提案手法の独自性は以下の 2 点である．

４自由度のロボットハンド（Barrett Technology 製 Bar-rettHand），カメラ（Microsoft 製 Kinect），視線表出ユ• RRT のノード生成に対する HMM の位置パラメー

ニット（Directed Perception 製 PTU-46-70 にロボットタの利用• RRT のノードの連結基準に対する HMM の速度パラメータの利用

ヘッドを取付）からなる．
オブジェクトの画像特徴量および 2 次元座標（カメラ座標系）は，固定したカメラを用いて得る．オブジェ

第12回システムインテグレーション部門講演会(SI2011)（2011年12月23日～25日・京都）- 1614 -SY0014/11/0000 - 1614 © 2011 SICE

クトの抽出およびトラッキングは，色およびカメラか成することである．具体的には，まず，既存手法によりら得られる距離に基づくヒューリスティックな手法に物体操作の軌道を生成する．その生成された軌道の障より行う．カメラのフレームレートは 30[frame/sec] で，害物との衝突部分に対して，提案手法を用いて軌道を再解像度は 320 × 240 とした．Fig. 1 の右図にカメラ画探索し障害物回避軌道を生成する．以下では，START像の例と抽出された動作の軌道を示す．

は障害物と衝突する直前の点とし，GOAL は障害物と3. 参照点に依存した HMM による動作生成の衝突が終了した直後の点とする．
以下では，4·1 節で提案手法の基盤となる RRT の概

本節では，既存手法 [1] における動作生成の概要に要について，4·2 節で提案手法について述べる．

ついて述べる．

HMM は事前に学習されているものとする．その学習された HMM から軌道を生成することを考える．状4·1 Rapidly-exploring Random Trees (RRT)

態系列 q̄ と HMM パラメータ W λ が与えられたうえで，

RRT は，一般的な経路計画法の一つである．前処理

生成軌道の確率を最大化する最適軌道 W Ξ̂ を得ることを必要とせず，高次元空間でも高速な探索が可能といを考える．世界座標系 W における最適軌道 W Ξ̂ は以下う利点がある．そのため，実時間での経路計画や軌道により与えられる．

生成などに使用されている．

WΞ̂ = argmax P(W Ξ| q̄, W λ )RRT の基本的なアルゴリズムを以下に示す．

(1)

WΞ

= argmax P(W Ξ| q̄, C λ , xrtraj , xr )1. START 地点を初期状態 qinit とし，木 T を初期化
2. 次のアルゴリズム３から６を K 回繰り返す

(2)

WΞ

3. 画像上の 1 点 qrand を以下に従って生成qrand ← RAND CONF()r

ここで，固有座標系を C，トラジェクタ初期位置を x traj ，参照点を xr とする．固有座標系については [1] を参照

4. 木 T 内から擬距離 d を尺度とし qrand と最も近い
ノード qnear を以下に従って得る
qnear ←NEAREST VERTEX(qrand , T )されたい．

HMM パラメータ λ は固有座標系上で学習されているため，軌道生成時には座標変換を行う必要がある．ま5. qnear から qrand までの長さが指定した距離 ε 以下の時は，qrand を新しいノード qnew とし，ε 以上の時は，qnear から ε だけ進んだ方向に新しいノードqnew を生成する．

ず，状態 s における位置，速度，加速度の平均ベクトル
Cµ (k) (s) と共分散行列 C Σ(k) (s) に関して，C から W への座標変換を行う．位置に関する平均ベクトル Cµ (0) (s)は，以下の同次変換行列により変換される．
[] [][]Wµ (0) (s)Cµ (0) (s) − Cµ (0) (1)R xrtraj=1101

6. ここで，qnear と qnew が接続可能ならば木 T にノー
ド qnew と枝 (qnear , qnew ) を追加．この時，障害物などが原因で接続できない場合は追加しない．
このアルゴリズムにより木 T のノードは障害物を回避(s = 1, 2, ..., N)しつつ，一様に広がる．RRT を用いて軌道生成を行う
ここで，R は回転行列を表す．同様に，R を用いて位置場合，qrand を選択する時に一定の確率で GOAL を出以外の平均ベクトルと共分散ベクトルを変換する．

現させる．このようにすることで，START から GOAL

µW (k)(s) = R Cµ (k) (s)への経路を効率良く探索することができる．

(k = 1, 2)

Σ (s) = R CΣ(k) (s) R⊤

W (k)(k = 0, 1, 2)

(3)
(4)

r

最後に，トラジェクタ初期位置 x traj を起点とする軌道を生成するために，正の数 ε ≪ 1 を，第 1 状態の位置の分散に設定する．

Σ (1) = ε IW (0)また，本研究ではより最適な経路を生成するため RRTによる探索終了後，木 T の全てのノードに対して擬距
離 d を尺度として最短経路を再探索した．

4·2 RRT への HMM の導入
本節では，RRT への HMM の導入について述べる．

(5)

物体操作タスクにおいて，単純に RRT を適用すると不以上より，C λ から W λ が得られる．

自然な軌道が生成される．これは，ユークリッド距離4. 障害物回避軌道の生成

は，ノードの連結方向を制約するため，HMM を用いを基準として qnear を選択しているためである．本節で本節では，HMM を用いた RRT について述べる．提案手法の目的は，障害物を回避しつつ自然な軌道の生たノード生成と，HMM を用いた擬距離 d の尺度について述べる．

- 1615 -HMM を用いたノード生成

Table 1 実験設定
学習セットサイズ

提案法では，上記の RRT アルゴリズムの qrand をHMM の位置パラメータの出力確率分布 (OPDF) からのサンプリングする．この手法により，qrand の出現す評価セットサイズる座標を制約できる．

探索回数

提案法におけるランダムサンプリング法の流れを以ε （ノード間の距離）
動作

下に示す．

• START における仮定された HMM の状態と GOAL

175
（35 × 5 動作，障害物あり 20，障害物なし 15）100（20 × 5 動作）
2000
5.0「回す」，「上げる」，「下げる」「載せる」，「飛越えさせる」

における HMM 状態の間でランダムに HMM の状態を決定

Table 2 評価を行った手法の組み合わせ手法
ノード生成 擬距離 d の尺度(1)
HMM 位置HMM 速度(2)
HMM 位置ユークリッド(3)
ランダム
HMM 速度(4)（ベースライン）
ランダム
ユークリッド• その状態の位置パラメータの OPDF から qrand をランダムサンプリング
HMM を用いたノードの連結提案法では，上記の RRT アルゴリズムの 4 において，ノードの連結基準となる擬距離 d の尺度に HMM の速度パラメータを用いる．提案法における擬距離 d の計算法を以下に示す．

• 各ノードに対して HMM の状態番号を仮定• 各ノードに対する HMM の状態の速度パラメータの OPDF の平均と分散を用いて下記の式より擬距離 d を計算．
d(∆qi , µk , Σk ) = N (∆qi |µk , Σk )1((6))

1
T −1=D
1 exp − (∆qi − µk ) Σk (∆qi − µk )2(2π ) 2 (|Σk |) 2

Fig.2 実験で生成された軌道の例

ここに，i は木のノード番号，∆qi = qrand − qi である．

µk ，
Σk はそれぞれ HMM の状態ｋにおける速度パラメー

ラメータを用いた距離尺度，「ユークリッド」はユークタの平均と共分散行列とする．

リッド距離を用いた距離尺度を示す．(4) はベースラインである．

5. 実験

各ノードの HMM の状態番号はユークリッド距離で既存手法で生成した軌道の障害物との衝突部分の軌最も近い位置パラメータの平均を持つ状態と仮定する．

道に対し障害物回避軌道の生成を行う．生成された軌

5·2 定性的結果

道と被験者が実行した軌道との差を評価する．

5·1

実験設定

実験設定を Table 1 に示す．提案法の評価のために，以下の 2 点について比較を行った．

Fig. 2 に実験で生成された軌道の例を示す．図における軌道は，オブジェクト 7 を持ってオブジェクト 8 の上を「飛越えさせる」軌道を示す．図において，（１）から（４）は，Table 2 に示す手法（１）から（４）に• ノードの生成

よって生成された軌道，（５）は同じ物体配置において– HMM の位置パラメータの OPDF からサンプリング

被験者が実行した軌道，（６）は既存手法において生成された軌道を示す．図より，既存手法により生成され– 画像上の座標からランダムサンプリング

た軌道は障害物であるオブジェクト 2 に衝突しているが，（１）から（４）により生成された軌道では，障害• 擬距離 d の尺度物を回避する軌道が生成されていることが確認できる．

– HMM の速度パラメータ– ユークリッド距離

5·3 定量的結果および考察

Table 2 に，評価を行う手法の組み合わせを示す．表

提案手法により生成された軌道 Ξ̂ を，被験者が実行の「HMM 位置」は HMM の位置パラメータの OPDFした軌道 Ξ と比較し，生成誤差を評価する．評価尺度からサンプリング，「ランダム」は画像上の座標からラとして，時系列の比較のために一般的に用いられていンダムサンプリング，「HMM 速度」は HMM の速度パるユークリッド距離を用いる [4]．生成誤差 D(Ξ, Ξ̂) は，- 1616 -フレーム長 T で正規化されたユークリッド距離で与えられる．

D(Ξ, Ξ̂) =1 T
∑
T t=1

√
|xt − x̂t |2(7)

ここに，xt および x̂t は，それぞれ Ξ と Ξ̂ のフレーム tにおける位置ベクトルを表す．ただし，x̂t は，xt と同数のフレームを含むようにリサンプリングを行うものとする．

Fig. 3 に各動作の評価結果を，Table 3 に各動作の探索成功率を示す．

HMM の速度パラメータを用いた擬距離 d の尺度のFig.3 (a)「回す」，(b)「上げる」，(c)「下げる」，(d)「載せる」，(e)「飛越えさせる」の評価結果決定法について評価と考察を行う．
（１）と（２），（３）Table 3 各動作における探索成功率 [%]
動作名
(1)
(2)
(3)
(4)と（４）をそれぞれ比較する． Fig. 3 より動作「回す」

「回す」
「上げる」
「下げる」
「載せる」
「飛越えさせる」以外の動作において，提案法により人が物体を操作した時の軌道に近づいたことがわかる．このことから，提案手法の擬距離 d の尺度の決定法を用いることで，物体操作タスクにおいてより自然な障害物回避軌道が生100
10085100
100

100
100
100
100
100

100100
85100
100

100
100
100
100100

成されたことがわかる．
動作「回す」において提案法による改善が見られなかった原因としては，動作「回す」は障害物を回避する時，障害物がない時の軌道と速度に対する方向が一ラメータを用いることで「上げる」，「載せる」，「飛越えさせる」に対して提案法の有効性が示された．

致しないことが多い．そのため，速度パラメータを用いると人が物体を操作した時の軌道との誤差が大きく[1] K. Sugiura, N. Iwahashi, H. Kashioka, and S.

なったと考える．

HMM の位置パラメータの OPDF からランダムサンプリングする手法についての評価と考察を行う．
（１）

Nakamura, Learning, Generation, and Recognition ofMotions by Reference-Point-Dependent Probabilisticと（３），（２）と（４）をそれぞれ比較する． Fig. 3 よModels, Advanced Robotics, Vol. 25, No. 6-7, pp. 825848, 2011.
[2] R. W. Langacker, Foundations of Cognitive Grammar:

り全ての動作において，大きな差はなく提案手法による性能の向上は見られなかった．これは，RRT による経路探索の後，生成された全てのノードに対して再探索を行ったため，探索途中でのランダムサンプリング

Theoretical Prerequisites, Stanford Univ Pr, 1987.
[3] S. M. LaValle and J. J. Kuffner, Rapidly-exploring
random trees: Progress and prospects, in Algorithmic
and Computational Robotics, New Directions, Wellesley, pp.293-308, 2001.

法の影響をほとんど受けなかったと考える．
また，Table 3 より，動作「下がる」において，探索成功率が下がっていることがわかる．動作「下がる」の速度パラメータの平均は，ｙ軸方向に対してほぼ下向きにある．そのため，RRT アルゴリズムの 4 の qnear の[4] E. Keogh and S. Kasetty, On the need for time series data mining benchmarks: A survey and empirical

選択の時に，下にあるノードが選ばれ易い．しかし，障害物も下にあるためその qnear からノードを伸ばす時，障害物に衝突して新たなノードが作成され難いと考える．このことから，動作「下がる」においては探索成功率が下がったと考える．

6. おわりに
本論文では，物体操作タスクにおいて障害物を回避する軌道の生成について述べた．提案手法の特徴は，RRTへの HMM の導入により障害物を回避しつつ自然な軌道を生成することである．結果より，HMM の速度パ- 1617 -demonstration, Data Mining and Knowledge Discovery, Vol. 7, No. 4, pp. 349-371, 2003.