Robotics-拉格朗日方程

从能量理解拉格朗日方程：以旋转伸缩机械臂为例

机器人动力学要回答的问题是：已知机器人准备怎样运动，电机应该输出多大的力或力矩？

牛顿–欧拉法逐根连杆分析受力；拉格朗日法则从能量出发。它的基本流程是：

选坐标 → 写动能和势能 → 求导 → 得到关节力与力矩。

本文以固定基座、刚性连杆、理想关节为前提，先忽略摩擦和外部接触力。最后用一个“可以旋转，也可以伸缩”的机械臂完成整套推导。

1. 动能、势能和功，不是一回事

动能与势能描述某一时刻的状态；功描述一段运动过程中传递的能量。

物理量	含义	例子
动能 \(K\)	因运动而具有的能量	\(K=\frac12mv^2\)
势能 \(U\)	与位置或构型有关的能量	\(U=mgh\)
功 \(W\)	力在位移过程中传递的能量	\(W=\int F\cdot dr\)

例如，匀速把物体抬高：动能不变，重力势能增加。你的提升力做正功，重力做负功。

对质量不变的质点，在惯性参考系中：

\[W_{\text{合力}}=\Delta K \]

这并不意味着动能就是“从开始到现在累计做的功”。只有初始静止等条件满足时，合功才数值上等于最终动能。对多体系统，还要考虑内力做功。

重力势能严格来说属于物体与地球组成的系统，并需要指定零点。不过，给势能加上常数不会改变运动方程，因为常数求导后为零。

2. 拉格朗日方程到底在算什么？

定义拉格朗日函数，也叫拉氏量：

\[\boxed{L=K-U} \]

它不是总机械能。总机械能是 \(E=K+U\)。

选取广义坐标 \(q=[q_1,\ldots,q_n]^T\)，每个自由度满足：

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

• 转动关节：\(q_i\) 是角度，\(\tau_i\) 是力矩。
• 移动关节：\(q_i\) 是位移，\(\tau_i\) 是力。
• \(n\) 个自由度对应 \(n\) 条方程。

这里把驱动输入统一记为 \(\tau\)。如果还有摩擦或接触力，需要在右侧加入相应的广义力。已经通过势能计入的重力，不要再在右侧重复添加。

最容易混淆的求导规则

求偏导时，固定其他变量；求时间导数时，沿实际运动考虑所有变量的变化。

操作	怎么处理
\(\partial L/\partial\dot q_i\)	固定 \(q\) 和其他速度分量
\(\partial L/\partial q_i\)	固定 \(\dot q\) 和其他位置分量
\(d/dt\)	使用 \(q=q(t)\)、\(\dot q=\dot q(t)\)，按链式法则求导

虽然实际运动中 \(\dot q=dq/dt\)，但在偏导运算中，\(q\) 和 \(\dot q\) 是独立的输入变量。

用竖直运动的质量块试一次

令向上的位移为 \(q\)：

\[K=\frac12m\dot q^2,\qquad U=mgq \]

于是：

\[\frac{d}{dt}\left(\frac{\partial L}{\partial\dot q}\right)=m\ddot q, \qquad \frac{\partial L}{\partial q}=-mg \]

得到：

\[\boxed{\tau=m\ddot q+mg} \]

也就是“加速所需的力，加上平衡重力所需的力”。拉格朗日方程与牛顿第二定律在这里给出相同结果。

3. 机器人动能为什么写成矩阵形式？

单根刚体连杆的动能是质心平动动能与绕质心转动动能之和：

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

其中，\(I_{C_i}\) 是关于质心的 \(3\times3\) 转动惯量矩阵，单位为 \(\mathrm{kg\,m^2}\)。这里必须使用质心速度，角速度与惯量矩阵也必须采用相同的坐标轴方向。

通过运动学，可以把连杆速度写成关节速度的线性函数：

\[v_{C_i}=J_{v_i}(q)\dot q, \qquad \omega_i=J_{\omega_i}(q)\dot q \]

代入并对所有连杆求和：

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

其中：

\[\boxed{ M(q)=\sum_i\left[m_iJ_{v_i}^TJ_{v_i} +J_{\omega_i}^TI_{C_i}J_{\omega_i}\right] } \]

\(I_{C_i}\) 描述单根连杆的转动惯性；\(M(q)\) 描述整个机器人的关节空间惯性，两者不是同一个矩阵。

这里使用的是每根连杆质心的雅可比，不是把末端雅可比套到所有连杆上。若角速度雅可比在基座系表达，而惯量给在连杆系中，需要先转换：

\[{}^0I_{C_i}={}^0R_i\,{}^iI_{C_i}\,({}^0R_i)^T \]

