逆运动学
约 6203 个字 22 张图片 预计阅读时间 21 分钟
1. 什么是逆运动学 (Inverse Kinematics)
1.1 基本概念
首先，我们需要理解逆运动学要解决什么问题。
定义
逆运动学 (Inverse Kinematics, IK)：在已知机器人末端执行器（end-effector，比如机械手爪）的位姿（位置和姿态） 的情况下，反向求解出机器人所有关节角度的过程。
我们可以通过下面这个简单的框图来理解它与正运动学（Forward Kinematics, FK）的关系：
- 正运动学：输入关节角度 (\(\theta_1, \theta_2, ..., \theta_n\) ) -> 输出末端位姿 (\(T\) )，这是一个“给定原因，求结果”的过程，通常有唯一解。
- 逆运动学：输入末端位姿 (\(T\)) -> 输出关节角度 (\(\theta_1, \theta_2, ..., \theta_n\))，这是一个“给定结果，求原因”的过程，解可能不唯一，甚至无解。
数学上，这个问题可以表示为：
已知末端执行器的齐次变换矩阵\(^0T_6\)(对于一个六轴机器人)，求解所有关节变量\(\theta_1, \theta_2, \theta_3, \theta_4, \theta_5, \theta_6\)。
逆运动学的挑战
与正运动学相比，求解逆运动学要复杂得多。它可能会遇到以下问题：
- 多解性 (Multiple Solutions)：机器人可能会有多种关节构型到达同一个目标位姿。如下图所示的简单二连杆机械臂，它可以“手肘向上”或“手肘向下”两种姿态到达同一个点。
- 无解 (No Solution)：如果目标位姿超出了机器人的工作空间（workspace），那么逆运动学就无解。
- 解析解与数值解：对于结构简单的机器人，我们可能能推导出封闭的数学公式（解析解）。但对于复杂机器人，往往只能依赖迭代计算（数值解）。
为了解决这个问题，有几种常用方法：
- 解耦技术 (Decoupling technique)：将复杂问题分解为几个简单的子问题。
- 几何法 (Generic method)：通过几何关系来求解。
- 数值解法 (Numeric solution)：通过迭代优化算法逼近解。
本次课程的重点是解耦技术。
2. 运动学解耦 (Kinematic Decoupling)
2.1 带有球形腕的机械臂
工业机器人最常见的一种设计是：一个拟人臂（Anthropomorphic Arm，也叫肘形机械臂） 串联一个球形腕（Spherical Wrist）。
什么是球形腕?
球形腕指的是最后三个旋转关节（关节4, 5, 6）的轴线交于同一点，这个点我们称之为腕点 (Wrist point)。
这种巧妙的设计是实现运动学解耦的关键！
2.2 结构分解
正是因为球形腕的存在，我们可以将整个逆运动学问题“解耦”成两个独立的、更简单的问题：
观察这个机械臂，可以知道，腕点的位置完全由Base, link1, link2 决定。由此可以做以下处理：
- 逆位置运动学 (Inverse Position)：求解前三个关节 (\(\theta_1, \theta_2, \theta_3\))，使得腕点能够到达目标位置。这部分由“臂”来完成。
- 逆姿态运动学 (Inverse Orientation)：求解后三个关节 (\(\theta_4, \theta_5, \theta_6\))，使得末端执行器能够达到目标姿态。这部分由“腕”来完成。
思路：
在逆运动学求解里，已知最后的工件的位置 \(\Rightarrow ^0\mathbf{d}_6\) 已知 \(\Rightarrow\) 只和 \(\theta_1,\theta_2,\theta_3\) 有关；
而 \(^3R_6\) 只和 \(\theta_4,\theta_5,\theta_6\) 有关；
2.3 解耦的核心方程
我们知道，从基座到末端的总变换可以写成：
\(^0T_6 = {}^0T_3 {}^3T_6\)
- \(^0T_3\)只与\(\theta_1, \theta_2, \theta_3\)有关，决定了腕点的位置。
- \(^3T_6\)只与\(\theta_4, \theta_5, \theta_6\)有关，决定了末端相对于腕的姿态。
这个公式可以展开成矩阵形式：
\(^0T_6 = \begin{bmatrix} ^0R_3 & ^0d_3 \\ \mathbf{0}_{1\times3} & 1 \end{bmatrix} \begin{bmatrix} ^3R_6 & ^3d_6 \\ \mathbf{0}_{1\times3} & 1 \end{bmatrix}\)
其中，对于球形腕，腕关节不产生位移，所以\(^3d_6 = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}\)。但为了表示从腕点到工具末端的偏移，我们通常会有一个偏移向量。
为了求解，我们需要建立两个关键方程。
关键方程 (4-1)：腕点位置方程
我们希望找到腕点\(^0d_{wrist}\)的位置。这个点实际上就是坐标系{3}的原点在基座坐标系{0}下的位置，即\(^0d_3\)。同时，它也可以从已知的末端位姿反推回来。
已知末端执行器的位置是\(^0d_6\)，姿态是\(^0R_6\)。从腕点到末端执行器原点的向量在坐标系{3}中表示为\(^3r_{wrist}\)，在我们的模型中通常是沿着\(z_6\)轴的偏移（例如工具长度），假设长度为\(l_3\)，则\(^3r_{wrist} = [0, 0, l_3]^T\)（ \(l_3\) 是link3的长度）。
因此，腕点的位置可以表示为：
$$
^0d_{wrist} = ^0d_3 + ^0R_3 {}^3r_{wrist} = ^0d_6
$$
- \(^0d_3\) : 从原点出发的矢量
- \(^0R_3 {}^3r_{wrist}\) 偏移矢量
这个方程的左边只和\(\theta_1, \theta_2, \theta_3\)有关，右边是已知量。我们可以用它来求解前三个关节角。
关键方程 (4-2)：腕部姿态方程
我们有链式法则：\(^0R_6 = {}^0R_3 {}^3R_6\)。
为了求出\(^3R_6\)(它只和\(\theta_4, \theta_5, \theta_6\)有关)，我们可以对上式左乘\((^0R_3)^{-1}\)，也就是\((^0R_3)^T\)(因为旋转矩阵的逆等于其转置)。
$$
^3R_6 = (^0R_3)^T\cdot {}^0 R_6 \quad \quad\quad\quad(4-2)
$$
这个方程的右边，在求出\(\theta_1, \theta_2, \theta_3\)后就是已知量。我们可以用它来求解后三个关节角。
2.4 三步求解法
逆运动学求解路线图
- 第一步 (求臂): 利用腕点位置方程 (4-1)，求解前三个关节角\(\theta_1, \theta_2, \theta_3\)。
- 第二步 (计算腕姿态): 将求得的\(\theta_1, \theta_2, \theta_3\)代入，计算出\(^0R_3\)。然后利用腕部姿态方程 (4-2) 计算出矩阵\(^3R_6\)。
- 第三步 (求腕): 根据\(^3R_6\)的表达式，反解出后三个关节角\(\theta_4, \theta_5, \theta_6\)。
3. 求解臂部关节 (Step 1: Inverse Position)
现在我们来详细推导如何求解\(\theta_1, \theta_2, \theta_3\)。
3.1 臂部正运动学
首先，我们需要臂部的正运动学模型。根据PPT第11页给出的DH参数表和推导：
我们得到了臂部末端（即腕点）的位姿矩阵\(^0T_3\)，从中可以分离出旋转部分\(^0R_3\)和位置部分\(^0d_3\)。
腕点位置\(^0d_{wrist}\)是由臂部位置\(^0d_3\)和一个附加的腕部偏移得到的，最终表达式及求解过程如下：
注意这里的 \(±\) 号意味着\(\theta_1\)有两个可能的解，有两组 \(±\) 号 \(\Rightarrow\) 4 组解；
臂部关节角解
假设腕点目标位置为\((d_x, d_y, d_z)\)，则臂部关节角的解为：
$$ \theta_1 = \text{atan2}(d_y, d_x) - \text{atan2}(d_2, \pm\sqrt{d_x^2+d_y^2-d_2^2})$$
$$ \theta_3 = \text{atan2}(s_3, c_3)$$
其中\(c_3 = \frac{(d_xc_1+d_ys_1)^2 + (d_z-l_1)^2 - l_2^2 - l_3^2}{2l_2l_3}\),\(s_3 = \pm\sqrt{1-c_3^2}\)
$$ \theta_2 = \text{atan2}((d_xc_1+d_ys_1)(l_2+l_3c_3) - (d_z-l_1)l_3s_3, (d_xc_1+d_ys_1)l_3s_3 + (d_z-l_1)(l_2+l_3c_3))$$
注意：由于\(\theta_1\)和\(\theta_3\)的解中都有 ± 号，组合起来总共有 四组 解。
例题 4-1-1
题目：对于一个拟人臂，给定参数 \(L_1=L_2=L_3=1, d_2=0.1\)。已知一个解是 \((q_1, q_2, q_3) = (0, \pi/4, -\pi/2)\)。证明 \((0, -\pi/4, \pi/2)\) 和 \((-3.2828, -3\pi/4, -\pi/2)\) 也是解。
分析：
-
第一步：将已知解 \((0, \pi/4, -\pi/2)\) 代入正运动学公式，计算出目标位置 \((d_x, d_y, d_z)\)。
\(d_x = 0.1, d_y = 1.4142, d_z = 1\) -
第二步：将另外两组给定的角度代入正运动学公式，验证它们是否也得到相同的位置 \((d_x, d_y, d_z)\)。
- 结论：通过计算可以验证，这三组不同的关节角确实都使机器人腕点到达了同一个目标位置，这直观地展示了逆运动学的多解性。
- 具体求解过程：
4. 求解腕部关节 (Step 3: Inverse Orientation)
在求出\(\theta_1, \theta_2, \theta_3\)后，我们进入第二步和第三步。
4.1 计算腕部姿态矩阵
我们已经知道\(^3R_6 = (^0R_3)^T \cdot {}^0R_6\)。
- \(^0R_6\)是已知的目标姿态矩阵。
- \(^0R_3\)可以通过将第一步求得的\(\theta_1, \theta_2, \theta_3\)代入其表达式得到。
这样，我们就能计算出矩阵\(^3R_6\)的所有9个元素，我们称之为\(r_{ij}\)。
4.2 求解过程
现在我们需要另一个\(^3R_6\)的表达式，这个表达式是关于\(\theta_4, \theta_5, \theta_6\)的。
通过连乘腕部关节的变换矩阵\(^3T_6 = {}^3T_4 {}^4T_5 {}^5T_6\)，我们可以得到 (参考PPT第16页)：
$$
^3R_6 = \begin{bmatrix}
c_4c_5c_6 - s_4s_6 & -c_4c_5s_6 - s_4c_6 & c_4s_5 \
s_4c_5c_6 + c_4s_6 & -s_4c_5s_6 + c_4c_6 & s_4s_5 \
-s_5c_6 & s_5s_6 & c_5
\end{bmatrix}
$$
现在，我们将这个矩阵与我们上一步算出的\(r_{ij}\)矩阵进行元素比较。
求解\(\theta_5\):
比较两个矩阵的 (3,3) 元素：
\(r_{33} = c_5\)
同时，\(\sqrt{r_{13}^2 + r_{23}^2} = \sqrt{(c_4s_5)^2 + (s_4s_5)^2} = \sqrt{s_5^2(c_4^2+s_4^2)} = \sqrt{s_5^2} = |s_5|\)。
假设我们选择\(\theta_5 \in [0, \pi]\)，则\(s_5 \ge 0\)。
所以，\(s_5 = \sqrt{r_{13}^2 + r_{23}^2}\)。
最终，
$$
\theta_5 = \text{atan2}(\sqrt{r_{13}^2 + r_{23}^2}, r_{33})
$$
注意，\(c_5\)也可以是负的，所以\(\theta_5 = \text{atan2}(-\sqrt{r_{13}^2 + r_{23}^2}, r_{33})\)也是一个解。这对应了 ± 的两种情况。
求解\(\theta_4, \theta_6\):
在求出\(\theta_5\)且\(s_5 \neq 0\)的情况下：
比较 \((1,3)\) 和 \((2,3)\) 元素：
\(r_{13} = c_4s_5\),\(r_{23} = s_4s_5\)
$$
\theta_4 = \text{atan2}(r_{23}/s_5, r_{13}/s_5) = \text{atan2}(r_{23}, r_{13})
$$
比较 \((3,1)\) 和 \((3,2)\) 元素：
\(r_{31} = -s_5c_6\),\(r_{32} = s_5s_6\)
$$
\theta_6 = \text{atan2}(r_{32}/s_5, -r_{31}/s_5) = \text{atan2}(r_{32}, -r_{31})
$$
奇异点 (Singularity)
当\(\theta_5 = 0\)或\(\pi\)时，\(s_5 = 0\)。此时，
- 矩阵\(^3R_6\)中的\(r_{13}, r_{23}, r_{31}, r_{32}\)都为0，我们无法用上面的公式求解\(\theta_4, \theta_6\)。
- 在这种姿态下，关节4和关节6的轴线重合，它们的作用变得无法区分。转动\(\theta_4\)的效果和转动\(\theta_6\)一样，只是方向相反。
- 这意味着\(\theta_4 + \theta_6\)(或\(\theta_4 - \theta_6\)) 是一个定值，但它们各自可以有无限多组解。
- 在实际应用中，遇到奇异点需要特殊处理，比如可以人为固定其中一个角（如令\(\theta_4=0\)），然后求解另一个角。
5. 实际应用中的工具位置
在很多情况下，我们关心的不是机器人法兰盘（frame 6）的位姿，而是安装在上面的工具末端（Tool Tip） 的位姿。
假设我们已知的是工具坐标系{n}的位姿\(^0T_n\)，而不是法兰盘坐标系{6}的位姿\(^0T_6\)。我们需要先把它转换成法兰盘的位姿，才能使用我们前面推导的公式。
设从法兰盘到工具末端的变换为\(^6T_n\)(这通常是已知的，通过工具标定得到)。
那么\(^0T_n = {}^0T_6 {}^6T_n\)。
所以，我们可以先计算出法兰盘的位姿：
$$
^0T_6 = {}^0T_n (^6T_n)^{-1}
$$
从中提取出\(^0R_6\)和\(^0d_6\)，然后就可以套用之前的逆运动学求解流程了。
6. 六自由度机械臂逆解总结
完整求解流程和公式
已知：目标位姿矩阵\(^0T_n\)和工具变换矩阵\(^6T_n\)。
- 计算法兰盘位姿：\(^0T_6 = {}^0T_n (^6T_n)^{-1}\)，得到\(^0R_6, ^0d_6\)。
- 计算腕点位置：\(^0d_{wrist} = ^0d_6 - {}^0R_6 {}^6d_{wrist}\)，得到\((d_x, d_y, d_z)\)。
- 求解臂部关节\(\theta_1, \theta_2, \theta_3\)：使用第3节中的公式，这会得到多组解（通常是4组）。
- 对每一组臂部解，求解腕部关节：
a. 计算\(^0R_3\)。
b. 计算\(^3R_6 = (^0R_3)^T {}^0R_6\)。
c. 使用第4节的公式，从\(^3R_6\)中求解\(\theta_4, \theta_5, \theta_6\)。
最终，我们会得到多组（最多8组）完整的关节角解。
例题 4-1-2
题目：对于一个6自由度机械臂，给定参数和一组初始关节角 \((0, \pi/4, -\pi/2, 0, \pi/4, \pi/2)\)，并且工具偏移 \(d_7=0.15\)。
分析：
- 正向验证：首先，将给定的6个关节角代入正运动学公式，计算出末端的位姿矩阵\(^0T_6\)(或者加上工具后的\(^0T_7\))。PPT第20页展示了这个计算过程。
-
逆向求解：
a. 从上一步计算出的位姿矩阵中，提取出\(^0R_6\)和\(^0d_6\)。
b. 应用总结中的逆运动学公式，一步步反解\(\theta_1\)到\(\theta_6\)。
c. PPT第20页展示了求解\(\theta_5, \theta_4, \theta_6\)的过程，并验证了可以得到45°或-45°等多组解。 -
证明其他解：题目要求证明另一组解 \((0, \pi/4, -\pi/2, -\pi, -\pi/4, \pi/2)\) 也是有效的。我们可以将这组解代入正运动学，看是否能得到与第一组解相同的末端位姿。如果可以，就证明了它也是一个合法的逆解。
附录：球形腕的种类
球形腕根据其关节类型（旋转R，移动P）和顺序，可以有不同的构型，如 RPR, RPY, PYR。
本课程主要讨论的是最常见的 RRR（旋转-旋转-旋转）型球形腕。不同的构型其DH参数和运动学方程会有所不同，但解耦的思想是通用的。
7. 通用代数解法 (Pieper's Technique)
7.1 基本思想
核心思想：逐个击破
Pieper's Technique 的核心思想是通过一系列的矩阵变换，逐步消去（或隔离）未知的关节变量，从而将复杂的非线性方程组简化，最终求解出所有关节角。这个过程有点像“剥洋葱”，一层一层地解开变量。
这个方法的基本步骤如下：
我们知道，总的变换矩阵是各个关节变换矩阵的连乘：
\(^0T_6 = {}^0T_1 {}^1T_2 {}^2T_3 {}^3T_4 {}^4T_5 {}^5T_6\)
这个方程里包含了所有6个未知数 \(\theta_1, ..., \theta_6\)，非常复杂。
为了求解，我们可以这样做：
1. 求解 \(\theta_1\): 将 \({}^0T_1\) 的逆矩阵 \(({}^0T_1)^{-1}\) 左乘到等式两边：
\(({}^0T_1)^{-1} \cdot {}^0T_6 = {}^1T_2 {}^2T_3 {}^3T_4 {}^4T_5 {}^5T_6\)
观察这个新方程：
* 左边：\(({}^0T_1)^{-1}\) 只含 \(\theta_1\)，\({}^0T_6\) 是已知的目标位姿。所以左边矩阵的每个元素都是关于 \(\theta_1\) 的函数。
* 右边：只包含 \(\theta_2, ..., \theta_6\)。
通过比较等式两边矩阵的某些元素，我们可以建立只关于 \(\theta_1\) 的方程并求解。
-
求解 \(\theta_2\): 在 \(\theta_1\) 已知后，我们继续左乘 \(({}^1T_2)^{-1}\)：
\(({}^1T_2)^{-1} ({}^0T_1)^{-1}\cdot {}^0T_6 = {}^2T_3 {}^3T_4 {}^4T_5 {}^5T_6\)- 此时，等式左边只含 \(\theta_2\) 这一个未知数，右边则从 \(\theta_3\) 开始。通过同样的方法，我们可以求解 \(\theta_2\)。
-
以此类推，直到所有关节变量都被解出。
7.2 实例分析：斯坦福机械臂
例题 4-2-1: 斯坦福机械臂 (Stanford Manipulator)
斯坦福机械臂是一个经典的早期工业机器人，它的结构不包含球形腕，是应用Pieper's Technique的绝佳例子。它的关节类型为：R-R-P-R-R-R（前三个关节为两个旋转和一个移动）。
下面我们来一步步求解它的逆运动学。
第一步：求解 \(\theta_1, \theta_2, d_3\)
我们从方程 \(({}^0T_1)^{-1} \cdot {}^0T_6 = {}^1T_6\) 开始。
通过计算，我们得到等式左边的矩阵 \(({}^0T_1)^{-1} {}^0T_6\) 和右边的矩阵 \({}^1T_6\)。比较这两个4x4矩阵的位置向量（最后一列）的元素，我们可以得到方程组：
$$
\begin{cases} d_3s_2 = d_xc_1 + d_ys_1 \ -d_3c_2 = d_z - d_1 \ d_2 = d_yc_1 - d_xs_1 \end{cases}
$$
(这里的\(d_x, d_y, d_z\) 是已知目标位置，\(d_1, d_2 ,...\) 是机器人连杆参数)。
-
求解 \(\theta_1\):
从第三个方程 \(d_yc_1 - d_xs_1 = d_2\) 入手，这个方程只包含 \(\theta_1\)。它的解在上一讲已经推导过：
$$
\theta_1 = \text{atan2}(d_y, d_x) - \text{atan2}(d_2, \pm\sqrt{d_x^2 + d_y^2 - d_2^2})
$$
这个解通常有两个。 -
求解 \(\theta_2\):
在 \(\theta_1\) 解出后，\(d_xc_1 + d_ys_1\) 成为已知值。我们联立前两个方程：
\(d_3s_2 = d_xc_1 + d_ys_1\)
\(d_3c_2 = d_1 - d_z\)
这是一个标准的反正切问题，可以解得：
$$
\theta_2 = \text{atan2}(d_xc_1 + d_ys_1, d_1 - d_z)
$$ -
求解 \(d_3\) (移动关节变量):
将 \(\theta_2\) 的解代回，可以得到：
$$
d_3 = \frac{d_1 - d_z}{\cos\theta_2}
$$
或者通过对上面两个方程平方和相加得到 \(d_3^2 = (d_xc_1 + d_ys_1)^2 + (d_1 - d_z)^2\) 来求解。
第二步：求解 \(\theta_4, \theta_5, \theta_6\)
现在前三个关节变量已知，我们可以继续求解腕部关节。
-
求解 \(\theta_4, \theta_5\):
我们建立方程 \(({}^3T_4)^{-1} ({}^2T_3)^{-1} ({}^1T_2)^{-1} ({}^0T_1)^{-1} {}^0T_6 = {}^4T_6\)。
等式左边只含未知数 \(\theta_4\)，而右边 \({}^4T_6 = {}^4T_5 {}^5T_6\) 包含 \(\theta_5, \theta_6\)。
通过复杂的代数运算和元素比较（如PPT第6页所示），可以建立关于 \(\theta_4\) 的方程并求解，然后利用得到的 \({}^4R_6\) 矩阵中的元素求解 \(\theta_5\)。
$$
\theta_4 = \text{atan2}(a_x c_1 c_2 + a_y s_1 c_2 - a_z s_2, a_x s_1 - a_y c_1)
$$
$$
\theta_5 = \text{atan2}(r_{13}, -r_{23})
$$
其中\(r_{13}\) 和\(r_{23}\) 是通过已知量和已求解的角度计算得到的中间变量。 -
求解 \(\theta_6\):
最后，我们建立方程 \(({}^4T_5)^{-1} ... ({}^0T_1)^{-1} {}^0T_6 = {}^5T_6\)。
通过比较矩阵元素，可以得到：
$$
\theta_6 = \text{atan2}(r_{21}, r_{11})
$$
总结
通用代数法虽然强大，但推导过程非常繁琐且容易出错，而且只适用于特定结构的机器人（通常是满足Pieper准则的机器人，即三个相邻旋转关节轴线相交或平行的组合）。对于更一般的机器人，我们往往需要求助于数值方法。
8. 数值解法 (Numerical Method)
8.1 为什么需要数值解法？
当机器人的结构非常复杂，或者存在多解、奇异点等问题时，很难或根本不可能找到解析解（也就是封闭的数学公式）。这时，数值解法就派上了用场。
数值解法的核心思想
数值解法不直接求解方程，而是采用迭代的方式。
- 先猜测一组关节角。
- 计算这组猜测值对应的末端位姿（正运动学）。
- 比较计算出的位姿和我们期望的位姿，得到一个误差。
- 根据这个误差，调整我们的猜测值，让它向正确的方向移动一小步。
- 重复2-4步，直到误差小到可以忽略不计。
最常用的数值方法之一是牛顿-拉弗森法 (Newton-Raphson Method)。
8.2 牛顿-拉弗森法
我们的目标是找到一组关节变量 \(\mathbf{q}\)，使得机器人的位姿 \(T(\mathbf{q})\) 等于我们期望的位姿 \(T_d\)。
这等价于寻找下面这个方程的根：
\(\mathbf{F}(\mathbf{q}) = T(\mathbf{q}) - T_d = 0\)
牛顿-拉弗森法求解步骤
-
初始化 (Step 1): 给定一个初始猜测的关节角向量 \(\mathbf{q}^{(0)}\)。
-
正运动学 (Step 2): 计算当前猜测值对应的末端位姿 \(T^{(i)} = T(\mathbf{q}^{(i)})\)。
-
计算误差 (Step 3): 计算当前位姿与目标位姿的差值 \(\delta T = T_d - T^{(i)}\)。
-
线性化与求解修正量 (Step 4):
位姿的微小变化 \(\delta T\) 和关节角的微小变化 \(\delta \mathbf{q}\) 之间近似成线性关系：
\(\delta T \approx \mathbf{J}(\mathbf{q}) \delta \mathbf{q}\)
这里的 \(\mathbf{J}(\mathbf{q})\) 就是大名鼎鼎的雅可比矩阵 (Jacobian Matrix)。它描述了末端执行器的速度与关节速度之间的关系，本质上是位姿函数对关节变量的偏导数。
为了求出关节角的修正量 \(\delta \mathbf{q}\)，我们对上式求逆：
\(\delta \mathbf{q} = \mathbf{J}^{-1}(\mathbf{q}) \delta T\) -
更新变量 (Step 5): 将修正量加到当前的猜测值上，得到新的猜测值：
\(\mathbf{q}^{(i+1)} = \mathbf{q}^{(i)} + \delta \mathbf{q}\) -
循环与终止: 重复步骤2-5，直到误差 \(||\delta T||\) 或者两次迭代的关节角变化 \(||\mathbf{q}^{(i+1)} - \mathbf{q}^{(i)}||\) 小于一个预设的阈值 \(\varepsilon\)。
例题 4-3-1: 2R平面臂的数值解
问题: 用牛顿-拉弗森法求解一个2R平面臂，使其末端到达目标点\([1, 1]^T\)。
分析:
- 初始猜测: \(\mathbf{q}^0 = [2\pi/3, -2\pi/3]^T\)。
- 第0次迭代:
- 计算得到初始位置 \(T^0 = [0.5, 0.866]^T\)。
- 误差 \(\delta T^0 = [1-0.5, 1-0.866]^T = [0.5, 0.134]^T\)。
- 计算雅可比矩阵\(J(\mathbf{q}^0)\) 及其逆\(J^{-1}(\mathbf{q}^0)\)。
- 计算修正量 \(\delta \mathbf{q}^0 = J^{-1} \delta T^0\)。
- 更新得到 \(\mathbf{q}^1 = \mathbf{q}^0 + \delta \mathbf{q}^0 = [1.517, -1.6717]^T\)。
- 后续迭代:
- 在第1次迭代后，位置变为 \(T^1 = [1.0418, 0.8445]^T\)，已经很接近目标了，但误差仍然大于阈值。
- 经过3次迭代后，位置 \(T^3\) 几乎就是\([1, 0.9999]^T\)，误差非常小。
- 此时的关节角 \(\mathbf{q}^3 = [1.5708, -1.5709]^T\)，这正是 \([\pi/2, -\pi/2]^T\) 的近似值，也就是我们期望的解析解之一。
8.3 数值方法的注意事项
使用数值方法需要注意
- 初始猜测很重要: 如果初始猜测离真实解太远，算法可能不收敛（即误差越来越大）或收敛到非期望的解。
- 解的非唯一性: 逆运动学通常有多解。数值法最终会收敛到哪个解，很大程度上取决于初始猜测。
- 奇异点问题: 当机器人处于奇异位形时，雅可比矩阵会变得不可逆（或接近不可逆），导致 \(\mathbf{J}^{-1}\) 无法计算或数值不稳定。这需要特殊的算法来处理（如阻尼最小二乘法）。
8.4 不同情况下的求解
雅可比矩阵 \(\mathbf{J}\) 的维度是 \(m \times n\)，其中 \(m\) 是任务空间的自由度（例如平面定位是2，空间位姿是6），\(n\) 是机器人的关节数。
-
Case 1: \(m = n\) (常规情况)
机器人自由度与任务自由度匹配（如6轴臂控制空间位姿）。\(\mathbf{J}\) 是方阵，可以直接求逆。这就是标准的牛顿-拉弗森法。 -
Case 2: \(m > n\) (欠驱动 Under-actuated)
任务要求比机器人能力多（如用3轴臂去完成6维空间位姿任务）。通常无解。我们只能退而求其次，寻找一个使误差最小的解。这通常通过最小二乘法实现：
\(\delta \mathbf{q} = (\mathbf{J}^T \mathbf{J})^{-1} \mathbf{J}^T \mathbf{r}\)
这里的 \(\mathbf{r}\) 就是误差向量。 -
Case 3: \(m < n\) (冗余 Redundant)
机器人自由度比任务要求多（如用7轴臂去抓取一个杯子）。存在无限多组解。这给了我们优化的空间，比如我们可以在完成主要任务（到达目标位姿）的同时，实现一些次要目标（如避开障碍物、保持关节在中间位置等）。
这时需要使用雅可比伪逆 (Pseudo-inverse) \(\mathbf{J}^+\) 来求解：
\(\delta \mathbf{q} = \mathbf{J}^+ \mathbf{r}\)
其中 \(\mathbf{J}^+ = \mathbf{J}^T(\mathbf{J}\mathbf{J}^T)^{-1}\)。
伪逆解给出的修正量 \(\delta \mathbf{q}\) 是所有可能修正量中长度最短的一个，这通常能让机器人运动更平滑。
例题 4-3-2: 3R平面臂 (冗余机械臂)
问题: 一个3R平面臂（3个关节）要到达一个2D点\([2, 2]^T\)。
请在这里插入第18页的图片，展示3R臂的求解。
分析:
这是一个典型的冗余问题 (\(n=3, m=2\))。我们不能直接用\(J^{-1}\) 因为\(J\) 不是方阵。
- 建立正运动学和雅可比矩阵\(J\)。
- 在迭代过程中，使用伪逆 \(J^+\) 来计算关节修正量 \(\delta \mathbf{q}\)。
- 从一个初始猜测开始，通过迭代最终可以找到一组满足条件的关节角，如$[59.01^\circ, -41.9^\circ, 40.9^\circ]^T`。