一种多臂机器人机械臂干涉分析方法与流程

本发明属于空间机器人领域，具体涉及一种多臂机器人机械臂干涉分析方法。

背景技术：

工业机器人是出现的最早，也是应用最广的一种机器人。但是随着机器人技术渗透到交通、医疗、教育等与人类生活密切相关的领域之后，工业机器人因为其笨重的外形和单一的动作受到了发展的限制，更加灵活可靠的多臂机器人应用而生。多臂机器人尤其是在人机协同操作方面表现出了巨大的优势，也是未来机器人技术发展的趋势之一。多臂机器人是更加接近人类动作的仿人型机器人，人类具有学习思考的思维能力，可以通过各种感官来判断两只手臂的位置，通过大脑来控制两只手臂协同完成相应的动作，但是机器人需要使用控制算法来保证机械手臂的协同运动，协同运动其中很重要的一点就是手臂不能发生碰撞，使他们在各自规划好的路径内运动，这就需要对多臂机器人进行机械臂的干涉分析和避碰控制。

所谓干涉分析，是指构造一定的数学判据和准则，来判断机器人手臂、各连杆之间及其与工作空间障碍物之间，是否处于干涉或不干涉两种状态，是否具有无干涉路径规划和避碰控制所要求的各种性质。空间机械臂的组成较为复杂，为了提高运算效率，在进行干涉分析时，需要将机械臂简化，最常用的方法就是利用简单几何体对实际模型进行包络。准确的进行机械臂的干涉分析可以找出机械臂之间，以及机械臂和障碍物之间干涉判断准则，为机械臂的避碰控制提供前期研究基础。

技术实现要素：

本发明针对上述问题提供了一种多臂机器人机械臂干涉分析方法来进行问题的解决。首先采用lss线性扫描球模型实现对复杂机械臂的简化，将其简化成中间是圆柱体两端是半球体的杆件模型，并进行区域划分。由于lss包围体边界面是连续的，可表示为时间t的函数：l^a(t)和l^b(t)。其组件可分别表示为：中间圆柱体c^a(t)和c^b(t)，前端半球体s1^a(t)和s1^b(t)，后端半球体s2^a(t)和s2^b(t)，两lss包围体圆柱及半球体的半径分别为r^a，r^b。

将有限长度的圆柱体c^a(t)和c^b(t)，沿轴线延长为长度无限的圆柱体c∞^a(t)和c∞^b(t)；用l∞^a和l∞^b分别表示c∞^a(t)和c∞^b(t)的轴线；用l^a(t)和l^b(t)表示包含在圆柱体c^a(t)和c^b(t)内部的轴线部分。p1^a(t)，p2^a(t)和p1^b(t)，p2^b(t)分别为线段l^a(t)和l^b(t)的两个端点，v^a(t)和v^b(t)分别为l^a(t)和l^b(t)的方向向量，p∞^a(λ;t)和p∞^b(λ;t)分别为l∞^a(t)和l∞^b(t)的直线方程，用o1^a(t)=p1^a(t)表示s1^a(t)的球心，o1^b(t)=p1^b(t)表示s1^b(t)的球心。

假设在t0∈[0,+∞]时刻，两个实体l^a(t)和l^b(t)的边界面发生干涉，则发生运动干涉的充要条件是：在它们的边界上有共同的交点q0。每一步的干涉分析过程中，需要先计算出lss模型各组件之间的干涉点q0，然后对q0是否同时位于各组件的边界上进行判断，即干涉点有效性判断，若干涉点均处于两个lss模型的组件上，则为有效干涉点，其他情况均为无效干涉点需进行下一步的干涉判断。干涉分析时，第一步先进行lss模型中两延长圆柱体的干涉分析；若第一步无法判定两lss体之间是否发生干涉，则需进一步判断圆柱体与半球体之间的干涉情况；如果前两步均无法判断出两个lss体的干涉状态，则需要经过两个半球体的干涉分析进行判断。

