OpenGL / Eigen3中的逆运动学：不稳定的Jacobian广义逆

Question

我正在尝试使用OpenGL、Eigen3和"Jacobian伪逆"方法实现简单的反向运动学测试。

使用“Jacobian转置”算法，系统运行良好，但是一旦我尝试使用“伪逆”，关节就变得不稳定并开始抽搐（最终它们完全冻结——除非我使用“Jacobian转置”回退计算）。我已经调查了这个问题，发现在某些情况下，Jacobian.inverse()*Jacobian的行列式为零，无法求逆。然而，我看到互联网上的其他演示（Youtube）声称使用相同的方法，他们似乎没有这个问题。因此，我不确定问题的原因在哪里。以下是代码：

*.h:

struct Ik{
    float targetAngle;
    float ikLength;
    VectorXf angles;
    Vector3f root, target;
    Vector3f jointPos(int ikIndex);
    size_t size() const;
    Vector3f getEndPos(int index, const VectorXf& vec);
    void resize(size_t size);
    void update(float t);
    void render();
    Ik(): targetAngle(0), ikLength(10){
    }
};

*.cpp:

size_t Ik::size() const{
    return angles.rows();
}

Vector3f Ik::getEndPos(int index, const VectorXf& vec){
    Vector3f pos(0, 0, 0);
    while(true){
        Eigen::Affine3f t;
        float radAngle = pi*vec[index]/180.0f;
        t = Eigen::AngleAxisf(radAngle, Vector3f(-1, 0, 0))
            * Eigen::Translation3f(Vector3f(0, 0, ikLength));
        pos = t * pos;

        if (index == 0)
            break;
        index--;
    }
    return pos;
}

void Ik::resize(size_t size){
    angles.resize(size);
    angles.setZero();
}

void drawMarker(Vector3f p){
    glBegin(GL_LINES);
    glVertex3f(p[0]-1, p[1], p[2]);
    glVertex3f(p[0]+1, p[1], p[2]);
    glVertex3f(p[0], p[1]-1, p[2]);
    glVertex3f(p[0], p[1]+1, p[2]);
    glVertex3f(p[0], p[1], p[2]-1);
    glVertex3f(p[0], p[1], p[2]+1);
    glEnd();
}

void drawIkArm(float length){
    glBegin(GL_LINES);
    float f = 0.25f;
    glVertex3f(0, 0, length);
    glVertex3f(-f, -f, 0);
    glVertex3f(0, 0, length);
    glVertex3f(f, -f, 0);
    glVertex3f(0, 0, length);
    glVertex3f(f, f, 0);
    glVertex3f(0, 0, length);
    glVertex3f(-f, f, 0);
    glEnd();
    glBegin(GL_LINE_LOOP);
    glVertex3f(f, f, 0);
    glVertex3f(-f, f, 0);
    glVertex3f(-f, -f, 0);
    glVertex3f(f, -f, 0);
    glEnd();
}

void Ik::update(float t){
    targetAngle += t * pi*2.0f/10.0f;
    while (t > pi*2.0f)
        t -= pi*2.0f;
    target << 0, 8 + 3*sinf(targetAngle), cosf(targetAngle)*4.0f+5.0f;

    Vector3f tmpTarget = target;
    Vector3f targetDiff = tmpTarget - root;
    float l = targetDiff.norm();
    float maxLen = ikLength*(float)angles.size() - 0.01f;
    if (l > maxLen){
        targetDiff *= maxLen/l;
        l = targetDiff.norm();
        tmpTarget = root + targetDiff;
    }

    Vector3f endPos = getEndPos(size()-1, angles);
    Vector3f diff = tmpTarget - endPos;


    float maxAngle = 360.0f/(float)angles.size();

    for(int loop = 0; loop < 1; loop++){
        MatrixXf jacobian(diff.rows(), angles.rows());
        jacobian.setZero();
        float step = 1.0f;
        for (int i = 0; i < angles.size(); i++){
            Vector3f curRoot = root;
            if (i)
                curRoot = getEndPos(i-1, angles);
            Vector3f axis(1, 0, 0);
            Vector3f n = endPos - curRoot;
            float l = n.norm();
            if (l)
                n /= l;
            n = n.cross(axis);
            if (l)
                n *= l*step*pi/180.0f;
            //std::cout << n << "\n";

            for (int j = 0; j < 3; j++)
                jacobian(j, i) = n[j];
        }

        std::cout << jacobian << std::endl;
        MatrixXf jjt = jacobian.transpose()*jacobian;
        //std::cout << jjt << std::endl;
        float d = jjt.determinant();
        MatrixXf invJ; 
        float scale = 0.1f;
        if (!d /*|| true*/){
            invJ = jacobian.transpose();
            scale = 5.0f;
            std::cout << "fallback to jacobian transpose!\n";
        }
        else{
            invJ = jjt.inverse()*jacobian.transpose();
            std::cout << "jacobian pseudo-inverse!\n";
        }
        //std::cout << invJ << std::endl;

        VectorXf add = invJ*diff*step*scale;
        //std::cout << add << std::endl;
        float maxSpeed = 15.0f;
        for (int i = 0; i < add.size(); i++){
            float& cur = add[i];
            cur = std::max(-maxSpeed, std::min(maxSpeed, cur));
        }
        angles += add;
        for (int i = 0; i < angles.size(); i++){
            float& cur = angles[i];
            if (i)
                cur = std::max(-maxAngle, std::min(maxAngle, cur));
        }
    }
}

