Robot motion control method and device

Abstract

The embodiment of the invention provides a robot motion control method and device, and relates to the technical field of robots. The method comprises the steps that under the condition that the robot is in a first dragging mode based on admittance control currently, a first translation performance index and a first rotation performance index corresponding to the current joint angle of the robot are calculated; under the condition that the current pose is close to the singular pose, first virtual force is obtained through calculation according to the first translation performance index and the first rotation performance index; judging whether the total force in the first virtual force is greater than a preset force or not and whether the total torque is greater than a preset torque or not; under the condition that the total force in the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment, calculating to obtain a target damping increment; and controlling the robot to move in the first dragging mode according to the first virtual force and the target damping increment. In this way, the robot can be prevented from being in a singular posture, and meanwhile the shaking degree of the robot possibly caused by speed changes can be weakened.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a robot motion control method and device. Background Art

[0002] Collaborative robots have been widely used in many fields due to their safety, flexibility and ease of use. In industrial manufacturing, they can perform tasks such as assembly, welding, and inspection. Unlike traditional industrial robots, collaborative robots can achieve breakthroughs in safety and flexibility through advanced sensors, algorithms and designs, and can directly collaborate with workers in the same workspace to complete production more efficiently.

[0003] For example, in the medical field, they assist in completing surgeries, help patients with rehabilitation training, and improve the quality of medical services. In addition, they are increasingly used in education, agriculture, and service industries, such as in experimental teaching, smart crop picking, and restaurant services. In all of these scenarios, robot teaching is required to record points.

[0004] The drag-to-teach method of collaborative robots is an intuitive and efficient way to program robots. Through drag-to-teach, users can directly guide the robot to move to the target position or execute a specific path by hand without writing complicated codes. This method greatly reduces the requirements for users when using robots, and even non-professionals can quickly master robot operations.

[0005] Currently, collaborative robot dragging is generally achieved through admittance control algorithms. However, in this human-machine interaction process, the robot is prone to reach a singular posture. Near a singular posture, the robot is in an unstable state, and joint overspeed may occur, affecting the user experience and may even cause danger. Summary of the invention

[0006] The embodiments of the present application provide a robot motion control method, device, electronic device and readable storage medium, which apply virtual force and adjust damping to avoid the robot from being in a singular position, while weakening the degree of robot shaking that may be caused by speed changes.

[0007] The embodiments of the present application can be implemented as follows:

[0008] In a first aspect, an embodiment of the present application provides a robot motion control method, the method comprising:

[0009] When the robot is currently in the first dragging mode based on admittance control, calculating a first translation performance index and a first rotation performance index corresponding to a current joint angle of the robot;

[0010] In the case of a current approaching singular posture, a first virtual force is calculated according to the first translation performance index and the first rotation performance index;

[0011] Determine whether the total force of the first virtual force is greater than a preset force and whether the total moment is greater than a preset moment;

[0012] When the total force of the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment, a target damping increase is calculated;

[0013] The robot is controlled to move in the first dragging mode according to the first virtual force and the target damping increase.

[0014] In a second aspect, an embodiment of the present application provides a robot motion control device, the device comprising:

[0015] An index calculation module, used to calculate a first translation performance index and a first rotation performance index corresponding to a current joint angle of the robot when the robot is currently in a first dragging mode based on admittance control;

[0016] A virtual force calculation module, configured to calculate a first virtual force according to the first translation performance index and the first rotation performance index when the current posture is close to a singular posture;

[0017] A judging module, used for judging whether the total force of the first virtual force is greater than a preset force and whether the total moment is greater than a preset moment;

[0018] a damping adjustment calculation module, configured to calculate a target damping increase when the total force in the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment;

[0019] A control module is used to control the movement of the robot in the first dragging mode according to the first virtual force and the target damping increase.

[0020] In a third aspect, an embodiment of the present application provides an electronic device, comprising a processor and a memory, wherein the memory stores machine executable instructions that can be executed by the processor, and the processor can execute the machine executable instructions to implement the robot motion control method described in the aforementioned embodiment.

[0021] In a fourth aspect, an embodiment of the present application provides a readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the robot motion control method as described in the aforementioned embodiment is implemented.

[0022] The robot motion control method, device, electronic device and readable storage medium provided by the embodiment of the present application, when currently in the first drag mode based on admittance control, calculate the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot, and then calculate the first virtual force according to the first translation performance index and the first rotation performance index when the robot is currently close to a singular posture; then, judge whether the total force in the first virtual force is greater than the preset force and whether the total torque is greater than the preset torque; when it is determined that the total force in the first virtual force is not greater than the preset force or the total torque is not greater than the preset torque, calculate the target damping increase, and control the robot motion in the first drag mode according to the first virtual force and the target damping increase. In this way, by applying virtual force and adjusting damping, the robot is prevented from being in a singular posture, and the degree of shaking of the robot that may be caused by speed changes is weakened. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0024] Figure 1 A block diagram of an electronic device provided in an embodiment of the present application;

[0025] Figure 2 One of the flow charts of the robot motion control method provided in the embodiment of the present application;

[0026] Figure 3 It is a schematic diagram that the direction of the six-dimensional force sensor is inconsistent with the direction of the end flange;

[0027] Figure 4 A second flow chart of the robot motion control method provided in an embodiment of the present application;

[0028] Figure 5 for Figure 2 A schematic flow chart of the sub-steps included in step S130;

[0029] Figure 6 The third flowchart of the robot motion control method provided in the embodiment of the present application;

[0030] Figure 7 Schematic diagram of common and unique types of collaborative robots;

[0031] Figure 8A fourth flowchart of the robot motion control method provided in an embodiment of the present application;

[0032] Fig. 9 A block diagram of a robot motion control device provided in an embodiment of the present application.

[0033] Icons: 100 - electronic device; 110 - memory; 120 - processor; 130 - communication unit; 200 - robot motion control device; 210 - index calculation module; 220 - virtual force calculation module; 230 - judgment module; 240 - damping adjustment calculation module; 250 - control module. DETAILED DESCRIPTION

[0034] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various different configurations.

[0035] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for which protection is sought, but merely represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application.

[0036] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0037] Currently, there are two ways to teach collaborative robot dragging: dragging based on current loops and dragging based on force sensors. Among them, dragging based on current loops requires the establishment of a collaborative robot dynamics model, and the dragging effect is affected by the friction of the joints, resulting in a poor dragging experience. Force sensor-based dragging requires the support of force sensors and admittance control algorithms, which directly act on the position loop of the motor; when the user applies external force, the robot can sense and adjust its movement in real time, thereby achieving a smooth and natural dragging process, and can achieve precise point-to-point teaching.

[0038] The principle of admittance control is shown in formula (1):

[0039]

[0040] Where M represents the inertia matrix; B represents the damping matrix; K represents the stiffness matrix; F(t) represents the external force information sensed by the sensor at the current moment (including external force and external force torque); x e (t) represents the current position increment generated by the admittance control algorithm; x e (t-1) represents the position increment at the previous moment generated by the admittance control algorithm; Indicates the speed at the last moment generated by the admittance control algorithm; It represents the speed at the current moment generated by the admittance control algorithm; It represents the acceleration generated by the external force information at the current moment; ΔT represents the integration time.

[0041] However, the admittance control algorithm has the following disadvantages. During human-machine interaction, the robot is prone to reach a singular position. Near a singular position, the robot is in an unstable state, and joint overspeed may occur, affecting the user experience and even causing danger. At present, most scholars have two ways to avoid this danger. Method 1: Stop the robot movement near a singular position. This method is simple and crude. Every time a singular position is reached, it is necessary to manually stay away from the singular point (i.e., the singular position), which is cumbersome. Method 2: When the robot approaches a singular position, a virtual force is applied to the end of the robot to avoid danger. Although method 2 does not require manual stay away from the singular position every time, when approaching a singular position (i.e., about to reach a singular position), the sudden application of a virtual force may cause acceleration discontinuity, resulting in violent shaking of the robot, causing users to misjudge the use of the robot, and affecting the promotion of collaborative robots. Therefore, how to avoid the singularity problem of the admittance control algorithm and seek a stable, safe, and smooth collaborative robot teaching method is a problem that scholars urgently need to solve.

