串联机械臂自碰撞预防方法、预防方法及设备与流程

本技术涉及机器人运动安全防护、机器人实时运动控制，具体而言，涉及一种串联机械臂自碰撞预测方法、预防方法及设备。

背景技术：

1、机械臂在实际工况运行时，连杆之间易发生自碰撞，同时易受外界环境干扰、外力扰动影响产生轨迹偏移，若无法预判并规避碰撞风险，极易造成机械臂连杆磨损、传动结构损坏。

2、相关技术中，提前存储机械臂标准作业轨迹，限定各关节固定运动路径、运动速度，作业中强制机械臂跟随预设固定轨迹运行，并针对预设轨迹上的固定点位、固定运动区间，提前设定固定安全间距阈值，机械臂运行时，校验机械臂实时位置是否偏离预设固定轨迹、固定安全阈值，以判定当前点位是否存在碰撞风险。

3、然而，针对预设轨迹的固定点位的静态校验方式，仅能对预设轨迹进行偏差判定，如果机械臂处于某些工况（如机械臂伺服滞后等），使得预设轨迹偏离，导致碰撞风险预判准确性降低，进而无法准确进行碰撞预防。

技术实现思路

1、有鉴于此，本技术提供了一种串联机械臂自碰撞预测方法、预防方法及设备，以解决如果机械臂处于某些工况（如机械臂伺服滞后等），使得预设轨迹偏离，导致碰撞风险预判准确性降低，进而无法准确进行碰撞预防的问题。

2、第一方面，本技术提供了一种串联机械臂自碰撞预测方法，该方法包括：获取观测数据集合；其中，观测数据集合包括机械臂每个控制周期的关节运动观测数据；基于观测数据集合和运动规划的关节指令值，多步递推预测未来多周期关节角度序列；基于未来多周期关节角度序列，从预存储的有效碰撞配对集合中计算最小连杆间距；根据最小连杆间距，预测机械臂在预设时段的运动轨迹是否存在自碰撞风险。

3、在一个可能的实现方式中，基于观测数据集合和运动规划的关节指令值，多步递推预测未来多周期关节角度序列，包括：基于观测数据集合内连续多帧关节运动观测数据，确定各关节对应的关节角速度和关节角加速度；基于关节运动观测数据、关节角速度以及关节角加速度，确定关节角度、角速度以及角加速度之间的时序演变规律和状态转移关系；基于时序演变规律、状态转移关系以及关节指令值，执行状态迭代更新，得到最优关节运动状态估计；基于最优关节运动状态估计，多步递推预测未来多周期关节角度序列。

4、在一个可能的实现方式中，基于时序演变规律、状态转移关系以及关节指令值，执行状态迭代更新，得到最优关节运动状态估计，包括：基于时序演变规律、状态转移关系、关节指令值以及上一周期的最优关节运动状态估计，推演得到本周期的关节预测运行状态估计；基于观测数据集合内本周期的关节运动观测数据，对关节预测运行状态估计进行校正处理，得到中间关节运动状态估计；重复执行基于时序演变规律、状态转移关系、关节指令值以及上一周期的最优关节运动状态估计，推演得到本周期的关节预测运行状态估计至基于观测数据集合内本周期的关节运动观测数据，对关节预测运行状态估计进行校正处理，得到中间关节运动状态估计的步骤，直至中间关节运动状态估计的波动幅值小于预设误差阈值，得到最优关节运动状态估计。

5、在一个可能的实现方式中，观测数据集合还包括机械臂在力控工况下的外力观测数据；基于观测数据集合和运动规划的关节指令值，多步递推预测未来多周期关节角度序列，包括：基于观测数据集合内连续多帧关节运动观测数据和运动规划的关节指令值，确定待处理关节角度序列；基于机械臂在力控工况下的外力观测数据，对当前预测周期对应的待处理关节角度进行扰动补偿校正，得到修正关节角度；以修正关节角度作为下一次预测周期的输入，重复执行基于机械臂在力控工况下的外力观测数据，对当前次预测周期对应的待处理关节角度进行扰动补偿校正，得到修正关节角度的步骤，直至待处理关节角度序列中的待处理关节角度均完成扰动补偿校正，得到未来多周期关节角度序列。

6、在一个可能的实现方式中，基于机械臂在力控工况下的外力观测数据，对当前次预测周期对应的待处理关节角度进行扰动补偿校正，得到修正关节角度，包括：基于机械臂在力控工况下的外力观测数据，确定机械臂外部扰动程度；基于机械臂外部扰动程度，调整不确定性参数，得到校正迭代参数；其中，不确定性参数指示用于修正时序演变幅度和状态转移幅度的修正系数；基于校正迭代参数，对当前次预测周期对应的待处理关节角度进行扰动补偿校正，得到修正关节角度。

7、在一个可能的实现方式中，基于未来多周期关节角度序列，从预存储的有效碰撞配对集合中计算最小连杆间距，包括：基于未来关节角度序列，确定目标预测时刻下机械臂连杆对应的世界坐标系位姿；其中，目标预测时刻为未来关节角度序列对应的任意一个预测时刻；基于目标预测时刻的连杆世界坐标系位姿和预存储的有效碰撞配对集合，确定目标预测时刻对应的风险连杆配对的最小欧氏间距；将多个最小欧式间距中最小的最小欧式间距确定为最小连杆间距。

8、在一个可能的实现方式中，根据最小连杆间距，预测机械臂在预设时段的运动轨迹是否存在自碰撞风险，包括：检测最小连杆间距是否小于第一预设安全阈值；若最小连杆间距小于第一预设安全阈值，则预测机械臂在预设时段的运动轨迹存在自碰撞风险；若最小连杆间距不小于第一预设安全阈值，则预测机械臂在预设时段的运动轨迹不存在自碰撞风险。

9、在一个可能的实现方式中，方法还包括：获取连杆配对组合；根据连杆父子从属关系，从连杆配对组合中剔除存在直接父子连接关系的连杆配对，以及剔除机械臂在任意位姿下均不会发生干涉的连杆配对，得到预存储的有效碰撞配对集合。

10、第二方面，本技术提供了一种串联机械臂自碰撞预防方法，该方法包括：采用上述串联机械臂自碰撞预测方法，确定机械臂在预设时段的运动轨迹的自碰撞风险判定结果；若自碰撞风险判定结果指示机械臂在预设时段的运动轨迹存在自碰撞风险，检测最小连杆间距是否小于第二预设安全阈值；若最小连杆间距不小于第二预设安全阈值，从预设的关联关系中确定机械臂的第一操作；若最小连杆间距小于第二预设安全阈值，从预设的关联关系中确定机械臂的第二操作。

11、第三方面，本技术提供了一种串联机械臂自碰撞预测装置，该装置包括：获取模块，用于获取观测数据集合；其中，观测数据集合包括机械臂每个控制周期的关节运动观测数据；第一预测模块，用于基于观测数据集合和运动规划的关节指令值，多步递推预测未来多周期关节角度序列；计算模块，用于基于未来多周期关节角度序列，从预存储的有效碰撞配对集合中计算最小连杆间距；第二预测模块，用于根据最小连杆间距，预测机械臂在预设时段的运动轨迹是否存在自碰撞风险。

12、第四方面，本技术提供了一种计算机设备，包括：存储器和处理器，存储器和处理器之间互相通信连接，存储器中存储有计算机指令，处理器通过执行计算机指令，从而执行上述第一方面或其对应的任一实施方式的串联机械臂自碰撞预测方法以及第二方面或其对应的任一实施方式的串联机械臂自碰撞预防方法。

13、第五方面，本技术提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机指令，计算机指令用于使计算机执行上述第一方面或其对应的任一实施方式的串联机械臂自碰撞预测方法以及第二方面或其对应的任一实施方式的串联机械臂自碰撞预防方法。

14、第六方面，本技术提供了一种计算机程序产品，包括计算机指令，计算机指令用于使计算机执行上述第一方面或其对应的任一实施方式的串联机械臂自碰撞预测方法以及第二方面或其对应的任一实施方式的串联机械臂自碰撞预防方法。

15、本技术实施例提供的串联机械臂自碰撞预测方法，通过获取观测数据集合，再结合观测数据集合与运动规划的关节指令值多步递推预测未来多周期关节角度序列，不依赖提前存储的机械臂标准作业轨迹，能够基于机械臂实时采集的关节运动观测数据动态推演真实运动趋势，可适配机械臂伺服滞后等引发轨迹偏离的各类工况，进一步根据未来多周期关节角度序列从预存储的有效碰撞配对集合中计算最小连杆间距，然后根据最小连杆间距预测机械臂在预设时段的运动轨迹是否存在自碰撞风险，相对于针对预设轨迹固定点位的静态校验逻辑，能够动态还原机械臂真实运动偏移情况，避免仅对预设轨迹做偏差判定带来的预判失真问题，从而提高碰撞风险预判的准确性。