void Ik::render(){
    glPushMatrix();
    glTranslatef(root[0], root[1], root[2]);
    for (int i = 0; i < angles.size(); i++){
        glRotatef(angles[i], -1, 0, 0);
        drawIkArm(ikLength);
        glTranslatef(0, 0, ikLength);
    }
    glPopMatrix();
    drawMarker(target);
    for (int i = 0; i < angles.size(); i++)
        drawMarker(getEndPos(i, angles));
}

- SigTerm

---

我怀疑你没有正确计算伪逆。我认为你应该使用Eigen::SVD来计算它。另请参见这里。 - sbabbi

我怀疑你没有正确计算伪逆。我已经提供了当前Ik类的完整源代码，所以如果你熟悉伪逆，你应该能够检查计算是否执行正确。 - SigTerm

你使用的OpenGL是旧的、已弃用的基于CPU的预管道方法，它没有像预期那样使用GPU和着色器。一旦你掌握了算法，我鼓励你重新考虑编写着色器而不是基于CPU的代码（即glBegin是一个警告标志）。 - Scott Stensland

Answer 1

听起来你的系统过于僵硬。

1. 尝试使用Eigen SVD方法：参见http://eigen.tuxfamily.org/dox-2.0/TutorialAdvancedLinearAlgebra.html。这个方法计算强度更大，但也更加安全。如果您正在解决一个aX=b问题，最好使用专门用于求逆矩阵的方法。（其中a是雅各比矩阵，X是您想要的结果）。
2. 最后，尝试通过将对角线乘以1.0001的因子来欺骗矩阵的条件。这不会改变解决方案（特别是对于游戏），但可以实现更好的解决。
3. 我很好奇为什么你选择不使用像RK4这样的时间迭代方案。

编辑：我没有仔细阅读你详细的帖子，所以我已经删除了关于弹簧的引用。我猜在你的情况下，元素会有某种形式的机械交互。

- Mikhail

---

"您的系统过于僵硬。" 系统没有任何硬度，也没有物理模拟，只有4个关节。 "尝试使用Eigen SVD方法" 我可能会尝试，但更愿意找出当前伪逆计算的问题所在。 "元素像振荡弹簧。" 它们不是弹簧，也不应该像弹簧一样行为。 "为什么选择不使用时间迭代方案" 因为没有“时间”。我需要/希望立即知道最终的关节配置，在当前帧中。 - SigTerm

关于SVD：据我所知，Ik的问题在于系统很可能有无限多个解或者没有解。我不确定你提到的模块是否适用于这种问题。 - SigTerm

#2 在一定程度上解决了问题。 - SigTerm

SVD可以从无限的选择中给出“最佳”结果。我认识很多从事机器人领域的人，他们会针对你所描述的问题使用SVD，而且我至少知道一个工业机器人也使用了SVD。 - Mikhail

"SVD" 我稍后会查看。不过，如果有代码示例的话，我会非常感激。 - SigTerm

我已经尝试了你关于SVD（Eigen 3中的JacobiSVD）的建议（通过jtjangles = jt*diff来解方程，求出角度），看起来它比雅可比伪逆方法稍微快一点，并且更加“稳定”。然而，我仍然需要将矩阵对角线乘以1.0001（否则会出现振荡），但一旦这个步骤完成，IK求解器就可以处理64到100个链接的链条而没有任何问题。相比之下，雅可比伪逆方法需要更大的对角线乘数才能在8到16个链接上工作。谢谢你的建议，我想... - SigTerm

Answer 2

应该可以这样做。

VectorXf solveViaSVD(const MatrixXf & jacobian,const VectorXf & diff) {
     Eigen::JacobiSVD<MatrixXf> svd (jacobian,ComputeThinU | ComputeThinV);
     return svd.solve(diff);
}

问题在于，正如您所说，当（J*J'）是奇异矩阵时，您的方法无法计算伪逆。 维基百科表示：“[伪逆]是通过将每个非零对角线条目替换为其倒数而形成的”。 将计算pinv(A)作为inv(A'*A)*A'失败了“非零”点。

- sbabbi

---

JacobiSVD 需要方阵吗？ - SigTerm

@SigTerm 我在一个2x4的矩阵上测试了这段代码，它运行良好（除了一些我无法真正理解的gcc警告）。 - sbabbi

好的，你的例子确实可以工作，但据我所知，你不需要ans变量。我已经为你的答案点赞，但是，我已经接受了Misha的答案，并且你在他的建议＃1中提供了与他相同的建议。 - SigTerm

@SigTerm，我最初使用的是eigen2，当我转移到eigen3时，我只是忘记删除那个“ans”。现在我已经修正了我的答案。 - sbabbi