Robotics-机器人动力学-从牛顿欧拉公式到拉格朗日方程

从受力到能量：理解机器人动力学

机器人运动学回答“关节转到哪里，末端会在哪里”；动力学进一步回答：要让机器人这样运动，电机需要输出多大的力或力矩？

建立动力学模型有两条常用路径：

牛顿–欧拉法：逐根连杆分析力与力矩。
拉格朗日法：从整个系统的动能与势能出发。

两种方法描述的是同一套物理规律，最终得到相同的运动方程。

1. 先看最终结果

对于具有 \(n\) 个独立关节坐标的机器人：

\[\boxed{M(q)\ddot q+V(q,\dot q)+G(q)=\tau} \]

符号	含义
\(q,\dot q,\ddot q\)	关节位置、速度、加速度
\(M(q)\)	\(n\times n\) 惯量矩阵
\(V(q,\dot q)\)	离心与科里奥利项，是 \(n\) 维向量
\(G(q)\)	重力项
\(\tau\)	关节驱动力或力矩

转动关节的 \(q_i\) 是角度，\(\tau_i\) 是力矩；移动关节的 \(q_i\) 是位移，\(\tau_i\) 是力。这种统一表达称为广义坐标与广义力。

可以将方程理解为：

驱动输入＝产生加速度所需的输入＋速度相关项＋重力补偿项。

本文默认刚性连杆、理想关节，暂不显式包含摩擦和外部接触力。

2. 刚体动力学的基础：平动与转动

平动：牛顿方程

质量不变时，线动量为 \(P=mv\)，合外力等于线动量的变化率：

\[F=\frac{dP}{dt}=ma \]

对于刚体，应使用质心加速度：

\[\boxed{F=ma_C} \]

合外力包括关节作用力、重力和接触力等。

转动：角动量与欧拉方程

质点关于惯性系固定点 \(O\) 的角动量为：

\[H_O=r\times mv \]

其中 \(r\) 是从 \(O\) 指向质点的位置向量。由于 \(\dot r=v\)，且 \(v\times mv=0\)：

\[\boxed{\frac{dH_O}{dt}=r\times F=N_O} \]

对刚体，将所有质量微元的角动量相加。关于质心：

\[H_C=I_C\omega \]

其中 \(I_C\) 是质心惯量矩阵，\(\omega\) 是角速度。最终得到欧拉方程：

\[\boxed{N_C=I_C\dot\omega+\omega\times(I_C\omega)} \]

为什么多了一个叉乘项？因为角动量方向也会随刚体旋转而改变。在随体坐标系中，\(I_C\) 不变，但必须计入坐标轴旋转带来的变化：

\[\left(\frac{dH_C}{dt}\right)_{\text{惯性系}} =\left(\frac{dH_C}{dt}\right)_{\text{随体系}}+\omega\times H_C \]

只有当 \(\omega\) 与 \(I_C\omega\) 平行时，叉乘项才为零。三维转动一般不能直接写成标量式 \(N=I\alpha\)。

3. 惯量矩阵：质量分布如何影响转动

设质量微元相对于取矩点的位置为：

\[r=\begin{bmatrix}x&y&z\end{bmatrix}^T,\qquad dm=\rho\,dV \]

惯量矩阵为：

\[\boxed{I=\int_V\left[(r^Tr)I_3-rr^T\right]dm} \]

展开：

\[I=\int_V \begin{bmatrix} y^2+z^2&-xy&-xz\\ -xy&x^2+z^2&-yz\\ -xz&-yz&x^2+y^2 \end{bmatrix}dm \]

对角项是绕坐标轴的转动惯量，例如 \(I_{xx}=\int(y^2+z^2)dm\)。
非对角项反映当前坐标轴下的惯性耦合。若定义惯性积为 \(\int xy\,dm\)，对应矩阵元素应取其负值。
\(I\) 是对称矩阵，单位为 \(\mathrm{kg\,m^2}\)。
选择惯量主轴后，矩阵成为对角矩阵；仅将原点放在质心，并不保证这一点。

这里 \(r^Tr\) 是标量，\(rr^T\) 是矩阵，两者不能混淆。

也可以定义叉乘矩阵：

\[\hat r=\begin{bmatrix} 0&-z&y\\ z&0&-x\\ -y&x&0 \end{bmatrix},\qquad \hat r a=r\times a \]

于是：

\[-\hat r^2=(r^Tr)I_3-rr^T \]

“帽子”在这里表示叉乘矩阵，不是单位向量。

平行轴定理

已知质心惯量 \(I_C\)，要换算到点 \(A\)，设 \(r_C\) 从 \(A\) 指向质心：

\[\boxed{I_A=I_C+m\left[(r_C^Tr_C)I_3-r_Cr_C^T\right]} \]

前提是两个矩阵采用相同的坐标轴方向。对于一对平行轴，它退化为：

\[I_A=I_C+md^2 \]

这里 \(d\) 是两条轴的垂直距离。这一关系成立的关键是，质心坐标满足 \(\int r\,dm=0\)，展开积分后的交叉项因而消失。

例如，均匀实心正方体边长为 \(a\)：

\[I_C=\frac{ma^2}{6}I_3 \]

移到顶点、坐标轴沿三条棱且质心坐标均为 \(a/2\) 时：

\[I_A=ma^2\begin{bmatrix} 2/3&-1/4&-1/4\\ -1/4&2/3&-1/4\\ -1/4&-1/4&2/3 \end{bmatrix} \]

可见，平移取矩点也可能产生非零的非对角项。

4. 牛顿–欧拉法：正向算运动，反向算受力

递归牛顿–欧拉法通常用于逆动力学：给定 \(q,\dot q,\ddot q\)，计算 \(\tau\)。

第一趟：基座 → 末端

后一级连杆的运动包含前一级带来的运动，因此从基座向外递推。

对于转动关节，设 \(z_{i+1}\) 为关节轴单位向量：

\[\omega_{i+1}=\omega_i+\dot\theta_{i+1}z_{i+1} \]

关节轴本身也随前一级转动，故：

\[\boxed{\dot\omega_{i+1}=\dot\omega_i+\ddot\theta_{i+1}z_{i+1} +\omega_i\times(\dot\theta_{i+1}z_{i+1})} \]

最后一项来自关节轴方向的变化。移动关节没有相对转动，因此不增加上述关节角速度、角加速度项。

若两原点之间的位移 \(p\) 固定在前一级连杆上：

\[\boxed{a_{i+1}=a_i+\dot\omega_i\times p +\omega_i\times(\omega_i\times p)} \]

三项分别是原点加速度、切向加速度和向心加速度。

若还存在沿关节轴的相对滑动，则需增加：

\[2\omega_i\times(\dot d_{i+1}z_{i+1})+\ddot d_{i+1}z_{i+1} \]

即科里奥利加速度和相对加速度。这里采用关节轴随前一级旋转、相对运动沿该轴的几何约定。

接着，从连杆原点换算到质心。设 \(r_{C_i}\) 从原点指向质心：

\[v_{C_i}=v_i+\omega_i\times r_{C_i} \]

\[\boxed{a_{C_i}=a_i+\dot\omega_i\times r_{C_i} +\omega_i\times(\omega_i\times r_{C_i})} \]

质心固定在连杆上，因此没有相对滑动项。由此计算：

\[F_i=m_i a_{C_i},\qquad N_i=I_{C_i}\dot\omega_i+\omega_i\times(I_{C_i}\omega_i) \]

第二趟：末端 → 基座

定义 \(f_i,n_i\) 为前一级对连杆 \(i\) 施加的力和力矩。下一级对它的作用则为 \(-f_{i+1},-n_{i+1}\)。

暂不计重力及其他外力，力平衡给出：

\[\boxed{f_i=F_i+f_{i+1}} \]

设 \(p_{i+1}\) 从当前关节原点指向下一关节原点。对质心取矩时，两端力的力臂分别为 \(-r_{C_i}\) 与 \(p_{i+1}-r_{C_i}\)：

