CN110449882B - 结合力控的搜索装配方法 - Google Patents

Abstract

本发明提出了一种结合力控的搜索装配方法，包括如下步骤：搭建柔性装配平台；将夹持工件的末端执行器的质量和惯性矩阵补偿给机器人控制器；协作机器人开始执行力控搜索装配，包括以下阶段：由协作机器人夹持第一工件到达待装配的第二工件上方的同轴心位置；将控制方式调整为力位混合控制，自动查找出第一工件的轴和第二工件的孔对齐的位置；将第一工件的轴和第二工件的孔对齐后，采用Z方向力控模式，将第一工件的轴向下插入第二工件的孔中；通过检测Z方向的位置判断是否装配完成，如果装配成功则协作机器人松开第一工件后退出。本发明在装配过程中体现柔性，不仅提高了装配成功率，同时也不会损坏机器人或者工具工件。

Description

结合力控的搜索装配方法

技术领域

本发明涉及工业机器人技术领域，特别涉及一种结合力控的搜索装配方法。

背景技术

在汽车制造和装配领域，大量零部件加工完成后需要进行装配。为满足自动化需求，工业机器人已经广泛应用到汽车装配现场，与人工装配相比较，使用传统工业机器人进行装配具有重复定位精度高，效率高，运动范围大，负载高的特点。

由于装配现场存在大量不确定性，传统机器人无法满足装配要求，例如冲压工件会出现加工精度差，一致性差等问题，那么在需要装配的两个工件之间会存在位置误差大于配合间隙的情况，由于传统工业机器人关节刚度较高，每次装配的运动轨迹都是不变的，很有可能出现装配失败等问题，导致工件损坏等后果。

现在一种解决方法是利用视觉定位技术，由视觉来引导机器人进行装配，这种方法在一定程度上可以减小工件之间的位置误差，但是在装配精度要求较高的场合，仍然有较大概率装配失败，影响生产过程。现在仍然有很多场合使用人工来进行装配，工人通过感知装配时的工件受力，时刻调整装配位置从而达到装配目的，但在应付质量较大的工件装配时效率较低，也不满足自动化的要求。

装配任务最大的特点就是需要机器人与环境相接触，这就需要机器人关节具有一定柔性，像人手一样具有力的感知，通过感知外部环境对工件的作用力，实时地搜索并调整工件位置从而完成装配任务。如何提供一种可以结合力控实现装配的方法，是当前需要解决的技术问题。

发明内容

本发明的目的旨在至少解决所述技术缺陷之一。

为此，本发明的目的在于提出一种结合力控的搜索装配方法。

为了实现上述目的，本发明的实施例提供一种结合力控的搜索装配方法，包括如下步骤：

步骤S1，搭建柔性装配平台，所述柔性装配平台包括：协作机器人本体、协作机器人控制器、上位机、末端执行器，其中，由协作机器人本体的关节内部的力矩传感器采集关节力矩信息；所述上位机与所述协作机器人控制器连接，以采集协作机器人的状态信息，并发送机器人状态控制指令至所述机器人控制器，以由所述机器人控制器对所述协作机器人进行控制；

步骤S2，将夹持工件的末端执行器的质量和惯性矩阵补偿给机器人控制器；

步骤S3，所述协作机器人开始执行力控搜索装配，到达指定工位夹取待装配的第一工件，然后进行装配，包括以下阶段：

接近阶段：由协作机器人夹持第一工件到达待装配的第二工件上方的同轴心位置；

搜索阶段：将控制方式调整为力位混合控制，自动查找出第一工件的轴和第二工件的孔对齐的位置；

插入阶段：将第一工件的轴和第二工件的孔对齐后，采用Z方向力控模式，将第一工件的轴向下插入第二工件的孔中；

插入完成阶段：通过检测Z方向的位置判断是否装配完成，如果装配成功则所述协作机器人松开所述第一工件后退出，如果装配超时则判断本次装配失败。

进一步，在所述步骤S3中，所述第一工件上有均匀分布的多个轴，固定在机器人的末端执行器上；所述第二工件上有均匀分布的多个孔，固定在工作台上面；工作台水平放置，轴的装配方向是竖直向下，即Z方向，当第一工件上的轴与第二工件上的孔全部对齐时才能进行装配。

