第 55 回自動制御連合講演会 2012 年 11 月 17 日，18 日　京都大学 2H106 拮抗型腱駆動機構を有する冗長関節ロボットの非ヤコビ制御 ○加藤亮祐井上貴浩 (岡山県大）平井慎一 (立命館大) Non-Jacobian Control of A Redundant Articulated Robot having Antagonistic Tendon-driven Mechanism. ∗R.Kato, T.Inoue (Okayama Pref. Univ.) and S.Hirai(Ritsumeikan Univ.) Abstract– This paper proposes a control law of tip position control in the two dimensional vertical plane of a redundant articulated robot having antagonistic tendon-driven mechanism. This robot consists of six tendons, springs inserted between the tendons, motors, and three links. Proposed control law does not include any apparent compensator for the gravity, and not contain the Jacobian matrices. We conduct simulations and three experimental tests using the proposed control law. We try a swing-up motion and horizontal posture control. Finally,we establish the eﬃciency of this control law by the simulation and experiment. Key Words: Tendon-driven,Jacobian,Redundant Antagonistic mechanism 1 はじめに θ3 X 腱駆動機構ではアームからアクチュエータをロボットのベース部に移すことにより，軽量化が容易であることや，腱の弾性によりアームに柔軟性を持たせることができるため，様々な分野のロボットへの応用が期待され研究も数多くされてきた 1)2)3) ．また人間と協調して作業を行うことができるロボットを生み出すために，コンプライアンス性を獲得するための研究も多数されている 4) ．人間に違和感を感じさせない軌道生成法についても研究が行われている 5) ．ヒト上肢のリーチング運動の際の速度推移はベル型と呼ばれるピークを一つもつ速度波形を示す．その速度波形を作り出すことによりロボットの親和性を高め，ロボットに対する違和感を無くすことができる．それらの研究で使用されるロボットアームの制御は位置，力制御などが知られる．特に位置制御の場合においてロボットアームが冗長関節であれば，順運動学は求まるが，逆運動学はマニピュレータの関節角度の解が無限に存在する不良設定問題により一意に定まらない 6)7) ．この問題を避けるために様々な研究が行われているが，いずれもヤコビ行列を使用するため，根本的な問題の解決とはならない．一方，本稿で提案する制御則は冗長アームの手先位置決め制御にもかかわらず動力学補償や線形化補償を行っていないためヤコビ行列を必要としない．以下本稿の流れを記述する．本稿ではヒト上肢の拮抗筋構造を模倣した拮抗型腱駆動ロボットアームの設計を行い，モデル化を行う 8)9) ．次に，拮抗構造に適したヤコビ行列を使用しない手先到達運動制御則を提案し，その制御則を用いた手先到達振り上げ運動の数値シミュレーションを行う．そして拮抗型腱駆動のロボットアームを製作し，提案制御則を用いて手先到達振り上げ運動とアーム水平姿勢の制御を行なう．以下，各章の概要について説明する． 2 章で 3 リンク拮抗型腱駆動冗長ロボットアームのモデル化を行う．3 章では，まず，拮抗型腱駆動ロボットで手先到達運動が可能となる制御則の提案を行う．そして手先到達運動，ここでは振り上げ運動のシミュレーションを行い提案制御則の有効性を明らかにする．4 章では製作した腱駆動ロボットアームを用いて振り上げ運動の実験とアーム水平姿勢の制御を行う．5 章で結 -Y gravity θ1 θ2 (a) configuration of the arm motors nonlinear spirngs idler idler idler final link paired wires for moving final link (b) wire-driven structure Fig. 1: A three degrees-of-freedom redundant arm in the gravity. 果に対する考察を述べる．モデル化 2 2.1 ロボットアームの概要本稿で取り扱うロボットアームの概略図は Fig. 1(a) であり，以下のような力学的・機構学的特徴を有する． • • • • 重力下での垂直 2 次元平面内運動 3 リンク冗長アーム拮抗腱駆動機構腱への非線形ばねの挿入 Fig. 1(a) のようにアームに沿って腱を取り付けており，それを DC モータで引いてアームを駆動させる．図中では第 3 リンクの腱のみ描かれているが，実際には第 1,2 リンクに対しても腱が取り付けてあり，それぞれの腱を個別の DC モータで引っ張る．またすべての腱には腱と DC モータとの間に非線形バネが挿入されており，アームに対する衝撃を吸収する働きを果たす． 2.2 運動方程式の導出本稿で取り扱う 3 リンク冗長アームの模式図を Fig. 2 に示す．ここで，システム全体の運動エネルギーを K ，バネの伸びの弾性エネルギーを Pei (i：第 i リンクに関与するバネ)，重力によるポテンシャルエネルギーを Pg とすると 3 リンクアームモデルのラグランジアン L は Eq. (1) となる． 1402 Table 1: Mechanical parameters and others X R -Y θ1 τc Kp1 Kp2 Kp3 Kd2 Kı1 L1 pc1 pc3 θ3 R θ2 pc2 L3 R 0.2 [Nm] 1.4 1.0 3.0 1.0 0.003 y τb1 τb2 τb3 Kd1 Kd3 Kı2 τc +1.0 [Nm] τc +1.0 [Nm] τc [Nm] 1.0 2.0 0.0032 y y target=(0.6,0.0) x x x L2 Fig. 2: A model of the three-links arm L=K− 3 ∑ Pei − Pg . start=(0.0,-0.9) (1) Fig. 3: Swing-up motion i=1 また，各関節とアクチュエータ軸の粘性項を Eq. (1) に加えることで，運動方程式 Eq. (2) を得ることができる． d ∂L ∂L − + D q˙ = u. dt ∂ q˙ ∂ q˙ (2) ここで，q は各リンク関節とモータ回転角を含めた一般化座標ベクトルであり，u は各アクチュエータへの制御入力である．また，D は粘性対角行列を意味する．シミュレーション 2.2 節で求めた運動方程式を基に振り上げ運動のシミュレーションを行う．シミュレーションにおいては Table 1 のメカニカルパラメータを用いた．ここで，ζ は制御則とシミュレーション制御則 0.3 0.3 0.1 0.1 -0.1 -0.1 y[m] y[m] アームに対して拮抗的に DC モータを配置し，Fig. 2 の各関節において反時計回りに駆動するアクチュエータトルクを根元から先端にかけて (u1 , u3 , u5 ) とし，時計回りを同様に (u2 , u4 , u6 ) とする. 振り上げ動作を実現するための制御則として，関節角を操作量とする PD 制御を行うため，アクチュエータの制御入力は Eq. (3) となる．   u1 = −Kp1 (θ1 − θ1d ) − Kd1 θ˙1 + τb1    u = −K (θ − θd ) − K θ˙ + τ d2 2 b2 3 p2 2 2 (3) d ˙  u = −K (θ − θ ) − K θ + τ p3 3 d3 3 b3  3  5  u = u = u = τ (const.) 2 4 6 c target -0.3 -0.7 start -0.9 0 0.2 0.4 x[m] -0.9 0 0.6 これは，ステップ状の指令タスク（(x, y) → (xd , y d )）における時間経過ごとの偏差の蓄積から関節角目標を生成することを意味する．したがって，上記 PD 制御則から分かるように，関節空間においては外形上軌道追従制御に相当する 0.2 0.6 (b) desired xy-locus -0.1 -0.3 -0.5 -0.7 4 8 12 time[s] 16 20 -0.9 0 20 90 80 70 60 50 40 30 20 10 0 -10 0 4 (c) x-locus 90 80 70 60 50 40 30 20 10 0 -10 0 0.4 x[m] 0.1 y[m] x[m] θ2 θ3 θ1 4 8 8 12 time[s] 16 12 16 time [s] (e) angle locus d θ2 d θ3 d 4 8 12 time [s] θ1 16 (f) desired angle locus Fig. 4: Simulation results of a successful swing-up motion of the arm 1403 20 (d) y-locus d Eq. (3) において，第 i リンクの比例・微分ゲインをそれぞれ Kpi , Kdi とし，関節角目標を θid とする．また，制御入力に重畳するバイアストルクを τbi と定義する．また，本稿での指令タスクは関節角度制御ではなくリンク先端の位置決め制御であるため，上記制御則内における目標関節角を積分ゲイン Kii を用いて次式のように与える．  ∫ d  (x − xd )dt, θ1 = −Ki1 ∫ (4) θ2d = Ki2 (y − y d )dt,   d θ3 = const. start -0.7 (a) xy-locus 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 target -0.3 -0.5 -0.5 θ [deg] 3.1 3.2 θ [deg] 3 この制御則の特徴として挙げられるのが，ヤコビ行列に起因する，ランク落ちにより制御不能となる特異姿勢を問題としない点である．また通常は冗長アームの場合，逆運動学が求まらない不良設定問題が発生するが，第 3 リンクを一定角度に固定することによって，アームの冗長性を打ち消し，不良設定問題を回避することができる． 20 Table 2: Apparatus and others used in the experiment Microcomputer Encoder Motor MotorDriver Spring used for 1link used for 2link used for 3link LEPRACAUN(SH4-7785) (GeneralRobotix,Inc.) E6J-CWZ1C(OMRON) RE-max21(Maxon Motor) A-max19(Maxon Motor) A-max16(Maxon Motor) JW-143-2(Okatech) 21.57[N/mm] 18.63[N/mm] 9.22 [N/mm] Fig. 5: A three-linked arm in experiment. 各関節プーリの粘性係数であり，ζm は各アクチュエータの粘性係数である．与えるタスクはアーム垂下姿勢時の先端座標 (x, y)=(0.0[m],-0.9[m]) から (x, y)=(0.6[m],0.0[m]) に先端を振り上げる動作である．(Fig. 3 参照)．なお，第 3 リンクは 40 °を目標角度とする． Fig. 4(a) は，xy 平面上におけるアーム先端の軌跡である．Fig. 4(b) は，Eq. (4) においてリアルタイムで生成される各アームの目標角度から算出した軌道を示す． Fig. 4(c)，(d) は x, y それぞれのアーム先端座標の時間軌道である．Fig. 4(a),(c) を見ると，一度 x 方向に振れてから目標点に到達したことがわかる．Fig. 4(e)， (f) は各アームの角度軌道と目標角度軌道を示す．目標角度と実際のアームの角度は大きく違うことがわかる．しかし，目標点にアーム先端は到達したので，Eq. (4) によって生成される角度目標は仮想的なものであるといえる． 4 実験結果 4.1 振り上げ動作 Fig. 5 が製作したロボットアームである．アームの上方に拮抗的にモータが 3 つずつ取り付けてあり，プーリを介してアームとモータを腱で結んでいる．またアームの関節はベアリングが挿入されており，関節の回転摩擦はほぼゼロに等しい．また，今回シミュレーションでは非線形バネを用いたが，実験では線形バネを使用して実験を行った． 3 章で記述した制御則で振り上げ動作の実験を行った (Fig. 6 参照)．ロボットアームの製作に使用したアクチュエータやマイコンなどを Table 2 に示す．各リンク長はシミュレーションと同じで各 0.3[m] である．まず，xy 平面上におけるアーム先端の軌跡を Fig. 7(a) に示す．目標付近で振動が発生しているが，最終的には先端が目標座標に到達した．また，Fig. 7(b) は，目標軌道であるが，ほぼ追従するように先端軌道が推移したことがわかる. 次に，x, y それぞれのアーム先端座標の時間軌道を Fig. 7(c)，(d) に示す．x 座標に関しては，Fig. 7(a) からもわかるように一度オーバーシュートしてから約 6 秒で目標に到達した．また，y 座標についても同様に約 6 秒で到達した．Fig. 7(e)，(f) は各アームの角度軌道と目標角度軌道を示した．目標 Fig. 6: An experiment of the swing-up motion 角度と各アームの角度第 3 リンクを除いて，少ないながらも偏差が存在することがわかる． 4.2 水平姿勢シミュレーションでは行なっていないが，アームを水平姿勢にする制御の実験を行った (Fig. 9 参照)．タスクとしてはアーム垂下姿勢時の先端座標 (x, y)=(0.0,-0.9) から (x, y)=(0.9,0.0) に先端を到達させる動作である． (Fig. 8 参照) 第 3 リンクは 0 °を目標角度とする． Fig. 10(a) はアーム先端の xy 平面上での軌跡を示す． Fig. 10(b) は，目標軌道であるが，(a) とほぼ一致することがわかる．次に,x, y それぞれのアーム先端座標の時間軌道を Fig. 10(c)，(d) に示す．Fig. 10(a)，Fig. 10(c)， (d) から x 方向は比較的振動が少ないが，y 方向には振動が発生したことがわかる．最後に Fig. 10(e)，(f) は各アームの角度軌道と目標角度軌道を示す． 4.3 振り上げ動作と水平姿勢最後にアーム振り上げ動作完了後，水平姿勢にする実験を行った．Fig. 11 は実験時の様子の連続写真である．タスクとしてはアーム垂下姿勢時の先端座標 (x, y)=(0.0,-0.9) から目標 1 (x, y)=(0.6,0.0) に先端を到達させてから，目標 2 (x, y)=(0.9,0.0) に到達させることである．目標 1 に到達後，ユーザ側で目標 2 に切り替える．まず，xy 平面上におけるアーム先端の軌跡を Fig. 12(a) に示す．目標 1 に到達後，目標 2 に到達したことがわかる．また，Fig. 12(b) は，目標軌道であるが，ほぼ追従するように先端軌道が推移したことがわかる. 次に，x, y それぞれのアーム先端座標の時間軌道を Fig. 12(c)，(d) に示す．x 座標について，目標 1 に関しては約 5 秒で，目標 2 に関しては約 13 秒でそ 1404 0.1 0.1 -0.1 target -0.3 y [m] -0.5 -0.5 start -0.9 0 0.2 0.4 x [m] -0.9 0 0.6 -0.1 Fig. 9: An experiment of the horizontal posture -0.5 4 6 8 time [s] 10 12 -0.9 0 2 4 6 8 time [s] θd [deg] 50 θ3 10 θ1 2 4 6 8 time [s] -0.5 12 -0.7 10 -0.9 0 0.2 -10 0 2 4 6 8 time [s] 10 -0.9 1.0 0 0.9 0.7 -0.1 0.5 -0.3 -0.1 0 4 start=(0.0,-0.9) 16 0.8 -0.9 20 0 4 80 80 θ1 60 40 20 θ3 -20 θ2 12 time[s] 20 16 20 d θ1 20 -20 8 16 40 0 4 12 time[s] 60 0 0 8 -40 d θ2 d θ3 0 4 8 12 time[s] 16 (f) desired angle locus Fig. 10: Experimental results of a successful horizontal posture of the arm じたと考えられる．またアーム先端の振動はバネの伸縮によって生じる．初期実験ではバネ定数が現在使用中のバネの 10 分の 1 程度のものを用いていた．その際の実験ではより大きな振動が発生していたが，現在使用中のバネ定数の高いものに取り替えることで振動は軽減された．よって，今後さらに大きなバネ定数のバネに取り替えることでさらに振動が軽減されるとかんがえられる．しかし，バネをより強くすることは同時にアームの剛性を高め，アームのやわらかさを失うことに繋がる．そこで，非線形特性をもつバネを使用することでアームの柔らかさを保ちつつ，振動も軽減できるのではと考えられる．実験において，振り上げ動作と水平姿勢ではゲインの Kp ，Kd や Ki1 , Ki2 の値を変更した．仮に，振り上げ動作のゲインで水平姿勢制御を試すと，目標には到達するものの極端に長い時間がかかってしまうか，大きな振動が発生して制御不能 1405 1.0 (d) y-locus (e) angle locus れぞれ到達したことがわかる．y 座標については目標 1 は約 6 秒，目標 2 は約 17 秒で到達したことがわかる． Fig. 12(e)，(f) は各アームの角度軌道と目標角度軌道を示す．目標角度と各アームの角度は第 3 リンクを除いて，偏差を残しながら追従していることがわかる．振り上げ動作のシミュレーションと実験，水平姿勢の実験の結果から提案制御則の有効性が示されたと考える．シミュレーションではリアルタイムに生成される目標角度と実際のアーム角度には差があったが，実験においては目標角度と偏差が存在するものの，追従する形でアーム角度が推移した．これは，実験において拮抗するモータに一定のトルクを与えた場合，駆動させる側のモータに過度の負担がかかると同時にモータパワー不足により先端が目標まで到達しなかった．そのため，実際の制御では一定トルクではなく腱が緩まない程度に調節したトルクを与えていた．そのため，シミュレーションと実験では目標角度への収束に差が生 12 time[s] 100 -40 Fig. 8: Horizontal posture 考察 8 100 x 0.6 x[m] -0.5 (c) x-locus y x 0.4 -0.7 0.1 (f) desired angle locus 0.2 (b) desired xy-locus 0.1 0.3 12 target=(0.9,0.0) 5 0.8 d θ3 d θ1 30 y x 0.6 x[m] (a) xy-locus Fig. 7: Experimental results of a successful swing-up motion of the arm y 0.4 θ 50 (e) angle locus start -0.7 d 2 10 12 target -0.3 -0.5 start 70 θ2 30 target -0.3 (d) y-locus 90 70 10 0.1 -0.1 y[m] 2 y[m] -0.7 0.1 -0.1 y[m] -0.3 90 θ[deg] 0.6 (b) desired xy-locus (c) x-locus -10 0 0.4 x [m] θ d [deg] 0 0.2 0.1 y [m] x [m] (a) xy-locus 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 start -0.7 x[m] -0.7 target -0.3 θ [deg] y [m] -0.1 20 Fig. 11: Snapshot of the combined motion of swing-up motion and horizontal posture 0.1 0.1 -0.1 target1 -0.3 y [m] y [m] -0.1 -0.5 target2 -0.7 start -0.9 0 0.2 0.4 0.6 x [m] 0.8 -0.5 -0.9 0 1 0.2 -0.5 -0.7 10 20 30 time[s] -0.9 40 0 10 20 30 40 time[s] (d) y-locus θ2 d 90 θ1 d θ2 70 θ3 θ1 50 d θ3 30 d 1 θ 10 10 -10 0 参考文献 -0.3 θ d [deg] θ [deg] 30 1 -0.1 70 50 0.8 0.1 (c) x-locus 90 0.4 0.6 x [m] (b) desired xy-locus y[m] x[m] target2 start -0.7 (a) xy-locus 1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 target1 -0.3 必要がある．今後の課題として挙げられるのは，目標点に対する最適なゲインの決定方法の確立や目標点にさらに早く到達するようにアクチュエータを変更することなどが考えられる．そのためには実験でも非線形特性を持つバネを用いることや，非線形アクチュエータを使用して実験を行うことが今後の課題として挙げられる． 10 20 time [s] 30 (e) angle locus 40 -10 0 10 d θ2 20 time [s] d θ3 30 40 (f) desired angle locus Fig. 12: Experimental results of a successful swing-up motion and horizontal posture of the arm となる．そのため，目標点によってゲインを調節する 1406 1) Gary S.Guthart and J.Kenneth Salisbury,Jr:”Intuitive Telesurgery System”,Proceeding of the IEEE International Conference on Robotics and Automation, vol.1 pp.618–621, 2000. 2) 小澤，小林，”非線形弾性腱を持つ腱駆動ロボット機構の定力制御”，日本ロボット学会誌，Vol.19，No. 3, pp.372– 379, 2001 3) H.Noborisaka,H.Kobayashi,”Design of a TendonDriven Articulated Finger-Hand Mechanism and Its Stiﬀness Adjustability,” JSME International Journal,Series C, vol.43, No.3, pp638–644 4) 岡田,”人間らしい柔らかな動きと駆動機構”，日本ロボット学会誌 Vol.17，No.6, pp.782–785, 1999 5) 田中，辻，金子，”人間の上肢運動を模擬したロボットの軌道生成法”，日本ロボット学会誌，Vol.18，No.5，pp.699– 705，2000 6) 井上，加藤，平井，”拮抗型ばね駆動モデルによる不良設定問題の解消”，日本機械学会ロボティクス・メカトロニクス講演会, 2P1-F06, 2012. 7) 井上，加藤，平井，”バックドライバビリティを有する拮抗腱駆動冗長関節ロボットの重力下での振り上げ到達運動”，日本ロボット学会誌, Vol.31, No.1, 掲載予定, 2013 8) 井上，加藤，平井，”非線形ばね要素を含んだ拮抗筋駆動指のモデリング”，第 12 回計測自動制御学会 SI 部門, pp2393–2396, 2011. 9) 加藤，井上，平井，”多関節冗長アームの重力下でのリーチング運動 ”，日本機械学会ロボティクス・メカトロニクス講演会, 2P1-G07, 2012.