一种基于CAN总线的关节协同控制方法、设备及介质与流程

本发明涉及工业机器人，特别是一种基于can总线的关节协同控制方法、设备及介质。

背景技术：

1、工业机器人多关节协同控制通常由机器人控制器通过can或canopen总线与各关节伺服驱动器进行周期通信，统一下发位置、速度及控制字，并采集关节位置、速度、力矩和状态信息。控制系统依据预设参考轨迹完成各关节同步插补、状态监测与运动约束校验；当节点出现通信异常时，常采用异常节点停机、其余关节减速保持、通信恢复后重新初始化节点，并通过轨迹重规划或插值方式恢复多关节协同运行。

2、常规方法主要存在两个方面的问题：其一，通信中断期间异常关节可能受保持、快速停止或失能等处理方式影响，仅依据恢复后的单帧位置或最后有效指令，难以准确判定其在原始参考轨迹中的对应进度，往复轨迹中还可能出现同位置不同运动阶段的混淆；其二，恢复后直接接入原轨迹或简单插值，难以兼顾运动状态连续性与力矩平稳性。

技术实现思路

1、鉴于上述现有技术存在的问题，提出了本发明。

2、因此，本发明提供了一种基于can总线的关节协同控制方法，解决了现有技术存在的通信恢复后轨迹进度难以准确匹配和再接入过程中运动状态与力矩连续性不足的问题。

3、为解决上述技术问题，本发明提供如下技术方案：

4、第一方面，本发明提供了一种基于can总线的关节协同控制方法，其包括：采集机器人各关节位置、速度、力矩和通信状态，并与原始参考轨迹的轨迹进度绑定；识别单一关节通信异常，记录异常前轨迹进度、位置、速度和故障处理方式，控制其余关节减速保持；通信恢复后核验节点配置、清除旧指令，并根据连续反馈确定待接入关节的运动状态；根据通信中断时间、故障处理方式和关节运动限制递推位置和速度可达区间，在原始参考轨迹中筛选速度可达区间且运动方向一致的候选轨迹进度，计算各候选轨迹进度对应的位置、速度和加速度与待接入关节在通信恢复后确定的实际运动状态之间的归一化状态距离，将归一化状态距离最小且次小距离与最小距离之比达到允许分离比的候选轨迹进度确定为估计轨迹进度；根据估计轨迹进度确定目标接入轨迹进度和目标接入时间，基于实际状态生成七次多项式目标再接入轨迹，并校验运动、力矩、末端位姿和碰撞约束；控制各关节执行目标再接入轨迹，在位置、速度和力矩变化满足接入条件后恢复协同控制。

5、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述与原始参考轨迹的轨迹进度绑定包括：获取机器人各关节对应的位置、速度、力矩和通信状态；确定各关节状态数据采集时对应的原始参考轨迹的轨迹进度；按照关节与轨迹进度的对应关系，将位置、速度、力矩和通信状态与对应的轨迹进度关联，形成各关节的运行状态记录。

6、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述识别单一关节通信异常，记录异常前轨迹进度、位置、速度和故障处理方式，控制其余关节减速保持包括：根据各关节的通信状态确定发生通信异常的单一关节，并将发生通信异常的单一关节确定为待接入关节；提取通信异常前待接入关节对应的轨迹进度、位置和速度，并记录针对通信异常采用的故障处理方式；向其余关节发送减速控制指令，使其余关节减速并进入保持状态。

7、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述根据连续反馈确定待接入关节的运动状态包括：读取通信恢复后的待接入关节的节点配置，并对节点配置进行核验；清除通信异常前向待接入关节发送且尚未执行完毕的旧指令；连续获取待接入关节的位置反馈和速度反馈；根据连续获取的位置反馈和速度反馈，确定待接入关节的实际位置、实际速度、实际加速度和运动方向，形成实际运动状态。

8、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述根据通信中断时间、故障处理方式和关节运动限制递推位置和速度可达区间包括：以通信异常前记录的位置和速度作为可达区间递推的初始状态，并根据通信中断时间确定递推时域；根据故障处理方式确定通信中断期间待接入关节的状态演化方式；在关节运动限制下，沿递推时域依次递推待接入关节的位置可达区间和速度可达区间；将通信恢复时刻对应的位置可达区间和速度可达区间确定为待接入关节的恢复可达区间。

9、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述确定为估计轨迹进度包括：在原始参考轨迹中提取对应速度处于速度可达区间内的轨迹进度；从提取的轨迹进度中筛除运动方向与待接入关节的实际运动方向不一致的轨迹进度，获得候选轨迹进度；分别获取各候选轨迹进度对应的位置、速度和加速度，并计算其与实际运动状态之间的归一化状态距离；按照归一化状态距离由小到大的顺序排列各候选轨迹进度，计算次小距离与最小距离之比；在次小距离与最小距离之比达到允许分离比时，将归一化状态距离最小的候选轨迹进度确定为估计轨迹进度。

10、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述根据估计轨迹进度确定目标接入轨迹进度和目标接入时间，基于实际状态生成七次多项式目标再接入轨迹，并校验运动、力矩、末端位姿和碰撞约束包括：根据估计轨迹进度以及原始参考轨迹中位于估计轨迹进度之后的轨迹状态，确定目标接入轨迹进度；根据实际运动状态、目标接入轨迹进度对应的轨迹状态和关节运动限制，确定目标接入时间；以实际运动状态作为起始边界，以目标接入轨迹进度对应的轨迹状态作为终止边界，并以目标接入时间作为轨迹时域，求解七次多项式，生成目标再接入轨迹；对目标再接入轨迹依次进行运动约束、力矩约束、末端位姿约束和碰撞约束校验，将通过各项约束校验的目标再接入轨迹确定为可执行目标再接入轨迹。

11、作为本发明所述基于can总线的关节协同控制方法的一种优选方案，其中，所述控制各关节执行目标再接入轨迹，在位置、速度和力矩变化满足接入条件后恢复协同控制包括：控制待接入关节和其余关节协同执行可执行目标再接入轨迹；在执行过程中获取各关节的位置、速度和力矩，确定各关节的位置变化、速度变化和力矩变化；判断位置变化、速度变化和力矩变化是否满足接入条件；在满足接入条件时，将各关节的控制轨迹切换至原始参考轨迹，并恢复机器人各关节的协同控制。

12、第二方面，本发明提供了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其中，所述计算机程序被处理器执行时实现如本发明第一方面所述的基于can总线的关节协同控制方法的任一步骤。

13、第三方面，本发明提供了一种计算机可读存储介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现如本发明第一方面所述的基于can总线的关节协同控制方法的任一步骤。

14、本发明有益效果为：通过递推关节运动可达区间并筛选候选轨迹进度，实现了通信恢复后轨迹匹配范围的准确缩小；通过归一化状态距离和候选结果分离比确定估计轨迹进度，实现了往复轨迹中不同运动阶段的准确区分；通过生成满足多阶运动状态边界条件的七次多项式再接入轨迹，实现了关节运动状态的连续平稳过渡；通过开展多重约束校验和接入状态闭环监测，实现了多关节协同控制的安全稳定恢复。