Skip to content

lab3 机械臂正运动学求解

写出ZJU-I型桌面机械臂的DH参数

根据这个图:

image-20241007164554401

再结合标准DH参数的定义,可以写出D-H参数表:

Frame No. \(a_i\) \(\alpha_i\) \(d_i\) \(\theta_i\)
1 0 -90 230 \(\theta_1\)
2 185 0 0 \(\theta_2\left(-90\right)\)
3 170 0 0 \(\theta_3\)
4 0 90 23 \(\theta_4\left(90\right)\)
5 0 90 77 \(\theta_5\left(90\right)\)
6 0 0 85.5 \(\theta_6\)

写出ZJU-I型机械臂的正运动学解,采用XY’Z’欧拉角表示末端执行器姿态

通过D-H参数表,我们就可以写出相应的 Transformation Matrix.

image-20241007165722314

根据这些矩阵,我们就可以通过矩阵乘法求解末端执行器的位置和姿态。其中,矩阵的最后一列的前三行表示其位置坐标,左上角\(3\times3\)的子矩阵就是它的旋转矩阵,通过这个可以求解出最后的姿态。

值得注意的是,通过欧拉角表示的时候,其数值和旋转顺序有很大的关系。

在我们上课的时候学的是X→Y→Z的旋转顺序,而仿真软件Coppelia中用的是Z→Y→X的旋转顺序,对应的结果是不一样的。

根据上课学的东西,我们分别有:

\[ \begin{align*} R_x &= \left(\begin{array}{ccc} 1 & 0 & 0\\ 0 & \cos\left(a_{1}\right) & -\sin\left(a_{1}\right)\\ 0 & \sin\left(a_{1}\right) & \cos\left(a_{1}\right) \end{array}\right)\\ R_y &= \left(\begin{array}{ccc} \cos\left(a_{2}\right) & 0 & \sin\left(a_{2}\right)\\ 0 & 1 & 0\\ -\sin\left(a_{2}\right) & 0 & \cos\left(a_{2}\right) \end{array}\right)\\ R_z &= \left(\begin{array}{ccc} \cos\left(a_{3}\right) & -\sin\left(a_{3}\right) & 0\\ \sin\left(a_{3}\right) & \cos\left(a_{3}\right) & 0\\ 0 & 0 & 1 \end{array}\right) \end{align*} \]

这三个矩阵就是分别绕X,Y,Z轴的旋转矩阵(\(a_1,a_2,a_3\)分别表示其绕X,Y,Z轴旋转的角度),而result1result2分别表示了两种不同的旋转顺序。

我们可以使用MATLAB很方便地求解出旋转矩阵:

当按照X→Y→Z的顺序旋转时,旋转矩阵是:

\[ \left(\begin{array}{ccc} \cos\left(a_{2}\right)\,\cos\left(a_{3}\right) & -\cos\left(a_{2}\right)\,\sin\left(a_{3}\right) & \sin\left(a_{2}\right)\\ \cos\left(a_{1}\right)\,\sin\left(a_{3}\right)+\cos\left(a_{3}\right)\,\sin\left(a_{1}\right)\,\sin\left(a_{2}\right) & \cos\left(a_{1}\right)\,\cos\left(a_{3}\right)-\sin\left(a_{1}\right)\,\sin\left(a_{2}\right)\,\sin\left(a_{3}\right) & -\cos\left(a_{2}\right)\,\sin\left(a_{1}\right)\\ \sin\left(a_{1}\right)\,\sin\left(a_{3}\right)-\cos\left(a_{1}\right)\,\cos\left(a_{3}\right)\,\sin\left(a_{2}\right) & \cos\left(a_{3}\right)\,\sin\left(a_{1}\right)+\cos\left(a_{1}\right)\,\sin\left(a_{2}\right)\,\sin\left(a_{3}\right) & \cos\left(a_{1}\right)\,\cos\left(a_{2}\right) \end{array}\right) \]

而按照Z→Y→X的顺序旋转的时候,旋转矩阵是:

\[ \left(\begin{array}{ccc} \cos\left(a_{2}\right)\,\cos\left(a_{3}\right) & \cos\left(a_{3}\right)\,\sin\left(a_{1}\right)\,\sin\left(a_{2}\right)-\cos\left(a_{1}\right)\,\sin\left(a_{3}\right) & \sin\left(a_{1}\right)\,\sin\left(a_{3}\right)+\cos\left(a_{1}\right)\,\cos\left(a_{3}\right)\,\sin\left(a_{2}\right)\\ \cos\left(a_{2}\right)\,\sin\left(a_{3}\right) & \cos\left(a_{1}\right)\,\cos\left(a_{3}\right)+\sin\left(a_{1}\right)\,\sin\left(a_{2}\right)\,\sin\left(a_{3}\right) & \cos\left(a_{1}\right)\,\sin\left(a_{2}\right)\,\sin\left(a_{3}\right)-\cos\left(a_{3}\right)\,\sin\left(a_{1}\right)\\ -\sin\left(a_{2}\right) & \cos\left(a_{2}\right)\,\sin\left(a_{1}\right) & \cos\left(a_{1}\right)\,\cos\left(a_{2}\right) \end{array}\right) \]

