[发明专利]一种可碰撞检测机械臂采摘场景下的避障问题处理方法

说明书

本发明属于工业机器人运动规划领域，公开了一种可碰撞检测机械臂采摘场景下的避障问题处理方法。本发明将碰撞检测时机械臂连杆与障碍物位置关系的三维空间问题转化为线段与球体和圆柱体的计算问题。将果实用球体表示，树枝用圆柱体表示，机械臂连杆用线段表示，进而将避障问题简化为求解线段与球体或者圆柱体不相交情况下解的情况。本发明不涉及复杂的数学计算，只是进行随机点产生，所有的计算仍停留在三维空间中，大大降低了计算的复杂度，适合应用于机械臂采摘设备。

技术领域

本发明属于工业机器人运动规划领域，具体涉及一种可碰撞检测机械臂采摘场景下的避障问题的处理方法。

背景技术

机械臂的路径规划是指在满足工作条件的基础上，尽可能地寻找一条从初始点到目标点距离最短且能避开障碍保证自身安全的最优路径。常见的路径规划算法大致可以分为三类：第一类是以A*算法为代表的基于搜索的规划算法；第二类是以RRT为代表的基于采样的规划算法；第三类是以遗传算法为代表的智能规划算法。在使用机械臂进行采摘工作并通过路径规划实现避障寻找目标物时主要需要面对两个问题，一个是六自由度机械臂的高维度问题，另一个就是采摘工作环境的障碍物参数带来的避障问题。

基于采样的路径规划算法是通过生产随机采样点的方式来进行空间探索和目标点寻找的，障碍物不需要从三维工作空间转化到六维构型空间，只需要对每次生成的新随机点与障碍物在三维工作空间进行一次碰撞检测即可。这样一来，就避免了高维度问题带来的障碍物无法在构型空间进行描述的问题。同时六维空间中不涉及复杂的数学计算，只是进行随机点的产生，所有的复杂计算仍停留在三维空间中，大大降低了计算的复杂度。

在进行避障路径规划时，最重要的一步便是对障碍物的碰撞检测以确定可达路径点来实现向目标点的逐渐靠拢。而针对机械臂的避障工作，不仅需要末端点与障碍物没有碰撞，更需要保证机械臂的各个连杆在运动过程中都不会与障碍物发生碰撞。因此在进行碰撞检测时如何处理机械臂连杆与障碍物位置关系的三维空间转化方法是本发明主要解决的问题。

发明内容

针对现有技术中存在的问题和不足，本发明的目的在于提供一种可碰撞检测机械臂采摘场景下的避障问题的处理方法。

基于上述目的，本发明采用的技术方案如下：

本发明第一方面提供了一种可碰撞检测机械臂采摘场景下的避障问题处理方法，包括如下步骤：

S1、将采摘场景下的障碍物简化为球体和圆柱体，将机械臂连杆简化为线段，将避障问题简化为求解线段与球体或者圆柱体是否相交的问题；

S2、处理球体的障碍物导致的避障问题；

S3、处理圆柱体的障碍物导致的避障问题。

优选地，所述步骤S1中所述球体代表果实，所述圆柱体代表树枝。

优选地，所述步骤S2中处理球体障碍物导致的避障问题的具体处理过程为：

S21、对球状果实进行避障时，采用穷举法讨论出线段在球体外时，只有两种情况下与球体不相交，一种是球心到线段的距离大于球体半径，另一种是球心到线段的距离小于等于球体半径，但球心与线段两端点形成的两夹角中有一个角不是锐角，因此对线段与球体不相交关系的求解主要包括角度和长度的求解；

S22、角度求解是通过三点形成的两个向量之间的点乘公式求得，如式(Ⅰ)所示；

S23、在长度求解中，一个是线段端点与球心的距离，可以直接通过线段端点与球心所构成的向量的2-范数求得，另一个则是球体C与线段AB之间的距离则可以通过式(Ⅱ)求得。

优选地，所述步骤S3中处理圆柱体障碍物导致的避障问题的具体处理过程为：