LBR Med逆向运动学

博主： L.CH
发布时间：2024 年 04 月 09 日
955 次浏览
暂无评论
5139字数
分类：机器人

由于Med/IIWA为7自由度冗余机械臂，那么其运动学逆解必然很难求解，但有2种方法：

- 通过认为设定某轴角度来减低自由度，例如KUKA官方冗余角E1就3轴角度，事先规定
- 通过增加操作空间数目，使方程组可解，所以提出了基于臂型角的参数化解析法

## 降低自由度

这个方法不详细展开说明，这个方法可以把Axis 3固定为0或者一个想要的值，接下来可以按照6-DOF机械臂求逆解的方法解出最多8组解，再选择合适的逆解即可

## 增加自由度

重点说明增加自由度的方法

参考的是下篇文章

https://doi.org/10.1016/j.mechmachtheory.2017.10.025

需要定义以下几个变量X, Y, Z, RX, RY, RZ, arm_angle，还有全局变量GC2, GC4, GC6

完整代码如下：

```python
#!/usr/bin/env python
# coding: utf-8
import numpy as np
import math

# np.set_printoptions(11)

dbs, dse, dew, dwf = 360, 420, 400, 126

a = [0, 0, 0, 0, 0, 0, 0]
alpha = [-math.pi/2, math.pi/2, math.pi/2, -math.pi/2, -math.pi/2, math.pi/2, 0]
d = [360, 0, 420, 0, 400, 0, 126]
theta_ini = [0, 0, 0, 0, 0, 0, 0]

def transformationMatrix(a, alpha, d, theta):
    theta = np.deg2rad(theta)
    T = np.array([[math.cos(theta), -math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha), a * math.cos(theta)], 
                  [math.sin(theta), math.cos(theta) * math.cos(alpha), -math.cos(theta) * math.sin(alpha), a * math.sin(theta)], 
                  [0, math.sin(alpha), math.cos(alpha), d], 
                  [0, 0, 0, 1]])
    return T

def rotateMatrix(rx,ry,rz):
    rx = np.deg2rad(rx)
    ry = np.deg2rad(ry)
    rz = np.deg2rad(rz)
    Rx = np.array([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]])
    Ry = np.array([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]])
    Rz = np.array([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
    return np.dot(np.dot(Rz, Ry), Rx)

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

p2to0 = np.array([[0, 0, dbs]]).T
p4to2 = np.array([[0, dse, 0]]).T
p6to4 = np.array([[0, 0, dew]]).T
p7to6 = np.array([[0, 0, dwf]]).T

theta3_virtual = 0

# 定义末端变量和臂型角
x, y, z, rx, ry, rz = -409.79, -410.04, 248.03, 179.99, 0.03, -134.98
arm_angle = 90
# 定义全局变量Global Configuration
GC2, GC4, GC6 = -1, 1, -1

T = []
for i in range(7):
    T.append(transformationMatrix(a[i], alpha[i], d[i], theta_ini[i]))

R = rotateMatrix(rx, ry, rz)

p6to2 = np.array([[x, y, z]]).T - p2to0 - (np.dot(R, p7to6))

theta4 = np.rad2deg(GC4 * math.acos((pow(np.linalg.norm(p6to2),2)- pow(dse,2) - pow(dew,2)) / (2 * dse * dew)))

if np.linalg.norm(np.cross(p6to2.T, T[0][0:3,2:3].T)) == 0:
    theta1_virtual = 0
else:
    theta1_virtual = np.rad2deg(math.atan2(p6to2[1][0], p6to2[0][0])) + 180

phi = np.rad2deg(math.acos((pow(dse,2) + pow(np.linalg.norm(p6to2),2) - pow(dew,2)) / (2 * dse * np.linalg.norm(p6to2))))

theta2_virtual = GC2 * np.rad2deg(math.atan2(math.sqrt(pow(p6to2[0][0],2) + pow(p6to2[1][0],2)), p6to2[2][0])) + GC4 * phi

theta3_virtual = 0

T30_virtual = np.dot(np.dot(transformationMatrix(a[0], alpha[0], d[0], theta1_virtual), transformationMatrix(a[1], alpha[1], d[1], theta2_virtual)), transformationMatrix(a[2], alpha[2], d[2], theta3_virtual))

R30_virtual = T30_virtual[0:3,0:3]

p6to2_unit = p6to2 / np.linalg.norm(p6to2)

R_psito0 = np.eye(3) + math.sin(np.deg2rad(arm_angle)) * skew(p6to2_unit.T[0]) + (1 - math.cos(np.deg2rad(arm_angle))) * np.dot(skew(p6to2_unit.T[0]), skew(p6to2_unit.T[0]))

As = np.dot(skew(p6to2_unit.T[0]), R30_virtual)
Bs = -np.dot(np.dot(skew(p6to2_unit.T[0]), skew(p6to2_unit.T[0])), R30_virtual)
Cs = np.dot(np.dot(p6to2_unit, p6to2_unit.T), R30_virtual)

theta1 = np.rad2deg(math.atan2(GC2 * (As[1][1] * math.sin(np.deg2rad(arm_angle)) + Bs[1][1] * math.cos(np.deg2rad(arm_angle)) + Cs[1][1]), GC2 * (As[0][1] * math.sin(np.deg2rad(arm_angle)) + Bs[0][1] * math.cos(np.deg2rad(arm_angle)) + Cs[0][1])))
theta2 = GC2 * np.rad2deg(np.arccos(As[2][1] * math.sin(np.deg2rad(arm_angle)) + Bs[2][1] * math.cos(np.deg2rad(arm_angle)) + Cs[2][1]))
theta3 = np.rad2deg(math.atan2(GC2 * (-As[2][2] * math.sin(np.deg2rad(arm_angle)) - Bs[2][2] * math.cos(np.deg2rad(arm_angle)) - Cs[2][2]), GC2 * (-As[2][0] * math.sin(np.deg2rad(arm_angle)) - Bs[2][0] * math.cos(np.deg2rad(arm_angle)) - Cs[2][0])))

T43 = transformationMatrix(a[3], alpha[3], d[3], theta4)
R43 = T43[0:3,0:3]

Aw = np.dot(np.dot(R43.T, As.T), R)
Bw = np.dot(np.dot(R43.T, Bs.T), R)
Cw = np.dot(np.dot(R43.T, Cs.T), R)

theta5 = np.rad2deg(math.atan2(GC6 * (Aw[1][2] * math.sin(np.deg2rad(arm_angle)) + Bw[1][2] * math.cos(np.deg2rad(arm_angle)) + Cw[1][2]), GC6 * (Aw[0][2] * math.sin(np.deg2rad(arm_angle)) + Bw[0][2] * math.cos(np.deg2rad(arm_angle)) + Cw[0][2])))
theta6 = GC6 * np.rad2deg(np.arccos(Aw[2][2] * math.sin(np.deg2rad(arm_angle)) + Bw[2][2] * math.cos(np.deg2rad(arm_angle)) + Cw[2][2]))
theta7 = np.rad2deg(math.atan2(GC6 * (Aw[2][1] * math.sin(np.deg2rad(arm_angle)) + Bw[2][1] * math.cos(np.deg2rad(arm_angle)) + Cw[2][1]), GC6 * (-Aw[2][0] * math.sin(np.deg2rad(arm_angle)) - Bw[2][0] * math.cos(np.deg2rad(arm_angle)) - Cw[2][0])))

print(f"theta1的角度为{theta1}\ntheta2的角度为{theta2}\ntheta3的角度为{theta3}\ntheta4的角度为{theta4}\ntheta5的角度为{theta5}\ntheta6的角度为{theta6}\ntheta7的角度为{theta7}")

```

