CN104625676A - 轴孔装配工业机器人系统及其工作方法 - Google Patents

Abstract

本发明提供了一种轴孔装配工业机器人系统包括:工业机器人、轴孔装配平台、力觉传感器、夹持装置、装配工件、装配孔及装配轴。本发明提供的轴孔装配工业机器人系统能够对接触力大小进行精确的控制，针对轴孔间距小、精度要求高、对轴孔接触力有严格要求的装配作业有明显的优势，避免了因仅位置控制模式下由于位置控制精度和接触力不可控性造成的装配作业失败甚至对装配工件的损坏，解决了某些高精度装配作业下依靠人工装配的低效率低质量等问题，扩大了装配工业机器人的应用领域。

Description

轴孔装配工业机器人系统及其工作方法

技术领域

本发明涉及工业机器人应用领域，尤其涉及一种轴孔装配工业机器人系统及其工作方法。

背景技术

数控加工在制造业已被广泛应用，随着加工零件的复杂度增加和精度的提高，一些精细零件的加工与装配作业依靠普通的数控机床已经不能满足需求。例如高精度的轴孔装配作业，由于环境中存在诸多不确定性和突发事件，使得装配作业时常发生故障，在常规位置控制下，装配工业机器人装配件之间的微小偏差可能使装配件不能完成装配，导致装配作业失败，甚至造成对装配工件及周边设备的损伤。

公开号CN101041220A的专利公开了一种利用低精度的工业机器人实现高精度轴孔装配的方法，该方法利用轴孔空间的“碗状约束域”实现了轴孔的装配，公开号CN101585137A的专利中公开了一种多轴孔装配装置，该装配装置是根据刚性轴孔装配任务的特点，针对弹性多轴孔装配动作而设计的，但是他们都是采用单一的位置控制模式设计的，而对接触力不能够进行控制，不适用于对接触力要求较为严格的装配作业。针对此问题CN102218652B号专利中提到柔顺性实现轴孔装配，但其是利用增大位置环增益完成的，可以实现对孔运动的情况下的轴孔装配作业，但其仍然是靠位置控制来实现轴孔装配，不能够对接触力进行控制，在对装配精度要求较高或刚度较低的工件进行装配作业时，其接触力的不可控性容易造成装配失败或者工件的损伤。

发明内容

针对现有技术存在的缺陷，本发明提供一种工作精度高且接触力可控的轴孔装配工业机器人系统。

一种轴孔装配工业机器人系统，包括：

工业机器人、轴孔装配平台、力觉传感器、夹持装置、装配工件、装配孔及装配轴，所述工业机器人包括运动计算模块、电机驱动模块及力控制模块；

所述工业机器人与所述轴孔装配平台的底座固定在同一基准面上；所述夹持装置安装于所述工业机器人的手臂末端；所述装配工件固定在所述轴孔装配平台上，所述装配工件上设有所述装配孔；所述力觉传感器安装在所述手臂末端与所述夹持装置之间，用于实时测量所述装配轴与所述装配孔间接触力；

所述工业机器人通过示教及轨迹规划生成一系列运动轨迹点，以使所述夹持装置夹持所述装配轴运动至所述装配孔上方；

所述运动计算模块将所述运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至所述电机驱动模块；

所述电机驱动模块驱动所述工业机器人运动并带动所述夹持装置夹持所述装配轴运动至所述装配孔上方；

所述力控制模块根据所述接触力及所述装配轴的位置，控制所述工业机器人运动使所述装配轴与所述装配孔以设定好的接触力进行接触，将所述装配轴顺利插入至所述装配孔中。

优选地，所述力觉传感器为六维力觉传感器，能够检测笛卡尔坐标系下三个方向的力和绕三个方向的力矩。

优选地，所述工业机器人还包括示教模块和轨迹规划模块，所述示教模块控制所述工业机器人在笛卡尔空间和关节空间下进行点动，直至所述夹持装置夹持所述装配轴到达所述装配孔正上方，并记录关键的示教点；所述轨迹规划模块将所述示教点解析成平滑的样条曲线，再将所述样条曲线按照时间点离散化成运动轨迹点。

