(10) 2016.6.22 電気通信大学大学院情報理工学研究科末廣尚士
14. ロボットアームの逆運動学 ( 幾何 学的 ( 解析的 ) 解法 ) 何をしたいか 手首, 手先, ツールの 3 次元空間での位置や姿勢から, それを実現する関節角度を計算する. アームソリューション, アームの解とも呼ぶ 何のために たとえばビジョンで認識された物をつかむ場合, 物の位置 姿勢は 3 次元空間で表現されることが普通である. したがってそれに必要な手首, 手先, ツールの位置や姿勢アームの位置や姿勢も 3 次元空間の位置 姿勢で与えられる. 逆運動学が不要な制御もあるが,,,. 2
- 3 自由度アームの逆運動学 手先の位置が θ 2,θ 3 を求める. base x wrist base y wrist base z wrist のとき, 関節角度 θ 1, 3
- 幾何学的 ( 解析的 ) 解法 まずは順運動学 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
- θ 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
- θ 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
- θ 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
- θ 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
- 結果のグラフィックス表示 1 ψ = sin 1 Z X 2 +Z 2 φ = sin 1 l3h sin θ 3 X 2 +Z 2 θ 2 = πτ2 ψ φ 9
- 結果のグラフィックス表示 2 ψ = sin 1 Z X 2 +Z 2 φ = sin 1 l3h sin θ 3 X 2 +Z 2 θ 2 = πτ2 ψ φ 10
- 実はまだ他に解がある θ 1 を裏に回して,θ 2 と θ 3 を反対に曲げる. θ 1 = atan2 base y wrist, base x wrist 11
- 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
- 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
- 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
- 逆運動学を使って動かす まず env_arm3dof.py を走らせる. >>> from env_arm3dof import * >>> >>> arm.make_shape() >>> arm.mark() >>> create_env() >>> a=arm.solve(box.where(arm.base)) >>> a [[-0.78539816339744828, 0.90198348365541947, 2.1401199701156761], [-0.78539816339744828, 3.0421034537710954, -2.1401199701156761], [2.3561944901923448, -0.90198348365541947, -2.1401199701156761], [2.3561944901923448, -3.0421034537710954, 2.1401199701156761]] >>> for pp in a: arm.set_joints(pp) sleep(1) >>> 15
- hand を box の位置に動かす? hand が box の場所に行くにはどうしたらよいか? ( 案 1) hand から wrist へ座標変換でもどしてやる >>> b=arm.solve(box.where(arm.base)*arm.wrist.where(arm.hand)) >>> b [[-0.78539816339744828, 0.63292249604039119, 2.2119557397743437], [-0.78539816339744828, 2.8448782358147344, -2.2119557397743437], [2.3561944901923448, -0.63292249604039119, -2.2119557397743437], [2.3561944901923448, -2.8448782358147344, 2.2119557397743437]] >>> for pp in b: arm.set_joints(pp) sleep(1) >>> 上手くいかない. なぜ? 16
- 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
- 6 自由度アームの解の求め方の例 18
- 例題 14-1: 6 関節アームの逆運動学 手首座標系が目標座標系 target に一致するように関節角を求めよ (arm6dof.py) 19
- 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
- 6 関節アームの逆運動学 (2) (2)1,2,3 は 前の l3h =l3h+l4h(0.4) としたときの 3 関節と同様に解くことが出来る def arm_sol_pos(self,pos) :... 21
- 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
- 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
- 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
- 例題 14-1: まとめ 1 (1)joint 5 の位置を求める 手首座標の z 軸方向 -0.15 の位置 (2)1-3 は l3h=l3+l4=0.4 としたときの 3 関節と同様に解くことが出来る (3)joint 5 の回転軸 (y) は 手首 z 軸と link3z 軸に垂直 回転角はそれらの間の角度 (2 方向あることに注意 ) 25
- 例題 14-1: まとめ 2 (4)joint 4 の回転角は link5y と link3y の成す角 (5)joint 6 の回転角は link5y と wrist y の成す角 26
- 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
- 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
- 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