一种基于力位双闭环的螺丝嵌件定位控制方法及系统与流程

摘要: 本发明涉及工业机器人装配,具体涉及一种基于力位双闭环的螺丝嵌件定位控制方法及系统。背景技术:1、螺丝嵌件的压入过程受到螺丝嵌件与装配孔同轴度、装配孔尺寸公差、装...
一种基于力位双闭环的螺丝嵌件定位控制方法及系统与流程

本发明涉及工业机器人装配,具体涉及一种基于力位双闭环的螺丝嵌件定位控制方法及系统。

背景技术:

1、螺丝嵌件的压入过程受到螺丝嵌件与装配孔同轴度、装配孔尺寸公差、装配基体材料刚度、装配基体局部壁厚、压入速度、压入温度、夹持机构稳定性及机器人定位误差的共同影响。单纯按照目标位置执行压入时,难以及时判断螺丝嵌件是否与装配孔的单侧孔壁接触,也难以区分单侧接触偏心、装配基体局部低刚度响应、局部高刚度阻滞和夹持松动等不同状态。部分装配设备利用六维力传感器检测法向力、切向力或力矩,但通常按照采样时间或单一载荷阈值判断接触状态,不同压入速度、暂停时间和减速过程会使相同压入深度对应不同的采样时刻,而相同大小的法向力还可能对应周向干涉压入、单侧孔壁接触或底端接近,因而不能仅凭法向力确定接触位置和异常状态。若将全部载荷异常均作为位置偏心进行横向找正,则在装配基体存在局部减薄、孔洞或装配孔内存在异物时仍可能继续施加载荷。固定载荷范围也难以反映不同压入深度、温度和速度条件下的载荷变化,未经质量检测的数据若直接用于更新参考范围,还可能使异常压入数据进入后续判定过程。因此,现有处理方式尚不能在统一压入深度条件下结合接触载荷作用位置、装配基体局部力学响应和压入历程确定对应控制状态,也不能在参考数据更新后出现质量失效率升高时自动恢复前一参考版本。

技术实现思路

1、本发明提供了一种基于力位双闭环的螺丝嵌件定位控制方法及系统,本发明要解决的技术问题是如何在螺丝嵌件的压入过程中减少不同压入速度和运动停顿造成的载荷位置错位,根据六维载荷、实际压入位置和同工况合格参考数据区分不同接触状态及异常响应,并生成满足当前接触状态运动边界的对应控制轨迹。

2、为解决上述技术问题,本发明所采用的技术方案是:

3、一种基于力位双闭环的螺丝嵌件定位控制方法,包括:获取六维力传感器采集的载荷和机器人末端执行器实际位置,对所述载荷执行时间同步、误差补偿、坐标转换和滤波,得到压入接触载荷净值序列;根据法向力确定初始接触位置,结合目标压入位置生成压入深度相位,对所述压入接触载荷净值序列和所述机器人末端执行器实际位置进行相位对齐,得到相位对齐载荷位置序列;根据所述相位对齐载荷位置序列中的法向力和2个切向力矩确定接触作用线偏移估计向量,根据所述接触作用线偏移估计向量与切向力向量之间的方向一致程度、所述压入深度相位及偏移方向连续性确定接触状态,并生成接触状态运动约束集合;根据所述相位对齐载荷位置序列确定单位压入深度载荷增量、装配基体局部刚度估计值和累计压入功,与匹配螺丝嵌件规格、装配孔规格、装配基体材料、压入温度区间及压入速度区间的合格压入相位包络进行相位对应比较,得到相位包络标准化残差,并结合所述接触状态确定异常原因标识;根据所述异常原因标识生成期望运动增量,将所述期望运动增量和所述接触状态运动约束集合输入受约束轨迹求解器,得到并执行满足运动约束的目标压入轨迹;将执行期间重新采集的载荷和机器人末端执行器实际位置反馈至相位对齐载荷位置序列生成过程,以更新下一控制周期的目标压入轨迹。

