CN110549338A - 一种圆-长方形复合孔类零件的机器人自动装配方法 - Google Patents

Abstract

一种圆‑长方形复合孔类零件的机器人自动装配方法，涉及机器人装配控制技术领域。本发明为了解决圆‑长方复合孔类零件类零件的自动装配面临接触状态繁多、装配策略难于确定的问题，本发明公开了一种根据力反馈信息进行接触状态判别，并针对不同接触状态进行相应调整的圆‑长方复合孔类零件自动装配方法。所述装配过程被分为接近阶段、搜孔阶段和***阶段，接近阶段使用5次样条轨迹规划的方法使装配件快速接近被装配件，搜孔阶段共有4种接触状态，***阶段的接触状态按接触点数量和相对位置共分为7大类41种，对每种接触状态均进行了受力分析并提出了相应的搜孔或***装配策略。进行了圆‑长方复合孔类零件的装配仿真，结果表明所提出的装配方法能在防止接触力过大的前提下完成装配。

Description

一种圆-长方形复合孔类零件的机器人自动装配方法

技术领域

本发明涉及一种不规则形状(圆-长方形复合孔类)零件的机器人自动装配策略与方法，涉及机器人装配控制技术领域。

背景技术

装配是产品制造流程中的重要环节，装配质量往往直接影响最终的产品质量。目前工业生产过程中仍有大量装配任务仍需要采用手工装配作业的方式来完成。但是，手工装配存在很多问题，例如，工作强度大、工作效率低、出错率高、费用高昂等。面对这些问题，制造业对于利用机器人代替人工完成装配作业有着越来越迫切的需求。机器人装配不仅能够提高生产效率，其应用的范围也更加广泛，如高温、辐射和真空等危险的环境中。

圆-长方形复合孔类零件的全称为圆柱形——长方形复合型轴孔类零件，它是由圆柱形的几何特征和长方形的几何特征组合而成的一类复合孔类的零件，这类零件的装配在制造业中非常常见，如平键的装配、花键的装配以及带键圆轴零件的装配等。

目前对于只有圆孔或只有方孔的零件的自动装配问题已公开发表了很多专利和学术论文，例如公开号为CN109382828A，专利号为ZL201811275792.8的发明专利提供了一种用于圆形孔和轴类零件装配的示教学习装配方法，机器人使用RCC柔性手腕补偿装配过程中误差，示教得到的调整运动用于减小轴件与孔件间位姿误差使其保持在RCC手腕可补偿的范围内；公开号为CN109531560A，专利号为ZL201910018179.6的发明专利提供了一种用于小间隙圆形轴孔配合的自动装配方法，通过微位移测量装置精确测定轴件相对于孔件的位姿误差，然后一次性动作完成装配。对于方孔零件的装配目前还没有专利公开，在Park等人于2012年发表的学术论文中将方形轴孔装配过程划分为六个阶段，并对每个阶段的接触情况进行了分析，最后在此基础上制定装配策略逐步实现装配操作；Kim等人在其2018年发表的学术论文中针对方形轴孔零件装配设计了一种带有角度测量***的夹具，多次实验表明夹具配合六维力传感器可以准确感知方柱和孔件之间的位姿误差。

不同于圆孔装配中接触点均在圆柱部分和方孔装配中接触点均在方柱部分的情况，在圆-长方复合孔类零件的装配中，孔类零件和与之相配合的轴类零件之间的接触点可能同时分布于圆柱和方柱以及二者的交线上，因此接触状态将比圆孔装配或方孔装配的情况更为复杂，与接触状态相对应的力学平衡方程也将截然不同。由此分析可知，若只简单套用上述专利和学术论文中关于圆孔装配和方孔装配的方法，将无法完成圆-长方复合孔类零件的装配作业。

综上所述，对于圆-长方形复合孔类零件的机器人自动装配问题，现有已公开的专利和学术研究中尚无完整的接触状态分析结果，也没有给出接触状态判别和与之对应的装配策略以及具体实施的控制方法。

发明内容

本发明所述装配方法面向解决几何元素中既有圆弧又有矩形的直边和直角的孔类零件的自动装配问题，避免装配过程中因轴孔间的位姿不当导致装配接触力过大而对装配件和机器人造成损坏。即本发明为了解决圆-长方复合孔类零件类零件的自动装配面临接触状态繁多、装配策略难于确定的问题，本发明将此种孔类零件其称之为圆-长方复合孔类零件，与之相配的轴类零件被称为圆-长方复合轴类零件，此类零件的几种常见形式包括：平键轮毂、花键轮毂、A型平键孔、C型平键孔、特型圆-长方孔等等。

本发明为解决上述技术问题采取的技术方案是：

一种圆-长方形复合孔类零件的机器人自动装配方法，所述装配方法的装配对象是几何要素中既有圆又有矩形的直边和直角的孔类零件和轴类零件，其中具有复合的圆孔和方孔的零件被称为圆-长方复合孔类零件，具有复合的圆柱和方柱的零件被称为圆-长方复合轴类零件(工程实际的常见零件中属于圆-长方复合孔/轴类零件的有：花键、A型平键、C型平键等等)，

应用所述装配方法的圆-长方复合孔类零件自动装配***由：上位机、基座、六自由度机器人、六维力/力矩传感器、装配件(与孔类零件适配的轴类零件)、夹持固定座、被装配件(圆-长方复合孔类零件)组成。其中六自由度机器人是指任意的能实现空间六自由度运动且位置控制精度满足装配作业的机器人，并不局限于关节型的串联六自由度操作臂；上位机与六自由度机器人的个伺服驱动/控制器以及六维力/力矩传感器通过总线通讯，使用的总线可以是以太网(Ethernet)、RS485网络、CAN总线等。

装配件被夹持在六维力/力矩传感器的工具侧接口上，六维力/力矩传感器的机器人侧接口与六自由度机器人的末端接口固连，带有圆-长方复合孔的被装配件固定于夹持固定座的接口内，夹持固定座和六自由度机器人均固定于基座上。坐标系ΣO-xyz为原点固定于基座的机器人接口中心的基坐标系，坐标系ΣO_P-xyz和ΣO_S-xyz的原点分别位于装配件的下表面圆心和六维力/力矩传感器的测力中心，这两个坐标系在装配作业中始终保持相对静止。在ΣO_P系中，z轴方向平行于装配件装入被装配件的***方向，x轴方向平行于装配件的几何轮廓对称面且与z轴垂直，y轴方向由z轴的单位矢量叉乘x轴的单位矢量所得到的矢量方向确定。

根据基座、夹持固定座、被装配件的设计图纸尺寸，或根据***组装好后的实际测量结果，可算出被装配件的圆-长方复合孔相对于六自由度机器人的相对位置，该相对位置被定义为被装配件的推定位置，受加工制造误差、测量误差等不确定因素的影响，此推定位置不等于被装配件的实际位置，因此在装配过程中若直接按着被装配件的推定位置控制装配件的运动，容易在被装配件和装配件间产生干涉，需要根据六维力/力矩传感器的反馈对装配件进行调整。

在所述圆-长方复合孔类零件的装配方法中，六自由度机器人需根据六维力/力矩传感器测得的力反馈信息，对装配件的位姿进行调整，使其最终装入被装配件的圆-长方复合孔中。上述装配过程被分为三个阶段：接近阶段、搜孔阶段、***阶段。

