CN112936272A - 一种机械臂的奇异构型的判断方法和求解方法 - Google Patents

Abstract

本发明公开了一种机械臂的奇异构型的判断方法，包括：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的至少部分三阶子式是否为零，若所述关节螺旋矩阵的三阶子式为零的数量越多，则所述机械臂离奇异状态越近，若所述关节螺旋矩阵的三阶子式为零的数量越少，则所述机械臂离奇异状态越远。本发明还公开一种机械臂的奇异构型的求解方法。本发明简化了冗余机械臂的奇异构型的判断，不存在理论误差，并且能够进行预判。

Description

一种机械臂的奇异构型的判断方法和求解方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种机械臂的奇异构型的判断方法和求解方法。

背景技术

传统的工业机械臂大多数只有六个自由度，六自由度的机械臂在实际工作过程中，很容易遇到奇异状态，此时机械臂的关节电机无论速度有多大，末端工具总是损失一个或多个自由度，导致末端在一个或多个方向上失去运动的能力。显然，机械臂处于奇异状态时，对机械臂电机的损伤是巨大的。为了规避机械臂奇异状态所带来的危害，实际工程人员会选择冗余机械臂。

冗余机械臂的出现，一方面是为了尽可能地减少机械臂的奇异状态，另一方面是为了让机械臂适应各种复杂的空间环境。相对于六自由度机械臂，冗余机械臂的奇异状态大大地减少了，但是它并没有消除奇异状态，在实际的工作过程中仍然有可能处于奇异构型。

六自由度机械臂的奇异构型可以用雅可比矩阵的行列式为0的方法求解，而对于冗余机械臂的奇异构型则需要用雅可比矩阵非行满秩来求解，这会带来计算量的急剧增加。

因此，简化冗余机械臂奇异构型的求解过程，用一些方法给出冗余机械臂特定的奇异构型，这在保护机械臂电机正常运转，保证机械臂的位置和速度规划具有连续性等方面有着重要的应用。

中国专利文献CN104385283A中公开了一种六自由度机械臂奇异位形的快速判断方法，包括：从六自由度机械臂的角度编码器中读取六个关节的当前角度值；判断六个关节角是否都没超出自身的实际运动范围；判断第3个关节角；判断第2个关节角；判断第5个关节角。此发明提出的方法将平面几何法应用于机械臂奇异位形的判断中。中国专利文献CN106003057A中公开了一种冗余自由度机械臂构型奇异快速判决方法，包括：针对冗余自由度机械臂，其末端速度与各关节角速度之间存在如下映射关系：

表示末端速度，

表示关节角速度，J表示雅可比矩阵；计算所述雅可比矩阵的最大奇异值与最小奇异值，以最大奇异值与最小奇异值的比值为条件数，当条件数大于设定数值时，则判定机械臂构型发生奇异；计算所述雅可比矩阵的条件数的过程中，采用基于广义逆计算中间结果迭代求取最大奇异值和最小奇异值的实时值，达到实时计算条件数的要求。但是上述所公开的机械臂奇异构型的判断方法要么只能应用于简单的非冗余机械臂，要么存在一定的理论误差、无法进行预判等缺陷。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

为解决上述技术问题，本发明提出一种机械臂的奇异构型的判断方法和求解方法，简化了冗余机械臂的奇异构型的判断，不存在理论误差，并且能够进行预判。

为了达到上述目的，本发明采用以下技术方案：

本发明的一个实施例公开了一种机械臂的奇异构型的判断方法，包括：

A1：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；

A2：基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；

A3：将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的至少部分三阶子式是否为零，若所述关节螺旋矩阵的三阶子式为零的数量越多，则所述机械臂离奇异状态越近，若所述关节螺旋矩阵的三阶子式为零的数量越少，则所述机械臂离奇异状态越远。

优选地，步骤A1中求解的所述机械臂的关节空间到工作空间的速度雅克比矩阵为：

式中，⁰J_vej为所述机械臂的关节空间到工作空间的速度雅克比矩阵，w为所述机械臂的各关节角速度组成的矢量，v_e为所述机械臂的末端的速度矢量，w_e为所述机械臂的角速度矢量。

优选地，步骤A2中所述速度雅克比矩阵与关节螺旋矩阵的关系为：对于以所述机械臂的DH的任意{k}坐标系，都存在rank(^kJ_vej)＝rank(^kS)，^kS为关节螺旋矩阵。

优选地，步骤A2中将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件具体为：

n为所述机械臂的关节的数量，n为奇数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵；n为偶数时，n为偶数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵。

优选地，步骤A3中将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的至少部分三阶子式是否为零具体为：当n为奇数时，将所述关节螺旋矩阵的奇异条件转化为判断

