一种机械臂运动学形式化分析方法

摘要

本发明涉及一种机械臂运动学形式化分析方法，包括如下步骤：步骤一，确定机械臂机构的结构参数；步骤二，用高阶逻辑语言建立机械臂机构运动学逻辑模型；步骤三，用逻辑公式描述需要验证的运动学约束或属性；步骤四，将机械臂机构运动学逻辑模型和需要验证的运动学约束或属性组成一个逻辑命题；步骤五，利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，否则，不成立表明所述模型不满足所述的约束或者不具备所述的属性。本发明中的形式化验证根据系统形式规范或属性，使用数学方法证明系统的正确性，对所验证的性质而言是精确和完备的。

权利要求

1.一种机械臂运动学形式化分析方法，其特征在于，包括如下步骤：步骤一，确定机械臂机构的结构参数；

步骤二，用高阶逻辑语言建立机械臂机构运动学逻辑模型；

步骤三，用逻辑公式描述需要验证的运动学约束或属性；

步骤四，将机械臂机构运动学逻辑模型和需要验证的运动学约束或属性组成一个逻辑命题；

步骤五，利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，否则，不成立表明所述模型不满足所述的约束或者不具备所述的属性；

其中，所述步骤二中，机械臂机构运动学逻辑模型是基于旋量理论或D-H参数法的高阶逻辑形式化表达。

2.根据权利要求1所述的机械臂运动学形式化分析方法，其特征在于，所述机械臂为n自由度旋转机械臂，其参数为：αi表示各旋转副转动轴之间的扭角，ai表示相邻两旋转副之间的偏距量，即连杆长度，di表示两相邻旋转副的旋转轴之间公法线垂足之间的距离，θi表示旋转副旋转的角度，i＝1,2,3…n；

根据上述参数依次确定各旋转关节的旋量坐标为：

其中，表示旋量的原部， 表示旋量的偶部，i＝1,2,3，旋量通过如下的方式确定：其中，zi,pi分别表示机械臂第i个旋转副在初始状态下的旋转轴和位置矢量。

3.根据权利要求2所述的机械臂运动学形式化分析方法，其特征在于，所述步骤二中构建的机构运动学逻辑模型为，定义1.旋转副的旋量，

val rotation_screw z p＝(z；p×z)其中，函数rotation_screw的输入变量z,p分别表示旋转副运动后的旋转轴和位置矢量，其返回值为旋转副的旋量，其数据类型为旋量；

定义2.串联n旋转副机械臂：

valserial_ndof_M A1A2...An＝A1+A2+...+An其中，函数serial_ndof_M的输入变量为各旋转副的旋量，函数的返回值为机械臂末端执行器的等价旋量；

定义3.平面n自由度初始状态：

val snm_initial_state＝(A1,A2,...,An)其中，变量A1,A2,...,An为旋量，其原部分别表示各旋转副在初始状态下的旋转轴，其偶部分别表示各旋转副在初始状态下的位置矢量。

4.根据权利要求3所述的机械臂运动学形式化分析方法，其特征在于，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的旋转角度等于各旋转副的旋转角度之和；所述步骤四中的逻辑命题为：上述逻辑命题中，sin_x函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的正弦函数值；cos_x函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的余弦函数值； 函数的功能是将对空间中的点或者向量绕固定轴r旋转角度θ，该函数返回值的数据类型为向量类型。

5.根据权利要求3所述的机械臂运动学形式化分析方法，其特征在于，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的可达位置位于半径为(a1+a2+a3)圆形区域之内；所述步骤四中的逻辑命题为，其中，transf为定义的高阶逻辑函数，其功能为将4维向量类型的数据取前三位顺序排列构成新的3维向量。

6.根据权利要求3所述的机械臂运动学形式化分析方法，其特征在于，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的可达区域分析；所述步骤四中的逻辑命题为，其中，在此逻辑命题中si_configuration(θ1,θ2,θ3,Q)是判断机构是否奇异的补偿函数，如果命题的返回值为真，则机械臂的在该位置可达，反之则不可达。

说明书

一种机械臂运动学形式化分析方法

技术领域

[0001] 本发明涉及机械臂技术领域，尤其涉及机械臂运动学形式化分析方法。

