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

DH(Denavit-Hartenberg)方法

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

JrAjaub.png!web

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}\)

正运动学公式

VzaERzj.png!web

逆运动学公式

\[ \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) \]

平面连杆正运动学仿真

zAFfU3F.png!web

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()

平面连杆逆运动学仿真

ZnyM3mB.png!web

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()

参考

机器人运动学中的D-H变换

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