【Matlab 六自由度机器人】求运动学逆解

往期回顾
前言
正文
- 一、运动学逆解
- - 1. Pieper 法
  - 2. 《机器人学》常规求解
- 二、代码实现
- - 1. 机器人模型的建立
  - 2. Pieper 法求六自由度机器人逆解
  - 3. 常规法求六自由度机器人逆解
总结
参考文献

往期回顾

【汇总】

【主线】

定义标准型及改进型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\}{0} 中的坐标如下：
0P4=0T12T32T3P4=[xyz1]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−1Ti=[cosθi−sinθi0ai−1sinθicosαi−1cosθicosαi−1−sinαi−1−sinαi−1disinθisinαi−1cosθisinαi−1cosαi−1cosαi−1di0001]^{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=4i=4i=4 时，
0P4=10T21T(32T3P4)=10T21T(32T[a3−d4sα3d4cα31])=10T21T[f1(θ3)f2(θ3)f3(θ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⎦⎥⎥⎤
应用齐次变换矩阵对于 32T{ }_{3}^{2} \boldsymbol{T}32T 可以得到 fif_{i}fi 的表达式:
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\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
对 10T{ }_{1}^{0} \boldsymbol{T}10T 和 21T{ }_{2}^{1} \boldsymbol{T}21T 应用齐次变换矩阵得到
0P4=[c1g1−s1g2s1g1+c1g2g31]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+c1g2g31]T
g1、g2、g3g_{1} 、 g_{2} 、 g_{3}g1、g2、g3 计算过程如下:
g1=c2f1−s2f2+a1g2=s2cα1f1+c2cα1f2−sα1f3−d2sα1g3=s2sα1f1+c2sα1f2+cα1f3+d2cα1\begin{aligned} &g_{1}=c_{2} f_{1}-s_{2} f_{2}+a_{1} \\ &g_{2}=s_{2} c \alpha_{1} f_{1}+c_{2} c \alpha_{1} f_{2}-s \alpha_{1} f_{3}-d_{2} s \alpha_{1} \\ &g_{3}=s_{2} s \alpha_{1} f_{1}+c_{2} s \alpha_{1} f_{2}+c \alpha_{1} f_{3}+d_{2} c \alpha_{1} \end{aligned} g1=c2f1−s2f2+a1g2=s2cα1f1+c2cα1f2−sα1f3−d2sα1g3=s2sα1f1+c2sα1f2+cα1f3+d2cα1
令 r=x2+y2+z2r=x^{2}+y^{2}+z^{2}r=x2+y2+z2，由上式可知
r=g12+g22+g22=f12+f22+f22+a12+d22+2d2f3+2a1(c2f1−s2f2)\begin{aligned} r &=g_{1}^{2}+g_{2}^{2}+g_{2}^{2} \\ &=f_{1}^{2}+f_{2}^{2}+f_{2}^{2}+a_{1}^{2}+d_{2}^{2}+2 d_{2} f_{3}+2 a_{1}\left(c_{2} f_{1}-s_{2} f_{2}\right) \end{aligned} r=g12+g22+g22=f12+f22+f22+a12+d22+2d2f3+2a1(c2f1−s2f2)
由上式写出 Z\mathrm{Z}Z 轴分量的方程，可得系统的两个方程:
r=(k1c2+k2s2)2a1+k3z=(k1s2−k2c2)sα2+k4\begin{aligned} &r=\left(k_{1} c_{2}+k_{2} s_{2}\right) 2 a_{1}+k_{3} \\ &z=\left(k_{1} s_{2}-k_{2} c_{2}\right) s \alpha_{2}+k_{4} \end{aligned} r=(k1c2+k2s2)2a1+k3z=(k1s2−k2c2)sα2+k4

对于k1、k2、k3、k4k_{1} 、 k_{2} 、 k_{3} 、 k_{4}k1、k2、k3、k4，其计算如下:
k1=f1k2=−f2k3=f12+f22+f32+a12+d22+2d2f3k4=f3cα1+d2cα1\begin{aligned} &k_{1}=f_{1} \\ &k_{2}=-f_{2} \\ &k_{3}=f_{1}^{2}+f_{2}^{2}+f_{3}^{2}+a_{1}^{2}+d_{2}^{2}+2 d_{2} f_{3} \\ &k_{4}=f_{3} c \alpha_{1}+d_{2} c \alpha_{1} \end{aligned} k1=f1k2=−f2k3=f12+f22+f32+a12+d22+2d2f3k4=f3cα1+d2cα1
之后求解 θ3\theta_{3}θ3，分三种情况:
(1) 若 a1=0a_{1}=0a1=0，则 r=k3r=k_{3}r=k3，此时 rrr 已知。 k3k_{3}k3 的右边是关于 θ3\theta_{3}θ3 的函数。代入下式后，由包含 tan⁡(θ3/2)\tan \left(\theta_{3} / 2\right)tan(θ3/2) 的一元二次方程可以解出 θ3\theta_{3}θ3;
u=tan⁡θ2,cos⁡θ=1−u21+u2,sin⁡θ=2u1+u2u=\tan \frac{\theta}{2}, \cos \theta=\frac{1-u^{2}}{1+u^{2}}, \sin \theta=\frac{2 u}{1+u^{2}} u=tan2θ,cosθ=1+u21−u2,sinθ=1+u22u

(2) 若 sα1=0s \alpha_{1}=0sα1=0，则 z=k4z=k_{4}z=k4，此时 zzz 已知。代入上式后，利用上面的一元二次方程解出θ3\theta_{3}θ3

(3) 否则，从式r=(k1c2+k2s2)2a1+k3z=(k1s2−k2c2)sα2+k4\begin{aligned} &r=\left(k_{1} c_{2}+k_{2} s_{2}\right) 2 a_{1}+k_{3} \\ &z=\left(k_{1} s_{2}-k_{2} c_{2}\right) s \alpha_{2}+k_{4} \end{aligned}r=(k1c2+k2s2)2a1+k3z=(k1s2−k2c2)sα2+k4中消去 s2s_{2}s2 和 c2c_{2}c2，得到
(r−k3)24a12+(z−k4)2s2α1=k12+k22\frac{\left(r-k_{3}\right)^{2}}{4 a_{1}^{2}}+\frac{\left(z-k_{4}\right)^{2}}{s^{2} \alpha_{1}}=k_{1}^{2}+k_{2}^{2} 4a12(r−k3)2+s2α1(z−k4)2=k12+k22
代入u=tan⁡θ2,cos⁡θ=1−u21+u2,sin⁡θ=2u1+u2u=\tan \frac{\theta}{2}, \cos \theta=\frac{1-u^{2}}{1+u^{2}}, \sin \theta=\frac{2 u}{1+u^{2}}u=tan2θ,cosθ=1+u21−u2,sinθ=1+u22u解出 θ3\theta_{3}θ3 后，可得到一个一元四次方程，由此可解出 θ3\theta_{3}θ3 。解出 θ3\theta_{3}θ3 后，再解出 θ2\theta_{2}θ2，再根据0P4=[c1g1−s1g2s1g1+c1g2g31]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+c1g2g31]T解出 θ1\theta_{1}θ1 。

Pieper 解法求解出了前三轴的关节变量，该解通常具有4组解或8组。由于后三轴的关节角只影响工具坐标系的方位，因此可以通过欧拉角解法求出后三轴的关节变量。

六自由度机器人工具坐标系的姿态，可以使用一组欧拉角(ψ,θ,φ)(\psi, \theta, \varphi)(ψ,θ,φ) 来表示，该转角序列表示绕 xxx 轴，yyy 轴和 zzz 轴的旋转序列：先绕 zzz 轴旋转 ψ\psiψ 角，再绕新坐标系的 yyy 轴旋转 θ\thetaθ 角，最后绕新坐标系的 zzz 轴旋转 φ\varphiφ 角。欧拉角变换 Euler (ψ,θ,φ)(\psi, \theta, \varphi)(ψ,θ,φ) 可以通过三个旋转矩阵相乘表示
Euler⁡(ψ,θ,φ)=Rot⁡(z,ψ)Rot⁡(y,θ)Rot⁡(z,φ)=[cψcθcφ−sψsφ−cψcθsφ−sψcφcψsθ0sψcθcφ+cψsφ−sψcθsφ+cψcφsψsθ0−sθcφsθsφcθ00001]\begin{aligned} \operatorname{Euler}(\psi, \theta, \varphi) &=\operatorname{Rot}(z, \psi) \operatorname{Rot}(y, \theta) \operatorname{Rot}(z, \varphi) \\ &=\left[\begin{array}{cccc} c \psi c \theta c \varphi-s \psi s \varphi & -c \psi c \theta s \varphi-s \psi c \varphi & c \psi s \theta & 0 \\ s \psi c \theta c \varphi+c \psi s \varphi & -s \psi c \theta s \varphi+c \psi c \varphi & s \psi s \theta & 0 \\ -s \theta c \varphi & s \theta s \varphi & c \theta & 0 \\ 0 & 0 & 0 & 1 \end{array}\right] \end{aligned} Euler(ψ,θ,φ)=Rot(z,ψ)Rot(y,θ)Rot(z,φ)=⎣⎢⎢⎡cψcθcφ−sψsφsψcθcφ+cψsφ−sθcφ0−cψcθsφ−sψcφ−sψcθsφ+cψcφsθsφ0cψsθsψsθcθ00001⎦⎥⎥⎤
如果已知一个表示任意旋转的齐次变换, 即已知姿态矩阵 R\boldsymbol{R}R
R=[nxoxaxnyoyaynzozaz]\boldsymbol{R}=\left[\begin{array}{lll} n_{x} & o_{x} & a_{x} \\ n_{y} & o_{y} & a_{y} \\ n_{z} & o_{z} & a_{z} \end{array}\right] R=⎣⎡nxnynzoxoyozaxayaz⎦⎤
则根据姿态矩阵 R\boldsymbol{R}R 可求出其对应的欧拉角:
ψ=atan⁡2(ax,ay)+180∘θ=atan⁡2(cψax+sψay,az)φ=atan⁡2(−sψnx+cψny,−sψox+cψoy)\begin{aligned} &\psi=\operatorname{atan} 2\left(a_{x}, a_{y}\right)+180^{\circ} \\ &\theta=\operatorname{atan} 2\left(c \psi a_{x}+s \psi a_{y}, a_{z}\right) \\ &\varphi=\operatorname{atan} 2\left(-s \psi n_{x}+c \psi n_{y},-s \psi o_{x}+c \psi o_{y}\right) \end{aligned} ψ=atan2(ax,ay)+180∘θ=atan2(cψax+sψay,az)φ=atan2(−sψnx+cψny,−sψox+cψoy)对于6R型六自由度机器人，已经求得前三轴的关节变量，即可求出齐次变换矩阵 30T{ }_{3}^{0} \boldsymbol{T}30T
30T=10T(θ1)21T(θ2)32T(θ3){ }_{3}^{0} \boldsymbol{T}={ }_{1}^{0} \boldsymbol{T}\left(\theta_{1}\right){ }_{2}^{1} \boldsymbol{T}\left(\theta_{2}\right){ }_{3}^{2} \boldsymbol{T}\left(\theta_{3}\right) 30T=10T(θ1)21T(θ2)32T(θ3)假设建立欧拉角 Euler (ψ,θ,φ)(\psi, \theta, \varphi)(ψ,θ,φ) 的坐标系为 {E}\{\mathrm{E}\}{E}，则从基座标系 {0}\{0\}{0} 到坐标系 {E}\{E\}{E} 的齐次变换矩阵为:
E0T=30TRot⁡(x,α3){ }_{E}^{0} \boldsymbol{T}={ }_{3}^{0} \boldsymbol{T} \operatorname{Rot}\left(x, \alpha_{3}\right) E0T=30TRot(x,α3)式中，α3\alpha_{3}α3 一表示改进 DH参数表中的第四轴的连杆扭角。欧拉角表示的资态矩阵的计算方法如式(2-21)所示:
R=E0R∗6TR\boldsymbol{R}={ }_{\mathrm{E}}^{{ }_{\mathrm{0}}} \boldsymbol{R}*{ }_{6}^{\mathrm{T}} \boldsymbol{R} R=E0R∗6TR其中E0RT{ }_{E}^{0} R^{\mathrm{T}}E0RT 一表示齐次变换矩阵 E0T{ }_{E}^{0} \boldsymbol{T}E0T 中的旋转矩阵的转置；60R{ }_{6}^{0} R60R 一表示齐次变换矩阵 60T{ }_{6}^{0} \boldsymbol{T}60T 中的旋转矩阵。

求出欧拉角姿态矩阵后，可通过ψ=atan⁡2(ax,ay)+180∘θ=atan⁡2(cψax+sψay,az)φ=atan⁡2(−sψnx+cψny,−sψox+cψoy)\begin{aligned} &\psi=\operatorname{atan} 2\left(a_{x}, a_{y}\right)+180^{\circ} \\ &\theta=\operatorname{atan} 2\left(c \psi a_{x}+s \psi a_{y}, a_{z}\right) \\ &\varphi=\operatorname{atan} 2\left(-s \psi n_{x}+c \psi n_{y},-s \psi o_{x}+c \psi o_{y}\right) \end{aligned}ψ=atan2(ax,ay)+180∘θ=atan2(cψax+sψay,az)φ=atan2(−sψnx+cψny,−sψox+cψoy)求出相应的欧拉角。所求得的欧拉角与后三轴的关苖变量的关系为:
θ4=ψ,θ5=−θ,θ6=φ\theta_{4}=\psi, \theta_{5}=-\theta, \theta_{6}=\varphi θ4=ψ,θ5=−θ,θ6=φ
由于机器人腕关节翻转，可以得到后三轴的另外一组解:
θ4′=θ4+180∘,θ5′=−θ5,θ6′=θ6+180∘\theta_{4}^{\prime}=\theta_{4}+180^{\circ}, \theta_{5}^{\prime}=-\theta_{5}, \theta_{6}^{\prime}=\theta_{6}+180^{\circ} θ4′=θ4+180∘,θ5′=−θ5,θ6′=θ6+180∘
上述算法可计算出的多组解，根据关节运动范围的限制可以舍弃一部分解，在余下的有效解中，通常选择最接近机器人当前位姿的“最短行程解”。

2. 《机器人学》常规求解

在求解运动学逆解前，我们先掌握运动学正解的知识，具体过程请参阅：【Matlab 六自由度机器人】运动学正解（附MATLAB机器人正解完整代码）
如前所述，矩阵右式的元素，或为零，或为常数，或者为第iii至第666个关节变量的函数。矩阵相等表明其对应元素分别相等，并可从每一矩阵方程得到121212个方程式，每个方程式对应于444个矢量n,o,a,pn,o,a,pn,o,a,p的每一分量。我们先以之前建立过的六自由度机器人为例来阐述这些方程的求解。

该机器人的运动方程（设为方程式1）如下：
0T6=[nxoxaxpxnyoyaypynxoxazpz0001]=0T1(θ1)1T2(θ2)2T3(θ3)3T4(θ4)4T5(θ5)5T6(θ6){ }^{0} \boldsymbol{T}_{6}=\left[\begin{array}{cccc} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{x} & o_{x} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]={ }^{0} \boldsymbol{T}_{1}\left(\theta_{1}\right)^{1} \boldsymbol{T}_{2}\left(\theta_{2}\right)^{2} \boldsymbol{T}_{3}\left(\theta_{3}\right)^{3} \boldsymbol{T}_{4}\left(\theta_{4}\right)^{4} \boldsymbol{T}_{5}\left(\theta_{5}\right)^{5} \boldsymbol{T}_{6}\left(\theta_{6}\right) 0T6=⎣⎢⎢⎡nxnynx0oxoyox0axayaz0pxpypz1⎦⎥⎥⎤=0T1(θ1)1T2(θ2)2T3(θ3)3T4(θ4)4T5(θ5)5T6(θ6)末端连杆的位姿已经给定，即 n,o,an,o,an,o,a 和 ppp 为已知，则求关节变量 θ1,θ2,⋯,θ6\theta_{1}, \theta_{2}, \cdots, \theta_{6}θ1,θ2,⋯,θ6 的值称为运动反解。用末知的连杆逆变换左乘方程式1两边，把关节变量分离出来，从而求解。具体步骤如下:

1. 求 θ1\theta_{1}θ1

可用逆变换 0T1−1(θ1){ }^{0} \boldsymbol{T}_{1}^{-1}\left(\theta_{1}\right)0T1−1(θ1) 左乘方程式1两边
0T1−1(θ1)0T6=1T2(θ2)2T3(θ3)3T4(θ4)4T5(θ5)5T6(θ6){ }^{0} \boldsymbol{T}_{1}^{-1}\left(\theta_{1}\right)^{0} \boldsymbol{T}_{6}={ }^{1} \boldsymbol{T}_{2}\left(\theta_{2}\right)^{2} \boldsymbol{T}_{3}\left(\theta_{3}\right)^{3} \boldsymbol{T}_{4}\left(\theta_{4}\right)^{4} \boldsymbol{T}_{5}\left(\theta_{5}\right)^{5} \boldsymbol{T}_{6}\left(\theta_{6}\right) 0T1−1(θ1)0T6=1T2(θ2)2T3(θ3)3T4(θ4)4T5(θ5)5T6(θ6)
[c1s100−s1c10000100001][nxoxaxpxnyoyaypynzoxazpx0001]=1T6\left[\begin{array}{rrrr} c_{1} & s_{1} & 0 & 0 \\ -s_{1} & c_{1} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{x} & a_{z} & p_{x} \\ 0 & 0 & 0 & 1 \end{array}\right]={ }^{1} \boldsymbol{T}_{6} ⎣⎢⎢⎡c1−s100s1c10000100001⎦⎥⎥⎤⎣⎢⎢⎡nxnynz0oxoyox0axayaz0pxpypx1⎦⎥⎥⎤=1T6
令上式矩阵方程两瑞的元素 (2,4)(2,4)(2,4) 对应相等，可得
−s1px+c1py=d2-s_{1} p_{x}+c_{1} p_{y}=d_{2} −s1px+c1py=d2
利用三角代换
px=ρcos⁡ϕ;py=ρsin⁡ϕp_{x}=\rho \cos \phi ; \quad p_{y}=\rho \sin \phi px=ρcosϕ;py=ρsinϕ
式中，ρ=px2+py2;ϕ=atan2⁡(py,px)\rho=\sqrt{p_{x}^{2}+p_{y}^{2}} ; \phi=\operatorname{atan2}\left(p_{y}, p_{x}\right)ρ=px2+py2;ϕ=atan2(py,px) ，通过代换得到 θ1\theta_{1}θ1 的解。
sin⁡(ϕ−θ1)=d2/ρ;cos⁡(ϕ−θ1)=±1−(d2/ρ)2ϕ−θ1=atan⁡2[d2ρ,±1−(d2ρ)2]θ1=atan⁡2(py,px)−atan⁡2(d2,±px2+py2−d22)}\left.\begin{array}{l} \sin \left(\phi-\theta_{1}\right)=d_{2} / \rho ; \cos \left(\phi-\theta_{1}\right)=\pm \sqrt{1-\left(d_{2} / \rho\right)^{2}} \\ \phi-\theta_{1}=\operatorname{atan} 2\left[\frac{d_{2}}{\rho}, \pm \sqrt{1-\left(\frac{d_{2}}{\rho}\right)^{2}}\right] \\ \theta_{1}=\operatorname{atan} 2\left(p_{y}, p_{x}\right)-\operatorname{atan} 2\left(d_{2}, \pm \sqrt{p_{x}^{2}+p_{y}^{2}-d_{2}^{2}}\right) \end{array}\right\} sin(ϕ−θ1)=d2/ρ;cos(ϕ−θ1)=±1−(d2/ρ)2ϕ−θ1=atan2[ρd2,±1−(ρd2)2]θ1=atan2(py,px)−atan2(d2,±px2+py2−d22)⎭⎪⎪⎪⎪⎪⎬⎪⎪⎪⎪⎪⎫
式中，正、负号对应于 θ1\theta_{1}θ1 的两个可能解。