[0042] In response to the above situation, the embodiments of the present application provide a robot motion control method, device, electronic device and readable storage medium, which apply virtual force and adjust damping to avoid the robot from being in a singular position, while weakening the degree of robot shaking that may be caused by speed changes.

[0043] In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0044] Please refer to Figure 1 , Figure 1 A block diagram of an electronic device 100 provided in an embodiment of the present application. The electronic device 100 may be, but is not limited to, a robot, a control device of a robot, etc. The electronic device 100 may include a memory 110, a processor 120, and a communication unit 130. The memory 110, the processor 120, and the communication unit 130 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components may be electrically connected to each other via one or more communication buses or signal lines.

[0045] The memory 110 is used to store programs or data. The memory 110 may be, but is not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), etc.

[0046] The processor 120 is used to read / write data or programs stored in the memory 110 and execute corresponding functions. For example, the memory 110 stores a robot motion control device 200, and the robot motion control device 200 includes at least one software function module that can be stored in the memory 110 in the form of software or firmware. The processor 120 executes various functional applications and data processing by running software programs and modules stored in the memory 110, such as the robot motion control device 200 in the embodiment of the present application, that is, realizing the robot motion control method in the embodiment of the present application.

[0047] The communication unit 130 is used to establish a communication connection between the electronic device 100 and other communication terminals through a network, and to send and receive data through the network.

[0048] It should be understood that Figure 1 The structure shown is only a schematic diagram of the structure of the electronic device 100. The electronic device 100 may also include Figure 1 More or fewer components as shown, or with Figure 1 Different configurations shown. Figure 1 Each component shown in the figure can be implemented by hardware, software or a combination thereof.

[0049] Please refer to Figure 2 , Figure 2 One of the flow diagrams of the robot motion control method provided in the embodiment of the present application. The method can be applied to the above-mentioned electronic device and executed by the above-mentioned electronic device. The specific process of the robot motion control method is described in detail below. In this embodiment, the method may include step S110, step S130 to step S150 and step S170.

[0050] Step S110, when the robot is currently in the first dragging mode based on admittance control, calculating a first translation performance index and a first rotation performance index corresponding to the current joint angle of the robot.

[0051] In this embodiment, when the admittance control algorithm is currently operating, that is, when the robot is currently in the first drag mode based on admittance control, the first translation performance index (also referred to as the first translation performance index) and the first rotation performance index corresponding to the robot can be calculated according to the joint angles of the current joints of the robot. The robot can be a collaborative robot, specifically a six-degree-of-freedom collaborative robot or a collaborative robot with other degrees of freedom. Among them, the first translation performance index and the first rotation performance index can be calculated by the following formula (2). Formula (2) divides the Jacobian matrix into the translation direction (i.e., the translation direction) and the rotation direction, thereby realizing the decoupling of the translation and rotation directions.

[0052]

[0053] in, represents the Jacobian matrix; represents the pseudo-inverse of the Jacobian matrix in the translation direction; represents the pseudo-inverse of the rotation direction Jacobian matrix; represents the transpose of the Jacobian matrix in the translation direction; represents the transpose of the Jacobian matrix in the rotation direction; I represents the identity matrix; w T (q) represents the first translation performance index; w R(q) represents the first rotation performance index.

[0054] Step S130: When the current posture is close to a singular posture, a first virtual force is calculated according to the first translation performance index and the first rotation performance index.

[0055] Whether the robot is currently close to a singular posture may be determined in any manner. For example, whether the robot is currently close to a singular posture may be determined based on human input, or the current state of the robot may be analyzed to determine whether the robot is currently close to a singular posture, etc. When it is determined that the robot is currently close to a singular posture and the first translation performance index and the first rotation performance index of the robot are obtained, the first virtual force may be calculated based on the first translation performance index and the first rotation performance index.

[0056] Step S140, determining whether the total force magnitude of the first virtual force is greater than a preset force and whether the total torque magnitude is greater than a preset torque.

[0057] Step S150: When the total force in the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment, a target damping increase is calculated.

[0058] Step S170: controlling the movement of the robot in the first dragging mode according to the first virtual force and the target damping increase.

[0059] In this embodiment, when the first virtual force is obtained, it can be determined whether the total force of the first virtual force is greater than the preset force, and whether the total torque of the first virtual force is greater than the preset torque. The total force of the first virtual force is determined based on the forces in each direction of the first virtual force, and the total torque of the first virtual force is determined based on the torque in each direction of the first virtual force. The magnitude of the preset force and the preset torque can be determined in combination with actual needs, and are not specifically limited here.

[0060] When the total force of the first virtual force is greater than the preset force or the total torque of the first virtual force is greater than the preset torque, the first virtual force plays a leading role and is used to stay away from the singular posture. That is, when the total force of the first virtual force is greater than the preset force, there is no need to adjust the damping corresponding to the translation direction; when the total torque of the first virtual force is greater than the preset torque, there is no need to adjust the damping corresponding to the rotation direction. If the total force of the first virtual force is greater than the preset force and the total torque of the first virtual force is greater than the preset torque, the robot movement can be controlled in the first dragging mode only according to the first virtual force, without adjusting the damping.

[0061] When the total force in the first virtual force is not greater than the preset force or the total torque in the first virtual force is not greater than the preset torque, adaptive damping is also introduced: the target damping increase is calculated, and the damping used in admittance control (i.e., adaptive damping) is calculated based on the target damping increase and the original damping, and then the robot movement is controlled in the first dragging mode based on the first virtual force and the calculated damping used in admittance control.

[0062] It can be seen from the above formula (1) that the admittance control is to obtain the current speed by the speed and integration of the previous moment. When it is determined that the current position is close to a singular posture, a virtual force will be generated, and the direction of the virtual force generated is opposite to the direction of the current speed, otherwise the robot will move to a singular posture. If the virtual force is directly applied at this time, the admittance control will have a sudden acceleration change, which will cause the speed to change, resulting in violent shaking of the robot. Moreover, the faster the current speed, the more obvious the shaking phenomenon. The reason for the above situation is that the speed at the current moment needs to be reversed instantly, but the speed at the previous moment needs to decay to 0, and then accelerate in the reverse direction, which will cause violent shaking.

[0063] In this embodiment, a virtual force threshold (i.e., the preset force and preset torque) and adaptive damping are introduced to quickly attenuate the speed at the previous moment, thereby weakening the vibration phenomenon. Even if the current speed generated by the admittance control is too large, it can still play a suppressive role.

[0064] In this embodiment, the six-axis force sensor can be first installed on the end flange of the robot, and the zero point calibration of the force sensor can be completed. In addition, when the direction of the six-axis force sensor is inconsistent with the direction of the end flange, for example Figure 3 When the state is shown, the transformation relationship [xy za bc] from the force sensor coordinate system to the terminal coordinate system (i.e., the terminal flange coordinate system) can be obtained, where x, y, and z represent the translation relationship between the coordinate systems, a represents the rotation angle around the x-axis, b represents the rotation angle around the y-axis, and c represents the rotation angle around the z-axis. Based on the above content, the homogeneous transformation matrix from the force sensor coordinate system to the terminal coordinate system can be established: It can be shown as formula (3).

[0065]

[0066] in, represents the rotation matrix from the force sensor coordinate system to the end flange coordinate system, Represents the translation matrix from the force sensor coordinate system to the end flange coordinate system.

[0067] During normal dragging based on the force sensor, the sensor data detected by the six-dimensional force sensor (used to indicate external force information) and the above homogeneous transformation matrix can be used to The sensor data in the terminal coordinate system is obtained, and then the robot motion is controlled based on the sensor data in the terminal coordinate system. For example, the robot motion is made through admittance control according to the sensor data in the terminal coordinate system.

