一种基于能量罐的机械臂分层变阻抗控制方法

本发明涉及机械臂控制，尤其涉及一种基于能量罐的机械臂分层变阻抗控制方法。

背景技术：

1、机械臂包括协作机械臂，作为与人类并肩工作的创新工具，其新一代设计亮点在于采用了轻量级结构并集成了7个自由度。这一革新不仅极大地提升了机械臂的灵活度与操作精度，更为其同时胜任多任务处理构筑了坚实的硬件基石，预示着协作机械臂应用领域与能力的广泛拓展与深化。

2、在控制方法层面，变阻抗控制作为一种先进的机器人控制方法，其核心在于模拟质量-弹簧-阻尼二阶系统的动态特性，使机器人能够智能调整其对外界力或力矩的响应参数。此方法赋予了机器人高度的环境适应性，尤其是在协作场景下执行精细作业时，展现出无与伦比的灵活性。医疗手术协作便是其应用的典范，通过力位混合控制方法，机械臂能根据预设的阻抗控制律及实时感知到的外力，精准执行如切割、穿刺等精细手术操作，而这一切均在操作员的远程精细控制下完成，实现了人机协作的新高度。

3、但是，在机械臂运用阻抗控制器执行精细操作或应对复杂多变的环境时，若其无法实时根据外部环境动态调整各关节的阻抗参数，控制器将不可避免地产生难以预测和控制的控制力矩。这一状况将严重削弱机械臂的操作可靠性和安全性，增加作业风险。

4、尽管通过离线调整阻抗参数能在一定程度上提升机械臂的环境适应性，但在面对复杂、非结构化的工作环境时，频繁进行离线参数调整显然不够灵活和高效，甚至可能因延误调整时机而导致操作失败或事故。

5、因此，为了提升机械臂在复杂环境中的适应性和操作稳定性，亟需一种能够实时感知环境变化并自动调整阻抗参数的智能控制方法。

技术实现思路

1、本发明的目的在于提出一种基于能量罐的机械臂分层变阻抗控制方法，确保机械臂在执行细腻操作或面对突发情况时，能够迅速而准确地调整其动力学特性，从而保持操作过程的安全性和可靠性。

2、为达此目的，本发明采用以下技术方案：

3、一种基于能量罐的机械臂分层变阻抗控制方法，包括以下步骤：

4、s1、通过关节传感器获得机器人机械臂的关节信息，其中关节信息包括关节角度位置信息、关节角速度信息和机械臂末端姿态信息；

5、s2、根据获取的关节信息建立机械臂的动力学模型；

6、s3、定义任务空间速度坐标，并计算出动力学模型中雅可比矩阵的零空间基矩阵，再根据定义的任务空间速度坐标以及零空间基矩阵将机械臂的动力学模型扩展为机械臂的分层控制模型；

7、s4、在分层控制模型的各个工作空间中设置作为储能元件的能量罐，同时通过能量罐的状态更新率引入机械臂在分层控制模型的各个工作空间的变阻抗特性；

8、s5、利用能量罐得到的变阻抗特性将机械臂的分层控制模型扩展为机械臂的分层变阻抗控制模型；

9、s6、通过关节传感器获得的机械臂末端姿态信息和预设的期望机械臂末端姿态信息计算出姿态误差，将姿态误差代入分层变阻抗控制模型计算出各个任务所需的控制力矩，输入到机器人机械臂实现对机械臂的控制。

10、优选的，在s2中，所述根据获取的关节信息建立机械臂的动力学模型，具体包括以下步骤：

11、s21、基于笛卡尔空间方程，建立具有n个自由度的机械臂的动力学模型：

12、

13、λ(ξ)＝j(q)-tm(q)j(q)-1     (2)

14、