2. 求 θ3\theta_{3}θ3

在选定 θ1\theta_{1}θ1 的一个解之后，再令矩阵方程1T6{ }^{1} \boldsymbol{T}_{6}1T6两端的元素的 (1,4)(1,4)(1,4) 和 (3,4)(3,4)(3,4) 分别对应相等，即得两方程
c1px+s1py=a3c23−d4s23+a2c2−px=a3s23+d4c23+a2s2}\left.\begin{array}{l} c_{1} p_{x}+s_{1} p_{y}=a_{3} c_{23}-d_{4} s_{23}+a_{2} c_{2} \\ -p_{x}=a_{3} s_{23}+d_{4} c_{23}+a_{2} s_{2} \end{array}\right\} c1px+s1py=a3c23−d4s23+a2c2−px=a3s23+d4c23+a2s2}
式−s1px+c1py=d2-s_{1} p_{x}+c_{1} p_{y}=d_{2}−s1px+c1py=d2与c1px+s1py=a3c23−d4s23+a2c2−px=a3s23+d4c23+a2s2}\left.\begin{array}{l} c_{1} p_{x}+s_{1} p_{y}=a_{3} c_{23}-d_{4} s_{23}+a_{2} c_{2} \\ -p_{x}=a_{3} s_{23}+d_{4} c_{23}+a_{2} s_{2} \end{array}\right\}c1px+s1py=a3c23−d4s23+a2c2−px=a3s23+d4c23+a2s2}的平方和为
a3c3−d4s3=ka_{3} c_{3}-d_{4} s_{3}=k a3c3−d4s3=k
式中，k=px2+py2+px2−a22−a32−d22−d422a2k=\frac{p_{x}^{2}+p_{y}^{2}+p_{x}^{2}-a_{2}^{2}-a_{3}^{2}-d_{2}^{2}-d_{4}^{2}}{2 a_{2}}k=2a2px2+py2+px2−a22−a32−d22−d42
方程a3c3−d4s3=ka_{3} c_{3}-d_{4} s_{3}=ka3c3−d4s3=k中已经消去 θ2\theta_{2}θ2，且方程a3c3−d4s3=ka_{3} c_{3}-d_{4} s_{3}=ka3c3−d4s3=k与−s1px+c1py=d2-s_{1} p_{x}+c_{1} p_{y}=d_{2}−s1px+c1py=d2具有相同形式，因而可由三角代换求解得到θ3\theta_{3}θ3
θ3=atan⁡2(a3,d4)−atan⁡2(k,±a32+d42−k2)\theta_{3}=\operatorname{atan} 2\left(a_{3}, d_{4}\right)-\operatorname{atan} 2\left(k, \pm \sqrt{a_{3}^{2}+d_{4}^{2}-k^{2}}\right) θ3=atan2(a3,d4)−atan2(k,±a32+d42−k2)
式中，正、负号对应 θ3\theta_{3}θ3 的两种可能解。

