关节型码垛机器人自主避障的方法研究

1 引言

研究码垛机器人的轨迹规划是提高码垛效率的重要环节，可以给生产生活带来巨大的经济效益。通常在设计码垛机器人工作路径时，往往假设抓取位置和垛盘之间不存在障碍物[1]。然而，在实际码垛作业中，抓取位置和垛盘之间可能存在障碍物，给码垛作业带来了困难，因此对码垛机器人的避障规划研究就显得尤为重要。目前解决避障问题的方法主要有概率路标图法，单元分解法、人工势场法、势场栅格法等[2]，上述方法主要是用来解决移动机器人的避障问题，而码垛机器人属于关节型机器人，因此这些方法不能直接用来解决码垛机器人的避障问题。

以一种新型码垛机器人为研究对象，首先分析研究其运动学模型及其工作空间，再通过分析障碍物信息后提出自主避障的方法策略，最后基于OpenGL仿真软件验证该方法的正确性。

2 码垛机器人运动学模型及运动空间

新型码垛机器人的机械结构，如图1所示。其基本组成部件包括机架、平行四边形机构、前臂、后臂、主臂、腕部、末端执行器等[3]。在机器人运动过程中，通过两个平行四边形机构保证末端执行器时刻处于水平位置。为保证机器人的精确控制，均采用电动机进行传动。驱动轴Ⅰ带动机器人腰部回转，实现机器人的转向；驱动轴Ⅱ通过小平行四边形机构力的传递带动机器人后臂转动，实现机器人主臂的上下运动；驱动轴Ⅲ带动机器人前臂转动，实现机器人工作范围在前后方向上的变化；驱动轴Ⅳ实现机器人腕部的回转。

2.1 正运动学分析

新型码垛机器人机构运动简图，如图2所示。根据解析几何法建立坐标系，确定末端执行器的位姿与各连杆参数间的关系[4]。

图1 新型码垛机器人机械结构图
Fig.1 Mechanical Structure of New Palletizing Robot

图2 新型码垛机器人机构运动简图
Fig.2 A Diagram of Movement of New Palletizing Robot

以A点为坐标原点，小平行四边形ABCD所在的平面为坐标平面，建立一个动坐标系O-xyz，动坐标系连同机器人各连杆绕着机架旋转。图中D点坐标为D（-300，-300），θ2为杆AB与x轴正方向的夹角，由于ABCD为平行四边形，故CD杆与x轴正方向的夹角也等于θ2，θ3为杆DF与x轴正方向的夹角。由坐标运算法通过计算点A，BC，D，E，F，G的坐标可以进一步求出机器人末端执行器形心ON在动坐标系中的坐标位置。如图2所示，在机器人机架上建立全局参考坐标系O0-x0y0z0，然后通过点的齐次坐标变换法得到在全局参考坐标系O0-x0y0z0中的坐标：

式中：sθi—sinθi；cθi—cosθi。

末端操作器的姿态变化是在全局参考坐标系O0-x0y0z0中绕z0轴旋转（θ1+θ2），由此可得机器人的运动学位姿方程为[5]：

其中，

2.2 逆运动学分析

逆运动学分析是已知机器人末端操作器形心ON在全局参考坐标系中的位置，求关节角θ1（驱动轴Ⅰ转角）、θ2（驱动轴Ⅱ轴角）、θ3（驱动轴Ⅲ轴角）、θ4（驱动轴Ⅳ转角）的过程[6]。

（1）θ1和 θ4求解

根据式（1）计算出θ1的值。假定搬运过程中货物的方位不变，需满足条件 θ1+θ4=0。

最终计算结果如式（3）所示：

（2）θ2的求解

整理式（2）中xON和yON的表达式得：

两式相除后整理得：

则

（2）θ3的求解

根据式（2）中zON的表达式得：

其中，m=（ZON+a3-a4）（lFE/lEG）-sθ2lCD-300

2.3 码垛机器人的工作空间

只有在机器人工作空间范围内进行轨迹规划才具有意义，故需要对其工作空间进行分析。根据前述机器人运动学位姿方程式（2），可以通过MATLAB以绘图的方式描述其工作范围[7]。机器人的基本结构参数，如表1所示。将参数代入式（2）进行绘图操作。

表1 码垛机器人基本结构参数
Tab.1 The Basic Structural Parameters of Robot Palletizer

如图3（a）和图3（b）所示，为了更清楚的表达码垛机器人工作空间的作用范围，可以通过码垛机器人可达工作空间的三维图进一步得到xoy坐标平面图和xoz坐标平面图。根据图中信息可以得到此码垛机器人在垂直方向上运动范围为［-2600mm，2300mm］，在水平方向上为一圆环区域，内外半径分别为|1000|mm和|4100|mm。