“速度的二次型”是什么意思？

以两个关节为例：

\[K=\frac12M_{11}\dot q_1^2 +M_{12}\dot q_1\dot q_2 +\frac12M_{22}\dot q_2^2 \]

动能里出现的是速度平方和速度乘积。固定姿态时，所有关节速度加倍，动能就变为四倍。

如果已经把动能展开成多项式，也可以直接读出 \(M\)，或计算：

\[M_{ij}=\frac{\partial^2K}{\partial\dot q_i\partial\dot q_j} \]

注意平方项前面的 \(1/2\)：平方项系数要乘以 2，才是对应的对角元素。

4. 从拉格朗日方程得到 \(M、V、G\)

假设势能只与位置有关，即 \(U=U(q)\)，则 \(\partial U/\partial\dot q=0\)。代入 \(L=K-U\)：

\[\frac{d}{dt}\left(\frac{\partial K}{\partial\dot q}\right) -\frac{\partial K}{\partial q} +\frac{\partial U}{\partial q}=\tau \]

加速度项与速度项

因为 \(M\) 对称：

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

沿时间求导时，\(M(q)\) 也可能变化：

\[\frac{d}{dt}(M\dot q)=M\ddot q+\dot M\dot q, \qquad \dot M=\sum_k\frac{\partial M}{\partial q_k}\dot q_k \]

另一方面：

\[\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} } \]

它包含离心与科里奥利项。不能把 \(\dot M\dot q\) 单独当作完整的 \(V\)，还必须减去动能对位置的偏导。

重力项

当 \(U\) 为重力势能时：

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

最终得到：

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

其中实际重力的广义力是 \(-G\)。所以静止保持姿态时，驱动输入为 \(\tau=G\)。

也可以直接通过雅可比计算重力项。设 \(\mathbf g\) 是指向下方的重力加速度向量，则：

\[U=-\sum_i m_i\mathbf g^Tp_{C_i}+\text{常数}, \qquad \boxed{G=-\sum_iJ_{v_i}^Tm_i\mathbf g} \]

克里斯托费尔符号：统一整理速度项

如果关节较多，可以定义：

\[b_{ijk}=\frac12\left( \frac{\partial M_{ij}}{\partial q_k} +\frac{\partial M_{ik}}{\partial q_j} -\frac{\partial M_{jk}}{\partial q_i} \right) \]

则：

\[V_i=\sum_j\sum_k b_{ijk}\dot q_j\dot q_k \]

这只是整理系数的记号，没有增加新的物理参数。因为 \(b_{ijk}=b_{ikj}\)，不同速度的交叉项可合并为 \(2b_{ijk}\dot q_j\dot q_k\)（\(j<k\)）。

课件有时将平方项与交叉项分别写为 \(C(q)[\dot q^2]+B(q)[\dot q\dot q]\)。这里的 \(C(q)\) 与常见写法 \(V=C(q,\dot q)\dot q\) 中的 \(C\) 定义不同，不能直接混用。

5. 完整例子：一边旋转，一边伸缩的 RP 机械臂

考虑在竖直平面内运动的两关节机械臂：

• 关节 1 是转动关节，角度为 \(\theta\)，从水平向右方向逆时针量起。
• 关节 2 是移动关节，伸缩坐标为 \(d\)，向外为正。
• 两个质心都位于伸缩轴线上；它们到基座转轴的距离分别为固定值 \(l_1\) 和可变值 \(d\)。
• 两根连杆质量为 \(m_1,m_2\)。
• 两根连杆绕各自质心、垂直于运动平面的转动惯量为 \(J_1,J_2\)。
• 重力向下，大小为 \(g\)；第二根连杆相对第一根只平移、不转动。

广义坐标与驱动输入为：

\[q=\begin{bmatrix}\theta\\d\end{bmatrix}, \qquad \tau=\begin{bmatrix}\tau_1\\F_2\end{bmatrix} \]

第一项是力矩，第二项是力。混合关节的广义力向量，不要求所有分量单位相同。

第一步：位置和速度

\[p_{C_1}=\begin{bmatrix}l_1\cos\theta\\l_1\sin\theta\end{bmatrix}, \qquad p_{C_2}=\begin{bmatrix}d\cos\theta\\d\sin\theta\end{bmatrix} \]

对时间求导后：

\[v_{C_1}^2=l_1^2\dot\theta^2, \qquad v_{C_2}^2=\dot d^2+d^2\dot\theta^2 \]

第二根连杆的伸缩速度沿杆方向，旋转速度垂直于杆方向，两者正交，所以没有速度乘积项。

