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

【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法

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

机械臂的逆动力学问题可以认为是:已知机械臂各个连杆的关节的运动(关节位移、关节速度和关节加速度),求产生这个加速度响应所需要的力/力矩。KDL提供了两个求解逆动力学的求解器,其中一个是牛顿欧拉法,这个方法是最简单和高效的方法。   牛顿欧拉法算法可以分为三个步骤: step1:计算每个连杆质心的速度和加速度; step2:计算产生这些加速度所需要的合力; step3:计算其它连杆通过关节对每个连杆施加的力。   KDL中的牛顿欧拉法的代码是基于文献《Rigid Body Dynamics Algorithms》写的,这本书中使用了spatial vector的概念,spatial vector将3维的刚体的线性运动(力)和3维的旋转运动(力)组合到一起形成6维的,这样处理会使代码便于阅读(关于Spatial Vector可以看另外一篇博客-KDL的精髓)。   下表为KDL中逆动力学的伪代码(这个表也是从《Rigid Body Dynamics Algorithms》截的图,KDL的代码和这些公式不是完全对应的,要完全理解KDL的思想需要把这本书过一遍)。    


牛顿欧拉法的伪代码


图1 牛顿欧拉法的伪代码
    Basic Equations部分表示各个连杆的速度 vivi、加速度 aiai、合力 fiBfiB 和力矩 ττ 的求解公式,这些参数不参考任何特定的坐标系。 Equations in Body Coordinates部分表示各个连杆的参数的参考坐标系的是本体坐标系。 Algorithm部分的伪代码对应就是KDL中的逆运动学代码(src/chainidsolver_recursive_newton_euler.cpp),如下所示:  

        int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
    {
        if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
            return (error = E_SIZE_MISMATCH);
        unsigned int j=0;

        for(unsigned int i=0;i<ns;i++){
            double q_,qdot_,qdotdot_;
            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
                q_=q(j);
                qdot_=q_dot(j);
                qdotdot_=q_dotdot(j);
                j++;
            }else
                q_=qdot_=qdotdot_=0.0;

            X[i]=chain.getSegment(i).pose(q_);
            Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
            S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));

            if(i==0){
                v[i]=vj;
                a[i]=X[i].Inverse(ag)+S[i]*qdotdot_+v[i]*vj;
            }else{
                v[i]=X[i].Inverse(v[i-1])+vj;
                a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
            }

            RigidBodyInertia Ii=chain.getSegment(i).getInertia();
            f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];

        }

        j=nj-1;
        for(int i=ns-1;i>=0;i--){
            if(chain.getSegment(i).getJoint().getType()!=Joint::None)
                torques(j--)=dot(S[i],f[i]);
            if(i!=0)
                f[i-1]=f[i-1]+X[i]*f[i];
        }
    return (error = E_NOERROR);
    }

  在阅读代码的时候,大家比较关心的可能是代码段与公式的对应关系,由于KDL的代码非常简短(原因是使用了Spatial Vector),所以这里把关键代码与文献中的公式对应起来,便于阅读代码):  

X[i]=chain.getSegment(i).pose(q_);

  求解转换矩阵【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法,表示父连杆坐标系向子连杆坐标系的坐标变换。  

vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));

  求解关节引起的连杆速度vJ,它的意义是求出连杆的速度后,再左乘一个转置矩阵,将速度的参考坐标系变换到本体上的坐标系,这与上图中的伪代码的公式(vJi=siqi)不一样。  

v[i]=X[i].Inverse(v[i-1])+vj;

  求解连杆末端的速度
【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法,它的参考坐标系为与本体固连的坐标系。  

a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;

  这行代码是求解连杆的加速度
【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法  

            RigidBodyInertia Ii=chain.getSegment(i).getInertia();
            f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];

  获取机械臂的动力学参数(质量、惯性张量、连杆坐标系到连杆质心偏移)、f[i]求解父连杆通过关节施加给连杆的力。代码中的运算符 * 是KDL中自定义的运算符,可见另一篇博客-KDL的精髓。  

torques(j--)=dot(S[i],f[i]);

  这行代码求解关节力或力矩
【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法,至此,各个关节的力或力矩均会被求出,算法结束。 参考文献:   [1] 《Rigid Body Dynamics Algorithms》. Roy Featherstone, 2008  


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明【机器人学】机器人开源项目KDL源码学习:(4)机械臂逆动力学的牛顿欧拉算法
喜欢 (0)

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

加载中……