4、在本发明的一方面,所述得到相位对齐载荷位置序列,包括:

5、将六维力传感器时间戳映射至机器人控制器时钟,将机器人末端执行器实际位置和机器人末端执行器实际姿态插值至各六维力传感器采样时刻,并在六维力传感器的原始载荷采样序列上依次执行灵敏度补偿、静态偏置补偿、工具重力载荷补偿、接触坐标系转换和低通滤波;

6、按照接触状态判定周期提取滤波后的三轴力、三轴力矩及对应的机器人末端执行器实际位置和机器人末端执行器实际姿态,得到压入接触载荷净值序列;

7、当所述压入接触载荷净值序列中的法向力连续达到初始接触阈值的控制周期数量达到初始接触确认周期数量时,将所述连续控制周期中第一个达到所述初始接触阈值的控制周期对应的机器人末端执行器实际位置确定为初始接触位置;在完成初始接触确认前,所述法向力降低至初始接触释放阈值以下时,清除初始接触计数;

8、分别确定当前机器人末端执行器实际位置和所述目标压入位置相对于所述初始接触位置在压入方向上的投影距离,将2个投影距离的比值确定为压入深度相位,并将所述压入深度相位限制在0至1;按照预设相位间隔,对首次正向越过各相位节点时的压入接触载荷净值序列和机器人末端执行器实际位置进行线性插值,得到相位对齐载荷位置序列,其中,停止期间不生成新的相位节点,回撤期间不反向覆盖已经形成的相位节点。

9、在本发明的一方面,所述确定接触作用线偏移估计向量及方向一致程度,包括:

10、从所述相位对齐载荷位置序列中提取法向力、2个切向力、2个切向力矩和法向力矩绝对值,将2个切向力矩统一转换为以n·mm表示的力矩值,并将所述2个切向力矩换算至随当前压入位置更新的接触评价原点;

11、在法向力连续达到初始接触阈值,且法向力矩绝对值和切向力向量模长未超过对应有效性阈值时,将第二切向力矩的相反数与经过法向力正则量修正的法向力之间的比值作为接触作用线偏移估计向量的第一偏移分量,将第一切向力矩与经过所述法向力正则量修正的法向力之间的比值作为所述接触作用线偏移估计向量的第二偏移分量;

12、分别对连续5个控制周期的第一偏移分量和第二偏移分量取中位数,将所得2个中位数组成滤波后的接触作用线偏移估计向量;在所述连续5个控制周期中有效接触作用线偏移估计向量少于3个时,不更新滤波结果;

13、根据所述滤波后的接触作用线偏移估计向量与切向力向量之间的负归一化内积确定方向一致程度;当相邻2个滤波后的接触作用线偏移估计向量的模长均达到可分辨作用线偏移距离,且2个向量的内积小于0时,记录1次偏移方向反转。

14、在本发明的一方面,所述确定接触状态并生成接触状态运动约束集合,包括:

15、在尚未满足初始接触确认条件时确定为未接触状态;根据六维力传感器切向力矩分辨率、初始接触阈值和法向力正则量确定可分辨作用线偏移距离,将螺丝嵌件外半径的10%与所述可分辨作用线偏移距离的3倍中的较大值确定为偏移阈值;

16、当所述接触作用线偏移估计向量模长不小于所述偏移阈值、所述方向一致程度不小于0.70、相邻接触作用线偏移估计向量未发生方向反转且连续保持3个控制周期时,确定为单侧孔壁接触状态;在不满足单侧孔壁接触状态时,当所述压入深度相位处于孔口进入区间、干涉建立区间或稳定压入区间且法向力高于初始接触阈值时,确定为周向干涉压入状态;在不满足单侧孔壁接触状态且所述压入深度相位进入底端接近区间时,确定为底端接近状态;

17、针对未接触状态禁止根据接触作用线偏移估计向量生成切向修正量;针对单侧孔壁接触状态限制压入方向位移增量、切向位移修正量及切向姿态修正量,并禁止沿所述接触作用线偏移估计向量的正方向运动;针对周向干涉压入状态,在异常原因标识不是单侧接触偏心状态时限制主动切向修正位移;针对底端接近状态降低压入方向位移增量,由此生成接触状态运动约束集合。

