CN106695793B - 一种xyzr四轴钻孔机器人主动柔顺控制装置及方法 - Google Patents

Abstract

本发明属于机器人控制领域，尤其涉及一种XYZR四轴钻孔机器人主动柔顺控制装置及方法。所述装置包括主控模块、伺服驱动控制模块以及人机交互模块；主控模块与伺服驱动控制模块相互连接，主控模块还与人机交互模块相互连接。本发明功能完善、结构清晰、实现简单、模块耦合度低；同时实现了***运动的同步性和平滑性；将主动柔顺控制决策模块单独进行设计，获取钻机推进过程中相关数据时避免了复杂的钻机推进过程建模和无指导性规律的重复拼凑实验，避免每台机器人都需要在线使用昂贵的加速段传感器和力传感器，节约成本，避免在线调整控制参数引入的不稳定因素和***宕机过程，运动学位置控制和主动柔顺力控制切换机制合理。

Description

一种XYZR四轴钻孔机器人主动柔顺控制装置及方法

技术领域

本发明属于机器人控制领域，尤其涉及一种XYZR四轴钻孔机器人主动柔顺控制装置及方法。

背景技术

目前箱包钻孔机器人机构设计往往采用传统机器人机构，如龙门式机器人、三轴XYZ直角坐标机器人、四轴XYZR钻孔机器人以及六轴关节型机器人等，机器人控制***往往采用基于运动学的控制方案，即以打孔位置为控制目标进行工作，比较适合用于厚板或块状等不易变形金属、塑料板、亚克力板等材料的钻孔。

现有研究中的主动柔顺控制机制依靠六维力传感器实现力和力矩的检测，依靠机器视觉传感器检测机器人当前位姿，综合上述信息设计位置/力混合控制器。也有研究靠ETHERCAT主站将机器人驱动器采集的电机周期性数据与设定的期望值进行叠加并进行阻抗控制,输出参考力矩值至机器人驱动，适用于接触性作业应用。或者在工业机器人上安装柔性研抛刀具,计算机通过机器人通讯接口与机器人控制器进行双向通讯连接,机器人控制器控制工业机器人动作,由柔性研抛刀具对固定于工作台上的工件实施研抛加工，力传感器将采集的研抛力数据做为闭环反馈信号送至计算机处理，且其关键技术在于适用多个力传感器感知环境作用力，使用绳驱机器人实现阻抗控制，适用于柔性管臂型环境作业。

基于位置控制的钻孔机器人在给薄片型易变形金属钻孔时，采用基于运动学的控制方案，以打孔位置为控制目标进行工作，推动钻头的力及加速度在控制动态过程中波动较大、无法保持在合理的特定时变值上，因此会导致钻孔对象发生变形、孔形破坏、毛刺过多不合格；目前主动柔顺控制方案多适用于表面接触式并且需要保持特定压力的工作场合，对于钻孔等穿破材料表面的工作场合不宜直接适用；目前主动柔顺控制方案多采用价格昂贵的力传感器作为力检测装置，成本较高。

发明内容

为了解决上述问题，本发明提出了一种XYZR四轴钻孔机器人主动柔顺控制装置，其控制对象为四路伺服电机模块、电钻模块，所述装置包括主控模块、伺服驱动控制模块以及人机交互模块；主控模块与伺服驱动控制模块相互连接，伺服驱动控制模块与电钻模块相连，主控模块还与人机交互模块相互连接；

所述主控模块用于完成运动学轨迹规划、主动柔顺控制决策、专家经验学习、加速度-力-电流离线匹配、加速度-力-电流在线匹配、通信管理、IO管理、驱动数据交互；

所述伺服驱动控制模块用于实时控制四路伺服电机模块；

所述电钻模块属于XYZR四轴钻孔机器人的基本部件，包括受控于主控模块的变频器和主轴电钻，用于控制主轴电钻，完成钻孔；

所述人机交互模块用于操作人员进行任务编辑、指令发送、显示机器人工作状态以及完成与主控模块通信。

一种XYZR四轴钻孔机器人主动柔顺控制装置，所述主控模块包括运动学轨迹规划子模块、主动柔顺控制决策子模块、专家经验学习子模块、加速度-力-电流离线匹配子模块，加速度-力-电流在线匹配子模块、RS485通信子模块、IO控制管理子模块、电源管理子模块、安全控制子模块、驱动数据交互子模块、工具工件标定子模块、PC调试通信子模块、SD卡读写子模块；

