运动学与动力学基础

核心运动与控制 kp-201 运动学动力学雅可比逆运动学

一句话定义

运动学研究"关节角 ↔ 末端位姿"的几何映射，动力学研究"力矩 ↔ 运动"的因果映射——两者是机械臂一切控制与规划的词汇表。

为什么重要

逆运动学（IK）是"让手到该去的地方"的数学翻译器；抓取检测输出的末端位姿（kp-102）必须经 IK 变成关节目标。
雅可比矩阵把"关节速度→末端速度"线性化，是速度控制、力估计、奇异性分析、冗余度优化的枢纽。
动力学方程是 MPC、计算力矩控制、仿真引擎（kp-401）的共同语言。

前置知识

线性代数（旋转矩阵、齐次变换）、微积分。

核心概念

正运动学（FK）：已知关节角 $\mathbf{q}$ ，求末端位姿 $\mathbf{T}(\mathbf{q}) \in SE(3)$ 。机械链逐节变换相乘。建模标准：DH 参数（Denavit-Hartenberg）或现代的旋量/指数积（PoE）公式。

逆运动学（IK）：已知末端位姿，求关节角。两条路线：

解析解：构型规则（如球面腕的 6 轴臂）可拆位置解与姿态解，快且唯一分支少。
数值解：迭代法，通用任意构型。雅可比伪逆法、阻尼最小二乘（DLS/Levenberg-Marquardt）、以及现代求解器（IKFast 预编译、Pinocchio/Drake 优化式）。

雅可比矩阵 $\mathbf{J}(\mathbf{q}) \in \mathbb{R}^{6\times n}$ ：

\dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\, \dot{\mathbf{q}}

速度映射；转置给出静力映射 $\boldsymbol{\tau} = \mathbf{J}^\top \mathbf{F}$ 。
$\det(\mathbf{J})$ 或最小奇异值趋零 → 奇异位形：某些方向失去运动能力，速度需求爆炸——规划必须绕开。
冗余臂（ $$n>6$$ ）用 $\mathbf{J}^\dagger$ （伪逆）+ 零空间投影做次级任务（避障、避奇异、保持姿态）。

动力学：拉格朗日方程给出的标准形式：

\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} + \mathbf{J}^\top \mathbf{F}_{ext}

$\mathbf{M}$ 惯性矩阵、 $\mathbf{C}$ 科氏/离心项、 $\mathbf{g}$ 重力项、 $\mathbf{F}_{ext}$ 外力。工程上一般不手推，由软件按 URDF 自动生成数值（MuJoCo、Pinocchio、Drake、KDL）。

原理与机制

IK 为什么经常多解或无解：FK 是多到一的映射（不同关节角可到同一位姿），其反演自然多解（肘上/肘下）；目标超出可达空间或落在奇异面另一侧时无解。数值 IK 的本质是把"位姿误差"定义在旋转流形上（指数坐标）并沿其梯度下降——理解这一点，所有 IK 求解器的报错都能对号入座。

重力补偿是每天都在跑的动力学：机械臂"静止悬停"需要持续输出 $\mathbf{g}(\mathbf{q})$ 的力矩。模型不准 → 位置漂移或电机发热；这就是 URDF 惯性参数需要系统辨识的原因。

奇异位的工程处理：规划层避免穿越（限制关节角域、加奇异鲁棒指标），控制层用阻尼最小二乘让穿越时速度需求有界而非发散。

公式与图示

     任务空间（末端 x ∈ R⁶）  ◄──FK──  关节空间（q ∈ Rⁿ）
            │                          ▲
            │──IK（可能有 0/1/多 解）───┘
            ▼
   抓取位姿 ──IK──► q* ──轨迹插值──► 各关节伺服跟踪

阻尼最小二乘 IK 单步迭代：

\Delta\mathbf{q} = \mathbf{J}^\top(\mathbf{J}\mathbf{J}^\top + \lambda^2 \mathbf{I})^{-1}\,\mathbf{e}

$\mathbf{e}$ 为当前位姿误差（用对数映射 $e = \log(T_{goal}T^{-1})$ 表示）， $\lambda$ 阻尼防奇异。

直观类比

肩肘腕链条：你的手臂是 7 自由度冗余臂。让手指尖碰鼻尖（任务），肩肘腕有无数种组合（冗余多解）——大脑默默选了最省力的那个，这就是零空间优化。
推门的手感：推门边缘越远越省力—— $\boldsymbol{\tau} = \mathbf{J}^\top \mathbf{F}$ 的生物版本；在门轴附近推（接近奇异），你会感觉怎么都使不上劲。

实例与案例

UR5 面向 6 轴、球面腕：可推导闭式 IK；ROS/MoveIt 里调 KDL/TRAC-IK 数值解同样可用。
Python 快速上手：roboticstoolbox-python（Peter Corke）几行代码完成 FK/IK/雅可比/工作空间可视化；MuJoCo 的 mj_jac 系列直接给雅可比。
Franka Panda 力控：7 轴臂 + 关节力矩传感， $\boldsymbol{\tau} = \mathbf{J}^\top\mathbf{F}$ 是其力控接口的底层（kp-203）。

常见误区

误区一：IK 无解=错误。目标在工作空间外（[kp-201] 可达空间 vs 灵巧空间的区别）或被奇异区隔断，IK 求解器会发散——先检查可达性。
误区二：把动力学当纯理论。重力补偿 $\mathbf{g}(\mathbf{q})$ 是所有臂控制器每天在跑的量；不懂它就看不懂为什么机械臂"上电后保持不动也要耗电甚至不需耗电"。
误区三：URDF 参数随手填。惯性参数（质量、惯量张量）填错，动力学仿真与基于模型的控制全部失真——真实系统的 URDF 需要系统辨识校准。

自测题

推导：为什么 $\boldsymbol{\tau} = \mathbf{J}^\top \mathbf{F}$ ？从虚功原理出发。
什么是奇异位形？举一个 2 连杆平面臂 $\theta_2 = 0$ （伸直）的例子。
冗余臂的零空间投影如何实现"动臂不动手"？

与其他知识点的关系

kp-202 轨迹规划：IK 提供路径终点，规划补上路径本身。
kp-203 控制：动力学方程是计算力矩与阻抗控制的骨架。
kp-501 硬件：减速比、柔性影响动力学模型保真度。

本节引用来源

Lynch & Park, Modern Robotics, 2017
Craig, Introduction to Robotics, 2005