进一步，在所述接近阶段中，所述协作机器人将第一工件移动到第二工件上方，将目标位姿态和速度发送至所述机器人控制器，采用轴空间运动来实现上述上述过程，所述协作机器人将第一工件到达待装配的第二工件上方的同轴心位置，并使两者处于刚好未接触的状态，记录下此刻的位姿即为目标位姿。

进一步，在所述搜索阶段，将控制方式调整为笛卡尔空间的力位混合阻抗控制方式，分别计算位置控制回路和力控制回路，位置控制采用逆运动学计算力矩控制方式，得到的力控回路的控制力矩τ_输出1与位置环控制输出力矩τ_输出2叠加输出，输出总力矩为：τ_输出＝τ_输出1+τ_输出2。

进一步，计算力控回路的控制力矩τ_输出1，包括：首先标定关节传感器偏差：

τ_零偏＝τ_初测-τ_初重，

其中，τ_初测为进入搜索阶段时测得的关节力矩，τ_初重为进入搜索阶段时根据关节角度计算出的重力矩；

然后通过实时通信接口实时采集各关节力矩传感器的值，进而实时计算出力矩差:

τ_偏差＝τ_测-τ_重-τ零_偏；

设定笛卡尔空间的期望力矩为F_d＝[0,0,F_z,0,0,0],即设定Z方向的力恒定保持F_z，根据期望力矩和雅克比矩阵，计算出期望关节力矩:

τ_d＝J^T·F_d

计算出的期望关节力矩再通过PI调节，最后输出一个关节力矩至所述机器人控制器，PI调节方程为：

k_p为比例调节系数,k_i为积分系数，通过PI调节，使得最后Z方向的力处于一个恒定状态。

进一步，所述计算环控制输出力矩τ_输出2，包括：

在XY平面上做圆弧搜索运动，给定初始最大运动半径为r_max,最大线速度为V_max,加减速时间为t_a,总的搜索时间即每次最大装配时间为t_max,这里的加减速采用梯形加减速的方法，装配过程中机器人运动速度为：

经过时间t，机器人走过的圆弧角度是：

相对于搜索开始时机器人的初始位置，经过时间t，X方向的位移量和Y方向的位移量分别为：

Δx＝r_max(1-cos(θ)),Δy＝r_max sin(θ)；

除了在XY方向上有一个偏移量，还有一个绕Z轴旋转的运动，设搜索开始时的机器人姿态矩阵是：

经过时间t,机器人绕Z轴旋转的角度θ_zt＝kθ_t,系数根据实际情况调节，一般取0.01-0.05，旋转后的姿态矩阵为：

经过时间t，机器人的位姿矩阵为:

其中，p_z的大小随着实际情况变化，第一工件和第二工件在Z方向上的压力保持F_z，

通过实时通信接口可以实时采集当前关节角度q、当前关节角速度d_q和当前末端位姿矩阵T_当前,根据目标位姿T₂反解出目标关节角度q_d,其中末端位姿用一个六维向量Q[x,y,z,α,β,γ]来表示，将目标末端位姿矩阵T₂表示成Q_目标，将当前位姿矩阵T_当前表示为Q_当前，据此位姿差Q_差＝Q_目标-Q_当前；

设置适当的刚度矩阵和阻尼矩阵，可以计算出此刻的期望关节力矩：

τ_输出2＝J^T(-B_d·Q_差-D_d·J·d_q)+τ_coriolis，

其中，τ_coriolis为科氏力，

B_d为刚度矩阵，将B_d设置为：diag(3000,3000,0,200,200,200)∈R^n×n，D_d为阻尼矩阵，将其设置为：diag(100,100,0,20,20,20)∈R^n×n，

当B_d＝diag(3000,3000,3000,200,200,200)时，认为此时的机器人是刚性的。

进一步，在所述插入阶段中，Z方向力控制器的作用下，机器人向下运动，同时停止XY平面内的位置控制，完成第一工件的轴插入到第二工件的孔的过程。

进一步，在所述插入完成阶段中，设定Z方向的阈值P_阈，在插入过程中实时计算第一工件在基坐标系下的Z方向上的位置P_z,若P_z＞P_阈，则判断插入阶段还未完成，继续插入；若P_z＜P_阈，则判断插入完成。