专家经验学习子模块，加速度-力-电流离线匹配子模块，加速度-力-电流在线匹配子模块，工具工件标定子模块关系为顺序依次串联关系，通过数据通信连接，将上述模块定义为第一类子模块，第一类子模块在***上线运行前只需要运行一次，生成必要的控制数据供后续使用；运动学轨迹规划子模块、主动柔顺控制决策子模块、IO控制管理子模块、驱动数据交互子模块关系为顺序依次串联关系，通过数据通信连接，将这些子模块定义为第二类子模块，第二类子模块为在***上线运行时必须运行的模块，第二类子模块使用第一类子模块生成的数据，两者之间通过数据通信连接；将RS485通信子模块、PC调试通信子模块定义为第三类子模块，第三类子模块用于人机交互操作，与其他类子模块之间的关系为并发同步关系；将电源管理子模块、安全控制子模块、SD卡读写子模块定义为第四类子模块，第四类子模块与其他类子模块关系为并发同步关系。

主控模块由基于INTEL X86硬件平台的WINDOWS***架构工控机或基于INTEL X86硬件平台的LINUX***架构工控机或基于INTEL X86硬件平台的VXWORKS***架构工控机或基于ARM架构的嵌入式控制平台构成。

所述人机交互模块包括任务编辑子模块、状态监控子模块、指令收发子模块和串口通信子模块；

人机交互模块中，任务编辑子模块、状态监控子模块为并发同步关系，两者分别与指令收发子模块、串口通信子模块通过数据线进行顺序依次串联；

人机交互模块由基于独立操作***的示教平台或基于与主控***共享操作***的示教平台或基于单片机的独立示教平台构成。

所述伺服驱动控制模块包括主控数据交互模块与伺服电机1驱动控制模块、伺服电机2驱动控制模块、伺服电机3驱动控制模块、伺服电机4驱动控制模块连接，伺服电机1驱动控制模块、伺服电机2驱动控制模块、伺服电机3驱动控制模块、伺服电机4驱动控制模块分别与伺服电机1驱动放大模块、伺服电机2驱动放大模块、伺服电机3驱动放大模块、伺服电机4驱动放大模块连接，然后再分别与四路伺服电机模块连接；四路伺服电机模块包括伺服电机1、伺服电机2、伺服电机3、伺服电机4，是伺服驱动控制模块的直接控制对象，也是构成XYZR四轴钻孔机器人的基本原动部件；

伺服驱动控制模块由多路带有电流控制、位置控制的完全独立式伺服驱动器或多片DSP或FPGA构成的相对一体式伺服驱动器或一片DSP构成的绝对一体式伺服驱动器或一片FPGA构成的绝对一体式伺服驱动器构成。

一种基于XYZR四轴钻孔机器人主动柔顺控制方法，具体为：

步骤1、操作人员通过人机交互模块与主控模块进行交互，控制机器人完成钻孔过程专家经验学习；

步骤2、通过人机交互模块命令***完成加速度-力-电流离线和在线匹配，确定驱动电流与参考加速度的比例参数，通过驱动数据交互子模块发送给伺服驱动控制模块使用；

步骤3、通过人机交互模块对机器人工作任务进行示教；

步骤4、主控模块根据示教任务完成运动学轨迹规划，生成驱动数据发送给伺服驱动控制模块参考执行；

步骤5、主控模块根据伺服驱动控制模块反馈的机器人状态进行主动柔顺控制决策，确定使用的位置控制参数和柔顺控制参数，通过驱动数据交互子模块发送给伺服驱动控制模块参考执行；

步骤6、主控模块同时完成IO控制管理子模块的管理和决策；

步骤7、伺服驱动控制模块根据主控指令完成各轴伺服电机的实时控制；

步骤8、电钻模块根据IO控制管理子模块的状态启停电钻，进行钻孔。

所述专家经验学习的过程即专家经验学习子模块的工作的具体步骤为：

步骤101、将电钻模块中的加速度传感器串口数据线连接到***PC调试串口，通过人机交互模块启动钻孔过程专家经验学习子模块，采集N组高级技术工人钻孔过程的钻孔时间和该时间段内加速度幅值数据，其中N>100；

步骤102、计算采集的N组数据的钻孔时间的平均值、方差以及这些数据的97.5％置信区间；

步骤103、检查N组数据的钻孔时间是否在步骤102中所述置信区间内，如果在保留，否则剔除，经此过程剩下的数据假设为N₁组；

步骤104、对步骤103中的N₁组数据进行时间归一化，钻孔时间大于平均时间的部分，按照平均时间进行数据压缩，钻孔时间小于平均时间的部分，以平均时间为基准进行数据扩展；

步骤105、对步骤104中得出的数据进行平滑处理，处理方法采用滑动平均法，之后将N₁组数据按采样时间点取平均，得到时间归一化后的平均钻孔加速度数据；

步骤106、对步骤105中得出的数据进行差分运算，根据差分结果将数据分为加加速数据、匀加速数据和减加速数据，三类数据对应的时间段分别为加加速时间段、匀加速时间段和减加速时间段。

所述步骤2中确定驱动电流与参考加速度的比例参数的具体过程为

加速度-力-电流离线匹配子模块和加速度-力-电流在线匹配子模块将打孔过程中需要控制的加速度通过力的关系转化为对推进电流的控制；在***样机研发阶段，将电钻模块中的加速度传感器的串口数据线连接到***PC调试串口，通过人机交互模块启动在线加速度-力-电流匹配功能，然后加速度-力-电流离线匹配子模块将实际采集到的加速度值与参考加速度进行比较，形成负反馈，调节驱动电流与参考加速度的比例参数，完成离线匹配；加速度-力-电流在线匹配子模块在首次实际试钻孔前，启用加速度-力-电流离线匹配子模块中的驱动电流与参考加速度的比例参数，控制驱动电流推动钻机前进，同时通过机器人当前位姿状态检测钻机位置进行二阶差分采集钻机推进的实际加速度，然后将该实际采集到的加速度值与参考加速度进行比较，形成负反馈，更新驱动电流与参考加速度的比例参数，完成在线匹配。

所述步骤4的具体过程为：

主控模块中的运动学轨迹规划子模块为实现机器人位置运动过程即非钻孔过程的运动时间同步性和运动过程平滑性，将当前各轴电机运动时间进行统一并通过分段控制实现电机速度平滑运动；

步骤401、将电机所要运动的过程分为7段，即加加速、匀加速、减加速、匀速、加减速、匀减速、减减速，设为加加速段时间、为匀加速段时间、为减加速段时间、为匀速段时间、为加减速段时间、为匀减速段时间、为减减速段时间，表示电机轴的编号；

步骤402、将该运动过程的起点速度v_s、终点速度v_e、最大速度v_m、最大加速度a_m、最大加加速度j_m和总位移S作为规划约束条件；

步骤403、规划第一轴即X轴变速段运动时间，判断是否含有均匀变速段，其中加速过程为：

若

则

t_1-3＝t_1-1；

若

则

t_1-2＝0,t_1-3＝t_1-1，则加速过程中的实际最大加速度没有达到限定值a_m；

按照求出的时间对应各个时间段的加速度进行积分，求出各段的相应速度，再对各段相应速度积分，求出加速段位移S_acc；

步骤404、变速段规划中的减速过程利用对称性得出，减速段位移S_dec＝S_acc,t_1-6＝t_1-2,t_1-5＝t_1-7＝t_1-3＝t_1-1；

步骤405、判断运动过程中有无匀速段，判断准则为如果S_dec+S_acc<S则含有匀速段，否则不含有匀速段；

步骤406、如果有匀速段，匀速段时间为

如果没有匀速段，匀速段时间t_1-4＝0；

步骤407、重复步骤103-106，至完成第二、三、四轴的规划；

步骤408、比较各轴的总体运行时间，取最大值为各轴同步运行总时间；

根据约束方程

求出t₂和t₄；

其中，、和分别为四轴电机同步运动约束后的各轴电机加加速、匀加速和匀速的时间；

若约束方程无解或t₂<0，则认为不存在加加速段和减减速段，此时约束方程转化为

根据该方程求出t₁和t₄，另外根据对称性t₇＝t₅＝t₃＝t₁,t₆＝t₂＝0；、、和分别为四轴电机同步运动约束后的各轴电机减加速、加减速、匀减速和减减速的时间；和是相应加加速和减加速阶段电机的位移；

步骤409、将上述求得的各个段时间及相应段上的位置量和速度量作为驱动数据传输至伺服驱动控制模块执行。

所述步骤5中的主动柔顺控制决策即根据当前机器人任务状态和机器人当前实际位姿状态调整主动柔顺力控制和运动学位置控制的权重因子，具体过程为：

主动柔顺控制决策子模块的输入量为人机交互的任务状态和机器人当前位姿状态，当人机交互任务为钻孔且机器人当前状态靠近或已经位于钻孔区域时，主动柔顺控制决策子模块提高柔顺控制权重、降低位置控制权重，进而生成控制参数调整指令：使用大柔顺控制参数、小位置控制参数；当人机交互任务为运动或机器人当前状态离钻孔区域较远时，主动柔顺控制决策子模块降低柔顺控制权重、提高位置控制权重，进而生成控制参数调整指令：使用小柔顺控制参数、大位置控制参数；其中柔顺控制参数和位置控制参数为事先调好并保存于***中的参数；柔顺控制参数包括大柔顺控制参数和小柔顺控制参数，位置控制参数包括大位置控制参数和小位置控制参数。

所述步骤7中的主控指令由运动学轨迹规划子模块和主动柔顺控制决策子模块生成，主控指令包括各轴电机参考位置、钻机推进电流和控制参数调整指令；