装配作业开始时首先进入接近阶段，装配件需要在六自由度机器人的控制下从初始位置快速接近被装配件，到达位于被装配件推定位置上方的装配准备位置，此准备位置可由推定位置加上增量矢量[0,0,Δz]^T算得。上述运动的轨迹规划应用次样条法，其中起点(装配件的初始位置)和终点(装配准备位置)处的速度和加速度均为0。

装配件4到达装配准备位置后进入搜孔阶段，此阶段开始后六自由度机器人2将使装配件4向被装配件6缓慢运动，进行试探，当z轴方向的接触力大于阈值F_max，装配作业的控制***将根据后若六维力/力矩传感器3的反馈判断装配件4与被装配件6之间的接触状态，然后将装配件4抬起一段距离并根据之前的接触状态调整其位姿，调整完成后将重复装配件4向被装配件6的试探过程，直到满足条件：

F_z＜F_max&h≥h_d (1)

其中F_z表示六维力/力矩传感器测得的z轴力分量(变换到ΣOP系内)，h表示装配件沿z 轴***被装配件的距离，h_d是搜孔完成的***深度判别阈值，F_max是接触力的阈值。

当式(1)中条件满足时，装配件的末端已***被装配件的圆-长方复合孔内，且***深度达到h_d，认为搜孔已完成，可切换到***阶段。***阶段内装配件不断沿z轴负方向前进，每当F_z(相当于***的阻力)达到阈值F_max时，认为***过程可能发生卡阻，此时需要对***的接触状态进行判断，之后根据接触状态调整装配件在孔内的位姿，并继续***，直到满足条件：

F_z＜F_max&h≥h_max (2)

其中h_max是装配完成的***深度判别阈值。当式(2)中条件满足时，装配件***被装配件的深度达到h_max，认为装配件已成功装入被装配件，装配作业结束。

应用所述圆-长方复合孔类零件装配方法的自动装配***中，需要使用力/位混合控制***进行控制，定义X＝[x,y,z,θ_P1,θ_P2,θ_P3]^T为装配件的位姿矢量，其中x、y、z分别是ΣO_P系在基坐标系ΣO内的位置坐标，θ_P1、θ_P2、θ_P3分别是ΣO_P系相对于ΣO系的3个欧拉角；θ＝[θ₁, θ₂,θ₃,θ₄,θ₅,θ₆]^T和τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T分别被定义为六自由度机器人的关节角和关节驱动力矩矢量，其中θ_i和τ_i(i＝1,2,…,6)分别表示第i个关节的关节角和驱动力矩；^SF_e是六维力/力矩传感器3在ΣO_S系内测得的力旋量(包含三个力分量和三个力矩分量)原始数据，^PF_e是将^SF_e变换到ΣO_P系内的力旋量；X_d、F_d、θ_d分别是控制过程中X、F、θ的目标值，δX、δF分别是控制过程中X的调整量和F理论值与实际值的偏差。

对于上述力位混合控制***，每个控制周期内的控制流程分为以下几步：

步骤一、反馈采样。对六自由度机器人的伺服电动机编码器和六维力/力矩传感器进行读数，分别得到机器人的关节角矢量θ和ΣO_S系内的力旋量^SF_e，根据六自由度机器人的正运动学仿真可由θ求得装配件的位姿矢量X，ΣO_P系内的力旋量^PF_e可按式(3)计算。

式(3)中R是ΣO_S系到ΣO_P系的旋转变换矩阵，P_S是O_S点在ΣO_P系内的位置矢量，0_3×3是3阶全零方阵。

步骤二、轨迹规划。按照装配流程进行装配件的位姿规划，若装配过程处于接近阶段，按5次样条函数计算当前时刻装配件的目标位姿X_d；若装配过程处于搜孔阶段或是***阶段，需首先判断接触力的z轴分量F_z是否达到阈值F_max，若F_z<F_max则按使装配件继续接近或***被装配件生成X_d，否则根据接触状态得出装配件的位姿调整量，然后根据此调整量生成X_d。

步骤三、阻抗控制。根据目标力旋量F_d和力反馈^PF_e的偏差δF计算装配件位姿X的调整量δX，使用的阻抗控制模型如式(4)所示。

其中M、B、K分别是装配件与被装配件间虚拟阻抗模型的惯性阵、阻尼阵和刚度阵。对于式(4)中的模型，则由δF到δX的传函如式(5)所示，实际控制时δX也可根据δF和此传递函数进行计算。

其中s表示传递函数中的拉普拉斯变量。

步骤四、位置控制。将轨迹规划得到的目标位姿X_d与阻抗控制得到的调整量δX叠加，按六自由度机器人的逆运动学方程计算得出关节角目标矢量θ_d，之后按式(6)的前馈+PD 反馈控制律计算关节驱动力矩τ，并将其发送给机器人各关节的伺服驱动/控制器，完成当前控制周期的指令输出。

其中M_R是六自由度机器人的广义惯性阵推定值，C_R和G_R分别是离心力/科氏力项的推定值和重力项的推定值，K_v和K_p分别是PD反馈控制中的微分项系数矩阵和比例项系数矩阵。

所述圆-长方复合孔类零件装配方法的搜孔阶段内，装配件的搜孔运动按每次与被装配件接触时的接触状态进行规划，所述接触状态按接触点的数量和在装配件底面(搜孔阶段与被装配件接触的平面)上的位置，共被分为4种。定义l和θ_C分别表示接触点在ΣO_P系的 xO_Py平面内的相角和矢径长度，a、b、c、r均是装配件底面轮廓的几何尺寸，r为圆弧半径，a为长方部分的宽度，b是圆弧部分与长方部分交线到O_P点的距离，c是长方部分的长度。

搜孔阶段的第一种接触状态中，单个接触点位于装配件底面轮廓的不完整圆弧内部，说明装配件底面轮廓内的一点与被装配件圆-长方复合孔的边缘发生干涉，装配件应向远离接触点的方向运动。

搜孔阶段的第二种接触状态中，单个接触点位于装配件底面轮廓的不完整圆弧边界上，说明装配件与被装配件处于边缘的尖角接触状态，此状态是搜孔完成前的临界状态，装配件应向远离接触点的方向运动较小的距离。

搜孔阶段的第三种接触状态中，两个接触点位于装配件底面轮廓的不完整圆弧边界上，说明两个接触点确定的猎狐段悬出被装配件的圆-长方复合孔边界，因此装配件应向远离接触点连线的方向运动，此种接触状态可等效为第一种接触状态，将两个接触点看作是连线中点处的一个接触点，这样调整策略完全相同。

搜孔阶段的第四种接触状态中，单个接触点位于装配件底面轮廓的长方部分内，说明装配件与被装配件由于对称面不平行产生了长方形部分的干涉，应绕z轴将装配件进行旋转，使接触点被旋出方形部分。

上述四种接触状态下，ΣO_P系内的力平衡方程均可被统一地写作：

其中F_x、F_y、F_z分别是将六维力/力矩传感器3测得的力反馈变换到ΣO_P系内的三个力分量，M_x、M_y、M_z分别将六维力/力矩传感器3测得的力反馈变换到ΣO_P系内的三个力分量，f是搜孔阶段接触点(对于第三种接触状态是指等效后的接触点)处的法向力的大小。对式(7)进行求解，可得：

