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

机械臂的逆动力学问题可以认为是：已知机械臂各个连杆的关节的运动（关节位移、关节速度和关节加速度），求产生这个加速度响应所需要的力/力矩。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部分表示各个连杆的速度 vi v_{i}、加速度 ai a_{i}、合力 fiB {{f}_{i}}^{B} 和力矩 τ \tau 的求解公式，这些参数不参考任何特定的坐标系。
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++;}elseq_=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_);

求解转换矩阵 λ(i)Xi {}^{\lambda \left( i \right)}{{X}_{i}}，表示父连杆坐标系向子连杆坐标系的坐标变换。

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

求解关节引起的连杆速度 vJ {v}_{J}，它的意义是求出连杆的速度后，再左乘一个转置矩阵，将速度的参考坐标系变换到本体上的坐标系，这与上图中的伪代码的公式（ vJi=siq⋅i {{v}_{Ji}}={{s}_{i}}{{\overset{\centerdot }{\mathop{q}}\,}_{i}}）不一样。

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

求解连杆末端的速度（ vi=iXλ(i)vλ(i)+vJ {{v}_{i}}={}^{i}{{X}_{\lambda (i)}}{{v}_{\lambda (i)}}+{{v}_{J}}），它的参考坐标系为与本体固连的坐标系。

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

这行代码是求解连杆的加速度， ai=iXλ(i)aλ(i)+Siq⋅⋅i+cJi+vi×vJi {{a}_{i}}={}^{i}{{X}_{\lambda \left( i \right)}}{{a}_{\lambda \left( i \right)}}+{{S}_{i}}{{\overset{\centerdot \centerdot }{\mathop{q}}\,}_{i}}+{{c}_{Ji}}+{{v}_{i}}\times {{v}_{Ji}}， cJi=0 {{c}_{Ji}}=0。

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

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

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

这行代码求解关节力或力矩（ τi=SiTfi {{\tau }_{i}}={{S}_{i}}^{T}{{f}_{i}}），至此，各个关节的力或力矩均会被求出，算法结束。
参考文献：
[1] 《Rigid Body Dynamics Algorithms》. Roy Featherstone, 2008

【机器人学】机器人开源项目KDL源码学习：（4）机械臂逆动力学的牛顿欧拉算法相关推荐

【机器人学】机器人开源项目KDL源码学习：（5）KDL如何求解几何雅克比矩阵
这篇文章试图说清楚两件事:1. 几何雅克比矩阵的本质:2. KDL如何求解机械臂的几何雅克比矩阵. 一.几何雅克比矩阵的本质机械臂的关节空间的速度可以映射到执行器末端在操作空间的速度,这种映射可以通 ...
今年我读了四个开源项目的源码，来分享下心得
今年来看了 RocketMQ.Kafka.Dubbo .Tomcat 的源码,之前也有读者询问过如何读源码,索性就来分享一下. 其实还看了一点点 Linux.Redis.jdk8,这几个阅读的目的和上 ...
开源项目实例源码_今年我读了四个开源项目的源码，来分享下心得
今年来看了 RocketMQ.Kafka.Dubbo .Tomcat 的源码,之前也有读者询问过如何读源码,索性就来分享一下. 其实还看了一点点 Linux.Redis.jdk8,这几个阅读的目的和上 ...
Go实现的5G核心网开源项目free5gc源码分析系列 | Gopher Daily (2021.01.08) ʕ◔ϖ◔ʔ
每日一谚:"Abstractions should be discovered, not created." Go技术新闻 Go实现的5G核心网开源项目free5gc源码分析系列 ...
鸿蒙开源源码,基于鸿蒙系统开源项目OpenHarmony源码静态分析
#ifndef __scc #define __scc(X) ((long) (X)) // 转为long类型 typedef long syscall_arg_t; #endif #define _ ...
BT开源项目Snark源码分析
BT开源项目Snark源码分析 Snark是国外一个开源Java的项目,实现了BitTorrent协议,通过分析此项目的源程序,可以更利于我们更加深入的了解当前流行的BT软件的原理,进而可以指导我们的 ...
开源项目Telegram源码 Telegram for Android Source
背景介绍 Telegram 是一款跨平台的即时通信软件,它的客户端是自由及开放源代码软件.用户可以相互交换加密与自毁消息,发送照片.影片等所有类型文件.官方提供手机版.桌面版和网页版等多种平台客户端. ...
python写个围棋程序_经典python实战项目blog源码学习
这是廖雪峰老师实际项目的源码, python知识学得再多,都不如实战用一回. Day 1 – 搭建开发环境 Day 2 – 编写Web App骨架 Day 3 – 编写ORM Day 4 – 编写Mo ...
区块链开源项目Asch源码初探
Asch这个名字是 App Side Chain 的缩写. 是一种基于区块链跨链技术的应用开发平台,目前全部核心代码已经在GitHub上开源. 区块链是比特币的底层技术,但是名气低于比特币,但是个人认 ...

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

【机器人学】机器人开源项目KDL源码学习：（4）机械臂逆动力学的牛顿欧拉算法相关推荐

最新文章

热门文章