基于卡尔曼滤波的恒力装置恒力跟随方法*

吴凌峰,易建业,谢 晖,2

(1.季华实验室,佛山 528000;2.大捷智能科技(广东)有限公司,佛山 528000)

0 引言

机器人打磨作业是取代人工打磨和提高打磨效率的重要手段,而在打磨作业过程中,机器人的打磨工具与加工件有接触力要求,需要实现机器人的柔顺控制。打磨机器人主动柔顺控制一般会采在机器人控制器中加入强化学习、阻抗控制、力/位混合控制等算法[1-4],虽然控制效果和鲁棒性优良,但普遍存在对机器人本体要求高、控制算法复杂不容易实现等问题。由于力控制由机器人末端的恒力装置实现,“机器人+恒力装置”的机器人打磨系统无需在工业机器人封闭式的控制器中加入力传感器和主动柔顺算法,具有力位解耦、实现简单、可移植性高、应用更广泛的优点。

恒力装置作为“机器人+恒力装置”中机器人被动柔顺核心部件,国内外学者研发出了各种形式的恒力装置,该装置从驱动方式方式上可分类弹簧式、电机式、气动式3种:AHN、HUANG、LIU等[5-7]都研究了基于弹簧等阻尼器的机械式恒力装置,该类装置可以实现恒力被动柔顺,但力值不可主动调整;赵亚平、MOHAMMAD等[8-9]都提出了基于电机作为力输出源的恒力装置,该类恒力装置通过控制电机控制恒力的输出,具有主动柔顺、调节响应迅速的特点,也存在力矩小、发热大的缺点;JIN等[10]研究了基于气囊的气动式恒力输出装置,稳态误差可达到2 N,但存在无法安装电主轴和更换打磨工具的问题,LIANG等[11]研究了基于气缸和柔性机构的恒力装置,实现恒力在轴向和径向的输出。

气缸式恒力装置具有控制简单、柔性好的优点,工程中打磨机器人也较多使用奥地利生产的ACF自适应法兰,该装置内置力传感器,结构和控制复杂,且价格昂贵、技术保密。马文超、袁乐天等[12-13]研究院校都对气缸式的打磨机器人恒力装置进行了研究,但该恒力装置缺乏对外部负载质量、速度和加速度的感知,存在输出力无法适应外部负载变化的情况。

对此,针对内部无力传感器的气缸式恒力装置存在的因外部负载变化而精度难以保证的问题,本文基于内部无力传感器的气缸式恒力装置,研究其恒力跟随方法,以提高其在负载质量变化和倾角变化时的恒力跟随精度。

1 无力传感的恒力装置

恒力装置主要由电气比例阀、电磁换向阀、气缸、位移传感器、倾角传感器、以及上法兰、下法兰、花键副组成,如图1所示为恒力装置恒力输出原理。

图1 恒力装置恒力输出工作原理

恒力装置工作时,一般会下挂一定负载的打磨工具(简称外部负载,后文都用外部负载),其接收外部给定的输出力目标值,输出相应的正压力(简称为输出力,后文都用输出力)如图2所示,其工作流程如下:控制器接收来自外部的恒力值,从位移传感器、倾角传感器、电气比例阀采集气压、位置、倾角信息,通过数据处理及算法运算得到气缸所需的气压值和气缸进气方向,并将气压值和进气方向发送给电气比例阀和电磁换向阀,使得气缸产生相应的推拉力,继而实现目标力的输出。

图2 恒力装置受力情况

2 基于卡尔曼滤波器的恒力跟随方法

2.1 恒力装置恒力跟随模型

当恒力装置被用于机器人打磨作业时,恒力装置上法兰连接机器人,下法兰安装打磨工具,对此时的恒力装置进行受力分析。如图2所示为恒力装置受力情况,由于气缸活塞、活塞杆、打磨工具为固定连接,将该三者视为一个弹簧阻尼系统,其受力平衡方程式为:

(1)

式中:Fn为打磨工具对工件施加的法向力(与目标输出力为一对反作用力),P1、P2为气缸的有杆腔气压和无杆腔气压,S1、S2为气缸的活塞有杆侧面积和无杆侧面积,m1为恒力装置的活塞和下法兰的质量(为定值),m2为打磨工具的质量,α为打磨工具与重力方向的倾角(由倾角仪测得),f为活塞受到的气缸内壁的摩擦力,u为阻尼系数,x为气缸活塞的压缩距离(由位移传感器测得)。

2.2 基于卡尔曼滤波器的速度加速度获取

卡尔曼滤波算法是一种除去噪声还原真实数据的一种数据处理技术,在测量协方差已知时,通过输入观测数据,可得到系统状态的最优估计。针对线性系统模型,卡尔曼滤波器可基于当前状态测量数据和和系统模型对系统状态进行最优估计[14-15]。因此,本文引入卡尔曼滤波器获取恒力装置恒力跟随模型中的位移、速度及加速度。