根据本发明实施例的结合力控的搜索装配方法，将装配方法划分为四个阶段，对每个阶段的进出条件进行约束，使得装配过程稳定可靠。控制方法充分发挥了协作机器人关节内部集成力矩传感器的优点，实现了力控制与位置控制的解耦，两种控制回路最后都叠加到关节空间输出关节力矩，同时提高了系统的动态响应特性。本发明解决由于工件精度和一致性差造成装配失败率高的问题，通过力控搜索的方法，自动找到工件之间的正确的装配位置，代替人工完成装配，充分发挥了协作机器人关节内部集成力矩传感器的优点，实现了力控制与位置控制的解耦，两种控制回路最后都叠加到关节空间输出关节力矩，相对于在机器人末端添加传感器的方案，提高了动态特性，实现了主动柔顺控制，在装配过程中体现柔性，不仅提高了装配成功率，同时也不会损坏机器人或者工具工件。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明实施例的结合力控的搜索装配方法的流程图；

图2为根据本发明实施例的结合力控的柔性装配平台示意图；

图3为根据本发明实施例的协作机器人实时通讯接口功能示意图；

图4为根据本发明实施例的力控混合控制框架示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

如图1所示，本发明实施例的结合力控的搜索装配方法，包括如下步骤：

步骤S1，基于协作机器人，结合力控和搜索算法等搭建柔性装配平台。如图2所示，柔性装配平台包括：协作机器人本体1，协作机器人控制器5，上位机6，末端执行器2、装配工件轴3和装配工件孔4等。其中，由协作机器人本体的关节内部的力矩传感器采集关节力矩信息，可以实时采集关节力矩信息，实现灵敏的拖动示教和碰撞检测等。上位机与协作机器人控制器连接，采用实时通信接口进行数据交换，采集协作机器人的状态信息，并发送机器人状态控制指令至机器人控制器，以由机器人控制器对协作机器人进行控制。如图3所示，上位机通过接口可以采集机器人姿态、速度、转矩等状态信息，同样可以发送位姿、速度和转矩给机器人控制器。由此可以设计搜索装配算法对机器人进行控制。

步骤S2，将夹持工件的末端执行器的质量和惯性矩阵补偿给机器人控制器，以便实现更加精确的力控制。

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

步骤S3，协作机器人开始执行力控搜索装配，到达指定工位夹取待装配的第一工件，然后进行装配。协作机器人的力控搜索装配策略：将整个装配过程分为：接近阶段、搜索阶段、插入阶段和插入完成四个阶段。整个装配过程结合位置控制和力控制，对每个阶段的进入和走出条件进行约束设计，从而有序稳定地完成装配过程。

(1)接近阶段：由协作机器人夹持第一工件到达待装配的第二工件上方的同轴心位置。

在接近阶段中，协作机器人将第一工件移动到第二工件上方，将目标位姿态和速度发送至机器人控制器，采用轴空间运动来实现上述上述过程。因为第二工件的位置相对于机器人基坐标系是不确定的，所以通过拖动示教的方式，协作机器人将第一工件到达待装配的第二工件上方的同轴心位置，并使两者处于刚好未接触的状态，记录下此刻的位姿即为目标位姿。

(2)搜索阶段：将控制方式调整为力位混合控制，自动查找出第一工件的轴和第二工件的孔对齐的位置。

具体的，将装配场景描述成轴和孔的装配，第一工件上有均匀分布的多个轴，固定在机器人的末端执行器上；第二工件上有均匀分布的多个孔，固定在工作台上面；工作台水平放置，轴的装配方向是竖直向下，即Z方向，只有当第一工件上的轴与第二工件上的孔全部对齐时才能进行装配。

在本阶段中，参考图4，协作机器人的控制模式由轴空间位置控制转换为笛卡尔空间的力位混合阻抗控制。分别计算位置控制回路和力控制回路，位置控制采用逆运动学计算力矩控制方式，得到的力控回路的控制力矩τ_输出1与位置环控制输出力矩τ_输出2叠加输出，输出总力矩为：

τ_输出＝τ_输出1+τ_输出2。

此控制方式的设计初衷是结合力控任务在接触坐标系下力控子控件和位置控制子控件的互补分解，得到独立的位置控制自由度和力控自由度，实现力控和位置控制的最大解耦。

(3.1)力控回路的控制力矩