根据各类接触状态的定义以及式(8)中的求解结果，可在表1中汇总给出搜孔阶段内的接触状态判别式及各接触状态对应装配件调整策略。

表1搜孔阶段内的接触状态判别式及各接触状态对应装配件调整策略

在自动装配作业的搜孔阶段内，每次装配件4与被装配件6进行试探接触后的搜孔轨迹可按以下步骤规划：

步骤一、根据力反馈变换得到的力旋量^PF_e，按式(8)求解表1第2列中的力平衡方程式，得到f、l、θ_C；

步骤二、根据表1第3列中的判别式确定搜孔阶段的接触状态种类；

步骤三、由表1第4列的调整策略，得到装配件在下一次试探中的位姿调整量，将其叠加到正常的调整运动轨迹中，得到下一次试探中装配件4的搜孔运动轨迹。

所述圆-长方复合孔类零件装配方法的***阶段内，每当六维力/力矩传感器3测得的z 轴力反馈F_z达到阈值F_max时，需要根据当前接触状态调整装配件4的位姿，使装配过程可以继续进行下去而不至于陷入卡死的情况。***阶段内装配件4的侧面和底面棱边可能与被装配件6接触，为对接触状态进行识别，按接触点在装配件4上所处位置的区别，定义了四种基本接触点，分别是：

第一类接触点位于装配件4的圆柱部分侧面，其力学平衡方程如式(9)所示，μ为装配件4与被装配件6之间的摩擦系数。

第二类接触点位于圆柱部分底面棱边，其力学平衡方程如式(10)所示。

第三类接触点位于方柱部分的侧面，其力学平衡方程如式(11)所示。

第三类接触点位于方柱部分的底面棱边上，其力学平衡方程如式(12)所示。

根据实际接触状态中接触点的数量、所属基本接触点类型、相对位置，将***阶段的接触状态分为了7大类41种，所分的七个大类分别是：含有一个第一或第二类基本接触点的单点第一类接触(图8)、含有一个第三或第四类基本接触点的单点第二类接触(图9)、含有一个第一类基本接触点和一个第二类基本接触点的两点第一类接触(图10)、含有一个第一或第二类基本接触点以及一个第三或第四类基本接触点的两点第二类接触(图11)、含有一个第三类基本接触点以及一个第四类基本接触点的两点第三类接触(图12)、含有一个第一或第二类基本接触点以及两个第三或第四类基本接触点的三点第一类接触(图 13)、含有两个第一或第二类基本接触点以及一个第三或第四类基本接触点的三点第二类接触(图14)。上述7个大类中分别含有2、8、1、16、4、2、8种接触状态，为方便后面的表示，以P-i-j作为***阶段接触大类的代号，其中i表示接触点的数量，j表示接触点数量确定后接触大类的序号；以P-i-j-k作为***阶段隶属于P-i-j大类的接触状态代号，k表示 P-i-j大类中的接触状态序号，例如两点第二类接触以P2-2表示，其中的第10种接触状态将被写作P2-2-10。

对于图8～图14中的接触状态，其力学平衡方程根据式(9)～式(12)中基本接触点的力学方程组合得到，接触状态大类的判别条件可依照各大类的力学方程特点分类给出，上述力学平衡方程和判别条件的推导结果在表2中给出，其中c_i和s_i(i＝1,2,3)分别是cosθ_Ci和sinθ_Ci的简写，θ_Ci和f_i分别是第i个接触点在ΣO_P系内的极角和法向接触力，k_mn (m＝1,2,…,7；n＝1,2,3,4)是第m个接触状态大类的第n个判别参数，用于对大类内的具体接触状态进行判别，k_mn的取值均在闭区间[-1,1]内。

表2***阶段接触状态大类的力学平衡方程及判别条件

对于表2内的接触状态大类，表3中给出了各大类内每种接触状态的判别条件和对应的调整运动，其中装配件的调整运动共有5种，即为在ΣOP系内沿x轴、y轴的两个平动和绕x轴、y轴、z轴的三个转动，以x、y、θ_x、θ_y、θ_z分别表示上述5种调整运动；每种调整运动都有正向和负向两个方向，分别以“+”和“-”表示；则调整运动被简写成变量符号和正负号复合的形式，例如使装配件4绕x轴负向转动在表3中被简写“θ_x-”。由于一些接触状态对应相同的装配件调整运动，在***阶段的控制中不必对其进行区分，因此将对应相同调整运动的接触状态的判别条件取并集后，这些接触状态在表3中被合并写作一行。

表3***阶段接触状态种类的判别条件及装配件位姿调整策略

在自动装配作业的***阶段内，每次装配件向下装配的装配力F_z达到阈值F_max时，装配件的调整运动可按以下步骤规划：

步骤一、根据力反馈变换得到的力旋量^PF_e中的力和力矩分量，按表2中第四列的大类判别条件判断当前接触状态所属的接触大类；

步骤二、对表2中第三列的力学平衡方程进行求解，得出反应接触点位置的θ_C1和基础种类判别参数k_mn；

步骤三、根据表3第三列的判别条件确定具体的接触状态，并在表3第四列中查得对应于当前接触状态的调整运动。

本发明的有益效果是：

本发明的圆-长方形复合孔类零件的装配理论与方法将装配运动过程划分为接近、搜索和***三个阶段，在接近阶段采用了在动力学前馈基础上的PD反馈控制，使机械臂可以快速接近孔件，提高了机械臂的动态控制性能；在搜索阶段采用了基于六维力传感器的平面搜孔方法，根据接触状态制定了搜孔策略，适合复合孔类零件搜孔作业任务；在***阶段利用传感器反馈的力信息和关节角信息，识别极端***过程中的接触状态，控制***的轨迹规划器在接触状态识别的基础上生成相应的位姿调整方案，并结合柔顺控制策略实现圆—长方形复合孔类零件的机器人装配任务，填补了国内外在针对复合孔类零件机器人装配的装配理论和方法方面的空白。

圆-长方复合孔类零件是一种常见零件，但因同时具有圆孔和方孔的特点，此类零件的自动装配面临接触状态繁多、装配策略难于确定的问题，本发明完全解决了上述问题。

本发明根据力反馈信息进行接触状态判别，并针对不同接触状态进行相应调整的圆-长方复合孔类零件自动装配方法。所述装配过程被分为接近阶段、搜孔阶段和***阶段，接近阶段使用5次样条轨迹规划的方法使装配件快速接近被装配件，搜孔阶段共有4种接触状态，***阶段的接触状态按接触点数量和相对位置共分为7大类41种，对每种接触状态均进行了受力分析并提出了相应的搜孔或***装配策略。进行了圆-长方复合孔类零件的装配仿真，结果表明所提出的装配方法能在防止接触力过大的前提下完成装配。

附图说明

图1是所述装配方法的装配对象(圆-长方复合孔类零件)的几何造型示意图，图中：a) 特形圆长方复合孔，b)开放式的特形圆长方复合孔，c)带键轮毂，d)c型平键槽，e)a型平键槽，f)花键轮毂；

