CN111515928B - 机械臂运动控制*** - Google Patents

Abstract

本发明提供了一种机械臂运动控制***，所述机械臂运动控制***包括智能柔顺装配平台、运动装配工件和静止装配工件，其中：所述智能柔顺装配平台控制六自由度协作机械臂，所述六自由度协作机械臂包括末端执行器，所述智能柔顺装配平台生成所述六自由度协作机械臂的状态信息；所述智能柔顺装配平台根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，所述末端执行器夹取运动装配工件进行装配，装配至所述静止装配工件上。

Description

机械臂运动控制***

技术领域

本发明涉及机器人装配技术领域，特别涉及一种机械臂运动控制***。

背景技术

多自由度机器人是一种能够完成模拟人手臂，手腕和手功能的机械电子装置。它可把任何物件或工具按空间(位置和姿态)的时变要求进行移动，从而完成某一工业生产的作业要求。在我国劳动成本不断上升的今天，自动化也会为企业带来效益。除了重型的机加工任务，原本依赖人类手指触觉才能完成的小部件装配任务，如手机或者平板电脑装配生产线，通过加装关节力矩传感器，机器人也能够被赋予触觉，协助人类或独立完成这些工作将极大的提高生产效率。

活塞装配或者齿轮装配这样的精密装配是多维力矩传感器的常见应用。这些精密安装的操作平面不会仅仅是垂直的或是水平的，某些安装情况下由于操作平台或者待装配工件以及机械臂的重复精度误差存在将为实际装配带来很大的难度，对于精度的要求也很难保证。工业上能够用于柔顺装配技术。出于全自动装配的考虑，装配过程中机器人的控制是否精确直接影响装配结果，目前大连理工大学和清华大学针对轴孔装配展开的深度学习通过视觉信息和位置信息来进行反馈，在光线不稳定或者空间狭小复杂多变的环境下，无法通过视觉来获取位置信息。

发明内容

本发明的目的在于提供一种机械臂运动控制***，以解决现有的全自动装配过程中机械臂控制精度难以保证的问题。

为解决上述技术问题，本发明提供一种机械臂运动控制***，所述机械臂运动控制***包括智能柔顺装配平台、运动装配工件和静止装配工件，其中：

所述智能柔顺装配平台控制六自由度协作机械臂，所述六自由度协作机械臂包括末端执行器，所述智能柔顺装配平台生成所述六自由度协作机械臂的状态信息；

所述智能柔顺装配平台根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；

所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，所述末端执行器夹取运动装配工件进行装配，装配至所述静止装配工件上。

可选的，在所述的机械臂运动控制***中，所述运动装配工件为轴，所述静止装配工件为孔。

可选的，在所述的机械臂运动控制***中，所述六自由度协作机械臂的每个关节均安装有力矩传感器；所述力矩传感器实时采集各个关节的状态信息，实现灵敏的拖动示教和碰撞检测；

所述智能柔顺装配平台包括上位机与机械臂控制器，所述上位机采用实时通信接口与所述机械臂控制器进行数据交换，所述上位机通过实时通信接口接收所述力矩传感器采集的六自由度协作机械臂的状态信息，根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；

所述上位机发送机械臂状态控制指令至所述机械臂控制器，以实现所述机械臂控制器输出搜索装配算法对所述六自由度协作机械臂进行控制；

所述状态信息包括姿态状态信息、速度状态信息和转矩状态信息，所述机械臂状态控制指令包括位姿控制指令、速度控制指令和转矩控制指令；

所述上位机将所述末端执行器的质量和惯性矩阵补偿给机械臂控制器，以实现力矩控制补偿。

可选的，在所述的机械臂运动控制***中，所述上位机通过获取所述力矩传感器输出的力矩信息τ_输出1τ_输出2τ_输出3τ_输出4τ_输出5τ_输出6，采集六自由度协作机械臂的状态信息并将所述状态信息进行处理，生成机械臂本体的状态集：

其中，F_x，F_y，F_z表示从六个关节的力矩传感器获得的平均力，M_x，M_y表示机械臂末端两个关节的力矩传感器检测的力矩；