背景技术

[0002] 近年来机械臂技术进入高速发展期，带有机械臂的机器人在工业、科学考察、军事、抢险救灾、医疗和家用等各个领域广泛应用。但是机器人执行任务时失败甚至出现安全事故也屡见不鲜。人们意识到机器人的验证技术与设计技术之间存在显著差距。机器人设计中的一个微小漏洞可能造成巨大的经济损失，甚至危及人身安全，因此设计者必须花大量的时间精力对设计进行分析和验证，以便能在产品成型前找到机器人设计的错误。

[0003] 目前，机械臂设计正确性的常用验证方法有：(1)基于纸笔运算的人工分析；(2)计算机模拟仿真；(3)计算机代数系统。基于纸笔运算的人工分析方法是在设计者经验的基础上，通过建立合适的数学模型对机械臂的关键性质进行验证。但考虑到机器人设计的复杂性，这种方法往往因计算过于繁琐而难以实现且容易出错。计算机模拟仿真方法是机械臂的设计与验证使用最为广泛的方法，该方法在合适的仿真软件(如MATLAB)上建立相应的仿真模型来对机械臂设计模型进行验证。这种方法虽然高效但受到仿真平台的运算速度和内存的限制，同时也无法给出完全精确的仿真结果。随着Maple、Mathematica等软件的发展，基于计算机代数系统对机械臂属性的验证变得流行。基于计算机代数系统的方法与计算模拟仿真方法相比精度更高，但其在边界条件的处理上存在短板，同时计算机代数系统内核中的符号计算算法也尚未得到验证，可能因其自身存在漏洞而影响其验证结果的精确性。

发明内容

[0004] 有鉴于此，本发明的目的在于提供一种机械臂运动学形式化分析方法，以解决现有技术中计算复杂、结果不太精确的问题。

[0005] 本发明提供的机械臂运动学形式化分析方法，包括如下步骤：

[0006] 步骤一，确定机械臂机构的结构参数；

[0007] 步骤二，用高阶逻辑语言建立机械臂机构运动学逻辑模型；

[0008] 步骤三，用逻辑公式描述需要验证的运动学约束或属性；

[0009] 步骤四，将机械臂机构运动学逻辑模型和需要验证的运动学约束或属性组成一个逻辑命题；

[0010] 步骤五，利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，否则，不成立表明所述模型不满足所述的约束或者不具备所述的属性；所述步骤二中，机械臂机构运动学逻辑模型是基于旋量理论或D-H参数法的高阶逻辑形式化表达。

[0011] 优选地，所述机械臂为n自由度旋转机械臂，其参数为：αi表示各旋转副转动轴之间的扭角，ai表示相邻两旋转副之间的偏距量，即连杆长度，di表示两相邻旋转副的旋转轴之间公法线垂足之间的距离，θi表示旋转副旋转的角度，i＝1,2,3…n；

[0012] 根据上述参数依次确定各旋转关节的旋量坐标为：

[0013]

[0014] 其中，表示旋量的原部， 表示旋量的偶部，i＝1,2,3，旋量通过如下的方式确定：

[0015]

[0016]

[0017] 其中，zi,pi分别表示机械臂第i个旋转副在初始状态下的旋转轴和位置矢量。

[0018] 优选地，所述步骤二中构建的机构运动学逻辑模型为，

[0019] 定义1.旋转副的旋量，

[0020] val rotation_screw z p＝(z；p×z)

[0021] 其中，函数rotation_screw的输入变量z,p分别表示旋转副运动后的旋转轴和位置矢量，其返回值为旋转副的旋量，其数据类型为旋量；

[0022] 定义2.串联n旋转副机械臂：

[0023] valserial_ndof_MA1A2...An＝A1+A2+...+An

[0024] 其中，函数serial_ndof_M的输入变量为各旋转副的旋量，函数的返回值为机械臂末端执行器的等价旋量；

[0025] 定义3.平面n自由度初始状态：

[0026] val snm_initial_state＝(A1,A2,...,An)

[0027] 其中，变量A1,A2,...,An为旋量，其原部分别表示各旋转副在初始状态下的旋转轴，其偶部分别表示各旋转副在初始状态下的位置矢量。

