CN117656080A - 一种未知表面自适应恒压力跟踪系统及其控制方法 - Google Patents

Abstract

本发明涉及一种未知表面自适应恒压力跟踪系统及其控制方法，该系统包括工业机器人和位置自适应执行器，所述位置自适应执行器一端安装于所述工业机器人末端，另一端连接加工工具并通过加工工具与待跟踪未知表面接触；所述位置自适应执行器包括直线移动关节，所述直线移动关节包括电机、丝杆机构、力传感器、姿态传感器和控制器，所述控制器基于力传感器和姿态传感器的测量数据计算实际接触力，并根据其与设定的目标接触力的差值控制电机工作，驱动丝杆机构伸缩运动，以使施加于待跟踪未知表面的力恒定不变。该系统通过工业机器人的位姿调整与执行器的一维力控即可实现三维的未知表面自适应恒压力跟踪，结构简单，易于实现，操作简单，控制灵活。

Description

一种未知表面自适应恒压力跟踪系统及其控制方法

技术领域

本发明涉及自动化设备及机器人技术领域，具体涉及一种未知表面自适应恒压力跟踪系统及其控制方法。

背景技术

随着制造业自动化技术的不断发展，在利用机器人进行精密加工、装配操作等接触式作业中，如何实现与未知形状表面的恒压力接触是关键的技术难题。

基于机器人本体进行恒力控制，是通过机器人的关节运动进行位姿变换来调整输出力，机器人整体操作复杂，与加工表面的接触过程为刚性接触，容易对加工表面造成损坏，且机器人自身存在位置误差，力控精度不足，响应速度缓慢；开发独立于机器人本体的末端执行器进行恒力控制已成为主流方式，机器人的力控制回路和位置控制回路解耦，末端执行器基于设定的接触力自动伸缩，保证加工工具始终贴合待跟踪未知表面轮廓实现自适应恒力接触，能有效解决刚性接触的问题，降低操作复杂度、提高力控响应速度与力控精度。

按照驱动方式分类，位置自适应恒力末端执行器主要分为机械式、气动式和电动式。机械式末端执行器利用弹簧等器件实现输出力被动柔顺控制，专利CN115946043A公开了一种机器人被动柔顺恒力打磨加工装置，其利用柔性铰链、折叠叶片产生的弹性形变以适应加工表面形状变化，保持输出力恒定，但不能主动调整输出力大小，可控性较差。气动式末端执行器通过调节气压实现输出力控制，专利CN116460718A公开了一种磨抛机器人力控浮动恒力末端执行机构及其使用方法，专利CN112238396A公开了一种紧凑型高精度恒力执行器及应用方法，专利CN111216162A公开了一种应用于工业机器人末端的恒力浮动装置，其均以气缸作为驱动元件，通过调节气缸内气体的压力实现输出力控制，控制简单且柔性好，但其相关配件过多，需要气压机、气动三联件等配套设施，占用体积大、集成度低，且对微小的力偏差响应迟滞、力控精度低。电动式末端执行器的优越性则可以解决上述问题，其具备较高的集成度，发展潜力最大。专利CN115847286A公开了一种面向复杂曲面加工的电驱高精度力控抛磨装置，专利CN113400143B公开了一种机器人力控末端执行器及其控制方法，其均以音圈电机为驱动元件，通过调节音圈电机输出力实现恒力控制，方案结构简单紧凑，响应速度快，但该类末端执行器的核心元件音圈电机成本高、推力小、体积和重量大，存在易振动、发热量大等缺点，难以在市场上大范围应用。

发明内容

本发明的目的在于提供一种未知表面自适应恒压力跟踪系统及其控制方法，该系统通过工业机器人的位姿调整与执行器的一维力控即可实现三维的未知表面自适应恒压力跟踪，结构简单，易于实现，操作简单，控制灵活。

为了实现上述目的，本发明采用的技术方案是：一种未知表面自适应恒压力跟踪系统，包括工业机器人和位置自适应执行器，所述位置自适应执行器一端安装于所述工业机器人末端，另一端连接加工工具并通过加工工具与待跟踪未知表面接触；所述位置自适应执行器包括直线移动关节，所述直线移动关节包括电机、丝杆机构、力传感器、姿态传感器和控制器，所述控制器基于力传感器和姿态传感器的测量数据计算实际接触力，并根据其与设定的目标接触力的差值控制电机工作，驱动丝杆机构伸缩运动，以使施加于待跟踪未知表面的力恒定不变。

