CN113378349B - S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 - Google Patents

Abstract

S‑R‑S结构七自由度机械臂逆运动学解析解的数值稳定算法，包括：(1)正运动学求解、(2)参考平面求解、(3)逆运动学求解、(4)奇异臂角获取、(5)关节限位及奇异值规避。本发明的数值稳定算法的核心部分为针对前人方法中的直接阈值比较的数值稳定问题，提出后续数值稳定的奇异值规避和关节限位规避的方法，具有以下优异的技术效果：1)可求得S‑R‑S结构机械臂的逆运动学解析解，并实现对关节限位及奇异位姿的规避；2)可求得满足关节限位及奇异值规避的可行冗余角ψ的全部范围；3)求得解析解过程中保障了数值的稳定性。

Description

S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法

技术领域

本发明涉及冗余机械臂逆运动学求解领域，尤其涉及一种S-R-S结构的七自由度机械臂逆运动学解析 解的数值稳定算法。

背景技术

七自由度串联机器人受到机器人界的广泛关注。与六自由度串联机器人相比，冗余的自由度使机器人 更加灵活，可以完成避障和避免关节极限及奇异点等问题。七自由度机械臂广泛采用球形-旋转-球形(S-R- S)机构。S-R-S机械臂由7个串联的旋转关节组成，其中前三个关节的轴相交于一个公共点，形成一个3自 由度的肩部(球形)，第四个关节用作肘关节(旋转)，最后三个关节的轴相交于一点形成一个3自由度的 腕部(球形)。这类机械臂也称为七自由度非偏置机械臂，又因结构模仿人类的上臂(肩肘腕)而被称为 仿人臂机械臂。

在机器人控制中，通常在任务空间(笛卡尔空间)给出机器人的目标六自由度位姿(位置和方向)或 轨迹，通过逆运动学将位姿映射到关节空间，求得相应关节角度进行控制。对于冗余机械臂，由于关节空 间自由度大于笛卡尔空间位姿(六自由度)的自由度，理论上存在无穷组关节解，因此其逆运动学求解是 一个不确定解的问题。

目前，逆运动学求解方法共分为两类：基于位置的方法和基于速度的方法。基于速度的方法通过求解 关节速度并积分求得关节位置控制机械臂，此方法需预先给定任务空间中机械臂末端执行器的预定轨迹， 通过对轨迹求导求得笛卡尔空间速度，并通过求解雅克比矩阵的广义逆求解关节速度。然而，基于速度的 方法存在计算量大、奇异点附近数值不稳定、累积误差大和需预先计算轨迹等问题，同时基于速度的方法 是间接通过速度作为媒介来最终求得关节角度的。相反，基于位置的方法则直接求解关节角度。基于位置 的方法又分为解析法和迭代法。迭代法采用数值优化技术，使给定初始姿态的非线性代价函数最小化，计 算过载通常很高，并且受初始猜测的影响，还存在局部极小解和无全局解等缺点。

与迭代法不同，解析法计算实时性高，且可以避开迭代法的诸多缺点。但只有特殊结构的机械臂才能 求得逆运动学解析解，其中S-R-S结构七自由度机械臂就是其中之一。

S-R-S结构七自由度机械臂由于冗余自由度的存在，因此要求得逆运动学解析解需要先定义一个冗余 参数，并将关节极限及奇异值均在冗余参数空间中剔除以实现既保证目标末端位姿，又避免奇异和关节限 位的目的。关于冗余参数的选择主要包括两种：一种为选择其中一个关节角为冗余参数，然后将逆运动学 问题解耦为易于求解的逆位置和逆姿态子问题。然而，冗余关节的选择依赖于复杂的工作空间分析，同时 选择关节作为冗余参数无法获得完整直观的自运动流形。除冗余关节外，研究人员还提出了选用臂角ψ作为冗余参数。臂角ψ的定义为实际机器人手臂平面(肩肘腕平面)与参考平面之间的角度，通过选用臂角 作为冗余参数直观且唯一地解决了冗余问题。一种改进办法是首先将关节空间映射到臂角空间，以避免关 节极限。另一改进方案为将全局构型控制添加到七自由度S-R-S机械臂的解析解中。还有一种改进方案提出 了基于位置的逆运动学算法，用于求解可规避奇异值和关节极限的问题。

