matlab六自由度运动平台代码（【Matlab 六自由度机器人】运动学逆解（附MATLAB机器人逆解代码））
往期回顾
【汇总】
相关资源：
【Matlab 六自由度机器人】系列文章汇总
\fcolorbox{green}{aqua}{【Matlab 六自由度机器人】系列文章汇总 }
【Matlab六自由度机器人】系列文章汇总【主线】
定义标准型及改进型D-H参数，建立机器人模型。 运动学正解 基于蒙特卡罗方法(Monte Carlo Method)构建机器人工作空间【补充说明】
关于灵活工作空间与可达工作空间的理解 关于改进型D-H参数(modified Denavit-Hartenberg)的详细建立步骤 关于旋转的参数化（欧拉角、姿态角、四元数）的相关问题 关于双变量函数atan2(x,y)的解释 关于机器人运动学反解的有关问题前言
本篇介绍机器人逆运动学及逆解的求解方法，首先介绍如何理解逆向运动学，并利用D-H参数及正向运动学的齐次变换矩阵对机器人运动学逆解进行求解。
在博主读研期间，刚开始对机器人运动学逆解不甚了解，但在经历一次自己完全去钻研、尝试，独立写出属于自己的运动学逆解后，对机器人的结构有更加深刻的理解。因此希望初学机器人的朋友们也能够尝试独立去编写、尝试出自己的逆解代码，我相信，当你看到逆解解出的角度完全符合预期，也能够产生相当的成就感。
下面是运动学逆解的正文内容，主要讲述运动学逆解的方式和公式推算，最后进行代码的实现。正文
一、运动学逆解
首先我们已经建立起了一个六自由度的机器人，在 D-H 参数 及机器人的 期望位姿 已知的情况下，求解机器人满足该期望位姿的各个关节变量，这种求解过程叫做机器人运动学逆解过程。求解机器人的逆运动学解法分为封闭解法和数值解法两种，其中封闭解法是指具有解析形式的解法，其求解速度比数值解法更快。
本文所研究的六自由度机器人，是具有六个旋转关节的串联型六自由度机器人，即RRR型的机器人。六自由度机器人的逆解封闭解法具体又可分为代数解法和几何解法，代数解法推导解析解的过程复杂，运算量较大，几何解法根据直观的几何关系获得关节变量的解析表达式，计算量较小。
本文主要讲解两种机器人运动学逆解的两种方法： 1. Pieper法 2. 常规法
1. Pieper 法
根据Pieper’s Solution的研究，如果六自由度机器人具有三个连续的轴交在同一点的情况，则手臂具有解析解。即六个关节均为旋转关节且后三个关节轴相交于一点的串联型机器人，其逆解具有封闭解。可以通过如下代数解法求解逆运动学的封闭解，该解法称为“三轴相交的 Pieper 解法”。下面进行各轴转角角度的公式推导。
我们知道，当后3个关节轴相交时，连杆坐标系{
4
}
,
{
5
}
,
{
6
}
\{4\},\{5\},\{6\}
{4},{5},{6} 的原点位于该交点，该交点在基坐标系{
}
\{0\}
{0}中的坐标如下：
P
4
=
T
1
2
T
3
2
T
3
P
4
=
[
x
y
z
1
]
T
{ }^{0} \boldsymbol{P}_{4}={ }^{0} \boldsymbol{T}^{1}{ }_{2} \boldsymbol{T}_{3}^{2} \boldsymbol{T}^{3} \boldsymbol{P}_{4}=\left[\begin{array}{llll} x & y & z & 1 \end{array}\right]^{\mathrm{T}}
0P4=0T12T32T3P4=[xyz1]T对于齐次变换矩阵的变换通式的理解请参阅【Matlab 六自由度机器人】运动学正解（附MATLAB机器人正解完整代码）
由改进D-H参数的连杆齐次变换矩阵式，应用齐次变换矩阵的变换通式i
−
1
T
i
=
[
c
o
s
θ
i
−
s
i
n
θ
i
a
i
−
1
s
i
n
θ
i
c
o
s
α
i
−
1
c
o
s
θ
i
c
o
s
α
i
−
1
−
s
i
n
α
i
−
1
−
s
i
n
α
i
−
1
d
i
s
i
n
θ
i
s
i
n
α
i
−
1
c
o
s
θ
i
s
i
n
α
i
−
1
c
o
s
α
i
−
1
c
o
s
α
i
−
1
d
i
1
]
^{i-1}T_i =\left[ \begin{matrix} cosθ_i&-sinθ_i&0&a_{i-1} \\ sinθ_icosα_{i-1}&cosθ_icosα_{i-1}&-sinα_{i-1}&-sinα_{i-1}d_{i} \\ sinθ_isinα_{i-1}&cosθ_isinα_{i-1}&cosα_{i-1}&cosα_{i-1}d_{i} \\ 0&0&0&1 \end{matrix} \right]
i−1Ti=⎣⎢⎢⎡cosθisinθicosαi−1sinθisinαi−10−sinθicosθicosαi−1cosθisinαi−100−sinαi−1cosαi−10ai−1−sinαi−1dicosαi−1di1⎦⎥⎥⎤ 可知当i
=
4
i=4
i=4时，
P
4
=
1
T
2
1
T
(
3
2
T
3
P
4
)
=
1
T
2
1
T
(
3
2
T
[
a
3
−
d
4
s
α
3
d
4
c
α
3
1
]
)
=
1
T
2
1
T
[
f
1
(
θ
3
)
f
2
(
θ
3
)
f
3
(
θ
3
)
1
]
{ }^{0} \boldsymbol{P}_{4}={ }_{1}^{0} \boldsymbol{T}{ }_{2}^{1} \boldsymbol{T}\left({ }_{3}^{2} \boldsymbol{T}^{3} \boldsymbol{P}_{4}\right)={ }_{1}^{0} \boldsymbol{T}{ }_{2}^{1} \boldsymbol{T}\left({ }_{3}^{2} \boldsymbol{T}\left[\begin{array}{c} a_{3} \\ -d_{4} s \alpha_{3} \\ d_{4} c \alpha_{3} \\ 1 \end{array}\right]\right)={ }_{1}^{0} \boldsymbol{T}{ }_{2}^{1} \boldsymbol{T}\left[\begin{array}{c} f_{1}\left(\theta_{3}\right) \\ f_{2}\left(\theta_{3}\right) \\ f_{3}\left(\theta_{3}\right) \\ 1 \end{array}\right]
0P4=10T21T(32T3P4)=10T21T⎝⎜⎜⎛32T⎣⎢⎢⎡a3−d4sα3d4cα31⎦⎥⎥⎤⎠⎟⎟⎞=10T21T⎣⎢⎢⎡f1(θ3)f2(θ3)f3(θ3)1⎦⎥⎥⎤ 应用齐次变换矩阵对于3
2
T
{ }_{3}^{2} \boldsymbol{T}
32T 可以得到f
i
f_{i}
fi的表达式:
f
1
=
a
3
c
3
+
d
4
s
α
3
s
3
+
a
2
f
2
=
a
3
c
α
2
s
3
−
d
4
s
α
3
c
α
2
c
3
−
d
4
s
α
2
c
α
3
−
d
3
s
α
2
f
3
=
a
3
s
α
2
s
3
−
d
4
s
α
3
s
α
2
c
3
+
d
4
c
α
2
c
α
3
+
d
3
c
α
2
\begin{aligned} &f_{1}=a_{3} c_{3}+d_{4} s \alpha_{3} s_{3}+a_{2} \\ &f_{2}=a_{3} c \alpha_{2} s_{3}-d_{4} s \alpha_{3} c \alpha_{2} c_{3}-d_{4} s \alpha_{2} c \alpha_{3}-d_{3} s \alpha_{2} \\ &f_{3}=a_{3} s \alpha_{2} s_{3}-d_{4} s \alpha_{3} s \alpha_{2} c_{3}+d_{4} c \alpha_{2} c \alpha_{3}+d_{3} c \alpha_{2} \end{aligned}
f1=a3c3+d4sα3s3+a2f2=a3cα2s3−d4sα3cα2c3−d4sα2cα3−d3sα2f3=a3sα2s3−d4sα3sα2c3+d4cα2cα3+d3cα2 对1
T
{ }_{1}^{0} \boldsymbol{T}
10T 和2
1
T
{ }_{2}^{1} \boldsymbol{T}
21T应用齐次变换矩阵得到
P
4
=
[
c
1
g
1
−
s
1
g
2
s
1
g
1
+
c
1
g
2
g
3
1
]
T
{ }^{0} \boldsymbol{P}_{4}=\left[\begin{array}{llll} c_{1} g_{1}-s_{1} g_{2} & s_{1} g_{1}+c_{1} g_{2} & g_{3} & 1 \end{array}\right]^{\mathrm{T}}
0P4=[c1g1−s1g2s1g1+c1g2g3