进一步地，所述工业机器人为6关节机器人，所述6关节机器人包括主要用于改变机器人末端位置的3个关节和主要用于调整机器人末端姿态的3个关节。

进一步地，所述位置自适应执行器还包括基本连接组件，所述基本连接组件包括机器人端连接法兰、上安装板、滑块扩展板、下安装板和工具端连接法兰，所述机器人端连接法兰与工业机器人末端固定连接，所述上安装板与机器人端连接法兰固定连接，所述电机、丝杆机构与上安装板固定连接，所述丝杆机构的滑块与滑块扩展板及下安装板固定连接，所述下安装板经力传感器与工具端连接法兰固定连接，所述工具端连接法兰与加工工具固定连接。

进一步地，所述丝杆机构包括滚珠丝杆、丝杆螺母、滑块、导轨以及导轨支架，所述滚珠丝杆与电机转子相连为一体作为电机出轴，所述丝杆螺母与滚珠丝杆配合并安装于滑块内部，所述滑块包括固定连接在一起的滑块螺母和滑动导块，所述滑块螺母与滑块扩展板固定连接，所述滑动导块在导轨上滑动，所述导轨安装于导轨支架上，所述导轨支架与上安装板固定连接。

进一步地，所述导轨支架的上、下两端分别连接有上限位挡板、下限位挡板，以对滑块的行程进行限位。

进一步地，所述控制器安装于上安装板上，所述控制器通过有线或无线方式与上位机进行通讯，所述控制器用于传递信号、处理数据并进行恒压力控制，所述上位机用于提供位置自适应执行器的控制功能、控制参数配置功能以及传感器数据信息显示功能；所述滑块扩展板上安装有位移传感器，所述姿态传感器安装在控制器上，所述力传感器、姿态传感器和位移传感器均与控制器电性连接，所述力传感器用于获取位置自适应执行器的输出力信息，所述姿态传感器用于获取位置自适应执行器的姿态信息，所述位移传感器用于获取滑块的实时位移信息。

进一步地，所述力传感器与工具端连接法兰之间连接有阻尼垫，用于隔离力传感器与工具端连接法兰的振动，减少位置自适应执行器与待跟踪未知表面接触时的刚性冲击。

进一步地，所述机器人端连接法兰为空心壳结构，所述空心壳结构侧面安装有若干航空插座，以作为电源口或数据通讯口；所述机器人端连接法兰与工具端连接法兰之间安装有可伸缩塑胶防尘罩。

本发明还提供了上述未知表面自适应恒压力跟踪系统的控制方法，包括如下步骤：

S1、在未开始作业时，所述位置自适应执行器保持竖直的初始姿态，通过力传感器获取加工工具的重力G；

S2、通过姿态传感器获取当前位置自适应执行器的姿态信息，计算出位置自适应执行器的中心轴线与水平方向的夹角θ，通过夹角求得加工工具在位置自适应执行器轴线方向的重力分量，实际接触力F为力传感器获取的力值F_o与加工工具重力分量的差值，即F＝F_o-Gsinθ；

S3、判断位置自适应执行器是否开启恒力模式；在非恒力模式下，当实际接触力发生变化时，位置自适应执行器不进行输出力调整，不驱动电机旋转，仅按照设定的上发周期向上位机发送传感器采样信息；当位置自适应执行器开启恒力模式，则执行步骤S4；

S4、将实际接触力F与设定的目标接触力F_d输入控制器，当实际接触力发生变化时，控制器根据实际接触力F与目标接触力F_d的差值ΔF_d进行力/位转换，控制电机旋转，使丝杆机构伸缩ΔX，进行力的补偿，使得实际接触力F趋向目标接触力F_d，并按照设定的上发周期向上位机发送传感器采样信息。

