一种机械臂力矩主动柔顺精确控制方法及系统与流程

1.本发明属于机械臂控制技术领域，涉及一种机械臂力矩主动柔顺精确控制方法及系统。


背景技术：

2.本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
3.工业自动化、智能化发展战略的实施和推进，大力发展机器人技术、促进制造业产业升级，是国家未来工业化发展主攻的方向。机械臂作为机器人领域最具代表性的一类机械设备，被用来代替人类从事劳动强度大、重复性高、安全风险高、操作环境复杂的工作，在工业生产、航工航天、军事医疗等领域被广泛应用。
4.电力巡检机器人是对变电站内的电力屏柜等设备进行巡检的一种智能设备，主要包括车体、机械臂、舵机云台和设置在舵机云台上的检测组件，进行巡检工作时，电力巡检机器人按照预设的巡航路线进行移动，到达预设的点位后，通过机械臂搭载的相机对电力屏柜进行检测与图像采集，并使用机械臂末端工具对电力屏柜指定按键或开关进行操作。但机械臂末端与作业目标接触时会产生较大的接触力，不仅影响作业精度，且易对电力屏柜产生破坏。因此需要通过机械臂的柔顺控制精确控制机械臂末端与作业目标的接触力。
5.柔顺指机械臂对外界环境变化的适应能力。若环境因受到机械臂的作用力而产生形变，机械臂仍能保持期望的接触力与环境相互接触，即为机械臂的柔顺性。柔顺控制可分为主动柔顺控制与被动柔顺控制。机器人凭借一些辅助的柔顺机构，使其在与环境接触时能够对外部作用力产生自然顺从，称为被动柔顺性；机器人利用力的反馈信息采用一定的控制策略去主动控制作用力，称为主动柔顺性。
6.在公开号cn216913821u专利文献中，设计了一种用于电力屏柜操作的机械手指，通过对机械臂末端工具设计来解决机械臂作业的柔顺性，此种方法采用被动柔顺装置进行作业，属于被动柔顺控制，虽然一定程度解决了机械臂对电力屏柜作业的柔顺性问题，但无法根除机器人高刚度与高柔顺性之间的矛盾，且机器人加上被动柔顺装置，其本身并不具备控制能力，给机器人控制带来了极大的困难，尤其在既需要控制作用力又需要严格控制定位的场合中，更为突出。
7.阻抗控制是一种常用的控制末端接触过程的主动柔顺控制算法，其将末端工具的位姿与接触力/力矩之间的关系视为一个弹簧-质量-阻尼系统，可通过测量接触力对末端位姿进行实时修正，在现代机械臂柔顺控制中被广泛应用。传统阻抗控制中，力与位置之间是一对相互矛盾的指标，力控制要求机械臂较低刚性以保证机械臂与环境接触时的柔性，而位置控制则要求机械臂有较高的伺服刚性以保证机械臂具有较高的位置控制精度，因此采用阻抗控制会导致机械臂抖动，精度下降。


技术实现要素：

8.本发明为了解决上述问题，提出了一种机械臂力矩主动柔顺精确控制方法及系统，本发明机械臂先以位置控制模式实现对目标位置处的自主规划作业轨迹及作业姿态，随后以力的阻抗控制模式实现主动柔顺控制，与目标环境进行接触作业，根据机械臂末端的期望位姿与实测位姿差值估计末端受力情况，通过机械臂各关节处六维力传感器计算出机械臂末端与环境的实际接触力，从而判断是否准确碰触被操作对象，随后通过作业工具的力反馈检测作业过程是否正常，并在操作失败或受力方向不符合预期时及时中止并重试，从而提升机械臂自主作业的稳定性，保证机械臂运动过程平稳的同时，精确控制机械臂末端与环境的接触力，满足工业机械臂的生产要求。
9.根据一些实施例，本发明采用如下技术方案：
10.一种机械臂力矩主动柔顺精确控制方法，包括以下步骤：
11.获取目标作业环境信息，将目标作业位置发送给机械臂；
12.以位置控制模式控制机械臂，实现对目标位置处的自主规划作业轨迹及作业姿态；
13.以力的阻抗控制模式实现机械臂末端与目标环境接触作业的主动柔顺控制，根据机械臂末端的期望位姿与实测位姿差值估计末端受力情况，判断是否准确碰触被操作对象，并调节机械臂末端与环境间的接触力。
14.作为可选择的实施方式，以位置控制模式控制机械臂时以机械臂关节的位置、速度和加速度为期望，对机械臂的期望轨迹进行跟踪。
15.作为可选择的实施方式，以位置控制模式控制机械臂的具体过程包括采用三次样条插补法，在起始点与目标作业位置之间形成带速度、加速度和时间信息的一系列路径点，求解运动学方程，进行机械臂逆运动学分析，根据轨迹规划产生的路径点信息，求出各个路径点机械臂各个关节的运动角度，通过机械臂各关节运动使机械臂末端通过上述一系列路径点准确到达目标位置。
16.作为可选择的实施方式，以力的阻抗控制模式实现机械臂末端与目标环境接触作业的主动柔顺控制的过程中，内部回路采用pid方法进行关节驱动力矩闭环控制，外部回路将轨迹偏差转化为力偏差。
17.作为可选择的实施方式，将机械臂末端实际轨迹与期望轨迹的偏差转化为关节驱动来调节机械臂末端与环境间的接触力。
18.作为可选择的实施方式，在机械臂运动控制过程中，实时进行碰撞检测，若因作业环境改变使机械臂碰到人或设备，根据机械臂力的反馈使机械臂立刻停止作业。
19.一种机械臂力矩主动柔顺精确控制系统，包括：
20.检测单元，被配置为获取目标作业环境信息，将目标作业位置发送给机械臂；
21.位置控制单元，被配置为以位置控制模式控制机械臂，实现对目标位置处的自主规划作业轨迹及作业姿态；
22.阻抗控制单元，被配置为以力的阻抗控制模式实现机械臂末端与目标环境接触作业的主动柔顺控制，根据机械臂末端的期望位姿与实测位姿差值估计末端受力情况，判断是否准确碰触被操作对象，并调节机械臂末端与环境间的接触力。
23.作为可选择的实施方式，所述检测单元包括布设在机械臂末端的深度相机、六维
力传感器，所述深度相机用于识别作业目标的特征点，获得机械臂对作业目标位置信息，所述六维力传感器，用于获取机械臂各关节力矩，以计算机械臂末端与目标环境接触作业的接触力。
24.作为可选择的实施方式，所述控制系统和机械臂通讯，所述控制系统采用点对点的分布式通信机制。
25.作为可选择的实施方式，所述控制系统还包括机械臂运动学编译器，用于求解运动学方程，进行机械臂逆运动学分析，根据轨迹规划产生的路径点信息，求出各个路径点机械臂各个关节的运动角度。
26.一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行所述方法中的步骤。
27.一种终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行所述的方法中的步骤。
28.一种机器人，执行所述的方法中的步骤，包括上述系统。
29.与现有技术相比，本发明的有益效果为：
30.本发明创新性提出一种机械臂力矩主动柔顺精确控制方法，将位置控制模式与阻抗控制模式结合，通过视觉信息处理系统获取目标作业环境信息，将目标作业位置发送给机械臂，对机械臂末端进行运动控制，通过机械臂轨迹规划及控制算法代码，机械臂先以位置控制模式实现对目标位置处的自主规划作业轨迹及作业姿态，随后以力的阻抗控制模式实现主动柔顺控制，与目标环境进行接触作业，根据机械臂末端的期望位姿与实测位姿差值估计末端受力情况，进行实施调整。还能够作业工具的力反馈检测作业过程是否正常，并在操作失败或受力方向不符合预期时及时中止并重试，从而提升机械臂自主作业的稳定性，保证机械臂运动过程平稳的同时，精确控制机械臂末端与环境的接触力。
31.本发明创新性提出一种机械臂力矩主动柔顺精确控制系统，基于ros运动规划执行系统，通过机械臂各关节处六维力传感器计算出与环境的实际接触力，将机械臂末端实际轨迹与期望轨迹的偏差转化为关节驱动来调节机械臂末端与环境间的接触力；规避了机械臂高刚度与高柔顺性之间的矛盾，解决了被动柔顺控制专用性强，适应能力差，使用范围受到限制的问题，实现了机械臂的作业位置精度与接触力控制的兼顾。
附图说明
32.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
33.图1是机械臂控制系统架构；
34.图2是机械臂运动轨迹规划流程图；
35.图3是机械臂运动轨迹规划验证流程图；
36.图4是基于位置的柔顺控制示意图。
具体实施方式
37.下面结合附图与实施例对本发明作进一步说明。
38.应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
39.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
40.本发明提供一种机械臂与作业环境接触过程中的机械臂力矩主动柔顺精确控制方法，通过视觉信息处理系统获取环境信息，将目标作业位置发送给机械臂，通过机械臂轨迹规划及控制算法代码，提升机械臂自主作业的稳定性，实现机械臂对目标位置处的自主规划作业轨迹及作业姿态。
41.本发明采用moveit！完成机械臂的规划与控制，使用ik-fast实现逆运动学求解，使用ompl(open motion planning library)进行运动轨迹规划，使用fcl(flexible collision library)实现碰撞检测与预测，使用trajectory processing routine生成满足关节速度和加速度的限制的运动轨迹，使用机械臂的阻抗控制实现主动柔顺控制，使用机械臂末端的期望位姿与实测位姿差值估计末端受力情况，从而判断是否准确碰触被操作对象，随后通过作业工具的力反馈检测作业过程是否正常，并在操作失败或受力方向不符合预期时及时中止并重试。
42.如图1所示，本发明所述机械臂力矩主动柔顺精确控制方法及系统主要由视觉信息定位系统与运动规划执行系统组成。机械臂视觉信息定位系统主要由深度相机构成，用于识别作业目标的特征点，获得机械臂对作业目标位置信息。
43.运动规划执行系统由工控机及机械臂组成，工控机采用ros机器人操作系统，采用点对点的分布式通信机制，实现模块间点对点的松耦合连接，利用其最核心的通信机制话题(topic)通信机制、服务(service)通信机制、参数(parameter)管理机制，实现运动规划执行系统与视觉定位系统及动规划执行系统内部的通信服务；moveit！是ros中一系列移动操作的功能包的组成，以move_group为核心节点，集成了各种组件，主要包含运动规划、碰撞检测、运动学、3d感知、操作控制等功能，为现有技术，在此不再赘述。
44.本发明以机械臂按压电力屏柜按钮为例，详细介绍本发明技术方案。机械臂视觉信息定位系统识别电力屏柜按键的特征点，获得电力屏柜按钮位置信息，发送至机械臂控制接口(manipulator_controller)，机械臂收到电力屏柜按钮位置信息，在该位置前确定一点，并进行轨迹规划，通过位置控制模式将机械臂末端以预先设定好的姿态运动至该位置。
45.位置控制模式指以机械臂关节的位置、速度和加速度为期望，对机械臂的期望轨迹进行跟踪的控制模式。轨迹规划指形成带速度、加速度等时间信息的一系列路径点。这些路径点以关节空间的角度表示，使用轨迹规划算法，将之变成连续光滑的曲线。
46.当机械臂控制接口收到目标位置信息后，如图2所示，利用ompl(open motion planning library)进行运动轨迹规划，采用三次样条插补法，在起始点与目标点之间形成带速度、加速度等时间信息的一系列路径点，路径点信息通过消息队列的方式发布至“/joint_states”话题，机械臂运动节点从“/joint_states”话题接收到路径点信息，通过机
械臂运动学编译器ikfast求解运动学方程，进行机械臂逆运动学分析，根据轨迹规划产生的路径点信息，求出各个路径点机械臂六个关节的运动角度，通过机械臂各关节运动使机械臂末端通过一个个路径点准确到达目标位置。
47.如图3所示，本发明通过xmate7_controller节点通过move_group_interface接口类与moveit！核心进行交互，控制机械臂运动，首先初始化规划组，设置参考坐标系，设置运动误差，设置极限参数，如果已经接收道运动指令，判断是否是笛卡尔空间运动，如果是，则开始笛卡尔空间规划，如果不是，则进行轴空间规划，如果规划成功，则开始运动，否则，重新开始规划过程。
48.机械臂结构是由模块化关节和连杆以及末端工具所组成，因而机械臂具有物理结构的特点，机械臂在实际工作过程中含有运动学限制以及由于运动产生的惯性，当机械臂与环境相接触时，可以使用阻抗描述机械臂的特性。
49.本发明采用基于力的阻抗控制模式，内部回路是采用pid方法进行关节驱动力矩闭环控制，外部回路通过阻抗控制器将轨迹偏差转化为力偏差，如图4所示。
50.分别表示机械臂末端实际运动位置、速度和加速度，分别表示机械臂末端期望位置、期望速度和期望加速度，md、bd分别为期望惯量矩阵、期望阻尼矩阵和期望刚度矩阵。机械臂末端实际轨迹与期望轨迹偏差经由阻抗控制器转换为机械臂末端与环境之间期望接触力fr，内部控制回路中关节力矩进行pid控制，使机械臂末端作与环境之间的接触力跟随期望接触力。基于力的柔顺控制本质就是将机械臂末端实际轨迹与期望轨迹的偏差转化为关节驱动来调节机械臂末端与环境间的接触力，机械臂末端期望接触力计算公式：
[0051][0052]
图4中，fe指的是在机械臂运动过程中由安装在末端的六维力传感器检测到的与环境的实际接触力，和分别为机械臂关节角度、关节角速度和关节角加速度，进而可以通过正运动学以及雅克比矩阵求出机械臂末端位置、末端速度和末端加速度。
[0053]
当机械臂运动至电力屏柜按钮前某一点后，机械臂控制模式由位置控制模式切换至阻抗控制模式，根据坐标位置信息，机械臂进行末端的轨迹规划，使机械臂末端工具以预设的力触碰电力屏柜按键，当接收到反馈力大于预设值时，机械臂末端撤回，远离屏柜按键。
[0054]
并且在机械臂运动的整个过程中使用fcl(flexible collision library)实现碰撞检测与预测，若因作业环境改变使机械臂碰到人或设备，根据机械臂力的反馈使机械臂立刻停止作业，保护人身和设备安全。
[0055]
本发明的使用场景并不仅限于电力屏柜的作业过程。
[0056]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0057]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程
图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0058]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0059]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0060]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
[0061]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。