3. 求 θ2\theta_{2}θ2

为求解 θ2\theta_{2}θ2，在矩阵方程1两边左乘逆变换 0T3−1{ }^{0} \boldsymbol{T}_{3}^{-1}0T3−1 。
[0T3−1(θ1,θ2,θ3)0T633T4(θ4)4T5(θ5)5T6(θ6)−c1s23−s1s23−s23−a23c3−s1c10−a2s30001][nxoxaxpxnyoyaypynzvzazpz0001]=3T6\left[\begin{array}{cccc} { }^{0} \boldsymbol{T}_{3}^{-1}\left(\theta_{1}, \theta_{2}, \theta_{3}\right)^{0} \boldsymbol{T}_{6} & { }^{3}{ }^{3} \boldsymbol{T}_{4}\left(\theta_{4}\right){ }^{4} \boldsymbol{T}_{5}\left(\theta_{5}\right)^{5} \boldsymbol{T}_{6}\left(\theta_{6}\right) \\ -c_{1} s_{23} & -s_{1} s_{23} & -s_{23} & -a_{23} c_{3} \\ -s_{1} & c_{1} & 0 & -a_{2} s_{3} \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{cccc} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & v_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]={ }^{3} \boldsymbol{T}_{6} ⎣⎢⎢⎡0T3−1(θ1,θ2,θ3)0T6−c1s23−s1033T4(θ4)4T5(θ5)5T6(θ6)−s1s23c10−s2300−a23c3−a2s31⎦⎥⎥⎤⎣⎢⎢⎡nxnynz0oxoyvz0axayaz0pxpypz1⎦⎥⎥⎤=3T6
式中，变换 3T6{ }^{3} \boldsymbol{T}_{6}3T6 由正解出来的矩阵推出。令上式矩阵方程两边的元素 (1,4)(1,4)(1,4) 和 (2.4) 分别对应相等可得
c1c23px+s1c23py−s23pz−a2c3=a3−c1s23pz−s1s23py−c23pz+a2s3=d4}\left.\begin{array}{l} c_{1} c_{23} p_{x}+s_{1} c_{23} p_{y}-s_{23} p_{z}-a_{2} c_{3}=a_{3} \\ -c_{1} s_{23} p_{z}-s_{1} s_{23} p_{y}-c_{23} p_{z}+a_{2} s_{3}=d_{4} \end{array}\right\} c1c23px+s1c23py−s23pz−a2c3=a3−c1s23pz−s1s23py−c23pz+a2s3=d4}联立求解得 s23s_{23}s23 和 c23c_{23}c23
{s23=(−a3−a2c3)pz+(c1px+s1py)(a2s3−d4)p22+(c1px+s1py)2c23=(−d4+a2s3)pz−(c1px+s1py)(−a2c3−a4)pz2+(c1px+s1py)2\left\{\begin{array}{l} s_{23}=\frac{\left(-a_{3}-a_{2} c_{3}\right) p_{z}+\left(c_{1} p_{x}+s_{1} p_{y}\right)\left(a_{2} s_{3}-d_{4}\right)}{p_{2}^{2}+\left(c_{1} p_{x}+s_{1} p_{y}\right)^{2}} \\ c_{23}=\frac{\left(-d_{4}+a_{2} s_{3}\right) p_{z}-\left(c_{1} p_{x}+s_{1} p_{y}\right)\left(-a_{2} c_{3}-a_{4}\right)}{p_{z}^{2}+\left(c_{1} p_{x}+s_{1} p_{y}\right)^{2}} \end{array}\right. {s23=p22+(c1px+s1py)2(−a3−a2c3)pz+(c1px+s1py)(a2s3−d4)c23=pz2+(c1px+s1py)2(−d4+a2s3)pz−(c1px+s1py)(−a2c3−a4)
s23s_{23}s23 和 c23c_{23}c23 表达式的分母相等，且为正。于是θ23=θ2+θ3=atan2⁡[−(a3+a2c3)pz+(c1px+s1py)(a2s3−d4),(−d4+a2s3)px+(c1px+s1py)(a2c3+a3)]\begin{aligned} \theta_{23}=\theta_{2}+\theta_{3}=& \operatorname{atan2}\left[-\left(a_{3}+a_{2} c_{3}\right) p_{z}+\left(c_{1} p_{x}+s_{1} p_{y}\right)\left(a_{2} s_{3}-d_{4}\right),\left(-d_{4}+a_{2} s_{3}\right) p_{x}\right.\\ &\left.+\left(c_{1} p_{x}+s_{1} p_{y}\right)\left(a_{2} c_{3}+a_{3}\right)\right] \end{aligned} θ23=θ2+θ3=atan2[−(a3+a2c3)pz+(c1px+s1py)(a2s3−d4),(−d4+a2s3)px+(c1px+s1py)(a2c3+a3)]
根据 θ1\theta_{1}θ1 和 θ3\theta_{3}θ3 解的四种可能组合，由上式可以得到相应的四种可能值 θ23\theta_{23}θ23，于是可得到 θ2\theta_{2}θ2 的四种可能解
θ2=θ23−θ3\theta_{2}=\theta_{23}-\theta_{3} θ2=θ23−θ3
式中，θ2\theta_{2}θ2 取与 θ3\theta_{3}θ3 相对应的值。

4. 求 θ4\theta_{4}θ4

因为式 (3.75) 的左边均为已知，令两边元素 (1,3)(1,3)(1,3) 和 (3,3)(3,3)(3,3) 分别对应相等，则可得
{axc1c23+ays1c23−axs23=−c4s5−axsy+ayc1=s4s5\left\{\begin{array}{l} a_{x} c_{1} c_{23}+a_{y} s_{1} c_{23}-a_{x} s_{23}=-c_{4} s_{5} \\ -a_{x} s_{y}+a_{y} c_{1}=s_{4} s_{5} \end{array}\right. {axc1c23+ays1c23−axs23=−c4s5−axsy+ayc1=s4s5
只要 s5≠0s_{5} \neq 0s5=0，便可求出 θ4\theta_{4}θ4
θ4=atan⁡2(−a4s1+ayc1,−axc1c23−ays1c23+axs23)\theta_{4}=\operatorname{atan} 2\left(-a_{4} s_{1}+a_{y} c_{1},-a_{x} c_{1} c_{23}-a_{y} s_{1} c_{23}+a_{x} s_{23}\right) θ4=atan2(−a4s1+ayc1,−axc1c23−ays1c23+axs23)
当 s5=0s_{5}=0s5=0 时，机械手处于奇㫒形位。此时，关节轴 4 和 6 重合，只能解出 θ4\theta_{4}θ4 与 θ6\theta_{6}θ6 的和或差。奇异形位可以由式 (3.79)中 atan⁡2\operatorname{atan} 2atan2 的两个变量是否都接近零来判别。若都接近零，则为奇异形位，否则，不是奇异形位。在奇异形位时，可任意选取 θ4\theta_{4}θ4 的值,再计箅相应的 θ6\theta_{6}θ6 值。

5. 求 θ5\theta_{5}θ5

据求出的 θ4\theta_{4}θ4，可进一步解出 θ5\theta_{5}θ5，将方程1两端左乘逆变换 0T4(θ1,θ2,θ3,θ4){ }^{0} \boldsymbol{T}_{4}{ }^{}\left(\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}\right)0T4(θ1,θ2,θ3,θ4),
0T4−1(θ1,θ2,θ3,θ4)0T6=4T5(θ5)5T6(θ6){ }^{0} \boldsymbol{T}_{4}^{-1}\left(\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}\right){ }^{0} \boldsymbol{T}_{6}={ }^{4} \boldsymbol{T}_{5}\left(\theta_{5}\right)^{5} \boldsymbol{T}_{6}\left(\theta_{6}\right) 0T4−1(θ1,θ2,θ3,θ4)0T6=4T5(θ5)5T6(θ6)
因下面方程式的左边 θ1,θ2,θ3\theta_{1}, \theta_{2}, \theta_{3}θ1,θ2,θ3 和 θ4\theta_{4}θ4 均已解出, 逆变换 0T4−1(θ1,θ2,θ3,θ4){ }^{0} T_{4}^{-1}\left(\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}\right)0T4−1(θ1,θ2,θ3,θ4) 为
[c1c23c4+s1s4s1c23c4−c1s4−s23c4−a2c3c4+d2s4−a3c4−c1c23s4+s1c4−s1c23s4−c1c4s23s4a2c3s4+d2c4+a3s4−c1s23−s1s23−c23a2s3−d40001]\left[\begin{array}{cccc} c_{1} c_{23} c_{4}+s_{1} s_{4} & s_{1} c_{23} c_{4}-c_{1} s_{4} & -s_{23} c_{4} & -a_{2} c_{3} c_{4}+d_{2} s_{4}-a_{3} c_{4} \\ -c_{1} c_{23} s_{4}+s_{1} c_{4} & -s_{1} c_{23} s_{4}-c_{1} c_{4} & s_{23} s_{4} & a_{2} c_{3} s_{4}+d_{2} c_{4}+a_{3} s_{4} \\ -c_{1} s_{23} & -s_{1} s_{23} & -c_{23} & a_{2} s_{3}-d_{4} \\ 0 & 0 & 0 & 1 \end{array}\right] ⎣⎢⎢⎡c1c23c4+s1s4−c1c23s4+s1c4−c1s230s1c23c4−c1s4−s1c23s4−c1c4−s1s230−s23c4s23s4−c230−a2c3c4+d2s4−a3c4a2c3s4+d2c4+a3s4a2s3−d41⎦⎥⎥⎤
上式方程式的右边 4T6(θ5,θ6)=4T5(θ5)5T6(θ6){ }^{4} \boldsymbol{T}_{6}\left(\theta_{5}, \theta_{6}\right)={ }^{4} \boldsymbol{T}_{5 } \left(\theta_{5}\right){ }^{5} \boldsymbol{T}_{6}\left(\theta_{6}\right)4T6(θ5,θ6)=4T5(θ5)5T6(θ6)，由正解得到的4T6{ }^{4} \boldsymbol{T}_{6}4T6给出。据矩阵两边元素 (1,3)(1,3)(1,3) 和 (3,3)(3,3)(3,3) 分别对应相等，可得
ax(c1c23c4+s1s4)+ay(s1c23c4−c1s4)−az(s23c4)=−s5ax(−c1s23)+ay(−sis23)+az(−c23)=c5}\left.\begin{array}{l} a_{x}\left(c_{1} c_{23} c_{4}+s_{1} s_{4}\right)+a_{y}\left(s_{1} c_{23} c_{4}-c_{1} s_{4}\right)-a_{z}\left(s_{23} c_{4}\right)&=-s_{5} \\ a_{x}\left(-c_{1} s_{23}\right)+a_{y}\left(-s_{i} s_{23}\right)+a_{z}\left(-c_{23}\right)&=c_{5} \end{array}\right\} ax(c1c23c4+s1s4)+ay(s1c23c4−c1s4)−az(s23c4)ax(−c1s23)+ay(−sis23)+az(−c23)=−s5=c5}
由此得到 θ5\theta_{5}θ5 的封闭解
θ5=atan⁡2(s5,c5)\theta_{5}=\operatorname{atan} 2\left(s_{5}, c_{5}\right) θ5=atan2(s5,c5)