18、在本发明的一方面,所述确定单位压入深度载荷增量、装配基体局部刚度估计值和累计压入功,包括:

19、确定相邻控制周期之间的法向力增量以及相邻2个法向力增量之差的绝对值,根据所述绝对值的指数衰减映射结果,在法向力增量计算窗口长度下限与法向力增量计算窗口长度上限之间确定当前法向力增量计算窗口长度,使法向力变化平稳时延长所述当前法向力增量计算窗口长度,使法向力突变时缩短所述当前法向力增量计算窗口长度;

20、在机器人末端执行器沿压入方向正向运动且控制周期正向压入位移达到最小有效压入位移时,根据相邻控制周期的法向力差值与所述控制周期正向压入位移确定单位压入深度载荷增量,并在所述当前法向力增量计算窗口长度限定的计算窗口内,根据压入方向位置和法向力的线性最小二乘斜率确定装配基体局部刚度估计值;

21、当所述计算窗口中的有效控制周期数据点少于法向力增量计算窗口长度下限,或者所述计算窗口中的最大压入方向位置与最小压入方向位置之差未达到位置跨度阈值时,将所述装配基体局部刚度估计值标记为无效,并禁止利用所述无效的装配基体局部刚度估计值确定异常原因标识;

22、从所述初始接触位置开始,将各正向压入区间的法向力与对应正向压入位移的乘积依次累加,得到累计压入功,并禁止将停止、保持或回撤期间的数据用于所述单位压入深度载荷增量、所述装配基体局部刚度估计值及所述累计压入功的更新。

23、在本发明的一方面,所述确定异常原因标识,包括:

24、针对相同的螺丝嵌件规格、装配孔规格、装配基体材料、压入温度区间和压入速度区间,选取至少30个通过质量检测的参考压入循环,按照压入深度相位分别确定法向力、法向力矩绝对值、接触作用线偏移估计向量模长、单位压入深度载荷增量、装配基体局部刚度估计值和累计压入功的中位数及中位绝对偏差,建立合格压入相位包络,并将同一压入深度相位下装配基体局部刚度估计值的中位数确定为基准局部刚度;

25、将当前特征量与相同压入深度相位下对应特征量的中位数之差,除以对应中位绝对偏差的1.4826倍与对应残差正则量之和,得到有符号的相位包络标准化残差;将相位包络标准化残差大于4确定为高于合格压入相位包络,将相位包络标准化残差小于负4确定为低于合格压入相位包络,将相位包络标准化残差处于负4至4确定为位于合格压入相位包络内;

26、按照夹持松动、局部高刚度阻滞、装配基体局部低刚度响应、单侧接触偏心状态、正常底端到位和无异常的顺序确定异常原因标识,在高优先级条件满足后停止执行低优先级判定;

27、当连续5个控制周期内发生不少于3次偏移方向反转且法向力矩绝对值高于合格压入相位包络,或者夹持机构位置传感器检测到螺丝嵌件相对于夹持机构的移动量超过夹持相对移动阈值时,确定夹持松动;当压入深度相位尚未进入底端接近区间、接触作用线偏移估计向量模长低于偏移阈值、装配基体局部刚度估计值不低于基准局部刚度的150%,且法向力或累计压入功高于合格压入相位包络时,确定局部高刚度阻滞;当接触作用线偏移估计向量模长低于偏移阈值、装配基体局部刚度估计值不高于基准局部刚度的60%,且累计压入功或装配基体局部刚度估计值低于合格压入相位包络时,确定装配基体局部低刚度响应;当接触作用线偏移估计向量模长不小于偏移阈值、方向一致程度不小于0.70、连续3个控制周期未发生偏移方向反转,且装配基体局部刚度估计值高于基准局部刚度的60%并低于基准局部刚度的150%时,确定单侧接触偏心状态;当压入深度相位进入底端接近区间、接触作用线偏移估计向量模长低于偏移阈值、累计压入功位于合格压入相位包络内、单位压入深度载荷增量高于稳定压入区间中位数但未超过底端接近区间上限,且根据机器人末端执行器实际位置确定的当前压入高度偏差处于允许公差内时,确定正常底端到位。

