[发明专利]一种基于行为树的机械臂任务规划系统及应用方法

权利要求书

1.一种基于行为树的机械臂任务规划系统，其特征在于，包括：轨迹分割模块、目标位姿检测模块、行为树设计模块、动态运动基元泛化模块、上位机系统和Kinova Jaco机械臂，各个模块是指上位机中编写的可运行的控制程序；

一方面，使用遥操作控制Kinova Jaco机械臂进行多次抓取任务演示，上位机系统将收集到的演示数据经轨迹分割模块得到动作基元，构建动作基元库；另一方面，根据抓取任务执行逻辑，经行为树设计模块创建抓取任务行为树；最终，执行新抓取任务时，Kinect传感器捕获物体深度图像经目标位姿检测模块估计出物体位置和姿态后，结合动作基元库中对应动作基元，再经由动态运动基元泛化模块进行泛化，将泛化得到的动作基元作为行为树动作节点，根据行为树将执行指令传送至上位机系统，控制Kinova Jaco机械臂执行抓取任务。

2.如权利要求1所述的系统的应用方法，其特征在于，包括以下步骤：

S1、根据抓取任务执行逻辑，将行为树分为抓取物体子树与放置物体子树，子树内部和子树之间采用带记忆功能的顺序节点和带记忆功能的选择节点作为控制节点；

在此步骤中，当触发信号输入到选择节点后，选择节点也将从左至右执行各个子节点，在依次执行过程中，若子节点状态为失败，则继续向右执行；若子节点为成功或正在运行，则顺序节点状态即为成功或正在运行，且该子节点的后续节点不再被执行；若所有子节点均失败，则选择节点状态为失败；

增加记忆功能后，若其拥有N(N＞2)个子节点，当第一个子节点的状态时成功时，继续执行第二个节点，若此时触发信号再一次到来，则选择节点自动记忆第一个节点的成功状态，不再进行第一个节点状态判断，且继续执行第二个子节点状态判断；

当触发信号输入到顺序节点后，顺序节点将按照从左至右的顺序依次执行子节点，在依次执行过程中，若子节点状态为成功，则继续向右执行；若子节点为失败或正在运行，则顺序节点状态即为失败或正在运行，且该子节点的后续节点不再被执行；若所有子节点均成功，则顺序节点状态为成功。增加记忆功能后，其与选择节点的记忆功能相同。带记忆的选择节点和顺序节点只在当前行为树运行时生效，若当前行为树运行结束，则不再具备记忆功能；

S2、将抓取任务过程中机械臂是否到达抓取点与投送点作为行为树条件节点；

在此步骤中，条件节点的作用是在运行某一节点时需要添加一些具体的判断条件，该节点的运行状态不再由其子节点决定，而是由具体的判断条件决定；若判断条件为真，则条件节点状态为成功，否则为失败；

S3、在抓取任务中，将机械臂移动、抓取、释放等典型动作作为行为树行为节点；

在此步骤中，若行为被顺利执行至结束，则行为节点状态为成功；若未能顺利执行，则节点状态为失败；若行为节点正在被执行，则节点状态为正在运行；

S4、将S1、S2、S3步骤中确定好的行为树控制节点、条件节点以及行为节点按照实际运行过程中逻辑无误的顺序编排行为树；

S6、在面对新任务场景时，修改步骤S4中所建立的行为树，根据新任务对原始行为树进行拓展，增加新任务所需的子树模块、控制节点和条件节点。

3.如权利要求2所述的方法，其特征在于，所述S2中条件节点根据机械臂在实际抓取场景中的执行逻辑确定，在实际抓取场景中，选择两个条件节点，条件节点C1为机械臂是否到达抓取点，条件节点C2为机械臂是否到达投送点。

4.如权利要求2所述的方法，其特征在于，所述S3中，需要对抓取任务进行分割，首先将抓取任务分割为移动、抓取、释放等典型机械臂动作，然后将分割后的每个动作定义为一个动态运动基元并建立基元库，之后行为树的行为节点将从已建立好的基元库中进行选择。