与现有技术相比，本发明具有以下有益效果：本发明提供了一种未知表面自适应恒压力跟踪系统及其控制方法，该系统在工业机器人末端安装基于电机驱动及控制器控制的位置自适应执行器，通过力传感器和姿态传感器的测量数据计算实际接触力并与目标接触力比较，驱动执行器伸缩运动，从而保证施加于待跟踪未知表面的力恒定不变；该系统可适配包括6关节机器人等不同类型的工业机器人，并连接不同的加工工具，通用性强，且结构简单，易于实现，操作简单，控制灵活，工业机器人负责系统的位置控制，按照预先规划的跟踪路径变换位置自适应执行器的位姿，位置自适应执行器负责系统的力控制，基于设定的输出力自动伸缩，保证加工工具始终贴合加工工件表面轮廓实现自适应恒力接触；在面对复杂的未知表面时，机器人无需对跟踪轨迹进行精确示教就能够自动进行位置补偿，弥补位置误差，贴合各类表面，减少了操作的复杂性，提升位置自适应执行器对未知环境的适应能力，通过对工业机器人的位置和姿态调整与对位置自适应执行器的一维力控便可灵活实现三维度的未知表面自适应恒压力跟踪。

附图说明

图1为本发明实施例的系统整体结构示意图；

图2为本发明实施例中位置自适应执行器的外观示意图；

图3为本发明实施例中位置自适应执行器的内部结构示意图之一；

图4为图3中以中心轴线为基准的剖面示意图；

图5为本发明实施例中位置自适应执行器的内部结构示意图之二；

图6为本发明实施例中丝杆机构、滑块与导轨的连接结构示意图；

图7为本发明实施例的控制方法的控制流程图。

图中：

100、6关节机器人；200、位置自适应执行器；300、待跟踪未知表面；400、磨抛工具；11、电机；13、力传感器；14、控制器；121、滚珠丝杆；122、丝杆螺母；123、滑块；124、导轨；125、导轨支架；126、上限位挡板；127、下限位挡板；1231、滑块螺母；1232、滑动导块；21、机器人端连接法兰；22、上安装板；23、滑块扩展板；24、下安装板；25、工具端连接法兰；26、姿态传感器；27、位移传感器；28、阻尼垫；29、防尘罩；211、电源口；212、数据通讯口。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

如图1-6所示，本实施例提供了一种未知表面自适应恒压力跟踪系统，包括工业机器人100和位置自适应执行器200，所述位置自适应执行器200一端安装于所述工业机器人100末端，另一端连接加工工具400并通过加工工具与待跟踪未知表面300接触；所述位置自适应执行器200包括直线移动关节，所述直线移动关节包括电机11、丝杆机构、力传感器13、姿态传感器26和控制器14，所述控制器14基于力传感器13和姿态传感器26的测量数据计算实际接触力，并根据其与设定的目标接触力的差值控制电机11工作，驱动丝杆机构伸缩运动，以使施加于待跟踪未知表面300的力恒定不变。

所述加工工具400可以为磨抛工具、螺栓装配工具、擦胶工具等不同类型的加工工具。所述工业机器人100可根据实际情况采用不同类型的工业机器人。在本实施例中，该系统应用于曲面或者非曲面零件表面研磨抛光的场景中，加工工具400采用磨抛工具，工业机器人100为6关节机器人，6关节机器人包括主要用于改变机器人末端位置的3个关节和主要用于调整机器人末端姿态的3个关节。6关节机器人100负责磨抛系统的位置控制，按照预先规划的磨抛路径变换位置自适应执行器200的位姿，位置自适应执行器200负责磨抛系统的力控制，基于设定的磨抛力自动伸缩，保证磨抛工具400始终贴合待跟踪未知表面300，实现自适应恒力接触。

所述位置自适应执行器200还包括基本连接组件，所述基本连接组件包括机器人端连接法兰21、上安装板22、滑块扩展板23、下安装板24和工具端连接法兰25，所述机器人端连接法兰21与工业机器人100末端固定连接，所述上安装板22与机器人端连接法兰21固定连接，所述电机11、丝杆机构与上安装板22固定连接，所述丝杆机构的滑块与滑块扩展板23及下安装板24固定连接，所述下安装板24经力传感器13与工具端连接法兰25固定连接，所述工具端连接法兰25与加工工具400固定连接。