图2是应用本发明提出的装配方法的自动装配***构成图；图3是所述自动装配***的虚拟样机三维实体；图4是本发明提出的圆-长方复合孔类零件装配流程图；图5是所述自动装配***的控制***框图；图6是搜孔阶段四种接触状态的示意图；

图7是***阶段四种基本接触点的示意图；图8是***阶段的单点第一类接触状态示意图；图9是***阶段的单点第二类接触状态示意图；图10是***阶段的两点第一类接触状态示意图(两点第一类接触状态P-2-1-1)；图11是***阶段的两点第二类接触状态示意图；图12是***阶段的两点第三类接触状态示意图；图13是***阶段的三点第一类接触状态示意图；图14是***阶段的三点第二类接触状态示意图；

图15是圆-长方复合孔类零件装配仿真中的装配对象尺寸参数图(图中：左上是装配件主视图，左下是俯视图；右上是被装配件主视图，右下是俯视图)；

图16是搜孔仿真的接触力及其产生的力矩以及位姿误差曲线图；图17是***阶段的接触状态识别仿真的接触力曲线；图18是圆-长方复合孔类零件装配仿真的录像截图；图19是圆-长方复合孔类零件装配仿真的接触力及位姿误差曲线图。

具体实施方式

具体实施方式一：所述装配方法面向解决几何元素中既有圆弧又有矩形的直边和直角的孔类零件的自动装配问题，避免装配过程中因轴孔间的位姿不当导致装配接触力过大而对装配件和机器人造成损坏。将此种孔类零件其称之为圆-长方复合孔类零件，与之相配的轴类零件被称为圆-长方复合轴类零件，图1给出了此类零件的几种常见形式，其中包括平键轮毂、花键轮毂、A型平键孔、C型平键孔、特型圆-长方孔等等。

工程实际的常见零件中属于圆-长方复合孔/轴类零件的有：花键、A型平键、C型平键等等。

如图2所示，应用所述装配方法的圆-长方复合孔类零件自动装配***由：上位机、基座1、六自由度机器人2、六维力/力矩传感器3、装配件4(与孔类零件适配的轴类零件)、夹持固定座5、被装配件6(圆-长方复合孔类零件)组成。其中六自由度机器人2是指任意的能实现空间六自由度运动且位置控制精度满足装配作业的机器人，并不局限于关节型的串联六自由度操作臂；上位机与六自由度机器人2的6个伺服驱动/控制器以及六维力/ 力矩传感器3通过总线通讯，使用的总线可以是以太网(Ethernet)、RS485网络、CAN总线等。

图3给出了上述自动装配***的虚拟样机图片，其中装配件4被夹持在六维力/力矩传感器3的工具侧接口上，六维力/力矩传感器3的机器人侧接口与六自由度机器人2的末端接口固连，带有圆-长方复合孔的被装配件6固定于夹持固定座5的接口内，夹持固定座5和六自由度机器人2均固定于基座1上。

图3中坐标系ΣO-xyz为原点固定于基座1的机器人接口中心的基坐标系，坐标系ΣO_P-xyz 和ΣO_S-xyz的原点分别位于装配件4的下表面圆心和六维力/力矩传感器3的测力中心，这两个坐标系在装配作业中始终保持相对静止。在ΣO_P系中，z轴方向平行于装配件4装入被装配件6的***方向，x轴方向平行于装配件4的几何轮廓对称面且与z轴垂直，y轴方向由z轴的单位矢量叉乘x轴的单位矢量所得到的矢量方向确定。

根据基座1、夹持固定座5、被装配件6的设计图纸尺寸，或根据***组装好后的实际测量结果，可算出被装配件6的圆-长方复合孔相对于六自由度机器人2的相对位置，该相对位置被定义为被装配件6的推定位置，受加工制造误差、测量误差等不确定因素的影响，此推定位置不等于被装配件6的实际位置，因此在装配过程中若直接按着被装配件6的推定位置控制装配件4的运动，容易在被装配件6和装配件4间产生干涉，需要根据六维力/ 力矩传感器3的反馈对装配件4进行调整。

具体实施方式二：在所述圆-长方复合孔类零件的装配方法中，六自由度机器人2需根据六维力/力矩传感器3测得的力反馈信息，对装配件4的位姿进行调整，使其最终装入被装配件6的圆-长方复合孔中。上述装配过程被分为三个阶段：接近阶段、搜孔阶段、***阶段，装配的流程图如图4所示，其中F_z表示六维力/力矩传感器3测得的z轴力分量(变换到ΣOP系内)，h表示装配件4沿z轴***被装配件6的距离，h_d和h_max分别是搜孔完成和装配完成的***深度判别阈值，F_max是接触力的阈值。

装配作业开始时首先进入接近阶段，装配件4需要在六自由度机器人2的控制下从初始位置快速接近被装配件6，到达位于被装配件6推定位置上方的装配准备位置，此准备位置可由推定位置加上增量矢量[0,0,Δz]^T算得。上述运动的轨迹规划应用5次样条法，其中起点(装配件4的初始位置)和终点(装配准备位置)处的速度和加速度均为0。

装配件4到达装配准备位置后进入搜孔阶段，此阶段开始后六自由度机器人2将使装配件4向被装配件6缓慢运动，进行试探，当z轴方向的接触力大于阈值F_max，装配作业的控制***将根据后若六维力/力矩传感器3的反馈判断装配件4与被装配件6之间的接触状态，然后将装配件4抬起一段距离并根据之前的接触状态调整其位姿，调整完成后将重复装配件4向被装配件6的试探过程，直到满足条件：

F_z＜F_max&h≥h_d (1)

当式(1)中条件满足时，装配件4的末端已***被装配件6的圆-长方复合孔内，且***深度达到h_d，认为搜孔已完成，可切换到***阶段。***阶段内装配件4不断沿z轴负方向前进，每当F_z(相当于***的阻力)达到阈值F_max时，认为***过程可能发生卡阻，此时需要对***的接触状态进行判断，之后根据接触状态调整装配件4在孔内的位姿，并继续***，直到满足条件：

F_z＜F_max&h≥h_max (2)

当式(2)中条件满足时，装配件4***被装配件6的深度达到h_max，认为装配件4已成功装入被装配件6，装配作业结束。其它步骤与具体实施方式一相同。

具体实施方式三：应用所述圆-长方复合孔类零件装配方法的自动装配***中，需要构建如图5所示的力/位混合控制***，定义X＝[x,y,z,θ_P1,θ_P2,θ_P3]^T为装配件4的位姿矢量，其中x、y、z分别是ΣO_P系在基坐标系ΣO内的位置坐标，θ_P1、θ_P2、θ_P3分别是ΣO_P系相对于ΣO系的3个欧拉角；θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T和τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T分别被定义为六自由度机器人2的关节角和关节驱动力矩矢量，其中θ_i和τ_i(i＝1,2,…,6)分别表示第i个关节的关节角和驱动力矩；^SF_e是六维力/力矩传感器3在ΣO_S系内测得的力旋量(包含三个力分量和三个力矩分量)原始数据，^PF_e是将^SF_e变换到ΣO_P系内的力旋量；X_d、F_d、θ_d分别是控制过程中X、F、θ的目标值，δX、δF分别是控制过程中X的调整量和F理论值与实际值的偏差。

