基于柔性液压机械臂雅可比矩阵的末端力软测量方法

本发明涉及柔性液压机械臂控制，具体涉及基于柔性液压机械臂雅可比矩阵的末端力软测量方法。

背景技术：

1、目前柔性液压机械臂已被广泛应用于大飞机清洗和隧道凿岩等接触性作业，面对多变的非结构化环境，以及柔性液压机械臂自身的惯量大，经常性接触容易造成末端力传感器的损坏，对末端力的测量造成影响。

2、目前已经有基于参数辨识的液压机械臂末端力软测量方法，例如专利公开号为cn113977578a，名为一种液压机械臂末端力软测量方法，为本团队2021年10月26日申请且以获得授权的专利，但此种末端力软测量方法只适用于刚性液压机械臂，柔性液压机械臂在运行过程中因为自身臂架结构的变形特性，动态特性难以描述，导致液压机械臂动力学参数存在不准确的问题，因此对于存在臂杆变形的长柔性多关节液压机械臂并不适用，未考虑臂杆变形对机械臂末端力软测量的影响。当柔性液压机械臂末端受力时，可将其等效为液压缸的受力，由液压缸驱动力矩克服末端负载力，故可利用雅克比矩阵将末端力分解到各关节上。但因为臂杆柔性产生的挠度变形会对转换的雅可比矩阵产生影响，直接用未考虑挠度变形的刚性雅可比矩阵会导致末端力计算结果不准。因此，考虑柔性臂杆变形特性，提出一种根据各臂杆挠度变形求解柔性臂雅可比矩阵的方法。基于柔性臂雅可比矩阵，提出基于柔性液压机械臂雅可比矩阵的末端力软测量方法，可以解决因臂杆变形造成末端力估计不准的问题。

技术实现思路

1、为此，本发明提供基于柔性液压机械臂雅可比矩阵的末端力软测量方法，通过建立柔性臂动力学模型，关节摩擦采用非线性关节摩擦模型，进而建立柔性臂最小惯性参数集模型。利用有限傅里叶级数求解激励轨迹，在激励轨迹下利用最小惯性参数集模型求解柔性臂动力学参数，通过倾角传感器计算柔性臂杆挠度变形解析式，根据柔性臂运动学特性，求解出含有臂杆末端挠度的柔性臂雅可比矩阵，进而计算外部作用力。

2、本发明采用的技术方案是：基于柔性液压机械臂雅可比矩阵的末端力软测量方法，包括以下步骤：

3、步骤1，建立柔性液压机械臂动力学模型，针对柔性液压机械臂运动范围广、关节摩擦不精确等问题，建立非线性关节摩擦力矩模型，根据柔性臂动力学模型确定三关节柔性臂液压驱动力矩与广义力之间的关系，并将动力学模型线性化处理，建立柔性臂最小惯性参数集及其对应的回归矩阵的线性模型；

4、步骤2，利用有限傅里叶级数设计激励轨迹，以步骤1的回归矩阵条件数最小为目标，求解有限傅里叶级数系数，生成激励轨迹；

5、步骤3，柔性臂在无负载条件下，运行一次激励轨迹，采集液压缸两腔压力传感器数值，并通过液压缸两腔压力计算液压驱动力矩τ；将各时刻的广义模态角度(含关节角度与模态参数)、广义角速度、广义角加速度带入回归矩阵y中，并合并成一个总矩阵；

6、步骤4，根据步骤3得到的液压驱动力矩τ和回归矩阵y计算柔性臂动力学参数；

7、步骤5，通过安装在臂段三处的倾角传感器以及臂杆首端(靠近基座端点)倾角传感器共4个倾角传感器测得数据，求解出臂杆末端挠度变形解析式；

8、步骤6，通过分析三自由度柔性臂运动学特性，求解柔性臂末端位置，通过末端位置对关节角度求偏导以及结合步骤5中求解出来的臂杆末端挠度变形解析式可求解出含有挠度变形的柔性臂雅可比矩阵；