所述丝杆机构包括滚珠丝杆121、丝杆螺母122、滑块123、导轨124以及导轨支架125，所述滚珠丝杆121与电机11转子相连为一体作为电机出轴，所述丝杆螺母122与滚珠丝杆121配合连接并安装于滑块123内部，所述滑块123包括固定连接在一起的滑块螺母1231和滑动导块1232，所述滑块螺母1231与滑块扩展板23固定连接，所述滑动导块1232在导轨124上滑动，所述导轨124安装于导轨支架125上，所述导轨支架125与上安装板22固定连接。所述导轨支架125的上、下两端分别连接有上限位挡板126、下限位挡板127，以对滑块123的行程进行限位。

所述电机11为步进电机，所述电机11作为动力输出元件驱动位置自适应执行器200动作，所述丝杆螺母122用于将电机11的旋转运动转换为位置自适应执行器200的直线运动。

所述导轨124主要负责位置自适应执行器200的轴向导向，此外导轨124还需要承受水平负载，防止滚珠丝杆121因受力而出现弯曲损坏；如图6所示，所述滑块123的运动配合关系被分成两部分，上半部分是滑块螺母1231与滚珠丝杆121相互配合，下半部分是滑动导块1232与导轨124相互配合，该配合关系可以确保导轨124起到轴向导向及承受水平负载的作用。

所述导轨支架125由4015铝型材制成，铝型材凹槽放置滑块螺母1231；所述上安装板22，用于承担机器人端连接法兰21和导轨支架125之间的受力，防止电机11直接作为中间连接件在工作过程中因受力而损坏。

机器人端连接法兰21、上安装板22、导轨支架125、电机11、导轨124等在工作模式中不会随着接触力的变化而移动，属于固定端；下安装板24、力传感器13、滑块扩展板23、滑块123、阻尼垫28相互连接成一个独立整体，在磨抛时会随着接触力的变化而上下运动，属于运动端。

所述控制器14安装于上安装板上，所述控制器14通过有线或无线方式与上位机进行通讯，所述控制器用于传递信号、处理数据并进行恒压力控制，所述上位机用于提供位置自适应执行器的控制功能、控制参数配置功能以及传感器数据信息显示功能。

所述滑块扩展板23上安装有位移传感器27，所述姿态传感器26安装在控制器14上，所述力传感器13、姿态传感器26和位移传感器27均与控制器14电性连接，所述力传感器13用于获取位置自适应执行器200的输出力信息；所述姿态传感器26用于获取位置自适应执行器200的姿态信息，当位置自适应执行器200随6关节机器人100动作时，姿态会发生改变，通过姿态信息的采集，位置自适应执行器200可以判断磨抛工具400和水平面的夹角，从而进行重力补偿；所述位移传感器用于获取滑块的实时位移信息，防止位置自适应执行器200在行程极限位置时继续驱动电机11旋转。当位置自适应执行器200运动到行程极限位置时，若力传感器13采集的接触力仍旧没有达到目标接触力，控制器14就会驱动电机11继续旋转，导致出现电机堵转的现象，长时间的堵转会造成电机发热、能量损耗以及电机损坏等不良影响；通过位移传感器27采集滑块123的实时位移信息，当滑块123移动到极限位置时，停止电机11旋转，防止位置自适应执行器200在极限位置持续工作。

所述力传感器13与工具端连接法兰25之间连接有阻尼垫28，用于隔离力传感器13与工具端连接法兰25的振动，减少位置自适应执行器200与待跟踪未知表面300接触时的刚性冲击。阻尼垫28由硅胶制成，其邵氏硬度为30，成型工艺采用3D打印，可通过调节连接螺钉进行预紧。

所述机器人端连接法兰21做成空心壳结构，空心壳结构侧面安装有两个GX16航空插座，两个航空插座分别用作电源口211以及数据通讯口212，位置自适应执行器200通过航空插座与上位机通讯、外接电源线即可独立工作。所述机器人端连接法兰21与所述工具端连接法兰25之间安装有可伸缩塑胶防尘罩29，通过金属卡箍固定，防止工作中的环境粉尘影响位置自适应执行器200性能。

