CN114536333A - 一种基于行为树的机械臂任务规划系统及应用方法 - Google Patents

Abstract

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

Description

一种基于行为树的机械臂任务规划系统及应用方法

技术领域

本发明涉及机械臂任务规划领域，具体涉及一种基于行为树的机械臂任务规划系统及应用方法。

背景技术

随着各式各样机器人正逐渐应用于家庭、医疗服务等领域，具备辅助进食、衣服整理、传递物体等操作技能，人们越来越期望机器人能够具备更强的自主操作能力，在面临更加复杂的实际环境中，能够帮助甚至代替人类完成更加复杂的操作任务。传统的复杂编程、遥操作等常规方法可以让机器人具备一定的操作技能，能够让其较好地工作在结构化工作环境中。然而伴随着机器人应用领域的不断扩大，机器人往往会面临着未知、复杂以及动态的环境，传统任务规划方法无法让机器人很好地适应非结构化工作环境，且传统机器人任务规划方法存在周期长、效率低、工作量大以及无法动态适应环境等诸多难题。综上所述，设计具备一定自主决策和学习能力的机器人任务规划方法及系统具有迫切的需求与重大的意义，其能够弥补传统编程等常规方法的缺陷，极大提高机器人对环境的适应能力。

本发明所提出的机械臂任务规划方法以典型抓取任务为例，基于动态运动基元算法和行为树框架。为实现行为树方法，需将抓取任务划分为移动、抓取、释放等典型机械臂动作并构建基元库，同时基元库中每个动作基元的模型参数需通过动态运动基元算法学习完成，然后根据示教任务的实际执行过程，记录下任务执行逻辑，创建抓取任务行为树模型。当环境发生动态变化时，利用动态运动基元算法对动作基元进行泛化，完成对新任务的执行；当面对新的任务场景时，在初始行为树基础上进行拓展，增加条件节点以及增加子树，使行为树能够适应多个不同任务场景。

发明内容

发明目的：本发明的目的是提供一种操作简单、泛化性能强的机械臂任务规划方法，本发明的另一目的在于提供基于行为树的机械臂任务规划方法系统，根据示教任务的实际执行过程，记录下任务执行逻辑，创建抓取任务行为树模型，在面对不同的任务场景时，根据行为树的灵活性、模块化性和可拓展性，在原有任务行为树框架的基础上，对行为树进行扩展，完成对新任务的执行。

技术方案如下：

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

方法主要包括以下步骤：

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

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

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

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

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

在此步骤中，条件节点的作用是在运行某一节点时需要添加一些具体的判断条件，如机械臂末端是否到达期望位置，机械臂是否已经闭合(张开)手爪。该节点的运行状态不再由其子节点决定，而是由具体的判断条件决定。若判断条件为真，则条件节点状态为成功，否则为失败。

本专利中条件节点根据机械臂在实际抓取场景中的执行逻辑确定，在实际抓取场景中，选择两个条件节点，条件节点C1为机械臂是否到达抓取点，条件节点C2为机械臂是否到达投送点。

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

在此步骤中，行为节点完成的功能就是实现某一具体行为，如人物的移动、机械臂末端移动，因此使用行为节点时需要添加执行某种具体行为的代码。若行为被顺利执行至结束，则行为节点状态为成功；若未能顺利执行，则节点状态为失败；若行为节点正在被执行，则节点状态为正在运行。

在实现本专利方法前，需要对抓取任务进行分割，首先将抓取任务分割为移动、抓取、释放等典型机械臂动作，然后将分割后的每个动作定义为一个动态运动基元并建立基元库，之后行为树的行为节点将从已建立好的基元库中进行选择。

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

在此步骤中，所建立行为树的逻辑为：

S41、行为树由根节点开始，首先判断机械臂是否靠近被抓取物体，如果机械臂未靠近被抓取物体位置，则执行物体抓取前的行为节点，该行为节点从基元库中选取；

S42、进一步地，当执行完抓取前行为节点时，机械臂已经靠近被抓取物体，此时条件节点C1(机械臂是否靠近被抓取物体)成立，继续下一步行为节点，闭合机械臂末端执行器手爪，抓取物体；

S43、进一步地，判断机械臂是否到达放置物体目标点，如果机械臂未到达目标位置，则此时应当执行此时放置物体行为节点，此时放置物体行为节点也是从基元库中选取得到；

S44、当机械臂靠近目标位置时，条件节点C2(机械臂是否靠近放置目标位置)成立，继续执行下一动作节点，张开机械臂末端执行器手爪，放置物体；

S45、最后机械臂回到起始位置，此时整个抓取过程结束。

S5、当物体目标位置改变时，利用动态运动基元算法(Dynamic MovementPrimitive，DMP)结合基元库泛化得到新动作基元，作为行为树行为节点；

在此步骤中，S4步骤建立完行为树后，在执行过程中，当抓取目标位置发生变化时，将采用DMP算法对行为节点选择的基元进行泛化，针对动作基元库中的动作基元运动数据x(t)，对每个时间步长t＝0,…,T计算其导数v(t)和

然后对正则系统

进行积分，即针对适当时间调整的时间缩放因子τ计算s(t)。根据转换系统

其中

计算得到f_target(s)为：

其中，再根据最小误差准则J＝∑_s(f_target(s)-f(s))²计算得到最佳权重ω_i，拟合得到运动学习时的位移、速度和加速度的离散序列，从而完成基元学习的过程。

其中，x、v和

表示系统运动时的位置、速度和加速度；x₀和g表示系统的起始位置和目标位置；K为弹簧的弹性系数；D是非线性函数为零时，线性弹簧系统处于临界阻尼状态下的阻尼系数，f是扰动，也称为非线性函数，用于生成任意复杂的运动。其中非线性函数f定义为：

其中ψ_i(s)＝exp(-h_i(s-c_i)²)为高斯基函数，也被称为径向基核函数，c_i为基核函数的中心，h_i为基核函数的宽度，ω_i为基核函数所对应的权重值。

进一步地，在动作基元泛化时，针对不同新目标位置对起始位置和终点位置进行改变，再结合学习得到的DMP模型参数计算对应的非线性项f，即可生成满足任务需求的轨迹，从而完成基元泛化。最后基于所建立行为树和DMP泛化后的动作基元实现新目标位置下的抓取任务。

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

S61、增加判断任务场景的条件节点，对所需要执行的任务进行判断；

S62、根据新任务执行逻辑重复步骤S4，创建新任务对应的行为树子模块；

S63、增加控制节点，将步骤S62所创建的行为树子模块添加至步骤S4所建行为树中，完成行为树拓展。

附图说明

图1描绘了本发明一种基于动态运动基元和行为树的机械臂任务规划方法的总体框架图；

图2描绘了用于控制机械臂抓取并放置的行为树结构示意图；

图3描绘了行为树拓展后的结构示意图；

图4为基于动态运动基元和行为树的机械臂任务规划系统结构图。

有益效果

本发明提供一种基于行为树的机械臂上层任务规划方法及系统，与现有技术相比，利用行为树进行上层任务规划，在复现任务的过程中采用DMP算法进行基元泛化，提高了机械臂的适应能力以及学习能力，同时保证了系统的泛化性能和稳定性能。

具体实施方式

为了更清晰地阐明本发明的技术内容，下面结合附图及具体实施例来详细说明本发明，其中具体实施例以及说明仅用来解释本发明，但并不作为对本发明的限定。

