一种基于力反馈的多机械臂保柔性性能安全协同控制方法

摘要: 本发明属于新一代信息,具体涉及基于导纳控制、全驱系统参数化、人工势场、柔性预设性能、滑模控制复合方法的多机械臂保性能安全协同跟踪控制问题。背景技术:1、近年来,...
一种基于力反馈的多机械臂保柔性性能安全协同控制方法

本发明属于新一代信息,具体涉及基于导纳控制、全驱系统参数化、人工势场、柔性预设性能、滑模控制复合方法的多机械臂保性能安全协同跟踪控制问题。

背景技术:

1、近年来,随着养老助残、医疗护理、柔性装配等服务机器人和工业机器人的需求不断增长,基于力反馈传感器的多机械臂保柔性性能机器人凭借其高柔顺性、高安全性、更大的工作空间和更高的负载能力得到广泛关注。然而,多机械臂系统本质上是一个多输入多输出、高度非线性、强耦合的时变动力学系统。在实际作业中,各机械臂不仅要克服重力、科里奥利力及关节摩擦等内部非线性因素,还必须通过分布式误差耦合与相邻机械臂保持精确的相对位形约束。

2、为了保证人体或工件与多机械臂机器人接触过程中的舒适性和柔顺性,在多机械臂交互作业控制时,常采用基于力反馈传感器信号的导纳控制、力位协同控制等策略。专利cn119200628a公开了一种基于多智能体强化学习的双臂机器人协同控制方法及装置,通过建立双机械臂协同装配模型进行训练,并调整导纳控制参数来保证机械臂的柔顺性,该专利采用的强化学习方法可以处理多源干扰和不确定性,但是需要海量的交互数据,直接在真实物理环境上训练易造成机械损伤,研发周期长,也没有考虑如何避开障碍物和保证控制性能。专利cn122667400a公开了基于多模态与力位协同控制的传送带码垛系统及控制方法,通过深度相机和六维力传感器多模态识别目标位置和挤压力,生成无碰撞接近路径与推挤位姿,托板及舌板对接至待码放位置,控制机械臂完成码垛作业任务,但其仅靠传感器信号来减少单个机械臂对货物造成的过度挤压和损坏,未考虑多个机械臂如何协同控制的问题。本发明也采用导纳控制保证了多机械臂协同交互式作业的柔顺性,另外,设计人工势场避免碰撞,还结合全驱系统方法消除非线性耦合对滑模动态的干扰,并设计柔性性能函数,实现每个机械臂安全避撞的同时保证机械臂协同跟踪精度,工程实现简单,适合安全性和控制精度要求严苛、研发周期短的应用场景。

3、目前,由于交互接触和未知障碍物等对多机械臂协同作业的影响巨大,许多国内外学者已经开展了此类问题的深入研究。研究表明,基于力反馈的导纳控制可以很好地实现柔顺式交互接触,而人工势场设计则可保证多机械臂系统在工作范围内的安全性和稳定性。在多机械臂交互式协同控制中,使用柔性预设性能函数可以在避碰的同时保证闭环系统控制性能,是国内外研究的重点之一。值得一提的是,这种方法在基于多机械臂协同控制的公开资料及其他文献中并未见有详细的报道。

技术实现思路

1、鉴于上述现有技术中的不足,本发明提出一种基于导纳控制、全驱系统参数化、人工势场、柔性预设性能、滑模控制复合的多机械臂保性能安全协同跟踪控制方法,主要由以下步骤组成:

2、步骤1、建立多机械臂系统的运动学和动力学模型:

3、首先,鉴于六自由度机械臂的结构与复杂程度, 采用连杆坐标系参数法对其进行运动学建模。

4、由连杆坐标系参数可以得到连杆坐标系相对于坐标系的变换矩阵,表示机械臂的关节序号:

5、

6、其中,为连杆长度;为连杆扭角;为关节偏距;为关节转角,将机械臂的各连杆参数代入上式得:

7、

8、

9、

10、然后,通过上式的连乘可以得到总的变换矩阵:

11、

12、最后,利用拉格朗日方程对关节进行动力学分析, 简化后的第个机械臂关节动力学方程为:

13、

