具有末端执行器方向的反向运动学？

Question

我正在尝试实现反向运动学求解器，但这一次即使端部执行器的方向也是如此。当末端执行器仅需要位置时，我成功了。

我了解到，在这种情况下，您可以像这样构造雅可比矩阵，其中w_i是全局空间中的第i_个旋转轴，p_i是从第i_个轴到目标位置的向量。

问题是当我必须在下面的公式中计算x_dot时。

当x_dot仅考虑位置且没有方向时，这非常简单。 但是现在x_dot需要6个条目（位置，方向）时，我不知道应该为方向部分做什么。我一直在用欧拉角表示程序中的方向。

我目前的想法只是将当前末端执行器的偏航，俯仰和横滚与目标的偏航，俯仰和横滚相减，然后将每个结果除以100。但这似乎有点复杂。有没有更好的方法来解决此问题？任何想法将不胜感激！

Answer 1

您需要将末端执行器的方向表示为3 x 3旋转矩阵。您可以在当前关节向量（Theta）下计算末端执行器的方向，然后向关节向量中的每个元素添加一个小的增量（在您的情况下为6个增量，因为六个关节）。在简单的情况下，如果您对每个微小的关节变化引起的尖端位置变化感兴趣，则可以计算出X，Y和Z位置的变化，这是theta位置的简单矢量减法，对于每个扰动的θ。要对角度执行相同操作，您需要找到将3x3 @ Theta（A）变为3x3 @ ThetaPrime（B）的旋转矩阵R。

因为A * R = B

AInv A R = AInv * B

AInve * A =身份

R = AInv * B

您可以从R中提取Delta滚动，俯仰，偏航Euler角。公式在这里

https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf

theta值是由每个theta的变化引起的偏航，俯仰和横滚变化。