📘 机器人学笔记：正运动学 (FK) 与 MDH 建模

---

1. 核心概念：正运动学 (Forward Kinematics)

1.1 定义

正运动学回答的是**“已知关节角，求末端位姿”**的问题。

• 输入 (关节空间)：$\Theta =[{\theta }_1,{\theta }_2,\dots ,{\theta }_n{]}^T$
• 输出 (笛卡尔空间)：末端执行器相对于基座的位置 $(P_x,P_y,P_z)$ 和姿态 $(R)$。

1.2 核心思想

通过在每一个连杆上建立一个坐标系，利用齐次变换矩阵将它们串联起来：

$${}_n^{0}T={}_1^{0}T({\theta }_1)\cdot {}_2^{1}T({\theta }_2)\cdot \cdots \cdot {}_n^{n-1}T({\theta }_n)$$

---

2. 建模标准：MDH (Modified DH) 参数法

工业界常用 Craig 提出的 MDH 方法。其核心特征是将坐标系 {i}\{i\}{i} 固连在连杆 $i$ 的首端（即关节 $i$ 处）。

2.1 四个“魔法参数”

描述从坐标系 {i−1}\{i-1\}{i−1} 到 {i}\{i\}{i} 的变换，只需 4 个参数：

参数	符号	定义 (参照基准)	记忆口诀
连杆扭转角	${\alpha }_{i-1}$	轴 $Z_{i-1}$ 到 $Z_i$ 的夹角 (绕 $X_{i-1}$)	上根轴歪没歪？
连杆长度	$a_{i-1}$	轴 $Z_{i-1}$ 到 $Z_i$ 的距离 (沿 $X_{i-1}$)	上根杆有多长？
关节角	${\theta }_i$	轴 $X_{i-1}$ 到 $X_i$ 的夹角 (绕 $Z_i$)	电机转了多少？
连杆偏距	$d_i$	轴 $X_{i-1}$ 到 $X_i$ 的距离 (沿 $Z_i$)	关节有没有凸出？

关键点：MDH 的参数下标 $i-1$ 表示它是上一根连杆的固有属性，而 $i$ 表示它是当前关节的变量或属性。

---

3. 通用齐次变换矩阵 ${}_i^{i-1}T$

根据 MDH 定义，从坐标系 {i}\{i\}{i} 到 {i−1}\{i-1\}{i−1} 的变换矩阵公式如下（严格对应您提供的图片公式）：

$${}_i^{i-1}T=\left[\begin{array}{cccc}c{\theta }_i& -s{\theta }_i& 0& a_{i-1}\\ s{\theta }_{i}c{\alpha }_{i-1}& c{\theta }_{i}c{\alpha }_{i-1}& -s{\alpha }_{i-1}& -s{\alpha }_{i-1}{d}_i\\ s{\theta }_{i}s{\alpha }_{i-1}& c{\theta }_{i}s{\alpha }_{i-1}& c{\alpha }_{i-1}& c{\alpha }_{i-1}{d}_i\\ 0& 0& 0& 1\end{array}\right]$$

注：$c{\theta }_i$ 代表 $\cos ({\theta }_i)$， $s{\theta }_i$ 代表 $\sin ({\theta }_i)$，以此类推。

---

4. 📝 3自由度平面机械臂实例

我们以经典的平面三轴机械臂为例，一步步演示如何从零开始算到末端位置。

第一步：建立坐标系 (Frame Assignment)

1. Z 轴：所有关节都是旋转关节，且在同一平面运动 $\to$ 所有 $Z$ 轴 ($Z_1,Z_2,Z_3$) 垂直纸面向外，相互平行。
2. X 轴：
   • $X_0$：任意设定（通常水平向右）。
   • $X_1$：沿着 Link 1 指向下一个关节。
   • $X_2$：沿着 Link 2 指向下一个关节。
   • $X_3$：沿着 Link 3 指向末端。
