一种液压机械臂末端力软测量方法与流程

1.本发明涉及液压机械臂控制技术领域，具体涉及一种液压机械臂末端力软测量方法。


背景技术：

2.目前液压机械臂已被广泛应用于产品加工、施工救援、水下作业等方面，当机械臂在重载情况下工作时，大惯量必然导致较大冲击，极易造成末端传感器的损坏，导致机械臂无法对末端力精确感知。为解决上诉问题，提出一种液压机械臂末端力软测量方法，以软测量方法代替力传感器，可以实现末端力的精确感知。
3.目前关于力软测量被广泛应用的是基于机械臂逆运动学模型的直接外力估计方法(koivumki j；mattila j.stability-guaranteed force-sensorless contact force/motion control of heavy-duty hydraulic manipulators.ieee transactions on robotics,2015)，但是这种方法需要准确的机械臂动力学参数，由于无法获知准确的动力学参数，故影响外力的估计性能。另一种方法是通过设计观测器来实现对外部作用力的估计文献(sebastian g.interaction force estimation using extended state observers:an application to impedance-based assistive and rehabilitation robotics，ieee robotics and automation letters，2019)设计了扩张状态观测器(extended state observer,eso)来估计外力，但是，这种方法仅适用于估计恒定或缓慢变化的相互作用力，适用于康复运动的助力训练，但是在液压机械臂高动态的作业工况下将会失效。


技术实现要素：