[0068] For example, suppose the data in the force sensor coordinates is F S =[F x_S F y_S F z_S M x_S M y_S M z_S ] T , then the sensor data in the terminal coordinate system is F E . F E As shown in formula (4).

[0069]

[0070] Among them, 0∈R 3×3 is a zero matrix.

[0071] In the operation based on the admittance control algorithm, the first translation performance index w corresponding to the current position of the robot can be calculated based on the above formula (2): T (q) and the first rotation performance index w R (q).

[0072] As a possible implementation, Figure 4 As shown, the first translation performance indicator w T (q) and the first rotation performance index w R (q) Determine whether the robot is currently approaching a singular posture. Figure 4 , Figure 4 The second flowchart of the robot motion control method provided in the embodiment of the present application. In this embodiment, after step S110, the method may further include steps S121 to S124.

[0073] Step S121, determining whether the first translation performance index is within a preset translation performance index range.

[0074] Step S122, determining whether the first rotation performance index is within a preset rotation performance index range.

[0075] If the first translation performance indicator is not within the preset translation performance indicator range and the first rotation performance indicator is not within the preset rotation performance indicator range, step S123 is executed.

[0076] Step S123, determining that the current position is not close to a singular posture.

[0077] If the first translation performance index is within the preset translation performance index range or the first rotation performance index is within the preset rotation performance index range, step S124 is executed.

[0078] Step S124, determining the current approaching singular posture.

[0079] In this embodiment, the possible singular postures of the robot can be analyzed and determined according to the number of joints of the robot, and then the minimum value w of the preset translation performance index range can be determined according to the translation performance index and rotation performance index corresponding to the different singular postures of the robot. Tmin and the minimum value w of the preset rotation performance index range Rmin ; and, controlling the robot to move from different possible singular postures toward the direction away from the singular posture, and determining the maximum value w of the preset translation performance index range by the corresponding translation performance index and rotation performance index Tmax and the maximum value w of the preset rotation performance index range Rmax Among them, the minimum value of the translation performance indicators corresponding to different singular postures can be used as the minimum value of the preset translation performance indicator range, and the minimum value of the rotation performance indicators corresponding to different singular postures can be used as the minimum value of the preset rotation performance indicator range; the maximum value of the translation performance indicators corresponding to the possible different singular postures moving in the direction away from the singular posture can be used as the maximum value of the preset translation performance indicator range; similarly, the maximum value of the rotation performance indicators corresponding to the possible different singular postures moving in the direction away from the singular posture can be used as the maximum value of the preset rotation performance indicator range.

[0080] As a possible implementation, the robot is a six-degree-of-freedom collaborative robot. Considering the shoulder singularity, elbow singularity and wrist singularity of the collaborative robot, the translation performance index and rotation performance index corresponding to the shoulder singularity, elbow singularity and wrist singularity can be measured through advance experiments, and the minimum value of the above translation performance index is used as the minimum value of the preset translation performance index range, and the minimum value of the above rotation performance index is used as the minimum value of the preset rotation performance index range. And, the robot is moved from the shoulder singularity to the direction away from the shoulder singularity, and the translation performance index and rotation performance index after the movement are obtained; the robot is moved from the elbow singularity to the direction away from the elbow singularity, and the translation performance index and rotation performance index after the movement are obtained; the robot is moved from the wrist singularity to the direction away from the wrist singularity, and the translation performance index and rotation performance index after the movement are obtained; then, the maximum value of the translation performance index after the above three movements is used as the maximum value of the preset translation performance index range, and the maximum value of the above rotation performance index is used as the maximum value of the preset rotation performance index range.

[0081] Set the minimum value w of the preset translation performance index range Tmin As the second translation performance index, the maximum value w of the preset translation performance index range is Tmax As the third translation performance index, and the minimum value w of the preset rotation performance index range Rmin As the third rotation and translation performance index, the maximum value w of the preset rotation performance index range is Rmax As the third translation performance indicator.

[0082] In the first translation performance index w T (q) is greater than the third translation performance index w Tmax When w T (q)>w Tmax When the virtual force in the translation direction is not generated; in the first rotation performance index w R (q) is greater than the third rotation performance index w Tmax When w R (q)>w Rmax Therefore, if the first translation performance index is not within the preset translation performance index range and the first rotation performance index is not within the preset rotation performance index range, it can be determined that the robot is currently in an area where dragging is allowed and can be operated at will. The robot is not currently close to a singular posture and does not need to calculate the first virtual force. At this time, normal dragging can be performed based on the force sensor.

[0083] In the first translation performance index w T (q) Within the preset translation performance index range, that is, w Tmin <w T (q)≤w Tmax When , a virtual force in the translation direction needs to be generated to prevent the robot from reaching a singular posture. R (q) within the preset rotation performance index range, that is, w Rmin <w R (q)≤w Rmax When the robot reaches a singular position, a virtual force in the rotation direction needs to be generated to prevent the robot from reaching a singular position. That is, if the first translation performance index is within the preset translation performance index range or the first rotation performance index is within the preset rotation performance index range, the current approaching singular position is determined and the first virtual force is calculated.

[0084] As a possible implementation, Figure 5 The first virtual force is calculated by the method shown. Figure 5 , Figure 5 for Figure 2Schematic diagram of the flow of sub-steps included in step S130. In this embodiment, step S130 may include sub-steps S131 to S134.

[0085] Sub-step S131, for each direction in the Cartesian space, according to the unit forward and reverse speeds in that direction, the preset duration and the current joint angle corresponding to that direction, calculate the first joint angle and the second joint angle at the next moment.

[0086] Sub-step S132, obtaining a first performance indicator corresponding to the first joint angle and a second performance indicator corresponding to the second joint angle.

[0087] Sub-step S133, obtaining a gradient direction value corresponding to the direction according to the first performance index and the second performance index corresponding to the direction.

[0088] Sub-step S134, calculating the first virtual force according to the gradient direction values ​​corresponding to each direction of the Cartesian space, the first translation performance index and the first rotation performance index.

[0089] In this embodiment, the corresponding control index can be calculated based on the first translation performance index and the first rotation performance index, and the gradient direction of the first virtual force can be calculated, and then the first virtual force can be obtained based on the control index and the gradient direction of the first virtual force.

[0090] Optionally, the first virtual force f1 may be calculated based on formula (5). The virtual force calculated by formula (5) is nonlinear, and the closer to the singular posture, the greater the virtual force generated, and vice versa, the smaller it is at the beginning.

[0091] f1=k(w)*A(q) (5)

[0092] Wherein, k(w) represents the control index, and k(w) includes two values: T (q) corresponding to k T (w) and the first rotation performance index w R (q) corresponding to k R (w), k(w) is determined by the current performance index, the maximum value and the minimum value corresponding to the current performance index; A(q) represents the gradient direction of the first virtual force. When calculating according to formula (5), it can be based on k T (w) and the components corresponding to the translation direction in A(q), and based on k R (w) and the components in A(q) corresponding to the rotation direction are calculated to obtain a first virtual force f1 having the same number of dimensions as A(q).

[0093] k(w) can be calculated based on formula (6).

[0094]

[0095] Among them, λ represents the control gain. The larger λ is, the more drastic the curve changes.

[0096] The gradient direction of the first virtual force can be expressed as A(q)=[A x A y A z A rx A ry A rz ]. The admittance control algorithm works in Cartesian space, and the performance index calculation requires joint angles. Therefore, the unit forward and reverse speed can be generated according to the direction of Cartesian space to calculate the robot joint angle at the next moment, thereby determining the direction in which the admittance control algorithm should control the robot to move.

[0097] In this embodiment, firstly, taking the X direction of the Cartesian space as an example, how to obtain the gradient direction value A in the X direction of the Cartesian space is introduced. x .

[0098] Based on formula (7), the first joint angle and the second joint angle at the next moment can be calculated according to the unit forward and reverse speeds in the X direction of the Cartesian space, the preset time length, and the current joint angle corresponding to the X direction of the Cartesian space (i.e., the current joint angle of the end joint).

