Microsoft PowerPoint - ロボットの運動制御forUpload'C6H [互換モード]

Size: px

Start display at page:

Download "Microsoft PowerPoint - ロボットの運動制御forUpload'C6H [互換モード]"

きみのしんめいこ
5 years ago
Views:

1 ロボットの運動制御ロボットの行動計画軌道計画軌道制御産業用ロボットの制御方式 -PP 制御と CP 制御ーロボット作業のプログラミング動作コマンドと補間制御動力学 - ラグランジュ法による運動方程式の導出 - 位置軌道制御 - 計算トルク法と PD フィードバックー力制御 - インピーダンス制御ー

2 行動計画軌道計画軌道制御行動計画 ( 作業計画 ) 動作計画何を行うかを決めること. ゴロを捕球して塁へ送球する, コンベヤ上を流れるボルトを掴んで部品 A を固定するのような複雑な行動 ( 作業 ) 内容を決めることから, 転がるボールの.2 秒後の位置へハンド先端を移動させる, 把持したボルトをネジ穴へ挿入する程度の動作を決めるレベル, 更に単純な動作を決めることまで様々な内容の決定. 軌道計画目的の動作を実現するためにハンド先端 (or 各関節角 ) を時間と共にどのように変化させるべきかを決めること. ボール位置 ( あるいはネジ穴へ ) にどんな速度でどんな軌跡を描いてハンド先端を移動させるか決めること. ロボットマニピュレータが関節を N 個もっていれば,θθ k (), 0 ( : 動作終了時刻 ), k=,2, N を決めることになる. 軌道制御計画された軌道 θ k (), 0, k=,2, N を実現するためにアクチュエータをどのように制御するか. 各関節がそれぞれつずつのアクチュエータで制御されているとすれば,N 個の制御入力 u(), k=,2, N を決めなければならない. PID 制御, 最適制御, ニューロ制御, ファジィ制御など

3 産業用ロボットの制御方式 P P 制御 (Pon o Pon Conrol) 終点のみを指定して点から点へ移動する制御 S θ 2 目標点 ( 終点 ) P ( x, y ) θ S PS( xs, ys) θ 2 現在位置 θ ( 始点 ) ( 順 ) 運動学ベース関節角座標系 θ2 ( θ, ) ( x, y ) 逆運動学 (Jon 座標系 ) 作業座標系 (Base 座標系 /World 座標系 ) 操作者 ( x, y ) 目標点座標 ( θ θ, 2 ) 逆運動学 θ θ 目標関節角 Δ θ = θ θ Δ θ = θ θ 2 S S メモリ ( Δθ, Δ θ ) 目標駆動角 τ 制御則 (ex. 台形則 ) () = f(, Δθ) τ () = f (, Δ θ ) τ τ V (, ()) V (, ()) 2 2 モータ特性関節トルクモータ印加電圧

4 PP 制御と CP 制御 P S P Pon o Pon Conrol Connuous Pah Conrol P S P 関節角補間直線補間円弧補間 P S P 直線補間の実際関節角補間の PP 制御 P S P M 2 数 msec P M P3 数 msec M 中間目標点 P P 4 M 数 msec 移動距離の短い関節角補間 PP 制御を繰返すことで擬似的な直線補間を実現

5 演習関節角補間によるマニピュレータ手先軌道初期姿勢からリンク角 θ が次式にしたがって変化する時, 手先位置はどのように移動するかその軌跡を示せ. ただしリンク, 2 の長さをそれぞれ 3, 2 とする. (=0.4, 0.8,.2,.6, 2.0 の時の手先位置を計算プロットして結ぶ ) () 4 2() 2 θ(), 02 2( ) ( ) 初期姿勢

6 演習マニピュレータ手先軌道の計算答え 3 2 3cos 2cos sn 2sn 4 4 x ( ) 3cos 2cos( ), 0 2 y () 3sn 2sn( ) 3 x(0.4) 3cos 2cos y(0.4) 3sn 2sn ( ) x(0.8) 3cos 2cos y(0.8) 3sn 2sn x(.2) 3cos 2cos y(.2) 3sn 2sn x(.6) 3cos 2cos y(.6) 3sn 2sn x(2.0) 3cos 2cos y(2.0) 3sn 2sn 2 2 2( ) ( )

