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

182
0
2021年2月17日 09时17分

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

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

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

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

(1)

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

【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解插图(1)表示机械臂的正运动学函数,操作空间共有m个自由度。

【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解插图(2)表示机械臂具有n个关节。如果n>m则机械臂运动学冗余。

【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解插图(3)表示机械臂末端的在操作空间维度的目标点。

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

(2)

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

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

(3)

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

其中:

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

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

 

表示当前的误差(目标点的位姿-当前点的位姿),这与(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<maxiter;i++){
                if (E_NOERROR > 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如何求解几何雅克比矩阵

发表评论

后才能评论