本系统中,卡尔曼滤波器中的系统状态的预测过程可表示为:

xk+1=Akxk+Bkuk+wk

(2)

式中:xk+1为第k+1时刻系统状态变量的预测值,xk为第k时刻的系统状态变量的最优估计值,Ak为系统状态转移矩阵,Bk为控制矩阵,uk为控制向量,wk为系统中的噪声误差。

卡尔曼滤波器中的系统状态的观测过程可表示为:

zk=Hkxk+vk

(3)

式中:zk+1为第k+1次观测的测量变量,Hk为观测矩阵,vk+1为第k+1次的协方差为Rk的观测误差。

基于预测过程和观测过程,卡尔曼滤波器的计算步骤主要由初始化滤波器、状态预估、状态观测和状态矫正4个步骤组成。

在初始化滤波器阶段中,基于卡尔曼滤波算法原理对系统的预测过程和观测过程进行初始化,主要包括系统的初始状态、观测矩阵Hk、状态转移矩阵Ak、观测误差协方差矩阵Rk、系统误差协方差矩阵Qk进行赋值。

在状态预估阶段,包括对状态变量的预测和误差协方差变量的预测,分别可通过式(4)和式(5)进行预测:

xk+1=Akxk+Bkuk

(4)

(5)

式中:Pk+1为第k+1次预测的系统误差协方差,Qk为初始的系统误差协方差。

在观测阶段,可通过式(3)得到系统的观测变量的值;在状态更新阶段,主要包括对系统状态的最优估计和系统误差协方差矩阵的更新,系统状态的最优估计和系统误差协方差矩阵的更新可通过式(6)～式(8)进行计算:

(6)

Pk+1=(I-Kk+1Hk+1)Pk+1

(7)

(8)

基于上述的卡尔曼滤波器原理,如式(9)～式(10)所示,本系统以位移、速度、加速度为系统的状态变量,位移为系统的观测变量,带入卡尔曼滤波器计算,即可得到恒力装置的气缸的位移、速度和加速度。

x=[pva]T

(9)

z=[p0 0]T

(10)

式中:p为恒力装置的气缸伸缩量,v为恒力装置的气缸伸缩过程中的速度,a为恒力装置的气缸伸缩过程中的加速度。

2.3 外部负载大小标定

由于恒力装置未装有力传感器测量外部负载的大小(即打磨工具的质量m2),为保证恒力输出的精度,对此提出了一种获取恒力跟随模型中的恒力装置外部负载大小的方法。

对式(1)进行变形处理,可得到外部负载大小m2满足如下关系式:

(11)

(12)

考虑到恒力装置采用的气缸为低摩擦气缸,摩擦力很小可忽略不计,式(12)可简化为:

(13)

式中:P1为有杆腔气压,P2为无杆腔气压(其值为大气压值,为已知)。

由于恒力装置可测得气压P1和负载倾角α,将外部负载处于静止平衡状态时的气压P1和倾角α带入式(13)中,即可求解得到外部负载m2值。

2.4 气缸进气方向及气压求解

在恒力跟随模型中其他参数确定后,恒力装置的输出力取决于气缸进气方向和气缸气压大小。而当外部负载大小和倾角发生变化时,为保证恒力输出精度,需相应确定气缸进气方向和气压大小。对此,对式(1)变形处理,并忽略可不计摩擦力,得到气缸气压与恒力的关系式:

(14)

记Fa为气缸所需产生的气压力,Fa满足:

(15)

由于有杆腔和无杆腔两者有一个与大气联通,因此有杆腔气压P1和无杆腔气压P2满足如下关系:

(16)

式中:Pa为大气气压值。

当气缸进气腔不同时,联立式(14)～式(15)气缸产生的气压力满足如下关系:

(17)

根据式(16)可知,可得到如下恒力跟随方法:

(1)当外部负载的质量、倾角、速度、加速度变化时,控制器按照式(15)实时计算Fa的大小;

(2)当Fa≥Pa(S1-S2)成立时,恒力装置选择进气腔为有杆腔;当Fa≤Pa(S1-S2)成立时,恒力装置选择进气腔为无杆腔;

(3)按照式(17)计算达到目标输出力所需的进气腔气压大小。

3 恒力跟随实验

3.1 实验平台

为验证所提出无力传感的恒力装置的恒力跟随方法,搭建了如下平台并进行恒力输出验证实验。如图3和图4所示,实验平台主要由恒力装置、实验台架、力传感器测量传输装置、气源4部分组成。

图3 恒力装置实物 图4 力装置实验台架