和/>

表示机械臂末端两个关节在二维坐标系的位置误差，x，y，z分别表示空间坐标轴的三个方向坐标。

可选的，在所述的机械臂运动控制***中，通过将正向运动学应用于机械臂编码器测量的关节角度计算机械臂末端两个关节在二维坐标系的位置误差；

计算

和/>

的取整值，当/>

和/>

的取整值为(–c，c)时，作为位置数据P_x和P_y代替原点(0，0)，静止装配工件的中心范围为-c<x<c，-c<y<c，其中c是位置误差的余量；

当

和/>

的取整值是(c，2c)时，/>

和/>

将被舍入为c，依此类推。

可选的，在所述的机械臂运动控制***中，根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测包括：将所述六自由度协作机械臂至于初始位姿，采用神经网络对所述六自由度协作机械臂进行控制，所述机械臂控制器设置的控制集为

其中，F_x ^d，F_y ^d，F_z ^d表示六个关节施加的平均力，R_x ^d，R_y ^d表示机械臂末端两个关节的位姿；

根据所述控制集通过控制策略网络产生各个关节的转矩控制指令u(t)，计算每个关节运行的优势函数估计值；

根据产生的训练数据，通过随机策略梯度按照多个步骤建立优化函数，并更新策略网络权重。

可选的，在所述的机械臂运动控制***中，所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，夹取待装配的工件进行装配包括：

接近阶段，所述六自由度协作机械臂夹持所述运动装配工件到达待装配的静止装配工件上方的同轴心位置；

搜索阶段，所述上位机将位姿控制指令和速度控制指令发送至所述机械臂控制器，所述六自由度协作机械臂采用轴空间运动使所述运动装配工件向静止装配工件移动，并使两者处于接触状态与未接触状态的临界点；

***阶段，将运动装配工件的轴和静止装配工件的孔对齐后，采用Z方向的力控算法，将运动装配工件的轴向下***静止装配工件的孔中；

***完成阶段，通过检测Z方向的位置判断是否装配完成，如果装配成功则所述六自由度协作机械臂松开所述运动装配工件后退出，如果装配超时则判断本次装配失败。

可选的，在所述的机械臂运动控制***中，所述搜索阶段包括四次搜索步骤，每个步骤的控制集分别为：

1)

2)

3)

4)

其中，

所述***阶段包括：采集六自由度协作机械臂的状态信息并将所述状态信息进行处理，当生成机械臂本体的状态集为下式时，***成功：

s＝[0,0,F_z,M_x,M_y,0,0]，

通过M_X和M_y判断运动装配工件的运动方向，通过F_z判断所述运动装配工件是否卡住，***动作的控制集为：

1)

2)

3)

4)

5)

可选的，在所述的机械臂运动控制***中，检测Z方向的位置判断是否装配完成包括，计算惩罚参数：

其中，d为运动装配工件与静止装配工件位置之间的实时距离，D为运动装配工件与静止装配工件位置之间的目标距离，d₀为静止装配工件的初始位置误差，根据惩罚参数计算是从静止装配工件的初始位置沿垂直方向向下的位移，

/>

其中Z是***目标深度，z是从静止装配工件的初始位置沿垂直方向向下的位移；

当-1≤r<1时，装配成功。

可选的，在所述的机械臂运动控制***中，将运动装配工件固定在六自由度协作机械臂的末端执行器上后，通过末端执行器的CAD三维模型，计算出重力矩阵和惯性矩阵；将末端执行器的质量、质心位置、重力矩阵和惯性矩阵补偿给机械臂控制器。

在本发明提供的机械臂运动控制***中，通过智能柔顺装配平台生成六自由度协作机械臂的状态信息，建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法，六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，末端执行器夹取运动装配工件进行装配，装配至静止装配工件上，解决了由于工件精度和一致性差造成装配失败率高的问题，通过力控算法及搜索装配算法，自动找到工件之间的正确的装配位置，代替人工完成装配，两种控制回路最后都叠加到关节空间输出关节力矩，相对于在机械臂末端添加传感器的方案，提高了动态特性，实现了主动柔顺控制，在装配过程体现柔性，不仅提高了装配成功率，同时也不会损坏机械臂或者工具工件。

