一种双臂复合机器人自适应任务规划控制系统的制作方法

本发明涉及机器人任务规划，具体涉及一种双臂复合机器人自适应任务规划控制系统。

背景技术：

1、双臂复合机器人由两个机械臂协同完成同一装配任务的机器人系统。常见的，一个是扶持机械臂，主要负责夹持、支撑和稳定主体工件；另一个是插装机械臂，主要负责夹持插装件，并完成接近、对准、插入、异常退出和重新对准等动作。也就是说，其是两个机械臂围绕同一个装配任务进行协同控制。

2、双臂机器人插装过程中的异常并非仅表现为简单的位置偏差或插入力升高，而往往同时受到主体工件扶持稳定性、插装入口实际空间姿态、插装件横向偏移以及接触阻力变化等因素影响。当主体工件在夹持后仍发生微小滑移或姿态变化时，原有插装方向和插装入口位置即可能发生改变；当插装件存在横向偏移时，又可能产生局部碰撞或摩擦，使轴向插装力升高但实际推进量不足。因此，仅依据机械臂运动距离、瞬时插装力或者预设轨迹是否执行完成，难以区分正常摩擦、主体工件状态变化和真实卡滞，容易造成异常误判。

3、进一步地，当前的插装控制通常沿用任务开始时建立的固定插装基准。当异常发生并退出后，若主体工件已经发生位置或姿态变化，继续采用原插装入口、原插装方向及原插装深度基准重新插装，容易使新的插装路径与主体工件当前实际状态不匹配。同时，若仍以前一次接触位置或累计运动距离作为插装深度依据，前一次异常推进、退出以及横向调整产生的位移还可能被带入新的插装过程，导致实际推进量异常。

技术实现思路

1、针对现有技术所存在的上述缺点，本发明提供了一种双臂复合机器人自适应任务规划控制系统，能够有效解决现有技术中双臂机器人插装过程中因主体工件位置姿态偏差、扶持不稳定及插装卡滞导致插装状态误判、对准异常的问题。

2、为实现以上目的，本发明通过以下技术方案予以实现：

3、本发明提供一种双臂复合机器人自适应任务规划控制系统，至少包括：

4、插装空间建立模块，依据主体工件确定插装入口中心点、插装完成位置、插装方向单位向量和预插装安全位置；

5、稳定扶持状态判断模块，用于连续获取所述主体工件的位置、姿态及扶持机械臂末端受力，并在对应波动均满足扶持条件后允许插装机械臂插装；

6、初始接触识别模块，控制插装机械臂由预插装安全位置沿插装方向单位向量靠近主体工件，根据沿所述插装方向单位向量获得的轴向插装力识别真实初始接触，并记录初始接触位置；

7、插装状态判断模块，以初始接触位置作为推进零点，根据插装件参考点位置沿插装方向单位向量的位移确定实际轴向推进量，根据连续状态判断窗口内的轴向推进增量和平均轴向插装力识别持续推进状态或疑似插装异常状态；

8、异常对准模块，在疑似插装异常状态下控制插装机械臂退出至预插装安全位置，重新确认主体工件稳定扶持状态，更新插装入口中心点、插装完成位置、插装方向单位向量及预插装安全位置，根据插装件前端中心点与插装入口中心点之间去除轴向分量后的空间偏差执行重新对准，重新识别初始接触位置；

9、插装完整确认模块，根据插装件参考点位置与插装完成位置确定插装完成位置误差，并根据主体工件与插装件的到位参考位置确定轴向装配间隙，依据二者确认插装任务完成状态。

10、进一步地，稳定扶持状态判断模块在所述扶持机械臂完成初始扶持后，建立稳定观察窗口，视觉采集单元连续获取设置于主体工件上的至少三个固定且不共线的主体工件基准特征点；

11、根据各采样时刻全部主体工件基准特征点的位置计算几何中心，并计算各采样时刻的几何中心相对于稳定观察窗口起始采样时刻几何中心的空间距离，从稳定观察窗口内选取最大的空间距离作为主体工件最大位置波动。

12、进一步地，稳定扶持状态判断模块还包括：

13、利用三个固定且不共线的所述主体工件基准特征点确定主体工件基准平面；

14、根据两个由不同主体工件基准特征点之间连线形成的空间向量进行叉乘，得到所述主体工件基准平面的法向方向；

15、再对法向方向进行单位化得到基准平面单位法向量；

16、根据第一主体工件基准特征点指向第二主体工件基准特征点的方向进行单位化得到面内基准方向单位向量；

17、分别计算各采样时刻的基准平面单位法向量与稳定观察窗口起始采样时刻基准平面单位法向量之间的夹角，以及各采样时刻的面内基准方向单位向量与稳定观察窗口起始采样时刻面内基准方向单位向量之间的夹角，

18、将两个夹角中的较大值作为对应采样时刻的主体工件姿态变化量，并将稳定观察窗口内最大的主体工件姿态变化量作为主体工件最大姿态波动。

19、进一步地，设置于扶持机械臂末端的第一力矩传感器，其中：

20、第一力矩传感器连续采集扶持机械臂末端力向量，对稳定观察窗口内全部扶持机械臂末端力向量求平均得到稳定平均扶持力向量；

21、分别计算各采样时刻扶持机械臂末端力向量与稳定平均扶持力向量之间的差向量，并计算各差向量的欧氏长度，从全部欧氏长度中选取最大值作为扶持机械臂末端力最大波动。

22、进一步地，当所述主体工件最大位置波动不高于允许位置波动上限、主体工件最大姿态波动不高于允许姿态波动上限且扶持机械臂末端力最大波动不高于允许扶持力波动上限时，确定所述主体工件处于稳定扶持状态。

23、进一步地，初始接触识别模块控制插装机械臂由预插装安全位置沿插装方向单位向量低速靠近所述插装入口中心点，其中，包括设置于插装机械臂末端的第二力矩传感器；

24、依据第二力矩传感器采集的插装机械臂末端力转换至机器人基坐标系，并将机器人基坐标系下的末端力向量投影至所述插装方向单位向量，得到每一采样时刻的轴向插装力；

25、当连续预设数量采样点对应的轴向插装力均不低于接触力阈值时，确定所述插装件与所述主体工件形成真实初始接触；

26、将首次满足连续接触条件对应的接触起始时刻确定为初始接触时刻，并保存初始接触时刻对应的插装件参考点位置作为初始接触位置。

27、进一步地，轴向推进增量的确定方法为：

28、将当前采样时刻的插装件参考点位置与初始接触位置进行空间位置相减，得到自真实初始接触形成以来的空间位移向量；

29、将空间位移向量投影至所述插装方向单位向量，以投影长度作为当前采样时刻的实际轴向推进量；

30、以当前采样时刻作为状态判断窗口结束时刻，读取状态判断窗口起始采样时刻和结束采样时刻对应的实际轴向推进量，并以前后两个实际轴向推进量之间的差值作为轴向推进增量。

31、进一步地，对状态判断窗口内全部轴向插装力求算术平均得到平均轴向插装力；当所述轴向推进增量不低于推进下限且平均轴向插装力不高于异常插装力阈值时，确定为持续推进状态；

32、当平均轴向插装力不低于异常插装力阈值且轴向推进增量低于推进下限，并且对应组合状态连续保持预设数量的状态判断窗口时，确定为疑似插装异常状态。

33、进一步地，去除轴向分量后的空间偏差执行重新对准，重新识别初始接触位置的方法为：

34、获取当前插装件前端中心点，以所述插装入口中心点与所述插装件前端中心点之间的位置差形成空间误差向量；

35、将所述空间误差向量在所述插装方向单位向量上的投影作为轴向分量，从空间误差向量中去除所述轴向分量，得到横向对准误差向量；

36、并以所述横向对准误差向量的欧氏长度作为横向对准误差大小；

37、控制所述插装机械臂沿所述横向对准误差向量指示的方向执行横向位置修正，直至所述横向对准误差大小不高于允许横向对准误差上限；

38、重新执行真实初始接触识别，并以重新识别得到的所述初始接触位置重新建立所述实际轴向推进量的推进零点。

39、进一步地，获取当前插装件参考点位置，计算插装件参考点位置与插装完成位置之间的欧氏距离，作为插装完成位置误差；

40、获取设置于主体工件上的主体工件到位参考点以及设置于插装件上的插装件到位参考点，根据所述主体工件到位参考点与所述插装件到位参考点之间的位置差得到到位位置差向量；

41、将到位位置差向量投影至插装方向单位向量并取投影长度的绝对值，得到当前轴向装配间隙；

42、比较当前轴向装配间隙与设计目标轴向装配间隙，以二者差值的绝对值作为轴向间隙误差：

43、当插装完成位置误差不高于允许完成位置误差上限且轴向间隙误差不高于允许轴向间隙误差上限，并连续保持预设数量采样点时，输出所述插装任务完成状态。

44、本发明提供的技术方案，与已知的现有技术相比，具有如下有益效果：

45、通过连续检测主体工件位置、姿态以及扶持机械臂末端受力，在三者波动均满足稳定条件后才启动插装，可降低主体工件尚未稳定造成的异常误判。以真实的初始接触位置作为实际轴向推进量零点，并结合状态判断窗口内的轴向推进增量和平均轴向插装力识别持续高受力、低推进的异常状态，减少瞬时摩擦峰值造成的误判。发生异常后，插装机械臂退出至预插装安全位置，重新检测主体工件状态、更新插装空间基准并剔除轴向分量后实施横向重新对准，再重新确定初始接触位置，避免前一次失败插装过程对重新插装产生累积影响。结合插装完成位置误差和实际轴向装配间隙连续确认插装完成状态，提高最终到位判断的可靠性。