[0099] q ± =q0+J -1 V x± Δt (7)

[0100] Where J represents the Jacobian matrix, which is used to convert the terminal linear velocity (in formula (7), the terminal linear velocity is V x± ) is converted into joint velocity; q0 represents the current joint angle; Δt represents the integration time, which can be determined according to actual needs, for example, 1ms; V x± =[±100000] T Indicates the unit forward and reverse velocity in the X direction of Cartesian space; q ± It represents the robot joint angle caused by the unit velocity, that is, the first joint angle and the second joint angle corresponding to the X direction of the Cartesian space at the next moment.

[0101] Afterwards, the first performance index can be calculated based on the first joint angle corresponding to the next moment in the X direction of the Cartesian space, and the second performance index can be calculated based on the second joint angle corresponding to the next moment in the X direction of the Cartesian space. ± Calculate the performance index w T (q± ). Then, based on the performance indicator w T (q ± ) Calculate the gradient direction value A corresponding to the X direction of Cartesian space x .

[0102] Optionally, the gradient direction value A corresponding to the X direction in the Cartesian space can be calculated as follows: x .

[0103] The first joint angle is an angle corresponding to a unit positive direction velocity, and the second joint angle is an angle corresponding to a unit negative direction velocity.

[0104] When the first performance index is less than the corresponding value or the second performance index is less than the corresponding value, the gradient direction value of the X direction of the Cartesian space is determined to be 0. The corresponding value is the index corresponding to the first translation performance index and the first rotation performance index. For example, if the first performance index corresponds to the translation direction, that is, to the X, Y, and Z directions, the corresponding value of the first performance index is the first translation performance index; if the first performance index corresponds to the rotation direction, that is, to the RX, RY, and RZ directions, the corresponding value of the first performance index is the first rotation performance index.

[0105] In the case where a first difference between the first performance indicator and the corresponding value is not less than a second difference between the second performance indicator and the corresponding value, a gradient direction value in the X direction of the Cartesian space is obtained based on the first difference. In the case where the first difference is less than the second difference, a gradient direction value in the X direction of the Cartesian space is obtained based on the second difference.

[0106] That is, the gradient direction value A in the X direction of the Cartesian space can be obtained by the following formula (8): x .

[0107]

[0108] Among them, w T (q + ) represents the first performance index corresponding to the positive direction speed of the X-axis in Cartesian space, w T (q - ) represents the second performance index corresponding to the unit reverse speed of the X-axis in the Cartesian space; (w T (q + )-w T (q)) represents the first difference; (w T (q - )-w T (q)) represents the second difference.

[0109] Afterwards, the unit forward and reverse velocities in the Y direction, Z direction, RX direction, RY direction and RZ direction in the Cartesian space are calculated in sequence to determine the gradient direction A(q) of the first virtual force.

[0110] The first virtual force can be expressed as: f1=[f x1 f y1 f z1 f rx1 f ry1 f rz1 ], assuming the preset force is f 1T , the preset torque is f 1R , it can be determined in the following way whether the total force in the first virtual force is greater than the preset force, whether the total torque in the first virtual force is greater than the preset torque, and then according to the relationship between the total force in the first virtual force and the preset force, or the relationship between the total torque in the first virtual force and the preset torque, whether it is necessary to calculate the target damping increase, and when it is necessary to calculate the target damping increase, which dimensions of the target damping increase need to be calculated, and the dimensions that need to be calculated are the target dimensions.

[0111] Can be judged as well as Is it established? When , it is determined that the total force of the first virtual force is greater than the preset force, and the force in the first virtual force f1 plays a leading role and is used to stay away from the singular posture, without adjusting the damping corresponding to the translation direction. When , it is determined that the total torque in the first virtual force is greater than the preset torque, and the torque in the first virtual force f1 plays a leading role and is used to stay away from the singular posture, without adjusting the damping corresponding to the rotation direction.

[0112] exist , the damping corresponding to the translation direction needs to be adjusted, that is, the target damping increase needs to be calculated, and the X, Y, and Z directions in the target damping increase are the target dimensions of the values ​​to be calculated, that is, the components of the X, Y, and Z directions in the target damping increase need to be calculated. , it is necessary to adjust the damping corresponding to the rotation direction, that is, it is necessary to calculate the target damping increase, and the RX, RY, and RZ directions in the target damping increase are the target dimensions of the values ​​to be calculated, that is, the components of the RX, RY, and RZ directions in the target damping increase need to be calculated.

[0113] From this we can see that or , it is necessary to calculate the target damping increase, and then introduce adaptive damping. The adaptive damping is calculated based on the original damping and the calculated target damping increase. Wherein, when the target damping increase needs to be calculated, if there is no need to adjust the damping corresponding to the translation direction, the values ​​of the dimensions corresponding to the translation direction (i.e., X, Y, and Z directions) in the target damping increase are 0, that is, the component corresponding to the translation direction in the target damping increase is 0, and only the values ​​of the dimensions corresponding to the rotation direction (i.e., RX, RY, and RZ directions) in the target damping increase need to be calculated; similarly, if there is no need to adjust the damping corresponding to the rotation direction, the values ​​of the dimensions corresponding to the rotation direction (i.e., RX, RY, and RZ directions) in the target damping increase are 0, that is, the component corresponding to the rotation direction in the target damping increase is 0, and only the values ​​of the dimensions corresponding to the translation direction (i.e., X, Y, and Z directions) in the target damping increase need to be calculated.

[0114] When calculating the target damping increase, for each target dimension in the target damping increase, when the product of the force or torque corresponding to the target dimension in the first virtual force and the current speed corresponding to the target dimension is greater than 0, the component of the target dimension in the target damping increase is determined to be 0. Wherein, when the total force in the first virtual force is not greater than the preset force, the target dimension includes the dimension corresponding to the translation direction, and when the total torque in the first virtual force is not greater than the preset torque, the target dimension includes the dimension corresponding to the rotation direction. The current speed corresponding to a target dimension can be the speed of the robot terminal speed in the target dimension.

[0115] When the product of the force or torque corresponding to the target dimension in the first virtual force and the current speed corresponding to the target dimension is less than 0, the component of the target dimension in the target damping increase is calculated according to the current speed corresponding to the target dimension. The larger the current speed, the larger the component.

[0116] The following takes the X direction as a target dimension as an example to introduce how to obtain the X direction component of the target damping increase.

[0117] Assume that the current Then, it is determined that the value of the component corresponding to the translation direction in the target damping increase needs to be calculated, that is, the X, Y, and Z directions in the target damping increase are the target dimensions, and the values ​​of the X, Y, and Z directions in the target damping increase need to be calculated. Taking the X direction as an example, the value of the X direction in the target damping increase can be calculated by the following formula (9).

[0118]

[0119] Among them, k is a constant used to control the overall decay speed; β>1, the faster the current speed, the greater the damping generated, causing it to decay quickly.

[0120] When the above formula (9) is used, when the force in the X direction of the first virtual force is equal to the current velocity v in this direction x If (t) is in the opposite direction, the value in the X direction of the target damping increase is greater than 0, thereby increasing the damping used in the admittance control to reduce the current speed of the admittance control.

[0121] After obtaining the target damping increase, the original damping used in the admittance control algorithm can be processed according to the target damping increase to obtain adaptive damping, and according to the adaptive damping and the first virtual force, the robot movement is controlled to avoid singular postures when using the admittance control algorithm. Optionally, the first virtual force can be directly controlled as an external force detected in the admittance control algorithm (i.e., as the external force F in the above formula (1)). In this embodiment, the default singular posture processing strategy is avoidance. When the singular posture processing strategy is avoidance, the above steps S110, S130 to S150, and S170 can be executed.