4.为此，本发明提供一种液压机械臂末端力软测量方法，通过建立机械臂动力学模型，进而建立机械臂最小惯性参数集模型。利用有限傅里叶级数求解激励轨迹，在激励轨迹下利用最小惯性参数集模型求解机械臂动力学参数，进而计算外部作用力。
5.本发明采用的技术方案是：一种液压机械臂末端力软测量方法，包括以下步骤：
6.步骤1，建立液压机械臂动力学模型，并将动力学模型线性化处理，建立机械臂最小惯性参数集及其对应的回归矩阵的线性模型。
7.步骤2，利用有限傅里叶级数设计激励轨迹，以步骤1的回归矩阵条件数最小为目标，求解有限傅里叶级数系数，生成激励轨迹。
8.步骤3，机械臂在无负载条件下，运行一次激励轨迹，采集液压缸两腔压力传感器数值，并通过液压缸两腔压力计算液压驱动力矩τ。将各时刻的关节角度、角速度、角加速度带入回归矩阵y中，并合并成一个总矩阵。
9.步骤4，根据步骤3得到的液压驱动力矩τ和回归矩阵y计算机械臂动力学参数。
10.步骤5，将步骤4中所得的动力学参数，带入到步骤1的线性模型中，计算末端力大小。
11.进一步地，步骤1中所述的机械臂动力学模型具体如下：
[0012][0013]
其中，q∈rn×1机械臂关节角度，关节角速度，关节角加速度，n表示机械臂自由度，τ为液压驱动力矩，m(q)为机械臂惯量矩，为机械臂科氏向心力矩，g(q)为机械臂重力矩，摩擦力矩，包括库伦摩擦和粘滞摩擦。中各关节角度可由角度编码器得出，角速度、角加速的由关节角度经一次和二次离线微分得到，由于微分后数据白噪声较高，故设计离线iir数字滤波器进行滤波处理。
[0014]
进一步地，步骤1中所述的机械臂最小惯性参数集线性模型具体如下：
[0015][0016]
其中，表示回归矩阵，l∈r
p
×1表示最小惯性参数集，p表示最小参数集内元素数目，τ为液压驱动力矩。
[0017]
进一步地，步骤2中所述的有限傅里叶级数表达式如下：
[0018][0019]
对于第i个关节，正弦项和余弦项的数目均为n，t表示激励轨迹的运行时间，wf＝2πff为基频，ff＝1/t，a
l,i
、b
l,i
、q
i0
为待求傅里叶系数，用含约束条件的多元函数求最优的方法求解。为了保证机器人平稳且在可以到达的安全范围之内运行，约束条件如下：
[0020][0021]
其中，cond
min
(y)表示以回归矩阵y的条件数最小为优化目标，q(t)表示t时刻的机械臂关节角度；q0为机械臂初始关节角，当一个周期结束时，t＝tf，tf表示终止时刻，机械臂回到初始位姿q0，从而可以连续执行下一周期，初始t＝0和结束t＝tf的速度和加速度设为0，有效避免冲击；q
min
、q
max
、分别为机械臂运动时角度、角速度和角加速度的最小值和最大值。通过限定角度、角速度、角加速度上下限，保证机械臂在安全的范围内运动。
[0022]
进一步地，步骤3中所述的回归矩阵y合并的矩阵，是先计算出各个时刻下的回归矩阵yk，k为时间间隔点数，再将所有回归矩阵yk合并成一个总矩阵y：
[0023][0024]
进一步地，步骤3中所述的液压驱动力矩τ为：
[0025]
τi＝(p
aiaai-p
biabi
)*ri[0026]
其中，i是关节序号，p
ai
，p
bi
为液压缸无杆腔和有杆腔压力，可由压力传感器测得，aai
，a
bi
为液压缸无杆腔和有杆腔面积，ri为液压缸有效力臂。
[0027]
进一步地，步骤4中所述的动力学参数求解如下，求解之后得到最小惯性参数集l：
[0028]
l＝(y
t
y)-1yt
τ
[0029]
进一步地，步骤5中所述的机械臂末端力求解如下：
[0030][0031]
其中，f为机械臂末端力；是机械臂雅克比矩阵的转置。
[0032]
本发明的有益效果：
[0033]
(1)本发明提出的末端力软测量方法可代替末端力传感器，解决了力传感器在复杂工况下易损坏，从而无法对末端力准确感知的问题。
[0034]
(2)与现有的基于无传感器的相互作用力估计方法，例如基于逆动力学模型的直接估计和基于eso的外力估计方法相比，本发明可提高外力软测量精度。
[0035]
(3)本发明针对液压机械臂提出一套通用的末端力软测量方法，经激励轨迹求得的动力学参数可实现不同应用场合下的需要。
附图说明
[0036]
图1是七自由度液压机械臂末端力软测量方法框图；
[0037]
图2是七自由度液压机械臂系统框图；
[0038]
图3是本发明实验例1的液压机械臂直线轨迹的末端力实际值与估计值对比图；
[0039]
图4是本发明实验例1的液压机械臂三角形轨迹的末端力实际值与估计值对比图；
[0040]
图5本发明实验例1的液压机械臂椭圆轨迹的末端力实际值与估计值对比图；
[0041]
图6本发明实验例1的液压机械臂变力负载下的末端力实际值与估计值对比图；
[0042]
图7本发明实验例1的液压机械臂变力负载下的末端力实际值与估计值对比图。
具体实施方式
[0043]
为了更好地理解本发明，下面结合附图及实施例进一步阐明本发明的内容，但本发明的内容不仅仅局限于下面的实施例。
[0044]
实施例1
[0045]
参阅图1，本实施例提出一种液压机械臂末端力软测量方法，具体实现步骤如下：
[0046]
步骤1，建立液压机械臂动力学模型，并将动力学模型线性化处理，建立机械臂最小惯性参数集及其对应的回归矩阵的线性模型。
[0047]
所述的机械臂动力学模型：
[0048][0049]
其中，q∈rn×1机械臂关节角度，关节角速度，关节角加速度，n表示机械臂自由度，τ为液压驱动力矩，m(q)为机械臂惯量矩，为机械臂科氏向心力矩，g(q)为机械臂的重力矩，摩擦力矩，包括库伦摩擦和粘滞摩擦。
[0050]
所述的机械臂最小惯性参数集的线性模型：
[0051]
[0052]
其中，表示回归矩阵，l∈r
p
×1表示最小参数集，p表示最小参数集内元素数目，τ为液压驱动力矩。最小惯性参数动力学模型是由机械臂动力学参数经线性化处理后，再将各线性相关项合并重组所得，当n＝3时，对于三自由度液压机械臂来说，最小惯性参数集动力学模型为：
[0053][0054]
其中，l(1)＝m1c
x1
m2a1 m3a1，，
[0055][0056][0057][0058][0059][0060][0061][0062][0063][0064]
[0065]
其中，对于三自由度液压机械臂来说，i＝1,2,3，mi表示机械臂各连杆质量，li表示各关节点到各个连杆质心的距离，c
x
和cy分别表示机械臂连杆质心在连杆上的位置，i
zz
表示绕z轴的转动惯量，以y(2,6)为例表示回归矩阵y的第二行、第六列的元素。
[0066]
此外，g为恒定的重心加速度，a1，a2为连杆长度，fc表示库伦摩擦力，fv表示粘性摩擦系数，ri为液压缸有效力臂。
[0067]
步骤2，利用有限傅里叶级数设计激励轨迹，以回归矩阵条件数最小为目标，求解有限傅里叶级数系数，生成激励轨迹。
[0068]
所述的有限傅里叶级数表达式如下：
[0069][0070]
对角度表达式微分可得角速度和角加速度表达式。对于第i个关节，正弦项和余弦项的数目均为n，t表示激励轨迹的运行时间，wf＝2πff为基频，ff＝1/t，本发明i＝3，n＝5，激励时间持续为20s，ff＝0.05hz，wf＝0.1π，a
l,i
、b
l,i
、q
i0
为待求傅里叶系数，用含约束条件的多元函数求最优的方法求解。为了保证机器人平稳且在可以到达的安全范围之内运行，约束条件如下：
[0071][0072]
其中，cond
min
(y)表示以回归矩阵y的条件数最小为优化目标，q0为机械臂初始关节角，q(t)表示t时刻的机械臂关节角度，当一个周期结束时(t＝tf)，tf表示终止时刻，机械臂回到初始位姿q0，从而可以连续执行下一周期，初始(t＝0)和结束(t＝tf)的速度和加速度设为0，有效避免冲击。q
min
、q
max
、分别为机械臂运动时角度、角速度和角加速度的最小值和最大值。通过限定角度、角速度、角加速度上下限，保证机械臂在安全的范围内运动。
[0073]
步骤3，机械臂在无负载条件下，运行一次激励轨迹，采取液压缸两腔压力传感器数值，并通过液压缸两腔压力计算液压驱动力矩τ。将各时刻的关节角度、角速度、角加速度带入回归矩阵y中，并合并成一个总矩阵。
[0074]
所述的回归矩阵y和液压驱动力矩τ合并的矩阵，是先计算出各个时刻下的回归矩阵yk，k为时间间隔点数，再将所有回归矩阵yk合并成一个总矩阵y：
[0075][0076]
其中各关节角度可由角度编码器得出，角速度、角加速的由关节角度经一次和二次离线微分得到，由于微分后数据白噪声较高，故设计离线iir数字滤波器进行滤波处理。
[0077]
所述的液压驱动力矩τ为：
[0078]
τi＝(p
aiaai-p
biabi
)*ri[0079]
其中，i是关节序号，p
ai
，p
bi
为液压缸无杆腔和有杆腔压力，可由压力传感器测得，a
ai
，a
bi
为液压缸无杆腔和有杆腔面积，ri为液压缸有效力臂。
[0080]
步骤4，根据步骤3得到的液压驱动力矩τ和回归矩阵y计算机械臂动力学参数。
[0081]
所述的动力学参数求解如下，求解之后得到最小惯性参数集l：
[0082]
l＝(y
t
y)-1yt
τ
[0083]
步骤5，以步骤4中所得的动力学参数，计算末端力大小。
[0084]
所述的末端力求解如下：
[0085][0086]
其中，f为末端力；是机械臂雅克比矩阵的转置。
[0087]
实验例1
[0088]
采用实施例1软测量方法，对图2所示的一款七自由度冗余液压机械臂应用系统给出具体的实施方案。
[0089]
首先建立液压机械臂动力学模型，并根据动力学模型建立机械臂最小惯性参数集模型。在激励轨迹下采集液压缸两腔压力数据和关节角度数据，经计算得到液压驱动力矩，经微分、滤波处理得关节角速度和角加速度，进而求解机械臂动力学参数，根据动力学参数计算机械臂末端所受外力。
[0090]
比较例1
[0091]
采用与实施例1基本相同的流程方法对图2所示的一款七自由度液压机械臂应用系统给出具体的实施方案，与实验例1不同的地方在于，在本比较例中，末端力软测量的实现所采用是文献(koivumki j；mattila j.stability-guaranteed force-sensorless contact force/motion control of heavy-duty hydraulic manipulators.ieee transactions on robotics,2015)提出的利用逆动力学直接估计的方法，直接估计法的模型为：
[0092][0093]
其中，f为末端力；是机械臂雅克比矩阵的转置；τ为液压驱动力矩，(q)为机械臂惯量矩，为机械臂科氏向心力矩，g(q)为机械臂重力矩，摩擦力矩，包括库伦摩擦和粘滞摩擦。
[0094]
效果检测：
[0095]
在实验例1的七自由度液压机械臂的末端施加恒力，机械臂末端在笛卡尔空间内分别运动直线、椭圆、三角形三种不同轨迹，受到的外部作用力的估计效果，以及与比较例1的估计效果的对比如图3～图5所示，“实际值”是末端所挂重物的重量，即末端真实受力的值，“估计值”是按照实验例1的流程方法计算所得末端力的值，“比较值”是按照对比例1的流程方法计算所得末端力的值，从图3～图5中可以看出，采用本发明提出的方法可以更有效的提高“软测量”的精度。在实验例1的七自由度液压机械臂的末端施加变力，设计弹簧施力装置实现变力负载的施加，受到的外部作用力的估计效果，以及与比较例1的估计效果的对比如图6～图7所示，“实际值”是通过力传感器直接测量的机械臂末端外部作用力的值，“估计值”是按照实验例1的流程方法计算所得末端力的值，“比较值”是按照对比例1的流程
方法计算所得末端力的值，从图6～图7可以看出本发明表现出良好的软测量效果。
[0096]
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。