在这篇报告中,为了和仿真软件对应,我们统一采用第一种旋转矩阵。

所以,就可以通过这个旋转矩阵求解欧拉角:

# phi, theta, psi分别表示绕X,Y,Z轴旋转的角度。
def T2eularAngle(T):
    R = T[:3, :3]
    location = T[:3, 3] / 1000
    theta = math.asin(R[0, 2]) * 180 / pi
    phi = math.atan2(-R[1, 2], R[2, 2]) * 180 / pi
    psi = math.atan2(-R[0, 1], R[0, 0]) * 180 / pi
    # 拼接坐标和角度
    return np.hstack((location, np.array([phi, theta, psi])))

计算结果和仿真结果的对比

将以下5组关节角参数带入正运动学解,计算机械臂末端Tip点的空间位置,计算末端执行器的姿态,以XY’Z’欧拉角表示结果:

image-20241007170803098

实验组号 x y z \(\phi\) \(\theta\) \(\psi\)
1 0.095 0.164 0.608 -104.50 -3.33 -154.29
2 0.246 0.254 0.347 -123.69 -25.66 -76.10
3 -0.097 0.246 0.460 -120.00 -60.00 -150.00
4 -0.271 0.209 0.473 -13.06 7.43 150.85
5 0.226 0.107 0.552 -148.00 36.35 -107.00

计算结果

实验组号 x y z \(\phi\) \(\theta\) \(\psi\)
1 0.090 0.164 0.607 -104.54 -3.36 -154.30
2 0.245 0.254 0.347 -123.74 -25.69 -76.05
3 -0.097 0.246 0.460 -120.07 -60.00 -150.04
4 -0.272 0.209 0.472 -13.15 7.38 150.87
5 0.226 0.107 0.552 -148.05 36.31 -107.00

仿真结果

由上面两个表可以看出,我们按照正运动学计算出的结果和仿真软件跑出来的结果基本上是一致的,可能有一些误差导致仿真软件的结果和我们的计算有一点不一样。

这个过程还是比较简单的,也就是通过上面的变换矩阵算出来一个总的齐次变换矩阵,这个矩阵里面包含了末端执行点的位置和姿态信息,根据一些规则就可以把这个矩阵里面计算的结果变成我们需要的样子。

比较tricky的两个点,一个就是前面提到的旋转顺序,另外就是需要把计算得到的弧度制换算成角度制。

附件:正运动学源代码(python)

import numpy as np
import math
from math import pi

theta = [pi/12, pi/12, pi/12, pi/12, pi/12, pi/12]

theta = np.array([theta[0], theta[1] - pi/2, theta[2], theta[3] + pi/2, theta[4] + pi/2, theta[5]])

a1, alpha1, d1 = 0, -pi/2, 230
a2, alpha2, d2 = 185, 0, 0
a3, alpha3, d3 = 170, 0, 0
a4, alpha4, d4 = 0, pi/2, 23
a5, alpha5, d5 = 0, pi/2, 77
a6, alpha6, d6 = 0, 0, 85.5

DH = np.array([
    [theta[0], d1, a1, alpha1],
    [theta[1], d2, a2, alpha2],
    [theta[2], d3, a3, alpha3],
    [theta[3], d4, a4, alpha4],
    [theta[4], d5, a5, alpha5],
    [theta[5], d6, a6, alpha6]
])

def T2eularAngle(T):
    R = T[:3, :3]
    location = T[:3, 3] / 1000
    theta = math.asin(R[0, 2]) * 180 / pi
    phi = math.atan2(-R[1, 2], R[2, 2]) * 180 / pi
    psi = math.atan2(-R[0, 1], R[0, 0]) * 180 / pi
    # 拼接坐标和角度
    return np.hstack((location, np.array([phi, theta, psi])))

def DH_matrix(theta, d, a, alpha):
    return np.array([
        [np.cos(theta), -np.sin(theta)*np.cos(alpha), np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
        [np.sin(theta), np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]
    ])


def forward_kinematics(theta):
    Total_T = np.eye(4)
    for i in range(6):
        theta_i = theta[i]
        d_i = DH[i, 1]
        a_i = DH[i, 2]
        alpha_i = DH[i, 3]
        Total_T = np.dot(Total_T, DH_matrix(theta_i, d_i, a_i, alpha_i))

    result = T2eularAngle(Total_T)


    return result

if __name__ == '__main__':
    result = forward_kinematics(theta)
    print(result)