教材翻到机械臂动力学那一章迎面就是一大片 cos、sin 矩阵。尤其讲到 2R 机械臂——两个连杆、两个旋转关节算是最简单的多关节系统了——拉格朗日方程展开式能占两三页符号密密麻麻怎么看都像一道超长的三角恒等式化简题。很多人就是在这里被劝退的。问题的关键不在推导本身而在教材默认你已经想明白了两件事关节空间和笛卡尔空间到底有什么不同以及这一堆公式里的每一项到底在物理上回答什么问题。这篇文章就来补上这个空缺。我会从关节空间和笛卡尔空间的基本差异讲起再把 2R 机械臂动力学的拉格朗日推导过程按步骤拆开最后回到坐标变换说说动力学公式为什么既要在关节空间写、又要折算到笛卡尔空间去用。如果你正在学机器人学、准备做机械臂动力学仿真或者刚开始接触机械臂的力控和阻抗控制这篇文章能帮你把教材省略的推导空白补齐。1. 教科书跳过的那三件事为什么是 2R为什么有两个空间1.1 2R 机械臂的经典地位从哪来先回答一个很多人不会问、但确实值得想一下的问题教材为什么偏爱 2R 机械臂答案不是因为它简单——单连杆更简单——而是因为它简单到能手推公式又不简单到能逼出所有关键现象。一个单连杆机械臂的动力学就是一个转动惯量乘以角加速度再加一个重力项本质和一个摆没区别。你无法用单连杆理解惯性耦合一个关节的运动影响另一个关节也无法理解科氏力两个关节速度交叉产生的力。而三连杆、六连杆机械臂推导过程动辄十几页矩阵运算手算既不现实也容易让人迷失在符号里。2R 机械臂恰好卡在中间两个广义坐标、两组拉格朗日方程、一个 2×2 的质量矩阵所有动力学核心效应都能体现公式量还在人可以手推的范围内。另一个原因是 2R 机械臂还能直观演示一个非常重要的几何现象同一个末端位置可能对应两组完全不同的关节角组合——也就是所谓的肘上和肘下解。这直接关系到关节空间和笛卡尔空间的映射关系也是后面理解雅可比矩阵奇异性的一把钥匙。1.2 关节空间和笛卡尔空间两种完全不同的语言关节空间和笛卡尔空间本质上是描述机械臂运动的两种坐标系。关节空间用关节角度 q (q₁, q₂) 和关节角速度 q̇ (q̇₁, q̇₂) 来描述状态这是电机驱动器和编码器直接看到的世界。你去按示教器上的 JOG 键一个轴一个轴地动操作的就是关节空间。笛卡尔空间则用末端执行器的位置 x (x, y) 和速度 ẋ (ẋ, ẏ) 来描述状态这是我们作为观察者看到的世界也是末端要执行任务的空间。你说让机械臂末端画一个圆这句话本质上是在笛卡尔空间里下的指令因为圆是 x-y 平面上的几何形状跟哪个关节转了多少度没有直接关系。关键在于这两个空间的映射不是线性的。关节角均匀变化时末端走的是一条弧线不是直线而末端走直线时两个关节角的变化曲线反而是复杂的非线性函数。你如果只在关节空间里设计运动轨迹末端在笛卡尔空间里就会走出完全不可预期的路径——这在很多实际应用中是不能接受的。1.3 按按钮和推末端两个空间里的动力学直觉理解了坐标系差异之后再看动力学就顺多了。动力学回答的问题是给定力/力矩机械臂怎么动但同样的力放在关节空间和笛卡尔空间里看效果完全不同。在关节空间里你给关节 1 一个力矩 τ₁关节 1 会产生角加速度这个加速度会通过连杆结构传递给关节 2。如果你在关节 2 锁定q̇₂ q̈₂ 0的情况下转动关节 1关节 1 感受到的惯性不是一个简单的常数——它取决于关节 2 当前的角度。换句话说同样一个电机力矩手臂伸直时和手臂折叠时启动速度完全不一样。在笛卡尔空间里推末端则更反直觉。你用同样大小的力推末端效果不仅依赖于当前臂形还依赖于你推的方向。最极端的例子是奇异位形当两臂完全伸直q₂ 0时你沿手臂方向推末端几乎推不动但垂直方向却很容易推动。力的大小一样方向不同响应却天差地别。这不是因为电机出力不同而是因为关节空间的动力学经过几何变换后在笛卡尔空间里呈现出了方向依赖性。这个现象在后面讲操作空间惯性矩阵时还会回到。2. 拉格朗日方程那一页能量是怎么一步步变成力矩的2.1 先做数学上一个关键简化质量集中的连杆模型教材推导 2R 机械臂动力学时几乎都不会从复杂的转动惯量张量开始而是先做一个简化假设把每根连杆的质量视为集中在某个点上。最常见的是把连杆 1 的质量 m₁ 集中在关节 1即第一个连杆的转轴处把连杆 2 的质量 m₂ 集中在末端点。这个假设把每根连杆的转动惯量都变成了质点运动的动能计算量骤减同时保留了机械臂动力学最重要的结构特征。实际工程里连杆质量是分布式的每根杆有自己的质心位置和转动惯量张量。处理方式其实一样只是在动能里多写几项如果 m₁ 的质心不在关节 1 而在距关节 1 距离 d₁ 的位置那么连杆 1 就多一个 (1/2)I₁q̇₁² 的旋转动能项。公式结构不变变的只是 M 矩阵里的具体系数。先用质心集中模型把逻辑理顺再往分布式模型推广是我比较建议的学习路径。模型参数先约定好连杆 1长度 l₁质量 m₁质心在关节 1 处连杆 2长度 l₂质量 m₂质心在末端点关节角 q₁、q₂其中 q₂ 是连杆 2 相对连杆 1 的角度重力加速度 g方向竖直向下2.2 末端点位置和速度动能展开中那个 cos(q₂) 是从哪冒出来的写出末端点的位置x l₁ cos(q₁) l₂ cos(q₁ q₂) y l₁ sin(q₁) l₂ sin(q₁ q₂)对时间求导得到末端速度ẋ -l₁ sin(q₁) q̇₁ - l₂ sin(q₁ q₂)(q̇₁ q̇₂) ẏ l₁ cos(q₁) q̇₁ l₂ cos(q₁ q₂)(q̇₁ q̇₂)教科书通常会直接跳到一个结论动能 T 等于一大串带 cos(q₂) 的表达式。中间那个关键的平方展开没写很多人就卡在这里。我把这一步补全v² ẋ² ẏ² l₁²q̇₁² l₂²(q̇₁ q̇₂)² 2l₁l₂[cos(q₁)cos(q₁q₂) sin(q₁)sin(q₁q₂)]q̇₁(q̇₁ q̇₂)方括号里那一项用三角恒等式 cos(a-b) cos(a)cos(b) sin(a)sin(b)正好等于 cos(q₂)。这就是为什么 M 矩阵里会出现 cos(q₂)——它本质上是两根连杆之间夹角 q₂ 的余弦因为末端点速度的交叉项来自两段连杆速度之间的投影关系。于是系统总动能可以写成T (1/2)m₁(l₁q̇₁)² (1/2)m₂[l₁²q̇₁² l₂²(q̇₁q̇₂)² 2l₁l₂cos(q₂)q̇₁(q̇₁q̇₂)]把平方项完全展开并按 q̇₁²、q̇₁q̇₂、q̇₂² 分组T (1/2)M₁₁q̇₁² M₁₂q̇₁q̇₂ (1/2)M₂₂q̇₂²其中M₁₁ (m₁ m₂)l₁² m₂l₂² 2m₂l₁l₂cos(q₂) M₁₂ m₂l₂² m₂l₁l₂cos(q₂) M₂₂ m₂l₂²这其实就是质量矩阵 M(q) 里的三个独立元素。注意动能表达式里没有单独的q̇₂²交叉项拆两半的歧义——写成 (1/2)M₁₁q̇₁² M₁₂q̇₁q̇₂ (1/2)M₂₂q̇₂² 时M₁₂ 的定义才和后面的拉格朗日求导不自洽问题完全对齐。2.3 势能重力的贡献写起来最直接势能只来自重力。质量 m₁ 集中在关节 1高度为 l₁sin(q₁)质量 m₂ 集中在末端高度为 l₁sin(q₁) l₂sin(q₁ q₂)。总势能V m₁g·l₁sin(q₁) m₂g·[l₁sin(q₁) l₂sin(q₁ q₂)] (m₁ m₂)gl₁sin(q₁) m₂gl₂sin(q₁ q₂)到这里还没用到拉格朗日方程但你已经能看出一些东西势能只依赖于位形 q不依赖于速度 q̇。也就是说重力项在动力学方程里完全由角度决定跟机械臂怎么运动、多快运动无关。2.4 欧拉-拉格朗日方程对两个广义坐标分别求导拉格朗日函数 L T - V然后对每个广义坐标 qᵢ 套欧拉-拉格朗日方程τᵢ d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ这个式子的物理含义很直观d/dt(∂L/∂q̇ᵢ) 是广义动量的变化率∂L/∂qᵢ 是广义力含惯性力、重力等的变化二者之差就是需要外部施加的力矩。对 q₁ 求∂L/∂q̇₁ M₁₁q̇₁ M₁₂q̇₂ d/dt(∂L/∂q̇₁) M₁₁q̈₁ M₁₂q̈₂ Ṁ₁₁q̇₁ Ṁ₁₂q̇₂注意这里 M₁₁ 和 M₁₂ 是 q₂ 的函数所以它们的导数不为零Ṁ₁₁ -2m₂l₁l₂sin(q₂)q̇₂ Ṁ₁₂ -m₂l₁l₂sin(q₂)q̇₂于是 d/dt(∂L/∂q̇₁) M₁₁q̈₁ M₁₂q̈₂ - 2m₂l₁l₂sin(q₂)q̇₁q̇₂ - m₂l₁l₂sin(q₂)q̇₂²∂L/∂q₁ ∂T/∂q₁ - ∂V/∂q₁ 0 - [(m₁m₂)gl₁cos(q₁) m₂gl₂cos(q₁q₂)]所以τ₁ d/dt(∂L/∂q̇₁) - ∂L/∂q₁ M₁₁q̈₁ M₁₂q̈₂ - 2m₂l₁l₂sin(q₂)q̇₁q̇₂ - m₂l₁l₂sin(q₂)q̇₂² (m₁m₂)gl₁cos(q₁) m₂gl₂cos(q₁q₂)对 q₂ 求类似可得τ₂ M₁₂q̈₁ M₂₂q̈₂ m₂l₁l₂sin(q₂)q̇₁² m₂gl₂cos(q₁q₂)这两个式子就是 2R 机械臂逆动力学的完整解析解。相比教材里那一整页公式这个推导过程其实只有十几行——关键在于中间那个速度平方展开和时间导数这两步不做后面全对不上。3. 质量矩阵、科氏力和重力项动力学公式里的三个角色3.1 M(q)质量矩阵里藏着有效惯量把上面的 τ₁ 和 τ₂ 整理成标准形式τ M(q)q̈ C(q, q̇)q̇ G(q)M(q) 就是质量矩阵也叫惯性矩阵三个独立元素分别是M₁₁ (m₁ m₂)l₁² m₂l₂² 2m₂l₁l₂cos(q₂) M₁₂ M₂₁ m₂l₂² m₂l₁l₂cos(q₂) M₂₂ m₂l₂²M₁₁ 是第一关节看到的总惯量。它由三部分组成连杆 1 的贡献 (m₁m₂)l₁²、连杆 2 末端自带的质量 m₂l₂²、以及耦合项 2m₂l₁l₂cos(q₂)。耦合项随 q₂ 变化意味着第一关节的有效惯量随时在变。取具体数值感受一下。设 m₁ m₂ 1 kgl₁ l₂ 1 mq₂ 0手臂完全伸直时M₁₁ 4 kg·m²这是最大惯量此时关节 1 转起来最重q₂ π手臂完全折叠时M₁₁ 1 kg·m²最小惯量此时关节 1 转起来最轻这个差异不是百分之几是四倍。所以如果在控制里把质量矩阵当成常数效果就是手臂伸直时力矩不足、折叠时力矩过大运动轨迹一定会出问题。M₁₂ M₂₁ 是耦合项。它表示关节 2 的加速度会对关节 1 产生反作用力矩反过来也一样。这是多关节系统的核心特征你动第 2 个关节第 1 个关节会感觉到一个额外力矩。如果做控制时忽略耦合项高速高加速运动下误差会非常明显。M₂₂ m₂l₂² 在质量集中假设下是常数。这符合直觉第二关节的惯量只跟自身连杆长度和质量有关跟第一关节角度无关。实际连杆若质量分布不均匀M₂₂ 还会有更多项。M(q) 还有一个重要性质对称正定。对称性是拉格朗日推导自然保证的正定性对应动能恒正只要速度不为零T 0。正定性还可以用来做自检如果算出的 M 矩阵某个主对角线元素小于等于零或者行列式小于等于零公式一定写错了。3.2 C(q, q̇)科氏力和离心力是假装成力的惯量项C 项对应的是科氏力和离心力。注意一个表面反直觉的事实其中并没有真正的力来源它们是描述在旋转参考系里运动的惯性效应。在我们推导的 2R 公式中科氏力-2m₂l₁l₂sin(q₂)q̇₁q̇₂在 τ₁ 中和 m₂l₁l₂sin(q₂)q̇₁²在 τ₂ 中离心力-m₂l₁l₂sin(q₂)q̇₂²在 τ₁ 中科氏力的特征是包含两个不同速度的乘积比如 q̇₁q̇₂。物理含义是关节 2 在转动这个转动会通过几何关系拽着关节 1让关节 1 感受到一个跟两个关节速度都成正比的力矩。速度越高这个交叉耦合越大。离心力的特征是某个关节自身速度的平方。比如 τ₁ 里的 -m₂l₁l₂sin(q₂)q̇₂²意思是关节 2 高速转动时会在关节 1 上产生一个往外甩的力矩。一个有用的生活类比坐在旋转木马上伸手碰栏杆。旋转本身会给你一个向外的离心感而当你试图在旋转同时做径向往返移动时会感受到一个垂直方向的科氏感——感觉像有东西在推你其实只是惯性在旋转坐标下的表现。C 项的符号还和 sin(q₂) 的符号有关。当 q₂ 在 (0, π) 区间肘部弯曲时 sin(q₂) 0当 q₂ 在 (-π, 0) 区间肘部反向弯曲时 sin(q₂) 0。这意味着同样的速度组合在肘部不同侧科氏力的方向可能完全相反。所以不要试图用固定方向的补偿项去抵消它必须实时根据 q₂ 计算。3.3 G(q)挂在手臂上的重力为什么不能省G(q) 是重力矩向量G₁ (m₁ m₂)gl₁cos(q₁) m₂gl₂cos(q₁ q₂) G₂ m₂gl₂cos(q₁ q₂)重力项只跟角度有关跟速度和加速度无关。它的物理意义很直接机械臂静止在某个位形时为了不掉下来电机必须持续输出的力矩。在机械臂完全静止时q̈ q̇ 0动力学方程只剩 τ G(q)这个情况下测得的力矩几乎就是重力矩的实测值——这正好可以用来验证动力学模型。在控制实现里重力补偿是最基本的前馈项。机械臂保持同一姿态静止时如果控制器完全没有重力补偿就需要靠 PD 控制器的积分项或稳态误差来硬扛重力。轻负载还好负载重时要么稳态误差增大要么积分饱和控制质量明显下降。工业机械臂标定完连杆参数后第一件事就是验证重力补偿的准确性。4. 从关节空间到笛卡尔空间坐标变换里藏着的动力学4.1 雅可比矩阵两个空间之间的翻译官关节空间到笛卡尔空间的运动学映射需要一个关键工具雅可比矩阵 J(q)。速度层面的映射关系是ẋ J(q)q̇对 2R 机械臂J [[-l₁sin(q₁) - l₂sin(q₁q₂), -l₂sin(q₁q₂)], [l₁cos(q₁) l₂cos(q₁q₂), l₂cos(q₁q₂)]]每一列对应一个关节每一行对应笛卡尔空间的一个方向。J(q) 的第 i 列第 j 行表示第 j 个关节速度对第 i 个笛卡尔速度分量的贡献。力/力矩的映射比速度映射更简洁不需要任何动力学信息由虚功原理直接得到τ JᵀF其中 F 是施加在末端的外力τ 是各关节需要输出的力矩。这个式子的推导思路是末端移动 δx 时外力做的功 Fᵀδx等于关节移动 δq 时电机做的功 τᵀδq代入 δx Jδq就得到 τ JᵀF。整个过程只有几何关系跟质量、速度、加速度无关。4.2 操作空间动力学为什么要在笛卡尔空间写方程既然已经有了关节空间的动力学 τ M(q)q̈ C(q,q̇)q̇ G(q)为什么还要研究笛卡尔空间操作空间的动力学原因是很多任务天然在笛卡尔空间定义。比如让末端保持恒定接触力打磨表面、让末端在碰撞时柔性退让这些需求如果用关节空间来表达会非常别扭而直接在笛卡尔空间设计力/阻抗控制则自然得多。操作空间的质量矩阵是这样折算的Λ(q) J⁻ᵀM(q)J⁻¹它把关节空间的惯量折算到笛卡尔空间描述末端沿某个方向加速时感受到的等效惯量。注意 Λ(q) 是随位形变化的而且它衡量的是方向相关的惯量——同一个末端位置沿 x 方向推和沿 y 方向推等效质量可能完全不同。这里有个非常重要的现象当机械臂接近奇异位形时J 的行列式趋于零J⁻¹ 不存在Λ 的某些特征值趋于无穷大。物理上对应的是在某些方向上末端无论施加多大的力都推不动。回到 1.3 里的场景——两臂伸直时沿臂方向推末端力会直接传递到机械臂的结构约束上关节几乎不产生角加速度但垂直方向却很容易。这就是奇异位形在动力学里的真实投影。计算一下 2R 机械臂 J 的行列式det(J) l₁l₂sin(q₂)当 q₂ 0 或 q₂ π 时det(J) 0机械臂进入奇异位形。这个简洁结果解释了为什么手臂伸直和手臂完全折叠如此特殊在这两个位形下末端丧失了一个方向上的运动能力笛卡尔空间描述会直接失效。4.3 关节空间 vs 笛卡尔空间控制里的实际选择实际控制系统的设计关节空间和笛卡尔空间各有分工。关节空间控制最成熟、最稳定。把笛卡尔轨迹经过逆运动学映射成关节角轨迹然后在关节空间做 PID 或计算力矩控制这是工业机械臂最常见的做法。原因很实际编码器装在电机侧力矩指令也是发给电机关节空间是控制器的母语。笛卡尔空间控制在需要力交互的场景里更自然。比如阻抗控制期望行为通常写成末端受力 期望阻抗 × 位置误差这天然是笛卡尔空间的表达。实现时要把期望的笛卡尔力 F 通过 τ JᵀF 映射回关节力矩命令再叠加动力学补偿。我的经验是不要在关节空间里强行拟合笛卡尔轨迹更不要无视奇异位形去逆解。一个稳妥的架构是笛卡尔空间做规划关节空间做执行交互任务加上笛卡尔空间阻抗层把输出通过雅可比映射回关节力矩。5. 纸上公式到仿真验证踩坑记录与验证技巧5.1 用对称正定性质快速自检 M 矩阵公式推完第一件事不是急着仿真而是做几个快速检查。用一组好算的参数m₁ m₂ 1 kgl₁ l₂ 1 m取 q₁ 45°、q₂ 60°则 cos(60°) 0.5sin(60°) ≈ 0.866。M₁₁ 2×1 1 2×1×1×0.5 4 M₁₂ 1 0.5 1.5 M₂₂ 1对称性M₁₂ M₂₁满足。 正定性主对角线元素 4 0、1 0行列式 det(M) 4×1 − 1.5² 1.75 0满足。如果代入参数后 M 不对称或者行列式不为正第一步不是怀疑物理而是回去检查推导。我用这个办法抓出过不少符号错误。5.2 逆动力学仿真的验证思路验证动力学公式最直接的方法是构造一组已知运动用逆动力学算出力矩再把这个力矩作为输入用正向动力学仿真机械臂运动看轨迹能不能回到原来的参考轨迹。这里给一个 2R 逆动力学的 Python 实现可以直接拿去对照验证import numpy as np def r2_inv_dyn(q, qd, qdd, m11.0, m21.0, l11.0, l21.0, g9.81): q1, q2 q dq1, dq2 qd ddq1, ddq2 qdd s2 np.sin(q2) c2 np.cos(q2) # 质量矩阵 M(q) M11 (m1 m2) * l1**2 m2 * l2**2 2 * m2 * l1 * l2 * c2 M12 m2 * l2**2 m2 * l1 * l2 * c2 M22 m2 * l2**2 # 科氏力和离心力 C(q, qd) * qd c1 -2 * m2 * l1 * l2 * s2 * dq1 * dq2 - m2 * l1 * l2 * s2 * dq2**2 c2 m2 * l1 * l2 * s2 * dq1**2 # 重力项 G(q) G1 (m1 m2) * g * l1 * np.cos(q1) m2 * g * l2 * np.cos(q1 q2) G2 m2 * g * l2 * np.cos(q1 q2) tau1 M11 * ddq1 M12 * ddq2 c1 G1 tau2 M12 * ddq1 M22 * ddq2 c2 G2 return np.array([tau1, tau2])验证思路分三步。第一步设 q̈ 0、q̇ 0此时输出的力矩应当恰好等于重力矩 G(q)和理论值逐项对比。第二步在重力补偿的前提下做小幅快速运动验证 M 矩阵项是否准确。第三步做匀速大幅运动q̈ ≈ 0此时力矩应主要等于 C 项和 G 项之和可以专门检验科氏力计算。正向动力学仿真时要解 M(q)q̈ τ − C − G 这个线性方程组。2R 机械臂的 M 是 2×2 矩阵直接解方程即可不要写 M 的逆矩阵再乘右边——数值稳定性差。5.3 几个必踩的坑第一个坑是 q₂ 的零位定义不一致。有的书用 q₁ q₂ 表示连杆 2 的绝对角度有的用 q₂ 表示相对角度。两个定义推导出来的公式M 矩阵和 C 项看起来完全不同但物理上是等价的。如果你照着某个公式写代码却用另一个定义去解释结果就会发现 M 矩阵不对称或者 G 项符号对不上。解决办法是先明确自己的 q 定义再用动能恒正和对称正定性做自检不要盲信公式。第二个坑是把 M 矩阵当成常数。M₁₁ 随 q₂ 变化这在第 3.1 节已经强调过。如果你想自己做计算力矩控制每步都必须根据当前 q 重新计算 M、C、G不能只在初始化时算一次。我见过不少人在仿真里用恒定 M低速问题不大一跑高速轨迹就发散找半天找不出原因。第三个坑是奇异位形下求逆。在 q₂ 0 或 q₂ π 时 J 不可逆直接算 J⁻¹ 会得到无穷大的数。如果确实需要笛卡尔空间的逆用阻尼最小二乘形式J* Jᵀ(JJᵀ λ²I)⁻¹其中 λ 是阻尼系数在接近奇异位形时取较大的值可以有效避免数值爆炸。这也是实际操作空间控制和阻抗控制时的常规做法。第四个坑是单位问题。角度用弧度还是度、长度用米还是毫米、质量用千克还是克都会直接改变公式输出的数量级尤其是 g 9.81 代入时单位错了重力项可以差出几个数量级。写代码时最好强制把所有量统一到 SI 单位再传入动力学函数。调试动力学代码时我的习惯是先测重力和静力矩固定位形给定角度读力矩值再测惯量项让一个关节做高频小幅振动另一个锁死最后测科氏力双关节同时大幅度匀速运动。每一步用一个独立测试确认不要等整套仿真跑起来再排查到时候错误来源一团乱麻定位成本高得多。2R 机械臂的动力学看起来是一堆复杂的三角函数拆开之后核心结构其实就三块质量矩阵、科氏力/离心力、重力项。把这三块的物理含义搞清楚再从关节空间通过雅可比折算到笛卡尔空间后面不管是做六轴机械臂的仿真还是写阻抗控制思路都能保持清晰。