协作机器人夹持第一工件在Z方向上做恒力控制，即保持第一工件与第二工件在Z方向上的接触力是恒定的；进入搜索阶段，先标定关节传感器偏差即：

τ_零偏＝τ_初测-τ_初重。

其中，τ_初测为进入搜索阶段时测得的关节力矩，τ_初重为进入搜索阶段时根据关节角度计算出的重力矩。通过实时通信接口可以实时采集各关节力矩传感器的值，进而可以实时计算出力矩差:

τ_偏差＝τ_测-τ_重-τ_零偏。

设定笛卡尔空间的期望力矩为F_d＝[0,0,F_z,0,0,0],即设定Z方向的力恒定保持F_z，根据期望力矩和雅克比矩阵，可以计算出期望关节力矩:

τ_d＝J^T·F_d

计算出的期望关节力矩再通过PI调节，最后输出一个关节力矩给机器人控制器，PI调节方程为：

其中，k_p为比例调节系数,k_i为积分系数。通过PI调节，使得最后Z方向的力处于一个恒定状态。最后对期望力进行滤波，减小误差。

进入搜索搜索阶段后，第一工件和第二工件处于刚好未接触的状态，为保持接触力为F_z，第一工件会竖直向下(Z方向)运动，直到两者接触。

(3.2)位置环控制输出力矩

在XY平面上做圆弧搜索运动，给定初始最大运动半径为r_max,最大线速度为V_max,加减速时间为t_a,总的搜索时间即每次最大装配时间为t_max,这里的加减速采用梯形加减速的方法，装配过程中机器人运动速度为：

经过时间t，机器人走过的圆弧角度是：

相对于搜索开始时机器人的初始位置，经过时间t，X方向的位移量和Y方向的位移量分别为：

Δx＝r_max(1-cos(θ)),Δy＝r_maxsin(θ)；。

除了在XY方向上有一个偏移量，还有一个绕Z轴旋转的运动，设搜索开始时的机器人姿态矩阵是：

经过时间t,机器人绕Z轴旋转的角度θ_zt＝kθ_t,系数根据实际情况调节，一般取0.01-0.05，旋转后的姿态矩阵为：

经过时间t，机器人的位姿矩阵为:

其中p_z的大小随着实际情况变化，第一工件和第二工件在Z方向上的压力保持F_z。

协作机器人在XY平面做笛卡尔空间的位置控制，实际上位置控制和力控制都可以看做是由一套控制框架构成，位置控制可以理解成阻抗控制，通过实时通信接口可以实时采集当前关节角度q、当前关节角速度d_q和当前末端位姿矩阵T_当前,根据目标位姿T₂反解出目标关节角度q_d,其中末端位姿可以用一个六维向量Q[x,y,z,α,β,γ]来表示，将目标末端位姿矩阵T₂表示成Q_目标，将当前位姿矩阵T_当前表示为Q_当前，据此位姿差Q_差＝Q_目标-Q_当前。

设置适当的刚度矩阵和阻尼矩阵，可以计算出此刻的期望关节力矩：

τ_输出2＝J^T(-B_d·Q_差-D_d·J·d_q)+τ_coriolis

其中，τ_coriolis为科氏力，B_d为刚度矩阵，这里应该将XY方向上的刚度设置较高，Z方向上的刚度设置较低，可以是0，因为Z方向是采用力控制，力控制与位置控制解耦，数值越小意味着Z方向越容易受到外力而改变其位置。将B_d设置为：diag(3000,3000,0,200,200,200)∈R^n×n。D_d为阻尼矩阵，将其设置为：diag(100,100,0,20,20,20)∈R^n×n。当B_d＝diag(3000,3000,3000,200,200,200)时，可以认为此时的机器人是刚性的。

(3)插入阶段：将第一工件的轴和第二工件的孔对齐后，采用Z方向力控模式，将第一工件的轴向下插入第二工件的孔中。

具体的，经过搜索阶段实现第一工件上的轴全部与第二工件的孔全部对齐。对齐之后，第一工件和第二工件之间接触力会突然减小，由于Z方向力控制器的作用，机器人会向下运动，同时停止XY平面内的位置控制，完成插入过程，插入过程速度与设置的期望力大小有关，此方法适合装配行程较短的场合。