[0028] 优选地，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的旋转角度等于各旋转副的旋转角度之和；所述步骤四中的逻辑命题为：

[0029]

[0030] 上述逻辑命题中，sin_x函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的正弦函数值；cos_x函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的余弦函数值； 函数的功能是将对空间中的点或者向量绕固定轴r旋转角度θ，该函数返回值的数据类型为向量类型。

[0031] 优选地，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的可达位置位于半径为(a1+a2+a3)圆形区域之内；所述步骤四中的逻辑命题为，[0032]

[0033] 其中，transf为定义的高阶逻辑函数，其功能为将4维向量类型的数据取前三位顺序排列构成新的3维向量。

[0034] 优选地，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的可达区域分析；所述步骤四中的逻辑命题为，

[0035]

[0036] 其中，在此逻辑命题中si_configuration(θ1,θ2,θ3,Q)是判断机构是否奇异的补偿函数，如果命题的返回值为真，则机械臂的在该位置可达，反之则不可达。。

[0037] 和传统方法不同，形式化验证根据系统形式规范或属性，使用数学方法证明系统的正确性，对所验证的性质而言是精确和完备的。而基于刚体运动学发展而来的旋量理论具有物理意义明确、表达形式简单、代数运算方便等优点，大大降低了机器人机构学研究的难度，本发明中，通过应用旋量理论，只需建立两个坐标系，只要分析出当前位形下的各个运动副旋量，便可得到其雅可比矩阵，由于运动副旋量的线性相关性与坐标系的选择无关，还可以通过恰当选择坐标系进一步简化其雅可比的解析形式。

附图说明

[0038] 通过以下参照附图对本发明实施例的描述，本发明的上述以及其他目的、特征和优点将更为清楚，在附图中：

[0039] 图1为自由度为3的3R串联机械臂结构示意图；

[0040] 图2为3R机械臂极限位置形式化验证流程图；

[0041] 图3为自由度为6的6R机械臂结构示意图。

具体实施方式

[0042] 以下将参照附图更详细地描述本发明的各种实施例。在各个附图中，相同的元件采用相同或类似的附图标记来表示。为了清楚起见，附图中的各个部分没有按比例绘制。

[0043] 刚体运动描述的是保持物体上任意两点距离不变的运动，它是研究机器人运动学、动力学、控制的核心。相比于简化的质点运动模型而言，刚体运动能将物理的运动描述得更为精确，因而物体的刚体运动模型在工程实际中应用得更为广泛。连续的刚体运动数学上表示为刚体变换。

[0044] 对于由 的子集O描述的给定刚体，其运动可以用一系列的连续变换来描述，也就是说将刚体上各点相对于某个固定坐标系的运动描述为时间的函数。这一系列的变换被称为刚体变换。这些映射的全体构成一个6维的李群，称为特殊欧几里德群(SE(3))，简称特殊欧氏群。由于李群同时具有微分流形和群的性质，即所有刚体变换的全体所构成的集合既满足群的4个基本特征，也构成一个可微分的流形，具有可积可微性。基于刚体运动学发展而来的旋量理论，具有物理意义明确、表达形式简单、代数运算方便等优点，大大降低了机器人机构学研究的难度，本申请中，采用旋量理论来对机械臂运动学进行形式化分析。

[0045] 典型的机械臂机构如图1、图3所示(图1中示出了自由度为3的3R串联机械臂，图3中示出了自由度为6的6R串联机械臂)，其所有的运动副都是转动副，机械臂自由度的数目等于运动副的数目。

[0046] 基于旋量理论，机械臂的末端执行器的位形由各关节的运动旋量的线性组合来表示，即：

[0047]

[0048] 上式中，ω1，ω2，…ωj分别表示各个关节的角速度参量， 表示各关节的单位旋量，各个旋量具有如下的特征：

[0049]

[0050] 根据旋量理论，旋量由2个三维矢量组成，一般写成对偶数的形式，如：

[0051]

[0052] 式中s和s0分别表旋量的原部和偶部，∈为对偶算子，满足：∈≠0，∈2＝∈3＝...＝0。旋量也可以写成Plucker坐标的形式，如：(s，s0)。