[0122] The admittance control algorithm may cause the robot to reach a singular position, making the robot unstable, which can be avoided by applying a virtual force. However, the virtual force is opposite to the force of the actual force sensor. Under the action of the admittance control algorithm, according to Newton's second law, it will cause a sudden change in the robot's acceleration and severe shaking. In view of this situation, in this embodiment, adaptive damping and virtual force buffers are set, which not only avoid the robot shaking caused by the sudden change of virtual force, but also prevent the avoidance algorithm from failing due to excessive actual force.

[0123] To increase flexibility, in this embodiment, two singular posture processing strategies can be set: singular posture crossing (also called singular point crossing) and singular posture avoiding (also called singular point avoiding). A singular posture processing strategy can be automatically selected, and then the process is performed according to the strategy when approaching a singular posture; or a singular posture processing strategy can be selected by the user, and then the process is performed according to the strategy when approaching a singular posture.

[0124] Please refer to Figure 6 , Figure 6 The third flow chart of the robot motion control method provided in the embodiment of the present application. In this embodiment, the method may also include step S101 and step S182. Wherein, when the robot is currently in the first drag mode based on admittance control, step S101 is performed before calculating the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot.

[0125] Step S101, determining whether the current singular posture processing strategy is avoidance or crossing.

[0126] When the robot is currently in the first dragging mode based on admittance control and the current singular posture processing strategy is avoidance, the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot can be calculated, and then steps S130 to S150 and step S170 are executed.

[0127] When the current mode is the first dragging mode based on admittance control and the current singular posture processing strategy is crossing, step S182 can be executed.

[0128] Step S182, when the current singular posture processing strategy is crossing, if it is determined that the current posture is close to a singular posture, the current mode is switched from the first drag mode to the second drag mode based on the current loop, and the robot movement is controlled in the second drag mode.

[0129] When the robot is currently in the first drag mode based on admittance control and the current singular posture processing strategy is crossing, it can be determined in any way whether the robot is currently close to a singular posture. For example, it can be determined based on human input whether the robot is currently close to a singular posture, or the current state of the robot can be analyzed to determine whether the robot is currently close to a singular posture, etc. For example, it can be judged based on the aforementioned first translation performance index and the first rotation performance index. If it is determined that the robot is currently close to a singular posture, the current mode can be switched from the first drag mode based on admittance control to the second drag mode based on the current loop, and then the robot movement can be controlled in the second drag mode to cross the singular posture.

[0130] Please refer again Figure 6 , after step S182, the method may further include step S183.

[0131] Step S183, after switching to the second drag mode, when it is determined that the robot is far away from the singular posture, the current mode is switched from the second drag mode to the first drag mode, and the robot is controlled to move in the first drag mode.

[0132] After switching to the second drag mode, if it is determined through control that the robot is away from the singular posture, the current mode can be switched from the second drag mode based on the current loop to the first drag mode based on admittance control, and then the robot movement can continue to be controlled in the first drag mode.

[0133] Please refer again Figure 6As a possible implementation, when the current singular posture processing strategy is crossing, the method may further include step S181, which can be Figure 6 Step S181 in the process determines whether the robot is currently approaching a singular posture.

[0134] Step S181, judging whether the robot is currently close to a singular posture according to the current joint state of the robot and a joint angle range or a preset distance corresponding to at least one singular posture.

[0135] like Figure 7 As shown in the figure, there are three common types of singularities in collaborative robots: elbow singularity, wrist singularity, and shoulder singularity. The elbow singularity and wrist singularity of collaborative robots correspond to the angles of joints 3 and 5 being close to 0, respectively. Shoulder singularity is caused by the fact that the center of the wrist joint is collinear with the axis of the first joint. Whether it is close to shoulder singularity can be determined based on the distance m. The distance m represents the distance between the midpoint 1 of the line connecting the centers of joints 5 and 6 and the midpoint 2 of the line connecting the centers of joints 1 and 2. The distance m can be calculated using the following formula (10).

[0136]

[0137] Among them, d6 represents the translation displacement of the 6th joint along the Z direction of the 5th joint; d4 represents the translation displacement of the 4th joint along the Z direction of the 3rd joint.

[0138] The homogeneous transformation matrix of the robot's end coordinate system relative to the base coordinate system is shown in formula (11).

[0139]

[0140] Where n = [nx ny nz] T Represents the projection of the terminal coordinate system on the X-axis of the base coordinate system; o = [ox oy oz] T Represents the projection of the Y axis of the terminal coordinate system on the base coordinate system; a = [ax ay az] T Represents the Z-axis projection of the terminal coordinate system on the base coordinate system; p = [px py pz] T Indicates the position of the origin of the terminal coordinate system in the base coordinate system.

[0141] In this embodiment, based on the above analysis, the joint angle ranges or preset distances corresponding to the various singular postures that the robot may have can be determined according to the various singular postures that the robot may have, and then based on the current joint state of the robot and the joint angle range or preset distance corresponding to at least one singular posture, it is possible to accurately judge whether the robot is currently close to a singular posture.

[0142] For example, if a singular posture can be determined based on the range of joint angles, then the relationship between the current angle of the robot and the range of joint angles can be used to determine whether the robot is currently close to the singular posture. Similarly, if a singular posture can be determined based on distance, then the relationship between the actual distance of the robot and the preset distance can be used to determine whether the robot is currently close to the singular posture.

[0143] For example, the robot is a six-degree-of-freedom collaborative robot. The joint angle range corresponding to the elbow singularity can be predetermined as the joint angle range of joint 3. If the joint angle q3 of the current joint 3 of the robot is within the preset joint angle range of joint 3, that is, |q3|≤q 3lim , then it is determined that the robot is currently approaching a singular posture, where -q 3lim ~+q 3lim Indicates the joint angle range of joint 3 corresponding to the elbow singularity. Similarly, the joint angle range corresponding to the wrist singularity can be predetermined as the joint angle range of joint 5. If the joint angle q5 of the current joint 5 of the robot is within the preset joint angle range of joint 5, that is, |q5|≤q 5lim , then it is determined that the robot is currently approaching a singular posture, where -q 5lim ~+q 5lim The joint angle range of the wrist singular joint 5 is shown in FIG. 1 . Similarly, the preset distance m corresponding to the shoulder singular joint can be determined in advance. min , if the robot's current distance m is less than the preset distance m min , that is, m≤m min , it is determined that the robot is currently approaching a singular posture.

[0144] When the current mode is the first drag mode, and it is determined that the current mode is close to a singular posture, and the current singular posture is processed as a crossing, it can be directly switched to the second drag mode based on the current loop.

[0145] In the second dragging mode based on the current loop, it is necessary to carry out dynamic modeling of the robot, and the model is shown in formula (12). The dragging effect is easily affected by friction.

[0146]

[0147] Among them, τ is the connecting rod torque obtained by dynamic calculation, which means that the robot is in the current loop drag mode, and the joint torque needs to be applied to maintain the current posture; q is the rotation angle of the connecting rod end; M(q) is the inertia matrix; are the Coriolis force and the centrifugal force, and G(q) represents the gravity term.

[0148] By transforming formula (12), in the drag mode, the model of the motor end can be shown as formula (13).

[0149] τ m =τ+τ f (13)

[0150] Among them, τ m is the motor driving torque; τ f is the joint friction torque;

[0151] In the second drag mode based on the current loop, the item that is difficult to model is the external force that the user needs to apply when using it. As a possible implementation method, in the second drag mode, the control law shown in formula (14) can be used to avoid the influence of external forces such as friction and realize current loop drag.

[0152]

[0153] Among them, λ τ is the drag gain.

[0154] The external force is detected by force sensor, converted to the joint through force Jacobian matrix, and then combined with the dynamic model, formula (14) can be organized into formula (15).

[0155]

[0156] Among them, F E It is the data of the force sensor in the end flange coordinate system.

[0157] At this point, dragging based on the current loop can be achieved.

