機構の強度的危険性を考慮したマニピュレータの動作計画○小松 祥人（筑波大院）

磯部 大吾郎（筑波大学）

Motion Planning of Manipulators Considering Its Structural Risk
*Akihito KOMATSU, Graduate School, Univ. of Tsukuba, Daigoro ISOBE, Univ. of Tsukuba
Abstract － In this paper, a methodology for motion planning of manipulators considering its structural risk
is discussed. In this algorithm, when the member yielding risk of a manipulator in motion becomes high, asafer attitude is searched to restrain its total strain energy, to avoid the structurally dangerous attitude. Risk formember yielding and total strain energy are calculated by using the Finite Element Method (FEM), and an
attitude for restraining the total strain energy is searched by using Direct Search Method (DSM). Somenumerical tests are carried out to examine the roles played by the two structural parameters in this algorithm.
Furthermore, the motion-planning algorithm is implemented into an experimental system to verify its validity and practicability.
Key Words: Motion planning, Finite Element Method, Manipulators, Structural risk

1. はじめにひずみエネルギを抑制する手法 (Scheme A)
従来からロボットの軽量化は，動作の高速化，消費・ 全ひずみエネルギのみを用い，その最小化を行エネルギの軽減や安全性等の点から重要視されてきた．
う手法 (Scheme B)
しかしその一方で，軽量化によって機構の強度が低下・ 断面力レベルで部材の降伏危険性を判定後，そし，破損の可能性が高まるという問題が生じている．の抑制を行う手法 (Scheme C)
近年，研究が盛んな生活支援ロボットや福祉関連ロボ
さらに，本アルゴリズムを導入したインターフェース
ットなどの人間と共存を目指すロボットは，人間に危を作成し，実機による模擬実験を行い，その有効性･実害を加えないように軽く，しかも様々な作業が行える用性について検証した．
機構であることが望まれる．また，突然の外力等によって機構が破損しないような対策も求められる．この2. 動作計画アルゴリズムの検討
ような様々な環境にさらされる可能性のあるロボット
本章では，まず Scheme A の動作計画アルゴリズムには，適切な強度設計がなされた上で，その動作段階[4]について説明し，簡単なタスクに対する動作計画結で外力に対して安全な姿勢をとらせることが，機構の果を示す．その後，全ひずみエネルギと断面力を個別破損を回避する有効な手段の一つであると考えられる． に用いた手法(Scheme B，Scheme C)による動作計画を
これは普段，人間が作業の負担を軽減するために様々行い，Scheme A との比較・検討を行う．
な姿勢をとることからも想像できる．
2.1 断面力と全ひずみエネルギを用いた動作計画
他方，ロボットの姿勢決定や動作計画については，
2.1.1
直接探索法による全ひずみエネルギの抑制
従来から様々な議論がなされており，人間の動作を模機構内に蓄積される全ひずみエネルギ U は，姿勢す擬した動作計画に関する研究[1][2]なども行われていなわち関節角に依存する多変数関数で表される．本研る．これらの研究では一般的にロボットの構成部材を究では，次式に基づき有限要素法によって数値的に求剛体として扱うことが多く，また柔軟構造を取り入れめる．
たロボットの研究[3]などにおいても，機構の強度的危nTT
険性を考慮した姿勢決定や動作計画はほとんど実施さ(1)U = ∫ {ε } {σ }dV = ∑ {ε }i {σ }i li
Viれていない．
ここで，i は要素番号，n は総要素数，{ε}i , {σ}i はそ
以上のような観点から，ロボット機構を構造工学的れぞれ要素i のひずみベクトルおよび断面力ベクトルに捉え，機構の部材強度を考慮した姿勢決定･動作計画であり，li は要素長である．このような多変数関数に対法が考案された[4]．本手法は，有限要素法を用いた実して直接探索法を適用し，その極小値を探索する．
時間内力分布解析により，断面力レベルでの部材降伏直接探索法は，1 変数の極値を探索するための計算手
危険性と機構内に蓄積される全ひずみエネルギという，法であり，変数の初期値と増分値を適当に与え，前後2 種類の構造力学的パラメータを監視することで，機構のステップにおける関数の大小の比較を反復的に行う．
にとって危険な姿勢を回避しようというものである．
例えば，関数 f(x)の極値を探索する場合には，前ステッ
本報告では，以下のような組み合わせで動作計画を実プs-1 と現ステップs における関数の関係がf(xs-1) > f(xs)施し，これらの 2 つの構造力学的パラメータが本動作であれば，変数の増分値を変化させずそのまま次ステ計画法において果たす役割について考察する．
ップに移行する．一方で，f(xs-1) < f(xs)となる場合には，・ 断面力レベルで部材の降伏危険性を判定後，全変数増分値を-1/2 倍に置き換えて次ステップへ移行し，収束が判定されるまで反復計算を行う．なお，本アルゴリズムの収束判定には次式の無次元化された条件式を用いた．
(2)