3 码垛机器人的避障路径规划

码垛机器人一般采用PTP的形式进行码放作业。码放货物时，码垛机器人通过其腰座、前臂、后臂、主臂、腕部的协调运动，按照图4所示的方式完成货物的抓取与放置。整个操作过程可以分成4个步骤，首先是末端操作器将货物从抓取点S抓取后运动到抓取预备点，然后从点运动到放置预备点T0，再从T0点运动到放置点T，最后末端操作器重新返回到抓取点S的位置[8]。由于step1和step4的操作过程较为简单，故需要重点分析码放过程中step2和step3两个阶段。

图3 机器人在不同坐标平面内的工作范围
Fig.3 The Working Range of Robots in Different Coordinate Planes

图4 机器人的码放作业流程图
Fig.4 The Operating Flow Chart of Robot Palletizing

3.1 障碍物信息分析

障碍物信息分析是进行避障规划的关键步骤，按照前述方法建立全局参考坐标系O0-x0y0z0。考虑到现场工作环境的复杂性，障碍物可能是形状不规则的物体，为了方便分析，可以利用立体包络的方法将其转化为规则物体。图5所示的障碍物1和2为不规则形状，采用最小外接长方体将其包络。该长方体的底面在坐标系x0o0y0平面中，假设该长方体的长宽高分别为a0，b0，c0。如图5所示，中间有交叉线的立方体为需要码放的货物，货物的长宽高分别为a11，b11，c11，为了简化计算将ON设定为与货物的上表面中心处重合。分析障碍物信息时，仅以其中一种障碍物作为研究对象。将障碍物向坐标系x0o0y0平面内投影，投影形状为矩形。设平面内投影矩形的四个顶点坐标为 A（x1，y1，z0），B（x2，y2，z0），C（x3，y3，z0），D（x4，y4，z0）矩形的四条边分别用 L1，L2，L3，L4 表示。将四个顶点分别与坐标原点连接后得到四条线段，如图5中虚线所示。这四条线段与 x0轴正方向所夹的角分别表示为 θ11，θ22，θ33，θ44。

图5 码放作业中避障规划原理图
Fig.5 Mapping of Obstacle Avoidance Planning in Palletizing Operations

障碍物存在的区域可以用 θ∈［θmin，θmax］来表示，其中，θmin，θmax按照式（5）取值[9]。

规定，

当 θmax-θmin≥180°，令：

其中，

综上所述，障碍物存在的区域表示为θ∈［θmin，θmax］。图5中障碍物1存在的范围为∠Do0x0～∠Bo0x0。

障碍物的位置信息还需要通过dmin（坐标原点o0到投影矩形四条边的最短距离）来确定。如图5所示，通过点o0做线段AB的垂线交点为M，Mo0为最短距离。由于投影矩形的四个顶点坐标已知，可以进一步求出矩形四条边的直线方程，然后再根据点到直线方程的距离公式，求解出点o0到矩形四边的最短距离ds。

由于可能求解了o0到线段延长线的距离，所得结果并不一定是原点o0到障碍物的最短距离，因此需要进一步求出顶点A，B，C，D到o0的距离di（i=A，B，C，D）。

式中：xj，yj—顶点的坐标值。

则原点到障碍物投影区域的最短距离dmin：

综合以上分析即可得到障碍物的相关信息，式（5）和式（7）表示了障碍物在坐标系x0o0y0平面内投影后的角度域和最短距离等信息，利用上述方法可以时刻更新障碍物信息。

3.2 运动检测及避障规划

码垛机器人码垛时，若轴Ⅰ与轴Ⅱ或者与轴Ⅲ一起转动时，码垛机器人末端执行器形成的轨迹为复杂的空间曲线，计算比较复杂，不便于控制，因此可以采用轴Ⅰ单独转动，轴Ⅱ配合轴Ⅲ同时转动的控制方式。

如图5所示，为了简化计算，设置一个中间点N0（xN，yN，zN），该点是以o0为圆心，S0o0为半径画圆后与T0o0连线的交点确定得到。当从S0运动到N0时，采用底座旋转的方式工作，末端执行器到达N0点以后，再通过轴Ⅱ和轴Ⅲ的共同作用完成从N0到T0的运动。这两个过程需要进行障碍物的检测，才能保证机构正常运动。

（1）末端操作器从S0（xs，ys，zs）运动到N0（xN，yN，zN）的过程。

此过程末端操作器搬运货物的轨迹向坐标系x0o0y0内投影为一半圆，转过的角度为ψ。判断障碍物是否对此运动过程造成影响，即需要判断障碍物是否在前述的投影区域内。

障碍物存在的条件：

其中

式中：rmax—腰座转动时轨迹的最外缘方程，其值等于：

