臂手协同控制方法、装置、设备、介质和程序产品与流程

本技术涉及机器人，具体地涉及一种基于分层架构的机器人臂手协同控制方法、装置、设备、介质和程序产品。

背景技术：

1、在搬运、交接、装配插合以及接触式作业等场景中，机器人往往需要在同一段作业内交替使用两种配合关系：一种是由左臂与右臂共同持取同一工件，另一种是由机械臂把灵巧手送到作业位置、再由灵巧手完成抓握与指形调整。

2、在相关技术中，配合关系的判定多依赖人工编排的作业序列表或者上层写入的模式标志位，与执行侧的真实进展之间没有闭合的关联。当某一步骤因工件位姿偏差或者夹持打滑而延长执行时间时，上层仍按原定节点把后续步骤切到另一种配合关系，轨迹被交给与当前实际阶段不相符的执行对象生成，已生成的轨迹需要整体作废并重新规划。此外，各段轨迹的起点由固定位姿给出，而不是上一段实际结束时所处的位姿，交界处的位姿跳变使控制器需要先驱动执行对象回到固定位姿，由此产生一段不产生作业效果的空行程。

3、直线插值只约束两端点的位置而速度不连续，三次多项式插值的加速度在段边界仍允许跳变，进入接触状态时接触力在瞬间出现远高于稳态值的峰值；左臂与右臂、机械臂与灵巧手各自按自身设定的运动时长推进，同一时刻取出的位置值属于不同的运动进度，构成相对位姿的系统性偏差，同一份任务描述还在不同环节被反复读取与转换。因此，如何使配合关系的判定与执行反馈建立闭合关联，如何使相邻两段轨迹在起点处连续衔接、加速度在段边界保持连续，以及如何使多个执行对象在同一时间基准上推进，成为需要解决的技术问题。

技术实现思路

1、鉴于上述问题，本技术提供了一种基于分层架构的机器人臂手协同控制方法、装置、设备、介质和程序产品。

2、根据本技术的第一个方面，提供了一种基于分层架构的机器人臂手协同控制方法，包括：将获取的任务描述解析为包括操作类型、目标位姿和力控参数的标准化任务序列；根据上一状态、当前操作类型以及执行上一操作类型后反馈的执行状态信号确定当前状态，其中，所述上一状态至少包括初始状态、双臂协同状态和臂手协同状态，所述上一状态为初始状态时，执行上一操作类型后反馈的执行状态信号为初始信号，所述当前状态至少包括双臂协同状态和臂手协同状态；在所述当前状态为双臂协同状态的情况下，根据当前操作类型对应的目标位姿和上一状态的双臂结束位姿，确定左臂起点位姿、左臂终点位姿、右臂起点位姿和右臂终点位姿，基于m次多项式插值生成左臂末端轨迹和右臂末端轨迹，对所述左臂末端轨迹和所述右臂末端轨迹进行时间同步，生成双臂同步轨迹，其中，所述上一状态为初始状态时，双臂结束位姿为双臂初始位姿，m为大于等于5的整数；在所述当前状态为臂手协同状态下，根据当前操作类型对应的目标位姿和上一状态的臂手结束位姿，确定臂起点位姿、臂终点位姿、手起点位姿和手终点位姿，基于n次多项式插值生成臂运动轨迹和手部指形轨迹，对所述臂运动轨迹和所述手部指形轨迹进行时间同步，生成臂手联动轨迹，其中，所述上一状态为初始状态时，臂手结束位姿为臂手初始位姿，n为大于等于5的整数；根据所述双臂同步轨迹或所述臂手联动轨迹以及对应的力控参数，以预定控制频率执行阻抗控制算法和力控闭环算法，生成精细指令；根据所述精细指令驱动左臂、右臂和灵巧手指运动。

3、根据本技术的实施例，所述方法在应用层、协同规划层和柔顺执行层中分别执行，其中所述应用层和所述协同规划层运行于第一计算环境的非实时线程中，所述柔顺执行层运行于第二计算环境的实时线程中，所述第一计算环境不同于所述第二计算环境，所述方法在应用层、协同规划层和柔顺执行层中分别执行的方式包括：所述应用层将获取的任务描述解析为包括操作类型、目标位姿和力控参数的标准化任务序列，并将所述标准化任务序列通过第一通信方式发送至所述协同规划层；所述协同规划层根据上一状态、当前操作类型以及执行上一操作类型后反馈的执行状态信号确定当前状态，所述上一状态至少包括初始状态、双臂协同状态和臂手协同状态；在所述当前状态为双臂协同状态的情况下，所述协同规划层根据当前操作类型对应的目标位姿和上一状态的双臂结束位姿，确定左臂起点位姿、左臂终点位姿、右臂起点位姿和右臂终点位姿，基于m次多项式插值生成左臂末端轨迹和右臂末端轨迹，对所述左臂末端轨迹和所述右臂末端轨迹进行时间同步，生成双臂同步轨迹；在所述当前状态为臂手协同状态下，所述协同规划层根据当前操作类型对应的目标位姿和上一状态的臂手结束位姿，确定臂起点位姿、臂终点位姿、手起点位姿和手终点位姿，基于n次多项式插值生成臂运动轨迹和手部指形轨迹，对所述臂运动轨迹和所述手部指形轨迹进行时间同步，生成臂手联动轨迹；所述协同规划层通过第二通信方式将所述双臂同步轨迹和/或所述臂手联动轨迹以及对应的所述力控参数发送至所述柔顺执行层；所述柔顺执行层根据所述双臂同步轨迹或所述臂手联动轨迹以及对应的力控参数，以预定控制频率执行阻抗控制算法和力控闭环算法，生成精细指令；所述柔顺执行层将所述精细指令通过第三通信方式输出至左臂、右臂和灵巧手指，驱动左臂、右臂和灵巧手指执行运动。

