スライド 1

Size: px

Start display at page:

Download "スライド 1"

れれみしま
5 years ago
Views:

1 (10) 電気通信大学大学院情報理工学研究科末廣尚士

2 14. ロボットアームの逆運動学 ( 幾何学的 ( 解析的 ) 解法 ) 何をしたいか手首, 手先, ツールの 3 次元空間での位置や姿勢から, それを実現する関節角度を計算する. アームソリューション, アームの解とも呼ぶ何のためにたとえばビジョンで認識された物をつかむ場合, 物の位置姿勢は 3 次元空間で表現されることが普通である. したがってそれに必要な手首, 手先, ツールの位置や姿勢アームの位置や姿勢も 3 次元空間の位置姿勢で与えられる. 逆運動学が不要な制御もあるが,,,. 2

3 - 3 自由度アームの逆運動学手先の位置が θ 2,θ 3 を求める. base x wrist base y wrist base z wrist のとき, 関節角度 θ 1, 3

θ 2 + θ 3 cos θ 1 base y wrist = L1 x hand sin θ 1 = l2h sin θ 2 +

4 - 幾何学的 ( 解析的 ) 解法まずは順運動学 base z wrist = bh + l1h + l2h cos θ 2 + l3h cos θ 2 + θ 3 base x wrist = L1 x hand cos θ 1 = l2h sin θ 2 + l3hsin θ 2 + θ 3 cos θ 1 base y wrist = L1 x hand sin θ 1 = l2h sin θ 2 + l3hsin θ 2 + θ 3 sin θ 1 where L1 x wrist = l2h sin θ 2 + l3h sin θ 2 + θ 3 4

5 - θ 1 の計算 base x wrist = L1 x wrist cos θ 1 base y wrist = L1 x wrist sin θ 1 よって,tan θ 1 = base y wrist base x wrist θ 1 = tan 1 base y wrist base x wrist π x < 0, y < 0 πτ2 x = 0, y < 0 tan 1 base y wrist base x wrist x > 0, y < 0 πτ2 x = 0, y > 0 tan 1 base y wrist base x wrist + π x < 0, y 0 unknown x = 0, y = 0 もしくは,θ 1 = atan2 base y wrist, base x wrist 5

6 - θ 3 の計算 base z wrist = bh + l1h + l2h cos θ 2 + l3h cos θ 2 + θ 3 L1 x wrist = l2h sin θ 2 + l3h sin θ 2 + θ 3 ここで, X = L1 x wrist Z = base z wrist bh l1h (X 2 = base 2 x wrist とすると, X = l2h sin θ 2 + l3h sin θ 2 + θ 3 + base 2 y wrist ) Z = l2h cos θ 2 + l3h cos θ 2 + θ 3 両辺を 2 乗して加える, X 2 = l2h 2 sin 2 θ 2 + 2l2hl3h sin θ 2 sin θ 2 + θ 3 + l3h 2 sin 2 θ 2 + θ 3 Z 2 = l2h 2 cos 2 θ 2 + 2l2hl3h cos θ 2 cos θ 2 + θ 3 + l3h 2 cos 2 θ 2 + θ 3 X 2 + Z 2 = l2h 2 + l3h 2 + 2l2hl3h cos θ 2 cos θ 2 + θ 3 + sin θ 2 sin θ 2 + θ 3 6

7 - θ 3 の計算 ( つづき ) X 2 + Z 2 = l2h 2 + l3h 2 + 2l2hl3h cos θ 2 cos θ 2 + θ 3 + sin θ 2 sin θ 2 + θ 3 ここで cos α β = cos α cos β + sin α sin β を思い出して, α = θ 2 + θ 3,β = θ 2 とすると, X 2 + Z 2 = l2h 2 + l3h 2 + 2l2hl3h cos θ 3 cos θ 3 = X2 +Z 2 l2h 2 l3h 2 2l2hl3h よって, θ 3 = ± cos 1 X2 +Z 2 l2h 2 l3h 2 2l2hl3h 7

