双连杆机械臂运动学正向和逆向 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()