6. 求 θ6\theta_{6}θ6

将方程式1改写为
0T51(θ1,θ2,⋯,θ5)0T6=5T6(θ6){ }^{0} \boldsymbol{T}_{5}{ }^{1}\left(\theta_{1}, \theta_{2}, \cdots, \theta_{5}\right)^{0} \boldsymbol{T}_{6}={ }^{5} \boldsymbol{T}_{6}\left(\theta_{6}\right) 0T51(θ1,θ2,⋯,θ5)0T6=5T6(θ6)
令上式的矩阵方程两边元素 (3,1)(3,1)(3,1) 和 (1,1)(1,1)(1,1) 分别对应相等，可得
−nx(c1c23s4−s1c4)−ny(s1c23s4+c1c4)+nz(s23s4)=s6nz[(c1c23c++s1s4)c5−cs23s5]+ny[(s1c23c4−c1s4)c5−s1s23s5]−nz(s23c4c5+c23s5)=c6\begin{aligned} -n_{x}\left(c_{1} c_{23} s_{4}-s_{1} c_{4}\right)-n_{y}\left(s_{1} c_{23} s_{4}+c_{1} c_{4}\right)+n_{z}\left(s_{23} s_{4}\right)&=s_{6} \\ n_{z}\left[\left(c_{1} c_{23} c_{+}+s_{1} s_{4}\right) c_{5}-c s_{23} s_{5}\right]+n_{y}\left[\left(s_{1} c_{23} c_{4}-c_{1} s_{4}\right) c_{5}-s_{1} s_{23} s_{5}\right] &-n_{z}\left(s_{23} c_{4} c_{5}+c_{23} s_{5}\right)&=c_{6} \end{aligned} −nx(c1c23s4−s1c4)−ny(s1c23s4+c1c4)+nz(s23s4)nz[(c1c23c++s1s4)c5−cs23s5]+ny[(s1c23c4−c1s4)c5−s1s23s5]=s6−nz(s23c4c5+c23s5)=c6
从而可求出 θ6\theta_{6}θ6 的封闭解
θ6=atan⁡2(s6,c6)\theta_{6}=\operatorname{atan} 2\left(s_{6}, c_{6}\right) θ6=atan2(s6,c6)六自由度机器人的运动学反解可能存在888种解。但是，由于结构的限制，例如各关节变量不能在全部 360∘360^{\circ}360∘ 范围内运动，有些解不能实现。在机器人存在多种解的情况下，应选取其中满意的一组解，以满足机器人的工作要求。