[0053] 通过这两个三维矢量可以同时表示向量的方向和位置，如刚体运动中的速度和角速度、力与力矩等。在分析复杂的刚体运动时，运用旋量理论可以把问题的描述和解决变得十分的简洁统一，且易于和其他方法(如向量法、矩阵法)进行转换。

[0054] 对串联机器人机构进行形式化；

[0055] 本申请中，使用剑桥大学研发的高阶逻辑定理证明器HOL4(Higer-Order-Logic)作为形式化工具。HOL4是最成熟的高阶逻辑定理证明器之一，拥有庞大的研究团队和用户群，在软硬件验证领域已有很多成功的应用。

[0056] 在HOL4中，机械臂机构被形式化表示如下：

[0057] 定义：串联机器人机构

[0058]

[0059] 在上述定义中，函数serial_manipulator的输入变量n,A分别表示机构关节的个数n,各关节旋量所组成的序列An。函数的返回值为机械臂机构的和旋量。

[0060] 然后，基于各关节的运动参数，即各关节的转动角速度和位置矢量，各关节的旋量也可以随之确定。根据上述定义，6自由度的6R机械臂机构可以形式化表示如下：

[0061]

[0062] 式中，ωi表示各关节的转动角速度，Pi表示各关节的位置矢量。

[0063] 根据各关节的旋量可以确定机构的旋量雅克比矩阵。

[0064] 定义：旋量雅克比矩阵

[0065] 空间6R串联机械臂机构的雅克比矩阵被定义如下：

[0066]

[0067] 在上述定义中，函数sc_jacobian的输入变量为各关节的旋量，返回值为6×6的雅克比矩阵。

[0068] 雅克比矩阵是表征机构是否处于奇异位形的重要参数。当机构处于奇异位形时，末端执行器失控，可能引起不可预料的后果，因此在设计机器人机构时，应尽可能避免使机器人机构处于奇异位形状态。

[0069] 如果雅克比矩阵的行列值为0，则说明机构处于奇异位形状态，生成错误的报告，结束本次形式化分析。

[0070] 刚体运动通常以一种连续的变换——刚体变换来描述，这种变换满足如下两个条件：

[0071] (1)保持刚体上任意两点间的距离不变，对于任意的点 均有：

[0072] ||g(p)-g(q)||＝||p-q||

[0073] (2)保持刚体上任意两点矢量间的相对姿态不变，对于任意的矢量，均有：

[0074] g(v×w)＝g(v)×g(w)

[0075] 刚体运动须同时描述刚体上任意一点的平动及刚体绕该点的转动。其中，刚体的平动可以直接用向量来描述，而刚体的转动只改变刚体的姿态，刚体的姿态可以用附着在刚体上的动坐标{B}(物体坐标系)相对于参考坐标系{A}的相对姿态来描述。具体用别表示物体坐标系相对于参考坐标系的坐标，写成矩阵形式

[0076] R＝[xAB yAB zAB]

[0077] R被称为旋转矩阵，且满足如下的关系式：

[0078] R＝[xAB yAB zAB]，且det(R)＝1

[0079] 针对于一般刚体运动，刚体上各点的运动情况都可以从物体坐标系的运动以及该点相对于物体坐标系的运动来得到，即：

[0080] pA＝RpB+t

[0081] 式中pA是点p在参考坐标系中的坐标表示，pB是点p在物体坐标系中的坐标表示，R,t分别表示旋转矩阵和位置矢量。

[0082] 将上式变换成射影几何中齐次变换的表达形式：

[0083]

[0084] 上式中，

[0085] 称为刚体运动的齐次变换矩阵，为4×4方阵。

[0086] 在HOL4中，齐次矩阵被定义如下：

[0087] 定义：齐次矩阵

[0088]

[0089] 此定义的类型为matrix→vector→matrix。

[0090] 对于一般刚体运动而言，Chalses定理指出，任意刚体运动都可以通过螺旋运动即通过绕某轴的旋转与沿该轴移动的复合运动来实现，也就是说，运动旋量的指数映射 与齐次变换矩阵：

[0091] 是等价的。

[0092] 如果刚体以单位角速度绕轴线ω在作匀速运动，那么刚体上一点p的速度可以表示为：

[0093]