9、步骤7，将步骤4中所得的动力学参数，带入到步骤1的线性模型中，结合步骤6中柔性臂雅可比矩阵计算柔性液压机械臂末端力大小；

10、进一步地，步骤1中所建立的柔性液压机械臂动力学模型具体如下：

11、

12、其中，p为柔性臂广义角度，p＝[θ q]t，θ为关节角度，q为模态坐标，为广义角速度，为广义角加速度，τ为液压驱动力矩，m(p)为柔性臂惯性矩阵，为柔性臂科氏力和向心力矩阵，k为柔性臂刚度矩阵，g为柔性臂重力矩阵，τf为非线性关节摩擦力矩。

13、进一步地，步骤1中所建立的非线性关节摩擦力矩模型为：

14、

15、其中，k1、k2、k3、k4是傅里叶级数的系数，k5、k6、k7、k8是三阶多项式的系数，为待辨识参数。

16、进一步地，步骤1中所述的柔性臂液压驱动力矩与柔性臂所受的广义力之间的关系为：

17、qi＝τi-τi+1

18、其中，qi为柔性臂所受的广义力，下标i为广义力序号，τi为柔性臂所受的关节力矩，下标i为关节序号。

19、进一步地，步骤1中所述的柔性臂最小惯性参数集线性模型具体如下：

20、

21、其中，表示回归矩阵，l表示最小惯性参数集。

22、进一步地，步骤2中所述的有限傅里叶级数表达式如下：

23、

24、对于第i个关节，正弦项和余弦项的数目均为n，t表示激励轨迹的运行时间，ωf＝2πff为基频，ff＝1/t，al，i、bl，i、θi0为待求傅里叶系数，用含约束条件的多元函数求最优的方法求解。为了保证机器人平稳且在能够到达的安全范围之内运行，约束条件如下：

25、

26、其中，condmin(y)表示以回归矩阵y的条件数最小为优化目标，θ(t)表示t时刻的柔性臂关节角度；θ0为柔性臂初始关节角，当一个周期结束时，t＝tf，tf表示终止时刻，柔性臂回到柔性臂初始关节角θ0，从而能够连续执行下一周期，初始t＝0和结束t＝tf的速度和加速度设为0，有效避免冲击；θmin、θmax、分别为机械臂运动时角度、角速度和角加速度的最小值和最大值；通过限定角度、角速度、角加速度上下限，保证柔性臂在安全的范围内运动。

27、进一步地，步骤3中所述的回归矩阵y合并的矩阵，是先计算出各个时刻下的回归矩阵yk，k为时间间隔点数，再将所有回归矩阵yk合并成一个总矩阵y：

28、

29、进一步地，步骤3中所述的实际液压驱动力矩τi为：

30、τi＝(paiaai-pbiabi)·ri

31、其中，i是关节序号，pai，pbi为液压缸无杆腔和有杆腔压力，由压力传感器测得，aai，abi为液压缸无杆腔和有杆腔面积，ri为液压缸有效力臂。

32、进一步地，步骤4中所述的动力学参数求解如下，求解之后得到最小惯性参数集l：

33、l＝(yty)-1ytq

34、进一步地，步骤5中基于倾角传感器测量的柔性液压机械臂臂杆末端挠度变形表达式为：

35、

36、其中，

37、

38、

39、

40、其中，n为各臂杆序号，a，b，c为倾角传感器安装位置，γa，γb，γc为倾角传感器在各点安装位置测量出来的角度。

41、进一步地，步骤6中，三自由度柔性液压机械臂在x方向和y方向的末端位置表达式为：

42、xarm＝l1cosθ1+w1 sinθ1+l2 cos(θ1+θ2)+w2 sin(θ1+θ2)+l3 cos(θ1+θ2+θ3)+w3 sin(θ1+θ2+θ3)

43、yarm＝l1sinθ1-w1cosθ1+l2sin(θ1+θ2)-w2 cos(θ1+θ2)+l3 sin(θ1+θ2+θ3)-w3 cos(θ1+θ2+θ3)

