C++ OpenGL/Eigen3逆运动学：不稳定雅可比伪逆

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

使用“雅可比转置”算法，系统运行良好，但是，只要我尝试使用“伪逆”，关节就会变得不稳定并开始抖动（最终它们完全冻结-除非我使用“雅可比转置”回退计算）。我研究了这个问题，发现在某些情况下，Jacobian.inverse（）*Jacobian的行列式为零，不能反转。然而，我在互联网（Youtube）上看到了其他声称使用相同方法的演示，它们似乎没有这个问题。所以我不确定问题的原因在哪里。代码如下：

*.h:

*.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));
}

size\u t Ik:：size（）常量{
返回角度。行（）；
}
向量3f Ik:：getEndPos（整数索引、常量向量x和向量）{
向量3f位置（0,0,0）；
while（true）{
特征：：仿射3f t；
浮点雷达角=pi*vec[指数]/180.0f；
t=Eigen:：AngleAxisf（radAngle，Vector3f（-1,0,0））
*本征：：平移3f（向量3f（0，0，ikLength））；
pos=t*pos；
如果（索引==0）
打破
索引--；
}
返回pos；
}
void Ik:：调整大小（大小）{
角度。调整大小（大小）；
angles.setZero（）；
}
空图纸标记（矢量3F p）{
glBegin（GL_行）；
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）；
格伦德（）；
}
空心抽芯杆（浮动长度）{
glBegin（GL_行）；
浮球f=0.25f；
glVertex3f（0，0，长度）；
glVertex3f（-f，-f，0）；
glVertex3f（0，0，长度）；
glVertex3f（f，-f，0）；
glVertex3f（0，0，长度）；
glVertex3f（f，f，0）；
glVertex3f（0，0，长度）；
glVertex3f（-f，f，0）；
格伦德（）；
glBegin（GL_线_环）；
glVertex3f（f，f，0）；
glVertex3f（-f，f，0）；
glVertex3f（-f，-f，0）；
glVertex3f（f，-f，0）；
格伦德（）；
}
无效Ik:：更新（浮动t）{
目标角+=t*pi*2.0f/10.0f；
而（t>pi*2.0f）
t-=pi*2.0f；
目标（最大值）{
targetDiff*=最大值/升；
l=目标差异规范（）；
tmpTarget=root+targetDiff；
}
向量3f endPos=getEndPos（大小（）-1，角度）；
向量3f diff=tmpTarget-endPos；
浮点最大角度=360.0f/（浮点）角度。大小（）；
for（int-loop=0；loop<1；loop++）{
矩阵xxf雅可比矩阵（diff.rows（），angles.rows（））；
jacobian.setZero（）；
浮动步长=1.0f；
对于（int i=0；i//听起来你的系统太僵硬了

尝试使用特征奇异值分解方法：参见。这在计算上更密集，但也可能更安全。如果您要解决aX=b问题，最好使用专用于矩阵求逆的方法。（a是您的雅可比矩阵，X是您想要的）
最后，试着通过将对角线乘以1.0001这样的因子来欺骗矩阵的条件。这不会对解决方案有太大的改变（特别是对于游戏而言），但可以实现更好的解决方案
我很好奇为什么您选择不使用时间迭代方案，比如RK4
编辑：我没有读太多你的博文，所以我删除了对弹簧的引用。我想在你的例子中，元素会有某种形式的机械交互作用。
类似的东西应该会起作用

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

VectorXf-solveViaSVD（常数矩阵xf&jacobian，常数向量xf&diff）{
本征：雅可比svd（雅可比矩阵，Computetinu | Computetinv）；
返回svd.solve（diff）；
}

问题是，正如您所说，当（
J*J'
）是奇异的时，您的方法无法计算伪逆

表示：“[伪逆]是通过将每个非零对角项替换为其倒数来形成的。”计算
pinv（A）
as
inv（A'*A）*A'
会使“非零”点失败。
我怀疑您没有正确计算伪逆。我认为您应该使用Eigen:：SVD来计算它。另请参阅。“我怀疑您没有正确计算伪逆"我已经为当前的Ik类提供了完整的源代码，因此如果您熟悉伪逆，您应该能够检查计算是否正确执行。您使用的OpenGL是旧的基于预管道CPU的方法，它不按预期使用GPU和着色器。一旦您掌握了算法，我鼓励您修改t编写着色器而不是基于CPU的代码（即glBegin是一个红旗）“您的系统太僵硬了。”系统没有任何类型的僵硬，没有物理模拟，只有4个关节。“尝试使用特征SVD方法”我可能会尝试一下，但更愿意找出当前伪逆计算的错误。“作为振荡弹簧的元素。”它们不是弹簧，也不应该表现得像弹簧。“为什么选择不使用时间迭代方案”，因为没有“时间”“。我需要/想要立即知道当前帧中的最终关节配置。关于SVD：据我所知，Ik的问题是系统很可能有无限多个解决方案或没有解决方案。我不确定您提到的模块是否适合此类问题。#2在某种程度上修复了问题。SVD为您提供了“最佳”解决方案。”我认识很多从事机器人技术的人，他们会为你所描述的问题提供SVD，我知道至少有一个工业机器人使用SVD。“SVD”我稍后会查看。我希望有一个代码示例，t
VectorXf solveViaSVD(const MatrixXf & jacobian,const VectorXf & diff) {
     Eigen::JacobiSVD<MatrixXf> svd (jacobian,ComputeThinU | ComputeThinV);
     return svd.solve(diff);
}