然而，在现有的基于位置的方法中，判断奇异值存在与否均是通过在正切关节中给定特定公差阈值并 比较Δ与0是否相等来判断的。在发明内容部分我们将说明由于阈值的选取与目标位姿相关，因此选则特 定的阈值将导致数值问题，引起判断错误。

发明内容

针对上述问题，本发明提出了一种数值稳定的S-R-S结构七自由度机械臂逆运动学解析解算法。本发 明通过将正切关节的奇异关节角与余弦关节的零角相关联，而非对正切关节使用独立的公差阈值判断的方 法来克服前人方法中的数值问题。本发明还给出了实现七自由度S-R-S机械臂全局构型控制和规避关节极 限及奇异值的实时数值稳定逆运动学解析算法。输入任意的可达位姿及关节限位，本发明的算法将相应地 给出表征自运动流形的所有可行臂角范围及特定姿态参数下的满足关节限位要求的关节角度。

本发明的实施例提供一种S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法，所述数值稳定 算法包括：(1)正运动学求解、(2)参考平面求解、(3)逆运动学求解、(4)奇异臂角获取、(5)关 节限位及奇异值规避。本发明的数值稳定算法的核心部分为针对前人方法中的直接阈值比较的数值稳定问 题，提出后续数值稳定的奇异值规避和关节限位规避的方法。

本发明具有以下优异的技术效果：

1)可求得S-R-S结构机械臂的逆运动学解析解，并实现对关节限位及奇异位姿的规避；

2)可求得满足关节限位及奇异值规避的可行冗余角ψ的全部范围；

3)求得解析解过程中保障了数值的稳定性。

附图说明

图1是KUKA Med 7 R800的结构简图、固连坐标系及修正DH参数。

图2是S-R-S结构简图及臂角定义。

图3是参考平面求解示意图。

图4是正切关节角的ψ-θ_i图。

图5是使用阈值判断奇异臂角的数值不稳定性示意图。

图6是余弦关节的ψ-θ_i图，(a)不存在奇异臂角，(b)存在奇异臂角。

图7是Δ<0的ψ-θ_i图及关节限位标识，(a)无翻转，(b)存在翻转。

图8是Δ>0的ψ-θ_i图及关节限位标识，(a)无翻转，(b)存在翻转。

图9是Δ＝0的ψ-θ_i图及关节限位标识，(a)“N”型，(b)“H”型。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进 一步详细说明。但本领域技术人员知晓，本发明并不局限于附图和以下实施例。

在发明的描述中，需要说明的是，对于方位词，如术语“长度”、“宽度”、“上”、“下”、“远”、“近”等所 指示的方位或位置关系是基于附图所示的方位或位置关系，仅是为了便于叙述本发明和简化描述，而不是 指示或暗示所指的装置或元件必须具有特定的方位、以特定方位构造和操作，不能理解为限制本发明的具 体保护范围。此外，术语“第一”、“第二”仅用于描述目的，用以区别技术特征，不具有实质含义，不能理 解为指示或暗示相对重要性或隐含指明技术特征的数量。

本发明实施例提供的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法包括：(1)正运动学 求解、(2)参考平面求解、(3)逆运动学求解、(4)奇异臂角获取、(5)关节限位及奇异值规避五个 步骤。其中核心部分为针对前人方法中的直接阈值比较的数值稳定问题，提出后续数值稳定的奇异值规避 和关节限位规避的方法。

以下内容将举例具体说明每一步骤。

(1)正运动学求解

机械臂的正运动学求解问题即给定关节角求解末端姿态和位置的问题。

如附图1所示，以KUKA Med 7 R800为例，对于SRS结构的机械臂，本发明的实施例采用修正DH参数 法对其进行正运动学求解。求解方法如下：

坐标系i相对于坐标系i-1(i∈[1,7])的转换矩阵为：

其中四个参数θ_i,a_i-1,α_i-1,d_i分别在图1的DH参数表中列出，通过坐标系的连乘规则：^i-1T_i+1＝^i-1T_i·ⁱT_i+1

即可得出机械臂任意坐标间的转换矩阵，解决正运动学求解问题。

其中，符号标识说明如下：

本文使用