的所有三阶子式是否为零；当n为偶数时，将所述关节螺旋矩阵的奇异条件转化为判断

的所有三阶子式是否为零。

本发明的一个实施例公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现上述的机械臂的奇异构型的判断方法。

本发明的一个实施例公开了一种机械臂的奇异构型的求解方法，包括：

B1：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；

B2：基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；

B3：将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的预设的部分三阶子式是否为零，并采用模拟退火算法来求解所述关节螺旋矩阵的预设的部分三阶子式均为零时所述机械臂的关节角的状态，即为所述机械臂的奇异构型。

优选地，步骤B1中求解的所述机械臂的关节空间到工作空间的速度雅克比矩阵为：

式中，⁰J_vej为所述机械臂的关节空间到工作空间的速度雅克比矩阵，w为所述机械臂的各关节角速度组成的矢量，v_e为所述机械臂的末端的速度矢量，w_e为所述机械臂的角速度矢量；

进一步地，步骤B2中所述速度雅克比矩阵与关节螺旋矩阵的关系为：对于以所述机械臂的DH的任意{k}坐标系，都存在rank(^kJ_vej)＝rank(^kS)，^kS为关节螺旋矩阵。

优选地，步骤B2中将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件具体为：

n为所述机械臂的关节的数量，n为奇数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵；n为偶数时，n为偶数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵。

进一步地，当n为奇数时，所述关节螺旋矩阵的预设的部分三阶子式为

的所有三阶子式；当n为偶数时，所述关节螺旋矩阵的预设的部分三阶子式为断

的所有三阶子式。

本发明的一个实施例公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现上述的机械臂的奇异构型的求解方法。

与现有技术相比，本发明的有益效果在于：本发明提出的机械臂的奇异构型的判断方法，采用关节螺旋法，简化冗余机械臂的雅克比矩阵，从而加速判断机械臂是否处于奇异构型，其中使用速度雅克比矩阵是否为行满秩矩阵判断机械臂是否发生奇异，理论上不存在误差；进一步通过关节螺旋矩阵的三阶子式为零的数量，可以对机械臂是否发生奇异矩阵进行预判。该判断方法不仅适用于非冗余机械臂，同时也适用于冗余机械臂，使用范围更广。本发明提出的机械臂的奇异构型的求解方法，进一步通过模拟退火算法，先验地求解出了一组冗余机械臂的奇异构型，这种方法具有推广价值，并且为工程人员提供速度规划、轨迹规划等指导。

附图说明

图1是本发明一实施例的机械臂的奇异构型的判断方法流程图；

图2是本发明另一实施例的机械臂的奇异构型的求解方法流程图；

图3是本发明优选实施例的绳驱柔性机械臂的结构示意图；

图4是图3机械臂的臂杆结构与现有技术的臂杆结构的对比图；

图5是图3机械臂的关节结构示意图；

图6是图3机械臂的DH坐标系的建立示意图；

图7是本发明优选实施例的模拟退火算法分析奇异条件流程图。

具体实施方式

为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

通过分析研究，发明人发现在中国专利文献CN104385283A中存在以下缺陷：其方法只能用于简单的非冗余机械臂，对于冗余机械臂此方法不再适用；因为对于六自由度的机械臂，可以将其奇异状态的解析解全部列出来，所以此方法具有较高的计算效率，但是不适用于复杂的冗余机械臂。在中国专利文献CN106003057A中存在以下缺陷：1)用某个条件数的阈值来决定机械臂是否发生奇异，这存在一定的理论误差。因为在理论上，机械臂发生奇异时，机械臂的条件数为无穷大，所以此方法判断的奇异状态可能在实际上并没有处于奇异位姿状态。2)只能判断当前机械臂状态是否为奇异状态，而不能事先预判出机械臂在哪些时候会发生奇异。此专利的方法只能根据当前机械臂的状态去计算条件数，从而判断当前机械臂是否处于奇异构型，不能直接给工程人员提出哪些状态构型为奇异构型。针对用一个条件数阈值来判断机械臂是否发生奇异的理论误差，本发明使用速度雅可比矩阵是否为行满秩矩阵来判断机械臂是否发生奇异，这样不存在理论误差。针对不能先验地给出机械臂的奇异构型，本发明使用模拟退火算法求解了冗余机械臂的一组奇异构型，并且模拟退火算法验证了使用非零子式数量作为机械臂靠近奇异状态的正确性。非零子式数量越少，机械臂越靠近奇异状态。针对只适用于非冗余机械臂的平面几何法来加速判断机械臂是否处于奇异构型，本发明使用关节螺旋法，简化冗余机械臂的雅可比矩阵，从而加速判断机械臂是否处于奇异构型，此方法不仅适用于非冗余机械臂，同时也适用于冗余机械臂，使用范围更广。

