基于臂角参数的SRS构型七自由度机械臂逆运动学优化方法

基于臂角参数的srs构型七自由度机械臂逆运动学优化方法
技术领域
1.本发明涉及机械臂控制领域，尤其涉及一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法。


背景技术：

2.随着工业自动化任务复杂程度的提高，传统的六自由度工业机器人已经不能满足很多作业场景的需求，因此近年来七自由度机械臂的运用也越来越广泛，逐渐出现在大众视野。七自由度机械臂相比于六自由度机械臂的主要特性为“自运动”，即保持末端位姿不变的情况下，仍然可以切换关节配置，因此也更加灵活，并且可以实现避奇异、避关节极限、优化力矩等。
3.运动学是机械臂控制的基础，对于冗余机械臂，可以采用数值解法或解析解法，但是数值解法一般速度较慢，不利于机械臂实时运动控制；采用固定关节的解析解法并没有充分利用七自由度机械臂的冗余性，违背了其设计初衷；对于具有特殊几何构型的s-r-s(球型-旋转-球型)机械臂，可以求解基于臂角的逆运动学解，几何意义明显，适合构型控制任务，且充分利用了七自由度机械臂的冗余性。为了提高机械臂的可操作度，机械臂在连续轨迹规划中，各个关节应该尽可能地远离关节极限，因此需要选取最优可行臂角对逆运动学进行优化达到避关节极限的目的。


技术实现要素：