8 - θ 2 の計算 θ 3 が, + の場合 - の場合 ψ = sin 1 Z X 2 + Z 2 アームの姿勢が変わっても実は式は同じ. ここの符号が変わるだけ. ψ = sin 1 Z X 2 + Z 2 φ = sin 1 l3h sin θ 3 X 2 + Z 2 θ 2 = Τ π 2 ψ φ φ = sin 1 l3h sin θ 3 X 2 + Z 2 θ 2 = Τ π 2 ψ φ 8

9 - 結果のグラフィックス表示 1 ψ = sin 1 Z X 2 +Z 2 φ = sin 1 l3h sin θ 3 X 2 +Z 2 θ 2 = πτ2 ψ φ 9

10 - 結果のグラフィックス表示 2 ψ = sin 1 Z X 2 +Z 2 φ = sin 1 l3h sin θ 3 X 2 +Z 2 θ 2 = πτ2 ψ φ 10

11 - 実はまだ他に解がある θ 1 を裏に回して,θ 2 と θ 3 を反対に曲げる. θ 1 = atan2 base y wrist, base x wrist 11

12 - 3 関節アーム : 逆運動学解追加逆運動学 arm3dof.py def arm_sol(self,trans) : x=trans.vec[0] y=trans.vec[1] z=trans.vec[2] th1_1=atan2(y,x) th1_2=atan2(-y, -x) zz=z-self.bh-self.l1h zz_2=zz**2 xx_2=x**2+y**2 th3=acos((xx_2+zz_2-self.l2h**2-self.l3h**2)/2.0/self.l2h/self.l3h) psi=asin(zz/sqrt(xx_2+zz_2)) phi=asin(self.l3h*sin(th3)/sqrt(xx_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 12

13 - 3 関節アーム : マークの追加リンクがどっちを向いているのかわかりやすくする arm3dof.py def mark_arm(self) : self.base.mark=visual.box(size=(0.11,0.01,0.1),pos=(0.055,0,0.05), color=visual.color.red) self.base.mark.frame=self.base.vframe self.links[0].mark=visual.box(size=(0.11,0.01,0.05),pos=(0.055,0,0.025), color=visual.color.red) self.links[0].mark.frame=self.links[0].vframe self.links[1].mark=visual.box(size=(0.05,0.01,0.4),pos=(0.025,0,0.2), color=visual.color.red) self.links[1].mark.frame=self.links[1].vframe self.links[2].mark=visual.box(size=(0.03,0.01,0.4),pos=(0.015,0,0.2), color=visual.color.red) self.links[2].mark.frame=self.links[2].vframe 13

14 - 3 関節アーム :create_arm の変更 solve と make_shape をメソッドにする import new from larm_w_hand import * from hand import * arm3dof.py def create_arm() : hand = create_hand() arm = LinkedArm(3,hand=hand) arm.make_shape=new.instancemethod(make_shape,arm,arm. class ) arm.ready_angle=[0,pi/4,pi/2] arm.park_angle=[0,0,0] arm.mark=new.instancemethod(mark_arm,arm,arm. class ) arm.solve=new.instancemethod(arm_sol,arm,arm. class ) arm.make_shape() return arm 14

15 - 逆運動学を使って動かすまず env_arm3dof.py を走らせる. >>> from env_arm3dof import * >>> >>> arm.make_shape() >>> arm.mark() >>> create_env() >>> a=arm.solve(box.where(arm.base)) >>> a [[ , , ], [ , , ], [ , , ], [ , , ]] >>> for pp in a: arm.set_joints(pp) sleep(1) >>> 15

16 - hand を box の位置に動かす? hand が box の場所に行くにはどうしたらよいか? ( 案 1) hand から wrist へ座標変換でもどしてやる >>> b=arm.solve(box.where(arm.base)*arm.wrist.where(arm.hand)) >>> b [[ , , ], [ , , ], [ , , ], [ , , ]] >>> for pp in b: arm.set_joints(pp) sleep(1) >>> 上手くいかない. なぜ? 16

17 - hand を box の位置に動かす 3 自由度アームの場合 hand と box の姿勢を一致させられないので, 座標系の逆変換ではうまくいかない. arm_sol のプログラムの中身を変更しなくてはならない. th3=acos((xx_2+zz_2-self.l2h**2-self.l3h**2)/2.0/self.l2h/self.l3h) psi=asin(zz/sqrt(xx_2+zz_2)) phi=asin(self.l3h*sin(th3)/sqrt(xx_2+zz_2)) これを長さだけでなく, 手先の位置姿勢に関して汎用的に処理するのは幾何的解法では結構難しい. 後で数値解法で解決する. 17

18 - 6 自由度アームの解の求め方の例 18

19 - 例題 14-1: 6 関節アームの逆運動学手首座標系が目標座標系 target に一致するように関節角を求めよ (arm6dof.py) 19

20 - 6 関節アームの逆運動学 (1) (1)joint 5 の位置を求める手首座標の z 軸方向 lh5+lh6(-0.15) の位置 def arm_sol(self,target) : self.solutions=[] pos=target.vec z_axis=target.mat.col(2) pos=pos-((self.l5h+self.l6h)*z_axis).. 20

21 - 6 関節アームの逆運動学 (2) (2)1,2,3 は前の l3h =l3h+l4h(0.4) としたときの 3 関節と同様に解くことが出来る def arm_sol_pos(self,pos) :... 21

22 - 6 関節アームの逆運動学 (3) (3)joint 5 の回転軸 (y) は手首 z 軸と link3z 軸に垂直回転角はそれらの間の角度 (2 方向あることに注意 ) def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).. 22

23 - 6 関節アームの逆運動学 (4) (4)joint 4 の回転角は link5y と link3y の成す角 def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).. 23

