Newton Euler算法

参考思路（参考丁教授第二讲ppt）矢量在不同坐标系下的转换关系速度的递推重心处的力和力矩力的递推完整公式代码

参考

北航丁希仑教授的机器人动力学课件：

检出概念: 第一讲.

推导公式: 第二讲.

书籍：

机械工业出版社的《机器人学导论》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]';