复现KDL:使用Msnhnet实现串联机器人运动数值逆解(一)

串联机械臂的逆解是一个求解多元非线性方程解的过程。当然,市面上的众多商业臂也都是采用符合pieper准则的构型,可以用解析法或者几何法直接推导出解析式,从而得到封闭解。

符合peiper准则的puma560机械臂

而对于不符合peiper准则的臂或者运动学冗余的臂,数值解也是不错的方法。当然,速度肯定是比不上解析法了,而且每次的解只有一个,且每次求解的结果都不一样,当然,好处就是在通用平台上,可以快速搭建机器人系统。

7轴冗余机械臂kuka iiwa

串联机器人运动学方程可写为:

[公式]

现已知 [公式] ,求每个关节的转角 [公式] ,由于 [公式] 带有三角函数,是一个典型的多元非线性问题。可选用Newton Raphson迭代法作为求解方案。

1、Newton Raphson迭代法

把函数 [公式]  [公式] 点某邻域内进行泰勒展开:

[公式]

取线性部分(泰勒展开前两项),令其等于0作为 [公式] 的近似方程:

[公式]

 [公式] 则其解为:

[公式]

得到迭代关系式:

[公式]

  • 举例

[公式]  [公式] :

[公式]

[公式]

[公式]

[公式]

  • 应用在机器人运动学

给定 [公式] 和初始猜测角度 [公式] ( [公式] 为空间自由度, [公式] 为关节数):

[公式]

可写成:

[公式] , [公式]

[公式] 为一阶偏导,即为雅可比矩阵。

左右同时左乘 [公式] :

[公式]

 [公式] ( [公式] 为迭代最小误差,如0.001):

[公式]

[公式]

得到:

[公式]

如果雅可比矩阵不是方阵或机器人位于奇异点,此时用雅可比矩阵的伪逆 [公式] .伪逆进行计算.可写成:

[公式]

实现:

  1. 求解 [公式] 处雅克比矩阵:

 [公式] 为单位角速度或线速度1时,每个关节对末端的速度旋量

[公式]

[公式]

2. 误差项 [公式] 的求取:

[公式]

位置误差: [公式]

姿态误差:以A为参考,A到B的姿态误差

[公式]

此时的 [公式] 需要把旋转矩阵表示成轴角的形式:

[公式]

最后将 [公式] 得转换到基于基座标 [公式] 为参考的姿态误差:

[公式]

  • 代码实现:
int Chain::ikNewton(const Frame &desireFrame, VectorXSDS &outJoints, int maxIter, double eps)
{

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

    Frame frame;
    Twist dtTwist;
    MatSDS jac(_numOfJoints,6);

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

        frame = fk(joints);

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

        if(dtTwist.closeToEps(eps))
        {

            outJoints = joints;
            return i;
        }

        jacobi(jac,joints);

        MatSDS dtJoints = jac.pseudoInvert()*dtTwist.toMat(); 

        for (int i = 0; i < joints.mN; ++i)
        {
            joints[i] = joints[i] + dtJoints[i];
        }
    }
    return -1;
}
  • 案例:

  • 结果:

对比Msnhnet实现和KDL实现,使用puma560机器人模型,分别测试约20万组点,在0点开始迭代,结果如下。Msnhnet成功率为96.02%, 单次求解速率为114us,KDL成功率为96.11%,单次求解速率为135us。(Win10 I7 10700KF)

Msnhnet Newton Raphson测试
KDL Newton Raphson测试
  • 后续:

后面会采用trac_ik的求解方案进行优化,欢迎继续关注。

  • 最后

欢迎关注Msnhnet框架,它是一个深度学习推理框架,也会慢慢变成一个机器人+视觉一体的框架

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