一种基于人机合作的核运维机器人轴孔装配方法与流程

本发明属于工业机器人技术领域，尤其涉及一种基于人机合作的核运维机器人轴孔装配方法。

背景技术：

在核电厂中机器人的装配过程处在非结构化环境，其操作环境存在很多不确定性，使用传统的控制方法并不能很好的解决问题。传统的控制方式是通过教机器人使用称为“示教器”的控制箱定义目标的关键位置和动作来执行装配任务。但是这种在线编程的方式通常需要消耗大量的时间，即使在完成编程后，由于生产环境的改变，仍然需要花费很长时间来调整用于将机器人部署在新环境的参数，因此并不适用于核电厂环境下的轴孔装配任务。另一种常见的控制方式是基于视觉传感器完成装配任务，这种控制方式需要测量销钉和孔之间的位置和方向。但是，在许多实际的装配任务中，孔的位置和方向通常是未知或不准确的，由于位置误差或部分遮挡而产生的较大接触力将导致装配过程失败，所以这种基于视觉传感器的装配方法难以成功完成装配任务。

最近的研究表明，深度强化学习(deepreinforcementlearning，drl)算法已被用来解决从游戏到机器人应用的控制方法中,深度强化学习算法是一种新兴的通用人工智能算法技术，是人工智能迈向智能决策的重要一步,将具有良好适应性和鲁棒性的深度强化学习算法应用在机器人轴孔装配任务中，可以有效提升机器人轴孔装配的精度。jianlanluo等人将机器人手臂腕传感器的扭矩测量结果合并为两个部分，将其集成到策略网络学习过程中，并在与神经网络耦合的导纳控制器中使用，这使得机器人可以学习接触环境复杂的装配任务，而无需精确的关节力矩控制或被动的机械柔韧性。zhiminhou等人提出了一种基于模型的深度确定性策略梯度(ddpg)算法，用来学习解决多孔轴孔装配问题的通用装配策略，为了在真实环境的组装任务中实现快速学习过程，在策略搜索阶段，采用了一种混合探索策略驱动的探索方式。yongxiangfan等人结合监督学习和强化学习的优点，利用监督学习的轨迹优化为策略提供初始指导，利用强化学习的ac算法来建立评估系统，进而提出了一种用于高精度工业装配的学习框架，与强化学习相比，所提出的学习框架更加有效，并且比监督学习具有更好的稳定性能。以上方法能够实现部分典型环境、对象、状态的任务，但针对核电厂这类具有典型特征对象的轴孔装配任务难以广泛应用。

因此，急需一种为降低运维人员所受辐射剂量，提高运维效率的核运维机器人轴孔装配任务控制方法。

技术实现要素：

针对现有技术中的上述不足，本发明提供的一种基于人机合作的核运维机器人轴孔装配方法，解决了核电厂这类具有工作环境复杂、辐射剂量较高、人工操作任务繁重以及人为失误可能引起更为严重的事故等特征对象的轴孔装配问题。

为了达到以上目的，本发明采用的技术方案为：

本方案提供一种基于人机合作的核运维机器人轴孔装配方法，包括以下步骤：

s1、搜索阶段：由主端机械臂利用操作空间映射算法控制从端机械臂将其由末端夹持器夹持住的销钉移至孔周围，控制销钉与孔端面接触，并利用基于力反馈信息对销钉进行平移，并判断反馈数据是否为0，若是，则销钉到达孔的中心位置，完成轴孔装配任务的搜索，并进入步骤s2，否则，重复步骤s1；

s2、插入阶段：获取销钉的位姿信息以及力矩传感器的信息，并利用已训练连续型动作的深度确定性策略网络推动销钉插入孔中，完成轴孔装配任务的插入。

进一步地，所述步骤s1中主端机械臂的末端夹持器的空间位置的表达式如下：

pm(t)＝(pmx(t),pmy(t),pmz(t))^t

其中，pm(t)表示主端机械臂的末端夹持器的空间位置，pmx(t),pmy(t)和pmz(t)分别表示在t时刻主机械臂末端在x，y和z三维空间中的操作范围。

再进一步地，所述步骤s1中从端机械臂的末端夹持器的空间位置的表达式如下：

ps(t)＝(psx(t),psy(t),psz(t))^t

