CN112264995B - 一种基于层级强化学习的机器人双轴孔装配方法 - Google Patents

Abstract

本发明提出一种基于层级强化学习的机器人双轴孔装配方法，属于人工智能技术应用领域。该方法将待学习的装配技术进行分解，通过将待学习的技术进行分解成多个选项，通过不断训练每个选项内部的行为策略最终学习出解决双轴孔的装配策略。本发明利用层级强化学习算法用于解决工业中的双轴孔装配问题，更好的利用人工智能的技术有效提高工业生产的效率。

Description

一种基于层级强化学习的机器人双轴孔装配方法

技术领域

本发明属于人工智能技术应用领域，特别涉及一种基于层级强化学习的机器人双轴孔装配方法。

背景技术

智能制造快速发展的趋势下，机器人自动化装配技术具有巨大的市场需求，随着人工智能技术的快速发展，人工智能技术比如强化学习技术已经被越来越多的应用在各种装配领域中。尽管传统强化学习算法如深度确定性策略梯度算法(DDPG)算法已经被用于解决机器人双轴孔装配问题，如“Feedback Deep Deterministic Policy Gradient WithFuzzy Reward for Robotic Multiple Peg-in-Hole Assembly Tasks”。但是针对双轴孔装配问题由于接触状态十分复杂，仅通过有限的力传感信息无法准确判断接触状态，并且双轴孔装配问题中状态和装配动作都是高维连续的量，针对机器人双轴孔装配问题中策略复杂无法通过简单试错的方法进行学习，现有强化学习方法解决学习该装配策略时需要大量尝试，学习效率较低

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种基于层级强化学习的机器人双轴孔装配方法。本方法通过设定选项个数，并且利用上层策略选择合适的选项分别进行训练，通过不断训练每个选项内部的行为策略，最终学习出解决双轴孔的装配策略，提高装配效率。

本发明提出一种基于层级强化学习的机器人双轴孔装配方法，其特征在于，该方法分为训练阶段和实际装配阶段，包括以下步骤：

1)训练阶段；具体步骤如下：

1-1)预先确定层级强化学习方法的装配训练回合总数为L；设定双轴孔装配的目标深度为D，当机器人携带轴到达目标深度D时，一个回合结束；

在双轴孔装配的一个回合中，按照时间顺序将该回合分为n个互不相同的装配阶段，则一个回合中装配阶段序号ω的取值范围为{1，2,3…n}，ω的n种取值分别与装配过程n个互不相同的装配阶段一一对应；

设定奖励函数r(s_t)，代表观测到环境状态s_t时收获的奖励值；设定动作价值函数的学习率α_u，终止函数的学习率α_β，上层策略函数的学习率α_Ω，下层策略函数的学习率α_ω和衰减率γ；

1-2)设定机器人双轴孔装配的上层策略函数为π_Ω(θ_ω|s_t,ω,θ_Ω)，下层策略函数为π_ω(a_t|s_t,θ_ω)，终止函数为β_ω(s_t,θ_β)；

其中上层策略函数的输入量为s_t和ω，s_t代表t时刻观测获得的环境状态，双轴孔装配中的环境状态包括机器人在该时刻的力和扭矩信息[F_x,F_y,F_z,M_x,M_y,M_z]以及该时刻轴的位置位姿信息[P_x,P_y,P_z,O_x，O_y，O_z]，其中，F_x,F_y，F_z分别为机器人在工作空间坐标系下受到的沿x，y,z轴正方向的力，M_x，M_y，M_z分别为机器人在工作空间坐标系下受到的沿x，y，z轴正方向的扭矩，P_x，P_y，P_z分别为轴末端点在工作空间坐标系下的沿x，y，z坐标轴的位置坐标，O_x,O_y,O_z分别为轴线方向相对于x,y,z坐标轴的位置角；θ_Ω代表上层策略函数的参数；θ_ω是上层策略函数的输出量，代表ω所对应装配阶段内的运动控制参数；