所述机器人端连接法兰21与工具端连接法兰25均分布有螺纹孔，位置自适应执行器200可以通过机器人端连接法兰21安装在不同型号的工业机器人末端，包括但不限于6关节机器人，工具端连接法兰25可以根据实际磨抛场景采用不同的磨抛工具。

本实施例中，所述上位机的人机交互界面分为基本操作模块、数据显示模块、参数配置模块；基本操作模块用于位置自适应执行器200设备的识别与添加，对位置自适应执行器200进行基本控制；数据显示模块用于实时接收控制器14上发的传感器采样信息并进行相应的处理，通过曲线图的方式显示实际接触力信息、倾角信息以及位移信息；参数配置模块可以对控制器14的参数进行查询与配置，主要用于设置位置自适应执行器200的通用控制参数、电机11控制参数以及恒压力控制的相关参数。

本实施例还提供了上述未知表面自适应恒压力跟踪系统的控制方法，包括如下步骤：

S1、在未开始作业时，将磨抛工具400连接在工具端连接法兰25上，位置自适应执行器200保持竖直即工具端连接法兰25平行于地平面的初始姿态，通过力传感器13获取磨抛工具400的重力G。

S2、在进行加工作业时，位置自适应执行器200随6关节机器人100动作，姿态发生改变。如图7所示，通过姿态传感器26获取当前位置自适应执行器200的姿态信息，计算出位置自适应执行器200的中心轴线与水平方向的夹角θ，通过夹角求得该姿态下磨抛工具400在位置自适应执行器200轴线方向的重力分量，实际接触力F为力传感器13获取的力值F_o与磨抛工具400重力分量的差值，即F＝F_o-G sinθ。

S3、判断位置自适应执行器是否开启恒力模式；在非恒力模式下，当实际接触力发生变化时，位置自适应执行器200不进行输出力调整，不驱动电机旋转，实际接触力会随着待跟踪未知表面300的起伏发生波动，仅按照设定的上发周期向上位机发送传感器采样信息。通过该模式，用户可以更加方便的检验位置自适应执行器200上发的数据是否准确，从而进行故障排查。当位置自适应执行器200开启恒力模式，则执行步骤S4。

S4、将实际接触力F与设定的目标接触力F_d输入控制器，当实际接触力发生变化时，控制器根据实际接触力F与目标接触力F_d的差值ΔF_d进行力/位转换，控制电机11旋转相应的角度，从而使丝杆机构伸缩ΔX，即调整位置自适应执行器200伸缩位移量ΔX，进行力的补偿，使得实际接触力F趋向目标接触力F_d，并按照设定的上发周期向上位机发送传感器采样信息。可以通过分析这些数据信息，进一步优化恒压力控制的相关参数，提高力控精度与动态响应性能。

本发明提供了一种未知表面自适应恒压力跟踪系统及其控制方法，本发明具备一定的通用性，位置自适应执行器可以通过机器人端连接法兰安装在不同型号的工业机器人末端，包括但不限于6关节机器人，工具端连接法兰可以根据实际需求连接不同的加工工具，例如磨抛工具、螺栓装配工具或者擦胶工具，拓展了位置自适应执行器的应用场景。本发明提供的6关节机器人负责系统的位置控制，按照预先规划的跟踪路径变换位置自适应执行器的位姿，位置自适应执行器负责系统的力控制，基于设定的输出力自动伸缩，保证所述加工工具始终贴合加工工件表面轮廓实现自适应恒力接触；在面对复杂的未知表面时，机器人无需对跟踪轨迹进行精确示教就能够自动进行位置补偿，弥补位置误差，贴合各类表面，减少了操作的复杂性，提升位置自适应执行器对未知环境的适应能力。本发明提供的系统通过对6关节机器人进行六自由度的位置和姿态调整与对位置自适应执行器的一维力控便可灵活实现三维度的未知表面自适应恒压力跟踪。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其他形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (9)

1.一种未知表面自适应恒压力跟踪系统，其特征在于，包括工业机器人和位置自适应执行器，所述位置自适应执行器一端安装于所述工业机器人末端，另一端连接加工工具并通过加工工具与待跟踪未知表面接触；所述位置自适应执行器包括直线移动关节，所述直线移动关节包括电机、丝杆机构、力传感器、姿态传感器和控制器，所述控制器基于力传感器和姿态传感器的测量数据计算实际接触力，并根据其与设定的目标接触力的差值控制电机工作，驱动丝杆机构伸缩运动，以使施加于待跟踪未知表面的力恒定不变。

