一种码垛机器人的避障方法

摘要

本发明公开了一种码垛机器人的避障方法，属于机器人技术领域。本发明提供的避障方法适用于圆柱坐标式四自由度混联码垛机器人在码垛过程中避免碰撞的问题。所述的码垛过程包括第一过程码垛机器人的腕部底面中心把货物从传送带一固定位置抓取点S到抓取点正上方一点S₀；第二过程码垛机器人的腕部底面中心从抓取点正上方一点S₀到放置点上方一点T₀，需要腰座转动，水平滑块以及竖直滑块移动，同时这一过程需要判断其所走路径之内是否有障碍物，如果有需要进行避障处理；第三过程码垛机器人的腕部底面中心从放置点上方一点T₀到放置点T；第四过程码垛机器人的腕部底面中心从放置点T到抓取点S。本发明的货物码垛避障方法适应性强，简单易行。

主权项

一种码垛机器人的避障方法，其特征在于：码垛机器人码垛经过四个过程：第一过程是指码垛机器人的腕部底面中心把货物从传送带一固定位置抓取点S取下，到抓取点正上方一点S₀的过程；第二过程是指码垛机器人的腕部底面中心从抓取点正上方一点S₀到放置点上方一点T₀的过程；第三过程是指码垛机器人的腕部底面中心从放置点上方一点T₀到放置点T的过程；第四过程是指码垛机器人的腕部底面中心从放置点T到抓取点S的过程；所述的避障方法针对码垛机器人第二过程，即码垛机器人的腕部底面中心从抓取点正上方一点S₀到放置点上方一点T₀的过程，如果满足公式(19)，同时满足公式(14)～(18)中的一个或者几个，并且不满足公式(10)，才能认定码垛的路径上有障碍物；其中，公式(19)如下：$\left\{\begin{array}{c}\min \{z_S+H_1,z_r+H_2\}-c_1\le z_p+c_0\le z_{\max }\\ \max \{z_S+H_1,z_T+H_2\}\le z_{\max }\end{array}\right.---\left(19\right)$其中：z_max为码垛机器人的腕部底面中心所能抬到的最大高度，数值上等于在求解工作空间时的最高点的z坐标值；H₁和H₂是常数，视具体情况设定的值，一般是货物的2～3倍；将障碍物投影到xOy平面内，并将障碍物四条边命名为l₁、l₂、l₃、l₄，转动腰座时的轨迹最外缘方程为：$\left\{\begin{array}{c}{x}^2+y^2=r_{\max }^2\\ \begin{array}{c}\begin{array}{cc}{r}_{\max }^2{=(x_i\pm \frac{a_1}{2})}^2+{(y_i^2\pm \frac{b_1}{2})}^2& i=S,T\end{array}\end{array}\end{array}\right.---\left(12\right)$其中：r_max为货物在随腰座回转过程中到动坐标系原点O最远点；障碍物四条边线方程为：$\left\{\begin{array}{cc}y=y_P(x_P\le x\le x_P+a_0)& l_1\\ x=x_P+a_0(y_P\le y\le y_P+b_0)& l_2\\ y=y_P+b_0(x_P\le x\le x_P+a_0)& l_3\\ x=x_P(y_P\le y\le y_P+b_0)& l_4\end{array}\right.---\left(13\right)$将l₁中的y值代入(12)中得$\left\{\begin{array}{c}x=\pm \sqrt{r_{\max }^2-y_P^2}\\ x_P\le x\le x_P+a_0\end{array}\right.---\left(14\right)$将l₂中的x值代入(12)中得$\left\{\begin{array}{c}y=\pm \sqrt{r_{\max }^2-{(x_P+a_0)}^2}\\ y_P\le y\le y_P+b_0\end{array}\right.---\left(15\right)$将l₃中的y值代入(12)中得$\left\{\begin{array}{c}x=\pm \sqrt{r_{\max }^2-{(y_P+b_0)}^2}\\ x_P\le x\le x_P+a_0\end{array}\right.---\left(16\right)$将l₄中的x值代入(12)中得$\left\{\begin{array}{c}y=\pm \sqrt{r_{\max }^2-x_P^2}\\ y_P\le y\le y_P+b_0\end{array}\right.---\left(17\right)$将障碍物的四个顶点到动坐标系O‑xyz的原点O的最近距离与r_max相比较：即$\min \{\sqrt{x_P^2+y_P^2},\sqrt{{(x_P+a_0)}^2+y_P^2},\sqrt{{(x_P+a_0)}^2+{(y_P+b_0)}^2},\sqrt{x_P^2+{(y_P+b_0)}^2}\}\le r_{\max }---\left(18\right)$方程(10)为障碍物的四个顶点坐标(x_i，y_i)满足的条件：$\left\{\begin{array}{c}{y}_i-\frac{y_s\frac{a_1}{2}}{x_s-\frac{b_1}{2}}{x}_i<0,i=\mathrm{1,2,3,4}\\ y_i-\frac{y_T-\frac{b_1}{2}}{x_T+\frac{a_1}{2}}{x}_i<0,i=\mathrm{1,2,3,4}\end{array}\right.---\left(10\right)$由于码垛路径上有障碍物，故需要进行避障处理，即：先将货物抬高H₁′，使$\left\{\begin{array}{c}{z}_P+c_0<\min \{z_S+H_1,z_T+H_2\}-c_1+H_1^{\prime }\le z_{\max }\\ \max \{z_S+H_1,z_T+H_2\}\le z_{\max }\end{array}\right.---\left(20\right)$然后同时转动腰座、移动水平滑块和竖直滑块；根据公式(9)：$\left\{\begin{array}{c}{\theta }_1=\arctan \frac{y_{\mathrm{ON}}}{x_{\mathrm{ON}}}\\ {\theta }_2=-\arctan \frac{y_{\mathrm{ON}}}{x_{\mathrm{ON}}}\\ x_C=\frac{a(\sqrt{{x_{\mathrm{ON}}}^2+{y_{\mathrm{ON}}}^2}+d_1-d_3)}{b+a}\\ z_A=-\frac{c}{d}(z_{\mathrm{ON}}+d_4-d_2)\end{array}\right.---\left(9\right)$腰座转动角度为：${\theta }_1=\arctan \frac{y_T}{x_T}-\arctan \frac{y_S}{x_S},$水平滑块移动：$x_C=\frac{a(\sqrt{{x_T}^2+{y_T}^2}-\sqrt{{x_N}^2+{y_N}^2})}{b+a},$竖直滑块移动：$z_A=-\frac{c}{d}|(z_T+H_2)-z_N|,$其中：$\left\{\begin{array}{c}{z}_N=z_S+H_1+H_1^{\prime }\\ x_N=\sqrt{{x_S}^2+{y_S}^2}\sin \left(\arctan \frac{y_T}{x_T}\right)\\ y_N=\sqrt{{x_S}^2+{y_S}^2}\sin \left(\arctan \frac{y_T}{x_T}\right)\\ \frac{y_N}{x_N}=\frac{y_T}{x_T}\end{array}\right.---\left(21\right)$按照上述的过程进行，避障实现抓取点正上方一点S₀到放置点正上方一点T₀的运动；θ₁为腰座转动角度值；θ₂为腕部转角值；a,b,c,d代表长度；d₁、d₂、d₃、d₄代表距离；x_ON、y_ON、z_ON是底面中心ON的位置；a₀,b₀,c₀分别为障碍物长宽高；a₁,b₁,c₁分别为货物的长宽高；x_P，y_P，z_P为障碍物长宽高左下角在基坐标系中的坐标；x_T，y_T，z_T为T₀的坐标，x_S，y_S，z_S为S₀的坐标；x_N，y_N，z_N为T₀和S₀中间点坐标。