下层策略函数的输入量为s_t，输出量为a_t，a_t代表装配过程中t时刻的装配动作

包括该时刻机器人在工作空间坐标系下分别绕x,y,z轴的三个旋转运动量

和三个平移运动量

上层策略函数的输出量θ_ω是下层策略函数的参数；

终止函数的输入量为s_t，θ_β是终止函数的参数，终止函数的输出值为取值{0，1}的判定结果，当终止函数的输出值等于1时代表双轴孔装配当前的装配阶段结束，当终止函数的输出值等于0时代表双轴孔装配当前的装配阶段未结束；

对上层策略函数参数θ_Ω和终止函数参数θ_β分别进行初始化；

定义动作价值函数Q_U(s_t,a_t,ω|θ_Q)，代表机器人在ω所对应装配阶段内，在状态s_t时，执行动作a_t时能够收到的奖励值，并对动作价值函数的参数θ_Q进行初始化；

设定装配训练回合数l的初始值为1；

1-3)在装配训练回合l开始时，机器人将轴运动到设定的初始位置，设定每个训练回合初始位置对应的时刻t＝1，设定该回合中装配阶段序号ω的初始值为1；在第1个训练回合的t＝1时刻，获取该时刻初始位置对应的s_t并输入初始的上层策略函数π_Ω(θ_ω|s_t,ω,θ_Ω)，该函数输出参数θ_ω作为下层策略函数π_ω(a_t|s_t,θ_ω)的参数的θ_ω的初始值；

1-4)在装配训练回合l中的t时刻，获取观测环境状态s_t，将s_t输入当前终止函数β_ω(s_t,θ_β)后，对当前终止函数的输出值进行判定：

如果输出值为1，代表当前装配阶段终止，则更新ω←ω+1，然后进入步骤1-5)；

如果输出值为0，代表当前装配阶段未终止，则进入步骤1-6)，θ_ω不更新；

1-5)将s_t输入当前上层策略函数π_Ω(θ_ω|s_t,θ_Ω)，该函数输出更新后的当前下层策略函数的参数θ_ω，然后进入步骤1-6)；

1-6)将s_t输入当前下层策略函数π_ω(a_t|s_t,θ_ω)，该函数输出t时刻机器人对应动作a_t；然后令机器人执行动作a_t，机器人携带轴抵达新的位置，得到t+1时刻的环境状态的状态s_t+1；

1-7)将s_t+1输入当前奖励函数，该函数输出t+1时刻收获的奖励值r(s_t+1),然后根据时间差分算法对当前动作价值函数Q_U(s_t,a_t,ω|θ_Q)的参数θ_Q进行更新，得到更新后的当前动作价值函数Q_U(s_t,a_t,ω|θ_Q)：

其中，δ_t表示t时刻的时序差分误差，a_t+1表示在t+1时刻当前下层策略函数输出的动作；

表示将s_t+1输入当前下层策略函数π_ω(a_t|s_t,θ_ω)得到t+1时刻的动作a_t+1；

1-8)利用更新后的当前动作价值函数Q_U(s_t,a_t,ω|θ_Q)对当前上层策略函数和当前终止函数进行更新学习，更新参数θ_Ω和θ_β，表达式如下：

其中，

A_Ω(s_t+1,ω)＝Q_Ω(s_t+1,ω)-V_Ω(s_t+1)，

Q_Ω(s_t+1,ω)＝∑_aπ_ω(a_t|s_t,θ_ω)Q_U(s_t,a_t,ω|θ_Q)，

V_Ω(s_t+1)＝∑_ωπ_Ω(θ_ω|s_t+1,ω,θ_Ω)Q_Ω(s_t+1,ω)；

θ_Ω和θ_β更新后，分别得到更新后的当前上层策略函数和更新后的当前终止函数；

1-9)根据s_t+1，判断机器人携带轴是否到达了指定的目标深度D：

如果抵达目标深度D，则当前装配训练回合l终止，执行步骤1-10)；

