Bax的正向运动学

Question

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

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表来设置变换后，找到了巴克斯特正向运动学的精确指南。我将在上面wiki文章的末尾查看修改过的DH参数，因为指南使用了这些参数。在

巴克斯特前进运动指南：https://www.ohio.edu/mechanical-faculty/williams/html/pdf/BaxterKinematics.pdf

在本文中，作者Robert Williams为Baxter机械臂设置了DH参数，得到的值与您现有的不同（我知道您使用的是普通的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轴旋转：这表示机器人关节旋转到任意θ。

^{pr2}$

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

（alpha绕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矩阵都可以分解成这些步骤）。链条应该是这样的：

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

这基本上就是DH参数帮助你做的，但是我喜欢考虑从一帧到下一帧的各个步骤，而不是用DH参数跳到那里。在

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