优选地，所述电机驱动模块包括位置环、速度环和电流环，所述位置环输入包括关节空间下所述工业机器人各关节转动角度和伺服系统反馈的位置；所述速度环输入包括所述位置环计算出的关节转动速度、伺服系统反馈的转动速度和力控制计算出的速度补偿值；所述电流环输入包括速度环计算出的电流值和伺服系统反馈的电流值。

优选地，所述力控制模块包括接触单元、搜孔单元及插入单元；

所述接触单元用于将所述接触力与设定好的接触力做差得出力补偿值，并将所述力补偿值转化为速度补偿值，并将所述速度补偿值加载到所述速度环输入上，实现所述接触力的反馈控制；

所述搜孔单元用于控制所述装配轴沿X方向和Y方向做搜孔运动的位置及控制Z方向的接触力及位置，再由所述力觉传感器检测所述装配轴与所述装配工件Z方向的接触力判断是否找到正确的装配孔；

插入单元通过所述力觉传感器检测所述装配轴与所述装配工件间产生的XYZ的力矩值判断姿态的偏差并作出相应的姿态补偿，并控制所述装配轴的Z向接触力和位置，以检测所述装配轴的落底状态。

另外，本发明还提供了一种轴孔装配工业机器人的工作方法，包括下述步骤：

步骤A：所述工业机器人通过示教及轨迹规划生成一系列运动轨迹点，以使所述夹持装置夹持所述装配轴运动至所述装配孔上方；

步骤B：所述运动计算模块将所述运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至所述电机驱动模块；

步骤C：所述电机驱动模块驱动所述工业机器人运动并带动所述夹持装置夹持所述装配轴运动至所述装配孔上方；

步骤D：所述力控制模块根据所述接触力及所述装配轴的位置，控制所述工业机器人运动使所述装配轴与所述装配孔以设定好的接触力进行接触，将所述装配轴顺利插入至所述装配孔中。

优选地，所述工业机器人还包括示教模块和轨迹规划模块，所述示教模块控制所述工业机器人在笛卡尔空间和关节空间下进行点动，直至所述夹持装置夹持所述装配轴到达所述装配孔正上方，并记录关键的示教点；所述轨迹规划模块将所述示教点解析成平滑的样条曲线，再将所述样条曲线按照时间点离散化成运动轨迹点。

优选地，所述电机驱动模块包括位置环、速度环和电流环，所述位置环输入包括关节空间下所述工业机器人各关节转动角度和伺服系统反馈的位置；所述速度环输入包括所述位置环计算出的关节转动速度、伺服系统反馈的转动速度和力控制计算出的速度补偿值；所述电流环输入包括速度环计算出的电流值和伺服系统反馈的电流值。

优选地，步骤D包括下述步骤：

所述力控制模块将所述接触力与设定好的接触力做差得出力补偿值，并将所述力补偿值转化为速度补偿值，并将所述速度补偿值加载到所述速度环输入上，实现所述接触力的反馈控制；

所述力控制模块控制所述装配轴沿X方向和Y方向做搜孔运动的位置及控制Z方向的接触力及位置，再由所述力觉传感器检测所述装配轴与所述装配工件Z方向的接触力判断是否找到正确的装配孔；

所述力控制模块通过所述力觉传感器检测所述装配轴与所述装配工件间产生的XYZ的力矩值判断姿态的偏差并作出相应的姿态补偿，并控制所述装配轴的Z向接触力和位置，以检测所述装配轴的落底状态。

采用上述技术方案，本发明的有益效果在于，

本发明上述实施例提供的轴孔装配工业机器人系统及其工作方法，所述工业机器人通过示教及轨迹规划生成一系列运动轨迹点，以使所述夹持装置夹持所述装配轴运动至所述装配孔上方；所述运动计算模块将所述运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至所述电机驱动模块；所述电机驱动模块驱动所述工业机器人运动并带动所述夹持装置夹持所述装配轴运动至所述装配孔上方；所述力控制模块根据所述接触力及所述装配轴的位置，控制所述工业机器人运动使所述装配轴与所述装配孔以设定好的接触力进行接触，将所述装配轴顺利插入至所述装配孔中。本发明提供的轴孔装配工业机器人系统能够对接触力大小进行精确的控制，针对轴孔间距小、精度要求高、对轴孔接触力有严格要求的装配作业有明显的优势，避免了因仅位置控制模式下由于位置控制精度和接触力不可控性造成的装配作业失败甚至对装配工件的损坏，解决了某些高精度装配作业下依靠人工装配的低效率低质量等问题，扩大了装配工业机器人的应用领域。

