PowerPoint Presentation

Size: px

Start display at page:

Download "PowerPoint Presentation"

さやいいはた
5 years ago
Views:

1 知能システム論 1 (11) 情報システム学研究科情報メディアシステム学専攻知能システム学講座末廣尚士

2 13. ロボットアームの逆運動学 ( 幾何的解法 ) 何をしたいか手首手先ツールの3 次元空間での位置や姿勢からそれを実現する関節角度を計算するアームソリューションアームの解とも呼ぶ何のためにたとえばビジョンで認識された物をつかむ場合物の位置姿勢は3 次元空間で表現されることが普通であるしたがってそれに必要な手首手先ツールの位置や姿勢アームの位置や姿勢も3 次元空間の位置姿勢で与えられる逆運動学が不要な制御もあるが 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 2

3 - 幾何学的解法 hand 手先の位置が hand hand z hand のときの hand 関節角度 1, 2, 3 を求める z hand z 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 3

4 - 幾何学的 ( 解析的 ) 解法 1 L1 まず順運動学の式は L1 hand = hand cos 1 =l2h sin 2 l3h sin 2 3 cos 1 2 L1 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 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 4

5 1 - の計算 1 これは簡単 L1 L1 hand = hand cos 1 L1 hand = hand sin 1 1 = hand arctan 0 hand hand arctan 0 hand /2 =0, 0 /2 =0, 0 hand hand =tan 1 不定 =0, = 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 5

6 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 l2hl3h cos 2 cos 2 3 l3h 2 cos X 2 =l2h 2 sin l2hl3hsin 2 sin 2 3 l3h 2 sin hand 3 l3h X 2 Z 2 z L1 2 l2h l1h bh X 2 Z 2 =l2h 2 l3h 2 2 l2hl3hcos 2 cos 2 3 sin 2 sin 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 6

7 3 - の計算 X 2 Z 2 =l2h 2 l3h 2 2 l2hl3hcos 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 を思い出して 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 7

8 2 - の計算の場合 - の場合 3 2 =arcsinz / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = /2 実はこの符号が反転するだけ =arcsinz / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = / 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 8

9 - 結果のグラフィックス表示 =arcsinz / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = / 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 9

10 - 結果のグラフィック表示 =arcsinz / X 2 Z 2 =arcsin l3hsin 3 X 2 Z 2 2 = / 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 10

11 - まだ他に解がある 1 を裏に回してとを反対に曲げる L1 1 = hand arctan 0 hand hand arctan 0 /2 hand =0, 0 /2 =0, 0 不定 =0, = 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 11

12 - プログラム :arm3dof_draft2.p def arm_sol(self,trans) : =trans.vec[0] =trans.vec[1] z=trans.vec[2] th1_1=atan2(,) th1_2=th1_1-pi zz=z-self.bh-self.l1h zz_2=zz**2 _2=**2+**2 th3=acos((_2+zz_2-self.l2h**2-self.l3h**2)/2.0/self.l2h/self.l3h) psi=asin(zz/sqrt(_2+zz_2)) phi=asin(self.l3h*sin(th3)/sqrt(_2+zz_2)) th2_1=pi/2-psi-phi th2_2=pi/2-psi+phi self.solutions=[[th1_1,th2_1,th3],[th1_1,th2_2,-th3], [th1_2,-th2_1,-th3],[th1_2,-th2_2,th3]] return self.solutions # arm.solve=new.instancemethod(arm_sol,arm,arm. class ) 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 12

13 - arm3dof_draft2.p の使い方まず env_arm3dof_draft2.p を走らせる. >>> arm.make_shape() >>> arm.mark() >>> create_env() >>> a=arm.solve(bo.where(arm.base)) >>> a [[ , , ], [ , , ], [ , , ], [ , , ]] >>> for pp in a: arm.set_joints(pp) sleep(1) >>> 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 13