\[N_i=n_i-n_{i+1}-r_{C_i}\times f_i -(p_{i+1}-r_{C_i})\times f_{i+1} \]

整理得到：

\[\boxed{n_i=N_i+n_{i+1}+r_{C_i}\times F_i +p_{i+1}\times f_{i+1}} \]

最后提取沿关节轴的分量：

\[\boxed{\tau_i=\begin{cases} z_i^Tn_i,&\text{转动关节}\\ z_i^Tf_i,&\text{移动关节} \end{cases}} \]

当前关节既承担本连杆的需求，也承担所有下游载荷。无末端外载荷时，可从 \(f_{n+1}=n_{n+1}=0\) 开始。

实现时的三个要点

统一坐标系。 若向量分别在不同连杆坐标系中表达，必须先用旋转矩阵转换，例如 \({}^{i}f={}^iR_{i+1}\,{}^{i+1}f\)。左上标表示表达坐标系，不是幂。惯量矩阵也必须采用相同的坐标轴方向。
重力只计入一次。 固定基座可用初始递推加速度 \(a_0=-g\) 纳入重力；此时递推中的加速度是计算辅助量，不应再额外添加重力。
大写与小写不同。 \(F_i,N_i\) 描述本连杆的动力学需求；\(f_i,n_i\) 是关节处传递的载荷，不能重复相加。

对于串联机械臂，两趟递推的计算量为 \(O(n)\)，无需先显式构造完整的 \(M,V,G\)。

5. 拉格朗日法：从能量得到运动方程

定义拉格朗日函数：

\[L=K-U \]

其中 \(K\) 是总动能，\(U\) 是总势能。每个关节满足：

\[\boxed{\frac{d}{dt}\left(\frac{\partial L}{\partial\dot q_i}\right) -\frac{\partial L}{\partial q_i}=\tau_i} \]

对于理想关节约束，这种方法不必显式求出关节约束反力。

动能决定惯量矩阵

每根连杆的动能由平动与转动组成：

\[K_i=\frac12m_iv_{C_i}^Tv_{C_i}+\frac12\omega_i^TI_{C_i}\omega_i \]

将各连杆动能相加，并用关节速度表示：

\[\boxed{K=\frac12\dot q^TM(q)\dot q} \]

因此，\(M(q)\) 也称动能矩阵。正常的非退化模型中，它对称正定；非对角元素体现关节之间的惯性耦合。

两种求导必须分清

对速度求偏导时，固定 \(q\)，所以 \(M(q)\) 视为常量：

\[\frac{\partial K}{\partial\dot q}=M\dot q \]

这是因为二次型的梯度为 \(\tfrac12(M+M^T)\dot q\)，而 \(M^T=M\)。结果 \(M\dot q\) 是广义动量。

也可取对称矩阵平方根 \(S=M^{1/2}\)，令 \(u=S\dot q\)，将动能写成 \(K=\tfrac12u^Tu\)。链式法则给出：

\[\frac{\partial K}{\partial\dot q}=S^Tu=M\dot q \]

这里的矩阵平方根不是逐元素开方，\(u\) 也不是实际的末端线速度。

但沿实际运动对时间求导时，\(M(q(t))\) 也会变化：

\[\boxed{\frac{d}{dt}(M\dot q)=M\ddot q+\dot M\dot q} \]

其中：

\[\dot M=\sum_k\frac{\partial M}{\partial q_k}\dot q_k \]

对位置求偏导时，则固定 \(\dot q\)：

\[\frac{\partial K}{\partial q_i} =\frac12\dot q^T\frac{\partial M}{\partial q_i}\dot q \]

因此：

\[\boxed{V(q,\dot q)=\dot M\dot q-\frac12 \begin{bmatrix} \dot q^T\dfrac{\partial M}{\partial q_1}\dot q\\ \vdots\\ \dot q^T\dfrac{\partial M}{\partial q_n}\dot q \end{bmatrix}} \]

这说明：知道完整的 \(M(q)\) 函数，就能通过求导计算 \(V\)。仅知道某个姿态下的数值矩阵还不够。