另外，本发明采用上述的轴孔装配工业机器人系统，将力控制融入装配作业中，弥补了传统位置控制在高质量高精度装配作业中的不足。采用位置与接触力同时控制的模式，通过将力觉传感器反馈与接触力设定值的差值转化为速度补偿值，并将该速度补偿值加到速度环的输入，实现了不破坏位置闭环的同时对接触力的闭环控制，装配精度高。

附图说明

图1为本发明提供的轴孔装配工业机器人系统的结构示意图；

图2为图1所示的工业机器人的功能示意图；

图3为电机驱动模块的功能示意图；

图4为力控制模块的功能示意图；

图5为搜孔单元搜孔路径示意图；

图6为本发明一实施例提供的装配机器人系统的工作流程图；

图7为本发明提供的轴孔装配工业机器人的工作方法的步骤流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清晰，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请参阅图1及图2，图1为本发明提供的轴孔装配工业机器人系统100的结构示意图，图2为本发明实施例提供的工业机器人的功能示意图。轴孔装配工业机器人系统100包括:

工业机器人110、轴孔装配平台120、力觉传感器130、夹持装置140、装配工件150、装配孔160及装配轴170，工业机器人110包括示教模块111、轨迹规划模块112、运动计算模块113、电机驱动模块114及力控制模块115。

其中，工业机器人110与轴孔装配平台120的底座固定在同一基准面上；夹持装置140安装于工业机器人110的手臂末端；装配工件150固定在轴孔装配平台120上，装配工件150上设有装配孔160；力觉传感器130安装在手臂末端与夹持装置140之间，用于实时测量装配轴170与装配孔160间接触力。

本发明中实施例中优选为六自由度工业工业机器人110，其在工作范围内除奇异点外能够自由调节工业机器人110手臂末端在笛卡尔坐标系下的三个轴向位置和绕三个坐标轴转动的姿态。

优选地，力觉传感器130为六维力觉传感器，能够检测笛卡尔坐标系下三个方向的力和绕三个方向的力矩。

其中，工业机器人110通过示教及轨迹规划生成一系列运动轨迹点，以使夹持装置140夹持装配轴170运动至装配孔160的上方；

具体地，示教模块111控制工业机器人110在笛卡尔空间和关节空间下进行点动，直至夹持装置140夹持装配轴170到达装配孔160正上方，并记录关键的示教点；轨迹规划模块112将示教点解析成平滑的样条曲线，再将所条曲线按照时间点离散化成运动轨迹点。

运动计算模块113将运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至电机驱动模块114；

电机驱动模块114驱动工业机器人110运动并带动夹持装置140夹持装配轴170运动至装配孔160的上方；

请参阅图3，图3为电机驱动模块114的功能示意图。电机驱动模块114包括位置环1141、速度环1142和电流环1143，位置环1141输入包括关节空间下工业机器人110各关节转动角度和伺服系统反馈的位置；速度环1142输入包括位置环1141计算出的关节转动速度、伺服系统反馈的转动速度和力控制计算出的速度补偿值；电流环1143输入包括速度环1142计算出的电流值和伺服系统反馈的电流值。

可以理解，通过对工业机器人110进行示教将装配轴170末端定位到装配孔160的正上方，工业机器人110记录示教过程中的关键路径点并通过轨迹规划生成一系列的运动轨迹路径点；工业机器人110在位置控制模式下按照上述的运动轨迹路径点运动至装配孔160的上方。

力控制模块115根据接触力及装配轴170的位置，控制工业机器人110运动使装配轴170与装配孔160以设定好的接触力进行接触，将装配轴170顺利插入至装配孔160中。

请参阅图4，图4为力控制模块115的功能示意图。力控制模块115包括接触单元1151、搜孔单元1152及插入单元1153；

接触单元1151用于将接触力与设定好的接触力做差得出力补偿值，并将所述力补偿值转化为速度补偿值，并将速度补偿值加载到速度环1142输入上，实现接触力的反馈控制；