24 - 6 関節アームの逆運動学 (5) (5)joint 6 の回転角は link5y と wrist(=target) y の成す角 def arm_sol_ori(self,target,th123) : T123=self.fk123(th123) zt=target.mat.col(2) z3=t123.mat.col(2).. 24

25 - 例題 14-1: まとめ 1 (1)joint 5 の位置を求める手首座標の z 軸方向の位置 (2)1-3 は l3h=l3+l4=0.4 としたときの 3 関節と同様に解くことが出来る (3)joint 5 の回転軸 (y) は手首 z 軸と link3z 軸に垂直回転角はそれらの間の角度 (2 方向あることに注意 ) 25

26 - 例題 14-1: まとめ 2 (4)joint 4 の回転角は link5y と link3y の成す角 (5)joint 6 の回転角は link5y と wrist y の成す角 26

27 - arm6dof.py(1) def arm_sol_pos(self,pos) : x=pos[0] y=pos[1] z=pos[2] th1_1=atan2(y,x) th1_2=atan2(-y,-x) lh=self.l3h+self.l4h zz=z-self.bh-self.l1h zz_2=zz**2 xx_2=x**2+y**2 th3=acos((xx_2+zz_2-self.l2h**2-lh**2)/2.0/self.l2h/lh) psi=asin(zz/sqrt(xx_2+zz_2)) phi=asin(lh*sin(th3)/sqrt(xx_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 27

28 - arm6dof.py(1) def fk123(self,th123): T1=FRAME(xyzabc=[0,0,self.bh,0,0,th123[0]]) T2=FRAME(xyzabc=[0,0,self.l1h,0,th123[1],0]) T3=FRAME(xyzabc=[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) (3)(4)(5) return [th4,th5,th6],[th4_2,-th5,th6_2] 28

29 - arm6dof.py(1) def arm_sol(self,target) : self.solutions=[] pos=target.vec z_axis=target.mat.col(2) pos=pos-((self.l5h+self.l6h)*z_axis) (1)(2) 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 29

30 - 次回の予告逆運動学の数値解法ニュートン法ヤコビアン 30

PowerPoint Presentation

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