本发明提出一种基于行为树的机械臂任务规划方法，其总体结构图如图1所示。主要包括上层任务规划和新任务执行两部分。其中上层任务规划中所设计的行为树结构示意图如图2所示。其主要的实现方案为：一方面，使用遥操作控制Kinova Jaco机械臂进行多次抓取任务演示，将收集到的演示数据经轨迹分割模块得到动作基元，构建动作基元库，同时通过DMP算法获取各个基元的模型参数；另一方面，根据抓取任务执行逻辑设计行为树控制流节点，任务执行过程中机械臂末端执行器手爪是否需要开合转换为行为树条件节点，经行为树设计模块创建抓取任务行为树，Kinect传感器捕获物体深度图像经目标位姿检测模块估计出物体位置和姿态后，结合动作基元库中对应动作基元，再经由动态运动基元泛化模块进行泛化，调整DMP算法参数，根据最小误差准则J＝∑_s(f_target(s)-f(s))²作为基元轨迹执行优劣的衡量指标，输出DMP泛化后的跟踪轨迹。其中f(s)表示原始输入轨迹的非线性扰动项，f_target(s)是针对新位置泛化得到的非线性扰动，当最小误差J越小，则表示泛化后轨迹与原始轨迹越接近，泛化性能越好。将泛化得到的动作基元作为行为树动作节点，根据行为树将执行指令传送至上位机系统，控制Kinova Jaco机械臂执行抓取任务。

当面对新的任务场景时，图3展示了在图2初始行为树基础上进行拓展后的行为树结构示意图，增加条件节点、控制节点以及增加子树，使行为树能够适应多个不同任务场景。

图4为基于动态运动基元和行为树的机械臂任务规划系统结构图，整个系统包含Kinova Jaco2机械臂8和桌子3和4，在桌子3上，放置有多个不同的物体，其包括篮筐1、篮筐2和Kinect深度相机5和上位机9；另外，在篮筐1中，装有多个不同的物体，被描述为方体6和圆柱体7，用于机械臂抓取。机械臂8包括6个可活动的关节Joint1、Joint2、Joint3、Joint4、Joint5、Joint6和用于抓握或拾取的夹持器Gripper1、Gripper2、Gripper3。现将描述所述的一种基于行为树的机械臂任务规划方法的典型使用，在用户带动机械臂8将篮筐1中的方体6抓取并放置至篮筐2进行多次演示后，获取得到动作基元以及建立好行为树结构；再次使用Kinect深度相机获取篮筐1中圆柱体7的位置以及姿态信息，发送给上位机，通过DMP泛化并结合行为树对新目标位置进行抓取放置。整个实验过程均在机械臂操作空间内进行。

具体的，其实施方式包括以下步骤：

S1、抓取放置物体示教动作演示并记录任务执行逻辑：

S11、机械臂开始位于其程序预定义的初始状态，转动机械臂8各个关节Joint1-6以及末端夹持器Gripper1-3到达篮筐1中方体6上方，调整机械臂位姿，并闭合机械臂末端夹持器使其抓握住方体6；

S12、继续带动机械臂8使其到达篮筐2中心上方，然后使机械臂向下运动，使其末端夹持器上抓握的物体离篮筐2的距离约为1-2cm；

S13、张开机械臂末端夹持器，将方体6放置在篮筐2中；

S14、带动机械臂各个关节以及末端夹持器回到初始状态；

S15、任务执行逻辑为：机械臂到达物体放置点-抓取物体-放置到目标点-放置物体-回到起始位置。

S2、构建行为树进行任务规划：

S21、对于任务规划过程，建立抓取任务过程的行为树架构，如图2所示，在行为树的顶部，由根节点1开始，其被标志为具有

的方框。根节点没有父节点，只连接有一个带记忆性的选择节点2，其被标志为具有“？”的方框。根节点产生“tick”信号，对选择节点进行触发；

S22、选择节点2连接两个子节点，分别为带记忆性的顺序节点3和动作节点4。选择节点2将从左至右对其子节点进行触发，首先触发其子节点3；

S23、带记忆性的顺序节点3通常由具有“-＞·”的方框表示，其包括子节点5、6和7，分别为条件节点5、动作节点6和选择节点7。节点3也将从左至右对其子节点进行触发，首先触发条件节点5，即“判断是否靠近圆柱体”，表示机械臂末端夹持器是否靠近篮筐1中的物体，这里机械臂末端和篮筐中的物体之间的阈值设置为2cm，如果未靠近，则条件节点5返回失败，根据顺序节点性质，将不再继续执行后续节点，条件节点5将其失败状态(Failure)返回给其父节点3，节点3又将失败状态返回给其父节点2；

