【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解

        对于串联机器人来说,求逆解的难度要大于求正解,市面上的工业机器人一般是利用的是利用解析法求封闭解,机器人有封闭解是有条件的---Pieper法则。另一种求逆解的方法是利用迭代法求数值解,适用于不满足Pieper法则的构型,特别适用于运动学冗余的机械臂。

        KDL提供了3种逆解方法:(1)纯牛顿拉普森迭代法;(2)关节限位的牛顿法;(3)基于LM的方法。其中(1)和(2)几乎是相同的,只是在迭代求解的时候加了一个关节限位,下边简单介绍KDL中第(1)种方法。

       可以将求机器人逆解的问题看成是一个求多元非线性方程组的问题,而牛顿拉普森迭代法就是求这个问题的一个可选的方法。

        第一步:方程组可写成如下形式:

【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解_第1张图片(1)

表示机械臂的正运动学函数,操作空间共有m个自由度。

表示机械臂具有n个关节。如果n>m则机械臂运动学冗余。

表示机械臂末端的在操作空间维度的目标点。

       第二步:求取雅克比矩阵:

【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解_第2张图片(2)

       第三步:迭代法求解下一个关节的位置:

【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解_第3张图片(3)

       其中:

       表示当前的误差(目标点的位姿-当前点的位姿),这与(1)式是相反的,所以(3)式的等式后是加号,这么写是为了要和KDL中的程序一致。

 

       逆矩阵使用雅克比矩阵的伪逆矩阵,KDL代码中并没有单独的伪逆矩阵的求解器,而是直接求的伪逆矩阵和误差的乘积。


       KDL中的代码(orocos_kdl\src\chainiksolverpos_nr):

int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
    {
        if(q_init.rows() != nj || q_out.rows() != nj)
            return (error = E_SIZE_MISMATCH);

            q_out = q_init;

            unsigned int i;
            for(i=0;i fksolver.JntToCart(q_out,f) )
                    return (error = E_FKSOLVERPOS_FAILED);
                delta_twist = diff(f,p_in);
                const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
                if (E_NOERROR > rc)
                    return (error = E_IKSOLVER_FAILED);
                // we chose to continue if the child solver returned a positive
                // "error", which may simply indicate a degraded solution
                
                Add(q_out,delta_q,q_out);
                //
                if(Equal(delta_twist,Twist::Zero(),eps))
                    // converged, but possibly with a degraded solution
                    return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
            }
            return (error = E_MAX_ITERATIONS_EXCEEDED);        // failed to converge
    }

       关键代码段说明:

 delta_twist = diff(f,p_in);//这行代码表示求取的是Err项

const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);//这行求的是上述第三步等号后边的第二项,把值传给了 delta_q
Add(q_out,delta_q,q_out);//这行进行的是第三步的运算,更新的值为 q_out,再进行下一次迭代,逐渐逼近真实解


       KDL中的雅克比矩阵是几何雅克比矩阵,原理可看《机器人学:建模、规划和控制》这本教材,KDL中是以一种很巧妙的方式求出来,具体可看:【机器人学】机器人开源项目KDL源码学习:(5)KDL如何求解几何雅克比矩阵


你可能感兴趣的:(【机器人学:运动学】)