7 ロボット言語コマンド三菱電機 RV-M2 の例コマンド ( 読み方 ) N NW (NeW) SP (Speed) L (ool) MJ (Move Jon) I (Ime) 機能ロボットの原点出しを行うプログラム及びポジションを全て消去するロボットの動作速度を設定する X-Y 平面内のツールの長さを設定する各関節を指定した角度だけ回転させる指定した時間だけ動作を停止する PD (Poson Defne) 指定したポジションの座標データを設定する MO (MOve) GO (Grp Open) GC (Grp Close) DW ED (EnD) PDで指定したポジションに関節補間で移動させるグリップを開くグリップを閉じる指定した距離だけ関節補間で移動するプログラムを終了する 5 自由度垂直多関節ロボットマニピュレータ RV-M2

8 各関節を指定した駆動角だけ回転させて, ハンド先端を順次移動させるプログラムメモリのクリア #nclude<osream.h> 動作速度の設定 #nclude<sdo.h> (20 段階の5) #nclude RS_com.h n man(n arge, char *argv[]) { RS_Com rs; rs.rs_open(); rs.send( NW n ); 作業プログラム例 NW SP 5 NW SP 5 MJ 33.4, 23.5,-48.6,82.,0 MJ 4.2, 27.5, 36.2, 8.9,0 MJ 25.2,-5.8, 2.4, 3.3,0 MJ コマンドで 5 関節をそれぞれ (); = 48.6, = 36.2, = } Δθ Δθ Δθ だけ回転させる. rs.send( Send( SPSP 5 n ); //////////////////////////////////////////////////////////////////// rs.send( MJ 33.4,23.5,-48.6,82.,0 n ); rs.send( Send( MJ n ); 4.2,27.5,36.2,8.9,0 n rs.send( MJ 25.2,-5.8,2.4,3.3,0 n ); //////////////////////////////////////////////////////////////////// rs.flush(); rs.rs_close(); reurn(0); プログラムをPCからロボットコントローラへ転送して実行させるC 言語プログラム

9 作業プログラム例 2 予め位置座標を指定しておき, ハンド先端を NW メモリのクリア SP 5 動作速度の設定 L 223 ツール長 (223mm) 設定 NW SP 5 L 223 PD334,33.4, ,-48.6,82.,0 PD 2,4.2, 27.5, 36.2, 8.9,0 MC,2 MC コマンドで 5 関節を順次順次それらの点へ直線補間で移動させる #nclude<osream.h> #nclude<sdo.h> #nclude RS_com.h n man(n arge, char *argv[]) { RS_Com rs; rs.rs_open(); rs.send( NW n ); rs.send( SP 5 n ); rs.send( L 223 n ); //////////////////////////////////////////////////////////////////// rs.send( PD,33.4,23.5,-48.6,82.,0 n ); rs.send( PD d( PD2, ,27.5,36.2,8.9,0 n ); rs.send( MC,2 n ); //////////////////////////////////////////////////////////////////// rs.flush(); rs.rs_close(); reurn(0); 2 θ = 48.6, θ = 36.2 } の姿勢にする. プログラムをPCからロボットコントローラへ転送して実行させるC 言語プログラム

10 補間制御コマンドオムロン ORL-2 MOVE と PAH の例 MOVE L : ロボットの手先を現在位置から目標点 Pへ直線補間で移動させる ( 目標点で停止する ) MOVE C : ロボットの手先を現在位置から中間点 Pを通って, 目標点 P2 へ円弧補間で移動させる ( 目標点で停止 ) PAH L : ロボットの手先を目標点 P の近傍を通過するように直線補間で移動させる ( 目標点で停止しない ) PAH C : ロボットの手先を中間点 P を通って, 目標点 P2 の近傍を通過するよう円弧補間で移動させる ( 目標点で停止しない ) MOVE L, P 現在位置 PAH L, P MOVE P2 現在位置 P P P2 MOVE C, P, P2 現在位置 PAH C, P, P2 MOVE P3 現在位置 P P0 P3 P P2 円を描かせるには? P P2 P3 P2

11 CP 制御の軌道生成 x (), x& (), && x () ( ) r r r 時々刻々の目標ハンド先端軌道時々刻々逆運動学を解く θ () θ& θ&& (, ( ), ( )) r r r 時々刻々の目標関節角軌道 ( ) θ () = Nθ () = NΛ x () mr r r mr() = N r() = N r () x Λ: 運動学関数 = Λ ( θ ) x l cosθ + l θ& θ& J x & () () & () θ&& () = Nθ&& () = NJ x () Jθ & & () (&& ) mr r r r CP 動作は PP 動作より遅い & ( 除特異点 ) J = 0 r cos( θ + θ ) 2 2 (, 2) y = Λ θ θ = lsnθ+ l2sn( θ+ θ2) x& = J θ r 両辺を微分 && x () = Jθ & & () + Jθ&& () r r r θ&& J && x r() = r() ヤコビ行列の定義