伺服驱动控制模块内有至少两套离线调试完成的位置环、电流环参数和一套速度环参数，其中位置环1、电流环1参数适合进行精准的运动学位置控制，位置环2、电流环2参数适合进行钻机推进时的主动柔顺力电流控制，速度环在两套控制参数中是一样的；当主控指令中控制参数调整指令为使用大柔顺控制参数、小位置控制参数时，伺服驱动控制模块选用位置环2、速度环、电流环2构成的控制模块，此时主控指令中钻机推进电流为主要控制目标，运动学位置控制此时主要起限制保护作用，防止钻机推进过程产生碰撞等意外发生；当主控指令中控制参数调整指令为使用小柔顺控制参数、大位置控制参数时，伺服驱动控制模块选用位置环1、速度环、电流环1构成的控制模块，此时主控指令中的电机参考位置控制为主要控制目标，实现***的快速精确运动。

有益效果

本发明采用力/位置混合控制架构的主动柔顺控制机制作为XYZR四轴钻孔机器人控制方案，在机器人点位运动控制过程中，以位置控制为主，保证运动过程的速度、平滑；在打孔过程中，以检测位置信息为切换阈值的力控制为主，保证钻孔过程中机器人推动钻头的加速度严格控制在期望值偏差内；为避免使用昂贵的力传感器，在主动柔顺控制过程中将加速度控制指标转为力控制指标、再将力控制指标转为电流控制指标进行设计，利用机器人控制***均带有的电流检测传感器作为检测手段进行感知，使用电流环进行控制得到期望电流，驱动机器人工作，实现主动柔顺控制；设计了一套自动离线/在线匹配加速度-力-电流的流程和装置，避免了加速度-力-电流之间耗时繁琐的手动匹配和计算过程；基于钻孔专家经验学***滑性；将主动柔顺控制决策子模块单独进行设计，不再将该部分混合在控制律中，具有清晰明朗、简单易于实现的优点；获取钻机推进过程中相关数据时避免了复杂的钻机推进过程建模和无指导性规律的重复拼凑实验，过程简单、实现方便、节约时间；避免每台机器人都需要在线使用昂贵的加速段传感器和力传感器，节约成本，方便用户使用；***控制参数是离线调试好的，避免在线调整控制参数引入的不稳定因素和***宕机过程，运动学位置控制和主动柔顺力控制切换机制合理，结构实现简单、安全性高、通用性强。

附图说明

图1为一种XYZR四轴钻孔机器人主动柔顺控制装置的组成示意图。

图2为主动柔顺控制决策子模块的工作过程示意图。

图3为伺服驱动控制模块的控制框图。

具体实施方式

下面结合附图，对本发明作详细说明。本发明提出了一种XYZR四轴钻孔机器人主动柔顺控制装置，如图1所示，其控制对象为四路伺服电机模块、电钻模块，所述装置包括主控模块、伺服驱动控制模块以及人机交互模块；主控模块与伺服驱动控制模块相互连接，伺服驱动控制模块与电钻模块相连，主控模块还与人机交互模块相互连接；

所述主控模块用于完成运动学轨迹规划、主动柔顺控制决策、专家经验学习、加速度-力-电流离线匹配、加速度-力-电流在线匹配、通信管理、IO管理、驱动数据交互；

所述伺服驱动控制模块用于实时控制四路伺服电机模块；

所述电钻模块属于XYZR四轴钻孔机器人的基本部件，包括受控于主控模块的变频器和主轴电钻，用于控制主轴电钻，完成钻孔；

所述人机交互模块用于操作人员进行任务编辑、指令发送、显示机器人工作状态以及完成与主控模块通信。

所述主控模块包括运动学轨迹规划子模块、主动柔顺控制决策子模块、专家经验学习子模块、加速度-力-电流离线匹配子模块，加速度-力-电流在线匹配子模块、RS485通信子模块、IO控制管理子模块、电源管理子模块、安全控制子模块、驱动数据交互子模块、工具工件标定子模块、PC调试通信子模块、SD卡读写子模块；

主控模块由基于INTEL X86硬件平台的WINDOWS***架构工控机或基于INTEL X86硬件平台的LINUX***架构工控机或基于INTEL X86硬件平台的VXWORKS***架构工控机或基于ARM架构的嵌入式控制平台构成。

所述人机交互模块包括任务编辑模块、状态监控模块、指令收发模块和串口通信模块；

人机交互模块由基于独立操作***的示教平台或基于与主控***共享操作***的示教平台或基于单片机的独立示教平台构成。

所述伺服驱动控制模块包括主控数据交互模块与伺服电机1驱动控制模块、伺服电机2驱动控制模块、伺服电机3驱动控制模块、伺服电机4驱动控制模块连接，伺服电机1驱动控制模块、伺服电机2驱动控制模块、伺服电机3驱动控制模块、伺服电机4驱动控制模块分别与伺服电机1驱动放大模块、伺服电机2驱动放大模块、伺服电机3驱动放大模块、伺服电机4驱动放大模块连接，然后再分别与四路伺服电机模块连接；四路伺服电机模块包括伺服电机1、伺服电机2、伺服电机3、伺服电机4，是伺服驱动模块的直接控制对象，也是构成XYZR四轴钻孔机器人的基本原动部件；