根据本发明实施的结合力控算法的搜索装配算法，将装配方法划分为四个阶段，对每个阶段的进出条件进行约束，使得装配过程稳定可靠。控制方法充分发挥了六自由度协作机械臂关节内部集成力矩传感器的优点，实现了力控制与位置控制的解耦，两种控制回路最后都叠加到关节空间输出关节力矩，同时提高了***的动态响应特性。

附图说明

图1是本发明一实施例机械臂运动控制***中六自由度协作机械臂示意图；

图2是本发明一实施例机械臂运动控制***中搜索装配算法示意图；

图3是本发明一实施例机械臂运动控制***中力控算法示意图。

具体实施方式

以下结合附图和具体实施例对本发明提出的机械臂运动控制***作进一步详细说明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。

本发明的核心思想在于提供一种机械臂运动控制***，以解决现有的全自动装配过程中机械臂控制精度难以保证的问题。

本发明考虑在安装有关节力矩传感器的机械臂中，通过深度强化学习将处于不同状态的关节力矩信息分类从而实现类人手触觉的柔顺装配。机器人力和力矩的参数准确性是精确控制的必要条件，由于装配时负载重力、安装误差等扰动，使机器人控制所需要的实际力和力矩难以准确计算，就需要对接触力和力矩进行预测，其预测结果可作为实际控制的重要参考，则预测精度越高，实际控制的装配效果越好。

实际装配中的难度不仅如此，当前该应用所需要的精度补偿和反馈大多由视觉提供，但由于实际装配时零件之间存在的间隙只有人类发丝直径的十分之一，所以仅靠视觉技术还是无法做到完美控制，例如目前大连理工大学和清华大学针对轴孔装配展开的深度学习通过视觉信息和位置信息来进行反馈，在光线不稳定或者空间狭小复杂多变的环境下，无法通过视觉来获取位置信息，因此在现有的实际应用中，通过采用在机械臂末端加入六维力传感器来代替触觉来实现柔顺控制。传统的六维力传感器可以测量x,y,z三个方向的力和力矩。但由于六位力传感器通常安装在机械臂末端，需要考虑机械臂的工作环境如粉尘，磕碰等，而且安装在末端的六维力传感器会占用机械臂的工作负载，会对机械臂的重心造成偏移，影响机械臂的准确性。仅靠在末端安装六维力传感器无法保障机械臂人机协作安全性。如果通过在机械臂的每个关节加装力矩传感器则在实现柔顺力控的同时还需要完善如触停、拖动示教等功能。如何提供一种可以用结合关节力矩控制实现柔性装配的方法，是当前需要解决的技术问题。

为实现上述思想，本发明提供了一种机械臂运动控制***，所述机械臂运动控制***包括智能柔顺装配平台、运动装配工件和静止装配工件，其中：所述智能柔顺装配平台控制六自由度协作机械臂，所述六自由度协作机械臂包括末端执行器，所述智能柔顺装配平台生成所述六自由度协作机械臂的状态信息；所述智能柔顺装配平台根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，所述末端执行器夹取运动装配工件进行装配，装配至所述静止装配工件上。

<实施例一>

本实施例提供一种机械臂运动控制***，所述机械臂运动控制***包括智能柔顺装配平台、运动装配工件和静止装配工件，其中：如图1所示，所述智能柔顺装配平台控制六自由度协作机械臂，所述六自由度协作机械臂包括末端执行器，所述智能柔顺装配平台生成所述六自由度协作机械臂的状态信息；所述智能柔顺装配平台根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，所述末端执行器夹取运动装配工件进行装配，装配至所述静止装配工件上。

