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

摘要

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

主权项

一种码垛机器人的避障方法，其特征在于：所述的避障方法针对码垛机器人第二过程，即码垛机器人的腕部底面中心从抓取点正上方一点S0到放置点上方一点T0的过程，如果满足公式（19），同时满足公式（14）~（18）中的一个或者几个，并且不满足公式（10），才能认定码垛的路径上有障碍物；其中，公式（19）如下： min { z S + H 1 , z T + H 2 } - c 1 ≤ z P + c 0 ≤ z max max { z S + H 1 , z T + H 2 } ≤ z max - - - ( 19 ) 其中：zmax为码垛机器人的腕部底面中心所能抬到的最大高度，数值上等于在求解工作空间时的最高点的z坐标值；将障碍物投影到xOy平面内，并将障碍物四条边命名为l1、l2、l3、l4，转动腰座时的轨迹最外缘方程为： x 2 + y 2 = r max 2 r max 2 = ( x i ± a 1 2 ) 2 + ( y i 2 ± b 1 2 ) 2 i = S , T - - - ( 12 ) 其中：rmax为货物在随腰座回转过程中到动坐标系原点O最远点；障碍物四条边线方程为： y = y P ( x P ≤ x ≤ x P + a 0 ) l 1 x = x P + a 0 ( y P ≤ y ≤ y P + b 0 ) l 2 y = y P + b 0 ( x P ≤ x ≤ x P + a 0 ) l 3 x = x P ( y P ≤ y ≤ y P + b 0 ) l 4 - - - ( 13 ) 将l1中的y值代入（12）中得 x = ± r max 2 - y p 2 x P ≤ x ≤ x P + a 0 - - - ( 14 ) 将l2中的x值代入（12）中得 x = ± r max 2 - ( x P + a 0 ) 2 y P ≤ y ≤ y P + b 0 - - - ( 15 ) 将l3中的y值代入（12）中得 x = ± r max 2 - ( y P + b 0 ) 2 x P ≤ x ≤ x P + a 0 - - - ( 16 ) 将l4中的x值代入（12）中得 y = ± r max 2 - x p 2 y P ≤ y ≤ y P + b 0 - - - ( 17 ) 将障碍物的四个顶点到动坐标系O‑xyz的原点O的最近距离与rmax相比较：即 min { x P 2 + y P 2 , ( x P + a 0 ) 2 + y P 2 , ( x P + a 0 ) 2 + ( y P + b 0 ) 2 , x P 2 + ( y P + b 0 ) 2 } ≤ r max - - - ( 18 ) 方程（10）为障碍物的四个顶点坐标(xi，yi)满足的条件： y i - y s - a 1 2 x s - b 1 2 x i < 0 , i = 1,2,3,4 y i - y T - b 1 2 x T + a 1 2 x i < 0 , i = 1,2,3,4 - - - ( 10 ) 由于码垛路径上有障碍物，故需要进行避障处理，即：先将货物抬高H1′，使 z P + c 0 < min { z S + H 1 , z T + H 2 } - c 1 + H 1 ′ ≤ z max max { z S + H 1 , z T + H 2 } ≤ z max - - - ( 20 ) 然后同时转动腰座、移动水平滑块和竖直滑块；根据公式（9）： θ 1 = arctan y ON x ON θ 2 = - arctan y ON x ON x C = a ( x ON 2 + y ON 2 + d 1 - d 3 ) b + a z A = - c d ( z ON + d 4 - d 2 ) - - - ( 9 ) 腰座转动角度为： θ 1 = arctan Y T X T - arctan Y S X S , 水平滑块移动： x C = a ( x T 2 + y T 2 - x N 2 + y N 2 ) b + a , 竖直滑块移动： z A = - c d | ( z T + H 2 ) - z N | , 其中： z N = z S + H 1 + H 1 ′ x N = x S 2 + y S 2 cos ( arctan y T x T ) y N = x S 2 + y S 2 sin ( arctan y T x T ) y N x N = y T x T - - - ( 21 ) 按照上述的过程进行，避障实现抓取点正上方一点S0到放置点正上方一点T0的运动。