最后修改：2024 年 04 月 17 日

如果觉得我的文章对你有用，请随意赞赏

发表评论取消回复
使用cookie技术保留您的个人信息以便您下次快速评论，继续评论表示您已同意该条款

评论 *

私密评论

名称 *

🎲

邮箱 *

地址

LBR Med逆向运动学

L.CH • 2024 年 04 月 09 日

由于Med/IIWA为7自由度冗余机械臂，那么其运动学逆解必然很难求解，但有2种方法：

## 降低自由度

## 增加自由度

重点说明增加自由度的方法

参考的是下篇文章

https://doi.org/10.1016/j.mechmachtheory.2017.10.025

需要定义以下几个变量X, Y, Z, RX, RY, RZ, arm_angle，还有全局变量GC2, GC4, GC6

完整代码如下：

```python
#!/usr/bin/env python
# coding: utf-8
import numpy as np
import math

# np.set_printoptions(11)

dbs, dse, dew, dwf = 360, 420, 400, 126

a = [0, 0, 0, 0, 0, 0, 0]
alpha = [-math.pi/2, math.pi/2, math.pi/2, -math.pi/2, -math.pi/2, math.pi/2, 0]
d = [360, 0, 420, 0, 400, 0, 126]
theta_ini = [0, 0, 0, 0, 0, 0, 0]

def skew(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

p2to0 = np.array([[0, 0, dbs]]).T
p4to2 = np.array([[0, dse, 0]]).T
p6to4 = np.array([[0, 0, dew]]).T
p7to6 = np.array([[0, 0, dwf]]).T

theta3_virtual = 0

# 定义末端变量和臂型角
x, y, z, rx, ry, rz = -409.79, -410.04, 248.03, 179.99, 0.03, -134.98
arm_angle = 90
# 定义全局变量Global Configuration
GC2, GC4, GC6 = -1, 1, -1

T = []
for i in range(7):
    T.append(transformationMatrix(a[i], alpha[i], d[i], theta_ini[i]))

R = rotateMatrix(rx, ry, rz)

p6to2 = np.array([[x, y, z]]).T - p2to0 - (np.dot(R, p7to6))

theta4 = np.rad2deg(GC4 * math.acos((pow(np.linalg.norm(p6to2),2)- pow(dse,2) - pow(dew,2)) / (2 * dse * dew)))

if np.linalg.norm(np.cross(p6to2.T, T[0][0:3,2:3].T)) == 0:
    theta1_virtual = 0
else:
    theta1_virtual = np.rad2deg(math.atan2(p6to2[1][0], p6to2[0][0])) + 180

phi = np.rad2deg(math.acos((pow(dse,2) + pow(np.linalg.norm(p6to2),2) - pow(dew,2)) / (2 * dse * np.linalg.norm(p6to2))))

theta2_virtual = GC2 * np.rad2deg(math.atan2(math.sqrt(pow(p6to2[0][0],2) + pow(p6to2[1][0],2)), p6to2[2][0])) + GC4 * phi

theta3_virtual = 0

R30_virtual = T30_virtual[0:3,0:3]

p6to2_unit = p6to2 / np.linalg.norm(p6to2)

R_psito0 = np.eye(3) + math.sin(np.deg2rad(arm_angle)) * skew(p6to2_unit.T[0]) + (1 - math.cos(np.deg2rad(arm_angle))) * np.dot(skew(p6to2_unit.T[0]), skew(p6to2_unit.T[0]))

As = np.dot(skew(p6to2_unit.T[0]), R30_virtual)
Bs = -np.dot(np.dot(skew(p6to2_unit.T[0]), skew(p6to2_unit.T[0])), R30_virtual)
Cs = np.dot(np.dot(p6to2_unit, p6to2_unit.T), R30_virtual)

T43 = transformationMatrix(a[3], alpha[3], d[3], theta4)
R43 = T43[0:3,0:3]

Aw = np.dot(np.dot(R43.T, As.T), R)
Bw = np.dot(np.dot(R43.T, Bs.T), R)
Cw = np.dot(np.dot(R43.T, Cs.T), R)

```

发表评论 取消回复 使用cookie技术保留您的个人信息以便您下次快速评论，继续评论表示您已同意该条款

LBR Med逆向运动学

发表评论取消回复
使用cookie技术保留您的个人信息以便您下次快速评论，继续评论表示您已同意该条款