基于上述研究中所存在的缺陷，本发明提出一种机械臂的奇异构型的判断方法和求解方法，既可以应用于冗余自由度机械臂，又可应用于非冗余自由度机械臂。

如图1所示，本发明一实施例公开了一种机械臂的奇异构型的判断方法，包括：

A1：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；

A2：基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；

A3：将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的至少部分三阶子式是否为零，若所述关节螺旋矩阵的三阶子式为零的数量越多，则所述机械臂离奇异状态越近，若所述关节螺旋矩阵的三阶子式为零的数量越少，则所述机械臂离奇异状态越远。

其中，提出的用关节螺旋矩阵的非零子式的数量来判断机械臂是否靠近奇异状态，具有推广性，适用于任意机械臂(无论机械臂是否冗余)。

基于上述实施例，本发明另一实施例还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现上述的机械臂的奇异构型的判断方法。

如图2所示，本发明一实施例公开了一种机械臂的奇异构型的求解方法，包括：

B1：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；

B2：基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；

B3：将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的预设的部分三阶子式是否为零，并采用模拟退火算法来求解所述关节螺旋矩阵的预设的部分三阶子式均为零时所述机械臂的关节角的状态，即为所述机械臂的奇异构型。

本发明针对冗余自由度(自由度数超过6即可被称为冗余自由度)机械臂，首先给出简化的关节螺旋矩阵，然后用模拟退火算法求解一组机械臂的奇异构型，为工程人员提供指导。其中提出的使用模拟退火算法来先验地得到一组机械臂的奇异构型，同样具有推广性，适用于任意机械臂(无论机械臂是否冗余)。

基于上述实施例，本发明另一实施例还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现上述的机械臂的奇异构型的求解方法。

为了便于理解，下述优选实施例以绳驱机械臂为例，讲述如何简化冗余机械臂的奇异条件，并用模拟退火算法求解冗余机械臂的一种奇异构型。

首先对本文中的术语进行解释：冗余自由度机械臂是指机械臂的自由度数量大于6；奇异状态是指机械臂末端在该状态下失去了一些自由度；奇异构型是指机械臂在奇异状态下的空间位姿形状；雅可比矩阵是指关节空间到工作空间的速度映射矩阵。驱动空间是指绳驱机械臂中关节相关的物理参数，如：关节力矩，关节角度，关节角速度等，此文讨论的是关节角度和关节角速度。关节空间是指绳驱机械臂中关节相关的物理参数，如：关节力矩，关节角度，关节角速度等，此文讨论的是关节角度和关节角速度。工作空间是指绳驱机械臂中末端相关的物理参数，如：末端所受外力矩，末端所受外力，末端速度等，此文讨论的是末端速度和角速度。

1、冗余机械臂的结构

如图3，以包括五个关节的机械臂为例，其中包括驱动基座30，5个臂杆11、12、13、14、15，5个关节21、22、23、24、25，15根绳索和末端执行器16(也是第6臂杆)，图3中末端执行器16上夹取了一重物块40。结合图4和图5，5个关节分别连接在每相邻两个臂杆之间以及最端部的臂杆15和末端执行器16之间，15根绳索的第一端均由驱动基座30进行驱动，15根绳索的第二端依次穿过臂杆两端设置的圆盘上的过绳孔61以分别连接在5个关节的端部，其中每个关节由3根绳索驱动，臂杆的两端部以及末端执行器的一端分别设有旋转支架且每个臂杆的两端部分别设置的旋转支架的旋转轴101、102相互垂直，每相邻两个臂杆的两个旋转支架以及最端部的臂杆和末端执行器的两个旋转支架分别通过销轴52连接在万向节51上以形成关节，且形成关节的每两个旋转支架之间相互垂直。

每个万向节的俯仰运动和偏航运动需要三根绳索驱动，因此穿过第1臂杆11的绳索最多，为15根，穿过第5臂杆15的绳索最少，为3根，穿过第5臂杆15的三根绳索最终控制着末端执行器。本实施例中只是示意，本发明的推导适用于任意关节数量，各个臂杆之间依靠万向节连接，一个万向节就是一个关节，驱动箱驱动绳索运动，绳索驱动臂杆运动，臂杆绕关节可以做俯仰运动和偏航运动，故一个关节有两个自由度。此机械臂的臂杆如图4的右边所示，臂杆的第一旋转轴101和第二旋转轴102是垂直的，而不是像左边一样是平行的，这样能够增加实际在机械臂臂杆装配时的成功率，并且简化推导的模型。臂杆之间通过万向节51相连，如图5所示。

