用Jacobian矩阵计算6自由度机械手的逆运动学

private void calculateMatrixNumerically()
{
    var deltaTheta = 0.01;
    var newAxisConfiguration = new AxisConfiguration(
        this.currentAxisConfiguration.joints[0] + deltaTheta,
        this.currentAxisConfiguration.joints[1] + deltaTheta,
        this.currentAxisConfiguration.joints[2] + deltaTheta,
        this.currentAxisConfiguration.joints[3] + deltaTheta,
        this.currentAxisConfiguration.joints[4] + deltaTheta,
        this.currentAxisConfiguration.joints[5] + deltaTheta
        );

    var ePos = this.kinematic.CalculateDirectKinematic(newAxisConfiguration);

    for (var column = 0; column < this.Columns; column++)
    {
        this.M[0, column] = (this.currentPos.Point.X - ePos.Point.X) / deltaTheta;
        this.M[1, column] = (this.currentPos.Point.Y - ePos.Point.Y) / deltaTheta;
        this.M[2, column] = (this.currentPos.Point.Z - ePos.Point.Z) / deltaTheta;
        this.M[3, column] = (this.currentPos.Orientations[0].A - ePos.Orientations[0].A) / deltaTheta;
        this.M[4, column] = (this.currentPos.Orientations[0].B - ePos.Orientations[0].B) / deltaTheta;
        this.M[5, column] = (this.currentPos.Orientations[0].C - ePos.Orientations[0].C) / deltaTheta;
    }
}

private void calculateMatrixAnalytically()
{
    var peMatrix = calculateJointPositions();
    var zMatrix = calculateZ();

    for (var column = 0; column < this.Columns; column++)
    {
        double[] p_p = new double[3];
        for(var row = 0; row < zMatrix.Rows; row++)
        {
            p_p[row] = peMatrix.M[row, this.Columns-1] - peMatrix.M[row, column];
        }

        this.M[0, column] = zMatrix.M[1, column] * p_p[2] - zMatrix.M[2, column] * p_p[1];
        this.M[1, column] = zMatrix.M[2, column] * p_p[0] - zMatrix.M[0, column] * p_p[2];
        this.M[2, column] = zMatrix.M[0, column] * p_p[1] - zMatrix.M[1, column] * p_p[0];
        this.M[3, column] = zMatrix.M[0, column];
        this.M[4, column] = zMatrix.M[1, column];
        this.M[5, column] = zMatrix.M[2, column];
    }
}

/// <summary>
/// Calculate the positions of every joint.
/// </summary>
/// <returns>The Matrix with the joint coordinate for each joint.</returns>
private Matrix calculateJointPositions()
{
    Matrix jointPositions = new Matrix(3,6);
    Position pos;

    for (var joint= 0; joint< this.currentAxisConfiguration.joints.Count(); joint++)
    {
        pos = this.kinematic.CalculateDirectKinematic(this.currentAxisConfiguration, joint);
        jointPositions.M[0, joint] = pos.Point.X;
        jointPositions.M[1, joint] = pos.Point.Y;
        jointPositions.M[2, joint] = pos.Point.Z;
    }

    return jointPositions;
}

private Matrix calculateZ()
{
    // (z0^T-z1^T-z2^T-...-z6^T)
    var ksEnd = Kinematics.TranslateRobotToWorld();
    var zMatrix = new Matrix(3, 6);

    for (var column = 0; column < this.currentAxisConfiguration.joints.Count(); column++)
    {
        for (var row = 0; row < zMatrix.Rows; row++)
            zMatrix.M[row, column] = Math.Round(ksEnd.M[row, 2], 7);

        ksEnd = ksEnd.Multiply(
            Kinematics.TranslateCoordinateSystem(
            Robo.theta[column] + this.currentAxisConfiguration.joints[column],
            Robo.d[column],
            Robo.alpha[column],
            Robo.a[column])
            );
    }
    return zMatrix;
}

        do
        {
            jacob = JacobiMatrix.GetJacobi(currentPosition, currentAxisConfiguration);
            jacobiTranspose = jacob.getTransposeMatrix();

            // deltaE = (x2-x1, y2-y1, z2-z1, a2-a1, b2-b1, c2-c1)    
            deltaE = Position
                .GetDistanceVector(currentPosition, targetPosition);

            deltaThetas = jacobiTranspose.Multiply(deltaE).
                                    Scale(beta);

            for (var axis = 0; axis < deltaThetas.Rows; axis++ )
                currentAxisConfiguration.joints[axis] += deltaThetas.M[axis, 0];

            currentPosition = this.CalculateDirectKinematic(currentAxisConfiguration);
        } while (Math.Abs(Position.Distance(currentPosition, targetPosition)) > epsilon);

#1 楼

免责声明！

我会尝试解决您的问题，但它可能无法解决您的代码问题！


TL; DR：存在两个可能的错误您的代码。在伪代码中，您使用的是jacobian的转置，而不是jacobian的伪逆（如参考幻灯片中的建议）。另一个可能的错误是在解析的jacobian代码中计算叉积。


对于未来的读者来说，答案很长：

