delta 机器人和传送带之间坐标的转换

传送带标定

传送带标定(conveyor calibration)就是确定传动带坐标系相对于机器人坐标系的位姿(位置和姿势,姿势包含以自身中心为原点的 x 轴,y 轴,z 轴的旋转角度,分别称为 pitch,yaw,roll,在航空中,pitch是围绕 x 轴旋转,也叫做俯仰角;yaw 是围绕 y 轴旋转,也叫偏航角;roll 是围绕Z轴旋转,也叫翻滚角)。
file
角类型的参考网址:https://www.jianshu.com/p/1e38ad3d4bfb

小葱科技的 delta 机器人坐标系

坐标系定义:结构是一个三角形,y 轴的负方向有主动臂,即 y 轴已确定,其可动范围为 -350 毫米到 350 毫米;y 轴正方向顺时针转 90 度即为 x 轴正方向,x 轴的可动范围与 y 轴相同;z 轴正方向是向上的,机器手臂是在 z 轴负方向的,在 z 轴的可动范围为:-520 毫米到 -400 毫米。

传送带坐标系和机器人坐标系的转换

设机器人坐标系为 R,传送带坐标系为 C,那么传送带坐标系相对于机器人坐标系的关系可通过转换矩阵 ${C}^{R}\textrm{T}$ 来表示。${C}^{R}\textrm{T}$ 由平移向量和旋转矩阵组成。如果知道这个转换矩阵和物体在传送带坐标系下面的位置 $^{C}\textrm{P}$ (C表示传送带),那么就能确定该物体在机器人坐标系下面的位置:

$$^{R}\textrm{P}=_{C}^{R}\textrm{T}\cdot ^{C}\textrm{P} (3.4.1)$$

$^{R}\textrm{P}$ :机器人的位置。$ _{C}^{R}\textrm{T} $:从传送带坐标系到机器人坐标系的转换矩阵。

传送带的标定实质就是确定 $ _{C}^{R}\textrm{T} $ 这个转换矩阵和传送带编码器的比例因子(encoder factor)。

所谓的传送带编码器比例因子就是传送带移动一段距离,这段距离分别在机器人坐标系和编码器上对应了一个变化值,而这两个变化值的比例关系即为比例因子。

传送带的标定方法

  1. 在传送带上确定一点 P ,移动 Delta 机器人,将其末端触碰到该点 P,记录传送带的编码器值为 $V{e1}$ 。
    机器人此时的末端位姿为 $^{R}\textrm{P}_{1}$(x1,y1,z1,yaw1,pitch1,roll1),注:小葱科技机器人的位姿只有前三个坐标系参数,外加一个吸盘状态。

  2. 移动传送带,在保证机器人可以再次触碰到 P 的条件下,让这段移动距离越大越好。再次移动机器人,使末端触碰到P点。记录编码器的值为 $V{e2}$ 。
    机器人末端的位姿为 $^{R}\textrm{P}_{2}$ (x2,y2,z2,yaw2,pitch2,roll2)。

  3. 让机器人沿着传送带垂直的方向移动一段距离,记录机器人末端的位姿 $^{R}\textrm{P}_{3}$ (x3,y3,z3,yaw3,pitch3,roll3)
    file

$O_{c}$ 点在 P1 和 P2 线段之间,可在机器人手到达范围内选取一点。

通过上述步骤之后,可以得到编码器比例因子:

$$\Delta L_{R} = \sqrt{\left ( x_{2} - x_{1} \right )^{2} + \left ( y_{2} - y_{1} \right )^{2} + \left ( z_{2} - z_{1} \right )^{2}} (3.4.2)$$$$\Delta L_{C} = V_{e2} - V_{e1} (3.4.3)$$

比例因子

$$Factor_{c} = \frac{\Delta L_{R}}{\Delta L_{C}} (3.4.4)$$

说明:这里x1,y1,z1等是机器人坐标系下的坐标。
根据式(3.4.4),当测量出传送带移动一段距离两个位置编码器的读数 $V{e2}$ 和 $V{e1}$ ,即可确定机器人移动的距离 $\Delta L_{R}$。

$$\Delta L_{R} = Factor_{c}\left ( V_{e2} - V_{e1} \right ) (3.4.5)$$

$$O_{C}^{R} = \left ( x_{C_{0}}^{R},y_{C_{0}}^{R},z_{C_{0}}^{R} \right)$$

为传送带坐标系原点相对于机器人坐标系的点,有:

$$\begin{cases} & \left ( P_{3}^{R}-O_{C}^{R} \right )\cdot \left ( P_{2}^{R}- P_{1}^{R}\right )=0 \\ & \left \| \left ( P_{1}^{R}-O_{C}^{R} \right )\times \left (P_{2}^{R}- P_{1}^{R} \right ) \right \|=0 \\ & \left ( \left ( P_{3}^{R}-O_{C}^{R} \right )\times \left ( P_{2}^{R}- P_{1}^{R} \right )\right )\cdot \left ( P_{2}^{R}- P_{1}^{R} \right ) = 0 \end{cases}$$

通过上式可以求得:

$$O_{C}^{R} = \left ( x_{C_{0}}^{R},y_{C_{0}}^{R},z_{C_{0}}^{R} \right)$$

其中:
file (3.4.6)

(3.4.6)的公式实在太长,懒得 输了^_^

得到传送带基坐标系的表达式:

$$\begin{cases} & \overrightarrow{X_{C}}=\frac{\left ( P_{2}^{R}-O_{C}^{R} \right )}{\left \| \left ( P_{2}^{R}-O_{C}^{R} \right ) \right \|} \\ & \overrightarrow{Y_{C}}=\frac{\left ( P_{3}^{R}-O_{C}^{R} \right )}{\left \| \left ( P_{3}^{R}-O_{C}^{R} \right ) \right \|} \\ & \overrightarrow{Z_{C}}=\overrightarrow{X_{C}}\times \overrightarrow{Y_{C}} \end{cases}$$

(3.4.7)

式(3.4.5)和式(3.4.7)可以求得传送带坐标系与机器人坐标系的转换矩阵 $H_{C}^{R}$ 。

$$H_{C}^{R}=\begin{bmatrix} \overrightarrow{X_{C}}& \overrightarrow{Y_{C}}& \overrightarrow{Z_{C}}& O_{C}^{R}\\ 0& 0& 0& 1\end{bmatrix}$$

(3.4.8)
传送带移动的方向为传送带坐标系的 $X_{C}$ 轴方向,相机测量在传送带坐标下初始点为 $P^{C}$ ,及测量传送带移动一段距离两个位置编码器的读数,则由式(3.4.1),式(3.4.5)和式(3.4.8)可以确定终点在机器人坐标系下的位置 $P^{R}$。

$$P^{R} = H_{C}^{R} Trans_{X,\Delta L} P^{C}$$

其中

$$Trans_{X,\Delta L}=\begin{bmatrix} 1& 0& 0& \Delta L_{R}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1 \end{bmatrix}$$

$P^{C}$ 的坐标:在确定传输带坐标系后,测量坐标系原点到相机测量传送带的初始位置的距离即可求得x轴的值;y轴的值也可根据传送带坐标系来确定;z轴的值表示的是物体厚度(这个怎么测?)。

参考文献

DELTA机器人传送带与视觉的综合标定方法,杨强,刘冠峰;
并联Delta机器人的传送带动态抓取系统设计,邓明星。

追求梦想,做最好的自己