一种可碰撞检测机械臂采摘场景下的避障问题处理方法与流程

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


背景技术：

2.机械臂的路径规划是指在满足工作条件的基础上，尽可能地寻找一条从初始点到目标点距离最短且能避开障碍保证自身安全的最优路径。常见的路径规划算法大致可以分为三类：第一类是以a*算法为代表的基于搜索的规划算法；第二类是以rrt为代表的基于采样的规划算法；第三类是以遗传算法为代表的智能规划算法。在使用机械臂进行采摘工作并通过路径规划实现避障寻找目标物时主要需要面对两个问题，一个是六自由度机械臂的高维度问题，另一个就是采摘工作环境的障碍物参数带来的避障问题。
3.基于采样的路径规划算法是通过生产随机采样点的方式来进行空间探索和目标点寻找的，障碍物不需要从三维工作空间转化到六维构型空间，只需要对每次生成的新随机点与障碍物在三维工作空间进行一次碰撞检测即可。这样一来，就避免了高维度问题带来的障碍物无法在构型空间进行描述的问题。同时六维空间中不涉及复杂的数学计算，只是进行随机点的产生，所有的复杂计算仍停留在三维空间中，大大降低了计算的复杂度。
4.在进行避障路径规划时，最重要的一步便是对障碍物的碰撞检测以确定可达路径点来实现向目标点的逐渐靠拢。而针对机械臂的避障工作，不仅需要末端点与障碍物没有碰撞，更需要保证机械臂的各个连杆在运动过程中都不会与障碍物发生碰撞。因此在进行碰撞检测时如何处理机械臂连杆与障碍物位置关系的三维空间转化方法是本发明主要解决的问题。


技术实现要素：