[0158] The drag effect can be determined by the parameter λ τ To determine, the parameter λ can be set according to actual needs. τ The value of λ τ,i = 0, the dragging force is resisted, and the corresponding joint i of the robot cannot be dragged, achieving the joint locking effect. τ,i <1, the robot is difficult to drag, and λ τ,i The smaller it is, the harder it is to drag. τ,i =1, there is no compensation effect, which is equivalent to the drag effect in normal torque mode. τ,i >1, the drag force is compensated to improve the dragging experience, and λ τ,i The larger it is, the easier it is to drag.

[0159] In this embodiment, after switching to the second drag mode for control, if it is determined that the robot is currently far away from the singular posture, the second drag mode can be switched to the first drag mode for control. Correspondingly, whether the robot is currently far away from the singular posture can be determined based on the method of approaching the singular posture used when switching to the second drag mode. For example, if the angles of joints 3 and 5 are not within the corresponding preset angle range, that is, |q3|≤q 3lim 、|q5|≤q 5lim Neither is true, and the distance m is not less than the preset distance m min , that is, m≤m min If it is not true, it is determined that the current position is far away from the singular posture, and the second drag mode can be switched to the first drag mode, and normal dragging based on the force sensor can be performed in the first drag mode.

[0160] Since the joint encoders of collaborative robots have high precision, in order to prevent the back-and-forth jump between the two modes and affect the dragging experience, when making a mode switching judgment, not only the current position is far away or close to the singular posture, but also the linear velocity and angular velocity of the robot end (i.e. the robot TCP (Tool Center Point) linear velocity and angular velocity) are considered. That is, whether to switch between the first dragging mode and the second dragging mode is determined based on whether the current position is far away or close to the singular posture and the linear velocity and angular velocity of the robot end.

[0161] Optionally, when the robot is currently in the first drag mode, if the robot is currently close to a singular posture, the linear velocity of the robot end is less than a preset linear velocity, and the angular velocity of the robot end is less than a preset angular velocity, the first drag mode can be switched to the second drag mode, that is, the admittance control mode is switched to the current loop drag mode, otherwise no switching is performed. If the linear velocity of the robot end is less than the preset linear velocity, and the angular velocity of the robot end is less than the preset angular velocity, the robot can be considered to be stationary; if the linear velocity of the robot end is not less than the preset linear velocity, or the angular velocity of the robot end is not less than the preset angular velocity, the robot can be considered to be moving.

[0162] It is also possible to switch from the second drag mode to the first drag mode when the robot is currently in the second drag mode, that is, the current loop drag mode is switched to the admittance control mode, if the robot is currently far away from the singular posture and the linear velocity of the robot end is not less than the preset linear velocity or the angular velocity of the robot end is not less than the preset angular velocity. Otherwise, no switching is performed.

[0163] The role of the admittance control algorithm is to achieve smooth human-machine interaction, that is, to achieve drag teaching. Taking the elbow singularity of the collaborative robot as an example, the strategy of singularity avoidance is adopted. There is always an angle difference between the 2nd and 3rd axes, which constrains the workspace of the collaborative robot. In this embodiment, the strategy of singular posture crossing is adopted, so that the robot can be dragged to teach anywhere in the workspace.

[0164] For the processing strategy of the admittance control algorithm near the singular position, in this embodiment, two strategies, avoidance and crossing, are provided for selection. In the avoidance strategy, in order to avoid the severe jitter and directional coupling problems existing in the current common processing methods, the Jacobian matrix is ​​divided into two directions, translation (i.e., translation) and rotation (i.e., rotation), and processed separately. Moreover, since the inverse of the Jacobian matrix does not exist or has low accuracy near the singular position, the pseudo-inverse of the Jacobian matrix is ​​used to calculate the direction of the virtual force. At the same time, adaptive damping and virtual force thresholds are introduced. When the virtual force is generated and does not exceed the threshold, the damping coefficient of the control algorithm is adjusted according to the current speed of the admittance control to make the speed decay rapidly; when the virtual force is greater than the threshold, the virtual force plays a leading role and stays away from the singular point.

[0165] When the crossing strategy is selected, the mode will be switched according to the joint angle of the robot. When the joint angle is less than the designed joint angle threshold, the admittance control switches to the current loop-based drag mode. At the same time, a drag control law based on the current loop is designed, and a drag gain is introduced to adjust the drag effect.

[0166] The collaborative robot has joint hard limits. Under the admittance control algorithm, the joints are at risk of reaching the hard limits. In this embodiment, in order to avoid the robot reaching the joint hard limits, a second virtual force is applied when approaching the hard limits to prevent the robot from reaching the hard limits and causing damage to the robot.

[0167] Please refer to Figure 8 , Figure 8 The fourth flowchart of the robot motion control method provided in the embodiment of the present application. In this embodiment, the method may further include step S162.

[0168] Step S162: When the joint angle of the target joint of the robot is currently in the hard limit buffer zone, a second virtual force for constraining the robot to reach the joint hard limit zone is calculated.

[0169] In this embodiment, the hard limit buffer is used to indicate the angle area that is about to reach the hard limit of the joint. It can be determined in any way whether the robot currently has a target joint whose joint angle is in the hard limit buffer. If the robot currently does not have a target joint whose joint angle is in the hard limit buffer, the second virtual force may not be calculated. If the robot currently has a target joint whose joint angle is in the hard limit buffer, the second virtual force used to constrain the robot to reach the joint hard limit may be calculated. Among them, the target joint is a pre-specified joint that needs to be judged whether it is in the hard limit buffer. The specific amount can be determined in combination with actual needs. For example, it can be set to each joint of the robot, or it can be set to some of the joints of the robot.

[0170] When the second virtual force and the first virtual force are calculated, the movement of the robot can be controlled in the first drag mode according to the first virtual force, the second virtual force and the target damping increase.

[0171] Please refer again Figure 8 In this embodiment, before step S162, the method may further include step S161.

[0172] Step S161, for each target joint of the robot, judging whether the current joint angle of the target joint is in the hard limit buffer zone according to the current joint angle of the target joint.

[0173] In this embodiment, the hard limit angle q of each joint of the robot can be obtained imin ,q imax , i corresponds to the number of joints of the robot. For example, if the robot has six degrees of freedom, then i = 1, 2, 3, 4, 5, 6. The joints are at two hard limit angles q imin ,q imax Based on the hard limit angle q of each joint imin ,q imax The hard limit buffer [q imin ,q imin +Δq] and [q imax -Δq,q imax ], Δq is a preset value, which can be set according to actual needs. For each target joint of the robot, it can be determined whether the current joint angle of the target joint is within the corresponding hard limit buffer [q imin ,q imin +Δq], [q imax -Δq,q imax ].

[0174] For each target joint whose current joint angle is in the hard limit buffer zone, the joint space virtual force corresponding to the target joint can be calculated based on the current joint angle of the target joint and the hard limit buffer zone in which it is located, and then the second virtual force can be calculated based on the joint space virtual forces corresponding to each target joint of the robot.

[0175] The following takes joint 6 as an example to introduce how to obtain the direction and magnitude of the joint space virtual force of a target joint.

[0176] The joint space virtual force corresponding to joint 6 can be calculated by formula (16).

[0177]

[0178] Among them, α is the virtual force gain.

[0179] The calculated joint space virtual force can be transformed to the base coordinate system using the force Jacobian matrix. For example, the joint space virtual force f6 corresponding to joint 6 is transformed to the base coordinate system using formula (17), and f 6base .

[0180] f 6base =(J T ) -1 n,6 f6 n=1,2,3,4,5,6 (17)

[0181] Among them, (J T ) -1 n,6 Represents the sixth column of the matrix.

[0182] The virtual force in the base coordinate system is transformed from the base coordinate system to the end flange coordinate system. For example, f is converted into 6base Convert to the terminal coordinate system and get f 6end .

[0183]

[0184] in, is the rotation matrix of the end coordinate system relative to the base coordinate system.

[0185] If the current joint angle of a target joint is not in the hard limit buffer, the joint space virtual force corresponding to the target joint can be directly determined to be 0. Finally, the second virtual force f2 in the base coordinate system can be calculated based on the joint space virtual forces corresponding to each target joint of the robot. When the current robot has a target joint in the hard limit buffer, is currently close to a singular posture and needs to adjust the damping, the robot movement can be controlled in the first drag mode based on the first virtual force f1, the second virtual force f2, and the target damping increase. In this way, by adding the virtual force generated by the hard limit of the joint to the virtual force generated in the singular posture area, and acting together on the admittance control algorithm, the robot is kept away from the singular posture and hard limit points, avoiding the instability of the robot system caused by the above phenomenon.

[0186] In this embodiment, when the robot is not currently close to a singular posture but the joint angles of the joints are in the hard limit buffer, the robot can be controlled to move in the first drag mode according to the second virtual force. When it is determined that the total force in the first virtual force is greater than the preset force and the total torque is greater than the preset torque, the robot can be controlled to move in the first drag mode according to the first virtual force and the second virtual force.

[0187] In the above method, in order to avoid the virtual force generated by the Cartesian space constraint from conflicting with the virtual force in the joint space, only the rotation direction of the collaborative robot is constrained. In this case, the target joint does not include joints that can be in a singular position and corresponds to the rotation direction. For example, when the robot is a six-degree-of-freedom collaborative robot, joints 4, 5, and 6 correspond to the rotation direction (i.e., the rotation direction), and joint 5 will have a wrist singularity. Therefore, joints 4 and 6 can be used as target joints, and the joint space virtual forces of the target joints 4 and 5 are calculated, and then the virtual forces generated by the joint hard limit are summarized.

[0188] It can be understood that, when it is determined that the joint angle of no joint of the robot is located in the hard limit buffer and is not currently close to a singular posture, normal dragging based on the force sensor can be performed under the admittance control algorithm.

[0189] The admittance control algorithm acts in Cartesian space, and the singular posture is also in Cartesian space. For collaborative machines, some joints have hard limits. When the admittance control is applied, the joint angles will reach the hard limits. Therefore, in this embodiment, a virtual force is added to consider the hard limits of the joints. When the joint angle reaches the buffer zone of the hard limit, a virtual force is generated according to the joint angle, which is then transformed to the base coordinate system through the force Jacobian matrix and then transformed to the end flange coordinate system. It is added to the virtual force generated in the singular posture area and acts together on the admittance control algorithm to achieve the stability and safety of human-computer interaction.

[0190] In the above embodiment, an admittance control algorithm for avoiding singular postures based on adaptive damping and virtual force is provided. In order to avoid the robot jitter phenomenon when avoiding singular postures, adaptive damping and virtual force thresholds are introduced. When the virtual force is generated and does not exceed the virtual force threshold, the current speed is rapidly attenuated to zero according to the admittance control speed. Afterwards, under the action of the virtual force, the function of accelerating away from the singular point is realized. An admittance control method that can cross singular postures is also provided: a judgment is made based on the actual angle of the robot and the designed joint angle threshold. When the joint angle is less than the set threshold, the admittance control is switched to a current loop-based drag mode; at the same time, the force sensor data is transformed to the joint, and added to the dynamic model, a new drag control law is designed, and the drag effect is adjusted by the drag gain. At the same time, a control method for constraining the hard limit of the robot is also provided: a hard limit buffer zone is established by the hard limit angle of the joint. Under the action of the admittance control algorithm, if the joint angle is within the hard limit buffer zone, the controller will calculate a virtual force to prevent the robot from reaching the hard limit; then, it is combined with the virtual force generated in the singular posture area and acts on the admittance control algorithm together, which can not only prevent the robot from reaching the hard limit, but also prevent the robot from reaching a singular posture, thereby ensuring the stability and safety of the robot system.

[0191] The robot control method provided in the above embodiment is an admittance control algorithm based on a six-dimensional force sensor. When the user uses the six-dimensional force sensor to complete the dragging operation or point teaching, the instability of the robot caused by algorithm defects is avoided, and the safety of human-computer interaction is improved. Without affecting the admittance control algorithm, the operation efficiency based on the force sensor can be improved, and the user experience can be enhanced.

[0192] In order to execute the corresponding steps in the above embodiments and various possible methods, a method for implementing a robot motion control device 200 is given below. Optionally, the robot motion control device can adopt the above Figure 1 The device structure of the electronic device 100 is shown. Fig. 9 , Fig. 9 A block diagram of a robot motion control device 200 provided in an embodiment of the present application. It should be noted that the basic principle and technical effects of the robot motion control device 200 provided in this embodiment are the same as those of the above-mentioned embodiments. For the sake of brief description, for parts not mentioned in this embodiment, reference may be made to the corresponding contents in the above-mentioned embodiments. In this embodiment, the robot motion control device 200 may include: an index calculation module 210, a virtual force calculation module 220, a judgment module 230, a damping adjustment calculation module 240 and a control module 250.

[0193] The index calculation module 210 is used to calculate the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot when the robot is currently in the first dragging mode based on admittance control.

[0194] The virtual force calculation module 220 is used to calculate the first virtual force according to the first translation performance index and the first rotation performance index when the current posture is close to a singular posture.

[0195] The determination module 230 is used to determine whether the total force magnitude of the first virtual force is greater than a preset force and whether the total torque magnitude is greater than a preset torque.

[0196] The damping adjustment calculation module 240 is used to calculate a target damping increase when the total force in the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment.

[0197] The control module 250 is used to control the movement of the robot in the first dragging mode according to the first virtual force and the target damping increase.

[0198] In this embodiment, the judgment module 230 may also be used to judge whether the current posture is close to a singular posture according to the first translation performance index and the first rotation performance index.

[0199] In this embodiment, the control module 250 is also used to determine whether the current singular posture processing strategy is avoidance or crossing when the robot is currently in the first drag mode based on admittance control. When the current singular posture processing strategy is avoidance, the index calculation module 210 calculates the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot. When the current singular posture processing strategy is crossing, if it is determined that the robot is currently approaching a singular posture, the control module 250 switches the current mode from the first drag mode to the second drag mode based on the current loop, and controls the robot movement in the second drag mode.

[0200] When the current singular posture processing strategy is crossing, the judgment module 230 is also used to: determine whether the robot is currently close to a singular posture according to the current joint state of the robot and the joint angle range or preset distance corresponding to at least one singular posture. The control module 250 is also used to: after switching to the second drag mode, when it is determined to be away from a singular posture, switch the current mode from the second drag mode to the first drag mode, and control the movement of the robot in the first drag mode, wherein whether to switch the mode is determined based on whether the current position is far away from or close to the singular posture, or based on whether the current position is far away from or close to the singular posture and the linear velocity and angular velocity of the end of the robot.

[0201] In this embodiment, the virtual force calculation module 220 is further used to calculate a second virtual force for constraining the robot to reach the joint hard limit when the joint angle of the target joint of the robot is currently in the hard limit buffer. The control module 250 is specifically used to control the movement of the robot in the first drag mode according to the first virtual force, the second virtual force and the target damping increase.

[0202] The control module 250 can also be used for: controlling the movement of the robot in the first drag mode according to the second virtual force when the current posture is not close to a singular posture but the joint angle of the target joint is in the hard limit buffer zone; and controlling the movement of the robot in the first drag mode according to the first virtual force and the second virtual force when it is determined that the total force in the first virtual force is greater than the preset force and the total torque is greater than the preset torque.

[0203] In this embodiment, the judgment module 230 can also be used to: for each target joint of the robot, determine whether the current joint angle of the target joint is in the hard limit buffer zone according to the current joint angle of the target joint, wherein the target joint does not include joints that can be in a singular position and corresponds to the rotation direction.

[0204] Optionally, the above modules can be stored in the form of software or firmware. Figure 1 The memory 110 shown in the figure may be fixed in the operating system (OS) of the electronic device 100 and may be Figure 1 Meanwhile, the data and program codes required for executing the above modules may be stored in the memory 110.