第一：基于梯度的jacobian方法，如果目标点不在机器人的工作空间之外，则您的算法会陷入无限循环，并试图达到错误阈值。在相反的方法中，雅各布会失去等级，并导致程序崩溃！

第二：请注意，在某些情况下，操纵器的6dof总是有一个封闭形式的解决方案，该解决方案为您提供有关IK位置和方向问题的6个答案。这些条件如下：


入射三个连续的旋转关节轴（例如球形腕）
三个连续的旋转关节轴平行

为了给您一个想法，请考虑一个拟人手臂。第一个关节将末端执行器带到答案所在的平面，然后两个关节解决位置问题。其余三个自由度猫都处理定向部分。该文件的第7页有一个简短的说明。

三：关于您的代码，我不确定以下内容是否正确（老实说，我不确定正在发生什么情况！）。

this.M[0, column] = zMatrix.M[1, column] * p_p[2] - zMatrix.M[2, column] * p_p[1];
this.M[1, column] = zMatrix.M[2, column] * p_p[0] - zMatrix.M[0, column] * p_p[2];
this.M[2, column] = zMatrix.M[0, column] * p_p[1] - zMatrix.M[1, column] * p_p[0];

要计算$ i ^ {th} $旋转关节运动对末端执行器速度的贡献，正确的方法是：

$$ J_ {Li}（q）= z_ {i-1} \ times（p_ {ee} -p_ {i-1}）$$
$$ J_ {Ai}（ q）= z_ {i-1} $$

其中


$ J_ {Li} $和$ J_ {Ai} $是线速度和角速度末端执行器的数量
$ q $是机器人当前配置的向量，而
$ z_ {i} $是表示关节在空间中的方向的向量。如果您使用的是Denavit Hartenberg约定，则这将是与关节相关的DH矩阵第三列的前三个元素。
$ p_ {i} $和$ p_ {ee} $是$ i ^的位置。 {th} $联合和末端执行器（DH第四列的前三个元素）。
最后但同样重要的是，$ \ times $是两个向量的叉积（我认为这是其中的一个）可能会导致您的代码出错）。

，这是计算几何jacobian的正确方法。一旦有了这个，就有几种方法可以用数字方式解决IK问题。除了转置，牛顿法，梯度法等。

要深入研究这些方法，请看一下这些非常好的幻灯片。







奖金！

我强烈建议，如果您要进行认真的机器人编程，请考虑使用一些线性代数库，例如boost :: numeric :: ublas或Eigen。它们快速，全面且通用。为了让您有一个深刻的了解，下面是如何在Eigen中为7dof机器人做一个简单的jacobian：

    //Hint:
    // A0i = A01*A12*A23...Ai
    // Aij = DH matrix related to transformation for ith to jth joint.
    Eigen::Vector3d z0, p0, pe;
        z0  << 0, 0, 1;
        p0  << 0, 0, 0;
        pe = A07.block<3,1>(0,3);

    // Ji linear velocity
    Jacobian.block<3,1>(0,0) = z0.cross(pe-p0);                                  
    Jacobian.block<3,1>(0,1) = A01.block<3,1>(0,2).cross(pe-A01.block<3,1>(0,3));
    Jacobian.block<3,1>(0,2) = A02.block<3,1>(0,2).cross(pe-A02.block<3,1>(0,3));
    Jacobian.block<3,1>(0,3) = A03.block<3,1>(0,2).cross(pe-A03.block<3,1>(0,3));
    Jacobian.block<3,1>(0,4) = A04.block<3,1>(0,2).cross(pe-A04.block<3,1>(0,3));
    Jacobian.block<3,1>(0,5) = A05.block<3,1>(0,2).cross(pe-A05.block<3,1>(0,3));
    Jacobian.block<3,1>(0,6) = A06.block<3,1>(0,2).cross(pe-A06.block<3,1>(0,3));

    // Ji angular velocity
    Jacobian.block<3,1>(3,0) = z0;                 
    Jacobian.block<3,1>(3,1) = A01.block<3,1>(0,2); 
    Jacobian.block<3,1>(3,2) = A02.block<3,1>(0,2); 
    Jacobian.block<3,1>(3,3) = A03.block<3,1>(0,2); 
    Jacobian.block<3,1>(3,4) = A04.block<3,1>(0,2); 
    Jacobian.block<3,1>(3,5) = A05.block<3,1>(0,2); 
    Jacobian.block<3,1>(3,6) = A06.block<3,1>(0,2); 

如果您确定自己的jacobian并非等级不足， ，计算右伪逆像这样简单：

inline Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> pInv(const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> &a) {
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> aT= a.transpose();
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> aaT = a*aT;
    return aT*(aaT.inverse());
}

评论

---

$ \ begingroup $
谢谢您的回答。接下来的几天我将详细介绍。使您感到困惑的代码行是$ J_ {Li}（q）= zi-1×（p_ {ee} -p_i-1）$的实现，其中$ p_p $是第i个联合位置之间的差和末端执行器位置。 $ zMatrix.M [1，列] $是关节的方向。 $ peMatrix $在每列中获得第i个关节的位置坐标，而$ zMatrix $在每列中获得第i个关节的方向。
$ \ endgroup $
–user3361135
2014年4月16日13:35

---