4、根据本技术的实施例，所述第一通信方式为机器人操作系统的话题通信或服务通信，所述第二通信方式为通过实时桥接节点将机器人操作系统的消息转换后写入共享内存的方式，所述第三通信方式为共享内存通信。

5、根据本技术的实施例，根据上一状态、当前操作类型以及执行上一操作类型后反馈的执行状态信号确定当前状态的步骤，包括：获取预置的状态转移规则表，其中，所述状态转移规则表包括多个状态转移条目，每个所述状态转移条目记录有上一状态、操作类型条件、执行状态信号条件与下一状态的对应关系；以所述上一状态、所述当前操作类型和所述执行状态信号作为查询条件，在所述状态转移规则表中进行匹配查询；当所述查询条件与所述状态转移规则表中的任一状态转移条目匹配成功时，将该状态转移条目中记录的所述下一状态确定为所述当前状态。

6、根据本技术的实施例，根据左臂起点位姿、左臂终点位姿、右臂起点位姿和右臂终点位姿，基于m次多项式插值生成左臂末端轨迹和右臂末端轨迹的步骤，包括：获取左臂起点位姿对应的左臂起点位置、左臂起点速度和左臂起点加速度，以及左臂终点位姿对应的左臂终点位置、左臂终点速度和左臂终点加速度；将所述左臂起点位置、所述左臂起点速度、所述左臂起点加速度、所述左臂终点位置、所述左臂终点速度和所述左臂终点加速度作为第一边界条件组，代入m次多项式θ(t)=a0+a1×t+a2×t2+a3×t3+a4×t4+a5×t5中，求解出第一组系数a0、a1、a2、a3、a4和a5，m=5；以时间为自变量，将所述第一组系数代入所述m次多项式，计算出左臂在运动时段内各时刻的位置值，生成所述左臂末端轨迹；获取右臂起点位姿对应的右臂起点位置、右臂起点速度和右臂起点加速度，以及右臂终点位姿对应的右臂终点位置、右臂终点速度和右臂终点加速度；将所述右臂起点位置、所述右臂起点速度、所述右臂起点加速度、所述右臂终点位置、所述右臂终点速度和所述右臂终点加速度作为第二边界条件组，代入m次多项式θ(t)=b0+b1×t+b2×t2+b3×t3+b4×t4+b5×t5中，求解出第二组系数b0、b1、b2、b3、b4和b5；以时间为自变量，将所述第二组系数代入所述m次多项式，计算出右臂在运动时段内各时刻的位置值，生成所述右臂末端轨迹。

7、根据本技术的实施例，对所述左臂末端轨迹和所述右臂末端轨迹进行时间同步，生成双臂同步轨迹的步骤，包括：设定左臂运动时段与右臂运动时段的总时长一致；将左臂在运动时段内各时刻的位置值与右臂在运动时段内各时刻的位置值中的各时刻按照同一时间轴对齐；将各时刻按照同一时间轴对齐后生成的左臂末端轨迹和右臂末端轨迹作为双臂同步轨迹。

8、根据本技术的实施例，根据臂起点位姿、臂终点位姿、手起点位姿和手终点位姿，基于n次多项式插值生成臂运动轨迹和手部指形轨迹的步骤，包括：获取臂起点位姿对应的臂起点位置、臂起点速度和臂起点加速度，以及臂终点位姿对应的臂终点位置、臂终点速度和臂终点加速度；将所述臂起点位置、所述臂起点速度、所述臂起点加速度、所述臂终点位置、所述臂终点速度和所述臂终点加速度作为第三边界条件组，代入n次多项式θ(t)=c0+c1×t+c2×t2+c3×t3+c4×t4+c5×t5中，求解出第三组系数c0、c1、c2、c3、c4和c5，n=5；以时间为自变量，将所述第三组系数代入所述n次多项式，计算出臂在运动时段内各时刻的位置值，生成所述臂运动轨迹；获取手起点位姿对应的手起点位置、手起点速度和手起点加速度，以及手终点位姿对应的手终点位置、手终点速度和手终点加速度；将所述手起点位置、所述手起点速度、所述手起点加速度、所述手终点位置、所述手终点速度和所述手终点加速度作为第四边界条件组，代入n次多项式θ(t)=d0+d1×t+d2×t2+d3×t3+d4×t4+d5×t5中，求解出第四组系数d0、d1、d2、d3、d4和d5；以时间为自变量，将所述第四组系数代入所述n次多项式，计算出灵巧手在运动时段内各时刻的位置值，生成所述手部指形轨迹。