可以理解，在执行上述过程中，由于示教定位的误差和位置控制的误差可能会导致装配轴170的轴线与装配孔160的中心线不重合的情况发生，本发明对力控制模块115进一步改进，以提高装配精度。

搜孔单元1152用于控制装配轴170沿X方向和Y方向做搜孔运动的位置及控制Z方向的接触力及位置，再由力觉传感器130检测装配轴170与装配工件Z方向的接触力判断是否找到正确的装配孔；

请参阅图5，图5为搜孔单元搜孔路径示意图，其中，数字先后顺序表示其路径的先后顺序。在此阶段工业机器人110控制装配轴170沿笛卡尔坐标系下的X轴与Y轴的轴向方向运动，运动轨迹，同时沿Z轴轴向进行接触力与位置的同时控制，以此方式搜索装配孔160，搜孔过程中保持设定的接触力为恒定值，当接触力突然大幅度减小时，说明装配轴170的轴线与装配孔160的中心线已经基本重合，在微小误差的允许范围内可以进行下一步的插孔运动。

插入单元1153通过力觉传感器130检测装配轴170与装配工件150间产生的XYZ的力矩值判断姿态的偏差并作出相应的姿态补偿，并控制装配轴170的Z向接触力和位置，以检测装配轴170的落底状态。

可以理解，在上一步搜孔单元1152找到正确的装配孔160后即可控制工业机器人110上的装配轴170进行沿Z轴轴向向下的插入运动，但此时可能会由于装配轴170与装配孔160的姿态不一致，而导致装配轴170与装配孔160卡死的现象发生甚至导致装配失败，为避免此情况发生，本发明提供的装配策略可通过插孔单元1132对装配轴170坐标系下的绕三个方向的力矩和姿态进行同时控制，通过力觉传感器130实时检测并反馈绕装配轴170坐标系三个坐标轴的接触力矩，通过对这三个力矩的控制实现了装配轴170与装配孔160的姿态基本一致，此时可以控制装配轴170继续向下运动，力觉传感器130实时检测并反馈沿Z轴方向的接触力，当此接触力突然增大且三个绕方向的力矩很小时说明装配轴已经插入到装配孔的底部，即完成了一次装配任务。

请参阅图6，图6为本发明实施例提供的装配机器人系统的工作流程图。该装配机器人系统先根据示教位置按照位置控制模式以速度v运动至子图a状态；然后在接触力和位置同时控制的方式下继续以速度v向下运动直至装配轴170与装配孔160间接触力达到设定的恒定值f，如子图b所示；然后采用搜孔单元进行搜孔，找到正确的装配孔160，但由于装配轴170与装配孔160的姿态不符可能会出现卡死现象如子图c1，为避免出现c1的情况发生，在找到装配孔160后装配机器人在接触力和位置同时控制的模式下，实时调整装配轴170的位姿，将装配轴170与装配孔160间的姿态扭矩控制在合理的微小范围内的同时控制装配轴170向下运动；当Z方向接触力突然增大且三个方向的姿态扭矩依然处于合理范围内时，说明装配轴170已经插入装配孔160的底部，如子图c2所示，此时完成装配。

请参阅图7，图7为本发明提供的轴孔装配工业机器人的工作方法，包括下述步骤：

步骤A：工业机器人100通过示教及轨迹规划生成一系列运动轨迹点，以使夹持装置140夹持装配轴170运动至装配孔160上方；

具体地，工业机器人110还包括示教模块111和轨迹规划模块112。示教模块111控制工业机器人110在笛卡尔空间和关节空间下进行点动，直至夹持装置140夹持装配轴170到达装配孔160正上方，并记录关键的示教点；轨迹规划模块112将示教点解析成平滑的样条曲线，再将所条曲线按照时间点离散化成运动轨迹点。

步骤B：运动计算模块113将运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至电机驱动模块114；

步骤C：电机驱动模块114驱动工业机器人110运动并带动夹持装置140夹持装配轴170运动至装配孔160上方；

优选地，电机驱动模块114包括位置环1141、速度环1142和电流环1143，位置环1141输入包括关节空间下工业机器人110各关节转动角度和伺服系统反馈的位置；速度环1142输入包括位置环1141计算出的关节转动速度、伺服系统反馈的转动速度和力控制计算出的速度补偿值；电流环1143输入包括速度环1142计算出的电流值和伺服系统反馈的电流值。