[0094] 其中r表示位置矢量，引入如下的4×4矩阵

[0095]

[0096] 其中，是反对称矩阵，满足 且v＝-r×ω。由此，刚体上一点的速度满足如下的齐次方程：

[0097]

[0098] 也可以写成：

[0099] 此微分方程的解为：

[0100] 上式中 被称为运动旋量。

[0101] 实施例一

[0102] 本实施例中，以平面3自由度机械臂为例对本申请中的机械臂运动学形式化分析方法进行具体说明。

[0103] 如图1所示的平面3自由度机械臂的D-H参数如表一所示：

[0104] 表一平面3自由度机械臂的D-H参数

[0105]关节 αi ai di θi
1 0 a1 a1 θ1
2 0 a2 a2 θ2
3 0 a3 a3 θ3

[0106] 表一中，αi表示各旋转副转动轴之间的扭角，ai表示相邻两旋转副之间的偏距量(连杆长度)，di表示两相邻旋转副的旋转轴之间公法线垂足之间的距离，θi表示旋转副旋转的角度。

[0107] 根据D-H参数依次确定各旋转关节的旋量坐标

[0108]

[0109] 其中，表示旋量的原部， 表示旋量的偶部，i＝1,2,3。旋量可以通过如下的方式确定。

[0110]

[0111] 其中，zi,pi分别表示机械臂第i个旋转副在初始状态下的旋转轴和位置矢量。

[0112] 在HOL4中构建平面3自由度机械臂的高阶逻辑形式模型。

[0113] 定义1.旋转副的旋量。

[0114]val rotation_screw z p＝(z；p×z)

[0115] 其中，函数ration_screw的输入变量z,p分别表示旋转副运动后的旋转轴和位置矢量，其返回值为旋转副的旋量，其数据类型为旋量(3维矢量对)。

[0116] 定义2.移动副的旋量

[0117]val prismatic_screw p＝(0；p)

[0118] 其中，函数prismatic_screw的输入变量p表示移动副移动的方向，其返回值为移动副的旋量。

[0119] 定义3.平面3自由度机械臂：

[0120]val planar_3dof_M A B C＝A+B+C

[0121] 其中，函数planar_3dof_M的输入变量为各旋转副的旋量，函数的返回值为机械臂末端执行器的等价旋量。

[0122] 定义4.平面3自由度初始状态。

[0123]val p3m_initial_state＝(A,B,C)

[0124] 其中，变量A,B,C为旋量，其原部分别表示各旋转副在初始状态下的旋转轴(3维矢量)，其偶部分别表示各旋转副在初始状态下的位置矢量。

[0125] 相关属性验证

[0126] 由于机械臂设计需要达到的相关约束条件，或者说相关属性是多方面的，本申请中以其中的几个属性作为示例，说明本申请中的形式化分析方法的具体分析过程。

[0127] 一、属性1：如图1所示的平面3自由度机械臂末端执行器的旋转角度等于各旋转副的旋转角度之和。

[0128] 建立属性1的形式化命题如下：

[0129]

[0130] 上述逻辑命题中，r，a,b,c是平面3自由度机构初始状态函数的输入值，r＝(0,0,1)为机构3个旋转副在初始状态下的旋转轴，a,b,c分别表示3个旋转副在初始状态下的位置矢量，命题的前件形式化表征了机构初始化的过程。sin_x是HOL4中定义的一个高阶逻辑函数，函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的正弦函数值。cos_x是HOL4中定义的一个高阶逻辑函数，函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的余弦函数值。 是HOL4中定义的一个高阶逻辑函数，其功能将对空间中的点或者向量绕固定轴r旋转角度θ，该函数返回值的数据类型为向量类型。

[0131] 将以上逻辑命题在HOL4的高阶逻辑引擎下进行推理，该命题的返回值为true，即属性1在定理证明器HOL4中验证通过，命题的正确性得到了验证。

[0132] 二、属性2：平面3自由度机械臂末端执行器可达域的分析与验证。

[0133] 通过如图2的流程可以对平面3自由度机械臂末端执行器的可达域进行分析与验证。

[0134] 首先确定机构参数，然后构建逻辑模型，再确定机构极限位姿