对于图4所示的机械臂模型，其臂杆是垂直型的，按照机器人学中经典的DH建模方法。可以建立图6所示的DH坐标系，通过DH坐标系之间的变化法则能够得到如表1所示的DH参数表，表中θ_l是相邻两连杆公垂线的夹角，d_l是第l个关节角(俯仰角或偏航角，l为奇数时为俯仰角，l为偶数时为偏航角)所连接的相邻两连杆公垂线间的距离，a_l是连杆长度，α^DH_l(°)是连杆扭转角，L₀为相邻关节中心之间的长度，L_g为第5关节(与末端执行器相邻的在末端的关节)到末端执行器中心的距离，α_i是第i关节的俯仰角，β_i是第i关节的偏航角，第i关节的i可取值为1、2、3、4、5。

表1绳驱机械臂的DH坐标系的参数表

2、冗余机械臂的速度雅克比矩阵

关节空间到工作空间的映射关系实际上就是已知关节角速度，求解末端执行器的线速度和角速度。在经典的DH坐标系下，可以用构造法求解关节空间到工作空间的速度雅可比(Jacobian)矩阵。

对绳驱冗余机械臂而言，所有的关节都是旋转关节，所以以下以旋转关节为例，进行Jacobian矩阵的分析。以图6中的坐标系{0}作为基坐标系，则：

⁰z_l-1＝⁰T_l-1(1：3，3) (1)

⁰T_l-1代表坐标系{l-1}相对于坐标系{0}的齐次变换矩阵；⁰z_l-1代表⁰T_i-1中第1～3行，且位于第3列的子矩阵(也即其中的括号内的逗号前面的代表行，逗号后面的代表列，A：B代表从第A行或列到第B行或列)；

其中⁰T_l-1(1：3，3)通过式(2)并结合表1中的绳驱机械臂的DH坐标系的参数表，运用齐次变换矩阵的右乘，即可计算出来。

⁰ρ_l→2n＝⁰p_2n-⁰p_l-1＝⁰T_2n(1：3，4)-⁰T_l-1(1：3，4) (3)

式中，c表示cos，s表示sin，θ_l、α_{DH_l}、a_l、d_l分别为绳驱机械臂的DH参数，⁰ρ_l→2n为坐标系{l-1}的原点指向坐标系{2n}的原点的位置矢量在坐标系{0}下的表示，0p2n为坐标系{2n}的原点在坐标系{0}下的表示，⁰p_l-1为坐标系{l-1}的原点在坐标系{0}下的表示，¹⁰T_2n(1：3，4)代表¹⁰T_2n中第1～3行且位于第4列的子矩阵，其中的括号内的逗号前面的代表行，逗号后面的代表列，A：B代表从第A行或列到第B行或列。

n表示机械臂的关节或臂杆的总数，本实施例中2n＝10，特别地，对于第1关节，有：

结合式(1)和(3)可以求得Jacobian矩阵的第l列，其计算公式为式(5)：

因此末端的速度与关节的角速度之间的关系为：

式中：v_e——末端的速度矢量，v_e＝[v_ex v_ey v_ez]^T。

w_e——末端的角速度矢量，w_e＝[w_ex w_ey w_ez]^T。

——各关节角速度组成的矢量，w＝[w₁，w₂，…，w₁₀]^T，w_l代表坐标系{l}的旋转角速度，其中l为奇数时为关节的俯仰角速度，l为偶数时为关节的偏航角速度。

⁰J_vej——为关节空间到工作空间的雅可比矩阵，⁰J_vej＝[⁰J₁，⁰J₂，...，⁰J₁₀]^T。

3、冗余机械臂的奇异条件分析

当速度雅可比矩阵⁰J_vej发生奇异时，机械臂至少损失一个自由度，使得机械臂不能完成指定任务，并且会影响关节角速度的解结构，因此分析机械臂的奇异条件在实际的应用中有重要的指导意义。奇异条件分析一直是各类机械臂的热点问题，而冗余机械臂的奇异条件分析是一个极其具有挑战的难题。为分析冗余机械臂的奇异条件，需要先对关节空间到工作空间的速度雅可比矩阵进行改造，简化分析对象。

对于式(5)有如下关系：

式中：

——矢量⁰p_2n的叉乘操作数；

与⁰p_2n各分量之间的关系如式(8)所示：

式中：p_2nz，p_2ny，p_2nz——矢量⁰p_2n的各个分量。

令

由式(7)和式(9)可得

⁰G_vej＝⁰M·⁰S (10)式中：⁰S——关节螺旋矩阵，⁰S∈R^6×10。

式中：⁰S_l——关节螺旋矩阵⁰S的第l列。

