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
正运动学公式
逆运动学公式
平面连杆正运动学仿真
1import numpy as np
2from numpy import sin, cos
3import matplotlib.pyplot as plt
4
5def dhmat(delta, d, a, alpha):
6 return np.array([,
7 ,
8 ,
9 ])
10
11def 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().T
22 P_1 = np.array().T
23 P_2 = np.array().T
24
25 fig, ax = plt.subplots()
26
27 for i in range(len(theta_1)):
28 P1_0 = dhmat(theta_1, d_1, l_1, alpha_1).dot(P_1)
29 P2_0 = dhmat(theta_1, d_1, l_1, alpha_1).dot(dhmat(theta_2, d_2, l_2, alpha_2)).dot(P_2)
30 ax.plot(, P1_0, P2_0], , P1_0, P2_0], '-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
36if __name__ == '__main__':
37 main()
平面连杆逆运动学仿真
1import numpy as np
2from numpy import linalg as LA
3from numpy import sin, cos, arctan
4import matplotlib.pyplot as plt
5
6
7def dhmat(delta, d, a, alpha):
8 return np.array([,
9 ,
10 ,
11 ])
12
13def 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().T
27 err_dist = np.zeros(len(theta_1))
28
29 P_0 = np.array().T
30 P_1 = np.array().T
31 P_2 = np.array().T
32
33 fig, ax = plt.subplots()
34
35 for i in range(len(theta_1)):
36 P1_0 = dhmat(theta_1, d_1, l_1, alpha_1).dot(P_1)
37 theta_2 = arctan((-sin(theta_1) * des_P2_0 + cos(theta_1) * des_P2_0)
38 / (cos(theta_1) * des_P2_0 + sin(theta_1) * des_P2_0 - l_1))
39 P2_0 = dhmat(theta_1, d_1, l_1, alpha_1).dot(dhmat(theta_2, d_2, l_2, alpha_2)).dot(P_2)
40 err_dist = LA.norm(des_P2_0 - P2_0)
41 ax.plot(, P1_0, P2_0], , P1_0, P2_0], '-bo')
42
43 # 双连杆平面机械运动逆向运动学仿真
44 ax.plot(des_P2_0, des_P2_0, '-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.plot(theta_1, 'k')
53 ax.set_ylabel('theta_1')
54 ax.plot(theta_2, 'k')
55 ax.set_ylabel('theta_2')
56 ax.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
68if __name__ == '__main__':
69 main()
参考
机器人运动学中的D-H变换(https://zhuanlan.zhihu.com/p/20944328)
页:
[1]