如果未抵达目标深度D，则当前装配训练回合l未终止，令t＝t+1，然后重新返回步骤1-4)；

1-10)判断装配训练的回合数l是否到达设定的装配训练回合总数L：

如果l≥L，则训练阶段结束，最终获得的训练完毕上层策略函数参数θ_Ω和终止函数参数θ_β分别记为

和

然后进入步骤2)；

如果l<L，则更新装配训练的回合数l←l+1，然后重新返回步骤1-3)；

2)实际装配阶段；具体步骤如下：

2-1)在实际装配开始时，将

作为终止函数β_ω(s_t，θ_β)的参数，此时的终止函数记为

将

作为上层策略函数π_Ω(θ_ω|s_t，ω，θ_Ω)的参数，此时的上层策略函数记为

初始化t＝1，ω＝1；机器人携带轴运动到设定的初始位置，获取此时刻对应的环境状态并作为t时刻的环境状态s_t；

2-2)将s_t输入上层策略函数

该函数输出参数θ_ω作为下层策略函数π_ω(a_t|s_t，θ_ω)的参数θ_ω的初始值；

2-3)将s_t输入终止函数

对该函数的输出值进行判定：

如果输出值为1，代表当前装配阶段终止，则更新ω←ω+1，进入步骤2-4)；如果输出值为0，代表当前装配阶段未终止，则进入步骤2-5),θ_ω不更新；

2-4)将s_t输入上层策略函数

该函数输出更新后的参数θ_ω；

2-5)将s_t输入下层策略函数π_ω(a_t|s_t，θ_ω)，该函数输出t时刻机器人对应动作a_t；然后令机器人执行动作a_t，携带轴抵达新的位置，更新环境状态为s_t+1；

2-6)根据s_t+1，判断机器人携带轴是否到达了目标深度D：

如果抵达目标深度D，则装配完成；如果未抵达目标深度D，则装配未完成，令t＝t+1，然后重新返回步骤2-3)。

本发明的特点及有益效果在于：

1)层级强化学习方法相比于传统基于马尔可夫理论的强化学习算法在解决复杂问题时将问题进行分解设定一些基础目标，然后根据设定的基础目标继续分别进行训练学习，因此其效率更高。

2)本发明将待学习的装配技术进行分解，通过将待学习的技术进行分解成多个选项，然后通过逐个选项学习有效提高学习效率。

3)本发明目的在于能够将要解决的问题进行分解，通过对待解决问题的分解能够减少很多无用的尝试，将能够提高基于学习强化学习算法在实际工业环境中的应用效率。

4)本发明利用层级强化学习算法主要用于解决工业中的双轴孔装配问题，更好的利用人工智能的技术有效提高工业生产的效率。

具体实施方式

本发明提出一种基于层级强化学习的机器人双轴孔装配方法，下面结合具体实施例对本发明进一步详细说明如下。

本发明提出一种基于层级强化学习的机器人双轴孔装配方法，其中，双轴孔装配系统包含轴、孔、工作平台，机器人，力传感器和控制计算机，其中，机器人底端与工作平台固结，机器人输出端与力传感器基座固结，力传感器测量端与轴固结，孔固结在工作平面上，控制计算机通过线缆连接并控制机器人。该系统各部分均可采用常规部件。

双轴孔装配中根据轴和孔的接触状态可以将双轴孔装配系统的一个完整的装配过程按照时间顺序分为n个互不相同的装配阶段，则装配阶段序号ω的取值范围为{1，2，3…n}，ω的n种取值分别与装配过程n个互不相同的装配阶段一一对应，当ω取i(i∈{1，2，3…n})时，代表装配系统处于第i个装配阶段。

利用层级强化学习方法解决双轴孔装配问题需要学习两个策略函数：