后续更新简单理解的步骤

二、代码实现

1. 机器人模型的建立

根据往期文章，对六自由度机器人具体建模方法可参阅以下文章：

定义标准型及改进型D-H参数建立机器人模型（附MATLAB建模代码）
【Matlab 六自由度机器人】关于改进型D-H参数(modified Denavit-Hartenberg)的详细建立步骤

2. Pieper 法求六自由度机器人逆解

建立好机器人模型后，已知期望位姿的矩阵aaa。将所求解的机器人的模型放入逆解脚本中，脚本的输入为期望位姿矩阵aaa，输出为角度JJJ

代码如下：

function [J] = MODikine(a)
%% MOD-DH参数
%定义连杆的D-H参数
%连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
%连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
%连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;%%
nx=a(1,1);ox=a(1,2);ax=a(1,3);px=a(1,4);
ny=a(2,1);oy=a(2,2);ay=a(2,3);py=a(2,4);
nz=a(3,1);oz=a(3,2);az=a(3,3);pz=a(3,4);% 求解关节角1theta1_1 = atan2(py,px)-atan2(d2,sqrt(px^2+py^2-d2^2));theta1_2 = atan2(py,px)-atan2(d2,-sqrt(px^2+py^2-d2^2));% 求解关节角3m3_1 = (px^2+py^2+pz^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);theta3_1 = atan2(a3,d4)-atan2(m3_1,sqrt(a3^2+d4^2-m3_1^2));theta3_2 = atan2(a3,d4)-atan2(m3_1,-sqrt(a3^2+d4^2-m3_1^2));% 求解关节角2ms2_1 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_1)-d4);mc2_1 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_1)+a3);theta23_1 = atan2(ms2_1,mc2_1);theta2_1 = theta23_1 - theta3_1;ms2_2 = -((a3+a2*cos(theta3_1))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_1)-d4);mc2_2 = (-d4+a2*sin(theta3_1))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_1)+a3);theta23_2 = atan2(ms2_2,mc2_2);theta2_2 = theta23_2 - theta3_1;ms2_3 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*sin(theta3_2)-d4);mc2_3 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_1)*px+sin(theta1_1)*py)*(a2*cos(theta3_2)+a3);theta23_3 = atan2(ms2_3,mc2_3);theta2_3 = theta23_3 - theta3_2;ms2_4 = -((a3+a2*cos(theta3_2))*pz)+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*sin(theta3_2)-d4);mc2_4 = (-d4+a2*sin(theta3_2))*pz+(cos(theta1_2)*px+sin(theta1_2)*py)*(a2*cos(theta3_2)+a3);theta23_4 = atan2(ms2_4,mc2_4);theta2_4 = theta23_4 - theta3_2;% 求解关节角4ms4_1=-ax*sin(theta1_1)+ay*cos(theta1_1);mc4_1=-ax*cos(theta1_1)*cos(theta23_1)-ay*sin(theta1_1)*cos(theta23_1)+az*sin(theta23_1);theta4_1=atan2(ms4_1,mc4_1);ms4_2=-ax*sin(theta1_2)+ay*cos(theta1_2);mc4_2=-ax*cos(theta1_2)*cos(theta23_2)-ay*sin(theta1_2)*cos(theta23_2)+az*sin(theta23_2);theta4_2=atan2(ms4_2,mc4_2);ms4_3=-ax*sin(theta1_1)+ay*cos(theta1_1);mc4_3=-ax*cos(theta1_1)*cos(theta23_3)-ay*sin(theta1_1)*cos(theta23_3)+az*sin(theta23_3);theta4_3=atan2(ms4_3,mc4_3);ms4_4=-ax*sin(theta1_2)+ay*cos(theta1_2);mc4_4=-ax*cos(theta1_2)*cos(theta23_4)-ay*sin(theta1_2)*cos(theta23_4)+az*sin(theta23_4);theta4_4=atan2(ms4_4,mc4_4);% 求解关节角5ms5_1=-ax*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))-ay*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)-cos(theta1_1)*sin(theta4_1))+az*(sin(theta23_1)*cos(theta4_1));mc5_1= ax*(-cos(theta1_1)*sin(theta23_1))+ay*(-sin(theta1_1)*sin(theta23_1))+az*(-cos(theta23_1));theta5_1=atan2(ms5_1,mc5_1);ms5_2=-ax*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))-ay*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)-cos(theta1_2)*sin(theta4_2))+az*(sin(theta23_2)*cos(theta4_2));mc5_2= ax*(-cos(theta1_2)*sin(theta23_2))+ay*(-sin(theta1_2)*sin(theta23_2))+az*(-cos(theta23_2));theta5_2=atan2(ms5_2,mc5_2);ms5_3=-ax*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))-ay*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)-cos(theta1_1)*sin(theta4_3))+az*(sin(theta23_3)*cos(theta4_3));mc5_3= ax*(-cos(theta1_1)*sin(theta23_3))+ay*(-sin(theta1_1)*sin(theta23_3))+az*(-cos(theta23_3));theta5_3=atan2(ms5_3,mc5_3);ms5_4=-ax*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))-ay*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)-cos(theta1_2)*sin(theta4_4))+az*(sin(theta23_4)*cos(theta4_4));mc5_4= ax*(-cos(theta1_2)*sin(theta23_4))+ay*(-sin(theta1_2)*sin(theta23_4))+az*(-cos(theta23_4));theta5_4=atan2(ms5_4,mc5_4);% 求解关节角6ms6_1=-nx*(cos(theta1_1)*cos(theta23_1)*sin(theta4_1)-sin(theta1_1)*cos(theta4_1))-ny*(sin(theta1_1)*cos(theta23_1)*sin(theta4_1)+cos(theta1_1)*cos(theta4_1))+nz*(sin(theta23_1)*sin(theta4_1));mc6_1= nx*(cos(theta1_1)*cos(theta23_1)*cos(theta4_1)+sin(theta1_1)*sin(theta4_1))*cos(theta5_1)-nx*cos(theta1_1)*sin(theta23_1)*sin(theta4_1)+ny*(sin(theta1_1)*cos(theta23_1)*cos(theta4_1)+cos(theta1_1)*sin(theta4_1))*cos(theta5_1)-ny*sin(theta1_1)*sin(theta23_1)*sin(theta5_1)-nz*(sin(theta23_1)*cos(theta4_1)*cos(theta5_1)+cos(theta23_1)*sin(theta5_1));theta6_1=atan2(ms6_1,mc6_1);ms6_2=-nx*(cos(theta1_2)*cos(theta23_2)*sin(theta4_2)-sin(theta1_2)*cos(theta4_2))-ny*(sin(theta1_2)*cos(theta23_2)*sin(theta4_2)+cos(theta1_2)*cos(theta4_2))+nz*(sin(theta23_2)*sin(theta4_2));mc6_2= nx*(cos(theta1_2)*cos(theta23_2)*cos(theta4_2)+sin(theta1_2)*sin(theta4_2))*cos(theta5_2)-nx*cos(theta1_2)*sin(theta23_2)*sin(theta4_2)+ny*(sin(theta1_2)*cos(theta23_2)*cos(theta4_2)+cos(theta1_2)*sin(theta4_2))*cos(theta5_2)-ny*sin(theta1_2)*sin(theta23_2)*sin(theta5_2)-nz*(sin(theta23_2)*cos(theta4_2)*cos(theta5_2)+cos(theta23_2)*sin(theta5_2));theta6_2=atan2(ms6_2,mc6_2);ms6_3=-nx*(cos(theta1_1)*cos(theta23_3)*sin(theta4_3)-sin(theta1_1)*cos(theta4_3))-ny*(sin(theta1_1)*cos(theta23_3)*sin(theta4_3)+cos(theta1_1)*cos(theta4_3))+nz*(sin(theta23_3)*sin(theta4_3));mc6_3= nx*(cos(theta1_1)*cos(theta23_3)*cos(theta4_3)+sin(theta1_1)*sin(theta4_3))*cos(theta5_3)-nx*cos(theta1_1)*sin(theta23_3)*sin(theta4_3)+ny*(sin(theta1_1)*cos(theta23_3)*cos(theta4_3)+cos(theta1_1)*sin(theta4_3))*cos(theta5_3)-ny*sin(theta1_1)*sin(theta23_3)*sin(theta5_3)-nz*(sin(theta23_3)*cos(theta4_3)*cos(theta5_3)+cos(theta23_3)*sin(theta5_3));theta6_3=atan2(ms6_3,mc6_3);ms6_4=-nx*(cos(theta1_2)*cos(theta23_4)*sin(theta4_4)-sin(theta1_2)*cos(theta4_4))-ny*(sin(theta1_1)*cos(theta23_4)*sin(theta4_4)+cos(theta1_2)*cos(theta4_4))+nz*(sin(theta23_4)*sin(theta4_4));mc6_4= nx*(cos(theta1_2)*cos(theta23_4)*cos(theta4_4)+sin(theta1_2)*sin(theta4_4))*cos(theta5_4)-nx*cos(theta1_2)*sin(theta23_4)*sin(theta4_4)+ny*(sin(theta1_2)*cos(theta23_4)*cos(theta4_4)+cos(theta1_2)*sin(theta4_4))*cos(theta5_1)-ny*sin(theta1_2)*sin(theta23_4)*sin(theta5_4)-nz*(sin(theta23_4)*cos(theta4_4)*cos(theta5_4)+cos(theta23_4)*sin(theta5_4));theta6_4=atan2(ms6_4,mc6_4);% 整理得到4组运动学非奇异逆解
theta_MOD1 = [ theta1_1,theta2_1,theta3_1,theta4_1,theta5_1,theta6_1;theta1_2,theta2_2,theta3_1,theta4_2,theta5_2,theta6_2;theta1_1,theta2_3,theta3_2,theta4_3,theta5_3,theta6_3;theta1_2,theta2_4,theta3_2,theta4_4,theta5_4,theta6_4;];
% 将操作关节‘翻转’可得到另外4组解
theta_MOD2 = ...[ theta1_1,theta2_1,theta3_1,theta4_1+pi,-theta5_1,theta6_1+pi;theta1_2,theta2_2,theta3_1,theta4_2+pi,-theta5_2,theta6_2+pi;theta1_1,theta2_3,theta3_2,theta4_3+pi,-theta5_3,theta6_3+pi;theta1_2,theta2_4,theta3_2,theta4_4+pi,-theta5_4,theta6_4+pi;];J = [theta_MOD1;theta_MOD2];
end