的形式来表示从A到B的向量，此向量不相对于任何参考系。ⁱP_AB表示在坐标系{i}中

向量的表示。pⁱ表示向量P的第i个分量，Mⁱ表示矩阵M的第i个列向量，M^ij表示矩阵M第i行j列的元素； x_i,y_i,z_i分别表示坐标系{i}的x、y、z单位轴。

表示向量p的单位向量；<p₁,p₂>和p₁×p₂分别为两个向 量p₁和p₂的点积和叉积。向量p的斜对称矩阵用[p]表示，其定义为

(2)参考平面求解

机械臂逆运动学问题即为给定末端姿态和位置反求关节角度的问题。

由于S-R-S结构为七自由度冗余结构，而笛卡尔空间中给定末端姿态和位置仅需6个参数即3个位置参 数和3个姿态参数。因此为了唯一确定关节角度，求得运动学逆解，需要人为增加1个冗余参数结合末端位 置和姿态6个参数共同确定关节角度。本实施例选用的冗余参数为相对参考平面的臂角ψ。具体说明如下：

如附图2所示，其中平面SE′W即为参考平面，冗余参数臂角ψ为参考平面SE′W沿向量

旋转至实 际关节平面SEW所需旋转的角度。其中本实施例中参考平面SE′W选择关节角θ₃＝0时的关节平面为参 考平面。

对于七自由度冗余结构，当设定第三个关节角θ₃＝0时即变为6自由度机械臂，而确定的末端位置和姿 态则提供了6个参数，6个位置姿态参数求解6个关节角变量，便是可解的。如附图1所示的S-R-S结构机械 臂，最后三个关节即5,6,7关节轴线相交于一点，即W点。由附图2可以看出，当关节θ₁～θ₄角度确定后，参 考平面SE′W即可确定。具体求解过程如下：

⁰p_BS＝(0,0,d₁)^T

⁰p_SW＝⁰t₇-⁰p_BS-⁰R₇ ⁷p_WF

如附图3所示，θ₄'可以通过余弦定理求得：

其中GC₄为姿态参数，表明θ₄'的正负，其取值范围为{-1,1}。由图3可知δ的正负号与θ₄'相同，因此：

θ₂'的正方向与δ相反：

θ₁'由几何关系可得：

其中ε是一个小阈值，可以取10^-12且

判断

或

是为 了判断

是否与z₀对齐，即W点是否位于1关节轴线上，因为这种姿态为一个奇异位置，θ₁'无法被唯一 确定。由于只需求得一个参考平面SE′W，因此在这种情况下，统一设定θ₁'＝0。

至此，求得θ₁′,θ₂′,θ₃′＝0,θ₄′，即可得参考平面SE′W。

(3)逆运动学求解

当给定参数ψ，即可进行逆运动学求解。其中θ₄与ψ无关，仅由末端位置和姿态确定，即θ₄＝θ₄'。 真实⁰R₄即为参考平面⁰R₄'沿

旋转ψ角求得。

⁰R₄＝⁰R_ψ ⁰R₄′

θ₄＝θ₄′

³R₄＝³R₄′

⁰R₃ ³R₄＝⁰R_ψ ⁰R₃′³R₄′

⁰R₃＝⁰R_ψ ⁰R₃′

通过以上推导得到真实⁰R₃即为参考平面⁰R₃'沿

旋转ψ角求得，其中⁰R_ψ为相对向量

旋转ψ 角形成的旋转矩阵，由罗德里格斯公式得到：

其中I为3×3单位矩阵，因此⁰R₃可以表示为：

⁰R₃(ψ)＝sinψA_s+cosψB_s+C_s

其中：

同时，由于1-3关节为ZYZ球关节角，通过正运动学求得的公式为：

由⁰R₃(θ₁,θ₂,θ₃)＝⁰R₃(ψ)可得：

当θ₂≠0或±π时

其中GC₂同GC₄，取值范围为{-1,1}，表明θ₂的正负，且：

当θ₂＝0或±π时

因此对应可求出θ₁～θ₃，θ₄＝θ₄'，之后便求解θ₅～θ₇。

同理5-7关节也为ZYZ球关节角，因此同样可对应求出θ₅～θ₇，即将上式完成如下替换即可：

