CN102773858A - 一种码垛机器人的避障方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于机器人技术领域，涉及一种码垛机器人的避障方法，具体地说，是指一种圆柱坐标式四自由度混联码垛机器人的自动避障方法。

背景技术

码垛机器人不仅提高产品的质量和劳动生产率，而且保障人身安全，改善劳动环境，减轻劳动强度，同时对于节约原材料消耗以及降低生产成本也有着十分重要的意义。将工业机器人技术应用于运输工业，使码垛自动化，可以加快物流速度，获得整齐一致的物垛，减少物料的破损和浪费。近年来，随着我国机械自动化水平的不断提高，码垛机器人应用逐渐广泛，并且已经形成了一种发展趋势。关于码垛机器人的相关文献如下：

参考文献[1]：张志强，臧冀原，贠超.混联码垛机器人运动学分析及仿真[J].机械设计，2010,27（11）：47-51。参考文献[2]：尹春娜.混联码垛机器人机构选型设计及最大工作空间分析[J].机电产品开发与创新，2011,24（1）：18-20。参考文献[3]：宁凤燕，贠超.MJR码垛机器人机构设计与工作空间分析[A].机械技术史及机械设计.北京：北京航空航天大学，2008：177-182。参考文献[4]：熊有伦，唐立新，丁汉.机器人技术基础[M].武汉：华中科技大学出版社，1996：1-110。参考文献[5]：余晓明.四自由度码垛机器人设计及其控制系统性能研究[D].四川：西南交通大学，2008。参考文献[6]：李伟光，许阳钊.搬运机器人的轨迹规划[J].控制与检测.2010，05：83-85。参考文献[7]：李传龙，郭刚，朱海宁.四自由度混联码垛机器人运动学分析与仿真[J].中国制造业信息化，2011，40（23）：46-48。

发明内容

在码垛机器人应用中，可能会有障碍物，本发明的目的是为了给圆柱坐标式四自由度混联码垛机器人提供一种避障的方法，以保证机器人在码垛中安全的快速运动。码垛机器人抓取箱式或袋式货物，将这些货物码成预定垛型。根据垛型或障碍物的不同，码垛机器人需尽可能快速的完成机器人动作，避免碰到障碍物。本发明提供的避障方法的特点是能够根据障碍物的位置和大小自动规划路径，达到避开障碍物快速完成码垛任务的目的。

本发明提供的避障方法适用于圆柱坐标式四自由度混联码垛机器人在码垛过程中避免碰撞的问题。所述的避障方法首先对码垛机器人各个过程进行轨迹规划，包括第一过程、第二过程、第三过程和第四过程，其中，第一过程是指码垛机器人的腕部底面中心把货物从传送带一固定位置抓取点S取下，到抓取点正上方一点S₀的过程；第二过程是指码垛机器人的腕部底面中心从抓取点正上方一点S₀到放置点上方一点T₀的过程；第三过程是指码垛机器人的腕部底面中心从放置点上方一点T₀到放置点T的过程；第四过程是指码垛机器人的腕部底面中心从放置点T到抓取点S的过程。本发明中所述的避障方法是指在第二过程，如果有障碍，需要进行如下过程来实现：

当腰座作回转运动时，在xOy平面投影，所扫过区域为一扇形，若此时障碍物不在此扇形区域内，则认为此时路径无障碍，此时障碍物四个顶点的坐标(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)$

当不满足公式（10）时，表明障碍物在扇形区域里面，但还不能说一定能在路径内，故需要满足另外条件，叙述如下：

将障碍物投影到xOy平面内，并将障碍物四条边命名为l₁、l₂、l₃、l₄，转动腰座时的轨迹最外缘方程为：

