知能システム論 1 (9) 201365 情報システム学研究科情報メディアシステム学専攻知能システム学講座末廣尚士
12 ロボットアームの逆運動学 ( 幾何学的 ( 解析的 ) 解法 ) 何をしたいか 手首 手先 ツールの3 次元空間での位置や姿勢から それを実現する関節角度を計算する アームソリューション アームの解とも呼ぶ 何のために たとえばビジョンで認識された物をつかむ場合 物の位置 姿勢は3 次元空間で表現されることが普通である したがってそれに必要な手首 手先 ツールの位置や姿勢アームの位置や姿勢も3 次元空間の位置 姿勢で与えられる 逆運動学が不要な制御もあるが 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 2
- 幾何学的解法 y hand y 手先の位置が hand y hand z hand のときの hand 関節角度 1, 2, 3 を求める z hand z 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 3
- 幾何学的 ( 解析的 ) 解法 1 L1 y y まず順運動学の式は L1 hand = hand cos 1 =l2h sin 2 l3h sin 2 3 cos 1 2 L1 y hand = hand sin 1 =l2hsin 2 l3hsin 2 3 sin 1 3 l3h l2h z hand =bhl1hl2hcos 2 l3hcos 2 3 z L1 l1h bh L1 hand =l2hsin 2 l3hsin 2 3 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 4
1 - の計算 1 L1 y y θ 1 = /2 y hand arctan( ) π ( <0, y<0) hand y hand =0, y0 arctan( ) ( >0) hand L1 hand = hand cos 1 L1 y hand = hand sin 1 π /2 ( =0, y>0) y hand arctan( )+π ( <0, y 0) hand y hand hand =tan 1 もしくは 不定 θ 1 =atan2( =0, y=0 y hand, hand ) 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 5
3 - の計算 z hand =bhl1hl2hcos 2 l3hcos 2 3 L1 hand =l2hsin 2 l3hsin 2 3 ここで とすると L1 X = X 2 hand = Z= z hand bh l1h 2 hand Z=l2hcos 2 l3hcos 2 3 X =l2hsin 2 l3hsin 2 3 両辺を 2 乗して加えると Z 2 =l2h 2 cos 2 2 2 l2hl3h cos 2 cos 2 3 l3h 2 cos 2 2 3 X 2 =l2h 2 sin 2 2 2 l2hl3hsin 2 sin 2 3 l3h 2 sin 2 2 3 2 y hand 3 l3h X 2 Z 2 z L1 2 l2h l1h bh X 2 Z 2 =l2h 2 l3h 2 2 l2h l3hcos 2 cos 2 3 sin 2 sin 2 3 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 6
3 - の計算 X 2 Z 2 =l2h 2 l3h 2 2 l2h l3hcos 2 cos 2 3 sin 2 sin 2 3 ここで cos =coscossin sin = 2 3 = 2 とすると X 2 Z 2 =l2h 2 l3h 2 2 l2hl3h cos 3 X 2 Z 2 l2h 2 l3h 2 =2 l2hl3h cos 3 cos 3 = X 2 Z 2 l2h 2 l3h 2 /2l2h l3h 3 =±arccos X 2 Z 2 l2h 2 l3h 2 /2 l2hl3h を思い出して 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 7
2 - の計算 3 2 3 + の場合 - の場合 3 2 ψ=arcsin(z / X 2 +Z 2 ) ϕ=arcsin( l3hsin(θ 3) X 2 +Z 2 ) 2 = /2 実はこの符号が反転するだけ =arcsin Z / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = /2 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 8
- 結果のグラフィックス表示 1 2 3 =arcsinz / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = /2 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 9
- 結果のグラフィック表示 2 2 3 =arcsin Z / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = /2 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 10
- まだ他に解がある 1 を裏に回して とを反対に曲げる 1 2 3 θ 1 =arctan2( y hand, hand ) L1 y y 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 11
- プログラム :arm3dofpy def arm_sol(self,trans) : =transvec[0] y=transvec[1] z=transvec[2] th1_1=atan2(y,) th1_2=atan2(-y, -) zz=z-selfbh-selfl1h zz_2=zz**2 _2=**2+y**2 th3=acos((_2+zz_2-selfl2h**2-selfl3h**2)/20/selfl2h/selfl3h) psi=asin(zz/sqrt(_2+zz_2)) phi=asin(selfl3h*sin(th3)/sqrt(_2+zz_2)) th2_1=pi/2-psi-phi th2_2=pi/2-psi+phi selfsolutions=[[th1_1,th2_1,th3],[th1_1,th2_2,-th3], [th1_2,-th2_1,-th3],[th1_2,-th2_2,th3]] return selfsolutions # armsolve=newinstancemethod(arm_sol,arm,arm class ) 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 12
- arm3dofpy の逆運動学解 まず env_arm3dofpy を走らせる >>> armmake_shape() >>> armmark() >>> create_env() >>> a=armsolve(bowhere(armbase)) >>> a [[-078539816339744828, 090198348365541947, 21401199701156761], [-078539816339744828, 30421034537710954, -21401199701156761], [23561944901923448, -090198348365541947, -21401199701156761], [23561944901923448, -30421034537710954, 21401199701156761]] >>> for pp in a: armset_joints(pp) sleep(1) >>> 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 13
- arm3dofpy の使い方 (2) hand が bo の場所に行くにはどうしたらよいか? hand から wrist へ座標変換でもどしてやる >>> b=armsolve(bowhere(armbase)*armwristwhere(armhand)) >>> b [[-078539816339744828, 063292249604039119, 22119557397743437], [-078539816339744828, 28448782358147344, -22119557397743437], [23561944901923448, -063292249604039119, -22119557397743437], [23561944901923448, -28448782358147344, 22119557397743437]] >>> for pp in b: armset_joints(pp) sleep(1) うまくいかない なぜ? 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 14
- arm3dofpy の使い方 (3) 実はこれは,arm_sol のプログラムの中身を変更しなくてはならないといういやらしい問題になっている th3=acos((_2+zz_2-selfl2h**2-selfl3h**2)/20/selfl2h/selfl3h) psi=asin(zz/sqrt(_2+zz_2)) phi=asin(selfl3h*sin(th3)/sqrt(_2+zz_2)) これを汎用的に処理するのは幾何的解法では難しい 後で数値解法で解決する 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 15
- 6 自由度の場合 位置だけでなく姿勢も指定できる手首に手先 3 自由度が集まった形になっている場合 位置と姿勢を分離して求めることが出来る まず手先の位置 姿勢から手首位置が決まる 手首位置について3 自由度の解を求める 手首姿勢と整合するように手先の 3 自由度を求める そうでない場合 一般には解くのが難しい 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 16
- 6 自由度アームの解の求め方の例 姿勢と先端位置が決まるとこの点が決まる ここは 3 自由度アームの求め方 根元側 3 自由度が決まると残り 3 自由度は姿勢合わせで簡単に求められる 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 17
- 例題 12:6 関節アームの逆運動学 hand wrist l h =01 l 6 =005 l 5 =01 l 4 =01 l 3 =03 手首座標系が目標座標系 target に一致するように関節角を求めよ (arm6dofpy) l 2 =04 base l 1 =005 l b =01 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 18
- 逆運動学 (1) (1)joint 5 の位置を求める 手首座標の z 軸方向 lh5+lh6(-015) の位置 def arm_sol(self,target) : selfsolutions=[] pos=targetvec z_ais=targetmatcol(2) pos=pos-((selfl5h+selfl6h)*z_ais) pos target y (selfl5h+selfl6h)*z_ais z 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 19
- 逆運動学 (2) (2)1,2,3 は 前の l3h =l3h+l4h(04) としたときの 3 関節と同様に解くことが出来る pos ここは 3 自由度アームの求め方 def arm_sol_pos(self,pos) : 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 20
- 逆運動学 (3) (3)joint 5 の回転軸 (y) は 手首 z 軸と link3z 軸に垂直 回転角はそれらの間の角度 (2 方向あることに注意 ) def arm_sol_ori(self,target,th123) : T123=selffk123(th123) zt=targetmatcol(2) z3=t123matcol(2) y T123 z z3 target y zt y5, th5 z 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 21
- 逆運動学 (4) (4)joint 4 の回転角は link5y と link3y の成す角 y T123 def arm_sol_ori(self,target,th123) : T123=selffk123(th123) zt=targetmatcol(2) z3=t123matcol(2) y5 th4 y3 z target y z 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 22
- 逆運動学 (5) (5)joint 6 の回転角は link5y と wrist(=target) y の成す角 y T123 def arm_sol_ori(self,target,th123) : T123=selffk123(th123) zt=targetmatcol(2) z3=t123matcol(2) yt y5 th6 z target y z 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 23
- 例題 12:6 関節アームの逆運動学 ( まとめ 1) hand wrist base l h =01 l 6 =005 l 5 =01 l 4 =01 l 3 =03 l 2 =04 l 1 =005 l b =01 (1)joint 5 の位置を求める 手首座標の z 軸方向 -015 の位置 (2)1-3 は l3h=l3+l4=04 としたときの 3 関節と同様に解くことが出来る (3)joint 5 の回転軸 (y) は 手首 z 軸と link3z 軸に垂直 回転角はそれらの間の角度 (2 方向あることに注意 ) 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 24
- 例題 12:6 関節アームの逆運動学 ( まとめ 2) hand wrist l h =01 l 6 =005 l 5 =01 l 4 =01 l 3 =03 l 2 =04 (4)joint 4 の回転角は link5y と link3y の成す角 (5)joint 6 の回転角は link5y と wrist y の成す角 base l 1 =005 l b =01 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 25
- arm6dofpy(1) def arm_sol_pos(self,pos) : =pos[0] y=pos[1] z=pos[2] th1_1=atan2(y,) th1_2=atan2(-y,-) lh=selfl3h+selfl4h zz=z-selfbh-selfl1h zz_2=zz**2 _2=**2+y**2 th3=acos((_2+zz_2-selfl2h**2-lh**2)/20/selfl2h/lh) psi=asin(zz/sqrt(_2+zz_2)) phi=asin(lh*sin(th3)/sqrt(_2+zz_2)) th2_1=pi/2-psi-phi th2_2=pi/2-psi+phi selfsolutions_pos=[[th1_1,th2_1,th3],[th1_1,th2_2,-th3], [th1_2,-th2_1,-th3],[th1_2,-th2_2,th3]] return selfsolutions_pos # 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 26
- arm6dofpy(2) (3)(4)(5) def fk123(self,th123): T1=FRAME(yzabc=[0,0,selfbh,0,0,th123[0]]) T2=FRAME(yzabc=[0,0,selfl1h,0,th123[1],0]) T3=FRAME(yzabc=[0,0,selfl3h+selfl4h,0,th123[2],0]) return T1*T2*T3 # def arm_sol_ori(self,target,th123) : T123=selffk123(th123) zt=targetmatcol(2) z3=t123matcol(2) return [th4,th5,th6],[th4_2,-th5,th6_2] 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 27
- arm6dofpy(3) (1) (2) def arm_sol(self,target) : selfsolutions=[] pos=targetvec z_ais=targetmatcol(2) pos=pos-((selfl5h+selfl6h)*z_ais) selfsolve_pos(pos) for th123 in selfsolutions_pos : rslt=selfsolve_ori(target,th123) selfsolutionsappend(th123+rslt[0]) selfsolutionsappend(th123+rslt[1]) return selfsolutions 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 28
- arm6dofpy の逆運動学解 まず env_arm6dofpy を走らせる >>> create_env() >>> arm <larm_w_handlinkedarm instance at 00350F5F8> >>> armsolve(bowhere(armbase)*armwristwhere(armhand)) [[-085196632717327214, 057165712277578096, 16628010732898744, 11903745009548912e-16, 090713445752413802, 22896263264165211], [-085196632717327214, 057165712277578096, 16628010732898744, -31415926535897931, -090713445752413802, -085196632717327225], [22896263264165211, -22344581960656553, 16628010732898744, -31415926535897931, 25699355308140124, 22896263264165211], [22896263264165211, -22344581960656553, 16628010732898744, 21016374975493564e-16, -25699355308140124, -085196632717327203]] >>> show_all_solutions(arm,bo) >>> 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 29
- 次回の予告 逆運動学の数値解法ニュートン法ヤコビアン 2013 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 30