S24、节点2接收到其第一个子节点3的失败状态后，根据其性质，节点2将继续执行其第二个子节点4，动作节点4，即“靠近圆柱体”，表示机械臂执行抓取过程的动作基元。当动作节点4执行结束后，返回成功状态(Success)，节点4将成功状态返回给其父节点2，节点2接收到成功状态后，节点2将成功状态返回到根节点，第一次“tick”结束，此时机械臂已靠近被抓取物体；

S25、根节点再次发出“tick”信号，重复S21-S23步骤，当再次触发到条件节点5时，此时机械臂已靠近篮筐1中的被抓取物体，条件成立，继续执行节点6；

S26、节点6为“抓取圆柱体(闭合手爪)”的动作节点，表示机械臂将闭合其末端夹持器，抓取物体，动作节点6执行成功后，返回成功状态至其父节点3，父节点3继续执行其下一个子节点7；

S27、节点7为选择节点，其连接两个子节点，分别为带记忆性的顺序节点8和动作节点9。选择节点7将从左至右对其子节点进行触发，首先触发其子节点8；

S28、节点8包括子节点10、11和12，分别为条件节点10、动作节点11和选择节点12。节点8将从左至右对其子节点进行触发，首先触发条件节点10，即“判断是否靠近放置点”，表示机械臂末端是否到达篮筐2中物体放置位置，这里机械臂末端和篮筐2中物体放置位置的阈值设置为2cm，如果未到达，则条件节点10返回失败，条件节点5将其失败状态返回给其父节点8，节点8将不再继续其后续子节点，节点8又将失败状态返回给其父节点7；

S29、选择节点7接收到其第一个子节点8的失败状态后，根据其性质，选择节点7将继续执行其第二个子节点9，节点9为动作节点，即“放置圆柱体，表示机械臂执行放置过程的动作基元。当动作节点9执行结束后，返回成功状态(Success)，节点9将成功状态返回给其父节点7，节点7再将成功状态返回给其父节点3，节点3将成功状态返回给其父节点2，最后节点将成功状态返回到根节点，第二次“tick”结束，此时机械臂已成功到达物体放置位置；

S210、根节点再次发出“tick”信号，重复S21-S28步骤，由于节点3为带记忆性的顺序节点，在每次触发时，其子节点会保持上一次触发时的状态，即节点5和节点6不需要再次执行。当再次触发到条件节点10时，此时机械臂已到达物体放置位置，条件成立，继续执行节点11；

S211、节点11为“放置圆柱体(张开手爪)”的动作节点，表示机械臂将张开其末端夹持器，放置物体，动作节点11执行成功后，继续执行节点12；

S212、节点12为“回到Home位置”的动作节点，表示机械臂回到初始状态位置，节点12执行成功后，返回成功状态至其父节点3，节点3将成功状态依次向上传递，最终到达根节点，整个抓取物体并放置物体的过程结束。

S3、针对新目标位置对动作基元进行DMP泛化

S31、当面对新目标位置的物体进行抓取时，如篮筐1中的圆柱体7，首先利用RGB-D相机5获取圆柱体7的位置x,y,z及抓取姿态信息q_x,q_y,q_z,q_w，发送到上位机；

S32、上位机针对获取得到的圆柱体7的位姿信息进行动作基元泛化，设置DMP二阶动态系统方程参数，令4β_y＝α_y，根据已有基元库中基元DMP模型参数计算对应的非线性项f，即可生成满足任务需求的动作基元。

S4、将步骤S3泛化得到的动作基元并结合步骤S2所构建的行为树架构，完成抓取并放置新物体。

S5、在任务场景不同时，对步骤S2所建立的行为树进行拓展，重复步骤S1，记录新任务场景下的执行任务逻辑，并重复步骤S2创建新任务行为树子模块，同时添加判断不同任务的条件节点，将所建立行为树子模块和条件节点添加至原行为树中，完成对不同任务的行为树拓展。

