• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 gpeng832 2983次浏览 0个评论

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

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

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

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

(1)

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

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

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

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

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

(2)

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

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

(3)

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

其中:

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

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

 

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


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明【机器人学】机器人开源项目KDL源码学习:(2)牛顿拉普森迭代法求机器人的数值解
喜欢 (0)

您必须 登录 才能发表评论!

加载中……