2.根据权利要求1所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述工业机器人为6关节机器人，所述6关节机器人包括主要用于改变机器人末端位置的3个关节和主要用于调整机器人末端姿态的3个关节。

3.根据权利要求1所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述位置自适应执行器还包括基本连接组件，所述基本连接组件包括机器人端连接法兰、上安装板、滑块扩展板、下安装板和工具端连接法兰，所述机器人端连接法兰与工业机器人末端固定连接，所述上安装板与机器人端连接法兰固定连接，所述电机、丝杆机构与上安装板固定连接，所述丝杆机构的滑块与滑块扩展板及下安装板固定连接，所述下安装板经力传感器与工具端连接法兰固定连接，所述工具端连接法兰与加工工具固定连接。

4.根据权利要求1所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述丝杆机构包括滚珠丝杆、丝杆螺母、滑块、导轨以及导轨支架，所述滚珠丝杆与电机转子相连为一体作为电机出轴，所述丝杆螺母与滚珠丝杆配合并安装于滑块内部，所述滑块包括固定连接在一起的滑块螺母和滑动导块，所述滑块螺母与滑块扩展板固定连接，所述滑动导块在导轨上滑动，所述导轨安装于导轨支架上，所述导轨支架与上安装板固定连接。

5.根据权利要求4所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述导轨支架的上、下两端分别连接有上限位挡板、下限位挡板，以对滑块的行程进行限位。

6.根据权利要求1所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述控制器安装于上安装板上，所述控制器通过有线或无线方式与上位机进行通讯，所述控制器用于传递信号、处理数据并进行恒压力控制，所述上位机用于提供位置自适应执行器的控制功能、控制参数配置功能以及传感器数据信息显示功能；所述滑块扩展板上安装有位移传感器，所述姿态传感器安装在控制器上，所述力传感器、姿态传感器和位移传感器均与控制器电性连接，所述力传感器用于获取位置自适应执行器的输出力信息，所述姿态传感器用于获取位置自适应执行器的姿态信息，所述位移传感器用于获取滑块的实时位移信息。

7.根据权利要求3所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述力传感器与工具端连接法兰之间连接有阻尼垫，用于隔离力传感器与工具端连接法兰的振动，减少位置自适应执行器与待跟踪未知表面接触时的刚性冲击。

8.根据权利要求3所述的一种未知表面自适应恒压力跟踪系统，其特征在于，所述机器人端连接法兰为空心壳结构，所述空心壳结构侧面安装有若干航空插座，以作为电源口或数据通讯口；所述机器人端连接法兰与工具端连接法兰之间安装有可伸缩塑胶防尘罩。

9.根据权利要求1-8任一项所述的一种未知表面自适应恒压力跟踪系统的控制方法，其特征在于，包括如下步骤：

S1、在未开始作业时，所述位置自适应执行器保持竖直的初始姿态，通过力传感器获取加工工具的重力G；

S2、通过姿态传感器获取当前位置自适应执行器的姿态信息，计算出位置自适应执行器的中心轴线与水平方向的夹角θ，通过夹角求得加工工具在位置自适应执行器轴线方向的重力分量，实际接触力F为力传感器获取的力值F_o与加工工具重力分量的差值，即F＝F_o-Gsinθ；

S3、判断位置自适应执行器是否开启恒力模式；在非恒力模式下，当实际接触力发生变化时，位置自适应执行器不进行输出力调整，不驱动电机旋转，仅按照设定的上发周期向上位机发送传感器采样信息；当位置自适应执行器开启恒力模式，则执行步骤S4；

S4、将实际接触力F与设定的目标接触力F_d输入控制器，当实际接触力发生变化时，控制器根据实际接触力F与目标接触力F_d的差值ΔF_d进行力/位转换，控制电机旋转，使丝杆机构伸缩ΔX，进行力的补偿，使得实际接触力F趋向目标接触力F_d，并按照设定的上发周期向上位机发送传感器采样信息。