一种基于多判据的六自由度串联机器人奇异性处理方法
【技术领域】
[0001] 本发明涉及机器人控制技术领域，特别涉及一种基于多判据的六自由度串联机器 人奇异性处理方法。
【背景技术】
[0002] 奇异性是6自由度工业机器人的固有特性，这是由其本身的机械结构决定的。当 机器人的构型靠近奇异位形时，其末端执行器的空间运动能力会极大的降低，甚至导致机 器人振动或者停机，严重影响机器人的正常工作。因此如何准确的判断机器人是否靠近奇 异位形，并针对不同的情形给出时给出合理的奇异性处理方案，直接关系到机器人整体的 空间作业性能。6自由度工业机器人的奇异性处理问题可分解为两个子问题：（1)判断是否 需要进行奇异性处理；（2)如果需要，采用何种处理方案。
[0003] 现有技术中常见的奇异性处理方法有以下三种：
[0004] (1)通过判断关节速度是否超过设定值来近似判断奇异性：一旦判断关机速度超 限，则立即以安全的方式停止机器人运动。这种方式虽然最容易实现，但极大的限制了机器 人的空间作业范围，在一些需要复杂连续运动的场合不适用，并且无法预测的突然停机可 能会对设备造成损坏。
[0005] (2)在奇异点附近采用关节插补的方式运动：通过雅克比矩阵或者其他方式计算 机器人的灵活度指标，结合末端实时轨迹规划的数据，判断机器人是否靠近奇异点，如果靠 近，则采用关节插补的方式通过奇异点。这种处理方案虽然可以保证机器人连续运动，但是 末端轨迹会偏离预期的轨迹，在要求机器人末端轨迹一直保持精确的场合是不适用的。
[0006] (3)通过提前计算避开奇异点：通过引入轨迹前瞻功能或者专家系统来获得机器 人奇异点的信息，在轨迹规划时避开这些奇异点。但是这种方法会限制机器人的工作空间 范围，降低机器人的适应性，同时由于需要提前计算，在控制器负荷较大时，可能会影响系 统的实时性。
[0007] 除此之外，由于工业机器人属于高度灵活的可编程设备，所面对的应用环境和工 况多种多样，上述三种方案仅能处理某种特定的奇异性问题，适应性较差。
【发明内容】
[0008] 本发明的目的旨在至少解决所述技术缺陷之一。
[0009] 为此，本发明的目的在于提出一种基于多判据的六自由度串联机器人奇异性处理 方法，在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机器人的奇异性进 行更细致更准确的划分，提高奇异性处理的准确性，根据三种判据的不同组合提出了不同 的处理方法，可适应多种实际运行工况的要求。
[0010] 为了实现上述目的，本发明的实施例提供一种基于多判据的六自由度串联机器人 奇异性处理方法，包括如下步骤：
[0011] 步骤S1，采用S形加减速算法规划机器人末端的笛卡尔轨迹，对所述笛卡尔轨迹 进行分析得到第一至第三奇异性判据，包括如下步骤：
[0012] 步骤S11，根据空间坐标变换算法计算出所述笛卡尔轨迹的空间速度V(P)，计算 所述机器人的关节空间速度到笛卡尔空间速度V(P)的映射矩阵，即雅克比矩阵J(q);
[0013] 步骤S12,计算所述雅克比矩阵J(q)的奇异值得到第一奇异性判据μ，其中，μ为 当前雅克比矩阵灵活度指标；
[0014] 步骤S13,将经由所述雅克比矩阵J(q)映射后得到的关节速度与预设速度进行比 较，得到第二奇异性判据0P，其中，OP为关节速度与预设速度的超速比；
[0015] 步骤S14,根据前瞻缓冲队列中5轴的角度值Θ 5，判断该角度值Θ 5的绝对值是否 小于预设角度值γ，将判断结果η作为第三奇异性判据；
[0016] 步骤S2,对所述第一至第三奇异性判据进行判断和分析，根据分析结果对所述机 器人采用对应的奇异性处理措施，包括：
[0017] 当η = 1时，将机器人的插补方式由笛卡尔空间插补改为关节空间插补以通过奇 异域；
[0018] 当OP中至少一个数值大于1，η = 〇，μ > σ时，采用降速算法通过奇异域，其 中，σ为预设灵活度阈值；
[0019] 当OP中数值均不大于1或满足η = 〇且μ > σ条件时，对所述机器人不作奇 异性处理。
[0020] 进一步，所述笛卡尔轨迹的空间速度1
[0022] V为所述笛卡尔轨迹的线速度，ω为所述笛卡尔轨迹的角速度。
[0025] 其中，ξ /为当前位形运动旋量，ξ i为初始参考位形运动旋量，Ad为将ξ ;变换 到ξ/的伴随矩阵，贫为第i个关节的期望角速度，i = 2，···，6。
[0026] 进一步，在所述步骤S12中，计算所述雅克比矩阵J(q)的奇异值为：
[0027] D = sqrt (abs (eig (J (q) tJ (q)))),
[0028] 其中，D为当前雅克比矩阵J (q)的奇异值向量，eig为计算矩阵的特征值操作，abs 为取绝对值操作，sqrt为开方操作；
[0029] 根据所述雅克比矩阵J(q)的奇异值计算第一奇异性判据μ，其中，
[0031] 进一步，在所述步骤S13中，
[0032] 将笛卡尔空间的速度映射到关节速度#为：
[0034] 其中，瑪为第i轴通过雅克比矩阵映射得到的期望关节速度；
[0035] 根据关节速度与预设速度计算所述第二奇异性判据OP为：
[0036] OP = (Opi) = (Op1, op2, op3, op4, op5, op6)，
[0037] 其中，OP1为预设的第i轴最大速度。
[0038] 进一步，在所述步骤S14中，所述第三奇异性判据Tl为：
[0040] 进一步，在所述步骤S2中，
[0041] 当所述第二奇异性判据OP中的每个数值均不大于1时，对所述机器人不作奇异性 处理，所述机器人正常运行；
[0042] 当满足η =〇且μ < σ条件时，对所述机器人不作奇异性处理，控制所述机器 人停机。
[0043] 进一步，在所述步骤S2中，当OP中至少一个数值大于1，η = 〇, μ > σ时，所述 机器人的各轴采用降速算法同比例降速以通过奇异域。
[0044] 根据本发明实施例的基于多判据的六自由度串联机器人奇异性处理方法，通过计 算出三种不同的奇异性判定条件，可针对不同的情况自动选用最佳的奇异性处理算法，可 较好的解决机器人在奇异点附近因速度突变导致的运行不稳定问题，增强机器人的笛卡尔 空间作业能力。本发明在奇异性判断上采用三种判据组合的方式，可以对六自由度串联机 器人的奇异性进行更细致更准确的划分，提高奇异性处理的准确性，根据三种判据的不同 组合提出了不同的处理方法，可适应多种实际运行工况的要求。
[0045] 本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0046] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变 得明显和容易理解，其中：
[0047] 图1为根据发明一个实施例的基于多判据的六自由度串联机器人奇异性处理方 法的流程图；
[0048] 图2为根据本发明实施例的计算第一至第三奇异性判据的流程图；
[0049] 图3为根据发明实施例的S形加减速规划示意图；
[0050] 图4为根据发明另一个实施例的基于多判据的六自由度串联机器人奇异性处理 方法的流程图。
【具体实施方式】
[0051] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终 相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附 图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0052] 如图1所示，本发明实施例的基于多判据的六自由度串联机器人奇异性处理方 法，包括如下步骤：
[0053] 步骤Sl，采用S形加减速算法规划机器人末端的笛卡尔轨迹，对笛卡尔轨迹进行 分析得到第一至第三奇异性判据。
[0054] 如图2所示，步骤S2具体包括如下步骤：
[0055] 步骤S11，根据空间坐标变换算法计算出笛卡尔轨迹的空间速度V(P)，计算机器 人的关节空间速度到笛卡尔空间速度V(P)的映射矩阵，即雅克比矩阵J (q)。
[0056] 图3为根据发明实施例的S形加减速规划示意图。
[0057] 使用S形加减速算法规划机器人末端的笛卡尔轨迹，结合空间坐标变换算法得到 轨迹的空间速度V (P)为：
[0060] V为笛卡尔轨迹的线速度，ω为笛卡尔轨迹的角速度。
[0061] 在本发明的一个实施例中，在计算出轨迹的空间速度之后，采用轨迹前瞻算法提 前计算多个空间位置点，保存上述空间位置点。
[0062] 然后，采用旋量法计算关节空间速度到笛卡尔空间速度的映射矩阵，即雅克比矩 阵
[0064] 其中，
为当前位形运动旋量，ξ i为初始参考 位形运动旋量，Ad为将ξ /变换到ξ /的伴随矩阵，与（Θ θ 2··· Θ i J有关，_为第i个 关节的期望角速度，i = 2,…，6。
[0065] 步骤S12,计算雅克比矩阵J(q)的奇异值得到第一奇异性判据μ，其中，μ为当 前雅克比矩阵灵活度指标。
[0066] 计算雅克比矩阵J (q)的奇异值为：
[0067] D = sqrt(abs(eig(J(q)TJ(q))))， (3)
[0068] 其中，D为当前雅克比矩阵J (q)的奇异值向量，eig为计算矩阵的特征值操作，abs 为取绝对值操作，sqrt为开方操作。
[0069] 根据雅克比矩阵J(q)的奇异值计算第一奇异性判据μ，其中，
[0071] 步骤S13,将经由雅克比矩阵J(q)映射