zmax为末端操作器所能抬高的最大高度，可由式（2）求得。若障碍物超过最缘方程或者小于图3（b）所示的最内圈半径时，此情况无需讨论。这里规定[9]：当障碍物在坐标系x0o0y0平面内的投影位于以为rmax半径的扇形区域内时，由于机器人本体可能与障碍物发生碰撞，若障碍物较低时为安全状态。为了方便讨论，此情况将视为有障碍物存在，货物需要抬高避免障碍物，且H满足的条件为：

式中：H1和H2—竖直方向上从S运动到S0和从S运动到N0的高度。

运动路径无障碍物时，驱动轴Ⅰ使操作器到达过渡点。

（2）末端操作器从N0（xN，yN，zN）运动到T0（xT，yT，zT）的过程。

如图5所示，为判别货物从N0运动到T0的过程中是否有障碍物干涉，可以将货物的运动轨迹用四边形A1B1C1D1包络起来，并将其投影到坐标系x0o0y0平面内进行讨论。四边形的长宽信息如下：

四边形A1B1C1D1的长宽表示为：

式中：l—四边形的长；

w—四边形的宽；

根据投影四边形A1B1C1D1的顶点坐标，利用多边形相交快速算法[10]来判断四边形A1B1C1D1与障碍物是否发生干涉。

若发生干涉，需要在z轴方向再判断，若障碍物在z轴上满足条件 z0+c0＞min（zN，zT），说明有干涉存在。发生干涉时，可以通过抬高货物并超过障碍物高度来进行避免，需要满足的条件，如式（10）所示。区别仅在于此时H1代表的是竖直方向上，从S运动到T0的高度，然后驱动轴Ⅱ使操作器运动到T0上方，最后驱动轴Ⅲ使其运动到T0点。

没有干涉存在时，驱动轴Ⅱ和轴Ⅲ使操作器到达点。

4 OpenGL仿真验证

基于QT和OpenGL的码垛机器人离线编程软件，能够将轨迹规划、碰撞检测相结合，实现码垛机器人的运动仿真[11]。软件系统经过轨迹规划后生成工作文件，然后通过运动仿真模块，可以实现在虚拟平台上的运动仿真，并能实时进行碰撞的检测。仿真过程是在2.3GHZ，内存为8GB的PC机上进行的。为了验证算法的正确性，分别针对有故障和无故障两种情况进行仿真验证。无障碍和有障碍的仿真效果图，如图6所示。其中红线表示末端执行器的路径，S代表末端操作器抓取的起始点。在两种不同的情况下同一货物堆放位置处的路径规划点位置和机器人关节角的相关信息，如表2、表3所示。在仿真软件中通过求解机器人运动学逆解来确定路径规划点位置所对应的机器人关节角度值。

仿真前需要设置相关环境信息。在全局坐标系中，托盘底面在坐标平面 x0o0y0上的顶点坐标分别为（1700，-1800，0），（1300，-1800，0），（1700，-2200，0），（1300，-2200，0）。末端执行器坐标为（1500，-1350，300）。x0o0y0平面上障碍物坐标为（1900，-1850，0），（1900，-2300，0），（2350，-1850，0），（2350，-2300，0） 障碍物高度600。货物抓取点的坐标为（3000，-1450，260）。

图6 码垛机器人避障仿真效果图
Fig.6 Obstacle Simulation Results of Palletizing Robot

表2、表3中数据表明，两种情况下，规划位置的区别主要体现在中间点和放置点的变化量上。这些变化是通过改变码垛机器人在水平方向和竖直方向上的运动形式来实现。水平方向上，末端操作器通过移动货物的水平位置，以避免将货物放置在存在障碍的区域。竖直方向上，末端操作器通过提升货物的高度使其达到安全高度从而避免发生干涉。仿真结果表明，该检测算法能够有效的判别障碍物并能合理规划避障路径。

表2 无障碍物存在的路径规划点坐标和机器人关节角
Tab.2 No Obstacle to the Existence of the Path Planning Point Coordinates and the Robot Joint Angle

表3 有障碍物存在的路径规划点坐标和机器人关节角
Tab.3 Obstacle to the Existence of the Path Planning Point Coordinates and the Robot Joint Angle

5 总结

通过求解码垛机器人运动学正逆解并结合其运动特点，得到其工作空间，再此基础上提出一种码垛机器人自主避障的算法，能够实现障碍物的检测并做出路径规划。利用码垛机器人离线编程软件对此算法进行了仿真验证。仿真结果表明此算法行之有效，能够正确分析障碍物信息并能实现障碍物的检测，系统生成的工作文件通过仿真模块可以实现码垛机器人的仿真运动，从而达到自主避障的目的。