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

权利要求书

1.一种可碰撞检测机械臂采摘场景下的避障问题处理方法，其特征在于，包括如下步骤：

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

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

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

2.根据权利要求1所述的可碰撞检测机械臂采摘场景下的避障问题处理方法，其特征在于，所述步骤S1中所述球体代表果实，所述圆柱体代表树枝。

3.根据权利要求1所述的可碰撞检测机械臂采摘场景下的避障问题处理方法，其特征在于，所述步骤S2中处理球体障碍物导致的避障问题的具体处理过程为：

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

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

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

4.根据权利要求2所述的可碰撞检测机械臂采摘场景下的避障问题处理方法，其特征在于，所述步骤S3中处理圆柱体障碍物导致的避障问题的具体处理过程为：

S31、对圆柱体树枝进行避障时，讨论得出仅在线段与圆柱体中轴线的距离大于圆柱体半径时，两者不相交；

S32、线段所在直线l₁和圆柱体轴线l₂可以用式(Ⅲ)表示：

其中，P(λ₁)和Q(λ₂)表示线段P₁P₂和圆柱体轴线段Q₁Q₂上的任意一点，且令

S33、距离求解时通过式(Ⅳ)求得：

其中，minf(λ₁,λ₂)为直线l₁和l₂上距离最近的两个点之间的距离。

5.根据权利要求3所述的可碰撞检测机械臂采摘场景下的避障问题处理方法，其特征在于，所述步骤S33中式(Ⅳ)的最小值需要因此式(Ⅳ)可以简化得到式(Ⅴ)。

6.一种利用权利要求1～5任一所述可碰撞检测机械臂采摘场景下的避障问题处理方法在机械臂采摘设备中的应用。