\(V\) 包含速度平方和速度乘积，通常分别对应离心项与科里奥利项，也常写成 \(C(q,\dot q)\dot q\)。

势能决定重力项

若 \(U=U(q)\) 是重力势能：

\[\boxed{G(q)=\frac{\partial U}{\partial q}} \]

实际重力的广义力是 \(-G\)，移到方程左侧便成为 \(+G\)。最终：

\[\boxed{M(q)\ddot q+V(q,\dot q)+G(q)=\tau} \]

注意：\(V\) 不是势能；本文用 \(U\) 表示势能。

6. 用单关节摆杆检查理解

设摆杆质量为 \(m\)，质心距转轴 \(l_C\)，绕固定转轴的惯量为 \(J\)，角度 \(q\) 从竖直向下方向量起：

\[K=\frac12J\dot q^2,\qquad U=-mgl_C\cos q \]

代入拉格朗日方程：

\[\boxed{\tau=J\ddot q+mgl_C\sin q} \]

这里：

\(M=J\) 为常量，因此 \(V=0\)。
\(G=mgl_C\sin q\)。
静止保持姿态时，\(\tau=G\)。
没有驱动力矩时，\(J\ddot q=-mgl_C\sin q\)。

需要注意：多关节机器人即使 \(\ddot q=0\)，只要 \(\dot q\ne0\)，\(V\) 仍可能不为零。

7. 同一个方程，两种用途

已知	求解	用途
\(q,\dot q,\ddot q\)	\(\tau\)	逆动力学：计算实现运动所需的驱动输入
\(q,\dot q,\tau\)	\(\ddot q\)	正动力学：预测驱动输入产生的运动

正动力学写为：

\[M(q)\ddot q=\tau-V(q,\dot q)-G(q) \]

实际计算通常直接求解这个线性方程，而不显式求 \(M^{-1}\)。

学习这些公式时，可以始终抓住两条线索：牛顿–欧拉法将“运动”转换为“受力”，拉格朗日法将“能量”转换为“运动方程”。它们最终连接的，都是关节运动与电机输出之间的关系。

什么时候用牛顿–欧拉法，什么时候用拉格朗日法？

想算“给定运动需要多大关节力矩”，牛顿–欧拉法通常更直接；想推导“整个系统的动力学方程”，拉格朗日法通常更方便。 两者并非互斥，正确建模后结果一致。

常见场景怎么选

场景	通常选择	原因
已知 \(q,\dot q,\ddot q\)，快速计算电机力矩	递归牛顿–欧拉法	两趟递推即可，对串联机械臂计算量为 \(O(n)\)
需要知道关节处的传递力、支撑反力等	牛顿–欧拉法	建模过程中直接处理各连杆受力
推导单摆、双摆、两关节机械臂的解析方程	拉格朗日法	写出动能、势能后统一求导，步骤清晰
希望明确得到 \(M(q)\)、\(V(q,\dot q)\)、\(G(q)\)	拉格朗日法	各项与能量的对应关系直接
分析能量、平衡点，推导控制模型	拉格朗日法	能量表达便于理论分析

用同一个机械臂举例

假设你研究一台六轴机械臂。

任务是：“沿这条轨迹运动，每一时刻六个电机需要多大力矩？”用递归牛顿–欧拉法很合适：输入 \(q,\dot q,\ddot q\)，直接算出 \(\tau\)。
任务是：“为什么关节 2 的运动会影响关节 1？模型中的耦合项是什么？”用拉格朗日法推导 \(M,V,G\)，更容易看清这些关系。
任务是：“关节轴承承受多大的载荷？”用牛顿–欧拉受力分析更直接。仅有驱动力矩 \(\tau\)，还不足以得到全部轴承载荷；若要分配到具体轴承，还需结合轴承布置与支撑模型。

两个容易误解的地方

拉格朗日法也能算逆动力学，牛顿–欧拉法也能用于获得 \(M,V,G\)。 上面的选择是便利性和计算效率的区别，不是能力边界。
已知力矩、求运动的正动力学，不是拉格朗日法专属。 得到动力学模型后，都可求解：