对于上述力位混合控制***，每个控制周期内的控制流程分为以下几步：

步骤一、反馈采样。对六自由度机器人2的伺服电动机编码器和六维力/力矩传感器3 进行读数，分别得到机器人的关节角矢量θ和ΣO_S系内的力旋量^SF_e，根据六自由度机器人2 的正运动学仿真可由θ求得装配件的位姿矢量X，ΣO_P系内的力旋量^PF_e可按式(3)计算。

式(3)中R是ΣO_S系到ΣO_P系的旋转变换矩阵，P_S是O_S点在ΣO_P系内的位置矢量，0_3×3是3阶全零方阵。

步骤二、轨迹规划。按照已介绍的装配流程(图4)进行装配件4的位姿规划，若装配过程处于接近阶段，按5次样条函数计算当前时刻装配件4的目标位姿X_d；若装配过程处于搜孔阶段或是***阶段，需首先判断接触力的z轴分量F_z是否达到阈值F_max，若F_z<F_max则按使装配件4继续接近或***被装配件6生成X_d，否则根据接触状态得出装配件4的位姿调整量，然后根据此调整量生成X_d。

步骤三、阻抗控制。根据目标力旋量F_d和力反馈^PF_e的偏差δF计算装配件位姿X的调整量δX，使用的阻抗控制模型如式(4)所示。

其中M、B、K分别是装配件4与被装配件6间虚拟阻抗模型的惯性阵、阻尼阵和刚度阵。对于式(4)中的模型，则由δF到δX的传函如式(5)所示，实际控制时δX也可根据δF和此传递函数进行计算。

其中s表示传递函数中的拉普拉斯变量。

步骤四、位置控制。将轨迹规划得到的目标位姿X_d与阻抗控制得到的调整量δX叠加，按六自由度机器人2的逆运动学方程计算得出关节角目标矢量θ_d，之后按式(6)的前馈+PD 反馈控制律计算关节驱动力矩τ，并将其发送给机器人各关节的伺服驱动/控制器，完成当前控制周期的指令输出。

其中M_R是六自由度机器人2的广义惯性阵推定值，C_R和G_R分别是离心力/科氏力项的推定值和重力项的推定值，K_v和K_p分别是PD反馈控制中的微分项系数矩阵和比例项系数矩阵。其它步骤与具体实施方式二相同。

具体实施方式四：所述圆-长方复合孔类零件装配方法的搜孔阶段内，装配件4的搜孔运动按每次与被装配件6接触时的接触状态进行规划，所述接触状态按接触点的数量和在装配件底面(搜孔阶段与被装配件接触的平面)上的位置，共被分为如图6所示的4种，其中接触点以红色圆点表示，l和θ_C分别表示接触点在ΣO_P系的xO_Py平面内的相角和矢径长度，a、b、c、r均是装配件底面轮廓的几何尺寸，r为圆弧半径，a为长方部分的宽度， b是圆弧部分与长方部分交线到O_P点的距离，c是长方部分的长度。

图6a)中为搜孔阶段的第一种接触状态，单个接触点位于装配件底面轮廓的不完整圆弧内部，说明装配件底面轮廓内的一点与被装配件圆-长方复合孔的边缘发生干涉，装配件应向远离接触点的方向运动。

图6b)中为搜孔阶段的第二种接触状态，单个接触点位于装配件底面轮廓的不完整圆弧边界上，说明装配件4与被装配件6处于边缘的尖角接触状态，此状态是搜孔完成前的临界状态，装配件应向远离接触点的方向运动较小的距离。

图6c)中为搜孔阶段的第三种接触状态，两个接触点位于装配件底面轮廓的不完整圆弧边界上，说明两个接触点确定的猎狐段悬出被装配件6的圆-长方复合孔边界，因此装配件应向远离接触点连线的方向运动，此种接触状态可等效为图6a)中的第一种接触状态，将两个接触点看作是连线中点处的一个接触点，这样调整策略完全相同。

图6d)中为搜孔阶段的第四种接触状态，单个接触点位于装配件底面轮廓的长方部分内，说明装配件4与被装配件6由于对称面不平行产生了长方形部分的干涉，应绕z轴(图 5中的垂直于纸面向外的轴线方向)将装配件进行旋转，使接触点被旋出方形部分。

上述四种接触状态下，ΣO_P系内的力平衡方程均可被统一地写作：

其中F_x、F_y、F_z分别是将六维力/力矩传感器3测得的力反馈变换到ΣO_P系内的三个力分量(此处的F_z与图3中的F_z定义相同)，M_x、M_y、M_z分别将六维力/力矩传感器3测得的力反馈变换到ΣO_P系内的三个力分量，f是搜孔阶段接触点(对于第三种接触状态是指等效后的接触点)处的法向力的大小。对式(7)进行求解，可得：

根据各类接触状态的定义以及式(8)中的求解结果，可在表1中汇总给出搜孔阶段内的接触状态判别式及各接触状态对应装配件调整策略。

表1搜孔阶段内的接触状态判别式及各接触状态对应装配件调整策略

在自动装配作业的搜孔阶段内，每次装配件4与被装配件6进行试探接触后的搜孔轨迹可按以下步骤规划：

步骤一、根据力反馈变换得到的力旋量^PF_e，按式(8)求解表1第2列中的力平衡方程式，得到f、l、θ_C；

步骤二、根据表1第3列中的判别式确定搜孔阶段的接触状态种类；

步骤三、由表1第4列的调整策略，得到装配件在下一次试探中的位姿调整量，将其叠加到正常的调整运动轨迹中，得到下一次试探中装配件4的搜孔运动轨迹。其它步骤与具体实施方式三相同。

具体实施方式五：所述圆-长方复合孔类零件装配方法的***阶段内，每当六维力/力矩传感器3测得的z轴力反馈F_z达到阈值F_max时，需要根据当前接触状态调整装配件4的位姿，使装配过程可以继续进行下去而不至于陷入卡死的情况。***阶段内装配件4的侧面和底面棱边可能与被装配件6接触，为对接触状态进行识别，按接触点在装配件4上所处位置的区别，定义了如图7所示的四种基本接触点。

按图7a)～d)的顺序，所定义的4种基本接触点分别是：

第一类接触点位于装配件4的圆柱部分侧面，其力学平衡方程如式(9)所示，μ为装配件4与被装配件6之间的摩擦系数。

第二类接触点位于圆柱部分底面棱边，其力学平衡方程如式(10)所示。

第三类接触点位于方柱部分的侧面，其力学平衡方程如式(11)所示。

第三类接触点位于方柱部分的底面棱边上，其力学平衡方程如式(12)所示。