如图3所示为恒力装置实物,为满足实验平台条件了,搭建了如图4所示的恒力装置实验台架。该实验台架由可转动台架、5 kg外部负载、称重式力传感器、直线模组及其驱动器组成。其中,称重式力传感器可直接测量恒力装置输出的力值,可转动台架可以调节恒力装置的倾角,直线模组可改变恒力装置的力传感器的位置以保证与负载末端接触,满足不同负载时的恒力装置输出力的测量需求。

3.2 恒力跟随实验

为验证所提出的恒力装置及其他恒力跟随方法,基于上述的实验平台进行了外部负载质量标定实验和不同负载倾角下恒力装置恒力输出精度的验证实验。

如图5所示,为对恒力装置进行外部负载质量标定实验,恒力装置下端挂一定配重,通过缓慢调节气缸气压实现外部负载的静态平台,控制器通过2.3节所用方法计算得到外部负载质量大小,并自动保存用于恒力跟随方法中。

图5 外部负载质量标定实验

如图6所示,在对外部负载标定后,将恒力装置分别倾斜至3.8°、22°、45°,进行动态循环实验,上位机软件采集恒力装置的位移、速度、倾角和力值信息,以验证所研制的恒力装置恒力输出精度。

(a) 3.8° (b) 22° (c) 45°图6 不同角度动态循环

如图7～图10所示,为上位机软件采集到的3.8°、22°、45°动态循环实验时恒力装置数据,其中,Ft为输出力目标值,N;Fs为力传感器测得的输出力实际值,N;P为气缸气压(0.1 MPa,即1个大气压,当P为正时,有杆腔进气;当P为负时,无杆腔进气);α为负载倾角,°;x为负载位置,mm;v为负载速度,mm/s;a为负载加速度,mm/s2。

图7 3.8°动态循环实验时负载运动状态 图8 3.8°动态循环实验时输出力情况

图9 22°动态循环实验时输出力情况 图10 45°动态循环实验时输出力情况

由于3.8°、22°、45°动态循环实验仅角度不同,实验过程类似,因此仅较为详细地叙述3.8°动态循环实验的过程和输出力情况。如图7所示在3.8°动态循环实验中,负载在直线模组的推动下以3 mm/s的速度做往复运动。每当负载做完往复运动时,上位机软件则发送一个新的输出力的目标值Ft给恒力装置(Ft给定顺序为20,25,…,100,95,…,20),恒力装置根据Ft和自身状态执行恒力输出算法并相应控制气缸动作,实现对输出力的目标值Ft的响应,并由力传感实时采集恒力装置输出力的实际值Fs。

对比图7中的x、v、a曲线可以发现,由卡尔曼滤波后获得的x、v都较为平滑,a由于直线模组加速和减速而有一些周期性凸点。由x、v、a曲线可以看出,卡尔曼滤波可以较为准确地获得负载的位置、速度和加速度,为恒力跟随方法获得较为准确负载运动参数。

由图7～图10可知,当输出力的目标值Ft变化时,恒力装置会根据Ft的变化实时调节气缸气压P以使得输出力的实际值Fs跟随Ft变化;当负载倾角不同和负载处在直线往复运动时,恒力装置的输出力仍能与输出力的目标值Ft保持较小误差跟随;证明了本文所提出的恒力跟随方法可以在负载倾角变化和负载处在慢速运动时实现实际输出力跟随目标输出力。

为获得恒力装置在3.8°、22°、45°动态循环实验中的实际输出力与目标输出力的跟随误差,对该3个实验中的恒力装置的输出力误差进行统计,并绘制了如图11所示的输出力误差情况图,为量化误差情况,统计了如表1所示的跟踪误差统计表。

图11 恒力装置输出力误差情况

表1 恒力装置的跟踪误差统计

由表1和图11可知,在3种倾角下,该恒力装置的跟踪误差小于2 N的时刻点占比为0.883 1、0.905 8、0.887 8,跟踪误差小于3 N的时刻点占比为0.946 1、0.946 7、0.938 9,验证了该恒力装置及恒力跟随方法的可行性。由于恒力装置会存在刚接收到输出力目标值而没时间响应就反馈实际输出力的情况,少数时刻误差大于4 N的。

4 结论

无力传感的恒力装置是打磨机器人被动柔顺的关键部件,为解决无力传感的恒力装置缺乏对外部负载质量、速度和加速度的感知和存在输出力无法适应外部负载变化的问题,本文提出了基于卡尔曼滤波的恒力跟随方法,最终得到结论如下:

(1)在恒力输出模型的基础上,利用卡尔曼滤波器和标定方法得到对外部负载的质量、速度加速度,实现了无力传感的恒力装置对外部负载的感知。

(2)在感知外部负载后,通过实时求解恒力装置的气缸气压大小和进气方向,可实现输出力对目标力的跟随,其跟随误差精度为3 N,基本满足机器人打磨作业的恒力精度要求,证明了所提方法的可行性。