步骤D：力控制模块115根据接触力及装配轴170的位置，控制工业机器人110运动使装配轴170与装配孔160以设定好的接触力进行接触，将装配轴170顺利插入至装配孔160中。

具体地，步骤D包括下述步骤：

力控制模块115的接触单元1151将接触力与设定好的接触力做差得出力补偿值，并将力补偿值转化为速度补偿值，并将速度补偿值加载到速度环1142输入上，实现接触力的反馈控制；

力控制模块115的搜孔单元1152控制装配轴170沿X方向和Y方向做搜孔运动的位置及控制Z方向的接触力及位置，再由力觉传感器130检测装配轴170与装配工件150Z方向的接触力判断是否找到正确的装配孔；

力控制模块115的插入单元1153通过力觉传感器130检测装配轴170与装配工件150间产生的XYZ的力矩值判断姿态的偏差并作出相应的姿态补偿，并控制装配轴170的Z向接触力和位置，以检测装配轴170的落底状态。

本发明上述实施例提供的轴孔装配工业机器人系统及其工作方法，所述工业机器人通过示教及轨迹规划生成一系列运动轨迹点，以使所述夹持装置夹持所述装配轴运动至所述装配孔上方；所述运动计算模块将所述运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至所述电机驱动模块；所述电机驱动模块驱动所述工业机器人运动并带动所述夹持装置夹持所述装配轴运动至所述装配孔上方；所述力控制模块根据所述接触力及所述装配轴的位置，控制所述工业机器人运动使所述装配轴与所述装配孔以设定好的接触力进行接触，将所述装配轴顺利插入至所述装配孔中。本发明提供的轴孔装配工业机器人系统能够对接触力大小进行精确的控制，针对轴孔间距小、精度要求高、对轴孔接触力有严格要求的装配作业有明显的优势，避免了因仅位置控制模式下由于位置控制精度和接触力不可控性造成的装配作业失败甚至对装配工件的损坏，解决了某些高精度装配作业下依靠人工装配的低效率低质量等问题，扩大了装配工业机器人的应用领域。

另外，本发明采用上述的轴孔装配工业机器人系统，将力控制融入装配作业中，弥补了传统位置控制在高质量高精度装配作业中的不足。采用位置与接触力同时控制的模式，通过将力觉传感器反馈与接触力设定值的差值转化为速度补偿值，并将该速度补偿值加到速度环的输入，实现了不破坏位置闭环的同时对接触力的闭环控制，装配精度高。

可以理解的是，对于本领域的普通技术人员来说，可以根据本发明的技术构思做出其他各种相应的改变与变形，而所有这些改变与变形都应属于本发明权利要求的保护范围。

Claims (9)

1.一种轴孔装配工业机器人系统，包括：

工业机器人、轴孔装配平台、力觉传感器、夹持装置、装配工件、装配孔及装配轴，所述工业机器人包括运动计算模块、电机驱动模块及力控制模块；

所述工业机器人与所述轴孔装配平台的底座固定在同一基准面上；所述夹持装置安装于所述工业机器人的手臂末端；所述装配工件固定在所述轴孔装配平台上，所述装配工件上设有所述装配孔；所述力觉传感器安装在所述手臂末端与所述夹持装置之间，用于实时测量所述装配轴与所述装配孔间接触力；

所述工业机器人通过示教及轨迹规划生成一系列运动轨迹点，以使所述夹持装置夹持所述装配轴运动至所述装配孔上方；

所述运动计算模块将所述运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至所述电机驱动模块；

所述电机驱动模块驱动所述工业机器人运动并带动所述夹持装置夹持所述装配轴运动至所述装配孔上方；

所述力控制模块根据所述接触力及所述装配轴的位置，控制所述工业机器人运动使所述装配轴与所述装配孔以设定好的接触力进行接触，将所述装配轴顺利插入至所述装配孔中。

2.根据权利要求1所述的轴孔装配工业机器人系统，其特征在于，所述力觉传感器为六维力觉传感器，能够检测笛卡尔坐标系下三个方向的力和绕三个方向的力矩。