一个是上层策略函数π_Ω(θ_ω|s_t，ω，θ_Ω)，其中上层策略的输入量为s_t和ω，其中s_t代表t(t∈{0，1，2…})时刻观测获得的环境状态，双轴孔装配中的环境状态包括通过力传感器获得的机器人在该时刻的六个力信号[F_x，F_y，F_z，M_x，M_y，M_z]，包括机器人工作空间坐标系下受到分别沿x，y，z轴正方向的力F_x，F_y，F_z和扭矩M_x，M_y，M_z，以及该时刻轴的位置位姿信息[P_x，P_y，P_z，O_x，O_y，O_z]，包括轴末端点在工作空间坐标系下的分别沿x，y，z坐标轴的位置坐标P_x,P_y，P_z和轴线方向分别相对于x，y，z坐标轴的位置角O_x，O_y，O_z；θ_Ω代表上层策略函数的参数；θ_ω是上层策略函数的输出量，代表ω所对应装配阶段内的运动控制参数。上层策略函数的工作过程可写为：

或θ_ω＝π_Ω(s_t，ω，θ_Ω)。

另一个是下层策略函数：π_ω(a_t|s_t，θ_ω)，其中下层策略的输入量为s_t，输出量为a_t，a_t代表装配过程中t时刻的装配动作

包括机器人在工作空间坐标系下分别绕x，y,z轴的三个旋转运动量

和三个平移运动量

上层策略函数的输出量θ_ω是下层策略函数的参数。下层策略函数的工作过程可写为：

或a_t＝π_ω(s_t,θ_ω)。

除学习策略之后，还需要学习一个装配阶段终止的终止函数β_ω(s_t,θ_β)，其中，终止函数的输入量为s_t，θ_β是终止函数的参数，终止函数的输出值为取值{0，1}的判定结果，当终止函数的输出值等于1时代表双轴孔装配系统当前所处的装阶段结束，需要根据上层策略函数选择下一个装配阶段对应的下层策略函数参数θ_ω。

另需预先确定分层强化学习算法的装配训练回合总数为L，双轴孔装配中的每一个装配回合的终止定义为机器人携带轴到达了指定的目标深度D(每一个回合的目标深度一致)；需预先确定奖励函数r(s_t)，代表观测到环境状态s_t时收获的奖励值；需预先确定动作价值函数的学习率α_u，终止函数的学习率α_β，上层策略函数的学习率α_Ω，下层策略函数的学习率α_ω和衰减率γ。

本发明提出一种基于层级强化学习的机器人双轴孔装配方法，分为训练阶段和实际装配阶段，包括以下步骤：

1)训练阶段；具体步骤如下：

1-1)预先确定层级强化学习方法的装配训练回合总数为L，(L建议取值范围100～200)；双轴孔装配中的每一个装配回合的终止定义为机器人携带轴到达了指定的目标深度D；预先确定奖励函数r(s_t)，代表观测到环境状态s_t时收获的奖励值；预先确定动作价值函数的学习率α_u(建议取值范围为0.9-0.99)，终止函数的学习率α_β(建议取值范围为0.9-0.99)，上层策略函数的学习率α_Ω(建议取值范围为0.9-0.99)，下层策略函数的学习率α_ω(建议取值范围为0.9-0.99)和衰减率γ(建议取值为0.9)。

1-2)设定机器人双轴孔装配的上层策略函数为π_Ω(θ_ω|s_t,ω,θ_Ω)，下层策略函数为π_ω(a_t|s_t,θ_ω)，终止函数为β_ω(s_t,θ_β)；

其中上层策略函数的输入量为s_t和ω，s_t代表t(t∈{0,1,2…})时刻观测获得的环境状态，双轴孔装配中的环境状态主要包括通过力传感器获得的机器人在该时刻六个力信号[F_x,F_y,F_z,M_x,M_y,M_z]，包括机器人该时刻在工作空间坐标系下受到分别沿x,y,z轴正方向的力F_x,F_y,F_z和扭矩M_x,M_y,M_z，以及该时刻轴的位置位姿信息[P_x,P_y,P_z,O_x,O_y,O_z]，包括轴末端点该时刻在工作空间坐标系下的分别沿x,y,z坐标轴的位置坐标P_x,P_y,P_z和轴线方向分别相对于x,y,z坐标轴的位置角O_x,O_y,O_z；O_Ω代表上层策略函数的参数；O_ω是上层策略函数的输出量，代表ω所对应装配阶段内的运动控制参数。上层策略函数的工作过程可写为：