其中，ps(t)表示从端机械臂的末端夹持器的空间位置，psx(t),psy(t)和psz(t)分别表示在t时刻从机械臂末端在x，y和z三维空间中的操作范围。

再进一步地，所述步骤s1中主端机械臂和从端机械臂的操作空间映射关系的表达式为：

其中，pmx(t)表示主端机械臂末端在x方向的位置分量，pmy(t)表示主端机械臂末端在y方向的位置分量，pmz(t)表示主端机械臂末端在z方向的位置分量，pmxmin表示主端机械臂末端在x方向的位置分量最小值，pmxmax表示主端机械臂末端在x方向的位置分量最大值，pmymin表示主端机械臂末端在y方向的位置分量最小值，pmymax表示主端机械臂末端在y方向的位置分量最大值，pmzmin表示主端机械臂末端在z方向的位置分量最小值，pmzmax表示主端机械臂末端在z方向的位置分量最大值，psx(t)表示从端机械臂末端在x方向的位置分量，psy(t)表示从端机械臂末端在y方向的位置分量，psz(t)表示从端机械臂末端在z方向的位置分量，psxmin表示从端机械臂末端在x方向的位置分量最小值，psxmax表示从端机械臂末端在x方向的位置分量最大值；psymin表示从端机械臂末端在y方向的位置分量最小值，psymax表示从端机械臂末端在y方向的位置分量最大值，pszmin表示从端机械臂末端在z方向的位置分量最小值，pszmax表示从端机械臂末端在z方向的位置分量最大值。

再进一步地，所述步骤s2中连续型动作的深度确定性策略梯度网络包括演说家网络actor和评论家网络critic；

所述演说家网络actor包括在线策略网络和目标策略网络；所述评论家网络critic包括在线价值网络以及目标价值网络。

再进一步地，所述步骤s2包括以下步骤：

s201、获取销钉的信息和力矩传感器的信息，并利用对销钉的信息和力矩传感器的信息进行筛选和计算得到销钉的位姿信息；

s202、将销钉位姿信息作为数据集发送至深度强化学习模型，并利用演说家网络actor根据当前数据集按照确定性行为策略选择动作，并同时发送动作指令至控制器控制从端机械臂运动；

s203、根据从端机械臂的运动获取下一个数据集，并将下一个数据集作为环境状态发送至从端机械臂；

s204、利用评论家网络critic确定当前的环境状态，并分别更新演说家网络actor和评论家网络critic；

s205、根据当前的环境状态和从端机械臂的动作判断是否获取最优控制策略，若是，则完成对连续型动作的深度确定性策略网络的训练，并进入步骤s206，否则，返回步骤s201；

s206、根据从端机械臂末端的力反馈信息和销钉的位姿信息，利用连续型动作的深度确定性策略网络对销钉的位置和方向进行调整，控制销钉插入孔中，完成轴孔装配任务的插入。

再进一步地，所述步骤s201具体为：

获取销钉的信息和力矩传感器的信息，并根据销钉的信息和力矩传感器的信息，利用正运动学从从端机械臂中编码器的测量关节角度筛选和计算得到销钉的位姿信息。

再进一步地，所述步骤s202中从端机械臂的动作空间的表达式如下：

at＝[px^d,py^d,pz^d,rx^d,ry^d,rz^d]

其中，at表示从端机械臂的动作空间，px^d表示销钉在x方向的位置分量，py^d表示销钉在y方向的位置分量，pz^d表示销钉在z方向的位置分量，rx^d表示销钉在x方向的方向分量，ry^d表示销钉在y方向的方向分量，rz^d表示销钉在z方向的方向分量。

再进一步地，所述步骤s202中更新评论家网络critic的表达式如下：

yi＝ri+γq'(si+1,μ'(si+1|θ^μ')|θ^q')

其中，l表示更新后的评论家网络critic，n表示转换经验样本，yi表示目标q值，q表示critic目标价值网络，si表示第i次的状态，ai表示第i次的确定性行为策略，θ^q表示目标价值网络的参数，ri表示第i次回合的奖励值，γ表示折扣因子，q'表示critic的目标价值网络，θ^q'表示critic的目标价值网络的参数，μ'表示目标策略网络，θ^μ'表示目标策略网络的参数；；

所述更新演说家网络actor的表达式如下：