3.根据权利要求1所述的轴孔装配工业机器人系统，其特征在于，所述工业机器人还包括示教模块和轨迹规划模块，所述示教模块控制所述工业机器人在笛卡尔空间和关节空间下进行点动，直至所述夹持装置夹持所述装配轴到达所述装配孔正上方，并记录关键的示教点；所述轨迹规划模块将所述示教点解析成平滑的样条曲线，再将所述样条曲线按照时间点离散化成运动轨迹点。

4.根据权利要求1所述的轴孔装配工业机器人系统，其特征在于，所述电机驱动模块包括位置环、速度环和电流环，所述位置环输入包括关节空间下所述工业机器人各关节转动角度和伺服系统反馈的位置；所述速度环输入包括所述位置环计算出的关节转动速度、伺服系统反馈的转动速度和力控制计算出的速度补偿值；所述电流环输入包括速度环计算出的电流值和伺服系统反馈的电流值。

5.根据权利要求4所述的轴孔装配工业机器人系统，其特征在于，所述力控制模块包括接触单元、搜孔单元及插入单元；

所述接触单元用于将所述接触力与设定好的接触力做差得出力补偿值，并将所述力补偿值转化为速度补偿值，并将所述速度补偿值加载到所述速度环输入上，实现所述接触力的反馈控制；

所述搜孔单元用于控制所述装配轴沿X方向和Y方向做搜孔运动的位置及控制Z方向的接触力及位置，再由所述力觉传感器检测所述装配轴与所述装配工件Z方向的接触力判断是否找到正确的装配孔；

插入单元通过所述力觉传感器检测所述装配轴与所述装配工件间产生的XYZ的力矩值判断姿态的偏差并作出相应的姿态补偿，并控制所述装配轴的Z向接触力和位置，以检测所述装配轴的落底状态。

6.一种轴孔装配工业机器人的工作方法，其特征在于，包括下述步骤：

步骤A：所述工业机器人通过示教及轨迹规划生成一系列运动轨迹点，以使所述夹持装置夹持所述装配轴运动至所述装配孔上方；

步骤B：所述运动计算模块将所述运动轨迹点解析成对应时间点的关节空间下各关节角度，并传送至所述电机驱动模块；

步骤C：所述电机驱动模块驱动所述工业机器人运动并带动所述夹持装置夹持所述装配轴运动至所述装配孔上方；

步骤D：所述力控制模块根据所述接触力及所述装配轴的位置，控制所述工业机器人运动使所述装配轴与所述装配孔以设定好的接触力进行接触，将所述装配轴顺利插入至所述装配孔中。

7.根据权利要求6所述的轴孔装配工业机器人系统，其特征在于，所述工业机器人还包括示教模块和轨迹规划模块，所述示教模块控制所述工业机器人在笛卡尔空间和关节空间下进行点动，直至所述夹持装置夹持所述装配轴到达所述装配孔正上方，并记录关键的示教点；所述轨迹规划模块将所述示教点解析成平滑的样条曲线，再将所述样条曲线按照时间点离散化成运动轨迹点。

8.根据权利要求6所述的轴孔装配工业机器人系统，其特征在于，所述电机驱动模块包括位置环、速度环和电流环，所述位置环输入包括关节空间下所述工业机器人各关节转动角度和伺服系统反馈的位置；所述速度环输入包括所述位置环计算出的关节转动速度、伺服系统反馈的转动速度和力控制计算出的速度补偿值；所述电流环输入包括速度环计算出的电流值和伺服系统反馈的电流值。

9.根据权利要求6所述的轴孔装配工业机器人系统，其特征在于，步骤D包括下述步骤：

所述力控制模块将所述接触力与设定好的接触力做差得出力补偿值，并将所述力补偿值转化为速度补偿值，并将所述速度补偿值加载到所述速度环输入上，实现所述接触力的反馈控制；

所述力控制模块控制所述装配轴沿X方向和Y方向做搜孔运动的位置及控制Z方向的接触力及位置，再由所述力觉传感器检测所述装配轴与所述装配工件Z方向的接触力判断是否找到正确的装配孔；

所述力控制模块通过所述力觉传感器检测所述装配轴与所述装配工件间产生的XYZ的力矩值判断姿态的偏差并作出相应的姿态补偿，并控制所述装配轴的Z向接触力和位置，以检测所述装配轴的落底状态。