15、其中，q和分别表示为关节传感器获得机械臂关节空间坐标位置和坐标速度，ξ、和分别表示为关节传感器获得的机械臂末端笛卡尔空间坐标位置、速度和加速度；λ(ξ)和是相对于坐标位置ξ与坐标速度的惯性矩阵和离心力矩阵，由式(2)和式(3)表示；fg(ξ)＝j(q)-tg(q)表示为机械臂笛卡尔空间重力，j(q)∈rm×n与分别表示为机械臂雅可比矩阵和雅可比矩阵的一阶导，g(q)∈rn×1表示为机械臂关节空间的重力项；fτ表示为模型的输入向量，fext表示为作用到机械臂末端执行器的外力，m(q)∈rn×n表示为机械臂关节空间中的惯量矩阵，表示为机械臂关节空间的科氏力矩阵；s22、令笛卡尔空间坐标速度与机械臂关节空间坐标速度满足以下关系：

16、

17、其中，j(q)∈rm×n为机械臂雅可比矩阵，m表示为末端执行器任务空间的维度，m表示为6，n为机械臂关节数；对于冗余机械臂，n大于6；

18、s23、令关节输入力矩τ与输入向量fτ满足以下关系：

19、τ＝j(q)tfτ       (5)

20、s24、令作用到机械臂末端执行器的外力fext映射到机械臂关节空间坐标q：

21、τext＝jt(q)fext       (6)

22、其中τext为机械臂关节上受到的外力；

23、s25、通过机器人正运动学方程，引入子任务的坐标向量：

24、

25、其中，r代表子任务的个数，每个子任务的维度为i＝1是最高优先级任务，i＝r是最低优先级任务，对于任意两个任务ia和ib，如果ia<ib，则表示ia比ib的任务优先级高。

26、优选的，在s3中，所述定义任务空间速度坐标，并计算出动力学模型中雅可比矩阵的零空间基矩阵，再根据定义的任务空间速度坐标以及零空间基矩阵将机械臂的动力学模型扩展为机械臂的分层控制模型，具体包括以下步骤：

27、s31、利用每个子任务的雅克比矩阵完成从关节空间速度到任务空间速度的映射：

28、

29、s32、由于式(8)描述的各个子任务的关节空间速度相互耦合，因此定义一种任务空间速度坐标vi表示为：

30、

31、其中，是待确定的扩展雅可比矩阵；

32、s33、通过选择使其是非奇异的以及保证零空间投影的动态一致性，被选择为：

33、

34、其中，最高优先级任务保持与原雅克比矩阵一致，为使符号的统一，令而剩余的低优先级任务和增广矩阵的一个行满秩的零空间基矩阵zi和机器人的惯性矩阵相关；

35、增广矩阵表示为：

36、

37、s34、为了确保低优先级任务不对高优先级任务造成影响，令零空间基矩阵zi表示为：

38、

39、其中，m+表示为机械臂惯性矩阵加权的伪逆，表示为的标准正交基，通过对进行奇异值分解获得；表示为最高优先级任务雅克比矩阵j1的惯性加权伪逆，表示为最低优先级任务利用机械臂所有剩余的自由度；

40、s35、由式(8)和式(11)得到扩展雅克比矩阵的逆矩阵：

41、

42、s36、将式(14)代入式(9)得到：

43、

44、s36、将式(15)代入式(8)得到各个子任务空间的任务空间速度：

45、

46、s37、将式(13)至式(16)代入式(1)至式(3)得到机械臂的分层控制模型：

47、

48、其中，λ表示为对角矩阵，表示为任务空间加速度坐标。

49、优选的，在s4中，所述在分层控制模型的各个工作空间中设置作为储能元件的能量罐，同时通过能量罐的状态更新率引入机械臂在分层控制模型的各个工作空间的变阻抗特性，具体包括以下步骤：

50、s41、将能量罐的储能函数表示为：

51、

52、s42、将能量罐的状态更新率设计为：

53、

