SCARA的完整运动学解—

笔者在完成“SCARA的运动学求解”的过程中，发现互联网上现存的资料普遍存在两个问题：1.代数解法较少 2.代数法求解过程中往往存在一些纰漏，且步骤之间存在较大的gap。因此，萌生了写这篇文章的冲动，以下过程部分整理自网络，笔者修正了其中错误，并补充了缺漏的部分，将完整过程呈现如下。

SCARA 图示

根据对SCARA机器人结构以及给出的坐标系关系进行分析，我们可以得到D-H参数表如下：


1
2
3
4

正运动学

为了完成运动学正逆解的求解，我们根据坐标系定义给出对应齐次坐标变换矩阵：
$T_1=\left(\begin{array}{cccc} \cos \left(\theta_1 \right) & -\sin \left(\theta_1 \right) & 0 & 0\\ \sin \left(\theta_1 \right) & \cos \left(\theta_1 \right) & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right)\qquad T_2=\left(\begin{array}{cccc} \cos \left(\theta_2 \right) & -\sin \left(\theta_2 \right) & 0 & l_1 \\ \sin \left(\theta_2 \right) & \cos \left(\theta_2 \right) & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right)$

$T_3=\left(\begin{array}{cccc} 1 & 0 & 0 & l_2 \\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & -d_3 \\ 0 & 0 & 0 & 1 \end{array}\right)\quad T_4=\left(\begin{array}{cccc} \cos \left(\theta_4 \right) & -\sin \left(\theta_4 \right) & 0 & 0\\ \sin \left(\theta_4 \right) & \cos \left(\theta_4 \right) & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{array}\right)$

当我们将它们按照运动坐标系的顺序连乘后可以得到：
$\left(\begin{array}{cccc} \cos \left(\theta_1 +\theta_2 +\theta_4 \right) & -\sin \left(\theta_1 +\theta_2 +\theta_4 \right) & 0 & l_2 \,\cos \left(\theta_1 +\theta_2 \right)+l_1 \,\cos \left(\theta_1 \right)\\ \sin \left(\theta_1 +\theta_2 +\theta_4 \right) & \cos \left(\theta_1 +\theta_2 +\theta_4 \right) & 0 & l_2 \,\sin \left(\theta_1 +\theta_2 \right)+l_1 \,\sin \left(\theta_1 \right)\\ 0 & 0 & 1 & -d_3 \\ 0 & 0 & 0 & 1 \end{array}\right)$

逆运动学

为了求解这个方程，我们可以设中间变量：

因此我们可以把方程组转化为：
$\left(\begin{array}{cccc}\cos{\phi_1} & -\sin{\phi_1} & 0 & l_2 \cos{\phi_2}+l_1 \cos{\phi_3}\\ \sin{\phi_1} & \cos{\phi_1} & 0 & l_2 \sin{\phi_2}+l_1 \sin{\phi_3}\\0 & 0 & 1 & -d_3 \\0 & 0 & 0 & 1\end{array}\right)= \left(\begin{array}{cccc}r_{11} & r_{12} & 0 & r_{14}\\ r_{21} & r_{22} & 0 & r_{24}\\0 & 0 & 1 & r_{34} \\0 & 0 & 0 & 1\end{array}\right)$
在求解过程中，值得一提的是：
$\begin{align} (l_2 \cos{\phi_2}+l_1 \cos{\phi_3})^2+ (l_2 \sin{\phi_2}+l_1 \sin{\phi_3})^2=&r_{14}^2+r_{24}^2\\ l_1^2+l_2^2+2l_1l_2\cos{\phi_2}\cos{\phi_3}+2l_1l_2\sin{\phi_2}\sin{\phi_3}=&r_{14}^2+r_{24}^2\\ \cos{\phi_2}\cos{\phi_3}+\sin{\phi_2}\sin{\phi_3}=&M\\ where\quad M=\frac{r_{14}^2+r_{24}^2-l_1^2-l_2^2}{2l_1l_2}\\ cos{\theta_2}=cos(\phi_2-\phi_3)=&M\quad M\in[-1,+1] \end{align}$
通过上式，我们可以求解出。此外，易见。至此，与为已知：

下面我们通过变换得到：
$\left(\begin{matrix} r_{11} \,\cos \left(\theta_1 \right)+r_{21} \,\sin \left(\theta_1 \right) & r_{12} \,\cos \left(\theta_1 \right)+r_{22} \,\sin \left(\theta_1 \right) & r_{13} \,\cos \left(\theta_1 \right)+r_{23} \,\sin \left(\theta_1 \right) & r_{14} \,\cos \left(\theta_1 \right)+r_{24} \,\sin \left(\theta_1 \right)\\ r_{21} \,\cos \left(\theta_1 \right)-r_{11} \,\sin \left(\theta_1 \right) & r_{22} \,\cos \left(\theta_1 \right)-r_{12} \,\sin \left(\theta_1 \right) & r_{23} \,\cos \left(\theta_1 \right)-r_{13} \,\sin \left(\theta_1 \right) & r_{24} \,\cos \left(\theta_1 \right)-r_{14} \,\sin \left(\theta_1 \right)\\ r_{31} & r_{32} & r_{33} & r_{34} \\ 0 & 0 & 0 & 1 \end{matrix}\right) \\=\left(\begin{array}{cccc} \cos \left(\theta_2 +\theta_4 \right) & -\sin \left(\theta_2 +\theta_4 \right) & 0 & l_1 +l_2 \,\cos \left(\theta_2 \right)\\ \sin \left(\theta_2 +\theta_4 \right) & \cos \left(\theta_2 +\theta_4 \right) & 0 & l_2 \,\sin \left(\theta_2 \right)\\ 0 & 0 & 1 & -d_3 \\ 0 & 0 & 0 & 1 \end{array}\right)$

关注矩阵(1,4),(2,4)位置的方程，其中右侧均为已知量：

根据矩阵求逆，可以得到（其中除非机械臂未曾在上移动，不然一定非零）：
$\begin{align} \cos{\theta_1}&=\frac{r_{14}(l_1+l_2\cos{\theta_2})+r_{24}(l_2\cos{\theta_2})}{r_{14}^2+r_{24}^2}\\ \sin{\theta_1}&=\frac{r_{14}(l_1+l_2\cos{\theta_2})-r_{24}(l_2\cos{\theta_2})}{r_{14}^2+r_{24}^2} \end{align}$
于是，我们有

我们可以解得如下结果：
$\left\{\begin{matrix} \theta_2=& atan2(\pm\sqrt{1-M^2},M) &where\ M=\frac{r_{14}^2+r_{24}^2-l_1^2-l_2^2}{2l_1l_2}\\ \theta_1=& atan2(\frac{r_{14}(l_1+l_2\cos{\theta_2})-r_{24}(l_2\cos{\theta_2})}{r_{14}^2+r_{24}^2},\frac{r_{14}(l_1+l_2\cos{\theta_2})+r_{24}(l_2\cos{\theta_2})}{r_{14}^2+r_{24}^2})\\ \theta_4=& atan2(r_{21}, r_{11}) - \theta_1 - \theta_2 \\ d_3=&-r_{34}\end{matrix}\right.$

您如果感到这篇文章对您有帮助的话，不妨给我点个赞，这也是对我最好的鼓励。有什么问题也欢迎留言，我会不定期回复；或者通过我个人介绍中的邮箱联系我，我会定期查看邮箱。

SCARA的完整运动学解——代数法求解

正运动学

逆运动学

你可能感兴趣的:(SCARA的完整运动学解——代数法求解)