具体的，在所述的机械臂运动控制***中，所述运动装配工件为轴，所述静止装配工件为孔。所述六自由度协作机械臂的每个关节均安装有力矩传感器；所述力矩传感器实时采集各个关节的状态信息，实现灵敏的拖动示教和碰撞检测；所述智能柔顺装配平台包括上位机与机械臂控制器，所述上位机采用实时通信接口与所述机械臂控制器进行数据交换，所述上位机通过实时通信接口接收所述力矩传感器采集的六自由度协作机械臂的状态信息，根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；所述上位机发送机械臂状态控制指令至所述机械臂控制器，以实现所述机械臂控制器输出搜索装配算法对所述六自由度协作机械臂进行控制；所述状态信息包括姿态状态信息、速度状态信息和转矩状态信息，所述机械臂状态控制指令包括位姿控制指令、速度控制指令和转矩控制指令；所述上位机将所述末端执行器的质量和惯性矩阵补偿给机械臂控制器，以实现力矩控制补偿。

进一步的，在所述的机械臂运动控制***中，所述上位机通过获取所述力矩传感器输出的力矩信息τ_输出1τ_输出2τ_输出3τ_输出4τ_输出5τ_输出6，采集六自由度协作机械臂的状态信息并将所述状态信息进行处理，生成机械臂本体的状态集：

/>

其中，如图3所示，F_x，F_y，F_z表示从六个关节的力矩传感器获得的平均力，M_x，M_y表示机械臂末端两个关节的力矩传感器检测的力矩；

和/>

表示机械臂末端两个关节在二维坐标系的位置误差，x，y，z分别表示空间坐标轴的三个方向坐标。在所述的机械臂运动控制***中，通过将正向运动学应用于机械臂编码器测量的关节角度计算机械臂末端两个关节在二维坐标系的位置误差；计算/>

和/>

的取整值，当/>

和/>

的取整值为(–c，c)时，作为位置数据P_x和P_y代替原点(0，0)，静止装配工件的中心范围为-c<x<c，-c<y<c，其中c是位置误差的余量；当/>

和/>

的取整值是(c，2c)时，/>

和/>

将被舍入为c，依此类推。

如图2所示，钉位置P通过将正向运动学应用于机器人编码器测量的关节角度来计算。在后面学习过程中，我们假设孔未设置到精确位置，并且存在位置误差，增加对推断期间可能发生的位置误差的鲁棒性。为了满足此假设，计算了取整值

和/>

作为位置数据P_x和P_y通过使用图二所示的网格。代替原点(0，0)，孔的中心可以位于-c<x<c，-c<y<c，其中c是位置误差的余量。因此，当值是(–c，c)时，它将被舍入为0。类似地，当值是(c，2c)时，它将被舍入为c，依此类推。这为网络提供了辅助信息，以加速学习收敛。

如图3所示，在所述的机械臂运动控制***中，根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测包括：将所述六自由度协作机械臂至于初始位姿，采用神经网络对所述六自由度协作机械臂进行控制，所述机械臂控制器设置的控制集为

其中，F_x ^d，F_y ^d，F_z ^d表示六个关节施加的平均力，R_x ^d，R_y ^d表示机械臂末端两个关节的位姿；根据所述控制集通过控制策略网络产生各个关节的转矩控制指令u(t)，计算每个关节运行的优势函数估计值；根据产生的训练数据，通过随机策略梯度按照多个步骤建立优化函数，并更新策略网络权重。

如图1所示，通过控制策略网络产生控制变量u(t)，即各个关节的力矩控制指令，同时计算每一步的优势函数估计值：

其中：δ_t＝r_t+γV(x(t+1))-V(x(t))，

根据产生的训练数据：

通过随机策略梯度按照k个步骤建立优化函数R_k，并更新策略网络权重：

R_k＝r_k+γr_k+1+γ²r_k+2+…+γ^n-kr_n＝r_k+γR_k+1。