12 ヤコビ行列を用いた逆運動学の解法逆運動学を解くのは容易ではない. リンク間にオフセットを付けると解が求まらないことも多い. ニュートンラフソン法に基づく, 逆ヤコビ行列を用いた逆運動学の解法ニュートンラフソン法の繰り返し計算 θ θ KJ θ x x L r = r + ( r )( r r ), =,2, + K: チューニングゲイン収束値 θ r を逆運動学の解 θ r として採用繰り返し不要現在位置と目標位置が極めて近い θ = θ + J θ x x ( )( ) r r r +

13 ロボットの動力学ロボットの運動エネルギーと位置エネルギー U 2 2 mv I ω mv v ω Iω ラグランジュ方程式 d による導出 d v v mgh m g r0, n = ( U ) ( U ) q& q = ロボットの動力学 : ロボットの動特性を定める運動方程式 Hqq+hqq+Gq ( )&& (, &) ( ) =τ τ C ロボット制御ボッ制御. 軌跡制御 ( 軌道制御 ): ロボット ( の手先 ) を望ましい軌跡に追従させるような制御 2. 力制御 : ロボットの手先が作業対象物に対して望ましい力を加えるような制御

14 Lagrange 方程式の考え方 d ( U) ( U) d x& x 2 mv 2 mx = f x から 2 2 = = & v mx && = f mg f ( U) m = = 0, = mx& g ( U) x x& = mx& U = mgx U U = mg,, = 0 x & x O x & x mg を導く d ( U) = mx && d x& d ( U) ( U) = d x& x mx && + mg = f

15 運動エネルギーと位置エネルギー mgh 運動エネルギーと位置エネルギー運動方程式 v x 2 並進運動エネルギー mv v mv v = ( vx vy vz) vy 2 2 vz 2 回転運動エネルギー ω Iω Iω 2 2 v 2 Inera = ρ r dv V ロボット全体の運動エネルギー I : 慣性モーメント n ( ( ) ( ) ( ) ( ) = m ) q& J L J L q& + q& J A IJ A q& 2 = θ& ( ) ( ) θ& ( ) q& = v C = J L q& = J L, ω = J A q& θ& θ& 2 並進速度ベクトル 2 = q& H q& H : 慣性行列 2 n = m g r ロボット全体の位置エネルギー 0, U = C = 回転速度ベクトル

16 Lagrange 方程式による運動方程式の導出 ( U ) ( U ) d = τ, =,2, L, n d q& q d L L = d q& q n n n τ, L = U : n q& Hq&, U m = = 2 = j j jk k j j= j= k = ラグランジュ関数 g r0, C 2 H q&& + h q& q& + G = τ, =,2, L, n ( 7.6) 慣性力遠心力コリオリ力重力外力 ( mx && + ( dx& 2 ) + mg = f ) に対応ただしベクトル行列でつにまとめると H H h G m n j jk ( j) jk =, = j g J L qk 2 q j = Hqq+hqq+Gq=τ ( )&& (,&) ( )

17 ロボットの位置軌道制御ロボット ( 手先位置 ) がある軌跡 p d () = ( x d (), y d (), z d () ) に追従するような制御系の構成法手先方向も追従させる場合軌跡として q =(x d () x d (), y d (), z d (), θ x (), θ y (), θ z ()) を考える基準座標系上で表現された手先位置 ( p=(x, y, z) R 3 ) や姿勢をもとに構成する方法関節座標系 ( 関節角ベクトル θ p R N ) をもとに構成する方法関節座標系 ( 関節角ベクトル θ R N ) をもとに構成する方法逆運動学目標関節角軌跡目標手先軌跡 p ( ) = ( x ( ), y ( ), z ( )) θ ( ) = ( θ ( ), θ ( ), θ ( ) Lθ ( )) d d d d d d 2d 3d Nd 運動学モデル動力学モデル H ( θθ+hθθ )&& (, &) +G( θ ) = τ

18 ロボットの軌跡制御 Hqq+hqq+Gq=τ ( )&& (,&) ( ) 非線形システム Mq && + Dq & + Kq = τ 線形システム ( 機械振動系 ) 制御しやすいトルク計算法線形システムへ変換線形システムを得るための予備的な非線形フィードバック τ =Hˆ( qu+hqq ) ˆ(, &) +Gq ˆ( ) ( H,h,G ˆ ˆ ˆ は H,h,G の推定値 ) Hˆ H, hˆ h, Gˆ G Hq&& + h + G = Hu ˆ + hˆ + Gˆ Hu + h + G Hq ( )( q&& - u=0 ) && q=u && q= u は Iq && + Oq & + Oq = u 特殊な形をした線形システム線形システムに対する制御理論を使える