3. 常规法求六自由度机器人逆解

由于自写的代码过长，阅读较为困难，因此贴上部分逆解代码过程。
建立好机器人模型后，已知期望位姿的矩阵aaa。将所求解的机器人的模型放入逆解脚本中，脚本的输入为期望位姿矩阵aaa，输出为角度JJJ

这里的robotrobotrobot代表的是机器人的模型，aaa是已知的期望位姿的矩阵，输出是各轴的角度JJJ

function [J] = Albert_MODikin(robot,a)
%% MOD-DH参数
%连杆偏移       %连杆长度       %连杆扭角
d1 = robot.d(1);a1 = robot.a(1);alpha1 = robot.alpha(1);
d2 = robot.d(2);a2 = robot.a(2);alpha2 = robot.alpha(2);
d3 = robot.d(3);a3 = robot.a(3);alpha3 = robot.alpha(3);
d4 = robot.d(4);a4 = robot.a(4);alpha4 = robot.alpha(4);
d5 = robot.d(5);a5 = robot.a(5);alpha5 = robot.alpha(5);
d6 = robot.d(6);a6 = robot.a(6);alpha6 = robot.alpha(6);%%
nx=a(1,1);ox=a(1,2);ax=a(1,3);px=a(1,4);
ny=a(2,1);oy=a(2,2);ay=a(2,3);py=a(2,4);
nz=a(3,1);oz=a(3,2);az=a(3,3);pz=a(3,4);%j1
j1a = py - ay*d6;
j1b = px - ax*d6;j11 = atan2(j1a,j1b)-atan2(-d2, sqrt(j1a^2+j1b^2-d2^2));
j12 = atan2(j1a,j1b)-atan2(-d2,-sqrt(j1a^2+j1b^2-d2^2));% 这部分是进行一个判断，将小于1e-16的数字看作0，建议删除。
if j11<1e-16j11 = 0;
end
if j12<1e-16j12 = 0;
end%j3
j3a1 = px*cos(j11) - d6*(ax*cos(j11) + ay*sin(j11)) + py*sin(j11) - a2;
j3b1 = pz - d1 - az*d6;
j3k1 = j3a1^2 + j3b1^2 - a4^2 - d4^2 - a3^2;
j3d1 = j3k1/(2*a3);
j3a2 = px*cos(j12) - d6*(ax*cos(j12) + ay*sin(j12)) + py*sin(j12) - a2;
j3b2 = pz - d1 - az*d6;
j3k2 = j3a2^2 + j3b2^2 - a4^2 - d4^2 - a3^2;
j3d2 = j3k2/(2*a3);j31 = atan2(j3d1, sqrt(abs(a4^2 + d4^2 - j3d1^2))) - atan2(a4,d4);%j11
j32 = atan2(j3d1,-sqrt(abs(a4^2 + d4^2 - j3d1^2))) - atan2(a4,d4);%j11
j33 = atan2(j3d2, sqrt(abs(a4^2 + d4^2 - j3d2^2))) - atan2(a4,d4);%j12
j34 = atan2(j3d2,-sqrt(abs(a4^2 + d4^2 - j3d2^2))) - atan2(a4,d4);%j12% 经过一系列运算后，最终得到J
J = [j11 j21 j31 j41 j51 j61;j11 j22 j32 j42 j52 j62;j12 j23 j33 j43 j53 j63;j12 j24 j34 j44 j54 j64;j11 j21 j31 j45 j55 j65;j11 j22 j32 j46 j56 j66;j12 j23 j33 j47 j57 j67;j12 j24 j34 j48 j58 j68];end