伺服驱动控制模块由多路带有电流控制、位置控制的完全独立式伺服驱动器或多片DSP或FPGA构成的相对一体式伺服驱动器或一片DSP构成的绝对一体式伺服驱动器或一片FPGA构成的绝对一体式伺服驱动器构成。

基于上述装置的XYZR四轴钻孔机器人主动柔顺控制方法，其具体过程为：

步骤1、操作人员通过人机交互模块与主控模块进行交互，控制机器人完成钻孔过程专家经验学习；

步骤101、将电钻模块中的加速度传感器串口数据线连接到***PC调试串口，通过人机交互模块启动钻孔过程专家经验学习子模块，采集N组高级技术工人钻孔过程的钻孔时间和该时间段内加速度幅值数据，其中N>100；

步骤102、计算采集的N组数据的钻孔时间的平均值、方差以及这些数据的97.5％置信区间；

步骤103、检查N组数据的钻孔时间是否在步骤102中所述置信区间内，如果在保留，否则剔除，经此过程剩下的数据假设为N₁组；

步骤104、对步骤103中的N₁组数据进行时间归一化，钻孔时间大于平均时间的部分，按照平均时间进行数据压缩，钻孔时间小于平均时间的部分，以平均时间为基准进行数据扩展；

步骤105、对步骤104中得出的数据进行平滑处理，处理方法采用滑动平均法，之后将N₁组数据按采样时间点取平均，得到时间归一化后的平均钻孔加速度数据；

步骤106、对步骤105中得出的数据进行差分运算，根据差分结果将数据分为加加速数据、匀加速数据和减加速数据，三类数据对应的时间段分别为加加速时间段、匀加速时间段和减加速时间段。

步骤2、通过人机交互模块命令***完成加速度-力-电流离线和在线匹配，确定驱动电流与参考加速度的比例参数，通过驱动数据交互子模块发送给伺服驱动模块使用；

加速度-力-电流离线匹配子模块和加速度-力-电流在线匹配子模块将打孔过程中需要控制的加速度通过力的关系转化为对推进电流的控制；在***样机研发阶段，将电钻模块中的加速度传感器的串口数据线连接到***PC调试串口，通过人机交互模块启动在线加速度-力-电流匹配功能，然后加速度-力-电流离线匹配子模块将实际采集到的加速度值与参考加速度进行比较，形成负反馈，调节驱动电流与参考加速度的比例参数，完成离线匹配；加速度-力-电流在线匹配子模块在首次实际试钻孔前，启用加速度-力-电流离线匹配子模块中的驱动电流与参考加速度的比例参数，控制驱动电流推动钻机前进，同时通过机器人当前位姿状态检测钻机位置进行二阶差分采集钻机推进的实际加速度，然后将该实际采集到的加速度值与参考加速度进行比较，形成负反馈，更新驱动电流与参考加速度的比例参数，完成在线匹配。

步骤3、通过人机交互模块对机器人工作任务进行示教；

步骤4、主控模块根据示教任务完成运动学轨迹规划，生成驱动数据发送给伺服驱动控制模块参考执行；

所述步骤4的具体过程为：

主控模块中的运动学轨迹规划子模块为实现机器人位置运动过程即非钻孔过程的运动时间同步性和运动过程平滑性，将当前各轴电机运动时间进行统一并通过分段控制实现电机速度平滑运动；

步骤401、将电机所要运动的过程分为7段，即加加速、匀加速、减加速、匀速、加减速、匀减速、减减速，设为加加速段时间、为匀加速段时间、为减加速段时间、为匀速段时间、为加减速段时间、为匀减速段时间、为减减速段时间，表示电机轴的编号；

步骤402、将该运动过程的起点速度v_s、终点速度v_e、最大速度v_m、最大加速度a_m、最大加加速度j_m和总位移S作为规划约束条件；

步骤403、规划第一轴即X轴变速段运动时间，判断是否含有均匀变速段，其中加速过程为：

若

则

t_1-3＝t_1-1；

若

则

t_1-2＝0,t_1-3＝t_1-1，则加速过程中的实际最大加速度没有达到限定值a_m；

按照求出的时间对应各个时间段的加速度进行积分，求出各段的相应速度，再对各段相应速度积分，求出加速段位移S_acc；

步骤404、变速段规划中的减速过程利用对称性得出，减速段位移S_dec＝S_acc,t_1-6＝t_1-2,t_1-5＝t_1-7＝t_1-3＝t_1-1；

步骤405、判断运动过程中有无匀速段，判断准则为如果S_dec+S_acc<S则含有匀速段，否则不含有匀速段；