5.针对现有技术中存在的问题和不足，本发明的目的在于提供一种可碰撞检测机械臂采摘场景下的避障问题的处理方法。
6.基于上述目的，本发明采用的技术方案如下：
7.本发明第一方面提供了一种可碰撞检测机械臂采摘场景下的避障问题处理方法，包括如下步骤：
8.s1、将采摘场景下的障碍物简化为球体和圆柱体，将机械臂连杆简化为线段，将避障问题简化为求解线段与球体或者圆柱体是否相交的问题；
9.s2、处理球体的障碍物导致的避障问题；
10.s3、处理圆柱体的障碍物导致的避障问题。
11.优选地，所述步骤s1中所述球体代表果实，所述圆柱体代表树枝。
12.优选地，所述步骤s2中处理球体障碍物导致的避障问题的具体处理过程为：
13.s21、对球状果实进行避障时，采用穷举法讨论出线段在球体外时，只有两种情况下与球体不相交，一种是球心到线段的距离大于球体半径，另一种是球心到线段的距离小
于等于球体半径，但球心与线段两端点形成的两夹角中有一个角不是锐角，因此对线段与球体不相交关系的求解主要包括角度和长度的求解；
14.s22、角度求解是通过三点形成的两个向量之间的点乘公式求得，如式(ⅰ)所示；
[0015][0016]
s23、在长度求解中，一个是线段端点与球心的距离，可以直接通过线段端点与球心所构成的向量的2
‑
范数求得，另一个则是球体c与线段ab之间的距离则可以通过式(ⅱ)求得。
[0017][0018]
优选地，所述步骤s3中处理圆柱体障碍物导致的避障问题的具体处理过程为：
[0019]
s31、对圆柱体树枝进行避障时，讨论得出仅在线段与圆柱体中轴线的距离大于圆柱体半径时，两者不相交；
[0020]
s32、线段所在直线l1和圆柱体轴线l2可以用式(ⅲ)表示：
[0021][0022]
其中，p(λ1)和q(λ2)表示线段p1p2和圆柱体轴线段q1q2上的任意一点，且令上的任意一点，且令
[0023]
s33、距离求解时通过式(ⅳ)求得：
[0024][0025]
其中，minf(λ1,λ2)为直线l1和l2上距离最近的两个点之间的距离。
[0026]
优选地，所述步骤s33中式(ⅳ)的最小值需要因此式(ⅳ)可以简化得到式(
ⅴ
)。
[0027][0028]
本发明第二方面提供了所述可碰撞检测机械臂采摘场景下的避障问题处理方法在机械臂采摘设备中的应用。
[0029]
与现有技术相比，本发明的有益效果如下：
[0030]
(1)本发明将碰撞检测时机械臂连杆与障碍物位置关系的三维空间问题转化为线段与球体和圆柱体的计算问题。不涉及复杂的数学计算，只是进行随机点生产，所有的计算仍停留在三维空间中，大大降低了计算的复杂度。
[0031]
(2)本发明将果实用球体表示，树枝用圆柱体表示，机械臂连杆用线段表示，将避障问题简化为求解线段与球体或者圆柱体不相交情况下解的情况。
具体实施方式
[0032]
为使本发明的目的、技术方案及优点更加清楚明白，以下通过实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0033]
需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将结合实施例来详细说明本技术。
[0034]
实施例1
[0035]
本发明实施例提供一种可碰撞检测机械臂采摘场景下的避障问题处理方法，包括如下步骤：
[0036]
s1、由于采摘工作树林环境中的障碍物多为圆柱形的树枝和球形的果实，因此在进行碰撞检测时本文将障碍物转化成球形和圆柱形。而机械臂的各个连杆可以看作位置不同的线段且线段两端的坐标可以通过正运动学求解出来，那么避障问题便可以转化成求解线段与球体或者圆柱体是否相交的问题。
[0037]
s2、在进一步处理球体的障碍物导致的避障问题时，讨论出线段与球体的位置关系一共分为三种：第一种是线段的两个端点a、b都在球c内，即线段ac、bc的长度均小于球体半径，则线段与球体不相交；第二种是线段的两个端点一个在球c内，另一个在球c外，即线段ac、bc中一个长度大于球体半径，一个长度小于球体半径，则线段与球体相交；最后一种是线段的两个端点都在球外，此时就会有两种情况继续讨论，如若球心c到线段ab的距离大于半径，则线段与球体不相交，若球心c到线段ab的距离小于半径，则在∠cab和∠cba 都是锐角的情况下，线段与球体相交，否则线段与球体不相交。
[0038]
机械臂的各个连杆两端点构成的线段不与障碍物球体相交即为机械臂不会与果实发生碰撞，便可以完成机械臂与果实的碰撞检测。
[0039]
在机械臂与球形果实的避障过程中主要包括角度和长度的求解。角度求解可以通过三点形成的两个向量之间的点乘公式求得，如式(ⅰ)所示。
[0040][0041]
在长度求解中，一个是线段端点与球心的距离，可以直接通过端点与球心所构成的向量的2
‑
范数求得。另一个则是球形c与线段ab之间的距离则可以通过式(ⅱ)求得。
[0042][0043]
s3、在进一步处理树枝的障碍物导致的避障问题时，我们发现对树枝进行避障时，由于树枝是从树干处生长出来的，因此树枝可以看作一个无限长度的圆柱体，避障计算即为对线段与无限长度圆柱体是否相交问题的求解。该圆柱体与线段的位置关系相对简单，只有两种情况：第一种是线段与圆柱体中轴线的距离大于圆柱半径，两者不相交，即机械臂与树枝不会发生碰撞；另一种便是线段与圆柱体中轴线距离小于圆柱半径，两者相交，即机械臂与树枝会发生碰撞。
[0044]
在对树枝避障的计算中主要求解的是两条直线之间的最短距离，若机械臂连杆两端的坐标值为p1(x1,y1,z1)、p2(x2,y2,z2)，圆柱体轴线上两点坐标为q1(x3,y3,z3)，q2(x4,y4,
z4)。且令)。且令则连杆直线l1和圆柱体轴线l2可以用式(ⅲ)表示，p(λ1)和q(λ2)可以用来表示连杆线段p1p2和圆柱体轴线段q1q2上的任意一点。
[0045][0046]
因此，两直线之间的距离问题可以转化求解线段上距离最近的两个点之间的距离，即式(ⅳ)所示。
[0047][0048]
要求得式(ⅳ)的最小值需要因此将公式(ⅳ)化简可以得到式(
ⅴ
) 以求得λ1，λ2的值。
[0049][0050]
当我们求得0≤λ1、λ2≤1时，表明两条直线的公垂线的垂足落在线段之内，两直线的最短距离即为线段上距离最近的两点之间的距离d
2min
＝f(λ1,λ2)。而当λ1,λ2中至少有一个不在[0,1]范围内时，说明两条直线的公垂线的垂足至少有一个不在线段内，此时需要分别求解 p1到l2的最短距离d1、p2到l2的最短距离d2、q1到l1的最短距离d3、q2到l1的最短距离d4，此时dmin＝{d1,d2,d3,d4}
min
。通过上述公式便可以求得连杆与树枝轴线之间的距离进而进行碰撞检测。
[0051]
综上所述，本发明有效克服了现有技术中的不足，且具高度产业利用价值。上述实施例的作用在于说明本发明的实质性内容，但并不以此限定本发明的保护范围。本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和保护范围。