第二步：写动能和势能

两根连杆的角速度均为 \(\dot\theta\)，因此：

\[\boxed{ K=\frac12\left(J_1+J_2+m_1l_1^2+m_2d^2\right)\dot\theta^2 +\frac12m_2\dot d^2 } \]

由两个质心的高度得到：

\[\boxed{U=(m_1l_1+m_2d)g\sin\theta} \]

记：

\[A(d)=J_1+J_2+m_1l_1^2+m_2d^2 \]

则 \(L=\tfrac12A(d)\dot\theta^2+\tfrac12m_2\dot d^2-U\)。

第三步：对转动坐标 \(\theta\) 求导

\[\frac{\partial L}{\partial\dot\theta}=A(d)\dot\theta \]

注意 \(A\) 随 \(d\) 变化，所以：

\[\frac{d}{dt}\left(A\dot\theta\right) =A\ddot\theta+2m_2d\dot d\dot\theta \]

同时：

\[\frac{\partial L}{\partial\theta} =-(m_1l_1+m_2d)g\cos\theta \]

于是：

\[\boxed{ \tau_1=A(d)\ddot\theta +2m_2d\dot d\dot\theta +(m_1l_1+m_2d)g\cos\theta } \]

第四步：对伸缩坐标 \(d\) 求导

\[\frac{d}{dt}\left(\frac{\partial L}{\partial\dot d}\right) =m_2\ddot d \]

\[\frac{\partial L}{\partial d} =m_2d\dot\theta^2-m_2g\sin\theta \]

所以：

\[\boxed{F_2=m_2\ddot d-m_2d\dot\theta^2+m_2g\sin\theta} \]

第五步：读出 \(M、V、G\)

\[\boxed{M(q)=\begin{bmatrix}A(d)&0\\0&m_2\end{bmatrix}} \]

\[\boxed{V(q,\dot q)=\begin{bmatrix} 2m_2d\dot\theta\dot d\\ -m_2d\dot\theta^2 \end{bmatrix}} \]

\[\boxed{G(q)=\begin{bmatrix} (m_1l_1+m_2d)g\cos\theta\\ m_2g\sin\theta \end{bmatrix}} \]

第一条方程里的 \(2m_2d\dot\theta\dot d\) 是科里奥利项：边转边伸缩会影响所需力矩。

第二条方程里的 \(-m_2d\dot\theta^2\) 是离心相关项。忽略重力、固定伸长量并保持旋转时，移动关节必须提供向内的力来维持圆周运动，所以该项为负。

即使 \(M\) 是对角矩阵，两个关节仍可能通过速度项发生动力学耦合。

用简单状态检查结果

静止时，全部速度和加速度为零：

\[\tau_1=(m_1l_1+m_2d)g\cos\theta, \qquad F_2=m_2g\sin\theta \]

• 水平放置，\(\theta=0\)：转动关节承担重力力矩，伸缩轴方向没有重力分量。
• 竖直向上，\(\theta=\pi/2\)：重力对基座转轴的力矩为零，移动关节需要向上提供 \(m_2g\)。

这两个检查能帮助发现角度零点、重力符号和正方向设置中的错误。

6. 建模和使用时，记住这些区别

容易混淆的概念	正确理解
\(L\) 与总能量	\(L=K-U\)；总机械能为 \(K+U\)
势能与功	势能是状态量，功是过程中的能量传递
\(I_C\) 与 \(M\)	前者是单根刚体的质心惯量，后者是整体关节空间惯量
\(V\) 与线速度	本文 \(V\) 是速度相关广义力向量，不是速度，也不是势能
匀速与无速度项	\(\ddot q=0\) 不意味着 \(V=0\)
\(G\) 与实际重力	左侧重力项为 \(G\)，实际重力广义力为 \(-G\)
偏导与时间导数	偏导固定其他变量；时间导数沿实际运动计算

拉格朗日法没有省掉质量、质心和惯量等物理信息，而是把它们放进了能量表达式。它通常适合推导解析模型、理解耦合和分析能量；对于多关节机械臂实时计算逆动力学，递归牛顿–欧拉法往往更直接。

得到模型以后，同一个方程有两种用途：

• 逆动力学：已知 \(q,\dot q,\ddot q\)，计算 \(\tau\)。
• 正动力学：已知 \(q,\dot q,\tau\)，求解 \(M\ddot q=\tau-V-G\)，再结合初始条件积分得到运动。

掌握拉格朗日法，不必先记住所有展开后的系数。先把当前状态的动能与势能写正确，再按“对速度求偏导、对时间求导、减去对位置的偏导”执行，就能得到可解释、可检查的动力学方程。