2.1.2 姿勢決定アルゴリズム
全ひずみエネルギを抑制した姿勢を決定する場合，実用上必ずしもその値を最小化する必要はなく，部材降伏が回避されるのみで十分と考えられる．そこで，機構が断面力レベルで部材降伏する際の全ひずみエネルギを臨界ひずみエネルギ Uc と定義し，これと抑制目
標レベル kU との積を全ひずみエネルギの抑制目標値とした．臨界ひずみエネルギ Uc は姿勢や外力の作用方向などによって変化するため，姿勢が修正されるたびに計算する必要がある．
2.1.3 動作計画アルゴリズム
本動作計画アルゴリズムは，ロボットの動作軌道上で，部材の断面力レベルでの降伏危険性が判定されると，前述した姿勢決定アルゴリズムにより安全な姿勢が探索され，得られた姿勢を経由して最終的な目標姿勢へ向かう軌道が再作成される．すなわち，機構が強度的に危険な姿勢になる度，安全な軌道を模索して軌道修正が行われるのである．
各要素の降伏危険性の判定には，次式に示す降伏関数 fy を用い，この式の左辺が指定した降伏危険性判定レベル ky (0<ky<1)を超えるか否かで判定される．
2X2

 M   My   N  + = k yf y ≡  x  +   M x0   M y0   N 0 (3)

ここで，Mx，My および N はそれぞれ，x，y 軸回りの曲げモーメントおよび軸力であり，添え字‘0’は全断面塑性値を意味する．なお，Mx，My については，はり理論にしたがって，最大値をとる位置である要素端での断面力を用いる．ky を大きく設定するほど部材の強度を活用することになり，危険な姿勢になって初めて軌道修正が行われるようになる．逆に、ky を低く設定
すると，それほど危険でなくても早めに軌道修正が行われるようになる．
2.1.4 ３リンクマニピュレータの動作計画
上述した動作計画アルゴリズムを，Fig.1 に示すような 3 リンクマニピュレータに適用し動作計画を実施しJoint 1

Link material: Aluminum
Link Length: 20 [cm]Link section: 1[cm]×1[cm]Motor mass: 1.0 [kg]Fig.1 3-link manipulator
0.6

Z-coordinate [m]ここで，αは収束判定ノルムであり，この値が小さいほど得られる解の精度は向上するが，その分計算時間を多く要することとなる．機構を構成する複数の駆動部に対して直接探索法を順に適用し，全ひずみエネルギの抑制を図る．

2Final target

Joint 3Joint 2U s − U s −1
<αU s −1

Initial attitude

ZFig.2 Target trajectoryky = 0.8
kU = 0.3

0.4

0.2

000.2
0.4
X-coordiate [m]0.6

Fig.3 Obtained motion plan by Scheme A

た．マニピュレータのモデル化においては，1 リンク部材を 1 つの 3 次はり要素で表現し，解析は鉛直面内で行った．また，モータには十分なトルクが発生していることを想定し関節部分は剛節とみなした．外力付加条件として FZ=-100[N]を与え，Fig.2 のような目標軌道を与えた場合に得られた動作計画を Fig.3 に示す．ただし，直接探索法は先端関節から順に(Joint 3 →Joint 2→Joint 1)適用し,(ky, kU)=(0.8,0.3)と設定し，α=0.01 とした．
Fig.3 のように，目標軌道への正確な追従は達成できなかったが，先端リンクを根元方向へ引き寄せて曲げモーメントの増大を防ぎ，機構の負担を軽減するような軌道が得られた．
2.2 全ひずみエネルギのみを用いた動作計画
本節では，Scheme A に対し，断面力による部材の降伏危険性判定や臨界ひずみエネルギの算出を行わずに，全ひずみエネルギのみを用いた動作計画(Scheme B)を実施する．全ひずみエネルギは，機構形態，姿勢，外力の付加条件等によって変化するため，その目標値を設定することができない．したがって姿勢探索時には，機構の全ての関節に対して直接探索法を適用し，個々の関節において(2)式が判定されるまで収束計算を行う
ことで，全ひずみエネルギの最小化を行う．
与えるタスク，モデル化においては 2.1 節と同様とし，α=0.01 とした．得られた動作計画を Fig.4 に示す．図のように，マニピュレータはタスクを遂行することができず，初期姿勢からほとんど移動することなく姿勢が収束している．今回のタスクにおいては，この姿勢が最も全ひずみエネルギが小さい姿勢であると考えられる．これは，Fig.5 のように，初期姿勢を 45 度傾け

