每次遇到动力学章节我都想吐槽两句前面几页讲正运动学画个坐标架、套个余弦定理末端位置就出来了觉得还挺简单。然后翻到动力学一脸 M(q)q̈ C(q,q̇)q̇ G(q) τ 拍在脸上满纸的下标和偏导数像在看天书。2R 机械臂已经是平面两连杆里最简单的一种了这几页到底在干什么为什么从关节空间到笛卡尔空间这么简单的一件事非要扯出一大堆惯性矩阵、科氏力、重力项这篇内容就是把我自己啃动力学时踩过的坑、核对过的公式、后来做仿真和实验时攒下来的理解重新梳理一遍。它不是什么新东西就是 2R 机械臂从运动学走向动力学的那几步推导适合正在学机器人学、准备做机械臂控制仿真、或者毕设里要用到动力学模型的人。如果你能跟着把公式手推一遍后半辈子看任何机械臂论文里的动力学方程都会顺眼很多。1. 先搞清楚两个空间再看动力学才有根1.1 关节空间所有“变量”真正住的地方机械臂的“关节空间”这个概念本质上就是在说这玩意儿到底有几个自由度每个自由度转了多少度。2R 机械臂有俩旋转关节所以关节空间就是由 (q1, q2) 两个角度张成的二维空间。你只要告诉控制器“关节 1 转到 30 度关节 2 转到 45 度”机械臂的形态就完全确定了。这里不需要关心末端在哪儿也不关心臂长多少只管两个电机的转角。为什么说“变量真正住在这里”因为在真实控制里伺服驱动器只知道关节角度、关节角速度、关节角加速度。你给电机发的指令是电流环的力矩反馈回来的是编码器的角度。也就是说控制闭环里的每一个信号本质都是关节空间的信号。笛卡尔空间的位置信息最终也要先反解成关节角再发给电机去执行。我第一次做仿真的时候犯过一个典型错误直接在笛卡尔空间画了个目标点然后告诉机械臂“你末端给我走过去”。结果根本没有这么玩的。你得先把目标点逆解成 (q1, q2)再考虑怎么让关节转过去。这就是关节空间作为底层空间的含义。1.2 笛卡尔空间用户视角下的末端位置笛卡尔空间就是平常说的 xyz 世界坐标系。对于 2R 机械臂它描述的是末端执行器第二根杆的末端一般叫 TCPTool Center Point在平面上的位置 (x, y)以及末端姿态。关节空间到笛卡尔空间的映射就是正运动学给你 (q1, q2)算出 (x, y)。反过来笛卡尔空间到关节空间的映射就是逆运动学给你 (x, y)反解 (q1, q2)。看起来这俩空间就是“正反算”的关系对吧但真正的难点在于用户天然习惯在笛卡尔空间里规划轨迹——“我要末端走一条直线”这种需求太自然了。可是要让直线轨迹能被执行每一步都得先反解成关节角序列再让关节一路追过去。这个过程中间就牵扯到一个核心问题笛卡尔空间里看起来均匀的一条直线在关节空间里对应的角速度/角加速度完全不是均匀的甚至可能非常剧烈。末端直线运动意味着两个关节要协同变速。这就是为什么我们需要研究速度层面的映射——不只是位置的映射而是位置的导数怎么映射这就是下一节要讲的雅可比矩阵。1.3 空间映射的三层递进我之前像背课文一样把这三层关系串过很多遍后来发现记这三句话就够了位置层面q → x正运动学用几何法解决速度层面q̇ → ẋ雅可比矩阵用求导解决力/动力学层面τ → q̈以及 q̇、q、末端力的关系动力学方程用拉格朗日或牛顿-欧拉法解决前两层解决的是“末端怎么动”的问题第三层解决的是“要让它这么动关节得使多大劲”的问题。一个机械臂如果你只让它慢慢动那前三层都无所谓反正重力顶着、慢慢挪就到位置了。但一旦要求它快速运动、带负载、或者精确跟踪轨迹动力学模型就从“选学内容”变成了“必学内容”。而且动力学还有一个容易被忽略的作用它告诉你关节之间不是独立的。你动关节 1会产生一个力矩作用到关节 2 上关节 2 加速也会反作用到关节 1。这就是科氏力和离心力的物理来源。不搞懂这个联合仿真里机械臂稍微跑快点就发散你还不知道哪里错了。2. 位置怎么映射2R 正运动学拆解2.1 2R 结构参数与坐标系建立2R 机械臂的模型到底长什么样每个人画的图可能略有不同我这里给一个最常用的几何建模方便后面统一口径杆 1 长度 L1质量 m1质心在杆 1 中点杆 2 长度 L2质量 m2质心在杆 2 中点关节点O 点是基座A 点是关节 1B 点是关节 2末端点是 Pq1 是杆 1 与 x 轴正方向的夹角q2 是杆 2 相对杆 1 延长线的夹角也就是相对角这个 q2 的定义方式有讲究因为后面算雅可比和动力学公式时正负号完全取决于 q2 是相对角还是绝对角建议从一开始就统一用一个约定别在两种约定之间反复横跳我试过真的会错乱。有了这个结构定义之后位置就不难推了。x 方向就是第一根杆的投影加上第二根杆在自己角度下的投影y 方向同理。2.2 末端位置的推导正运动学公式[ x L_1 \cos q_1 L_2 \cos(q_1 q_2) ][ y L_1 \sin q_1 L_2 \sin(q_1 q_2) ]这个公式看起来平平无奇但在做动力学之前值得把它的结构再看一眼末端位置是三角函数嵌套的也就是说对 q1、q2 求导的时候会出现非常多的项并且到处都带着 sin(q1q2)、cos(q1q2) 这种复合角。这就是为什么后面雅可比矩阵和动力学方程的符号会像滚雪球一样膨胀。如果你用几何法而不是 DH 参数法建的模型那么这一步就是纯粹的正余弦展开很好理解。如果是用 DH 参数做的那也只是一堆旋转矩阵乘出来的结果最后化简得到的还是上面这个式子。2.3 一个小例子算一算就知道空间差在哪举个例子假设 L1 0.5mL2 0.5m目标点位在 (0.6, 0.6)。显然如果直接用笛卡尔坐标我们只知道末端位置但机械臂两个关节各是多少度并不直观。逆解一下[ r \sqrt{x^2y^2} \approx 0.8485 ][ \cos q_2 \frac{r^2 - L_1^2 - L_2^2}{2L_1L_2} \frac{0.72 - 0.25 - 0.25}{0.5} 0.44 ]于是 q2 ≈ ±63.9°再代进去求 q1会得到两个解。这就是“笛卡尔空间一个点关节空间两个构型”的物理含义。做轨迹规划的时候你不仅要选路径点还得选“关节构型分支”不然机械臂会走一个你完全没预料到的姿态。位置层面的映射到这里就结束了。它回答的是“我该转动多少末端才能到那儿”。但紧接着的问题是如果末端要以某个速度沿着某条路径运动关节得转多快3. 速度怎么映射雅可比矩阵3.1 为什么在关节空间想事更方便在开始推导雅可比之前我想先回答一个问题为什么我们要老在关节空间里折腾而不是直接在笛卡尔空间里把机械臂当成一个点质量来建模因为末端不是单独存在的它被连杆约束着。你没法在笛卡尔空间里给末端随便施加一个力机械臂就会乖乖地朝那个方向加速。你实际能控制的只有关节力矩 τ1 和 τ2。终端执行器那个点在物理上只是杆件结构的一部分没有独立的执行器。这就导致所有笛卡尔空间的需求最终都要变成关节空间的力矩/速度指令。雅可比矩阵就是这两个空间速度之间的“汇率换算表”。3.2 雅可比从哪来对关节变量求偏导把正运动学公式对时间求导[ \dot{x} -L_1 \sin q_1 \dot{q}_1 - L_2 \sin(q_1q_2)(\dot{q}_1\dot{q}_2) ][ \dot{y} L_1 \cos q_1 \dot{q}_1 L_2 \cos(q_1q_2)(\dot{q}_1\dot{q}_2) ]写成矩阵形式[ \begin{bmatrix} \dot{x} \ \dot{y} \end{bmatrix}\begin{bmatrix} -L_1 \sin q_1 - L_2 \sin(q_1q_2) -L_2 \sin(q_1q_2) \ L_1 \cos q_1 L_2 \cos(q_1q_2) L_2 \cos(q_1q_2) \end{bmatrix} \begin{bmatrix} \dot{q}_1 \ \dot{q}_2 \end{bmatrix} ]这个 2×2 的矩阵就是雅可比矩阵 J。每一列对应一个关节的微小转动对末端位置造成的影响。雅可比矩阵特别值得仔细看因为它的每一项都是位置的偏导数。也就是说雅可比矩阵本身是随关节角度变化的。机械臂在某个位形下速度映射的比例完全不一样。比如当两个手臂完全伸直时关节 2 转动对末端点 x 方向的影响和两个手臂折叠时的影响差出好几倍。3.3 奇异位形的直觉理解说到雅可比就不可能绕开奇异位形。当 q2 0两个杆完全展开成一条直线或者 q2 π两个杆完全折叠重合时雅可比矩阵的秩会降为 1。这意味着末端在某些方向上是无法获得速度的。直观理解就是伸直了胳膊你只能沿着胳膊长度方向推拉没法横向猛地发力。任何想沿着奇异性方向运动的轨迹都会让关节速度趋向无穷大。我当时做轨迹规划随便在笛卡尔空间画直线穿过奇异点附近一仿真就发现关节速度飙到天上然后被驱动器的速度限制截断末端轨迹变形。后来才知道要在轨迹规划阶段就检测奇异躲开这些位置。速度层面讲完了。但是光知道关节该转多快还不够控制器真正发出的指令是力矩。而力矩和运动之间的关系就是动力学方程的地盘。4. 力怎么映射2R 机械臂动力学方程4.1 拉格朗日法的思路高频考点来了。2R 机械臂动力学推导最常见的方法是拉格朗日法它在思想上极其简单先算系统的总动能 K 和总势能 V然后构造拉格朗日量 L K - V再代入第二类拉格朗日方程[ \frac{d}{dt} \frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} \tau_i ]这个方程的物理含义是系统沿着真实轨迹演化时动能和势能之间的变化与外力做的功必须匹配。你不需要像牛顿法那样把每个连杆之间的内力全拆开只需要站在全局能量的角度算。但这句话说起来容易做起来全是偏导数和链式法则。我第一次推的时候光算动能项就反复对了好几遍因为杆 2 的速度是“牵连运动 相对运动”既包含关节 1 的转动带来的速度也包含关节 2 的转动带来的速度展开的时候特别容易漏项。4.2 2R 动力学方程长什么样完整推导过程如果全写出来会很长而且很容易让读者以为自己在看教材。我直接给出使用了点质量或集中质量后的常见结果方便对照你的教材和代码。令总动能 K K1 K2其中[ K_1 \frac12 m_1 v_{c1}^2 \frac12 I_1 \dot{q}_1^2 ][ K_2 \frac12 m_2 v_{c2}^2 \frac12 I_2 (\dot{q}_1\dot{q}_2)^2 ]注意v_{c1} 是杆 1 质心速度v_{c2} 是杆 2 质心速度。严格推导时v_{c2} 需要对 q1、q2 都求偏导这会让你得到特别多带 cos(q2) 的交叉项。最终整理成标准形式之后就是[ M(q)\ddot{q} C(q,\dot{q})\dot{q} G(q) \tau ]其中各项的具体表达形式根据“把质心放在哪、转动惯量是否集中”会有差异。我用点质量模型m1、m2 集中在各自杆件末端来写一个最常见的简化结果方便大家和教材对照[ M_{11} m_1 l_1^2 m_2(l_1^2 l_2^2 2l_1 l_2 \cos q_2) I_1 I_2 ][ M_{12} M_{21} m_2(l_2^2 l_1 l_2 \cos q_2) I_2 ][ M_{22} m_2 l_2^2 I_2 ]这里的 I1、I2 是杆件绕自身质心的转动惯量。如果你做仿真直接把转动惯量设成 0那就是一个纯点质量模型公式会简单很多但也会丢掉一部分动力学特性。科氏项和离心项合在一起写的话[ C(q,\dot{q})\dot{q}\begin{bmatrix} -m_2 l_1 l_2 \sin q_2 (2\dot{q}_1 \dot{q}_2 \dot{q}_2^2) \ m_2 l_1 l_2 \sin q_2 \dot{q}_1^2 \end{bmatrix} ]第一行含 2q̇1q̇2 的是科氏力矩含 q̇2² 的是离心力矩第二行只有 q̇1²也是离心力矩。这俩在实际控制中非常容易混淆我的经验是记公式的时候把“哪一项导致哪一项产生”想清楚——科氏力必须有两个速度同时不为零才出现离心力则只要对应关节自己在转就有。重力项[ G(q) \begin{bmatrix} (m_1 m_2)g l_1 \cos q_1 m_2 g l_2 \cos(q_1q_2) \ m_2 g l_2 \cos(q_1q_2) \end{bmatrix} ]这个表达式看起来复杂物理含义其实一句话就能说清楚你想让关节保持静止不动就得额外输出一个力矩去抵消重力。在低速运动时重力项甚至比惯性项和科氏项加起来还大。4.3 惯性、科氏和重力三项的物理意义不要机械地去背 M 矩阵里的每一项要理解每一项到底在跟谁较劲。先说惯性矩阵 M(q)。它描述的是当两个关节各自加速时需要多大的力矩来推动。这里有个非常有意思的现象M11 和 M12 都含有 cos(q2) 项。这意味着当第二个关节的角度变化时第一个关节的“等效惯量”也在变化。两个手臂完全伸展开时整个机械臂对关节 1 来说就像一个长杆等效惯量最大两个手臂折叠起来时质量离关节 1 更近等效惯量更小。这就是机器人学里常说的“变惯量”特性。普通线性系统不会遇到这个问题但机械臂是强耦合非线性系统惯量随位形变化是它的天生属性。再说 C 项包括科氏力和离心力。科氏力出现的原因是当一个关节在转动时另一个关节也在运动前者的转动会使得后者的运动轨迹变成曲线从而需要额外的力来维持这种“曲线运动”。日常生活的类比就是你在旋转木马上从边缘往里走会感觉到一种横向的力推你这就是科氏力。而离心力则是当一个关节匀速转动时另一个关节想保持静止也会被“甩”出去的趋势。重力项 G 最朴实就是地球引力对每个杆件产生的力矩。机械臂在真实环境里待机悬停时电机一直在“偷偷使劲”顶着重力这就是重力补偿在起作用。把这三项拆开之后你就知道 2R 动力学这几页到底在干什么它把一个看起来大概是“多连杆相互牵连的复杂系统”用一组二阶非线性微分方程描述了出来。虽然符号吓人但它每一个符号背后都是一个物理作用力的来源。5. 动力学这几个公式到底怎么用5.1 仿真已知力矩怎么得到运动很多人学动力学学完就完了也不知道这堆公式能干嘛。第一件能做的事就是做正向动力学仿真。正向动力学解决的是“给定关节力矩求关节加速度”的问题。把标准方程变换一下[ \ddot{q} M(q)^{-1} \left[ \tau - C(q,\dot{q})\dot{q} - G(q) \right] ]也就是说如果我现在给两个关节施加特定的力矩 τ1 和 τ2那么机械臂在这一瞬间的加速度是多少完全由上面的公式决定。然后你拿这个加速度去积分得到速度再积分得到位置就能在计算机里模拟出机械臂整个运动过程。这也解释了一个问题为什么仿真里机械臂的运动会那么“自然”因为它遵循的物理规律和真实机械臂一致。你在仿真里看到机械臂会因为惯性冲过头、会因为有科氏力导致末端轨迹歪掉这些现象都是动力学方程直接带给你的不是写代码的人故意加的。5.2 前馈控制模型算出的力才是前馈第二个用途就是计算力矩控制。如果你只用一个简单的 PID 控制器去控制机械臂你会发现效果很差。不是因为 PID 不好而是 PID 不知道机械臂内部复杂的非线性力学关系只能靠误差一点点去“试错”。增益调得低跟踪慢增益调得高又会振荡甚至发散。更聪明的做法是根据期望的关节加速度 q̈_des提前用动力学模型算一遍把所需的力矩算出来然后再加上一个修正反馈。控制律长这样[ \tau M(q) \left( \ddot{q}{des} K_d(\dot{q}{des}-\dot{q}) K_p(q_{des}-q) \right) C(q,\dot{q})\dot{q} G(q) ]这个控制律看起来复杂但思路非常简单前馈项负责“预测需要多大劲”反馈项负责“预测不准的地方给补上”。你会发现使用了动力学前馈之后同样的跟踪精度要求下可以省下大量调 PID 增益的时间。我之前调过一个模拟机械臂没有前馈时末端走个圆形轨迹误差大得离谱加了动力学前馈之后同样的 PID 参数跟踪误差直接小了一个数量级。5.3 参数辨识M(q) 里的数字怎么来很多初学者拿到的 2R 机械臂动力学公式都是“字母版”的——m1、m2、l1、l2、I1、I2 都是符号。但真正拿到一台实物机械臂这些参数不是写在铭牌上的至少不是直接写在铭牌上的。那怎么弄有一个很标准的流程叫“动力学参数辨识”。做法是让机械臂按一组设计好的激励轨迹运动采集关节角度、速度、加速度以及电流/力矩信号然后把动力学方程改写成关于待辨识参数的线性形式[ \tau Y(q, \dot{q}, \ddot{q}) \pi ]这里的 Y 叫回归矩阵π 就是所有待辨识参数构成的向量。然后通过最小二乘法等一系列手段反推出模型参数。等这套流程跑完你会发现动力学方程里的 M、C、G 再也不是符号了而是带着单位的一堆具体数。有了具体数才能做高精度的前馈控制才能真正在实物上把控制器调得像样。6. 实操心得与常见问题速查6.1 手推一遍才知道的事儿我在学 2R 机械臂动力学的时候最大的心得就是别直接抄公式至少自己手推一遍。说个真事儿。当时我按教材推导算出来的 M 矩阵居然不对称而且 M12 和 M21 差了一个 I2 的系数。翻来覆去对了好几遍才发现是我在算杆 2 的动能时把“绕关节 2 的转动”和“绕杆 2 质心的转动”重复计算了一遍。这种错误如果你不自己一步步推而是直接看最终公式根本发现不了而一旦动力学方程写错后面前馈控制就全乱了。另外如果你用的是拉格朗日法一定要特别注意“链式求导”过程中对 q̇ 和对 q 的偏导对象。一个容易被坑的点是动能项对 q̇ 求偏导得到的东西再对时间求导会产生张量级别的项。你在教科书里看到的很多“神奇化简”其实背后都是因为这里能合并同类项。推完之后强烈建议用符号计算工具验证一遍。我自己用的是 Python 生态里的 SymPy把 q1、q2 设成符号变量定义好 x、y 表达式然后一阶导、二阶导全部交给符号引擎自动展开动能、求偏导、整理运动方程。你会发现手推出来的结果和符号工具算出来的结果对上了那种踏实感是无与伦比的。6.2 常见错误速查表错误现象可能原因排查方式质量矩阵 M 不对称推导时动能项漏算或重复计算检查杆 2 速度是否同时含 q1 和 q2 的影响检查 I2 是否被多算一次仿真中末端振动越来越剧烈用了不正确的 C 矩阵没抵消科氏/离心项用能量守恒检查无外力时系统总能量应基本不变前馈力矩太大导致电机饱和转动惯量参数设得过大把 I 参数和集中质量参数分开核对必要时用小角度阶跃辨识奇异位置附近速度突跳忽略了雅可比条件数异常实时计算 Jacobian 的条件数超过阈值时切换轨迹或降低速度动力学仿真发散积分器步长过大刚性问题改用变步长积分器或把质量矩阵求逆改为求解线性方程组6.3 工具与验证建议如果你不想全手写直接上现成库我的建议是先理解再做别上来就调库。否则你根本不知道库里输出的是哪一项。常用的工具链包括Python SymPy符号推导验证快速检查自己手推的 M、C、G 是否正确Python NumPy/SciPy正向动力学仿真用 solve 而不是显式求逆数值稳定性更好Pinocchio专业机器人动力学库支持 RNEA递归牛顿-欧拉法和 CRBA复合刚体算法算出来的动力学结果可直接和自写模型对比MATLAB / Simulink Robotics Toolbox适合快速原型验证界面方便但参数辨识功能还是得自己写我当时验证自写动力学模型的方式很简单固定一组 (q, q̇, q̈)分别用拉格朗日公式手算一个 τ再用 Pinocchio 算同一个 τ比对误差。如果误差在 1e-8 量级说明模型没问题控制律也可以放心地用。6.4 新手常见的三个困惑“为什么教材里的 C 矩阵写法都不一样”——这是新手最容易迷惑的地方。因为科氏项和离心项写成矩阵形式时不唯一C(q, q̇) 可以有很多种形式但 C(q, q̇)q̇ 这个向量是唯一的。所以关键的永远是你算出来的那个总力矩对不对而不是纠结 C 矩阵的某个元素。“关节角加速度 q̈ 怎么获得”——真实系统里你往往测不到加速度得通过差分或者状态观测器估计。而在仿真里q̈ 一般就是你解正向动力学方程直接得到的输出。这俩是完全不同的获取路径别搞混。“动力学模型是不是一定要非常精确才有用”——不是。哪怕你的模型只有七八成准前馈也能承担大部分工作剩下的误差由反馈修正。怕的不是模型不准而是根本没有模型全靠反馈硬扛那才是又累又难调。最后再分享一个小技巧在实际做 2R 机械臂的控制和仿真时有一个超容易被忽略的细节重力项到底取正号还是负号。很多初学的人照着公式抄下来发现机械臂在仿真里会往下坠落——这显然不对但也不是公式错而是坐标系方向定义和重力加速度方向没有统一。比如你把 q1 定义为和 x 轴正向的夹角而 y 轴向下那重力项里就不该有额外的负号抵消但如果 y 轴向上重力方向向下重力势能表达式里必须带着 -mgy 这种“负号”。我吃过这个亏整整排查了一天最后发现就是坐标系方向定义不统一导致的符号问题。所以凡是涉及到动力学公式第一件事永远是把你的坐标系定义、正方向约定白纸黑字写下来再写推导。不然后面所有环节都会被这个看似微小的问题反复折磨。如果你现在正卡在 2R 动力学推导这一步我真诚的建议是拿一张大纸从位置方程开始一项一项推不要跳步也不要用“显然”安慰自己。推完了再用 SymPy 验证一遍你会突然发现之前那些符号串串里面其实每一块都是物理。