9、根据本技术的实施例，对所述臂运动轨迹和所述手部指形轨迹进行时间同步，生成臂手联动轨迹的步骤，包括：设定臂运动时段与灵巧手运动时段的总时长一致；将臂在运动时段内各时刻的位置值与灵巧手在运动时段内各时刻的位置值中的各时刻按照同一时间轴对齐；将各时刻按照同一时间轴对齐后生成的臂运动轨迹和手部指形轨迹作为臂手联动轨迹。

10、根据本技术的实施例，根据所述双臂同步轨迹或所述臂手联动轨迹以及对应的力控参数，以预定控制频率执行阻抗控制算法和力控闭环算法，生成精细指令的步骤，包括：获取所述双臂同步轨迹或所述臂手联动轨迹中的期望位姿；读取机器人硬件执行机构的实际位姿和外部接触力，其中，机器人硬件执行机构包括左臂、右臂和灵巧手；获取所述力控参数中的期望接触力、期望惯性矩阵、期望阻尼矩阵和期望刚度矩阵；根据所述期望位姿、所述实际位姿和所述外部接触力，按照阻抗控制算法计算阻抗控制量；根据所述外部接触力和所述期望接触力计算力偏差，按照力控闭环算法计算力跟踪控制量；将所述阻抗控制量和所述力跟踪控制量叠加，生成柔顺控制输出；将所述柔顺控制输出经逆运动学转换，生成包括关节角度指令和关节力矩指令的精细指令。

11、本技术的第二方面提供了一种臂手协同控制装置，包括：解析模块，用于将获取的任务描述解析为包括操作类型、目标位姿和力控参数的标准化任务序列；第一确定模块，用于根据上一状态、当前操作类型以及执行上一操作类型后反馈的执行状态信号确定当前状态，其中，所述上一状态至少包括初始状态、双臂协同状态和臂手协同状态，所述上一状态为初始状态时，执行上一操作类型后反馈的执行状态信号为初始信号，所述当前状态至少包括双臂协同状态和臂手协同状态；第二确定模块，用于在所述当前状态为双臂协同状态的情况下，根据当前操作类型对应的目标位姿和上一状态的双臂结束位姿，确定左臂起点位姿、左臂终点位姿、右臂起点位姿和右臂终点位姿，基于m次多项式插值生成左臂末端轨迹和右臂末端轨迹，对所述左臂末端轨迹和所述右臂末端轨迹进行时间同步，生成双臂同步轨迹，其中，所述上一状态为初始状态时，双臂结束位姿为双臂初始位姿，m为大于等于5的整数；第三确定模块，用于在所述当前状态为臂手协同状态下，根据当前操作类型对应的目标位姿和上一状态的臂手结束位姿，确定臂起点位姿、臂终点位姿、手起点位姿和手终点位姿，基于n次多项式插值生成臂运动轨迹和手部指形轨迹，对所述臂运动轨迹和所述手部指形轨迹进行时间同步，生成臂手联动轨迹，其中，所述上一状态为初始状态时，臂手结束位姿为臂手初始位姿，n为大于等于5的整数；生成模块，用于根据所述双臂同步轨迹或所述臂手联动轨迹以及对应的力控参数，以预定控制频率执行阻抗控制算法和力控闭环算法，生成精细指令；驱动模块，用于根据所述精细指令驱动左臂、右臂和灵巧手指运动。

12、根据本技术的第三方面，提供了一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个计算机程序，其中，上述一个或多个处理器执行上述一个或多个计算机程序以实现上述方法的步骤。

13、根据本技术的第四方面，还提供了一种计算机可读存储介质，其上存储有计算机程序或指令，上述计算机程序或指令被处理器执行时实现上述方法的步骤。

14、根据本技术的第五方面，还提供了一种计算机程序产品，包括计算机程序或指令，上述计算机程序或指令被处理器执行时实现上述方法的步骤。

15、根据本技术的一些实施例，通过上一状态、当前操作类型和执行状态信号三者共同确定当前状态，使配合关系的判定由执行侧的真实反馈闭合给出，减少了因判定与实际阶段脱节而生成无效轨迹并重新规划的计算量；在当前状态为双臂协同状态时以目标位姿和上一状态的双臂结束位姿确定左右臂起终点位姿，使本段起点承接上一段的实际结束位姿，避免了回到固定位姿所产生的空行程，并基于m次多项式插值使段边界的加速度不再跳变；在当前状态为臂手协同状态时以同样方式确定臂与手的起终点位姿并基于n次多项式插值生成臂运动轨迹和手部指形轨迹；通过分别执行时间同步把各对象的运动时段归并到同一时间轴；通过以预定控制频率执行阻抗控制算法和力控闭环算法，使位置偏差与力偏差在同一控制周期内被解算并叠加为统一的精细指令；通过由同一条精细指令驱动左臂、右臂和灵巧手指运动，三类对象在同一节拍下更新输出。通过上述处理，本技术实施例提升了配合关系判定的一致性、轨迹衔接的连续性与相对位姿的精确性。