3. 原点：坐标系 {i}\{i\}{i} 的原点建在关节 $i$ 的中心。

第二步：提取 MDH 参数表

我们需要逐行填写表格。

变换层级 (${}_i^{i-1}T$)	${\alpha }_{i-1}$	$a_{i-1}$	$d_i$	${\theta }_i$	解析 (为什么这样填？)
${}_1^{0}T$ (基座$\to$J1)	$0$	$0$	$0$	${\theta }_1$	原点重合($a_0=0$)，Z轴平行(${\alpha }_0=0$)
${}_2^{1}T$ (J1$\to$J2)	$0$	$L_1$	$0$	${\theta }_2$	沿 $X_1$ 走了 $L_1$ 到 J2，Z轴平行
${}_3^{2}T$ (J2$\to$J3)	$0$	$L_2$	$0$	${\theta }_3$	沿 $X_2$ 走了 $L_2$ 到 J3，Z轴平行

注意：图中的末端点 $P$ 并不是关节，它是在坐标系 {3}\{3\}{3} 中的一个固定点，距离为 $L_3$。我们将在最后一步处理它。

第三步：生成单关节变换矩阵

将参数代入通用公式。由于所有 $\alpha =0,d=0$，矩阵会大幅简化（$c\alpha =1,s\alpha =0$）。