进一步的，在所述的机械臂运动控制***中，所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，夹取待装配的工件进行装配包括：接近阶段，所述六自由度协作机械臂夹持所述运动装配工件到达待装配的静止装配工件上方的同轴心位置；搜索阶段，所述上位机将位姿控制指令和速度控制指令发送至所述机械臂控制器，所述六自由度协作机械臂采用轴空间运动使所述运动装配工件向静止装配工件移动，并使两者处于接触状态与未接触状态的临界点；***阶段，将运动装配工件的轴和静止装配工件的孔对齐后，采用Z方向的力控算法，将运动装配工件的轴向下***静止装配工件的孔中；***完成阶段，通过检测Z方向的位置判断是否装配完成，如果装配成功则所述六自由度协作机械臂松开所述运动装配工件后退出，如果装配超时则判断本次装配失败。

具体的，在所述的机械臂运动控制***中，以轴孔装配固有误差60μm为例，使用LSTM(也可使用其他类似算法)分阶段进行学习，根据六轴力矩传感器反馈的数据，使用以下公式对四个搜索动作进行定义，所述搜索阶段包括四次搜索步骤，每个步骤的控制集分别为：

1)

2)

3)

4)

其中，

F_z ^d＝20N；使得轴孔保持恒力与孔板接触，保证搜索阶段的连续运行。

所述***阶段包括：采集六自由度协作机械臂的状态信息并将所述状态信息进行处理，当生成机械臂本体的状态集为下式时，***成功：

s＝[0,0,F_z,M_x,M_y,0,0]，

通过M_X和M_y判断运动装配工件的运动方向，通过F_z判断所述运动装配工件是否卡住，***动作的控制集为：

1)

2)

3)

4)

5)

可选的，在所述的机械臂运动控制***中，检测Z方向的位置判断是否装配完成包括，计算惩罚参数：

其中，d为运动装配工件与静止装配工件位置之间的实时距离，D为运动装配工件与静止装配工件位置之间的目标距离，d₀为静止装配工件的初始位置误差，根据惩罚参数计算是从静止装配工件的初始位置沿垂直方向向下的位移，

其中Z是***目标深度，z是从静止装配工件的初始位置沿垂直方向向下的位移；当-1≤r<1时，装配成功。奖励旨在保持在-1≤r<1。最高奖励少于1，如果在搜索阶段钉子位置和目标位置的距离大于D，则训练中断。在***阶段，当销钉卡在孔的入口点时，r变为最小值-1。

在所述装配阶段，根据深度强化学习算法建立装配策略π；

π(s)＝argmax_aQ(s,a)

建立Q函数的实现表格，状态s为行，动作a为列,使用Bellman方程进行更新；

Q(s,a)←Q(s,a)+α(r+γmax_a′Q(s′,a′)-Q(s,a))，

通过深度递归神经网络进行参数θ的更新。α为学习率，

表梯度

建立损失函数如下

参数更新方程写为

输出装配动作a^t经过多次重复后，装配深度达到目标值Z后，再经历多次训练过程优化网络参数，将得到的深度强化学习网络用于实际装配过程，将产生的装配动作生成用于控制机器人的控制质量完成多轴孔装配任务。

进一步的，在所述的机械臂运动控制***中，将运动装配工件固定在六自由度协作机械臂的末端执行器上后，通过末端执行器的CAD三维模型，计算出重力矩阵和惯性矩阵；将末端执行器的质量、质心位置、重力矩阵和惯性矩阵补偿给机械臂控制器。

在本发明提供的机械臂运动控制***中，通过智能柔顺装配平台生成六自由度协作机械臂的状态信息，建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法，六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，末端执行器夹取运动装配工件进行装配，装配至静止装配工件上，解决了由于工件精度和一致性差造成装配失败率高的问题，通过力控算法及搜索装配算法，自动找到工件之间的正确的装配位置，代替人工完成装配，两种控制回路最后都叠加到关节空间输出关节力矩，相对于在机械臂末端添加传感器的方案，提高了动态特性，实现了主动柔顺控制，在装配过程体现柔性，不仅提高了装配成功率，同时也不会损坏机械臂或者工具工件。