进一步的，两圆柱体发生干涉的充分必要条件是存在干涉时刻t0∈[0,+∞]，两延长圆柱体c∞^a(t)和c∞^b(t)间的距离为0，当v^a(t)×v^b(t)=0时，q0必为有效干涉点；当v^a(t)×v^b(t)≠0时，设l∞^a(t0)上的点p∞^a(λa1;t0)与l∞^b(t0)上的点p∞^b(λb1;t0)之间距离为两直线间的最短距离，通过方程[v^a(t)×v^b(t)]×[p^a(λa1;t0)-p^b(λb1;t0)]=0可以求得λa1与λb1的值，当点p^a(λa1;t0)必在线段l^a(t0)上且p^b(λb1;t0)必在线段l^b(t0)上，即0≤λa1≤1，0≤λb1≤1时，q0为有效干涉点。

进一步的，圆柱体和半球体发生干涉的充要条件是存在干涉时刻t0∈[0,+∞]，圆柱体c^a(t)和半球体s1^b(t)间的距离为0，如果满足λa2∈[0,1]且指向球心o1^b(t0)的向量o1^b(t0)-p^a(λa2;t0)与通过球心o1^b(t0)方向向外的l^b(t0)轴线单位向量v^b(t0)的夹角α∈[π/2,π]，则q0为有效干涉点。

进一步的，两半球体发生干涉的充分必要条件是存在干涉时刻t0∈[0,+∞]，两个半球s1^a(t)和s1^b(t)间的距离为0，在半球体s1^a(t0)上，通过半球表面中心的向量v^a(t0)与指向球心的任意向量的最小夹角为π/2，同理在半球体s1^b(t0)上，通过半球表面中心的向量v^b(t0)与指向球心的任意向量的最小夹角为π/2，则q0为有效干涉点。

与现有技术相比，本发明的有益效果是：提出了lss线性扫描球模型简化复杂机械臂的方法，有效实现了空间多臂机器人机械臂在任意位置下的干涉分析方法，提出了干涉分析判断准则。对于机器人运动控制中进一步实现机械臂避碰控制研究奠定了基础。

附图说明

图1是机械臂干涉分析的流程图。

图2是lss模型区域划分示意图。

图3是圆柱体与圆柱体之间的干涉分析数学模型图。

图4是圆柱体与半球体之间的干涉分析数学模型图。

图5是半球体与半球体之间的干涉分析数学模型图。

具体实施方式

下面将结合附图对本发明做详细的介绍。

如图1所示，本发明提出的干涉分析方法的具体步骤如下：

第一步：判断两圆柱体间的干涉情况。将两个有限长度的圆柱体，沿轴线方向进行无限延长。计算这两个无限长圆柱体的距离，若不发生干涉，则有限长度圆柱体必定不会发生干涉，并且两个lss模型也不会发生干涉。若发生干涉，则进一步判断发生干涉的点的有效性，即判断干涉点是否都处于lss模型的圆柱体上。若干涉点均处于两个lss模型的圆柱体上，则该点为有效干涉点，即两圆柱体会发生干涉，可以判断lss模型发生了干涉；其他情况（仅在一个圆柱体上或不在任何一个圆柱体上）则为两个圆柱体的无效干涉点，两圆柱体没有发生干涉，但此时还无法判断两个lss模型是否有干涉，需用进行进一步的判断。

第二步：若第一步的干涉点仅在一个圆柱体上（假设为左侧手臂圆柱体，若为右侧则交换左右手臂的lss数据），继续判断该点是否为此圆柱体与右侧lss模型中半球体的有效干涉点。计算右侧lss端点到左侧lss轴线的距离最小点。通过向量方向判断干涉点的有效性。若有效，则圆柱体与半球体发生干涉，否则，柱体与半球体不发生干涉，但无法判断两个lss是否处于干涉状态，进入第三步。

第三步：若第一步的干涉点不在任何一个在圆柱体上，计算左右lss端点向量与轴线向量的方向性，就可判断出干涉点的有效性，此时，有效的干涉点可断定lss发生干涉，无效的干涉点则断定lss不会发生干涉。

