３．１０．４　厚壁の細長い隙間を通り抜けて位置決めする運動解説　　　２０１５−０３−２３

（実行ファイル3-10-4ThickSlitPassexe ）

「詳説ロボットの運動」第８章８.２.４項参照）

目次：

［3-10-4ThickSlitPass(15-03-16)に含まれるフォルダ・ファイル］               1p

［実施項目］       1p

［シミュレーションにおける前提］　　　　　　　　　　　　　　　　　　　　　　　　　　1p

［シミュレータの使い方］  2p

［厚壁隙間通り抜け・ワーク位置決めのシミュレーションの例］                          4p

［厚壁細長隙間通り抜け位置決め問題の特徴］ 7p

［参考１］隙間通り抜けてゴールへワークを位置決めする「ゴールポーズ」の求め方  7p

［参考２］壁外で壁床干渉を避けながら経路逆生成する方策　−局所干渉回避法− 7p

［参考３］壁内で壁床干渉を避けながら経路逆生成する方策 8p

［3-10-4ThickSlitPass(15-03-16)に含まれるフォルダ・ファイル］

・シミュレータ：フォルダ名 3-10-4ThickSlitPass

・解説のファイル：本ファイルThickSlitPassManual

・補足説明

a:干渉回避しながらゴールポーズから初期ポーズへの経路逆生成−局所的干渉回避法− WOutPassGeneration

b:P節による旋回円と旋回円上の特徴点計算   JMCirclePnt

c:壁内外で単位ベクトルを壁床から遠ざける回転運動  WFApartMotion

d:ワーク（RP関節の先の関節）の目標を干渉しない方向に修正  WorkDirRevise

e:ロボットアームの自己干渉の検証  SelfCollision

f:隙間面内の位置決め逆運動学   Two-DinverseKinematics

［実施項目］

（A）厚壁の細長い隙間を通り抜けてワークを目標位置へ位置決めするゴールポーズを求める．

（B）初期ポーズから隙間を通りながらゴールポーズまでの経路を生成する（実際には逆に生成する）．

（C）初期ポーズからゴールポーズまで経路上の運動をする．

・細隙間にアームを置くために2自由度，ワークを目標に位置姿勢決めするために6自由度，計8自由度が必要である．

・但し，隙間内をアームを動かすために壁内（壁から見てロボットベース側）には1自由度余分に必要であり，３自由度以上なければならない．従って壁外側（壁から見てロボットベースと反対側）には最大5自由度残ることになり，ワークの位置決めには壁内の1自由度を使わねばならない．

・すなわち，隙間を通り抜けと目標に位置決めする各計算は互いに影響し合い，ここでは（A）問題の最終解は隙間通り抜け（‘b’）と目標に位置決めする逆運動学（’a’）との繰り返し演算で求める（第８章８.２.５項）．

［シミュレーションにおける前提］

・ロボットタイプ：

８自由度ロボット４×２種（前半関節(1-1).RPP,  (1-2)RQP,  (1-3)PQQ,  (1-4)PRP,  後半関節(2-1)RP,  (2-2)QP, 及びリストRPR）についてシミュレーションする．

（P, Q, RはX, Y, Z軸を回転軸とする関節．直動関節は扱わない．オフセットなし）

・関節角の制限

　隣同士のアームが喰い込むことを避けるために，P, Q関節に角度の制限を設ける． R関節の制限はない．

・隙間通り抜け・位置決めする関節

　厚壁隙間を通り抜けるために必要な関節は２つ(L, M)，位置決めする関節は３つ(I, J, K)である．姿勢決めを含めて８自由度が必要である．

隙間を通り抜ける運動ができるためには壁の内側の関節は３以上必要である．２関節だと隙間を通るアームは存在するが動けない．

　リストの関節( 6, 7, 8)=(RPR)は姿勢決めをする．これは変更しない．

・ロボットの位置・諸元

　ロボットベースは静止座標系の原点とする．ロボットタイプが変わってもアーム長・太さなどは変えない．

・ロボットの初期ポーズはベースからワークまで垂直に立っている状態とする．

・障害物（厚壁，隙間）

薄壁隙間の基点位置を，隙間中央線は垂直，壁面の法線ベクトルを（Y軸方向）とする．隙間はロボットアーム・関節部が通り抜けられる程度の幅を持っているとする．

・ワークの目標位置姿勢

ワークの目標は床上に置かれるとし，その位置は(X, Y)方向に変えられる．

(Z方向へ変えることもできるが，床下へ置くことは好ましくない→床干渉のエラーとなる)

　姿勢は直方体ワークを上から下向きに置くとする．変更しない．