14、其中,、、 分别为第个机械臂关节的位置、速度和加速度向量,表示机械臂的数量;为对称正定的惯性矩阵;为科氏力和离心力矩阵; 为重力向量;为关节摩擦力向量,采用包含粘性与平滑库仑摩擦的模型计算;为关节控制力矩向量,为作用于内部关节空间的干扰力矩向量。

15、

16、其中,为粘性摩擦系数,为库仑摩擦幅值,为摩擦平滑系数。

17、步骤2、设计基于力反馈的导纳控制实现柔顺式交互接触:

18、首先,设计基于力触觉传感器反馈信号的导纳控制层:

19、为了实现多机械臂与外部环境交互过程中的柔顺运动,引入导纳控制层。对于第个机械臂,导纳微分方程定义为:

20、

21、其中,第个机械臂导纳虚拟位移误差,为导纳修正后的末端目标位置,为原始期望末端位置,为导纳虚拟速度向量,为导纳虚拟加速度向量,、和分别为正定的导纳虚拟质量矩阵、虚拟阻尼矩阵和虚拟刚度矩阵;为柔性力反馈传感器测得的外部接触力,为期望接触力;

22、导纳层根据外部接触力与期望接触力之间的偏差对目标轨迹进行修正:

23、

24、在导纳修正轨迹的基础上进一步叠加人工势场避障位移,获得同时满足接触柔顺性和障碍物安全约束的末端安全参考轨迹。

25、步骤3、设计基于力反馈的人工势场实现柔顺式安全避障:

26、定义第个机械臂的三维末端安全参考位置为,其中t表示向量的转置,即为向量x的转置向量,定义任务空间到平面的投影矩阵为:

27、

28、设第个机械臂的安全参考位置在平面内的投影为,球形障碍物中心在平面内的位置为,则安全参考轨迹与障碍物中心之间的距离为:

29、

30、设为人工势场的内边界距离,为人工势场的作用范围,满足。

31、为避免人工势场在作用范围边界处产生突变,构造连续可微的第个机械臂排斥势能函数:

32、

33、其中,为人工势场增益,人工势场在平面内产生的排斥力为:,即:

34、

35、将其扩展为三维任务空间排斥力:

36、

37、其中为转置矩阵,为了避免将排斥力直接作为突变关节力矩输入,设置柔顺式避障位移误差,并构造第个机械臂柔顺式避障动态方程:

38、

39、其中,和分别为柔顺式避障位移误差的速度和加速度,、和分别为正定的避障虚拟质量矩阵、避障虚拟阻尼矩阵和避障虚拟刚度矩阵。

40、第个机械臂最终安全参考轨迹定义为:

41、

42、其安全参考加速度为:

43、

44、其中,为导纳修正后的末端目标加速度,为柔顺式避障产生的加速度修正量,的逆矩阵。

45、步骤4、设计保柔性预设性能约束函数过程如下:

46、首先,定义第个机械臂相对于安全参考轨迹的位置跟踪误差为,为第个机械臂的实际末端位置,定义任务空间分布式安全协同跟踪误差为:

47、

48、其中,,第 j 个机械臂相对于安全参考轨迹的位置跟踪误差为表示机械臂的数量,为第个机械臂的邻居集合,表示第j个机械臂到第i个机械臂的通信连接权重,存在通信连接时,,为轨迹跟踪权重。

49、然后,设计第个机械臂柔性预设性能函数:

50、

51、其中,为初始性能系数,为性能系数,且,为性能边界收缩速率参数,为预设性能收敛时间,为时间。

52、再后,定义第个机械臂相对于安全参考轨迹的基础性能边界:

53、

54、其中,为第个任务空间方向的性能边界尺度参数,,控制目标为:

55、

56、其中,为在方向的分量;

57、为了表示机械臂相对于原始期望轨迹的偏移,定义第个机械臂原始轨迹跟踪误差为:

58、

59、其中,;

60、其次,定义第个机械臂安全参考轨迹相对于原始期望轨迹的柔性偏移量:

61、

62、由此得到:

63、

64、定义第个机械臂柔性性能边界,其中为在方向的分量,从而有。