Z-coordinate [m]ky = 0.8
tky = 0.3

Z-coordinate [m]Z-coordinate [m]

0.6

0.6

0.6

0.4

0.4

0.2

0.2

00.2
X-coordinate [m]

00.4

0.2
X-coordinate [m]00.4

Fig.4 Obtained motion plan by Fig.5 Obtained motion plan byScheme B, initial attitude 0°°0.20000.40.20.4
X-coordinate [m]0.6

Fig.6 Obtained motion plan by Scheme C

Scheme B, initial attitude 45°°た状態からタスクを開始した場合にも，同様な姿勢へと収束していくことからも確認できる．このように，全ひずみエネルギを最小化すると姿勢が 1 つに収束してしまい，ロボットなどのように移動や姿勢を変えることによって作業を行う機構に適用する場合には，本アルゴリズムは不適切であると言える．
2.3 断面力のみを用いた動作計画
本節では，(3)式の部材の断面力レベルでの降伏危険性の判定後の安全な姿勢探索において，全ひずみエネルギというパラメータは用いずに，断面力のみの抑制によって姿勢決定を行う動作計画(Scheme C)を実施する．具体的には，(3)式の fy の値を，新たに設定した指定目標値 tky (0 < tky ≦ ky) 以下に抑制するような姿勢探索を行う．断面力から求まる fy は要素ごとに算出されるため，それらの fy のうち最大となるものをその都度採用し，次式に示すように最大値が tky より小さくなった時点で姿勢を決定する．
f y < tk y(4)また，姿勢探索時の収束計算における収束判定条件にも fy の値を使用し，次式のように設定した．
f ys − f ys −1 < α(5)Fig.7 An example of complicated mechanismにまたは同時に強度的に危険な状態になる場合が予想される．そのため，断面力のような局所的な情報を用いた方法では，特定の部材についてのみ断面力を抑制し，それによって他の部材の強度的危険性が増大してしまう可能性が生じる．したがってこのような場合には，系全体としての負担を軽減するような姿勢変更が必要であり，これには機構全体に蓄積される全ひずみエネルギの抑制が効果的であると考える．
以上のように，断面力と全ひずみエネルギの 2 つの構造力学的パラメータをそれぞれ個別に用いて動作計画を行い，その特徴について考察した．その結果，局所的な情報と全体的な情報はその用途によって使い分けるべきであり，例えば，機構の強度的危険性は局所的な情報である断面力により監視し，危険性が判定された場合には，全体的な情報である全ひずみエネルギを抑制するべきであることが確認された．

3. 直立状態維持を目標とした動作計画

本章では，直立状態維持を目標とした 3 リンクマニ
2.1 節と同様な条件で解析を実施し，得られた動作計ピュレータに対し，その先端に荷重を加えた場合の動作計画を実施した．モデル化は 2.1 節と同様とする．外画を Fig.6 に示す．ただし，(ky, tky)=(0.8,0.3) と設定し，力の付加条件は水平方向のみに 100 [N] を与えた場合α=0.01 とした．図から分かるように，従来の方法で得られた動作計画 (Fig.3 参照) とよく似た結果となった． (case 1), 水平方向 100 [N] と同時に垂直方向にも 80[N]を与えた場合(case 2)とした．
これは，今回のタスクが比較的単純で，常にマニピュ(ky, kU)=(0.7,0.64)と設定した場合に得られた動作計
レータの根元リンクに最も大きな負担がかかるため，画を Fig.8 に示す．水平方向のみに荷重を与えた場合
全ひずみエネルギおよび断面力の抑制のどちらを目標(case 1)では，初めに，大きく外力方向に沿って姿勢をにしても，根元部分の負担を軽減する方向に姿勢を変変更して全ひずみエネルギの抑制を図っている．一方，化させるという点で同じだからであると考えられる．
水平方向と垂直方向に同時に荷重を与えた場合(case 2)
一方で，荷重条件または機構形態がそれほど単純ではには，マニピュレータの先端リンクを荷重の方向に向ない例として，Fig.7 に示すようなリンク系で 2 つのマかって傾けるように姿勢変更が行われた．これは，荷ニピュレータが協調して作業を行う場合を考える．こ重方向に対して有効な軸剛性を利用し，ひずみエネルの場合には，先の例のように常にある部分だけに局所ギを抑制しているものと考えられる．このように，初的に負担がかかるという状態は稀で，複数箇所が交互めに荷重の大きさと方向に応じた姿勢変更を行い，そZ-coordinate [m]Z-coordinate [m]0.40.4FF0.20.2-0.1