其中，表示更新后的演说家网络actor，表示梯度更新后的在线策略网络，s表示当前的状态，a表示当前的确定性行为策略，μ和θ^μ均表示actor的在线策略网络，表示梯度更新后的确定性行为策略。

再进一步地，所述步骤s205中最优控制策略的表达式如下：

μ^*＝argmaxjβ(μ)

jβ(μ)＝eμ[r1+γr2+γ²r2+…+γⁿrn]

其中，μ^*表示最优控制策略，jβ(μ)表示ddpg算法的目标函数，eμ表示折扣累积奖励的期望，γ表示折扣因子，rn表示正向奖励，c表示常数，k表示装配完成时的实际步数，kmax表示是一个回合的最大步数。

本发明的有益效果：

(1)本发明通过将核运维机器人轴孔装配分为搜索阶段和插入阶段，有效地提高核设施日常运行维护的效率；保障核设施的安全运行；降低工作人员的受辐射剂量；可以实现核电厂堆外核测探测器安装过程中，探测器与仪器井的轴孔装配问题。

(2)本发明为降低运维人员所受辐射剂量，提高运维效率，在搜索阶段把机器人智能跟人的智能充分结合起来：利用人的经验知识增强操作过程的可靠性，保证风险可控；同时，在插入阶段利用人工智能算法使机器人在局部区域自主动作。

附图说明

图1为本发明的方法流程图。

图2为本发明中插入阶段的流程图。

图3为本实施例中搜索阶段寻孔模型图。

图4为本实施例中轴孔装配的作业流程图。

图5为本实施例中主从机械臂操作空间映射算法。

图6为本实施例中插入阶段算法结构图。

图7为本实施例中搜索阶段的主从异构遥操作系统。

图8为本实施例中搜索阶段的ros节点图。

图9为本实施例中搜索阶段主端机械臂末端运行轨迹示意图。

图10为本实施例中搜索阶段从端机械臂末端运行轨迹示意图。

图11为本实施例中插入阶段的仿真系统示意图。

图12为本实施例中每个回合的累积奖励示意图。

图13为本实施例中每个回合的装配步数示意图。

图14为本实施例中单一回合中力及力矩的变化示意图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

实施例

为降低运维人员所受辐射剂量，提高运维效率，本文提出一种基于人机合作的核运维机器人轴孔装配方法，在堆外核测探测器安装流程中引入机器人自主控制技术，但所有作业必须在有人监控的前提下进行，即“遥操作+局部自主”。把机器人智能跟人的智能充分结合起来：利用人的经验知识完成轴孔装配的寻孔任务，增强操作过程的可靠性，保证风险可控；同时，利用drl算法使机器人在局部区域自主完成销钉的插入任务，如图1所示，其实现方法如下：

s1、搜索阶段：由主端机械臂利用操作空间映射算法控制从端机械臂将其由末端夹持器夹持住的销钉移至孔周围，控制销钉与孔端面接触，并利用基于力反馈信息对销钉进行平移，并判断反馈数据是否为0，若是，则销钉到达孔的中心位置，完成轴孔装配任务的搜索，并进入步骤s2，否则，重复步骤s1；

s2、插入阶段：获取销钉的位姿信息以及力矩传感器的信息，并利用已训练连续型动作的深度确定性策略网络推动销钉插入孔中，完成轴孔装配任务的插入，如图2所示，其实现方法如下：

s201、获取销钉的信息和力矩传感器的信息，并利用对销钉的信息和力矩传感器的信息进行筛选和计算得到销钉的位姿信息；

s202、将销钉位姿信息作为数据集发送至深度强化学习模型，并利用演说家网络actor根据当前数据集按照确定性行为策略选择动作，并同时发送动作指令至控制器控制从端机械臂运动；

s203、根据从端机械臂的运动获取下一个数据集，并将下一个数据集作为环境状态发送至从端机械臂；

s204、利用评论家网络critic确定当前的环境状态，并分别更新演说家网络actor和评论家网络critic；

s205、根据当前的环境状态和从端机械臂的动作判断是否获取最优控制策略，若是，则完成对连续型动作的深度确定性策略网络的训练，并进入步骤s206，否则，返回步骤s201；