根据实际接触状态中接触点的数量、所属基本接触点类型、相对位置，将***阶段的接触状态分为了7大类41种，所分的七个大类分别是：含有一个第一或第二类基本接触点的单点第一类接触(图8)、含有一个第三或第四类基本接触点的单点第二类接触(图9)、含有一个第一类基本接触点和一个第二类基本接触点的两点第一类接触(图10)、含有一个第一或第二类基本接触点以及一个第三或第四类基本接触点的两点第二类接触(图11)、含有一个第三类基本接触点以及一个第四类基本接触点的两点第三类接触(图12)、含有一个第一或第二类基本接触点以及两个第三或第四类基本接触点的三点第一类接触(图 13)、含有两个第一或第二类基本接触点以及一个第三或第四类基本接触点的三点第二类接触(图14)。上述7个大类中分别含有2、8、1、16、4、2、8种接触状态，为方便后面的表示，以P-i-j作为***阶段接触大类的代号，其中i表示接触点的数量，j表示接触点数量确定后接触大类的序号；以P-i-j-k作为***阶段隶属于P-i-j大类的接触状态代号，k表示 P-i-j大类中的接触状态序号，例如两点第二类接触以P2-2表示，其中的第10种接触状态将被写作P2-2-10。

对于图8～图14中的接触状态，其力学平衡方程根据式(9)～式(12)中基本接触点的力学方程组合得到，接触状态大类的判别条件可依照各大类的力学方程特点分类给出，上述力学平衡方程和判别条件的推导结果在表2中给出，其中c_i和s_i(i＝1,2,3)分别是cosθ_Ci和sinθ_Ci的简写，θ_Ci和f_i分别是第i个接触点在ΣO_P系内的极角和法向接触力，k_mn (m＝1,2,…,7；n＝1,2,3,4)是第m个接触状态大类的第n个判别参数，用于对大类内的具体接触状态进行判别，k_mn的取值均在闭区间[-1,1]内。

表2***阶段接触状态大类的力学平衡方程及判别条件

对于表2内的接触状态大类，表3中给出了各大类内每种接触状态的判别条件和对应的调整运动，其中装配件的调整运动共有5种，即为在ΣOP系内沿x轴、y轴的两个平动和绕x轴、y轴、z轴的三个转动，以x、y、θ_x、θ_y、θ_z分别表示上述5种调整运动；每种调整运动都有正向和负向两个方向，分别以“+”和“-”表示；则调整运动被简写成变量符号和正负号复合的形式，例如使装配件4绕x轴负向转动在表3中被简写“θ_x-”。由于一些接触状态对应相同的装配件调整运动，在***阶段的控制中不必对其进行区分，因此将对应相同调整运动的接触状态的判别条件取并集后，这些接触状态在表3中被合并写作一行。

表3***阶段接触状态种类的判别条件及装配件位姿调整策略

在自动装配作业的***阶段内，每次装配件4向下装配的装配力F_z达到阈值F_max时，装配件的调整运动可按以下步骤规划：

步骤一、根据力反馈变换得到的力旋量^PF_e中的力和力矩分量，按表2中第四列的大类判别条件判断当前接触状态所属的接触大类；

步骤二、对表2中第三列的力学平衡方程进行求解，得出反应接触点位置的θ_C1和基础种类判别参数k_mn；

步骤三、根据表3第三列的判别条件确定具体的接触状态，并在表3第四列中查得对应于当前接触状态的调整运动。其它步骤与具体实施方式四相同。

具体实施方式六：作为对本发明中提出的搜孔阶段、***阶段、以及整体的自动装配控制方法的可行性验证，进行了圆-长方复合孔类零件的自动装配仿真。在此仿真中，自动装配***使用图3中的虚拟样机***，装配件和被装配件选择了一种用于核工业的间隙配合圆-长方形复合孔类零件，其具体的尺寸参数如图15所示，圆柱部分的半径为1.5mm，方柱部分的长和宽分别为28mm和1.8mm，装配件中圆柱和方柱的厚度为30mm，装配件与被装配件的轴孔间隙为0.03mm。

所进行的自动装配仿真共分为3个部分：搜孔仿真、***阶段接触状态判别的仿真和整体的自动装配仿真。搜孔仿真的目的是为对所提出的搜孔方法进行验证，初始状态下，自动装配控制***中只存有被装配件6的推定位姿，此位姿与被装配件6位姿真值的偏差被称为搜孔误差，其中沿x轴、y轴平移的搜孔误差被称为搜孔的位置误差，初始的搜孔位置误差为±1mm范围内的随机数；绕z轴转动的搜孔误差被称为搜孔的角度误差，初始的搜孔角度误差为±4°范围内的随机数(沿z轴平动、绕x轴和y轴转动方向上未设置搜孔误差)。对于上述范围内的不同初始搜孔误差，进行了多组搜孔仿真，如图16所示是其中一组搜孔仿真的接触力曲线和搜孔误差曲线。

在图16所示的仿真过程中，为简化装配件位姿的调节运动，对三个方向(沿x轴和y轴平移、绕z轴转动)的搜孔误差分别单独进行补偿，因此在搜孔过程中搜孔的位置误差和角度误差呈现出一定的波动。在搜孔结束的时刻，位置均小于0.1mm，角度误差小于0.3°，同时整个搜孔过程中接触力小于10N，接触力产生的力矩不到0.1Nm，由此可知所提出的搜孔方法能在不损坏装配件与被装配件(接触力和力矩较小)的前提下，将搜孔误差减小到可开始进行***的程度，证明了所提出方法的有效性。

为验证所提出的***阶段内接触状态判别方法，所进行的第二部分仿真为***阶段的接触状态判别仿真，仿真过程中从每大类接触中随机选择一种接触状态，每个接触状态持续300个采样周期，然后切换到下一大类的接触状态，如此可以得到对应于7个接触状态的2100点采样序列，对此序列中的每个点根据表2和表3中的判别条件进行判别，即可得到接触状态的判别结果。

按上述方法，进行了多组接触状态的判别仿真，其中的一组仿真中预设的7个接触状态分别是P-1-1-1、P-1-2-2、P-2-1-1、P-2-2-9、P-2-3-3、P-3-1-2、P-3-2-7，图17给出了仿真过程得到的判别结果(图上方的接触状态示意图和代号)和接触力曲线(其中)，可以看出识别结果的正确率为100％。

为对所提出的自动装配方法进行完整的可行性验证，进行了包含接近阶段、搜孔阶段、***阶段的连贯的装配仿真，仿真得到的录像截图如图18所示，其中0～6s为接近阶段的远景录像截图；6s后主要是装配件4在被装配件6附近的微调，因此切换到近景录像，可以看到在20s左右搜孔过程结束，此时装配件4已基本对正被装配件6；20s后开始***阶段，至45s装配件4已完全装入被装配件6，完成了装配作业。

图19给出了装配过程中装配力和装配件4距离装配完成位置的误差曲线，可以看到整个装配过程中的装配力受力小于16N，力矩小于0.04Nm，由此可知所提出的圆-长方复合孔类零件装配方法能够在保证装配柔顺的前提下，完成圆-长方复合孔类零件的自动装配作业。

Claims (5)

1.一种圆-长方形复合孔类零件的机器人自动装配方法，所述装配方法的装配对象是几何要素中既有圆又有矩形的直边和直角的孔类零件和轴类零件，其中具有复合的圆孔和方孔的零件被称为圆-长方复合孔类零件，具有复合的圆柱和方柱的零件被称为圆-长方复合轴类零件，

