后得到的关节速度与预设速度进行比较， 得到第二奇异性判据0P，其中，OP为关节速度与预设速度的超速比。
[0072] 将笛卡尔空间的速度映射到关节速度#为：
[0074] 其中，為为第i轴通过雅克比矩阵映射得到的期望关节速度。
[0075] 根据关节速度与预设速度计算第二奇异性判据OP为：
[0076] OP = (Opi) = (Op1, op2, op3, op4, op5, op6)， (6)
[0077] 其中，Opi为预设的第i轴最大速度
％.为预设的i轴最大速度。
[0078] 步骤S14,根据前瞻缓冲队列中5轴的角度值Θ 5，判断该角度值Θ 5的绝对值是否 小于预设角度值γ，将判断结果η作为第三奇异性判据。
[0079] 第三奇异性判据η为：
[0081] 步骤S2,对第一至第三奇异性判据进行判断和分析，根据分析结果对机器人采用 对应的奇异性处理措施。
[0082] 根据第一至第三奇异性判据的数值和组合的不同，可以对机器人采取不同的奇异 性处理措施：
[0083] (1)当η = 1时，则从前瞻计算出的插补点队列中找出η由1变为〇的点，在这 两个位置之间将机器人的插补方式由笛卡尔空间插补改为关节空间插补以通过奇异域。 [0084] (2)当OP中至少一个数值大于1，即Op1 i = 1，2. .. 6至少有一个大于1，且满足 η =〇, μ > σ时，机器人的各轴采用降速算法同比例降速以通过奇异域，其中，σ为预设 灵活度阈值。
[0085] (3)当OP中数值均不大于1或满足η = 〇且μ < σ条件时，对机器人不作奇异 性处理。
[0086] (3. 1)当第二奇异性判据OP中的每个数值均不大于1时，则认为机器人当前所处 的位形不存在奇异性，不做奇异性处理，机器人正常运行。
[0087] (3.2)当满足Tl =0且μ < σ条件时，则机器人靠近除腕部奇异性之外的其他 奇异位形，
[0088] 无法使用关节空间插补通过，且如果使用降速算法，损失的速度过大，因此这种情 况下不做奇异性处理，直接停机。对机器人不作奇异性处理，控制机器人停机。
[0089] 图4为根据发明另一个实施例的基于多判据的六自由度串联机器人奇异性处理 方法的流程图。
[0090] 步骤S401，设置机器人的插补方式为笛卡尔空间插补。
[0091] 步骤S402,逆解，求出机器人6轴的角度值Θ Θ 6。
[0092] 步骤S403,计算空间速度伪向量vx、vy、νζ、ωχ、co y、ωζ。
[0093] 步骤S404,计算第一至第三奇异性判据η、OP和μ。
[0094] 步骤S405,判断η是否等于1，如果是，则执行步骤S406,否则执行步骤S407。
[0095] 步骤S406,将机器人的插补方式修改为关节空间插补，以通过奇异域。
[0096] 步骤S407,判断μ是否大于σ，如果是则执行步骤S408,否则执行步骤S409。
[0097] 步骤S408,判断OP中每个数值是否均大于1，如果是，则执行步骤S411，否则执行 步骤S410。
[0098] 步骤S409,奇异性过大，控制机器人安全停机。
[0099] 步骤S410,非奇异区域状态正常赋值退出（RETURN NORMAL)。
[0100] 步骤S411，采用各轴同比例降速，然后根据情况分别执行步骤S412、S413和S414。
[0101] 步骤S412,靠近奇异点，报错退出（RETURN SING)。
[0102] 步骤S413,降速后1个插补周期内移动的距离没有超过当前正常插补点角度 (RETURN SIN_THIS_STEP)〇
[0103] 步骤S414,降速后1个插补周期内移动的距离超过当前正常插补点角度（RETURN SIN_NEXT_STEP)〇
[0104] 根据本发明实施例的基于多判据的六自由度串联机器人奇异性处理方法，通过计 算出三种不同的奇异性判定条件，可针对不同的情况自动选用最佳的奇异性处理算法，可 较好的解决机器人在奇异点附近因速度突变导致的运行不稳定问题，增强机器人的笛卡尔 空间作业能力。本发明在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机 器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，根据三种判据的不同 组合提出了不同的处理方法，可适应多种实际运行工况的要求。
[0105] 在本说明书的描述中，参考术语"一个实施例"、"一些实施例"、"示例"、"具体示 例"、或"一些示例"等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特 点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不 一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何 的一个或多个实施例或示例中以合适的方式结合。
[0106] 尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围 由所附权利要求极其等同限定。
【主权项】
1. 一种基于多判据的六自由度串联机器人奇异性处理方法，其特征在于，包括如下步 骤： 步骤S1，采用S形加减速算法规划机器人末端的笛卡尔轨迹，对所述笛卡尔轨迹进行 分析得到第一至第三奇异性判据，包括如下步骤： 步骤S11，根据空间坐标变换算法计算出所述笛卡尔轨迹的空间速度v(P)，计算所述 机器人的关节空间速度到笛卡尔空间速度v(P)的映射矩阵，即雅克比矩阵J(q); 步骤S12,计算所述雅克比矩阵J(q)的奇异值得到第一奇异性判据y，其中，y为当 前雅克比矩阵灵活度指标； 步骤S13,将经由所述雅克比矩阵J(q)映射后得到的关节速度与预设速度进行比较， 得到第二奇异性判据0P，其中，0P为关节速度与预设速度的超速比； 步骤S14,根据前瞻缓冲队列中5轴的角度值05，判断该角度值05的绝对值是否小于 预设角度值Y，将判断结果n作为第三奇异性判据； 步骤S2,对所述第一至第三奇异性判据进行判断和分析，根据分析结果对所述机器人 采用对应的奇异性处理措施，包括： 当n=1时，将机器人的插补方式由笛卡尔空间插补改为关节空间插补以通过奇异 域； 当op中至少一个数值大于1，n=〇,y> 〇时，采用降速算法通过奇异域，其中，〇 为预设灵活度阈值； 当OP中数值均不大于1或满足n= 〇且y< 〇条件时，对所述机器人不作奇异性 处理。2. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，所述笛卡尔轨迹的空间速度其中，v为所述笛卡尔轨迹的线速度，《为所述笛卡尔轨迹的角速度。3. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，所述雅克比矩阵J(q) =U'JuI' 2,I' 3,I' 4,I' 5,I' 6)，其中，I'i为当前位形运动旋量，Ii为初始参考位形运动旋量，Ad为将| ;变换到I',的伴随矩阵，每为第i个关节的期望角速度，i= 2,…，6。4. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，在所述步骤S12中，计算所述雅克比矩阵J(q)的奇异值为： D=sqrt(abs(eig(J(q)TJ(q)))), 其中，D为当前雅克比矩阵J(q)的奇异值向量，eig为计算矩阵的特征值操作，abs为 取绝对值操作，sqrt为开方操作； 根据所述雅克比矩阵J(q)的奇异值计算第一奇异性判据y，其中，5. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，在所述步骤S13中， 将笛卡尔空间的速度映射到关节速度4为：其中，病为第i轴通过雅克比矩阵映射得到的期望关节速度； 根据关节速度与预设速度计算所述第二奇异性判据0P为： OP= (op；) = (opj,op2,op3,op4,op5,op6), 其中，〇Pl为预设的第i轴最大速度。6. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，在所述步骤S14中，所述第三奇异性判据n为：7. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，在所述步骤S2中， 当所述第二奇异性判据0P中的每个数值均不大于1时，对所述机器人不作奇异性处 理，所述机器人正常运行； 当满足n=〇且y< 〇条件时，对所述机器人不作奇异性处理，控制所述机器人停 机。8. 如权利要求1所述的基于多判据的六自由度串联机器人奇异性处理方法，其特征在 于，在所述步骤S2中，当op中至少一个数值大于1，n=〇,y> 〇时，所述机器人的各轴 采用降速算法同比例降速以通过奇异域。
【专利摘要】本发明提出了一种基于多判据的六自由度串联机器人奇异性处理方法，包括如下步骤：采用S形加减速算法规划机器人末端的笛卡尔轨迹，对笛卡尔轨迹进行分析得到第一至第三奇异性判据，包括根据空间坐标变换算法计算出笛卡尔轨迹的空间速度，计算机器人的关节空间速度到笛卡尔空间速度的映射矩阵；计算第一至第三奇异性判据；对第一至第三奇异性判据进行判断和分析，根据分析结果对机器人采用对应的奇异性处理措施。本发明在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，可适应多种实际运行工况的要求。
【IPC分类】B25J9/16
【公开号】CN105082134
【申请号】CN201510477991
【发明人】韩峰涛
【申请人】珞石（北京）科技有限公司
【公开日】2015年11月25日
【申请日】2015年8月6日