值得注意的是，式(6)中使用的是相对于基坐标系{0}的雅可比矩阵，所以求得的末端速度也是末端相对于基坐标系{0}的速度。在实际运动规划中，常常是先找到世界坐标系与基坐标系{0}的位姿关系，再通过基坐标系{0}找到世界坐标系与末端工具坐标系之间的位姿关系，因此在进行末端速度计算时，需要以基坐标系{0}作为参考。但是在进行奇异分析时，目标是找到雅可比矩阵的奇异条件，因此不必局限于只选取基坐标系{0}作为参考，可以证明不同坐标系下的雅可比矩阵的奇异条件是等价。

相对于任意坐标系{k}作为参考坐标系时，都有式(12)成立

^kJ_vej＝^kM·^kS (12)

而^kM始终为一个可逆矩阵，所以rank(^kJ_vej)＝rank(^kS)，因此分析雅可比矩阵^kJ_vej的奇异条件可以转化为分析改进的关节螺旋^kS的奇异条件。

^kS中的第l列的元素包含^kz_l-1和^kz_l-1×^kp_l-1，显然当选择坐标系{l-1}为参考坐标系时，^kS中的第l列表达式最为简洁，第l-1列和第l+1列次之，离第l列越远，那一列的表达式越复杂。

因为^kS共有10列，所以选择坐标系{4}作为参考坐标系时，会使得第5列的表达式最为简洁，会使得整个矩阵的表达式较为简洁。将式(11)的参考坐标系{0}换为参考坐标系{4}，计算后即可得到⁴S的表达式。

式中

式(14)和(15)的元素S₄₁，S₆₁，S₆₂，S₁₉，S₁₁₀，S₂₉，S₂₁₀，S₃₁₀，S₄₉，S₄₁₀，S₅₉，S₅₁₀，S₆₁₀的值的表达式如下：