根据本发明实施的结合力控算法的搜索装配算法，将装配方法划分为四个阶段，对每个阶段的进出条件进行约束，使得装配过程稳定可靠。控制方法充分发挥了六自由度协作机械臂关节内部集成力矩传感器的优点，实现了力控制与位置控制的解耦，两种控制回路最后都叠加到关节空间输出关节力矩，同时提高了***的动态响应特性。

以轴孔装配结合力矩控制的深度强化学***台。协作机器人本体，协作机器人控制器，上位机，末端执行器，装配工件轴，和装配工件孔。其中，由协作机器人本体的关节内部的力矩传感器采集关节力矩信息，可以实时采集关节力矩信息，实现灵敏的拖动示教和碰撞检测等。上位机与协作机器人控制器连接，采用实时通信接口进行数据交换，采集协作机器人的状态信息，并发送机器人状态控制指令至机器人控制器，以由机器人控制器对协作机器人进行控制，如图：上位机通过接口可以采集机器人姿态、速度、转矩等状态信息，同样可以发送位姿、速度和转矩给机器人控制器，由此可以设计搜索装配算法对机器人进行控制。将夹持工件的末端执行器的质量和惯性矩阵补偿给机器人控制器，以便实现更加精确的力控制。

具体的，将第一工件固定在协作机器人的末端执行器上，通过末端执行器的CAD三维模型，计算出重力和惯性矩阵。末端执行器本身的质量会影响计算结果，所以需要末端执行器的质量G和质心位置P惯性矩阵I补偿给机器人控制器，以便求得更加精确的结果，也是为了实现更加精确的力控制。若补偿结果不准确，会造成重力矩补偿不准确，拖动示教有偏差，运动轨迹精度下降。

综上，上述实施例对机械臂运动控制***的不同构型进行了详细说明，当然，本发明包括但不局限于上述实施中所列举的构型，任何在上述实施例提供的构型基础上进行变换的内容，均属于本发明所保护的范围。本领域技术人员可以根据上述实施例的内容举一反三。

上述描述仅是对本发明较佳实施例的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于权利要求书的保护范围。

Claims (7)

1.一种机械臂运动控制***，其特征在于，所述机械臂运动控制***包括智能柔顺装配平台、运动装配工件和静止装配工件，其中：

所述智能柔顺装配平台控制六自由度协作机械臂，所述六自由度协作机械臂包括末端执行器，所述智能柔顺装配平台生成所述六自由度协作机械臂的状态信息；

所述智能柔顺装配平台根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法，所述智能柔顺装配平台包括上位机与机械臂控制器；

所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，所述末端执行器夹取运动装配工件进行装配，装配至所述静止装配工件上；

所述六自由度协作机械臂执行力控算法和搜索装配算法到达指定工位，夹取待装配的工件进行装配包括：

接近阶段，所述六自由度协作机械臂夹持所述运动装配工件到达待装配的静止装配工件上方的同轴心位置；

搜索阶段，所述上位机将位姿控制指令和速度控制指令发送至所述机械臂控制器，所述六自由度协作机械臂采用轴空间运动使所述运动装配工件向静止装配工件移动，并使两者处于接触状态与未接触状态的临界点；

***阶段，将运动装配工件的轴和静止装配工件的孔对齐后，采用Z方向的力控算法，将运动装配工件的轴向下***静止装配工件的孔中；

***完成阶段，通过检测Z方向的位置判断是否装配完成，如果装配成功则所述六自由度协作机械臂松开所述运动装配工件后退出，如果装配超时则判断本次装配失败；

所述搜索阶段包括四次搜索步骤，每个步骤的控制集分别为：

1)

2)

3)

4)

其中，

所述***阶段包括：采集六自由度协作机械臂的状态信息并将所述状态信息进行处理，当生成机械臂本体的状态集为下式时，***成功：

s＝[0,0,F_z,M_x,M_y,0,0]，

通过M_X和M_y判断运动装配工件的运动方向，通过F_z判断所述运动装配工件是否卡住，***动作的控制集为：

1)

2)

3)

/>

4)

5)

