CN113378349A - S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 - Google Patents
S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 Download PDFInfo
- Publication number
- CN113378349A CN113378349A CN202110322567.0A CN202110322567A CN113378349A CN 113378349 A CN113378349 A CN 113378349A CN 202110322567 A CN202110322567 A CN 202110322567A CN 113378349 A CN113378349 A CN 113378349A
- Authority
- CN
- China
- Prior art keywords
- joint
- psi
- theta
- angle
- singular
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
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机械臂由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,ai-1,αi-1,di分别在图1的DH参数表中列出,通过坐标系的连乘规则:i-1Ti+1=i-1Ti·iTi+1
即可得出机械臂任意坐标间的转换矩阵,解决正运动学求解问题。
其中,符号标识说明如下:
本文使用的形式来表示从A到B的向量,此向量不相对于任何参考系。iPAB表示在坐标系{i}中向量的表示。pi表示向量P的第i个分量,Mi表示矩阵M的第i个列向量,Mij表示矩阵M第i行j列的元素; xi,yi,zi分别表示坐标系{i}的x、y、z单位轴。表示向量p的单位向量;<p1,p2>和p1×p2分别为两个向 量p1和p2的点积和叉积。向量p的斜对称矩阵用[p]表示,其定义为
(2)参考平面求解
机械臂逆运动学问题即为给定末端姿态和位置反求关节角度的问题。
由于S-R-S结构为七自由度冗余结构,而笛卡尔空间中给定末端姿态和位置仅需6个参数即3个位置参 数和3个姿态参数。因此为了唯一确定关节角度,求得运动学逆解,需要人为增加1个冗余参数结合末端位 置和姿态6个参数共同确定关节角度。本实施例选用的冗余参数为相对参考平面的臂角ψ。具体说明如下:
对于七自由度冗余结构,当设定第三个关节角θ3=0时即变为6自由度机械臂,而确定的末端位置和姿 态则提供了6个参数,6个位置姿态参数求解6个关节角变量,便是可解的。如附图1所示的S-R-S结构机械 臂,最后三个关节即5,6,7关节轴线相交于一点,即W点。由附图2可以看出,当关节θ1~θ4角度确定后,参 考平面SE′W即可确定。具体求解过程如下:
0pBS=(0,0,d1)T
0pSW=0t7-0pBS-0R7 7pWF
如附图3所示,θ4'可以通过余弦定理求得:
其中GC4为姿态参数,表明θ4'的正负,其取值范围为{-1,1}。由图3可知δ的正负号与θ4'相同,因此:
θ2'的正方向与δ相反:
θ1'由几何关系可得:
其中ε是一个小阈值,可以取10-12且判断或是为 了判断是否与z0对齐,即W点是否位于1关节轴线上,因为这种姿态为一个奇异位置,θ1'无法被唯一 确定。由于只需求得一个参考平面SE′W,因此在这种情况下,统一设定θ1'=0。
至此,求得θ1′,θ2′,θ3′=0,θ4′,即可得参考平面SE′W。
(3)逆运动学求解
0R4=0Rψ 0R4′
θ4=θ4′
3R4=3R4′
0R3 3R4=0Rψ 0R3′3R4′
0R3=0Rψ 0R3′
其中I为3×3单位矩阵,因此0R3可以表示为:
0R3(ψ)=sinψAs+cosψBs+Cs
其中:
同时,由于1-3关节为ZYZ球关节角,通过正运动学求得的公式为:
由0R3(θ1,θ2,θ3)=0R3(ψ)可得:
当θ2≠0或±π时
其中GC2同GC4,取值范围为{-1,1},表明θ2的正负,且:
当θ2=0或±π时
因此对应可求出θ1~θ3,θ4=θ4',之后便求解θ5~θ7。
同理5-7关节也为ZYZ球关节角,因此同样可对应求出θ5~θ7,即将上式完成如下替换即可:
θ1←θ5,θ2←θ6,θ3←θ7,s←w,GC2←GC6
其中:
GC2、GC4和GC6均为姿态参数,取值为{-1,1},分别表明θ2、θ4和θ6的正负,臂角ψ为自运动冗余 参数。至此完成逆运动学求解过程。
*臂角与关节角映射的数值稳定问题
如前所述,θ4独立于臂角ψ,仅由目标姿态和GC4确定。除θ4外,可以发现θ2和θ6的臂角与关节 角映射表达具有余弦形式,θ1,θ3,θ5,θ7具有正切形式。而前人在处理臂角与关节角映射问题时,都是单独 处理余弦关节和正切关节,而忽略了这两类关节奇异臂角之间的联系。他们选用一个阈值来描述每个关节 的奇异臂角,这将导致一定的数值问题,从而使臂角与关节角映射错误。下面本发明将展示此数值问题, 并于下一部分解决这些问题。
由前面推导可得,正切形式的关节角表达为θi=atan2(u,v),其中:
对于i∈{1,3}时k=2,i∈{5,7}时k=6。θi相对ψ的求导公式为:
其中:
at=cnbd-bncd
bt=ancd-cnad
ct=anbd-bnad
Qu2+v2≥0∴令atsinψ+btcosψ+ct=0即可求得驻点公式:
如图4所示,
(a)Δ<0时,无驻点,ψ-θi图单调;
(b)Δ>0时,存在两个驻点,ψ-θi图有两个极值;
由于atan2函数值域为[-π,π],因此便产生了图4中的翻转现象。
对于Δ=0的情况,前人的处理方法为在冗余臂角空间中去除Δ=0相对应的ψs。然而实际上,由于 浮点数固有的数值误差,不能简单地将Δ与0进行比较来判断是否存在奇异臂角,而是设置一个小的阈值 t(例如t=10-12)并让条件|Δ|<t触发奇异情况。但是,这种策略数值不稳定,因为t在不同的机械臂姿态下 的选择是不同的。如图5(a)和(b)显示了给定两个不同目标位姿的关节1的ψ-θi图。由于图5(a)中 的Δ比图5(b)中的Δ还要小,因此便无法找到一个合适的阈值t来区分这两种情况。而奇异臂角的错误 判定将导致臂角映射的错误分类,从而导致关节极限规避的失败。
除此之外,奇异臂角表达式在数值上也不稳定。当at和bt-ct都接近0时,将会 产生错误解。如图5(c)所示,从图中可以看出奇异臂角约为±π,然而通过求得的ψs约为0,计算求得值与实际模拟所得值不符,尤其当奇异臂角接近±π时,常会出现此类数值问题。
因此,使用单个阈值来判断奇异臂角存在数值问题。下面步骤将介绍本方法提出的数值稳定的逆运动 学解析解算法。
(4)奇异臂角获取
如前一节所说,通过Δ=0来判断奇异臂角的方法存在数值问题,在此,本发明提出通过考虑整个“球 关节”而不是单个关节来表征奇异臂角。对于S-R-S机械臂,关节1、2、3组成一个名为“肩”的球关节; 关节5、6、7组成一个名为“腕”的球关节。由于肩关节和腕关节在本质上是相同的,本发明将它们统称为 球关节。球关节有三个Z-Y-Z欧拉角变量θ1、θ2、θ3,其中θ1和θ3为正切关节,θ2为余弦关节。由ZYZ 欧拉角形成的旋转矩阵0R3(θ1,θ2,θ3)可知,当且仅当θ2=0或±π时,θ1和θ3是无法唯一确定的,且此时 对应表达式中u=v=0。反之,当位于奇异臂角ψs时,正切关节的u=v=0,由于0R3(θ1,θ2,θ3)为正 交矩阵,即可证明此时cos(θ2)=±1即θ2=0或±π。因此θ1和θ3的奇异臂角ψs即为θ2=0或±π时对应 的臂角。这也表明奇异臂角不是单个关节的特殊臂角,而是综合球关节的特殊臂角。因此得出结论,奇异 臂角是机械臂局部自运动流形中的奇异位置,在球关节θ2=0或±π时出现。
描述球关节的奇异臂角的关键是找到使θi=0或±π的ψs(肩部i=2,腕部i=6),此臂角ψs同时也是 与其相关的两个正切关节的奇异臂角。
余弦关节表达式为:θi=GCiarccos(asinψ+bcosψ+c)
由表达式可知θi=0或±π对应于asin(ψ)+bcos(ψ)+c=±1,而asin(ψ)+bcos(ψ)+c的 值均位于±1间。即使得θi=0或±π的ψs一定为asin(ψ)+bcos(ψ)+c的极值。因此,通过使其导数 为零可得两个ψs值:
注意当ψ→ψs时sinθi→0,此时导数在ψs处不是连续的。图6为余弦关节的典型ψ-θi图。 实现对球关节的奇异臂角ψs的表达后就可以对其相应的正切关节进行正确分类,并进行关节极限映射, 这一部分将在下一节中描述。算法1给出了求得球关节奇异臂角ψs的详细算法:
(5)关节限位及奇异值规避
因为S-R-S结构七自由度机械臂为冗余机械臂,因此可以实现对各关节限位的规避,实现方案为:将每 个关节i的极限范围映射到冗余臂角ψ的范围内,求取各关节可行臂角Ψi的交集即可求得最终冗余臂角范 围。因为在步骤(3)中已经得到θi关于臂角ψ的表达式,因此求取各关节可行臂角范围的关键是求关节 限位线与ψ-θi图的交点,下面将按照关节分类进行阐述。在这里用和来表示第i个关节的上下关 节角极限。其中θ4与ψ无关,其关节限位规避可直接由末端位姿求得θ4后与关节限位对比判断是否超 限。
5.1余弦关节
余弦关节包括第2和6关节,其中图6是余弦关节的ψ-θi示意图。根据实际情况此图可能会发生垂直 翻转或上下/左右平移的情况。给定θi,可通过万能公式求得两个对应的ψ(如果存在),ψ的计算公 式为:
由前文计算奇异解的分析可知,和对应于θi的最大值或最小值,记为和下一步, 本发明将分别为关节下限和上限找到对应的可行范围Ψil和Ψiu,最终得到此关节避免关节限位后 对应的可行臂角范围:Ψi=Ψil∩Ψiu。其基本思想是利用上式(2)求出极限关节角对应的ψ1和ψ2, 并通过ψ1和ψ2点的导数的正负判断ψ1、ψ2是可行区间的入口点还是离开点。例如,结合图6可 知,若且ψ1为关节上限通过式(2)所求得,则1为离开点。算法2给出了余弦关节极限映 射的详细算法。
5.2正切关节
正切关节的情况相对复杂,前文图4是正切关节的基本ψ-θi图。正切关节极限映射可分为Δ<0、Δ >0和Δ=0,三种情况进行讨论。如前文所述,由于atan2函数值域为[-π,π],因此图4中存在有±π间翻 转现象,这使得正切关节的ψ与θi对应及关节极限规避更为复杂。
给定θi,可通过万能公式求得两个对应的ψ(如果存在),ψ的计算公式为:
其中
ap=(cd-bd)tanθi+bn-cn
bp=2(adtanθi-an)
cp=(bd+cd)tanθi-bn-cn
至此,已能够使用前文算法1正确识别奇异臂角情况。接下来将根据这三种情况来解决正切关节的关 节极限映射问题。
(I)Δ<0,为单调
如附图7所示,Δ<0时ψ-θi图除±π间翻转外整体保持单调。即与和均各有一个 交点(翻转现象是由于atan2的值域为[-π,π]导致的,虽然图(b)中看起来关节角极限与翻转线有交点, 但实际根本取不到),这意味着根据式(3)计算的只有一个解是正确的。判断具体为哪个解只需将计算结 果反代回θ关于ψ的公式验证即可。求解时当ap=0时,解由给出。设ψl和ψu分别为与下极限和上极限的交点。设ψ1=min(ψl,ψu),ψ2=max(ψl,ψu),则由图7可知,关 节极限内的可选臂角范围为Ψi=[ψ1,ψ2]或者Ψi=[-π,ψ1]∪[ψ2,π]。具体为哪个范围可通过计算 ψ1是进入点还是离开点判断,也可通过观察偏转是否发生在ψ1和ψ2之间判断。偏转是否发生可通过计 算是否在极限关节角范围内进行判断。算法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整体算法
Claims (6)
1.一种S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法,其特征在于,所述数值稳定算法包括:(1)正运动学求解、(2)参考平面求解、(3)逆运动学求解、(4)奇异臂角获取、(5)关节限位及奇异值规避。
2.根据权利要求1所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法,其特征在于,所述(1)正运动学求解包括:
机械臂的正运动学求解问题即给定关节角求解末端姿态和位置的问题,对于SRS结构的机械臂,采用修正DH参数法对其进行正运动学求解,求解方法如下:
坐标系i相对于坐标系i-1(i∈[1,7])的转换矩阵为:
其中四个参数θi,ai-1,αi-1,di分别在图1的DH参数表中列出,通过坐标系的连乘规则:
i-1Ti+1=i-1Ti·iTi+1
即可得出机械臂任意坐标间的转换矩阵,解决正运动学求解问题;
其中,符号标识说明如下:
使用的形式来表示从A到B的向量,此向量不相对于任何参考系;iPAB表示在坐标系{i}中向量的表示;pi表示向量P的第i个分量,Mi表示矩阵M的第i个列向量,Mij表示矩阵M第i行j列的元素;xi,yi,zi分别表示坐标系{i}的x、y、z单位轴。表示向量p的单位向量;<p1,p2>和p1×p2分别为两个向量p1和p2的点积和叉积。向量p的斜对称矩阵用[p]表示,其定义为
3.根据权利要求1或2所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法,其特征在于,所述(2)参考平面求解包括:
机械臂逆运动学问题即为给定末端姿态和位置反求关节角度的问题,由于S-R-S结构为七自由度冗余结构,而笛卡尔空间中给定末端姿态和位置仅需6个参数即3个位置参数和3个姿态参数,因此为了唯一确定关节角度,求得运动学逆解,需要人为增加1个冗余参数结合末端位置和姿态6个参数共同确定关节角度,本算法选用的冗余参数为相对参考平面的臂角ψ,具体说明如下:
对于七自由度冗余结构,当设定第三个关节角θ3=0时即变为6自由度机械臂,而确定的末端位置和姿态则提供了6个参数,6个位置姿态参数求解6个关节角变量,便是可解的;如附图1所示的S-R-S结构机械臂,最后三个关节即5,6,7关节轴线相交于一点,即W点;由附图2可以看出,当关节θ1~θ4角度确定后,参考平面SE′W即可确定;求解过程如下:
如附图3所示,θ4'可以通过余弦定理求得:
其中GC4为姿态参数,表明θ4'的正负,其取值范围为{-1,1};由图3可知δ的正负号与θ4'相同,因此:
θ2'的正方向与δ相反:
θ1'由几何关系可得:
其中ε是一个小阈值,可以取10-12且判断或是为了判断是否与z0对齐,即W点是否位于1关节轴线上,因为这种姿态为一个奇异位置,θ1'无法被唯一确定;由于只需求得一个参考平面SE′W,因此在这种情况下,统一设定θ1'=0;
至此,求得θ′1,θ′2,θ′3=0,θ′4,即可得参考平面SE′W。
4.根据权利要求1-3任一项所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法,其特征在于,所述(3)逆运动学求解包括:
0R4=0Rψ 0R′4
θ4=θ′4
3R4=3R′4
0R3 3R4=0Rψ 0R′3 3R′4
0R3=0Rψ 0R′3;
其中I为3×3单位矩阵,因此0R3可以表示为:
0R3(ψ)=sinψAs+cosψBs+Cs
其中:
同时,由于1-3关节为ZYZ球关节角,通过正运动学求得的公式为:
由0R3(θ1,θ2,θ3)=0R3(ψ)可得:
当θ2≠0或±π时
其中GC2同GC4,取值范围为{-1,1},表明θ2的正负,且:
当θ2=0或±π时
因此对应可求出θ1~θ3,θ4=θ4',之后便求解θ5~θ7;
同理,5-7关节也为ZYZ球关节角,因此同样可对应求出θ5~θ7,即将上式完成如下替换即可:
θ1←θ5,θ2←θ6,θ3←θ7,s←w,GC2←GC6
其中:
GC2、GC4和GC6均为姿态参数,取值为{-1,1},分别表明θ2、θ4和θ6的正负,臂角ψ为自运动冗余参数,至此完成逆运动学求解过程。
5.根据权利要求1-4任一项所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法,其特征在于,所述(4)奇异臂角获取包括:
所述数值稳定算法考虑整个“球关节”而不是单个关节来表征奇异臂角;对于S-R-S机械臂,关节1、2、3组成一个名为“肩”的球关节;关节5、6、7组成一个名为“腕”的球关节;由于肩关节和腕关节在本质上是相同的,统称为球关节;球关节有三个Z-Y-Z欧拉角变量θ1、θ2、θ3,其中θ1和θ3为正切关节,θ2为余弦关节;由ZYZ欧拉角形成的旋转矩阵0R3(θ1,θ2,θ3)可知,当且仅当θ2=0或±π时,θ1和θ3是无法唯一确定的,且此时对应表达式中u=v=0;反之,当位于奇异臂角ψs时,正切关节的u=v=0,由于0R3(θ1,θ2,θ3)为正交矩阵,即可证明此时cos(θ2)=±1即θ2=0或±π;因此θ1和θ3的奇异臂角ψs即为θ2=0或±π时对应的臂角;这也表明奇异臂角不是单个关节的特殊臂角,而是综合球关节的特殊臂角;因此得出结论,奇异臂角是机械臂局部自运动流形中的奇异位置,在球关节θ2=0或±π时出现;
描述球关节的奇异臂角的关键是找到使θi=0或±π的ψs(肩部i=2,腕部i=6),此臂角ψs同时也是与其相关的两个正切关节的奇异臂角;
余弦关节表达式为:
θi=GCiarccos(asinψ+bcosψ+c)
θi相对ψ求导公式为:
由表达式可知θi=0或±π对应于asin(ψ)+bcos(ψ)+c=±1,而asin(ψ)+bcos(ψ)+c的值均位于±1间;即使得θi=0或±π的ψs一定为asin(ψ)+bcos(ψ)+c的极值;因此,通过使其导数为零可得两个ψs值:
注意当ψ→ψs时sinθi→0,此时导数在ψs处不是连续的;图6为余弦关节的典型ψ-θi图;实现对球关节的奇异臂角ψs的表达后就可以对其相应的正切关节进行正确分类,并进行关节极限映射;算法1给出了求得球关节奇异臂角ψs的详细算法:
6.根据权利要求1-5任一项所述的S-R-S结构七自由度机械臂逆运动学解析解的数值稳定算法,其特征在于,所述(5)关节限位及奇异值规避包括:
因为S-R-S结构七自由度机械臂为冗余机械臂,因此可以实现对各关节限位的规避,实现方案为:将每个关节i的极限范围映射到冗余臂角ψ的范围内,求取各关节可行臂角Ψi的交集即可求得最终冗余臂角范围;因为在步骤(3)中已经得到θi关于臂角ψ的表达式,因此求取各关节可行臂角范围的关键是求关节限位线与ψ-θi图的交点,下面将按照关节分类进行阐述;在这里用和来表示第i个关节的上下关节角极限;其中θ4与ψ无关,其关节限位规避可直接由末端位姿求得θ4后与关节限位对比判断是否超限;
5.1余弦关节
余弦关节包括第2和6关节,其中图6是余弦关节的ψ-θi示意图;根据实际情况此图可能会发生垂直翻转或上下/左右平移的情况;给定θi,可通过万能公式求得两个对应的ψ(如果存在),ψ的计算公式为:
由前文计算奇异解的分析可知,和对应于θi的最大值或最小值,记为和下一步将分别为关节下限和上限找到对应的可行范围Ψil和Ψiu,最终得到此关节避免关节限位后对应的可行臂角范围:Ψi=Ψil∩Ψiu;其基本思想是利用上式(2)求出极限关节角对应的ψ1和ψ2,并通过ψ1和ψ2点的导数的正负判断ψ1、ψ2是可行区间的入口点还是离开点;例如,结合图6可知,若且ψ1为关节上限通过式(2)所求得,则1为离开点;算法2给出了余弦关节极限映射的详细算法;
5.2正切关节
正切关节的情况相对复杂,前文图4是正切关节的基本ψ-θi图;正切关节极限映射可分为Δ<0、Δ>0和Δ=0,三种情况进行讨论;如前文所述,由于atan2函数值域为[-π,π],因此图4中存在有±π间翻转现象,这使得正切关节的ψ与θi对应及关节极限规避更为复杂;
给定θi,可通过万能公式求得两个对应的ψ(如果存在),ψ的计算公式为:
其中
ap=(cd-bd)tanθi+bn-cn
bp=2(adtanθi-an)
cp=(bd+cd)tanθi-bn-cn
至此,已能够使用前文算法1正确识别奇异臂角情况;接下来将根据这三种情况来解决正切关节的关节极限映射问题;
(I)Δ<0,为单调
如附图7所示,Δ<0时ψ-θi图除±π间翻转外整体保持单调;即与和均各有一个交点(翻转现象是由于atan2的值域为[-π,π]导致的,虽然图(b)中看起来关节角极限与翻转线有交点,但实际根本取不到),这意味着根据式(3)计算的只有一个解是正确的;判断具体为哪个解只需将计算结果反代回θ关于ψ的公式验证即可;求解时当ap=0时,解由给出;设ψl和ψu分别为与下极限和上极限的交点;设ψ1=min(ψl,ψu),ψ2=max(ψl,ψu),则由图7可知,关节极限内的可选臂角范围为Ψi=[ψ1,ψ2]或者Ψi=[-π,ψ1]∪[ψ2,π];具体为哪个范围可通过计算ψ1是进入点还是离开点判断,或者通过观察偏转是否发生在ψ1和ψ2之间判断;偏转是否发生可通过计算是否在极限关节角范围内进行判断;算法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整体算法
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110322567.0A CN113378349B (zh) | 2021-03-25 | 2021-03-25 | S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110322567.0A CN113378349B (zh) | 2021-03-25 | 2021-03-25 | S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113378349A true CN113378349A (zh) | 2021-09-10 |
CN113378349B CN113378349B (zh) | 2022-05-20 |
Family
ID=77569794
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110322567.0A Active CN113378349B (zh) | 2021-03-25 | 2021-03-25 | S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113378349B (zh) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114474077A (zh) * | 2022-03-31 | 2022-05-13 | 哈尔滨工业大学 | 空间机器人关节的双旋变融合参数在轨标定方法 |
CN115229786A (zh) * | 2022-07-12 | 2022-10-25 | 中国地质大学(武汉) | 一种带肘部偏置的七自由度机械臂的几何逆解方法 |
CN117464667A (zh) * | 2023-10-09 | 2024-01-30 | 骨圣元化机器人(深圳)有限公司 | 七轴机械臂的运动规划方法、装置及机械臂 |
CN117798938A (zh) * | 2024-03-01 | 2024-04-02 | 北京长木谷医疗科技股份有限公司 | 一种多关节机器人非奇异评价控制方法及装置 |
CN118003322A (zh) * | 2024-01-30 | 2024-05-10 | 哈尔滨工业大学 | 一种基于改进臂角法的ssrms机械臂位置级逆解算法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4967126A (en) * | 1990-01-30 | 1990-10-30 | Ford Aerospace Corporation | Method of controlling a seven degree of freedom manipulator arm |
CN103955619A (zh) * | 2014-05-09 | 2014-07-30 | 大连大学 | 一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法 |
CN106625666A (zh) * | 2016-12-16 | 2017-05-10 | 广州视源电子科技股份有限公司 | 冗余机械臂的控制方法及装置 |
CN107066645A (zh) * | 2016-12-01 | 2017-08-18 | 西北工业大学 | 一种七自由度偏置机械臂逆解方法 |
-
2021
- 2021-03-25 CN CN202110322567.0A patent/CN113378349B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US4967126A (en) * | 1990-01-30 | 1990-10-30 | Ford Aerospace Corporation | Method of controlling a seven degree of freedom manipulator arm |
CN103955619A (zh) * | 2014-05-09 | 2014-07-30 | 大连大学 | 一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法 |
CN107066645A (zh) * | 2016-12-01 | 2017-08-18 | 西北工业大学 | 一种七自由度偏置机械臂逆解方法 |
CN106625666A (zh) * | 2016-12-16 | 2017-05-10 | 广州视源电子科技股份有限公司 | 冗余机械臂的控制方法及装置 |
Non-Patent Citations (1)
Title |
---|
胡奎等: "针对关节限位优化的7自由度机械臂逆运动学解法", 《清华大学学报(自然科学版)》 * |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114474077A (zh) * | 2022-03-31 | 2022-05-13 | 哈尔滨工业大学 | 空间机器人关节的双旋变融合参数在轨标定方法 |
CN114474077B (zh) * | 2022-03-31 | 2023-10-20 | 哈尔滨工业大学 | 空间机器人关节的双旋变融合参数在轨标定方法 |
CN115229786A (zh) * | 2022-07-12 | 2022-10-25 | 中国地质大学(武汉) | 一种带肘部偏置的七自由度机械臂的几何逆解方法 |
CN117464667A (zh) * | 2023-10-09 | 2024-01-30 | 骨圣元化机器人(深圳)有限公司 | 七轴机械臂的运动规划方法、装置及机械臂 |
CN118003322A (zh) * | 2024-01-30 | 2024-05-10 | 哈尔滨工业大学 | 一种基于改进臂角法的ssrms机械臂位置级逆解算法 |
CN117798938A (zh) * | 2024-03-01 | 2024-04-02 | 北京长木谷医疗科技股份有限公司 | 一种多关节机器人非奇异评价控制方法及装置 |
CN117798938B (zh) * | 2024-03-01 | 2024-05-28 | 北京长木谷医疗科技股份有限公司 | 一种多关节机器人非奇异评价控制方法及装置 |
Also Published As
Publication number | Publication date |
---|---|
CN113378349B (zh) | 2022-05-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113378349B (zh) | S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法 | |
Shademan et al. | Robust jacobian estimation for uncalibrated visual servoing | |
WO2018090323A1 (zh) | 一种坐标系标定方法、系统及装置 | |
CN110116407A (zh) | 柔性机器人位姿测量方法及装置 | |
Gong et al. | An uncalibrated visual servo method based on projective homography | |
CN107378952B (zh) | 一种冗余度机械臂末端执行器姿态保持的解决方法 | |
CN110000773B (zh) | 机器人的控制装置及机器人的逆变换处理方法 | |
CN107966907A (zh) | 一种应用于冗余度机械臂的障碍物躲避解决方法 | |
JP2774939B2 (ja) | ロボットのツールパラメータ導出方法及び較正方法 | |
CN110561420B (zh) | 臂型面约束柔性机器人轨迹规划方法及装置 | |
WO2017132905A1 (zh) | 控制运动系统的方法和装置 | |
CN112476435B (zh) | 重力加速度方向的标定方法、标定装置及存储介质 | |
CN113119096A (zh) | 机械臂空间位置调整方法、装置、机械臂及存储介质 | |
JPS60151711A (ja) | ロボツト手先視覚座標系較正方式 | |
Kang et al. | Adaptive trajectory tracking of wheeled mobile robots based on a fish-eye camera | |
CN113084791B (zh) | 机械臂控制方法、机械臂控制装置及终端设备 | |
Jia et al. | Switched visual servo control of nonholonomic mobile robots with field-of-view constraints based on homography | |
CN115771146A (zh) | 一种机器人姿态插补方法、装置及存储介质 | |
CN114474068B (zh) | 一种puma构型机器人腕关节奇异规避方法及系统 | |
CN109048911A (zh) | 一种基于矩形特征的机器人视觉控制方法 | |
Tsoy et al. | Estimation of 4-DoF manipulator optimal configuration for autonomous camera calibration of a mobile robot using on-board templates | |
Zhang et al. | Teaching-playback of robot manipulator based on human gesture recognition and motion tracking | |
JP2021122894A (ja) | 逆運動学演算装置及び逆運動学演算方法 | |
CN114492668B (zh) | 姿态相似度的判断方法、数据处理装置、存储介质 | |
JP2000094370A (ja) | ロボットの作業表面の傾き測定方法およびその測定装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |