(8) 2017.6.7 電気通信大学大学院情報理工学研究科末廣尚士
9. ロボットアームの逆運動学 ( 幾何 学的 ( 解析的 ) 解法 ) 何をしたいか 手首, 手先, ツールの 3 次元空間での位置や姿勢から, それを実現する関節角度を計算する. アームソリューション, アームの解とも呼ぶ 何のために たとえばビジョンで認識された物をつかむ場合, 物の位置 姿勢は 3 次元空間で表現されることが普通である. したがってそれに必要な手首, 手先, ツールの位置や姿勢アームの位置や姿勢も 3 次元空間の位置 姿勢で与えられる. 2
例題 9-1: 3 自由度アームの関節角を [pi/6, pi/4, pi/2] としたときのベースから見たハンドの位置 >>> arm=create_arm() >>> arm.set_joints([pi/6,pi/4,pi/2]) >>> arm.hand.where(arm.base).xyzabc() [0.5511351921262151, 0.31819805153394637, 0.07928932188134533, 0.46364760900080626, -0.6590580358264089, -2.4568734505875103] >>> では, 手先を v:[0.5, 0.3, 0.1] にするには関節角はいくつにしたらよいか? 3
- 3 自由度アームの逆運動学 手先の位置が θ 2,θ 3 を求める. base x wrist base y wrist base z wrist のとき, 関節角度 θ 1, 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 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 πτ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 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 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 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 ψ φ 9
- 結果のグラフィックス表示 1 ψ = sin 1 Z X 2 +Z 2 φ = sin 1 l3h sin θ 3 X 2 +Z 2 θ 2 = πτ2 ψ φ 10
- 結果のグラフィックス表示 2 ψ = sin 1 Z X 2 +Z 2 φ = sin 1 l3h sin θ 3 X 2 +Z 2 θ 2 = πτ2 ψ φ 11
- 実はまだ他に解がある θ 1 を裏に回して,θ 2 と θ 3 を反対に曲げる. θ 1 = atan2 base y wrist, base x wrist 12
例題 9-1: 解答例 表裏が分かるようにマークを付ける 目標は ( 一応 )FRAME で与える >>> arm.mark() >>> a=arm.solve(frame(vec=vector(0.5,0.3,0.1))) >>> a [[0.5404195002705842, 0.9061234910050131, 1.5004257618081907], [0.5404195002705842, 2.4065492528132038, -1.5004257618081907], [-2.601173153319209, -0.9061234910050131, -1.5004257618081907], [-2.601173153319209, -2.4065492528132038, 1.5004257618081907]] >>> arm.set_joints(a[0]) >>> arm.wrist.where(arm.base).xyzabc() [0.5, 0.30000000000000004, 0.10000000000000009, - 2.7062318564303522, 0.6126669119414491, 2.4614837221760357] >>> for pp in a :... arm.set_joints(pp)... sleep(2)... 13
例題 9-2 手首を直線的に動かす 3 自由度アームを [pi/6,pi/4,pi/2] から [-pi/6,pi/8,2*pi/3] へ手首が直線的に移動するように動かす 14
例題 9-2 解答方針 まず順運動学. 関節角が [pi/6,pi/4,pi/2],[-pi/6,pi/8,2*pi/3] のときの手首の 3 次元位置を求める ( 順運動学 ). 上記二つの三次元位置を結ぶ線分上に多数の分割点を生成する. そして逆運動学. それぞれの分割点における関節角度を求める. 得られた関節角度をロボットアームに順次指令する 15
手首位置を刻む関数 def mk_pos_seq(arm,th1,th2,n) : arm.set_joints(th1) pos1=arm.wrist.where(arm.base).vec arm.set_joints(th2) pos2=arm.wrist.where(arm.base).vec posd=(1.0/n)*(pos2-pos1) pos=pos1 pos_seq=[pos] for j in range(n) : pos = pos+posd pos_seq.append(pos) return pos_seq 16
逆運動学 刻んだ手首位置でアームを動かす def mv_arm_seq(arm,pos_seq): for pos in pos_seq : ths=arm.solve(frame(vec=pos)) arm.set_joints(ths[0]) visual.rate(10) return ths[0] 刻んだ手首位置の関節角度を求める def mk_ik_seq(arm,pos_seq) : return [arm.solve(frame(vec=pos))[0] for pos in pos_seq] 17
例題 9-2 解答例 まずは手首位置を刻んで動かしてみる arm=create_arm() nn= 100 th1=[pi/6,pi/4,pi/2] th2=[-pi/6,pi/8,2*pi/3] pos_seq= mk_pos_seq(arm,th1,th2,nn) arm.set_joints(th1) raw_input("ok?") mv_arm_seq(arm,pos_seq) raw_input("ok?") 18
手首位置をプロット tt= [1.0*i/nn for i in range(nn+1)] plt.plot(tt,pos_seq) plt.xlabel("div") plt.ylabel("xyz") plt.legend(["x","y","z"]) plt.show() 19
手首位置をプロット 手首位置は刻んだ通り直線的に変化している 20
関節角をプロット th_seq= mk_ik_seq(arm,pos_seq) plt.plot(tt,th_seq) plt.xlabel("div") plt.ylabel("theta") plt.legend(["th1","th2","th3"]) plt.show() 21
関節角をプロット 関節角は曲線を描く 22
手首位置を三次元プロットする 参考 :http://matplotlib.org/api/figure_api.html from mpl_toolkits.mplot3d import Axes3D fig = plt.figure() ax = fig.gca(projection='3d') xx=[p[0] for p in pos_seq] yy=[p[1] for p in pos_seq] zz=[p[2] for p in pos_seq] ax.plot(xx,yy,zz) ax.legend(["xyz"]) plt.show() 23
手首位置を三次元プロットする 24
例題 9-3 hand を目標位置に動かす では, ハンドの位置を v:[0.5, 0.3, 0.1] にするには関節角はいくつにしたらよいか? 25
例題 9-3 解答例 ( 失敗 ) ( 案 1) hand から wrist へ座標変換でもどしてやる hand から wrist へどうやってもどす? とりあえず wrist で解いて,z 方向に戻してみる >>> zz=arm.wrist.where(arm.base).mat.col(2) >>> hh=frame(vec=vector(0.5,0.3,0.1)-0.1*zz) >>> b=arm.solve(hh) >>> arm.set_joints(b[0]) >>> arm.wrist.where(arm.base).xyzabc() [0.4424948650639732, 0.2654969190383839, 0.17418016487003452, -2.6955552700672656, 0.6233575626211233, 2.4552974074870324] >>> arm.hand.where(arm.base).xyzabc() [0.5008713178859492, 0.30052279073156957, 0.10093142380197177, 0.44603738352252786, -0.6233575626211232, -2.4552974074870324] 失敗! 微妙に違う 26
- 失敗の理由 3 自由度アームの場合, 位置を変えると姿勢も変わってしまう. 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)) ツール位置が z 方向のシフトだけならこれでよいが, 位置や姿勢がかかわると汎用的に処理するのは幾何的解法では結構難しい. 後で数値解法で解決する. 27
例題 9-4: 6 関節アームの逆運動学 手首座標系が目標座標系 target に一致するように関節角を求めよ (arm6dof.py) 28
- 6 自由度アームの解の求め方の例 6 自由度アームの場合, ハンドの位置だけでなく姿勢も目標に合わせることができるので目標の座標系にハンド座標が一致しているとして逆変換で手首座標系の目標座標系を設定することができる 29
- 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).. 30
- 6 関節アームの逆運動学 (2) (2)1,2,3 は 前の l3h =l3h+l4h(0.4) としたときの 3 関節と同様に解くことが出来る def arm_sol_pos(self,pos) :... 31
- 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).. 32
- 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).. 33
- 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).. 34
例題 9-4: 解答例のまとめ (1)joint 5 の位置を求める 手首座標の z 軸方向 -0.15 の位置 (2)1-3 は l3h=l3+l4=0.4 としたときの 3 関節と同様に解くことが出来る (3)joint 5 の回転軸 (y) は 手首 z 軸と link3z 軸に垂直 回転角はそれらの間の角度 (2 方向あることに注意 ) 35
例題 9-4: 解答例のまとめ (4)joint 4 の回転角は link5y と link3y の成す角 (5)joint 6 の回転角は link5y と wrist y の成す角 36
- 次回の予告 逆運動学の数値解法 ニュートン法 ヤコビアン 37