机械臂动力学建模（3）- Newton Euler牛顿欧拉算法

参考

北航丁希仑教授的机器人动力学课件：
检出概念: 第一讲.
推导公式: 第二讲.
书籍：
机械工业出版社的《机器人学导论》136页开始
《Rigid Body Dynamics Algorithms》
《MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL》
各类博客：
博客: 三自由度连杆动力学Newton-Euler方法.
博客: 递归牛顿欧拉（逆）动力学算法.
博客: 机器人学回炉重造（4）：动力学仿真（附牛顿-欧拉递归逆动力学算法matlab代码）.
知乎: matlab递推牛顿-欧拉法解机械臂动力学方程.

思路（参考丁教授第二讲ppt）

矢量在不同坐标系下的转换关系

因为

所以矢量在不同坐标系下的转换关系：

这个变换在后面的线速度和角速度会多次用到。

速度的递推

则速度可以表示为

使用第一步的变换公式，可以变化为：

再次读速度求导，得到加速度

放在机械臂中：

同样可以推导出角速度，配合第一步中的转换：

以及速度：

对于aubo i5机器人，每个关节都是旋转关节，只有绕z轴旋转的角速度：

所以就有
以及角加速度

同样的对于向量P应用第一部分的变换

进一步化简线速度

线加速度

重心处的力和力矩

通过速度就可以算出重心处的力

通过角速度就可以算出重心处的力矩

力的递推

对于连杆i存在静力平衡

对于原点Oi存在力矩平衡
这里的n就是需要计算的关节力tau。

完整公式

代码

外推：

            w{i} =  R{i}*w0 + qd(i)*e;
            wd{i} = R{i}*wd0 + qdd(i)*e + cross(R{i}*w0,qd(i)*e);
            v{i} =  R{i}*(v0 + cross(w0,p{i}));
            vd{i} = R{i}*(vd0 + cross(w0, cross(w0, p{i})) + cross(wd0, p{i}));
            vc{i} = vd{i} + cross(wd{i},c{i}) + cross(w{i}, cross(w{i}, c{i}));
       		F{i} = M(i) *  vc{i};
        	T{i} = I{i}* wd{i} +  cross(w{i},I{i}* w{i});

内推：

            f{i} = R{i+1}'* f{i+1} + F{i};
            t{i} = T{i} + R{i+1}'*t{i+1} + cross(p{i+1},R{i+1}'* f{i+1}) + cross(c{i},F{i});
           	tau(i) = t{i}'*[0 0 1]';