28、在本发明的一方面,所述得到并执行满足运动约束的目标压入轨迹,包括:

29、当异常原因标识为单侧接触偏心状态时,降低压入方向位移增量,沿接触作用线偏移估计向量的反方向生成切向位移修正量,根据第一偏移分量生成绕第二切向轴的反向姿态修正量,并根据第二偏移分量生成绕第一切向轴的正向姿态修正量;当异常原因标识为装配基体局部低刚度响应时,禁止切向找正,停止压入并沿已执行轨迹回撤;当异常原因标识为局部高刚度阻滞时,停止正向运动并沿已执行轨迹回撤;当异常原因标识为夹持松动时,停止压入、执行规定距离的回撤并输出夹持机构检查指令;当确定正常底端到位时,停止压入方向运动并执行到位保持;

30、将2个切向位移增量、压入方向位移增量及3个姿态增量按照固定顺序组合为期望运动增量,根据接触状态和异常原因标识选择接触状态运动约束集合,分别按照各运动分量的单周期允许最大值进行归一化,以候选运动增量与期望运动增量之间的归一化偏差平方和最小为求解目标,得到满足运动约束的运动增量,并根据当前位姿、所述满足运动约束的运动增量以及机器人速度和加速度限制生成满足运动约束的目标压入轨迹;

31、控制机器人末端执行器执行所述满足运动约束的目标压入轨迹,并持续检测法向力、切向力、切向力矩、机器人末端执行器位置偏差、通信状态和轨迹求解时间;当法向力、切向力、切向力矩或机器人末端执行器位置偏差达到对应硬限值时,生成安全停止标识,中止正向运动并执行低速回撤;当通信超时或单次轨迹求解超时时,保持机器人末端执行器当前位置,连续2个控制周期通信超时或轨迹求解超时时执行回撤,其中,所述安全停止标识独立于异常原因标识。

32、在本发明的一方面,所述方法还包括:

33、获取压入高度检测结果、螺丝嵌件倾斜角检测结果、装配基体裂纹检测结果和螺纹连接检测结果,只有全部检测结果合格且对应压入循环未发生回撤、重新压入、硬限值触发、夹持松动、通信异常或轨迹求解超时,才将对应相位对齐载荷位置序列写入合格参考数据集合;

34、根据最近30个质量合格参考压入循环重新确定候选中位数和候选中位绝对偏差,针对每一种特征量和每一个压入深度相位节点,将当前合格压入相位包络中的统计值与候选统计值按照不大于0.02的包络更新系数进行对应加权,得到候选合格压入相位包络;

35、在备用存储区对所述候选合格压入相位包络执行数据长度检查、数值范围检查、相位节点完整性检查和校验和检查,全部检查通过后,通过原子指针切换启用更新后的合格压入相位包络;

36、为更新后的合格压入相位包络分配版本标识,并监测更新后连续50个压入循环的质量失效率,当所述质量失效率比前一版本最近500个压入循环对应的基准质量失效率高至少2个百分点时,停用更新后的合格压入相位包络并恢复前一版本。

37、在本发明的另一方面,本发明还涉及一种基于力位双闭环的螺丝嵌件定位控制系统,包括压入相位生成模块、接触状态确定模块、异常原因确定模块以及轨迹控制与包络更新模块;

38、所述压入相位生成模块用于获取六维力传感器采集的载荷、机器人末端执行器实际位置和机器人末端执行器实际姿态,对所述载荷执行时间同步、灵敏度补偿、静态偏置补偿、工具重力载荷补偿、接触坐标系转换和低通滤波,得到压入接触载荷净值序列,根据法向力确定初始接触位置,结合目标压入位置生成压入深度相位,并对所述压入接触载荷净值序列和所述机器人末端执行器实际位置进行相位对齐,得到相位对齐载荷位置序列;

