Baxter 的正向运动学

Question

我根据其hardware specs 和以下关节轴为 Baxter 手臂机器人组合了这个正向运动学功能： 以下正向运动学的关节位置与对应的笛卡尔坐标不匹配，我在这里做错了什么？

def FK_function_2(joints):
    def yaw(theta): #(rotation around z)
        y = np.array([[np.cos(theta), -np.sin(theta), 0],
                      [np.sin(theta), np.cos(theta), 0],
                      [0, 0, 1] ])
        return y

    R01 = yaw(joints[0]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R12 = yaw(joints[1]).dot(np.array([[0,      0,   -1],
                                       [-1,     0,   0],
                                       [0,      1,   0]]))
    R23 = yaw(joints[2]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R34 = yaw(joints[3]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R45 = yaw(joints[4]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R56 = yaw(joints[5]).dot(np.array([[-1,     0,   0],
                                       [0,      0,   1],
                                       [0,      1,   0]]))
    R67 = yaw(joints[6]).dot(np.array([[1,      0,   0],
                                       [0,      1,   0],
                                       [0,      0,   1]]))

    d = np.array([0.27035, 0, 0.36435, 0, 0.37429, 0, 0.229525])
    a = np.array([0.069, 0, 0.069, 0, 0.010, 0, 0])

    l1 = np.array([a[0]*np.cos(joints[0]), a[0]*np.sin(joints[0]), d[0]]);
    l2 = np.array([a[1]*np.cos(joints[1]), a[1]*np.sin(joints[1]), d[1]]); 
    l3 = np.array([a[2]*np.cos(joints[2]), a[2]*np.sin(joints[2]), d[2]]); 
    l4 = np.array([a[3]*np.cos(joints[3]), a[3]*np.sin(joints[3]), d[3]]); 
    l5 = np.array([a[4]*np.cos(joints[4]), a[4]*np.sin(joints[4]), d[4]]);
    l6 = np.array([a[5]*np.cos(joints[5]), a[5]*np.sin(joints[5]), d[5]]);
    l7 = np.array([a[6]*np.cos(joints[6]), a[6]*np.sin(joints[6]), d[6]]);

    unit = np.array([0, 0, 0, 1])
    H0 = np.concatenate((np.concatenate((R01, l1.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H1 = np.concatenate((np.concatenate((R12, l2.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H2 = np.concatenate((np.concatenate((R23, l3.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H3 = np.concatenate((np.concatenate((R34, l4.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H4 = np.concatenate((np.concatenate((R45, l5.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H5 = np.concatenate((np.concatenate((R56, l6.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)
    H6 = np.concatenate((np.concatenate((R67, l7.reshape(3, 1)), axis=1), unit.reshape(1,4)), axis=0)


    T = H0.dot(H1).dot(H2).dot(H3).dot(H4).dot(H5).dot(H6)

    return T[0:3, 3]

Answer 1

好的，所以我一直在查看并检查了您的代码。该代码很好，可以与您定义的运动链一起使用，并从机器人手臂的底部转换到末端。

(H0 * H1 * H2 * H3 * H4 * H5 * H6) 是正确的运动链，其中每个表示从手臂底部开始的链中从一个关节到下一个关节的转换。

问题是你的转换是错误的。您对 H0 到 H6 的表示不正确，正是这些矩阵中的数字导致您的转换与发生的实际转换不匹配。您需要从底部一直到手臂末端进行正确的转换。除此之外，您的方法是正确的。

看起来您正在为转换矩阵使用普通的 DH 参数。您的 a 和 d 值（以及代码中未显示的 alpha）已关闭，并导致转换表达不正确。 DH参数见https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters。

在通过修改后的 DH 表设置转换后，我找到了 Baxter 正向运动学的精确指南以提供帮助。我会在上面的 wiki 文章末尾查看修改后的 DH 参数，因为指南使用了它。

百特正向运动学指南： https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

在本文中，作者 Robert Williams 为 Baxter 机械臂设置了 DH 参数，并获得了与您所拥有的不同的值（我知道您使用的是正常的 DH 参数，但我会考虑使用修改后的那些）。他的桌子是：

长度为：

并使用修改后的 DH 矩阵：

现在您可以计算矩阵 H0 到 H6，如果您有额外的 H7，还可以添加末端执行器几何形状。将它们全部组合在一起后，您应该获得正确的正向运动学变换（有关其他资源，请参见论文）。左右臂具有相同的运动学。

当您将它们相乘时，您会得到来自手臂底部的 x7、y7 和 z7 坐标的表达式，它们是关节旋转和机器人手臂几何形状的函数。有关 x7、y7 和 z7 的表达式，请参见第 17 页的论文。另请参见第 14 页了解各个转换。

不要忘记以弧度表示角度，因为您的代码使用常规三角函数。

最后一次更新： 我只记得我更容易一个接一个地考虑中间平移和旋转步骤（而不是直接跳到 DH 矩阵）。这两种方法是等效的，但我喜欢考虑从一个旋转框架到下一个旋转框架所需的每一步。

为此，您可以使用这些构建块。

纯翻译：

[1   0   0   u;
0    1   0   v;
0    0   1   w;
0    0   0    1]

其中 u 是从前一个 x 帧轴测量的从前一帧到新帧的距离。

其中 v 是从前一个 y 帧轴测量的从前一帧到新帧的距离。

其中 w 是从前一个 z 帧轴测量的从前一帧到新帧的距离。

以任意 theta 绕 z 轴旋转： 这表示机器人关节旋转到任意 theta。

[cos(theta)    -sin(theta)        0 0;
sin(theta)     cos(theta)         0 0;
0                   0             1 0;
0                   0             0 1]

围绕中间帧的旋转组合以到达最终帧位置：（这些角度通常以 pi/2 或 pi 为增量，以便能够到达最终方向） 可以使用如下所示的围绕中间 x 轴、y 轴或 z 轴的旋转。

（绕 x 轴旋转 alpha）

R_x(alpha) =         [1             0           0              0;
                      0         cos(alpha)  -sin(alpha)        0;
                      0         sin(alpha)  cos(alpha)         0;
                      0            0            0              1];

（绕 y 轴旋转 beta）

R_y(beta) =   [  cos(beta)     0      sin(beta)    0;
                     0         1          0        0;
                 -sin(beta)    0      cos(beta)    0;
                     0         0          0        1];

（围绕 z 轴旋转 gamma）：

[cos(gamma)  -sin(gamma)     0      0;
sin(gamma)    cos(gamma)     0      0;
       0          0          1      0;
       0          0          0      1]

因此，使用这些构建块，您可以构建从一帧到另一帧的步骤序列（基本上任何 H 矩阵都可以分解为这些步骤）。链将类似于：

[H](从前一帧到下一帧的变换) = [从前一个关节到新关节的​​纯平移，在前一个关节的帧中表示] * [围绕前一帧的 z 轴旋转 theta (对于关节) (由于关节有很多位置，θ 保留为符号）] * [所有其他中间旋转以到达新的关节框架方向，表示为围绕中间轴框架的旋转]

这基本上是 DH 参数可以帮助您做的事情，但我喜欢考虑从一帧到下一帧的各个步骤，而不是使用 DH 参数跳转到那里。

使用更正后的 H0 到 H6 转换，您的方法是正确的。只需在代码中更改 H0 到 H6 的定义。