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。

我找到了Baxter前向运动学的精确指南，以便在通过修改后的DH表设置转换后提供帮助。我会在上面的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页。

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

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

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

纯翻译：

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

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

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

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

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

[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围绕x轴旋转）

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

（按测试版围绕y轴旋转）

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

（通过伽玛绕z轴旋转）：

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

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

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

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

通过纠正的H0到H6转换，您的方法是正确的。只需在代码中更改H0到H6的定义即可。

Baxter的正向运动学

1 个答案: