WO2022166329A1

WO2022166329A1 - Human-robot collaboration control method and device for robot, and robot

Info

Publication number: WO2022166329A1
Application number: PCT/CN2021/132999
Authority: WO
Inventors: 曾献文; 刘益彰; 陈金亮; 张美辉; 熊友军
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2021-02-07
Filing date: 2021-11-25
Publication date: 2022-08-11
Also published as: CN112936278A; CN112936278B

Abstract

A human-robot collaboration control method for a robot. The method is applied to the robot comprising a plurality of robotic arms. Each robotic arm end is provided with respective control modes in different directions of motion, and an admittance force control mode is provided in a normal direction of a load contact surface. The method comprises: when an actual contact force of each robotic arm end in the normal direction is controlled, according to the admittance force control mode, to reach a desired acting force, detecting an external force applied by the outside; and on the basis of an external force component applied to each robotic arm end in each direction of motion, calculating a position offset in a corresponding direction of motion according to the control mode in the corresponding direction, the position offsets in all directions of motion being used for collectively responding to a load following operation. According to the method, when the plurality of robotic arms collectively clamp a load, the load can move along with the hands and stay at a required position, thereby realizing human-robot collaboration. Also disclosed are a human-robot collaboration control device for the robot, the robot, and a readable storage medium.

Description

机器人的人机协作控制方法、装置和机器人Human-robot cooperative control method, device and robot for robot

相关申请的交叉引用CROSS-REFERENCE TO RELATED APPLICATIONS

本申请要求于2021年02月07日提交中国专利局的申请号为2021101792493、名称为“机器人的人机协作控制方法、装置和机器人”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。This application claims the priority of the Chinese Patent Application No. 2021101792493 and entitled "Human-Robot Collaborative Control Method, Device and Robot for Robots" filed with the China Patent Office on February 7, 2021, the entire contents of which are incorporated by reference in in this application.

技术领域technical field

本申请涉及机器人控制技术领域，尤其涉及一种机器人的人机协作控制方法、装置和机器人。The present application relates to the technical field of robot control, and in particular, to a man-machine cooperative control method, device and robot of a robot.

背景技术Background technique

通常地，人形机器人作为服务机器人出现在生产生活场景中时，不可避免的存在人机交互、协同作业的场景。例如小朋友做手工时，机器人进行辅助，如双手夹持拿起架子上的手工材料盒，并移动到小朋友做手工的桌子旁边，当小朋友需要用到材料盒中的某件工具时，小朋友通过轻轻拖动机器人夹持材料盒的手，使机器人夹持材料盒移动到小朋友需要的位置，并保持在此位置或者回到初始位置等等。Generally, when humanoid robots appear in production and life scenarios as service robots, scenarios of human-robot interaction and collaborative work are inevitable. For example, when a child is doing handicraft, the robot assists, such as picking up the handmade material box on the shelf with both hands, and moving it to the table where the child is doing handicraft. When the child needs to use a tool in the material box, the child will light Gently drag the hand of the robot holding the material box to move the robot holding the material box to the position the child needs, and keep it at this position or return to the original position, etc.

然而，上述简单的应用场景却面对很多技术挑战，例如，在采用离线编程的机器人位置控制时，很难匹配人机协作时人意图动作在时间和空间位置上的随机性和不可预知性。又或者，机器人双臂在协同夹持负载时，需要保证负载不会掉落而造成物损甚至人的意外伤害等。However, the above simple application scenarios face many technical challenges. For example, when using offline programming for robot position control, it is difficult to match the randomness and unpredictability of human intentional actions in time and space during human-robot collaboration. Or, when the two arms of the robot cooperate to clamp the load, it is necessary to ensure that the load will not fall and cause material damage or even accidental injury to people.

申请内容Application content

有鉴于此，本申请的目的是为了克服现有技术中的不足，提供一种机器人的人机协作控制方法、装置和机器人。In view of this, the purpose of this application is to overcome the deficiencies in the prior art, and to provide a method, device and robot for man-machine cooperative control of a robot.

本申请的实施例提供一种机器人的人机协作控制方法，应用于包括多个机械臂的机器人，每个机械臂末端在不同运动方向上设有各自的控制模式，所述不同运动方向包括负载接触面的法线方向，所述法线方向设有导纳力控模式，所述多个机械臂能用于当接触负载时在所述法线方向上受到方向相反的作用力，所述方法包括：The embodiments of the present application provide a human-robot cooperative control method for a robot, which is applied to a robot including a plurality of mechanical arms. The end of each mechanical arm is provided with its own control mode in different movement directions, and the different movement directions include loads. the normal direction of the contact surface, the normal direction is provided with an admittance force control mode, the plurality of manipulator arms can be used to receive a force in the opposite direction in the normal direction when contacting the load, the method include:

在依据所述导纳力控模式控制每个机械臂末端在沿所述负载接触面的法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力；In the case where the actual contact force of each manipulator end along the normal direction of the load contact surface is controlled to reach the desired force according to the admittance force control mode, it is detected whether there is an external force applied to any manipulator end and /or external force on said load;

若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到所述外力时的初始位置控制所述对应机械臂进行运动以响应负载跟随操作。If there is an external force, the position offset in the corresponding direction is calculated according to the external force component received by the end of each robot arm in each movement direction according to the control mode in the corresponding movement direction, according to the position of the corresponding robot arm in each movement direction The offset and the initial position when subjected to the external force control the corresponding mechanical arm to move in response to the load following operation.

在一种实施例中，所述检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力之前，所述方法还包括：In an embodiment, before the detection of whether there is an external force exerted on the end of any manipulator arm and/or the load, the method further includes:

在控制对应机械臂末端保持与所述期望作用力相等的实际接触力的情况下，依据预设规划路线控制所有机器臂运动到目标位置。Under the condition of controlling the end of the corresponding robotic arm to maintain an actual contact force equal to the expected force, all robotic arms are controlled to move to the target position according to a preset planned route.

在一种实施例中，所述不同运动方向包括与所述负载接触面的法线方向垂直的重力方向、与所述重力方向和所述法线方向分别垂直的另一方向，所述重力方向和所述另一方向上各自设有导纳阻抗模式或导纳拖动模式，每个机械臂末端在所述重力方向上和所述另一方向上设有对应的预设阈值；In an embodiment, the different movement directions include a gravitational direction perpendicular to the normal direction of the load contact surface, and another direction perpendicular to the gravitational direction and the normal direction respectively, the gravitational direction and an admittance impedance mode or an admittance drag mode respectively set in the other direction, and each end of the mechanical arm is provided with a corresponding preset threshold in the gravity direction and the other direction;

计算对应机械臂末端在所述重力方向或所述另一方向上的位置偏移量，包括：Calculate the position offset of the end of the corresponding robotic arm in the direction of gravity or the other direction, including:

根据对应机械臂末端在重力方向或另一方向上受到的外力分量与对应的预设阈值之间的差值依据对应方向上的控制模式计算所述对应机械臂末端在重力方向或另一方向上的位置偏移量。Calculate the position of the corresponding end of the robot arm in the direction of gravity or the other direction according to the difference between the external force component received by the end of the corresponding robot arm in the direction of gravity or the other direction and the corresponding preset threshold according to the control mode in the corresponding direction Offset.

在一种实施例中，在所述导纳阻抗模式下，当检测到外力分量消失预定时间时，控制所述机械臂末端在对应运动方向上返回至受到外力分量时的初始位置；In an embodiment, in the admittance impedance mode, when it is detected that the external force component disappears for a predetermined time, the end of the manipulator is controlled to return to the initial position when the external force component is received in the corresponding movement direction;

在所述导纳拖动模式下，当检测到外力分量消失时，控制所述机械臂末端在对应运动方向上停留在当前位置。In the admittance dragging mode, when it is detected that the external force component disappears, the end of the robotic arm is controlled to stay at the current position in the corresponding movement direction.

在一种实施例中，所述导纳力控模式的控制方程为：In an embodiment, the control equation of the admittance force control mode is:

其中，F _f为机械臂末端在所述法线方向上反馈的实际接触力，F _d为机械臂末端在所述法线方向上的期望作用力；M _d和B _d分别为惯性矩阵和阻尼矩阵；

和

分别为机械臂末端在所述法线方向上的初始位置的一阶导数和二阶导数；

和

分别为机械臂末端在所述法线方向上的期望速度和期望加速度。 Among them, F _f is the actual contact force fed back by the end of the manipulator in the normal direction, F _d is the expected force of the end of the manipulator in the normal direction; M _d and B _d are the inertia matrix and damping, respectively matrix;

and

are the first derivative and the second derivative of the initial position of the end of the manipulator in the normal direction, respectively;

and

are the desired velocity and desired acceleration of the end of the manipulator in the normal direction, respectively.

在一种实施例中，所述导纳阻抗模式的控制方程为：In one embodiment, the control equation of the admittance impedance mode is:

其中，M _d、B _d和K _d分别为惯性矩阵、阻尼矩阵和刚度矩阵，F _外为机械臂末端在所述重力方向或所述另一方向上受到的外力分量；X _r2、

和

依次为机械臂末端在所述重力方向或所述另一方向上的初始位置及所述初始位置的一阶导数和二阶导数；X _c2、

和

依次为机械臂末端在所述重力方向或所述另一方向上的期望位置、期望速度和期望加速度。 Among them, M _d , B _d and K _d are the inertia matrix, damping matrix and stiffness matrix respectively, F is the _external force component received by the end of the manipulator in the direction of gravity or the other direction; X _r2 ,

and

are the initial position of the end of the manipulator in the direction of gravity or the other direction, and the first and second derivatives of the initial position; X _c2 ,

and

The order is the desired position of the end of the robot arm in the direction of gravity or the other direction, the desired velocity and the desired acceleration.

在一种实施例中，所述导纳拖动模式的控制方程为：In one embodiment, the control equation of the admittance drag mode is:

其中，F _外为机械臂末端在所述重力方向或所述另一方向上受到的外力分量；B _d为阻尼矩阵；

为机械臂末端在所述重力方向或所述另一方向上的初始位置的一阶导数；

为机械臂末端在所述重力方向或所述另一方向上的期望速度。 Among them, F _outside is the external force component received by the end of the manipulator in the direction of gravity or in the other direction; B _d is the damping matrix;

is the first derivative of the initial position of the end of the manipulator in the direction of gravity or the other direction;

is the desired velocity of the end of the robot arm in the direction of gravity or the other direction.

在一种实施例中，每个机械臂末端设有六维力传感器，每个机械臂末端在所述重力方向上和所述另一方向上的预设阈值的预先获取，包括：In an embodiment, a six-dimensional force sensor is provided at the end of each robotic arm, and the pre-acquisition of preset thresholds in the direction of gravity and the other direction at the end of each robotic arm includes:

在控制对应机械臂末端保持与所述期望作用力相等的实际接触力的情况下，控制所有机械臂末端沿世界坐标系的Z方向向上运动预设高度，并在到达所述预设高度时，通过所述六维力传感器检测得到对应机械臂末端在所述重力方向和所述另一方向上的接触力，对应方向上的接触力作为所述预设阈值。Under the condition of controlling the corresponding end of the manipulator to maintain the actual contact force equal to the expected force, control all ends of the manipulator to move up a preset height along the Z direction of the world coordinate system, and when reaching the preset height, The contact force of the corresponding end of the mechanical arm in the gravity direction and the other direction is detected by the six-dimensional force sensor, and the contact force in the corresponding direction is used as the preset threshold.

本申请的实施例还提供一种机器人的人机协作控制装置，应用于包括多个机械臂的机器人，每个机械臂末端在不同运动方向上设有各自的控制模式，所述不同运动方向包括负载接触面的法线方向，所述法线方向设有导纳力控模式，所述多个机械臂能用于当接触负载时在所述法线方向上产生方向相反的作用力，所述装置包括：Embodiments of the present application also provide a human-robot cooperative control device for a robot, which is applied to a robot including a plurality of robotic arms. The end of each robotic arm is provided with its own control mode in different movement directions, and the different movement directions include the normal direction of the load contact surface, the normal direction is provided with an admittance force control mode, the plurality of manipulator arms can be used to generate opposing forces in the normal direction when contacting the load, the The device includes:

接触控制模块，用于依据所述导纳力控模式控制对应机械臂末端在沿所述负载接触面的法线方向上的实际接触力达到期望作用力；a contact control module, configured to control the actual contact force of the corresponding manipulator end along the normal direction of the load contact surface to reach a desired force according to the admittance force control mode;

外力检测模块，用于在每个机械臂末端在沿所述法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力；The external force detection module is used to detect whether there is an external force applied to any end of the robot arm and/or the load when the actual contact force of each end of the robot arm in the direction of the normal reaches the desired force external force;

拖动响应模块，用于若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到所述外力时的初始位置控制所述对应机械臂进行运动以响应负载跟随操作。The drag response module is used to calculate the position offset in the corresponding direction according to the external force component received by the end of each manipulator in each motion direction according to the control mode in the corresponding motion direction if there is an external force. The position offset in each movement direction and the initial position when subjected to the external force control the corresponding mechanical arm to move in response to the load following operation.

本申请的实施例还提供一种机器人，包括处理器、存储器和至少两个机械臂，所述至少两个机械臂用于协同夹持负载，所述存储器存储有计算机程序，所述计算机程序在所述处理器上执行时，实施上述的机器人的人机协作控制方法。Embodiments of the present application also provide a robot, including a processor, a memory, and at least two robotic arms, wherein the at least two robotic arms are used to cooperatively grip a load, the memory stores a computer program, and the computer program is When executed on the processor, the above-mentioned man-machine cooperative control method of the robot is implemented.

在一种实施例中，所述机器人为双臂机器人。In one embodiment, the robot is a dual-arm robot.

本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上执行时，实施上述的机器人的人机协作控制方法。Embodiments of the present application further provide a readable storage medium, which stores a computer program, and when the computer program is executed on a processor, implements the above-mentioned method for controlling human-robot cooperation of a robot.

本申请的实施例具有如下有益效果：The embodiments of the present application have the following beneficial effects:

本申请实施例的机器人的人机协作控制方法通过在机械臂末端的不同运动方向上设有以导纳控制为基础的多种控制模式，其中，在负载接触面的法线方向上设有用于实现力跟踪的导纳力控模式，而这些机械臂末端由于与负载的接触位置不同而在法线方向上能够产生方向相反(即相对方向)的作用力，故可实现所有机械臂在法线方向上对负载的恒力夹持，同时在检测到施加的外力时，根据对应运动方向上的外力分量利用对应运动方向上的控制模式进行运动控制，还可以实现负载在夹持下作跟随用户操作，达到人机协作目的等。The man-machine cooperative control method of the robot in the embodiment of the present application provides multiple control modes based on admittance control in different motion directions of the end of the manipulator, wherein, in the normal direction of the load contact surface, there is a The admittance force control mode of force tracking is realized, and the ends of these manipulators can generate force in the opposite direction (ie, the opposite direction) in the normal direction due to the different contact positions with the load, so all manipulators can be realized in the normal direction. The constant force clamps the load in the direction, and when the applied external force is detected, the motion control is performed using the control mode in the corresponding motion direction according to the external force component in the corresponding motion direction, and the load can also follow the user under clamping. operation, to achieve the purpose of human-machine collaboration, etc.

附图说明Description of drawings

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。In order to illustrate the technical solutions of the embodiments of the present application more clearly, the following drawings will briefly introduce the drawings that need to be used in the embodiments. It should be understood that the following drawings only show some embodiments of the present application, and therefore do not It should be regarded as a limitation of the scope, and for those of ordinary skill in the art, other related drawings can also be obtained according to these drawings without any creative effort.

图1示出了本申请实施例的机器人的人机协作控制方法的流程示意图；FIG. 1 shows a schematic flowchart of a man-machine cooperative control method for a robot according to an embodiment of the present application;

图2示出了本申请实施例的机器人的人机协作控制方法的导纳控制流程示意图；FIG. 2 shows a schematic diagram of the admittance control process of the man-machine cooperative control method of the robot according to the embodiment of the present application;

图3示出了本申请实施例的机器人的人机协作控制方法的在负载接触面的法线方向进行力控制的示意图；3 shows a schematic diagram of force control in the normal direction of the load contact surface in the man-machine cooperative control method of the robot according to the embodiment of the present application;

图4示出了本申请实施例的机器人的人机协作控制装置的结构示意图。FIG. 4 shows a schematic structural diagram of a human-robot cooperative control device for a robot according to an embodiment of the present application.

具体实施方式Detailed ways

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments.

通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。The components of the embodiments of the present application generally described and illustrated in the drawings herein may be arranged and designed in a variety of different configurations. Thus, the following detailed description of the embodiments of the application provided in the accompanying drawings is not intended to limit the scope of the application as claimed, but is merely representative of selected embodiments of the application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work fall within the protection scope of the present application.

在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。Hereinafter, the terms "comprising", "having" and their cognates, which may be used in various embodiments of the present application, are only intended to denote particular features, numbers, steps, operations, elements, components, or combinations of the foregoing, and should not be construed as first excluding the presence of or adding one or more other features, numbers, steps, operations, elements, components or combinations of the foregoing or the possibility of a combination of the foregoing.

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the various embodiments of this application belong. The terms (such as those defined in commonly used dictionaries) will be interpreted as having the same meaning as the contextual meaning in the relevant technical field and will not be interpreted as having an idealized or overly formal meaning, unless explicitly defined in the various embodiments of this application.

实施例1Example 1

图1示出了本申请实施例的机器人的人机协作控制方法的一种流程示意图。该方法可以应用于具有多个机械臂的机器人进行人机交互、协同作业等场景，例如，该机器人可以是指具有双臂的人形机器人或具有三条或三条以上机械臂的机器人等。其中，这些机械臂均为根据相应位置指令进行关节控制的机械臂。FIG. 1 shows a schematic flowchart of a method for man-machine cooperative control of a robot according to an embodiment of the present application. The method can be applied to scenarios such as human-robot interaction and collaborative work performed by a robot with multiple robotic arms. For example, the robot may refer to a humanoid robot with two arms or a robot with three or more robotic arms. Among them, these manipulators are all manipulators that perform joint control according to corresponding position commands.

本实施例中，该机器人的人机协作控制方法，通过在机械臂末端的不同运动方向上设置相应的以导纳控制为基础的控制模式，可以实现较好的人机协作功能，例如，在实现多个机械臂对重物的夹持操作时，还可以实现跟随人手移动而保证负载不会掉落，提高了人机协作时的安全性等。此外，当采用了不同的控制模式时，可以实现使负载停留在外界施加的力/力矩消失的位置或者在到达期望位置后会自动返回到初始位置，从而满足不同场景下的需求等。In this embodiment, the man-machine cooperation control method of the robot can achieve better man-machine cooperation function by setting corresponding control modes based on admittance control in different motion directions of the end of the manipulator. When the clamping operation of the heavy objects is realized by multiple mechanical arms, it can also follow the movement of the human hand to ensure that the load does not fall, which improves the safety of human-machine cooperation. In addition, when different control modes are adopted, the load can be made to stay at the position where the force/torque applied by the outside world disappears or automatically return to the initial position after reaching the desired position, so as to meet the needs in different scenarios, etc.

其中，导纳控制是对机械臂末端输入外力以调整机械臂末端的运动状态的控制方式。例如，图2示出了一种导纳控制***。其中，M _d、B _d和K _d依次为构建的阻抗模型的惯性矩阵、阻尼矩阵和刚度矩阵，F为机器人的机械臂末端反馈的实际接触力，可通过设置在机械臂末端处的力传感器检测获得；X _c为机器人的机械臂末端在笛卡尔空间(也称为任务空间)的期望位置，X _r为机器人的机械臂末端在笛卡尔空间的参考位置，位置偏移量ΔX＝X _c-X _r，通过将计算得到的位置偏移量ΔX和参考位置X _r叠加输入到位置控制器中作为位置指令，从而实现对机械臂各个关节的控制。 Among them, admittance control is a control method in which an external force is input to the end of the manipulator to adjust the motion state of the end of the manipulator. For example, Figure 2 shows an admittance control system. Among them, M _d , B _d and K _d are the inertia matrix, damping matrix and stiffness matrix of the constructed impedance model in turn, and F is the actual contact force fed back by the end of the manipulator arm of the robot. Obtained by detection; X _c is the desired position of the robot arm end in Cartesian space (also called task space), X _r is the reference position of the robot arm end in Cartesian space, and the position offset ΔX=X _c -X _r , by superimposing and inputting the calculated position offset ΔX and the reference position X _r into the position controller as a position command, so as to realize the control of each joint of the manipulator.

为实现该机器人既能够对负载进行夹持，还可以同时响应用户对负载的拖动操作(即负载跟随操作)，本实施例设有至少两种控制模式以用于对各个机械臂末端在不同方向上进行运动控制。通常地，对于机器人的各个机械臂末端，在笛卡尔空间中，单个机械臂末端具有三个运动方向，在一种实施方式中，这三个运动方向可依次选取为该机械臂末端待作用的负载接触面的法线方向、负载重力方向和分别与法线方向、重力方向垂直的另一方向。其中，上述的负载接触面是指当前机械臂末端与负载产生接触时的负载的一个面。对于多个机械臂，每个机械臂接触的负载接触面往往不同，例如，对于矩形负载，位置相对的两个机械臂则可在平行的两个接触面产生接触。可知，由于多个机械臂同时接触负载时，各个机械臂将在法线方向上产生方向相反的接触力，从而实现稳定夹持。In order to realize that the robot can not only clamp the load, but also respond to the user's drag operation on the load (that is, the load following operation), at least two control modes are provided in this embodiment for different Motion control in the direction. Generally, for each manipulator end of a robot, in Cartesian space, a single manipulator end has three motion directions. In one embodiment, these three motion directions can be sequentially selected as the motions to be acted on by the manipulator end. The normal direction of the load contact surface, the load gravity direction, and another direction perpendicular to the normal direction and the gravity direction, respectively. Wherein, the above-mentioned load contact surface refers to a surface of the load when the front end of the robot arm is in contact with the load. For multiple manipulators, the load contact surfaces of each manipulator are often different. For example, for a rectangular load, two manipulators in opposite positions can make contact on two parallel contact surfaces. It can be seen that when multiple manipulators contact the load at the same time, each manipulator will generate contact forces in opposite directions in the normal direction, so as to achieve stable clamping.

另外，为方便计算，可将力传感器的坐标系与机械臂末端的运动坐标系对应设置，以使得机械臂末端上的力传感器检测的三个方向上的力直接作为该机械臂末端在三个方向上受到的力，例如，法线方向对应Z方向，重力方向对应X方向，而另一方向对应Y方向。In addition, for the convenience of calculation, the coordinate system of the force sensor can be set corresponding to the motion coordinate system of the end of the robot arm, so that the forces in the three directions detected by the force sensor on the end of the robot arm are directly used as the end of the robot arm in three directions. The force in the direction, for example, the normal direction corresponds to the Z direction, the gravitational direction corresponds to the X direction, and the other direction corresponds to the Y direction.

示范性地，本实施例的控制模式可包括但不限于包括导纳力控模式、以及导纳阻抗模式和/或导纳拖动模式等。其中，导纳力控模式可用于在对应方向上实现精准的力跟随；导纳阻抗模式的控制方程中存在刚度项，故能够实现在外力消失后，从外力消失的位置逐渐返回在对应方向上的初始位置；导纳拖动模式可用于实现在外力消失后，在对应方向上停留在外力消失的位置。由于导纳阻抗模式或导纳拖动模式存在冲突，通常地，机器人不会出现导纳阻抗模式和导纳拖动模式同时工作，对于多功能的机器人，可以通过模式选择选取导纳阻抗模式或导纳拖动模式在不同的运用场景下工作。Exemplarily, the control modes of this embodiment may include, but are not limited to, an admittance force control mode, an admittance impedance mode and/or an admittance drag mode, and the like. Among them, the admittance force control mode can be used to achieve precise force following in the corresponding direction; there is a stiffness term in the control equation of the admittance impedance mode, so it can gradually return to the corresponding direction from the position where the external force disappears after the external force disappears The initial position of ; the admittance drag mode can be used to stay at the position where the external force disappears in the corresponding direction after the external force disappears. Due to the conflict between the admittance impedance mode and the admittance drag mode, usually, the robot will not work in the admittance impedance mode and the admittance drag mode at the same time. Admittance drag mode works in different usage scenarios.

在一种实施方式中，可通过上述的导纳力控模式控制每个机械臂末端在负载接触面的法线方向上的运动；而对于重力方向和另一方向，可选取相同的控制模式，如上述的导纳阻抗模式或导纳拖动模式等，当然也可以选取不同的控制模式，例如，重力方向采用导纳阻抗模式或导纳拖动模式，而另一方向采用位置控制模式等。In one embodiment, the above-mentioned admittance force control mode can be used to control the movement of each manipulator end in the normal direction of the load contact surface; and for the gravity direction and the other direction, the same control mode can be selected, Like the above admittance impedance mode or admittance drag mode, of course, different control modes can also be selected. For example, the gravity direction adopts the admittance impedance mode or the admittance drag mode, and the other direction adopts the position control mode.

下面对上述的三种控制模式的控制方式进行说明。可以理解，本实施例的机器人的每个机械臂的控制原理相同，下文的控制步骤是以单个机械臂作为控制对象并进行描述的。The control modes of the above three control modes will be described below. It can be understood that the control principle of each manipulator of the robot in this embodiment is the same, and the following control steps are described with a single manipulator as the control object.

对于导纳力控模式，其主要用于实现机械臂末端在相应方向上的力跟踪，从而达到恒力控制。示范性地，该导纳力控模式的控制方程为：For the admittance force control mode, it is mainly used to realize the force tracking of the end of the manipulator in the corresponding direction, so as to achieve constant force control. Exemplarily, the control equation of the admittance force control mode is:

其中，F _f为机械臂末端在相应方向上的实际接触力，F _d为机械臂末端在相应方向上的期望作用力；M _d和B _d分别为期望阻抗模型的惯性矩阵和阻尼矩阵；

和

分别为机械臂末端在所述法线方向上的期望速度和期望加速度。可以理解，当导纳力控模式用于控制机械臂末端在负载接触面的法线方向上的运动时，上述的相应方向即为该法线方向。 Among them, F _f is the actual contact force of the end of the manipulator in the corresponding direction, F _d is the expected force of the end of the manipulator in the corresponding direction; M _d and B _d are the inertia matrix and damping matrix of the desired impedance model, respectively;

and

are the desired velocity and desired acceleration of the end of the manipulator in the normal direction, respectively. It can be understood that when the admittance force control mode is used to control the movement of the end of the manipulator in the normal direction of the load contact surface, the above-mentioned corresponding direction is the normal direction.

利用上述方程可在已知实际接触力和期望作用力的情况下求解出机械臂末端在法线方向上由初始位置到期望位置之间的位置偏移量，进而根据该位置偏移量对机械臂末端进行在法线方向上的位置移动控制，使得机械臂末端到达期望位置以对负载产生所需的期望作用力。应当理解的是，由于方程中不存在刚度项，若先施加外力，在检测到外力分量消失的情况下，该机械臂末端在该方向上将会停留在力消失的位置。Using the above equation, the position offset of the end of the manipulator from the initial position to the desired position in the normal direction can be solved when the actual contact force and the expected force are known, and then the mechanical The end of the arm performs position movement control in the normal direction, so that the end of the robotic arm reaches the desired position to generate the desired desired force on the load. It should be understood that since there is no stiffness term in the equation, if the external force is applied first, when the disappearance of the external force component is detected, the end of the manipulator will stay at the position where the force disappears in this direction.

例如，可根据负载的重量等信息计算在所有机械臂在共同夹持该负载时各个机械臂末端在该法线方向上所需的期望作用力。以双臂机器人为例，如图3所示，该期望作用力即为机器人双臂在相对水平方向上的恒力控制。而实际接触力即机械臂末端与外界之间的交互力，例如，可通过设置在机械臂末端处的六维力传感器检测得到。For example, the expected force required by each end of each manipulator in the normal direction can be calculated according to information such as the weight of the load when all the manipulators jointly clamp the load. Taking a dual-arm robot as an example, as shown in Figure 3, the desired force is the constant force control of the robot's arms in the relative horizontal direction. The actual contact force is the interaction force between the end of the robot arm and the outside world, for example, it can be detected by a six-dimensional force sensor disposed at the end of the robot arm.

对于导纳阻抗模式，其主要用于实现机械臂末端在与采用导纳力控模式的方向不同的方向上的柔性控制。示范性地，该导纳阻抗模式的控制方程为：For the admittance impedance mode, it is mainly used to realize the flexible control of the end of the manipulator in a direction different from the direction of the admittance force control mode. Exemplarily, the governing equation of the admittance impedance mode is:

其中，M _d、B _d和K _d分别为期望阻抗模型的惯性矩阵、阻尼矩阵和刚度矩阵，F _外为外界施加的在对应方向上的外力或外力分量，可通过机械臂末端处的力传感器检测得到；X _r2、

和

依次为机械臂末端在所述重力方向或所述另一方向上的期望位置、期望速度和期望加速度。可以理解，当上述的重力方向和另一方向均采用该导纳阻抗模式时，该重力方向或另一方向即为上述的对应方向。 Among them, M _d , B _d and K _d are the inertia matrix, damping matrix and stiffness matrix of the desired impedance model, respectively, and F is the _external force or external force component in the corresponding direction applied by the outside world, which can be passed through the force sensor at the end of the manipulator. detected; X _r2 ,

and

The order is the desired position of the end of the robot arm in the direction of gravity or the other direction, the desired velocity and the desired acceleration. It can be understood that when the above-mentioned gravity direction and the other direction both adopt the admittance impedance mode, the gravity direction or the other direction is the above-mentioned corresponding direction.

值得注意的是，由于该控制方程中存在刚度项，故在检测到该外力分量消失后，在控制使机械臂末端停留预定时间后，控制器将继续输出以控制机械臂末端从该另一方向上的期望位置返回到在另一方向上受到外力时开始运动时的初始位置。重力方向同理。It is worth noting that due to the existence of the stiffness term in the control equation, after detecting the disappearance of the external force component, after the control makes the end of the manipulator stay for a predetermined time, the controller will continue to output to control the end of the manipulator from the other direction. The desired position returns to the initial position when the movement begins when an external force is applied in the other direction. The direction of gravity is the same.

对于导纳拖动模式，其主要用于实现机械臂末端在与采用导纳力控模式的方向不同的对应方向上的移动。示范性地，该导纳拖动模式的控制方程为：For the admittance drag mode, it is mainly used to realize the movement of the end of the manipulator in a corresponding direction different from the direction in which the admittance force control mode is adopted. Exemplarily, the control equation of the admittance drag mode is:

其中，F _外为外界施加在对应方向上的外力或外力分量，可通过机械臂末端处的力传感器检测得到；B _d为期望阻抗模型的阻尼矩阵；

为机械臂末端在对应方向上的初始位置的一阶导数；

为机械臂末端在对应方向上的期望速度。由于该控制方程中不存在刚度项，在检测到外力分量消失时，机械臂末端将在该方向上停留在力消失时的位置。 Among them, F _outside is the external force or external force component exerted by the outside world in the corresponding direction, which can be detected by the force sensor at the end of the manipulator; B _d is the damping matrix of the desired impedance model;

is the first derivative of the initial position of the end of the manipulator in the corresponding direction;

is the desired speed of the end of the manipulator in the corresponding direction. Since there is no stiffness term in this governing equation, when the disappearance of the external force component is detected, the end of the manipulator will stay at the position where the force disappears in that direction.

在一种实施方式，该具机器人的每个机械臂在负载接触面的法线方向上采用导纳力控模式，而其他运动方向上则采用了相同的导纳阻抗模式或导纳拖动模式。于是，如图1所示，下面对该机器人的人机协作控制方法进行详细说明。In one embodiment, each manipulator of the robot adopts an admittance force control mode in the normal direction of the load contact surface, and adopts the same admittance impedance mode or admittance drag mode in other motion directions . Therefore, as shown in FIG. 1 , the man-machine cooperative control method of the robot will be described in detail below.

步骤S110，在依据导纳力控模式控制每个机械臂末端在沿负载接触面的法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或负载上的外力。Step S110, under the condition that the actual contact force of each manipulator end along the normal direction of the load contact surface reaches the desired force according to the admittance force control mode, it is detected whether there is an external force applied to any manipulator end and/or or external force on the load.

在实际场景中，如图3所示，以一矩形料盒负载为例，该负载接触面分别为水平方向上的两个侧面，此时机器人将通过对待夹持的负载在水平方向上进行力控制以实现在上述的负载接触面的法线方向上对负载的稳定夹持。In the actual scene, as shown in Figure 3, taking a rectangular material box load as an example, the contact surfaces of the load are two sides in the horizontal direction. At this time, the robot will force the load to be clamped in the horizontal direction. Controlled to achieve stable clamping of the load in the direction normal to the aforementioned load contact surface.

示范性地，当通过设置在机械臂末端的力传感器检测到每个机械臂末端对负载的实际接触力达到期望作用力，即表明该机器人的所有机械臂末端与负载进行了接触且在各个负载接触面产生与期望作用力相等的实际接触力。可以理解，该期望作用力通常能够保证当控制所有机械臂向上运动时，该负载能被顺利拿起而不会掉落。Exemplarily, when it is detected by the force sensor provided at the end of the robot arm that the actual contact force of each end of the robot arm to the load reaches the expected force, it means that all the ends of the robot arm of the robot are in contact with the load, and each end of the robot is in contact with the load. The contact surface produces an actual contact force equal to the desired force. It can be understood that the expected force can generally ensure that the load can be smoothly picked up without falling off when all the robotic arms are controlled to move upward.

其中，在检测到实际接触力达到期望作用力之前，该方法还包括：Wherein, before detecting that the actual contact force reaches the expected force, the method further includes:

根据对应机械臂末端在负载接触面的法线方向上的初始位置、实际接触力和期望作用力按照导纳力控模式控制对应机械臂末端到达期望位置，以使得对应机械臂末端对负载产生该接触力。According to the initial position of the corresponding manipulator end in the normal direction of the load contact surface, the actual contact force and the expected force, the corresponding manipulator end is controlled to reach the desired position according to the admittance force control mode, so that the corresponding manipulator end can produce this effect on the load. contact force.

可选地，当对应机械臂末端在沿所述法线方向上的实际接触力达到期望作用力之后，可控制各个机械臂保持与期望作用力相等的实际接触力，并依据预设规划路线控制所有机器臂运动到目标位置。Optionally, after the actual contact force of the corresponding manipulator end along the normal direction reaches the desired force, each manipulator can be controlled to maintain an actual contact force equal to the desired force, and the control can be performed according to a preset planned route. All robotic arms move to the target position.

例如，以双臂机器人与小朋友画画时进行人机协作的这一应用场景为例，当机器人从架子上夹持料盒这一负载时，可先拿起并行走到小朋友所在的位置，然后可进入等待响应小朋友的拖动操作。For example, taking the application scenario of human-robot collaboration between a two-armed robot and a child drawing as an example, when the robot clamps the load of the material box from the shelf, it can first pick it up and walk to the child's position, and then You can enter and wait to respond to the child's drag operation.

步骤S120，若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到外力时的初始位置控制对应机械臂进行运动以响应负载跟随操作。Step S120, if there is an external force, calculate the position offset in the corresponding direction according to the external force component received by the end of each robot arm in each movement direction according to the control mode in the corresponding movement direction, and calculate the position offset in the corresponding direction according to the corresponding robot arm in each movement direction. The position offset and the initial position when subjected to external force control the corresponding robot arm to move in response to the load following operation.

示范性地，当用户需要对该负载进行拖动操作时，可直接施加外力在任意机械臂末端和/或负载上。通常地，所述的外力包括三个运动方向的外力分量，可通过机械臂末端安装的力传感器进行检测得到，如六维力传感器等，当然若在某个方向上没有受到外力作用，此时该方向的外力分量则为0。Exemplarily, when the user needs to perform a drag operation on the load, an external force may be directly applied to any end of the robotic arm and/or the load. Generally, the external force includes external force components in three motion directions, which can be detected by a force sensor installed at the end of the robotic arm, such as a six-dimensional force sensor, etc. Of course, if there is no external force in a certain direction, then The external force component in this direction is 0.

由于需要夹持负载运动，为排除该负载的重量在重力方向上及另一方向上的摩擦力的影响，本实施例中，每个机械臂末端在重力方向和另一方向上预设有重量阈值，若在重力方向或另一方向上受到的外力分量大于对应方向上的预设阈值，则将在对应方向上产生位置偏移量，否则不移动。Since the load needs to be clamped, in order to exclude the influence of the friction force of the weight of the load in the direction of gravity and the other direction, in this embodiment, each end of the robot arm is preset with a weight threshold in the direction of gravity and the other direction, If the external force component received in the direction of gravity or another direction is greater than the preset threshold value in the corresponding direction, a position offset will be generated in the corresponding direction, otherwise it will not move.

在一种实施方式中，计算对应机械臂末端在重力方向上的位置偏移量，包括：根据对应机械臂末端在重力方向上受到的外力分量与该重量阈值的差值依据采用的导纳阻抗模式或导纳拖动模式的控制方程可计算该对应机械臂末端在重力方向上的位置偏移量。对于另一方向，同理可计算得到该另一方向上的位置偏移量。In one embodiment, calculating the positional offset of the corresponding end of the manipulator in the direction of gravity includes: according to the difference between the external force component received by the end of the corresponding manipulator in the direction of gravity and the weight threshold, according to the adopted admittance impedance The control equation of the mode or the admittance drag mode can calculate the position offset of the corresponding end of the robot arm in the direction of gravity. For the other direction, the position offset in the other direction can be calculated in the same way.

而为获取每个机械臂末端在重力方向上和另一方向上的预设阈值，可通过使接触的负载脱离支撑面的情况下进行检测得到。例如，可在控制对应机械臂末端保持与期望作用力相等的实际接触力的情况下，控制所有机械臂末端沿世界坐标系的Z方向向上运动预设高度，并在到达所述预设高度时，通过设置在机械臂末端的力传感器检测得到对应机械臂末端在重力方向和另一方向上的接触力，对应方向上的接触力即设置为上述的预设阈值。In order to obtain the preset thresholds of each end of the robot arm in the direction of gravity and the other direction, it can be obtained by detecting when the contacted load is separated from the support surface. For example, under the condition of controlling the corresponding manipulator end to maintain the actual contact force equal to the expected force, all manipulator ends can be controlled to move up a preset height along the Z direction of the world coordinate system, and when the preset height is reached , the contact force in the gravitational direction and the other direction corresponding to the end of the robot arm is detected by the force sensor disposed at the end of the robot arm, and the contact force in the corresponding direction is set to the above-mentioned preset threshold.

对于步骤S120，示范性地，对于每个机械臂末端能够产生位置偏移的方向，可根据对应方向上的外力分量与存在的预设阈值利用相应控制模式的控制方程计算出在该对应方向上的位移偏移量，进而利用该位移偏移量和受到外力时的初始位置可确定在该对应方向上的期望位置。当仅在一个方向上存在位移时，负载将随外力在该方向上平移；若在多个方向上均产生位置偏移量，该机械臂将沿外力方向上进行运动，即沿三个方向的外力分量的合力方向进行运动。For step S120, exemplarily, for the direction in which the position deviation can be generated at the end of each manipulator, the corresponding direction can be calculated by using the control equation of the corresponding control mode according to the external force component in the corresponding direction and the existing preset threshold value. The displacement offset, and then the desired position in the corresponding direction can be determined by using the displacement offset and the initial position when subjected to external force. When there is displacement in only one direction, the load will translate in that direction with the external force; if there are positional offsets in multiple directions, the robotic arm will move in the direction of the external force, that is, in three directions Movement in the direction of the resultant force of the external force components.

可以理解，若机器人的每个机械臂在负载接触面的法线方向上也受到外力分量，以双臂机器人为例，由于左、右机械臂末端的实际接触力也将变化，利用导纳力控模式同样可计算得到相应的位置偏移量，根据该位置偏移量对对应机械臂进行力跟随控制，从而保证负载不会掉落。It can be understood that if each robotic arm of the robot is also subjected to an external force component in the normal direction of the load contact surface, taking a dual-arm robot as an example, since the actual contact force at the ends of the left and right robotic arms will also change, the admittance force control is used to control the load. The mode can also calculate the corresponding position offset, and perform force following control on the corresponding mechanical arm according to the position offset, so as to ensure that the load will not fall.

本实施例的机器人的人机协作控制方法通过对于各个机械臂末端在负载被夹持的接触面上的法向上利用导纳力控模式进行控制，实现对负载的恒力夹持；同时在其他的两个运动方向则采用如导纳阻抗模式或导纳拖动模式等控制模式，以实现负载在被夹持的情况下，还能响应人对负载的拖动操作，达到较好的人机协作目的，也进一步提高了用户体验等。The man-machine cooperative control method of the robot in this embodiment realizes the constant force clamping of the load by using the admittance force control mode to control the end of each manipulator on the normal direction of the contact surface where the load is clamped; Control modes such as admittance impedance mode or admittance drag mode are adopted for the two moving directions of the load, so that the load can also respond to the drag operation of the load when the load is clamped, so as to achieve a better man-machine operation. Collaboration purposes, but also further improve the user experience and so on.

实施例2Example 2

请参照图4，基于上述实施例1的方法，本实施例提出一种机器人的人机协作控制装置100，应用于包括多个机械臂的机器人，每个机械臂末端在不同运动方向上设有各自的控制模式，所述不同运动方向包括负载接触面的法线方向，所述法线方向设有导纳力控模式，所述多个机械臂能用于当接触负载时在所述法线方向上产生方向相反的作用力。示范性地，该机器人的人机协作控制装置100包括：Referring to FIG. 4 , based on the method of the above-mentioned Embodiment 1, this embodiment proposes a human-robot cooperative control device 100 for a robot, which is applied to a robot including a plurality of mechanical arms. Respective control modes, the different motion directions include the normal direction of the load contact surface, the normal direction is provided with an admittance force control mode, and the plurality of mechanical arms can be used for the normal direction when contacting the load. A force in the opposite direction is produced. Exemplarily, the human-robot cooperative control device 100 of the robot includes:

接触控制模块110，用于依据所述导纳力控模式控制对应机械臂末端在沿所述负载接触面的法线方向上的实际接触力达到期望作用力；The contact control module 110 is configured to control the actual contact force of the corresponding manipulator end along the normal direction of the load contact surface to reach the desired force according to the admittance force control mode;

外力检测模块120，用于在每个机械臂末端在沿所述法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力；The external force detection module 120 is used to detect whether there is an external force applied to any end of the robot arm and/or the load when the actual contact force of each end of the robot arm in the normal direction reaches the desired force external force;

拖动响应模块130，用于若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到所述外力时的初始位置控制所述对应机械臂进行运动以响应负载跟随操作。The drag response module 130 is used to calculate the position offset in the corresponding direction according to the external force component received by the end of each robot arm in each movement direction according to the control mode in the corresponding movement direction if there is an external force, and according to the corresponding robot arm The position offset in each movement direction and the initial position when subjected to the external force control the corresponding robotic arm to move in response to a load following operation.

可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。It can be understood that the apparatus of this embodiment corresponds to the method of the foregoing Embodiment 1, and the options in the foregoing Embodiment 1 are also applicable to this embodiment, so the description is not repeated here.

本申请还提供了一种机器人，示范性地，该机器人包括处理器、存储器和至少两个机械臂，其中，该至少两个机械臂能够用于协同夹持负载，存储器存储有计算机程序，处理器通过运行该计算机程序，从而使移动终端执行上述的机器人的人机协作控制方法或者上述机器人的人机协作控制装置中的各个模块的功能。例如，该机器人可为双臂机器人等。The present application also provides a robot, exemplarily, the robot includes a processor, a memory and at least two robotic arms, wherein the at least two robotic arms can be used to cooperatively grip a load, the memory stores a computer program for processing By running the computer program, the computer enables the mobile terminal to execute the functions of each module in the above-mentioned man-machine cooperative control method for a robot or the above-mentioned man-machine cooperative control device for a robot. For example, the robot may be a dual-arm robot or the like.

本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。The present application also provides a readable storage medium for storing the computer program used in the above-mentioned robot.

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may also be implemented in other manners. The apparatus embodiments described above are only schematic. For example, the flowcharts and structural diagrams in the accompanying drawings show the possible implementation architectures and functions of the apparatuses, methods and computer program products according to various embodiments of the present application. and operation. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more functions for implementing the specified logical function(s) executable instructions. It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It is also noted that each block of the block diagrams and/or flow diagrams, and combinations of blocks in the block diagrams and/or flow diagrams, can be implemented using dedicated hardware-based systems that perform the specified functions or actions. be implemented, or may be implemented in a combination of special purpose hardware and computer instructions.

另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。In addition, each functional module or unit in each embodiment of the present application may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application can be embodied in the form of a software product in essence, or the part that contributes to the prior art or the part of the technical solution. The computer software product is stored in a storage medium, including Several instructions are used to cause a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, Read-Only Memory (ROM, Read-Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk and other media that can store program codes .

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。The above are only specific embodiments of the present application, but the protection scope of the present application is not limited to this. should be covered within the scope of protection of this application.

Claims

一种机器人的人机协作控制方法，其特征在于，应用于包括多个机械臂的机器人，每个机械臂末端在不同运动方向上设有各自的控制模式，所述不同运动方向包括负载接触面的法线方向，所述法线方向设有导纳力控模式，所述多个机械臂能用于当接触负载时在所述法线方向上受到方向相反的作用力，所述方法包括：A human-robot cooperative control method for a robot is characterized in that, it is applied to a robot comprising a plurality of mechanical arms, and the end of each mechanical arm is provided with its own control mode in different moving directions, and the different moving directions include load contact surfaces The normal direction of , the normal direction is provided with an admittance force control mode, the plurality of manipulator arms can be used to receive force in the opposite direction in the normal direction when contacting the load, and the method includes:

在依据所述导纳力控模式控制每个机械臂末端在沿所述负载接触面的法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力；In the case where the actual contact force of each manipulator end along the normal direction of the load contact surface is controlled to reach the desired force according to the admittance force control mode, it is detected whether there is an external force applied to any manipulator end and /or external force on said load;

若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到所述外力时的初始位置控制所述对应机械臂进行运动以响应负载跟随操作。If there is an external force, the position offset in the corresponding direction is calculated according to the external force component received by the end of each robot arm in each movement direction according to the control mode in the corresponding movement direction, according to the position of the corresponding robot arm in each movement direction The offset and the initial position when subjected to the external force control the corresponding mechanical arm to move in response to the load following operation.
根据权利要求1所述的方法，其特征在于，所述检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力之前，所述方法还包括：The method according to claim 1, characterized in that, before detecting whether there is an external force exerted on the end of any manipulator arm and/or on the load, the method further comprises:

在控制对应机械臂末端保持与所述期望作用力相等的实际接触力的情况下，依据预设规划路线控制所有机器臂运动到目标位置。Under the condition of controlling the end of the corresponding robotic arm to maintain an actual contact force equal to the expected force, all robotic arms are controlled to move to the target position according to a preset planned route.
根据权利要求1所述的方法，其特征在于，所述不同运动方向包括与所述负载接触面的法线方向垂直的重力方向、与所述重力方向和所述法线方向分别垂直的另一方向，所述重力方向和所述另一方向上各自设有导纳阻抗模式或导纳拖动模式，每个机械臂末端在所述重力方向上和所述另一方向上设有对应的预设阈值；The method according to claim 1, wherein the different moving directions include a gravitational direction perpendicular to the normal direction of the load contact surface, and another direction perpendicular to the gravitational direction and the normal direction, respectively. The direction of gravity and the other direction are respectively provided with an admittance impedance mode or an admittance drag mode, and each end of the robotic arm is provided with a corresponding preset threshold in the gravity direction and the other direction ;

计算对应机械臂末端在所述重力方向或所述另一方向上的位置偏移量，包括：Calculate the position offset of the end of the corresponding robotic arm in the direction of gravity or the other direction, including:

根据对应机械臂末端在重力方向或另一方向上受到的外力分量与对应的预设阈值之间的差值依据对应方向上的控制模式计算所述对应机械臂末端在重力方向或另一方向上的位置偏移量。Calculate the position of the corresponding end of the robot arm in the direction of gravity or the other direction according to the difference between the external force component received by the end of the corresponding robot arm in the direction of gravity or the other direction and the corresponding preset threshold according to the control mode in the corresponding direction Offset.
根据权利要求3所述的方法，其特征在于，在所述导纳阻抗模式下，当检测到外力分量消失预定时间时，控制所述机械臂末端在对应运动方向上返回至受到外力分量时的初始位置；The method according to claim 3, wherein, in the admittance impedance mode, when it is detected that the external force component disappears for a predetermined time, the end of the manipulator is controlled to return to the state when the external force component is received in the corresponding movement direction. initial position;

在所述导纳拖动模式下，当检测到外力分量消失时，控制所述机械臂末端在对应运动方向上停留在当前位置。In the admittance drag mode, when it is detected that the external force component disappears, the end of the robotic arm is controlled to stay at the current position in the corresponding movement direction.
根据权利要求3所述的方法，其特征在于，所述导纳阻抗模式的控制方程为：The method according to claim 3, wherein the control equation of the admittance impedance mode is:

其中，M _d、B _d和K _d分别为惯性矩阵、阻尼矩阵和刚度矩阵，F _外为机械臂末端在所述重力方向或所述另一方向上受到的外力分量；X _r2、
和
依次为机械臂末端在所述重力方向或所述另一方向上的初始位置及所述初始位置的一阶导数和二阶导数；X _c2、
和
依次为机械臂末端在所述重力方向或所述另一方向上的期望位置、期望速度和期望加速度。 Among them, M _d , B _d and K _d are the inertia matrix, damping matrix and stiffness matrix respectively, F is the _external force component received by the end of the manipulator in the direction of gravity or the other direction; X _r2 ,
and
are the initial position of the end of the manipulator in the direction of gravity or the other direction, and the first and second derivatives of the initial position; X _c2 ,
and
The order is the desired position of the end of the robot arm in the direction of gravity or the other direction, the desired velocity and the desired acceleration.
根据权利要求3所述的方法，其特征在于，所述导纳拖动模式的控制方程为：The method according to claim 3, wherein the control equation of the admittance drag mode is:

其中，F _外为机械臂末端在所述重力方向或所述另一方向上受到的外力分量；B _d为阻尼矩阵；
为机械臂末端在所述重力方向或所述另一方向上的初始位置的一阶导数；
为机械臂末端在所述重力方向或所述另一方向上的期望速度。 Among them, F _outside is the external force component received by the end of the manipulator in the direction of gravity or in the other direction; B _d is the damping matrix;
is the first derivative of the initial position of the end of the manipulator in the direction of gravity or the other direction;
is the desired velocity of the end of the robot arm in the direction of gravity or the other direction.
根据权利要求1至6中任一项所述的方法，其特征在于，所述导纳力控模式的控制方程为：The method according to any one of claims 1 to 6, wherein the control equation of the admittance force control mode is:

其中，F _f为机械臂末端在所述法线方向上反馈的实际接触力，F _d为机械臂末端在所述法线方向上的期望作用力；M _d和B _d分别为惯性矩阵和阻尼矩阵；
和
分别为机械臂末端在所述法线方向上的初始位置的一阶导数和二阶导数；
和
分别为机械臂末端在所述法线方向上的期望速度和期望加速度。 Among them, F _f is the actual contact force fed back by the end of the manipulator in the normal direction, F _d is the expected force of the end of the manipulator in the normal direction; M _d and B _d are the inertia matrix and damping, respectively matrix;
and
are the first derivative and the second derivative of the initial position of the end of the manipulator in the normal direction, respectively;
and
are the desired velocity and desired acceleration of the end of the manipulator in the normal direction, respectively.
根据权利要求3所述的方法，其特征在于，每个机械臂末端设有六维力传感器，每个机械臂末端在所述重力方向上和所述另一方向上的预设阈值的预先获取，包括：The method according to claim 3, wherein a six-dimensional force sensor is provided at the end of each manipulator, and the pre-acquisition of preset thresholds in the direction of gravity and the other direction at the end of each manipulator, include:

在控制对应机械臂末端保持与所述期望作用力相等的实际接触力的情况下，控制所有机械臂末端沿世界坐标系的Z方向向上运动预设高度，并在到达所述预设高度时，通过所述六维力传感器检测得到对应机械臂末端在所述重力方向和所述另一方向上的接触力，对应方向上的接触力作为所述预设阈值。Under the condition of controlling the corresponding end of the manipulator to maintain the actual contact force equal to the expected force, control all ends of the manipulator to move up a preset height along the Z direction of the world coordinate system, and when reaching the preset height, The contact force of the corresponding end of the mechanical arm in the gravity direction and the other direction is detected by the six-dimensional force sensor, and the contact force in the corresponding direction is used as the preset threshold.
一种机器人的人机协作控制装置，其特征在于，应用于包括多个机械臂的机器人，每个机械臂末端在不同运动方向上设有各自的控制模式，所述不同运动方向包括负载接触面的法线方向，所述法线方向设有导纳力控模式，所述多个机械臂能用于当接触负载时在所述法线方向上产生方向相反的作用力，所述装置包括：A human-robot cooperative control device for a robot is characterized in that, it is applied to a robot including a plurality of mechanical arms, and the end of each mechanical arm is provided with its own control mode in different moving directions, and the different moving directions include load contact surfaces The normal direction of , the normal direction is provided with an admittance force control mode, and the plurality of mechanical arms can be used to generate opposing forces in the normal direction when contacting the load, and the device includes:

接触控制模块，用于依据所述导纳力控模式控制对应机械臂末端在沿所述负载接触面的法线方向上的实际接触力达到期望作用力；a contact control module, configured to control the actual contact force of the corresponding manipulator end along the normal direction of the load contact surface to reach a desired force according to the admittance force control mode;

外力检测模块，用于在每个机械臂末端在沿所述法线方向上的实际接触力达到期望作用力的情况下，检测是否存在外界施加于任意机械臂末端和/或所述负载上的外力；The external force detection module is used to detect whether there is an external force applied to any end of the robot arm and/or the load when the actual contact force along the normal direction of each end of the robot arm reaches the desired force external force;

拖动响应模块，用于若存在外力，则根据每个机械臂末端在各个运动方向上受到的外力分量依据对应运动方向上的控制模式计算对应方向上的位置偏移量，根据对应机械臂在每个运动方向上的位置偏移量和受到所述外力时的初始位置控制所述对应机械臂进行运动以响应负载跟随操作。The drag response module is used to calculate the position offset in the corresponding direction according to the external force component received by the end of each robot arm in each movement direction according to the control mode in the corresponding movement direction, if there is an external force, and according to the position of the corresponding robot arm. The position offset in each movement direction and the initial position when subjected to the external force control the corresponding mechanical arm to move in response to the load following operation.
一种机器人，其特征在于，包括处理器、存储器和至少两个机械臂，所述至少两个机械臂用于协同夹持负载，所述存储器存储有计算机程序，所述计算机程序在所述处理器上执行时，实施权利要求1-8中任一项所述的机器人的人机协作控制方法。A robot, characterized in that it comprises a processor, a memory and at least two mechanical arms, the at least two mechanical arms are used for cooperatively clamping a load, the memory stores a computer program, and the computer program is used in the processing When executed on the machine, the man-machine cooperative control method of the robot according to any one of claims 1-8 is implemented.
根据权利要求10所述的机器人，其特征在于，所述机器人为双臂机器人。The robot according to claim 10, wherein the robot is a dual-arm robot.
一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施根据权利要求1-8中任一项所述的机器人的人机协作控制方法。A readable storage medium, characterized in that it stores a computer program, and when the computer program is executed on a processor, implements the human-robot cooperative control method for a robot according to any one of claims 1-8.