步骤406、如果有匀速段，匀速段时间为

如果没有匀速段，匀速段时间t_1-4＝0；

步骤407、重复步骤103-106，至完成第二、三、四轴的规划；

步骤408、比较各轴的总体运行时间，取最大值为各轴同步运行总时间；

根据约束方程

求出t₂和t₄；

其中，、和分别为四轴电机同步运动约束后的各轴电机加加速、匀加速和匀速的时间；

若约束方程无解或t₂<0，则认为不存在加加速段和减减速段，此时约束方程转化为

根据该方程求出t₁和t₄，另外根据对称性t₇＝t₅＝t₃＝t₁,t₆＝t₂＝0；、、和分别为四轴电机同步运动约束后的各轴电机减加速、加减速、匀减速和减减速的时间；和是相应加加速和减加速阶段电机的位移；

步骤409、将上述求得的各个段时间及相应段上的位置量和速度量作为驱动数据传输至伺服驱动控制模块执行。

步骤5、主控模块根据伺服驱动控制模块反馈的机器人状态进行主动柔顺控制决策，确定使用的位置控制参数和柔顺控制参数，通过驱动数据交互子模块发送给伺服驱动模块参考执行；

主动柔顺控制决策即根据当前机器人任务状态和机器人当前实际位姿状态调整主动柔顺力控制和运动学位置控制的权重因子，具体过程如图2所示：

主动柔顺控制决策子模块的输入量为人机交互的任务状态和机器人当前位姿状态，当人机交互任务为钻孔且机器人当前状态靠近或已经位于钻孔区域时，主动柔顺控制决策子模块提高柔顺控制权重、降低位置控制权重，进而生成控制参数调整指令：使用大柔顺控制参数、小位置控制参数；当人机交互任务为运动或机器人当前状态离钻孔区域较远时，主动柔顺控制决策子模块降低柔顺控制权重、提高位置控制权重，进而生成控制参数调整指令：使用小柔顺控制参数、大位置控制参数；其中柔顺控制参数和位置控制参数为事先调好并保存于***中的参数；柔顺控制参数包括大柔顺控制参数和小柔顺控制参数，，位置控制参数包括大位置控制参数和小位置控制参数，。

步骤6、主控模块同时完成IO控制管理子模块的管理和决策；

步骤7、伺服驱动模块根据主控指令完成各轴伺服电机的实时控制；主控指令由运动学轨迹规划子模块和主动柔顺控制决策子模块生成，主控指令包括各轴电机参考位置、钻机推进电流和控制参数调整指令；如图3所示，伺服驱动控制模块内有至少两套离线调试完成的位置环、电流环参数和一套速度环参数，其中位置环1、电流环1参数适合进行精准的运动学位置控制，位置环2、电流环2参数适合进行钻机推进时的主动柔顺力电流控制，速度环在两套控制参数中是一样的；当主控指令中控制参数调整指令为使用大柔顺控制参数、小位置控制参数时，伺服驱动控制模块选用位置环2、速度环、电流环2构成的控制模块，此时主控指令中钻机推进电流为主要控制目标，运动学位置控制此时主要起限制保护作用，防止钻机推进过程产生碰撞等意外发生；当主控指令中控制参数调整指令为使用小柔顺控制参数、大位置控制参数时，伺服驱动控制模块选用位置环1、速度环、电流环1构成的控制模块，此时主控指令中的电机参考位置控制为主要控制目标，实现***的快速精确运动。

步骤8、电钻模块根据IO控制管理子模块的状态启停电钻，进行钻孔。

Claims (9)

1.一种XYZR四轴钻孔机器人主动柔顺控制装置，其控制对象为四路伺服电机模块、电钻模块，其特征在于，所述装置包括主控模块、伺服驱动控制模块以及人机交互模块；主控模块与伺服驱动控制模块相互连接，伺服驱动控制模块与电钻模块相连，主控模块还与人机交互模块相互连接；

所述主控模块用于完成运动学轨迹规划、主动柔顺控制决策、专家经验学习、加速度-力-电流离线匹配、加速度-力-电流在线匹配、通信管理、IO管理、驱动数据交互；

所述伺服驱动控制模块用于实时控制四路伺服电机模块；

所述电钻模块属于XYZR四轴钻孔机器人的基本部件，包括受控于主控模块的变频器和主轴电钻，用于控制主轴电钻，完成钻孔；

所述人机交互模块用于操作人员进行任务编辑、指令发送、显示机器人工作状态以及完成与主控模块通信；