θ₁←θ₅,θ₂←θ₆,θ₃←θ₇,s←w,GC₂←GC₆

其中：

GC₂、GC₄和GC₆均为姿态参数，取值为{-1,1}，分别表明θ₂、θ₄和θ₆的正负，臂角ψ为自运动冗余 参数。至此完成逆运动学求解过程。

*臂角与关节角映射的数值稳定问题

如前所述，θ₄独立于臂角ψ，仅由目标姿态和GC₄确定。除θ₄外，可以发现θ₂和θ₆的臂角与关节 角映射表达具有余弦形式，θ₁,θ₃,θ₅,θ₇具有正切形式。而前人在处理臂角与关节角映射问题时，都是单独 处理余弦关节和正切关节，而忽略了这两类关节奇异臂角之间的联系。他们选用一个阈值来描述每个关节 的奇异臂角，这将导致一定的数值问题，从而使臂角与关节角映射错误。下面本发明将展示此数值问题， 并于下一部分解决这些问题。

由前面推导可得，正切形式的关节角表达为θ_i＝atan2(u,v)，其中：

对于i∈{1,3}时k＝2,i∈{5,7}时k＝6。θ_i相对ψ的求导公式为：

其中：

a_t＝c_nb_d-b_nc_d

b_t＝a_nc_d-c_na_d

c_t＝a_nb_d-b_na_d

Qu²+v²≥0∴令a_tsinψ+b_tcosψ+c_t＝0即可求得驻点公式：

令

则存在如附图4所示的三种情况。

如图4所示，

(a)Δ＜0时，无驻点，ψ-θ_i图单调；

(b)Δ＞0时，存在两个驻点，ψ-θ_i图有两个极值；

(c)Δ＝0时，此时u和v均等于0，存在奇异点。此时ψ-θ_i图在此奇异臂角位置上存在大小为π的 跳变，其中奇异臂角可由Δ＝0求得：

由于atan2函数值域为[-π,π]，因此便产生了图4中的翻转现象。

对于Δ＝0的情况，前人的处理方法为在冗余臂角空间中去除Δ＝0相对应的ψ_s。然而实际上，由于 浮点数固有的数值误差，不能简单地将Δ与0进行比较来判断是否存在奇异臂角，而是设置一个小的阈值 t(例如t＝10^-12)并让条件|Δ|<t触发奇异情况。但是，这种策略数值不稳定，因为t在不同的机械臂姿态下 的选择是不同的。如图5(a)和(b)显示了给定两个不同目标位姿的关节1的ψ-θ_i图。由于图5(a)中 的Δ比图5(b)中的Δ还要小，因此便无法找到一个合适的阈值t来区分这两种情况。而奇异臂角的错误 判定将导致臂角映射的错误分类，从而导致关节极限规避的失败。

除此之外，奇异臂角表达式

在数值上也不稳定。当a_t和b_t-c_t都接近0时，将会 产生错误解。如图5(c)所示，从图中可以看出奇异臂角约为±π，然而通过

求得的ψ_s约为0，计算求得值与实际模拟所得值不符，尤其当奇异臂角接近±π时，常会出现此类数值问题。

因此，使用单个阈值来判断奇异臂角存在数值问题。下面步骤将介绍本方法提出的数值稳定的逆运动 学解析解算法。

(4)奇异臂角获取

如前一节所说，通过Δ＝0来判断奇异臂角的方法存在数值问题，在此，本发明提出通过考虑整个“球 关节”而不是单个关节来表征奇异臂角。对于S-R-S机械臂，关节1、2、3组成一个名为“肩”的球关节； 关节5、6、7组成一个名为“腕”的球关节。由于肩关节和腕关节在本质上是相同的，本发明将它们统称为 球关节。球关节有三个Z-Y-Z欧拉角变量θ₁、θ₂、θ₃，其中θ₁和θ₃为正切关节，θ₂为余弦关节。由ZYZ 欧拉角形成的旋转矩阵⁰R₃(θ₁,θ₂,θ₃)可知，当且仅当θ₂＝0或±π时，θ₁和θ₃是无法唯一确定的，且此时 对应表达式中u＝v＝0。反之，当位于奇异臂角ψ_s时，正切关节的u＝v＝0，由于⁰R₃(θ₁,θ₂,θ₃)为正 交矩阵，即可证明此时cos(θ₂)＝±1即θ₂＝0或±π。因此θ₁和θ₃的奇异臂角ψ_s即为θ₂＝0或±π时对应 的臂角。这也表明奇异臂角不是单个关节的特殊臂角，而是综合球关节的特殊臂角。因此得出结论，奇异 臂角是机械臂局部自运动流形中的奇异位置，在球关节θ₂＝0或±π时出现。