如图2所示，首先利用lss线性扫描球模型将复杂机械臂简化成中间为圆柱体，两端为半球体的空间模型。并进行区域划分，用符号l表示lss模型，c表示lss模型中的圆柱体，s1和s2表示两端半球体。处于相对运动中的两个连杆的lss包围体，可表示为关于时间t的函数：l^a(t)和l^b(t)。其组件可分别表示为：c^a(t)，s1^a(t)，s2^a(t)和c^b(t)，s1^b(t)，s2^b(t)；两lss包围体圆柱及半球体的半径分别为r^a，r^b。

接下来介绍lss体三种组件不同组合的干涉判别方法。

第一种，圆柱体与圆柱体的干涉分析判别。

如图3所示，将有限长度的圆柱体c^a(t)和c^b(t)，沿轴线延长为长度无限的圆柱体c∞^a(t)和c∞^b(t)。用l∞^a和l∞^b分别表示c∞^a(t)和c∞^b(t)的轴线；用l^a(t)和l^b(t)表示包含在圆柱体c^a(t)和c^b(t)内部的轴线部分。

设p1^a(t)和p2^a(t)分别为线段l^a(t)的两个端点，那么l^a(t)的方向向量为：v^a(t)=p2^a(t)-p1^a(t)，则l∞^a(t)的直线方程可表示为p∞^a(λ;t)=p1^a(t)+λv^a(t)，即p∞^a(λa;t)=(1-λa)p1^a(t)+λap2^a(t)。同理，l^b(t)的方向向量为v^b(t)，则l∞^b(t)的方程为：p∞^b(λb;t)=(1-λb)p1^b(t)+λbp2^b(t)。显然，当λa∈[0,1]时，可知点p∞^a(λ;t)∈l^a(t)；λb∈[0,1]时，点p∞^b(λ;t)∈l^b(t)。将两个圆柱体在空间上的相对位置分为平行和不平行两种状态进行探讨。

（1）当v^a(t)×v^b(t)≠0，即轴线l∞^a(t)与l∞^b(t)不平行时，直线l∞^a(t)和l∞^b(t)间的距离为dl1(t)=‖u(t)(p1^a(t)-p1^b(t))‖，其中u(t)=(v^a(t)×v^b(t))/‖v^a(t)×v^b(t)‖，u(t)为垂直于直线l∞^a(t)和l∞^b(t)的单位向量。

两圆柱体c∞^a(t)和c∞^b(t)之间的距离为dcc(t)=dl1(t)-(r^a+r^b)。设t0为干涉时刻，令方程fcc(t)=[(v^a(t)×v^b(t))(p1^a(t)-p1^b(t))]²-(r^a+r^b)²‖v^a(t)×v^b(t)‖²为0，求其在[0,+∞]的实根，两圆柱体发生干涉的充分必要条件为dcc(t)=0，t0∈[0,+∞]。

接下来需要判断q0是否为有效的干涉点。设l∞^a(t0)上的点p^a(λa1;t0)与l∞^b(t0)上的点p^b(λb1;t0)之间距离为两直线间的最短距离。

可通过方程[v^a(t)×v^b(t)]×[p^a(λa1;t0)-p^b(λb1;t0)]=0求得λa1与λb1的值。

如果q0为有效干涉点，那么点p^a(λa1;t0)必在线段l^a(t0)上，并且p^b(λb1;t0)必在线段l^b(t0)上，即0≤λa1≤1，0≤λb1≤1。

（2）当v^a(t)×v^b(t)=0，即轴线l∞^a(t)与l∞^b(t)平行时，直线l∞^a(t)和l∞^b(t)之间的距离可表示为：dl2(t)=‖(v^a(t)/‖v^a(t)‖)×[p1^b(t)-p1^a(t)]‖。

两圆柱体c∞^a(t)和c∞^b(t)间的距离为dcc(t)=dl2(t)-(r^a+r^b)。当dcc(t)=0时，必有一个有效干涉点q0，使得c^a(t)和c^b(t)发生干涉，即l^a(t)和l^b(t)发生干涉，不需要进行干涉点有效性判断。

第二种，圆柱体和半球体的干涉分析判别。

根据前述分析，此时有λa1不属于[0,1]，λb1属于[0,1]或者λa1属于[0,1]，λb1不属于[0,1]。设λa1属于[0,1]，λb1不属于[0,1]，判断圆柱体c^a(t)与半球体s1^b(t)的干涉情况如图4所示。

