本发明涉及农业并联机器人控制技术领域,尤其涉及一种水果分拣并联机器人对串类水果夹取的自适应变阻抗控制方法,实现水果分拣并联机器人对串类水果的无损夹取。
背景技术:
并联机构采用闭链结构,具有结构稳定、高刚度、高精度、动力性能好等优点,尤其适用于对抓取稳定性要求较高的水果分拣操作。然而串类水果柔软、易损伤,实现无损分拣的关键问题之一是如何高性能实现夹持机构对水果的柔顺夹取控制。并联机器人夹持机构是与目标果实直接接触的机构,其功能类似于人手。
“果蔬采摘机器人末端执行器的柔顺抓取力控制”(姬伟,罗大伟,李俊乐,农业工程学报,2014年,第30卷,第9期,第19-26页)一文中针对采摘机器人的果蔬采摘,建立末端执行器和驱动电机控制模型,设计了一种基于广义比例积分的抓取力矩控制方法。但仅gpi力矩反馈控制器,较双环控制,难以对力进行精确跟踪。
“机器人柔性抓取试验平台的设计与抓持力跟踪阻抗控制”(王学林,肖永飞,毕淑慧,农业工程学报,2015年,第31卷,第1期,第58-63页)一文中为减小机器人在采摘过程中对果蔬的损伤,基于笛卡尔空间末端执行器双指抓持方向,提出了一种双指抓取果蔬的抓持力跟踪阻抗控制算法。但文中将手指力/位控制等效为期望的惯量-阻尼-刚度模型,而固定阻抗模型参数的阻抗控制不能适应水果质量、位置的变化。
技术实现要素:
为克服现有技术的不足,本发明针对水果分拣并联机器人夹持机构对串类水果的无损夹取问题,提出一种自适应变阻抗控制方法,该方法对阻尼参数进行基于期望夹持力与采集实际接触力的偏差的实时调整,提高系统对夹持速度和夹持负载变化的自适应性,同时提升阻抗控制的性能,以实现水果分拣并联机器人夹持机构对串类水果的无损夹取。
本发明采用的技术方案是采用如下步骤:
一种水果分拣并联机器人夹持机构的自适应变阻抗控制方法,包括如下步骤:
1)以水果分拣并联机器人夹持机构为被控对象,采用解析法对水果分拣并联机器人夹持机构进行运动学分析,求得主动关节丝杠转动的角度与夹持机构夹指的位置转换关系;
2)采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型;
3)基于步骤2)中利用拉格朗日法建立的动力学模型,采用双环控制结构:在外环力控制中,设计一种自适应变阻抗控制器;在内环位置控制中,设计一种光滑滑模控制器;
4)针对步骤3)基于水果分拣并联机器人夹持机构光滑滑模位置控制的阻抗控制器,设计一种水果分拣并联机器人夹持机构的自适应变阻抗控制算法,对阻尼参数进行基于期望夹持力与采集实际接触力的偏差的实时调整;
5)基于步骤2)、步骤3)和步骤4)构成水果分拣并联机器人夹持机构的自适应变阻抗控制器;
6)通过软件编程,实现水果分拣并联机器人夹持机构的自适应变阻抗控制方法。
进一步,所述步骤1)中,采用解析法对并联机器人夹持机构进行运动学分析,求得主动关节丝杠转动的角度与夹持机构夹指的位置转换关系,得:
式中,ps为丝杠导程;θ为主动关节丝杠转动的角度;l1为从动杆的长度;e为滑块的结构长度;x为夹持机构夹指在x轴方向的位置;d0为滑块距离坐标原点初始位置;l3为夹指距离滑座在y轴方向结构长度;l5为夹指距离滑座在x轴方向结构长度。
进一步,所述步骤2)中,采用拉格朗日法建立的动力学模型为:
式中,m(q)为惯性矩阵,
由q=x为夹指的位姿,上述动力学模型可转化为:
式中,m(x)为对称正定的惯性矩阵,
进一步,所述步骤3)中,在外环力控制中,通过建立二阶微分方程来表示夹持机构夹指受到的力和实际位置偏离期望位置的差之间的关系,期望的目标阻抗如下:
式中,md、bd、kd分别表示夹持机构的等效质量、阻尼、刚度参数;x、
所设计的自适应变阻抗控制器:
式中,fd(t)为夹持机构夹指施加给水果的期望接触力;fe(t)为压力传感器实际采集接触力;m、b分别表示夹持机构的等效质量参数和阻尼参数;
在内环位置控制中,所设计的光滑滑模控制器滑模面为:
式中:e(t)=qd(t)-q(t),
对s(t)求导,并将e(t),
取等速趋近律:
式中:k(t)为切换增益,表示系统的运动点趋近切换面s=0的速率;
由于sgn(s(t))函数在实际控制系统中存在抖振问题,为减少抖振用一个连续函数s(t)代替函数sgn(s(t)),则有:
进一步,所述步骤4)中,基于水果分拣并联机器人夹持机构光滑滑模位置控制的阻抗控制器,设计一种水果分拣并联机器人夹持机构的自适应变阻抗控制算法,对阻尼参数进行基于期望夹持力与采集实际接触力的偏差的实时调整,所设计的自适应律为:
式中,b表示夹持机构的等效阻尼参数,δb(t)为所设计的阻尼参数自适应律,
进一步,所述步骤5)中,所构成的自适应变阻抗控制器为:
式中fd(t)为夹持机构夹指施加给水果的期望接触力,fe(t)为压力传感器实际采集接触力,m、b分别表示夹持机构的等效质量参数和阻尼参数,δb(t)为所设计的阻尼参数自适应律,
本发明首次提出一种水果分拣并联机器人夹持机构的自适应变阻抗控制方法,应用于实现水果分拣并联机器人夹持机构对串类水果的无损夹取,其特点和有益效果是:
1、对水果分拣并联机器人夹持机构进行运动学分析,后采用拉格朗日法建立其动力学模型,可得到具有参数变化、未建模动态和外部干扰等诸多不确定性的动力学模型。
2、引入基于拉格朗日法动力学模型的水果分拣并联机器人夹持机构光滑滑模位置控制的阻抗控制方法,解决传统滑模位置控制的阻抗控制因其滑模控制的不连续性产生的抖振问题,能提高并联机器人夹持机构的控制性能。
3、针对基于拉格朗日法动力学模型的水果分拣并联机器人夹持机构光滑滑模位置控制的阻抗控制参数,对阻尼参数进行基于期望夹持力与采集实际接触力的偏差的实时调整,提高系统对夹持速度和夹持负载变化的自适应性,同时提升阻抗控制的性能。
附图说明
图1是水果分拣并联机器人夹持机构结构图。
图中:1.连接块2.滑动丝杠3.滑块4.驱动电机5.压力传感器6.底部连板7.左夹指8.右夹指9.从动杆10.滑座11.弹簧
图2是自适应变阻抗控制系统原理框图。
图3是水果分拣并联机器人夹持机构结构简图。
图4是水果分拣并联机器人控制系统总体结构图。
图5是水果分拣并联机器人夹持机构夹指期望运动轨迹。
图6是期望运动轨迹1,0.5kg葡萄串夹指运动跟踪误差。
图7是期望运动轨迹1,0.5kg葡萄串夹持力曲线。
图8是期望运动轨迹2,0.5kg葡萄串夹指运动跟踪误差。
图9是期望运动轨迹1,0.8kg葡萄串夹持力曲线。
具体实施方式
下面结合附图进一步说明书本发明具体实施方式。
本发明采用的技术方案是采用如下步骤:
1)以水果分拣并联机器人夹持机构为被控对象,采用解析法对水果分拣并联机器人夹持机构进行运动学分析,求得主动关节丝杠转动的角度与夹持机构夹指的位置转换关系;
2)采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型;
3)基于步骤2)中利用拉格朗日法建立的动力学模型,在外环力控制中,采用双环控制结构:设计一种阻抗控制器;在内环位置控制中,设计一种光滑滑模控制器。
4)针对步骤3)基于水果分拣并联机器人夹持机构光滑滑模位置控制的阻抗控制器,设计一种水果分拣并联机器人夹持机构的自适应变阻抗控制算法,对阻尼参数进行基于期望夹持力与采集实际接触力的偏差的实时调整。基于采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型和基于外环力控制中的阻抗控制器以及内环位置控制中的光滑滑模控制器构成水果分拣并联机器人夹持机构的自适应变阻抗控制器
5)基于步骤2)、步骤3)和步骤4)构成水果分拣并联机器人夹持机构的自适应变阻抗控制器;
6)通过软件编程,实现水果分拣并联机器人夹持机构的自适应变阻抗控制方法。
如图1所示,图中:1.连接块2.滑动丝杠(上端连接皮带,下端连接滑块)3.滑块4.驱动电机5.压力传感器6.底部连板7.左夹指8.右夹指9.从动杆10.滑座11.弹簧。图3是水果分拣并联机器人夹持机构结构简图。
首先,对水果分拣并联机器人夹持机构进行运动学分析,求得主动关节丝杠转动的角度与夹持机构夹指的位置转换关系;其次,采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型;然后,在外环力控制中,设计一种阻抗控制器;在内环位置控制中,根据滑动模态的存在和到达条件,设计滑模面s,设计一种光滑滑模控制器。接着,针对基于拉格朗日法动力学模型的水果分拣并联机器人夹持机构光滑滑模位置控制的阻抗控制参数,对阻尼参数进行基于期望夹持力与采集实际接触力的偏差的实时调整,完成水果分拣并联机器人夹持机构的自适应变阻抗控制器设计;最后,通过软件编程,实现水果分拣并联机器人夹持机构的自适应变阻抗控制。具体方法如下:
1、求解主动关节丝杠转动的角度与夹持机构夹指的位置转换关系
选取并联机器人夹持机构夹指的位姿参数q=x作为系统广义坐标,x为夹持机构夹指在x轴方向的位移(单位为m);采用解析法对机构进行运动学分析求得其主动关节丝杠转动的角度与夹持机构夹指的位置转换关系。
式中,ps为丝杠导程;θ为主动关节丝杠转动的角度(单位为rad);l1为从动杆的长度(单位为m);e为滑块的结构长度(单位为m)。x为夹持机构夹指在x轴方向的位置(单位为m);d0为滑块距离坐标原点初始位置(单位为m)。
2、采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型
式中,m(q)为惯性矩阵,
由q=x为夹指的位姿,上述动力学模型可转化为:
式中,m(x)为对称正定的惯性矩阵,
3、基于通过拉格朗日法建立的水果分拣并联机器人夹持机构动力学模型,在外环力控制中,通过建立二阶微分方程来表示夹持机构夹指受到的力和实际位置偏离期望位置的差之间的关系,期望的目标阻抗如下:
式中,md、bd、kd分别表示夹持机构的等效质量、阻尼、刚度参数;x、
所设计的自适应变阻抗控制器:
式中,fd(t)为夹持机构夹指施加给水果的期望接触力,fe(t)为压力传感器实际采集接触力,m、b分别表示夹持机构的等效质量参数和阻尼参数,
在内环位置控制中,所设计的光滑滑模控制器滑模面为:
式中,e(t)=qd(t)-q(t),
对公式(7)求导,并将e(t),
取等速趋近律:
式中,k(t)为切换增益,表示系统的运动点趋近切换面s=0的速率。
由于sgn(s(t))函数在实际控制系统中存在抖振问题,为减少抖振用一个连续函数s(t)代替函数sgn(s(t))。则公式(9)为:
4、设计一种自适应变阻抗控制的阻尼参数自适应律为:
式中,b表示夹持机构的等效阻尼参数,δb(t)为所设计的阻尼参数自适应律,
5、基于采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型和基于外环力控制中的阻抗控制器以及内环位置控制中的光滑滑模控制器构成水果分拣并联机器人夹持机构的自适应变阻抗控制器。
水果分拣并联机器人夹持机构的自适应变阻抗控制律为:
式中fd(t)为夹持机构夹指施加给水果的期望接触力,fe(t)为压力传感器实际采集接触力,m、b分别表示夹持机构的等效质量参数和阻尼参数,δb(t)为所设计的阻尼参数自适应律,
6、通过软件编程,实现水果分拣并联机器人夹持机构的自适应变阻抗控制。
由于水果分拣并联机器人夹持机构中采用交流伺服电机通过皮带与丝杠直联实现滑块的轴向移动。因此,需将步骤2所确定的控制器输出量τ经过一定的转换才能得到主动关节驱动电机实际所需转矩。
伺服电机转矩为:
式中,η为皮带的传递效率,τ0为电机驱动力。
编写基于水果分拣并联机器人夹持机构的自适应变阻抗控制算法软件程序,将计算结果(即驱动电机所需转矩)经控制系统数/模转换得到的电压模拟量,发送给电机对应的伺服驱动器,控制电机驱动丝杠,从而驱动水果分拣并联机器人夹持机构实现水果分拣并联机器人串类水果夹取。
以下提供本发明的实施例:
实施例1
本发明针对水果分拣并联机器人夹持机构对串类水果的无损夹取问题,提出一种水果分拣并联机器人夹持机构的自适应变阻抗控制方法,该方法需建立水果分拣并联机器人夹持机构的数学模型,提高系统对夹持速度和夹持负载变化的自适应性,同时提升阻抗控制的性能,能够很好地实现水果分拣并联机器人对串类水果的无损夹取。水果分拣并联机器人夹持机构的自适应变阻抗控制原理图如图2所示,该控制方法的具体实施方式如下:
1、求解主动关节丝杠转动的角度与夹持机构夹指的位置转换关系
在图2中,采用从动杆长度约束方程,根据夹持机构结构整理可得机构运动学方程:
式中,
将式(17)改写为
x2+ex+f=0(15)
式中e=-2(e-l5),
根据机构的装配模式,求解式(18)可得:
求得主动关节丝杠转动的角度与夹持机构夹指的位置转换关系,得:
式中,ps为丝杠导程;θ为主动关节丝杠转动的角度(单位分rad);l1为从动杆的长度(单位为m);e为滑块的结构长度(单位为m)。x为夹持机构夹指在x轴方向的位置(单位为m);d0为滑块距离坐标原点初始位置(单位为m);l3为夹指距离滑座在y轴方向结构长度(单位为m);l5为夹指距离滑座在x轴方向结构长度(单位为m)。
2、采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型
式中,m(q)为惯性矩阵,
由q=x为夹指的位姿,上述动力学模型可转化为:
式中,m(x)为对称正定的惯性矩阵,
为将广义力转化为关节驱动力,需要做如下变换:
3、基于通过拉格朗日法建立的水果分拣并联机器人夹持机构动力学模型,在外环力控制中,通过建立二阶微分方程来表示夹持机构夹指受到的力和实际位置偏离期望位置的差之间的关系,期望的目标阻抗如下:
式中,md、bd、kd分别表示夹持机构的等效质量、阻尼、刚度参数;x、
所设计的自适应变阻抗控制器:
式中,fd(t)为夹持机构夹指施加给水果的期望接触力,fe(t)为压力传感器实际采集接触力,m、b分别表示夹持机构的等效质量参数和阻尼参数,
在内环位置控制中,所设计的光滑滑模控制器滑模面为:
式中,e(t)=qd(t)-q(t),
对式(23)求导,并将e(t),
取等速趋近律:
式中,k(t)为切换增益,表示系统的运动点趋近切换面s=0的速率。
由于sgn(s(t))函数在实际控制系统中存在抖振问题,为减少抖振用一个连续函数s(t)代替函数sgn(s(t))。则式(25)为:
4、设计一种自适应变阻抗控制的阻尼参数自适应律为:
式中,b表示夹持机构的等效阻尼参数,δb(t)为所设计的阻尼参数自适应律,
5、基于采用拉格朗日法建立水果分拣并联机器人夹持机构的动力学模型和基于外环力控制中的阻抗控制器以及内环位置控制中的光滑滑模控制器构成水果分拣并联机器人夹持机构的自适应变阻抗控制器。
水果分拣并联机器人夹持机构的自适应变阻抗控制律为:
式中fd(t)为夹持机构夹指施加给水果的期望接触力,fe(t)为压力传感器实际采集接触力,m、b分别表示夹持机构的等效质量参数和阻尼参数,δb(t)为所设计的阻尼参数自适应律,
6、通过软件编程,实现水果分拣并联机器人夹持机构的自适应变阻抗控制方法。
由于水果分拣并联机器人夹持机构中采用交流伺服电机通过皮带与丝杠直联实现滑块的轴向移动。因此,需将步骤2所确定的控制器输出量τ经过一定的转换才能得到主动关节驱动电机实际所需转矩。
具体地,驱动电机转矩由皮带传递机械效率η=0.95决定:
水果分拣并联机器人采用“上位机(pc)+下位机(umac多轴运动控制器)”的分布式控制系统,其控制系统总体结构示意图如图4所示。控制系统运行过程:上位机(pc)完成系统初始化、代码编译等任务,上位机(pc)串口实时读取实际采集压力值,根据主控中心发出的指令要求,通过以太网口(ethernet)实时向umac控制器发出姿态调整指令,umac实时处理相关指令,并通过acc-24e2a板卡实现对伺服驱动器的差分脉冲指令控制以及六路差分编码器信息的读取,进而控制并联机器人相应关节以指令速度产生相应位移和转动,最后主动关节位置和速度信息通过编码器反馈至umac,umac完成控制功能后将结果返回给pc。
以vc++6.0为软件开发平台,基于mfc和deltatau公司提供的pcomm32w.dll动态链接库设计上位机应用程序,主要实现系统初始化、数据管理、代码编译和机构状态实时监控等功能。
根据所设计的水果分拣并联机器人夹持机构的自适应变阻抗控制算法开发umac伺服算法程序,根据所需期望运动轨迹及期望夹持力开发机构运动程序;将水果分拣并联机器人夹持机构的自适应变阻抗控制算法程序下载到umac中,设置umac相关参数,执行机构运动程序即可得到并联机器人夹持机构按照期望运动轨迹及期望夹持力实现水果夹取。
图5是水果分拣并联机器人夹持机构夹指期望运动轨迹。
水果分拣并联机器人夹持机构夹指运动轨迹如图5所示;在期望运动轨迹1下,0.5kg葡萄串夹指运动跟踪误差如图6所示,0.5kg葡萄串夹持力曲线如图7所示;在期望运动轨迹2下,0.5kg葡萄串夹指运动跟踪误差如图8所示,0.8kg葡萄串夹持力曲线如图9所示。
图6和图7表明,当系统存在上述集总扰动时,所提出的水果分拣并联机器人夹持机构的自适应变阻抗控制算法,能够使水果分拣并联机器人夹持机构控制系统具有更高的位置跟踪精度,并能使夹持机构输出力更快速、低超调地到达期望的设定力。比较图6和图8表明,当改变夹持机构夹指运动速度时,所提出的水果分拣并联机器人夹持机构的自适应变阻抗控制算法,由于其阻尼参数b随δf自适应调整,因此提高了控制系统对夹持速度改变的自适应性,系统仍能保持较高的位置跟踪精度。比较图7和图9表明,当改变水果质量时,所提出的水果分拣并联机器人夹持机构的自适应变阻抗控制算法,由于其阻尼参数b随δf自适应调整,因此提高了控制系统对负载变化的自适应性,其夹持机构输出力仍能快速、低超调地到达期望设定力。
应理解上述实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。