其特征在于，应用所述装配方法的圆-长方复合孔类零件自动装配***由：上位机、基座(1)、六自由度机器人(2)、六维力/力矩传感器(3)、装配件(4)(圆-长方复合轴类零件)、夹持固定座(5)、被装配件(6)组成；其中六自由度机器人2是指任意的能实现空间六自由度运动且位置控制精度满足装配作业的机器人，上位机与六自由度机器人(2)的6个伺服驱动/控制器以及六维力/力矩传感器(3)通过总线通讯，装配件(4)是指圆-长方复合轴类零件，被装配件(6)是指圆-长方复合孔类零件，

装配件(4)被夹持在六维力/力矩传感器(3)的工具侧接口上，六维力/力矩传感器(3)的机器人侧接口与六自由度机器人(2)的末端接口固连，带有圆-长方复合孔的被装配件(6)固定于夹持固定座(5)的接口内，夹持固定座(5)和六自由度机器人(2)均固定于基座(1)上；

根据基座(1)、夹持固定座(5)、被装配件(6)的设计图纸尺寸，或根据***组装好后的实际测量结果，可算出被装配件(6)的圆-长方复合孔相对于六自由度机器人(2)的相对位置，该相对位置被定义为被装配件(6)的推定位置，此推定位置不等于被装配件(6)的实际位置，因此在装配过程中若直接按着被装配件(6)的推定位置控制装配件(4)的运动，容易在被装配件(6)和装配件(4)间产生干涉，需要根据六维力/力矩传感器(3)的反馈对装配件(4)进行调整。

2.根据权利要求1中所述的圆-长方形复合孔类零件的机器人自动装配方法，其装配过程被分为三个阶段，即接近阶段、搜孔阶段、***阶段；

装配作业开始时首先进入接近阶段，装配件(4)需要在六自由度机器人(2)的控制下从初始位置快速接近被装配件(6)，到达位于被装配件(6)推定位置上方的装配准备位置，此准备位置可由推定位置加上增量矢量[0,0,Δz]^T算得，Δz为竖直方向的位置增量；上述运动的轨迹规划应用5次样条法，其中起点和终点处的速度和加速度均为0，所述起点是指装配件(4)的初始位置，所述终点是指装配准备位置；

装配件(4)到达装配准备位置后进入搜孔阶段，此阶段开始后六自由度机器人(2)将使装配件(4)向被装配件(6)缓慢运动，进行试探，当竖直方向的接触力大于阈值F_max，装配作业的控制***将根据后若六维力/力矩传感器(3)的反馈判断装配件(4)与被装配件(6)之间的接触状态，然后将装配件(4)抬起一段距离并根据之前的接触状态调整其位姿，调整完成后将重复装配件(4)向被装配件(6)的试探过程，直到满足条件：

F_z＜F_max&h≥h_d (1)

其中F_z表示六维力/力矩传感器(3)测得的竖直方向力分量，F_max是接触力的阈值，h表示装配件(4)***被装配件(6)的深度，h_d是搜孔完成***深度判别阈值；

当式(1)中条件满足时，装配件(4)的末端已***被装配件(6)的圆-长方复合孔内，且***深度达到h_d，认为搜孔已完成，可切换到***阶段；***阶段内装配件(4)不断沿z轴负方向前进，即装配件竖直向下***被装配件的方向，每当F_z达到阈值F_max时，认为***过程可能发生卡阻，此时需要对***的接触状态进行判断，之后根据接触状态调整装配件(4)在孔内的位姿，并继续***，直到满足条件：

F_z＜F_max&h≥h_max (2)

其中h_max是装配完成的***深度判别阈值；

当式(2)中条件满足时，装配件(4)***被装配件(6)的深度达到h_max，认为装配件(4)已成功装入被装配件(6)，装配作业结束。

3.根据权利要求1或2中所述的圆-长方形复合孔类零件的机器人自动装配方法，其特征在于，装配作业过程中的六自由度机器人(2)由力/位混合控制***进行控制，

首先进行坐标系定义，基坐标系ΣO-xyz的原点固定于基座(1)的机器人接口中心，装配件坐标系ΣO_P-xyz和测量ΣO_S-xyz的原点分别位于装配件(4)的下表面圆心和六维力/力矩传感器(3)的测力中心；在ΣO_P系中，z轴方向平行于装配件(4)装入被装配件(6)的***方向，x轴方向平行于装配件(4)的几何轮廓对称面且与z轴垂直，y轴方向由z轴的单位矢量叉乘x轴的单位矢量所得到的矢量方向确定；

控制***的变量定义如下：X＝[x,y,z,θ_P1,θ_P2,θ_P3]^T为装配件(4)的位姿矢量，其中x、y、z分别是ΣO_P系在基坐标系ΣO内的位置坐标，θ_P1、θ_P2、θ_P3分别是ΣO_P系相对于ΣO系的3个欧拉角；θ＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]^T和τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T分别被定义为六自由度机器人(2)的关节角和关节驱动力矩矢量，其中θ_i和τ_i(i＝1,2,…,6)分别表示第i个关节的关节角和驱动力矩；^SF_e是六维力/力矩传感器(3)在ΣO_S系内测得的力旋量原始数据，^PF_e是将^SF_e变换到ΣO_P系内的力旋量；X_d、F_d、θ_d分别是控制过程中X、F、θ的目标值，δX、δF分别是控制过程中X的调整量和F理论值与实际值的偏差；

对于圆-长方形复合孔类零件自动装配用的力/位混合控制***，每个控制周期内的控制流程分为以下几步：

步骤一、反馈采样：对六自由度机器人(2)的伺服电动机编码器和六维力/力矩传感器3进行读数，分别得到机器人的关节角矢量θ和ΣO_S系内的力旋量^SF_e，根据六自由度机器人2的正运动学仿真可由θ求得装配件的位姿矢量X，ΣO_P系内的力旋量^PF_e可按式(3)计算；

式(3)中R是ΣO_S系到ΣO_P系的旋转变换矩阵，P_S是O_S点在ΣO_P系内的位置矢量，0_3×3是3阶全零方阵；

步骤二、轨迹规划：按照权利要求3中的装配流程进行装配件(4)的位姿轨迹规划，若装配过程处于接近阶段，按5次样条函数计算当前时刻装配件(4)的目标位姿X_d；若装配过程处于搜孔阶段或是***阶段，需首先判断接触力的z轴分量F_z是否达到阈值F_max，若F_z<F_max则按使装配件(4)继续接近或***被装配件(6)生成X_d，否则根据接触状态得出装配件4的位姿调整量，然后根据此调整量生成X_d；

步骤三、阻抗控制：根据目标力旋量F_d和力反馈^PF_e的偏差δF计算装配件位姿X的调整量δX，使用的阻抗控制模型如式(4)所示，

其中M、B、K分别是装配件(4)与被装配件(6)间虚拟阻抗模型的惯性阵、阻尼阵和刚度阵；对于式(4)中的模型，则由δF到δX的传函如式(5)所示，实际控制时δX根据δF和此传递函数进行计算，

其中s表示传递函数中的拉普拉斯变量；

步骤四、位置控制：将轨迹规划得到的目标位姿X_d与阻抗控制得到的调整量δX叠加，按六自由度机器人(2)的逆运动学方程计算得出关节角目标矢量θ_d，之后按式(6)的前馈+PD反馈控制律计算关节驱动力矩τ，并将其发送给机器人各关节的伺服驱动/控制器，完成当前控制周期的指令输出，