描述球关节的奇异臂角的关键是找到使θ_i＝0或±π的ψ_s(肩部i＝2，腕部i＝6)，此臂角ψ_s同时也是 与其相关的两个正切关节的奇异臂角。

余弦关节表达式为：θ_i＝GC_iarccos(asinψ+bcosψ+c)

θ_i相对ψ求导公式为：

由表达式可知θ_i＝0或±π对应于asin(ψ)+bcos(ψ)+c＝±1，而asin(ψ)+bcos(ψ)+c的 值均位于±1间。即使得θ_i＝0或±π的ψ_s一定为asin(ψ)+bcos(ψ)+c的极值。因此，通过使其导数 为零可得两个ψ_s值：

容易判断求得

和

对应于θ_i的最大值或最小值。为了检验是否存在奇异臂角ψ_s，只需将

i∈{1,2}与1进行比较。本发明提出了一种数值稳定的确定ψ_s的方法：

注意当ψ→ψ_s时sinθ_i→0，此时

导数在ψ_s处不是连续的。图6为余弦关节的典型ψ-θ_i图。 实现对球关节的奇异臂角ψ_s的表达后就可以对其相应的正切关节进行正确分类，并进行关节极限映射， 这一部分将在下一节中描述。算法1给出了求得球关节奇异臂角ψ_s的详细算法：

(5)关节限位及奇异值规避

因为S-R-S结构七自由度机械臂为冗余机械臂，因此可以实现对各关节限位的规避，实现方案为：将每 个关节i的极限范围映射到冗余臂角ψ的范围内，求取各关节可行臂角Ψ_i的交集即可求得最终冗余臂角范 围。因为在步骤(3)中已经得到θ_i关于臂角ψ的表达式，因此求取各关节可行臂角范围的关键是求关节 限位线与ψ-θ_i图的交点，下面将按照关节分类进行阐述。在这里用

和

来表示第i个关节的上下关 节角极限。其中θ₄与ψ无关，其关节限位规避可直接由末端位姿求得θ₄后与关节限位对比判断是否超 限。

5.1余弦关节

余弦关节包括第2和6关节，其中图6是余弦关节的ψ-θ_i示意图。根据实际情况此图可能会发生垂直 翻转或上下/左右平移的情况。给定θ_i，可通过万能公式求得两个对应的ψ(如果存在)，ψ的计算公 式为：

由前文计算奇异解的分析可知，

和

对应于θ_i的最大值或最小值，记为

和

下一步， 本发明将分别为关节下限

和上限

找到对应的可行范围Ψ_il和Ψ_iu，最终得到此关节避免关节限位后 对应的可行臂角范围：Ψ_i＝Ψ_il∩Ψ_iu。其基本思想是利用上式(2)求出极限关节角对应的ψ₁和ψ₂， 并通过ψ₁和ψ₂点的导数

的正负判断ψ₁、ψ₂是可行区间的入口点还是离开点。例如，结合图6可 知，若

且ψ₁为关节上限

通过式(2)所求得，则1为离开点。算法2给出了余弦关节极限映 射的详细算法。

5.2正切关节

正切关节的情况相对复杂，前文图4是正切关节的基本ψ-θ_i图。正切关节极限映射可分为Δ<0、Δ >0和Δ＝0，三种情况进行讨论。如前文所述，由于atan2函数值域为[-π,π]，因此图4中存在有±π间翻 转现象，这使得正切关节的ψ与θ_i对应及关节极限规避更为复杂。

给定θ_i，可通过万能公式求得两个对应的ψ(如果存在)，ψ的计算公式为：

其中

a_p＝(c_d-b_d)tanθ_i+b_n-c_n

b_p＝2(a_dtanθ_i-a_n)

