The Japan Society of Mechanical Engineers 䢴䣃䢳䢯䣘䢲䢳 移動マニピュレータによるピッキング作業のための台車位置計画      ○正 原田研介 産総研 正 永田和之 産総研   !              正 辻徳生（九州大） 音田弘（産総研） 菊地宏平（トヨタ自動車 株） 河井良浩（産総研）     " #        $  %       "& ' ()  *  ' "*( "(  '"  +)  , )  & "& - + .&  *  '  "*( "( -  "( + +   */+ (+&    '  (( % . 0(  "& ' ()  "+ )  +(( *-  * &  ((  +&  - -  "& ' &")  *   1+ '  & "&  +."& * "+( 0"(               はじめに 近年，工場において製品製造のロボット化が積極的に進められ ている．しかしながら，特に製品の組み立てラインにおいては， 未だ人手に頼る作業が多いのが現状である．ここで，組み立て作 業を完全に自動化するための最初のステップとして，組立ライン に部品を供給する作業を自動化することが期待されている． この部品供給作業においては，それぞれの部品を箱から必要 な数だけ取り出し，通い箱と呼ばれる箱に移し替える．そして， この通い箱を組立ラインに供給する．ここで，部品供給作業をロ ボット化するために，移動台車にマニピュレータが搭載されたロ ボット 移動マニピュレータ を用いることを考える．ここで，マ ニピュレータの先端には多指ハンドが取り付けられている．この 時，マニピュレータと比較して，移動台車は素早い動作が困難で ある．このため，作業のタクトタイムを上げるためには，台車の 移動回数がなるべく少なくなるような台車の位置を求め，その位 置に台車を固定した上で，マニピュレータでなるべく多くの部品 を通い箱に入れる必要がある． 本研究では，ロボットが箱の中に入った部品を取り出す作業を 想定し，箱の中のなるべく広い範囲の部品を取り出すことができ る移動台車の位置・姿勢を求める．ここで，実際の現場で用いる ことを想定し，高速に台車の位置・姿勢を求めることが可能なア ルゴリズムを提案する．まず，移動台車が地面から並進２自由度 と回転１自由度の関節により接続されているとしてモデル化し， 与えられた手先の位置・姿勢に対して逆運動学を解くことで移動 台車の位置・姿勢を求める．ここで，逆運動学を解く場合，ロボッ トと箱などとの干渉を考慮し，干渉をさけるような解を導出する 必要がある．このため，本研究では２次計画問題を解くことで干 渉を考慮した逆運動学問題  を用いる．ここで，２次計画問題 を解く場合，精密なポリゴンモデル同士で干渉をチェックしたの では計算時間がかかるので，本研究では環境やロボットのリンク に楕円を仮定する．そして，２次曲面同士の距離計算としては，  ら  の手法を用いることにする．最終的に，箱の四隅対 象物が置かれている場合を想定し，これら全てをロボットがピッ クできるような台車の位置を求めるために，２分法を用いて逆運 動学問題を繰り返し解く手法を提案する．提案する手法を用いる と，箱の中の広い範囲の対象物をマニピュレータの先端に取り付 けられた多指ハンドで安定に把持することが可能な台車の位置・ 姿勢が高速に求められる．なお，本研究は組み立てラインに部品 を供給するロボットを想定しているが，本技術が適用できる範囲 は，組み立てラインに部品を供給するロボットに留まらず，一般 的な移動マニピュレータによる部品のピッキング作業に適用でき ると考えられる． 本論文では， 章で関連研究について説明した後に，３章では 逆運動学問題について説明する．次に，４章で台車の位置・姿勢 を探索する手法を提案する．最後に５章では、いくつかの数値例 により提案する手法の有効性を示す．  くの研究が行われている（例えば文献    ! " 等）．それに 対して，移動マニピュレータにおいてマニピュレータの先端にハ ンドを仮定してハンドで対象物を安定に把持することを想定して 移動台車の位置・姿勢を求める問題が近年解かれている  #． $ % らによって開発されているソフトウェア & '() には * %  + , と呼ばれる機能  が備わっており， 目的とする対象物を安定に把持することができる移動台車の位  ら 置・姿勢の集合を求めることができる．また，( + # はオフラインで生成したデータベースを基に逆運動学を解く 手法を提案しており，この中で移動台車の位置の集合を求めてい る．しかしながら，これらの手法は与えられた対象物の位置・姿 勢に対して，移動台車の位置・姿勢を求めるものである．それに 対して，提案する手法は箱の中に置かれた対象物に対して，対象 物がどのような位置にあってもロボットが対象物を把持できる移 動台車の位置・姿勢を求めるものである．また，本手法は逆運動 学を解く際に周辺環境との干渉を考慮しながら解くことを考えて いる．  逆運動学 本章では，マニピュレータの手先の位置・姿勢が与えられた場 合に，マニピュレータの関節角，ならびに移動台車の位置・姿勢 を求める問題の解法を示す． - . に示すように，移動マニピュ レータのモデルとして，床面から水平面内の並進２自由度関節と 鉛直軸まわりの回転１自由度関節を介して移動台車が接続されて いるモデルを考える．ここで，移動台車が非ホロノミックな拘束 条件を持ち，移動台車の移動経路も考慮する場合は，このような モデルによってモデル化することはできない．それに対して，本 研究で用いる移動マニピュレータは移動台車が動作中にマニピュ レータは動作しないと仮定し，非ホロノミックな拘束条件を持つ 移動マニピュレータに対しても，- . に示すようなモデルを適 用する． 本研究では，箱の中に置かれた対象物をハンドで把持できるよ うな，移動マニピュレータの台車の位置・姿勢を求めるために， - . に示すモデルに対して逆運動学問題を解く．ここで，箱の 中の置かれた対象物をハンドで把持する場合，箱とマニピュレー タとの干渉を考慮しながら逆運動学問題を解かなくてはならない． そこで，本研究においては，金広ら  の手法を用いて逆運動学 問題を解くことにする．- . に示すモデルに対して，手首の速 度・角速度を合わせた６次元ベクトルを とする．また，ロボッ トの関節角ベクトル  と台車の位置，姿勢     を合わせて， ベクトル  /      を定義する．このとき，以下のような 関係式が成立する． 関連研究 本研究では移動台車にマニピュレータが搭載された構成のロ ボット 移動マニピュレータ において，移動台車の位置・姿勢 を求める．移動マニピュレータにおいて台車の位置・姿勢を求め る問題は，例えば可操作性  を仮定するなどして，従来から多 /  0  上式を用いて，逆運動学問題は以下の２次計画問題を 0 について 解くことに相当する ． 䣐䣱䢰䢢䢳䢶䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢶䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣱䣻䣣䣯䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢷䢯䢴䢻䢮䢢䢴䢲䢳䢶 䢴䣃䢳䢯䣘䢲䢳䢪䢳䢫 NII-Electronic Library Service The Japan Society of Mechanical Engineers  2((& " 0 0   対象物を箱の中心に配置し，対象物が把持可能とな る台車の位置を把持データベースを探索しながら逆運動学 問題を解くことによりにより求める．   8  で得られた台車の位置を初期位置として，箱 の四隅それぞれに置かれた対象物を把持可能な台車の位置 を把持データベースを探索しながら逆運動学問題を解くこ とにより求める．   8  で求めた四つの台車の位置の平均をとる． /  0     0     0 ここで，  はロボットの自己干渉や環境との干渉を避けるため に使う行列であり，  は関節可動範囲の限界や速度限界を超 えることを避けるために使う行列である ．２次計画問題のソ ルバとしては，)4 5 66 7 を用いた．ここで，この２ 次計画問題は速度レベルの拘束条件を考慮して手先の速度を求め るものである．文献  では，２次計画問題を実時間で解くこと で，サンプリングタイム毎の目標関節角を求めている．それに対 して，本研究は移動マニピュレータの台車の位置・姿勢を求める ことを目標としている．この問題に対して上記２次計画問題を適 用するために，初期手先位置・姿勢と最終的な目標手先位置・姿 勢を直線で繋ぎ，この直線上を微小ステップ毎に進むように目標 手先位置・姿勢を逐次的に更新しながら上記２次計画問題を解く こととした．これにより，本研究で解く逆運動学問題は，次の形 により定式化できる．  /      ¼      8 # で得られた台車の位置を初期位置として，四 隅に配置した対象物が把持可能かを台車の位置・姿勢を固 定して把持データベースを探索しながら解くことでチェック する．四隅の対象物全てが把持可能であればアルゴリズム を終了する．  8 7 で把持ができなかった隅に置かれた対象物 を把持するように，8  で求めた台車の位置に台車を移 動する．ここで，8 7 で把持ができなかった隅が複数あ る場合は，それぞれの隅に置かれた対象物を把持できる台 車の位置の平均をとり，その位置に台車を移動する．  8  で求めた位置に台車を固定した状態で，四 隅に配置した対象物が把持可能かを把持データベースを探 索しながら逆運動学問題を解くことでチェックする．四隅の 対象物全てが把持可能であればアルゴリズムを終了する．  8 # で求めた台車の位置と 8  で求めた台車 の位置の間を二分探索することにより，把持可能な隅に置 かれた対象物の数が最大となる台車の位置を求める．  ここで，¼ は  の初期値であり， は手先の位置・姿勢を合わ せた  次元ベクトルの目標値である． ２次計画問題において，自己干渉や周辺環境との干渉を避ける ために使う式     0 に着目する．２次計画問題が解を持つた めに，干渉を避けなければならない二つの物体は狭義に凸な形状 を持たなくてはならない．ロボットのリンクやロボットの周辺環 境は通常狭義に凸ではなく複雑な形状をしており，その形状モデ ルをそのまま用いて２次計画を解いたのでは，計算時間的に厳し くなることが想像される．そこで，本研究ではロボットのリンク や周辺環境を楕円で近似することを考える．本論文においては， - . に示すように，マニピュレータの手首リンクと下腕リンク に楕円を仮定し，また，箱の各エッジに細長い楕円を仮定した． ここで，楕円を用いることは近似であり，厳密な意味で干渉回避 ができている訳ではない．そこで，本研究では逆運動学問題を解 く度に，干渉回避が出来ているかを確認する意味で，干渉チェッ クを一回実行することにする．次に，楕円体同士の干渉回避を実 現するためには，二つの楕円体の距離計算が必要である．計算コ ストが小さな楕円体同士の距離計算として，  ら  は距離 計算を固有値問題に帰着させる手法を提案している．本手法は非 常に計算コストが小さく，本手法でも彼らの手法を用いることに する．  * & 23 とが可能である．なお，この把持データベースにおいては，把持 安定性の高い方から順にデータがソートされている．このため， データベースを探索する際は，データが並んでいる順に探索し， 解が見つかった時点で探索を終了する．箱の中心と四隅から指を 挿入できるだけのマージンを持った位置に対象物を仮定し，これ ら全てに対して，同じ台車の位置から対象物を安定に把持できる ような台車の位置を求めることを考える．この台車の位置・姿勢 の算出アルゴリズムは以下のフェーズから構成されている．  &( ' *( ( "  1 2+   3  "& ' * 腰位置探索アルゴリズム 本章では，移動マニピュレータの台車に関して，特に姿勢を固 定し，２次元平面内の位置を求める問題を考える．まず，各対象 物に対して，ハンドで安定に把持できる把持姿勢の集合が，デー タベースの形式で予め与えられていると仮定する  ．このデー タベースにおいて，各データは対象物座標系からみた手先の位 置・姿勢とハンドの指関節の関節角度から構成されている．この データベースを用いることで，対象物の位置・姿勢が与えられる と，各データに対応した手先の位置・姿勢，ならびにハンドの指 関節が与えられる．この手先の位置・姿勢を用いて逆運動学問題 を解き，これが解を持つと，ハンドで対象物を安定に把持するこ 上記のアルゴリズムにおいては，そのまま把持データベースを 探索したのでは，同じデータが何度も探索されることになり，そ の都度逆運動学問題が呼ばれるため，計算量的に非効率である． ここで，把持データベース中にはハンドと箱が干渉する把持姿勢 が含まれており，探索が実行される度この把持姿勢を探索するこ とを避けるため，把持データベースの探索時にこのような把持姿 勢があったら，把持データベースから削除し，次回からは探索し ないようにする．  数値例 本研究の有効性を示すために，いくつかの数値例を示す．- .# に示すように，双腕ロボットが箱の中に入ったドアノブを把持す る状況を考える．このドアノブのモデルに対して把持データベー スを作成したところ， 個の把持姿勢から構成されるデータ ベースが構築された．また，このロボットは床面から３自由度関 節を通じて腰リンクが取り付けられている． まず，- .7  に示すようなロボットと箱との位置関係を仮定 し，また， に示すようなサイズの箱を仮定した．この時，提 案するアルゴリズムを適用し，腰の位置・姿勢を求めた結果を  に示す．この例では，8 7 でアルゴリズムは終了した．また， 計算時間はクロック周波数が #9:1 の ;< を用いて .# を要 した． 次に，箱の大きさを - .7 の場合よりも少し大きくした場合を 考える．この時のロボットと箱との位置関係，ならびに箱のサイ ズを - . に示す．この場合の計算結果を   に示す．この 䣐䣱䢰䢢䢳䢶䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢶䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣱䣻䣣䣯䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢷䢯䢴䢻䢮䢢䢴䢲䢳䢶 䢴䣃䢳䢯䣘䢲䢳䢪䢴䢫 NII-Electronic Library Service The Japan Society of Mechanical Engineers   '  20"( 5  20"( 6 "+( 20"( 20"( 4 表に示すように，8 7 の時点では箱の右奥にある対象物が把持 できておらず，8  では左手前の対象物が把持できていない． それにもかかわらず，最終的には四角全てに置かれた対象物が把 持できるようになっていることが分かる．この計算には .! を 要した． 箱の大きさを更に大きくした場合のロボットと箱との位置関 係，ならびに箱のサイズを - . に示す．この場合は，本研究で 提案するアルゴリズムでは右奥と左手前の対象物を最終的に把持 することが不可能であった．この計算には ."! を要した． 最後に，箱と箱の中の対象物のポイントクラウドを =  セ ンサでキャプチャし，キャプチャしたポイントクラウドに対象物 のポリゴンモデルを当てはめ，当てはめられた対象物に対して ピックアンドプレースを行った結果を - . に示す．図に示すよ うに，ロボットが箱との干渉を避けながら，指定された場所に対 象物を置く動作が実現されている．   おわりに 本論文では移動台車の上にマニピュレータが搭載されたロボッ ト（移動マニピュレータ）が箱に収められた部品をピッキングす るタスクを想定し，箱の中のなるべく広い範囲の対象物をピッキ ングできるような台車の位置・姿勢を探索した．特に，マニピュ レータの自己干渉や環境との干渉を考慮しながら逆運動学問題を 解くことで，マニピュレータのリンクが箱と干渉することを避け るような姿勢を導出した．また，この逆運動学問題を用いて，上 記目的を達成する探索アルゴリズムとして２分法を基にした手法 を提案した． ここで，本研究で用いた逆運動学を解くアルゴリズムではリン クや箱を楕円体でモデル化し，楕円体同士の干渉をチェックする ことで干渉回避を実現した．ここで，環境やマニピュレータのリ ンク形状によっては，楕円体でうまく近似できなくなるような場 合も想定される．高精度で実時間性の高い逆運動学アルゴリズム を開発することは今後の課題であると考えられる．      ).  5 8. . ,5 >&  ; <  )5 -? @. A *    5  8, %. ! . B "" .  金広 他 >実機の物理的制約を考慮した即応的脚動作生成手 法? 日本ロボット学会論文誌 %. ! .  . B  . # 辻 原田 山野辺 永田 中村 長谷川 >一般物体把持のた めの把持姿勢変換? ロボティクスシンポジア予稿集 . "B7 . 7 宇都 辻 原田 田原 長谷川 倉爪 >区分的な二次曲面近似 に基づく把持姿勢候補の生成? ロボティクスシンポジア予 稿集 . B# .  -. 9.  5 @.B;. ; >B;  5 ; C   &1  A 5 5   B AD   8,?             . #B  "".  :. 8 3 >' & B  ' +  ;5 5 B , 5    ?          . !B# ""#.   =. + 5 . 1,   >*    =   5 $E, A    ?          . 7"#B7"! . ! .  , @.BF. -G 5 .  5 >  B , ' , A    ?          . B . " F. F   5 H. F  >< C5 ' ,  B , 5   , A    ?          . B  """.  . F+ 2  >  , A  + ? * . @. A   + %. 7 .  . #B" "!.  . $ % 5 @. =I  >& '()J '   'B + A '  ?      ! !!"#!$%&   &   ! '  !.  +JDD  %. D5D   D  %,DE . # K. ( +  . 'A 5 . $  >)Æ  * % =   ;   5   + , ' ,? * . @. A : 5  %. " . 7 . 䣐䣱䢰䢢䢳䢶䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢶䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣱䣻䣣䣯䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢷䢯䢴䢻䢮䢢䢴䢲䢳䢶 䢴䣃䢳䢯䣘䢲䢳䢪䢵䢫 NII-Electronic Library Service The Japan Society of Mechanical Engineers     " 89:9 8789 8;65 85<< 8;88 86=6 腰位置  7  :  9 ( ' 20"( 5 左奥 ○ ○ ○ 右奥 × ○ ○ 左手前 ○ × ○ (a) (b) (c) (d)  右手前 ○ ○ ○ ( ' >+ & >(+  7 )4 5 66 +JDD222...5D +DG 5 .+.  =. :  5  . >9    A    9 2+ -E,   9  8A ?         (  .  =. :  5  . > 5     A $ B'   ?          .   . 3 =. :  5  =. =   -. = + 5 =.  ,   >9    A C 5 : 5 2+ : 5 ? @. A  5 + B   (. K. . #B#! . ! =.:  5  . >&3      A   5    ? . A *)))D 8@ * . ; A.  *     5 8, . "!B"! . " 辻 原田 >    A ;+ 5? 日本ロボット学 会誌 %. # . # . #B# #. 䣐䣱䢰䢢䢳䢶䢯䢴䢢䣒䣴䣱䣥䣧䣧䣦䣫䣰䣩䣵䢢䣱䣨䢢䣶䣪䣧䢢䢴䢲䢳䢶䢢䣌䣕䣏䣇䢢䣅䣱䣰䣨䣧䣴䣧䣰䣥䣧䢢䣱䣰䢢䣔䣱䣤䣱䣶䣫䣥䣵䢢䣣䣰䣦䢢䣏䣧䣥䣪䣣䣶䣴䣱䣰䣫䣥䣵䢮䢢䣖䣱䣻䣣䣯䣣䢮䢢䣌䣣䣲䣣䣰䢮䢢䣏䣣䣻䢢䢴䢷䢯䢴䢻䢮䢢䢴䢲䢳䢶 䢴䣃䢳䢯䣘䢲䢳䢪䢶䢫 NII-Electronic Library Service