4.本发明主要解决的技术问题是：解决srs构型七自由度机械臂以避关节极限为优化目标下最优关节配置选取问题。为了解决以上技术问题，本发明的技术方案提供一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，首先通过定义冗余参数臂角、参考平面和臂平面，给出了肘关节、肩关节和腕关节关于臂角的参数表达式，并结合机械臂各个关节的限位，求解出机械臂在给定末端位姿下的所有可行臂角范围，并考虑了算法奇异时有效臂角的选取问题。针对笛卡尔空间轨迹规划，引入了虚拟力的概念，并在此基础上定义了关节限位优化的目标函数，通过前一个路径点的臂角和该路径点末端位姿求解出唯一的最优可行臂角，用于得到给定末端位姿下的唯一关节配置。
5.本发明提供的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，包括以下步骤：
6.s101：根据srs构型七自由度机械臂结构定义冗余参数臂角、参考平面和臂平面；
7.s102：基于参考平面和臂平面，求解srs构型七自由度机械臂的肘关节角θ4、肩关节角θ1、θ2、θ3和腕关节角θ5、θ6、θ7的表达式；
8.s103：结合srs构型七自由度机械臂各关节角的限位，求解机械臂在给定末端位姿下的所有可行臂角ψ的范围。
9.进一步地，步骤s101中，所述参考平面由机械臂的肘部e的最高点、肩部s和腕部w构成；所述臂平面随机械臂运动而变化，当前臂平面由机械臂当前肘部e、肩部s和腕部w构
成；其中，肩部s为机械臂的前三个关节旋转轴的交点；腕部w为机械臂的后三个关节旋转轴交点。
10.进一步地，肘关节角θ4的表达式如下：
[0011][0012]
式中，acos()表示反余弦函数；d
se
表示肩部s到肘部e的距离；d
ew
表示肘部e到腕部w的距离；p
sw
＝[-d
wt
*ax+px-d
wt
*ay+py-d
wt
*az+pz-d
bs
]
t
，末端位姿d
wt
表示腕部w与最后一个关节角坐标系原点t的距离；d
bs
表示肩部s与机械臂基坐标系原点b的距离。
[0013]
进一步地，所述肩关节角θ1、θ2、θ3的表达式如下：
[0014]
cosθ2＝-as(3,2)sinψ-bs(3,2)cosψ-cs(3,2)
[0015][0016][0017]
式中，式中，为臂角ψ为0时，肘部e位于最高点时前三个关节旋转矩阵；usw为肩部s和腕部w构成的向量对应的单位向量；[usw
×
]为usw的反对称矩阵；as(i,j)、bs(i,j)、cs(i,j)分别表示矩阵as、bs、cs的第i行第j列元素。
[0018]
进一步地，所述腕关节角θ5、θ6、θ7的表达式如下：
[0019]
cosθ6＝aw(3,3)sinψ+bw(3,3)cosψ+cw(3,3)
[0020][0021][0022]
式中：
[0023][0024][0025][0026]
[0027][0028][0029][0030]
其中cosθi为ci，sinθi为si，*表示忽略项。
[0031]
进一步地，步骤s103中求解机械臂在给定末端位姿下的所有可行臂角ψ的范围，具体如下：
[0032]
关节角θ4和θ7对应可行臂角ψ区间均为[-pi,pi]；
[0033]
关节角θ2和θ6的可行臂角ψ求解为：已知求解时ψ的有效范围，具体为：
[0034]
当z
min
》1或z
max
《-1，此时无有效臂角，即
[0035]
当z
min
《-1且-1《＝z
max
《＝1，此时
[0036]
当z
min
《-1且z
max
》1，此时ψ∈[-pi,pi]；
[0037]
当-1《＝z
min
《1且-1《＝z
max
《＝1，此时
[0038][0039]
当-1《＝z
min
《＝1且z
max
》1，此时
[0040]
其中a,b,c为关节角2和6的项系数：cosθi＝a sinψ+b cosψ+c。
[0041]
进一步地，关节角θ1、θ3、θ5的可行臂角ψ求解如下式：
[0042][0043]
当a
t2
+b
t2-c
t2
《0时，根据关节角上下极限求解臂角的上下范围，除以下两种情况：
[0044]
若c
t
》0，臂角ψ有效范围为
[0045]
若c
t
《0时，臂角有效范围为
[0046]
当a
t2
+b
t2-c
t2
》0时，关节角θ上下极限分别对应的臂角如下：
[0047]
[0048][0049]
若θ
min
》θuorθ
max
《θ
l
，此时无有效臂角，
[0050]
若θ
l
≤θ
min
≤θuandθ
l
≤θ
max
≤θu，可行臂角ψ范围为[-pi,pi]；
[0051]
若θ
min
《θ
l
andθ
l
≤θ
max
≤θu，进一步分类，若θ
l
》atan2(c
n-bn,c
d-bd)此时可行臂角ψ范围为若θ
l
≤atan2(c
n-bn,c
d-bd)，此时可行臂角ψ范围为
[0052]
若θ
l
≤θ
min
≤θuandθ
max
》θu，进一步分类，若θu》atan2(c
n-bn,c
d-bd)，此时可行臂角ψ范围为若θu≤atan2(c
n-bn,c
d-bd)，此时可行臂角ψ范围为
[0053]
若θ
min
《θ
l
andθ
max
》θu，根据θ
l
和θu位置不同进一步分类，若θ
l
≤atan2(c
n-bn,c
d-bd)andθu≤atan2(c
n-bn,c
d-bd)，此时可行臂角ψ范围为若θ
l
≤atan2(c
n-bn,c
d-bd)andθu》atan2(c
n-bn,c
d-bd)，如果θ关于ψ变化趋势是先极大后极小，那么可行臂角ψ范围为：
[0054]
否则可行臂角ψ范围为：
[0055][0056]
若θ
l
》atan2(c
n-bn,c
d-bd)，那么可行臂角ψ范围为
[0057]
当采用奇异方式处理求解可行臂角ψ的范围。
[0058]
步骤s103求解可行臂角ψ的范围后，通过虚拟力权重方式，结合可行臂角ψ的范围，求解机械臂路径规划最优可行解。
[0059]
与现有技术相比，本发明的有益效果包括：1.采用臂角参数的解析逆运动学求解方法，求解速度快，几何意义明显，适用于包含避障在内的构型控制任务。2.针对任意给定的末端位姿，在已知关节极限的情况下求解出所有可行臂角，并给出了算法奇异发生时有效臂角的处理方法。3.通过定义以虚拟力为权重的目标函数来进行关节限位优化，可以获得使各个关节尽可能远离关节极限的最优臂角，一定程度上也提高了机器人的可操作度。
附图说明
[0060]
图1是本发明提供的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法的流程示意图；
[0061]
图2是自运动描述示意图；
[0062]
图3是参考平面示意图；
[0063]
图4是a
t2
+b
t2-c
t2
《0时的示意图；
[0064]
图5是a
t2
+b
t2-c
t2
《0时臂角范围求解的特殊情况示意图。
[0065]
图6是a
t2
+b
t2-c
t2
》0的求解情况示意图；
[0066]
图7是c
t
》0且lim
left
》lim
right
的关节角与臂角关系示意图；
[0067]
图8是θ-≥0的臂角求解示意图；
[0068]
图9是θ-≤0的臂角求解示意图；
[0069]
图10是c
t
《0andlim
left
《lim
right
的臂角求解情况示意图；
[0070]
图11是c
t
《0且θ-≥0的臂角求解情况示意图；
[0071]
图12是c
t
《0且θ-≤0的臂角求解情况示意图；
[0072]
图13是机械臂末端轨迹仿真示意图；
[0073]
图14是臂角变化示意图；
[0074]
图15是关节角变化示意图。
具体实施方式
[0075]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0076]
本发明提供了一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法。请参考图1，图1是本发明方法的流程图。
[0077]
方法包括以下步骤：
[0078]
s101：根据srs构型七自由度机械臂结构定义冗余参数臂角、参考平面和臂平面；
[0079]
作为一种实施例，本发明所涉及的机械臂为七自由度srs构型机械臂，机械臂dh参数如下表1所示：
[0080]
表1 srs构型七自由度机械臂模型
[0081][0082]
七自由度机械臂为冗余机械臂，即机械臂自由度超过操作空间所需自由度，因此具有“自运动”特性，即末端位姿不变时，仍然可以切换关节配置。srs构型七自由度机械臂的特殊性在于它的自运动体现为末端位姿不变时，肘部可以绕着肩部和腕部连线做圆周运动，自运动描述如下图2所示。图2是自运动描述示意图。
[0083]
图2中，b为基座，即坐标系0原点；s为肩部，即前三个关节旋转轴交点；e为肘部，即坐标系4的原点；w为腕部，即后三个关节旋转轴交点；t为坐标系7原点。取肘部最高点、s、w构成的平面为参考平面，当前肘部e、s、w构成的平面为当前臂平面。
[0084]
因此臂角ψ定义为参考平面按照右手螺旋定则绕向量p
sw
转到当前臂平面所需的角度，臂角的有效取值范围为[-pi,pi]。
[0085]
给定末端位姿和臂角ψ，末端位姿对应的旋转矩阵为
[0086]
s102：基于参考平面和臂平面，求解srs构型七自由度机械臂的肘关节角θ4、肩关节角θ1、θ2、θ3和腕关节角θ5、θ6、θ7的表达式；
[0087]
作为一种实施例，求解肘部关节角θ4的过程如下：
[0088]
将末端位置沿着z7相反方向平移d
wt
即可达到腕部位置，因此
[0089][0090]
pw＝[-d
wt
*ax+px
ꢀ‑dwt
*ay+py
ꢀ‑dwt
*az+pz]
t (2)
[0091]
p
sw
＝[-d
wt
*ax+px
ꢀ‑dwt
*ay+py
ꢀ‑dwt
*az+pz-d
bs
]
t (3)
[0092]
所以由图2几何关系可知
[0093][0094]
可以得到肘部关节角
[0095][0096]
式中，acos()表示反余弦函数；d
se
表示肩部s到肘部e的距离；d
ew
表示肘部e到腕部w的距离；d
wt
表示腕部w与最后一个关节角坐标系原点t的距离；d
bs
表示肩部s与机械臂基坐标系原点b的距离；
[0097]
作为一种实施例，所述肩关节角θ1、θ2、θ3的表达式的求解过程具体如下：
[0098]
给出罗德里格斯如下
[0099]
r(k,θ)＝i+sinθ*[k
×
]+(1-cosθ)[k
×
]2,k＝[k
1 k
2 k3]
t (6)
[0100]
其中i为单位矩阵，[k
×
]为k的反对称矩阵，该公式表示绕三维空间任意向量k旋转θ所得到的旋转矩阵。给出参考平面示意图如下图3所示，图3时参考平面示意图；由dh坐标系的建系规则可知：
[0101]
p
sw
+y3*d
se
＝z4*d
ew
[0102]
为了计算方便，令cosθi为ci，sinθi为si，由运动学公式可知
[0103][0104]
所以
[0105][0106]
令臂角为0，即肘部位于最高点时前三个关节旋转矩阵为因此可以得到
[0107][0108]
将式(10)代入式(7),可以得到
[0109][0110]
任意臂角时前三个关节旋转矩阵是由自运动得到的，也就是由于机械臂肘部的圆周运动导致前三个关节旋转矩阵发生了变化，因此可以得到
[0111][0112]
上式代入罗德里格斯公式，所以：
[0113][0114][0115]
其中：
[0116][0117]
又由正运动学可知
[0118][0119]
其中*代表忽略该项。结合式(16)和式(13),对比矩阵元素，可以得到
[0120][0121]
式(17)中，式(17)中，为臂角ψ为0时，肘部e位于最高点时前三个关节旋转矩阵；usw为肩部s和腕部w构成的向量对应的单位向量；[usw
×
]为usw的反对称矩阵；as(i,j)、bs(i,j)、cs(i,j)分别表示矩阵as、bs、cs的第i行第j列元素。
[0122]
作为一种实施例，所述腕关节角θ5、θ6、θ7的表达式的求解过程具体如下：
[0123]
因为旋转矩阵均为正交矩阵，所以
[0124][0125]
因此
[0126][0127]
其中
[0128][0129]
又由正运动学公式可以得到后三个关节旋转矩阵
[0130][0131]
其中*代表忽略该项。同理对比矩阵元素可知：
[0132][0133]
s103：结合srs构型七自由度机械臂各关节角的限位，求解机械臂在给定末端位姿下的所有可行臂角ψ的范围。
[0134]
作为一种实施例，本发明对于有效臂角区间求解如下。
[0135]
任何机械系统，均存在机械上的不可达性。在设计机械臂关节的时候，不可能使每个关节运动范围均可达到
±
360
°
，一方面是机械结构的限制另一方面也是为了防止机械臂发生自碰撞，因此每个关节均有关节极限约束。对于每个给定的末端位姿，srs构型七自由度机械臂自运动时不一定可以完成完整的圆周运动，因为其臂角所对应的关节角度可能超过了关节极限，因此必须根据各个关节极限求解其对应的有效臂角区间，才可以选取有效的关节配置来达到给定的末端位姿。因为θ4大小与臂角无关且θ7运动范围为
±
360
°
，因此θ4和θ7对应有效臂角区间均为[-pi,pi]。
[0136]
由逆运动学求解可知，关节角和臂角的关系可以分为cos型和tan型两种，因此下面就这两种情况分别讨论其有效臂角区间。
[0137]
(1)cos型关节角
[0138]
cos型关节角主要包含关节2和6，其格式如下
[0139]
[0140]
记记令θi取值范围为zi取值范围为[z
min
,z
max
]，考虑到关节2和6运动范围均为[-120
°
,120
°
]。因此因此cos型关节角有效臂角求解问题转化为已知求解时ψ的有效范围，因此分别以下几种情况讨论：
[0141]
当z
min
》1或z
max
《-1，此时无有效臂角，即
[0142]
当z
min
《-1且-1《＝z
max
《＝1，此时
[0143]
当z
min
《-1且z
max
》1，此时ψ∈[-pi,pi]；
[0144]
当-1《＝z
min
《1且-1《＝z
max
《＝1，此时
[0145][0146]
当-1《＝z
min
《＝1且z
max
》1，此时
[0147]
其中
[0148]
2)tan型关节角
[0149]
因为关节7运动范围为
±
360
°
，因此这里只考虑关节1、3、5，其通用格式如下：
[0150][0151]
上式两边同时对ψ求导可以得到：
[0152][0153]
其中θ关于ψ有单调和周期两种情况。令导数为0求解出相应的臂角。
[0154][0155]
当a
t2
+b
t2-c
t2
《0时，c
t
》0时式(25)恒大于0因此θ与ψ正相关，c
t
《0时式(25)恒小于0因此θ与ψ负相关，综上θ相对ψ是单调的，即θ与ψ一一对应。
[0156]
根据θ反解ψ时可能会出现两个解，因此需要根据ψ,θ判断sinθ,cosθ符号正确性进而去除无效解。但是由于正切函数的特殊性，θ关于ψ的函数可能会发生跳跃间断点。在臂角范围[-pi,pi]内，若θ到达了
±
pi，则会发生2*pi的跳变。
[0157]
因此a
t2
+b
t2-c
t2
《0时可以分为以下几种情况：请参考图4，图4是a
t2
+b
t2-c
t2
《0时的示意图。
[0158]
通过分析可以得知有以下两种情况需要特殊讨论,关节角上下极限如蓝色虚线所
示，其他情况的有效臂角范围都可以按照单调的处理方式来求解，
[0159]
请参考图5，图5是a
t2
+b
t2-c
t2
《0时臂角范围求解的特殊情况示意图。
[0160]
针对图5的特殊情况，我们给出相应的臂角范围如下：
[0161]
当c
t
》0，臂角有效范围为
[0162]
当c
t
《0，臂角有效范围为
[0163]
在a
t2
+b
t2-c
t2
》0时(a
t2
+b
t2-c
t2
等于0时为奇异情况，在后文进行解释)，此时存在两个驻点，其中一个使θ达到极大值，另一个使θ达到极小值。我们把θ极大值定义为θ
max
，其对应的臂角为ψ
max
；把θ极小值定义为θ
min
，其对应的臂角为ψ
min
。因此式(25)两边再同时对ψ求导可以分析得出ψ
min,max
分别对应θ的全局最大值和全局最小值且ψ＝
±
pi时对应的θ相同。因此a
t2
+b
t2-c
t2
》0时可以分为以下2种情况：请参考图6，图6是a
t2
+b
t2-c
t2
》0的求解情况示意图；
[0164]
先给出θ上下极限分别对应的臂角如下先给出θ上下极限分别对应的臂角如下
[0165]
下面开始进行分类讨论：
[0166]
(i)θ
min
》θuorθ
max
《θ
l
，无有效臂角；
[0167]
(ii)θ
l
≤θ
min
≤θuandθ
l
≤θ
max
≤θu，可行臂角范围为[-pi,pi]；
[0168]
(iii)θ
min
《θ
l
andθ
l
≤θ
max
≤θu，这里还需要分类讨论，即
[0169]
[1]θ
l
》atan2(c
n-bn,c
d-bd)，此时臂角有效范围为
[0170]
[2]θ
l
≤atan2(c
n-bn,c
d-bd)，此时臂角有效范围为
[0171]
(iv)θ
l
≤θ
min
≤θuandθ
max
》θu，这里还需要分类讨论，即
[0172]
[1]θu》atan2(c
n-bn,c
d-bd)，此时臂角有效范围为
[0173]
[2]θu≤atan2(c
n-bn,c
d-bd)，此时臂角有效范围为
[0174]
(v)θ
min
《θ
l
andθ
max
》θu[0175]
根据θ
l
和θu位置不同可分为以下几种情况：
[0176]
[1]θ
l
≤atan2(c
n-bn,c
d-bd)andθu≤atan2(c
n-bn,c
d-bd)，此时有效臂角范围为：
[0177][0178]
[2]θ
l
≤atan2(c
n-bn,c
d-bd)andθu》atan2(c
n-bn,c
d-bd)，如果θ关于ψ变化趋势是先极大后极小，那么臂角有效范围为：
[0179][0180]
否则有效臂角范围为：
[0181][0182]
[3]θ
l
》atan2(c
n-bn,c
d-bd)，此时有效臂角范围为：
[0183][0184]
步骤s103中求解机械臂在给定末端位姿下的所有可行臂角ψ的范围时，还采用奇异处理方式进行求解。
[0185]
作为一种实施例，下面就cos型关节角和tan型关节角存在的算法奇异问题进行讨论。
[0186]
(1)cos型关节角算法奇异
[0187]
如果a＝b＝0，此时cosθi＝c，则关节角θ为常数，因此其对应的有效臂角范围为[-pi,pi]。
[0188]
(2)tan型关节角算法奇异
[0189]
fd(ψ)＝0时会发生算法奇异。a
t2
+b
t2-c
t2
＝0时仅存在一个驻点此时可以证明fn(ψ)＝fd(ψ)＝0,无法确定θ大小进而出现算法奇异,可以证明除了奇异点ψ0之外符号不会发生变化,即θ相对于ψ要么正相关要么负相关。代入ψ0+δ到式(24)得到
[0190][0191]
求得奇异点左右极限
[0192][0193]
易知二者对应的θ相差pi。在进行下面的讨论之前，我们需要知道关节极限所对应的臂角所在区间的分布状态。令为下极限对应在区间[-pi,ψ0]的臂角，为下极限对应在区间[ψ0,pi]的臂角；令为上极限对应在区间[-pi,ψ0]的臂角，为
上极限对应在区间[ψ0,pi]的臂角，且令θ-＝atan2(c
n-bn,c
d-bd)。下面开始分类讨论
[0194]
(a)c
t
＝0
[0195]
此时a
t
＝b
t
＝0，因此即θ不随ψ变化，因此有效臂角范围为[-pi,pi]。
[0196]
(b)c
t
》0
[0197]
θ与ψ正相关，即函数除了奇异点之外单调增。
[0198]
(i)lim
left
》lim
right
，请参考图7所示，图7是c
t
》0且lim
left
》lim
right
的关节角与臂角关系示意图；
[0199]
[1]θu《lim
right orθ
l
》lim
left
，无有效臂角；
[0200]
[2]θ
l
《lim
right
andlim
right
≤θu《θ-，有效臂角范围为
[0201]
[3]θ
l
《lim
right andθ-≤θu≤lim
left
，有效臂角范围为
[0202]
[4]θ
l
《lim
right andθu》lim
left
，有效臂角范围为[-pi,pi]；
[0203]
[5]θ
l
≥lim
right
andθu《θ-，有效臂角范围为
[0204]
[6]lim
right
≤θ
l
《θ-andθ-≤θu≤lim
left
，有效臂角范围为
[0205]
[7]lim
right
≤θ
l
《θ-andθu》lim
left
，有效臂角范围为[8]θ-≤θ
l
andθu≤lim
left
，有效臂角范围为
[0206]
[9]θ-≤θ
l
≤lim
left andθu》lim
left
，有效臂角范围为
[0207]
(ii)lim
left
《lim
right
[0208]
[1]θ-≥0，请参考图8，图8是θ-≥0的求解示意图。
[0209]
a)lim
left
≤θu≤lim
right
，有效臂角范围为
[0210]
b)lim
right
≤θu≤θ-andθ
l
≤lim
left
，有效臂角范围为
[0211]
c)lim
right
≤θu≤θ-andlim
left
≤θ
l
≤lim
right
，有效臂角范围为
[0212]
d)θ-《θuandθ
l
《lim
left
，有效臂角范围为
[0213]
e)θ-《θuand lim
left
≤θ
l
，有效臂角范围为(ψ0,pi]；
[0214]
[2]θ-≤0，请参考图9，图9是θ-≤0的臂角求解示意图；
[0215]
a)θ
l
《θ-and lim
right
≤θu≤lim
left
，有效臂角范围为(ψ0,pi]；
[0216]
b)θ-≤θ
l
《lim
right and lim
right
≤θu≤lim
left
，有效臂角范围为
[0217]
c)θ
l
《θ-andθu》lim
left
，有效臂角范围为
[0218]
d)θ-≤θ
l
《lim
right
andθu》lim
left
，有效臂角范围为
[0219]
e)lim
right
≤θ
l
≤lim
left andθu》lim
left
，有效臂角范围为
[0220]
(c)c
t
《0，θ与ψ负相关，即除了奇异点之外单调减；
[0221]
(i)lim
left
《lim
right
，请参考图10，图10是c
t
《0andlim
left
《lim
right
的臂角求解情况示意图；
[0222]
a)θu《lim
left
orθ
l
》lim
right
无有效臂角；
[0223]
b)θ
l
《lim
left and lim
left
≤θu《θ-，有效臂角范围为
[0224]
c)θ
l
《lim
left andθ-≤θu≤lim
right
，有效臂角范围为
[0225]
d)θ
l
《lim
left
andθu》lim
right
，有效臂角范围为[-pi,pi]；
[0226]
e)θ
l
≥lim
left andθu《θ-，有效臂角范围为
[0227]
f)lim
left
≤θ
l
《θ-andθ-≤θu≤lim
right
，有效臂角范围为
[0228]
g)lim
left
≤θ
l
《θ-andθu》lim
right
，有效臂角范围为
[0229]
h)θ-≤θ
l
andθu≤lim
right
，有效臂角范围为
[0230]
i)θ-≤θ
l
≤lim
right andθu》lim
right
，有效臂角范围为
[0231]
(ii)lim
left
》lim
right
[0232]
[1]θ-≥0，请参考图11，图11是c
t
《0且θ-≥0的臂角求解情况示意图；
[0233]
a)lim
right
≤θu≤lim
left andθ
l
《lim
right
，有效臂角范围为
[0234]
b)lim
left
《θu≤θ-andθ
l
《lim
right
，有效臂角范围为
[0235]
c)lim
left
《θu≤θ-and lim
right
≤θ
l
≤lim
left
，有效臂角范围为
[0236]
d)θu》θ-andθ
l
《lim
right
，有效臂角范围为
[0237]
e)θu》θ-andlim
right
≤θ
l
≤lim
left
，有效臂角范围为[-pi,ψ0)；
[0238]
[2]θ-≤0，请参考图12，图12是c
t
《0且θ-≤0的臂角求解情况示意图；
[0239]
a)θ
l
《θ-andlim
right
≤θu≤lim
left
，有效臂角范围为(ψ0,pi]；
[0240]
b)θ-≤θ
l
《lim
right andlim
right
≤θu≤lim
left
，有效臂角范围为
[0241]
c)θ
l
《θ-andθu》lim
left
，有效臂角范围为
[0242]
d)θ-≤θ
l
《lim
right andθu》lim
left
，有效臂角范围为
[0243]
e)lim
right
≤θ
l
≤lim
left andθu》lim
left
，有效臂角范围为
[0244]
步骤s103求解可行臂角ψ的范围后，通过虚拟力权重方式，结合可行臂角ψ的范围，求解机械臂路径规划最优可行解。
[0245]
作为一种实施例，机械臂在进行连续轨迹规划中，各个关节远离关节极限才可以有更大的操作空间。srs七自由度冗余机械臂在给定末端位姿下仍然可能存在无数组关节配置，因此需要选取最优臂角来使各个关节尽可能地远离关节极限。本专利讨论的机械臂关节极限如下表所示：
[0246]
表2机械臂关节极限
[0247][0248][0249]
机械臂各个关节运动范围不同，因此需要进行归一化处理，定义用来衡量关节角距离关节极限的远近程度。给出安全区域的概念，安全区域为各个关节运动范围的80％，关节7运动范围为
±
360
°
，无需考虑安全区域。因此除关节7以外的各个关节安全区域如下表所示：
[0250]
表3机械臂各关节安全区域
[0251][0252]
引入虚拟力的概念，并给出虚拟力g(x)的表达式
[0253][0254]
我们希望虚拟力在关节可运动范围的80％～90％内变化较慢，90％～100％变化较快，再结合安全区域的概念，令a＝0.8,b＝1。由虚拟力的表达式可知，关节处在安全区域内时，无虚拟力作用，当关节越靠近关节极限，虚拟力越大。连续轨迹规划中若路径点存在关节角不位于安全区域，应该调整臂角使下一个路径点的相应关节不再继续靠近关节极限，使相邻路径点的关节角大小尽可能保持一致。因此以虚拟力作为权重，给出优化目标函数如下：
[0255][0256]
由逆运动学求解过程可知，臂角和关节角的关系十分复杂，为了简化计算，我们认为在一个微小的臂角变化量δψ内，臂角和关节角的关系是线性的，线性关系如下：
[0257][0258]
其中下标k和(k+1)分别代表路径点k和路径点(k+1)。将简化后的臂角和关节角的关系代入到优化目标函数可以得到新的优化目标函数
[0259]
可以得出f(ψ
k+1
)为ψ
k+1
的二次函数，因此当目标函数取最小值时相应的臂角应该取为二次函数的极值点。当二次函数的极值点并不属于有效臂角时，需要另行处理。如果选取离极值点最近的有效臂角为最优臂角的话，可能因为计算误差使该臂角对应的关节角超过关节极限，而且区间端点一般会使某个关节达到关节极限。基于上述考虑，最优臂角在距离二次函数极值点最近的有效臂角区间内取，最优臂角到距离极值点最近的有效臂角区间端点的距离为该有效臂角区间长度的30％。
[0260]
请参考图13，图13是机械臂末端轨迹仿真示意图；仿真验证该优化算法的有效性，考虑三段直线轨迹作为机械臂末端轨迹，即从点(0.6,0,0.4)运动到点(0.6,-0.5,0.4)再运动到(-0.3,-0.5,0.4)最终到达(-0.3,-0.5,0.6)。
[0261]
给定初始位姿初始路径点对应臂角为-75
°
，初始路径点对应的关节配置为[42.20
°ꢀ
61.95
°ꢀ‑
94.34
°ꢀ
78.47
°ꢀ
63.05
°ꢀ
99.18
°ꢀ‑
39.56
°
]，机械臂在运动过程中姿态始终保持不变。
[0262]
下面给出仿真结果，其中虚线代表未优化的臂角和关节角，实线代表采用本优化算法优化后的臂角和关节角。请参考图14和图15；图14是臂角变化示意图；图15是关节角变化示意图；
[0263]
从最终仿真结果得知，机械臂在运动过程中，只有关节6和7进入了危险区。如果不调整臂角，关节6在运动过程中会不断向极限靠近，最终到达116.79
°
。采用本优化算法后，关节6不再向极限靠近，且最终只会到达100
°
。对于关节7，该优化算法可以使该关节最终进入安全区，本专利提出的优化算法在一定程度上实现了避关节极限，并提高了机械臂的可操作度。
[0264]
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本技术实施例的实施过程构成任何限定。
[0265]
本发明的有益效果是：1.采用臂角参数的解析逆运动学求解方法，求解速度快，几何意义明显，适用于包含避障在内的构型控制任务。2.针对任意给定的末端位姿，在已知关节极限的情况下求解出所有可行臂角，并给出了算法奇异发生时有效臂角的处理方法。3.通过定义以虚拟力为权重的目标函数来进行关节限位优化，可以获得使各个关节尽可能远离关节极限的最优臂角，一定程度上也提高了机器人的可操作度。
[0266]
以上所述本发明的具体实施方式，并不构成对本发明保护范围的限定。任何根据本发明的技术构思所做出的各种其他相应的改变与变形，均应包含在本发明权利要求的保护范围内。