c_p＝(b_d+c_d)tanθ_i-b_n-c_n

至此，已能够使用前文算法1正确识别奇异臂角情况。接下来将根据这三种情况来解决正切关节的关 节极限映射问题。

(I)Δ<0，为单调

如附图7所示，Δ<0时ψ-θ_i图除±π间翻转外整体保持单调。即与

和

均各有一个 交点(翻转现象是由于atan2的值域为[-π,π]导致的，虽然图(b)中看起来关节角极限与翻转线有交点， 但实际根本取不到)，这意味着根据式(3)计算的只有一个解是正确的。判断具体为哪个解只需将计算结 果反代回θ关于ψ的公式验证即可。求解时当a_p＝0时，解由

给出。设ψ_l和ψ_u分别为与下极限

和上极限

的交点。设ψ₁＝min(ψ_l,ψ_u)，ψ₂＝max(ψ_l,ψ_u)，则由图7可知，关 节极限内的可选臂角范围为Ψ_i＝[ψ₁,ψ₂]或者Ψ_i＝[-π,ψ₁]∪[ψ₂,π]。具体为哪个范围可通过计算 ψ₁是进入点还是离开点判断，也可通过观察偏转是否发生在ψ₁和ψ₂之间判断。偏转是否发生可通过计 算

是否在极限关节角范围内进行判断。算法3给出了详细的解法。

(II)Δ>0

如图8所示，ψ-θ_i图有两个局部极值，可通过式(1)求得，并通过二阶导的正负判断出局部极小值 和极大值，并分别表示为

和

则设定

之后通过求解关节极限 与ψ-θ_i图的交点，可得关节极限

和

对应的可选臂角区域

和

以

为例，如果极限超出 了由

和

组成的区域，则选臂角范围

或

否则， 则用

和

表示

与ψ-θ_i的两个交点(并设定

)，则关节下极限

对应的可选臂角范围

通过判断

处导数的符号，判断为进入点或离开点，确定臂角范围

或

关节上极限对应臂角范围

使用相同的规则获得。最后，Ψ_i由

还 是

求得取决于是否存在翻转及

的相对位置。算法4给出了详细的计算方 法。

(III)Δ＝0

如前所述，当Δ＝0时存在奇异臂角。如图9所示，ψ-θ_i图在使用算法1获得的奇异臂角 ψ_s处具有π的跳变(图中标为突变)。请注意，此奇异臂角的求得是通过与此正切关节相关 的余弦关节求得的。如图9(a)和(b)所示为ψ-θ_i图的两种形式，其中突变的正负与突变 前后的导数正负相反时，我们称之为“N”型(图8(a))。否则，我们称之为“H”型(图8(b))。在求解时设定一个阈值tol(例如取tol＝0.05rad)，令

和

为了避免奇异，在臂角范围内排除范围

此时考虑如果ψ_s接近±π，则

或

可能从 [-π,π]中脱落，所以最终得到的奇异臂角范围

在实际计算可行臂角范围时应避开此奇异臂角范围。

设

且

如果

则轮廓为“N”型，否则为“H”型。 之后计算思路与第二种Δ>0情况相似，即求得关节极限与图的交点，并为上下极限各求得一 个可行区域，最终的可行区域按照判断选择两个极限线区域的交集或并集。算法5给出了详细 的计算方法。

5.3整体算法

至止求得了各关节的可行臂角范围Ψ_i，则最终避免关节限位及奇异值的臂角范围为

对于任一末端位姿，于其姿态对应的可行臂角范围内任意选出一 冗余臂角即可进行数值稳定的逆运动学求解，实现S-R-S结构的七自由度机械臂逆运动学解析 解的获得。

Claims (3)

1.一种S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法，其特征在于，所述数值稳定算法包括：(1)正运动学求解、(2)参考平面求解、(3)逆运动学求解、(4)奇异臂角获取、(5)关节限位及奇异值规避；

所述(3)逆运动学求解包括：

当给定参数ψ，即可进行逆运动学求解；其中θ₄与ψ无关，仅由末端位置和姿态确定，即θ₄＝θ₄'；真实⁰R₄即为参考平面⁰R₄'沿

旋转ψ角求得，

⁰R₄＝⁰R_ψ ⁰R′₄

θ₄＝θ′₄