14 - 6 自由度の場合位置だけでなく姿勢も指定できる手首に手先 3 自由度が集まった形になっている場合位置と姿勢を分離して求めることが出来るまず手先の位置姿勢から手首位置が決まる手首位置について3 自由度の解を求める手首姿勢と整合するように手先の 3 自由度を求めるそうでない場合一般には解くのが難しい 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 14

15 - 6 自由度アームの解の求め方の例姿勢と先端位置が決まるとこの点が決まるここは 3 自由度アームの求め方根元側 3 自由度が決まると残り 3 自由度は姿勢合わせで簡単に求められる 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 15

16 - 例題 :6 関節アームの逆運動学 hand wrist l h =0.1 l 6 =0.05 l 5 =0.1 l 4 =.01 l 3 =0.3 手首座標系が目標座標系 target に一致するように関節角を求めよ l 2 =0.5 base l 1 =0.05 l b = 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 16

17 - 逆運動学 (1) (1)joint 5 の位置を求めるハンド座標の z 軸方向 lh5+lh6(-0.15) の位置 def arm_sol(self,target) : self.solutions=[] pos=target.vec z_ais=target.mat.col(2) pos=pos-((self.l5h+self.l6h)*z_ais).. pos target (self.l5h+self.l6h)*z_ais z 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 17

18 - 逆運動学 (2) (2)1,2,3 は前の l3h =l3h+l4h(0.4) としたときの 3 関節と同様に解くことが出来る pos ここは 3 自由度アームの求め方 def arm_sol_pos(self,pos) : 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 18

19 - 逆運動学 (3) (3)joint 5 の回転軸 () は手首 z 軸と link3z 軸に垂直回転角はそれらの間の角度 (2 方向あることに注意 ) def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).. T123 z z3 target zt 5, th5 z 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 19

20 - 逆運動学 (4) (4)joint 4 の回転角は link5 と link3 の成す角 T123 def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).. 5 th4 3 z target z 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 20

21 - 逆運動学 (5) (5)joint 6 の回転角は link5 と wrist(=target) の成す角 T123 def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).. t 5 th6 z target z 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 21

22 - 例題 :6 関節アームの逆運動学 hand wrist base l h =0.1 l 6 =0.05 l 5 =0.1 l 4 =.01 l 3 =0.3 l 2 =0.5 l 1 =0.05 l b =0.1 (1)joint 5 の位置を求めるハンド座標の z 軸方向の位置 (2)1-3 は l3h=l3+l4=0.4 としたときの 3 関節と同様に解くことが出来る (3)joint 5 の回転軸 () は手首 z 軸と link3z 軸に垂直回転角はそれらの間の角度 (2 方向あることに注意 ) 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 22

23 - 例題 :6 関節アームの逆運動学 hand wrist l h =0.1 l 6 =0.05 l 5 =0.1 l 4 =.01 l 3 =0.3 l 2 =0.5 (4)joint 4 の回転角は link5 と link3 の成す角 (5)joint 6 の回転角は link5 と wrist の成す角 base l 1 =0.05 l b = 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 23

24 - arm6dof_draft2.p(1) def arm_sol_pos(self,pos) : =pos[0] =pos[1] z=pos[2] th1_1=atan2(,) th1_2=th1_1-pi lh=self.l3h+self.l4h zz=z-self.bh-self.l1h zz_2=zz**2 _2=**2+**2 th3=acos((_2+zz_2-self.l2h**2-lh**2)/2.0/self.l2h/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 self.solutions_pos=[[th1_1,th2_1,th3],[th1_1,th2_2,-th3], [th1_2,-th2_1,-th3],[th1_2,-th2_2,th3]] return self.solutions_pos # 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 24

25 - arm6dof_draft2.p(2) (3)(4)(5) def fk123(self,th123): T1=FRAME(zabc=[0,0,self.bh,0,0,th123[0]]) T2=FRAME(zabc=[0,0,self.l1h,0,th123[1],0]) T3=FRAME(zabc=[0,0,self.l3h+self.l4h,0,th123[2],0]) return T1*T2*T3 # def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).... return [th4,th5,th6],[th4_2,-th5,th6_2] 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 25

26 - arm6dof_draft2.p(3) (1) (2) def arm_sol(self,target) : self.solutions=[] pos=target.vec z_ais=target.mat.col(2) pos=pos-((self.l5h+self.l6h)*z_ais) self.solve_pos(pos) for th123 in self.solutions_pos : rslt=self.solve_ori(target,th123) self.solutions.append(th123+rslt[0]) self.solutions.append(th123+rslt[1]) return self.solutions 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 26

27 - arm6dof_draft2.p の使い方まず env_arm6dof_draft2.p を走らせる. >>> create_env() >>> arm <larm_w_hand.linkedarm instance at 00350F5F8> >>> arm.solve(bo.where(arm.base)*arm.wrist.where(arm.hand)) [[ , , , e-16, , ], [ , , , [ , , , , , ], [ , , , e-16, , ]] >>> show_all_solutions(arm,bo) >>> 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 27

28 - アームの逆運動学の数値解法いろいろな場合に応用できるニュートン法の応用 1 変数 : 微分多変数 : ヤコビ行列 ( 偏微分 ) 解の収束性が問題良い初期近似があると良いロボットアームの場合 : 軌跡に沿って逐次計算それでも越えられないところがあるはずだがロボットアームの場合特異点に近寄らなければ大丈夫なことが多い 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 28

29 - 1 変数のニュートン法 ( 復習 ) 関数 f の 0 回りでのテーラー展開を考える f f 0 f ' ここをもとめたい f = f 近似的に f =0 とするを求める 1 0= f 0 f ' これを解いて 1 = 0 f 0 / f ' 0 これを繰り返すことで十分に良い近似解を求めることが出来る微分を使うと逆が簡単に解ける 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 29

30 - 1 変数の一次近似テーラー展開 f f 0 f ' 当然誤差はある 0 次 1 次 = f 変形して f f 0 = f ' 0 0 = f ' 0 f 0 0 つまり微小な変化は簡単な比例で近似できるということ 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 30

31 - ニュートン法で困る例単純なニュートン法だと極値が零に近くなるとで発散してしまう発散しないように用心深くすると停留点を越えられない多くの場合別な初期値からやり直すのが良い = f 本当の解はここ 0 停留点 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 31

32 - ニュートン法で困る例 2 解が停留点の場合微分して0を使うなど工夫が必要解がここだと工夫が必要 = f 0 停留点 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 32

33 - 多変数の場合 ( アームのヤコビアン ) 制御したい ( 解を求めたい ) パラメタと制御できる ( 関節角度など ) パラメタとの微小変化量の関係を (1 次偏微分係数で ) 表した行列を使う一般にこのような多変数関数の1 次偏微分係数行列はヤコビ行列ヤコビアンなどと呼ばれるイメージとしては ( 超 ) 曲面の接 ( 超 ) 平面による近似一次微分が曲線を接線で近似していたことを思い出して欲しい 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 33

34 - ロボットアームのヤコビアンの例関節角度を少し動かしたとき手先の位置姿勢がどのように変化するか : いわゆるロボットアームのヤコビアン関節角度を少し動かしたときカメラ画像中の位置がどのように変化するか : ビジュアルヤコビアンなどと呼ばれる 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 34

35 - 次回の予告ヤコビアンについて詳しく 2012 年度前学期電気通信大学大学院情報システム学研究科情報メディアシステム学専攻知能システム論 1 35

PowerPoint Presentation

PowerPoint Presentation 知能システム論 1 (9) 201365 情報システム学研究科情報メディアシステム学専攻知能システム学講座末廣尚士 12 ロボットアームの逆運動学 ( 幾何学的 ( 解析的 ) 解法 ) 何をしたいか手首手先ツールの3 次元空間での位置や姿勢からそれを実現する関節角度を計算するアームソリューションアームの解とも呼ぶ何のためにたとえばビジョンで認識された物をつかむ場合物の位置姿勢は3