复现trac_ik:使用Msnhnet实现串联机器人运动数值逆解_2.5倍于trac_ik(二）

上一篇文章对于串联机械臂逆解问题, 使用Newton Raphson法进行了简单的实现.但是其速度和准确率都存在比较大的问题.
使用Msnhnet实现串联机器人运动数值逆解vs KDL(一）

1.改进一

首先, 对于机械臂来说, 一般都会有角度限位, 那么可以对每次迭代之后的 \Theta 进行角度限制, 缩小搜索范围, 可以提高求解效率.

2. 改进二

使用最大迭代次数作为求解结束的条件,那么对于最终迭代时间是无法确定的, 比如迭代7轴的臂和迭代4轴的臂,迭代次数一样的前提下, 迭代时间肯定不一样.那么, 就需要对迭代时间进行把控. 所以, 可以设置最大迭代时间, 不管有没有达到最大迭代次数, 只要达到了最大迭代时间, 就退出迭代.

3. 改进三

之前的实现, 在迭代过程中, 会出现困在局部最小值的情况. 所以需要对陷入局部最小值的情况进行处理. 那么我们可以通过以下条件进行判断.
$${\Theta }_{i+1}-{\Theta }_i\approx 0,diff(T_{i+1},T_{desire})>eps$$
出现以上情况时, 可以在最小限位和最大限位的角度中, 随机出一个点, 让其跳出局部最小.

• 代码实现(源码)

int Chain::ikNewtonRR(const Frame &desireFrame, VectorXSDS &outJoints, const Twist &bounds, const bool &randomStart, const bool &wrap, int maxIter, double eps, double maxTime)
{

    if(outJoints.mN != _numOfJoints)
    {
        return -2;
    }

    Frame frame;
    Twist dtTwist;
    auto joints     = outJoints;

    double timeM = maxTime * 1000.0;
    double timeNow = 0;

    MatSDS jac(_numOfJoints,6);

    auto st = std::chrono::high_resolution_clock::now();

    for (int i = 0; i < maxIter; ++i)
    {

        auto so = std::chrono::high_resolution_clock::now();

        timeNow = std::chrono::duration <double,std::milli> ((so-st)).count();

        if(timeNow > timeM)
        {
            return -6;
        }

        frame = fk(joints);

        dtTwist = Frame::diffRelative(desireFrame,frame);  

        if (std::abs(dtTwist.v[0]) <= std::abs(bounds.v[0]))
            dtTwist.v[0] = 0;

        if (std::abs(dtTwist.v[1]) <= std::abs(bounds.v[1]))
            dtTwist.v[1] = 0;

        if (std::abs(dtTwist.v[2]) <= std::abs(bounds.v[2]))
            dtTwist.v[2] = 0;

        if (std::abs(dtTwist.omg[0]) <= std::abs(bounds.omg[0]))
            dtTwist.omg[0] = 0;

        if (std::abs(dtTwist.omg[1]) <= std::abs(bounds.omg[1]))
            dtTwist.omg[1] = 0;

        if (std::abs(dtTwist.omg[2]) <= std::abs(bounds.omg[2]))
            dtTwist.omg[2] = 0;

        if(dtTwist.closeToEps(eps))
        {

            outJoints = joints;
            return i;
        }

        dtTwist = Frame::diff(frame,desireFrame);

        jacobi(jac,joints);

        auto UDVt = jac.svd();
        double sum = 0;
        VectorXSDS tmp(jac.mWidth);

        VectorXSDS dtJoints(jac.mWidth);

        for (int i = 0; i < jac.mWidth; ++i)
        {
            sum = 0;
            for (int j = 0; j < jac.mHeight; ++j)
            {
                sum +=  UDVt[0](i,j)*dtTwist[j];
            }

            if(fabs(UDVt[1][i])<eps)
            {
                tmp[i] = 0;
            }
            else
            {
                tmp[i] = sum/UDVt[1][i];
            }
        }

        for (int i = 0; i < jac.mWidth; ++i)
        {
            sum = 0;
            for (int j=0;j<jac.mWidth;j++)
            {
                sum+=UDVt[2](i,j)*tmp[j];
            }

            dtJoints[i] = sum;
        }

        VectorXSDS currJoints(joints.mN);

        for (int i = 0; i < joints.mN; ++i)
        {
            currJoints[i] = joints[i] + dtJoints[i];

            auto mtype      = _jointMoveTypes[i];
            auto minJoint   = _minJoints[i];
            auto maxJoint   = _maxJoints[i];

            if( mtype == Joint::ROT_CONTINUOUS_MOVE)
            {
                continue;
            }

            if(currJoints[i] < minJoint)
            {
                if(!wrap ||  mtype == Joint::TRANS_MOVE)
                {
                    currJoints[i] = minJoint;
                }
                else
                {
                    double diffAngle = fmod(minJoint - currJoints[i], MSNH_2_PI);
                    double currAngle = minJoint - diffAngle + MSNH_2_PI;
                    if (currAngle > maxJoint)
                        currJoints[i] = minJoint;
                    else
                        currJoints[i] = currAngle;
                }
            }

            if(currJoints[i] > maxJoint)
            {
                if(!wrap ||  mtype == Joint::TRANS_MOVE)
                {
                    currJoints[i] = maxJoint;
                }
                else
                {
                    double diffAngle = fmod(currJoints[i] - maxJoint, MSNH_2_PI);
                    double currAngle = maxJoint + diffAngle - MSNH_2_PI;
                    if (currAngle < minJoint)
                        currJoints[i] = maxJoint;
                    else
                        currJoints[i] = currAngle;
                }
            }

            joints[i] = joints[i] - currJoints[i];
        }

        if(joints.isFuzzyNull(MSNH_F32_EPS))
        {
            if(randomStart)
            {
                for (int j = 0; j < currJoints.mN; j++)
                {
                    if (_jointMoveTypes[j] == Joint::ROT_CONTINUOUS_MOVE)
                        currJoints[j] = fRand(currJoints[j] - MSNH_2_PI, currJoints[j] + MSNH_2_PI);
                    else
                        currJoints[j] = fRand(_minJoints[j], _maxJoints[j]);
                }
            }
        }

        joints = currJoints;
    }
    return -1;
}

• 测试

使用puma560机器人模型，分别测试约20万组点，在0点开始迭代，与改进之前的效果对比, 在成功率上提升不大, 但是在速度上获得了约1.3倍提升(Win10 I7 10700KF).

4. 改进四(思路来源于Trac_IK)

理论上来说, 关节是无约束运动, 采用Newton法这类无约束最优化方法可以获得比较好的结果. 但是在实际机器人求解中, 许多机器人的关节都有严格的关节限制, 通过Newton Raphson法迭代的结果, 肯定会超出关节极限. 所以每次在在超过极限位置后, 需要把结果约束到最大值和最小值之间, 故而其搜索空间不再平滑.

为解决这一问题, 可以采用有约束的优化方法用于求解. 这里采用序列二次规划的方法(SQP)用来求解, SQP算法是目前公认的求解约束非线性优化问题最有效的方法之一,具有收敛性好、计算效率高、边界搜索能力强的优点.

SQP算法思想

对于一般约束最优化问题::
$$\begin{gathered} minf(x), \\ s.t.c_i(x)=0,i\in \mathcal{E}, \\ {c}_i(x)\ge 0,i\in \mathcal{I} \end{gathered}$$
利用泰勒展开将非线性约束问题的目标函数在迭代点 $x_k$ 处简化成二次函数，将约束条件简化成线性函数后得到如下二次规划问题来求解.
$$\begin{gathered} min\frac{1}{2}{d}^T{W}_{k}d+g_k^{T}d+f_k, \\ s.t.c_i(x_k)+a_i(x_k{)}^T=0,i\in \mathcal{E}, \\ {c}_i(x_k)+a_i(x_k{)}^{T}d\ge 0,i\in \mathcal{I} \end{gathered}$$
其中对于上式中的拉格朗日函数的Hessian矩阵 $W_k$ ,使用拟牛顿法进行修正. 拟牛顿法可以使用BFGS公式或者DFP公式进行计算.

具体原理可以参看numerical optimization

这里SQP方法采用nlopt库中的SLSQP算子, SLSQP采用BFGS拟牛顿法对二次规划方法中的Hessian矩阵进行拟合, 同样, BFGS也存在局部最小值问题, 一样需要在局部最小时加入随机项.

目标函数选取,

误差项$e$的计算详见上一篇文章:
$$minf=e\cdot e^T$$

• 代码实现(源码)

// =============================== 目标函数 ==========================================
inline double minfuncSumSquared(const std::vector<double>& x, std::vector<double>& grad, void* data)
{
    Chain *chain = (Chain *) data;

    std::vector<double> vals(x);

    double jump = 0.000001;
    double result[1];
    chain->cartSumSquaredErr(vals, result);

    if (!grad.empty())
    {
        double v1[1];
        for (unsigned int i = 0; i < x.size(); i++)
        {
            double original = vals[i];

            vals[i] = original + jump;
            chain->cartSumSquaredErr(vals, v1);

            vals[i] = original;
            grad[i] = (v1[0] - result[0]) / (2.0 * jump);
        }
    }

    return result[0];
}

void Chain::cartSumSquaredErr(const std::vector<double> &x, double error[])
{
    if(_optStatus != -3)
    {
        _opt.force_stop();
        return;
    }

    Frame frame = fk(x);

    if(std::isnan(frame.trans[0]))
    {
        error[0] = std::numeric_limits<float>::max();
        _optStatus = -1;
        return;
    }

    Twist dtTwist = Frame::diffRelative(_desireFrameSQP, frame);

    if (std::abs(dtTwist.v[0]) <= std::abs(_boundsSQP.v[0]))
        dtTwist.v[0] = 0;

    if (std::abs(dtTwist.v[1]) <= std::abs(_boundsSQP.v[1]))
        dtTwist.v[1] = 0;

    if (std::abs(dtTwist.v[2]) <= std::abs(_boundsSQP.v[2]))
        dtTwist.v[2] = 0;

    if (std::abs(dtTwist.omg[0]) <= std::abs(_boundsSQP.omg[0]))
        dtTwist.omg[0] = 0;

    if (std::abs(dtTwist.omg[1]) <= std::abs(_boundsSQP.omg[1]))
        dtTwist.omg[1] = 0;

    if (std::abs(dtTwist.omg[2]) <= std::abs(_boundsSQP.omg[2]))
        dtTwist.omg[2] = 0;

    error[0] = Vector3DS::dotProduct(dtTwist.v,dtTwist.v) + Vector3DS::dotProduct(dtTwist.omg,dtTwist.omg);

    if(dtTwist.closeToEps(_epsSQP))
    {
        _optStatus = 1;
        _bestXSQP  = x;
        return;
    }
}
// ===============================================================================


// SQP迭代
int Chain::ikSQPSumSqr(const Frame &desireFrame, VectorXSDS &outJoints, const Twist &bounds, int maxIter, double eps, double maxTime)
{

    if(outJoints.mN != _numOfJoints)
    {
        return -2;
    }

    _boundsSQP = bounds;

    _epsSQP    = eps;

    _opt.set_maxtime(maxTime);

    double minf; /* the minimum objective value, upon return */

    _desireFrameSQP = desireFrame;

    std::vector<double> x(_numOfJoints);

    for (unsigned int i = 0; i < x.size(); i++)
    {
        x[i] = outJoints[i];

        if (_jointMoveTypes[i] == Joint::ROT_CONTINUOUS_MOVE)
            continue;

        if (_jointMoveTypes[i] == Joint::TRANS_MOVE)
        {
            x[i] = std::min(x[i], _maxJoints[i]);
            x[i] = std::max(x[i], _minJoints[i]);
        }
        else
        {

            if (x[i] > _maxJoints[i])
            {

                double diffangle = fmod(x[i] - _maxJoints[i], MSNH_2_PI);

                x[i] = _maxJoints[i] + diffangle - MSNH_2_PI;
            }

            if (x[i] < _minJoints[i])
            {

                double diffangle = fmod(_minJoints[i] - x[i], MSNH_2_PI);

                x[i] = _minJoints[i] - diffangle + MSNH_2_PI;
            }

            if (x[i] > _maxJoints[i])
                x[i] = (_maxJoints[i] + _minJoints[i]) / 2.0;
        }
    }

    _bestXSQP   = x;
    _optStatus  = -3;

    std::vector<double> artificialLowerLimits(_minJoints.size());

    for (unsigned int i = 0; i < _minJoints.size(); i++)
    {
        if (_jointMoveTypes[i] == Joint::ROT_CONTINUOUS_MOVE)
        {
            artificialLowerLimits[i] = _bestXSQP[i] - MSNH_2_PI;
        }
        else if (_jointMoveTypes[i] == Joint::TRANS_MOVE)
        {
            artificialLowerLimits[i] = _minJoints[i];
        }
        else
        {
            artificialLowerLimits[i] = std::max(_minJoints[i], _bestXSQP[i] - MSNH_2_PI);
        }
    }

    _opt.set_lower_bounds(artificialLowerLimits);

    std::vector<double> artificialUpperLimits(_minJoints.size());

    for (unsigned int i = 0; i < _maxJoints.size(); i++)
    {
        if (_jointMoveTypes[i] == Joint::ROT_CONTINUOUS_MOVE)
        {
            artificialUpperLimits[i] = _bestXSQP[i] + MSNH_2_PI;
        }
        else if (_jointMoveTypes[i] == Joint::TRANS_MOVE)
        {
            artificialUpperLimits[i] = _maxJoints[i];
        }
        else
        {
            artificialUpperLimits[i] = std::min(_maxJoints[i], _bestXSQP[i] + MSNH_2_PI);
        }
    }

    _opt.set_upper_bounds(artificialUpperLimits);

    try
    {
        _opt.optimize(x, minf);
    }
    catch (...)
    {
    }

    if (_optStatus == -1) 

    {
        _optStatus = -3;
    }

    int q = 0;

    if (!(_optStatus < 0)) 

    {

        for (int z = 0; z < 100; ++z)
        {

            q = z;
            if(!(_optStatus < 0)) 

            {
                break;
            }

            for (unsigned int i = 0; i < x.size(); i++)
            {
                x[i] = fRand(artificialLowerLimits[i], artificialUpperLimits[i]);
            }

            try
            {
                _opt.optimize(x, minf);
            }
            catch (...) {}

            if (_optStatus == -1) 

            {
                _optStatus = -3;
            }

        }
    }

    if(q == (maxIter-1))
    {
        return -1;
    }

    for (unsigned int i = 0; i < x.size(); i++)
    {
        outJoints[i] = _bestXSQP[i];
    }

    return q;
}

• 测试

使用puma560机器人模型，分别测试约20万组点，在0点开始迭代，可以看出使用SQP方法可以极大提高准确率, 但是由于计算量的关系, 计算速度也大幅度下降(Win10 I7 10700KF).


5. 改进五(不开源)

基于SQP和Newton的方法各有优势, 可以将这二者进行结合, 进行并行优化, 多个求解器同时计算, 以最快的结果为准同时停止其他求解器. 同时相关计算进行深度优化, Simd指令集优化, 矩阵乘法优化等等, 最后得到最终优化版本, 并与trac_ik进行比较(ubuntu20.04 I7 10700KF).

• 测试

使用puma560机器人模型，分别测试约300万组点，在0点开始迭代，可以看出Msnhnet Fast版本在相同的求解准确率下, 其求解速度比Trac_Ik快了约2.5倍.

• 最后
  欢迎关注Msnhnet框架，它是一个深度学习推理框架，也会慢慢变成一个机器人+视觉一体的框架
  Msnhnet
  知乎介绍

机器人学建模、规划与控制.Bruno Siciliano
现代机器人学：机构、规划与控制. Kevin M. Lynch.
机器人操作的数学导论.李泽湘
机器人学导论。John J. Craig
Numerical Optimization. Jorge Nocedal Stephen J. Wright