³R₄＝³R′₄

⁰R₃ ³R₄＝⁰R_ψ ⁰R′₃ ³R′₄

⁰R₃＝⁰R_ψ ⁰R′₃；

通过以上推导得到真实⁰R₃即为参考平面⁰R₃'沿

旋转ψ角求得，其中⁰R_ψ为相对向量

旋转ψ角形成的旋转矩阵，由罗德里格斯公式得到：

其中I为3×3单位矩阵，因此⁰R₃可以表示为：

⁰R₃(ψ)＝sinψA_s+cosψB_s+C_s

其中：

同时，由于1-3关节为ZYZ球关节角，通过正运动学求得的公式为：

由⁰R₃(θ₁,θ₂,θ₃)＝⁰R₃(ψ)可得：

当θ₂≠0或±π时

其中GC₂同GC₄，取值范围为{-1,1}，表明θ₂的正负，且：

当θ₂＝0或±π时

因此对应可求出θ₁～θ₃，θ₄＝θ₄'，之后便求解θ₅～θ₇；

同理，5-7关节也为ZYZ球关节角，因此同样可对应求出θ₅～θ₇，即将上式完成如下替换即可：

θ₁←θ₅,θ₂←θ₆,θ₃←θ₇,s←w,GC₂←GC₆

其中：

GC₂、GC₄和GC₆均为姿态参数，取值为{-1,1}，分别表明θ₂、θ₄和θ₆的正负，臂角ψ为自运动冗余参数，至此完成逆运动学求解过程；

所述(4)奇异臂角获取包括：

奇异臂角是机械臂局部自运动流形中的奇异位置，在球关节θ₂＝0或±π时出现；

描述球关节的奇异臂角的关键是找到使θ_i＝0或±π的ψ_s，肩部i＝2，腕部i＝6，此臂角ψ_s同时也是与其相关的两个正切关节的奇异臂角；

余弦关节表达式为：

θ_i＝GC_iarccos(a sinψ+b cosψ+c)

θ_i相对ψ求导公式为：

由表达式可知θ_i＝0或±π对应于a sin(ψ)+b cos(ψ)+c＝±1，而a sin(ψ)+b cos(ψ)+c的值均位于±1间；即使得θ_i＝0或±π的ψ_s一定为a sin(ψ)+b cos(ψ)+c的极值；因此，通过使其导数为零可得两个ψ_s值：

容易判断求得

和

对应于θ_i的最大值或最小值；为了检验是否存在奇异臂角ψ_s，只需将

i∈{1,2}与1进行比较；采用一种数值稳定的确定ψ_s的方法：

所述(5)关节限位及奇异值规避包括：

将每个关节i的极限范围映射到冗余臂角ψ的范围内，求取各关节可行臂角Ψ_i的交集即可求得最终冗余臂角范围；用

和

来表示第i个关节的上下关节角极限；其中θ₄与ψ无关，其关节限位规避可直接由末端位姿求得θ₄后与关节限位对比判断是否超限；

5.1余弦关节

余弦关节包括第2和6关节，给定θ_i，通过万能公式求得两个对应的ψ，ψ的计算公式为：

和

对应于θ_i的最大值或最小值，记为θ_i ^max和θ_i ^min；下一步将分别为关节下限

和上限

找到对应的可行范围Ψ_il和Ψ_iu，最终得到此关节避免关节限位后对应的可行臂角范围：Ψ_i＝Ψ_il∩Ψ_iu；其基本思想是利用上式(2)求出极限关节角对应的ψ₁和ψ₂，并通过ψ₁和ψ₂点的导数

的正负判断ψ₁、ψ₂是可行区间的入口点还是离开点；若

且ψ₁为关节上限

通过式(2)所求得，则1为离开点；5.2正切关节

给定θ_i，通过万能公式求得两个对应的ψ，ψ的计算公式为：

其中

a_p＝(c_d-b_d)tanθ_i+b_n-c_n

b_p＝2(a_dtanθ_i-a_n)

c_p＝(b_d+c_d)tanθ_i-b_n-c_n

接下来将根据这三种情况来解决正切关节的关节极限映射问题；

(I)Δ<0，为单调

Δ<0时ψ-θ_i图除±π间翻转外整体保持单调；即与