其中M_R是六自由度机器人2的广义惯性阵推定值，C_R和G_R分别是离心力/科氏力项的推定值和重力项的推定值，K_v和K_p分别是PD反馈控制中的微分项系数矩阵和比例项系数矩阵。

4.根据权利要求3中所述的圆-长方形复合孔类零件的机器人自动装配方法，其特征在于，搜孔阶段内装配件(4)与被装配件(6)的接触状态共有4种，装配件(4)的搜孔运动可根据每次试探后的接触状态确定；

搜孔阶段的第一种接触状态中，单个接触点位于装配件底面轮廓的不完整圆弧内部，说明装配件底面轮廓内的一点与被装配件圆-长方复合孔的边缘发生干涉，装配件应向远离接触点的方向运动；

搜孔阶段的第二种接触状态中，单个接触点位于装配件底面轮廓的不完整圆弧边界上，说明装配件(4)与被装配件(6)处于边缘的尖角接触状态，此状态是搜孔完成前的临界状态，装配件应向远离接触点的方向运动较小的距离；

搜孔阶段的第三种接触状态中，两个接触点位于装配件底面轮廓的不完整圆弧边界上，说明两个接触点确定的猎狐段悬出被装配件(6)的圆-长方复合孔边界，因此装配件应向远离接触点连线的方向运动，此种接触状态可等效为搜孔阶段的第一种接触状态，将两个接触点看作是连线中点处的一个接触点，这样调整策略完全相同；

搜孔阶段的第四种接触状态中，单个接触点位于装配件底面轮廓的长方部分内，说明装配件(4)与被装配件(6)由于对称面不平行产生了长方形部分的干涉，应绕z轴将装配件进行旋转，使接触点被旋出方形部分；

上述四种接触状态下，ΣO_P系内的力平衡方程均可被统一地写作：

其中F_x、F_y、F_z分别是将六维力/力矩传感器(3)测得的力反馈变换到ΣO_P系内的三个力分量，此处的Fz与权利要求3中的F_z定义相同；

M_x、M_y、M_z分别将六维力/力矩传感器(3)测得的力反馈变换到ΣO_P系内的三个力分量，f是搜孔阶段接触点处的法向力的大小；对式(7)进行求解，可得：

根据各类接触状态的定义以及式(8)中的求解结果，在表1中汇总给出搜孔阶段内的接触状态判别式及各接触状态对应装配件调整策略：

表1 搜孔阶段内的接触状态判别式及各接触状态对应装配件调整策略

在自动装配作业的搜孔阶段内，每次装配件(4)与被装配件(6)进行试探接触后的搜孔轨迹可按以下步骤规划：

步骤一、根据力反馈变换得到的力旋量^PF_e，按式(8)求解表1第2列中的力平衡方程式，得到f、l、θ_C；

步骤二、根据表1第3列中的判别式确定搜孔阶段的接触状态种类；

步骤三、由表1第4列的调整策略，得到装配件在下一次试探中的位姿调整量，将其叠加到正常的调整运动轨迹中，得到下一次试探中装配件(4)的搜孔运动轨迹。

5.根据权利要求4中所述的圆-长方形复合孔类零件的机器人自动装配方法，其特征在于，***阶段内存在四大类41种接触状态，且每种接触状态均对应独立的装配调整策略，

***阶段内每当六维力/力矩传感器(3)测得的z轴力反馈F_z达到阈值F_max时，需要根据当前接触状态调整装配件(4)的位姿，使装配过程可以继续进行下去而不至于陷入卡死的情况；***阶段内装配件(4)的侧面和底面棱边可能与被装配件(6)接触，为对接触状态进行识别，按接触点在装配件(4)上所处位置的区别，定义了四种基本接触点，分别是：

第一类接触点位于装配件(4)的圆柱部分侧面，其力学平衡方程如式(9)所示，μ为装配件(4)与被装配件(6)之间的摩擦系数，

第二类接触点位于圆柱部分底面棱边，其力学平衡方程如式(10)所示，

第三类接触点位于方柱部分的侧面，其力学平衡方程如式(11)所示，

第三类接触点位于方柱部分的底面棱边上，其力学平衡方程如式(12)所示，

根据实际接触状态中接触点的数量、所属基本接触点类型、相对位置，将***阶段的接触状态分为了7大类41种，所分的七个大类分别是：含有一个第一或第二类基本接触点的单点第一类接触、含有一个第三或第四类基本接触点的单点第二类接触、含有一个第一类基本接触点和一个第二类基本接触点的两点第一类接触、含有一个第一或第二类基本接触点以及一个第三或第四类基本接触点的两点第二类接触、含有一个第三类基本接触点以及一个第四类基本接触点的两点第三类接触、含有一个第一或第二类基本接触点以及两个第三或第四类基本接触点的三点第一类接触、含有两个第一或第二类基本接触点以及一个第三或第四类基本接触点的三点第二类接触；上述7个大类中分别含有2、8、1、16、4、2、8种接触状态，为方便后面的表示，以P-i-j作为***阶段接触大类的代号，其中i表示接触点的数量，j表示接触点数量确定后接触大类的序号；以P-i-j-k作为***阶段隶属于P-i-j大类的接触状态代号，k表示P-i-j大类中的接触状态序号，例如两点第二类接触以P2-2表示，其中的第10种接触状态将被写作P2-2-10；

对于上述7大类接触状态，其力学平衡方程根据式(9)～式(12)中基本接触点的力学方程组合得到，表2中给出了接触状态大类的力学平衡方程和对应的判别条件，

表2 ***阶段接触状态大类的力学平衡方程及判别条件

表2中c_i和s_i(i＝1,2,3)分别是cosθ_Ci和sinθ_Ci的简写，θ_Ci和f_i分别是第i个接触点在ΣO_P系内的极角和法向接触力，k_mn(m＝1,2,…,7；n＝1,2,3,4)是第m个接触状态大类的第n个判别参数，用于对大类内的具体接触状态进行判别，k_mn的取值均在闭区间[-1,1]内；

对于表2内的接触状态大类，表3中给出了各大类内每种接触状态的判别条件和对应的调整运动，其中装配件的调整运动共有5种，即为在ΣOP系内沿x轴、y轴的两个平动和绕x轴、y轴、z轴的三个转动，以x、y、θ_x、θ_y、θ_z分别表示上述5种调整运动；每种调整运动都有正向和负向两个方向，分别以“+”和“-”表示；则调整运动被简写成变量符号和正负号复合的形式，使装配件4绕x轴负向转动在表3中被简写“θ_x-”；将对应相同调整运动的接触状态的判别条件取并集后，这些接触状态在表3中被合并写作一行，

表3 ***阶段接触状态种类的判别条件及装配件位姿调整策略

在自动装配作业的***阶段内，每次装配件(4)向下装配的装配力F_z达到阈值F_max时，装配件的调整运动可按以下步骤规划：

步骤一、根据力反馈变换得到的力旋量^PF_e中的力和力矩分量，按表2中第四列的大类判别条件判断当前接触状态所属的接触大类；

步骤二、对表2中第三列的力学平衡方程进行求解，得出反应接触点位置的θ_C1和基础种类判别参数k_mn；

步骤三、根据表3第三列的判别条件确定具体的接触状态，并在表3第四列中查得对应于当前接触状态的调整运动。