44、进一步地，步骤6中对柔性液压机械臂末端位置关于关节角度求偏导，如下：

45、

46、其中，

47、j11＝-l1sinθ1+w1cosθ1-l2sin(θ1+θ2)+w2cos(θ1+θ2)-l3sin(θ1+θ2+θ3)+w3cos(θ1+θ2+θ3)

48、j12＝-l2sin(θ1+θ2)+w2cos(θ1+θ2)-l3sin(θ1+θ2+θ3)+w3cos(θ1+θ2+θ3)

49、j13＝-l3sin(θ1+θ2+θ3)+w3cos(θ1+θ2+θ3)

50、j21＝l1cosθ1+w1sinθ1+l2cos(θ1+θ2)+w2sin(θ1+θ2)+l3cos(θ1+θ2+θ3)+w3sin(θ1+θ2+θ3)

51、j22＝l2cos(θ1+θ2)+w2sin(θ1+θ2)+l3cos(θ1+θ2+θ3)+w3sin(θ1+θ2+θ3)

52、j23＝l3cos(θ1+θ2+θ3)+w3sin(θ1+θ2+θ3)

53、进一步地，步骤7中所述的柔性液压机械臂末端力求解如下：

54、

55、其中，f为机械臂末端力；j(p)是柔性液压机械臂雅克比矩阵，表示回归矩阵，l表示最小惯性参数集，τf为非线性关节摩擦力矩。

56、本发明的有益效果：

57、(1)本发明提出的柔性液压机械臂末端力软测量方法可代替末端力传感器，解决了力传感器在复杂工况下易损坏，从而无法对末端力准确感知的问题。

58、(2)考虑到大飞机清洗，隧道喷湿，玻璃墙面擦拭等应用场景需求工作范围大的机械臂，因为臂杆变长，所以存在柔性变形的特性，尤其在末端施加力的时候，变形会更为明显，所以需要在进行末端力测量的时候将臂杆柔性变形考虑进去，基于该实际的引用场景。本发明针对柔性液压机械臂臂杆变形的特性，提出了含有柔性臂变形特性的雅可比矩阵，适用于柔性液压机械臂的末端力软测量；与此前公开的刚性液压机械臂相比，区别在于柔性液压机械臂存在臂杆变形，而在进行末端力计算时，需要借助雅可比矩阵将末端力转换到各个关节上，刚性臂的雅可比矩阵不包含形变部分，柔性臂的雅可比矩阵包含了变形部分，由于在末端施加力的时候，臂杆会存在微量的变形，本发明通过分析柔性臂变形后的运动学特性求解了柔性臂末端位置的表达式，该表达式中变形量可通过倾角传感器测得，省去了求解雅可比矩阵对模态参数的计算，且通过倾角传感器测得的臂杆变形量比理论计算的臂杆变形量更为精准，更具实际应用价值，雅可比矩阵里面考虑该变形，末端力感知的精度有所提升。

59、(3)本发明针对柔性液压机械臂提出一套通用的末端力软测量方法，经激励轨迹求得的动力学参数可实现不同应用场合下的需要。

60、(4)与此前公开的刚性液压机械臂相比，本发明公开的技术方案并不是本领域技术人员的常规替换，因为，柔性液压机械臂是一种典型的多输入、多输出、高度非线性、刚柔耦合的复杂系统，多连杆柔性液压机械臂在动态建模与分析方面要比刚性液压机械臂复杂的多且更具有难度，臂架的柔性变形对运动及控制的影响较大，柔性臂在动力学建模过程中如何描述其臂杆柔性变形尤为重要，而刚性臂只需常规动力学建模，无需考虑臂杆变形的影响，本文在动力学建模过程中引入了模态参数用以表征柔性臂杆变形特性，因此动力学建模与辨识的计算过程要比刚性臂复杂，需要花费大量的时间以及创造性的劳动。