54、其中，∈∈r+表示为能量罐设定的最低能量阈值，当能量罐的能量低于设定的最低能量阈值，能量罐就不能为机械臂的变刚度行为提供能量，σ∈[0,1]表示为调节能量罐的阻尼项从机械臂提取能量效率的参数，βi>0表示为一个常数，表示为能量罐储能的上界，表示为实际空间速度与目标空间坐标速度的差值，ωi是机械臂的变刚度方法，kic为固定刚度，di为固定阻尼，kit为可变刚度。

55、优选的，在s5中，所述利用能量罐得到的变阻抗特性将机械臂的分层控制模型扩展为机械臂的分层变阻抗控制模型，具体包括以下步骤：

56、s51、设计基于能量罐的机械臂分层变阻抗控制器，令总体控制器结构表示为：

57、

58、其中，τi表示为各个子任务的控制器，τμ表示为抵消科里奥利或者离心力做出的补偿，g表示为重力补偿；

59、s52、令所有子任务的控制器τi的控制输入都具有如下形式：

60、

61、其中，第i个任务中机械臂末端位置误差ξi表示为第i个任务中机械臂末端实际的位置，ξi,d表示为第i个任务中机械臂末端期望的位置；第i个任务中机械臂末端速度误差表示为第i个任务中机械臂末端实际的速度，是第i个任务中机械臂末端期望的速度；ωixti用于将变刚度方法增加到控制输入，由式(23)表示；

62、s53、通过设计式(25)中τμ反馈补偿μ中的非块对角线项：

63、

64、其中，表示为位于μ中i行j列维度为mi×mj的矩阵块；

65、s54、令μi,j＝-μj,it，则式(27)是一个守恒的反馈，即传输功率不会主动产生能量；

66、s55、由于雅克比矩阵是可逆的，则机械臂关节上受到的外力τext与各分层变阻抗控制模型中机械臂末端执行器的外力之间的关系表示为：

67、

68、s56、将式(25)至式(28)代入式(17)，得到解耦的闭环动力学方程：

69、

70、s57、对于最高优先级任务(i＝1)，由式(11)和式(13)可得因此最高优先级闭环动力学方程表示为：

71、

72、s58、由式(13)和式(16)可得为证明整个闭环系统的无源性,先证明最高优先级任务(i＝1)的无源性，选取描述最高优先级闭环系统的能量函数表示为：

73、

74、s59、对式(31)求导可得：

75、

76、s510、将能量罐通过保电互联与最高优先级闭环系统相连，选取一个总储能函数w1表示为：

77、w1＝v1+t1       (33)

78、s511、对式(33)求导并带入式(22)和式(32)可令最高优先级闭环系统满足无源性条件：

79、

80、由于σ1∈[0,1]，有：

81、

82、因此有：

83、s512、对于次优先级任务(i>1)的闭环系统，选取储能函数vi为：

84、

85、s513、将能量罐通过保电互联与次优先级闭环系统相连，选取一个总储能函数wi表示为：

86、wi＝vi+ti        (38)

87、s514、对式(38)求导并带入式(22)和式(38)可令次优先级闭环系统满足无源性条件：

88、

89、由于σi∈[0,1]，有：

90、

91、因此有：

92、s515、对于整个机器人机械臂系统，选取总储能函数w为：

93、

94、s516、将式(9)、式(28)、式(36)和式(41)代入式(42)可令机器人闭环系统满足无源性条件：

95、

96、上述技术方案中的一个技术方案具有以下有益效果：将机械臂的变阻抗特性拓展至多任务控制领域，通过以下步骤实现：首先，利用关节传感器实时采集关节状态信息，构建精准的动力学模型，并计算雅可比矩阵的零空间，为后续控制提供理论基础。其次，在各工作空间内引入创新的储能元件——能量罐，其能够智能地吸收与释放能量，从而增强机械臂在执行变阻抗任务时的稳定性与鲁棒性。最后，结合机械臂的位置误差信息与分层变阻抗模型，精确计算出各关节所需的控制力矩，并实时反馈至机械臂执行机构，实现高效、稳定的控制。