00.10.2

X-coordinate [m](1) Case 1

0.3150100500-50-100-15000FXFYFZ200

External force [N]Mass container
Force sensor

0.6

0.6

-0.2 -0.1

00.1

0100

X-coordinate [m](2) Case 2

200

Steps

0.2

Fig.9 3-link manipulator usedin the experiments

Fig.10 External force detected byforce sensor

Fig.8 Obtained motion plan (ky = 0.7，kU = 0.64)

Fの後，軌道修正を繰り返しながら徐々に目標姿勢へ近づけていく動作軌道が得られた．

4. 実験
本研究では，マニピュレータに加わる外力を検出するための 3 軸力覚センサを実機に搭載し，本動作計画アルゴリズムとの間のインターフェースを作成した．
力覚センサには BL AUTOTEC 社製ビーエル・NANOセンサ(2.5/2)を使用し，Fig.9 に示すように，マニピュレータ先端にマスコンテナと共に取り付けた．そして，時々刻々と変化する外力に対して実時間で動作計画を実施し，その動作過程を確認した．マニピュレータのモデル化は前章と同様とした．
本実験では，(ky, kU)=(0.7,0.64)と設定し，直立状態維持を目標とするマニピュレータに対し，その先端に斜め下方向へ荷重を加えた(前章 case 2 と同様)．なお，本
実験ではその動作過程の確認を目的とするため，実際には大きな荷重を加えずに，力覚センサから得られた値を 100 倍して外力値とした．実験において，力覚センサで検出された外力値を Fig.10 に示す．また，Fig.11には，(a)，(b)，(c)，(d)の順に，マニピュレータが姿勢を変更していく様子を示す．
荷重を加えると，Fig.11(b)のように先端リンクを荷重方向に向かって傾け，軸剛性を利用して機構の負担を軽減する姿勢をとった．そして，ひずみエネルギを抑制した安全な姿勢が得られると，腕を引くようにしてマニピュレータの先端を目標位置へと近づけた(Fig.11(ｃ))．その後も荷重を加え続けたが大きな姿勢変更は行われず，Fig.11(d)のように荷重に応じて微妙に先端リンクの傾きを調節し，柔軟に姿勢を変化させる様子が観察された．

5. まとめ本研究では，機構の強度的危険性を考慮した動作計画において用いられる 2 つの構造力学的パラメータ，断面力と全ひずみエネルギの果たす役割について考察するため，個別のアルゴリズムについて検証した．また，2 つの構造力学的パラメータを導入したアルゴリズ
ムを実機に適用し，力覚センサからの入力に対する動作実験を行った結果，解析と同様な動作過程が観察さZX

(a)

(b)

(c)
(d)
Fig.11 A motion of the 3-link manipulator

れた．実時間で変動する外力に応じた動作が得られた
ことから，本動作計画法とインターフェースの信頼性が確認されたものと思われる．
今後は，より効率的な収束計算法の導入による短時間処理可能なアルゴリズムの開発，および動的効果の考慮などを行っていく予定である．

参 考 文 献[1] 田中良幸, 辻敏夫, 金子真:“人間の上肢運動を模擬したロボットの軌道生成法”, 日本ロボット学会誌, Vol.18,
No.5, pp.699～705, 2000.
[2] 加島正, 石動善久:“ヒトの運動軌道を目指したマニピュレータの軌道計画”, 日本ロボット学会誌, Vol.16, No.8,
pp.1131～1137. 1998.
[3] 水内郁夫, 松木健, 稲葉雅幸, 井上博充:“柔軟構造を含む人間型ロボットのシミュレーション環境と実ロボットの動作生成への適応”, 日本機械学会ロボティクス・メカ
トロニクス講演会‘99 講演論文集, 2A1-47-085, 2000.
[4] 磯部大吾郎, 石井悠一郎:“構造強度に着目したロボット機構の姿勢決定および動作計画”, 日本ロボット学会誌,
Vol.22, No.1, pp.75～82. 2004.