(4)插入完成阶段：通过检测Z方向的位置判断是否装配完成，如果装配成功则协作机器人松开第一工件后退出，如果装配超时则判断本次装配失败。

具体的，经过插入阶段，设定Z方向的阈值P_阈，在插入过程中实时计算工件在基坐标系下的Z方向上的位置P_z,若P_z＞P_阈，则判断插入阶段还未完成，继续插入；若P_z＜P_阈，则判断插入完成。控制器转变成纯位置控制，机器人末端执行器松开第一工件，装配完成，机器人退出，继续下一个工件的装配。

根据本发明实施例的结合力控的搜索装配方法，将装配方法划分为四个阶段，对每个阶段的进出条件进行约束，使得装配过程稳定可靠。控制方法充分发挥了协作机器人关节内部集成力矩传感器的优点，实现了力控制与位置控制的解耦，两种控制回路最后都叠加到关节空间输出关节力矩，同时提高了系统的动态响应特性。本发明解决由于工件精度和一致性差造成装配失败率高的问题，通过力控搜索的方法，自动找到工件之间的正确的装配位置，代替人工完成装配，充分发挥了协作机器人关节内部集成力矩传感器的优点，实现了力控制与位置控制的解耦，两种控制回路最后都叠加到关节空间输出关节力矩，相对于在机器人末端添加传感器的方案，提高了动态特性，实现了主动柔顺控制，在装配过程中体现柔性，不仅提高了装配成功率，同时也不会损坏机器人或者工具工件。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求及其等同限定。

Claims (5)

1.一种结合力控的搜索装配方法，其特征在于，包括如下步骤：

步骤S1，搭建柔性装配平台，所述柔性装配平台包括：协作机器人本体、协作机器人控制器、上位机、末端执行器、第一工件、第二工件，其中，由协作机器人本体的关节内部的力矩传感器采集关节力矩信息；所述上位机与所述协作机器人控制器连接，以采集协作机器人的状态信息，并发送协作机器人状态控制指令至所述协作机器人控制器，以由所述协作机器人控制器对所述协作机器人本体进行控制；

步骤S2，将夹持工件的末端执行器的质量和惯性矩阵补偿给协作机器人控制器；

步骤S3，所述协作机器人开始执行力控搜索装配，到达指定工位夹取待装配的第一工件，然后进行装配，包括以下阶段：

接近阶段：由所述协作机器人夹持第一工件到达待装配的第二工件上方的同轴心位置；

搜索阶段：将控制方式调整为力位混合控制，自动查找出第一工件的轴和第二工件的孔对齐的位置；

在所述搜索阶段，将控制方式调整为笛卡尔空间的力位混合阻抗控制方式，分别计算位置控制回路和力控制回路，位置控制采用逆运动学计算力矩控制方式，得到的力控回路的控制力矩τ_输出1与位置环控制输出力矩τ_输出2叠加输出，输出总力矩为：τ_输出＝τ_输出1+τ_输出2；

计算力控回路的控制力矩τ_输出1，包括：首先标定搜索运动过程中力矩传感器的零偏：

τ_零偏＝τ_初测-τ_初重，

其中，τ_初测为进入搜索阶段时测得的关节力矩，τ_初重为进入搜索阶段时根据关节角度计算出的重力矩；

然后通过实时通信接口实时采集各关节力矩传感器的值，进而实时计算出力矩差:

τ_偏差＝τ_测-τ_重-τ_零偏；

其中，τ_测搜索运动过程中力矩传感器的测量力矩值；τ_重搜索运动过程中机器人重力矩；τ_零偏搜索运动过程中力矩传感器的零偏；

设定笛卡尔空间的期望力矩为F_d＝[0,0,F_z,0,0,0],即设定Z方向的力恒定保持F_z，根据期望力矩和雅克比矩阵J，计算出期望关节力矩:

τ_d＝J^TF_d

计算出的期望关节力矩再通过PI调节，最后输出一个关节力矩至所述协作机器人控制器，PI调节方程为：

k_p为比例调节系数,k_i为积分系数，通过PI调节，使得最后Z方向的力处于一个恒定状态；

计算环控制输出力矩τ_输出2，包括：

在XY平面上做圆弧搜索运动，给定初始最大运动半径为r_max,最大线速度为V_max,加减速时间为t_a,总的搜索时间即每次最大装配时间为t_max,这里的加减速采用梯形加减速的方法，装配过程中协作机器人运动速度为：