［シミュレータの使い方］（シミュレータのフォルダ名：ThickSlitPass）

　操作は全て描画画面上でキーによって行う．

（１）フォルダ内使用ファイル

・program名：ThickSlitPass.c, THICKSLIT0.c, POINV2.c, funcall.c, graph3d.c

（２）文字画面でのキー操作

・設定変更やシミュレーションは全て描画画面で操作する．

・計算の節目に計算結果が印刷されるので，必要があれば見ればよい．

・ゴールポーズを求める計算をＡモード，経路逆生成計算をＢモード，経路上の運動をＣモードとする．

　計算はＡから始める．

（３）描画画面におけるキー操作

　　　　　　　　　　　　　　　　図0．キー操作の指示

・シミュレーションの概略の順序

スタート →(1)

(1) ’F’ロボットの選択 →(2)

(2) ’A’計算 (通り抜け・ワーク位置決め計算)

　 (2-1)条件設定(隙間位置・ワーク目標位置)→

(2-2)’A’計算：解なしのとき → (1) or (2)へ

(2-3)隙間通り抜けとゴール位置決め 解ありのとき → (3へ)

(3) ’B’計算(経路逆生成)

(3-1)隙間通り抜け（アーム引き抜き）運動→

(3-2)運動行き詰りのとき→(3)の再計算or (1) or (2)へ

(3-3)生成終了→ (4)へ

(4) ’C’経路運動

・それぞれのキー操作

(1) 条件設定・変更のキー操作（図0）

　‘F’：ロボット選択モード → ４×２種から11, 12, 〜41, 42のどれかをキーインして選択（図1）

→自動的に画面がＡモードへ戻り，選択したロボット表示（図2）

　‘O’：障害物設定モード：(1,4)，(2,5)キーで隙間中央線を±(X,Y)方向へ移動 → ‘A’でＡモードへ

→ 描画画面へ（図0）

ワークの目標位置変更は’A’で，(1,4)，(2,5)，(3,6)キーで目標が±(X,Y,Z)方向へ移動（図0）

［註１］ロボットタイプ・目標位置・壁（隙間）位置等は仮に設定されている．変更したいときに行う．

［註２］設定変更はAモードで行う．但し変更した項目の前の情報は失われる．

　　　　壁隙間の変更はBモードの経路逆生成時にもできる．但し通り抜け中ではX方向に動かしてはならない．

　　　　通り抜け後の自由空間運動では，現ロボットと干渉しない程度にY方向に動かすことができる．

［註３］壁（隙間位置）はY軸正の方向（図の右側）にあることが望ましい．

 (2) Ａモード：隙間通り抜けワーク位置決めのゴールポーズを求める計算

　‘A’：(1) ワークの目標位置変更：ワークの目標位置を(1,4)，(2,5)，(3,6)キーで±(X,Y,Z)方向へ移動

　　　 (2) 隙間位置の変更：’O’により変更．’A’でＡモードへ戻す．

(3) ロボットの最終目標ポーズ計算(calA)：隙間を通り抜けて位置決めする最終ポーズを得る計算

(3-1) SPキー：計算前に繰り返し計算の候補となる初期ポーズが複数表示されるので１つを選択（図2）

(3-2) ‘b’：隙間通り抜け計算(calAb) (図3) → ‘a’：位置決め計算(calAa)（図4） → この繰り返し（’a’,または’b’だけの繰り返しも可）

解が得られたら’B’によりＢモードへ(図6)

繰り返しても解が収束しなければ適当な段階で解なしと判断し，条件設定・変更へ戻る．

［註４］’a’によってワーク目標への位置決めの解を得るが隙間を通り抜ける保証はない．同様に’b’によって隙間を通り抜けても目標に位置決めされるとは限らない．’a’, ’b’の繰り返しによって通り抜けと位置決めの両方を満足する解に収束する．

［註５］’a’によって位置決めの解を得てもその解へ急には変えず，少しづつ進めるようにしている．’b’も同様である．そうでないと最終解に収束しない恐れがある．’a’または’b’だけを続けてもよい．

［註６］目標が特異状態に近いときは収束しないか，非常に遅くなる．その場合はワークの目標位置を解があると思われる方向にわずかに変えてそのゴールポーズの計算をし，その状態から目標位置を戻して計算を繰り返せば解が容易に得られる．（A計算中に目標位置だけを変えることができる）

(3) Ｂモード：ゴールポーズから初期ポーズまで隙間を通り抜けながら経路を逆生成の計算

　‘B’：’s’により自動的に進む．(図7~図14)

　　　　行き詰ったときはその表示が出る．(図17，図18)