39、所述接触状态确定模块用于接收所述相位对齐载荷位置序列,根据法向力和2个切向力矩确定接触作用线偏移估计向量,根据所述接触作用线偏移估计向量与切向力向量之间的方向一致程度、压入深度相位及偏移方向连续性确定接触状态,并生成接触状态运动约束集合;

40、所述异常原因确定模块用于接收所述相位对齐载荷位置序列和所述接触状态,确定单位压入深度载荷增量、装配基体局部刚度估计值和累计压入功,与合格压入相位包络进行相位对应比较,得到相位包络标准化残差,并根据预设判定优先级确定异常原因标识;

41、所述轨迹控制与包络更新模块用于接收所述异常原因标识和所述接触状态运动约束集合,根据所述异常原因标识生成期望运动增量,将所述期望运动增量和所述接触状态运动约束集合输入受约束轨迹求解器,得到并执行满足运动约束的目标压入轨迹,将执行期间重新采集的载荷和机器人末端执行器实际位置反馈至所述压入相位生成模块,并根据质量检测结果和压入循环事件记录更新合格压入相位包络、切换包络版本或恢复前一包络版本。

42、在本发明的一方面,所述系统还包括:六维力传感器、温度传感器、夹持机构位置传感器、机器人控制器、质量检测设备、非易失性存储器以及至少一个处理器,所述六维力传感器、所述温度传感器和所述机器人控制器与所述压入相位生成模块连接,所述夹持机构位置传感器与所述异常原因确定模块连接,所述机器人控制器与所述轨迹控制与包络更新模块连接,所述质量检测设备与所述轨迹控制与包络更新模块连接;

43、所述质量检测设备包括高度测量单元、倾斜角测量单元、裂纹检测单元和螺纹连接检测单元;所述非易失性存储器用于存储产品工艺配方、六维力传感器标定参数、合格压入相位包络、接触状态运动约束集合、异常原因标识与控制动作之间的映射关系、版本数据和质量检测结果;所述至少一个处理器用于执行所述压入相位生成模块、所述接触状态确定模块、所述异常原因确定模块以及所述轨迹控制与包络更新模块的处理操作;

44、压入控制线程、接触状态判定线程和安全监控线程分别具有独立的数据缓冲区,各线程通过带时间戳的只读数据快照传递数据,所述安全监控线程的执行优先级高于所述压入控制线程和所述接触状态判定线程,并在检测到载荷硬限值、位置偏差硬限值、通信超时或轨迹求解超时时中止正向运动指令。

45、与现有技术相比,本发明具有以下有益效果:

46、本发明根据机器人末端执行器实际位置沿压入方向的投影确定初始接触位置和压入深度相位,并按照首次正向越过的相位节点对压入接触载荷净值序列和机器人末端执行器实际位置进行相位对齐,使不同压入循环的数据能够在相同压入深度相位下进行比较。根据法向力和换算至接触评价原点的2个切向力矩确定接触作用线偏移估计向量,再结合切向力方向、压入深度相位和偏移方向连续性判定未接触、单侧孔壁接触、周向干涉压入和底端接近。利用单位压入深度载荷增量、装配基体局部刚度估计值和累计压入功与匹配产品、材料、温度及速度条件的合格压入相位包络进行比较,可分别输出单侧接触偏心状态、装配基体局部低刚度响应、局部高刚度阻滞和夹持松动标识。系统根据异常原因标识选择切向修正、停止、回撤、夹持检查或到位保持动作,并通过接触状态运动约束集合限制运动方向和运动幅度。机器人执行后的载荷和实际位置反馈至下一控制周期,形成力反馈与位置反馈相互关联的闭环控制过程。质量检测结果用于限制合格压入相位包络的更新,包络版本对应的质量失效率升高时恢复前一版本。