Geek专栏：LejuRoban-1特别专题：双连杆机械臂运动学正向和...

本帖最后由 lou 于 2020-4-27 11:28 编辑

Geek专栏：LejuRoban-1特别专题：双连杆机械臂运动学正向和逆向 Python 仿真

今天Geek专栏为大家带来
乐聚机器人王松博士的
LejuRoban-1特别专题
“ 双连杆机械臂运动学正向和逆向 Python 仿真”

包括LejuRoban-1在内的机器人中前向运动学主要关心机械臂各关节的变量与机械臂末段机构位置与方向的关系。

DH(Denavit-Hartenberg)方法
DH 方法描述了相邻坐标系之间的旋转与移动的关系，只需要找到 4 个参数，以基座坐标（x0,y0,z0）为参考，就可以依次求出相连接的连杆坐标系（xi,yi,zi）和末端执行器坐标系（xn,yn,zn）的远点与方向。

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

xi和zi-1轴垂直
xi和zi-1轴相交

4 个 DH 参数分别为：

关节转角(joint angle) θi
连杆扭角(link twist) αi
连杆距离(link offset) di
连杆长度(link length) ai

正运动学公式

逆运动学公式

平面连杆正运动学仿真

1 import numpy as np
2 from numpy import sin, cos
3 import matplotlib.pyplot as plt
4
5 def dhmat(delta, d, a, alpha):
6 return np.array([[cos(delta), -sin(delta)*cos(alpha), sin(delta)*sin(alpha), a*cos(delta)],
7 [sin(delta), cos(delta)*cos(alpha), -cos(delta)*sin(alpha), a*sin(delta)],
8 [0, sin(alpha), cos(alpha), d],
9 [0, 0, 0, 1]])
10
11 def main():
12 theta_1 = np.radians(np.arange(20, 50, 1))
13 theta_2 = np.radians(np.arange(0, 30, 1))
14 d_1 = 0
15 d_2 = 0
16 l_1 = 0.1
17 l_2 = 0.2
18 alpha_1 = 0
19 alpha_2 = 0
20
21 P_0 = np.array([0, 0, 0, 1]).T
22 P_1 = np.array([0, 0, 0, 1]).T
23 P_2 = np.array([0, 0, 0, 1]).T
24
25 fig, ax = plt.subplots()
26
27 for i in range(len(theta_1)):
28 P1_0 = dhmat(theta_1[i], d_1, l_1, alpha_1).dot(P_1)
29 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)
30 ax.plot([P_0[0], P1_0[0], P2_0[0]], [P_0[1], P1_0[1], P2_0[1]], '-bo')
31 ax.set_xlabel('x (m)')
32 ax.set_ylabel('y (m)')
33 fig.savefig('./twp_link_planar_robot.png')
34 plt.show()
35
36 if __name__ == '__main__':
37 main()

复制代码

平面连杆逆运动学仿真

1 import numpy as np
2 from numpy import linalg as LA
3 from numpy import sin, cos, arctan
4 import matplotlib.pyplot as plt
5
6
7 def dhmat(delta, d, a, alpha):
8 return np.array([[cos(delta), -sin(delta)*cos(alpha), sin(delta)*sin(alpha), a*cos(delta)],
9 [sin(delta), cos(delta)*cos(alpha), -cos(delta)*sin(alpha), a*sin(delta)],
10 [0, sin(alpha), cos(alpha), d],
11 [0, 0, 0, 1]])
12
13 def main():
14 d_1 = 0
15 d_2 = 0
16 l_1 = 0.1
17 l_2 = 0.2
18 alpha_1 = 0
19 alpha_2 = 0
20
21 theta_1 = np.radians(np.arange(20, 50, 1))
22 theta_2 = np.zeros(len(theta_1))
23
24 st = 1
25
26 des_P2_0 = np.array([0.22, 0.2, 0, 1]).T
27 err_dist = np.zeros(len(theta_1))
28
29 P_0 = np.array([0, 0, 0, 1]).T
30 P_1 = np.array([0, 0, 0, 1]).T
31 P_2 = np.array([0, 0, 0, 1]).T
32
33 fig, ax = plt.subplots()
34
35 for i in range(len(theta_1)):
36 P1_0 = dhmat(theta_1[i], d_1, l_1, alpha_1).dot(P_1)
37 theta_2[i] = arctan((-sin(theta_1[i]) * des_P2_0[0] + cos(theta_1[i]) * des_P2_0[1])
38 / (cos(theta_1[i]) * des_P2_0[0] + sin(theta_1[i]) * des_P2_0[1] - l_1))
39 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)
40 err_dist[i] = LA.norm(des_P2_0 - P2_0)
41 ax.plot([P_0[0], P1_0[0], P2_0[0]], [P_0[1], P1_0[1], P2_0[1]], '-bo')
42
43 # 双连杆平面机械运动逆向运动学仿真
44 ax.plot(des_P2_0[0], des_P2_0[1], '-rx')
45 ax.set_xlabel('x (m)')
46 ax.set_ylabel('y (m)')
47 fig.savefig('./ik_two_link_planar_robot_0.png')
48 plt.show()
49
50 # theta_1, theta_2 角度变化轨迹
51 fig, ax = plt.subplots(nrows=2)
52 ax[0].plot(theta_1, 'k')
53 ax[0].set_ylabel('theta_1')
54 ax[1].plot(theta_2, 'k')
55 ax[1].set_ylabel('theta_2')
56 ax[1].set_xlabel('time (s)')
57 fig.savefig('./ik_two_link_planar_robot_1.png')
58 plt.show()
59
60 # 目标点与实际点之间的距离误差
61 fig, ax = plt.subplots()
62 ax.plot(err_dist, 'k')
63 ax.set_ylabel('distance error (m)')
64 ax.set_xlabel('time (s)')
65 fig.savefig('./ik_two_link_planar_robot_2.png')
66 plt.show()
67
68 if __name__ == '__main__':
69 main()