1. 关节 1 矩阵 (${}_1^{0}T$)
代入：$a_0=0,\theta ={\theta }_1$
$${}_1^{0}T=\left[\begin{array}{cccc}{c}_1& -s_1& 0& 0\\ s_1& c_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

2. 关节 2 矩阵 (${}_2^{1}T$)
代入：$a_1=L_1,\theta ={\theta }_2$
$${}_2^{1}T=\left[\begin{array}{cccc}{c}_2& -s_2& 0& L_1\\ s_2& c_2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

3. 关节 3 矩阵 (${}_3^{2}T$)
代入：$a_2=L_2,\theta ={\theta }_3$
$${}_3^{2}T=\left[\begin{array}{cccc}{c}_3& -s_3& 0& L_2\\ s_3& c_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

第四步：链式乘法 (Chain Multiplication)

我们需要计算总变换矩阵 ${}_3^{0}T={}_1^{0}T\cdot {}_2^{1}T\cdot {}_3^{2}T$。为了演示清楚，我们分两步乘。

1. 先算前两节 (${}_2^{0}T={}_1^{0}T\cdot {}_2^{1}T$)

$$\left[\begin{array}{cccc}{c}_1& -s_1& 0& 0\\ s_1& c_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{cccc}{c}_2& -s_2& 0& L_1\\ s_2& c_2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

利用三角恒等式 ($c_1{c}_2-s_1{s}_2=c_{1+2}$)，结果为：
$${}_2^{0}T=\left[\begin{array}{cccc}{c}_{12}& -s_{12}& 0& L_1{c}_1\\ s_{12}& c_{12}& 0& L_1{s}_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
物理意义：坐标系 {2} 相对于基座旋转了 ${\theta }_1+{\theta }_2$，平移了 $(L_1{c}_1,L_1{s}_1)$。

2. 再乘第三节 (${}_3^{0}T={}_2^{0}T\cdot {}_3^{2}T$)

$$\left[\begin{array}{cccc}{c}_{12}& -s_{12}& 0& L_1{c}_1\\ s_{12}& c_{12}& 0& L_1{s}_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{cccc}{c}_3& -s_3& 0& L_2\\ s_3& c_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

运算结果：
$${}_3^{0}T=\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0& L_1{c}_1+L_2{c}_{12}\\ s_{123}& c_{123}& 0& L_1{s}_1+L_2{s}_{12}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
物理意义：关节 3 (肘部) 的绝对位置是 $(L_1{c}_1+L_2{c}_{12},L_1{s}_1+L_2{s}_{12})$。

第五步：计算末端点 P

最后，不要忘了图中的末端点 $P$。
在坐标系 {3}\{3\}{3} 中，点 $P$ 的坐标是固定的：${}^{3}P=[L_3,0,0,1{]}^T$。

将它转换到世界坐标系：
$${}^{0}P={}_3^{0}T\cdot {}^{3}P$$

$$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\\ 1\end{array}\right]=\left[\begin{array}{cccc}{c}_{123}& -s_{123}& 0& L_1{c}_1+L_2{c}_{12}\\ s_{123}& c_{123}& 0& L_1{s}_1+L_2{s}_{12}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}{L}_3\\ 0\\ 0\\ 1\end{array}\right]$$

🎉 最终公式：
$$\begin{array}{rl}{x}_p& =L_1\cos ({\theta }_1)+L_2\cos ({\theta }_1+{\theta }_2)+L_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)\\ y_p& =L_1\sin ({\theta }_1)+L_2\sin ({\theta }_1+{\theta }_2)+L_3\sin ({\theta }_1+{\theta }_2+{\theta }_3)\\ z_p& =0\end{array}$$

---

📘 机器人逆运动学 (Inverse Kinematics)

核心内容：IK 概念、解析法几何推导、数值法公式底层来源 (泰勒展开与梯度下降)

---

1. 逆运动学 (IK) 核心定义

1.1 什么是 IK？

逆运动学是寻找 “为了达到目标位置，关节需要怎么动” 的过程。

• 正运动学 (FK)：映射 $f:\text{关节空间}\to \text{笛卡尔空间}$
  • 公式：$x=f(\mathbf{q})$
  • 性质：唯一解 (One-to-One)
• 逆运动学 (IK)：映射 $f^{-1}:\text{笛卡尔空间}\to \text{关节空间}$
  • 公式：$\mathbf{q}=f^{-1}(x)$
  • 性质：多解 (One-to-Many)、可能无解、非线性。

1.2 三大挑战

1. 多解性 (Redundancy)：同一个末端位置可能对应多种关节姿态（如“肘部朝上”或“肘部朝下”）。
2. 奇异性 (Singularity)：当雅可比矩阵行列式为 0 时（如手臂伸直），运动失去自由度，数值解法会发散。
3. 非线性 (Non-linearity)：方程包含大量的 $\sin ,\cos$，无法用简单的线性代数直接求解。

---

2. 方法一：解析法 (Analytical Method)

核心思想：利用几何关系（三角形余弦定理）和代数消元，推导出一个精确的闭合公式。
适用场景：结构简单的标准机械臂（如 3自由度平面臂、符合 Pieper 准则的 6轴工业臂）。

🎓 3自由度平面臂 (3-DOF Planar)

已知：末端位姿 $(x,y,\varphi )$，杆长 $L_1,L_2,L_3$。
求解：关节角 ${\theta }_1,{\theta }_2,{\theta }_3$。

步骤 1：去尾 (Wrist Decoupling)

由于末端姿态 $\varphi$ 已知，且 $L_3$ 长度固定，我们可以算出 手腕关节中心 $(W_x,W_y)$。这相当于把 3自由度降维成 2自由度。

$$\{\begin{array}{l}{W}_x=x-L_3\cos (\varphi )\\ W_y=y-L_3\sin (\varphi )\end{array}$$

步骤 2：解三角形 (求解 ${\theta }_2$)

构建由 $L_1,L_2$ 和原点到手腕距离 $r$ 组成的三角形。

• 辅助变量：$r^2=W_x^2+W_y^2$

• 余弦定理应用：
  $$r^2=L_1^2+L_2^2-2L_1{L}_2\cos (\pi -{\theta }_2)$$
  注意：三角形内角是 $(180^{\circ }-{\theta }_2)$。
  利用 $\cos (\pi -\theta )=-\cos (\theta )$，得：
  $$r^2=L_1^2+L_2^2+2L_1{L}_2\cos ({\theta }_2)$$

• 推导 ${\theta }_2$：
  $$D=\cos ({\theta }_2)=\frac{r^2-L_1^2-L_2^2}{2L_1{L}_2}$$
  $${\theta }_2=\text{atan2}(\pm \sqrt{1-D^2},D)$$
  正负号分别对应肘部下/上两个解。

步骤 3：几何差角 (求解 ${\theta }_1$)

${\theta }_1$ 是手腕的几何极角 $\alpha$ 减去三角形内角 $\beta$。

• 总角 $\alpha$：$\alpha =\text{atan2}(W_y,W_x)$
• 内角 $\beta$ (利用几何投影)：
  $$\beta =\text{atan2}(L_2\sin ({\theta }_2),L_1+L_2\cos ({\theta }_2))$$
• 结果：
  $${\theta }_1=\alpha -\beta$$

步骤 4：姿态补全 (求解 ${\theta }_3$)

平面内角度直接相加：
$$\varphi ={\theta }_1+{\theta }_2+{\theta }_3\Rightarrow {\theta }_3=\varphi -({\theta }_1+{\theta }_2)$$

---

3. 方法二：数值迭代法 (Numerical Iterative Method)

核心思想：从一个初始猜测出发，利用导数（雅可比矩阵）不断修正，逼近真实解。
适用场景：任意构型的机器人、冗余机械臂、无法推导解析解的情况。

3.1 核心工具：雅可比矩阵 (Jacobian Matrix)

$$J=\frac{\partial \mathbf{x}}{\partial \mathbf{q}}$$
它描述了关节微小变化 $\Delta \mathbf{q}$ 如何引起 末端微小位移 $\Delta \mathbf{x}$。

---

3.2 算法 A：牛顿-拉夫逊法 (Newton-Raphson)

这是最常用的 IK 算法，利用一阶泰勒展开进行线性化求解。

📐 公式推导 (为什么是 $J^{-1}$ ?)

1. 问题定义：
   设正运动学函数为 $f(\mathbf{q})$，我们的目标是找到 $\mathbf{q}$ 使得 $f(\mathbf{q})={\mathbf{x}}_{target}$。

2. 泰勒级数展开 (Linearization)：
   在当前角度 ${\mathbf{q}}_{curr}$ 附近，我们将非线性函数 $f$ 展开为一阶近似（把曲线看作切线）：
   $$f({\mathbf{q}}_{curr}+\Delta \mathbf{q})\approx f({\mathbf{q}}_{curr})+\underset{J}{\underbrace{f^{\prime }({\mathbf{q}}_{curr})}}\cdot \Delta \mathbf{q}$$

3. 设定目标：
   我们要让下一步的位置等于目标位置：
   $${\mathbf{x}}_{target}\approx {\mathbf{x}}_{curr}+J\cdot \Delta \mathbf{q}$$

4. 构建误差方程：
   $$\underset{\mathbf{e}\text{ (误差)}}{\underbrace{{\mathbf{x}}_{target}-{\mathbf{x}}_{curr}}}=J\cdot \Delta \mathbf{q}$$
   即：
   $$\mathbf{e}=J\cdot \Delta \mathbf{q}$$

5. 求解增量：
   两边左乘 $J$ 的逆矩阵：
   $$\Delta \mathbf{q}=J^{-1}\cdot \mathbf{e}$$

⚙️ 工程实现细节

• 迭代公式：${\mathbf{q}}_{k+1}={\mathbf{q}}_k+J^{-1}({\mathbf{q}}_k)\cdot \mathbf{e}$
• 伪逆 (Pseudo-Inverse)：当 $J$ 不可逆（如非方阵或奇异）时，使用 $J^{†}=J^T(J{J}^T{)}^{-1}$。
• 阻尼最小二乘 (DLS)：为了防止奇异点导致 $J^{-1}$ 爆炸，公式修正为：
  $$\Delta \mathbf{q}=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}\mathbf{e}$$

---

3.3 算法 B：梯度下降法 (Gradient Descent)

这是基于最优化理论的方法，利用损失函数的梯度来寻找极小值。

📐 公式推导 (为什么是 $J^T$ ?)

1. 定义损失函数 (Loss Function)：
   我们要最小化当前位置与目标位置的距离平方：
   $$L(\mathbf{q})=\frac{1}{2}\Vert {\mathbf{x}}_{target}-f(\mathbf{q}){\Vert }^2=\frac{1}{2}{\mathbf{e}}^T\mathbf{e}$$

2. 计算梯度 (Gradient)：
   我们需要求 $L$ 对关节 $\mathbf{q}$ 的导数 $\nabla L$。根据链式法则：
   $$\nabla L=\frac{\partial L}{\partial \mathbf{q}}=\frac{\partial L}{\partial \mathbf{e}}\cdot \frac{\partial \mathbf{e}}{\partial \mathbf{q}}$$

   • 第一部分：$\frac{\partial (\frac{1}{2}{\mathbf{e}}^T\mathbf{e})}{\partial \mathbf{e}}={\mathbf{e}}^T$（视为行向量）。
   • 第二部分：$\mathbf{e}={\mathbf{x}}_{target}-f(\mathbf{q})$，所以 $\frac{\partial \mathbf{e}}{\partial \mathbf{q}}=-\frac{\partial f}{\partial \mathbf{q}}=-J$。

   组合得到：
   $$\nabla L={\mathbf{e}}^T\cdot (-J)=-({\mathbf{e}}^{T}J)$$
   转置为列向量形式（梯度方向）：
   $$\nabla L=-J^T\mathbf{e}$$

3. 梯度下降更新规则：
   为了最小化损失，必须沿着梯度的反方向移动关节角：
   $$\Delta \mathbf{q}=-\alpha \cdot \nabla L$$
   其中 $\alpha$ 是学习率（步长）。

4. 代入梯度：
   $$\Delta \mathbf{q}=-\alpha \cdot (-J^T\mathbf{e})$$
   $$\Delta \mathbf{q}=\alpha \cdot J^T\cdot \mathbf{e}$$

⚙️ 工程实现细节

• 特点：不需要求逆矩阵，计算量小，永不崩溃（即使在奇异点）。
• 缺点：收敛速度慢（线性收敛），容易在目标附近震荡。

---

4. 总结对比表

维度	解析法 (Analytical)	牛顿-拉夫逊法 (Newton-Raphson)	梯度下降法 (Gradient Descent)
核心数学	几何、三角变换	泰勒展开、矩阵求逆	链式法则、最优化
公式特征	$\theta =\text{atan2}(...)$	$\Delta \theta =J^{-1}e$	$\Delta \theta =\alpha J^{T}e$
物理直觉	解三角形	沿切线一步到位	沿坡度慢慢下滑
速度	🚀 极快 (无迭代)	🚄 快 (二次收敛)	🚶 慢 (线性收敛)
稳定性	✅ 完美	⚠️ 奇异点会崩溃 (需DLS)	✅ 非常稳健
通用性	❌ 仅限特定构型	✅ 任意机器人	✅ 任意机器人

---

5. 附录：通用数值法伪代码

def solve_ik(target_pose, initial_guess):
    q = initial_guess
    for i in range(max_iter):
        # 1. 正运动学
        current_pose = forward_kinematics(q)
        
        # 2. 计算误差
        error = target_pose - current_pose
        if norm(error) < tolerance:
            return q # 成功
            
        # 3. 计算雅可比 (有限差分法)
        J = compute_jacobian(q)
        
        # 4. 选择算法计算增量
        # 算法 A: 牛顿法 (DLS优化版)
        dq = J.T * inv(J*J.T + lambda*I) * error
        
        # 算法 B: 梯度下降法 (备选)
        # dq = alpha * J.T * error
        
        # 5. 更新
        q = q + dq
        
    return Fail