罗汉杰. 一种机械臂的轨迹规划方法及装置：中国，201610452034[P/OL].
2016-11-23 [2018-03-20].
https://patentimages.storage.googleapis.com/8a/5f/bc/325d10a3dee6fc/CN106041941A.pdf
SCARA(Selective Compliant Articulated Robot for
Assembly)型机械臂是机械臂的一种。它拥有4个关节(图.1)，其中\(J_1\)，\(J_2\)，\(J_4\)为转动关节，\(J_3\)为滑动关节。
scara_rea
图. 1: SCARA型机械臂。
scara
图. 2: SCARA型机械臂工作区域。
图.2显示了机械臂的工作空间。在机械臂工作的过程中，很多任务需要知道机械臂可到达的边缘位置，比如在示教功能中，用户通过手持设备发送指令，使得机械臂一直沿某个方向直线运动，直至到达到工作区域的边缘时，速度和加速度将降为零。
传统的方法中，可以通过不断监测当前机械臂的是否已经是到达极限位置来实现。但是这种方法需要对路径上所有的位置点进行检测，效率低，计算量大。
在本文中，我们提出了一种基于几何的方法来确定SCARA型机械臂工作区域边缘的方法。给定起始点和方向后，能给出与该射线相交的最近的工作区域边缘位置。利用该方法，在机器人运动前就可以提前知道终点的位置，方便机器人进行运动学规划。
数学模型
我们按照图.3所示对SCARA型机械臂进行DH(Denavit-Hartenberg)坐标系分配。其中转动关节\(J_i\)的转动轴与各自的\(z_i\)轴互相平衡，臂长为\(\alpha _i\)，\(\lbrace i\mid i \in \lbrace 1,2,4
\rbrace \rbrace\)；滑动关节\(J_3\)的轴跟\(J_4\)的轴平行。
scara_mode
图.3: SCARA型机械臂的DH坐标系分配。
Scara_range_report
图.4: SCARA型机械臂工作区域俯视几何图。
图.3显示了SCARA型机械臂工作区域的俯视几何图。由图可知，机械臂工作区域边界是由\(\overparen{HAB}\)，\(\overparen{BC}\)，\(\overparen{CEG}\)，\(\overparen{GH}\)四段圆弧所围成的。而圆弧分别位于\(\odot{O_1}\)，\(\odot{O_2}\)，\(\odot{O_3}\)，\(\odot{O_4}\)四个圆上。圆心\(O_1\)，\(O_4\)与\(J_1\)轴重合；\(O_2\)和\(O_3\)分别为\(J_1\)轴到转动到正负极限时\(J_2\)轴的位置。\(j_{Max}^i\)和\(j_{Min}^i\)分别是电机轴\(J_i\)的正/负方向的最大活动范围，\(j_{Min}^i \le j_{Max}^i\)，\(\lbrace j^i \mid - \pi \le j^i \le \pi
\rbrace\)。
由几何关系，我们可以得到：
表.1: 组成工作区域的4个圆弧的几何参数
| \(\overparen{HAB}\) |
\(\odot{O_1}\) |
\({[0,0]}^{^\mathrm{T}}\) |
\(\alpha _1 + \alpha _2\) |
\(j_{Min}^1\) |
\(j_{Max}^1\) |
| \(\overparen{BC}\) |
\(\odot{O_2}\) |
\({[\alpha _1\cos(\theta _{Max}^1), \alpha
_1\sin(\theta _{Max}^1)]}^{^\mathrm{T}}\) |
\(\alpha _2\) |
\(0\) |
\(j_{Max}^2\) |
| \(\overparen{CEG}\) |
\(\odot{O_3}\) |
\({[\alpha _1\cos(\theta _{Min}^1), \alpha
_1\sin(\theta _{Min}^1)]}^{^\mathrm{T}}\) |
\(\alpha _2\) |
\(j_{Min}^2\) |
\(0\) |
| \(\overparen{GH}\) |
\(\odot{O_4}\) |
\({[0,0]}^{^\mathrm{T}}\) |
\(\left\lvert \overrightarrow {CO_4}
\right\rvert\) |
\(\theta _{Min}^4\) |
\(\theta _{Max}^4\) |
其中：
\[\begin{aligned}
\overrightarrow {CO_4} &= \left[ {\begin{array}{*{20}{c}}
{\alpha _2\cos \theta _{Max}^2\cos \theta _{Max}^1 - \alpha _2\sin
\theta _{Max}^2\sin \theta _{Max}^1 + \alpha _1\cos \theta _{Max}^1}
\newline
{\alpha _2\cos \theta _{Max}^2\sin \theta _{Max}^1 + \alpha _2\sin
\theta _{Max}^2\cos \theta _{Max}^1 + \alpha _1\sin \theta _{Max}^1}
\end{array}} \right] \newline
&= \left[ {f_x}(\theta _{Max}^1,\theta _{Max}^2), {f_y}(\theta
_{Max}^1,\theta _{Max}^2) \right]^{^\mathrm{T}} \newline
\newline
\theta _{Max}^4 &= \arctan({f_y}(\theta _{Max}^1,\theta _{Max}^2),
{f_x}(\theta _{Max}^1,\theta _{Max}^2)) \newline \newline
\theta _{Min}^4 &= \arctan({f_y}(\theta _{Min}^1,\theta _{Min}^2),
{f_x}(\theta _{Min}^1,\theta _{Min}^2))
\end{aligned}\]
假设点\(s(x,y)\)是圆\(\odot{O_i}\)边上的一个点，则点\(s\)相对于圆\(\odot{O_i}\)的弧度\(\theta ^i\)为：
\[\begin{equation}
\label{SCARA_range_e1}
\theta ^i =
\begin{cases}
\arctan(y, x), & \text{if i=1} \newline
\arctan( - x\sin \theta _{Max}^1 + y\cos \theta _{Max}^1,x\cos \theta
_{Max}^1 + y\sin \theta _{Max}^1), & \text{if i=2} \newline
\arctan( - x\sin \theta _{Min}^1 + y\cos \theta _{Min}^1,x\cos \theta
_{Min}^1 + y\sin \theta _{Min}^1), & \text{if i=3} \newline
\arctan(y, x), & \text{if i=4}
\end{cases}
\end{equation}\]
我们假设有射线\(R(u)=I+u \cdot \lVert
\mathbf{n} \rVert\)，起点为\(I\)，方向向量为\(\mathbf{n}\)，\(\lVert \mathbf{n} \rVert\)为方向向量\(\mathbf{n}\)的单位向量。如果起点在工作区域内，则射线必与某圆\(\odot{O_i}\)有交点\(\kappa _j^{i}\)，\(\kappa ^i\)为射线与圆\(\odot{O_i}\)的所有交点。\(\xi = \lbrace \kappa ^i \rbrace\)，\(\xi\)为所有交点的集合。
图.5显示了其中的一种情况，射线\(IP\)分别与圆\(\odot{O_1}\)相交于点\(N\)；与圆\(\odot{O_2}\)相交于点\(K\)和点\(M\)；与圆\(\odot{O_4}\)相交于点\(J\)和点\(L\)；与圆\(\odot{O_3}\)没有交点。我们也可以发现，点\(K\)，\(N\)虽然在圆上，但是并不在工作区域的弧线上，所以我们在得到交点后，还要查看此时的弧度\(\theta ^i\)是否满足\(\theta _{Min}^i \le \theta ^i \le \theta
_{Max}^i\)。点\(J\)为离射线起点最近的有效交点，是我们所寻找的点。
图.5: 射线IP与圆\(\odot{O_1}\)相交于点\(N\)，\(\odot{O_2}\)相交于点\(M\)、\(K\)，\(\odot{O_4}\)相交于点\(L\)、\(J\)。其中点\(J\)为射线方向的最近工作区域边缘位置。
算法流程
射线与圆的交点
为了确定与给出射线相交的最近的工作区域边缘位置，我们分别计算出射线与每一个圆的交点集\(\kappa
^i\)。求射线与圆的交点，可以使用传统的参数方程法求交点，另一种方法是使用Akenine-Möller等提出的优化法，算法流程如下：
SCARA射线圆伪代码
算法.1:Akenine-Möller优化法
该算法的优点是在计算交点前，先排除掉没有交点的情况，从而减少运算量。
整个算法流程
输入： 1. 射线\(R(u)=I+u \cdot \lVert
\mathbf{n} \rVert\) 2. 轴\(J_1\)，\(J_2\)各自的臂长\(\alpha_{1}\)，\(\alpha_{2}\)以及活动范围$j_{Max}^1 \(，\) j_{Min}^1 \(，\) j_{Max}^2 \(，\) j_{Min}^2$。
输出： 1. 最近的工作区域边缘位置\(\kappa_{intersection}\)。
SCARA边缘计算流程图
算法.2: 边缘计算流程图