或θ_ω＝π_Ω(s_t,ω,θ_Ω)。

下层策略函数的输入量为s_t，输出量为a_t，a_t代表装配过程中t时刻的装配动作

包括该时刻机器人在工作空间坐标系下分别绕x,y,z轴的三个旋转运动量

和三个平移运动量

上层策略函数的输出量θ_ω是下层策略函数的参数。下层策略函数的工作过程可写为：

或a_t＝π_ω(s_t,θ_ω)。

终止函数的输入量为s_t，θ_β是终止函数的参数，终止函数的输出值为取值{0，1}的判定结果，当终止函数的输出值等于1时代表双轴孔装配当前的装配阶段结束，需要根据上层策略函数选择下一个装配阶段对应的下层策略函数参数θ_ω。当终止函数的输出值等于0时代表双轴孔装配当前的装配阶段未结束。

对上层策略函数参数θ_Ω和终止函数参数θ_β分别进行初始化。

定义动作价值函数Q_U(s_t,a_t,ω|θ_Q)，代表机器人在ω所对应装配阶段内，在状态s_t时，执行动作a_t时能够收到的奖励值，并对动作价值函数的参数θ_Q进行初始化。所述参数θ_Ω，θ_β，θ_Q的初始值可以取-1～1之间的随机值；

设定装配训练回合数l的初始值为1。

1-3)在装配训练回合l(l∈{1,2,3…L})开始时，机器人将轴运动到设定的初始位置，设定每个训练回合初始位置对应的时刻t＝1，设定该回合中装配阶段序号ω的初始值为1。

在第1个训练回合的t＝1时刻，获取该时刻初始位置对应的s_t并输入初始的上层策略函数π_Ω(θ_ω|s_t,ω,θ_Ω)，该函数输出参数θ_ω作为下层策略函数π_ω(a_t|s_t,θ_ω)的参数θ_ω的初始值。

1-4)在装配训练回合l中的t时刻，观测环境状态s_t，将s_t输入当前终止函数后，对当前终止函数的输出值进行判定：

如果输出值为1，代表当前装配阶段终止，则更新ω←ω+1，然后执行步骤1-5)。

如果输出值为0，代表当前装配阶段未终止，则跳过步骤1-5)执行步骤1-6)，此时θ_ω不更新：θ_ω与当前装配训练回合上一时刻(即t-1时刻)的θ_ω取值相同；若t时刻是装配回合开始的第一个时刻(即t＝1)，则θ_ω取上一个装配训练回合结束时的θ_ω取值(若当前训练回合为第1个回合，则θ_ω保持初始值不变)。

1-5)将s_t输入当前上层策略函数π_Ω(θ_ω|s_t,θ_Ω)，该函数输出更新后的当前下层策略函数的参数θ_ω，然后进入步骤1-6)；

1-6)将s_t输入当前下层策略函数π_ω(a_t|s_t,θ_ω)，该函数输出t时刻机器人对应动作a_t。然后令机器人执行动作a_t，机器人携带轴抵达新的位置，得到新的状态s_t+1(即t+1时刻的环境状态)。

1-7)观测机器人下一时刻(即t+1时刻)的环境状态s_t+1，将s_t+1输入当前奖励函数，该函数输出t+1时刻收获的奖励值r(s_t+1),然后根据时间差分算法对当前动作价值函数Q_U(s_t,a_t,ω|θ_Q)的参数θ_Q进行更新：

其中，δ_t表示t时刻的时序差分误差，a_t+1表示在t+1时刻当前下层策略函数输出的动作；