19 PD フィードバック制御 q && = u に対して線形フィードバック制御を行う所望のq() qd() u() = q&& d() q&& () = q&& d() desred 実際にはモデルは誤差を含むのでは誤差を含むの理想的な加速度を生じるような力を制御入力として加える d モデルや初期条件が正確なら q() q () を実現できる && d V( & d ) P( d ) P V u=q + K q q + K q q q&& = q&& d + KV( q& d q) + KV( qd q) e& e e&& + K e& + K e= 0 V P e () が 0 に収束するようにK V, K P を決めれば q() q () が実現する d u= K e K e& PD 制御 2 s I + s K K = V P 0 の特性根の実数部が負

20 ロボットの力制御ロボットの作業 : 手先軌跡だけでなく, 手先が対象物に対して加える力の制御が必要 Ex. バリ取り, 研磨, 組み立て [] インピーダンス制御外力に対して望ましいインピーダンス特性をもつように制御入力を決める [2] ハイブリッド制御位置制御と力制御を同時に行うのに, 誤差選択行列を用いて必要情報を抽出する

21 インピ - ダンス制御システムの入出力特性が望ましいイン v () R L () C () v () 0 d() + + () o C () d d C = = d() q () = d () q () = (), q () = d () + &&() + () = (), () o = () R() L () d v (), v ピーダンスをもつように入力を決める制御 2 次遅れ系のステップ応答 ω Gs () = n s ςω 2 ns+ ωn q y () 2 ζ 0.7の理想的応答 u () = & && y () = L [ Gsus ( ) ( )] Rq & Lq C q v v C q 望ましい応答を実現する理想ラプラス変換的なω n,ζζ がある Rsqs Lsqs C qs v s v s C qs 伝達関数 v ( ) o() s LC G s = = () 2 = v 2 R s LCs + LRs+ s + s+ C LC 2 () + () + () = (), 0() = () インピーダンス Z = R + ( ωl ) ωc 2 2 望ましい応答を実現する微分方程式の係数望ましい応答を実現するインピーダンス (L,R,C の組合せ ) がある

22 ロボットのインピーダンス制御ロボットに作用した外力と, それにより生じる変位量との関係が, 望ましいバネ - マス - ダンパ系と等しくなるよう, ロボット駆動力を与える制御システムの入出力特性が望ましいインピーダンスをもつように入力を決める制御 mx&& = (7.33) 例 f f e 望ましいインピーダンス特性 ( 入出力特性 ) m 0 f f e x d md( && x && xd) + dd( x& x& d) + kd( x xd) = fe xd : Consan x& d = 0, && xd = 0 mx && + dx& + k ( x x ) = f d d d d e インピーダンス制御 ( ) (7.34) ( ˆm :mの推定値) f = + mˆ f ˆ ˆ e m ddx m kd( x xd) md md md & (7.35) x

23 インピーダンス制御 - 多自由度ロボットへの拡張 - (7.35) を m= ˆm として (7.33) に代入 ( ) mx && = + m fe m d & dx m k d ( x xd ) f md md md mm d&& x = mf md d x& mk d ( x x d ) e mx && + dx& + k( x x) = f d d d d e 望ましい入出力特性が実現 e 多自由度ロボットシステムに拡張 τ = J F ヤコビアン行列運動方程式 Hp&& p+ hp + Gp = F Fe (7.39) Md&& p+ Dd p& + Kd( p pd) = Fe ˆ ˆ F = ( I + H Md ) F e H Md D d p& ˆ H ˆ ˆ pm K ( p p ) + hp + G p (7.45) 所望インピーダンス制御則 p p d d d (7.44)

Microsoft PowerPoint - ロボットの運動学forUpload'C5Q [互換モード]

Microsoft PowerPoint - ロボットの運動学forUpload'C5Q [互換モード] ロボットの運動学順運動学とは座標系の回転と並進同次座標変換行列 Denavit-Hartenberg の表記法多関節ロボットの順運動学レポート課題 & 中間試験について逆運動学とはヤコビアン行列運動方程式 ( 微分方程式 ) ロボットの運動学動力学 Equation of motion f ( ( t), ( t), ( t)) τ( t) 姿勢 ( 関節角の組合せ ) Posture