$\left\{\begin{array}{c}{x}^2+y^2=r_{\max }^2\\ \begin{array}{cc}{r}_{\max }^2={(x_i\&PlusMinus;\frac{a_1}{2})}^2+{(y_i^2\&PlusMinus;\frac{b_1}{2})}^2& i=S,T\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=\&PlusMinus;\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}x=\&PlusMinus;\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=\&PlusMinus;\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=\&PlusMinus;\sqrt{r_{\max }^2-x_p^2}\\ y_P\&le;y\&le;y_P+a_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)$

当公式（14）~（18）其中一个或几个成立时，即可判断出xOy平面投影上路径有障碍。但还需要在xOz平面上进行高度上的判断，分析如下：

$\left\{\begin{array}{c}\min \{z_S+H_1,z_T+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坐标值。这里规定：当障碍物在xOy平面的投影位于以r_max为半径的扇形区域的内部时，由于小臂和前大臂的移动以及腰座的旋转可能与障碍物有干涉，但当障碍物高度过低时也是安全的，为了讨论方便，将当障碍物在xOy平面的投影位于以r_max为半径的扇形的内部时的情况视为有障碍处理。

故当满足公式（19）同时满足公式（14）~（18）中的一个或者几个，并且不满足公式（10），才能认定码垛的路径上有障碍物。由于码垛路径上有障碍物，故需要进行避障处理，即：先将货物抬高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）可知，腰座转动角度为： ${\mathrm{\&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}\cos \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₀的运动。

本发明的优点在于：

（1）适应性强。本发明解决了码垛机器人避障问题，使其具有适应不同工作环境的能力。

（2）算法结构简单。本发明根据机器人腕部底面中心所经的位置和障碍物得出避障算法，简单易行。

附图说明

图1是圆柱坐标式四自由度混联码垛机器人的机械结构示意图；

图2是圆柱坐标式四自由度混联码垛机器人的机构简图；

图3是本发明提供的避障方法的过程示意图；

图4是避障方法中第二过程中在xOy平面内避障算法的原理图；

图5是将障碍物投影到xOy平面的投影图。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。

本发明提供的一种码垛机器人的避障方法是针对圆柱坐标式四自由度混联码垛机器人的自动避障方法，所述的圆柱坐标式四自由度混联码垛机器人可以参照背景技术部分的参考文献，基本的机械结构如图1所示，所述的圆柱坐标式四自由度混联码垛机器人包括腰座1、腰部旋转轴2、水平关节导轨3、前后大臂连杆4、辅助连杆5、前大臂6、三角架7、小臂8、辅助连杆9、腕部10、垂直关节导轨11、后大臂12和辅助连杆13。其中腰座1是整个机构的基础，腰部旋转轴2可以相对腰座转动。所述的水平关节导轨3和垂直关节导轨11分别固定在腰部旋转轴2上，可以随腰座转动。前大臂6和后大臂12分别与两个导轨滑块铰接在一起，通过两个导轨滑块在水平关节导轨3和垂直关节导轨11中的运动，实现前大臂6在水平面内的调节，和后大臂12在竖直平面内的调节。所述的小臂8一端连接前大臂6和后大臂12，另一端连接腕部10，通过滚珠丝杠的驱动实现小臂8位姿的调整。

码垛工艺要求在码垛过程中，货物在水平方需要保持水平。为始终保持腕部10的水平位置，可采用平行四边形辅助连杆机构实现此功能。腕部姿态调整机构由两个平行四边形连杆机构和一个三运动副杆(三角支架)组成。腕部10、小臂8、三角支架7和辅助连杆9组成第一个平行四边形机构；前大臂6、三角支架7、辅助连杆13和辅助连杆5组成第二个平行四边形机构。辅助连杆9分别与腕部10和三角支架7铰接；辅助连杆13分别与三角支架7和辅助连杆5铰接；辅助连杆5分别与水平滑导轨3上的水平滑块轴和辅助连杆13铰接；三角架与前大臂6和辅助连杆9、13铰接。

图2为图1所述的混联码垛机器人机构简图，图中自由度及坐标系设定说明如下：

混联码垛机器人能实现以下4种运动：腰座旋转(绕z₀轴)，旋转角度记为θ₁；水平导轨滑块C运动(沿x轴)，运动位移记为x_C；垂直导轨滑块A上下运动(沿z轴)，运动位移记为z_A；腕部10回转运动(绕u轴)，回转角度记为θ₂。

以腰座1上表面中心为原点O₀，垂直于腰座1上表面向上的方向为z₀轴正方向，按照右手法则建立笛卡尔坐标系O₀-x₀y₀z₀，为基坐标系；以水平导轨滑块C和垂直导轨滑块A的滑道延长线交点为原点O，垂直于腰座1上表面向上的方向为z轴正方向，按照右手法则建立笛卡尔坐标系O-xyz，此坐标系随同腰座1绕z₀轴一起转动，为动坐标系。

图2中，后大臂12对应AD段，CE段对应为前大臂6，DF段对应为小臂8。DF段与CE段铰接在E点，BC段与AD段铰接在B点，BDEC组成一个平行四边形。

混联码垛机器人在进行码垛的过程中如图3所示，码垛经过四个点：抓取点S、抓取点正上方一点S₀、放置点正上方一点T₀和放置点T。

抓取点S为抓取货物的点，是个固定点；抓取点上方一点S₀是抓取货物后将货物移动到一个安全位置的点，是个固定点，抓取点S与抓取点正上方一点S₀之间的高度差是2~3倍货物的高度；放置点T为货物码放在栈板上的码放点，是个变化点；放置点正上方一点T₀是为了平稳的将货物放在目标位置上而设计的一个速度变化点，这个点随着放置点T的变化而变化；S到S₀的过程称为第一过程，这一过程只需将腕部底面中心抬高；S₀到T₀的过程称为第二过程，这一过程比较复杂，需要腰座1上的转动轴2转动，水平滑块以及竖直滑块移动，同时这一过程需要判断其所走路径之内是否有障碍物，如果有需要进行避障处理；从T₀到T的过程称为第三过程，只需将机械手下放一定距离即可；T到S的过程称为第四过程，即回程过程；第一过程、第二过程、第三过程称为行程过程。

本发明提供一种适用于码垛机器人的避障方法，所述的避障方法基于上述的码垛机器人在码垛过程中遇到障碍物的情形提出的，避障方法步骤如下：

步骤一：算出机器人正解。

在动坐标系O-xyz中，不考虑腕部10旋转时，码垛机器人的自由度为2。当a，b,c,d长度一定时，图2中腕部10与小臂8的铰接点F的运动规律仅取决于移动副原动件竖直导轨滑块A和水平导轨滑块C的运动规律。先在y=0平面内讨论，在动坐标系O-xyz中分析F(x_F，0，z_F)与z_A，x_C之间的运动关系。关节坐标：A(0，0，z_A)，C(x_C，0，0)，B(x_B，0，z_B)，D(x_D，0，zD)，E(x_E，0，z_E)，F(x_F，0，z_F)。角度α为AD与x轴正方向的夹角，β为DF与x轴负方向的夹角；长度CE为b，AD为a+b，BC（或DE）为c，DF为c+d；基坐标原点O₀位于动坐标原点O偏右d₁，偏下d₂的位置，腕部10底面中心O_N位于F点偏右d₃，偏下d₄的位置，O_N点垂直向上方向为u轴。根据上述的几何关系，有如下的方程成立：

$\left\{\begin{array}{c}{x}_B=a\cos \mathrm{\&alpha;}\\ z_B=z_A+a\sin \mathrm{\&alpha;}\end{array}\right.---\left(1\right)$

$\left\{\begin{array}{c}{x}_C=a\cos \mathrm{\&alpha;}+c\cos \mathrm{\&beta;}\\ z_C=c\sin \mathrm{\&beta;}-a\sin \mathrm{\&alpha;}\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{x}_E=x_C+b\cos \mathrm{\&alpha;}\\ z_E=b\sin \mathrm{\&alpha;}\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}{x}_F=x_E+d\cos \mathrm{\&beta;}\\ z_F=z_E-d\sin \mathrm{\&beta;}\end{array}\right.---\left(4\right)$

由式（1）~（4），并令a×d＝b×c，得到：

$\left\{\begin{array}{c}{x}_F=(1+\frac{b}{a})x_C\\ z_F=-\frac{d}{c}{z}_A\end{array}\right.---\left(5\right)$

由公式（5）可知，当满足条件a×d＝b×c，a，b,c,d一定时，腕部10与小臂8的铰接点F的位置只与水平导轨滑块C和竖直导轨滑块A的位置有关，即x_F、z_F的值只取决于x_C、z_A的值。

不考虑腰座1绕z₀轴转动以及腕部10绕u轴转动的情况下，由公式（5）可知O_N在动坐标系O-xyz中的坐标为（此时y=0）：

$\left\{\begin{array}{c}{x}_F=(1+\frac{b}{a})x_C+d_3\\ y_F=0\\ z_F=-\frac{d}{c}{z}_A-d_4\end{array}\right.---\left(6\right)$

通过坐标的齐次变换可得O_N在基座标系O₀-x₀y₀z₀中的坐标为：

$\left[\begin{array}{c}{x}_{\mathrm{ON}}\\ y_{\mathrm{ON}}\\ z_{\mathrm{ON}}\\ 1\end{array}\right]=\left[\begin{array}{c}\left[(1+\frac{b}{a})x_C+d_3-d_1\right]\cos {\mathrm{\&theta;}}_1\\ [(1+\frac{b}{a})x_C+d_3-d_1]{\sin \mathrm{\&theta;}}_1\\ -\frac{d}{c}{z}_A-d_4+d_2\\ 1\end{array}\right]---\left(7\right)$

由此便得出O_N在基坐标系O₀-x₀y₀z₀中的坐标值。可以看出x_ON和y_ON的值取决于水平滑块的位置x_C和腰座旋转角度θ₁的值，而z_ON只由z_A决定。

步骤二：算出机器人逆解。

逆解是已知机器人腕部底面中心O_N的位置，即x_ON、y_ON、z_ON，求出广义坐标θ₁(腰座转角)、θ₂(腕部转角)、x_C(水平滑块位移)、z_A(竖直滑块位移)的值。假定码垛机器人在搬运过程中始终能够保持货物方位不变，则：

θ₁+θ₂=0                    （8）

将公式（7）和公式（8）联立，解得：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=\arctan \frac{y_{\mathrm{ON}}}{x_{\mathrm{ON}}}\\ {\mathrm{\&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)$

公式（9）即为码垛机器人腕部底面中心O_N的位置逆解。可以看出，θ₁只与x_ON和y_ON有关，而θ₂正好与θ₁相反，反映了货物姿态相对不变，x_C与x_ON、y_ON都有关系，且不同的x_ON、y_ON的值可以对应相同的x_C，z_A的值只与z_ON有关，故z_A与θ₁、θ₂、x_C三个输入在运动学上解耦，方便控制。

步骤三：对码垛机器人各个过程进行轨迹规划。

码垛机器人工作过程就是自动码垛的过程，即利用机器人腰座1、手臂（包括前大臂6、后大臂12和小臂8）、腕部10的协调运动，把货物从传送带一固定位置抓取点S取下，按照一定顺序自动将其码放到栈板上合适位置即放置点T的过程。码放过程如图3所示。

针对每个过程对其轨迹进行分别讨论，以达到对码放的整个过程实施轨迹规划的目的，具体如下：

第一过程：O_N从抓取点S到抓取点正上方一点S₀的过程。

设抓取点S在基坐标系O₀-x₀y₀z₀中的坐标为S（x_S，y_S，z_S），抓取点正上方一点S₀在基坐标系O₀-x₀y₀z₀中的坐标为S₀（x_S，y_S，z_S+H₁）。H₁为一常数，视具体情况自己选定，一般取2~3倍货物高度。只需移动竖直滑块，由公式（9）可知移动的距离为即竖直滑块向下运动

即可完成从抓取点S到抓取点正上方一点S₀的运动。

第二过程：O_N从抓取点正上方一点S₀到放置点上方一点T₀的过程。

设放置点上方一点T₀在基坐标系O₀-x₀y₀z₀中的坐标为T_O（x_T，y_T，z_T+H₂），而抓取点上方一点S₀的坐标为S₀（x_S，y_S，z_S+H₁），由于两点的x坐标、y坐标、z坐标都不相同，故腰座、水平滑块、竖直滑块都需要转动或移动才能达到要求。为了提高码垛机器人工作效率让三个轴联动，即腰座、水平滑块、竖直滑块同时运动，从抓取点上方一点S₀（x_S，y_S，z_S+H₁）直接到达放置点上方一点T₀（x_T，y_T，z_T+H₂），此过程中为了计算方便设一中间点N（x_N，y_N，z_N）。

假设障碍物为长宽高分别平行于基坐标系O₀-x₀y₀z₀的三条坐标轴，并令其左下角在基坐标系中的坐标为P(x_P，y_P，z_P)，其长宽高分别为a₀,b₀,c₀；货物的长宽高分别为a₁,b₁,c₁，搬运过程中货物的长宽高也分别平行于基坐标系的三条坐标轴；认为O_N与货物上表面中心重合。具体过程分如下两种情况：

A、路径无障碍：

当腰座作回转运动时，在xOy平面投影，所扫过区域为一扇形。若此时障碍物不在此扇形区域内，则认为此时路径无障碍，下面分析不构成障碍的条件，如图4所示，此时需满足：

$\left\{\begin{array}{cc}{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)$

令障碍物在xOy平面投影的四个顶点坐标为(x_i，y_i)，i＝1,2,3,4；如图4所示，将左下角定为点1，逆时针方向依次定义为点2、3、4；则(x₁,y₁)＝(x_P,y_P)，(x₂,y₂)＝(x_P+a₀,y_P)，(x₃,y₃)＝(x_P+a₀,y_P+b₀)，(x₄,y₄)＝(x_P,y_P+b₀)。此时同时转动腰座、水平滑块、竖直滑块，三个轴联动使O_N从点S₀到点T₀，根据式（9）可知：腰座转动角度为：水平滑块移动： $x_C=\frac{a(\sqrt{{x_T}^2+{x_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\\ x_N=\sqrt{{x_S}^2+{y_S}^2}\cos \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(11\right)$

B、路径有障碍：

当不满足公式（10）时，表明障碍物在扇形区域里面，但还不能说一定能在路径内，故需要满足另外条件，叙述如下：

如图5所示：将障碍物投影到xOy平面内，则四个顶点的坐标便可知，并将障碍物四条边命名为l₁、l₂、l₃、l₄。

转动腰座时的轨迹最外缘方程为： $\left\{\begin{array}{c}{x}^2+y^2=r_{\max }^2\\ \begin{array}{cc}{r}_{\max }^2={(x_i\&PlusMinus;\frac{a_1}{2})}^2+{(y_i^2\&PlusMinus;\frac{b_1}{2})}^2& i=S,T\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=\&PlusMinus;\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}x=\&PlusMinus;\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=\&PlusMinus;\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=\&PlusMinus;\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)$

当公式（14）~（18）其中一个或几个成立时，即可判断出xOy平面投影上路径有障碍。但还需要在xOz平面上进行高度上的判断，分析如下：

$\left\{\begin{array}{c}\min \{z_S+H_1,{z_T+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坐标值。这里规定：当障碍物在xOy平面的投影位于以r_max为半径的扇形区域的内部时，由于图1中小臂8和前大臂6的移动以及腰座的旋转可能与障碍物有干涉，但当障碍物高度过低时也是安全的，为了讨论方便，将当障碍物在xOy平面的投影位于以r_max为半径的扇形的内部时的情况视为有障碍处理。

故当满足公式（19）同时满足公式（14）~（18）中的一个或者几个，并且不满足公式（10），才能认定码垛的路径上有障碍物。由于码垛路径上有障碍物，故需要进行避障处理，即：先将货物抬高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）可知，腰座转动角度为：水平滑块移动： $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}\cos \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₀的运动。

第三过程：腕部底面中心从放置点上方一点T₀到放置点T的过程。

此过程类似于第一过程，就是将货物下放一高度即可，竖直滑块移动就能满足要求，参照第一过程，根据公式（9），此时竖直滑块移动向上运动

即可。

第四过程：O_N从放置点T到抓取点S的过程。

此过程即回程过程，由于此时没有货物，故路径可与行程路径相同，沿着第一过程、第二过程、第三过程的路径逆向运动，只是速度可略快，以便提高效率。以后便是第一过程、第二过程、第三过程、第四过程的循环过程。

上述的货物码垛避障方法适应性强，适用于所有具有上述的基本特征的混联码垛机器人，对于码垛机器人在第二过程可能遇到障碍物的情形，本发明提供了一种避障方法，该方法对第二过程中的障碍物进行有效判断，然后执行避障，解决了码垛机器人避障问题，使其具有适应不同工作环境的能力。本发明所提供的避障方法简单易行，具有很好的应用前景。

Claims (3)

1.一种码垛机器人的避障方法，其特征在于：所述的避障方法针对码垛机器人第二过程，即码垛机器人的腕部底面中心从抓取点正上方一点S₀到放置点上方一点T₀的过程，如果满足公式（19），同时满足公式（14）~（18）中的一个或者几个，并且不满足公式（10），才能认定码垛的路径上有障碍物；其中，公式（19）如下：

$\left\{\begin{array}{c}\min \{z_S+H_1,z_T+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坐标值；

将障碍物投影到xOy平面内，并将障碍物四条边命名为l₁、l₂、l₃、l₄，转动腰座时的轨迹最外缘方程为：

$\left\{\begin{array}{c}{x}^2+y^2=r_{\max }^2\\ \begin{array}{cc}{r}_{\max }^2={(x_i\&PlusMinus;\frac{a_1}{2})}^2+{(y_i^2\&PlusMinus;\frac{b_1}{2})}^2& i=S,T\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=\&PlusMinus;\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}x=\&PlusMinus;\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=\&PlusMinus;\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=\&PlusMinus;\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}{\mathrm{\&theta;}}_1=\arctan \frac{y_{\mathrm{ON}}}{x_{\mathrm{ON}}}\\ {\mathrm{\&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)$

腰座转动角度为：

${\mathrm{\&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}\cos \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₀的运动。

2.根据权利要求1所述的一种码垛机器人的避障方法，其特征在于：码垛机器人码垛经过四个过程：第一过程是指码垛机器人的腕部底面中心把货物从传送带一固定位置抓取点S取下，到抓取点正上方一点S₀的过程；第二过程是指码垛机器人的腕部底面中心从抓取点正上方一点S₀到放置点上方一点T₀的过程；第三过程是指码垛机器人的腕部底面中心从放置点上方一点T₀到放置点T的过程；第四过程是指码垛机器人的腕部底面中心从放置点T到抓取点S的过程。

3.根据权利要求1所述的一种码垛机器人的避障方法，其特征在于：在进行所述的避障方法前需要先算出机器人的正解和逆解，具体为：

步骤一：算出机器人正解：

在动坐标系O-xyz中，不考虑腕部旋转时，码垛机器人的自由度为2，当a，b,c,d长度一定时，腕部与小臂的铰接点F的运动规律仅取决于移动副原动件竖直导轨滑块和水平导轨滑块的运动规律，先在y=0平面内讨论，在动坐标系O-xyz中分析F(x_F，0，z_F)与z_A，x_C之间的运动关系，关节坐标：A(0，0，z_A)，C(x_C，0，0)，B(x_B，0，z_B)，D(x_D，0，z_D)，E(x_E，0，z_E)，F(x_F，0，z_F)。角度α为AD与x轴正方向的夹角，β为DF与x轴负方向的夹角；长度CE为b，AD为a+b，BC为c，DF为c+d；基坐标原点O₀位于动坐标原点O偏右d₁，偏下d₂的位置，腕部底面中心O_N位于F点偏右d₃，偏下d₄的位置，O_N点垂直向上方向为u轴，根据上述的几何关系，有如下的方程成立：

$\left\{\begin{array}{c}{x}_B=a\cos \mathrm{\&alpha;}\\ z_B=z_A+a\sin \mathrm{\&alpha;}\end{array}\right.---\left(1\right)$

$\left\{\begin{array}{c}{x}_C=a\cos \mathrm{\&alpha;}+c\cos \mathrm{\&beta;}\\ z_C=c\sin \mathrm{\&beta;}-a\sin \mathrm{\&alpha;}\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}{x}_E=x_C+b\cos \mathrm{\&alpha;}\\ z_E=b\sin \mathrm{\&alpha;}\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}{x}_F=x_E+d\cos \mathrm{\&beta;}\\ z_F=z_E-d\sin \mathrm{\&beta;}\end{array}\right.---\left(4\right)$

由式（1）~（4），并令a×d＝b×c，得到：

$\left\{\begin{array}{c}{x}_F=(1+\frac{b}{a})x_C\\ z_F=-\frac{d}{c}{z}_A\end{array}\right.---\left(5\right)$

由公式（5）可知，当满足条件a×d＝b×c，a，b,c,d一定时，腕部与小臂的铰接点F的位置只与水平导轨滑块和竖直导轨滑块的位置有关，即x_F、z_F的值只取决于x_C、z_A的值；

不考虑腰座绕z₀轴转动以及腕部绕u轴转动的情况下，由公式（5）知O_N在动坐标系O-xyz中的坐标为：

$\left\{\begin{array}{c}{x}_F=(1+\frac{b}{a})x_C+d_3\\ y_F=0\\ z_F=-\frac{d}{c}{z}_A-d_4\end{array}\right.---\left(6\right)$

通过坐标的齐次变换得O_N在基座标系O₀-x₀y₀z₀中的坐标为：

$\left[\begin{array}{c}{x}_{\mathrm{ON}}\\ y_{\mathrm{ON}}\\ z_{\mathrm{ON}}\\ 1\end{array}\right]=\left[\begin{array}{c}\left[(1+\frac{b}{a})x_C+d_3-d_1\right]\cos {\mathrm{\&theta;}}_1\\ [(1+\frac{b}{a})x_C+d_3-d_1]{\sin \mathrm{\&theta;}}_1\\ -\frac{d}{c}{z}_A-d_4+d_2\\ 1\end{array}\right]---\left(7\right)$

由此便得出O_N在基坐标系O₀-x₀y₀z₀中的坐标值；

步骤二：算出机器人逆解：

逆解是已知机器人腕部底面中心O_N的位置，即x_ON、y_ON、z_ON，求出广义坐标腰座转角θ₁、腕部转角θ₂、水平滑块位移x_C、竖直滑块位移z_A的值，假定码垛机器人在搬运过程中始终能够保持货物方位不变，则：

θ₁+θ₂=0               （8）

将公式（7）和公式（8）联立，解得：

公式（9）即为码垛机器人腕部底面中心O_N的位置逆解。

Images