其中，F_x，F_y，F_z表示从六个关节的力矩传感器获得的平均力，M_x，M_y表示机械臂末端两个关节的力矩传感器检测的力矩；F_x ^d，F_y ^d，F_z ^d表示六个关节施加的平均力，R _x ^d，R_y ^d表示机械臂末端两个关节的位姿。

2.如权利要求1所述的机械臂运动控制***，其特征在于，所述六自由度协作机械臂的每个关节均安装有力矩传感器；所述力矩传感器实时采集各个关节的状态信息，实现灵敏的拖动示教和碰撞检测；

所述上位机采用实时通信接口与所述机械臂控制器进行数据交换，所述上位机通过实时通信接口接收所述力矩传感器采集的六自由度协作机械臂的状态信息，根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测，获取力控算法和搜索装配算法；

所述上位机发送机械臂状态控制指令至所述机械臂控制器，以实现所述机械臂控制器输出搜索装配算法对所述六自由度协作机械臂进行控制；

所述状态信息包括姿态状态信息、速度状态信息和转矩状态信息，所述机械臂状态控制指令包括位姿控制指令、速度控制指令和转矩控制指令；

所述上位机将所述末端执行器的质量和惯性矩阵补偿给机械臂控制器，以实现力矩控制补偿。

3.如权利要求2所述的机械臂运动控制***，其特征在于，所述上位机通过获取所述力矩传感器输出的力矩信息τ_输出1τ_输出2τ_输出3τ_输出4τ_输出5τ_输出6，采集六自由度协作机械臂的状态信息并将所述状态信息进行处理，生成机械臂本体的状态集：

其中，F_x，F_y，F_z表示从六个关节的力矩传感器获得的平均力，M_x，M_y表示机械臂末端两个关节的力矩传感器检测的力矩；

和/>

表示机械臂末端两个关节在二维坐标系的位置误差，x，y，z分别表示空间坐标轴的三个方向坐标。

4.如权利要求3所述的机械臂运动控制***，其特征在于，通过将正向运动学应用于机械臂编码器测量的关节角度计算机械臂末端两个关节在二维坐标系的位置误差；

计算

和/>

的取整值，当/>

和/>

的取整值为(–c，c)时，作为位置数据P_x和P_y代替原点(0，0)，静止装配工件的中心范围为-c<x<c，-c<y<c，其中c是位置误差的余量；

当

和/>

的取整值是(c，2c)时，/>

和/>

将被舍入为c，依此类推。

5.如权利要求4所述的机械臂运动控制***，其特征在于，根据所述六自由度协作机械臂的状态信息建立训练模型，实现拖动示教和碰撞检测包括：将所述六自由度协作机械臂置于初始位姿，采用神经网络对所述六自由度协作机械臂进行控制，所述机械臂控制器设置的控制集为：

/>

其中，F_x ^d，F_y ^d，F_z ^d表示六个关节施加的平均力，R _x ^d，R_y ^d表示机械臂末端两个关节的位姿；

根据所述控制集通过控制策略网络产生各个关节的转矩控制指令u(t)，计算每个关节运行的优势函数估计值；

根据产生的训练数据，通过随机策略梯度按照多个步骤建立优化函数，并更新策略网络权重。

6.如权利要求5所述的机械臂运动控制***，其特征在于，检测Z方向的位置判断是否装配完成包括，计算惩罚参数：

其中，d为运动装配工件与静止装配工件位置之间的实时距离，D为运动装配工件与静止装配工件位置之间的目标距离，d₀为静止装配工件的初始位置误差，根据惩罚参数计算是从静止装配工件的初始位置沿垂直方向向下的位移，

其中Z是***目标深度，z是从静止装配工件的初始位置沿垂直方向向下的位移；

当-1≤r<1时，装配成功。

7.如权利要求6所述的机械臂运动控制***，其特征在于，将运动装配工件固定在六自由度协作机械臂的末端执行器上后，通过末端执行器的CAD三维模型，计算出重力矩阵和惯性矩阵；将末端执行器的质量、质心位置、重力矩阵和惯性矩阵补偿给机械臂控制器。

Images