所述主控模块包括加速度-力-电流离线匹配子模块，加速度-力-电流在线匹配子模块，其中，所述加速度-力-电流离线匹配子模块将实际采集到的加速度值与参考加速度进行比较，形成负反馈，调节驱动电流与参考加速度的比例参数，完成离线匹配；所述加速度-力-电流在线匹配子模块在首次实际试钻孔前，启用加速度-力-电流离线匹配子模块中的驱动电流与参考加速度的比例参数，控制驱动电流推动钻机前进，同时通过机器人当前位姿状态检测钻机位置进行二阶差分采集钻机推进的实际加速度，然后将该实际采集到的加速度值与参考加速度进行比较，形成负反馈，更新驱动电流与参考加速度的比例参数，完成在线匹配。

2.根据权利要求1所述的一种XYZR四轴钻孔机器人主动柔顺控制装置，其特征在于，所述主控模块还包括运动学轨迹规划子模块、主动柔顺控制决策子模块、专家经验学习子模块、RS485通信子模块、IO控制管理子模块、电源管理子模块、安全控制子模块、驱动数据交互子模块、工具工件标定子模块、PC调试通信子模块、SD卡读写子模块；

专家经验学习子模块，加速度-力-电流离线匹配子模块，加速度-力-电流在线匹配子模块，工具工件标定子模块关系为顺序依次串联关系，通过数据通信连接，将上述模块定义为第一类子模块，第一类子模块在***上线运行前只需要运行一次，生成必要的控制数据供后续使用；运动学轨迹规划子模块、主动柔顺控制决策子模块、IO控制管理子模块、驱动数据交互子模块关系为顺序依次串联关系，通过数据通信连接，将这些子模块定义为第二类子模块，第二类子模块为在***上线运行时必须运行的模块，第二类子模块使用第一类子模块生成的数据，两者之间通过数据通信连接；将RS485通信子模块、PC调试通信子模块定义为第三类子模块，第三类子模块用于人机交互操作，与其他类子模块之间的关系为并发同步关系；将电源管理子模块、安全控制子模块、SD卡读写子模块定义为第四类子模块，第四类子模块与其他类子模块关系为并发同步关系；

主控模块由基于INTEL X86硬件平台的WINDOWS***架构工控机或基于INTEL X86硬件平台的LINUX***架构工控机或基于INTEL X86硬件平台的VXWORKS***架构工控机或基于ARM架构的嵌入式控制平台构成。

3.根据权利要求1所述的一种XYZR四轴钻孔机器人主动柔顺控制装置，其特征在于，所述人机交互模块包括任务编辑子模块、状态监控子模块、指令收发子模块和串口通信子模块；

人机交互模块中，任务编辑子模块、状态监控子模块为并发同步关系，两者分别与指令收发子模块、串口通信子模块通过数据线进行顺序依次串联；

人机交互模块由基于独立操作***的示教平台或基于与主控***共享操作***的示教平台或基于单片机的独立示教平台构成。

4.根据权利要求1所述的一种XYZR四轴钻孔机器人主动柔顺控制装置，其特征在于，所述伺服驱动控制模块包括主控数据交互模块与伺服电机1驱动控制模块、伺服电机2驱动控制模块、伺服电机3驱动控制模块、伺服电机4驱动控制模块连接，伺服电机1驱动控制模块、伺服电机2驱动控制模块、伺服电机3驱动控制模块、伺服电机4驱动控制模块分别与伺服电机1驱动放大模块、伺服电机2驱动放大模块、伺服电机3驱动放大模块、伺服电机4驱动放大模块连接，然后再分别与四路伺服电机模块连接；四路伺服电机模块包括伺服电机1、伺服电机2、伺服电机3、伺服电机4，是伺服驱动控制模块的直接控制对象，也是构成XYZR四轴钻孔机器人的基本原动部件；

伺服驱动控制模块由多路带有电流控制、位置控制的完全独立式伺服驱动器或多片DSP或FPGA构成的相对一体式伺服驱动器或一片DSP构成的绝对一体式伺服驱动器或一片FPGA构成的绝对一体式伺服驱动器构成。

5.一种基于权利要求1所述装置的XYZR四轴钻孔机器人主动柔顺控制方法，其特征在于，所述方法具体为：

步骤1、操作人员通过人机交互模块与主控模块进行交互，控制机器人完成钻孔过程专家经验学习；

步骤2、通过人机交互模块命令***完成加速度-力-电流离线和在线匹配，确定驱动电流与参考加速度的比例参数，通过驱动数据交互子模块发送给伺服驱动控制模块使用；

步骤3、通过人机交互模块对机器人工作任务进行示教；

步骤4、主控模块根据示教任务完成运动学轨迹规划，生成驱动数据发送给伺服驱动控制模块参考执行；

步骤5、主控模块根据伺服驱动控制模块反馈的机器人状态进行主动柔顺控制决策，确定使用的位置控制参数和柔顺控制参数，通过驱动数据交互子模块发送给伺服驱动控制模块参考执行；

步骤6、主控模块同时完成IO控制管理子模块的管理和决策；