首先，计算出延长圆柱体c∞^a(t)和半球体s1^b(t)之间的干涉点q0，进而判断q0是否为同时位于圆柱体c^a(t)和半球体s1^b(t)上的有效干涉点。延伸圆柱体轴线l∞^a(t)和球心o1^b(t)间的距离为：do2(t)=‖(v^a(t)/‖v^a(t)‖)×[o1^b(t)-p1^a(t)]‖，其中o1^b(t)=p1^b(t)表示s1^b(t)的球心。

圆柱体c^a(t)和半球体s1^b(t)间的距离为：dcs(t)=do2(t)-(r^a+r^b)c∞^a(t)。c^a(t)和s1^b(t)发生干涉的充要条件是dcs(t)=0，t0∈[0,+∞]。即方程fsc(t)=[v^a(t)×(o1^b(t)-p1^a(t))]²-(r^a+r^b)²‖v^a(t)‖²=0存在正实根。

可以解得t0时刻，c∞^a(t0)和s1^b(t0)的干涉点q0，接下来需要判断该q0点的有效性。假设从点o1^b(t0)到轴l∞^a(t0)的最短距离的点为p^a(λa2;t0)。易知向量o1^b(t0)-p^a(λa2;t0)垂直于直线l∞^a(t0)。同时满足下述两个条件的q0，则为有效干涉点。如果λa2不属于[0,1]，则需要通过第三种情况进行判断。

（1）如果q0在l^a(t0)的边界上，即位在圆柱体c^a(t0)的边界上，则必有λa2∈[0,1]。

（2）如果q0在l^b(t0)的边界上，即位在半球体s1^b(t)的边界上，如图4所示，在半球体s1^b(t0)上，通过半球表面中心的向量v^b(t0)，与指向球心的任意向量的最小夹角为π/2，可知指向球心o1^b(t0)的向量o1^b(t0)-p^a(λa2;t0)与通过球心o1^b(t0)方向向外的l^b(t0)轴线单位向量v^b(t0)的夹角α∈[π/2,π]，即需要满足v^b(t0)[o1^b(t0)-p^a(λa2;t0)]≤0。

第三种，半球体与半球体的干涉分析判别。

如图5所示，由前述分析可知，此时有：λa1不属于[0,1]且λb1不属于[0,1]。则，q0不处于c^a(t0)和c^b(t0)上，干涉只可能发生在两lss体的半球体之间。

假设，可能发生干涉的两个半球是s1^a(t)和s1^b(t)，用o1^a(t)=p1^a(t)，o1^b(t)=p1^b(t)分别表示它们的球心。两个球心间的距离为do2(t)=‖o1^a(t)-o1^b(t)‖两半球间的距离为dss(t)=do2(t)-(r^a+r^b)。s1^a(t)和s1^b(t)发生干涉的充分必要条件是方程fss(t)=‖o1^a(t)-o1^b(t)‖-(r^a+r^b)=0存在实根t0∈[0,+∞]。

用q0表示s1^a(t0)和s1^b(t0)边界面上的干涉点，进行q0有效性的判断。如果q0为有效干涉点，则q0即位于s1^a(t0)上，同时又位于s1^b(t0)上，可以推出q0必在点o1^a(t0)和点o1^b(t0)的连线上，可以通过判断向量o1^a(t0)-o1^b(t0)的性质来判断q0的有效性。

在半球体s1^a(t0)上，通过半球表面中心的向量v^a(t0)，与指向球心的任意向量的最小夹角为π/2，即满足v^a(t0)[o1^a(t0)-o1^b(t0)]≤0。同理，也需要满足v^b(t0)[o1^b(t0)-o1^a(t0)]≤0。若两个条件都成立，则q0为有效干涉点，s1^a(t0)和s1^b(t0)会发生干涉，也即两个lss体发生运动干涉；否则q0无效，干涉不会发生。

以上所述仅为本发明具体的实施方式，熟悉本领域的技术人员在本发明所述的技术方案范围内所进行的替换都应包含在本发明的保护范围之内。