第4章 逆运动学方程

已知P(x,y)，Φ及连杆长度L1,L2,L3。求关 节变量θ1 ，θ 2 ，θ 3 。
y (l1 + l2 c2 ) − x(l2 s2 ) x(l + l c ) + y (l s ) , c1 = 1 2 2 2 2 2 2 (x2 + y2 ) (x + y ) θ1 = arctan 2( s1 , c1 ) s1 = 由图中几何关何可得： φ = θ1 + θ 2 + θ 3 , x = p x − l3 cos φ , y = p y − l3 sin φ
x c123 - s123 0 l1c1 + l2 c12 0 l1c1 + l2 c12 y s c123 0 l1s1 + l2 s12 0 l1s1 + l2 s12 123 0 = Pw = = z 0 0 0 1 0 0 1 1 1 0 0 0 1 cφ = c123 , sφ = s123 , φ = arctan 2( s123 , c123 )
已知( x, y ) = ( 3 + 2,1), 及连杆长度L1 = L2 = 2, 求关节变量所有解.
题目
2 ( 3 + 2) 2 + 12 − 2 2 − 2 2 x 2 + y 2 − l12 − l2 = ± arccos 解：θ 2 = ± arccos 2l1l2 8
提醒：逆运动学是在连杆结构参数已知的前提下，只能求 得每一个连杆的一个变量参数，而不是机器人每个连杆的 四个参数都能求出。
一个简单示例
已知(x,y)，求关节变量s和θ。 解：
θ = arctan( y, x)
s= x +y
2 2
二连杆操作臂逆运动学
已知(x,y)，及连杆长度L1,L2。求 关节变量θ1 ，θ 2。 提醒：可能存在多重解，也可能 无解。
第四章 逆运动学方程
机器人逆运动学
逆运动学求解：已知末端手的位姿，求关节变量 qi。
nx n 0 1 1 T = 0T (q1 ) 2T (q2 ) 2T (q3 ) ⋅ ⋅⋅ N −N T (q N ) = y N 1 3 nz 0 ox oy oz 0 ax ay az 0 Px Py Pz 1
2 x 2 + y 2 − l12 − l2 θ 2 = ± arccos 2l1l2 y (l + l c ) − x(l s ) x(l + l c ) + y (l s ) s1 = 1 22 2 2 2 2 , c1 = 1 2 2 2 2 2 2 (x + y ) (x + y ) θ1 = arctan 2( s1 , c1 ),θ 3 = φ - θ1 - θ 2
l sin( θ ) θ 1 = α − arcsin 2 2 2 2 = arctan 2 ( y / x ) − arcsin x +y
二连杆操作臂逆运动学代数解法
解：1 = cos θ1 , c12 = cos(θ1 + θ 2 ) c
x = l1c1 + l2 c12 y = l1s1 + l2 s12 (1) ( 2)
2 3 x 2 + y 2 − l12 − l2 = ± arccos θ 2 = ± arccos 2 = ±30 2l1l2
当θ 2 = 30 时，s2 = 0.5, c2 =
3 2 , x + y2 = 8 + 4 3 2 (l + l c ) y − l s x (l + l c ) x + l s y s1 = 1 2 22 2 2 2 = 0, c1 = 1 2 22 2 2 2 = 1 x +y x +y
(1) 2 + (2) 2 = x 2 + y 2
2 2 2 2 = (l12 c12 + l2 c12 + 2l1l2 c1c12 ) + (l12 s12 + l2 s12 + 2l1l2 s1s12 ) 2 = l12 + l2 + 2l1l2 (c1c12 + s1s12 ) 2 2 = l12 + l2 + 2l1l2 c12−1 = l12 + l2 + 2l1l2 c2 2 x 2 + y 2 − l12 − l2 cos(θ 2 ) = 2l1l2 2 x 2 + y 2 − l12 − l2 θ 2 = ± arccos( ) 2l1l2
现在θ２已知，求θ１
二连杆操作臂逆运动学代数解法
解： x = l1c1 + l2 c12 = l1c1 + l2 c1c2 − l2 s1s2 = c1 (l1 + l2c2 ) − s1 (l2 s2 )
y = l1s1 + l2 s12 = l1s1 + l2 s1c2 + l2c1s2 = c1 (l2 s2 ) + s1 (l1 + l2 c2 ) x + s1 (l2 s2 ) x + s1 (l2 s2 ) c1 = ,y= (l2 s2 ) + s1 (l1 + l2 c2 ) (l1 + l2 c2 ) (l1 + l2 c2 ) x(l2 s2 ) + s1 (l2 s2 ) 2 + s1 (l1 + l2 c2 ) 2 x(l2 s2 ) + s1[(l2 s2 ) 2 + (l1 + l2c2 ) 2 ] y= = (l1 + l2 c2 ) (l1 + l2c2 )
2 x 2 + y 2 − l12 − l2 因此，θ 2 = ± arccos 2l1l2 y (l + l c ) − x(l s ) x(l + l c ) + y (l s ) s1 = 1 22 2 2 2 2 , c1 = 1 2 2 2 2 2 2 (x + y ) (x + y ) θ1 = arctan 2( s1 , c1 )
θ1 = arctan 2( s1 , c1 ) = 0 , θ 3 = φ − θ1 − θ 2 = 30
当θ 2 = -30 时，s2 = -0.5, c2 = 3 2 , x + y2 = 8 + 4 3 2 (l + l c ) y − l s x (l + l c ) x + l s y 3 + 2 3 s1 = 1 2 22 2 2 2 = 0.5, c1 = 1 2 22 2 2 2 = x +y x +y 4+2 3
c1 =
θ1 = arctan 2( s1 , c1 ) = arctan 2( 3 3 ) = 30
上题的matlab代码 代码 上题的
三连杆操作臂逆运动学
先求出0 PW 坐标，那么θ1，θ 2可用二连杆逆运动学方程求得 令x = 0PWx , y = 0PWy , 则，
2 x 2 + y 2 − l12 − l2 ) θ 2 = ± arccos( 2l1l2
题目
已知三连杆操作臂变换矩阵 0T = 3 求关节变量所有解. 解：x = 3 + 2, y = 1
2 3 x 2 + y 2 − l12 − l2 = ± arccos θ 2 = ± arccos 2 = ±30 2l1l2
对三角形ABC运用正弦定理
sin( α − θ 1 ) sin( 180 − θ 2 ) sin( θ 2 ) = = l2 x2 + y2 x2 + y2
α = arctan 2 ( y / x ), sin( α − θ 1 ) =
l 2 sin( θ 2 ) x2 + y2 l sin( θ ) 2 2 x2 + y2
0.5 3 0 0 2
− 3 0.5 0 0
2
0 2 + 3 0 1 及连杆长度L1 = L2 = 2, L3 = 1, 1 0 0 1
当θ 2 = 30 时，s2 = 0.5, c2 =
3 2 , x + y2 = 8 + 4 3 2 (l + l c ) y − l s x (l + l c ) x + l s y s1 = 1 2 22 2 2 2 = 0, c1 = 1 2 22 2 2 2 = 1 x +y x +y
二连杆操作臂逆运动学几何解法
解：对三角形ABC运用余弦定理
2 x 2 + y 2 = l12 + l2 − 2l1l2 cos(180 − θ 2 )
cos(180 − θ 2wenku.baidu.com)
2 x 2 + y 2 − l12 − l2 cos(θ 2 ) = 2l1l2 2 x 2 + y 2 − l12 − l2 θ 2 = ± arccos( ) 2l1l2
c1 =
θ1 = arctan 2( s1 , c1 ) = 0
当θ 2 = -30 时，s2 = -0.5, c2 = 3 2 , x + y2 = 8 + 4 3 2 3 (2 + 2 * ) *1 + 2 * 0.5 * ( 3 + 2) (l + l c ) y − l s x 2 s1 = 1 2 22 2 2 2 = = 0.5 x +y 8+4 3 (l1 + l2 c2 ) x + l2 s2 y = x2 + y2 (2 + 2 * 3 ) * ( 3 + 2) - 2 * 0.5 *1 3+ 2 3 2 = 8+4 3 4+2 3
θ 3 = φ - θ1 - θ 2
式中，x = p x − l3 cos φ , y = p y − l3 sin φ
题目
已知三连杆操作臂P点 = ( 3 + 2.5,1 + 3 / 2), φ = 60 及连杆长度L1 = L2 = 2, L3 = 1, 求关节变量所有解. 解：x = p x − l3 cos φ = 3 + 2.5 − 1* cos(60 ) = 3 + 2 y = p y − l3 sin φ = 1 + 3 / 2 − 1* sin(60 ) = 1
2 x(l2 s2 ) + s1 (l12 + l2 + 2l1l2 c2 ) x(l2 s2 ) + s1 ( x 2 + y 2 ) = = (l1 + l2 c2 ) (l1 + l2 c2 )
y (l1 + l2 c2 ) − x(l2 s2 ) s1 = , 2 2 (x + y ) x(l + l c ) + y (l s ) 同理，c1 = 1 2 2 2 2 2 2 (x + y ) θ1 = arctan 2( s1 , c1 )
θ1 = arctan 2( s1 , c1 ) = 0 ,θ 3 = φ − θ1 − θ 2 = 30
当θ 2 = -30 时，s2 = -0.5, c2 = 3 2 , x + y2 = 8 + 4 3 2 (l + l c ) y − l s x (l + l c ) x + l s y 3 + 2 3 s1 = 1 2 22 2 2 2 = 0.5, c1 = 1 2 22 2 2 2 = x +y x +y 4+2 3
θ1 = arctan 2( s1 , c1 ) = 30 ,θ 3 = φ − θ1 − θ 2 = 60
三连杆操作臂逆运动学
若已知坐标系{3}到 {0}的变换矩阵03T，及连杆长度, 求各关节变量 由操作臂运动学方程(教材p84, 式(4 - 6))： c123 - s123 s B 0 123 c123 w T = 3T = 0 0 0 0 0 Pw = 03T[0,0,0,1]' 0 l1c1 + l2 c12 0 l1s1 + l2 s12 1 0 0 1
3 = ± arccos 2 = ±30 当θ 2 = 30 时，s2 = 0.5, c2 = 3 2 , x + y2 = 8 + 4 3 2 3 (2 + 2 * ) *1 − 2 * 0.5 * ( 3 + 2) (l + l c ) y − l s x 2 s1 = 1 2 22 2 2 2 = =0 x +y 8+4 3 (l1 + l2 c2 ) x + l2 s2 y = x2 + y2 (2 + 2 * 3 ) * ( 3 + 2) + 2 * 0.5 *1 2 =1 8+4 3