　　　　→ 'B'によりもう1回トライできる．

　　　　　（1回目はアーム引き抜き優先，2回目はアーム持ち上げ優先）

→ 2回トライして駄目なら’A’でＡモードに戻し，条件変更する．

(4) Ｃモード：初期ポーズからゴールポーズまでの経路上の運動

　‘C’： ‘s’で運動が1ステップずつ進む．(図15，図16)

　ESC：終了

・画面表示の操作

‘x’, ‘X’,  ’y’, ‘Y’,  ‘z’,’Z’：Ｘ, Y, Z座標の正負に移動

‘p’, ‘P’,  ‘q’, ’Q’,  ‘r’, ’R’：X,Ｙ,Z軸回りに正逆に回転

　‘0’：軌道上の運動を初期状態に戻す

［厚壁隙間通り抜け・ワーク位置決めのシミュレーションの例］

・ロボットタイプ=RPQ-QP-RPRの場合について実行した例を示す．

図1．’F’によるロボットの選択　　　　　　　　　　　　　図2．選択したロボットの表示

前半関節を’1’-‘4’，後半を’1’-‘2’で選択 　　SPキーにより選択ロボットを次々と変えられる．

選択後は自動的にＡモードに入る．

［註６］初期ポーズによって最終解が異なることも同じこともある．また解が収束しないこともある．

図3．A計算：’b’で通り抜け計算　　　　　　　　　　　　図4．’a’でワークの位置決め計算

A計算：　‘a’，’b’キーによって(Aa)計算と(Ab)計算の繰り返し

［註７］‘a’，’b’の交互でも続けて’a’，’b’でも構わない．繰り返しの仕方によっては異なる最終解に到達する場合もあり，また本来解があるのに解なしの結果となる場合もある．

　途中で’A’キーを押せば始点に戻りやり直しができる．

図5．通り抜けと位置決めできた状態　　　　　　　　　　　図6．’B’によりＢモードへ

　　誤差があるとＢへ移れない．

図7．’s’により1ステップずつ経路逆生成　　　　　　図8．アームを抜くために一旦下に下がっている．

壁外アームは干渉しない限り真っ直ぐ伸ばす．

図9．壁隙間から離れる前　　　　　　　　　　　　　　　　図10．壁から離れた直後

図11．壁から遠ざける運動　　　　　　　　　　図12．初期ポーズへ直線運動

図13．初期ポーズへ関節角直線運動　　　　　　　　　　　図14．初期ポーズへ到達

図15．’C’により経路上の運動へ　　　　　　　　　　　　　図16．ゴールポーズへ到達，終了

　‘s’により1ステップずつ動く．

図17．通り抜け不能となったケース　　　　　　　　　　　　図18．壁直後の関節が床と干渉し

　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　経路生成不能となったケース

［厚壁細長隙間通り抜け位置決め問題の特徴］

ロボットタイプによる隙間通り抜け・最終位置決めの難易

・ロボットは８自由度でその関節は全て回転関節としたが，直動関節は８自由度では位置決めできる領域が狭い．

・Ｚ軸回りの関節軸をＲ，Ｘ，Ｙ軸回りの関節をＰ，Ｑと名付ける．

・ベースの関節J123=RQPのロボット（ロボットタイプ２）は隙間を通り抜けて位置決めできる領域が広い．

・J123=RPPのタイプ１は隙間が真正面(J1が隙間中央面内)にないときは殆ど通り抜け不可能である．.

・J1=P, Qのタイプ３，４も通り抜け位置決めできるは領域が狭い．

・中間にＲ関節を持つタイプは先の方のアームを比較的自由にどの方向へも伸ばせるので，最終解を得るのに有利である．

・しかし，Ｒ関節を含むアームが長いので（ロボットはどのタイプも全長を等しくしている），壁隙間位置がロボットベースに近いとアームを隙間から引き抜くのが難しくなる．

・壁の向こう側で手が届く限り位置決めできるようにするには，壁の向こうに６自由度が欲しい．８自由度で可能であるが，この問題には９自由度以上が操作上望ましい．

［参考１］隙間通り抜けてゴールへワークを位置決めする「ゴールポーズ」の求め方

