小区花园里，爷爷推着孙女荡秋千。孙女说：“爷爷，推高一点！”爷爷便在她荡回来时轻轻一推——秋千越荡越高。此时，如果换成一位工程师路过，他脑海中会浮现两个完全相反的问题：问题一：如果我用这个力度推，秋千会荡到多高？问题二：如果我想让秋千在3秒后荡到那个高度，我现在该用多大的力？ 第一个问题，从“我做了什么”推演出“会发生什么”，顺着因果关系走——这就是正动力学。第二个问题，从“我想要什么结果”反过来求解“我该做什么”——这就是逆动力学。机器人不会自己荡秋千，但它做手术、搬箱子、跑跳翻跟头时，大脑里的控制器每时每刻都在解这两个问题：仿真器用正动力学预测运动，控制器用逆动力学计算发力。这篇文章，我们就从“推秋千”出发，把这两个力学的“双胞胎”掰开揉碎，看清它们的求解流程、数值积分、实时性差异和各自的应用主场。

岗位关联

正动力学与逆动力学是机器人仿真和控制的“任督二脉”——掌握二者的求解逻辑与选型思路，是从事机器人控制、仿真系统开发与AI具身智能落地的必备能力。

目录
  1. 可视化演示
  2. 正逆定义
  3. 求解流程
  4. 数值积分
  5. 实时对比
  6. 应用场景
  7. 选型指南
  8. AI 应用场景
  9. Python 演示
  10. 常见误区
  11. 面试题

可视化演示

动态演示深入理解正动力学/逆动力学的核心机制，通过全局/局部/动态/静态四种模式展示关键概念与交互流程。

本节配有交互动效（进入视口后自动加载，约 3KB）。

深入理解正动力学/逆动力学：正动力学 正动力学求解与加速度计算 逆动力学求解与力矩计算 核心机制可视化 深入理解正动力学/逆动力学整体架构与关键组件

正逆定义

还记得前面那位推秋千的爷爷吗？他发现孙女荡回来时轻轻一推，秋千就高了；而如果她在最高点伸手去够树叶，爷爷就得踩着节奏“预判”她的需要。这其实是两个方向截然相反的问题：正动力学问的是“我这么推，秋千会怎么荡”，逆动力学问的是“秋千要这样荡，我得怎么推”。机器人学中，正动力学（Forward Dynamics）给定关节驱动力矩 \(\boldsymbol{\tau}\)，求解关节的运动状态——加速度、速度与位置；逆动力学（Inverse Dynamics）则反过来，给定期望的关节运动轨迹，求解需要施加的力矩。

两兄弟并不是两套独立的物理，它们共用同一座桥——机器人运动方程：

把方程重新整理一下，正动力学就是解一个“线性系统”：

逆动力学则直接代入：

推秋千的爷爷最终明白了一件事：用力是手段，荡高是目的。正动力学关心“手段造成的结果”，逆动力学关心“结果反推的手段”。二者方向相反，但共用同一套物理规律——这就是它们的定义内核。