和

均各有一个交点，这意味着根据式(3)计算的只有一个解是正确的；判断具体为哪个解只需将计算结果反代回θ关于ψ的公式验证即可；求解时当a_p＝0时，解由

给出；设ψ_l和ψ_u分别为与下极限

和上极限

的交点；设ψ₁＝min(ψ_l,ψ_u)，ψ₂＝max(ψ_l,ψ_u)，关节极限内的可选臂角范围为Ψ_i＝[ψ₁,ψ₂]或者Ψ_i＝[-π,ψ₁]∪[ψ₂,π]；具体为哪个范围可通过计算ψ₁是进入点还是离开点判断，或者通过观察偏转是否发生在ψ₁和ψ₂之间判断；偏转是否发生可通过计算

是否在极限关节角范围内进行判断；(II)Δ>0

ψ-θ_i图有两个局部极值，可通过式(1)求得，并通过二阶导的正负判断出局部极小值和极大值，并分别表示为

和

则设定

之后通过求解关节极限与ψ-θ_i图的交点，可得关节极限

和

对应的可选臂角区域

和

对于

如果极限超出了由

和

组成的区域，则选臂角范围

或

否则，则用

和

表示

与ψ-θ_i的两个交点，并设定

则关节下极限

对应的可选臂角范围

通过判断

处导数的符号，判断为进入点或离开点，确定臂角范围

或

关节上极限对应臂角范围

使用相同的规则获得；最后，Ψ_i由

还是

求得取决于是否存在翻转及

θ_i ^min、θ_i ^max的相对位置；

(III)Δ＝0

如前所述，当Δ＝0时存在奇异臂角；在求解时设定一个阈值tol，令

和

为了避免奇异，在臂角范围内排除范围

此时考虑如果ψ_s接近±π，则

或

可能从[-π,π]中脱落，所以最终得到的奇异臂角范围

在实际计算可行臂角范围时应避开此奇异臂角范围；

设

且

如果

则轮廓为“N”型，否则为“H”型；之后计算思路与第二种Δ>0情况相似，即求得关节极限与图的交点，并为上下极限各求得一个可行区域，最终的可行区域按照判断选择两个极限线区域的交集或并集；

5.3整体算法

至止求得了各关节的可行臂角范围Ψ_i，则最终避免关节限位及奇异值的臂角范围为

对于任一末端位姿，于其姿态对应的可行臂角范围内任意选出一冗余臂角即可进行数值稳定的逆运动学求解，实现S-R-S结构七自由度机械臂逆运动学解析解的获得。

2.根据权利要求1所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法，其特征在于，所述(1)正运动学求解包括：

机械臂的正运动学求解问题即给定关节角求解末端姿态和位置的问题，对于SRS结构的机械臂，采用修正DH参数法对其进行正运动学求解，求解方法如下：

坐标系i相对于坐标系i-1，i∈[1,7]，的转换矩阵为：

其中，通过坐标系的连乘规则：

^i-1T_i+1＝^i-1T_i·ⁱT_i+1

即可得出机械臂任意坐标间的转换矩阵，解决正运动学求解问题。

3.根据权利要求1或2所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法，其特征在于，所述(2)参考平面求解包括：

本算法选用的冗余参数为相对参考平面的臂角ψ，具体说明如下：

其中平面SE′W即为参考平面，冗余参数臂角ψ为参考平面SE′W沿向量

旋转至实际关节平面SEW所需旋转的角度，其中本算法参考平面SE′W选择关节角θ₃＝0时的关节平面为参考平面；

对于七自由度冗余结构，当设定第三个关节角θ₃＝0时即变为6自由度机械臂，而确定的末端位置和姿态则提供了6个参数，6个位置姿态参数求解6个关节角变量，便是可解的；求解过程如下：

θ₄'可以通过余弦定理求得：

其中GC₄为姿态参数，表明θ₄'的正负，其取值范围为{-1,1}；δ的正负号与θ₄'相同，因此：

θ₂'的正方向与δ相反：

θ₁'由几何关系可得：

其中ε是阈值，取10^-12且

统一设定θ₁'＝0；

至此，求得θ′₁,θ′₂,θ′₃＝0,θ′₄，即可得参考平面SE′W。

Images