088、机器人动力学：牛顿-欧拉法

机器人动力学：牛顿-欧拉法

从一次机械臂抖动说起

去年调试一台六轴协作机器人，空载运行还算平稳，一夹持3kg负载做高速圆弧插补，末端就开始高频抖动。PID参数调了三轮，陷波滤波器加了两个，效果都不理想。后来拆开关节看，发现电机电流波形在加减速阶段有明显的毛刺——这不是控制问题，是动力学模型没算对，前馈力矩给错了。

那台机器人的动力学模型用的是拉格朗日法，但实际调试中发现，拉格朗日法推导出来的惯性矩阵和科里奥利项，在嵌入式上实时计算时，矩阵运算的浮点开销太大，导致控制周期从1kHz掉到500Hz。后来换成牛顿-欧拉法，迭代计算，每个关节的力矩直接递推出来，计算量降了一个数量级，抖动问题迎刃而解。

今天这篇笔记，就聊聊牛顿-欧拉法在机器人动力学里的工程落地。不扯虚的，直接上推导和代码。

牛顿-欧拉法的核心思想

拉格朗日法是从能量角度出发，先算动能和势能，再对广义坐标求偏导。好处是数学上漂亮，坏处是计算量大，尤其对于六轴以上的机器人，惯性矩阵的维度爆炸。

牛顿-欧拉法换个思路：把机器人拆成一个个连杆，每个连杆当成刚体，用牛顿第二定律算平动，用欧拉方程算转动。从基座开始向外递推速度、加速度，再从末端向内递推力、力矩。两个方向各跑一遍，力矩就出来了。

这里踩过坑：很多人以为牛顿-欧拉法只适合串联机器人，其实并联机器人也能用，只是约束方程要单独处理。后面有机会再聊。

符号约定与坐标系

先定规矩。每个连杆i固连一个坐标系{i}，原点在关节i的轴线上。定义