表示将s_t+1输入当前下层策略函数π_ω(a_t|s_t,θ_ω)得到t+1时刻的动作a_t+1；

将更新后的参数θ_Q作为动作价值函数新的参数，得到更新后的当前动作价值函数Q_U(s_t,a_t,ω|θ_Q)。

1-8)利用更新后的当前动作价值函数Q_U(s_t,a_t,ω|θ_Q)对当前上层策略函数和当前终止函数进行更新学习，因此参数θ_Ω和θ_β可以利用策略梯度下降理论进行更新：

其中α_ω代表下层策略函数的学习率，将更新后的θ_Ω作为新的当前上层策略函数的参数。

其中α_β代表终止函数的学习率，A_Ω(s_t+1,ω)＝Q_Ω(s_t+1,ω)-V_Ω(s_t+1)，其中Q_Ω(s_t+1,ω)＝∑_aπ_ω(a_t|s_t,θ_ω)Q_U(s_t,a_t,ω|θ_Q)，V_Ω(s_t+1)＝∑_ωπ_Ω(θ_ω|s_t+1,ω,θ_Ω)Q_Ω(s_t+1,ω)。将更新后的θ_β作为新的当前终止函数参数。

1-9)根据机器人t+1时刻观测到的环境状态s_t+1，判断机器人携带轴是否到达了指定的目标深度D：

如果抵达目标深度D，则当前装配训练回合l终止，执行步骤1-10)。

如果未抵达目标深度D，则当前装配训练回合l未终止，令t＝t+1，然后重新返回步骤1-4)；

1-10)判断装配训练的回合数l是否到达设定的装配训练回合总数L：

如果达到(即l≥L)，则训练阶段结束，最终获得的训练完毕上层策略函数参数θ_Ω和终止函数参数θ_β分别记为

和

然后进入步骤2)。

如果未达到即(l<L)，则更新装配训练的回合数l←l+1，然后重新返回步骤1-3)。

2)实际装配阶段；在实际装配阶段，双轴孔装配的目标深度为D；具体步骤如下：

2-1)在实际装配开始时，将

作为终止函数β_ω(s_t,θ_β)的参数，此时终止函数记为

将

作为上层策略函数π_Ω(θ_ω|s_t,ω,θ_Ω)的参数，此时上层策略函数记为

初始化t＝1，ω＝1。机器人携带力传感器及轴运动到设定的初始位置，获取此时刻对应的环境状态并作为t时刻的环境状态s_t；

2-2)将s_t输入上层策略函数

该函数输出参数θ_ω作为下层策略函数π_ω(a_t|s_t,θ_ω)的参数θ_ω的初始值。

2-3)将s_t输入终止函数

对该函数的输出值进行判定：

如果输出值为1，代表当前装配阶段终止，则更新ω←ω+1，继续执行步骤2-4)。

如果输出值为0，代表当前装配阶段未终止，则跳过步骤2-4)执行步骤2-5),θ_ω不更新，此时θ_ω与上一时刻(即t-1时刻)的θ_ω取值相同，若t时刻是装配回合开始的第一个时刻(即t＝1)，则θ_ω取初始值。

2-4)将s_t输入上层策略函数

该函数输出更新后的参数θ_ω。

2-5)将s_t输入下层策略函数π_ω(a_t|s_t,θ_ω)，该函数输出t时刻机器人对应动作a_t。然后令机器人执行动作a_t，携带轴抵达新的位置，更新环境状态为s_t+1，(即t+1时刻的环境状态)。

2-6)观测机器人下一时刻(即t+1时刻)的环境状态s_t+1，判断机器人携带轴是否到达了指定的目标深度D：如果抵达目标深度D，则装配完成。如果未抵达目标深度D，则装配未完成，令t＝t+1，然后重新返回执行步骤2-3)。

Claims (1)

1.一种基于层级强化学习的机器人双轴孔装配方法，其特征在于，该方法分为训练阶段和实际装配阶段，包括以下步骤：