[0205] An embodiment of the present application also provides a readable storage medium having a computer program stored thereon, and the computer program implements the robot motion control method when executed by a processor.

[0206] In summary, the embodiments of the present application provide a robot motion control method, device, electronic device and readable storage medium. When the robot is currently in the first drag mode based on admittance control, the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot are calculated, and then when the robot is currently close to a singular posture, the first virtual force is calculated based on the first translation performance index and the first rotation performance index; then, it is determined whether the total force in the first virtual force is greater than the preset force and whether the total torque is greater than the preset torque; when it is determined that the total force in the first virtual force is not greater than the preset force or the total torque is not greater than the preset torque, the target damping increase is calculated, and the robot motion is controlled in the first drag mode based on the first virtual force and the target damping increase. In this way, the robot is prevented from being in a singular posture by applying a virtual force and adjusting the damping, while the degree of robot shaking that may be caused by speed changes is weakened.

[0207] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0208] In addition, the functional modules in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.

[0209] 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 essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0210] The above description is only an optional embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A robot motion control method, characterized in that: The method comprises: When the robot is currently in the first dragging mode based on admittance control, calculating a first translation performance index and a first rotation performance index corresponding to a current joint angle of the robot; In the case of a current approaching singular posture, a first virtual force is calculated according to the first translation performance index and the first rotation performance index; Determine whether the total force of the first virtual force is greater than a preset force and whether the total moment is greater than a preset moment; When the total force of the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment, a target damping increase is calculated; The robot is controlled to move in the first dragging mode according to the first virtual force and the target damping increase.

2. The method according to claim 1, characterized in that The step of calculating the first virtual force according to the first translation performance index and the first rotation performance index when the current position is close to a singular posture includes: For each direction in Cartesian space, the first joint angle and the second joint angle of the next moment are calculated according to the unit forward and reverse speeds in that direction, the preset duration, and the current joint angle corresponding to that direction; Obtaining a first performance indicator corresponding to the first joint angle and a second performance indicator corresponding to the second joint angle; Obtaining a gradient direction value corresponding to the direction according to a first performance indicator and a second performance indicator corresponding to the direction; The first virtual force is calculated according to the gradient direction values ​​corresponding to each direction of the Cartesian space, the first translation performance index and the first rotation performance index.

3. The method according to claim 2, characterized in that The first joint angle is an angle corresponding to a unit positive direction velocity, the second joint angle is an angle corresponding to a unit negative direction velocity, and obtaining the gradient direction value corresponding to the direction according to the first performance index and the second performance index corresponding to the direction includes: When the first performance index is less than a corresponding value or the second performance index is less than the corresponding value, determining that the gradient direction value of the direction is 0, wherein the corresponding value is an index corresponding to the first translation performance index and the first rotation performance index in the first translation performance index; In a case where a first difference between the first performance indicator and the corresponding value is not less than a second difference between the second performance indicator and the corresponding value, obtaining a gradient direction value in the direction based on the first difference; When the first difference is smaller than the second difference, a gradient direction value in the direction is obtained based on the second difference.

4. The method according to claim 1, characterized in that: The target damping increase amount is obtained by calculation, including: For each target dimension in the target damping increase, when the product of the force or torque corresponding to the target dimension in the first virtual force and the current speed corresponding to the target dimension is greater than 0, determine that the component of the target dimension in the target damping increase is 0, wherein when the total force in the first virtual force is not greater than the preset force, the target dimension includes the dimension corresponding to the translation direction, and when the total torque in the first virtual force is not greater than the preset torque, the target dimension includes the dimension corresponding to the rotation direction; When the product of the force or torque corresponding to the target dimension in the first virtual force and the current speed corresponding to the target dimension is less than 0, the component of the target dimension in the target damping increase is calculated according to the current speed corresponding to the target dimension, wherein the larger the current speed, the larger the component.

5. The method according to claim 1, characterized in that The method further comprises: Determine whether the first translation performance index is within a preset translation performance index range, wherein the minimum value of the preset translation performance index range is determined according to the translation performance index corresponding to the robot in different singular postures, and the maximum value of the preset translation performance index range is determined according to the translation performance index corresponding to the robot after moving from different singular postures toward a direction away from the singular posture; Determine whether the first rotation performance index is within a preset rotation performance index range, wherein the minimum value of the preset rotation performance index range is determined according to the rotation performance index corresponding to the robot in different singular postures, and the maximum value of the preset rotation performance index range is determined according to the rotation performance index corresponding to the robot after moving from different singular postures toward a direction away from the singular posture; If the first translation performance indicator is within the preset translation performance indicator range or the first rotation performance indicator is within the preset rotation performance indicator range, the current approaching singular posture is determined.

6. The method according to any one of claims 1 to 5, characterized in that: In the case of being in the first dragging mode based on admittance control, before calculating the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot, the method further includes: Determine whether the current singular posture processing strategy is avoidance or crossing; When the current singular posture processing strategy is avoidance, executing the step of calculating the first translation performance index and the first rotation performance index corresponding to the current joint angle of the robot; When the current singular posture processing strategy is crossing, if it is determined that the current position is close to a singular posture, the current mode is switched from the first drag mode to the second drag mode based on the current loop, and the robot movement is controlled in the second drag mode.

7. The method according to claim 6, characterized in that The method further comprises: Determining whether the robot is currently close to a singular posture according to the current joint state of the robot and the joint angle range or preset distance corresponding to at least one singular posture; and / or, After switching to the second drag mode, when it is determined that the robot is far away from the singular posture, the current mode is switched from the second drag mode to the first drag mode, and the robot movement is controlled in the first drag mode, wherein whether to switch the mode is determined based on whether the robot is currently far away from or close to the singular posture, or based on whether the robot is currently far away from or close to the singular posture and the linear velocity and angular velocity of the end of the robot.

8. The method according to any one of claims 1 to 5, characterized in that: The method further comprises: When the joint angle of the target joint of the robot is currently in the hard limit buffer zone, a second virtual force for constraining the robot to reach the joint hard limit is calculated; When the robot is not currently close to a singular posture but the joint angle of a target joint is in a hard limit buffer, the robot is controlled to move in the first drag mode according to the second virtual force; when it is determined that the total force of the first virtual force is greater than a preset force and the total torque is greater than a preset torque, the robot is controlled to move in the first drag mode according to the first virtual force and the second virtual force; The controlling the robot to move in the first drag mode according to the first virtual force and the target damping increase includes: The robot is controlled to move in the first dragging mode according to the first virtual force, the second virtual force and the target damping increase.

9. The method according to claim 8, characterized in that The method further comprises: For each target joint of the robot, judging whether the current joint angle of the target joint is in a hard limit buffer zone according to the current joint angle of the target joint, wherein the target joint does not include a joint that can be located in a singular posture and corresponds to a rotation direction; The method of calculating a second virtual force for constraining the robot to reach a joint hard limit when the joint angle of a target joint of the robot is currently in a hard limit buffer zone comprises: For each target joint of the robot, when the joint angle of the target joint is in a hard limit buffer zone, a joint space virtual force corresponding to the target joint is calculated according to the current joint angle of the target joint and the hard limit buffer zone in which the target joint is located; The second virtual force is calculated according to the joint space virtual forces corresponding to each target joint of the robot.

10. A robot motion control device, characterized in that: The device comprises: An index calculation module, used to calculate a first translation performance index and a first rotation performance index corresponding to a current joint angle of the robot when the robot is currently in a first dragging mode based on admittance control; A virtual force calculation module, configured to calculate a first virtual force according to the first translation performance index and the first rotation performance index when the current posture is close to a singular posture; A judging module, used for judging whether the total force of the first virtual force is greater than a preset force and whether the total moment is greater than a preset moment; a damping adjustment calculation module, configured to calculate a target damping increase when the total force in the first virtual force is not greater than the preset force or the total moment is not greater than the preset moment; A control module is used to control the movement of the robot in the first dragging mode according to the first virtual force and the target damping increase.

Images