经过时间t，协作机器人走过的圆弧角度是：

其中，V是时间t时的线速度；

相对于搜索开始时协作机器人的初始位置，经过时间t，X方向的位移量和Y方向的位移量分别为：

Δx＝r_max(1-cos(θ)),Δy＝r_maxsin(θ)；

其中，θ是时间t时，圆弧运动相对于初始位置的角度；

除了在XY方向上有一个偏移量，还有一个绕Z轴旋转的运动，设搜索开始时的协作机器人姿态矩阵是：

经过时间t,协作机器人绕Z轴旋转的角度θ_zt＝kθ_t,系数根据实际情况调节，一般取0.01-0.05，旋转后的姿态矩阵为：

其中，姿态矩阵是3×3矩阵，其中n o a分别是姿态矩阵中的3列向量；

θ_zt＝kθ_t中θ_t是时间t时绕Z方向相对于初始位置的旋转角度；

经过时间t，协作机器人的位姿矩阵为:

其中，p_z的大小随着实际情况变化，第一工件和第二工件在Z方向上的压力保持F_z，

通过实时通信接口可以实时采集当前关节角度q、当前关节角速度d_q和当前末端位姿矩阵T_当前,根据目标位姿反解出目标关节角度q_d,其中目标位姿用一个六维向量Q[x,y,z,α,β,γ]来表示，将目标末端位姿矩阵表示成Q_目标，将当前位姿矩阵T_当前表示为Q_当前，据此位姿差Q_差＝Q_目标-Q_当前；其中，p_x,p_y分别是搜索运动开始前机器人在X方向和Y方向上的位置，Δ_x，Δ_y分别是搜索运动过程中机器人目标位置在x方向和y方向上相对于初始位置的增量；J是机器人的在对应关节角度的雅可比矩阵；

设置适当的刚度矩阵和阻尼矩阵，可以计算出此刻的期望关节力矩：

τ_输出2＝J^T(-B_d·Q_差-D_d·J·d_q)+τ_coriolis，

其中，τ_coriolis为科氏力，

B_d为刚度矩阵，将B_d设置为：diag(3000,3000,0,200,200,200)∈R^n×n，D_d为阻尼矩阵，将其设置为：diag(100,100,0,20,20,20)∈R^n×n，

当B_d＝diag(3000,3000,3000,200,200,200)时，认为此时的协作机器人是刚性的；

插入阶段：将第一工件的轴和第二工件的孔对齐后，采用Z方向力控模式，将第一工件的轴向下插入第二工件的孔中；

插入完成阶段：通过检测Z方向的位置判断是否装配完成，如果装配成功则所述协作机器人松开所述第一工件后退出，如果装配超时则判断本次装配失败。

2.如权利要求1所述的结合力控的搜索装配方法，其特征在于，在所述步骤S3中，所述第一工件上有均匀分布的多个轴，固定在协作机器人的末端执行器上；所述第二工件上有均匀分布的多个孔，固定在工作台上面；工作台水平放置，轴的装配方向是竖直向下，即Z方向，当第一工件上的轴与第二工件上的孔全部对齐时才能进行装配。

3.如权利要求1所述的结合力控的搜索装配方法，其特征在于，在所述接近阶段中，所述协作机器人本体将第一工件移动到第二工件上方，将目标位姿态和速度发送至所述协作机器人控制器，采用轴空间运动来实现上述过程，所述协作机器人将第一工件到达待装配的第二工件上方的同轴心位置，并使两者处于刚好未接触的状态，记录下此刻的位姿即为目标位姿。

4.如权利要求1所述的结合力控的搜索装配方法，其特征在于，在所述插入阶段中，Z方向力控制器的作用下，协作机器人向下运动，同时停止XY平面内的位置控制，完成第一工件的轴插入到第二工件的孔的过程。

5.如权利要求1所述的结合力控的搜索装配方法，其特征在于，在所述插入完成阶段中，设定Z方向的阈值P_阈，在插入过程中实时计算第一工件在基坐标系下的Z方向上的位置P_z,若P_z＞P_阈，则判断插入阶段还未完成，继续插入；若P_z＜P_阈，则判断插入完成。

Images