我以前是机器人专业，不过学的不多，教程应该是灰色封面的《机器人学导论》。3年前学的了，软件仿真学的是ABB，上手操作是KUKA的机器人。本文是给别人解决问题的记录，写个笔记。代码是matlab的，不免费分享，但是看我的解析应该也能自己写出来。我不从事这个行业，很多东西已经模糊了。
文章目录
一、DH参数二、正向运动学三、逆向运动学3.1 逆向运动学的求解方法：3.11 解析法（Analytical Method）3.12 数值法（Numerical Method）
3.2 应用和挑战
四、牛顿法机械臂逆运动学求解详解4.1 牛顿法4.2 牛顿法逆运动学求解算法4.3 代码（matlab）4.31 正向4.32 逆向
五、齐次变换矩阵推导5.1 基本变换5.2 齐次变换矩阵的推导5.3 齐次变换矩阵的合成
六、牛顿法详细推导
一、DH参数
DH参数（Denavit-Hartenberg参数）是用于描述机械臂关节和连杆之间几何关系的一种标准方法。通过DH参数表，可以将机械臂的复杂几何结构简化为一系列标准化的变换，这些变换可以用于建立机械臂的运动学模型。
DH参数包括四个变量：
α
\alpha
α、
a
a
a、
d
d
d和
θ
\theta
θ，它们分别描述了相邻连杆之间的空间关系。具体描述如下：
α
i
\alpha_i
αi（杆件扭角）：
定义：第
i
i
i根连杆和第
i
−
1
i-1
i−1根连杆的坐标系
Z
Z
Z轴之间的旋转角度，绕着第
i
−
1
i-1
i−1根连杆的
X
i
−
1
X_{i-1}
Xi−1轴进行旋转。作用：描述相邻两连杆之间的“扭曲”程度。
a
i
a_i
ai（杆件长度）：
定义：第
i
i
i根连杆上，坐标系
Z
i
−
1
Z_{i-1}
Zi−1轴沿
X
i
−
1
X_{i-1}
Xi−1轴到达
Z
i
Z_i
Zi轴的距离，即相邻连杆
Z
Z
Z轴之间的距离。作用：描述相邻两连杆的“位移”距离。
d
i
d_i
di（关节距离）：
定义：第
i
i
i个关节沿
Z
i
Z_i
Zi轴的偏移量，即从第
i
i
i根连杆的原点到第
i
+
1
i+1
i+1根连杆的原点的位移。作用：描述关节在其轴线上的移动。
θ
i
\theta_i
θi（关节角）：
定义：第
i
i
i个关节绕着
Z
i
Z_i
Zi轴旋转的角度。作用：描述关节的旋转角度。
如果你操作过机械臂或者近距离见过机械臂就能很好的理解这些参数，看那些示意图，画几个坐标是不够清晰的。
你可以使用ABB机器人仿真软件RobotStudio，仿真、操作机械臂。
如下图，可以看到这个机械臂只能转动（绝大多数机械臂都是这样的），每个关节转动到一定的角度，末端执行器就能到达不同的位姿（位置和姿态）。
机械臂的转动部位通常是关节。在机械臂中，关节（Joint）是用于连接相邻连杆（Link）的部件，并赋予机械臂一定的自由度，使得机械臂能够在空间中执行各种复杂的运动。每个关节通常会允许一个连杆相对于另一个连杆进行旋转或平移。
机械臂的关节可以分为以下几种类型：
旋转关节（Revolute Joint）：这种关节允许连杆围绕一个固定轴进行旋转，类似于人类的肘关节。DH参数中的关节角
θ
\theta
θ就是描述旋转关节的旋转角度。
滑动关节（Prismatic Joint）：这种关节允许连杆沿着一个固定方向进行平移，类似于推拉门的运动。DH参数中的关节距离
d
d
d就是描述滑动关节的平移距离。
球形关节（Spherical Joint）：这种关节允许连杆绕着一个固定点进行多个方向的旋转，类似于人类的肩关节。
螺旋关节（Helical Joint）：这种关节结合了旋转和滑动两种运动，即连杆在旋转的同时也进行平移。
最下面是底座，通常是固定的，与第一个连杆使用转动关节连接，它可以绕着竖直方向转，这个方向就是Z轴，X、Y轴则是右手定则确定。
以此类推，每个连杆都可以绕着前一个连杆末端的轴转动，最末端黑色那里要安装末端执行器，比如抓手、焊枪等等，上面还会有一些控制线缆来控制末端执行器，图中没显示。（这些关节转动角度通常是有角度限制的）
转动距离底座越近的关节，末端执行器的位姿变化就越大。
自由度是指机械臂在空间中可以独立控制的方向数（比如绕某个轴旋转、平移）,上图机械臂的自由度是多少？
现在再结合坐标系理解一下：坐标系0（基坐标系）和1通常是重合的。
二、正向运动学
正向运动学（Forward Kinematics，FK）是机器人学中研究机械臂末端执行器在已知关节参数条件下的位置和姿态的计算方法。换句话说，正向运动学是给定机械臂各个关节的角度或位移，求解机械臂末端执行器在空间中的位置和朝向。
🟢基本概念：
连杆（Link）：机械臂的刚性部分，连接两个相邻的关节。关节（Joint）：机械臂的运动部分，连接相邻的连杆。关节可以是旋转关节或滑动关节。末端执行器（End-Effector）：机械臂的末端部分，用于执行任务，例如抓取、焊接等。其位置和姿态由机械臂的关节参数决定。关节变量：描述关节运动的参数，对于旋转关节是旋转角度
θ
\theta
θ，对于滑动关节是位移
d
d
d。
前面图中的机械臂，只能转动，通过确定各个关节的角度，就可以确定末端执行器的位姿。
🟢基本流程：
建立坐标系：为每个连杆和关节分配一个局部坐标系，一般使用DH（Denavit-Hartenberg）参数来描述这些坐标系之间的关系。定义DH参数：根据机械臂的几何结构，计算每个连杆的DH参数，包括
α
i
\alpha_i
αi（杆件扭角）、
a
i
a_i
ai（杆件长度）、
d
i
d_i
di（关节距离）和
θ
i
\theta_i
θi（关节角）。建立齐次变换矩阵：通过DH参数，可以为每个关节建立一个齐次变换矩阵
T
i
T_i
Ti，该矩阵描述了从关节
i
−
1
i-1
i−1的坐标系到关节
i
i
i的坐标系的变换关系。矩阵形式如下(推导看本文后面小节)：
T
i
=
[
cos
θ
i
−
sin
θ
i
cos
α
i
sin
θ
i
sin
α
i
a
i
cos
θ
i
sin
θ
i
cos
θ
i
cos
α
i
−
cos
θ
i
sin
α
i
a
i
sin
θ
i
0
sin
α
i
cos
α
i
d
i
0
0
0
1
]
T_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}
Ti=
cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1
连乘齐次变换矩阵：将所有的齐次变换矩阵按照关节的顺序连乘，得到从基坐标系到末端执行器坐标系的变换矩阵
T
0
,
n
T_{0,n}
T0,n，即：
T
0
,
n
=
T
1
×
T
2
×
⋯
×
T
n
T_{0,n} = T_1 \times T_2 \times \dots \times T_n
T0,n=T1×T2×⋯×Tn
其中，
T
0
,
n
T_{0,n}
T0,n表示末端执行器相对于基座的齐次变换矩阵,即从坐标系0到n，有的写法是写在左边，如：
6
0
T
_{6}^{0}T
60T
提取末端执行器的位置和姿态：从
T
0
,
n
T_{0,n}
T0,n中可以提取出末端执行器的位置和姿态。位置由变换矩阵中的平移部分给出，姿态由旋转部分给出。
🟢末端执行器的位姿：
通过正向运动学的计算，我们可以得到机械臂末端执行器的位姿（位置和朝向）。假设
T
0
,
n
T_{0,n}
T0,n为末端执行器的齐次变换矩阵，则：
位置：末端执行器在空间中的位置由齐次变换矩阵的最后一列前三个元素表示，即：
位置
=
[
x
y
z
]
=
[
T
0
,
n
(
1
,
4
)
T
0
,
n
(
2
,
4
)
T
0
,
n
(
3
,
4
)
]
\text{位置} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} T_{0,n}(1,4) \\ T_{0,n}(2,4) \\ T_{0,n}(3,4) \end{bmatrix}
位置=
xyz
=
T0,n(1,4)T0,n(2,4)T0,n(3,4)
姿态：末端执行器在空间中的朝向由齐次变换矩阵的旋转部分表示，即前三行前三列的3x3矩阵：
姿态
=
[
T
0
,
n
(
1
,
1
)
T
0
,
n
(
1
,
2
)
T
0
,
n
(
1
,
3
)
T
0
,
n
(
2
,
1
)
T
0
,
n
(
2
,
2
)
T
0
,
n
(
2
,
3
)
T
0
,
n
(
3
,
1
)
T
0
,
n
(
3
,
2
)
T
0
,
n
(
3
,
3
)
]
\text{姿态} = \begin{bmatrix} T_{0,n}(1,1) & T_{0,n}(1,2) & T_{0,n}(1,3) \\ T_{0,n}(2,1) & T_{0,n}(2,2) & T_{0,n}(2,3) \\ T_{0,n}(3,1) & T_{0,n}(3,2) & T_{0,n}(3,3) \end{bmatrix}
姿态=
T0,n(1,1)T0,n(2,1)T0,n(3,1)T0,n(1,2)T0,n(2,2)T0,n(3,2)T0,n(1,3)T0,n(2,3)T0,n(3,3)
🟢 正向运动学的应用 正向运动学在机器人学中的应用非常广泛，主要包括以下几个方面：
机器人控制：给定关节角度，通过正向运动学计算末端执行器的目标位置和姿态，并用于控制算法中。路径规划：通过正向运动学计算不同关节角度下的末端执行器位置，用于生成合理的运动路径。仿真与验证：在机器人仿真软件中，正向运动学用于验证机械臂的运动范围和操作能力。
三、逆向运动学
逆向运动学（Inverse Kinematics, IK）是机器人学中的一个重要问题，它的目标是给定机械臂末端执行器的目标位置和姿态，求解各个关节的角度或位移，使得机械臂能够到达这一目标位姿。
逆向运动学的求解相对复杂，通常没有通用的解析解法，需要根据机械臂的结构和应用场景选择适合的方法。
🟢基本概念：
末端执行器位姿：指机械臂末端执行器在空间中的位置和朝向。通常用一个齐次变换矩阵
T
0
,
n
T_{0,n}
T0,n或位置矢量
p
=
[
x
,
y
,
z
]
T
\mathbf{p} = [x, y, z]^T
p=[x,y,z]T和姿态矩阵
R
=
[
R
x
,
R
y
,
R
z
]
R = [R_{x}, R_{y}, R_{z}]
R=[Rx,Ry,Rz]来表示。关节变量：描述机械臂每个关节的位置或角度。对于旋转关节，关节变量是角度
θ
i
\theta_i
θi，对于滑动关节，关节变量是位移
d
i
d_i
di。冗余度：如果机械臂的自由度（关节数）大于任务要求的自由度，则机械臂具有冗余度。冗余度允许机械臂在满足末端执行器位姿的同时，优化某些指标（如避障、最小能量消耗等）。
3.1 逆向运动学的求解方法：
逆向运动学的求解方法主要分为解析法和数值法两大类。
3.11 解析法（Analytical Method）
解析法试图通过代数运算直接求解机械臂的关节变量。这种方法通常适用于几何结构相对简单的机械臂（如6自由度机械臂），并且要求机械臂的运动学模型有明确的解析解。
步骤：
建立正向运动学模型：根据机械臂的DH参数，建立机械臂的正向运动学模型，得到末端执行器的齐次变换矩阵
T
0
,
n
T_{0,n}
T0,n。分解方程：将
T
0
,
n
T_{0,n}
T0,n分解为位置部分和姿态部分的方程。代数求解：通过代数方法（如三角函数关系、几何方法等）求解出关节变量。
优点：
计算速度快，适合实时控制。解法清晰明确。
缺点：
只适用于几何结构简单的机械臂。可能存在多个解或无解的情况。
3.12 数值法（Numerical Method）
对于复杂的机械臂结构，解析解通常难以获得，因此常用数值法求解逆向运动学。数值法基于迭代优化算法，通过逐步逼近目标位姿来求解关节变量。
常用方法：
牛顿-拉夫森法（Newton-Raphson Method）：
基于正向运动学模型，通过迭代求解误差函数最小化来得到关节变量。误差函数通常定义为末端执行器实际位姿与目标位姿之间的差异。优点：收敛速度快，适用于初始估计接近真实解的情况。缺点：可能收敛到局部最优解，需良好的初始估计。 雅可比矩阵法（Jacobian Method）：
使用雅可比矩阵描述关节变量对末端执行器位姿的敏感性，通过雅可比矩阵的逆来调整关节变量。优点：适用于多自由度机械臂，能处理冗余度问题。缺点：雅可比矩阵可能出现奇异性，导致算法不稳定。 优化法（Optimization Method）：
将逆向运动学问题转换为优化问题，通过优化算法（如遗传算法、粒子群优化等）来求解关节变量。优点：可以处理高度非线性、无解析解的问题。缺点：计算量大，实时性较差。
优点：
能处理复杂的机械臂结构。可处理冗余自由度、避障等多目标问题。
缺点：
计算复杂，迭代过程可能需要大量计算资源。可能收敛慢，或陷入局部最优解。
3.2 应用和挑战
🟢 逆向运动学的应用：
逆向运动学在机器人控制、路径规划和虚拟现实等领域有广泛应用。例如：
机器人抓取：机器人需要根据目标物体的位置，调整自身关节角度，使末端执行器能够准确抓取物体。动画制作：在计算机动画中，逆向运动学用于生成角色的自然运动。医学领域：逆向运动学在康复机器人和手术机器人中用于精确控制机械臂。
🟢逆向运动学的挑战：
多解问题：一个给定的末端执行器位姿可能对应多个关节配置。需要选择最优解或者符合物理限制的解。无解问题：有些情况下，目标位姿超出了机械臂的工作空间，无法找到有效的关节角度。奇异性问题：在某些特殊位姿下，机械臂的雅可比矩阵可能退化，导致求解困难。
四、牛顿法机械臂逆运动学求解详解
4.1 牛顿法
牛顿法的核心思想是利用函数的切线来逼近函数的根。
牛顿法（Newton’s Method），也称为牛顿-拉夫森法（Newton-Raphson Method），是一种求解非线性方程或方程组的迭代方法。它的基本思想是利用函数的导数信息，通过逐步逼近的方法找到方程的根，即找到使得函数值为零的变量值。
🟢牛顿法的基本原理：
假设我们有一个一元非线性方程
f
(
x
)
=
0
f(x) = 0
f(x)=0，牛顿法的核心思想是从一个初始猜测值
x
0
x_0
x0开始，使用函数的切线逼近来找到更接近方程根的值。具体的迭代公式为：
x
n
+
1
=
x
n
−
f
(
x
n
)
f
′
(
x
n
)
x_{n+1} = x_n - \frac{f(x_n)}{f'(x_n)}
xn+1=xn−f′(xn)f(xn)
其中：
x
n
x_n
xn是第
n
n
n次迭代的当前估计值。
f
(
x
n
)
f(x_n)
f(xn)是函数在
x
n
x_n
xn处的值。
f
′
(
x
n
)
f'(x_n)
f′(xn)是函数
f
(
x
)
f(x)
f(x)在
x
n
x_n
xn处的一阶导数。
🟢 牛顿法的几何解释：
在几何上，牛顿法的思想可以解释为：
从当前的猜测点
x
n
x_n
xn出发，计算
f
(
x
n
)
f(x_n)
f(xn)和
f
′
(
x
n
)
f'(x_n)
f′(xn)。通过点
(
x
n
,
f
(
x
n
)
)
(x_n, f(x_n))
(xn,f(xn))，绘制函数
f
(
x
)
f(x)
f(x)在该点的切线。切线与
x
x
x轴的交点给出下一个猜测点
x
n
+
1
x_{n+1}
xn+1。重复以上步骤，直到
x
n
x_n
xn收敛到一个足够精确的值。
🟣 多元方程组的牛顿法：
对于多元非线性方程组
f
(
x
)
=
0
f(\mathbf{x}) = \mathbf{0}
f(x)=0，其中
x
\mathbf{x}
x是包含多个变量的向量，牛顿法可以推广为多元形式。假设
x
=
[
x
1
,
x
2
,
…
,
x
n
]
T
\mathbf{x} = [x_1, x_2, \ldots, x_n]^T
x=[x1,x2,…,xn]T，方程组为：
F
(
x
)
=
[
f
1
(
x
)
f
2
(
x
)
⋮
f
n
(
x
)
]
=
0
\mathbf{F}(\mathbf{x}) = \begin{bmatrix} f_1(\mathbf{x}) \\ f_2(\mathbf{x}) \\ \vdots \\ f_n(\mathbf{x}) \end{bmatrix} = \mathbf{0}
F(x)=
f1(x)f2(x)⋮fn(x)
=0
牛顿法的迭代公式为：
x
n
+
1
=
x
n
−
J
(
x
n
)
−
1
F
(
x
n
)
\mathbf{x}_{n+1} = \mathbf{x}_n - \mathbf{J}(\mathbf{x}_n)^{-1} \mathbf{F}(\mathbf{x}_n)
xn+1=xn−J(xn)−1F(xn)
其中，
J
(
x
n
)
\mathbf{J}(\mathbf{x}_n)
J(xn)是雅可比矩阵（Jacobian Matrix），它是函数向量
F
(
x
)
\mathbf{F}(\mathbf{x})
F(x)对变量向量
x
\mathbf{x}
x的偏导数矩阵：
J
(
x
)
=
[
∂
f
1
∂
x
1
∂
f
1
∂
x
2
⋯
∂
f
1
∂
x
n
∂
f
2
∂
x
1
∂
f
2
∂
x
2
⋯
∂
f
2
∂
x
n
⋮
⋮
⋱
⋮
∂
f
n
∂
x
1
∂
f
n
∂
x
2
⋯
∂
f
n
∂
x
n
]
\mathbf{J}(\mathbf{x}) = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_n}{\partial x_1} & \frac{\partial f_n}{\partial x_2} & \cdots & \frac{\partial f_n}{\partial x_n} \end{bmatrix}
J(x)=
∂x1∂f1∂x1∂f2⋮∂x1∂fn∂x2∂f1∂x2∂f2⋮∂x2∂fn⋯⋯⋱⋯∂xn∂f1∂xn∂f2⋮∂xn∂fn
通过求解雅可比矩阵
J
(
x
n
)
\mathbf{J}(\mathbf{x}_n)
J(xn)的逆矩阵并与函数值相乘，可以得到新的估计向量
x
n
+
1
\mathbf{x}_{n+1}
xn+1。
🟠牛顿法的收敛性：
牛顿法的收敛性主要取决于以下几个因素：
初始值的选择：如果初始值
x
0
x_0
x0足够接近方程的实际根，牛顿法通常收敛非常快，甚至可以达到二次收敛，即误差平方级别的收敛速度。
函数的光滑性：如果函数
f
(
x
)
f(x)
f(x)在解附近是光滑的且导数不为零，则牛顿法更容易收敛。
奇异性问题：如果在迭代过程中
f
′
(
x
n
)
≈
0
f'(x_n) \approx 0
f′(xn)≈0，可能会导致迭代公式中的分母过小，使得迭代发散或收敛缓慢。这种情况下，通常需要调整算法或重新选择初始点。
多解和无解问题：对于多解问题，牛顿法可能会收敛到不同的根，具体取决于初始点的选择。对于无解问题，牛顿法通常会发散。
牛顿法的优缺点
优点：
高效性：在初始估计值接近实际根时，牛顿法具有非常快的收敛速度，通常是二次收敛。广泛适用性：适用于求解广泛的非线性方程和方程组。
缺点：
依赖初始值：如果初始值选择不当，可能会导致算法不收敛。计算量大：对于多元方程组，求解雅可比矩阵及其逆矩阵可能会增加计算复杂度。可能的发散问题：在某些情况下，特别是函数导数接近零时，牛顿法可能会发散。
牛顿法的改进
阻尼牛顿法：通过在每次迭代中引入一个阻尼因子(乘个系数即可)，以控制步长，从而避免迭代发散。拟牛顿法：使用近似的雅可比矩阵来替代真实的雅可比矩阵，从而减少计算量，例如Broyden法。多重根处理：对于具有多重根的方程，可以使用修正的牛顿法来提高收敛速度。
应用
牛顿法在工程和科学计算中有广泛的应用，包括：
机器人学中的逆向运动学求解：用于求解机械臂末端执行器的目标位置和关节角度之间的关系。优化问题：作为优化算法的一部分，用于求解无约束优化问题。根寻找：在数值分析中，用于求解各种非线性方程的根。经济学和物理学模型：用于求解复杂模型中的平衡点或状态变量。
牛顿法是求解非线性问题的一个重要工具，虽然存在一定的局限性，但通过适当的改进和应用策略，仍然可以在许多领域中实现高效、准确的求解。
4.2 牛顿法逆运动学求解算法
🟣🟢类似的，逆运动学已知末端执行器位姿，求解关节角度向量。
对六自 由度机械臂，利用牛顿迭代法求其运动学逆解。
构建非线性方程组：
F
(
θ
)
=
0
,
F
(
θ
)
=
(
f
1
,
f
2
,
⋯
,
f
n
)
T
,
θ
=
(
θ
1
,
θ
2
,
⋯
,
θ
n
)
T
\boldsymbol{F}(\boldsymbol{\theta})=0 ,\quad\boldsymbol{F}(\boldsymbol{\theta})=(f_1,f_2,\cdots,f_n)^\mathrm{T},\quad\boldsymbol{\theta}=(\theta_1,\theta_2,\cdots,\theta_n)^\mathrm{T}
F(θ)=0,F(θ)=(f1,f2,⋯,fn)T,θ=(θ1,θ2,⋯,θn)T
已知最终所需腕部坐标系{6}相对于基坐标系{0}的其次变换矩阵
6
0
T
d
e
s
_6^0T_{\mathrm{des}}
60Tdes（位姿已知嘛），对于每次迭代也可得到一个齐次变换矩阵：
6
0
T
θ
(
θ
1
i
,
θ
2
i
,
θ
3
i
,
θ
4
i
,
θ
5
i
,
θ
6
i
)
{}_{6}^{0}T_{\theta}(\theta_{1}^{i},\theta_{2}^{i},\theta_{3}^{i},\theta_{4}^{i},\theta_{5}^{i},\theta_{6}^{i})
60Tθ(θ1i,θ2i,θ3i,θ4i,θ5i,θ6i)
i为迭代次数。
将2个矩阵的元素一一相减得方程组：
F
(
θ
)
=
(
0
6
T
θ
−
0
6
T
d
e
s
)
=
0
F\left(\theta\right)=\left(\begin{smallmatrix}0\\6\end{smallmatrix}T_{\theta}-\begin{smallmatrix}0\\6\end{smallmatrix}T_{\mathrm{des}}\right)=0
F(θ)=(06Tθ−06Tdes)=0
即：
f
1
(
θ
)
=
6
0
T
θ
(
1
,
4
)
−
6
0
T
d
e
s
(
1
,
4
)
=
0
f
2
(
θ
)
=
6
0
T
θ
(
2
,
4
)
−
6
0
T
d
e
s
(
2
,
4
)
=
0
f
3
(
θ
)
=
6
0
T
θ
(
3
,
4
)
−
6
0
T
d
e
s
(
3
,
4
)
=
0
f
4
(
θ
)
=
6
0
T
θ
(
2
,
3
)
−
6
0
T
d
e
s
(
2
,
3
)
=
0
f
5
(
θ
)
=
6
0
T
θ
(
3
,
3
)
−
6
0
T
d
e
s
(
3
,
2
)
=
0
f
6
(
θ
)
=
6
0
T
θ
(
3
,
2
)
−
6
0
T
d
e
s
(
3
,
2
)
=
0
f
7
(
θ
)
=
6
0
T
θ
(
1
,
1
)
−
6
0
T
d
e
s
(
1
,
1
)
=
0
f
8
(
θ
)
=
6
0
T
θ
(
1
,
2
)
−
6
0
T
d
e
s
(
1
,
2
)
=
0
f
9
(
θ
)
=
6
0
T
θ
(
1
,
3
)
−
6
0
T
d
e
s
(
1
,
3
)
=
0
f
10
(
θ
)
=
6
0
T
θ
(
2
,
1
)
−
6
0
T
d
e
s
(
2
,
1
)
=
0
f
11
(
θ
)
=
6
0
T
θ
(
2
,
2
)
−
6
0
T
d
e
s
(
2
,
2
)
=
0
f
12
(
θ
)
=
6
0
T
θ
(
3
,
1
)
−
6
0
T
d
e
s
(
3
,
1
)
=
0
\begin{aligned}&f_{1}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(1,4)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(1,4)=0\\&f_{2}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(2,4)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(2,4)=0\\&f_{3}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(3,4)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(3,4)=0\\&f_{4}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(2,3)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(2,3)=0\\&f_{5}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(3,3)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(3,2)=0\\&f_{6}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(3,2)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(3,2)=0\\&f_{7}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(1,1)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(1,1)=0\\&f_{8}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(1,2)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(1,2)=0\\&f_{9}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(1,3)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(1,3)=0\\&f_{10}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(2,1)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(2,1)=0\\&f_{11}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(2,2)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(2,2)=0\\&f_{12}(\theta)={}_{6}^{0}\boldsymbol{T}_{\theta}(3,1)-{}_{6}^{0}\boldsymbol{T}_{\mathrm{des}}(3,1)=0\end{aligned}
f1(θ)=60Tθ(1,4)−60Tdes(1,4)=0f2(θ)=60Tθ(2,4)−60Tdes(2,4)=0f3(θ)=60Tθ(3,4)−60Tdes(3,4)=0f4(θ)=60Tθ(2,3)−60Tdes(2,3)=0f5(θ)=60Tθ(3,3)−60Tdes(3,2)=0f6(θ)=60Tθ(3,2)−60Tdes(3,2)=0f7(θ)=60Tθ(1,1)−60Tdes(1,1)=0f8(θ)=60Tθ(1,2)−60Tdes(1,2)=0f9(θ)=60Tθ(1,3)−60Tdes(1,3)=0f10(θ)=60Tθ(2,1)−60Tdes(2,1)=0f11(θ)=60Tθ(2,2)−60Tdes(2,2)=0f12(θ)=60Tθ(3,1)−60Tdes(3,1)=0
式中，
6
0
T
θ
(
a
,
b
)
{}_6^0T_\theta(\mathfrak{a},\mathfrak{b})
60Tθ(a,b)为
6
0
T
θ
_{6}^{0}T_{\theta}
60Tθ齐次变换矩阵第a行第b列元素，
6
0
T
d
e
s
(
a
,
b
)
{}_{6}^{0}T_{\mathrm{des}}(a,b)
60Tdes(a,b)同理。
这个方程组的意思就是：我们迭代得到各个关节的角度值，然后根据求得的关节角度+已知DH参数，可以求得此关节角度对应的末端执行器位姿，然后与目标位姿比较，如果误差不满足要求，就继续改变关节角度值，一直迭代。 我们会给一个各个关节角度的初始值（一个向量嘛），那么如何根据这个初始值，推出下一个角度向量呢？显然，前面的牛顿迭代法已经解决这个问题了。
方程组的雅可比矩阵为：
J
i
(
θ
i
)
=
[
∂
f
1
∂
θ
1
i
∂
f
1
∂
θ
2
i
⋯
∂
f
1
∂
θ
6
i
∂
f
2
∂
θ
1
i
∂
f
2
∂
θ
2
i
⋯
∂
f
2
∂
θ
6
i
⋮
⋮
⋮
∂
f
12
∂
θ
1
i
∂
f
12
∂
θ
2
i
⋯
∂
f
12
∂
θ
6
i
]
J_i\left(\theta^i\right)=\begin{bmatrix}\frac{\partial f_1}{\partial\theta_1^i}&\frac{\partial f_1}{\partial\theta_2^i}&\cdots&\frac{\partial f_1}{\partial\theta_6^i}\\\frac{\partial f_2}{\partial\theta_1^i}&\frac{\partial f_2}{\partial\theta_2^i}&\cdots&\frac{\partial f_2}{\partial\theta_6^i}\\\vdots&\vdots&&\vdots\\\frac{\partial f_{12}}{\partial\theta_1^i}&\frac{\partial f_{12}}{\partial\theta_2^i}&\cdots&\frac{\partial f_{12}}{\partial\theta_6^i}\end{bmatrix}
Ji(θi)=
∂θ1i∂f1∂θ1i∂f2⋮∂θ1i∂f12∂θ2i∂f1∂θ2i∂f2⋮∂θ2i∂f12⋯⋯⋯∂θ6i∂f1∂θ6i∂f2⋮∂θ6i∂f12
求解方程组的牛顿迭代公式为：
θ
i
+
1
=
θ
i
−
J
−
1
F
(
θ
i
)
\theta^{i+1}=\theta^{i}-J^{-1}F(\theta^{i})
θi+1=θi−J−1F(θi)
但雅可比矩阵
1
0
T
_{1}^{0}T
10T是一个12*6的矩阵，只能求其伪逆：
θ
i
+
1
=
θ
i
−
(
J
i
T
J
i
)
−
1
J
i
T
(
θ
i
)
F
(
θ
i
)
\theta^{i+1}=\theta^i-(J_i^{\mathrm{T}}J_i)^{-1}J_i^{\mathrm{T}}(\theta^i)F(\theta^i)
θi+1=θi−(JiTJi)−1JiT(θi)F(θi)
利用此式，不断迭代，可求解6自由度机械臂逆运动学。
🟢有人要问了：6自由度机械臂的雅可比矩阵不是6*6的嘛？ 那是你还没理解牛顿法的核心思想、雅可比矩阵的含义，以及前面这个方程组的含义，仔细看一遍就明白了。
4.3 代码（matlab）
4.31 正向
带公式计算就行了：
function T = forward_kinematics(DH_params)
% DH_params: DH参数矩阵，每行代表一个连杆的DH参数
% T: 末端执行器的变换矩阵
% 初始化变换矩阵为单位矩阵
T = eye(4);
% 遍历所有的DH参数
for i = 1:size(DH_params, 1)
% alpha = DH_params(i, 2) * pi / 180; % 将角度转为弧度
alpha = deg2rad(DH_params(i,2));
a = DH_params(i, 3);
d = DH_params(i, 4);
% theta = DH_params(i, 5) * pi / 180; % 将角度转为弧度
theta = deg2rad(DH_params(i,5));
% 计算当前连杆的变换矩阵
A_i = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta);
sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta);
0 sin(alpha) cos(alpha) d;
0 0 0 1];
% 累乘到总变换矩阵T
T = T * A_i;
end
end
注意DH参数与代码对应：
% 杆件编号, 杆件扭角 (alpha), 杆件长度 (a), 关节距离 (d), 关节角 (theta)
DH_params = [
1, -45, 0, 560, 10;
2, 45, 0, 450, 0;
3, 45, 0, 410, 10;
4, -45, 0, 380, -20;
5, 45, 0, 360,0;
6, 0, 0, 210, 40
];
T = forward_kinematics(DH_params);
4.32 逆向
根据前面的详细介绍你应该能写代码了，我不提供免费的：https://mbd.pub/o/bread/ZpqZkp5y
核心：
% 牛顿法的核心思想是利用函数的切线来逼近函数的根。
for iter = 1:max_iter
% 计算当前的 F(theta) 和雅可比矩阵
F = compute_F(forward_kinematics(q, DH_params),T_des);
J = compute_jacobian(q, DH_params,T_des);
% 打印调试信息
% fprintf('Iteration %d: Norm of F = %f\n', iter, norm(F));
% 计算雅可比矩阵的伪逆
J_pseudo_inv = pinv(J);
% 更新关节角度
q = q - J_pseudo_inv * F;
% 限制关节角在[-90, 90]度范围内
q = mod(q + 90, 2*90) - 90;
% 检查收敛性
if norm(F) < tol
break;
end
end
测试：
（1）求得关节角和实际值相同：
（2）得到与实际值不同的角度向量：因为同一位姿，可能有多种关节角度组合。当然也有可能无解。
将DH参数中的关节角替换为求得的角度，再计算末端执行器位姿，即可验证。
(买了代码的）调参数的时候调图中的：你们对比位姿即可，目的是达到那个位姿，因为得到的关节角度很有可能与你DH参数中那个不同。
下面是一个7自由度的参数（DH参数照着换成7自由度的即可）。我上传的那个里面是6自由度的。
五、齐次变换矩阵推导
齐次变换矩阵是描述从一个坐标系到另一个坐标系的位姿（位置和姿态）变化的重要工具。在机器人学中，齐次变换矩阵通常用于将机械臂的一个关节坐标系转换到下一个关节坐标系。推导齐次变换矩阵涉及到平移和旋转两种基本变换。
5.1 基本变换
绕
Z
Z
Z轴的旋转变换： 设一个点在坐标系
O
i
X
i
Y
i
Z
i
O_iX_iY_iZ_i
OiXiYiZi中的坐标为
(
x
i
,
y
i
,
z
i
)
(x_i, y_i, z_i)
(xi,yi,zi)，经过绕
Z
Z
Z轴旋转角度
θ
i
\theta_i
θi后，在新的坐标系
O
i
+
1
X
i
+
1
Y
i
+
1
Z
i
+
1
O_{i+1}X_{i+1}Y_{i+1}Z_{i+1}
Oi+1Xi+1Yi+1Zi+1中的坐标为
(
x
i
+
1
,
y
i
+
1
,
z
i
+
1
)
(x_{i+1}, y_{i+1}, z_{i+1})
(xi+1,yi+1,zi+1)。旋转变换矩阵为：
R
z
(
θ
i
)
=
[
cos
θ
i
−
sin
θ
i
0
sin
θ
i
cos
θ
i
0
0
0
1
]
R_z(\theta_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 \\ \sin\theta_i & \cos\theta_i & 0 \\ 0 & 0 & 1 \end{bmatrix}
Rz(θi)=
cosθisinθi0−sinθicosθi0001
沿
Z
Z
Z轴的平移变换： 沿
Z
Z
Z轴的平移变换将一个点沿
Z
Z
Z轴移动距离
d
i
d_i
di，变换矩阵为：
T
z
(
d
i
)
=
[
1
0
0
0
0
1
0
0
0
0
1
d
i
0
0
0
1
]
T_z(d_i) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}
Tz(di)=
10000100001000di1
沿
X
X
X轴的平移变换： 沿
X
X
X轴的平移变换将一个点沿
X
X
X轴移动距离
a
i
a_i
ai，变换矩阵为：
T
x
(
a
i
)
=
[
1
0
0
a
i
0
1
0
0
0
0
1
0
0
0
0
1
]
T_x(a_i) = \begin{bmatrix} 1 & 0 & 0 & a_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
Tx(ai)=
100001000010ai001
绕
X
X
X轴的旋转变换： 设一个点绕
X
X
X轴旋转角度
α
i
\alpha_i
αi后，变换矩阵为：
R
x
(
α
i
)
=
[
1
0
0
0
0
cos
α
i
−
sin
α
i
0
0
sin
α
i
cos
α
i
0
0
0
0
1
]
R_x(\alpha_i) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\alpha_i & -\sin\alpha_i & 0 \\ 0 & \sin\alpha_i & \cos\alpha_i & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
Rx(αi)=
10000cosαisinαi00−sinαicosαi00001
5.2 齐次变换矩阵的推导
为了将一个坐标系
O
i
X
i
Y
i
Z
i
O_iX_iY_iZ_i
OiXiYiZi变换到下一个坐标系
O
i
+
1
X
i
+
1
Y
i
+
1
Z
i
+
1
O_{i+1}X_{i+1}Y_{i+1}Z_{i+1}
Oi+1Xi+1Yi+1Zi+1，需要依次进行以下变换：
绕
Z
i
Z_i
Zi轴旋转
θ
i
\theta_i
θi：引入关节角度
θ
i
\theta_i
θi，得到变换矩阵
R
z
(
θ
i
)
R_z(\theta_i)
Rz(θi)：
R
z
(
θ
i
)
=
[
cos
θ
i
−
sin
θ
i
0
0
sin
θ
i
cos
θ
i
0
0
0
0
1
0
0
0
0
1
]
R_z(\theta_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i & 0 & 0 \\ \sin\theta_i & \cos\theta_i & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
Rz(θi)=
cosθisinθi00−sinθicosθi0000100001
沿
Z
i
Z_i
Zi轴平移
d
i
d_i
di：沿
Z
i
Z_i
Zi轴平移
d
i
d_i
di，得到平移矩阵
T
z
(
d
i
)
T_z(d_i)
Tz(di)：
T
z
(
d
i
)
=
[
1
0
0
0
0
1
0
0
0
0
1
d
i
0
0
0
1
]
T_z(d_i) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}
Tz(di)=
10000100001000di1
沿
X
i
X_i
Xi轴平移
a
i
a_i
ai：沿
X
i
X_i
Xi轴平移
a
i
a_i
ai，得到平移矩阵
T
x
(
a
i
)
T_x(a_i)
Tx(ai)：
T
x
(
a
i
)
=
[
1
0
0
a
i
0
1
0
0
0
0
1
0
0
0
0
1
]
T_x(a_i) = \begin{bmatrix} 1 & 0 & 0 & a_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
Tx(ai)=
100001000010ai001
绕
X
i
X_i
Xi轴旋转
α
i
\alpha_i
αi：引入扭角
α
i
\alpha_i
αi，绕
X
i
X_i
Xi轴旋转
α
i
\alpha_i
αi，得到变换矩阵
R
x
(
α
i
)
R_x(\alpha_i)
Rx(αi)：
R
x
(
α
i
)
=
[
1
0
0
0
0
cos
α
i
−
sin
α
i
0
0
sin
α
i
cos
α
i
0
0
0
0
1
]
R_x(\alpha_i) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\alpha_i & -\sin\alpha_i & 0 \\ 0 & \sin\alpha_i & \cos\alpha_i & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
Rx(αi)=
10000cosαisinαi00−sinαicosαi00001
5.3 齐次变换矩阵的合成
现在，将上述所有变换组合在一起，得到齐次变换矩阵
T
i
T_i
Ti：
T
i
=
T
z
(
d
i
)
⋅
R
z
(
θ
i
)
⋅
T
x
(
a
i
)
⋅
R
x
(
α
i
)
T_i = T_z(d_i) \cdot R_z(\theta_i) \cdot T_x(a_i) \cdot R_x(\alpha_i)
Ti=Tz(di)⋅Rz(θi)⋅Tx(ai)⋅Rx(αi)
将这些矩阵逐项相乘，得到齐次变换矩阵
T
i
T_i
Ti的最终表达式：
T
i
=
[
cos
θ
i
−
sin
θ
i
cos
α
i
sin
θ
i
sin
α
i
a
i
cos
θ
i
sin
θ
i
cos
θ
i
cos
α
i
−
cos
θ
i
sin
α
i
a
i
sin
θ
i
0
sin
α
i
cos
α
i
d
i
0
0
0
1
]
T_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}
Ti=
cosθisinθi00−sinθicosαicosθicosαisinαi0sinθisinαi−cosθisinαicosαi0aicosθiaisinθidi1
🟢 解释
旋转部分：由
3
×
3
3\times3
3×3的子矩阵构成，描述了坐标系之间的旋转关系。平移部分：由最后一列前三个元素组成，描述了坐标系之间的平移关系。齐次部分：最后一行为
[
0
0
0
1
]
[0\ 0\ 0\ 1]
[0 0 0 1]，确保齐次变换矩阵的性质。
该矩阵用于描述从关节
i
−
1
i-1
i−1的坐标系到关节
i
i
i的坐标系的变换，通过逐级相乘，可以将基座坐标系下的位姿变换到末端执行器坐标系。
六、牛顿法详细推导
牛顿法的推导基于泰勒展开与几何切线逼近的思想，具体推导步骤如下：
目标：求解非线性方程
f
(
x
)
=
0
f(x) = 0
f(x)=0 的根，即寻找使
f
(
x
)
=
0
f(x) = 0
f(x)=0 的点
x
x
x。
切线逼近： 假设有一个当前的近似值
x
n
x_n
xn，并希望通过迭代找到更接近根的值
x
n
+
1
x_{n+1}
xn+1。
在
x
n
x_n
xn 处，通过泰勒展开的思想，用
f
(
x
)
f(x)
f(x) 的线性近似来替代非线性函数
f
(
x
)
f(x)
f(x)。这意味着用
x
n
x_n
xn 处的切线来逼近函数。
对
f
(
x
)
f(x)
f(x) 在
x
n
x_n
xn 点进行一阶泰勒展开：
f
(
x
)
≈
f
(
x
n
)
+
f
′
(
x
n
)
(
x
−
x
n
)
f(x) \approx f(x_n) + f'(x_n)(x - x_n)
f(x)≈f(xn)+f′(xn)(x−xn) 其中
f
′
(
x
n
)
f'(x_n)
f′(xn) 是
f
(
x
)
f(x)
f(x) 在
x
n
x_n
xn 处的一阶导数。
切线方程： 切线的表达式可以写作：
y
=
f
(
x
n
)
+
f
′
(
x
n
)
(
x
−
x
n
)
y = f(x_n) + f'(x_n)(x - x_n)
y=f(xn)+f′(xn)(x−xn) 现在我们希望找到切线与
x
x
x 轴的交点，这意味着要让
y
=
0
y = 0
y=0。所以我们有：
0
=
f
(
x
n
)
+
f
′
(
x
n
)
(
x
n
+
1
−
x
n
)
0 = f(x_n) + f'(x_n)(x_{n+1} - x_n)
0=f(xn)+f′(xn)(xn+1−xn)
求解
x
n
+
1
x_{n+1}
xn+1： 解这个方程求
x
n
+
1
x_{n+1}
xn+1：
f
(
x
n
)
+
f
′
(
x
n
)
(
x
n
+
1
−
x
n
)
=
0
f(x_n) + f'(x_n)(x_{n+1} - x_n) = 0
f(xn)+f′(xn)(xn+1−xn)=0
f
′
(
x
n
)
(
x
n
+
1
−
x
n
)
=
−
f
(
x
n
)
f'(x_n)(x_{n+1} - x_n) = -f(x_n)
f′(xn)(xn+1−xn)=−f(xn)
x
n
+
1
=
x
n
−
f
(
x
n
)
f
′
(
x
n
)
x_{n+1} = x_n - \frac{f(x_n)}{f'(x_n)}
xn+1=xn−f′(xn)f(xn)
这就是牛顿法迭代公式的推导过程。通过不断迭代更新
x
n
x_n
xn，使其逐渐接近方程
f
(
x
)
=
0
f(x) = 0
f(x)=0 的解。
参考文献： [1] 张栩曼，张中哲，王燕波，杨涛，邓涛.基于空间六自由度机械臂的逆运动学数值解法[J] 导弹与航天运载技术,2016,345(3):81-84