一种绳驱机械臂变刚度规划方法及系统、计算机存储介质

1.本发明涉及机器人控制领域，尤其是一种绳驱机械臂变刚度规划方法及系统、计算机存储介质。


背景技术：

2.与传统刚性机械臂相比，绳驱机械臂的结构更加轻便，运动更加灵活。绳驱机械臂的电气控制部分与机械臂部分通常采用分离设计，既能减轻机械臂的重量和尺寸，又更能灵活的适应各种恶劣的环境。
3.由于绳驱机械臂的自由度超冗余的特点，进行绳驱机械臂刚度建模十分困难。绳驱机械臂的变刚度控制一直是绳驱机械臂研究的重点和难点。nan等人提出了一种带有中间支撑的2自由度机械手，建立了单关节刚度模型，并且通过实验验证了该刚度模型与实验测试的偏差；在liu等人的文章中，一种可提高刚度和负载能力的绳驱机械臂被提出，该机械臂由三个连接段组成；pang等人设计一种基于张力放大机构的7自由度的绳索驱动机械臂，解决了机械臂在运行时的绳索松弛问题，并建立了腕关节和肘关节的刚度模型。很多学者只对绳驱机械臂的单个关节进行了刚度建模，不过yan等人通过解析法和数值法分别建立了整个绳驱机械臂的刚度模型。该刚度模型避免了伪逆运算引起的计算误差。它与传统刚度模型相比极大的提高了计算精度。
4.由于绳驱机械臂的关节数多，转动灵活，所以绳驱机械臂刚度控制十分困难。绳驱机械臂的刚度通常由绳索张力和机械臂的构型决定。xu等人提出了一种改进的模态法来求解绳驱机械臂的逆运动学，该方法通过在求逆解过程中优化局部关节的角度，规划三维构型，来实现轨迹规划和避障功能。mu等人提出了一种同时考虑机器人总体构型和末端姿态的规划方法，通过改变绳驱机械臂的构型来实现避障和避奇点的目的。
5.根据以上研究可知，到目前为止，很多文章通过设计开发一种新型的机构来实现机械臂的刚度控制，或者通过改变绳驱机械臂的构型来实现轨迹规划和避障。但是缺少一种新型的可实现绳驱机械臂的变刚度的算法。


技术实现要素：