S51、修改步骤S2所建行为树，新增动作节点，该动作节点功能为检测目标物位置并发送给上位机，从而实现针对新目标位置的动态运动基元泛化；

S52、新增条件节点，该条件节点功能为判断篮筐中抓取的物体种类；

S53、重复步骤S2，新增抓取方块6子树模块；

S54、新增控制节点，将子树模块添加到步骤S2所建行为树，完成行为树拓展。

以上所述仅为本发明的较佳实施例而已，并不用于限制本发明，凡在本发明的精神和原则范围之内所作的任何修改、等同替换以及改进等，均应包含在本发明的保护范围之内。

Claims (7)

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中，需要对抓取任务进行分割，首先将抓取任务分割为移动、抓取、释放等典型机械臂动作，然后将分割后的每个动作定义为一个动态运动基元并建立基元库，之后行为树的行为节点将从已建立好的基元库中进行选择。

5.如权利要求2所述的方法，其特征在于，所述S4中，建立行为树的逻辑为：

S41、行为树由根节点开始，首先判断机械臂是否靠近被抓取物体，如果机械臂未靠近被抓取物体位置，则执行物体抓取前的行为节点，该行为节点从基元库中选取；

S42、当执行完抓取前行为节点时，机械臂已经靠近被抓取物体，此时条件节点C1(机械臂是否靠近被抓取物体)成立，继续下一步行为节点，闭合机械臂末端执行器手爪，抓取物体；

S43、判断机械臂是否到达放置物体目标点，如果机械臂未到达目标位置，则此时应当执行此时放置物体行为节点，此时放置物体行为节点也是从基元库中选取得到；

S44、当机械臂靠近目标位置时，条件节点C2(机械臂是否靠近放置目标位置)成立，继续执行下一动作节点，张开机械臂末端执行器手爪，放置物体；

S45、最后机械臂回到起始位置，此时整个抓取过程结束。

6.如权利要求2所述的方法，其特征在于，所述S5中，当S4步骤建立完行为树后，在执行过程中，当抓取目标位置发生变化时，将采用DMP算法对行为节点选择的基元进行泛化，针对动作基元库中的动作基元运动数据x(t)，对每个时间步长t＝0,…,T计算其导数v(t)和

然后对正则系统

进行积分，即针对适当时间调整的时间缩放因子τ计算s(t)；根据转换系统

其中

计算得到f_target(s)为：

其中，再根据最小误差准则J＝∑_s(f_target(s)-f(s))²计算得到最佳权重ω_i，拟合得到运动学习时的位移、速度和加速度的离散序列，从而完成基元学习的过程；

其中，x、v和

表示系统运动时的位置、速度和加速度；x₀和g表示系统的起始位置和目标位置；K为弹簧的弹性系数；D是非线性函数为零时，线性弹簧系统处于临界阻尼状态下的阻尼系数，f是扰动，也称为非线性函数，用于生成任意复杂的运动。其中非线性函数f定义为：

其中ψ_i(s)＝exp(-h_i(s-c_i)²)为高斯基函数，也被称为径向基核函数，c_i为基核函数的中心，h_i为基核函数的宽度，ω_i为基核函数所对应的权重值。

在动作基元泛化时，针对不同新目标位置对起始位置和终点位置进行改变，再结合学习得到的DMP模型参数计算对应的非线性项f，即可生成满足任务需求的轨迹，从而完成基元泛化；最后基于所建立行为树和DMP泛化后的动作基元实现新目标位置下的抓取任务。

7.如权利要求2所述的方法，其特征在于，所述S6具体如下：

S61、增加判断任务场景的条件节点，对所需要执行的任务进行判断；

S62、根据新任务执行逻辑重复步骤S4，创建新任务对应的行为树子模块；

S63、增加控制节点，将步骤S62所创建的行为树子模块添加至步骤S4所建行为树中，完成行为树拓展。

Images