技术特征：
1.一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：包括以下步骤：s101：根据srs构型七自由度机械臂结构定义冗余参数臂角、参考平面和臂平面；s102：基于参考平面和臂平面，求解srs构型七自由度机械臂的肘关节角θ4、肩关节角θ1、θ2、θ3和腕关节角θ5、θ6、θ7的表达式；s103：结合srs构型七自由度机械臂各关节角的限位，求解机械臂在给定末端位姿下的所有可行臂角ψ的范围。2.如权利要求1所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：步骤s101中，所述参考平面由机械臂的肘部e的最高点、肩部s和腕部w构成；所述臂平面随机械臂运动而变化，当前臂平面由机械臂当前肘部e、肩部s和腕部w构成；其中，肩部s为机械臂的前三个关节旋转轴的交点；腕部w为机械臂的后三个关节旋转轴交点。3.如权利要求1所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：肘关节角θ4的表达式如下：式中，acos()表示反余弦函数；d
se
表示肩部s到肘部e的距离；d
ew
表示肘部e到腕部w的距离；p
sw
＝[-d
wt
*ax+px
ꢀ‑
d
wt
*ay+py
ꢀ‑
d
wt
*az+pz-d
bs
]
t
，末端位姿d
wt
表示腕部w与最后一个关节角坐标系原点t的距离；d
bs
表示肩部s与机械臂基坐标系原点b的距离。4.如权利要求1所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：所述肩关节角θ1、θ2、θ3的表达式如下：cosθ2＝-a
s
(3,2)sinψ-b
s
(3,2)cosψ-c
s
(3,2)(3,2)式中，式中，为臂角ψ为0时，肘部e位于最高点时前三个关节旋转矩阵；usw为肩部s和腕部w构成的向量对应的单位向量；[usw
×
]为usw的反对称矩阵；a
s
(i,j)、b
s
(i,j)、c
s
(i,j)分别表示矩阵a
s
、b
s
、c
s
的第i行第j列元素。5.如权利要求4所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：所述腕关节角θ5、θ6、θ7的表达式如下：
cosθ6＝a
w
(3,3)sinψ+b
w
(3,3)cosψ+c
w
(3,3)(3,3)式中：式中：式中：式中：式中：式中：式中：其中cosθ
i
为c
i
，sinθ
i
为s
i
，*表示忽略项。6.如权利要求1所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：步骤s103中求解机械臂在给定末端位姿下的所有可行臂角ψ的范围，具体如下：关节角θ4和θ7对应可行臂角ψ区间均为[-pi,pi]；关节角θ2和θ6的可行臂角ψ求解为：已知求解时ψ的有效范围，具体为：当z
min
>1或z
max
<-1，此时无有效臂角，即当z
min
<-1且-1<＝z
max
<＝1，此时当z
min
<-1且z
max
>1，此时ψ∈[-pi,pi]；当-1<＝z
min
<1且-1<＝z
max
<＝1，此时当-1<＝z
min
<＝1且z
max
>1，此时其中a,b,c为关节角2和6的项系数：cos
θ
i
＝a sinψ+b cosψ+c。7.如权利要求6所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：关节角θ1、θ3、θ5的可行臂角ψ求解如下式：当a
t2
+b
t2-c
t2
<0时，根据关节角上下极限求解臂角的上下范围，除以下两种情况：若c
t
>0，臂角ψ有效范围为若c
t
<0时，臂角有效范围为当a
t2
+b
t2-c
t2
>0时，关节角θ上下极限分别对应的臂角如下：>0时，关节角θ上下极限分别对应的臂角如下：若θ
min
>θ
u
orθ
max
<θ
l
，此时无有效臂角，若θ
l
≤θ
min
≤θ
u
andθ
l
≤θ
max
≤θ
u
，可行臂角ψ范围为[-pi,pi]；若θ
min
<θ
l
andθ
l
≤θ
max
≤θ
u
，进一步分类，若θ
l
>atan2(c
n-b
n
,c
d-b
d
)此时可行臂角ψ范围为若θ
l
≤atan2(c
n-b
n
,c
d-b
d
)，此时可行臂角ψ范围为若θ
l
≤θ
min
≤θ
u
andθ
max
>θ
u
，进一步分类，若θ
u
>atan2(c
n-b
n
,c
d-b
d
)，此时可行臂角ψ范围为若θ
u
≤atan2(c
n-b
n
,c
d-b
d
)，此时可行臂角ψ范围为若θ
min
<θ
l
andθ
max
>θ
u
，根据θ
l
和θ
u
位置不同进一步分类，若θ
l
≤atan2(c
n-b
n
,c
d-b
d
)andθ
u
≤atan2(c
n-b
n
,c
d-b
d
)，此时可行臂角ψ范围为若θ
l
≤atan2(c
n-b
n
,c
d-b
d
)andθ
u
>atan2(c
n-b
n
,c
d-b
d
)，如果θ关于ψ变化趋势是先极大后极小，那么可行臂角ψ范围为：否则可行臂角ψ范围为：若θ
l
>atan2(c
n-b
n
,c
d-b
d
)，那么可行臂角ψ范围为
当采用奇异方式处理求解可行臂角ψ的范围。8.如权利要求1所述的一种基于臂角参数的srs构型七自由度机械臂逆运动学优化方法，其特征在于：步骤s103求解可行臂角ψ的范围后，通过虚拟力权重方式，结合可行臂角ψ的范围，求解机械臂路径规划最优可行解。

技术总结
本发明公开一种基于臂角参数的SRS构型七自由度机械臂逆运动学优化方法，包括：根据SRS构型七自由度机械臂结构定义冗余参数臂角、参考平面和臂平面；基于参考平面和臂平面，求解SRS构型七自由度机械臂的肘关节角θ4、肩关节角θ1、θ2、θ3和腕关节角θ5、θ6、θ7的表达式；结合SRS构型七自由度机械臂各关节角的限位，求解机械臂在给定末端位姿下的所有可行臂角ψ的范围。本发明的有益效果是：求解速度快，几何意义明显，适用于包含避障在内的构型控制任务，一定程度上也提高了机器人的可操作度。一定程度上也提高了机器人的可操作度。一定程度上也提高了机器人的可操作度。


技术研发人员：陈鑫 梅义胜 王行澳
受保护的技术使用者：中国地质大学（武汉）
技术研发日：2021.12.06
技术公布日：2022/3/8