双连杆机械臂运动学正向和逆向 Python 仿真
双连杆机械臂运动学正向和逆向 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) \]
平面连杆正运动学仿真
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()
平面连杆逆运动学仿真
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()