6.本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的是提供一种绳驱机械臂变刚度规划方法及系统、计算机存储介质，用于实现驱机械臂变刚度规划控制。
7.本发明所采用的技术方案是：一种绳驱机械臂变刚度规划方法，包括以下步骤：
8.建立绳驱机械臂的解析刚度模型；
9.选取绳驱机械臂的末端点和部分中间关节的交点作为被控制点，在被控制点附近自由设定对应的控制点；
10.计算被控制点与控制点之间的欧几里得距离(欧式距离)，通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；
11.实时利用刚度模型计算当前构型的刚度并判断刚度是否满足作业需求，若判断结果为否，通过调节绳索张力进一步优化刚度；
12.若调节绳索张力无法使判断结果为是，则重新选取新的控制点重复上述过程，直至判断结果为是，判断所述绳驱机械臂变刚度规划成功。
13.进一步地，对于被控制点的选择，充分利用绳驱机械臂自由度的冗余性，将每n个自由度之间的关节节点设定为绳驱机械臂的被控点；
14.对于控制点的选择，在经典d-h(denavit-hartenberg)坐标系的yoz平面设置一个半径为r的控制圆，控制圆上的任意一点均可以被选择为控制点。
15.进一步地，通过梯度投影法优化被控制点与控制点之间的欧几里得距离，使其达到最小值的最优状态；
16.判断任意欧式距离是否为零，若判断结果为是，判断绳驱机械臂的构型与控制点设定的期望构型完全一致；
17.判断任意欧式距离是否为零，若判断结果为否，判断当前构型为不影响末端位置和姿态的极限构型。
18.进一步地，将期望构型和给定初始绳索张力带入刚度模型计算当前构型的刚度并判断刚度是否满足作业需求，若判断结果为是，判断所述绳驱机械臂变刚度规划成功，输出对应构型与绳索张力；
19.若判断结果为否，通过调节绳索张力进一步优化刚度，直至判断结果为是，判断所述绳驱机械臂变刚度规划成功；
20.若调整绳索张力无法达到判断结果为是，则重新选取新的控制点重复上述过程，直至判断结果为是，判断所述绳驱机械臂变刚度规划成功。
21.进一步地，通过调整绳索张力实现所述绳驱机械臂的变刚度规划处理，绳索张力等比例增大时，机械臂刚度增加，绳索张力等比例减小时，机械臂刚度减小；通过调整变刚度规划参数实现所述绳驱机械臂的变刚度规划处理。
22.本发明所采用的另一技术方案是：一种绳驱机械臂变刚度规划系统，包括：
23.刚度需求检测单元：用于实时检测所述绳驱机械臂刚度，判断刚度是否满足作业需求，若判断结果为否，则对所述绳驱机械臂进一步实施变刚度规划处理；
24.位置检测单元：用于检测绳驱机械臂当前位置，以计算计算控制点和被控制点之间的欧几里得距离；
25.位置判断单元：用于判断欧式距离是否大于对应误差允许值，判断控制点和被控制点之间的欧式距离是否为零；
26.变刚度规划单元：用于对所述绳驱机械臂进行变刚度规划处理；
27.输入单元：用于输入规划完成的关节构型、绳驱机械臂关节角的角速度、绳索张力到绳驱机械臂，实现绳驱机械臂的运动；
28.规划结果判断单元：用于判断所述绳驱机械臂经过变刚度规划处理后刚度是否符合作业需求，若判断结果为否，上述变刚度处理过程，直至判断结果为是，判断所述绳驱机械臂变刚度规划成功。
29.进一步地，通过调整绳索张力实现所述绳驱机械臂的变刚度规划处理，绳索张力等比例增大时，机械臂刚度增加，绳索张力等比例减小时，机械臂刚度减小；通过调整变刚
度规划参数实现所述绳驱机械臂的变刚度规划处理。
30.本发明所采用的另一技术方案是：一种计算机存储介质，其上存储有计算机程序，所述程序被处理器执行时实现以下步骤：
31.建立绳驱机械臂的解析刚度模型；
32.选取绳驱机械臂的末端点和部分中间关节的交点作为被控制点，在被控制点附近自由设定对应的控制点；
33.计算被控制点与控制点之间的欧几里得距离(欧式距离)，通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；
34.实时利用刚度模型计算当前构型的刚度并判断刚度是否满足作业需求，若判断结果为否，通过调节绳索张力进一步优化刚度；
35.若调节绳索张力无法使判断结果为是，则重新选取新的控制点重复上述过程，直至判断结果为是，判断所述绳驱机械臂变刚度规划成功。
36.本发明的有益效果是：
37.本发明提供了一种绳驱机械臂变刚度规划方法及系统、计算机存储介质，用于控制绳驱机械臂在各种有刚度需求的环境中作业。本发明结合静力学和运动学，建立了绳驱机械臂的解析刚度模型，解决了刚度计算的难题。本发明基于选择的控制点和被控制点，采用梯度投影法优化绳驱机械臂的整体配置，从而获得符合机械臂作业需求的最优刚度所需构型，如果不满足要求，只需通过同时增加或减少绳索张力来进一步优化刚度。同样，也可以根据实际应用的需要选择构型，这样不仅满足了机械臂末端执行器位姿的任务要求，而且还实现了变刚度的功能。绳驱机械臂的变刚度特点可保证它进行低刚度迅速移动和接触，高刚度的带负载动作；在遇到碰撞冲击时，绳驱机械臂可通过低刚度的跟随运动，减少或避免碰撞过程中产生的冲击。上述特点有利于绳驱机械臂完成各种有复杂刚度需求的任务
附图说明
38.图1是本发明中一种绳驱机械臂变刚度规划方法具体实施例d-h坐标系建立方式与结构示意图。
39.图2是本发明中一种绳驱机械臂变刚度规划方法具体实施例原理图。
40.图3是本发明中一种绳驱机械臂变刚度规划方法具体实施例考虑位姿需求的规划方法示意图。
41.图4是本发明中一种绳驱机械臂变刚度规划方法流程图。
具体实施方式
42.为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本技术中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭
露及保护范围。
43.此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“进一步地”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。此外对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。
44.参考图1(a)，首先在本实施例所述的绳驱机械臂上建立关节空间到末端任务空间的运动学模型。图1是本发明中一种绳驱机械臂变刚度规划方法具体实施例d-h坐标系建立方式与结构示意图。在本实施例所述的绳驱机械臂的基础上建立d-h坐标系，相邻两个万向节中心间距为l。由机器人运动学中齐次变换关系(1)可推导出从基坐标系{0}到末端坐标系{n}的变换关系式(2)。
[0045][0046]
式中：
[0047][0048]
参考图1(b)一般情况下，当绳驱机械臂执行空间任务时，关节发生旋转运动。整个系统会受到机械臂末端的外力、驱动绳索上的绳索张力、机械臂自身重力、驱动绳索与绳孔之间的摩擦力。由于绳驱机械臂运动较为缓慢，可将绳驱机械臂系统等效为静态平衡状态。为了简化计算，本发明忽略了摩擦力的影响。根据静力分析(3)可知，每个机械臂关节均有三根绳索控制，绳索分布参考图1(c)。由虚功原理可知系统的静态平衡方程:
[0049]ft
·
δp-f
tt
·
δl-g
t
·
δh＝0
ꢀꢀ
(3)
[0050]
在静态平衡方程中，外力为,f＝[f
1 f2ꢀ…ꢀfn
]
t
，绳索张力为,f
t
＝[f
1 f2ꢀ…ꢀf3m
]
t
，自身重力为,g＝[f
1 f2ꢀ…ꢀfm
]
t
，绳驱机械臂末端位置及方向为p＝[p
x p
y p
z ξ
α ξ
β ξ
γ
]
t
，绳索长度向量为l＝[l
1 l2ꢀ…ꢀ
l
3m
]
t
，每个关节的重心高度为h＝[h
1 h2ꢀ…ꢀhm
]
t
。式中m为机械臂总的万向节数目，n为末端自由度数目。
[0051]
将代入式(3)可得：
[0052]
[0053]
式中每个关节的转动角度为θ＝[θ
1 θ2ꢀ…ꢀ
θ
2m
]，等式两边同时约去δθ并将雅可比矩阵代入(4),得出绳驱机械臂的静力学模型:
[0054]ftjp-f
ttjl-g
t
jh＝0
ꢀꢀ
(5)
[0055]
刚度对于绳驱机械臂来说是一个极其重要的性能指标。机械臂的刚度越大，受到外力时抵抗变形的能力越强，定位精度越高，反之，刚度越小，抵抗变形的能力越差，定位精度越低。为了进行绳驱机械臂的变刚度规划，绳驱机械臂刚度模型需要被准确的建立。本发明在静力学模型的基础上进行了公式推导。通过直接计算和化简公式，得到了绳驱机械臂的解析刚度模型。
[0056]
首先对静力学模型式(5)进行转置运算。然后，对等式两边取θ1×
2m
的偏导，得到式(6)：
[0057][0058]
因为机械臂自身重力为固定常数，所以令
[0059]
原来的式子变为(7)：
[0060][0061]
移项得：
[0062][0063]
由于所以原式变为：
[0064][0065]
根据刚度的定义可知得到刚度模型为：
[0066][0067]
在上式中，j
p
为6
×
2m矩阵，为2m
×
6矩阵，j
l
为3m
×
2m矩阵，为2m
×
3m矩阵，为2m
×
m矩阵，为3m
×
3m的表示绳索自身的刚度对角矩阵。由于
是海森矩阵，计算困难，运算量大，可通过改变运算顺序变为式(11),当f
t
，g，f为已知常数时，先进行分子运算，均可化简为2m
×
1矩阵。然后在对θ求偏导后转换为2m
×
2m矩阵，以上三个矩阵都是2m
×
2m的，可进行连续加法运算。在与j
p
，进行矩阵运算后，最终得到6
×
6的刚度矩阵k。本实施例用刚度矩阵k的主对角线元素来评价整个绳驱机械臂系统的刚度。
[0068][0069]
传统方法中的海森矩阵是可以忽略不计的。所以在式(11)中，可以忽略包含海森矩阵的项对刚度的影响。那么末端位姿的雅可比矩阵j
p
、绳索的雅可比矩阵j
l
、绳索自身的刚度对角矩阵和绳索的绳索张力f
t
将对绳驱机械臂的刚度具有重要的影响作用。由此可见，绳驱机械臂的构型、驱动绳索的刚度和绳索所受的绳索张力是决定绳驱机械臂刚度的重要因素。
[0070][0071]
根据矩阵的运算规则可知，式(11)得到6
×
6的刚度矩阵k每一行都与j
p
相关。j
p
展开式如(12)所示，由此可得到式(13)。
[0072][0073]
在绳驱机械臂执行任务时，通常给定绳驱机械臂末端的位置和姿态p。然后，进行末端笛卡尔空间到关节空间的逆运动学计算。通常逆运动学解得到的关节角度θ具有多解性。因此j
p
具有多种情况，即绳驱机械臂的刚度在相同的位置和姿态下，但构型不同时，绳驱机械臂的刚度也不同。
[0074]
考虑整体构型和绳索张力的绳驱机械臂的变刚度规划方法首先需要确定控制点和被控制点。根据图1(a)中建立的d-h坐标系可知，{i}坐标系位于万向节中心。为了方便表示和计算，本发明将关节中心设置为刚度被控制点。由正运动学方程可知，在笛卡尔坐标系下的刚度被控制点0bi可表示为：
[0075]0bi＝0ti(1:3；4)＝pi＝[xi,yi,zi]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(14)
[0076]
参考图2，图2是本发明中一种绳驱机械臂变刚度规划方法具体实施例原理图。本发明在坐标系{i}的yioizi平面内设置一个半径为r的控制圆。控制圆上的任意一点可以被选择为控制点
ici
。通过正运动学方程可知，在笛卡尔坐标系下的控制点0ci的坐标矩阵可表示为：
[0077]0ci＝0ti·
[0,rsin(αi),rcos(αi),1]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(15)由上述(14)和(15)可知，在笛卡尔坐标系下的欧氏距离可表示为：
[0078]di
＝|0c
i-0bi
|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0079]
绳驱机械臂多为冗余自由度的(自由度》6)。在进行绳驱机械臂的逆运动学计算时，运算结果具有多解性。在相同的末端位置和姿态下，绳驱机械臂有多种空间构型。因此为绳驱机械臂的变刚度规划提供了多种的空间构型选择。本发明采用梯度投影法优化控制点和被控制点之间的欧氏距离以改变绳驱机械臂的空间构型。并在多个空间构型中选取最适合任务需求的空间构型作为最优构型。
[0080]
在雅可比矩阵行满秩的条件下，绳驱机械臂的逆运动学通解可表示为：
[0081][0082]
首先将梯度投影法中的距离性能指标函数h(θ)表示为式(18)，
[0083][0084]
式中si为安全阈值。
[0085]
则其梯度向量可表示为:
[0086]
[0087]
本发明采用梯度投影法，通过改变关节速度矢量的取值来实现距离性能指标的优化。可表示为:
[0088][0089]
结合式(17)和(20)，关节速度矢量与距离性能指标函数h(q)的关系如下所示。
[0090][0091]
式中是绳驱机械臂在关节空间的速度矢量，是绳驱机械臂在笛卡尔空间的速度矢量，j是绳驱机械臂的末端位姿雅可比矩阵，j
+
雅可比矩阵的伪逆，k
p
是比例因子，i是单位矩阵。是方程(21)中由伪逆法得到的最小范数解，l是方程(21)的齐次解。然而方程的齐次解是不影响绳驱机械臂末端的位置和姿态的，与无关，只与有关。最小范数解决定着绳驱机械臂整体的运动趋势，采用梯度投影法的齐次解决定着绳驱机械臂的空间构型变化趋势。因此本发明可通过改变绳驱机械臂的距离性能指标函数h(θ)来改变空间构型，最终达到优化绳驱机械臂刚度的目的。该过程而不影响末端的位置和速度。
[0092]
比例因子k
p
在方程(21)中具有重要作用，它决定着整体运动趋势和空间构型变化趋势的在最终运动规划中所占的比重。当k
p
趋近于0时，无构型优化，仅由伪逆法得到的最小范数解起作用。我们是无法控制绳驱机械臂的构型和刚度的。当k
p
过大时，绳驱机械臂整体的运动趋势影响变小，极易产生末端定位误差。并且可能会产生不合理的关节空间角度，影响整个绳驱机械臂的主体运动。因此，我们需要设置k
p
的参数值来提高绳驱机械臂的刚度。
[0093]
参考图3，图3是本发明中一种绳驱机械臂变刚度规划方法具体实施例考虑位姿需求的规划方法示意图。本发明中绳驱机械臂变刚度规划方法是可根据任务要求灵活设定的。在绳驱机械臂有特殊末端位姿与构型需求时，可通过该规划方法选择合适的构型。既能满足机械臂的位姿需求，又能实现低刚度的灵活运动、高刚度的带负载运动刚度需求。
[0094]
参考图4，图4是本发明中一种绳驱机械臂变刚度规划方法流程图。变刚度规划方法可以通过调整整臂构型和绳索张力，实现对绳驱机械臂的变刚度控制，给定条件有目标点、期望刚度、初始构型和绳索张力。进一步地，选择控制点和被控制点，然后用梯度投影法优化选定的控制点和被控制点之间的距离，以找到最合适的刚度构型。如果满足误差允许的条件，则计算当前刚度，并判断是否满足作业所需刚度需求。如果刚度不满足需求，则通过调整绳索张力来进一步优化刚度。如果通过调整绳索张力无法使刚度满足要求，则通过选取新的控制点来执行新一轮的刚度优化。一旦确定了整臂构型和绳索张力，就可以计算绳驱机械臂的刚度。
[0095]
通过重复上述过程，可以计算选择出最合适的刚度，达到变刚度控制的目的。类似地，也可以根据实际应用选择适当的构型来实现可变刚度控制，不仅能满足末端执行器的位姿任务要求，还能实现变刚度功能，从而实现变刚度规划的目的。
[0096]
基于以上方法，本发明还提供一种绳驱机械臂变刚度规划系统，包括：
[0097]
刚度需求检测单元：用于实时检测所述绳驱机械臂刚度，判断刚度是否满足作业需求，若判断结果为否，则对所述绳驱机械臂进一步实施变刚度规划处理；
[0098]
位置检测单元：用于检测绳驱机械臂当前位置，以计算控制点和被控制点之间的
欧几里得距离；
[0099]
位置判断单元：用于判断欧式距离是否大于对应误差允许值，判断控制点和被控制点之间的欧式距离是否为零；
[0100]
变刚度规划单元：用于对所述绳驱机械臂进行变刚度规划处理；
[0101]
输入单元：用于输入规划完成的关节构型、绳驱机械臂关节角的角速度、绳索张力到绳驱机械臂，实现绳驱机械臂的运动；
[0102]
规划结果判断单元：用于判断所述绳驱机械臂经过变刚度规划处理后刚度是否符合作业需求，若判断结果为否，上述变刚度处理过程，直至判断结果为是，判断所述绳驱机械臂变刚度规划成功。
[0103]
其中，可通过调整绳索张力和调整变刚度规划参数实现所述绳驱机械臂的变刚度规划处理，绳索张力等比例增大时，机械臂刚度增加，绳索张力等比例减小时，机械臂刚度减小。一种绳驱机械臂变刚度规划系统的具体工作过程描述可参照上述一种绳驱机械臂变刚度规划方法的描述，在此不再赘述。
[0104]
另外，本发明还提供一种计算机存储介质，其上存储有计算机程序，程序被处理器执行时实现以下步骤：
[0105]
建立绳驱机械臂的解析刚度模型；
[0106]
选取绳驱机械臂的末端点和部分中间的交点作为被控制点，在被控制点附近自由设定对应的控制点；
[0107]
计算被控制点与控制点之间的欧几里得距离(欧式距离)，通过基于梯度投影法的全局优化使得欧式距离逼近零，从而得到绳驱机械臂在空间的期望构型；
[0108]
实时利用刚度模型计算当前构型的刚度并判断刚度是否满足作业需求，若判断结果为否，通过调节绳索张力进一步优化刚度；
[0109]
若调节绳索张力无法使判断结果为是，则重新选取新的控制点重复上述过程，直至判断结果为是，说明所述绳驱机械臂变刚度规划成功。
[0110]
计算机存储介质上存储的计算机程序的工作过程可参照上述一种绳驱机械臂变刚度规划方法的具体描述，不再赘述。
[0111]
同时，本发明的方法除了对绳驱机械臂适用外，也适用于其余多种超冗余机械臂。
[0112]
以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本技术权利要求所限定的范围内。