1)训练阶段；具体步骤如下：

1-1)预先确定层级强化学习方法的装配训练回合总数为L；设定双轴孔装配的目标深度为D，当机器人携带轴到达目标深度D时，一个回合结束；

在双轴孔装配的一个回合中，按照时间顺序将该回合分为n个互不相同的装配阶段，则一个回合中装配阶段序号ω的取值范围为{1，2，3…n}，ω的n种取值分别与装配过程n个互不相同的装配阶段一一对应；

设定奖励函数r(s_t)，代表观测到环境状态s_t时收获的奖励值；设定动作价值函数的学习率α_u，终止函数的学习率α_β，上层策略函数的学习率α_Ω，下层策略函数的学习率α_ω和衰减率γ；

1-2)设定机器人双轴孔装配的上层策略函数为π_Ω(θ_ω|s_t，ω，θ_Ω)，下层策略函数为π_ω(a_t|s_t，θ_ω)，终止函数为β_ω(s_t，θ_β)；

其中上层策略函数的输入量为s_t和ω，s_t代表t时刻观测获得的环境状态，双轴孔装配中的环境状态包括机器人在该时刻的力和扭矩信息[F_x，F_y，F_z，M_x，M_y，M_z]以及该时刻轴的位置位姿信息[P_x，P_y，P_z，O_x，O_y，O_z]，其中，F_x，F_y，F_z分别为机器人在工作空间坐标系下受到的沿x，y，z轴正方向的力，M_x，M_y，M_z分别为机器人在工作空间坐标系下受到的沿x，y，z轴正方向的扭矩，P_x，P_y，P_z分别为轴末端点在工作空间坐标系下的沿x，y，z坐标轴的位置坐标，O_x，O_y，O_z分别为轴线方向相对于x，y，z坐标轴的位置角；θ_Ω代表上层策略函数的参数；θ_ω是上层策略函数的输出量，代表ω所对应装配阶段内的运动控制参数；

下层策略函数的输入量为s_t，输出量为a_t，a_t代表装配过程中t时刻的装配动作

包括该时刻机器人在工作空间坐标系下分别绕x，y，z轴的三个旋转运动量

和三个平移运动量

上层策略函数的输出量θ_ω是下层策略函数的参数；

终止函数的输入量为s_t，θ_β是终止函数的参数，终止函数的输出值为取值{0，1}的判定结果，当终止函数的输出值等于1时代表双轴孔装配当前的装配阶段结束，当终止函数的输出值等于0时代表双轴孔装配当前的装配阶段未结束；

对上层策略函数参数θ_Ω和终止函数参数θ_β分别进行初始化；

定义动作价值函数Q_U(s_t，a_t，ω|θ_Q)，代表机器人在ω所对应装配阶段内，在状态s_t时，执行动作a_t时能够收到的奖励值，并对动作价值函数的参数θ_Q进行初始化；

设定装配训练回合数l的初始值为1；

1-3)在装配训练回合l开始时，机器人将轴运动到设定的初始位置，设定每个训练回合初始位置对应的时刻t＝1，设定该回合中装配阶段序号ω的初始值为1；在第1个训练回合的t＝1时刻，获取该时刻初始位置对应的s_t并输入初始的上层策略函数π_Ω(θ_ω|s_t，ω，θ_Ω)，该函数输出参数θ_ω作为下层策略函数π_ω(a_t|s_t，θ_ω)的参数的θ_ω的初始值；

1-4)在装配训练回合l中的t时刻，获取观测环境状态s_t，将s_t输入当前终止函数β_ω(s_t，θ_β)后，对当前终止函数的输出值进行判定：

如果输出值为1，代表当前装配阶段终止，则更新ω←ω+1，然后进入步骤1-5)；

如果输出值为0，代表当前装配阶段未终止，则进入步骤1-6)，θ_ω不更新；