S₄₁＝-L₀sα₂sβ₁((sβ₂(cα₁sα₂+cα₂cβ₁sα₁)+cβ₂sα₁sβ₁)(cβ₁sα₁+cβ₂(cα₁sα₂+cα₂cβ₁sα₁)-sα₁sβ₁sβ₂)-(cβ₁cβ₂-cα₂sβ₁sβ₂)(sβ₁+cβ₁sβ₂+cα₂cβ₂sβ₁)+(sβ₂(sα₁sα₂-cα₁cα₂cβ₁)-cα₁cβ₂sβ₁)(cβ₂(sα₁sα₂-cα₁cα₂cβ₁)-cα₁cβ₁+cα₁sβ₁sβ₂)-L₀sα₂(cβ₁cβ₂-cα₂sβ₁sβ₂)

S₆₁＝L₀sα₂sβ₁((cβ₂(cα₁sα₂+cα₂cβ₁sα₁)-sβ₂sα₁sβ₁)(cβ₁sα₁+cβ₂(cα₁sα₂+cα₂cβ₁sα₁)-sα₁sβ₁sβ₂)+(cβ₁sβ₂+cα₂sβ₁cβ₂)(sβ₁+cβ₁sβ₂+cα₂cβ₂sβ₁)+(cβ₂(sα₁sα₂cα₁cα₂cβ₁)+cα₁sβ₂sβ₁)(cβ₂(sα₁sα₂-cα₁cα₂cβ₁)-cα₁cβ₁+cα₁sβ₁sβ₂)-L₀sα₂(cβ₁sβ₂+cα₂sβ₁cβ₂)

S₁₉＝-sβ₄(sα₃sα₄-cα₃cα₄cβ₃)+cα₃cβ₄sβ₃

S₂₉＝sβ₄(cα₃sα₄-cα₃cα₄sβ₃)+sα₃cβ₄sβ₃

S₄₉＝-L₀(sβ₄(cα₃sα₄+cα₄cβ₃sα₃)+cβ₄sα₃sβ₃)(sβ₃+cβ₃sβ₄+cα₄cβ₄sβ₃)-L₀(cβ₃cβ₄-cα₄sβ₃sβ₄)(cβ₃sα₃+cβ₄(cα₃sα₄+cα₄cβ₃sα₃)-sα₃sβ₃sβ₄)

S₅₉＝-L₀(cβ₃cβ₄-cα₄sβ₃sβ₄)(cβ₄(sα₃sα₄-cα₃cα₄cβ₃)-cα₃cβ₃+cα₃sβ₃sβ₄)-L₀(sβ₄(sα₃sα₄-cα₃cα₄cβ₃)-cα₃cβ₄sβ₃)(sβ₃+cβ₃sβ₄+cα₄cβ₄sβ₃)

S₁₁₀＝sα₅(cβ₄(sα₃sα₄-cα₃cα₄cβ₃)+cα₃sβ₃sβ₄)-cα₅(cα₄sα₃+cα₃cβ₃sα₄)S₂₁₀＝cα₅(cα₃cα₄-cβ₃sα₃sα₄)-sα₅(cβ₄(cα₃sα₄+cα₄cβ₃sα₃)-sα₃sβ₃sβ₄)S₃₁₀＝sα₅(cβ₃sβ₄+cα₄cβ₄sβ₃)+cα₅sα₄sβ₃

S₄₁₀＝-L₀(sα₃sα₅sβ₄+cα₃cα₄cα₅sβ₃+cα₃cα₅cβ₄sβ₃-cα₅sα₃sα₄sβ₄+cα₃cα₄cα₅cβ₃sβ₄-cα₃cβ₄sα₄sα₅sβ₃)

S₅₁₀＝-L₀(cα₄cα₅sα₃sβ₃-cα₃sα₅sβ₄+cα₃cα₅sα₄sβ₄+cα₅cβ4sα₃sβ₃+cα₄cα₅cβ₃sα₃sβ₄-cβ₄sα₃sα₄sα₅sβ₃)

S₆₁₀＝-L₀(cα₄cα₅cβ₃+cα₅cβ₃cβ₄-cα₄cα₅sβ₃sβ₄-cβ₃cβ₄sα₄sα₅)

上述式中，c表示cos，s表示sin，α_i是第i关节的俯仰角，β_i是第i关节的偏航角，第i关节的i可取值为1、2、3、4、5。

根据sα₃是否等于0，对式(13)进行如下讨论。

(1)当sα₃＝0时，由于α₃∈[-35°，35°]，故α₃＝0°，式(13)化简为

对式(16)做如下初等变换，用

代表变换后的矩阵

变换后可得

式中

由

可知，

对于

是行满秩阵的情况，有

成立。

由前面的讨论可知，要求解⁴J_vej的奇异条件等价于求解

的情况，即

通过前述的一系列推导，已经将一个6×10的雅可比矩阵的奇异问题，转化为式中3×7的关节螺旋矩阵的非行满秩问题，矩阵维数降低，减少了运算量。

根据矩阵的秩的性质，可以知道

由式(23)可知，当矩阵

非行满秩时，对应于方阵

不可逆，即：

(2)当sα₃≠0时，对式(13)进行式(25)所示的初等变换，化简为式(26)

式中

由

可知，

对于

是行满秩阵的情况，有

成立。

与sα₃＝0的情况类似，要求解⁴J_vej的奇异条件等价于求解：

因此，对于十自由度的冗余机械臂，其奇异条件为式(24)和式(30)。相比于原来，需要判断6×10维的雅可比矩阵⁰J_vej是否行满秩，化简后奇异条件只需要判断3×7维的关节螺旋矩阵

和

是否行满秩，大大简化了运算量。

4、模拟退火算法求解一组奇异构型

虽然式(24)和式(30)已经简化了奇异条件，但是想要先验地知道哪些机械臂的构型是奇异构型，这仍然很困难。因为要提前知道机械臂在哪些位姿状态下处于奇异状态，就意味着需要解析地求解出式(24)和式(30)，但是式(24)和式(30)是高维非线性方程组的求解，不可能解析地求解出来，因此需要用一些特殊的方法先验地求解出机械臂的奇异构型，而这样的奇异构型能够给工程人员一个参考，使得工程人员在对冗余机械臂进行轨迹规划或速度规划时，尽量地避开这个奇异构型。

根据Cauchy-Binet定理，有等式(31)成立。

式中，M_j表示矩阵S的各三阶子式，对于

和

来说，都是共有

项。

只考察sα₃＝0的情况，结合式(24)与式(31)，可以得到

的非行满秩条件为：

M_j＝0，j＝1，2，…，35 (32)

即需要

的所有三阶子式同时为0，雅可比矩阵才能发生奇异，从这也能看出，机械臂的自由度越多，所需要同时为0的子式也越多，雅可比矩阵越难发生奇异。实际上，判断3阶子式是否为0还可以转化为判断3阶子矩阵是否满秩。

本发明提出一个评价机械臂离奇异状态距离的指标：非零子式的数量。非零子式的数量越少，则机械臂离奇异状态越近，反之则越远。

由于α₃＝0°，故接下来需要分析其它9个关节角等于何值时能使得机械臂发生奇异，这是一个寻优搜索问题，此问题的目标是找到使非零子式的数量为0的条件。显然，这样的条件可能有很多，但一定存在一个对关节角要求最少的条件，为了找到这个条件，将9个关节角划分为两种情况，一种是关节角为0°(记为0)，另一种是关节角为(-90°，90°)的任意值(记为1)，包括0°。单独把0°挑选出来是因为式(32)中存在大量耦合的正余弦，而0°是能使得式(32)为0的重要条件。

本发明采用模拟退火算法求解当非零子式的数量为0时，机械臂关节角的状态，其具体流程图如图7所示。

在图7中，模拟退火参数包含初始温度T₀、结束温度T_end、降温速率q以及链长L，其中初始温度T₀决定了在搜索最优解时跳出局部最优的能力，链长L决定了在某一固定温度时产生新解的数量，结束温度T_end和降温速率q共同决定了算法的迭代次数。初始序列S₁是通过随机数以及给定的关节置零数量产生的，在全为1的数组中随机选定一些位置置零。count和k都是用来跟踪算法进行的过程。产生新解的过程也是随机过程，随机挑选数组的两个位置进行置换，为了保证置换前后数组不同，在子函数里还设置了条件判断语句。将置换前的旧解和置换后的新解用Metropolis准则判断，用两组解的非零子式的数量作为目标函数，当非零子式的数量为0时，式(32)成立，机械臂发生奇异。将用Metropolis准则判断得到的更优解作为新的S₁，并存储下来，然后进行下一步的迭代。当一次链长K的迭代结束后，进行降温，直到温度小于结束温度T_end，退出循环，输出所有最优解序列。

图7的Metropolis准则为式(33)的概率模型，其中df＝f(S₂)-f(S₁)，f即为在给定的关节状态下，非零子式的个数。

以本文的10自由度机械臂为例，用模拟退火算法进行运动奇异分析，分析雅可比矩阵的奇异条件。设置初始温度T₀＝100、结束温度T_end＝10^-2、降温速率q＝0.7，一次迭代会产生链长L＝20个新解，一次迭代后进行降温。在α₃＝0的前提下，将MATLAB的运算结果绘制成表2。

表2当α₃＝0时，不同关节置零数量的模拟退火算法求解结果

从表2中可以看出，关节置零数量越多，非零子式的数量越少，离奇异状态越近，这说明了用非零子式的数量作为奇异条件判断的合理性。此外，用非零子式的数量作为模拟退火算法的目标函数，所寻找到的关节状态也非常合理，在关节置零数量为1-5时，虽然没有找到奇异条件，但是通过观察关节状态可以知道，关节α₁，α₅，β₁，β₅对奇异状态影响很小，因为在最优解对应的关节状态中它们的值始终为“1”。当关节置零数量为6时，算法找到了唯一的奇异条件。至此，算法的计算次数为20×14＝280，小于循环搜索算法的计算次数。当关节置零数量为7、8时，存在多组奇异条件，分别为

组，这也进一步证明了α₁，α₅，β₁，β₅对奇异状态没有影响。

通过模拟退火算法得到，当式(34)成立时，式(32)成立，此时机械臂发生奇异。式(34)表示的是一组机械臂的奇异构型，从这个构型中可以看到，当关节2,3,4的俯仰角和偏航角都为0°的时候，无论关节1和关节5的关节角处于何种状态，机械臂都发生了奇异。

α₂＝0，α₃＝0，α₄＝0，β₂＝0，β₃＝0，β₄＝0 (34)

本文提供的这一寻找奇异条件的方法可以推广到更多自由度的机械臂中，用非零子式的数量作为目标函数的模拟退火算法可以很快地求解出冗余机械臂的一组奇异构型。

本发明优选实施例建立了十自由度机械臂的关节螺旋矩阵，对其进行了讨论，简化了冗余机械臂奇异状态的判断；提出用关节螺旋矩阵的非零子式的数量来描述机械臂靠近奇异状态的程度：非零子式数量越少，机械臂越靠近奇异状态；用模拟退火算法求解了冗余机械臂的一组奇异构型，为工程人员的轨迹规划和速度规划提供参考，在做规划的时候能够避免使机械臂处于奇异状态。

本发明优选实施例提出的机械臂的奇异构型的判断方法和求解方法，存在以下优点：

(1)对冗余机械臂的奇异判断更加严谨：使用速度雅可比矩阵是否为行满秩矩阵来判断机械臂是否发生奇异，这样做更加严谨，理论上不存在误差。

(2)简化冗余机械臂的奇异条件：本发明针对十自由度的绳驱机械臂，采用关节螺旋法，对速度雅可比矩阵进行简化，将原本需要判断6×10的雅可比矩阵是否行满秩，简化为判断3×7的雅可比矩阵是否行满秩，大大减少计算量，提升计算效率。

(3)提出一种靠近奇异状态的评价指标：本发明提出用关节螺旋矩阵的非零子式的数量来判断机械臂是否靠近奇异状态。如果关节螺旋矩阵的非零子式数量越少，那么机械臂越靠近奇异状态，当非零子式的数量达到0时，机械臂发生奇异。

(4)提出使用模拟退火算法来先验地得到一组机械臂的奇异构型：冗余机械臂的奇异条件较为复杂，通常是由多个非线性方程构成的，这些非线性方程无法联立求出解析解，故无法先验地得到机械臂奇异构型，只能在冗余机械臂实际工作过程中，通过关节的角度来判断机械臂是否发生奇异。而本专利通过模拟退火算法，先验地求解出了一组冗余机械臂的奇异构型，这种方法具有推广价值，并且为工程人员提供速度规划、轨迹规划等指导。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (10)

1.一种机械臂的奇异构型的判断方法，其特征在于，包括：

A1：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；

A2：基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；

A3：将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的至少部分三阶子式是否为零，若所述关节螺旋矩阵的三阶子式为零的数量越多，则所述机械臂离奇异状态越近，若所述关节螺旋矩阵的三阶子式为零的数量越少，则所述机械臂离奇异状态越远。

2.根据权利要求1所述的机械臂的奇异构型的判断方法，其特征在于，步骤A1中求解的所述机械臂的关节空间到工作空间的速度雅克比矩阵为：

式中，⁰J_vej为所述机械臂的关节空间到工作空间的速度雅克比矩阵，w为所述机械臂的各关节角速度组成的矢量，v_e为所述机械臂的末端的速度矢量，w_e为所述机械臂的角速度矢量。

3.根据权利要求1所述的机械臂的奇异构型的判断方法，其特征在于，步骤A2中所述速度雅克比矩阵与关节螺旋矩阵的关系为：对于以所述机械臂的DH的任意{k}坐标系，都存在rank(^kJ_vej)＝rank(^kS)，^kS为关节螺旋矩阵。

4.根据权利要求1所述的机械臂的奇异构型的判断方法，其特征在于，步骤A2中将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件具体为：

n为所述机械臂的关节的数量，n为奇数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵；n为偶数时，n为偶数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵。

5.根据权利要求4所述的机械臂的奇异构型的判断方法，其特征在于，步骤A3中将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的至少部分三阶子式是否为零具体为：当n为奇数时，将所述关节螺旋矩阵的奇异条件转化为判断

的所有三阶子式是否为零；当n为偶数时，将所述关节螺旋矩阵的奇异条件转化为判断

的所有三阶子式是否为零。

6.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现权利要求1至5任一项所述的机械臂的奇异构型的判断方法。

7.一种机械臂的奇异构型的求解方法，其特征在于，包括：

B1：基于所述机械臂的DH坐标系，求解所述机械臂的关节空间到工作空间的速度雅克比矩阵；

B2：基于关节螺旋法，得到所述速度雅克比矩阵与关节螺旋矩阵的关系，以将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件；

B3：将所述关节螺旋矩阵的奇异条件转化为判断所述关节螺旋矩阵的预设的部分三阶子式是否为零，并采用模拟退火算法来求解所述关节螺旋矩阵的预设的部分三阶子式均为零时所述机械臂的关节角的状态，即为所述机械臂的奇异构型。

8.根据权利要求7所述的机械臂的奇异构型的求解方法，其特征在于，步骤B1中求解的所述机械臂的关节空间到工作空间的速度雅克比矩阵为：

式中，⁰J_vej为所述机械臂的关节空间到工作空间的速度雅克比矩阵，w为所述机械臂的各关节角速度组成的矢量，v_e为所述机械臂的末端的速度矢量，w_e为所述机械臂的角速度矢量；

进一步地，步骤B2中所述速度雅克比矩阵与关节螺旋矩阵的关系为：对于以所述机械臂的DH的任意{k}坐标系，都存在rank(^kJ_vej)＝rank(^kS)，^kS为关节螺旋矩阵。

9.根据权利要求7所述的机械臂的奇异构型的求解方法，其特征在于，步骤B2中将所述速度雅克比矩阵的奇异条件转化为所述关节螺旋矩阵的奇异条件具体为：

n为所述机械臂的关节的数量，n为奇数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵；n为偶数时，n为偶数时，所述关节螺旋矩阵的奇异条件为：

为所述关节螺旋矩阵的子矩阵。

进一步地，当n为奇数时，所述关节螺旋矩阵的预设的部分三阶子式为

的所有三阶子式；当n为偶数时，所述关节螺旋矩阵的预设的部分三阶子式为断

的所有三阶子式。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令在被处理器调用和执行时，所述计算机可执行指令促使处理器实现权利要求7至9任一项所述的机械臂的奇异构型的求解方法。

Images