s206、根据从端机械臂末端的力反馈信息和销钉的位姿信息，利用连续型动作的深度确定性策略网络对销钉的位置和方向进行调整，控制销钉插入孔中，完成轴孔装配任务的插入。

本实施例中，根据核运维机器人在核电厂环境中的运行特点，将轴孔装配任务分为搜索阶段和插入阶段：搜索阶段：主端机械臂控制夹持住销钉的从端机械臂进行寻孔，将销钉放置在孔的中心位置；插入阶段：从端机械臂依据力反馈信息实时调整销钉的位置和方向，驱动销钉插入孔中。

本实施例中，核电厂堆腔底部的堆外核测探测器安装作业空间狭小，且安装环境中其他设备较为精密，不能发生碰撞，机器人自主完成搜索阶段的寻孔任务难度较大，需要借助人工辅助搜索的方式完成。主端机械臂通过操作空间映射算法控制从端机械臂将由末端夹持器夹持住的销钉移动到孔的周围，需要说明的是，从端机械臂每次都是以相同的姿态夹持销钉相同的位置。然后再控制销钉与孔端面接触，基于力反馈信息对销钉进行平移，当力反馈数据为0时，销钉到达孔的中心位置。搜索阶段寻孔模型如图3所示。

本实施例中，在核电厂探测器与仪器井的轴孔装配任务中，销钉和孔之间的间隙很小，通常只有10微米，对于操作人员来说是极其困难的。对于机器人来说，完成比自身位置精度更小的插入任务也是非常困难的。因此，机器人需要采用基于力矩传感器信息的连续型动作的深度确定性策略梯度网络自主完成精确的插入任务。

本实施例中，根据对于核运维机器人轴孔装配任务的问题描述，轴孔装配的作业流程如下图4所示。在搜索阶段建立主从机械臂控制系统，实现异构遥操作控制，将销钉放置在孔的中心位置。在插入阶段基于drl算法模型，以从端机械臂末端的力反馈和销钉位姿两种信息为依据构建马尔科夫决策过程，通过训练得到从端机械臂当前状态与动作的映射关系从而获取最优控制策略，依据力反馈信息实时调整销钉的位置和方向，推动销钉插入孔中。

本实施例中，如图5所示，在核运维机器人轴孔装配任务中，主端机械臂和从端机械臂是异构系统，通过关节空间映射的方法建立主从机械臂各关节之间的控制系统将非常复杂，而采用操作空间映射的方法，将主端机械臂末端的位姿通过操作空间映射算法映射到从端机械臂末端的位姿可以解决这一问题。由于核电厂堆腔底部的堆外核测探测器安装作业空间是不规则的，因此在进行操作空间映射时需要对主从机械臂的操作空间进行限定，设主从机械臂末端在三维空间中的操作范围为pmd∈[pmdmin,pmdmax]和psd∈[psdmin,psdmax]，d分别取x，y，z方向，pmd与psd分别为主从机械臂末端在某一方向的位置分量，主从机械臂操作空间映射算法如图4所示。在t时刻时，主从机械臂末端执行器空间位置分别为pm(t)＝(pmx(t),pmy(t),pmz(t))^t,ps(t)＝(psx(t),psy(t),psz(t))^t，映射关系为：

通过公式(1)得到从端机械臂末端在工作空间中的位置ps，根据逆运动学可以计算出一组关节角作为输入传递到从端机械臂的编码器中，进而实现主从机械臂的跟随控制。

本实施例中，可以将核运维机器人轴孔装配任务的插入阶段公式化为一个马尔可夫决策过程模型(markovdecisionprocess，mdp)。mdp作为强化学习的基本框架，由一个四元组构成，即(s,a,psa,r)，其中，s为状态空间集，a为动作空间集，psa为状态转移概率，r为奖励函数。在插入阶段，从端机械臂作为强化学习中的智能体，当从端机械臂执行某个动作后，装配环境将会转到一个新的状态。对于该新的状态，装配环境会给出一个奖励信号(正向奖励或者负向奖励)。智能体根据新的状态和装配环境反馈的奖励信号，按照一定的策略执行新的动作。智能体学习的目标就是学会将状态映射到动作从而求得从端机械臂控制的最优策略μ^*。

从端机械臂的状态空间定义为：

st＝[fx,fy,fz,mx,my,mz,px,py,pz,rx,ry,rz](2)

其中，f和m分别表示力矩传感器返回的力和力矩；p和r分别表示根据正运动学从从端机械臂编码器测量的关节角度计算出的由末端夹持器夹持的销钉的位置和方向，下标x，y，z表示从端机械臂末端坐标轴的方向。

从端机械臂的动作空间定义为：

at＝[px^d,py^d,pz^d,rx^d,ry^d,rz^d](3)

其中，p^d是销钉的位置，r^d是销钉的方向，根据逆运动学计算出一组关节角作为输入传递到从端机械臂的编码器中。

强化学习算法通过使累积奖励最大化来学习机器人装配策略：

rk＝rk+γrk+1+γ²rk+2+…+γ^n-krn＝rk+γrk+1(4)

其中，γ是折扣因子，是介于[0,1]的常数；rk是分配给每个状态-动作对的即时奖励；k是步数的索引；n是一个回合的总步数。

机器人轴孔装配的插入阶段从开始到结束的过程称为一个回合。在本文提出的算法中，仅在每个回合的最后一步计算一个奖励r。如果装配成功，则会向网络提供一个正向奖励：

其中，c为常数；k是装配完成时的实际步数；kmax是一个回合的最大步数。

本文提出的算法目标是使用最少的步数完成装配任务，如果不能在kmax内完成任务，则会向网络提供一个负向奖励：

其中，d是孔的深度，dt是时间步长t时销钉插入孔的深度

本实施例中，在ddpg算法中，将深度神经网络与确定性策略梯度(deterministicpolicygradient，dpg)算法进行融合，并使用演说家-评论家(actor-critic，ac)算法作为该算法的基本框架。分别使用参数为θ^μ的策略网络来表示确定性行为策略a＝μ(s|θ^μ)，输入为当前的状态s，输出确定性的动作值a；使用参数为θ^q的价值网络来表示动作值函数q(s,a|θ^q)，用于求解贝尔曼方程。ddpg算法框架满足强化学习的马尔可夫决策过程，actor和critic为智能体的内容，其中，actor负责策略网络，critic负责价值网络。通过智能体与环境进行交互的过程，把交互所产生的样本存储在经验池中，下一时间步从经验池中随机提取小批量样本数据传递给actor和critic进行计算。

本实施例中，ddpg算法分别为策略网络和价值网络各自创建两个神经网络，一个为在线网络，另一个为目标网络。在线策略网络，负责策略网络参数θ^μ的迭代更新，负责根据当前状态st选择当前动作at，用于和环境交互生成st+1,rt；目标策略网络，负责根据经验池中采样的下一状态si+1选择最优下一动作ai+1；在线价值网络，负责价值网络参数θ^q的迭代更新，负责计算当前q值q(si,ai|θ^q)；目标价值网络，负责计算目标q值q'(si+1,μ'(si+1|θ^μ')|θ^q')部分。ddpg算法使用了4个网络模型，其更新关系是在结束一次小批量样本数据的训练后，通过梯度上升或梯度下降算法更新在线网络的参数，然后再通过软更新(每一时间步都会更新目标网络)算法更新目标网络的参数，核运维机器人轴孔装配任务的插入阶段算法结构如图6所示。

本实施例中，机器人在对轴孔装配环境进行探索，其目的是寻找更多潜在的更优策略，因此在网络模型的训练过程中，引入随机噪声影响动作的选择。ddpg算法中使用ou随机过程(ornstein-uhlenbeck)作为引入的随机噪声。

ddpg算法的目标函数被定义为折扣累积奖励的期望，即：

jβ(μ)＝eμ[r1+γr2+γ²r2+…+γⁿrn](7)

其中，β为行为策略，行为策略根据当前在线策略网络μ和随机ou噪声生成随机过程，并从该随机过程进行采样获得动作值at。

ddpg算法的目标是求解最优确定性行为策略μ^*，即目标函数最大化的策

μ^*＝argmaxjβ(μ)(8)

从经验池中随机采样小批量的n个转换经验样本(si,ai,ri,si+1)，使用最小化损失函数来更新critic网络：

其中，yi＝ri+γq'(si+1,μ'(si+1|θ^μ')|θ^q')，θ^μ'为actor目标策略网络的参数，θ^q'为critic目标价值网络的参数，使用梯度下降算法更新网络模型中的参数。actor目标策略网络μ'和critic目标价值网络q'使得网络在学习过程中更加稳定，更容易收敛。