1-5)将s_t输入当前上层策略函数π_Ω(θ_ω|s_t，θ_Ω)，该函数输出更新后的当前下层策略函数的参数θ_ω，然后进入步骤1-6)；

1-6)将s_t输入当前下层策略函数π_ω(a_t|s_t，θ_ω)，该函数输出t时刻机器人对应动作a_t；然后令机器人执行动作a_t，机器人携带轴抵达新的位置，得到t+1时刻的环境状态的状态s_t+1；

1-7)将s_t+1输入当前奖励函数，该函数输出t+1时刻收获的奖励值r(s_t+1)，然后根据时间差分算法对当前动作价值函数Q_U(s_t，a_t，ω|θ_Q)的参数θ_Q进行更新，得到更新后的当前动作价值函数Q_U(s_t，a_t，ω|θ_Q)：

其中，δ_t表示t时刻的时序差分误差，a_t+1表示在t+1时刻当前下层策略函数输出的动作；

表示将s_t+1输入当前下层策略函数π_ω(a_t|s_t，θ_ω)得到t+1时刻的动作a_t+1；

1-8)利用更新后的当前动作价值函数Q_U(s_t，a_t，ω|θ_Q)对当前上层策略函数和当前终止函数进行更新学习，更新参数θ_Ω和θ_β，表达式如下：

其中，

A_Ω(s_t+1，ω)＝Q_Ω(s_t+1，ω)-V_Ω(s_t+1)，

Q_Ω(s_t+1，ω)＝∑_aπ_ω(a_t|s_t，θ_ω)Q_U(s_t，a_t，ω|θ_Q)，

V_Ω(s_t+1)＝∑_ωπ_Ω(θ_ω|s_t+1，ω，θ_Ω)Q_Ω(s_t+1，ω)；

θ_Ω和θ_β更新后，分别得到更新后的当前上层策略函数和更新后的当前终止函数；

1-9)根据s_t+1，判断机器人携带轴是否到达了指定的目标深度D：

如果抵达目标深度D，则当前装配训练回合l终止，执行步骤1-10)；

如果未抵达目标深度D，则当前装配训练回合l未终止，令t＝t+1，然后重新返回步骤1-4)；

1-10)判断装配训练的回合数l是否到达设定的装配训练回合总数L：

如果l≥L，则训练阶段结束，最终获得的训练完毕上层策略函数参数θ_Ω和终止函数参数θ_β分别记为

和

然后进入步骤2)；

如果l＜L，则更新装配训练的回合数l←l+1，然后重新返回步骤1-3)；

2)实际装配阶段；具体步骤如下：

2-1)在实际装配开始时，将

作为终止函数β_ω(s_t，θ_β)的参数，此时的终止函数记为

将

作为上层策略函数π_Ω(θ_ω|s_t，ω，θ_Ω)的参数，此时的上层策略函数记为

初始化t＝1，ω＝1；机器人携带轴运动到设定的初始位置，获取此时刻对应的环境状态并作为t时刻的环境状态s_t；

2-2)将s_t输入上层策略函数

该函数输出参数θ_ω作为下层策略函数π_ω(a_t|s_t，θ_ω)的参数θ_ω的初始值；

2-3)将s_t输入终止函数

对该函数的输出值进行判定：

如果输出值为1，代表当前装配阶段终止，则更新ω←ω+1，进入步骤2-4)；如果输出值为0，代表当前装配阶段未终止，则进入步骤2-5)，θ_ω不更新；

2-4)将s_t输入上层策略函数

该函数输出更新后的参数θ_ω；

2-5)将s_t输入下层策略函数π_ω(a_t|s_t，θ_ω)，该函数输出t时刻机器人对应动作a_t；然后令机器人执行动作a_t，携带轴抵达新的位置，更新环境状态为s_t+1；

2-6)根据s_t+1，判断机器人携带轴是否到达了目标深度D：

如果抵达目标深度D，则装配完成；如果未抵达目标深度D，则装配未完成，令t＝t+1，然后重新返回步骤2-3)。