・壁内の2つの関節による隙間通り抜け(Aa)と残りの6関節によるゴール位置決め計算(Ab：3関節によるリストの位置決め([註1])を交互に繰り返し，目標とする通り抜け・位置決め解に近づける．

・この繰り返しは必ずしも目標解に収束するとは限らないので（実際に解の回りで行ったり来たりする），ある程度近づいたら5関節によって通り抜け（通り抜け条件2つ）とリスト位置決め（条件３つ）に近づける5×5のNewton法を適用する．

・ロボットベースJ1が隙間中央平面内にある特殊ケースはロボット関節J123構成によって異なる計算法によって解析的に通り抜け・位置決めのゴールポーズを求めることができる．

［参考２］壁外で壁床干渉を避けながら経路逆生成する方策　−局所干渉回避法−

・経路の生成は隙間を通り抜けて目標に位置決めされた状態（ゴールポーズ）から初期状態（初期ポーズ）へと逆に生成する．正の経路生成よりこの方が運動の制約が少ないからである．

・壁外側にある関節・アームが壁を通り抜ける可能とするために関節を順並び（アームを壁から外側に伸びた状態）とする．

・同様に壁内側（壁隙間からワークを含めて全て引き抜いた後）で初期ポーズへ到る経路逆生成においても，関節を順並び（アームを壁から内側に伸びた状態）とする．

・逆並びから順並びへ無条件に伸ばせば壁床との干渉の可能性がある．

・それを避けるために単純な「局所干渉回避法」を採用する．

・これは「ワーク(Nw)・リスト(N, N -1, N-2)・肘(N -3)・．．．の順（降順）に，アームを３干渉（壁・床・自己干渉）なしに壁床から遠い方向へ伸ばせれば一気に伸ばす．途中で干渉があることが分かれば１ステップ運動させる」，という方法である．

・降順に伸ばせるアームは先に伸ばしてしまうというものであるが，若い番号の関節アーム（JMとする）が例えば下向きに動けば先方の関節(JM+1)が床干渉を起こす可能性がある．その場合はアームを曲げてJM+1を持ち上げる．

　少なくともJMより低くなることはない．持ち上げた結果壁と干渉することがあれば行き詰りであり、経路生成不能を意味する．

［計算手順］

・壁直後の関節番号をMw，次の関節をMとする．

（TH）アーム引き抜き運動

・壁内関節角を微小に変えて隙間通り抜け計算をし，アームが隙間から引き抜かれる方向へ動く解を採用する．

・解が現状のままか元に戻る解しかないときは引き抜き不能で，経路生成できないことを意味する．

（O）壁外で関節を順並びとする方法

・昇順に，J[M]の一気運動可能性判断と1刻み運動→各1刻み毎にJ[M+1]の一気運動可能性判断と1ステップ運動→

　・・・・・､ワークの一気運動を行う．より具体的には

(WO-1)  J[M=Mw+1]の目標と運動

(1-1):J[M]が一気運動（J[M]が壁床と干渉せずにアームを伸ばすまで運動すること）できるかどうか判定する．

J[Mw-1,Mw]=RP(RQ)のときは真上を目標とする．この2関節でJ[M]を任意の方向へ向けられるのでたとえ壁床が近くにあっても目標まで一気運動が可能である．

(1-2):J[Mw-1],Mw]=PP(QP)のときはq[Mw]=0（アームを真っ直ぐに伸ばす方向）を目標とする．q[Mw]による旋回運動であり､途中壁床干渉がなければ一気運動，あれば1ステップ運動（q[Mw]の微小運動）とする．

(1-3):J[M]の1ステップ運動または一気運動を分割してその1刻み分の運動させる．

(1-4):(1-2)以下の運動が済んでから1ステップ運動なら終了，一気運動なら次の1刻み運動

(WO-2)  J[M+1]の目標と運動

・同様にJ[M+1]の目標と一気か1ステップ運動かを決め，1刻みの運動をする．

・以下J[M+2],．．．．，リスト，ワークの一気運動まで同様に繰り返す．

(WO-3)  J[M],J[M+1],…の1刻み毎に干渉チェックする．干渉あれば前回の値に戻して一気運動の刻み運動を終了する．

［参考３］壁内で壁床干渉を避けながら経路逆生成する方策

・全ての関節とワークが壁の隙間を通り抜け後は自由空間で初期ポーズに到る経路を逆生成する．

・全ての関節が壁から十分離れていて上方に順並びとすることを方策とする．

・全ての関節が順並びとなった後に初期ポーズまで直線的に動かすことは容易である．

［上方へ順並びとする計算手順］

・壁最近関節をJ[MC]とする．

(WI-1)  J[MC]を壁から遠ざける（-Y方向）と共に上方へ1刻みづつ動かす．

(WI-2)  J[MC]を含めて全ての関節が上方へ順並びとなるまで続ける．

(WI-3) 順並びとならなくてもJ[MC]（MCは運動中に変わることもある），壁から十分離れたら順並びと見なす．

(WI-4) 順並び後は初期ポーズに向けてワーク（リスト）を直線的に動かす．