程序运行后的结果如下：

我们可以看到，左侧机器人示教器中，第二根轴的角度设为60∘60^{\circ}60∘，其位置矩阵为[−99.234,0,2605.139][-99.234,0,2605.139][−99.234,0,2605.139]。右上方正解的最后一列完全对应，右下方逆解得到的数值与输入的角度数一致，因此可知逆解程序完全可行。

总结

以上就是逆向运动学的内容，本文详细介绍了如何理解运动学逆解，以及对运动学逆解的求解方法的罗列及原理说明。最后对运动学逆解实现代码的编程，使逆解能够有效得到目标位姿所对应的各轴转角。

参考文献

MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵
六轴机器人matlab写运动学逆解函数（改进DH模型）
六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现

【Matlab 六自由度机器人】运动学逆解（附MATLAB机器人逆解代码）相关推荐

【Matlab 六自由度机器人】运动学正解（附MATLAB机器人正解完整代码）
[Matlab 六自由度机器人]求运动学正解往期回顾前言正文一.运动学正解 1. 齐次变换矩阵 2. 总变换二.代码实现 1. 定义各连杆参数 2. 齐次变换矩阵及总变换 3. 代码运行结果 ...
【Matlab 六自由度机器人】关于机器人运动学反解的有关问题
[Matlab 六自由度机器人]关于机器人运动学反解的有关问题近期更新前言正文反解的三类问题一.中间点在工作空间之外二.在奇异点附近关节速度骤增三.起始点和目标点有多重解总结参考资料 ...
【Matlab 六自由度机器人】定义标准型及改进型D-H参数建立机器人模型（附MATLAB建模代码）
Matlab建立六自由度机器人模型近期更新前言 1.Matlab机器人工具箱 2.研究对象-六自由度机器人正文一.D-H参数(Denavit–Hartenberg parameters) 1. ...
【Matlab 六自由度机器人】关于灵活工作空间与可达工作空间的理解（附MATLAB推导代码）
[Matlab 六自由度机器人]灵活工作空间与可达工作空间的解释往期回顾前言正文一.两种工作空间的定义及区别 1. 可达工作空间 2. 灵活工作空间二.辅助理解工作空间的代码 1. 可达工作 ...
【Matlab 六自由度机器人】基于蒙特卡罗方法(Monte Carlo Method)构建机器人工作空间（附MATLAB建模仿真完整代码）
[Matlab 六自由度机器人]基于蒙特卡洛法构建工作空间往期回顾前言正文一.蒙特卡洛方法及机器人工作空间的概述 1. 蒙特卡洛法 2. 机器人工作空间二.rand函数的应用 1. rand ...
【Matlab 六自由度机器人】系列文章汇总
[Matlab 六自由度机器人]系列文章汇总前言正文 ⧫[主线]\blacklozenge \fcolorbox{black}{aqua}{[主线]}⧫[主线] ⧫[补充说明]\blackloz ...
六自由度机械臂建模仿真（matlab程序），有控制面板，标价即为真实价格，代码可流畅运行
六自由度机械臂建模仿真(matlab程序),有控制面板,标价即为真实价格,代码可流畅运行 1.机器人运动学正逆解.动力学建模仿真与轨迹规划,雅克比矩阵求解 2.蒙特卡洛采样画出末端执行器工作空间 3. ...
【机械】基于简化几何解法的六轴机械臂位置规划附matlab代码
1 内容介绍基于简化几何解法的六轴机械臂位置规划附matlab代码 2 部分代码 clc; clear; %载入数据 importfile('shuiping.mat'); theta_shuipi ...
【路径规划】基于DWA实现机器人动态避障附matlab代码
1 内容介绍 DWA 算法是基于机器人运动学与动力学理论的一种局部避障算法,它将对机器人的位置控制转换为对机器人的速度控制.DWA 算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样 ...
matlab 二自由度振动仿真,基于Adams和Matlab二自由度系统振动的仿真分析研究
化学工程与装备 2017 年第 11 期 28 Chemical Engineering & Equipment 2017 年 11 月基于 Adams 和 Matlab 二自由度系统振 ...

【Matlab 六自由度机器人】运动学逆解（附MATLAB机器人逆解代码）