[0135] 判断目标位置是否超出极限位置，如果超出，则判断为不可达，如果没有超出，则进一步判断是否为机构奇异位形，如果是奇异位形，则判断为不可达，不是奇异位形，则判断为可达。

[0136] 2.1根据机构D-H参数确定机构的齐次变换矩阵，并将其形式化。

[0137] 机构的齐次变换矩阵可以通过逐级计算关节的D-H变换矩阵确定。

[0138] 关节1的D-H变换矩阵为

[0139]

[0140] 关节2的D-H变换矩阵为

[0141]

[0142] 关节3的D-H变换矩阵为

[0143]

[0144] 则整个机构的齐次变换矩阵为

[0145]

[0146] 其中，cθ＝cosθ,sθ＝sinθ,θ12＝θ1+θ2。

[0147] 上述求齐次变换矩阵的过程可以在HOL4构造相应的逻辑命题如下：

[0148]

[0149] 其中， 依次是按照(1)(2)(3)(4)式构造的高阶逻辑函数，这些函数的输入变量为θ1,θ2,θ3，函数的返回值为4×4的矩阵。该命题在HOL4中推理的返回值为true。

[0150] 确定机构末端执行器的极限位置

[0151] 末端执行器的极限位姿区域为半径为(a1+a2+a3)圆形区域。因此末端执行器的可能可达位置不可能位于圆形区域以外。

[0152] 此属性的可以形式化为如下的命题。

[0153]

[0154] 其中，transf为定义的高阶逻辑函数，其功能为将4维向量类型的数据取前三位顺序排列构成新的3维向量。

[0155] 在形式化推理过程中，goal3最终转换如下的命题,

[0156]

[0157] 命题的返回值为true,该命题验证通过。

[0158] 2.2可达性分析。

[0159] 基于上述步骤，并且结合图2的验证流程，可以构造末端执行器可达区域验证的高阶逻辑验证命题如下。

[0160]

[0161] 其中，在此逻辑命题中si_configuration(θ1,θ2,θ3,Q)是一个判断机构是否奇异的补偿函数，对于不同的机构此补偿函数的形式也不同，有些奇异位形甚至无法用确定的数学表达式来表达，但在实际机器人控制过程中，奇异位形是需要尽可能避免的，并且是否处于奇异位形是不容易判断的。对于goal4而言，如果命题的返回值为真，则机械臂的在该位置可达，反之则不可达。

[0162] 实施例二

[0163] 本实施例中，以一种6R机器人的奇异位形进行判定

[0164] 图3示出了一种6R机器人处于奇异位形的状态图，其中标黑的旋转副处于奇异位形，该机构具有如下的特殊参数：

[0165]关节 zi θi di
1 (0,0,1)T 任意角度 1
2 (0,1,0)T 0 1
T
3 (0,0,1) 任意角度 1
4 任取 任意角度 1
5 任取 任意角度 1
6 任取 任意角度 1

[0166] 表2 6R机器人的一种奇异位形参数

[0167] 表2中，zi分别表示机构各关节的旋转轴，θi表示机构各关节的旋转角度，[0168] di表示连杆长度。

[0169]

[0170] 其中， 表示旋量的原部， 表示旋量的偶部，i＝1,2,3,4,5,6，。旋量可以通过如下的方式确定。

[0171]

[0172]

[0173] 确定该机构的速度雅克比具有如下的形式：

[0174]

[0175] 在HOL4中构造形如上式的6×6的矩阵函数，调用HOL4库有关行列式的函数，构造如下的命题：

[0176]

[0177] 其中，Jacobian函数的输入值为各关节的旋量，返回值为6×6的矩阵。6R机器人初始状态具有如下的定义：

[0178] 定义5 6R机器人初始状态

[0179]val 6r_initial_state＝(A,B,C,D,E,F)

[0180] 其中，变量A,B,C,D,E,F为旋量，其原部分别表示各旋转副在初始状态下的旋转轴(3维矢量)，其偶部分别表示各旋转副在初始状态下的位置矢量。

[0181] 显然，机构的速度雅克比的行列式值为零，goal5的返回值为false,即该机构处于奇异位形。

[0182] 最后应说明的是：显然，上述实施例仅仅是为清楚地说明本发明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明的保护范围之中。