步骤7、伺服驱动控制模块根据主控指令完成各轴伺服电机的实时控制；

步骤8、电钻模块根据IO控制管理子模块的状态启停电钻，进行钻孔。

6.根据权利要求5所述的方法，其特征在于，所述专家经验学习的过程即专家经验学习子模块的工作的具体步骤为：

步骤101、将电钻模块中的加速度传感器串口数据线连接到***PC调试串口，通过人机交互模块启动钻孔过程专家经验学习子模块，采集N组高级技术工人钻孔过程的钻孔时间和该时间段内加速度幅值数据，其中N>100；

步骤102、计算采集的N组数据的钻孔时间的平均值、方差以及这些数据的97.5％置信区间；

步骤103、检查N组数据的钻孔时间是否在步骤102中所述置信区间内，如果在保留，否则剔除，经此过程剩下的数据假设为N₁组；

步骤104、对步骤103中的N₁组数据进行时间归一化，钻孔时间大于平均时间的部分，按照平均时间进行数据压缩，钻孔时间小于平均时间的部分，以平均时间为基准进行数据扩展；

步骤105、对步骤104中得出的数据进行平滑处理，处理方法采用滑动平均法，之后将N₁组数据按采样时间点取平均，得到时间归一化后的平均钻孔加速度数据；

步骤106、对步骤105中得出的数据进行差分运算，根据差分结果将数据分为加加速数据、匀加速数据和减加速数据，三类数据对应的时间段分别为加加速时间段、匀加速时间段和减加速时间段。

7.根据权利要求5所述的方法，其特征在于，所述步骤2中确定驱动电流与参考加速度的比例参数的具体过程为

加速度-力-电流离线匹配子模块和加速度-力-电流在线匹配子模块将打孔过程中需要控制的加速度通过力的关系转化为对推进电流的控制；在***样机研发阶段，将电钻模块中的加速度传感器的串口数据线连接到***PC调试串口，通过人机交互模块启动在线加速度-力-电流匹配功能，然后加速度-力-电流离线匹配子模块将实际采集到的加速度值与参考加速度进行比较，形成负反馈，调节驱动电流与参考加速度的比例参数，完成离线匹配；加速度-力-电流在线匹配子模块在首次实际试钻孔前，启用加速度-力-电流离线匹配子模块中的驱动电流与参考加速度的比例参数，控制驱动电流推动钻机前进，同时通过机器人当前位姿状态检测钻机位置进行二阶差分采集钻机推进的实际加速度，然后将该实际采集到的加速度值与参考加速度进行比较，形成负反馈，更新驱动电流与参考加速度的比例参数，完成在线匹配。

8.根据权利要求5所述的方法，其特征在于，所述步骤5中的主动柔顺控制决策即根据当前机器人任务状态和机器人当前实际位姿状态调整主动柔顺力控制和运动学位置控制的权重因子，具体过程为：

主动柔顺控制决策子模块的输入量为人机交互的任务状态和机器人当前位姿状态，当人机交互任务为钻孔且机器人当前状态靠近或已经位于钻孔区域时，主动柔顺控制决策子模块提高柔顺控制权重、降低位置控制权重，进而生成控制参数调整指令：使用大柔顺控制参数、小位置控制参数；当人机交互任务为运动或机器人当前状态离钻孔区域较远时，主动柔顺控制决策子模块降低柔顺控制权重、提高位置控制权重，进而生成控制参数调整指令：使用小柔顺控制参数、大位置控制参数；其中柔顺控制参数和位置控制参数为事先调好并保存于***中的参数；柔顺控制参数包括大柔顺控制参数和小柔顺控制参数，位置控制参数包括大位置控制参数和小位置控制参数。

9.根据权利要求5所述的方法，其特征在于，所述步骤7中的主控指令由运动学轨迹规划子模块和主动柔顺控制决策子模块生成，主控指令包括各轴电机参考位置、钻机推进电流和控制参数调整指令；

伺服驱动控制模块内有至少两套离线调试完成的位置环、电流环参数和一套速度环参数，其中位置环1、电流环1参数适合进行精准的运动学位置控制，位置环2、电流环2参数适合进行钻机推进时的主动柔顺力电流控制，速度环在两套控制参数中是一样的；当主控指令中控制参数调整指令为使用大柔顺控制参数、小位置控制参数时，伺服驱动控制模块选用位置环2、速度环、电流环2构成的控制模块，此时主控指令中钻机推进电流为主要控制目标，运动学位置控制此时主要起限制保护作用，防止钻机推进过程产生碰撞等意外发生；当主控指令中控制参数调整指令为使用小柔顺控制参数、大位置控制参数时，伺服驱动控制模块选用位置环1、速度环、电流环1构成的控制模块，此时主控指令中的电机参考位置控制为主要控制目标，实现***的快速精确运动。

Images