τ=M(q)q¨+C(q,q˙)q˙+G(q)\boldsymbol{\tau} = \mathbf{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \mathbf{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \mathbf{G}(\boldsymbol{q})
这座桥左边是“力矩”，右边是“运动”。正动力学是从右边翻到左边？不——更准确地说，正动力学是已知 \(\boldsymbol{\tau}\) 求 \(\ddot{\boldsymbol{q}}\)，逆动力学是已知 \(\ddot{\boldsymbol{q}}\) 求 \(\boldsymbol{\tau}\)。同一个方程，两个方向。
  1. \(\boldsymbol{\tau}\)（关节力矩向量）机器人每个关节上施加的驱动力矩。好比爷爷推秋千的推力，是一组数值。
    τ=[τ1,τ2,…,τn]⊤\boldsymbol{\tau} = [\tau_1, \tau_2, \ldots, \tau_n]^{\top}
  2. \(\mathbf{M}(\boldsymbol{q})\)（质量矩阵）与当前关节角度 \(\boldsymbol{q}\) 有关的惯性矩阵，决定“这个姿态下要花多大力气才能产生加速度”。
    M(q)∈Rn×n\mathbf{M}(\boldsymbol{q}) \in \mathbb{R}^{n\times n}
  3. \(\mathbf{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}}\)（科里奥利与离心项）关节运动时因耦合和旋转产生的“额外阻力”，好比秋千荡偏高时，爷爷必须顺着旋转感发力。
    C(q,q˙)q˙∈Rn\mathbf{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} \in \mathbb{R}^{n}
  4. \(\mathbf{G}(\boldsymbol{q})\)（重力项）支撑自身重量所需的力矩。秋千在最高点“停住”的瞬间，爷爷需要一个托举力。
    τ=M(q)q¨+C(q,q˙)q˙+G(q)\boldsymbol{\tau} = \mathbf{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \mathbf{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \mathbf{G}(\boldsymbol{q})

求解流程

爷爷推秋千有个固定套路：先看秋千现在在哪、荡得多快，再决定用什么力度推。机器人的求解流程也讲究“先看状态、再算结论”。

正动力学的求解流程像一场“接力赛”：第一步，已知当前角度 \(\boldsymbol{q}\)、角速度 \(\dot{\boldsymbol{q}}\) 和外加力矩 \(\boldsymbol{\tau}\)；第二步，由运动方程解出角加速度 \(\ddot{\boldsymbol{q}}\)；第三步，对角加速度做两次积分——先积分得速度，再积分得位置；第四步，进入下一个控制周期，重复以上循环。仿真器正是这样“一帧一帧”地把机器人的运动推演出来的。

逆动力学的求解流程则像“抄近路”：第一步，已知期望轨迹 \(\boldsymbol{q}_d(t),\dot{\boldsymbol{q}}_d(t),\ddot{\boldsymbol{q}}_d(t)\)；第二步，直接把轨迹代入运动方程右侧；第三步，解出所需的力矩 \(\boldsymbol{\tau}\)。整个过程不需要求逆、不需要积分，一条代数通道直达结果。

如果要求解正动力学中的加速度，核心是“矩阵左除”：

其中 \(\mathbf{b}\) 合并了科里奥利项与重力项。多数机器人工具箱并不直接求逆矩阵，而是用高斯消元、Cholesky 分解等数值线性代数方法解这个线性方程组，因为直接求逆在数值上既慢又不够稳定。

正动力学的每一步都需要计算质量矩阵与偏差项，再解一次线性方程组；逆动力学则省去了最昂贵的“求逆”环节。好比爷爷如果要知道“我推这一下，秋千最终能荡多高”，得等它在空中转好几圈才知道（正++）；但如果他只看“要使秋千以这个速度经过最低点，该用多大力”，心里一算就有数了（逆）。流程的差异，决定了它们的计算成本与适用场景分道扬镳。

q¨=M(q)−1(τ−b(q,q˙))\ddot{\boldsymbol{q}} = \mathbf{M}(\boldsymbol{q})^{-1}\big(\boldsymbol{\tau} - \mathbf{b}(\boldsymbol{q}, \dot{\boldsymbol{q}})\big)
  1. 先算加速度正动力学的“牛鼻子”就是加速度 \(\ddot{\boldsymbol{q}}\)，一旦有了它，速度和位置都能靠积分得到。
    q¨=M−1(q)(τ−b)\ddot{\boldsymbol{q}} = \mathbf{M}^{-1}(\boldsymbol{q})(\boldsymbol{\tau} - \mathbf{b})
  2. 积分一次得速度加速度是速度的变化率，做一次数值积分即可更新角速度。
    q˙k+1=q˙k+q¨k Δt\dot{\boldsymbol{q}}_{k+1} = \dot{\boldsymbol{q}}_k + \ddot{\boldsymbol{q}}_k\,\Delta t
  3. 再积分一次得位置有了角速度，再积分一次得到关节角度，完成从“力”到“运动”的接力。
    qk+1=qk+q˙k+1 Δt\boldsymbol{q}_{k+1} = \boldsymbol{q}_k + \dot{\boldsymbol{q}}_{k+1}\,\Delta t
  4. 逆动力学一步到位把期望的轨迹代入运动方程，直接得到力矩，不需要积分环节。
    τ=M(q)q¨+b(q,q˙)\boldsymbol{\tau} = \mathbf{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \mathbf{b}(\boldsymbol{q}, \dot{\boldsymbol{q}})

数值积分

爷爷推秋千时从不连续推，而是“一下、一下”地推。每一小步之间的空隙，就是他心里的“积分步长”。机器人仿真也一样：动力学方程是连续的微分方程，但计算机只能一小步一小步地算，这就是数值积分。

最原始的方法是欧拉法（Euler Method）：假设在一个很小的 \(\Delta t\) 内加速度不变，直接用当前导数外推下一步状态。它简单、快速，但每一步都会有截断误差，长时间仿真会“漂移”——就像爷爷每次都少推五厘米，久了秋千就荡不到想要的最高点了。

改进的做法是改进欧拉法（Heun's Method）：先预测一个中间状态，计算“半步”处的导数，再用这个平均斜率更新。它多花了一次导数计算，却把精度大幅提升。

更常用的是四阶龙格库塔法（RK4）：它在一个步长内做四次“试探性评估”，再加权平均。RK4 的精度和稳定性远优于欧拉法，是机器人仿真中最常见的积分选择，代价是每步计算量约为欧拉法的四倍。

欧拉法的数学形式是：

其中 \(\boldsymbol{x} = [\boldsymbol{q}; \dot{\boldsymbol{q}}]\) 是状态向量，\(h\) 是步长，\(\boldsymbol{f}\) 是动力学函数。RK4 的经典形式为：

如果把整条秋千轨迹比作一幅画，欧拉法是用直线段粗糙地连出轮廓，RK4 则是每段都取“四点半的折线”让曲线更圆滑。数值积分方法的选择，本质上是在精度与计算成本之间走钢丝——而这根钢丝的另一端，正是实时性的要求。

xk+1=xk+h f(xk,tk)\boldsymbol{x}_{k+1} = \boldsymbol{x}_k + h\,\boldsymbol{f}(\boldsymbol{x}_k, t_k)
k1=f(xk,tk),k2=f(xk+h2k1, tk+h2)\boldsymbol{k}_1 = \boldsymbol{f}(\boldsymbol{x}_k, t_k),\quad\boldsymbol{k}_2 = \boldsymbol{f}\big(\boldsymbol{x}_k + \tfrac{h}{2}\boldsymbol{k}_1,\ t_k+\tfrac{h}{2}\big)
k3=f(xk+h2k2, tk+h2),k4=f(xk+hk3, tk+h)\boldsymbol{k}_3 = \boldsymbol{f}\big(\boldsymbol{x}_k + \tfrac{h}{2}\boldsymbol{k}_2,\ t_k+\tfrac{h}{2}\big),\quad\boldsymbol{k}_4 = \boldsymbol{f}\big(\boldsymbol{x}_k + h\boldsymbol{k}_3,\ t_k+h\big)
xk+1=xk+h6(k1+2k2+2k3+k4)\boldsymbol{x}_{k+1} = \boldsymbol{x}_k + \frac{h}{6}(\boldsymbol{k}_1 + 2\boldsymbol{k}_2 + 2\boldsymbol{k}_3 + \boldsymbol{k}_4)
  1. \(\boldsymbol{x}_k\)（当前状态）包含所有关节角度和角速度的向量，是积分的“起点”。
    xk=[q1,…,qn,q˙1,…,q˙n]⊤\boldsymbol{x}_k = [q_1,\dots,q_n,\dot{q}_1,\dots,\dot{q}_n]^{\top}
  2. \(h\)（积分步长）两次计算之间的时间间隔，步长越小越精确，但计算量越大。
    h=tk+1−tkh = t_{k+1} - t_k
  3. \(\boldsymbol{k}_1\) 到 \(\boldsymbol{k}_4\)（四个斜率估计）RK4 在步长内取三个中间点作“试算”，再用加权平均近似整个步长的变化斜率。
    k2=f(xk+h2k1, tk+h2)\boldsymbol{k}_2 = \boldsymbol{f}\big(\boldsymbol{x}_k + \tfrac{h}{2}\boldsymbol{k}_1,\ t_k + \tfrac{h}{2}\big)
  4. 合并更新四个斜率按 \(1:2:2:1\) 加权，得到更接近真实积分的下一步状态。
    xk+1=xk+h6(k1+2k2+2k3+k4)\boldsymbol{x}_{k+1} = \boldsymbol{x}_k + \frac{h}{6}(\boldsymbol{k}_1 + 2\boldsymbol{k}_2 + 2\boldsymbol{k}_3 + \boldsymbol{k}_4)

实时对比

爷爷推秋千必须“卡点”——秋千荡到跟前时推，晚了或早了都不行。机器人的控制循环同样有“卡点”：控制周期通常是 1 毫秒或 2 毫秒，所有计算必须在这个时间内完成。在这个硬约束下，正动力学与逆动力学的实时性呈现出鲜明的对比。

正动力学的核心运算包含质量矩阵的组装与一次线性方程组的求解（等价于矩阵求逆），在关节数 \(n\) 较大时，计算复杂度约为 \(O(n^3)\)。相比之下，逆动力学使用递推牛顿-欧拉算法（Recursive Newton-Euler Algorithm），计算复杂度只有 \(O(n)\)，随关节数的增长呈线性，轻量得多。

一个典型的 6 轴工业机械臂，逆动力学正解一次约需几十微秒；而正动力学求一次加速度，若用质量矩阵求逆，通常需要数百微秒到毫秒级。对 1kHz 控制频率的实时系统来说，逆动力学绰绰有余，正动力学则捉襟见肘。

从运动方程角度看二者的复杂度差异：

正动力学像“把秋千推上天后还要全程录像复盘”，逆动力学像“眼睛盯着目标，手随心动”。因此，在实际工程中，控制回路里几乎总是用逆动力学，正动力学则被安排在算力充裕的仿真与离线分析环节。理解这层“实时性温差”，是选型的关键分水岭。

q¨=M(q)−1 h⏟正动力学：需求逆,τ=M(q)q¨+b⏟逆动力学：只需乘加\underbrace{\ddot{\boldsymbol{q}} = \mathbf{M}(\boldsymbol{q})^{-1}\,\mathbf{h}}_{\text{正动力学：需求逆}},\qquad\underbrace{\boldsymbol{\tau} = \mathbf{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \mathbf{b}}_{\text{逆动力学：只需乘加}}
  1. \(\mathbf{M}^{-1}\) 的代价正动力学需要求解线性方程组，这一步的运算量随关节数快速增长。
    M−1h∼O(n3)\mathbf{M}^{-1}\mathbf{h} \sim O(n^3)
  2. \(\mathbf{M}\ddot{\boldsymbol{q}} + \mathbf{b}\) 的代价逆动力学只做矩阵乘向量与向量加法，线性复杂度。
    Mq¨+b∼O(n2)∼O(n) (递推算法)\mathbf{M}\ddot{\boldsymbol{q}} + \mathbf{b} \sim O(n^2) \sim O(n) \ (\text{递推算法})
  3. 控制周期 \(T_s\)实时控制器必须保证在固定周期内算完一步，逆动力学更容易满足这个硬截止时间。
    Ts≥tcompute,tinv≪tfwdT_s \geq t_{\text{compute}},\qquad t_{\text{inv}} \ll t_{\text{fwd}}

应用场景

明白了实时性差异，再看应用场景就清晰多了。如果把机器人公司比作一个大型游乐场，那么正动力学负责“排演”，逆动力学负责“实演”。

场景一：机器人仿真与数字孪生。 在仿真器中，我们希望预测“给定控制信号，机器人会发生什么运动”。这正是正动力学的主场——通过数值积分，生成海量的运动数据，用于训练强化学习策略（例如 NVIDIA Isaac Gym 中训练四足机器人）。仿真器不需要严格实时，可以在虚拟时间里“加速跑”，所以正动力学的计算成本可以被接受。

场景二：工业机械臂的力控打磨与装配。 机械臂要沿着工件表面法线方向施加恒定压力，又要沿切线方向运动。控制器需要根据期望的轨迹实时解算每个关节的力矩，偏了哪怕一毫秒，零件就可能被刮伤。逆动力学在这里“手起刀落”，以微秒级延迟输出力矩指令，保证柔顺与安全。

场景三：仿人机器人的全身平衡控制。 双足机器人在站立时，需要把质心加速度与地面反作用力映射到全身关节力矩，这是一个典型的逆动力学问题——“全身力矩分配”（Whole-Body Control）。每一步都要求实时求解，逆动力学的 \(O(n)\) 复杂度让其成为可行方案。

如果仿真想要“无重力效果”，可以人为地把重力项置零：

正动力学像是“彩排导演”——预先模拟所有可能，让演员不会在舞台上出丑；逆动力学则是“现场指挥”——每一拍都精确发力，分秒不差。仿真求“真”用正，控制求“快”用逆，二者的舞台虽然不同，但都是机器人从算法走向物理世界的必经之路。

τctrl=M(q)q¨+C(q,q˙)q˙+G(q)−Gcomp(q)\boldsymbol{\tau}_{\text{ctrl}} = \mathbf{M}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \mathbf{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}} + \mathbf{G}(\boldsymbol{q}) - \mathbf{G}_{\text{comp}}(\boldsymbol{q})
  1. 仿真中的补偿在导纳控制或零重力演示中，控制器反解出重力补偿力矩，抵消真实重力影响。
    τcomp=G(q)\boldsymbol{\tau}_{\text{comp}} = \mathbf{G}(\boldsymbol{q})
  2. 力控中的前馈逆动力学力矩直接作为前馈，叠加到反馈控制器上，提高轨迹跟踪精度。
    τ=τID(qd,q˙d,q¨d)+Kpe+Kde˙\boldsymbol{\tau} = \boldsymbol{\tau}_{\text{ID}}(\boldsymbol{q}_d,\dot{\boldsymbol{q}}_d,\ddot{\boldsymbol{q}}_d) + \mathbf{K}_p\boldsymbol{e} + \mathbf{K}_d\dot{\boldsymbol{e}}

选型指南

爷爷推秋千多年，总结出一套经验：孙女性格文静就轻轻推，性格奔放就使劲推；风大的时候少推一点，风停了多推一点。选正动力学还是逆动力学，也有一套“经验法则”。

按任务类型选：如果任务是“给定动作，看会发生什么”——仿真预测、数据集生成、事故复盘，选正动力学。如果任务是“给定目标，算出动作”——轨迹跟踪、力控、平衡控制，选逆动力学。

按实时性选：控制周期在 1kHz 以上、算力有限的嵌入式控制器，选逆动力学。在服务器或云端、允许几毫秒级延迟的仿真训练任务，可以考虑正动力学。

按精度需求选：需要精确模拟物理过程、评估奇异构型或碰撞后响应，选正动力学；需要稳定平滑的力矩输出、不追求长时程预测精度，选逆动力学。

混合方案：工程中最常见的做法是“逆动力学在线控制 + 正动力学离线验证”。控制端用逆动力学做实时力矩计算，开发端用正动力学做大量仿真测试，两者互补。如果被控对象模型不准确（比如负载变化大），还可以在逆动力学基础上叠加学习项：

选型没有绝对的对错。爷爷推了半辈子秋千，最终明白：好的推手既懂“顺势而为”的正向预测，也懂“以终为始”的反向求解。工程师也一样——在正确的场景选择正确的动力学方向，比盲目追求“更高级”的算法更重要。正动力学与逆动力学从来不是对手，而是机器人工程中一枚硬币的两面。

τ=τID(q,q˙,q¨)+f^learn(q,q˙)\boldsymbol{\tau} = \boldsymbol{\tau}_{\text{ID}}(\boldsymbol{q},\dot{\boldsymbol{q}},\ddot{\boldsymbol{q}}) + \hat{\boldsymbol{f}}_{\text{learn}}(\boldsymbol{q},\dot{\boldsymbol{q}})
  1. 基础逆动力学项模型已知部分的力矩由逆动力学精确算出。
    τID=Mq¨+Cq˙+G\boldsymbol{\tau}_{\text{ID}} = \mathbf{M}\ddot{\boldsymbol{q}} + \mathbf{C}\dot{\boldsymbol{q}} + \mathbf{G}
  2. 学习补偿项模型误差与未知外力由数据驱动模型在线补偿，弥补纯动力学方法的不足。
    f^learn≈Δτunknown\hat{\boldsymbol{f}}_{\text{learn}} \approx \Delta\boldsymbol{\tau}_{\text{unknown}}

AI 应用场景

机器人仿真与强化学习数据生成

阶段：数据准备与模型训练

解决的问题：真实机器人试错成本高、安全风险大。传统方式需要大量物理样机测试，时间长且难以覆盖极端工况。以正动力学为核心的仿真引擎可以在虚拟环境中批量生成“状态-动作-下一状态”转移数据，供强化学习策略快速试错迭代。

具体案例：某四足机器人团队使用 MuJoCo（基于正动力学）训练运动策略，在 10 万条仿真轨迹上完成训练，再将策略迁移到真实机器人。由于正动力学准确模拟了腿部关节的惯性耦合与地面接触力，策略迁移后仅需少量真机微调即可稳定行走。

工业机械臂实时力控打磨

阶段：模型推理与控制执行

解决的问题：传统位置控制模式在打磨、抛光等接触任务中刚性过大，容易损坏工件。传统阻抗控制只做简单低通滤波，难以应付复杂曲面。基于逆动力学的计算力矩控制，可根据期望接触力和轨迹实时解算关节力矩，实现柔顺打磨。

具体案例：某汽车零部件产线上的六轴机械臂，使用逆动力学前馈 + 力传感器反馈的混合控制，在铝合金轮毂曲面打磨中，将表面粗糙度波动从 ±0.8μm 降低至 ±0.2μm，同时避免了对薄壁件的过压损伤。

仿人机器人全身平衡与跌倒恢复

阶段：在线决策与控制

解决的问题：双足机器人要在推搡、斜坡、负重变化中保持平衡，传统 PID 只能逐关节独立控制，无法协调全身。基于逆动力学的全身控制（Whole-Body Control）将质心加速度期望映射到全身关节力矩，天然处理了多关节耦合与接触力约束。

具体案例：某实验室双足机器人在受到 30N 侧向冲击时，通过逆动力学实时重算髋、膝、踝的力矩分配，在 0.3 秒内恢复稳定站立，而传统方法会直接摔倒。

Python 演示

python68 行
import math class TwoLinkRobot:    def __init__(self):        self._m1 = 4.0      # link1 mass        self._m2 = 2.5      # link2 mass        self._l1 = 0.6      # link1 length        self._l2 = 0.5      # link2 length        self._g = 9.81     def mass_matrix(self, q1, q2):        m1, m2 = self._m1, self._m2        l1, l2 = self._l1, self._l2        c2 = math.cos(q2)        I1 = m1 * l1 * l1 / 3.0        I2 = m2 * l2 * l2 / 3.0        m11 = I1 + I2 + m2 * l1 * l1 + 2 * m2 * l1 * l2 * c2        m12 = I2 + m2 * l1 * l2 * c2        m22 = I2        return [[m11, m12], [m12, m22]]     def bias(self, q1, q2, v1, v2):        m2, l1, l2, g = self._m2, self._l1, self._l2, self._g        s2 = math.sin(q2)        c1, c12 = math.cos(q1), math.cos(q1 + q2)        h1 = -m2 * l1 * l2 * (2 * v1 * v2 + v2 * v2) * s2        h2 = m2 * l1 * l2 * v1 * v1 * s2        G1 = (self._m1 * l1 / 2 + m2 * l1) * g * c1 + m2 * l2 * g * c12        G2 = m2 * l2 * g * c12        return [h1 + G1, h2 + G2]     def forward(self, tau1, tau2, q1, q2, v1, v2, dt=0.002):        M = self.mass_matrix(q1, q2)        b = self.bias(q1, q2, v1, v2)        det = M[0][0] * M[1][1] - M[0][1] * M[1][0]        if abs(det) < 1e-9:            det = 1e-9        a1 = ((tau1 - b[0]) * M[1][1] - (tau2 - b[1]) * M[0][1]) / det        a2 = (-(tau1 - b[0]) * M[0][1] + (tau2 - b[1]) * M[0][0]) / det        v1 += a1 * dt        v2 += a2 * dt        q1 += v1 * dt        q2 += v2 * dt        return q1, q2, v1, v2     def inverse(self, q1, q2, v1, v2, a1, a2):        M = self.mass_matrix(q1, q2)        b = self.bias(q1, q2, v1, v2)        tau1 = M[0][0] * a1 + M[0][1] * a2 + b[0]        tau2 = M[0][1] * a1 + M[1][1] * a2 + b[1]        return tau1, tau2     def simulate_swing(self, steps=1000, dt=0.002):        q1, q2, v1, v2 = 0.0, 0.0, 0.0, 0.0        trail = []        for k in range(steps):            tau1 = 2.0 * math.sin(k * dt * 2.0)            tau2 = 1.0 * math.cos(k * dt * 2.0)            q1, q2, v1, v2 = self.forward(tau1, tau2, q1, q2, v1, v2, dt)            trail.append((k * dt, q1, q2))        return trail if __name__ == "__main__":    robot = TwoLinkRobot()    tri = robot.inverse(0.3, 0.5, 0.2, -0.1, 1.0, 0.5)    print("inverse torque:", round(tri[0], 4), round(tri[1], 4))    traj = robot.simulate_swing()    print("fwd end q1:", round(traj[-1][1], 4), "q2:", round(traj[-1][2], 4))
目的：演示 2 连杆机器人的正动力学（输入力矩，数值积分得到运动轨迹）与逆动力学（输入期望加速度，解算关节力矩）的完整求解流程。
  1. mass_matrix(q1, q2)根据关节角度计算质量矩阵，这是正逆动力学共用的“桥梁”。
  2. bias(q1, q2, v1, v2)计算科里奥利项与重力项的合称偏差项，是动力学方程中的“阻力来源”。
  3. v1 += a1 * dt; q1 += v1 * dt半隐式欧拉积分，先用加速度更新速度，再用新速度更新位置。
  4. inverse(...)逆动力学只做矩阵乘法和加法，一步到位得到力矩，没有积分环节。

常见误区

误区我一直以为正动力学就是“正常的动力学”，所以机器人走路时算力矩用的就是正动力学；逆动力学是反过来求位置，主要用于仿真。

正解恰好相反。正动力学是“给力矩，算运动”，它是仿真的核心；逆动力学是“给期望运动，算力矩”，它是控制的核心。可以记成：正 = 顺势（原因 → 结果），逆 = 逆推（结果 → 原因）。推秋千时，爷爷“推 → 荡”是正，“想荡那么高 → 该推多大力”是逆。

为何常见：“正”字容易让人联想到“正常、主要、正确”，而实际中控制器比仿真器更常见，导致许多人把逆动力学误当成“正”。

误区大家都说控制里不用正动力学，是因为它慢。所以正动力学在任何情况下都不如逆动力学实用。

正解严格说，正动力学的瓶颈在于“求解线性方程组”（矩阵求逆约 \(O(n^3)\)），但这不等于它绝对不可用。在关节数少（如 2-3 轴）、算力充足或离线仿真场景下，正动力学完全可以实时或准实时运行。此外，现代算法如铰接体算法（Articulated Body Algorithm，ABA）也能把正动力学降到 \(O(n)\) 复杂度，只是常数较大。

为何常见：很多人把“6 轴以上机械臂的 1kHz 控制”这一特定场景的经验，推广到所有场景，忽略了关节数量和算力差异。

误区正运动学是不是就是正动力学的简化版？反正都是已知角度求位置，那我用运动学没法算力矩，是不是就用动力学？

正解运动学只研究几何关系——角度、角速度、位置、速度；动力学研究力与运动的关系。正运动学是“角度 → 末端位置”，正动力学是“力矩 → 加速度”。两者维度完全不同。就像看秋千：运动学描述“秋千在哪个位置”，动力学解释“为什么它会到那个位置”。

为何常见：二者都有“正/逆”的提法，名称相似，初学者极易混淆，尤其当教程只强调公式而忽视物理含义时。

误区我把期望轨迹代入逆动力学，算出来的力矩直接发给电机，机器人不就能完美走轨迹了？

正解逆动力学是基于模型的“前馈”计算，它依赖精确的质量、惯量参数和外力模型。实际中存在摩擦、关节柔性、负载变化等未建模项，直接开环使用会积累误差。工程上通常采用“逆动力学前馈 + PD 反馈”的计算力矩控制结构：前馈负责大头，反馈消除扰动。

为何常见：教科书中逆动力学的公式看起来很完美，让人误以为模型是万能的，忽略了真实物理世界的噪声与不确定性。

误区只要我的电脑算力够强，正动力学也能跑 1kHz，那就不用在意正逆区别了。

正解实时性的关键是“确定性的截止时间”（deadline），而不是“平均算得快”。实时控制要求每次计算最坏情况耗时也不能超过控制周期，否则会出现抖动甚至失稳。正动力学的最坏情况耗时通常远大于逆动力学，且受矩阵条件数影响波动更大，因此即使在强力硬件上，系统设计者也会优先选择计算边界清晰的逆动力学。

为何常见：通用计算中人们习惯了“平均性能”，而实时系统关心的是“最坏情况性能”，这是两种完全不同的思维方式。

面试题

Q1. 正动力学解决的问题是？

单选题 · easy

  • A.已知关节力矩求关节运动← 正确答案
  • B.已知关节运动求关节力矩
  • C.已知末端位置求关节角度
  • D.已知末端速度求关节角速度

答案：A。正动力学是“给力矩、算运动”，即从输入力矩求解加速度、速度和位置，是仿真器的基础。B 是逆动力学，C 是逆运动学，D 是运动学问题，均与“力与运动关系”不同。

Q2. 逆动力学的典型输出是什么？

单选题 · easy

  • A.关节角加速度
  • B.关节驱动力矩← 正确答案
  • C.末端笛卡尔速度
  • D.质量矩阵

答案：B。逆动力学输入期望轨迹，输出所需的关节力矩，用于控制器生成指令。加速度是正动力学求解的中间量，速度是运动学内容，质量矩阵是方程的组成部分而非输出。

Q3. 与逆动力学相比，正动力学的主要计算瓶颈是？

单选题 · mid

  • A.计算科里奥利力
  • B.求解线性方程组（等效矩阵求逆）← 正确答案
  • C.读取传感器数据
  • D.处理重力项

答案：B。正动力学需要从运动方程中解出加速度，即求解 \(\mathbf{M}\ddot{\boldsymbol{q}} = \boldsymbol{\tau}-\mathbf{b}\)，核心是解线性方程组，复杂度约 \(O(n^3)\)。逆动力学的递推牛顿-欧拉算法只需 \(O(n)\)，因此实时性更好。

Q4. 采用 RK4 进行数值积分的主要优点是？

单选题 · mid

  • A.计算量最小
  • B.在相同步长下精度和稳定性更高← 正确答案
  • C.不需要动力学模型
  • D.可以直接求解解析解

答案：B。RK4 在一步内做 4 次导数估计并加权平均，截断误差远小于欧拉法，在相同步长下精度更高、长期仿真更稳定。欧拉法计算量更小但误差更大；动力学模型是积分的前提，解析解则不是数值方法能直接给出的。

Q5. 下列哪些场景更适合使用逆动力学？

多选题 · mid

  • A.实时力控打磨← 正确答案
  • B.双足机器人平衡控制← 正确答案
  • C.离线的强化学习仿真数据生成
  • D.数字孪生长时程预测

答案：AB。实时力控和平衡控制都要求毫秒级力矩解算，逆动力学的线性复杂度适合硬实时。离线仿真数据生成和长时程预测是正动力学的主场，因为仿真器需要准确预测“输入力 → 运动”的演化，且不介意较高计算成本。

Q6. 关于正动力学和逆动力学的计算复杂度，正确的说法是？

单选题 · hard

  • A.二者复杂度完全相同
  • B.正动力学的复杂度通常高于逆动力学← 正确答案
  • C.逆动力学复杂度总是高于正动力学
  • D.复杂度与关节数无关

答案：B。常规实现中，正动力学因需求解线性方程组，复杂度约 \(O(n^3)\)；逆动力学的递推牛顿-欧拉算法约 \(O(n)\)。因此正动力学通常更重。复杂度显然与关节数 n 强相关。

Q7. 在机器人运动方程 \(\boldsymbol{\tau} = \mathbf{M}\ddot{\boldsymbol{q}} + \mathbf{C}\dot{\boldsymbol{q}} + \mathbf{G}\) 中，\(\mathbf{G}(\boldsymbol{q})\) 表示？

单选题 · easy

  • A.科里奥利力
  • B.离心力
  • C.重力项← 正确答案
  • D.摩擦力

答案：C。\(\mathbf{G}\) 是重力项，表示各关节为支撑自身重量所需克服的力矩；\(\mathbf{C}\dot{\boldsymbol{q}}\) 是科里奥利与离心项。摩擦力通常作为扩展项单独建模，不包含在这个基本方程中。

Q8. 实时控制系统中，正动力学较少用于在线计算的深层次原因是？

多选题 · hard

  • A.矩阵求逆的最坏情况耗时波动大← 正确答案
  • B.正动力学精度一定低于逆动力学
  • C.控制周期硬截止时间难以保证← 正确答案
  • D.正动力学无法用于仿真

答案：AC。实时系统关心最坏情况的确定性，正动力学因矩阵条件数变化导致求解耗时波动大，且 \(O(n^3)\) 复杂度难以在 1kHz 周期内稳定完成，因此难用于在线控制。正动力学在离线仿真中精度可做得很高，故 B、D 错误。

Q9. “正动力学前馈 + PD 反馈”这种控制结构解决的核心问题是？

单选题 · mid

  • A.消除逆动力学的计算误差
  • B.补偿模型误差和外部扰动← 正确答案
  • C.把逆动力学变成正动力学
  • D.减少传感器噪声

答案：B。逆动力学前馈基于模型，但摩擦、负载变化等未建模项会造成跟踪误差；PD 反馈根据当前位置误差和速度误差实时修正，二者叠加可以在保证快速响应的同时抑制模型不确定性。前馈与反馈无法互相转换或替代。

Q10. 关于混合动力学方案，下列哪些说法正确？

多选题 · extreme

  • A.在线逆动力学控制，离线正动力学验证← 正确答案
  • B.数据驱动补偿项可以弥补逆动力学的模型误差← 正确答案
  • C.正动力学可以完全替代逆动力学
  • D.仿真与真实机器人的差距可以用学习项缩小← 正确答案

答案：ABD。工程中常用“逆控制+正验证”的组合；学习项在逆动力学的模型项误差较大时能提供补偿，也能缩小仿真到真实的 Sim-to-Real gap。正动力学是仿真工具，无法在实时控制中完全替代轻量的逆动力学，故 C 错误。