目标函数jβ(μ)关于策略网络参数θ^μ的梯度，等价于动作值函数q(s,a|θ^q)关于θ^μ的期望梯度，因此遵循链式求导法则对目标函数进行求导，使用梯度策略算法更新actor网络：

对公式(10)使用梯度上升算法的目标函数进行优化计算，使用梯度上升的目标是提高折扣累积奖励的期望。最终使得算法沿着提升动作值函数q(s,a|θ^q)的方向更新actor策略网络的参数θ^μ。

本实施例中，在真实场景中搭建基于phantomomni力反馈设备和ur5机械臂的主从异构遥操作系统，编写python语言脚本通过机器人开源操作系统ros完成主从机械臂的跟随控制，并完成搜索阶段的寻孔任务。搜索阶段的主从异构遥操作系统如图7所示。首先操作者控制主端机械臂完成一系列动作，然后ros系统获取主端机械臂节点omni1的末端位姿信息并发布名为/phantom/pose的主题，从端机械臂在python脚本中设置一个节点ur5_control实时订阅该主题，最后将订阅的主端机械臂末端位姿信息通过操作空间映射算法发送给从端机械臂，控制销钉到达孔的中心位置。其中，节点robot_state_publisher用来订阅节点omni1发布的名为joint_states的主题，该节点可以观察主端机械臂末端的实时位置信息，用于判断末端是否在主端工作空间内。搜索阶段的ros节点图如图8所示。通过主从异构遥操作系统完成搜索阶段的寻孔任务，在一次寻孔的过程中，我们可视化了机器人寻孔的轨迹，如图9、图10所示，图9中phantomomni为力反馈设备为主端机械臂，图10中ur5为从端机械臂。由于充当主从机械臂的两个设备的空间坐标轴设置存在差异，为了便于操作，将从端机械臂末端的y坐标轴对应主端机械臂末端的-z坐标轴，从端机械臂末端的z坐标轴对应主端机械臂末端的y坐标轴，从图11、图12中观察到，主从机械臂末端可以实现实时跟随并完成搜索阶段的寻孔任务。

本实施例中，在机器人仿真软件webots中建立轴孔装配仿真环境，编写python语言脚本控制仿真环境中的机器人完成轴孔装配任务的插入阶段。其中包括ur5机械臂，六维力矩传感器，夹持器和轴孔装配工件。仿真环境中，基于drl算法模型，通过训练得到从端机械臂当前状态与动作的映射关系从而获取最优控制策略，依据力反馈信息实时调整销钉的位置和方向，驱动销钉插入孔中。插入阶段的仿真系统如图11所示。首先获取webots中销钉位姿和力矩传感器信息，将获取的信息进行筛选和计算，将其结果作为环境状态发送给drl智能体进行决策；actor网络根据当前状态按照确定性行为策略选择合适的动作，同时发送动作指令给控制器控制从端机械臂运动，然后获取下一个数据作为环境状态再次发送给智能体；critic网络判断当前状态的价值并更新网络。重复以上过程，直到装配成功或者达到最大的训练回合数。

本实施例中，索阶段已将销钉放置在孔的中心位置，插入阶段的drl训练实验进行了300回合，对于仿真中的一个回合，销钉的方向是在随机初始化的，每回合最大步数kmax为300，部分参数见表1。实验训练结果如图12和图13所示，图11为训练过程中每个回合的累积奖励变化情况，图13为训练过程中每个回合的装配步数变化情况，从图中可以看到，刚开训练时，装配失败的次数较多，每个回合得到的累积奖励很低且装配步数较多，随着装配成功次数及训练回合数的增加，智能体逐渐学会如何通过调整销钉的位姿完成装配，当训练回合数达到200回合时，累积奖励趋于稳定且达到最大值附近，单个回合的装配步数也逐渐变少。智能体通过不断的训练，学会了插入阶段的轴孔装配控制策略。

训练完成之后，基于学习到的插入阶段轴孔装配控制策略执行插入阶段的装配任务，在一次成功装配的单一回合中，装配过程中的力和力矩变化如图14所示。从图中可以看到插入过程中从端机械臂末端的力和力矩分量最终都在零值附近，说明销钉成功插入孔中。