65、最后,完成安全协同跟踪误差的barrier函数变换:

66、定义第个机械臂归一化误差:

67、

68、其中,;

69、定义第个机械臂barrier函数变换误差:

70、;

71、对barrier函数变换误差求导可得

72、

73、其中,,增益项,偏移项。

74、定义,,则。

75、步骤5、设计基于全驱系统参数化的滑模复合控制器:

76、首先,构造第个机械臂滑模变量:

77、

78、其中,为滑模面斜率矩阵,对滑模变量求导:

79、

80、其中为变换误差的二阶导数,为中间变量,,一阶和二阶导数,;

81、第个机械臂末端运动学关系为:

82、

83、

84、其中为位置雅可比矩阵,为位置雅可比矩阵的时间导数;

85、第个机械臂的安全协同跟踪耦合系数,对安全协同跟踪误差求二阶导数,得到,可简记为,,其中为第j个机械臂末端的实际加速度,为第j个机械臂的安全参考加速度,且;

86、将上述关系代入滑模变量导数可得:

87、

88、然后,设计滑模趋近律:

89、为抑制高频抖振,采用双曲正切函数代替符号函数构造指数趋近律,设计第个机械臂趋近律如下:

90、

91、其中为趋近律增益,为鲁棒切换增益,为边界层厚度;

92、最后,设计全驱系统参数化与滑模变量复合控制律:

93、假设位置雅可比矩阵在工作空间内满行秩,定义其右伪逆为,满足,为单位矩阵,设计第个机械臂关节伪加速度控制量设计为:

94、

95、根据步骤1的机械臂动力学模型,设计关节力矩控制律:

96、

97、将控制律代入机械臂实际动力学方程,得到:

98、

99、再代入滑模变量导数,得到:

100、

101、其中为变换后的集总扰动;

102、从而第个机械臂闭环滑模动态为:

103、。

104、步骤6、分析证明个机械臂闭环控制系统稳定性:

105、首先,定义李雅普诺夫函数,,:

106、

107、然后,对求导得:

108、

109、

110、由于为对称矩阵,得:

111、

112、假设排斥力有界,为一正常数,表示排斥力上界,则得到:

113、

114、其中,为的最小特征值;

115、令,得:

116、

117、证明虚拟误差 在稳态下收敛到一个极小有界区域,是最终一致有界稳定的;

118、再后,对求导得:

119、

120、将其展开:

121、

122、对于任意变量,满足,改写为,从而有:

123、

124、假设扰动上界满足,则,合并得到:

125、

126、

127、其中,为负值,设常数,,最终得到:

128、

129、在有界集总扰动和有限边界层厚度条件下,滑模变量、变换误差和安全协同跟踪误差均最终一致有界稳定;当集总扰动趋于零时,滑模变量和安全协同跟踪误差进一步渐近收敛;证明了内环跟踪误差和是最终一致有界稳定的;

130、最后,对求导得:

131、

132、其中,得到:

133、

134、在实际接触作业中,由于传感器测得的实际外部接触力有界,与恒定的期望接触力之差构成有界的广义作用力,即满足,其中为一正常数,则,得到:

135、

136、其中,为的最小特征值;

137、令,得:

138、

139、由此可得当时,恒有,证明虚拟误差 在稳态下收敛到一个极小有界区域;综上可知中所有误差变量最终一致有界稳定,因此上述多机械臂闭环控制系统是最终一致有界稳定的。

140、与已有技术相比,本发明的有益效果体现在以下方面:

141、(1) 根据本发明提出的一种基于力反馈的多机械臂保柔性性能安全协同控制方法具有跟踪速度快、控制精度高等优点,并保证了多机械臂在面对和包括人在内的外部环境交互时柔顺式的交互能力;

142、(2) 根据本发明提出的一种基于力反馈的多机械臂保柔性性能安全协同控制方法考虑了未知障碍物的不利影响,可以在保证柔顺式交互接触基础上安全避障;

143、(3) 根据本发明提出的一种基于力反馈的多机械臂保柔性性能安全协同控制方法够通过设计分布式柔性预设性能函数,成功实现多机械臂安全避障和交互式协同作业时跟踪误差保持在预设范围内。