双连杆机械臂运动学正向和逆向 Python 仿真

DH(Denavit-Hartenberg)方法

DH 方法描述了相邻坐标系之间的旋转与移动的关系,只需要找到 4 个参数,以基座坐标 \((x_0, y_0, z_0)\) 为参考,就可以依次求出相连接的连杆坐标系 \((x_i, y_i, z_i)\) 和末端执行器坐标系 \((x_n, y_n, z_n)\) 的远点与方向。

DH 方法要求相邻坐标系之间满足下面约定

  • \(x_i\)\(z_{i-1}\) 轴垂直;
  • \(x_i\)\(z_{i-1}\) 轴相交;

4 个 DH 参数分别为:

  • 关节转角(joint angle) \(\theta_{i}\)
  • 连杆扭角(link twist) \(\alpha_{i}\)
  • 连杆距离(link offset) \(d_{i}\)
  • 连杆长度(link length) \(a_{i}\)

正运动学公式

逆运动学公式

\[ \theta _ { 2 } = \tan ^ { - 1 } \left( \frac { \sin \theta _ { 2 } } { \cos \theta _ { 2 } } \right) = \tan ^ { - 1 } \left( \frac { \frac { - S _ { 1 } ^ { \theta } p _ { x } ^ { 0 } + C _ { 1 } ^ { \theta } p _ { y } ^ { 0 } } { l _ { 2 } } } { \frac { C _ { 1 } ^ { \theta } p _ { x } ^ { 0 } + S _ { 1 } ^ { \theta } p _ { y } ^ { 0 } - l _ { 1 } } { l _ { 2 } } } \right) = \tan ^ { - 1 } \left( \frac { - S _ { 1 } ^ { \theta } p _ { x } ^ { 0 } + C _ { 1 } ^ { \theta } p _ { y } ^ { 0 } } { C _ { 1 } ^ { \theta } p _ { x } ^ { 0 } + S _ { 1 } ^ { \theta } p _ { y } ^ { 0 } - l _ { 1 } } \right) \]

平面连杆正运动学仿真

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
import numpy as np
from numpy import sin, cos
import matplotlib.pyplot as plt

def dhmat(delta, d, a, alpha):
return np.array([[cos(delta), -sin(delta)*cos(alpha), sin(delta)*sin(alpha), a*cos(delta)],
[sin(delta), cos(delta)*cos(alpha), -cos(delta)*sin(alpha), a*sin(delta)],
[0, sin(alpha), cos(alpha), d],
[0, 0, 0, 1]])

def main():
theta_1 = np.radians(np.arange(20, 50, 1))
theta_2 = np.radians(np.arange(0, 30, 1))
d_1 = 0
d_2 = 0
l_1 = 0.1
l_2 = 0.2
alpha_1 = 0
alpha_2 = 0

P_0 = np.array([0, 0, 0, 1]).T
P_1 = np.array([0, 0, 0, 1]).T
P_2 = np.array([0, 0, 0, 1]).T

fig, ax = plt.subplots()

for i in range(len(theta_1)):
P1_0 = dhmat(theta_1[i], d_1, l_1, alpha_1).dot(P_1)
P2_0 = dhmat(theta_1[i], d_1, l_1, alpha_1).dot(dhmat(theta_2[i], d_2, l_2, alpha_2)).dot(P_2)
ax.plot([P_0[0], P1_0[0], P2_0[0]], [P_0[1], P1_0[1], P2_0[1]], '-bo')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
fig.savefig('./twp_link_planar_robot.png')
plt.show()

if __name__ == '__main__':
main()

平面连杆逆运动学仿真

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
import numpy as np
from numpy import linalg as LA
from numpy import sin, cos, arctan
import matplotlib.pyplot as plt


def dhmat(delta, d, a, alpha):
return np.array([[cos(delta), -sin(delta)*cos(alpha), sin(delta)*sin(alpha), a*cos(delta)],
[sin(delta), cos(delta)*cos(alpha), -cos(delta)*sin(alpha), a*sin(delta)],
[0, sin(alpha), cos(alpha), d],
[0, 0, 0, 1]])

def main():
d_1 = 0
d_2 = 0
l_1 = 0.1
l_2 = 0.2
alpha_1 = 0
alpha_2 = 0

theta_1 = np.radians(np.arange(20, 50, 1))
theta_2 = np.zeros(len(theta_1))

st = 1

des_P2_0 = np.array([0.22, 0.2, 0, 1]).T
err_dist = np.zeros(len(theta_1))

P_0 = np.array([0, 0, 0, 1]).T
P_1 = np.array([0, 0, 0, 1]).T
P_2 = np.array([0, 0, 0, 1]).T

fig, ax = plt.subplots()

for i in range(len(theta_1)):
P1_0 = dhmat(theta_1[i], d_1, l_1, alpha_1).dot(P_1)
theta_2[i] = arctan((-sin(theta_1[i]) * des_P2_0[0] + cos(theta_1[i]) * des_P2_0[1])
/ (cos(theta_1[i]) * des_P2_0[0] + sin(theta_1[i]) * des_P2_0[1] - l_1))
P2_0 = dhmat(theta_1[i], d_1, l_1, alpha_1).dot(dhmat(theta_2[i], d_2, l_2, alpha_2)).dot(P_2)
err_dist[i] = LA.norm(des_P2_0 - P2_0)
ax.plot([P_0[0], P1_0[0], P2_0[0]], [P_0[1], P1_0[1], P2_0[1]], '-bo')

# 双连杆平面机械运动逆向运动学仿真
ax.plot(des_P2_0[0], des_P2_0[1], '-rx')
ax.set_xlabel('x (m)')
ax.set_ylabel('y (m)')
fig.savefig('./ik_two_link_planar_robot_0.png')
plt.show()

# theta_1, theta_2 角度变化轨迹
fig, ax = plt.subplots(nrows=2)
ax[0].plot(theta_1, 'k')
ax[0].set_ylabel('theta_1')
ax[1].plot(theta_2, 'k')
ax[1].set_ylabel('theta_2')
ax[1].set_xlabel('time (s)')
fig.savefig('./ik_two_link_planar_robot_1.png')
plt.show()

# 目标点与实际点之间的距离误差
fig, ax = plt.subplots()
ax.plot(err_dist, 'k')
ax.set_ylabel('distance error (m)')
ax.set_xlabel('time (s)')
fig.savefig('./ik_two_link_planar_robot_2.png')
plt.show()

if __name__ == '__main__':
main()

参考